diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..444dbdf --- /dev/null +++ b/.gitignore @@ -0,0 +1,54 @@ ++++++++++### Main git ignore files +### Beware, there are som gitignore files in subdirectories, for local and specific files + +# Generated executables +*.elf +*.hex + +# Generated CMake associated files +build/ +CMakeCache* +CMakeFiles* +cmake_install* + +# Generated make associated files +Makefile* + +# Generated scons / arduino files +.sconsign.dblite +*find_port + +# Generated python files +__pycache__/ +*calculate_xy + +# Generated npm / node files +node_modules/ +npm-debug.log + +# IDE specific files +*.sublime* +.idea +.vscode/ +cmake-build-debug/ + +# UTCoupe folders +bin/ + +# UTCoupe files +tmp.bmp +config.js + +# Some other files +*get_distance +*get_distance_intensity +*get_multiecho +*get_multiecho_intensity +*sensor_parameter +*sync_time_stamp +*liburg_c.a +*.swp +.directory + +# SymLink to default ROS CMakeLists.txt +ros_ws/src/CMakeLists.txt diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..b553cae --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "libs/json"] + path = libs/json + url = https://github.com/nlohmann/json diff --git a/C2018_Rules_final_FR.pdf b/C2018_Rules_final_FR.pdf new file mode 100644 index 0000000..33f04bb Binary files /dev/null and b/C2018_Rules_final_FR.pdf differ diff --git a/LICENCE.md b/LICENCE.md new file mode 100644 index 0000000..9cecc1d --- /dev/null +++ b/LICENCE.md @@ -0,0 +1,674 @@ + 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. + + {one line to give the program's name and a brief idea of what it does.} + Copyright (C) {year} {name of author} + + 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: + + {project} Copyright (C) {year} {fullname} + 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 0000000..82be9c6 --- /dev/null +++ b/README.md @@ -0,0 +1,84 @@ + Code source des robots d'UTCoupe 2018 +======= + +# Configuration + +Avant toute chose, il faut cloner le répertoire sur votre ordinateur : +``` +git clone git@github.com:utcoupe/coupe18.git +``` + +### Configurer l'environnement de développement + +Un script d'installation automatique est disponible. Allez dans le dossier coupe18, et lancer simplement : +``` +./scripts/install_utcoupe_setup.sh +``` + +Si c'est votre première installation, répondez "y" à toutes les questions. + +### Compiler le système + +Tout le système de cette année repose sur ROS (http://www.ros.org/), il faut donc compiler le système après l'avoir récupéré et configuré : +``` +cd coupe18/ros_ws +catkin_make +``` + +# Lancement + +//TODO + +ou must then source the workspace with `source devel/setup.bash` or `source devel/setup.zsh` each +time you open a new terminal. Adding this line to your `~/.bashrc` or `~/.zshrc` (with the full +path to the setup file) will simply automate this step. + +# Règles et Guidelines + +Afin d'avoir un projet organisé et fonctionnel, voici quelques règles (par convention ou importantes pour le +fonctionnement du projet) à suivre pour la création de branches git, paquets, noeuds ros, etc : + +### Git + +- Créer des branches sur git de la forme `namespace/package` si la branche correspond à un paquet ROS. (e.g. `ai/scheduler`, `memory/map`, etc) + +### Paquets ROS + +- Créer des paquets ROS nommés de la forme `namespace_package` (utile une fois qu'ils seront tous ensemble, ils seront ordonnés par +ordre alphabétique : plus visuel) + +- Créer des serveurs de `topics`/`services`/`actions` nommés de la forme `/namespace/package/server_name` s'ils peuvent être accédés par des paquets +extérieurs (ATTENTION : avec un `/` au début pour créer un nom absolu), `server_name` s'ils sont internes. + +- Nommer les fichiers de définition `.msg`/`.srv`/`.action` en PascalCase (e.g. `GetValues.srv`) et les variables dedans en minuscules (format `var_name`). + +### Python + +- Afin de respecter le PEP8 : 4 espaces d'intentation (et non tabs). + +### Données + +- Unités de distance en mètres, transportées par des `float32`. + +- Lors de la description d'une position d'une forme (cercle, ligne, rectangle, point...), donner la position par rapport au centre de la forme (sauf précision explicite et nécessaire). Par exemple, donner la position du centre d'un rectangle et non d'un coin. + +# Webclient + +Pour installer les dépendances du webclient : +``` +cd webclient +npm install --only=prod +``` + +Pour lancer le webclient : +``` +npm start +``` + +S'assurer que le noeud ROS `rosbridge_server` est bien lancé. + +Le webclient peut être lancé depuis le robot, ou depuis un ordinateur connecté à la Raspberry. + +Si le serveur est lancé sur le robot, se rendre sur `http://:8080`. + +Sinon, se rendre sur [http://localhost:8080](http://localhost:8080) et vérifier que le client se connecte bien à l'IP du robot dans les paramètres. diff --git a/arduino/CMakeLists.txt b/arduino/CMakeLists.txt new file mode 100644 index 0000000..d1f6fd9 --- /dev/null +++ b/arduino/CMakeLists.txt @@ -0,0 +1,47 @@ +cmake_minimum_required(VERSION 2.8) + +#todo include it in utcoupe workspace ! +set(CMAKE_TOOLCHAIN_FILE $ENV{UTCOUPE_WORKSPACE}/libs/arduino-cmake/cmake/ArduinoToolchain.cmake) # Arduino Toolchain + +set(ARDUINO_CXX_FLAGS "${ARDUINO_C_FLAGS} -std=c++11") + +# target are : arduino_r_t with r = pr/gr and t = asserv/others +project(arduino C CXX) + +set(ARDUINO_DEFAULT_BOARD mega2560) + +SET(TARGET_ARDUINO "mega2560" CACHE STRING "User defined arduino target") +SET(TARGET_ROBOT "gr" CACHE STRING "User defined robot") +SET(TARGET_PROGRAM "asserv" CACHE STRING "User defined program") + +#todo use the script to determine which port to use +if (TARGET_ARDUINO STREQUAL "nano328") + set(ARDUINO_DEFAULT_PORT /dev/ttyUSB0) +else() + set(ARDUINO_DEFAULT_PORT /dev/ttyACM0) +endif() + +SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_BINARY_DIR}/../bin) + +#set(TARGET_ROBOT gr) +#set(TARGET_PROGRAM asserv) + +file(GLOB_RECURSE SRC_FILES ${TARGET_ROBOT}/${TARGET_PROGRAM}/*.cpp ${TARGET_ROBOT}/${TARGET_PROGRAM}/*.c) +include_directories(${TARGET_ROBOT}/${TARGET_PROGRAM} common/${TARGET_PROGRAM} common/shared) + +#asserv or others extra common stuff +#set(EXTRA_SRC_FILES "") +file(GLOB_RECURSE EXTRA_SRC_FILES common/${TARGET_PROGRAM}/*.c common/${TARGET_PROGRAM}/*.cpp common/shared/*.cpp) +#if (TARGET_PROGRAM STREQUAL "asserv") +# message("asserv program detected") +# file(GLOB_RECURSE EXTRA_SRC_FILES common/${TARGET_PROGRAM}/*.c common/${TARGET_PROGRAM}/*.cpp) +#endif() + +generate_arduino_firmware(${TARGET_ROBOT}_${TARGET_PROGRAM}_${TARGET_ARDUINO} + SRCS ${SRC_FILES} ${EXTRA_SRC_FILES} +# HDRS ${HDR_FILES} +# LIBS HardwareSerial + PORT ${ARDUINO_DEFAULT_PORT} + BOARD ${TARGET_ARDUINO} +) + diff --git a/arduino/common/asserv/PID.c b/arduino/common/asserv/PID.c new file mode 100644 index 0000000..48e75bd --- /dev/null +++ b/arduino/common/asserv/PID.c @@ -0,0 +1,66 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 15/04/15 * + ****************************************/ + +#include "parameters.h" +#include "PID.h" +#include "compat.h" + +PID_t PID_left, PID_right; + +void PIDInit(PID_t *pid) { + pid->P = 0; + pid->I = 0; + pid->D = 0; + pid->bias = 0; + pid->error_sum = 0; + pid->last_error = 0; + pid->init_done = 0; +} + +void PIDReset(PID_t *pid) { + pid->error_sum = 0; + pid->last_error = 0; + pid->init_done = 0; +} + +void PIDSet(PID_t *pid, float P, float I, float D, float bias) { + I *= PID_I_RATIO; + D *= PID_D_RATIO; + I /= HZ; + D *= HZ; + pid->P = P; + pid->I = I; + pid->D = D; + pid->bias = bias; + PIDReset(pid); +} + +float PIDCompute(PID_t *pid, float error) { + float error_D, bias, P_part, I_part, D_part; + + if(!pid->init_done){ + //Lors du premier compute, on ne tient pas compte de D + error_D = 0; + pid->init_done = 1; + } else { + //derivée = deltaErreur/dt - dt est la période de compute + error_D = (error - pid->last_error); + } + + pid->error_sum = (pid->error_sum) + error; + pid->last_error = error; + + //calcul de la sortie avec le PID + bias = pid->bias; + P_part = pid->P * error; + I_part = pid->I * pid->error_sum; + if (I_part > PID_I_MAX) I_part = PID_I_MAX; + D_part = pid->D * error_D; + pid->output = bias + P_part + I_part + D_part; + if (pid->output > PID_OUT_MAX) pid->output = PID_OUT_MAX; + + return pid->output; +} diff --git a/arduino/common/asserv/PID.h b/arduino/common/asserv/PID.h new file mode 100644 index 0000000..865ff7c --- /dev/null +++ b/arduino/common/asserv/PID.h @@ -0,0 +1,23 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 15/04/15 * + ****************************************/ +#ifndef PID_H +#define PID_H + +typedef struct PID { + float P, I, D, bias; + float error_sum, last_error; + float output; + int init_done; +} PID_t; + + +extern PID_t PID_left, PID_right; +void PIDInit(PID_t *pid); +void PIDReset(PID_t *pid); +void PIDSet(PID_t *pid, float P, float I, float D, float bias); +float PIDCompute(PID_t *pid, float error); + +#endif diff --git a/arduino/common/asserv/asserv.cpp b/arduino/common/asserv/asserv.cpp new file mode 100644 index 0000000..e2e1105 --- /dev/null +++ b/arduino/common/asserv/asserv.cpp @@ -0,0 +1,106 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 13/10/13 * + ****************************************/ + +#include +#include "block.h" +#include "compat.h" +#include "parameters.h" +#include "protocol.h" +#include "control.h" +#include "pins.h" +#include "sender.h" +#include + +/** + * Main loop function, check if emergency stop and computes the new command to apply. + * The asserv is working with an internal fifo to execute orders. + * The loop is activate through a Timer (see arduino library). + */ +void asservLoop(); +void asservStatus(); + +// Run the loop for asserv at 100 Hz +Timer asservLoopTimer = Timer(10, &asservLoop); +Timer asservStatusTimer = Timer(100, &asservStatus); + +/** + * Read a \n ending string from serial port. + * The timeout is 50ms. + * When a string is received, execute the corresponding order. + */ +void serialRead() { + String receivedString; + if (Serial.available() > 0) { + receivedString = Serial.readStringUntil('\n'); + // SerialSender::SerialSend(SERIAL_INFO, receivedString); + receivedString.replace("\n", ""); + if (receivedString != "") { + parseAndExecuteOrder(receivedString); + } + } +} + +/** + * Arduino setup function, initialize pins and registers. + */ +void setup() { +#ifdef __AVR_ATmega32U4__ + Serial.begin(BAUDRATE); +#else + Serial.begin(BAUDRATE, SERIAL_TYPE); +#endif + Serial.setTimeout(5); + +#ifdef __AVR_ATmega2560__ + TCCR3B = (TCCR3B & 0xF8) | 0x01 ; + TCCR1B = (TCCR1B & 0xF8) | 0x01 ; +#else +#ifdef __AVR_ATmega328P__ + TCCR1B = (TCCR1B & 0xF8) | 0x01 ; +#endif +#endif + initPins(); + ControlInit(); + + asservLoopTimer.Start(); + asservStatusTimer.Start(); +} + +/** + * Arduino loop function, read from serial port and send internal serial port data. + * If it is the time to execute asserv, execute it. + */ +void loop() { + if (!flagArduinoConnected) { + SerialSender::SerialSend(SERIAL_INFO, "%s", ARDUINO_ID); + serialRead(); + } else { + asservLoopTimer.Update(); + asservStatusTimer.Update(); + } + SerialSender::SerialSendTask(); + if (!flagArduinoConnected) { + delay(1000); + } +} + +void asservLoop() { + //Action asserv + ComputeIsBlocked(); + ControlCompute(); + + // That's an ugly way to do it, but not working in another way... + // lastReachedID is defined in control.h file + if(lastReachedID != 0) { + SerialSender::SerialSend(SERIAL_INFO, "%d;", (int)lastReachedID); + lastReachedID = 0; + } +} + +void asservStatus() { + serialRead(); + ProtocolAutoSendStatus(); +} diff --git a/arduino/common/asserv/block.c b/arduino/common/asserv/block.c new file mode 100644 index 0000000..1a1858d --- /dev/null +++ b/arduino/common/asserv/block.c @@ -0,0 +1,44 @@ +#include "parameters.h" +#include "block.h" +#include "robotstate.h" +#include "goals.h" +#include "compat.h" + +void ComputeIsBlocked(void) { +#if BLOCK_TIME + static int last_goal_nr = -1; + static long last_time = 0; + static pos_t last_pos = {0, 0, 0, 0}; + long now; + float dist; + goal_t *current_goal; + + now = timeMillis(); + if (now - last_time < BLOCK_TIME) + return; + last_time = now; + + current_goal = FifoCurrentGoal(); + if (current_goal->type == NO_GOAL || + current_goal->type == TYPE_PWM || + current_goal->type == TYPE_SPD) + goto end; + if (fifo.current_goal != last_goal_nr) { + last_goal_nr = fifo.current_goal; + goto end; + } + + // goals type is pos or angle, goal didn't change and + // last calculation was at least BLOCK_TIME ms ago + + dist = sqrt(pow(current_pos.x - last_pos.x, 2) + pow(current_pos.y - last_pos.y, 2)); + dist += abs(current_pos.angle - last_pos.angle)*ENTRAXE_ENC/2.0; + if (dist < BLOCK_MIN_DIST) { + // we did not move enough, we are probably blocked, + // consider the goal reached + current_goal->is_reached = 1; + } +end: + last_pos = current_pos; +#endif +} diff --git a/arduino/common/asserv/block.h b/arduino/common/asserv/block.h new file mode 100644 index 0000000..fbd6673 --- /dev/null +++ b/arduino/common/asserv/block.h @@ -0,0 +1,10 @@ +#ifndef BLOCK_H +#define BLOCK_H + +#ifdef __cplusplus +extern "C" void ComputeIsBlocked(void); +#else +void ComputeIsBlocked(void); +#endif + +#endif diff --git a/arduino/common/asserv/compat.cpp b/arduino/common/asserv/compat.cpp new file mode 100644 index 0000000..40705ed --- /dev/null +++ b/arduino/common/asserv/compat.cpp @@ -0,0 +1,23 @@ +#include "compat.h" +#include "parameters.h" + +extern "C" void serial_print_int(int i) { + SERIAL_MAIN.print(i); +} + +extern "C" void serial_print_float(float f) { + SERIAL_MAIN.print(f); +} + +extern "C" void serial_print(const char *str) { + SERIAL_MAIN.println(str); +} + +extern "C" void serial_send(char data) { //Envoi d'un octet en serial, dépend de la plateforme + SERIAL_MAIN.write(data); +} + +extern "C" char generic_serial_read(){ + return SERIAL_MAIN.read(); +} + diff --git a/arduino/common/asserv/compat.h b/arduino/common/asserv/compat.h new file mode 100644 index 0000000..9028925 --- /dev/null +++ b/arduino/common/asserv/compat.h @@ -0,0 +1,70 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 13/10/13 * + ****************************************/ +#ifndef COMPAARDUINO_H +#define COMPAARDUINO_H + +#include +#include "encoder.h" +#include "parameters.h" +#include "pins.h" + +extern inline unsigned long timeMillis(){ + return millis(); +} +extern inline unsigned long timeMicros(){ + return micros(); +} + +#ifdef __cplusplus +extern "C" void serial_print_int(int i); +extern "C" char generic_serial_read(); +extern "C" void serial_send(char data); +extern "C" void serial_print(const char *str); +extern "C" void serial_print_float(float f); +#else +void serial_print_int(int i); +char generic_serial_read(); +void serial_send(char data); +void serial_print(const char *str); +void serial_print_float(float f); +#endif + +extern inline void initPins(){ + //set mode des pins pour arduino + pinMode(PIN_ENC_LEFT_A,INPUT_PULLUP); + pinMode(PIN_ENC_LEFT_B,INPUT_PULLUP); + pinMode(PIN_ENC_RIGHT_A,INPUT_PULLUP); + pinMode(PIN_ENC_RIGHT_B,INPUT_PULLUP); + + pinMode(LED_DEBUG, OUTPUT); + pinMode(LED_MAINLOOP, OUTPUT); + pinMode(LED_BLOCKED, OUTPUT) ; + + digitalWrite(LED_DEBUG, LOW); //LOW = eteinte + digitalWrite(LED_MAINLOOP, LOW); //LOW = eteinte + digitalWrite(LED_BLOCKED, LOW); //LOW = eteinte + //Definition des interruptions arduino en fonction du type d'évaluation +#ifdef LED_JACK + pinMode(LED_JACK, OUTPUT); +#endif +#if ENCODER_EVAL == 4 + attachInterrupt(INTERRUPT_ENC_LEFT_A,leftInterruptA,CHANGE); + attachInterrupt(INTERRUPT_ENC_RIGHT_A,rightInterruptA,CHANGE); + attachInterrupt(INTERRUPT_ENC_LEFT_B,leftInterruptB,CHANGE); + attachInterrupt(INTERRUPT_ENC_RIGHT_B,rightInterruptB,CHANGE); +#elif ENCODER_EVAL == 2 + attachInterrupt(INTERRUPT_ENC_LEFT_A,leftInterruptA,CHANGE); + attachInterrupt(INTERRUPT_ENC_RIGHT_A,rightInterruptA,CHANGE); +#elif ENCODER_EVAL == 1 + attachInterrupt(INTERRUPT_ENC_LEFT_A,leftInterruptA,RISING); + attachInterrupt(INTERRUPT_ENC_RIGHT_A,rightInterruptA,RISING); +#endif +#ifdef PIN_JACK + pinMode(PIN_JACK,INPUT_PULLUP); +#endif +} + +#endif diff --git a/arduino/common/asserv/control.c b/arduino/common/asserv/control.c new file mode 100644 index 0000000..2e3f5d1 --- /dev/null +++ b/arduino/common/asserv/control.c @@ -0,0 +1,337 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 29/11/13 * + ****************************************/ + +#include "encoder.h" +#include "robotstate.h" +#include "goals.h" +#include "control.h" +#include "compat.h" +#include "motor.h" +#include "local_math.h" +#include + +#define ANG_REACHED (0x1) +#define POS_REACHED (0x2) +#define REACHED (ANG_REACHED | POS_REACHED) + +#define sign(x) ((x)>=0?1:-1) + +uint16_t lastReachedID = 0; + +control_t control; + +void goalPos(goal_t *goal); +void goalPwm(goal_t *goal); +void goalSpd(goal_t *goal); +void goalAngle(goal_t *goal); +int controlPos(float dd, float da); + +float calcSpeed(float init_spd, float dd, float max_spd, float final_speed); +void applyPID(void); +void applyPwm(void); +void allStop(void); +void stopRobot(void); + +void ControlSetStop(int mask) { + control.status_bits |= mask; +} + +void ControlUnsetStop(int mask) { + control.status_bits &= ~mask; +} + +void ControlInit(void) { + control.reset = 1; + control.status_bits = 0; + control.speeds.angular_speed = 0, + control.speeds.linear_speed = 0; + control.last_finished_id = 0; + + control.max_acc = ACC_MAX; + control.max_spd = SPD_MAX; + control.rot_spd_ratio = RATIO_ROT_SPD_MAX; + + MotorsInit(); + RobotStateInit(); + FifoInit(); + PIDInit(&PID_left); + PIDInit(&PID_right); + PIDSet(&PID_left, LEFT_P, LEFT_I, LEFT_D, LEFT_BIAS); + PIDSet(&PID_right, RIGHT_P, RIGHT_I, RIGHT_D, RIGHT_BIAS); +} + +void ControlReset(void) { + control.speeds.linear_speed = 0; + control.last_finished_id = 0; + FifoClearGoals(); + RobotStateReset(); + ControlPrepareNewGoal(); +} + +void ControlPrepareNewGoal(void) { + control.order_started = 0; + PIDReset(&PID_left); + PIDReset(&PID_right); +} + +void ControlCompute(void) { +#if TIME_BETWEEN_ORDERS + static long time_reached = -1; + long now; + now = timeMicros(); +#endif + + goal_t* current_goal = FifoCurrentGoal(); + RobotStateUpdate(); + + if (control.status_bits & EMERGENCY_BIT || control.status_bits & PAUSE_BIT || control.status_bits & TIME_ORDER_BIT) { + stopRobot(); + } else { + switch (current_goal->type) { + case TYPE_ANG: + goalAngle(current_goal); + break; + case TYPE_POS: + goalPos(current_goal); + break; + case TYPE_PWM: + goalPwm(current_goal); + break; + case TYPE_SPD: + goalSpd(current_goal); + break; + default: + stopRobot(); + break; + } + } + + applyPwm(); + + if (current_goal->is_reached) { + control.last_finished_id = current_goal->ID; + // Instead of calling SerialSend directly (does not work), we use a global variable to send the id from main +// SerialSendWrapVar(SERIAL_INFO, "%d;", (int)control.last_finished_id); + lastReachedID = control.last_finished_id; + FifoNextGoal(); + ControlPrepareNewGoal(); + +#if TIME_BETWEEN_ORDERS + time_reached = now; + } + if (time_reached > 0 && (now - time_reached) < TIME_BETWEEN_ORDERS*1000000) { + ControlSetStop(TIME_ORDER_BIT); + } else { + ControlUnsetStop(TIME_ORDER_BIT); + time_reached = -1; +#endif + } +} + +/* INTERNAL FUNCTIONS */ + +void goalPwm(goal_t *goal) { + static long start_time; + long now = timeMicros(); + if (!control.order_started){ + start_time = now; + control.order_started = 1; + } + if ((now - start_time)/1000.0 <= goal->data.pwm_data.time){ + float pwmR, pwmL; + pwmL = goal->data.pwm_data.pwm_l; + pwmR = goal->data.pwm_data.pwm_r; + + control.speeds.pwm_left = pwmL; + control.speeds.pwm_right = pwmR; + } + else { + control.speeds.pwm_left = 0; + control.speeds.pwm_right = 0; + goal->is_reached = 1; + } +} + +void goalSpd(goal_t *goal) { + static long start_time; + long now = timeMicros(); + if (!control.order_started){ + start_time = now; + control.order_started = 1; + } + if ((now - start_time)/1000.0 <= goal->data.spd_data.time){ + float time_left, v_dec; + time_left = (goal->data.spd_data.time - ((now - start_time)/1000.0)) / 1000.0; + v_dec = time_left * control.max_acc; + + control.speeds.linear_speed = min(min( + control.speeds.linear_speed+DT*control.max_acc, + goal->data.spd_data.lin), + v_dec); + control.speeds.angular_speed = min(min( + control.speeds.angular_speed+DT*control.max_acc, + goal->data.spd_data.ang), + v_dec); + } + else { + control.speeds.linear_speed = 0; + control.speeds.angular_speed = 0; + goal->is_reached = 1; + } + applyPID(); +} + +void goalAngle(goal_t *goal) { + float angle, da; + angle = goal->data.ang_data.angle; + da = angle - current_pos.angle; + + if (goal->data.ang_data.modulo) { + da = moduloTwoPI(da); + } + + if (controlPos(0, da) & ANG_REACHED) { + goal->is_reached = 1; + } + applyPID(); +} + +void goalPos(goal_t *goal) { + int x, y; + float dx, dy, da, dd, goal_a; + + x = goal->data.pos_data.x; + y = goal->data.pos_data.y; + dx = x - current_pos.x; + dy = y - current_pos.y; + goal_a = atan2(dy, dx); + da = (goal_a - current_pos.angle); + da = moduloTwoPI(da); + dd = sqrt(pow(dx, 2.0)+pow(dy, 2.0)); + + if (goal->data.pos_data.d == ANY) { + if (abs(da) > CONE_ALIGNEMENT) { + da = moduloPI(da); + dd = - dd; + } + } else if (goal->data.pos_data.d == BACKWARD) { + dd = - dd; + da = moduloTwoPI(da+M_PI); + } + + if (controlPos(dd, da) & POS_REACHED) { + goal->is_reached = 1; + } + applyPID(); +} + +int controlPos(float dd, float da) { + int ret; + float dda, ddd, max_speed; + float ang_spd, lin_spd; + + dda = da * (ENTRAXE_ENC/2); + ddd = dd * exp(-abs(K_DISTANCE_REDUCTION*da)); + + max_speed = control.max_spd; + if (control.status_bits & SLOWGO_BIT) { + max_speed *= EMERGENCY_SLOW_GO_RATIO; + } + + ang_spd = control.speeds.angular_speed; + lin_spd = control.speeds.linear_speed; + + control.speeds.angular_speed = calcSpeed(ang_spd, dda, + max_speed * control.rot_spd_ratio, 0); + control.speeds.linear_speed = calcSpeed(lin_spd, ddd, + max_speed, 0); + + ret = 0; + if (abs(dd) < ERROR_POS) { + ret |= POS_REACHED; + } + if (abs(da) < ERROR_ANGLE) { + ret |= ANG_REACHED; + } + + return ret; +} + +float calcSpeed(float init_spd, float dd, float max_spd, float final_speed) { + float dd_abs, acc_spd, dec_spd, target_spd; + int d_sign; + dd_abs = abs(dd); + d_sign = sign(dd); + + init_spd *= d_sign; + acc_spd = init_spd + (control.max_acc*DT); + dec_spd = sqrt(pow(final_speed, 2) + 2*control.max_acc*dd_abs); + target_spd = min(max_spd, min(acc_spd, dec_spd))*d_sign; + return target_spd; +} + +void stopRobot(void) { + int sign; + float speed; + + speed = abs(control.speeds.angular_speed); + if (BRK_COEFF != 0.0) { + speed -= control.max_acc * DT * BRK_COEFF; + } else { + speed = 0.0; + } + speed = max(0, speed); + control.speeds.angular_speed = speed; + + sign = sign(control.speeds.linear_speed); + speed = abs(control.speeds.linear_speed); + if (BRK_COEFF != 0.0) { + speed -= control.max_acc * DT * BRK_COEFF; + } else { + speed = 0.0; + } + speed = max(0, speed); + control.speeds.linear_speed = sign*speed; + + if (abs(wheels_spd.left) + abs(wheels_spd.right) < SPD_TO_STOP) { + allStop(); + } else { + applyPID(); + } +} + +void allStop(void) { + control.speeds.pwm_left = 0; + control.speeds.pwm_right = 0; + control.speeds.linear_speed = 0; + control.speeds.angular_speed = 0; +} + +void applyPwm(void) { + set_pwm_left(control.speeds.pwm_left); + set_pwm_right(control.speeds.pwm_right); +} + +float speedToPwm(float speed) { + float pwm = SPD_TO_PWM_A*speed; + if (speed > 0.001) pwm += SPD_TO_PWM_B; + else if(speed < -0.001) pwm -= SPD_TO_PWM_B; + return pwm; +} + +void applyPID(void) { + float left_spd, right_spd; + float left_ds, right_ds; + left_spd = control.speeds.linear_speed - control.speeds.angular_speed; + right_spd = control.speeds.linear_speed + control.speeds.angular_speed; + left_ds = left_spd - wheels_spd.left; + right_ds = right_spd - wheels_spd.right; + // Control feed forward + //control.speeds.pwm_left = speedToPwm(left_spd) + PIDCompute(&PID_left, left_ds); + //control.speeds.pwm_right = speedToPwm(right_spd) + PIDCompute(&PID_right, right_ds); + control.speeds.pwm_left = PIDCompute(&PID_left, left_ds); + control.speeds.pwm_right = PIDCompute(&PID_right, right_ds); +} diff --git a/arduino/common/asserv/control.h b/arduino/common/asserv/control.h new file mode 100644 index 0000000..bfd98d5 --- /dev/null +++ b/arduino/common/asserv/control.h @@ -0,0 +1,58 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 29/11/13 * + ****************************************/ +#ifndef CONTROL_H +#define CONTROL_H + +#include +#include "PID.h" +#include "parameters.h" + +#define PAUSE_BIT (1<<0) +#define EMERGENCY_BIT (1<<1) +#define SLOWGO_BIT (1<<2) +#define TIME_ORDER_BIT (1<<3) + +#define ANY 0 +#define FORWARD 1 +#define BACKWARD -1 + +typedef struct control { + struct speeds { + int pwm_left, pwm_right; + float angular_speed, linear_speed; + } speeds; + float max_acc, max_spd, rot_spd_ratio; + uint8_t reset; + uint16_t last_finished_id; + uint16_t order_started; + int status_bits; +} control_t; + +extern control_t control; +extern PID_t PID_left, PID_right; + +void ControlPrepareNewGoal(void); +void ControlReset(void); +void ControlSetStop(int mask); +void ControlUnsetStop(int mask); + +#ifdef __cplusplus +extern "C" void ControlInit(void); +extern "C" void ControlCompute(void); +#else +void ControlInit(void); +void ControlCompute(void); +#endif + +#ifdef __cplusplus +extern "C" { +#endif +extern uint16_t lastReachedID; +#ifdef __cplusplus +} +#endif + +#endif diff --git a/arduino/common/asserv/encoder.c b/arduino/common/asserv/encoder.c new file mode 100644 index 0000000..47e978b --- /dev/null +++ b/arduino/common/asserv/encoder.c @@ -0,0 +1,119 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 18/04/15 * + ****************************************/ +#include "encoder.h" +#include "compat.h" +#include "pins.h" + +volatile long left_ticks = 0; +volatile long right_ticks = 0; + +int left_last_value_A = 0; +int left_last_value_B = 0; +int right_last_value_A = 0; +int right_last_value_B = 0; + +void left_encoder_reset(void) { + left_ticks = 0; +} + +void right_encoder_reset(void) { + right_ticks = 0; +} + +#if ENCODER_EVAL == 4 +inline void interruptA(volatile long *ticks, int *last_value_A, int last_value_B, int pin) { + int new_value; + new_value = digitalRead(pin); + if(new_value == 1) + if(last_value_B == 1) + (*ticks)--; + else + (*ticks)++; + + else + if(last_value_B == 1) + (*ticks)++; + else + (*ticks)--; + *last_value_A = new_value; +} + +inline void interruptB(volatile long *ticks, int *last_value_B, int last_value_A, int pin) { + bool new_value; + new_value = digitalRead(pin); + if(new_value == 1) + if(last_value_A == 1) + (*ticks)++; + else + (*ticks)--; + + else + if(last_value_A == 1) + (*ticks)--; + else + (*ticks)++; + *last_value_B = new_value; +} + +void leftInterruptA(void) { + interruptA(&left_ticks, &left_last_value_A, + left_last_value_B, PIN_ENC_LEFT_A); +} + +void rightInterruptA(void) { + interruptA(&right_ticks, &right_last_value_A, + right_last_value_B, PIN_ENC_RIGHT_A); +} + +void leftInterruptB(void) { + interruptA(&left_ticks, &left_last_value_B, + left_last_value_A, PIN_ENC_LEFT_A); +} + +void rightInterruptB(void) { + interruptA(&right_ticks, &right_last_value_B, + right_last_value_A, PIN_ENC_RIGHT_A); +} +#elif ENCODER_EVAL == 2 +void interruptA(volatile long *ticks, int pin_a, int pin_b){ + int value_A, value_B; + value_A = digitalRead(pin_a); + value_B = digitalRead(pin_b); + if(value_A == 1) + if(value_B == 1) + (*ticks)--; + else + (*ticks)++; + else + if(value_B == 1) + (*ticks)++; + else + (*ticks)--; +} + +void leftInterruptA(void) { + interruptA(&left_ticks, PIN_ENC_LEFT_A, PIN_ENC_LEFT_B); +} + +void rightInterruptA(void) { + interruptA(&right_ticks, PIN_ENC_RIGHT_A, PIN_ENC_RIGHT_B); +} +#elif ENCODER_EVAL == 1 +void interruptA(volatile long *ticks, int pin_b){ + if(digitalRead(pin_b) == 1) + (*ticks)--; + else + (*ticks)++; +} + +void leftInterruptA(void) { + interruptA(&left_ticks, PIN_ENC_LEFT_B); +} + +void rightInterruptA(void) { + interruptA(&right_ticks, PIN_ENC_RIGHT_B); +} +#endif diff --git a/arduino/common/asserv/encoder.h b/arduino/common/asserv/encoder.h new file mode 100644 index 0000000..9a16f73 --- /dev/null +++ b/arduino/common/asserv/encoder.h @@ -0,0 +1,38 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 18/04/15 * + ****************************************/ +#ifndef ENCODER_H +#define ENCODER_H + +#include "parameters.h" + +extern volatile long left_ticks; +extern volatile long right_ticks; + +void left_encoder_reset(void); +void right_encoder_reset(void); + +extern inline void encoders_reset(void) { + left_encoder_reset(); + right_encoder_reset(); +} + +#ifdef __cplusplus +extern "C" void leftInterruptA(void); +extern "C" void rightInterruptA(void); +#if ENCODER_EVAL == 4 +void leftInterruptB(void); +void rightInterruptB(void); +#endif +#else +void leftInterruptA(void); +void rightInterruptA(void); +#if ENCODER_EVAL == 4 +void leftInterruptB(void); +void rightInterruptB(void); +#endif +#endif + +#endif diff --git a/arduino/common/asserv/goals.c b/arduino/common/asserv/goals.c new file mode 100644 index 0000000..668ec04 --- /dev/null +++ b/arduino/common/asserv/goals.c @@ -0,0 +1,57 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 13/10/13 * + ****************************************/ +#include "goals.h" + +fifo_t fifo; + +void FifoInit() { + int i; + fifo.nb_goals = 0; + fifo.current_goal = 0; + fifo.last_goal = -1; + for (i=0; i= MAX_GOALS) { + return -1; + } + + fifo.last_goal = (fifo.last_goal + 1) % MAX_GOALS; + new_goal = &fifo.fifo[fifo.last_goal]; + + new_goal->type = type; + new_goal->data = data; + new_goal->ID = ID; + new_goal->is_reached = 0; + + fifo.nb_goals++; + return 0; + +} + +goal_t* FifoCurrentGoal() { + return &fifo.fifo[fifo.current_goal]; +} + +goal_t* FifoNextGoal() { + goal_t *current_goal = FifoCurrentGoal(); +#if KEEP_LAST_GOAL + if (current_goal->type != NO_GOAL && + fifo.nb_goals > 1) { +#else + if (current_goal->type != NO_GOAL) { +#endif + current_goal->type = NO_GOAL; + current_goal->is_reached = 0; + fifo.current_goal = (fifo.current_goal + 1) % MAX_GOALS; + fifo.nb_goals--; + } + return FifoCurrentGoal(); +} diff --git a/arduino/common/asserv/goals.h b/arduino/common/asserv/goals.h new file mode 100644 index 0000000..bb64ae4 --- /dev/null +++ b/arduino/common/asserv/goals.h @@ -0,0 +1,67 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 13/10/13 * + ****************************************/ +#ifndef GOALS_H +#define GOALS_H + +#include +#include "parameters.h" +#define MAX_GOALS 15 //nombre max de goals dans la file, évite surcharge mémoire + +#define TYPE_POS 1 +#define TYPE_ANG 2 +#define TYPE_PWM 3 +#define TYPE_SPD 4 +#define NO_GOAL -1 + +typedef struct pos_data { + int x, y, d; +} pos_data_t; + +typedef struct ang_data { + float angle; + int modulo; +} ang_data_t; + +typedef struct pwm_data { + float time; + int pwm_l, pwm_r; +} pwm_data_t; + +typedef struct spd_data { + float time; + int lin, ang; +} spd_data_t; + +typedef union goal_data { + pos_data_t pos_data; + ang_data_t ang_data; + pwm_data_t pwm_data; + spd_data_t spd_data; +} goal_data_t; + +typedef struct goal { + goal_data_t data; + int type; + uint16_t ID; + int is_reached; +} goal_t; + +typedef struct fifo { + goal_t fifo[MAX_GOALS]; + int nb_goals; + int current_goal; + int last_goal; + +} fifo_t; + +extern fifo_t fifo; +void FifoInit(); +int FifoPushGoal(int ID, int type, goal_data_t data); +goal_t* FifoCurrentGoal(); +goal_t* FifoNextGoal(); +extern inline void FifoClearGoals() { FifoInit(); } + +#endif diff --git a/arduino/common/asserv/local_math.c b/arduino/common/asserv/local_math.c new file mode 100644 index 0000000..024da2d --- /dev/null +++ b/arduino/common/asserv/local_math.c @@ -0,0 +1,28 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 25/10/13 * + ****************************************/ + +#include "local_math.h" +#include + +float moduloTwoPI(float angle){ + if(angle >= 0) + while(angle > M_PI) + angle -= 2.0*M_PI; + else + while(angle <= -M_PI) + angle += 2.0*M_PI; + return angle; +} + +float moduloPI(float angle){ + if(angle >= 0) + while(angle > M_PI/2) + angle -= M_PI; + else + while(angle <= -M_PI/2) + angle += M_PI; + return angle; +} diff --git a/arduino/common/asserv/local_math.h b/arduino/common/asserv/local_math.h new file mode 100644 index 0000000..2f412da --- /dev/null +++ b/arduino/common/asserv/local_math.h @@ -0,0 +1,15 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 25/10/13 * + ****************************************/ + +#ifndef LOCAL_MATH_H +#define LOCAL_MATH_H + +#define MAX(a,b) a > b ? a : b + +float moduloTwoPI(float angle); +float moduloPI(float angle); + +#endif diff --git a/arduino/common/asserv/pins.h b/arduino/common/asserv/pins.h new file mode 100644 index 0000000..b9821e2 --- /dev/null +++ b/arduino/common/asserv/pins.h @@ -0,0 +1,63 @@ +#ifndef PINS_H +#define PINS_H + +// nano +#ifdef __AVR_ATmega328P__ +#define MOTOR1_EN 7 +#define MOTOR2_EN 12 + +#define MOTOR1_SPD 9 +#define MOTOR2_SPD 10 + +#define MOTOR1_BRK 8 +#define MOTOR2_BRK 11 + +#define PIN_ENC_LEFT_A 2 +#define PIN_ENC_LEFT_B 4 +#define PIN_ENC_RIGHT_A 3 +#define PIN_ENC_RIGHT_B 5 + +#define INTERRUPT_ENC_LEFT_A 0 +#define INTERRUPT_ENC_RIGHT_A 1 + +#define PIN_SHARP_FORWARD 6 +#define PIN_SHARP_BACKWARD 7 + +#define LED_MAINLOOP 14 +#define LED_DEBUG 13 +#define LED_BLOCKED 14 +#endif + +// mega +#ifdef __AVR_ATmega2560__ +#define MOTOR1_EN 30 +#define MOTOR2_EN 34 + +#define MOTOR1_SPD 3 +#define MOTOR2_SPD 2 + +#define MOTOR1_RDY 32 +#define MOTOR2_RDY 36 + +#define PIN_ENC_LEFT_A 19 +#define PIN_ENC_LEFT_B 18 +#define PIN_ENC_RIGHT_A 20 +#define PIN_ENC_RIGHT_B 21 + +#define PIN_SHARP_FORWARD 14 +#define PIN_SHARP_BACKWARD 15 + +#define INTERRUPT_ENC_LEFT_A 4 +#define INTERRUPT_ENC_LEFT_B 5 +#define INTERRUPT_ENC_RIGHT_A 3 +#define INTERRUPT_ENC_RIGHT_B 2 + +//#define PIN_JACK 52 +//#define LED_JACK 51 +// +#define LED_DEBUG 16 +#define LED_BLOCKED 16 +#define LED_MAINLOOP 16 +#endif + +#endif diff --git a/arduino/common/asserv/protocol.cpp b/arduino/common/asserv/protocol.cpp new file mode 100644 index 0000000..742d0cd --- /dev/null +++ b/arduino/common/asserv/protocol.cpp @@ -0,0 +1,299 @@ +// +// Created by tfuhrman on 09/05/17. +// + +#include + +#include +#include +#include +#include "protocol.h" +#include "sender.h" +#include + +//get from old protocol file +extern "C" { + #include "compat.h" + #include "robotstate.h" + #include "control.h" + #include "goals.h" +} + +unsigned char flagArduinoConnected = 0; + +void autoSendStatus() { + SerialSender::SerialSend(SERIAL_INFO, "%c;%i;%i;%i;%i;%i;%i;%i;%i;%i;", AUTO_SEND, control.last_finished_id, (int)current_pos.x, (int)current_pos.y,(int)(current_pos.angle*FLOAT_PRECISION), + control.speeds.pwm_left, control.speeds.pwm_right, (int)(control.speeds.linear_speed), (int)wheels_spd.left, (int)wheels_spd.right); +#if DEBUG_TARGET_SPEED +// index += sprintf(message+index, ";%i;%i;%i;%i", +// (int)wheels_spd.left, +// (int)wheels_spd.right, +// (int)(control.speeds.linear_speed - control.speeds.angular_speed), +// (int)(control.speeds.linear_speed + control.speeds.angular_speed)); +#endif +} + + +void ProtocolAutoSendStatus() { + autoSendStatus(); +} + +uint8_t getLog10(const uint16_t number) { + if(number>=10000) return 5; + if(number>=1000) return 4; + if(number>=100) return 3; + if(number>=10) return 2; + return 1; +} + +void emergencyStop(const uint8_t enable) { + // Reset the PID to remove the error sum + PIDReset(&PID_left); + PIDReset(&PID_right); + if (enable == 0) { + digitalWrite(LED_DEBUG, LOW); + ControlUnsetStop(EMERGENCY_BIT); + } else { + digitalWrite(LED_DEBUG, HIGH); + ControlSetStop(EMERGENCY_BIT); + } +} + +//order is order;id_servo;params +void parseAndExecuteOrder(const String& order) { + static char receivedOrder[25]; + char* receivedOrderPtr = receivedOrder; + // + 1 because of terminal character + order.toCharArray(receivedOrder, order.length() + 1); + char orderChar = receivedOrder[ORDER_INDEX]; + uint16_t order_id = (uint16_t) atoi(&receivedOrder[ID_INDEX]); + uint8_t numberDigits = getLog10(order_id); + // Move to the first parameter of the order + receivedOrderPtr += ID_INDEX + numberDigits + (uint8_t)1; + switch (orderChar) { + case START: + { + emergencyStop(0); + // Ack that arduino has started + SerialSender::SerialSend(SERIAL_INFO, "%d;", order_id); + SerialSender::SerialSend(SERIAL_DEBUG, "Arduino %s has started (%d)", ARDUINO_ID, order_id); + flagArduinoConnected = 1; + break; + } + case HALT: + { + emergencyStop(1); + // Ack that arduino has stopped + SerialSender::SerialSend(SERIAL_INFO, "%d;", order_id); + SerialSender::SerialSend(SERIAL_DEBUG, "Arduino %s has stopped (%d)", ARDUINO_ID, order_id); + flagArduinoConnected = 0; + break; + } + case PINGPING: + //todo add LED on arduino + digitalWrite(LED_DEBUG, HIGH); + delay(1); + digitalWrite(LED_DEBUG, LOW); + SerialSender::SerialSend(SERIAL_INFO, "%d;", order_id); + break; + case GET_CODER: + SerialSender::SerialSend(SERIAL_INFO, "%d;%d;%d;", order_id, left_ticks, right_ticks); + break; + case GOTO: + { + int x, y, direction; + direction = 0; + sscanf(receivedOrderPtr, "%i;%i;%i", &x, &y, &direction); + goal_data_t goal; + goal.pos_data = {x, y, direction}; + FifoPushGoal(order_id, TYPE_POS, goal); + break; + } + case GOTOA: + { + int x, y, a_int, direction; + float a; + direction = 0; + sscanf(receivedOrderPtr, "%i;%i;%i;%i", &x, &y, &a_int, &direction); + a = a_int / (float)FLOAT_PRECISION; + goal_data_t goal; + goal.pos_data = {x, y, direction}; + FifoPushGoal(order_id, TYPE_POS, goal); + goal.ang_data = {a, 1}; + FifoPushGoal(order_id, TYPE_ANG, goal); + break; + } + case ROT: + { + int a_int; + float a; + sscanf(receivedOrderPtr, "%i", &a_int); + a = a_int / (float)FLOAT_PRECISION; + goal_data_t goal; + goal.ang_data = {a, 1}; + FifoPushGoal(order_id, TYPE_ANG, goal); + break; + } + case ROTNOMODULO: + { + long a_int; + float a; + sscanf(receivedOrderPtr, "%li", &a_int); + a = a_int / (float)FLOAT_PRECISION; + goal_data_t goal; + goal.ang_data = {a, 0}; + FifoPushGoal(order_id, TYPE_ANG, goal); + break; + } + case PWM: + { + int l, r, t; + sscanf(receivedOrderPtr, "%i;%i;%i", &l, &r, &t); + goal_data_t goal; + goal.pwm_data = {(float)t, l, r}; + FifoPushGoal(order_id, TYPE_PWM, goal); + break; + } + case SPD: + { + int l, a, t; + sscanf(receivedOrderPtr, "%i;%i;%i", &l, &a, &t); + goal_data_t goal; + goal.spd_data = {(float)t, l, a}; + FifoPushGoal(order_id, TYPE_SPD, goal); + break; + } + case PIDALL: + case PIDRIGHT: + case PIDLEFT: + { + long p_int, i_int, d_int; + float p, i, d; + sscanf(receivedOrderPtr, "%li;%li;%li", &p_int, &i_int, &d_int); + p = p_int / (float)FLOAT_PRECISION; + i = i_int / (float)FLOAT_PRECISION; + d = d_int / (float)FLOAT_PRECISION; + if (orderChar == PIDLEFT) + PIDSet(&PID_left, p, i, d, LEFT_BIAS); + else if (orderChar == PIDRIGHT) + PIDSet(&PID_right, p, i, d, RIGHT_BIAS); + else { + PIDSet(&PID_left, p, i, d, LEFT_BIAS); + PIDSet(&PID_right, p, i, d, RIGHT_BIAS); + } + SerialSender::SerialSend(SERIAL_INFO, "%d;", order_id); + break; + } + case KILLG: + FifoNextGoal(); + ControlPrepareNewGoal(); + SerialSender::SerialSend(SERIAL_INFO, "%d;", order_id); + break; + case CLEANG: + FifoClearGoals(); + ControlPrepareNewGoal(); + SerialSender::SerialSend(SERIAL_INFO, "%d;", order_id); + break; + case RESET_ID: + control.last_finished_id = 0; + SerialSender::SerialSend(SERIAL_INFO, "%d;", order_id); + break; + case SET_POS: + { + int x, y, a_int; + float angle; + sscanf(receivedOrderPtr, "%i;%i;%i;", &x, &y, &a_int); + angle = a_int / (float)FLOAT_PRECISION; + RobotStateSetPos(x, y, angle); + SerialSender::SerialSend(SERIAL_INFO, "%d;%i;", order_id, a_int); + break; + } + case GET_POS: + { + int x, y, a_int; + float a; + a = current_pos.angle; + x = round(current_pos.x); + y = round(current_pos.y); + a_int = a * (float)FLOAT_PRECISION; + SerialSender::SerialSend(SERIAL_INFO, "%d;%d;%d;%d;", order_id, x, y, a_int); + break; + } + case GET_SPD: + { + int l, r; + l = wheels_spd.left; + r = wheels_spd.right; + SerialSender::SerialSend(SERIAL_INFO, "%d;%d;%d;", order_id, l, r); + break; + } + case GET_TARGET_SPD: + { + int left_spd, right_spd; + left_spd = control.speeds.linear_speed - control.speeds.angular_speed; + right_spd = control.speeds.linear_speed + control.speeds.angular_speed; + SerialSender::SerialSend(SERIAL_INFO, "%d;%d;%d;", order_id, left_spd, right_spd); + break; + } + case GET_POS_ID: + { + int x, y, a_int; + float a; + a = current_pos.angle; + x = round(current_pos.x); + y = round(current_pos.y); + a_int = a * (float)FLOAT_PRECISION; + SerialSender::SerialSend(SERIAL_INFO, "%d;%d;%d;%d;", order_id, x, y, a_int, control.last_finished_id); + break; + } + case SPDMAX: + { + int r_int, s; + float r; + sscanf(receivedOrderPtr, "%i;%i", &s, &r_int); + r = r_int / (float)FLOAT_PRECISION; + control.max_spd = s; + control.rot_spd_ratio = r; + SerialSender::SerialSend(SERIAL_INFO, "%d;", order_id); + break; + } + case ACCMAX: + { + int a; + sscanf(receivedOrderPtr, "%i", &a); + control.max_acc = a; + SerialSender::SerialSend(SERIAL_INFO, "%d;", order_id); + break; + } + case GET_LAST_ID: + SerialSender::SerialSend(SERIAL_INFO, "%d", control.last_finished_id); + break; + case PAUSE: + ControlSetStop(PAUSE_BIT); + SerialSender::SerialSend(SERIAL_INFO, "%d;", order_id); + break; + case RESUME: + ControlUnsetStop(PAUSE_BIT); + SerialSender::SerialSend(SERIAL_INFO, "%d;", order_id); + break; + case WHOAMI: + SerialSender::SerialSend(SERIAL_INFO, "%d;%s;", order_id, ARDUINO_ID); + break; + case SETEMERGENCYSTOP: + { + int enable; + sscanf(receivedOrderPtr, "%i", &enable); + if (enable == 0) { + emergencyStop(0); + } else { + emergencyStop(1); + } + SerialSender::SerialSend(SERIAL_INFO, "%d;", order_id); + break; + } + default: + SerialSender::SerialSend(SERIAL_INFO, "Order %c is wrong !", orderChar); + } +} + + diff --git a/arduino/common/asserv/protocol.h b/arduino/common/asserv/protocol.h new file mode 100644 index 0000000..c5d4822 --- /dev/null +++ b/arduino/common/asserv/protocol.h @@ -0,0 +1,90 @@ +// +// Created by tfuhrman on 09/05/17. +// + +#ifndef ASSERV_PROTOCOL_H +#define ASSERV_PROTOCOL_H + +#include "parameters.h" +#include + + +class String; + +// COMMANDS : +// 'ordre;id;arg1;arg2;argn' +// For example : +// 'c;3;120;1789;31400' +// 'j;0;' +// issues the order GOTOA with +// ID 3 to X=120, Y=1789 and angle = 3.14 +// +// WARNING : order should be +// ONE ascii char long +// +// float are transmitted as integer +// therefore any number refered as +// "decimal" is actually an int +// multiplied by FLOAT_PRECISION + +// BEGIN_ORDERS - Do not remove this comment +#define START 'S' //no args, start the program +#define HALT 'H' //no args, halt the program +#define GOTOA 'c' // x(int);y(int);a(decimal);direction(int) - (mm and radian), direction is optionnal : 1 is forward, -1 is backward, 0 is any +#define GOTO 'd' // x(int);y(int);direction(int) - (mm), direction is optionnal : 1 is forward, -1 is backward, 0 is any +#define ROT 'e' // a(decimal) - (radian), can't turn more than 1 turn +#define ROTNOMODULO 'a' // a(decimal) - radian, can turn more than 1 turn +#define KILLG 'f' // no args, go to next order +#define CLEANG 'g' // no args, cancel all orders +#define PIDLEFT 'p' // p(decimal);i(decimal);d(decimal) - set left PID +#define PIDRIGHT 'i' // p(decimal);i(decimal);d(decimal) - set right PID +#define PIDALL 'u' // p(decimal);i(decimal);d(decimal) - set both PID +#define GET_CODER 'j' // no args, response : l(long);r(long) +#define PWM 'k' // l(int);r(int);duration(int) - set left and right pwm for duration ms +#define SPD 'b' // l(int);a(int);duration(int) - set linear and angular spd for duration ms +#define ACCMAX 'l' // a(int) - set max acceleration (mm/s-2) +#define SPDMAX 'x' // v(int),r(decimal) - set max spd (mm/s) and rotation ratio +#define SET_POS 'm' // x(int);y(int);a(decimal) - set pos (mm / radians) +#define GET_POS 'n' // no args, response : x(int);y(int);a(decimal) - get current pos (mm and radians) +#define GET_SPD 'y' // no args, respond : l(int);r(int) - get wheels speed (mm/s) +#define GET_TARGET_SPD 'v' // no args, respond : l(int);r(int) - get target wheels speed (mm/s) +#define GET_POS_ID 'o' // no args, response : x(int);y(int);a(decimal);id(int) - get current pos and last id (mm and radians) +#define GET_LAST_ID 't' // no args, response : id(int) +#define PAUSE 'q' // no args, pauses control +#define RESUME 'r' // no args, resumes control +#define RESET_ID 's' // no args, reset last finished id to 0 +#define PINGPING 'z' // no args, switch led state +#define WHOAMI 'w' // no args, answers 'ASSERV' or 'PAP' +#define SETEMERGENCYSTOP 'A' // enable(int) +// END_ORDERS - Do not remove this comment + +#define AUTO_SEND '~' // x(int);y(int);a(decimal) +#define JACK 'J' + +#define JACK_SEND_NR 5 +#define FLOAT_PRECISION 1000.0 +#define FAILED_MSG "FAILED" +#define MAX_COMMAND_LEN 60 +#define MAX_ID_VAL 32767 +#define MAX_ID_LEN 5 +#define ID_START_INDEX 2 +#define MAX_RESPONSE_LEN 50 + +#define MAX_BYTES_PER_IT (0.9*BAUDRATE/(HZ*10)) + +#define ORDER_INDEX (uint8_t)0 +#define ID_INDEX (uint8_t)2 + +#ifdef DEBUG_TARGET_SPEED +#define MAX_AUTOSEND_SIZE (48) +#else +#define MAX_AUTOSEND_SIZE (24) +#endif + +extern unsigned char flagArduinoConnected; +void parseAndExecuteOrder(const String& order); +uint8_t getLog10(const uint16_t number); +void ProtocolAutoSendStatus(); +void emergencyStop(const uint8_t enable); + +#endif //ASSERV_PROTOCOL_H diff --git a/arduino/common/asserv/robotstate.c b/arduino/common/asserv/robotstate.c new file mode 100644 index 0000000..b22029a --- /dev/null +++ b/arduino/common/asserv/robotstate.c @@ -0,0 +1,85 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 13/10/13 * + ****************************************/ +#include "robotstate.h" +#include "compat.h" +#include "local_math.h" +#include "encoder.h" +#include + +#define FC (10) +#define RC (1/(2*PI*FC)) +#define ALPHA (DT / (RC + DT)) + +pos_t current_pos; +wheels_spd_t wheels_spd; + +void PosUpdateAngle() { + if (current_pos.angle > M_PI) { + current_pos.angle -= 2.0*M_PI; + current_pos.modulo_angle++; + } + else if (current_pos.angle <= -M_PI) { + current_pos.angle += 2.0*M_PI; + current_pos.modulo_angle--; + } +} + +void RobotStateInit() { + current_pos.x = 0; + current_pos.y = 0; + current_pos.angle = 0; + current_pos.modulo_angle = 0; + wheels_spd.left = 0; + wheels_spd.right = 0; + encoders_reset(); +} + +void RobotStateSetPos(float x, float y, float angle) { + current_pos.x = x; + current_pos.y = y; + current_pos.angle = angle; + PosUpdateAngle(); +} + +void lowPass(wheels_spd_t *old_spd, wheels_spd_t *new_spd, float a) { + new_spd->left = old_spd->left + a * (new_spd->left - old_spd->left); + new_spd->right = old_spd->right + a * (new_spd->right - old_spd->right); +} + +void RobotStateUpdate() { + static long left_last_ticks = 0, right_last_ticks = 0; + static float last_angle = 0; + float dd, dl, dr, d_angle; + long lt, rt; + wheels_spd_t old_wheels_spd = wheels_spd; + + lt = left_ticks; + rt = right_ticks; + + dl = (lt - left_last_ticks)*TICKS_TO_MM_LEFT; + dr = (rt - right_last_ticks)*TICKS_TO_MM_RIGHT; + wheels_spd.left = dl * HZ; + wheels_spd.right = dr * HZ; + + // low pass filter on speed + lowPass(&old_wheels_spd, &wheels_spd, ALPHA); + + //d_angle = atan2((dr - dl), ENTRAXE_ENC); //sans approximation tan + d_angle = (dr - dl)/ENTRAXE_ENC; // approximation tan + current_pos.angle += d_angle; +#if MODULO_TWOPI + PosUpdateAngle(); +#endif + + dd = (dr + dl) / 2.0; + current_pos.x += dd*cos((current_pos.angle + last_angle)/2.0); + current_pos.y += dd*sin((current_pos.angle + last_angle)/2.0); + + // prepare la prochaine update + right_last_ticks = rt; + left_last_ticks = lt; + last_angle = current_pos.angle; +} diff --git a/arduino/common/asserv/robotstate.h b/arduino/common/asserv/robotstate.h new file mode 100644 index 0000000..9883d6b --- /dev/null +++ b/arduino/common/asserv/robotstate.h @@ -0,0 +1,44 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 13/10/13 * + ****************************************/ +#ifndef ROBOTSTATE_H +#define ROBOTSTATE_H + +#include "parameters.h" +#include "encoder.h" +#include + +#if ENCODER_EVAL == 4 + #define TICKS_PER_TURN (ENC_RESOLUTION * 4) +#elif ENCODER_EVAL == 2 + #define TICKS_PER_TURN (ENC_RESOLUTION * 2) +#elif ENCODER_EVAL == 1 + #define TICKS_PER_TURN ENC_RESOLUTION +#endif +#define TICKS_TO_MM_LEFT ((float)((2.0*M_PI*ENC_LEFT_RADIUS)/(TICKS_PER_TURN)))// = mm/ticks +#define MM_TO_TICKS_LEFT ((float)(1/ENC_TICKS_TO_MM_LEFT)) +#define TICKS_TO_MM_RIGHT ((float)((2.0*M_PI*ENC_RIGHT_RADIUS)/(TICKS_PER_TURN)))// = mm/ticks +#define MM_TO_TICKS_RIGHT ((float)(1/ENC_TICKS_TO_MM_RIGHT)) + +typedef struct pos { + float x; + float y; + float angle; + int modulo_angle; +} pos_t; + +typedef struct wheels_spd { + float left, right; +} wheels_spd_t; + +extern pos_t current_pos; +extern wheels_spd_t wheels_spd; + +void RobotStateInit(); +void RobotStateUpdate(); +void RobotStateSetPos(float x, float y, float angle); +extern inline void RobotStateReset(void) { RobotStateInit(); }; + +#endif diff --git a/arduino/common/hmi/hmi.ino b/arduino/common/hmi/hmi.ino new file mode 100644 index 0000000..cfd4e7e --- /dev/null +++ b/arduino/common/hmi/hmi.ino @@ -0,0 +1,407 @@ +/* +Using the Arduino IDE, install the library "ESP8266 SSD1306 OLED" Display lib. +This code was developed with a Wemos D1 Mini (ESP8266) and a 128x64 i2c OLED. +Author: Pierre LACLAU, December 2017, UTCoupe. +*/ + +#include "pins.h" + +// Including rosserial +#include +#include // ROS sets strategy names for displaying on hmi. +#include // ROS sets teams names for displaying on hmi. +#include // HMI sends events : JACK, GAME_STOP +#include // ROS sends events : ASK_JACK, GAME_STOP +#include // ROS sends game and init status (to determine when RPi ready). +#include // ROS sends the timer status (for the ingame progress bar). +#include // ROS sends the timer status (for the ingame progress bar). +ros::NodeHandle nh; + +//Creating OLED display instances +#include // Only needed for Arduino 1.6.5 and earlier +#include "SSD1306.h" // alias for `#include "SSD1306Wire.h"` +#include "OLEDDisplayUi.h" +SSD1306 display(0x3c, LCD_SDA, LCD_SCK); +OLEDDisplayUi ui(&display); + +//Component variables +uint8_t current_frame; +uint8_t cursor_pos; + +const uint8_t activeSymbol[] PROGMEM = { + B00000000, + B00000000, + B00011000, + B00100100, + B01000010, + B01000010, + B00100100, + B00011000 +}; + +const uint8_t inactiveSymbol[] PROGMEM = { + B00000000, + B00000000, + B00000000, + B00000000, + B00011000, + B00011000, + B00000000, + B00000000 +}; + +//HMI variables +String strats[12]; +uint8_t strats_count = 0; + +String teams[12]; +uint8_t teams_count = 0; + +int chosen_strat = -1; +int chosen_team = -1; +int current_score = 0; + +int game_status = -1; +int init_status = -1; +float game_duration = -1; +float elapsed_time = -1; + +bool _is_arming = false; +bool _is_launching = false; + +//Input +#define PIN_BTN_VCC_1 BTN_L2 +#define PIN_BTN_VCC_2 BTN_L1 +#define PIN_BTN_IN_1 BTN_R1 +#define PIN_BTN_IN_2 BTN_R2 +#define PIN_JACK JACK + +bool _prev_up_state = false; +bool _prev_down_state = false; +bool _prev_left_state = false; +bool _prev_right_state = false; +bool _prev_jack_state = false; + +bool up_pressed = false; +bool down_pressed = false; +bool left_pressed = false; +bool right_pressed = false; +bool jack_pulled = false; + +void check_input() { + digitalWrite(PIN_BTN_VCC_1, HIGH); + digitalWrite(PIN_BTN_VCC_2, LOW); + delay(2); + up_pressed = digitalRead(PIN_BTN_IN_1) && !_prev_up_state; + _prev_up_state = digitalRead(PIN_BTN_IN_1); + down_pressed = digitalRead(PIN_BTN_IN_2) && !_prev_down_state; + _prev_down_state = digitalRead(PIN_BTN_IN_2); + +// if (digitalRead(PIN_JACK) == 0) { +// nh.loginfo("bas"); +// } else if (digitalRead(PIN_JACK) == 1) { +// nh.loginfo("haut"); +// } + + jack_pulled = digitalRead(PIN_JACK) && !_prev_jack_state; // 1 when inserted + _prev_jack_state = digitalRead(PIN_JACK); + + digitalWrite(PIN_BTN_VCC_1, LOW); + digitalWrite(PIN_BTN_VCC_2, HIGH); + delay(2); + left_pressed = digitalRead(PIN_BTN_IN_1) && !_prev_left_state; + _prev_left_state = digitalRead(PIN_BTN_IN_1); + right_pressed = digitalRead(PIN_BTN_IN_2) && !_prev_right_state; + _prev_right_state = digitalRead(PIN_BTN_IN_2); +} + +//LEDs +#define PIN_LED_ALIVE LED_GREEN +#define PIN_LED_INIT LED_RED + +void update_leds() { + digitalWrite(PIN_LED_ALIVE, game_status != -1); + + if(init_status != -1) { + if(init_status == 0) // some nodes have not responded yet + analogWrite(PIN_LED_INIT, 127 * (1 + cos(float(millis()) / 200.0))); // wave + else if(init_status == 1) // init finished and all nodes are ready + analogWrite(PIN_LED_INIT, 255); //solid ON + else if(init_status == 2) // init finished but at least one node failed + analogWrite(PIN_LED_INIT, 255 * (millis() % 1000 < 700)); // blink + } + else digitalWrite(PIN_LED_INIT, LOW); +} + + +//ROS methods +void on_set_strategies(const drivers_ard_hmi::SetStrategies& msg){ + strats_count = msg.strategies_names_length; + for(int i=0; i < msg.strategies_names_length; i++) { + strats[i] = msg.strategies_names[i]; + } +} + +void on_set_teams(const drivers_ard_hmi::SetTeams& msg){ + teams_count = msg.teams_names_length; + for(int i=0; i < msg.teams_names_length; i++) { + teams[i] = msg.teams_names[i]; + } +} + +void on_game_status(const ai_game_manager::GameStatus& msg){ + game_status = msg.game_status; + init_status = msg.init_status; + + if(game_status == 1 && current_frame != 5 /*INGAME*/) + ui.switchToFrame(5); +} + +void on_game_timer(const ai_game_manager::GameTime& msg){ + if(msg.is_active) { + game_duration = msg.game_time_duration; + elapsed_time = msg.game_elapsed_time; + } +} + +void on_score(const ai_scheduler::AIScore& msg){ + current_score = msg.score; +} + +void on_ros_event(const drivers_ard_hmi::ROSEvent& msg){ + if(msg.event == 0) //asked to respond for JACK + ui.switchToFrame(4); +} + +ros::Subscriber sub_strats ("/feedback/ard_hmi/set_strategies", &on_set_strategies); +ros::Subscriber sub_teams ("/feedback/ard_hmi/set_teams", &on_set_teams); +ros::Subscriber sub_ros_events ("/feedback/ard_hmi/ros_event", &on_ros_event); +ros::Subscriber sub_game_status("/ai/game_manager/status", &on_game_status); +ros::Subscriber sub_game_timer ("/ai/game_manager/time", &on_game_timer); +ros::Subscriber sub_score ("/ai/scheduler/score", &on_score); + +drivers_ard_hmi::HMIEvent hmi_event_msg; +ros::Publisher hmi_event_pub("/feedback/ard_hmi/hmi_event", &hmi_event_msg); + + + +void drawFrameTitleComponent(OLEDDisplay *display, OLEDDisplayUiState* state, int16_t x, int16_t y, String text) { + display->setFont(ArialMT_Plain_16); + display->setTextAlignment(TEXT_ALIGN_CENTER); + display->drawString(64 + x, 0 + y, text); +} + +void drawBigCentralMessageComponent(OLEDDisplay *display, OLEDDisplayUiState* state, int16_t x, int16_t y, String text) { + display->setFont(ArialMT_Plain_24); + display->setTextAlignment(TEXT_ALIGN_CENTER); + display->drawString(64 + x, 22 + y, text); +} + +void drawSmallCentralMessageComponent(OLEDDisplay *display, OLEDDisplayUiState* state, int16_t x, int16_t y, String text) { + display->setFont(ArialMT_Plain_10); + display->setTextAlignment(TEXT_ALIGN_CENTER); + display->drawString(64 + x, 28 + y, text); +} + +void drawMCQComponent(OLEDDisplay *display, OLEDDisplayUiState* state, int16_t x, int16_t y, String options[], uint8_t options_count) { + if(options_count > 0) { + int y_offset = 12; + display->setFont(ArialMT_Plain_10); + display->setTextAlignment(TEXT_ALIGN_LEFT); + + int start = cursor_pos - 1; + if(cursor_pos == 0) start = 0; + if(cursor_pos == options_count - 1) { + start = options_count - 3; + if(options_count == 2) start = options_count - 2; + if(options_count == 1) start = options_count - 1; + } + + for(int i = 0; i < 3; i++) { + display->drawString(11 + x, 16 + y + y_offset * i, options[start + i]); + if(cursor_pos == start + i) { + display->drawString(4 + x, 16 + y + y_offset * i, ">"); + } + } + + char b[5]; + sprintf(b, "%d/%d",cursor_pos + 1,options_count); + display->drawString(4 + x, 52 + y, b); + } + else { + drawSmallCentralMessageComponent(display, state, x, y, "No options."); + } +} + +//Frames rendering methods +void drawHelloFrame(OLEDDisplay *display, OLEDDisplayUiState* state, int16_t x, int16_t y) { + current_frame = 0; + drawFrameTitleComponent(display, state, x, y, "RPi init..."); + if(init_status == -1) drawBigCentralMessageComponent(display, state, x, y, "WAIT"); + else drawBigCentralMessageComponent(display, state, x, y, "READY"); + + if(game_status == 0 && init_status != 0 && strats_count && teams_count) + ui.nextFrame(); + if(right_pressed && game_status != -1) // go to next screen if rpi is alive only. + ui.nextFrame(); +} + +void drawTeamFrame(OLEDDisplay *display, OLEDDisplayUiState* state, int16_t x, int16_t y) { + current_frame = 1; + drawFrameTitleComponent(display, state, x, y, "Team select"); + drawMCQComponent(display, state, x, y, teams, teams_count); + + if(right_pressed) { + chosen_team = cursor_pos; + cursor_pos = 0; + ui.nextFrame(); + } + if(left_pressed) { + cursor_pos = 0; + ui.previousFrame(); + } + if(down_pressed && teams_count) { + cursor_pos += 1; + if(cursor_pos >= teams_count) cursor_pos = teams_count - 1; + } + if(up_pressed && teams_count) { + if(cursor_pos > 0) cursor_pos -= 1; + } +} + +void drawStrategyFrame(OLEDDisplay *display, OLEDDisplayUiState* state, int16_t x, int16_t y) { + current_frame = 2; + drawFrameTitleComponent(display, state, x, y, "Strat select"); + drawMCQComponent(display, state, x, y, strats, strats_count); + + if(right_pressed) { + chosen_strat = cursor_pos; + cursor_pos = 0; + ui.nextFrame(); + } + if(left_pressed) { + cursor_pos = 0; + ui.previousFrame(); + } + if(down_pressed && strats_count) { + cursor_pos += 1; + if(cursor_pos >= strats_count) cursor_pos = strats_count - 1; + } + if(up_pressed && strats_count) { + if(cursor_pos > 0) cursor_pos -= 1; + } +} + +void drawArmFrame(OLEDDisplay *display, OLEDDisplayUiState* state, int16_t x, int16_t y) { + current_frame = 3; + drawFrameTitleComponent(display, state, x, y, "Calibration"); + if(!_is_arming) drawBigCentralMessageComponent(display, state, x, y, "ARM?"); + else drawBigCentralMessageComponent(display, state, x, y, "ARMING..."); + + if(left_pressed) { + _is_arming = false; //todo good ? + ui.previousFrame(); + } + if(right_pressed) { + if(!_is_arming) { + hmi_event_msg.event = hmi_event_msg.EVENT_START; // Will start ai/scheduler with selected config. + hmi_event_msg.chosen_team_id = chosen_team; + hmi_event_msg.chosen_strategy_id = chosen_strat; + hmi_event_pub.publish(&hmi_event_msg); // send config. + _is_arming = true; + } + } +} + +void drawJackFrame(OLEDDisplay *display, OLEDDisplayUiState* state, int16_t x, int16_t y) { + current_frame = 4; + drawFrameTitleComponent(display, state, x, y, "Jack"); + if(!_is_launching) { + if(_prev_jack_state) drawBigCentralMessageComponent(display, state, x, y, "JACK?"); + else drawSmallCentralMessageComponent(display, state, x, y, "Insert jack."); + } + else drawBigCentralMessageComponent(display, state, x, y, "JACKED!"); + + if(left_pressed) { + _is_launching = false; //TODO good ? + ui.previousFrame(); + } + if(jack_pulled) { + if(!_is_launching) { + hmi_event_msg.event = hmi_event_msg.EVENT_JACK_FIRED; //JACKED + hmi_event_msg.chosen_strategy_id = 0; // resetting values + hmi_event_msg.chosen_team_id = 0; // from previous send. + hmi_event_pub.publish(&hmi_event_msg); + _is_launching = true; + } + } +} + +void drawInGameFrame(OLEDDisplay *display, OLEDDisplayUiState* state, int16_t x, int16_t y) { + current_frame = 5; + ui.disableIndicator(); + drawFrameTitleComponent(display, state, x, y, "In Game..."); + + display->setTextAlignment(TEXT_ALIGN_CENTER); + display->setFont(ArialMT_Plain_24); + String text = String(current_score) + "pts"; + display->drawString(64 + x, 22 + y, text); + + int percentage = 0; + if(elapsed_time != -1 && game_duration > 0) percentage = int(100 * elapsed_time / game_duration); + display->drawProgressBar(x + 0, y + 54, 120, 8, percentage); // shows time left + + if(left_pressed) ui.previousFrame(); + if(right_pressed) ui.nextFrame(); +} + +// frames are the single views that slide in +FrameCallback frames[] = {drawHelloFrame, drawTeamFrame, drawStrategyFrame, drawArmFrame, drawJackFrame, drawInGameFrame}; +int frameCount = 6; + +void setup() { + pinMode(PIN_BTN_VCC_1, OUTPUT); // init input buttons + pinMode(PIN_BTN_VCC_2, OUTPUT); + pinMode(PIN_BTN_IN_1, INPUT); + pinMode(PIN_BTN_IN_2, INPUT); + pinMode(PIN_JACK, INPUT); + + pinMode(PIN_LED_ALIVE, OUTPUT); + pinMode(PIN_LED_INIT, OUTPUT); + + nh.initNode(); + nh.subscribe(sub_strats); + nh.subscribe(sub_teams); + nh.subscribe(sub_game_status); + nh.subscribe(sub_game_timer); + nh.subscribe(sub_score); + nh.subscribe(sub_ros_events); + nh.advertise(hmi_event_pub); + + ui.setTargetFPS(30); + + ui.setActiveSymbol(activeSymbol); + ui.setInactiveSymbol(inactiveSymbol); + ui.setFrameAnimation(SLIDE_LEFT); + ui.setFrames(frames, frameCount); + + ui.disableAutoTransition(); + ui.init(); + + display.flipScreenVertically(); +} + + +void loop() { + check_input(); + update_leds(); + ui.update(); + + if(game_status == -1) { + hmi_event_msg.event = hmi_event_msg.EVENT_HMI_INITIALIZED; // tell ros that hmi is alive until it responds back + hmi_event_pub.publish(&hmi_event_msg); + } + + nh.spinOnce(); + delay(30); // Needed for input to work properly, no idea why... +} diff --git a/arduino/common/hmi/pins.h b/arduino/common/hmi/pins.h new file mode 100644 index 0000000..94d7d73 --- /dev/null +++ b/arduino/common/hmi/pins.h @@ -0,0 +1,25 @@ +// +// Created by tfuhrman on 22/04/18. +// + +#ifndef ARDUINO_PINS_H +#define ARDUINO_PINS_H + +// LCD pins +#define LCD_SDA D2 //can not use another one +#define LCD_SCK D1 //can not use another one + +// Buttons matrix pins +#define BTN_L1 D6 +#define BTN_L2 D7 +#define BTN_R1 D5 +#define BTN_R2 D0 + +// LED pins +#define LED_RED D3 +//#define LED_BLUE D4 +#define LED_GREEN D8 + +#define JACK D4 + +#endif //ARDUINO_PINS_H diff --git a/arduino/common/others/AFMotor.cpp b/arduino/common/others/AFMotor.cpp new file mode 100644 index 0000000..80cb0ba --- /dev/null +++ b/arduino/common/others/AFMotor.cpp @@ -0,0 +1,724 @@ +// Adafruit Motor shield library +// copyright Adafruit Industries LLC, 2009 +// this code is public domain, enjoy! + + +#if (ARDUINO >= 100) + #include "Arduino.h" +#else + #if defined(__AVR__) + #include + #endif + #include "WProgram.h" +#endif + +#include "AFMotor.h" + + +static uint8_t latch_state; + +#if (MICROSTEPS == 8) +uint8_t microstepcurve[] = {0, 50, 98, 142, 180, 212, 236, 250, 255}; +#elif (MICROSTEPS == 16) +uint8_t microstepcurve[] = {0, 25, 50, 74, 98, 120, 141, 162, 180, 197, 212, 225, 236, 244, 250, 253, 255}; +#endif + +AFMotorController::AFMotorController(void) { + TimerInitalized = false; +} + +void AFMotorController::enable(void) { + // setup the latch + /* + LATCH_DDR |= _BV(LATCH); + ENABLE_DDR |= _BV(ENABLE); + CLK_DDR |= _BV(CLK); + SER_DDR |= _BV(SER); + */ + pinMode(MOTORLATCH, OUTPUT); + pinMode(MOTORENABLE, OUTPUT); + pinMode(MOTORDATA, OUTPUT); + pinMode(MOTORCLK, OUTPUT); + + latch_state = 0; + + latch_tx(); // "reset" + + //ENABLE_PORT &= ~_BV(ENABLE); // enable the chip outputs! + digitalWrite(MOTORENABLE, LOW); +} + + +void AFMotorController::latch_tx(void) { + uint8_t i; + + //LATCH_PORT &= ~_BV(LATCH); + digitalWrite(MOTORLATCH, LOW); + + //SER_PORT &= ~_BV(SER); + digitalWrite(MOTORDATA, LOW); + + for (i=0; i<8; i++) { + //CLK_PORT &= ~_BV(CLK); + digitalWrite(MOTORCLK, LOW); + + if (latch_state & _BV(7-i)) { + //SER_PORT |= _BV(SER); + digitalWrite(MOTORDATA, HIGH); + } else { + //SER_PORT &= ~_BV(SER); + digitalWrite(MOTORDATA, LOW); + } + //CLK_PORT |= _BV(CLK); + digitalWrite(MOTORCLK, HIGH); + } + //LATCH_PORT |= _BV(LATCH); + digitalWrite(MOTORLATCH, HIGH); +} + +static AFMotorController MC; + +/****************************************** + MOTORS +******************************************/ +inline void initPWM1(uint8_t freq) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer2A on PB3 (Arduino pin #11) + TCCR2A |= _BV(COM2A1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2a + TCCR2B = freq & 0x7; + OCR2A = 0; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 11 is now PB5 (OC1A) + TCCR1A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc1a + TCCR1B = (freq & 0x7) | _BV(WGM12); + OCR1A = 0; +#elif defined(__PIC32MX__) + #if defined(PIC32_USE_PIN9_FOR_M1_PWM) + // Make sure that pin 11 is an input, since we have tied together 9 and 11 + pinMode(9, OUTPUT); + pinMode(11, INPUT); + if (!MC.TimerInitalized) + { // Set up Timer2 for 80MHz counting fro 0 to 256 + T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 + TMR2 = 0x0000; + PR2 = 0x0100; + MC.TimerInitalized = true; + } + // Setup OC4 (pin 9) in PWM mode, with Timer2 as timebase + OC4CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 + OC4RS = 0x0000; + OC4R = 0x0000; + #elif defined(PIC32_USE_PIN10_FOR_M1_PWM) + // Make sure that pin 11 is an input, since we have tied together 9 and 11 + pinMode(10, OUTPUT); + pinMode(11, INPUT); + if (!MC.TimerInitalized) + { // Set up Timer2 for 80MHz counting fro 0 to 256 + T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 + TMR2 = 0x0000; + PR2 = 0x0100; + MC.TimerInitalized = true; + } + // Setup OC5 (pin 10) in PWM mode, with Timer2 as timebase + OC5CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 + OC5RS = 0x0000; + OC5R = 0x0000; + #else + // If we are not using PWM for pin 11, then just do digital + digitalWrite(11, LOW); + #endif +#elif defined(__AVR_ATmega32U4__) + pinMode(11, OUTPUT); + digitalWrite(11, LOW); +#else + #error "This chip is not supported!" +#endif + #if !defined(PIC32_USE_PIN9_FOR_M1_PWM) && !defined(PIC32_USE_PIN10_FOR_M1_PWM) + pinMode(11, OUTPUT); + #endif +} + +inline void setPWM1(uint8_t s) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer2A on PB3 (Arduino pin #11) + OCR2A = s; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 11 is now PB5 (OC1A) + OCR1A = s; +#elif defined(__PIC32MX__) + #if defined(PIC32_USE_PIN9_FOR_M1_PWM) + // Set the OC4 (pin 9) PMW duty cycle from 0 to 255 + OC4RS = s; + #elif defined(PIC32_USE_PIN10_FOR_M1_PWM) + // Set the OC5 (pin 10) PMW duty cycle from 0 to 255 + OC5RS = s; + #else + // If we are not doing PWM output for M1, then just use on/off + if (s > 127) + { + digitalWrite(11, HIGH); + } + else + { + digitalWrite(11, LOW); + } + #endif +#elif defined(__AVR_ATmega32U4__) + if (s > 127) + { + digitalWrite(11, HIGH); + } + else + { + digitalWrite(11, LOW); + } +#else + #error "This chip is not supported!" +#endif +} + +inline void initPWM2(uint8_t freq) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer2B (pin 3) + TCCR2A |= _BV(COM2B1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2b + TCCR2B = freq & 0x7; + OCR2B = 0; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 3 is now PE5 (OC3C) + TCCR3A |= _BV(COM1C1) | _BV(WGM10); // fast PWM, turn on oc3c + TCCR3B = (freq & 0x7) | _BV(WGM12); + OCR3C = 0; +#elif defined(__PIC32MX__) + if (!MC.TimerInitalized) + { // Set up Timer2 for 80MHz counting fro 0 to 256 + T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 + TMR2 = 0x0000; + PR2 = 0x0100; + MC.TimerInitalized = true; + } + // Setup OC1 (pin3) in PWM mode, with Timer2 as timebase + OC1CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 + OC1RS = 0x0000; + OC1R = 0x0000; +#elif defined(__AVR_ATmega32U4__) + pinMode(3, OUTPUT); + digitalWrite(3, LOW); +#else + #error "This chip is not supported!" +#endif + + pinMode(3, OUTPUT); +} + +inline void setPWM2(uint8_t s) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer2A on PB3 (Arduino pin #11) + OCR2B = s; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 11 is now PB5 (OC1A) + OCR3C = s; +#elif defined(__PIC32MX__) + // Set the OC1 (pin3) PMW duty cycle from 0 to 255 + OC1RS = s; +#elif defined(__AVR_ATmega32U4__) + if (s > 127) + { + digitalWrite(3, HIGH); + } + else + { + digitalWrite(3, LOW); + } +#else + #error "This chip is not supported!" +#endif +} + +inline void initPWM3(uint8_t freq) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer0A / PD6 (pin 6) + TCCR0A |= _BV(COM0A1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on OC0A + //TCCR0B = freq & 0x7; + OCR0A = 0; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 6 is now PH3 (OC4A) + TCCR4A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc4a + TCCR4B = (freq & 0x7) | _BV(WGM12); + //TCCR4B = 1 | _BV(WGM12); + OCR4A = 0; +#elif defined(__PIC32MX__) + if (!MC.TimerInitalized) + { // Set up Timer2 for 80MHz counting fro 0 to 256 + T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 + TMR2 = 0x0000; + PR2 = 0x0100; + MC.TimerInitalized = true; + } + // Setup OC3 (pin 6) in PWM mode, with Timer2 as timebase + OC3CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 + OC3RS = 0x0000; + OC3R = 0x0000; +#elif defined(__AVR_ATmega32U4__) + pinMode(6, OUTPUT); + digitalWrite(6, LOW); +#else + #error "This chip is not supported!" +#endif + pinMode(6, OUTPUT); +} + +inline void setPWM3(uint8_t s) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer0A on PB3 (Arduino pin #6) + OCR0A = s; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 6 is now PH3 (OC4A) + OCR4A = s; +#elif defined(__PIC32MX__) + // Set the OC3 (pin 6) PMW duty cycle from 0 to 255 + OC3RS = s; +#elif defined(__AVR_ATmega32U4__) + if (s > 127) + { + digitalWrite(6, HIGH); + } + else + { + digitalWrite(6, LOW); + } +#else + #error "This chip is not supported!" +#endif +} + + + +inline void initPWM4(uint8_t freq) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer0B / PD5 (pin 5) + TCCR0A |= _BV(COM0B1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on oc0a + //TCCR0B = freq & 0x7; + OCR0B = 0; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 5 is now PE3 (OC3A) + TCCR3A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc3a + TCCR3B = (freq & 0x7) | _BV(WGM12); + //TCCR4B = 1 | _BV(WGM12); + OCR3A = 0; +#elif defined(__PIC32MX__) + if (!MC.TimerInitalized) + { // Set up Timer2 for 80MHz counting fro 0 to 256 + T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 + TMR2 = 0x0000; + PR2 = 0x0100; + MC.TimerInitalized = true; + } + // Setup OC2 (pin 5) in PWM mode, with Timer2 as timebase + OC2CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 + OC2RS = 0x0000; + OC2R = 0x0000; +#elif defined(__AVR_ATmega32U4__) + pinMode(5, OUTPUT); + digitalWrite(5, LOW); +#else + #error "This chip is not supported!" +#endif + pinMode(5, OUTPUT); +} + +inline void setPWM4(uint8_t s) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer0A on PB3 (Arduino pin #6) + OCR0B = s; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 6 is now PH3 (OC4A) + OCR3A = s; +#elif defined(__PIC32MX__) + // Set the OC2 (pin 5) PMW duty cycle from 0 to 255 + OC2RS = s; +#elif defined(__AVR_ATmega32U4__) + if (s > 127) + { + digitalWrite(5, HIGH); + } + else + { + digitalWrite(5, LOW); + } +#else + #error "This chip is not supported!" +#endif +} + +AF_DCMotor::AF_DCMotor(uint8_t num, uint8_t freq) { + motornum = num; + pwmfreq = freq; + + MC.enable(); + + switch (num) { + case 1: + latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B); // set both motor pins to 0 + MC.latch_tx(); + initPWM1(freq); + break; + case 2: + latch_state &= ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // set both motor pins to 0 + MC.latch_tx(); + initPWM2(freq); + break; + case 3: + latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B); // set both motor pins to 0 + MC.latch_tx(); + initPWM3(freq); + break; + case 4: + latch_state &= ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // set both motor pins to 0 + MC.latch_tx(); + initPWM4(freq); + break; + } +} + +void AF_DCMotor::run(uint8_t cmd) { + uint8_t a, b; + switch (motornum) { + case 1: + a = MOTOR1_A; b = MOTOR1_B; break; + case 2: + a = MOTOR2_A; b = MOTOR2_B; break; + case 3: + a = MOTOR3_A; b = MOTOR3_B; break; + case 4: + a = MOTOR4_A; b = MOTOR4_B; break; + default: + return; + } + + switch (cmd) { + case FORWARD: + latch_state |= _BV(a); + latch_state &= ~_BV(b); + MC.latch_tx(); + break; + case BACKWARD: + latch_state &= ~_BV(a); + latch_state |= _BV(b); + MC.latch_tx(); + break; + case RELEASE: + latch_state &= ~_BV(a); // A and B both low + latch_state &= ~_BV(b); + MC.latch_tx(); + break; + } +} + +void AF_DCMotor::setSpeed(uint8_t speed) { + switch (motornum) { + case 1: + setPWM1(speed); break; + case 2: + setPWM2(speed); break; + case 3: + setPWM3(speed); break; + case 4: + setPWM4(speed); break; + } +} + +/****************************************** + STEPPERS +******************************************/ + +AF_Stepper::AF_Stepper() { + is_init = 0; +} + +AF_Stepper::AF_Stepper(uint16_t steps, uint8_t num) { + if (is_init == 0) + init(steps, num); +} + +void AF_Stepper::init(uint16_t steps, uint8_t num) { + MC.enable(); + + revsteps = steps; + steppernum = num; + currentstep = 0; + + if (steppernum == 1) { + latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) & + ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0 + MC.latch_tx(); + + // enable both H bridges + pinMode(11, OUTPUT); + pinMode(3, OUTPUT); + digitalWrite(11, HIGH); + digitalWrite(3, HIGH); + + // use PWM for microstepping support + initPWM1(STEPPER1_PWM_RATE); + initPWM2(STEPPER1_PWM_RATE); + setPWM1(255); + setPWM2(255); + + } else if (steppernum == 2) { + latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) & + ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0 + MC.latch_tx(); + + // enable both H bridges + pinMode(5, OUTPUT); + pinMode(6, OUTPUT); + digitalWrite(5, HIGH); + digitalWrite(6, HIGH); + + // use PWM for microstepping support + // use PWM for microstepping support + initPWM3(STEPPER2_PWM_RATE); + initPWM4(STEPPER2_PWM_RATE); + setPWM3(255); + setPWM4(255); + } + is_init = 1; +} + +void AF_Stepper::setSpeed(uint16_t rpm) { + usperstep = 60000000 / ((uint32_t)revsteps * (uint32_t)rpm); + steppingcounter = 0; +} + +void AF_Stepper::release(void) { + if (steppernum == 1) { + latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) & + ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0 + MC.latch_tx(); + } else if (steppernum == 2) { + latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) & + ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0 + MC.latch_tx(); + } +} + +void AF_Stepper::step(uint16_t steps, uint8_t dir, uint8_t style) { + uint32_t uspers = usperstep; + uint8_t ret = 0; + + if (style == INTERLEAVE) { + uspers /= 2; + } + else if (style == MICROSTEP) { + uspers /= MICROSTEPS; + steps *= MICROSTEPS; +#ifdef MOTORDEBUG + Serial.print("steps = "); Serial.println(steps, DEC); +#endif + } + + while (steps--) { + ret = onestep(dir, style); + delay(uspers/1000); // in ms + steppingcounter += (uspers % 1000); + if (steppingcounter >= 1000) { + delay(1); + steppingcounter -= 1000; + } + } + if (style == MICROSTEP) { + while ((ret != 0) && (ret != MICROSTEPS)) { + ret = onestep(dir, style); + delay(uspers/1000); // in ms + steppingcounter += (uspers % 1000); + if (steppingcounter >= 1000) { + delay(1); + steppingcounter -= 1000; + } + } + } +} + +uint8_t AF_Stepper::onestep(uint8_t dir, uint8_t style) { + uint8_t a, b, c, d; + uint8_t ocrb, ocra; + + ocra = ocrb = 255; + + if (steppernum == 1) { + a = _BV(MOTOR1_A); + b = _BV(MOTOR2_A); + c = _BV(MOTOR1_B); + d = _BV(MOTOR2_B); + } else if (steppernum == 2) { + a = _BV(MOTOR3_A); + b = _BV(MOTOR4_A); + c = _BV(MOTOR3_B); + d = _BV(MOTOR4_B); + } else { + return 0; + } + + // next determine what sort of stepping procedure we're up to + if (style == SINGLE) { + if ((currentstep/(MICROSTEPS/2)) % 2) { // we're at an odd step, weird + if (dir == FORWARD) { + currentstep += MICROSTEPS/2; + } + else { + currentstep -= MICROSTEPS/2; + } + } else { // go to the next even step + if (dir == FORWARD) { + currentstep += MICROSTEPS; + } + else { + currentstep -= MICROSTEPS; + } + } + } else if (style == DOUBLE) { + if (! (currentstep/(MICROSTEPS/2) % 2)) { // we're at an even step, weird + if (dir == FORWARD) { + currentstep += MICROSTEPS/2; + } else { + currentstep -= MICROSTEPS/2; + } + } else { // go to the next odd step + if (dir == FORWARD) { + currentstep += MICROSTEPS; + } else { + currentstep -= MICROSTEPS; + } + } + } else if (style == INTERLEAVE) { + if (dir == FORWARD) { + currentstep += MICROSTEPS/2; + } else { + currentstep -= MICROSTEPS/2; + } + } + + if (style == MICROSTEP) { + if (dir == FORWARD) { + currentstep++; + } else { + // BACKWARDS + currentstep--; + } + + currentstep += MICROSTEPS*4; + currentstep %= MICROSTEPS*4; + + ocra = ocrb = 0; + if ( (currentstep >= 0) && (currentstep < MICROSTEPS)) { + ocra = microstepcurve[MICROSTEPS - currentstep]; + ocrb = microstepcurve[currentstep]; + } else if ( (currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2)) { + ocra = microstepcurve[currentstep - MICROSTEPS]; + ocrb = microstepcurve[MICROSTEPS*2 - currentstep]; + } else if ( (currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3)) { + ocra = microstepcurve[MICROSTEPS*3 - currentstep]; + ocrb = microstepcurve[currentstep - MICROSTEPS*2]; + } else if ( (currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4)) { + ocra = microstepcurve[currentstep - MICROSTEPS*3]; + ocrb = microstepcurve[MICROSTEPS*4 - currentstep]; + } + } + + currentstep += MICROSTEPS*4; + currentstep %= MICROSTEPS*4; + +#ifdef MOTORDEBUG + Serial.print("current step: "); Serial.println(currentstep, DEC); + Serial.print(" pwmA = "); Serial.print(ocra, DEC); + Serial.print(" pwmB = "); Serial.println(ocrb, DEC); +#endif + + if (steppernum == 1) { + setPWM1(ocra); + setPWM2(ocrb); + } else if (steppernum == 2) { + setPWM3(ocra); + setPWM4(ocrb); + } + + + // release all + latch_state &= ~a & ~b & ~c & ~d; // all motor pins to 0 + + //Serial.println(step, DEC); + if (style == MICROSTEP) { + if ((currentstep >= 0) && (currentstep < MICROSTEPS)) + latch_state |= a | b; + if ((currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2)) + latch_state |= b | c; + if ((currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3)) + latch_state |= c | d; + if ((currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4)) + latch_state |= d | a; + } else { + switch (currentstep/(MICROSTEPS/2)) { + case 0: + latch_state |= a; // energize coil 1 only + break; + case 1: + latch_state |= a | b; // energize coil 1+2 + break; + case 2: + latch_state |= b; // energize coil 2 only + break; + case 3: + latch_state |= b | c; // energize coil 2+3 + break; + case 4: + latch_state |= c; // energize coil 3 only + break; + case 5: + latch_state |= c | d; // energize coil 3+4 + break; + case 6: + latch_state |= d; // energize coil 4 only + break; + case 7: + latch_state |= d | a; // energize coil 1+4 + break; + } + } + + + MC.latch_tx(); + return currentstep; +} + diff --git a/arduino/common/others/AFMotor.h b/arduino/common/others/AFMotor.h new file mode 100644 index 0000000..92392fd --- /dev/null +++ b/arduino/common/others/AFMotor.h @@ -0,0 +1,207 @@ +// Adafruit Motor shield library +// copyright Adafruit Industries LLC, 2009 +// this code is public domain, enjoy! + +/* + * Usage Notes: + * For PIC32, all features work properly with the following two exceptions: + * + * 1) Because the PIC32 only has 5 PWM outputs, and the AFMotor shield needs 6 + * to completely operate (four for motor outputs and two for RC servos), the + * M1 motor output will not have PWM ability when used with a PIC32 board. + * However, there is a very simple workaround. If you need to drive a stepper + * or DC motor with PWM on motor output M1, you can use the PWM output on pin + * 9 or pin 10 (normally use for RC servo outputs on Arduino, not needed for + * RC servo outputs on PIC32) to drive the PWM input for M1 by simply putting + * a jumber from pin 9 to pin 11 or pin 10 to pin 11. Then uncomment one of the + * two #defines below to activate the PWM on either pin 9 or pin 10. You will + * then have a fully functional microstepping for 2 stepper motors, or four + * DC motor outputs with PWM. + * + * 2) There is a conflict between RC Servo outputs on pins 9 and pins 10 and + * the operation of DC motors and stepper motors as of 9/2012. This issue + * will get fixed in future MPIDE releases, but at the present time it means + * that the Motor Party example will NOT work properly. Any time you attach + * an RC servo to pins 9 or pins 10, ALL PWM outputs on the whole board will + * stop working. Thus no steppers or DC motors. + * + */ +// 09/15/2012 Modified for use with chipKIT boards + + +#ifndef _AFMotor_h_ +#define _AFMotor_h_ + +#include +#if defined (__AVR_ATmega32U4__) + #define MICROSTEPS 16 // 8 or 16 + + #define MOTOR12_64KHZ 0 + #define MOTOR12_39KHZ 0 + #define MOTOR12_19KHZ 0 + #define MOTOR12_8KHZ 0 + #define MOTOR12_4_8KHZ 0 + #define MOTOR12_2KHZ 0 + #define MOTOR12_1KHZ 0 + + #define MOTOR34_64KHZ 0 + #define MOTOR34_39KHZ 0 + #define MOTOR34_19KHZ 0 + #define MOTOR34_8KHZ 0 + #define MOTOR34_4_8KHZ 0 + #define MOTOR34_2KHZ 0 + #define MOTOR34_1KHZ 0 + + #define DC_MOTOR_PWM_RATE MOTOR34_8KHZ // PWM rate for DC motors + #define STEPPER1_PWM_RATE MOTOR12_64KHZ // PWM rate for stepper 1 + #define STEPPER2_PWM_RATE MOTOR34_64KHZ // PWM rate for stepper 2 +#elif defined(__AVR__) + #include + + //#define MOTORDEBUG 1 + + #define MICROSTEPS 16 // 8 or 16 + + #define MOTOR12_64KHZ _BV(CS20) // no prescale + #define MOTOR12_8KHZ _BV(CS21) // divide by 8 + #define MOTOR12_2KHZ _BV(CS21) | _BV(CS20) // divide by 32 + #define MOTOR12_1KHZ _BV(CS22) // divide by 64 + + #define MOTOR34_64KHZ _BV(CS00) // no prescale + #define MOTOR34_8KHZ _BV(CS01) // divide by 8 + #define MOTOR34_1KHZ _BV(CS01) | _BV(CS00) // divide by 64 + + #define DC_MOTOR_PWM_RATE MOTOR34_8KHZ // PWM rate for DC motors + #define STEPPER1_PWM_RATE MOTOR12_64KHZ // PWM rate for stepper 1 + #define STEPPER2_PWM_RATE MOTOR34_64KHZ // PWM rate for stepper 2 + +#elif defined(__PIC32MX__) + //#define MOTORDEBUG 1 + + // Uncomment the one of following lines if you have put a jumper from + // either pin 9 to pin 11 or pin 10 to pin 11 on your Motor Shield. + // Either will enable PWM for M1 + //#define PIC32_USE_PIN9_FOR_M1_PWM + //#define PIC32_USE_PIN10_FOR_M1_PWM + + #define MICROSTEPS 16 // 8 or 16 + + // For PIC32 Timers, define prescale settings by PWM frequency + #define MOTOR12_312KHZ 0 // 1:1, actual frequency 312KHz + #define MOTOR12_156KHZ 1 // 1:2, actual frequency 156KHz + #define MOTOR12_64KHZ 2 // 1:4, actual frequency 78KHz + #define MOTOR12_39KHZ 3 // 1:8, acutal frequency 39KHz + #define MOTOR12_19KHZ 4 // 1:16, actual frequency 19KHz + #define MOTOR12_8KHZ 5 // 1:32, actual frequency 9.7KHz + #define MOTOR12_4_8KHZ 6 // 1:64, actual frequency 4.8KHz + #define MOTOR12_2KHZ 7 // 1:256, actual frequency 1.2KHz + #define MOTOR12_1KHZ 7 // 1:256, actual frequency 1.2KHz + + #define MOTOR34_312KHZ 0 // 1:1, actual frequency 312KHz + #define MOTOR34_156KHZ 1 // 1:2, actual frequency 156KHz + #define MOTOR34_64KHZ 2 // 1:4, actual frequency 78KHz + #define MOTOR34_39KHZ 3 // 1:8, acutal frequency 39KHz + #define MOTOR34_19KHZ 4 // 1:16, actual frequency 19KHz + #define MOTOR34_8KHZ 5 // 1:32, actual frequency 9.7KHz + #define MOTOR34_4_8KHZ 6 // 1:64, actual frequency 4.8KHz + #define MOTOR34_2KHZ 7 // 1:256, actual frequency 1.2KHz + #define MOTOR34_1KHZ 7 // 1:256, actual frequency 1.2KHz + + // PWM rate for DC motors. + #define DC_MOTOR_PWM_RATE MOTOR34_39KHZ + // Note: for PIC32, both of these must be set to the same value + // since there's only one timebase for all 4 PWM outputs + #define STEPPER1_PWM_RATE MOTOR12_39KHZ + #define STEPPER2_PWM_RATE MOTOR34_39KHZ + +#endif + +// Bit positions in the 74HCT595 shift register output +#define MOTOR1_A 2 +#define MOTOR1_B 3 +#define MOTOR2_A 1 +#define MOTOR2_B 4 +#define MOTOR4_A 0 +#define MOTOR4_B 6 +#define MOTOR3_A 5 +#define MOTOR3_B 7 + +// Constants that the user passes in to the motor calls +#define FORWARD 1 +#define BACKWARD 2 +#define BRAKE 3 +#define RELEASE 4 + +// Constants that the user passes in to the stepper calls +#define SINGLE 1 +#define DOUBLE 2 +#define INTERLEAVE 3 +#define MICROSTEP 4 + +/* +#define LATCH 4 +#define LATCH_DDR DDRB +#define LATCH_PORT PORTB + +#define CLK_PORT PORTD +#define CLK_DDR DDRD +#define CLK 4 + +#define ENABLE_PORT PORTD +#define ENABLE_DDR DDRD +#define ENABLE 7 + +#define SER 0 +#define SER_DDR DDRB +#define SER_PORT PORTB +*/ + +// Arduino pin names for interface to 74HCT595 latch +#define MOTORLATCH 12 +#define MOTORCLK 4 +#define MOTORENABLE 7 +#define MOTORDATA 8 + +class AFMotorController +{ + public: + AFMotorController(void); + void enable(void); + friend class AF_DCMotor; + void latch_tx(void); + uint8_t TimerInitalized; +}; + +class AF_DCMotor +{ + public: + AF_DCMotor(uint8_t motornum, uint8_t freq = DC_MOTOR_PWM_RATE); + void run(uint8_t); + void setSpeed(uint8_t); + + private: + uint8_t motornum, pwmfreq; +}; + +class AF_Stepper { + public: + // The no arg constructor is a hack, if you use it, you have to call the init(uint16_t, uint8_t) function ! + AF_Stepper(); + AF_Stepper(uint16_t, uint8_t); + void init(uint16_t, uint8_t); + void step(uint16_t steps, uint8_t dir, uint8_t style = SINGLE); + void setSpeed(uint16_t); + uint8_t onestep(uint8_t dir, uint8_t style); + void release(void); + uint16_t revsteps; // # steps per revolution + uint8_t steppernum; + uint32_t usperstep, steppingcounter; + private: + uint8_t currentstep; + uint8_t is_init; + +}; + +uint8_t getlatchstate(void); + +#endif diff --git a/arduino/common/others/actuators.cpp b/arduino/common/others/actuators.cpp new file mode 100644 index 0000000..5366bcb --- /dev/null +++ b/arduino/common/others/actuators.cpp @@ -0,0 +1,97 @@ +// +// Created by tfuhrman & Elwan Héry on 29/04/18. +// + +#include "actuators.h" +#include "config_actuators.h" +#include + +//********************************************************************************************************************// +// +// Regulated actuators +// +//********************************************************************************************************************// + +#ifdef REGULATED_ACTUATORS_ENABLED + +ros::NodeHandle* node_handle_regulated_actuators = NULL; + +void interrupt_regulated_actuators(); +void reset_internal_values(); + +unsigned long ticks_counter = 0; +uint8_t regulation_activated = 0; +float regulation_reference_value = 0; +unsigned long last_ticks = 0; +float sum_speed_error = 0.0; +float last_speed_error = 0.0; +unsigned long last_control_time = 0; + +void init_regulated_actuators(ros::NodeHandle* nh) { + node_handle_regulated_actuators = nh; + pinMode(REGULATED_ACTUATORS_INTERRUPT_PIN, INPUT_PULLUP); + attachInterrupt(digitalPinToInterrupt(REGULATED_ACTUATORS_INTERRUPT_PIN), interrupt_regulated_actuators, RISING); + pinMode(REGULATED_ACTUATORS_PIN, OUTPUT); +} + +void loop_regulated_actuators() { + unsigned long current_control_time = millis(); + if (regulation_activated > 0) { + unsigned long delta_time = current_control_time - last_control_time; + if (delta_time >= REGULATED_ACTUATORS_CONTROL_LOOP_MS) { + last_control_time = current_control_time; + float nbr_revolution = (float)((float)(ticks_counter - last_ticks) / float(REGULATED_ACTUATORS_TICS_PER_REVOLUTION)); + float wheel_speed = (float)(nbr_revolution / (float)((float)delta_time / 1000.0)); + last_ticks = ticks_counter; + float speed_error = regulation_reference_value - wheel_speed; + sum_speed_error += speed_error; + float pwm_p = REGULATED_ACTUATORS_PID_P * speed_error; + if (pwm_p > 255) { + pwm_p = 255; + } else if (pwm_p < -255) { + pwm_p = -255; + } + float pwm_i = REGULATED_ACTUATORS_PID_I * sum_speed_error; + if (pwm_i > 255) { + pwm_i = 255; + } + float pwm_d = REGULATED_ACTUATORS_PID_D * (speed_error - last_speed_error); + int pwm_to_apply = pwm_p + pwm_i + pwm_d; + last_speed_error = speed_error; + if (pwm_to_apply > REGULATED_ACTUATORS_PWM_MAX) { + pwm_to_apply = REGULATED_ACTUATORS_PWM_MAX; + } else if (pwm_to_apply < REGULATED_ACTUATORS_PWM_MIN) { + pwm_to_apply = 0; + } + analogWrite(REGULATED_ACTUATORS_PIN, pwm_to_apply); +// String debug("pwm:" + String(pwm_to_apply) + " spd:" + String(wheel_speed) + " p:" + String(pwm_p) + " i:" + String(pwm_i) + " rev:" + String(nbr_revolution) + " tick:" + String(ticks_counter)); +// node_handle_regulated_actuators->loginfo(debug.c_str()); + } + } else { + analogWrite(REGULATED_ACTUATORS_PIN, 0); + } +} + +void activate_regulated_actuators(float reference_value) { + regulation_reference_value = reference_value; + regulation_activated = 1; + reset_internal_values(); +} + +void deactivate_regulated_actuators() { + regulation_activated = 0; + regulation_reference_value = 0; +} + +void interrupt_regulated_actuators() { + ticks_counter++; +} + +void reset_internal_values() { + last_ticks = 0; + sum_speed_error = 0.0; + last_speed_error = 0.0; + last_control_time = 0; +} + +#endif \ No newline at end of file diff --git a/arduino/common/others/actuators.h b/arduino/common/others/actuators.h new file mode 100644 index 0000000..b06ac89 --- /dev/null +++ b/arduino/common/others/actuators.h @@ -0,0 +1,26 @@ +// +// Created by tfuhrman & Elwan Héry on 29/04/18. +// + +#ifndef ARDUINO_ACTUATORS_H +#define ARDUINO_ACTUATORS_H + +#include +#include + +//********************************************************************************************************************// +// +// Regulated actuators +// +//********************************************************************************************************************// + +#ifdef REGULATED_ACTUATORS_ENABLED + +void init_regulated_actuators(ros::NodeHandle* nh); +void loop_regulated_actuators(); +void activate_regulated_actuators(float reference_value); +void deactivate_regulated_actuators(); + +#endif + +#endif //ARDUINO_ACTUATORS_H \ No newline at end of file diff --git a/arduino/common/others/config_actuators.h b/arduino/common/others/config_actuators.h new file mode 100644 index 0000000..40e7616 --- /dev/null +++ b/arduino/common/others/config_actuators.h @@ -0,0 +1,28 @@ +// +// Created by tfuhrman on 13/04/18. +// + +#ifndef ARDUINO_CONFIG_ACTUATORS_H +#define ARDUINO_CONFIG_ACTUATORS_H + +#include "config_robots.h" + +//********************************************************************************************************************// +// +// Regulated actuators +// +//********************************************************************************************************************// + +#define REGULATED_ACTUATORS_INTERRUPT_PIN 2 +#define REGULATED_ACTUATORS_PIN 8 +// other coeff for control with integral instead of proportional : P = 25 and I = 0.15 +#define REGULATED_ACTUATORS_PID_P 50.0f +#define REGULATED_ACTUATORS_PID_I 0.08f +#define REGULATED_ACTUATORS_PID_D 0.0f +#define REGULATED_ACTUATORS_CONTROL_LOOP_MS 100 +#define REGULATED_ACTUATORS_TICS_PER_REVOLUTION 10.0f +#define REGULATED_ACTUATORS_PWM_MIN 10 +#define REGULATED_ACTUATORS_PWM_MAX 255 + + +#endif //ARDUINO_CONFIG_ACTUATORS_H diff --git a/arduino/common/others/config_robots.cpp b/arduino/common/others/config_robots.cpp new file mode 100644 index 0000000..03f5570 --- /dev/null +++ b/arduino/common/others/config_robots.cpp @@ -0,0 +1,68 @@ +// +// Created by tfuhrman on 28/04/18. +// + +#include "config_robots.h" +#include "stdint.h" +#include "Arduino.h" + +// TODO this way to do is kind of ugly, think how to not duplicate variable declaration +// TODO find a way to just declares pins as define and to automatically fill the variable structures + +//********************************************************************************************************************// +// +// PR ROBOT +// +//********************************************************************************************************************// + +#if defined(PR_ROBOT) && !defined(GR_ROBOT) + +uint8_t pins_digital_actuators[NUM_DIGITAL_ACTUATORS] = {12}; +bool digital_actuators_states[NUM_DIGITAL_ACTUATORS] = {true}; +// Names : main_led + +uint8_t pins_pwm_actuators_pwm[NUM_PWM_ACTUATORS] = {8}; +uint8_t pwm_actuators_states[NUM_PWM_ACTUATORS] = {0}; +// Names : canon + +uint8_t pins_servo_actuators_pwm[NUM_SERVO_ACTUATORS] = {7, 6, 3, 5}; +int16_t servo_actuators_states[NUM_SERVO_ACTUATORS] = {10, 10, 10, 70}; +// Names : servo_front_lift, servo_back_lift, servo_lock, servo_flipper_side_canon + +int16_t stepper_actuators_states[NUM_STEPPER_ACTUATORS] = {}; +// Names : + +uint8_t pins_belt_sensors_shut[NUM_BELT_SENSORS] = {40, 42}; +uint8_t belt_sensors_addresses[NUM_BELT_SENSORS] = {22, 24}; +String belt_sensors_names[NUM_BELT_SENSORS] = {"sensor1", "sensor2"}; + +#endif + +//********************************************************************************************************************// +// +// GR ROBOT +// +//********************************************************************************************************************// + +#if defined(GR_ROBOT) && !defined(PR_ROBOT) + +uint8_t pins_digital_actuators[NUM_DIGITAL_ACTUATORS] = {2}; +bool digital_actuators_states[NUM_DIGITAL_ACTUATORS] = {false}; +// Names : + +uint8_t pins_pwm_actuators_pwm[NUM_PWM_ACTUATORS] = {}; +uint8_t pwm_actuators_states[NUM_PWM_ACTUATORS] = {}; +// Names : + +uint8_t pins_servo_actuators_pwm[NUM_SERVO_ACTUATORS] = {37}; +int16_t servo_actuators_states[NUM_SERVO_ACTUATORS] = {10}; +// Names : + +int16_t stepper_actuators_states[NUM_STEPPER_ACTUATORS] = {0, 1}; +// Names : + +uint8_t pins_belt_sensors_shut[NUM_BELT_SENSORS] = {40, 42}; +uint8_t belt_sensors_addresses[NUM_BELT_SENSORS] = {22, 24}; +String belt_sensors_names[NUM_BELT_SENSORS] = {"sensor1", "sensor2"}; + +#endif diff --git a/arduino/common/others/config_robots.h b/arduino/common/others/config_robots.h new file mode 100644 index 0000000..f36f628 --- /dev/null +++ b/arduino/common/others/config_robots.h @@ -0,0 +1,84 @@ +// +// Created by tfuhrman on 27/04/18. +// + +#ifndef ARDUINO_CONFIG_ROBOTS_H +#define ARDUINO_CONFIG_ROBOTS_H + +// You have to select a robot and only one !!! +#define PR_ROBOT +//#define GR_ROBOT + +/* + * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + * !!! !!! + * !!! DON'T FORGET TO PUT THE PIN NUMBERS AND INIT VALUES IN config_robot.cpp FILE !!! + * !!! !!! + * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + */ + +// If a stepper shield is connected, don't use the following pins : 3 4 7 8 11 12 13 + +//********************************************************************************************************************// +// +// PR ROBOT +// +//********************************************************************************************************************// + +#if defined(PR_ROBOT) && !defined(GR_ROBOT) + +// ---- DIGITAL ACTUATORS ---- +#define NUM_DIGITAL_ACTUATORS 1 + +// ---- PWM ACTUATORS ---- +#define NUM_PWM_ACTUATORS 1 +#define REGULATED_ACTUATORS_ENABLED + +// ---- SERVO ACTUATORS ---- +#define NUM_SERVO_ACTUATORS 4 + +// ---- STEPPER ACTUATORS ---- +#define NUM_STEPPER_ACTUATORS 0 + +// ---- BELT SENSOR ---- +#define SENSOR_BELT_ENABLED +#define NUM_BELT_SENSORS 2 + +// ---- COLOR SENSOR ---- +//#define SENSOR_COLOR_ENABLED +#define SENSOR_COLOR_S0 53 +#define SENSOR_COLOR_S1 51 +#define SENSOR_COLOR_S2 49 +#define SENSOR_COLOR_S3 47 +#define SENSOR_COLOR_LED 50 +#define SENSOR_COLOR_SENSOR_VALUE 45 + +#endif + +//********************************************************************************************************************// +// +// GR ROBOT +// +//********************************************************************************************************************// + +#if defined(GR_ROBOT) && !defined(PR_ROBOT) + +// ---- DIGITAL ACTUATORS ---- +#define NUM_DIGITAL_ACTUATORS 1 + +// ---- PWM ACTUATORS ---- +#define NUM_PWM_ACTUATORS 0 + +// ---- SERVO ACTUATORS ---- +#define NUM_SERVO_ACTUATORS 1 + +// ---- STEPPER ACTUATORS ---- +#define NUM_STEPPER_ACTUATORS 2 + +// ---- BELT SENSOR ---- +#define SENSOR_BELT_ENABLED +#define NUM_BELT_SENSORS 2 + +#endif + +#endif //ARDUINO_CONFIG_ROBOTS_H diff --git a/arduino/common/others/config_sensors.h b/arduino/common/others/config_sensors.h new file mode 100644 index 0000000..4090949 --- /dev/null +++ b/arduino/common/others/config_sensors.h @@ -0,0 +1,45 @@ +// +// Created by tfuhrman on 13/04/18. +// + +#ifndef ARDUINO_CONFIG_SENSORS_H +#define ARDUINO_CONFIG_SENSORS_H + +#include "config_robots.h" + +//********************************************************************************************************************// +// +// Belt sensor +// +//********************************************************************************************************************// + +#ifdef SENSOR_BELT_ENABLED +//TODO all this stuff ! +#endif + +//********************************************************************************************************************// +// +// Color sensor +// +//********************************************************************************************************************// + +#ifdef SENSOR_COLOR_ENABLED + +// Pins +#define S0 SENSOR_COLOR_S0 +#define S1 SENSOR_COLOR_S1 +#define S2 SENSOR_COLOR_S2 +#define S3 SENSOR_COLOR_S3 +#define LED SENSOR_COLOR_LED +#define SENSOR_VALUE SENSOR_COLOR_SENSOR_VALUE + +// Component configuration +#define COLOR_ACCUMULATE_NB 4 +#define COLOR_MEDIAN_SIZE 5 +#define COLOR_SENSOR_TIMEOUT 100000//in µs +#define SATURATION_MAX_VALUE 360 +#define HUE_MAX_VALUE 340 + +#endif + +#endif //ARDUINO_CONFIG_SENSORS_H diff --git a/arduino/common/others/others.ino b/arduino/common/others/others.ino new file mode 100644 index 0000000..5783d04 --- /dev/null +++ b/arduino/common/others/others.ino @@ -0,0 +1,289 @@ +//ROS includes +#include + +//Arduino includes +#include "Arduino.h" +#include +#include + +//Sensors includes +#include "sensors.h" +#include "config_sensors.h" + +//Actuators includes +#include "Servo.h" +#include "AFMotor.h" +#include +#include +#include +#include +#include +#include "actuators.h" +#include "config_actuators.h" + +#include "config_robots.h" + +ros::NodeHandle nh; + +// ---- SENSORS DEPARTMENT ---- + +void loop_sensors() { +#ifdef SENSOR_BELT_ENABLED + loop_belt_sensors(); +#endif +#ifdef SENSOR_COLOR_ENABLED + color_sensor_loop(); +#endif +} + +Timer sensors_loop_timer = Timer(50, &loop_sensors); + +void init_sensors() { +#ifdef SENSOR_BELT_ENABLED + init_belt_sensors(&nh); +#endif +#ifdef SENSOR_COLOR_ENABLED + color_sensor_init(&nh); +#endif + sensors_loop_timer.Start(); +} + +// ---- ACTUATORS DEPARTMENT ---- + +extern const uint8_t pins_digital_actuators[]; +extern bool digital_actuators_states[]; + +extern const uint8_t pins_pwm_actuators_pwm[]; +extern uint8_t pwm_actuators_states[]; + +extern const uint8_t pins_servo_actuators_pwm[]; +extern int16_t servo_actuators_states[]; +Servo servo_actuators_objects[NUM_SERVO_ACTUATORS]; + +extern int16_t stepper_actuators_states[]; +AF_Stepper stepper_actuators_objects[NUM_STEPPER_ACTUATORS]; + +// Actuators ROS callbacks + +drivers_ard_others::MoveResponse move_response_msg; +ros::Publisher move_responses_pub("/drivers/ard_others/move_response", &move_response_msg); + +void send_move_response(int16_t order_nb, bool success) { + if(success) nh.loginfo("Move request succeeded."); + else nh.logerror("Move request failed."); + + move_response_msg.order_nb = order_nb; + move_response_msg.success = success; + move_responses_pub.publish(&move_response_msg); +} + +void on_move(const drivers_ard_others::Move& msg){ + bool success = true; + + switch(msg.type) { + case msg.TYPE_DIGITAL: + if(msg.id >= 0 && msg.id <= NUM_DIGITAL_ACTUATORS) { + if(msg.dest_value == 0 || msg.dest_value == 1) + digital_actuators_states[msg.id] = bool(msg.dest_value); + else { + nh.logerror("MOVE failed : dest_value invalid (0 or 1)."); + success = false; + } + } else { + nh.logerror("MOVE failed : invalid id."); + success = false; + } + break; + case msg.TYPE_PWM: + if(msg.id >= 0 && msg.id <= NUM_PWM_ACTUATORS) { + if(msg.dest_value >= 0 && msg.dest_value <= 255) { + pwm_actuators_states[msg.id] = msg.dest_value; +#ifdef REGULATED_ACTUATORS_ENABLED + if (msg.dest_value == 0) { + deactivate_regulated_actuators(); + nh.loginfo("Deactivate regulation"); + } else { + activate_regulated_actuators(msg.dest_value); + nh.loginfo("Activate regulation"); + } +#endif + } + else { + nh.logerror("MOVE failed : dest_value invalid (0 to 255)."); + success = false; + } + } else { + nh.logerror("MOVE failed : invalid id."); + success = false; + } + break; + case msg.TYPE_SERVO: + if(msg.id >= 0 && msg.id <= NUM_SERVO_ACTUATORS) { + if(msg.dest_value >= 0 && msg.dest_value <= 180) + servo_actuators_states[msg.id] = msg.dest_value; + else { + nh.logerror("MOVE failed : dest_value invalid (0 to 180)."); + success = false; + } + } else { + nh.logerror("MOVE failed : invalid id."); + success = false; + } + break; + case msg.TYPE_STEPPER: + if(msg.id >= 0 && msg.id <= NUM_STEPPER_ACTUATORS) { + stepper_actuators_states[msg.id] = msg.dest_value; + if (msg.dest_value == 0) { + stepper_actuators_objects[msg.id].release(); + } + if (msg.dest_value < 0) { + stepper_actuators_objects[msg.id].setSpeed(120); + stepper_actuators_objects[msg.id].step(-msg.dest_value, FORWARD, SINGLE); + } + else { + stepper_actuators_objects[msg.id].setSpeed(120); + stepper_actuators_objects[msg.id].step(msg.dest_value, BACKWARD, SINGLE); + } + success = true; + } else { + nh.logerror("MOVE failed : invalid id."); + success = false; + } + break; + } + nh.loginfo("Finished move order handling."); + if(msg.order_nb != 0) send_move_response(msg.order_nb, success); // send response if order_nb provided. +} + +ros::Subscriber sub_move("/drivers/ard_others/move", &on_move); + +drivers_ard_others::ActDigitalStates digital_states_msg; +ros::Publisher digital_states_pub("/drivers/ard_others/digital_act_states", &digital_states_msg); +drivers_ard_others::ActPWMStates pwm_states_msg; +ros::Publisher pwm_states_pub("/drivers/ard_others/pwm_act_states", &pwm_states_msg); +drivers_ard_others::ActServoStates servo_states_msg; +ros::Publisher servo_states_pub("/drivers/ard_others/servo_act_states", &servo_states_msg); +drivers_ard_others::ActServoStates stepper_states_msg; +ros::Publisher stepper_states_pub("/drivers/ard_others/stepper_act_states", &stepper_states_msg); + +// Digital actuators +void init_digital_actuators() { + for(uint8_t i = 0; i < NUM_DIGITAL_ACTUATORS; i++) { + pinMode(pins_digital_actuators[i], OUTPUT); + } +} + +void loop_digital_actuators() { + for(uint8_t i = 0; i < NUM_DIGITAL_ACTUATORS; i++) + digitalWrite(pins_digital_actuators[i], digital_actuators_states[i]); + + digital_states_msg.states_length = NUM_DIGITAL_ACTUATORS; + digital_states_msg.states = digital_actuators_states; + digital_states_pub.publish(&digital_states_msg); +} + +// PWM actuators +void init_pwm_actuators() { + for(uint8_t i = 0; i < NUM_PWM_ACTUATORS; i++) { +#ifdef REGULATED_ACTUATORS_ENABLED + init_regulated_actuators(&nh); +#else + pinMode(pins_pwm_actuators_pwm[i], OUTPUT); +#endif + } +} + +void loop_pwm_actuators() { + for(uint8_t i = 0; i < NUM_PWM_ACTUATORS; i++) { +#ifdef REGULATED_ACTUATORS_ENABLED + loop_regulated_actuators(); +#else + analogWrite(pins_pwm_actuators_pwm[i], pwm_actuators_states[i]); +#endif + } + + pwm_states_msg.states_length = NUM_PWM_ACTUATORS; + pwm_states_msg.states = pwm_actuators_states; + pwm_states_pub.publish(&pwm_states_msg); +} + +// Servo actuators +void init_servo_actuators() { + for(uint8_t i = 0; i < NUM_SERVO_ACTUATORS; i++) { + pinMode(pins_digital_actuators[i], OUTPUT); + servo_actuators_objects[i].attach(pins_servo_actuators_pwm[i]); + } +} + +void loop_servo_actuators() { + for(uint8_t i = 0; i < NUM_SERVO_ACTUATORS; i++) + servo_actuators_objects[i].write(servo_actuators_states[i]); + + servo_states_msg.states_length = NUM_SERVO_ACTUATORS; + servo_states_msg.states = servo_actuators_states; + servo_states_pub.publish(&servo_states_msg); +} + +// Stepper actuators +void init_stepper_actuators() { + for(uint8_t i = 0; i < NUM_STEPPER_ACTUATORS; i++) { + //TODO better initialisation with different stepper_number + stepper_actuators_objects[i].init(200, 1); + stepper_actuators_objects[i].setSpeed(60); + } +} + +void loop_stepper_actuators() { + stepper_states_msg.states_length = NUM_STEPPER_ACTUATORS; + stepper_states_msg.states = stepper_actuators_states; + stepper_states_pub.publish(&stepper_states_msg); +} + +void loop_actuators() { + loop_digital_actuators(); + loop_pwm_actuators(); + loop_servo_actuators(); + loop_stepper_actuators(); +} + +Timer actuators_loop_timer = Timer(50, &loop_actuators); + +void init_actuators() { + init_digital_actuators(); + init_pwm_actuators(); + init_servo_actuators(); + init_stepper_actuators(); + actuators_loop_timer.Start(); +} + +// ---- MAIN FUCNTIONS ---- + +void setup() { + // ROS init + nh.initNode(); + + nh.subscribe(sub_move); + nh.advertise(move_responses_pub); + nh.advertise(digital_states_pub); + nh.advertise(pwm_states_pub); + nh.advertise(servo_states_pub); + nh.advertise(stepper_states_pub); + + // Libs init + Wire.begin(); + + // Components init + init_sensors(); + init_actuators(); + + nh.loginfo("Node '/arduinos/others' initialized correctly."); +} + +void loop() { + // Components loop + sensors_loop_timer.Update(); + actuators_loop_timer.Update(); + + // ROS loop + nh.spinOnce(); +} diff --git a/arduino/common/others/sensors.cpp b/arduino/common/others/sensors.cpp new file mode 100644 index 0000000..e4239bd --- /dev/null +++ b/arduino/common/others/sensors.cpp @@ -0,0 +1,274 @@ +// +// Created by tfuhrman on 13/04/18. +// + +#include "sensors.h" +#include "VL53L0X.h" +#include +#include + +//********************************************************************************************************************// +// +// Belt sensor +// +//********************************************************************************************************************// + +#ifdef SENSOR_BELT_ENABLED + +ros::NodeHandle* node_handle_belt_sensor = NULL; + +extern uint8_t pins_belt_sensors_shut[]; +extern uint8_t belt_sensors_addresses[]; +extern String belt_sensors_names[]; +VL53L0X belt_sensors[NUM_BELT_SENSORS]; + +drivers_ard_others::BeltRange belt_range_msg; +ros::Publisher belt_ranges_pub("/drivers/ard_others/belt_ranges", &belt_range_msg); + +void init_belt_sensors(ros::NodeHandle* nh) { + node_handle_belt_sensor = nh; + if (node_handle_belt_sensor) { + node_handle_belt_sensor->advertise(belt_ranges_pub); + } + for(uint8_t i = 0; i < NUM_BELT_SENSORS; i++) { + pinMode(pins_belt_sensors_shut[i], OUTPUT); + digitalWrite(pins_belt_sensors_shut[i], LOW); + } + + for(uint8_t i = 0; i < NUM_BELT_SENSORS; i++) { + pinMode(pins_belt_sensors_shut[i], INPUT); + delay(50); + belt_sensors[i].init(true); + delay(100); + belt_sensors[i].setAddress(belt_sensors_addresses[i]); + } + + for(uint8_t i = 0; i < NUM_BELT_SENSORS; i++) { + belt_sensors[i].setTimeout(500); + // Do not use this as the time of first measurement will be too big +// belt_sensors[i].setMeasurementTimingBudget(200000); + belt_sensors[i].startContinuous(); + } +} + +void loop_belt_sensors() { + for(uint8_t i = 0; i < NUM_BELT_SENSORS; i++) { + belt_range_msg.sensor_id = belt_sensors_names[i].c_str(); + unsigned long now = millis(); + belt_range_msg.range = belt_sensors[i].readRangeContinuousMillimeters() / 1000.0; //in meters + unsigned long diff = millis() - now; + if (belt_range_msg.range > 65534) + belt_range_msg.range = -1; + belt_ranges_pub.publish(&belt_range_msg); +// node_handle_belt_sensor->loginfo(String("Belt : " + String(diff)).c_str()); + } +} + +#endif + +//********************************************************************************************************************// +// +// Color sensor +// +//********************************************************************************************************************// + +#ifdef SENSOR_COLOR_ENABLED + +ros::NodeHandle* node_handle_color_sensor = NULL; + +drivers_ard_others::Color color_msg; +ros::Publisher color_pub("/drivers/ard_others/color", &color_msg); + +// This structure is used to store the current values +uint8_t color_sensor_rgb_values[3]; + +// This structure is used to store the median computed values in order to compute a mean value +uint16_t color_sensor_previous_tsl_values[COLOR_ACCUMULATE_NB][3]; +uint8_t color_sensor_previous_tsl_values_index = 0; + +//used to map rawFrequency read from sensor to a RGB value on 8 bytes +//those data have to be calibrated to be optimal +//the index is rgb_name_enum +uint8_t rgbMinMaxFrequency[3][2] = { + {25, 54}, + {15, 100}, + {10, 90} +}; + +void color_sensor_init(ros::NodeHandle* nh) { + node_handle_color_sensor = nh; + if (node_handle_color_sensor) { + node_handle_color_sensor->advertise(color_pub); + } + pinMode(S0, OUTPUT); + pinMode(S1, OUTPUT); + pinMode(S2, OUTPUT); + pinMode(S3, OUTPUT); + pinMode(LED, OUTPUT); + pinMode(SENSOR_VALUE, INPUT); + // Setting frequency-scaling to 20% + digitalWrite(S0,HIGH); + digitalWrite(S1,LOW); + // TODO LED never powered off + digitalWrite(LED, HIGH); +} + +void color_sensor_loop() { +// unsigned long start_time = micros(); + uint16_t rgbColorAccumulator[3] = {0, 0, 0}; + uint8_t rgbMeanValues[3] = {0, 0, 0}; + uint16_t tslValues[3] = {0, 0, 0}; + uint8_t color_sensor_rgb_values_array[COLOR_MEDIAN_SIZE][3]; + uint8_t color_sensor_rgb_values_sorted_array[COLOR_MEDIAN_SIZE][3]; + // First accumulate color sensor values to be more accurate + for (uint8_t accumulator_nb = 0; accumulator_nb < COLOR_MEDIAN_SIZE; accumulator_nb++) { + color_sensor_values_capture(); + color_sensor_rgb_values_array[accumulator_nb][RGB_RED] = color_sensor_rgb_values[RGB_RED]; + color_sensor_rgb_values_array[accumulator_nb][RGB_GREEN] = color_sensor_rgb_values[RGB_GREEN]; + color_sensor_rgb_values_array[accumulator_nb][RGB_BLUE] = color_sensor_rgb_values[RGB_BLUE]; + } + // Sort the values to compute the median + for (uint8_t accumulator_nb = 0; accumulator_nb < COLOR_MEDIAN_SIZE; ++accumulator_nb) { + uint8_t min_values_rgb[3] = {color_sensor_rgb_values_array[accumulator_nb][RGB_RED], + color_sensor_rgb_values_array[accumulator_nb][RGB_GREEN], + color_sensor_rgb_values_array[accumulator_nb][RGB_BLUE]}; + for (uint8_t min_nb = accumulator_nb; min_nb < COLOR_MEDIAN_SIZE; ++min_nb) { + if (color_sensor_rgb_values_array[min_nb][RGB_RED] < min_values_rgb[RGB_RED]) { + min_values_rgb[RGB_RED] = color_sensor_rgb_values_array[min_nb][RGB_RED]; + } + if (color_sensor_rgb_values_array[min_nb][RGB_GREEN] < min_values_rgb[RGB_GREEN]) { + min_values_rgb[RGB_GREEN] = color_sensor_rgb_values_array[min_nb][RGB_GREEN]; + } + if (color_sensor_rgb_values_array[min_nb][RGB_BLUE] < min_values_rgb[RGB_BLUE]) { + min_values_rgb[RGB_BLUE] = color_sensor_rgb_values_array[min_nb][RGB_BLUE]; + } + } + color_sensor_rgb_values_sorted_array[accumulator_nb][RGB_RED] = min_values_rgb[RGB_RED]; + color_sensor_rgb_values_sorted_array[accumulator_nb][RGB_GREEN] = min_values_rgb[RGB_GREEN]; + color_sensor_rgb_values_sorted_array[accumulator_nb][RGB_BLUE] = min_values_rgb[RGB_BLUE]; + } + // Extract the median + rgbMeanValues[RGB_RED] = color_sensor_rgb_values_sorted_array[COLOR_MEDIAN_SIZE / 2][RGB_RED]; + rgbMeanValues[RGB_GREEN] = color_sensor_rgb_values_sorted_array[COLOR_MEDIAN_SIZE / 2][RGB_GREEN]; + rgbMeanValues[RGB_BLUE] = color_sensor_rgb_values_sorted_array[COLOR_MEDIAN_SIZE / 2][RGB_BLUE]; + // Compute the corresponding tsl colors + color_sensor_rgb_to_tsl(rgbMeanValues, tslValues); + // Add the data to the previous values + color_sensor_previous_tsl_values[color_sensor_previous_tsl_values_index][TSL_HUE] = tslValues[TSL_HUE]; + color_sensor_previous_tsl_values[color_sensor_previous_tsl_values_index][TSL_SATURATION] = tslValues[TSL_SATURATION]; + color_sensor_previous_tsl_values[color_sensor_previous_tsl_values_index][TSL_LIGHTNESS] = tslValues[TSL_LIGHTNESS]; + color_sensor_previous_tsl_values_index = ++color_sensor_previous_tsl_values_index % COLOR_ACCUMULATE_NB; + // Compute the shifting mean value of tsl values + uint16_t tsl_mean_values[3] = {0, 0, 0}; + color_sensor_mean_value_compute(tsl_mean_values); + // Finally send the data + color_sensor_values_publish(tsl_mean_values); +// unsigned long diff_time = micros() - start_time; +// String tmp_str = String(diff_time, DEC); +// node_handle->loginfo(tmp_str.c_str()); +} + +void color_sensor_values_capture() { + uint8_t rawFrequency = 0; + for (uint8_t color_id = 0; color_id < 3; color_id++) { + color_sensor_filter_apply((rgb_name_enum)color_id); + rawFrequency = pulseIn(SENSOR_VALUE, LOW); + if (rawFrequency > 0) { + color_sensor_rgb_values[color_id] = constrain(map(rawFrequency, rgbMinMaxFrequency[color_id][0], rgbMinMaxFrequency[color_id][1], 255, 0), 0, 255); + } else { + node_handle_color_sensor->logwarn("Color sensor timed out, no values..."); + } + } +} + +void color_sensor_filter_apply(rgb_name_enum color) { + switch (color) { + case RGB_RED: + digitalWrite(S2,LOW); + digitalWrite(S3,LOW); + break; + case RGB_GREEN: + digitalWrite(S2,HIGH); + digitalWrite(S3,HIGH); + break; + case RGB_BLUE: + digitalWrite(S2,LOW); + digitalWrite(S3,HIGH); + break; + default: + break; + } +} + +void color_sensor_rgb_to_tsl(uint8_t rgbValues[3], uint16_t tslColors[3]) { + uint16_t maxColorValue = (uint16_t)rgbValues[RGB_RED]; + uint16_t minColorValue = (uint16_t)rgbValues[RGB_RED]; + uint8_t maxColorValueIndex = 0; + // Compute min and max color + for (uint8_t index = 1; index < 3; index++) { + if ((uint16_t)rgbValues[index] > maxColorValue) { + maxColorValue = (uint16_t)rgbValues[index]; + maxColorValueIndex = index; + } + if ((uint16_t)rgbValues[index] < minColorValue) { + minColorValue = (uint16_t)rgbValues[index]; + } + } + // Use float values, not best idea but standard TSL calculation can't be done in integers... + uint16_t delta = maxColorValue - minColorValue; + if (delta != 0) { + float deltaf = (float)delta; + float hue; + if ((rgb_name_enum)maxColorValueIndex == RGB_RED) + { + hue = (float)(rgbValues[RGB_GREEN] - rgbValues[RGB_BLUE]) / delta; + } + else + { + if ((rgb_name_enum)maxColorValueIndex == RGB_GREEN) + { + hue = 2.0 + (float)(rgbValues[RGB_BLUE] - rgbValues[RGB_RED]) / delta; + } + else + { + hue = 4.0 + (float)(rgbValues[RGB_RED] - rgbValues[RGB_GREEN]) / delta; + } + } + hue *= 60.0; + if (hue < 0) hue += 360; + if (hue >= HUE_MAX_VALUE) { + hue = 0; + } + tslColors[TSL_HUE] = (uint16_t)hue; + } + // Compute the tsl values + uint16_t saturation = (uint16_t)((uint16_t)100 * (uint16_t)((maxColorValue - minColorValue)) / (uint16_t)maxColorValue); + if (saturation >= SATURATION_MAX_VALUE) { + saturation = 0; + } + tslColors[TSL_SATURATION] = saturation; + tslColors[TSL_LIGHTNESS] = (uint16_t)((uint16_t)100 * maxColorValue) / (uint16_t)255; +} + +void color_sensor_values_publish(uint16_t tslColors[3]) { + color_msg.hue = tslColors[TSL_HUE]; + color_msg.saturation = tslColors[TSL_SATURATION]; + color_msg.lightness = tslColors[TSL_LIGHTNESS]; + if (node_handle_color_sensor) { + color_pub.publish(&color_msg); + } +} + +void color_sensor_mean_value_compute(uint16_t tslColors[3]) { + uint16_t tsl_accumulator[3] = {0, 0, 0}; + for(uint8_t counter = 0; counter < COLOR_ACCUMULATE_NB; ++counter) { + tsl_accumulator[TSL_HUE] += color_sensor_previous_tsl_values[counter][TSL_HUE]; + tsl_accumulator[TSL_SATURATION] += color_sensor_previous_tsl_values[counter][TSL_SATURATION]; + tsl_accumulator[TSL_LIGHTNESS] += color_sensor_previous_tsl_values[counter][TSL_LIGHTNESS]; + } + tslColors[TSL_HUE] = tsl_accumulator[TSL_HUE] / COLOR_ACCUMULATE_NB; + tslColors[TSL_SATURATION] = tsl_accumulator[TSL_SATURATION] / COLOR_ACCUMULATE_NB; + tslColors[TSL_LIGHTNESS] = tsl_accumulator[TSL_LIGHTNESS] / COLOR_ACCUMULATE_NB; +} + +#endif diff --git a/arduino/common/others/sensors.h b/arduino/common/others/sensors.h new file mode 100644 index 0000000..1e6c35f --- /dev/null +++ b/arduino/common/others/sensors.h @@ -0,0 +1,58 @@ +// +// Created by tfuhrman on 13/04/18. +// + +#ifndef ARDUINO_SENSOR_H +#define ARDUINO_SENSOR_H + +#include "config_sensors.h" +#include +#include + +//********************************************************************************************************************// +// +// Belt sensor +// +//********************************************************************************************************************// + +#ifdef SENSOR_BELT_ENABLED + +void init_belt_sensors(ros::NodeHandle* nh); +void loop_belt_sensors(); + +#endif + +//********************************************************************************************************************// +// +// Color sensor +// +//********************************************************************************************************************// + +#ifdef SENSOR_COLOR_ENABLED + +// Public interface +enum rgb_name_enum { + RGB_RED = 0, + RGB_GREEN, + RGB_BLUE +}; + +enum tsl_name_enum { + TSL_HUE = 0, + TSL_SATURATION, + TSL_LIGHTNESS +}; + +void color_sensor_init(ros::NodeHandle* nh); +void color_sensor_loop(); + +// Private stuff +void color_sensor_values_capture(); +void color_sensor_filter_apply(rgb_name_enum color); +void color_sensor_rgb_to_tsl(uint8_t rgbValues[3], uint16_t tslColors[3]); +void color_sensor_values_publish(uint16_t tslColors[3]); +void color_sensor_mean_value_compute(uint16_t tslColors[3]); + +#endif + +#endif //ARDUINO_SENSOR_H \ No newline at end of file diff --git a/arduino/common/rosserial_template.ino b/arduino/common/rosserial_template.ino new file mode 100644 index 0000000..b3c188e --- /dev/null +++ b/arduino/common/rosserial_template.ino @@ -0,0 +1,46 @@ +#include +#include +#include +ros::NodeHandle nh; + +/* +This is a simple template that shows how to create a subscriber and publisher +with rosserial. This template works with coupe18's system. + +Created by Pierre Laclau, UTCoupe 2018. +*/ + + +// ROS methods +void on_set_strategies(const drivers_ard_hmi::SetStrategies& msg){ + int array_count = msg.strategies_names_length; + for(int i=0; i < msg.strategies_names_length; i++) { + String a = msg.strategies_names[i]; + } +} + +// Subscriber example +ros::Subscriber sub_strats("/feedback/ard_hmi/set_strategies", &on_set_strategies); + +// Publisher example +drivers_ard_hmi::HMIEvent hmi_event_msg; +ros::Publisher hmi_event_pub("/feedback/ard_hmi/hmi_event", &hmi_event_msg); + + +void setup() { + nh.initNode(); + nh.subscribe(sub_strats); + nh.advertise(hmi_event_pub); +} + +void loop() { + // publish example + hmi_event_msg.event = hmi_event_msg.EVENT_JACK_FIRED; + hmi_event_msg.chosen_strategy_id = 0; + hmi_event_msg.chosen_team_id = 0; + hmi_event_pub.publish(&hmi_event_msg); + + // spin Once to sync publishes and subscriptions with ros.h + nh.spinOnce(); + delay(30); +} \ No newline at end of file diff --git a/arduino/common/shared/sender.cpp b/arduino/common/shared/sender.cpp new file mode 100644 index 0000000..a24cfb2 --- /dev/null +++ b/arduino/common/shared/sender.cpp @@ -0,0 +1,95 @@ +/** + * \file sender.c + * \author Thomas Fuhrmann + * \brief Functions to send data accross the serial communication line + * \date 06/12/2016 + * \copyright Copyright (c) 2016 UTCoupe All rights reserved. + */ + +#include "sender.h" +#include "parameters.h" + +#include +#include + +/* Initialize static members */ +QueueList SerialSender::dataToSend; + +SerialSender::SerialSender() { +} + +void SerialSender::SerialSend(SerialSendEnum level, String data) { + if (level <= DEBUG_LEVEL && data != "") { + dataToSend.push(data); + } +} + +void SerialSender::SerialSend(SerialSendEnum level, const char* str, ...) { + uint8_t i, j, count = 0; + String serialData, tmpString = ""; + if (level <= DEBUG_LEVEL) { + va_list argv; + va_start(argv, str); + for (i = 0, j = 0; str[i] != '\0'; i++) { + if (str[i] == '%') { + count++; + + tmpString = CharArrayToString((str + j), i - j); + serialData.concat(tmpString); + + switch (str[++i]) { + case 'i': + case 'd': + tmpString = String(va_arg(argv, int)); + break; + case 'l': + tmpString = String(va_arg(argv, long)); + break; + case 'f': //tmpString = String(va_arg(argv, float), 4); + break; + case 'c': + tmpString = String((char) va_arg(argv, int)); + break; + case 's': + tmpString = String(va_arg(argv, char *)); + break; +// case '%': +// Serial.print("%"); +// break; + default:; + } + serialData.concat(tmpString); + j = i + 1; + } + } + va_end(argv); + + if (i > j) { + tmpString = CharArrayToString((str + j), i - j); + serialData.concat(tmpString); + } + + dataToSend.push(serialData); + } +} + +void SerialSender::SerialSendTask() { + String dataToPrint; + while (!dataToSend.isEmpty()) { + dataToPrint = dataToSend.pop(); + Serial.println(dataToPrint); + Serial.flush(); + } +} + +String SerialSender::CharArrayToString(const char * str, uint8_t size) { + String returnedString = ""; + //todo size as define + if ((str != nullptr) && (size > 0) && (size < 71)) { + static char tmpBuffer[70]; + memcpy(tmpBuffer, str, size); + tmpBuffer[size] = '\0'; + returnedString = String(tmpBuffer); + } + return returnedString; +} diff --git a/arduino/common/shared/sender.h b/arduino/common/shared/sender.h new file mode 100644 index 0000000..985ed72 --- /dev/null +++ b/arduino/common/shared/sender.h @@ -0,0 +1,47 @@ +/** + * \file sender.h + * \author Thomas Fuhrmann + * \brief Functions to send data accross the serial communication line + * \date 06/12/2016 + * \copyright Copyright (c) 2016 UTCoupe All rights reserved. + */ + +#ifndef ARDUINO_SENDER_H +#define ARDUINO_SENDER_H + +#include +#include + +#include + +#ifndef SENDER_ENUM +#define SENDER_ENUM +typedef enum +{ + SERIAL_ERROR = 0, + SERIAL_INFO, + SERIAL_DEBUG +} SerialSendEnum; +#endif + +//#ifdef __cplusplus +//extern "C" void SerialSendC(SerialSendEnum level, String data); +//#endif + +class SerialSender +{ +public: + SerialSender(); + ~SerialSender() {} + //to be used everywhere + //todo check why its only working with minimal 1 variadic argument + static void SerialSend(SerialSendEnum level, const char* data, ...); + static void SerialSend(SerialSendEnum level, String data); + //to be used in the task + static void SerialSendTask(); +private: + static String CharArrayToString(const char * str, uint8_t size); + static QueueList dataToSend; +}; + +#endif //ARDUINO_SENDER_H diff --git a/arduino/common/shared/sender_wrapper.cpp b/arduino/common/shared/sender_wrapper.cpp new file mode 100644 index 0000000..1535ec0 --- /dev/null +++ b/arduino/common/shared/sender_wrapper.cpp @@ -0,0 +1,24 @@ +// +// Created by tfuhrman on 29/01/17. +// + +#include "sender_wrapper.h" +#include "sender.h" +#include + +//static void SerialSendWrapVariadic(SerialSendEnum level, const char* data, va_list args); + +static void SerialSendWrapVariadic(SerialSendEnum level, const char* data, va_list args) { + SerialSender::SerialSend(level, data, args); +} + +void SerialSendWrap(SerialSendEnum level, String data) { + SerialSender::SerialSend(level, data); +} + +void SerialSendWrapVar(SerialSendEnum level, const char* data, ...) { + va_list args; + va_start(args, data); + SerialSendWrapVariadic(level, data, args); + va_end(args); +} diff --git a/arduino/common/shared/sender_wrapper.h b/arduino/common/shared/sender_wrapper.h new file mode 100644 index 0000000..08575eb --- /dev/null +++ b/arduino/common/shared/sender_wrapper.h @@ -0,0 +1,28 @@ +// +// Created by tfuhrman on 29/01/17. +// + +#ifndef ARDUINO_SENDER_WRAPPER_H +#define ARDUINO_SENDER_WRAPPER_H + +#include + +#ifndef SENDER_ENUM +#define SENDER_ENUM +typedef enum +{ + SERIAL_ERROR = 0, + SERIAL_INFO, + SERIAL_DEBUG +} SerialSendEnum; +#endif + +#ifdef __cplusplus +extern "C" void SerialSendWrap(SerialSendEnum level, String data); +extern "C" void SerialSendWrapVar(SerialSendEnum level, const char* data, ...); +#else +//void SerialSendWrap(SerialSendEnum level, String data); +void SerialSendWrapVar(SerialSendEnum level, const char* data, ...); +#endif + +#endif //ARDUINO_SENDER_WRAPPER_H diff --git a/arduino/generate_arduino.sh b/arduino/generate_arduino.sh new file mode 100755 index 0000000..af9307e --- /dev/null +++ b/arduino/generate_arduino.sh @@ -0,0 +1,56 @@ +#!/bin/bash + +USER_TARGET="" +USER_ROBOT="" +USER_PROGRAM="" + +function generate_cmake() { + rm -rf build + mkdir -p build + cd build + cmake ../ -DTARGET_ARDUINO:STRING=$USER_TARGET -DTARGET_ROBOT:STRING=$USER_ROBOT -DTARGET_PROGRAM:STRING=$USER_PROGRAM +} + +function compile_program() { + if [ ! "${PWD##*/}" = "build" ]; then + printf "Not in the right place for build..." + fi + make +} + +function upload_program() { + #TODO check if an arduino is present + make upload +} + +function ask_user() { + printf "What's the target (nano328, uno, leonardo, mega2560) ?" + read answer + #TODO shorter names ? (eg mega...) + if [ "$answer" = "nano328" ] || [ "$answer" = "uno" ] || [ "$answer" = "leonardo" ] || [ "$answer" = "mega2560" ]; then + USER_TARGET="$answer" + fi + + printf "What's the robot (pr, gr) ?" + read answer + if [ "$answer" = "pr" ] || [ "$answer" = "gr" ]; then + USER_ROBOT="$answer" + fi + + printf "What's the program (asserv, others) ?" + read answer + if [ "$answer" = "asserv" ] || [ "$answer" = "others" ]; then + USER_PROGRAM="$answer" + fi + + if [ "$USER_ROBOT" = "" ] || [ "$USER_ROBOT" = "" ] || [ "$USER_PROGRAM" = "" ]; then + printf "Incorrect parameters, please relaunch the script with correct parameters...\n" + exit 1 + fi +} + +# "Main" +ask_user +generate_cmake +compile_program +upload_program diff --git a/arduino/gr/asserv/AFMotor.cpp b/arduino/gr/asserv/AFMotor.cpp new file mode 100644 index 0000000..4686cfa --- /dev/null +++ b/arduino/gr/asserv/AFMotor.cpp @@ -0,0 +1,145 @@ +// Adafruit Motor shield library +// copyright Adafruit Industries LLC, 2009 +// this code is public domain, enjoy! +// Adapté pour UTCoupe2011 par Arthur, 19/01/2011 +// Moteurs 1 et 2 correspondent à la carte Rugged + + +#include +#include + + +#include "AFMotor.h" + + +/****************************************** + MOTORS +******************************************/ +inline void initPWM1(uint8_t freq) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer2B (pin 3) + TCCR2A |= _BV(COM2B1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2b + TCCR2B = freq & 0x7; + OCR2B = 0; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 3 is now PE5 (OC3C) + TCCR3A |= _BV(COM1C1) | _BV(WGM10); // fast PWM, turn on oc3c + TCCR3B = (freq & 0x7) | _BV(WGM12); + OCR3C = 0; + +#else + #error "This chip is not supported!" +#endif + + pinMode(3, OUTPUT); +} + +inline void setPWM1(uint8_t s) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer2A on PB3 (Arduino pin #11) + OCR2B = s; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 11 is now PB5 (OC1A) + OCR3C = s; +#else + #error "This chip is not supported!" +#endif +} + +inline void initPWM2(uint8_t freq) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer2A on PB3 (Arduino pin #11) + TCCR2A |= _BV(COM2A1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2a + TCCR2B = freq & 0x7; + OCR2A = 0; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 11 is now PB5 (OC1A) + TCCR1A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc1a + TCCR1B = (freq & 0x7) | _BV(WGM12); + OCR1A = 0; +#else + #error "This chip is not supported!" +#endif + pinMode(11, OUTPUT); +} + +inline void setPWM2(uint8_t s) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer2A on PB3 (Arduino pin #11) + OCR2A = s; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 11 is now PB5 (OC1A) + OCR1A = s; +#else + #error "This chip is not supported!" +#endif +} + +AF_DCMotor::AF_DCMotor(uint8_t num, uint8_t freq) { + motornum = num; + pwmfreq = freq; + + switch (num) { + case 1: + pinMode(MOTOR1_DIR, OUTPUT); + digitalWrite(MOTOR1_DIR, LOW); + initPWM1(freq); + break; + case 2: + pinMode(MOTOR2_DIR, OUTPUT); + digitalWrite(MOTOR2_DIR, LOW); + initPWM2(freq); + break; + } +} + +void AF_DCMotor::run(uint8_t cmd) { + uint8_t dirPin; + switch (motornum) { + case 1: + dirPin = MOTOR1_DIR; break; + case 2: + dirPin = MOTOR2_DIR; break; + default: + return; + } + + switch (cmd) { + case FORWARD: + digitalWrite(dirPin, LOW); + break; + case BACKWARD: + digitalWrite(dirPin, HIGH); + break; + case RELEASE: + //TODO + break; + } +} + +void AF_DCMotor::setSpeed(uint8_t speed) { + switch (motornum) { + case 1: + setPWM1(speed); break; + case 2: + setPWM2(speed); break; + } +} + + diff --git a/arduino/gr/asserv/AFMotor.h b/arduino/gr/asserv/AFMotor.h new file mode 100644 index 0000000..b6c34d5 --- /dev/null +++ b/arduino/gr/asserv/AFMotor.h @@ -0,0 +1,46 @@ +// Adafruit Motor shield library +// copyright Adafruit Industries LLC, 2009 +// this code is public domain, enjoy! + +#ifndef _AFMotor_h_ +#define _AFMotor_h_ + +#include +#include + + +#define MOTOR12_64KHZ _BV(CS20) // no prescale +#define MOTOR12_8KHZ _BV(CS21) // divide by 8 +#define MOTOR12_2KHZ _BV(CS21) | _BV(CS20) // divide by 32 +#define MOTOR12_1KHZ _BV(CS22) // divide by 64 + +#define MOTOR1_DIR 12 +#define MOTOR2_DIR 13 + +#define MOTOR1_A 2 +#define MOTOR1_B 3 +#define MOTOR2_A 1 +#define MOTOR2_B 4 +#define MOTOR4_A 0 +#define MOTOR4_B 6 +#define MOTOR3_A 5 +#define MOTOR3_B 7 + +#define FORWARD 1 +#define BACKWARD 2 +#define BRAKE 3 +#define RELEASE 4 + + +class AF_DCMotor +{ + public: + AF_DCMotor(uint8_t motornum, uint8_t freq = MOTOR12_8KHZ); + void run(uint8_t); + void setSpeed(uint8_t); + + private: + uint8_t motornum, pwmfreq; +}; + +#endif diff --git a/arduino/gr/asserv/motor.cpp b/arduino/gr/asserv/motor.cpp new file mode 100644 index 0000000..59b9ed4 --- /dev/null +++ b/arduino/gr/asserv/motor.cpp @@ -0,0 +1,62 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 29/11/13 * + ****************************************/ +#include "parameters.h" +#include "AFMotor.h" +#include + +#ifndef PWM_MIN +#define PWM_MIN 0 +#endif + +//Controleur : +//-255:0 : Marche arrière +//0:255 : Marche avant + +AF_DCMotor motor_left(1, MOTOR12_64KHZ); +AF_DCMotor motor_right(2, MOTOR12_64KHZ); + +extern "C" void set_pwm_right(int pwm); +extern "C" void set_pwm_left(int pwm); +extern "C" void get_breaking_speed_factor(float *angular_speed, float *linear_speed);; + +void set_pwm_left(int pwm){ + pwm = -pwm;//les moteurs sont faces à face, pour avancer il faut qu'il tournent dans un sens différent + if (pwm > 0) + pwm += PWM_MIN; + else if (pwm < 0) + pwm -= PWM_MIN; + + if(pwm > 255) + pwm = 255; + else if(pwm < -255) + pwm = -255; + + if(pwm >= 0) + motor_left.run(FORWARD); + else + motor_left.run(BACKWARD); + + motor_left.setSpeed(abs(pwm)); +} + +void set_pwm_right(int pwm){ + if (pwm > 0) + pwm += PWM_MIN; + else if (pwm < 0) + pwm -= PWM_MIN; + + if(pwm > 255) + pwm = 255; + else if(pwm < -255) + pwm = -255; + + if(pwm >= 0) + motor_right.run(FORWARD); + else + motor_right.run(BACKWARD); + + motor_right.setSpeed(abs(pwm)); +} diff --git a/arduino/gr/asserv/motor.h b/arduino/gr/asserv/motor.h new file mode 100644 index 0000000..039e74b --- /dev/null +++ b/arduino/gr/asserv/motor.h @@ -0,0 +1,17 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 31/03/13 * + ****************************************/ +#ifndef MOTOR_H +#define MOTOR_H + +void set_pwm(int side, int pwm); + +inline void MotorsInit(void) {}; + +void set_pwm_left(int pwm); + +void set_pwm_right(int pwm); + +#endif diff --git a/arduino/gr/asserv/parameters.h b/arduino/gr/asserv/parameters.h new file mode 100644 index 0000000..39b22c5 --- /dev/null +++ b/arduino/gr/asserv/parameters.h @@ -0,0 +1,98 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 29/11/13 * + ****************************************/ +#ifndef PARAMETERS_H +#define PARAMETERS_H + +#define BAUDRATE 57600 +#define SERIAL_TYPE SERIAL_8N1 +#define ARDUINO_ID "ard_asserv" +#define DEBUG_TARGET_SPEED 0 +#define DEBUG_MAINLOOP 0 +//todo add an enum with the different debug levels available +#define DEBUG_LEVEL 2 + +/* Simple ou Double ou Quadruple evaluation ? + * La quadruple evaluation utilise 4 interruption par tick + * (une pour chaque changement de valeur des fils A et B) + * + * La double evaluation utilise 2 interruptions par tick + * (une pour chaque changement de valeur du fil A + * + * La simple evaluation utilise 1 interruption par tick + * (front montant du fil A) + * + * Ces trois méthodes sont equivalentes + * La quadruple evaluation est plus précise mais utilise + * plus de puissance processeur + * Il convient de tester la plus adapté selon le processeur + * et le nombre de ticks du codeur + * + * OPTIONS : '1' - '2 - '4' */ + +#define ENCODER_EVAL 2 + +#define USE_SHARP 0 +#define EMERGENCY_STOP_DISTANCE 0.32 // m +#define EMERGENCY_WAIT_TIME 30 // seconds +#define EMERGENCY_SLOW_GO_RATIO 0.5 // spd = 0.3*max_spd in slow_go mode + +#define HZ 100 +#define DT (1.0/HZ) +#define AUTO_STATUS_HZ 20 // must be a divider a HZ or 0 to disable + +#define SPD_MAX 1000 //mm/s 12000 +#define ACC_MAX 3000 //mm/s2 3000 +#define RATIO_ROT_SPD_MAX 0.6 +#define K_DISTANCE_REDUCTION 10 // réduction de la vitesse linéaire quand on tourne + +#define BLOCK_TIME 5000 // ms - time between each block check +#define BLOCK_MIN_DIST 5 // mm - distance to move to consider we moved + +#define ENC_RESOLUTION 1024 //resolution du codeur + +#define ENC_LEFT_RADIUS 32.01 //20.02 //REGLE PAR TEST - rayon de la roue codeuse +#define ENC_RIGHT_RADIUS 32.01 //20.02 //REGLE PAR TEST - rayon de la roue codeuse +#define ENTRAXE_ENC 310.0 //200.0 //REGLE PAR TES - Distance entre chaque roue codeuse en mm + +#define ERROR_ANGLE 0.030 //erreur en angle(radians) maximale pour considérer l'objectif comme atteint +#define ERROR_POS 5 // erreur en position (mm) maximale pour considérer l'objectif comme atteint +#define SPD_TO_STOP 10 + +#define CONE_ALIGNEMENT (M_PI/2.0) // 100 = NEVER + +#define PID_P 1.3 //1.5 +#define PID_I 25 //30 +#define PID_D 0 //5 +#define PID_BIAS 0 +#define PID_I_MAX 150 +#define PID_OUT_MAX 255 + +#define BRK_COEFF 0.0 + +// Control feed-forward, pwm = a*spd + b +#define SPD_TO_PWM_A 0.2 +#define SPD_TO_PWM_B 5 + +#define LEFT_P (PID_P) +#define LEFT_I (PID_I) +#define LEFT_D (PID_D) +#define LEFT_BIAS (PID_BIAS) + +#define RIGHT_P (PID_P) +#define RIGHT_I (PID_I) +#define RIGHT_D (PID_D) +#define RIGHT_BIAS (PID_BIAS) + +#define PID_I_RATIO (1/1000.0) +#define PID_D_RATIO (1/1000.0) + +#define TIME_BETWEEN_ORDERS 0 // s +#define KEEP_LAST_GOAL 0 + +//DEFINES ARDUINO +#define SERIAL_MAIN Serial + +#endif diff --git a/arduino/pr/asserv/brushlessMotor.c b/arduino/pr/asserv/brushlessMotor.c new file mode 100644 index 0000000..218cad5 --- /dev/null +++ b/arduino/pr/asserv/brushlessMotor.c @@ -0,0 +1,73 @@ +//Par Quentin pour UTCoupe2013 01/04/2013 +//Commande de shield arduino brushless by UTCoupe + +#include +#include "brushlessMotor.h" + +#define NO_PWM 127 + +/****************************************** + MOTORS +******************************************/ +/********************************* +DIG1 and DIG2 defined to 0 : +linear adjustment of the speed + +might be configured to smth else in order use speed control +see datasheet of DEC-MODULE-24/2 +***********************************/ + +void BrushlessMotorsInit() { + pinMode(MOTOR1_SPD, OUTPUT); + pinMode(MOTOR1_EN, OUTPUT); + pinMode(MOTOR1_BRK, OUTPUT); + pinMode(MOTOR2_SPD, OUTPUT); + pinMode(MOTOR2_EN, OUTPUT); + pinMode(MOTOR2_BRK, OUTPUT); + + digitalWrite(MOTOR1_BRK, HIGH); + digitalWrite(MOTOR2_BRK, HIGH); + digitalWrite(MOTOR1_EN, LOW); + digitalWrite(MOTOR2_EN, LOW); + + analogWrite(MOTOR1_SPD, NO_PWM); + analogWrite(MOTOR2_SPD, NO_PWM); +} + +void BrushlessMotorSetPwm(int motor_side, int pwm) { + static int last_pwms[2] = {NO_PWM, NO_PWM}; + int *last_pwm; + if (motor_side == MOTOR_LEFT) { + last_pwm = &last_pwms[0]; + } else { + last_pwm = &last_pwms[1]; + } + if (pwm == *last_pwm) { + return; + } + else { + *last_pwm = pwm; + } + switch (motor_side) { + case MOTOR_LEFT:{ + analogWrite(MOTOR1_SPD, pwm); + if (pwm == NO_PWM) { + digitalWrite(MOTOR1_EN,LOW); + } + else { + digitalWrite(MOTOR1_EN,HIGH); + } + break; + } + case MOTOR_RIGHT:{ + analogWrite(MOTOR2_SPD, pwm); + if (pwm == NO_PWM) { + digitalWrite(MOTOR2_EN,LOW); + } + else { + digitalWrite(MOTOR2_EN,HIGH); + } + break; + } + } +} diff --git a/arduino/pr/asserv/brushlessMotor.h b/arduino/pr/asserv/brushlessMotor.h new file mode 100644 index 0000000..b26972e --- /dev/null +++ b/arduino/pr/asserv/brushlessMotor.h @@ -0,0 +1,24 @@ +//Adapted from Adafruit Motor Shield by Quentin C for UTCoupe +//Defined for brushless controler shield designed by UTCoupe +//08/04/2013 + +#ifndef BRUSHLESSMOTOR_H +#define BRUSHLESSMOTOR_H + +#include "parameters.h" +#include "pins.h" + +#define MOTOR_LEFT 1 +#define MOTOR_RIGHT 2 + +#define LEFT_READY_SHIFT 1 +#define RIGHT_READY_SHIFT 2 + +#define LEFT_READY (1< 255) + pwm = 255; + else if(pwm < 0) + pwm = 0; + + BrushlessMotorSetPwm(side, pwm); +} diff --git a/arduino/pr/asserv/motor.h b/arduino/pr/asserv/motor.h new file mode 100644 index 0000000..fa1817f --- /dev/null +++ b/arduino/pr/asserv/motor.h @@ -0,0 +1,28 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 31/03/13 * + ****************************************/ +#ifndef MOTOR_H +#define MOTOR_H + +#include "brushlessMotor.h" + +#define MOTOR_LEFT 1 +#define MOTOR_RIGHT 2 + +void set_pwm(int side, int pwm); + +inline void MotorsInit(void) { + BrushlessMotorsInit(); +} + +inline void set_pwm_left(int pwm){ + set_pwm(MOTOR_LEFT, pwm); +} + +inline void set_pwm_right(int pwm){ + set_pwm(MOTOR_RIGHT, pwm); +} + +#endif diff --git a/arduino/pr/asserv/parameters.h b/arduino/pr/asserv/parameters.h new file mode 100644 index 0000000..13ec0c0 --- /dev/null +++ b/arduino/pr/asserv/parameters.h @@ -0,0 +1,100 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 29/11/13 * + ****************************************/ +#ifndef PARAMETERS_H +#define PARAMETERS_H + +#define BAUDRATE 57600 +#define SERIAL_TYPE SERIAL_8N1 +#define ARDUINO_ID "ard_asserv" +#define DEBUG_TARGET_SPEED 0 +#define DEBUG_MAINLOOP 0 +//todo add an enum with the different debug levels available +#define DEBUG_LEVEL 2 + +/* Simple ou Double ou Quadruple evaluation ? + * La quadruple evaluation utilise 4 interruption par tick + * (une pour chaque changement de valeur des fils A et B) + * + * La double evaluation utilise 2 interruptions par tick + * (une pour chaque changement de valeur du fil A + * + * La simple evaluation utilise 1 interruption par tick + * (front montant du fil A) + * + * Ces trois méthodes sont equivalentes + * La quadruple evaluation est plus précise mais utilise + * plus de puissance processeur + * Il convient de tester la plus adapté selon le processeur + * et le nombre de ticks du codeur + * + * OPTIONS : '1' - '2 - '4' */ + +#define ENCODER_EVAL 1 + +#define USE_SHARP 0 +#define EMERGENCY_STOP_DISTANCE 0.3 // m + +#define HZ 100 +#define DT (1.0/HZ) +#define AUTO_STATUS_HZ 10 // must be a divider a HZ or 0 to disable + +#define SPD_MAX 1000 //mm/s 1000 +#define ACC_MAX 1500 //mm/s2 +#define RATIO_ROT_SPD_MAX 0.6 +#define K_DISTANCE_REDUCTION 20 // réduction de la vitesse linéaire quand on tourne +#define EMERGENCY_WAIT_TIME 30 // seconds +#define EMERGENCY_SLOW_GO_RATIO 0.3 // spd = 0.3*max_spd in slow_go mode + +#define BLOCK_TIME 5000 // ms - time between each block check +#define BLOCK_MIN_DIST 5 // mm - distance to move to consider we moved + +#define ENC_RESOLUTION 1024 //resolution du codeur + +// 31,24 and 31,38 initial +//31,05 and 31,20 +#define ENC_LEFT_RADIUS 31.40 //REGLE PAR TEST - rayon de la roue codeuse (31.38 origin 31.15 good) +#define ENC_RIGHT_RADIUS 31.58 //REGLE PAR TEST - rayon de la roue codeuse (31.24) +#define ENTRAXE_ENC 201.35 //REGLE PAR TES - Distance entre chaque roue codeuse en mm (202.37), base 200,8 + +#define ERROR_ANGLE 0.030 //erreur en angle(radians) maximale pour considérer l'objectif comme atteint +#define ERROR_POS 5 // erreur en position (mm) maximale pour considérer l'objectif comme atteint +#define SPD_TO_STOP 10 + +#define CONE_ALIGNEMENT (M_PI/2.0) // 100 = NEVER + +#define PID_P 0.26 //0.25 +#define PID_I 250.0 //130 +#define PID_D 0.5 //13 +#define PID_BIAS 0 +#define PID_I_MAX 150 +#define PID_OUT_MAX 255 + +#define BRK_COEFF 3.0 + +// Control feed-forward, pwm = a*spd + b +#define SPD_TO_PWM_A 0.15 +#define SPD_TO_PWM_B 5 + +#define LEFT_P (PID_P) +#define LEFT_I (PID_I) +#define LEFT_D (PID_D) +#define LEFT_BIAS (PID_BIAS) + +#define RIGHT_P (PID_P) +#define RIGHT_I (PID_I) +#define RIGHT_D (PID_D) +#define RIGHT_BIAS (PID_BIAS) + +#define PID_I_RATIO (1/1000.0) +#define PID_D_RATIO (1/1000.0) + +#define TIME_BETWEEN_ORDERS 0 // s +#define KEEP_LAST_GOAL 0 + +//DEFINES ARDUINO +#define SERIAL_MAIN Serial + +#endif diff --git a/cross-compilation/.gitignore b/cross-compilation/.gitignore new file mode 100644 index 0000000..462106e --- /dev/null +++ b/cross-compilation/.gitignore @@ -0,0 +1 @@ +generated_install/ diff --git a/cross-compilation/readme.md b/cross-compilation/readme.md new file mode 100644 index 0000000..4d384bb --- /dev/null +++ b/cross-compilation/readme.md @@ -0,0 +1,37 @@ +# Cross-compilation using docker +The goal is to use fast computer to compile our project on slow computer platforms (raspberry pi in our case). The docker image contains the basic setup to compile the project. It may work on Apple MacOS and Microsoft Windows (not tested). + +For example, on a raspberry pi 3, it makes in average 12 minutes to compile on j1 (j2 doesn't work due to RAM limitation). On a laptop (i7-4710HQ), using this technic, it makes 5 minutes in average using j8. + +The main downside is, because x86 doesn't natively support armhf instructions, the arm assembly instructions must be translated to x86 instructions, so why we need the software `qemu`. As a result, compilations are slower than usual on the x86 PC. + +## Requirements: +- Docker installed and configured properly. See [installation instruction for Ubuntu](https://docs.docker.com/install/linux/docker-ce/ubuntu/#install-docker-ce). +- For arm compilation on a x86 system: the apt packages `qemu-user-static` and `binfmt-support`. +- The folder `$UTCOUPE_WORKSPACE` must contains all submodules and unpacked libs required by the ros packages. + +TODO Make install script for ubuntu. + +## How to use +### V1 +The script will build the `Dockerfile` inside the selected folder. +Then, it launches the container with a binding to your `$UTCOUPE_WORKSPACE` folder, expecting the image to launch automaticly the compilation using `catkin_make install`. The generated files will be located in the folder `cross_compilation/generated_install/the_selected_arch`. If you have important files in it, please make sure you backup it before launching the script because it will erase it. + +The script will create a tgz file containing this folder. You can then send it to the robot by using scp, for example if you chose `armv7`: +``` +scp path/to/armv7.tgz user@robotname:~/compiled_binairies +``` +and then in ssh (`user@robotname:~/compiled_binairies`) +``` +tar -xzf armv7.tgz +``` + +In order to use the folder as a ROS environnement you need to do the folowing steps in all your remote terminals (or copy/paste this in the remote .bashrc): +``` +export _CATKIN_SETUP_DIR=/absolute/path/to/armv7 +source path/to/armv7/setup.sh +``` +At this point your ready to use it! + +### V2 +The script will download the last docker image if availlable (not planned yet). diff --git a/cross-compilation/run_compilation.sh b/cross-compilation/run_compilation.sh new file mode 100755 index 0000000..ae7b51c --- /dev/null +++ b/cross-compilation/run_compilation.sh @@ -0,0 +1,42 @@ +#!/bin/bash + +# TODO select architecture +compile_arch="armv7" + +img_tag="utcoupe-ros-kinetic-${compile_arch}" +img_dir="utcoupe-ros-kinetic-${compile_arch}" + +img_ws_root_dir="/utcoupe/coupe18" + +cross_compilation_dir="${UTCOUPE_WORKSPACE}/cross-compilation" +cross_compilation_install_dir="${cross_compilation_dir}/generated_install/${compile_arch}" + +docker build -t ${img_tag} "${cross_compilation_dir}/${img_dir}" + +mkdir -p "${cross_compilation_install_dir}" + +# looks slower than the other method +# docker run \ +# -i \ +# --mount type=bind,source="${UTCOUPE_WOKSPACE}",target="${img_ws_root_dir}" \ +# ${img_tag} + +echo "Generating files in \"${cross_compilation_install_dir}\"" + +docker run \ + -i \ + --mount type=bind,source="${UTCOUPE_WORKSPACE}"/ros_ws/src,target="${img_ws_root_dir}"/ros_ws/src,readonly \ + --mount type=bind,source="${UTCOUPE_WORKSPACE}"/libs,target="${img_ws_root_dir}"/libs,readonly \ + --mount type=bind,source="${cross_compilation_install_dir}",target="${img_ws_root_dir}"/ros_ws/install \ + ${img_tag} +# Makes cmake crash +# --tmpfs "${img_ws_root_dir}"/ros_ws/devel +# --tmpfs "${img_ws_root_dir}"/ros_ws/build + +echo "Creating archive..." +last_directory=$(pwd) +cd "${cross_compilation_install_dir}/../" +tar -czf "${compile_arch}.tgz" "${compile_arch}" +cd "${last_directory}" + +echo "DONE, ENJOY THE CROSS-COMPILED BINARIES!" diff --git a/cross-compilation/utcoupe-ros-kinetic-armv7/Dockerfile b/cross-compilation/utcoupe-ros-kinetic-armv7/Dockerfile new file mode 100644 index 0000000..386f6c0 --- /dev/null +++ b/cross-compilation/utcoupe-ros-kinetic-armv7/Dockerfile @@ -0,0 +1,23 @@ +FROM arm32v7/ros:kinetic-ros-base + +RUN mkdir -p /utcoupe/coupe18/ros_ws/src +ENV UTCOUPE_WORKSPACE /utcoupe/coupe18 +ENV ROS_LANG_DISABLE=genlisp:geneus + +RUN apt-get update +RUN apt-get install wget libsfml-dev libarmadillo-dev sudo -qq -y +RUN apt-get install ros-kinetic-tf2 ros-kinetic-tf2-ros ros-kinetic-rviz ros-kinetic-diagnostic-updater ros-kinetic-roslint ros-kinetic-camera-info-manager ros-kinetic-rosserial-arduino ros-kinetic-rosbridge-suite ros-kinetic-tf2-web-republisher ros-kinetic-serial ros-kinetic-dynamixel-sdk ros-kinetic-rosserial-python ros-kinetic-tf2-geometry-msgs ros-kinetic-urg-c ros-kinetic-urg-node -y -qq +RUN rm -rf /var/lib/apt/lists/* + +RUN mkdir -p /temp/utcoupe_install +WORKDIR /temp/utcoupe_install +RUN wget https://github.com/utcoupe/coupe18/raw/master/scripts/install_external_nodes.sh && chmod +x install_external_nodes.sh +RUN bash -c "source /opt/ros/kinetic/setup.sh && ./install_external_nodes.sh" +RUN rm -rf /tmp/utcoupe* + +WORKDIR $UTCOUPE_WORKSPACE/ros_ws +RUN bash -c "source /opt/ros/kinetic/setup.sh && catkin_make" + + +WORKDIR $UTCOUPE_WORKSPACE/ros_ws +CMD bash -c "echo $UTCOUPE_WORKSPACE && rm -rf build/* devel/* install/* && catkin_make install -DCMAKE_BUILD_TYPE=Release" diff --git a/doc/software_architecture/communication.png b/doc/software_architecture/communication.png new file mode 100644 index 0000000..974f7ce Binary files /dev/null and b/doc/software_architecture/communication.png differ diff --git a/doc/software_architecture/communication.uxf b/doc/software_architecture/communication.uxf new file mode 100644 index 0000000..71eedd1 --- /dev/null +++ b/doc/software_architecture/communication.uxf @@ -0,0 +1,1363 @@ + + + 5 + + UMLPackage + + 655 + 225 + 135 + 90 + + *Scheduler* +-- +ai_node.py +halign=left +valign=top +bg=orange +layer=10 + + + + UMLPackage + + 655 + 350 + 135 + 70 + + *Scripts* +-- +scripts_node.py +halign=left +valign=top +bg=orange +layer=10 + + + + UMLPackage + + 655 + 430 + 135 + 70 + + *Timer* +-- +timer_node.py +halign=left +valign=top +bg=orange +layer=10 + + + + UMLPackage + + 915 + 225 + 120 + 90 + + *Map* +-- +map_node.py +halign=left +valign=top +bg=orange +layer=10 + + + + UMLFrame + + 645 + 200 + 155 + 325 + + ai +-- +bg=light_gray +lt=.. + + + + UMLFrame + + 905 + 200 + 140 + 235 + + memory +-- +bg=light_gray +lt=.. + + + + UMLPackage + + 915 + 365 + 120 + 55 + + *Definitions* +-- +/No nodes./ +No communication. Files +are retrieved thanks to the +ros_retrieve package. +halign=left +valign=top +bg=orange +layer=10 + + + + UMLPackage + + 1215 + 220 + 135 + 70 + + *WebClient* +-- +/Linked with / +/Rosbridge/ +halign=left +valign=top +bg=orange +layer=10 + + + + UMLFrame + + 1205 + 195 + 155 + 270 + + feedback +-- +bg=light_gray +lt=.. + + + + UMLPackage + + 1215 + 300 + 135 + 70 + + *Display* +-- +display_node.py +halign=left +valign=top +bg=magenta +layer=10 + + + + UMLFrame + + 645 + 545 + 470 + 285 + + perception +-- +bg=light_gray +lt=.. + + + + UMLPackage + + 865 + 665 + 85 + 55 + + *CubeFinder* +-- +finder_node.py + +/(2018 Specific)/ +halign=left +valign=top +bg=pink +layer=10 + + + + UMLPackage + + 865 + 595 + 85 + 55 + + *EnemyTracker* +-- +tracker_node.py +halign=left +valign=top +bg=magenta +layer=10 + + + + UMLFrame + + 850 + 565 + 250 + 245 + + recognition +-- +bg=light_gray +lt=.. +layer=2 + + + + + UMLFrame + + 665 + 565 + 150 + 245 + + processing +-- +bg=light_gray +lt=.. +layer=2 + + + + + UMLPackage + + 680 + 595 + 90 + 55 + + *LidarObjects* +-- +lidarobjects_node.py +halign=left +valign=top +bg=orange +layer=10 + + + + UMLPackage + + 865 + 735 + 85 + 55 + + *BallsFinder* +-- +finder_node.py + +/(2018 Specific)/ +halign=left +valign=top +bg=pink +layer=10 + + + + UMLPackage + + 1000 + 595 + 85 + 55 + + *CPRecognizer* +-- +tracker_node.py +/(Recognizes the/ +/construction plan)/ +/(2018 Specific)/ +halign=left +valign=top +bg=pink +layer=10 + + + + UMLPackage + + 1000 + 665 + 85 + 55 + + *CubeFinder* +-- +finder_node.py + +/(2018 Specific)/ +halign=left +valign=top +bg=orange +layer=10 + + + + UMLClass + + 635 + 75 + 80 + 15 + + /Made with Umlet/ +lt=.. + + + + UMLFrame + + 1170 + 565 + 295 + 245 + + navigation +-- +bg=light_gray +lt=.. + + + + UMLPackage + + 1180 + 665 + 125 + 55 + + *pathfinder* +-- +pathfinder_node.cpp +halign=left +valign=top +bg=orange +layer=10 + + + + UMLPackage + + 1330 + 650 + 125 + 55 + + *localizer* +-- +localizer_node.py +halign=left +valign=top +bg=orange +layer=10 + + + + UMLPackage + + 1330 + 585 + 125 + 55 + + *collisions* +-- +collisions_node.py +halign=left +valign=top +bg=orange +layer=10 + + + + UMLPackage + + 1180 + 585 + 125 + 70 + + *navigator* +-- +navigator_node.py +halign=left +valign=top +bg=orange +layer=10 + + + + UMLFrame + + 1150 + 545 + 570 + 285 + + movement +-- +bg=light_gray +lt=.. + + + + UMLPackage + + 1565 + 575 + 125 + 50 + + *actuators* +-- +actuators_node.py +halign=left +valign=top +bg=orange +layer=10 + + + + UMLFrame + + 895 + 900 + 515 + 295 + + drivers +-- +bg=light_gray +lt=.. + + + + UMLFrame + + 915 + 920 + 280 + 265 + + sensors +-- +bg=light_gray +lt=.. +layer=2 + + + + + UMLFrame + + 1240 + 920 + 150 + 265 + + movement +-- +bg=light_gray +lt=.. +layer=2 + + + + + UMLPackage + + 925 + 950 + 125 + 75 + + *lidar* +-- +/urg_node package/ +/for the Hokuyo./ +halign=left +valign=top +bg=orange +layer=10 + + + + UMLPackage + + 1060 + 950 + 125 + 55 + + *imu* +-- +see if package exists +already ? +halign=left +valign=top +bg=orange +layer=10 + + + + UMLPackage + + 925 + 1035 + 125 + 85 + + *camera* +-- +camera_node.py +halign=left +valign=top +bg=orange +layer=10 + + + + UMLPackage + + 1060 + 1015 + 125 + 55 + + *displacement* +-- +see if official node +exists already ? +halign=left +valign=top +bg=orange +layer=10 + + + + UMLPackage + + 1060 + 1080 + 125 + 45 + + *arduino_sensors* +-- +arduinosen_node.py +halign=left +valign=top +bg=orange +layer=10 + + + + UMLPackage + + 1255 + 950 + 125 + 55 + + *ax12* +-- +see if official package +exists already ? +halign=left +valign=top +bg=orange +layer=10 + + + + UMLPackage + + 1255 + 1015 + 125 + 45 + + *arduino_actuators* +-- +arduinoact_node.py +halign=left +valign=top +bg=orange +layer=10 + + + + UMLPackage + + 1255 + 1070 + 125 + 105 + + *arduino_asserv* +-- +arduinoasserv_node.py +halign=left +valign=top +bg=orange +layer=10 + + + + UMLClass + + 665 + 255 + 90 + 15 + + /ai/scheduler/config +layer=15 +bg=green +transparency=0 +halign=left + + + + UMLClass + + 925 + 295 + 105 + 15 + + /memory/map/conditions +layer=15 +transparency=0 +halign=left +bg=green + + + + UMLClass + + 665 + 380 + 110 + 15 + + /ai/scripts/aiconditions +layer=15 +bg=green +transparency=0 + + + + UMLClass + + 665 + 400 + 75 + 15 + + /ai/scripts/script +layer=15 +bg=red +transparency=0 + + + + UMLClass + + 665 + 460 + 65 + 15 + + /game_timer +layer=15 +bg=yellow +transparency=0 +valign=center + + + + UMLClass + + 925 + 255 + 85 + 15 + + /memory/map/get +layer=15 +bg=green +transparency=0 + + + + UMLClass + + 925 + 275 + 85 + 15 + + /memory/map/set +layer=15 +bg=green +transparency=0 + + + + Relation + + 725 + 220 + 95 + 255 + + lt=[v]-[>] +layer=20 +fg=gray + 100.0;30.0;100.0;10.0;170.0;10.0;170.0;490.0;10.0;490.0 + + + Relation + + 745 + 180 + 325 + 215 + + lt=..[DEF] +layer=20 + 580.0;410.0;630.0;410.0;630.0;10.0;20.0;10.0;20.0;110.0 + + + Relation + + 785 + 255 + 150 + 55 + + lt=[>]- +layer=20 +fg=blue + 10.0;10.0;130.0;10.0;130.0;90.0;280.0;90.0 + + + Relation + + 785 + 240 + 150 + 50 + + lt=[>]- +layer=20 +fg=blue + 10.0;10.0;150.0;10.0;150.0;80.0;280.0;80.0 + + + Relation + + 785 + 295 + 150 + 85 + + lt=[>]- +layer=20 +fg=blue + 10.0;150.0;130.0;150.0;130.0;10.0;280.0;10.0 + + + UMLClass + + 665 + 480 + 85 + 15 + + /ai/timer/activate +layer=15 +bg=green +transparency=0 + + + + Relation + + 620 + 300 + 55 + 195 + + lt=[<]- +layer=20 +fg=blue + 70.0;10.0;10.0;10.0;10.0;370.0;90.0;370.0 + + + Relation + + 725 + 345 + 95 + 130 + + lt=[v]-[>] +layer=20 +fg=gray + 100.0;30.0;100.0;10.0;170.0;10.0;170.0;240.0;10.0;240.0 + + + UMLPackage + + 460 + 215 + 115 + 40 + + *Fixed package* +-- +Generic package reusable +year after year. +halign=left +valign=top +bg=orange +layer=10 + + + + UMLPackage + + 460 + 265 + 115 + 40 + + *2018 package* +-- +Package used for UTCoupe +2018-specific features. +halign=left +valign=top +layer=10 +bg=pink + + + + UMLPackage + + 460 + 315 + 115 + 55 + + *Feature package* +-- +Package useful for certain +aplications (robot arm, ...). +Can be freely removed if +building a new robot. +halign=left +valign=top +layer=10 +bg=magenta + + + + UMLPackage + + 490 + 10 + 135 + 140 + + *Scheduler* +-- +ai_node.py + +*Handlers* +- + + + +*Requests (service or topic)* +- + + +halign=left +valign=top +bg=orange +layer=10 + + + + UMLPackage + + 1565 + 660 + 125 + 55 + + *arm* +-- +arm_node.py +halign=left +valign=top +bg=magenta +layer=10 + + + + Relation + + 630 + 170 + 750 + 160 + + lt=[>]- +layer=20 +fg=blue + 1440.0;300.0;1480.0;300.0;1480.0;10.0;10.0;10.0;10.0;180.0;70.0;180.0 + + + UMLClass + + 1225 + 330 + 115 + 15 + + /feedback/display/activate +layer=15 +bg=green +transparency=0 +halign=left + + + + Relation + + 785 + 285 + 450 + 240 + + lt=[>]- +layer=20 +fg=blue + 10.0;10.0;70.0;10.0;70.0;460.0;810.0;460.0;810.0;100.0;880.0;100.0 + + + Relation + + 785 + 270 + 150 + 40 + + lt=[>]- +layer=20 +fg=blue + 10.0;10.0;90.0;10.0;90.0;60.0;280.0;60.0 + + + Relation + + 785 + 300 + 410 + 330 + + lt=[>]- +layer=20 +fg=red + 10.0;10.0;90.0;10.0;90.0;450.0;710.0;450.0;710.0;640.0;800.0;640.0 + + + UMLClass + + 1185 + 615 + 115 + 15 + + /navigation/navigator/goto +layer=15 +bg=red +transparency=0 + + + + Relation + + 630 + 300 + 210 + 115 + + lt=[>]- +layer=20 +fg=red + 320.0;10.0;400.0;10.0;400.0;50.0;10.0;50.0;10.0;210.0;70.0;210.0 + + + Relation + + 785 + 300 + 795 + 320 + + lt=[>]- +layer=20 +fg=red + 10.0;10.0;90.0;10.0;90.0;450.0;1540.0;450.0;1540.0;620.0;1570.0;620.0 + + + UMLClass + + 1260 + 1135 + 40 + 15 + + /odom +layer=15 +bg=yellow +transparency=0 +valign=center + + + + UMLClass + + 1570 + 605 + 85 + 15 + + /actuators/action +layer=15 +bg=red +transparency=0 + + + + Relation + + 785 + 395 + 55 + 15 + + lt=[>]- +layer=20 +fg=red + 10.0;10.0;90.0;10.0 + + + Relation + + 1285 + 530 + 200 + 620 + + lt=[v]-[>] +layer=20 +fg=gray + 10.0;130.0;10.0;10.0;380.0;10.0;380.0;1220.0;30.0;1220.0 + + + Relation + + 785 + 275 + 150 + 120 + + lt=[>]- +layer=20 +fg=blue + 10.0;220.0;150.0;220.0;150.0;10.0;280.0;10.0 + + + Relation + + 835 + 275 + 135 + 350 + + lt=[>]- +layer=20 +fg=blue + 230.0;680.0;250.0;680.0;250.0;520.0;10.0;520.0;10.0;10.0;180.0;10.0 + + + Relation + + 705 + 105 + 40 + 40 + + lt=[needed?].> +fg=red + 40.0;10.0;10.0;60.0 + + + UMLClass + + 930 + 1005 + 85 + 15 + + /drivers/lidar/points +layer=15 +bg=yellow +transparency=0 +valign=center + + + + UMLClass + + 1065 + 985 + 75 + 15 + + /drivers/imu/data +layer=15 +bg=yellow +transparency=0 +valign=center + + + + UMLClass + + 930 + 1080 + 115 + 15 + + /drivers/camera/get_image +layer=15 +transparency=0 +valign=center +bg=green + + + + UMLClass + + 1065 + 1050 + 115 + 15 + + /drivers/displacement/data +layer=15 +bg=yellow +transparency=0 +valign=center + + + + UMLClass + + 1065 + 1105 + 110 + 15 + + /drivers/arduino_sen/data +layer=15 +bg=yellow +transparency=0 +valign=center + + + + UMLClass + + 930 + 1100 + 90 + 15 + + /drivers/camera/feed +layer=15 +bg=yellow +transparency=0 +valign=center + + + + UMLClass + + 930 + 1060 + 70 + 15 + + /drivers/camera +layer=15 +transparency=0 +valign=center +bg=green + + + + UMLClass + + 930 + 985 + 65 + 15 + + /drivers/lidar +layer=15 +transparency=0 +valign=center +bg=green + + + + UMLClass + + 1260 + 985 + 75 + 15 + + /drivers/ax12/set +layer=15 +transparency=0 +valign=center +bg=green + + + + UMLClass + + 1260 + 1040 + 105 + 15 + + /drivers/arduino_act/set +layer=15 +transparency=0 +valign=center +bg=red + + + + UMLClass + + 1260 + 1095 + 105 + 15 + + /drivers/asserv/goto +layer=15 +transparency=0 +valign=center +bg=red + + + + UMLClass + + 1260 + 1115 + 105 + 15 + + /drivers/arduino_act/set +layer=15 +transparency=0 +valign=center +bg=green + + + + Relation + + 1135 + 640 + 135 + 470 + + lt=[<]- +layer=20 +fg=red + 90.0;10.0;10.0;10.0;10.0;490.0;190.0;490.0;190.0;920.0;250.0;920.0 + + + Relation + + 1360 + 590 + 380 + 465 + + lt=[>]- +layer=20 +fg=red + 660.0;10.0;740.0;10.0;740.0;910.0;10.0;910.0 + + + UMLClass + + 1260 + 1155 + 90 + 15 + + /odom/debugvalues +layer=15 +bg=yellow +transparency=0 +valign=center + + + diff --git a/doc/software_architecture/simple_arch.png b/doc/software_architecture/simple_arch.png new file mode 100644 index 0000000..0ebaa2c Binary files /dev/null and b/doc/software_architecture/simple_arch.png differ diff --git a/doc/software_architecture/simple_links.png b/doc/software_architecture/simple_links.png new file mode 100644 index 0000000..f9e2946 Binary files /dev/null and b/doc/software_architecture/simple_links.png differ diff --git a/doc/software_architecture/simple_links.uxf b/doc/software_architecture/simple_links.uxf new file mode 100644 index 0000000..af74cd8 --- /dev/null +++ b/doc/software_architecture/simple_links.uxf @@ -0,0 +1,960 @@ + + + 12 + + UMLClass + + 552 + 312 + 240 + 48 + + scheduler +layer=5 +transparency=0 +valign=center +group=2 +bg=white + + + + UMLFrame + + 528 + 264 + 288 + 264 + + ai +-- +lt=.. +layer=2 + +group=2 +bg=red + + + + UMLClass + + 552 + 384 + 240 + 48 + + scripts +layer=5 +transparency=0 +valign=center +group=2 +bg=white + + + + UMLClass + + 552 + 456 + 240 + 48 + + game_status +layer=5 +transparency=0 +valign=center +group=2 +bg=white + + + + UMLFrame + + 528 + 600 + 288 + 192 + + memory +-- +bg=green +lt=.. +layer=2 + +group=3 + + + + UMLClass + + 552 + 648 + 240 + 48 + + map +layer=5 +bg=white +transparency=0 +valign=center +group=3 + + + + UMLClass + + 552 + 720 + 240 + 48 + + definitions +layer=5 +bg=white +transparency=0 +valign=center +group=3 + + + + UMLFrame + + 168 + 264 + 288 + 264 + + feedback +-- +lt=.. +layer=2 + +bg=pink +group=1 + + + + UMLClass + + 192 + 312 + 240 + 48 + + webclient +layer=5 +bg=white +transparency=0 +valign=center +group=1 + + + + UMLClass + + 192 + 384 + 240 + 48 + + rviz +layer=5 +bg=white +transparency=0 +valign=center +group=1 + + + + UMLClass + + 192 + 456 + 240 + 48 + + simulator +layer=5 +bg=white +transparency=0 +valign=center +group=1 + + + + UMLFrame + + 888 + 264 + 288 + 264 + + navigation +-- +lt=.. +layer=2 + +bg=white +group=5 + + + + UMLClass + + 912 + 312 + 240 + 48 + + navigator +layer=5 +bg=white +transparency=0 +valign=center +group=5 + + + + UMLClass + + 912 + 384 + 240 + 48 + + pathfinder +layer=5 +bg=white +transparency=0 +valign=center +group=5 + + + + UMLClass + + 912 + 456 + 240 + 48 + + collisions +layer=5 +bg=white +transparency=0 +valign=center +group=5 + + + + UMLFrame + + 888 + 600 + 288 + 120 + + movement +-- +lt=.. +layer=2 + +bg=cyan +group=4 + + + + UMLClass + + 912 + 648 + 240 + 48 + + actuators +layer=5 +bg=white +transparency=0 +valign=center +group=4 + + + + UMLFrame + + 1248 + 264 + 288 + 192 + + processing +-- +bg=light_gray +lt=.. +layer=2 + +group=6 + + + + UMLClass + + 1272 + 312 + 240 + 48 + + lidar_objects +layer=5 +bg=white +transparency=0 +valign=center +group=6 + + + + UMLClass + + 1272 + 384 + 240 + 48 + + belt_interpreter +layer=5 +bg=white +transparency=0 +valign=center +group=6 + + + + UMLFrame + + 1248 + 528 + 288 + 336 + + recognition +-- +bg=orange +lt=.. +layer=2 + +group=7 + + + + UMLClass + + 1272 + 576 + 240 + 48 + + localizer +layer=5 +bg=white +transparency=0 +valign=center +group=7 + + + + UMLClass + + 1272 + 648 + 240 + 48 + + enemy_tracker +layer=5 +bg=white +transparency=0 +valign=center +group=7 + + + + UMLClass + + 1272 + 720 + 240 + 48 + + cube_finder +layer=5 +bg=white +transparency=0 +valign=center +group=7 + + + + UMLClass + + 1272 + 792 + 240 + 48 + + cp_recognizer +layer=5 +bg=white +transparency=0 +valign=center +group=7 + + + + UMLFrame + + 1608 + 264 + 288 + 696 + + drivers +-- +bg=gray +lt=.. +layer=2 + +group=8 + + + + UMLClass + + 1632 + 312 + 240 + 48 + + lidar +layer=5 +bg=white +transparency=0 +valign=center +group=8 + + + + UMLClass + + 1632 + 384 + 240 + 48 + + ax12 +layer=5 +bg=white +transparency=0 +valign=center +group=8 + + + + UMLClass + + 1632 + 456 + 240 + 48 + + ard_others +layer=5 +bg=white +transparency=0 +valign=center +group=8 + + + + UMLClass + + 1632 + 528 + 240 + 48 + + ard_asserv +layer=5 +bg=white +transparency=0 +valign=center +group=8 + + + + UMLClass + + 1632 + 672 + 240 + 48 + + displacement +layer=5 +bg=white +transparency=0 +valign=center +group=8 + + + + UMLClass + + 1632 + 744 + 240 + 48 + + camera +layer=5 +bg=white +transparency=0 +valign=center +group=8 + + + + UMLClass + + 1632 + 816 + 240 + 48 + + pico_flexx +layer=5 +bg=white +transparency=0 +valign=center +group=8 + + + + UMLClass + + 1632 + 888 + 240 + 48 + + ard_hmi +layer=5 +bg=white +transparency=0 +valign=center +group=8 + + + + UMLClass + + 1632 + 600 + 240 + 48 + + imu +layer=5 +bg=white +transparency=0 +valign=center +group=8 + + + + UMLFrame + + 144 + 216 + 1776 + 768 + + Architecture ROS - connexions principales - coupe18 +bg=light_gray + + + + Relation + + 564 + 348 + 36 + 60 + + layer=10 +fg=black + 10.0;10.0;10.0;30.0 + + + Relation + + 528 + 336 + 48 + 156 + + layer=10 +fg=black + 20.0;10.0;10.0;10.0;10.0;110.0;20.0;110.0 + + + Relation + + 780 + 336 + 72 + 348 + + layer=10 +fg=red + 10.0;10.0;40.0;10.0;40.0;270.0;10.0;270.0 + + + Relation + + 780 + 312 + 156 + 36 + + layer=10 +fg=red + 10.0;10.0;110.0;10.0 + + + Relation + + 924 + 348 + 36 + 60 + + layer=10 + 10.0;10.0;10.0;30.0 + + + Relation + + 888 + 336 + 48 + 156 + + layer=10 + 20.0;10.0;10.0;10.0;10.0;110.0;20.0;110.0 + + + Relation + + 780 + 324 + 156 + 360 + + layer=10 +fg=red + 10.0;10.0;50.0;10.0;50.0;280.0;110.0;280.0 + + + Relation + + 504 + 480 + 1152 + 444 + + layer=10 +fg=red + 40.0;10.0;10.0;10.0;10.0;350.0;940.0;350.0 + + + Relation + + 1500 + 312 + 156 + 36 + + layer=10 + 10.0;10.0;110.0;10.0 + + + Relation + + 1140 + 384 + 516 + 300 + + layer=10 +fg=blue + 10.0;230.0;50.0;230.0;50.0;80.0;380.0;80.0;380.0;10.0;410.0;10.0 + + + Relation + + 1140 + 480 + 516 + 216 + + layer=10 +fg=blue + 10.0;160.0;60.0;160.0;60.0;10.0;410.0;10.0 + + + Relation + + 1500 + 408 + 156 + 84 + + layer=10 + 10.0;10.0;40.0;10.0;40.0;50.0;110.0;50.0 + + + Relation + + 1140 + 312 + 516 + 252 + + layer=10 +fg=blue + 10.0;10.0;40.0;10.0;40.0;160.0;380.0;160.0;380.0;190.0;410.0;190.0 + + + Relation + + 1140 + 396 + 156 + 96 + + layer=10 +fg=gray + 10.0;60.0;60.0;60.0;60.0;10.0;110.0;10.0 + + + Relation + + 1500 + 576 + 156 + 60 + + layer=10 + 10.0;10.0;60.0;10.0;60.0;30.0;110.0;30.0 + + + Relation + + 1500 + 588 + 156 + 120 + + layer=10 + 10.0;10.0;50.0;10.0;50.0;80.0;110.0;80.0 + + + Relation + + 1500 + 768 + 156 + 60 + + layer=10 + 10.0;30.0;50.0;30.0;50.0;10.0;110.0;10.0 + + + Relation + + 1500 + 600 + 156 + 252 + + layer=10 + 10.0;10.0;40.0;10.0;40.0;190.0;110.0;190.0 + + + Relation + + 1212 + 312 + 84 + 384 + + layer=10 +fg=gray + 50.0;300.0;10.0;300.0;10.0;10.0;50.0;10.0 + + + Relation + + 1224 + 408 + 72 + 276 + + layer=10 +fg=gray + 40.0;210.0;10.0;210.0;10.0;10.0;40.0;10.0 + + + Relation + + 1212 + 312 + 84 + 444 + + layer=10 +fg=gray + 50.0;350.0;10.0;350.0;10.0;10.0;50.0;10.0 + + + Relation + + 780 + 720 + 72 + 36 + + layer=10 +fg=black + 10.0;10.0;40.0;10.0 + + + Relation + + 780 + 732 + 72 + 36 + + layer=10 +fg=black + 10.0;10.0;40.0;10.0 + + + Relation + + 780 + 744 + 72 + 36 + + layer=10 +fg=black + 10.0;10.0;40.0;10.0 + + + Relation + + 420 + 312 + 72 + 36 + + layer=10 +fg=black + 10.0;10.0;40.0;10.0 + + + Relation + + 420 + 324 + 72 + 36 + + layer=10 +fg=black + 10.0;10.0;40.0;10.0 + + + Relation + + 420 + 336 + 72 + 36 + + layer=10 +fg=black + 10.0;10.0;40.0;10.0 + + + Relation + + 420 + 384 + 72 + 36 + + layer=10 +fg=black + 10.0;10.0;40.0;10.0 + + + Relation + + 420 + 396 + 72 + 36 + + layer=10 +fg=black + 10.0;10.0;40.0;10.0 + + + Relation + + 420 + 408 + 72 + 36 + + layer=10 +fg=black + 10.0;10.0;40.0;10.0 + + + Relation + + 780 + 672 + 516 + 108 + + layer=10 +fg=orange + 10.0;10.0;80.0;10.0;80.0;70.0;410.0;70.0 + + + Relation + + 1140 + 324 + 156 + 168 + + layer=10 +fg=gray + 10.0;120.0;60.0;120.0;60.0;10.0;110.0;10.0 + + diff --git a/doc/software_architecture/summup.png b/doc/software_architecture/summup.png new file mode 100644 index 0000000..b5fc533 Binary files /dev/null and b/doc/software_architecture/summup.png differ diff --git a/doc/software_architecture/summup.uxf b/doc/software_architecture/summup.uxf new file mode 100644 index 0000000..54aaa16 --- /dev/null +++ b/doc/software_architecture/summup.uxf @@ -0,0 +1,548 @@ + + + 11 + + UMLClass + + 660 + 330 + 220 + 44 + + scheduler +layer=5 +bg=white +transparency=0 +valign=center + + + + UMLFrame + + 638 + 286 + 264 + 242 + + ai +-- +lt=.. +layer=2 + +bg=red + + + + UMLClass + + 660 + 396 + 220 + 44 + + scripts +layer=5 +bg=white +transparency=0 +valign=center + + + + UMLClass + + 660 + 462 + 220 + 44 + + game_status +layer=5 +bg=white +transparency=0 +valign=center + + + + UMLFrame + + 638 + 550 + 264 + 176 + + memory +-- +bg=green +lt=.. +layer=2 + + + + + UMLClass + + 660 + 594 + 220 + 44 + + memory +layer=5 +bg=white +transparency=0 +valign=center + + + + UMLClass + + 660 + 660 + 220 + 44 + + definitions +layer=5 +bg=white +transparency=0 +valign=center + + + + UMLFrame + + 352 + 286 + 264 + 242 + + feedback +-- +lt=.. +layer=2 + +bg=pink + + + + UMLClass + + 374 + 330 + 220 + 44 + + webclient +layer=5 +bg=white +transparency=0 +valign=center + + + + UMLClass + + 374 + 396 + 220 + 44 + + rviz +layer=5 +bg=white +transparency=0 +valign=center + + + + UMLClass + + 374 + 462 + 220 + 44 + + simulator +layer=5 +bg=white +transparency=0 +valign=center + + + + UMLFrame + + 924 + 286 + 264 + 242 + + navigation +-- +lt=.. +layer=2 + +bg=blue + + + + UMLClass + + 946 + 330 + 220 + 44 + + navigator +layer=5 +bg=white +transparency=0 +valign=center + + + + UMLClass + + 946 + 396 + 220 + 44 + + pathfinder +layer=5 +bg=white +transparency=0 +valign=center + + + + UMLClass + + 946 + 462 + 220 + 44 + + collisions +layer=5 +bg=white +transparency=0 +valign=center + + + + UMLFrame + + 924 + 550 + 264 + 110 + + movement +-- +lt=.. +layer=2 + +bg=cyan + + + + UMLClass + + 946 + 594 + 220 + 44 + + actuators +layer=5 +bg=white +transparency=0 +valign=center + + + + UMLFrame + + 1210 + 286 + 264 + 176 + + processing +-- +bg=orange +lt=.. +layer=2 + + + + + UMLClass + + 1232 + 330 + 220 + 44 + + lidar_objects +layer=5 +bg=white +transparency=0 +valign=center + + + + UMLClass + + 1232 + 396 + 220 + 44 + + belt_interpreter +layer=5 +bg=white +transparency=0 +valign=center + + + + UMLFrame + + 1210 + 484 + 264 + 308 + + recognition +-- +bg=yellow +lt=.. +layer=2 + + + + + UMLClass + + 1232 + 528 + 220 + 44 + + localizer +layer=5 +bg=white +transparency=0 +valign=center + + + + UMLClass + + 1232 + 594 + 220 + 44 + + enemy_tracker +layer=5 +bg=white +transparency=0 +valign=center + + + + UMLClass + + 1232 + 660 + 220 + 44 + + cube_finder +layer=5 +bg=white +transparency=0 +valign=center + + + + UMLClass + + 1232 + 726 + 220 + 44 + + cp_recognizer +layer=5 +bg=white +transparency=0 +valign=center + + + + UMLFrame + + 1496 + 286 + 264 + 638 + + drivers +-- +bg=gray +lt=.. +layer=2 + + + + + UMLClass + + 1518 + 330 + 220 + 44 + + lidar +layer=5 +bg=white +transparency=0 +valign=center + + + + UMLClass + + 1518 + 396 + 220 + 44 + + imu +layer=5 +bg=white +transparency=0 +valign=center + + + + UMLClass + + 1518 + 462 + 220 + 44 + + displacement +layer=5 +bg=white +transparency=0 +valign=center + + + + UMLClass + + 1518 + 528 + 220 + 44 + + camera +layer=5 +bg=white +transparency=0 +valign=center + + + + UMLClass + + 1518 + 660 + 220 + 44 + + ax12 +layer=5 +bg=white +transparency=0 +valign=center + + + + UMLClass + + 1518 + 726 + 220 + 44 + + ard_asserv +layer=5 +bg=white +transparency=0 +valign=center + + + + UMLClass + + 1518 + 792 + 220 + 44 + + ard_others +layer=5 +bg=white +transparency=0 +valign=center + + + + UMLClass + + 1518 + 858 + 220 + 44 + + ard_hmi +layer=5 +bg=white +transparency=0 +valign=center + + + + UMLClass + + 1518 + 594 + 220 + 44 + + pico_flexx +layer=5 +bg=white +transparency=0 +valign=center + + + + UMLFrame + + 330 + 242 + 1452 + 704 + + Architecture ROS - coupe18 + + + diff --git a/doc/software_architecture/system_init_workflow.jpg b/doc/software_architecture/system_init_workflow.jpg new file mode 100644 index 0000000..d631408 Binary files /dev/null and b/doc/software_architecture/system_init_workflow.jpg differ diff --git a/libs/.gitignore b/libs/.gitignore new file mode 100644 index 0000000..ce0d3dd --- /dev/null +++ b/libs/.gitignore @@ -0,0 +1,7 @@ +# Ignore all files... +* + +#..other than archive +!.gitignore +!*.tgz +!*.tar.gz diff --git a/libs/arduino-cmake.tgz b/libs/arduino-cmake.tgz new file mode 100644 index 0000000..19f4be0 Binary files /dev/null and b/libs/arduino-cmake.tgz differ diff --git a/libs/arduino-libraries.tgz b/libs/arduino-libraries.tgz new file mode 100644 index 0000000..198f30e Binary files /dev/null and b/libs/arduino-libraries.tgz differ diff --git a/ros_ws/.gitignore b/ros_ws/.gitignore new file mode 100644 index 0000000..0ad6fe6 --- /dev/null +++ b/ros_ws/.gitignore @@ -0,0 +1,164 @@ +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python +env/ +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +install/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +*.egg-info/ +.installed.cfg +*.egg + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +.hypothesis/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# pyenv +.python-version + +# celery beat schedule file +celerybeat-schedule + +# SageMath parsed files +*.sage.py + +# dotenv +.env + +# virtualenv +.venv +venv/ +ENV/ + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ + + +# ROS GITIGNORE +build/ +devel/ +bin/ +lib/ +msg_gen/ +srv_gen/ +msg/*Action.msg +msg/*ActionFeedback.msg +msg/*ActionGoal.msg +msg/*ActionResult.msg +msg/*Feedback.msg +msg/*Goal.msg +msg/*Result.msg +msg/_*.py +.catkin_workspace + +# Generated by dynamic reconfigure +*.cfgc +/cfg/cpp/ +/cfg/*.py + +# Ignore generated docs +*.dox +*.wikidoc + +# eclipse stuff +.project +.cproject + +# qcreator stuff +CMakeLists.txt.user + +srv/_*.py +*.pcd +*.pyc +qtcreator-* +*.user + +/planning/cfg +/planning/docs +/planning/src + +*~ + +# Emacs +.#* + +# KDevelop 4 +*.kdev4 +.kdev4/ + +# Catkin custom files +CATKIN_IGNORE + + +#CUSTOM ADDS +nohup.out +*.swp + diff --git a/ros_ws/src/ai_game_manager/CMakeLists.txt b/ros_ws/src/ai_game_manager/CMakeLists.txt new file mode 100644 index 0000000..23d1476 --- /dev/null +++ b/ros_ws/src/ai_game_manager/CMakeLists.txt @@ -0,0 +1,114 @@ +cmake_minimum_required(VERSION 2.8.3) +project(ai_game_manager) + +add_compile_options(-std=c++14) + +find_package(catkin REQUIRED COMPONENTS + rospy + roscpp + std_msgs + ai_scheduler + message_generation +) + +catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +add_message_files( + FILES + GameStatus.msg + NodesStatus.msg + ArmRequest.msg + + GameTime.msg +) + +add_service_files( + FILES + SetStatus.srv + NodeReady.srv + + SetTimer.srv + Delay.srv +) + +generate_messages( + DEPENDENCIES + std_msgs +) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +################################### +## catkin specific configuration ## +################################### + +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS message_runtime + LIBRARIES ${PROJECT_NAME} +) + +########### +## Build ## +########### + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +link_directories(${catkin_LIBRARY_DIRS}) + +add_library( + ${PROJECT_NAME} + src/init_service.cpp +) +target_link_libraries( + ${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +add_dependencies( + ${PROJECT_NAME} + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${${PROJECT_NAME}_generate_messages_cpp} + ${catkin_EXPORTED_TARGETS} +) + +############# +## Install ## +############# + +install( + TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +install( + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + PATTERN "*.pyc" EXCLUDE +) + +catkin_install_python( + PROGRAMS src/game_manager_node.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install( + DIRECTORY src/game_manager + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + PATTERN "*.pyc" EXCLUDE +) + +############# +## Testing ## +############# diff --git a/ros_ws/src/ai_game_manager/include/ai_game_manager/__init__.py b/ros_ws/src/ai_game_manager/include/ai_game_manager/__init__.py new file mode 100644 index 0000000..bbe9c89 --- /dev/null +++ b/ros_ws/src/ai_game_manager/include/ai_game_manager/__init__.py @@ -0,0 +1 @@ +from init_service import * diff --git a/ros_ws/src/ai_game_manager/include/ai_game_manager/init_service.h b/ros_ws/src/ai_game_manager/include/ai_game_manager/init_service.h new file mode 100644 index 0000000..804290c --- /dev/null +++ b/ros_ws/src/ai_game_manager/include/ai_game_manager/init_service.h @@ -0,0 +1,60 @@ +#ifndef AI_GAME_STATUS_INIT_SERVICE_H +#define AI_GAME_STATUS_INIT_SERVICE_H + +#include + +#include "ai_game_manager/GameStatus.h" +#include "ai_game_manager/ArmRequest.h" +#include "ai_game_manager/NodeReady.h" + +#include + +/** + * @brief Main class of the lib `ai_game_manager`. + * It implements most of the python script `init_service.py` does. + */ +class StatusServices +{ +public: + /** @brief Type of the callback for the arm request */ + using ArmCallback_t = std::function; + /** @brief Type of the callback for the status event */ + using StatusCallback_t = std::function; + + /** + * @brief Initialize the status service and saves the callbacks for the future requests and events. + * + * Creates the service `(nodename)/arm` that will be called by `ai_game_manager` main node when all the node in our network must arm. + * It subscribes also to the topic `/ai/game_manager/status`. To avoid any compatibility problems between std C++ and boost, statusCallback is not directly connected, it will be called through _on_arm member function. + * + * @param namespaceName The name of the node namespace. + * @param packageName The name of the package or node. + * @param armCallback Callback called when a resquest to arm the node has been received. The value returned by the callback is send as a response. + * @param statusCallback Callback called when there is an update on the status topic. + */ + StatusServices(const std::string& namespaceName, const std::string& packageName, ArmCallback_t armCallback = nullptr, StatusCallback_t statusCallback = nullptr); + + /** + * @brief Notify the `ai/game_manager` main node that the current node tried to launch and say if it succeeded. + * + * Calls the service `/ai/game_manager/node_ready` with the node name and the success boolean as parameters. + * + * @param success Indicates if the node succeeded to start and initialize. + */ + void setReady(bool success); + +private: + enum class Errors {SERVICE_TIMEOUT, SERVICE_NOT_RESPONDING}; + + std::string _nodeName; + ArmCallback_t _armCallback; + StatusCallback_t _statusCallback; + ros::NodeHandle _nodeHandle; + ros::Subscriber _armServer; + ros::Subscriber _gameStatusSubscriber; + + void _on_arm(const ai_game_manager::ArmRequest::ConstPtr& msg); + void _on_gameStatus(const ai_game_manager::GameStatus::ConstPtr& msg); +}; + +#endif // AI_GAME_STATUS_INIT_SERVICE_H diff --git a/ros_ws/src/ai_game_manager/include/ai_game_manager/init_service.py b/ros_ws/src/ai_game_manager/include/ai_game_manager/init_service.py new file mode 100644 index 0000000..0d4a253 --- /dev/null +++ b/ros_ws/src/ai_game_manager/include/ai_game_manager/init_service.py @@ -0,0 +1,53 @@ +#!/usr/bin/python +import rospy + +''' +Imported from ai/game_manager. +''' + +from ai_game_manager.msg import GameStatus, ArmRequest +from ai_game_manager.srv import NodeReady + + +class StatusServices(object): + READY_SRV = "/ai/game_manager/node_ready" # Service to call when the node has finished its init phase (successful or not). + ARM_SRV = "/ai/game_manager/arm" # Server the node can use if it needs to be calibrated at one point (called by scheduler before jack) + HALT_SRV = "/ai/game_manager/status" # Topic that can be used to know when HALT is activated (if the node needs to be stopped). + + def __init__(self, namespace, packagename, arm_cb = None, status_cb = None): + self.node_name = "/{}/{}".format(namespace, packagename) + if arm_cb: rospy.Subscriber(self.ARM_SRV, ArmRequest, arm_cb) + if status_cb: rospy.Subscriber(self.HALT_SRV, GameStatus, status_cb) + + def ready(self, success): + try: + rospy.wait_for_service(self.READY_SRV, timeout = 4.0) + _ready_pub = rospy.ServiceProxy(self.READY_SRV, NodeReady) + _ready_pub(self.node_name, success) + + if success: rospy.loginfo("Node '{}' initialized successfully.".format(self.node_name)) + else: rospy.logerr( "Node '{}' didn't initialize correctly.".format(self.node_name)) + except: + rospy.logerr("status_services couldn't contact ai/game_manager to send init notification.") + + +''' +EXAMPLE USAGE + +def on_arm(): + # ... calibration ... + return True # True if arm successful, False otherwise. + +def on_status(req): + if req.game_status == req.STATUS_HALT: + pass # ... e.g. stop updating TFs ... + +def start(): + rospy.init_node("nodename", log_level=rospy.INFO) + self.status_services = StatusServices("recognition", "localizer", on_arm, on_status) + # ... initialization ... + self.status_services.ready(True) # True if init successful, False otherwise. + +if __name__ == "__main__": + start() +''' diff --git a/ros_ws/src/ai_game_manager/msg/ArmRequest.msg b/ros_ws/src/ai_game_manager/msg/ArmRequest.msg new file mode 100644 index 0000000..747f8c1 --- /dev/null +++ b/ros_ws/src/ai_game_manager/msg/ArmRequest.msg @@ -0,0 +1 @@ +# No arguments. \ No newline at end of file diff --git a/ros_ws/src/ai_game_manager/msg/GameEnd.msg b/ros_ws/src/ai_game_manager/msg/GameEnd.msg new file mode 100644 index 0000000..e43ed38 --- /dev/null +++ b/ros_ws/src/ai_game_manager/msg/GameEnd.msg @@ -0,0 +1,7 @@ +# Not used, use that for general halt ? or keep GameStatus for everything ? + +# # All nodes that need to stop after a general HALT (game ended, critical problem) +# # will listen to this notification. +# uint8 REASON_TIMER_END = 0 +# uint8 REASON_CRITICAL_HALT = 1 +# uint8 reason \ No newline at end of file diff --git a/ros_ws/src/ai_game_manager/msg/GameStatus.msg b/ros_ws/src/ai_game_manager/msg/GameStatus.msg new file mode 100644 index 0000000..2d6d239 --- /dev/null +++ b/ros_ws/src/ai_game_manager/msg/GameStatus.msg @@ -0,0 +1,9 @@ +uint8 STATUS_INIT = 0 # Initializing, initialized of waiting for scheduler start command. +uint8 STATUS_INGAME = 1 # Scheduler was activated and set the game to start. Independent from timer. +uint8 STATUS_HALT = 2 # A node asked for a general HALT or gae ended (timer finished). +uint8 game_status + +uint8 INIT_INITIALIZING = 0 # All nodes didn't respond and we didn't reach the init timeout yet. +uint8 INIT_INITIALIZED = 1 # All nodes responded successfully and are initialized. +uint8 INIT_FAILED = 2 # Nodes responded false or didn't respond after before the init timeout. +uint8 init_status \ No newline at end of file diff --git a/ros_ws/src/ai_game_manager/msg/GameTime.msg b/ros_ws/src/ai_game_manager/msg/GameTime.msg new file mode 100644 index 0000000..686bf39 --- /dev/null +++ b/ros_ws/src/ai_game_manager/msg/GameTime.msg @@ -0,0 +1,5 @@ +# Timer related statistics +bool is_active +float32 game_time_duration +float32 game_elapsed_time +float32 game_time_left \ No newline at end of file diff --git a/ros_ws/src/ai_game_manager/msg/NodesStatus.msg b/ros_ws/src/ai_game_manager/msg/NodesStatus.msg new file mode 100644 index 0000000..8508928 --- /dev/null +++ b/ros_ws/src/ai_game_manager/msg/NodesStatus.msg @@ -0,0 +1,3 @@ +string[] pending_nodes # Node names that didn't notify they are init or not yet. +string[] ready_nodes # Node names that responded they initialized correctly. +string[] failed_nodes # Node names that responded they couldn't init. \ No newline at end of file diff --git a/ros_ws/src/ai_game_manager/package.xml b/ros_ws/src/ai_game_manager/package.xml new file mode 100644 index 0000000..749077c --- /dev/null +++ b/ros_ws/src/ai_game_manager/package.xml @@ -0,0 +1,18 @@ + + + ai_game_manager + 0.0.1 + The ai_game_manager package + GPLv3 + MadeInPierre + Mindstan + UTCoupe + + catkin + ai_scheduler + message_generation + rospy + roscpp + std_msgs + message_runtime + diff --git a/ros_ws/src/ai_game_manager/setup.py b/ros_ws/src/ai_game_manager/setup.py new file mode 100644 index 0000000..00f9b3b --- /dev/null +++ b/ros_ws/src/ai_game_manager/setup.py @@ -0,0 +1,11 @@ +## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=['ai_game_manager'], + package_dir={'': 'include'}) + +setup(**setup_args) diff --git a/ros_ws/src/ai_game_manager/src/game_manager/__init__.py b/ros_ws/src/ai_game_manager/src/game_manager/__init__.py new file mode 100644 index 0000000..45ac205 --- /dev/null +++ b/ros_ws/src/ai_game_manager/src/game_manager/__init__.py @@ -0,0 +1,3 @@ +from status_config import Status +from status_manager import StatusManager +from timer import TimerManager diff --git a/ros_ws/src/ai_game_manager/src/game_manager/status_config.py b/ros_ws/src/ai_game_manager/src/game_manager/status_config.py new file mode 100644 index 0000000..df2d9d4 --- /dev/null +++ b/ros_ws/src/ai_game_manager/src/game_manager/status_config.py @@ -0,0 +1,37 @@ +class Status(): + STATUS_INIT = 0 # All nodes initializing, didn't respond yet. + STATUS_INGAME = 1 # Scheduler started, doing its job. + STATUS_HALT = 2 # Robot stopped (game end, critical HALT requested by a node...) + + INIT_INITIALIZING = 0 # All nodes didn't respond and we didn't reach the init timeout yet. + INIT_INITIALIZED = 1 # All nodes responded successfully and are initialized. + INIT_FAILED = 2 # Nodes responded false or didn't respond after before the init timeout. + + INIT_CHECKLIST = { # Please comment the lines instead of deleting them. + "/ai/scheduler": None, + + "/memory/map": None, + "/memory/definitions": None, + + "/navigation/navigator": None, + "/navigation/pathfinder": None, + "/navigation/collisions": None, + + "/movement/actuators": None, + + "/recognition/localizer": None, + "/recognition/enemy_tracker": None, + # "/recognition/cube_finder": None, + # "/recognition/cp_recognizer": None, + "/recognition/objects_classifier": None, + + "/processing/belt_interpreter": None, + # "/processing/lidar_objects": None, + + "/drivers/ard_asserv": None, + #"/drivers/ard_hmi": None, + #"/drivers/ard_others": None, + "/drivers/port_finder": None, + "/drivers/ax12": None, + "/drivers/teraranger": None, + } diff --git a/ros_ws/src/ai_game_manager/src/game_manager/status_manager.py b/ros_ws/src/ai_game_manager/src/game_manager/status_manager.py new file mode 100644 index 0000000..277dcfc --- /dev/null +++ b/ros_ws/src/ai_game_manager/src/game_manager/status_manager.py @@ -0,0 +1,91 @@ +import time, rospy + +from ai_game_manager.msg import GameStatus, NodesStatus, ArmRequest +from ai_game_manager.srv import SetStatus, SetStatusResponse, NodeReady, NodeReadyResponse +from drivers_ard_hmi.msg import HMIEvent + +from status_config import Status + +class StatusManager(): + INIT_TIMEOUT = 40 # seconds to wait for the nodes to send their init response before timeout. + + def __init__(self): + self._node_ready_notif = rospy.Service("/ai/game_manager/node_ready", NodeReady, self.on_node_ready) + self._set_status_srv = rospy.Service("/ai/game_manager/set_status", SetStatus, self.on_set_status) + self._game_status_pub = rospy.Publisher("/ai/game_manager/status", GameStatus, queue_size = 10) + self._arm_pub = rospy.Publisher("/ai/game_manager/arm", ArmRequest, queue_size = 1) + self._nodes_status_pub = rospy.Publisher("/ai/game_manager/nodes_status", NodesStatus, queue_size = 10) + + rospy.Subscriber("/feedback/ard_hmi/hmi_event", HMIEvent, self.on_hmi_event) + + self.game_status = Status.STATUS_INIT + self.init_status = Status.INIT_INITIALIZING + + self._init_start_time = time.time() + + def update(self): + if self.init_status == Status.INIT_INITIALIZING: + self.check_init_checklist() + if time.time() - self._init_start_time > self.INIT_TIMEOUT: + rospy.logdebug("Waited %d seconds" % self.INIT_TIMEOUT) + if len([n for n in Status.INIT_CHECKLIST if Status.INIT_CHECKLIST[n] in [None, False]]) > 0: + self.set_init_status(Status.INIT_FAILED) + else: + self.set_init_status(Status.INIT_INITIALIZED) + self.publish_statuses() # publish game status at 5Hz. + + def set_init_status(self, new_status): + if new_status == Status.INIT_INITIALIZING: + rospy.loginfo("game_status set to INITIALIZING.") + elif new_status == Status.INIT_INITIALIZED: + rospy.loginfo("All nodes initialized successfully, ready to start !") + elif new_status == Status.INIT_FAILED: + rospy.logerr("Init timeout reached, certain nodes failed ({}) or didn't respond ({}) ! " + "System will continue, but be careful..".format( + ', '.join([n for n in Status.INIT_CHECKLIST if Status.INIT_CHECKLIST[n] == False]), + ', '.join([n for n in Status.INIT_CHECKLIST if Status.INIT_CHECKLIST[n] == None]))) + self.init_status = new_status + + def publish_statuses(self): + m = GameStatus() + m.game_status = self.game_status + m.init_status = self.init_status + self._game_status_pub.publish(m) + + m = NodesStatus() + for node in Status.INIT_CHECKLIST: + if Status.INIT_CHECKLIST[node] is None: + m.pending_nodes.append(node) + elif Status.INIT_CHECKLIST[node] is True: + m.ready_nodes.append(node) + elif Status.INIT_CHECKLIST[node] is False: + m.failed_nodes.append(node) + self._nodes_status_pub.publish(m) + + def check_init_checklist(self): + for node in Status.INIT_CHECKLIST: + if Status.INIT_CHECKLIST[node] in [None, False]: + return + if self.init_status == Status.INIT_INITIALIZING: + self.set_init_status(Status.INIT_INITIALIZED) + else: + rospy.logerr("Unexpected behaviour : init_status checklist got full but status is not INITIALIZING.") + + def on_node_ready(self, msg): + if msg.node_name in Status.INIT_CHECKLIST: + Status.INIT_CHECKLIST[msg.node_name] = msg.success + return NodeReadyResponse() + else: + rospy.logwarn("Node name '{}' not in ai/game_manager init checklist, passing.".format(msg.node_name)) + return NodeReadyResponse() + + def on_set_status(self, req): + self.game_status = req.new_game_status + return SetStatusResponse(True) + + def on_hmi_event(self, req): + if req.event == req.EVENT_START: + rospy.loginfo("Received HMI event, publishing arm request") + self._arm_pub.publish(ArmRequest()) + elif req.event == req.EVENT_GAME_CANCEL: + self.game_status = GameStatus.STATUS_HALT \ No newline at end of file diff --git a/ros_ws/src/ai_game_manager/src/game_manager/timer.py b/ros_ws/src/ai_game_manager/src/game_manager/timer.py new file mode 100644 index 0000000..446b6fe --- /dev/null +++ b/ros_ws/src/ai_game_manager/src/game_manager/timer.py @@ -0,0 +1,106 @@ +import time, rospy + +from ai_game_manager.msg import GameStatus +from ai_game_manager.srv import SetStatus +from ai_game_manager import StatusServices + +from ai_game_manager.srv import SetTimer, SetTimerResponse, Delay, DelayResponse +from ai_game_manager.msg import GameTime + +class Status(): + STATUS_INIT = 0 + STATUS_INGAME = 1 + STATUS_HALT = 2 + + +class Timer(): + def __init__(self): + self.game_duration = -1 # Holds the match duration. + self.started = False # Set to true when Timer is active. + + self._start_time = -1 + + def reset(self): + self._start_time = time.time() * 1000 + + def start(self, duration): + rospy.loginfo("Starting {} seconds timer.".format(duration)) + self.reset() + self.game_duration = duration + self.started = True + + def stop(self): + self.__init__() + + def elapsed_time(self): + return (time.time() * 1000 - self._start_time) / 1000.0 # return elapsed time in seconds + + def time_left(self): + return self.game_duration - self.elapsed_time() + + def is_finished(self): + return True if self.time_left() < 0 else False + + +class TimerManager(): + def __init__(self): + self._set_timer_srv = rospy.Service("/ai/game_manager/set_timer", SetTimer, self.on_set_timer) + self._delay_srv = rospy.Service("/ai/game_manager/delay", Delay, self.on_delay) + self._timer_pub = rospy.Publisher("/ai/game_manager/time", GameTime, queue_size = 10) + + rospy.Subscriber("/ai/game_manager/status", GameStatus, self.on_status) + self._game_status = Status.STATUS_INIT + + self.timer = Timer() + + def update(self): + if self.timer.started: + if self.timer.time_left() <= 0: + rospy.logwarn_throttle(20, "Timer ended! setting HALT to ai/game_manager, stopping timer.") + self.set_game_status(GameStatus.STATUS_HALT) + self.timer.stop() + else: + rospy.loginfo_throttle(20, "Timer seconds left : {}".format(int(round(self.timer.time_left()))) + " s") + self.publish_timer() + + def publish_timer(self): + m = GameTime() + m.is_active = self.timer.started + m.game_time_duration = self.timer.game_duration + m.game_elapsed_time = self.timer.elapsed_time() if self.timer.started else -1 + m.game_time_left = self.timer.time_left() if self.timer.started else -1 + self._timer_pub.publish(m) + + def on_set_timer(self, req): + if req.action == req.ACTION_START: + if not self.timer.started: + self.timer.start(req.duration) + else: + rospy.logerr("ERROR Timer already started.") + return SetTimerResponse(False) + elif req.action == req.ACTION_RESET: + if self.timer.started: + self.timer.reset() + else: + rospy.logerr("ERROR Tried to reset timer but it is not running.") + return SetTimerResponse(False) + elif req.action == req.ACTION_STOP: + if self.timer.started: + self.timer.stop() + else: + rospy.logerr("ERROR Tried to stop timer but it is not running.") + return SetTimerResponse(False) + return SetTimerResponse(True) + + def on_delay(self, req): + rospy.loginfo("[timer] Sleeping for {}s (service callback)...".format(req.duration)) + time.sleep(req.duration) + return DelayResponse(True) + + def on_status(self, req): + self._game_status = req.game_status + + def set_game_status(self, status): + rospy.wait_for_service("/ai/game_manager/set_status", timeout = 2) + service = rospy.ServiceProxy("/ai/game_manager/set_status", SetStatus) + return service(status) \ No newline at end of file diff --git a/ros_ws/src/ai_game_manager/src/game_manager_node.py b/ros_ws/src/ai_game_manager/src/game_manager_node.py new file mode 100755 index 0000000..98c39e7 --- /dev/null +++ b/ros_ws/src/ai_game_manager/src/game_manager_node.py @@ -0,0 +1,16 @@ +#!/usr/bin/python +import time +import rospy + +from game_manager import StatusManager, TimerManager + +if __name__ == "__main__": + rospy.init_node("game_manager", log_level = rospy.INFO) + status = StatusManager() + timer = TimerManager() + + r = rospy.Rate(5) + while not rospy.is_shutdown(): + status.update() + timer.update() + diff --git a/ros_ws/src/ai_game_manager/src/init_service.cpp b/ros_ws/src/ai_game_manager/src/init_service.cpp new file mode 100644 index 0000000..8c16f2f --- /dev/null +++ b/ros_ws/src/ai_game_manager/src/init_service.cpp @@ -0,0 +1,55 @@ +#include + +#include "ai_game_manager/init_service.h" + +using namespace std; + +const string READY_SRV = "/ai/game_manager/node_ready"; +const string ARM_SRV = "/ai/game_manager/arm"; +const string HALT_SRV = "/ai/game_manager/status"; +const auto TIMEOUT_READY_SRV = ros::Duration(15.0); + +StatusServices::StatusServices(const string& namespaceName, const string& packageName, ArmCallback_t armCallback, StatusCallback_t statusCallback) : + _armCallback(std::move(armCallback)), + _statusCallback(std::move(statusCallback)) +{ + _nodeName = "/" + namespaceName + "/" + packageName; + if (_armCallback) + _armServer = _nodeHandle.subscribe(ARM_SRV, 10, &StatusServices::_on_arm, this); + if (_statusCallback) + _gameStatusSubscriber = _nodeHandle.subscribe(HALT_SRV, 10, &StatusServices::_on_gameStatus, this); +} + +void StatusServices::setReady(bool success) +{ + try + { + if (!ros::service::waitForService(READY_SRV, TIMEOUT_READY_SRV)) + throw Errors::SERVICE_TIMEOUT; + ros::ServiceClient readyPub = _nodeHandle.serviceClient(READY_SRV); + ai_game_manager::NodeReady msg; + msg.request.success = success; + msg.request.node_name = _nodeName; + if (!readyPub.call(msg)) + throw Errors::SERVICE_NOT_RESPONDING; + if (success) + ROS_INFO_STREAM("Node " << _nodeName << " initialized successfully."); + else + ROS_ERROR_STREAM("Node " << _nodeName << " didn't initialize correctly."); + } + catch(...) + { + ROS_ERROR("status_services couldn't contact ai/game_manager to send init notification."); + } +} + + +void StatusServices::_on_arm(const ai_game_manager::ArmRequest::ConstPtr& msg) +{ + _armCallback(msg); +} + +void StatusServices::_on_gameStatus(const ai_game_manager::GameStatus::ConstPtr& msg) +{ + _statusCallback(msg); +} \ No newline at end of file diff --git a/ros_ws/src/ai_game_manager/srv/Delay.srv b/ros_ws/src/ai_game_manager/srv/Delay.srv new file mode 100644 index 0000000..59aeb6d --- /dev/null +++ b/ros_ws/src/ai_game_manager/srv/Delay.srv @@ -0,0 +1,3 @@ +float32 duration +--- +bool success \ No newline at end of file diff --git a/ros_ws/src/ai_game_manager/srv/NodeReady.srv b/ros_ws/src/ai_game_manager/srv/NodeReady.srv new file mode 100644 index 0000000..dbcad81 --- /dev/null +++ b/ros_ws/src/ai_game_manager/srv/NodeReady.srv @@ -0,0 +1,5 @@ +# Client nodes use this service to notify ai/game_manager that +# their initialization is done. +string node_name # Origin node name (format /namespace/packagename) +bool success # Whether the node initialized correctly +--- \ No newline at end of file diff --git a/ros_ws/src/ai_game_manager/srv/SetStatus.srv b/ros_ws/src/ai_game_manager/srv/SetStatus.srv new file mode 100644 index 0000000..85de435 --- /dev/null +++ b/ros_ws/src/ai_game_manager/srv/SetStatus.srv @@ -0,0 +1,7 @@ +# uint8 STATUS_INITIALIZING = 0 # Not allowed. +# uint8 STATUS_INITIALIZED = 1 # Not allowed. +uint8 STATUS_INGAME = 2 +uint8 STATUS_HALT = 3 +uint8 new_game_status +--- +bool success \ No newline at end of file diff --git a/ros_ws/src/ai_game_manager/srv/SetTimer.srv b/ros_ws/src/ai_game_manager/srv/SetTimer.srv new file mode 100644 index 0000000..d334d0a --- /dev/null +++ b/ros_ws/src/ai_game_manager/srv/SetTimer.srv @@ -0,0 +1,10 @@ +uint8 ACTION_START = 0 # Start the timer. Will be triggered at the set duration. +# uint8 ACTION_PAUSE = 1 # Pause the timer. Won't count the time until resumed. +# uint8 ACTION_RESUME = 2 # Resume the timer. +uint8 ACTION_RESET = 3 # Set elapsed time to zero, no matter the state of the timer. +uint8 ACTION_STOP = 4 # Stops the timer. CAUTION Will set game_status to HALT (will stop everything). +uint8 action + +float32 duration # For ACTION_START only: set the timer duration before it sends HALT. +--- +bool success \ No newline at end of file diff --git a/ros_ws/src/ai_scheduler/CMakeLists.txt b/ros_ws/src/ai_scheduler/CMakeLists.txt new file mode 100644 index 0000000..cd26ccc --- /dev/null +++ b/ros_ws/src/ai_scheduler/CMakeLists.txt @@ -0,0 +1,77 @@ +cmake_minimum_required(VERSION 2.8.3) +project(ai_scheduler) + +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs + geometry_msgs + message_generation + + drivers_ard_hmi +) + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +add_message_files( + FILES + TaskResult.msg + AIScore.msg +) + +add_service_files( + FILES + test.srv +) + +generate_messages( + DEPENDENCIES + std_msgs + geometry_msgs +) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +################################### +## catkin specific configuration ## +################################### + +catkin_package( + CATKIN_DEPENDS message_runtime +) + +########### +## Build ## +########### + +include_directories( + ${catkin_INCLUDE_DIRS} +) + +############# +## Install ## +############# + +catkin_install_python( + PROGRAMS src/scheduler_node.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install ( + PROGRAMS src/scheduler_communication.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install( + DIRECTORY src/ai + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + PATTERN "*.pyc" EXCLUDE +) + +############# +## Testing ## +############# diff --git a/ros_ws/src/ai_scheduler/README.md b/ros_ws/src/ai_scheduler/README.md new file mode 100644 index 0000000..44ff863 --- /dev/null +++ b/ros_ws/src/ai_scheduler/README.md @@ -0,0 +1,95 @@ +# Description : AIScheduler + +This package takes care of choosing what actions to take based on an `actions` definition list. +The definition files are located under `src/Definitions`. + +This package also contains the main launch files that start all system nodes. +In order to launch the main program with all system nodes, run `roslaunch robot_ai_scheduler main.launch` after sourcing the workspace. + +## AI Scheduler + +The scheduler is a fully-recursive task manager and executer. Calling the `findNext()` function in the `Strategy` class +will recursively call the ActionLists and Actions until an Order is returned. This Order is then triggered with its function +`execute()`, which will send an `action` message to the node specified in the XML definition file. + +__NOTE 1__ : For now, the orders are implemented as services. They will soon be sent as `action` messages. This will enable the AI to execute several +Orders at the same time. + +__NOTE 2__ : Conditions (e.g. ask for the Map server if there are enough elements in a container) are +yet to be implemented. + +### Order definitions +The orders are defined in the file `3_orders.xml`. The `` node has to have a `ref` attribute, in order for it to be referenced from the outside. It can take an optional `duration` attribute, which is a manual estimation. This node must have a `` child node, with a `dest` attributes, representing the service or action the message will be sent to. A message node holds multiple ``, mirroring the structure of the ROS request message sent. Each `` has a `name`, matching the name of the parameter in the ROS message, and a `type`, used to parse the parameter. A parameter can be `optional`, which means it doesn't have to be filled when the order is called (the default value will be put in by ROS). It can be `preset` : in that case, the parameter cannot be set when the order is called, the value is constant and cannot be changed. To finish, default values can be set in the order definition. Note that a param with a default value is therefore optional, and a param cannot be preset and optional. + +#### Example +Let's assume we want to send a request to `/asserv/goto`, with the following ROS definition : +``` +geometry_msgs/Pose2D position +float64 number +uint8 command +string message +``` + +Here is a valid example of the order definition, with all the elements seen above : +```xml + + + + 42.8 + 5 + + + +``` + +### Order references +The orders can be referenced from the actions or strategies definition files. To reference an order, a node `` has to have its `ref` attribute matching the `ref` attribute of the referenced order. As childs of the node, the parameters (non-preset) are set, with the tag of the child matching the name of the parameter. + +#### Example +Let's continue with our defined order from above. Suppose we want to reference it, here is a valid way to do it : +```xml + + + 55.2 + 57.1 + 3.14159 + + hello world! + +``` + +### Action definitions + +The actions are defined in the file `2_actions.xml`. The `` node has to have a `ref` attribute, in order for it to be referenced from the outside. This node has three children : +- The `` node is yet to be implemented. +- The `` node holds all the parameters used when calling this action. They are defined identically to the parameters in the message of an order, seen above, with the `name`, `type`, `preset` and `optional` attributes. For them to be useful, the parameters given to an action have to be passed to the orders or the actions that are called from this action. This will be discussed in the next part. +- The `` node doc is TODO + +### Action parameters binding +An action has a bunch of children (`orderref` and/or `actionref`). The parameters of the parent action can be passed to these children for them to use them. To do that, we use the `bind` attributes on the children's parameters like so : + +```xml + + + + + + + + 6.5 + 7.5 + 7 + + + + + +``` +Here, the order `wheels_goto` requires two parameters. The `target_pos` one is given directly in the `orderref`. However, no value is passed for the `speed` one. Instead, it is binded to the parameter named `speed` of the parent action. The `speed` parameter of this action is defined in the `params` node. When the `goto_spawn` action will be referenced, the `speed` parameter given to it will be passed to the order reference of `wheels_goto`. Note that the names for the parent parameter and the binded parameter do not have to match. + +### Action references + +The call of an action works the same as the order reference, replacing `orderref` by `actionref`. + +### Adding new parameter types +In order to parse correctly all wanted type, one has to add a parser class, child of the `Param` class, for each type of parameter. Check out `ai_params.py` for more details. diff --git a/ros_ws/src/ai_scheduler/msg/AIScore.msg b/ros_ws/src/ai_scheduler/msg/AIScore.msg new file mode 100644 index 0000000..68d48af --- /dev/null +++ b/ros_ws/src/ai_scheduler/msg/AIScore.msg @@ -0,0 +1 @@ +uint16 score # Current reward points estimated by ai/scheduler. \ No newline at end of file diff --git a/ros_ws/src/ai_scheduler/msg/TaskResult.msg b/ros_ws/src/ai_scheduler/msg/TaskResult.msg new file mode 100644 index 0000000..d6c3a77 --- /dev/null +++ b/ros_ws/src/ai_scheduler/msg/TaskResult.msg @@ -0,0 +1,9 @@ +# Message module that can be included in services. Serves as a universal +# way of responding if a service was successfully executed. +uint8 RESULT_SUCCESS = 0 # Execution successful. +uint8 RESULT_PAUSE = 1 # Can be retried later. E.g. navigator couldn't reach + # a goal because of the enemy blocking the way. +uint8 RESULT_FAIL = 2 # Execution didn't succeed. +uint8 result + +string verbose_reason # if RESULT_FAIL, this string will be logged by ai/scheduler. \ No newline at end of file diff --git a/ros_ws/src/ai_scheduler/package.xml b/ros_ws/src/ai_scheduler/package.xml new file mode 100644 index 0000000..90a5f1d --- /dev/null +++ b/ros_ws/src/ai_scheduler/package.xml @@ -0,0 +1,16 @@ + + + ai_scheduler + 0.0.0 + The ai_scheduler package + GPLv3 + MadeInPierre + UTCoupe Association + catkin + message_generation + rospy + std_msgs + message_runtime + + + diff --git a/ros_ws/src/ai_scheduler/src/ai/__init__.py b/ros_ws/src/ai_scheduler/src/ai/__init__.py new file mode 100644 index 0000000..1777770 --- /dev/null +++ b/ros_ws/src/ai_scheduler/src/ai/__init__.py @@ -0,0 +1,2 @@ +from ai import RobotAI +from tasks import GameProperties diff --git a/ros_ws/src/ai_scheduler/src/ai/ai.py b/ros_ws/src/ai_scheduler/src/ai/ai.py new file mode 100644 index 0000000..0072348 --- /dev/null +++ b/ros_ws/src/ai_scheduler/src/ai/ai.py @@ -0,0 +1,34 @@ +#!/bin/usr/python +import rospy +from timer_client import TimerClient +from game_status_client import GameStatusClient, GameStatusConstants +from ai_loader import AILoader + +class RobotAI(): + def __init__(self): + self.timer = TimerClient() # Timer client. + self.game_status = GameStatusClient() + self._loader = AILoader() + + def load_game_properties(self): + return self._loader.load_game_properties() + + def start(self, communicator): + strategy = self._loader.load(communicator) + self.execute(strategy) + + def execute(self, strategy): + strategy.PrettyPrint() + # Run the whole AI until there are no orders left to execute + while not rospy.is_shutdown(): + if self.game_status.status == GameStatusConstants.STATUS_HALT: + rospy.logwarn("[AI] detected game_status STATUS_HALT, aborting actions.") + break + + if strategy.canContinue(): + strategy.getNext().execute(strategy.communicator) + else: + rospy.loginfo("[AI] In-Game actions finished!") + break + strategy.sendReward(strategy.communicator) + strategy.PrettyPrint() diff --git a/ros_ws/src/ai_scheduler/src/ai/ai_loader.py b/ros_ws/src/ai_scheduler/src/ai/ai_loader.py new file mode 100644 index 0000000..efffa46 --- /dev/null +++ b/ros_ws/src/ai_scheduler/src/ai/ai_loader.py @@ -0,0 +1,73 @@ +#!/usr/bin/python +import os +import rospy +import xml.etree.ElementTree as ET +from tasks import GameProperties, Strategy, TaskList, Action, Order + +from memory_definitions.srv import GetDefinition + +class AILoader(): + STRATEGIES_FILE = "1_strategies.xml" + ACTIONS_FILE = "2_actions.xml" + SYSTEM_ORDERS_FILE = "system_orders.xml" + ORDERS_FILE = "3_orders.xml" + + def __init__(self): + pass# self.xml_dirpath = os.path.dirname(__file__) + "/../../def/" + + def load_game_properties(self): + strat_path = self._get_path(self.STRATEGIES_FILE) + xml_strategies = ET.parse(strat_path).getroot() + + GameProperties.AVAILABLE_STRATEGIES = [strat.attrib["name"] for strat in xml_strategies.findall("strategy")] + GameProperties.AVAILABLE_TEAMS = [team.attrib["name"] for team in xml_strategies.find("teams")] + + def load(self, communicator): + orders = self._load_orders() + return self._load_strategy(self._load_actions(orders), orders, communicator) + + def _load_orders(self): + xml_system_orders = ET.parse(self._get_path(self.SYSTEM_ORDERS_FILE)).getroot() + xml_orders = ET.parse(self._get_path(self.ORDERS_FILE)).getroot() + + orders = [] + for order_xml in xml_system_orders.findall("order") + xml_orders.findall("order"): + # used for robot orders to override system orders that have the same ref. + conflicts = [o for o in orders if o.Ref == order_xml.attrib["ref"]] + if conflicts: + for c in conflicts: + orders.remove(c) + orders.append(Order(order_xml)) + return orders + + def _load_actions(self, orders): + xml_actions = ET.parse(self._get_path(self.ACTIONS_FILE)).getroot() + + # actions_names = [action.attrib["ref"] for action in xml_actions] + actions = [] + for action_xml in xml_actions: + actions.append(Action(action_xml, actions, orders)) + return actions + + def _load_strategy(self, actions, orders, communicator): + xml_strategies = ET.parse(self._get_path(self.STRATEGIES_FILE)).getroot() + + strategy_xml = xml_strategies.findall("strategy[@name='{}']".format(GameProperties.CURRENT_STRATEGY)) + if len(strategy_xml) == 1: + return Strategy(strategy_xml[0], actions, orders, communicator) + else: + rospy.logerr("FAIL Too many or no strategy to load with name '{}'. Aborting.".format(GameProperties.CURRENT_STRATEGY)) + return None + + def _get_path(self, filename): + get_def = rospy.ServiceProxy('/memory/definitions/get', GetDefinition) + + try: + get_def.wait_for_service(10) + res = get_def('ai/' + filename) + if not res.success: + rospy.logerr("Error when fetching '{}' definition file".format(filename)) + return res.path + except rospy.ServiceException as exc: + rospy.logerr("Unhandled error while getting def file: {}".format(str(exc))) + raise Exception() diff --git a/ros_ws/src/ai_scheduler/src/ai/game_status_client.py b/ros_ws/src/ai_scheduler/src/ai/game_status_client.py new file mode 100644 index 0000000..d573f48 --- /dev/null +++ b/ros_ws/src/ai_scheduler/src/ai/game_status_client.py @@ -0,0 +1,17 @@ +import rospy +from ai_game_manager.msg import GameStatus + +class GameStatusConstants(): + STATUS_INIT = 0 + STATUS_INGAME = 1 + STATUS_HALT = 2 + +class GameStatusClient(): + def __init__(self): + rospy.Subscriber("/ai/game_manager/status", GameStatus, self.on_new_status) + self.status = 0 + self.init_status = 0 + + def on_new_status(self, msg): + self.status = msg.game_status + self.init_status = msg.init_status diff --git a/ros_ws/src/ai_scheduler/src/ai/tasks/__init__.py b/ros_ws/src/ai_scheduler/src/ai/tasks/__init__.py new file mode 100644 index 0000000..bc05735 --- /dev/null +++ b/ros_ws/src/ai_scheduler/src/ai/tasks/__init__.py @@ -0,0 +1,5 @@ +from definitions import GameProperties +from strategy import Strategy +from tasklist import TaskList +from action import Action +from order import Order diff --git a/ros_ws/src/ai_scheduler/src/ai/tasks/action.py b/ros_ws/src/ai_scheduler/src/ai/tasks/action.py new file mode 100644 index 0000000..78e3ad2 --- /dev/null +++ b/ros_ws/src/ai_scheduler/src/ai/tasks/action.py @@ -0,0 +1,57 @@ +# -*- coding: utf-8 -*- +from definitions import * +from task import Task +from tasklist import TaskList + + +class Action(TaskList): + def __init__(self, xml, actions, orders): + super(Action, self).__init__(xml.find("actions"), actions, orders) + self.Ref = xml.attrib["ref"] + self.Name = self.Ref if not self.Name else self.Name + self.fetchBoundParams(xml) + + def getParamForBind(self, bind): + for task in self.TASKS: + if task.getParamForBind(bind): + return task.getParamForBind(bind) + + def fetchBoundParams(self, xml): + if "params" not in [node.tag for node in xml]: + return + boundParamsList = [] + for param in xml.find("params"): + if "name" not in param.attrib: + raise KeyError("Parameters need a 'name' attribute ! (action '{}')".format(self.Name)) + name = param.attrib["name"] + finalBind = self.getParamForBind(name) + + if not finalBind: + raise KeyError("No parameter bound with '{}' !".format(name)) + else: + boundParamsList.append(finalBind) + self.BoundParams = {p.bind: p for p in boundParamsList} + + def setParameters(self, orderref_xml): + for child in orderref_xml: + name = child.tag + if name not in self.BoundParams: + raise KeyError("No bind found for '{}' !".format(name)) + self.BoundParams[name].parseValue(child) + + for p in self.BoundParams: + if self.BoundParams[p].bind == p: + self.BoundParams[p].checkValues() + + def __repr__(self): + c = Console();c.setstyle(Colors.BOLD);c.setstyle(Colors.BLUE) + c.addtext("[{} Action]".format(self.getStatusEmoji())) + c.endstyle();c.setstyle(Colors.BLUE);c.addtext(" {0} ".format(self.Name));c.endstyle();c.setstyle(Colors.GRAY) + + c.addtext("[{}{}{}{}{}]".format(ExecutionMode.toEmoji(self.executionMode), + " " + ExecutionOrder.toEmoji(self.executionOrder), + " {}/{}".format(str(self._repeats), str(self._repeats_max)) \ + + RepeatMode.toEmoji(self.repeatMode) if self.repeatMode != RepeatMode.ONCE else "", + ", {}⚡".format(self.getReward()) if self.getReward() else "", + ", ~{}⌛".format(int(self.getDuration())) if self.getDuration() else "")) + return c.getText() diff --git a/ros_ws/src/ai_scheduler/src/ai/tasks/definitions.py b/ros_ws/src/ai_scheduler/src/ai/tasks/definitions.py new file mode 100644 index 0000000..9f8e9ca --- /dev/null +++ b/ros_ws/src/ai_scheduler/src/ai/tasks/definitions.py @@ -0,0 +1,96 @@ +# -*- coding: utf-8 -*- + +class TaskStatus(): + CRITICAL = ('CRITICAL' , '💔') # Fatal error, system will shutdown. + NEEDSPREVIOUS = ('NEEDSPREVIOUS' , '↳') # Task can't execute yet because it needs the previous task to be at SUCCESS still. + PENDING = ('PENDING' , '⋯') # Active when an order (or order child in list) is waiting for response callback. + FREE = ('FREE' , '⬜') # Free task, not activated yet. + PAUSED = ('PAUSED' , '') # TODO implement entire pause engine + ERROR = ('ERROR' , '❌') # Error. Order couldn't be done, AI will try to find an alternative path of orders in the tree. + BLOCKED = ('BLOCKED' , '⛔') # Node can't execute because conditions aren't fully satisfied. + SUCCESS = ('SUCCESS' , '🆗', 0.0) # Order and lists complete. # + @staticmethod + def toEmoji(status): + return status[1] + +class ExecutionMode(): + ALL = ('all' , '⚫') # Will execute all tasks (except the blocked ones). + ONE = ('one' , '1') # Will execute tasks in the list until one is successful. + ATLEASTONE = ('+' , '+') # At least one task in the list must be executed, will try to execute all. + @staticmethod + def toEmoji(mode): + return mode[1] + @staticmethod + def fromText(text): + if text == 'all' : return ExecutionMode.ALL + elif text == 'one' : return ExecutionMode.ONE + elif text == '+' : return ExecutionMode.ATLEASTONE + else: raise ValueError, "ExecutionMode '{}' not recognized.".format(text) + +class ExecutionOrder(): + LINEAR = ('linear' , '⬇') # Will follow linearly the order given in the XML file. + RANDOM = ('random' , '🌀') # Will pick a random free task. + SIMULTANEOUS = ('simultaneous' , '⇶') # Will activate all tasks at once. + FASTEST = ('fastest' , '🕒') # Will sort the tasks from least Duration to most. + MOSTREWARD = ('mostreward' , '⚡') # Will sort the tasks from most Reward to least. Execute tasks with same reward amount linearly. + @staticmethod + def toEmoji(order): + return order[1] + @staticmethod + def fromText(text): + if text == 'linear' : return ExecutionOrder.LINEAR + elif text == 'random' : return ExecutionOrder.RANDOM + elif text == 'simultaneous': return ExecutionOrder.SIMULTANEOUS + elif text == 'fastest' : return ExecutionOrder.FASTEST + elif text == 'mostreward' : return ExecutionOrder.MOSTREWARD + else: raise ValueError, "ExecutionOrder '{}' not recognized.".format(text) + +class RepeatMode(): + ONCE = ('once' , '1') # Will execute the block one time. + FOR = ('for' , '') # Will execute the given amount of times, no matter the fails. + WHILE = ('while' , '∞') # Will execute tasks in the list until one is successful. + @staticmethod + def toEmoji(mode): + return mode[1] + @staticmethod + def fromText(text): + if text == 'once' : return RepeatMode.ONCE + elif text.isdigit() : return RepeatMode.FOR + elif text == 'while' : return RepeatMode.WHILE + else: raise ValueError, "RepeatMode '{}' not recognized.".format(text) + +class GameProperties(): + GAME_DURATION = None + REWARD_POINTS = 0 + + AVAILABLE_STRATEGIES = [] + AVAILABLE_TEAMS = [] + + CURRENT_STRATEGY = '' + CURRENT_TEAM = '' + + +class Colors(): + BOLD = "\033[1m" + GRAY = "\033[92m" + BLUE = "\033[95m" + RED = "\033[91m" + RESET = "\033[0m" + + +class Console(): + def __init__(self): + self.Text = Colors.RESET + + def setstyle(self, color): + self.Text += color + + def addtext(self, text): + self.Text += text + + def endstyle(self): + self.Text += Colors.RESET + + def getText(self): + self.endstyle() + return self.Text diff --git a/ros_ws/src/ai_scheduler/src/ai/tasks/order.py b/ros_ws/src/ai_scheduler/src/ai/tasks/order.py new file mode 100644 index 0000000..f680f95 --- /dev/null +++ b/ros_ws/src/ai_scheduler/src/ai/tasks/order.py @@ -0,0 +1,159 @@ +# -*- coding: utf-8 -*- +import rospy +from definitions import * +from task import Task +import params + +class Order(Task): + def __init__(self, xml): + super(Order, self).__init__(xml) + self.Ref = xml.attrib["ref"] + if not self.Name: + self.Name = self.Ref + + self._prev_status = self.getStatus() + + self.Duration = float(xml.attrib["duration"]) if "duration" in xml.attrib else 0.0 # Manually estimated time to execute this action + self.Message = Message(xml.find("message")) + + self.Responses = [Response(res) for res in xml.findall("response")] + #TODO check if at least one node exists, except if message is PUB_MSG type + + self.TimeTaken = None + + def setParameters(self, orderref_xml): + childs = [child for child in orderref_xml] + self.Message.setParameters(childs) + + def getParamForBind(self, bind): + for p in self.Message.Parameters: + for b in p.getBoundParams(): + if b.bind == bind: + return b + return False + + def getDuration(self): + return self.Duration + + def resetStatus(self, refresh_parent=False): # wipes all progress of this list and all descendent tasks. + self.setStatus(TaskStatus.FREE, refresh_parent) #TODO on all types of tasks, reset to needsprevious if it was like that at init + + def execute(self, communicator): + self._prev_status = self.getStatus() + self.setStatus(TaskStatus.PENDING) + rospy.loginfo("Executing task: {}...".format(self.__repr__())) + + self.Message.send(communicator, self.callback) + + def callback(self, res, time_taken): + self.TimeTaken = time_taken + if isinstance(res, bool): + new_status = TaskStatus.SUCCESS if res else TaskStatus.ERROR + elif res is None: #occurs in timeout situations + new_status = TaskStatus.PAUSED + else: + ranks = [TaskStatus.SUCCESS, TaskStatus.ERROR, TaskStatus.PAUSED] # defines which valid response gets prioritized. + # last is most important. + results = [response.Result for response in self.Responses if response.compare(res)] + if results: # If one/several responses checks succeed, take the max ranked status. + new_status = ranks[max([ranks.index(r) for r in results])] + else: # if the response corresponds to nothing in the XML, default to failed. + new_status = TaskStatus.ERROR + + if self._prev_status == TaskStatus.PAUSED and new_status == TaskStatus.PAUSED: + rospy.logwarn("Order was resumed but failed again, blocking it.") + new_status = TaskStatus.BLOCKED # do not let a paused action go back to paused (potential infinite loop) + + print "setting new status " + str(new_status) + self.setStatus(new_status) + + if new_status == TaskStatus.SUCCESS: + rospy.loginfo("Task succeeded: {}".format(self.__repr__())) + elif new_status in [TaskStatus.ERROR, TaskStatus.BLOCKED]: + rospy.logerr("Task failed: {}".format(self.__repr__())) + elif new_status == TaskStatus.PAUSED: + rospy.logwarn("Task paused: {}. Will eventually try again later.".format(self.__repr__())) + + def __repr__(self): + c = Console() + c.setstyle(Colors.BOLD) + c.addtext("[{}{} Order] ".format(self.getStatusEmoji(), + " , {0:.1f}⌛".format(self.TimeTaken) if self.getStatus() in [TaskStatus.SUCCESS, + TaskStatus.ERROR, TaskStatus.PAUSED, TaskStatus.CRITICAL] else "")) + c.endstyle() + c.addtext("{}".format(self.Name)) + c.endstyle();c.setstyle(Colors.GRAY) + c.addtext("{}".format(" [{}⚡]".format(self.Reward) if self.Reward else "")) + return c.getText() + + +class Message(): + def __init__(self, xml): + if "dest" not in xml.attrib: + raise KeyError("PARSE ERROR ! Messages need a 'dest' attribute") + + self.Destination = xml.attrib["dest"] + self.Timeout = int(xml.attrib["timeout"]) if "timeout" in xml.attrib else None + self.Parameters = [] + + # parse declaration of parameters + paramsNames = [] + for param in xml.findall("param"): + p = params.ParamCreator(param) + self.Parameters.append(p) + if p.name in paramsNames: + raise KeyError("PARSE ERROR ! Param {} already defined here".format(p.name)) + paramsNames.append(p.name) + + def send(self, communicator, callback): + ros_params = {p.name: p.getRos() for p in self.Parameters} + communicator.SendRequest(self.Destination, ros_params, self.Timeout, callback) + + def setParameters(self, xml_list): # populate values of params + for child in xml_list: + name = child.tag + parameters = [p for p in self.Parameters if p.name == name] + + if len(parameters) != 1: + raise KeyError("PARSE ERROR ! Param {} defined {} times" + .format(child.tag, len(parameters))) + + param = parameters[0] + if param.preset: + raise KeyError("PARSE ERROR ! Param {} is preset, \ + cannot modify it".format(param.name)) + param.parseValue(child) + [p.checkValues() for p in self.Parameters if not p.bind] + + +class Response(object): + def __init__(self, xml): + results = { + "success": TaskStatus.SUCCESS, + "pause": TaskStatus.PAUSED, + "error": TaskStatus.ERROR + } + if "result" in xml.attrib and xml.attrib["result"] not in results.keys(): + raise ValueError("PARSE ERROR! Order response must have a valid 'result' attribute.") + self.Result = results[xml.attrib["result"]] if "result" in xml.attrib else TaskStatus.SUCCESS + + self.Parameters = [] # TODO recursive params from actions/strategies? + _param_names = [] + for param in xml.findall("param"): + p = params.ParamCreator(param) + if p.name in _param_names: + raise KeyError("PARSE ERROR ! Param {} already defined here.".format(p.name)) + self.Parameters.append(p) + _param_names.append(p.name) + + if not self.Parameters: + raise ValueError("PARSE ERROR! Order response must have at least one param to be checked.") + #TODO implement relations (and/or between params) + + def compare(self, response): + for param in self.Parameters: + if not hasattr(response, param.name): + raise ValueError("PARSE ERROR! Response message has no variable named '{}'.".format(param.name)) + if not param.compare(getattr(response, param.name)): + return False + return True diff --git a/ros_ws/src/ai_scheduler/src/ai/tasks/params.py b/ros_ws/src/ai_scheduler/src/ai/tasks/params.py new file mode 100644 index 0000000..fae9135 --- /dev/null +++ b/ros_ws/src/ai_scheduler/src/ai/tasks/params.py @@ -0,0 +1,272 @@ +#!/usr/bin/env python + +# Functionalities of params : +# - default : 78 +# => if the value is not set in orderref, the param will take this value +# (defaults can be set not fully) : +# 75 +# 78 +# +# => x and y can be overwritten or not in orderref, but 'theta' has +# to be set +# +# - preset : 78 +# => the param can't be set in orderref, this fixed value will be sent in +# the message +# => a preset has to be fully set in the declaration +# +# - optional : +# => the value doesn't have to be set in orderref, default value of ROS +# will be set + +import copy +import std_msgs.msg +import geometry_msgs.msg +import memory_map.msg + +import rospy + + +class Param(object): # base class for parsing xml to param object + TYPE_NAME = "" # name used in the xml (=", "<", ">"]: + exec("result = int(obj_ros) {} self.value['data']".format(self.condition)) + return result + else: + rospy.logerr("Error : message response check has no '{}' condition in '{}' type.".format(self.condition, self.TYPE_NAME)) + + +class FloatParser(Param): + TYPE_NAME = "float" + TYPE_ROS = std_msgs.msg.Float64 + + def __init__(self, xml=None): + self.value = { + 'data': None + } + super(FloatParser, self).__init__(xml) + + def parseValue(self, xml): + self.parseBind(xml) + if xml.text: + self.value["data"] = float(xml.text) + + def getRos(self): + return self.value["data"] + + def compare(self, obj_ros): + if self.condition in ["==", "!=", "<=", ">=", "<", ">"]: + exec("result = float(obj_ros) {} self.value['data']".format(self.condition)) + return result + else: + rospy.logerr("Error : message response check has no '{}' condition in '{}' type.".format(self.condition, self.TYPE_NAME)) + + +# complex classes +# you can choose the override parseValue and getRos if you need to, +# but for the simplest, just declaring the struct of self.value should do +class Pose2DParser(Param): + TYPE_NAME = "pose2d" + TYPE_ROS = geometry_msgs.msg.Pose2D + + def __init__(self, xml=None): + self.value = { + 'x': FloatParser(), + 'y': FloatParser(), + 'theta': FloatParser() + } + super(Pose2DParser, self).__init__(xml) + + +class WaypointParser(Param): + TYPE_NAME = "waypoint" + TYPE_ROS = memory_map.msg.Waypoint + + def __init__(self, xml=None): + self.value = { + 'name': StringParser(), + 'frame_id': StringParser(), + 'pose': Pose2DParser() + } + super(WaypointParser, self).__init__(xml) diff --git a/ros_ws/src/ai_scheduler/src/ai/tasks/strategy.py b/ros_ws/src/ai_scheduler/src/ai/tasks/strategy.py new file mode 100644 index 0000000..d94fe3e --- /dev/null +++ b/ros_ws/src/ai_scheduler/src/ai/tasks/strategy.py @@ -0,0 +1,35 @@ +# -*- coding: utf-8 -*- +import rospy +from definitions import * +from tasklist import TaskList +from task import Task + + +class Strategy(Task): + def __init__(self, xml, actions, orders, communicator): + super(Strategy, self).__init__(xml) + self.Name = xml.attrib["name"] + self.communicator = communicator + self.loadxml(xml, actions, orders) + + def loadxml(self, xml, actions, orders): + self.TASKS = TaskList(xml, actions, orders) + + def canContinue(self): + return self.getStatus() in [TaskStatus.FREE, TaskStatus.PENDING, TaskStatus.PAUSED] + + def getNext(self): # Returns the next free task (TaskList, Action or Order). + return self.TASKS.getNext() + + def sendReward(self, communicator): + communicator.SendRequest("/ai/scheduler/score", {"score": self.TASKS.getActiveReward()}) + + def getStatus(self, text_version=False): + return self.TASKS.getStatus(text_version) + + def PrettyPrint(self): + rospy.loginfo("[STRATEGY {}⚡ ACTIVE] {}".format(self.TASKS.getActiveReward(), self.__repr__())) + self.TASKS.prettyprint(1) + + def __repr__(self): + return self.Name diff --git a/ros_ws/src/ai_scheduler/src/ai/tasks/task.py b/ros_ws/src/ai_scheduler/src/ai/tasks/task.py new file mode 100644 index 0000000..332b230 --- /dev/null +++ b/ros_ws/src/ai_scheduler/src/ai/tasks/task.py @@ -0,0 +1,39 @@ +# -*- coding: utf-8 -*- +import rospy +from definitions import * + +class Task(object): + def __init__(self, xml, status = TaskStatus.FREE): + self.Name = xml.attrib["name"] if "name" in xml.attrib else None + self.Reward = int(xml.attrib["reward"]) if "reward" in xml.attrib else 0 + self.Status = status + + self.Parent = None + self.NeedsPrevious = False + + def getReward(self): + return self.Reward + + def getActiveReward(self): + return self.getReward() if self.getStatus() == TaskStatus.SUCCESS else 0 + + def setParent(self, parent): + self.Parent = parent + + def setStatus(self, new_status, refresh_parent = True): + if self.Status != new_status: + self.Status = new_status + if refresh_parent and self.Parent: + self.Parent.refreshStatus() + + def getStatus(self, text_version = False): + return self.Status if not text_version else self.Status[0] + + def getStatusEmoji(self): + return self.Status[1] + + def prettyprint(self, indentlevel): + rospy.loginfo("\033[0m" + " ║ " * (indentlevel - 1) + " ╠═" + self.__repr__()) + + def __repr__(self): + return "" diff --git a/ros_ws/src/ai_scheduler/src/ai/tasks/tasklist.py b/ros_ws/src/ai_scheduler/src/ai/tasks/tasklist.py new file mode 100644 index 0000000..b67893d --- /dev/null +++ b/ros_ws/src/ai_scheduler/src/ai/tasks/tasklist.py @@ -0,0 +1,213 @@ +# -*- coding: utf-8 -*- +import copy +import random +import rospy +from definitions import * +from task import Task +from order import Order + + +class TaskList(Task): + MAX_REPEATS = 50 # If repeat mode is 'while', this will be the repeat limit. + + def __init__(self, xml, actions, orders): + super(TaskList, self).__init__(xml) + self.executionMode = ExecutionMode.fromText( xml.attrib["exec"]) if "exec" in xml.attrib else ExecutionMode.ALL + self.repeatMode = RepeatMode.fromText( xml.attrib["repeat"]) if "repeat"in xml.attrib else RepeatMode.ONCE + self._repeats = 0 # used to track how many times the list already repeated + self._successful_repeats = 0 + self._repeats_max = TaskList.MAX_REPEATS + if self.repeatMode == RepeatMode.ONCE: self._repeats_max = 1 + if self.repeatMode == RepeatMode.FOR: self._repeats_max = int(xml.attrib["repeat"]) + + self.executionOrder = ExecutionOrder.fromText(xml.attrib["order"]) if "order" in xml.attrib else ExecutionOrder.LINEAR + self.TASKS = self.loadxml(xml, actions, orders) + + def loadxml(self, xml, actions, orders): + tasks = [] + for node_xml in xml: + tag = node_xml.tag + if tag == "tasklist": + i = TaskList(node_xml, actions, orders) + i.setParent(self) + if "needsprevious" in node_xml.attrib and node_xml.attrib["needsprevious"] == 'true': + i.Status = TaskStatus.NEEDSPREVIOUS + tasks.append(i) + elif tag == "actionref" or tag == "orderref": + instances = [action for action in actions if action.Ref == node_xml.attrib["ref"]] if tag == "actionref" else \ + [order for order in orders if order.Ref == node_xml.attrib["ref"]] + if len(instances) != 1: + raise KeyError, "{} action or order instance(s) found with the name '{}'.".format(len(instances), node_xml.attrib["ref"]) + i = copy.deepcopy(instances[0]) + i.setParent(self) + i.Reward = int(node_xml.attrib["reward"]) if "reward" in node_xml.attrib else i.Reward + i.setParameters(node_xml) + if "needsprevious" in node_xml.attrib and node_xml.attrib["needsprevious"] == 'true': + i.Status = TaskStatus.NEEDSPREVIOUS + if "name" in node_xml.attrib: + i.Name = node_xml.attrib["name"] + if tag == "orderref": + i.Message.Timeout = float(node_xml.attrib["timeout"]) if "timeout" in node_xml.attrib else i.Message.Timeout + tasks.append(i) + elif tag == "team": + if node_xml.attrib["name"] == GameProperties.CURRENT_TEAM: + tasks += self.loadxml(node_xml, actions, orders) + else: + rospy.logwarn("WARNING Element skipped at init because '{}' type was not recognized.".format(tag)) + return tasks + + def getReward(self): + return self.Reward + sum([task.getReward() for task in self.TASKS]) + + def getActiveReward(self): + return self.getReward() if self.getStatus() == TaskStatus.SUCCESS else sum([task.getActiveReward() for task in self.TASKS]) + + def getDuration(self): + return sum([task.getDuration() for task in self.TASKS]) + + def getNext(self): + # Decides the next task(s) to execute based on the childs' statuses and the ExecutionOrder XML setting. + for task in self.TASKS: # Execute any pending task in any case #TODO#1 will create problems ? + if task.getStatus() == TaskStatus.PENDING: return task + + if self.executionOrder == ExecutionOrder.LINEAR: + for task in self.TASKS: + if task.getStatus() in [TaskStatus.FREE, TaskStatus.PENDING]: return task + for task in self.TASKS: + if task.getStatus() == TaskStatus.PAUSED: return task + + + elif self.executionOrder == ExecutionOrder.RANDOM: + tasks = [task for task in self.TASKS if task.getStatus() == TaskStatus.FREE] + if not tasks: + tasks = [task for task in self.TASKS if task.getStatus() == TaskStatus.PAUSED] + return tasks[random.randint(0, len(tasks) - 1)] + + elif self.executionOrder == ExecutionOrder.SIMULTANEOUS: + return [task for task in self.TASKS if task.getStatus() in [TaskStatus.FREE, TaskStatus.PENDING, TaskStatus.PAUSED]] #TODO#1 + + elif self.executionOrder == ExecutionOrder.FASTEST: + record, next_task = 10000000, None + tasks = [task for task in self.TASKS if task.getStatus() == TaskStatus.FREE] + if not tasks: + tasks = [task for task in self.TASKS if task.getStatus() == TaskStatus.PAUSED] + for task in tasks: + if task.getDuration() < record: + record = task.getDuration() + next_task = task + return next_task + + elif self.executionOrder == ExecutionOrder.MOSTREWARD: + record, result = -1, None + tasks = [task for task in self.TASKS if task.getStatus() == TaskStatus.FREE] + if not tasks: + tasks = [task for task in self.TASKS if task.getStatus() == TaskStatus.PAUSED] + for task in tasks: + if task.getReward() > record: + record = task.getReward() + result = task + return result + + def execute(self, communicator): + if self.getStatus() in [TaskStatus.FREE, TaskStatus.PENDING, TaskStatus.PAUSED]: + next_tasks = self.getNext() + if type(next_tasks) is list: + raise NotImplementedError, "Simultaneous task launches aren't supported yet !" #TODO + next_tasks.execute(communicator) + else: + raise ValueError, "ERROR asked to execute '{}' task that's not free".format(self.__repr__()) + + def resetStatus(self, refresh_parent=False): # wipes all progress of this list and all descendent tasks. + self.setStatus(TaskStatus.FREE, refresh_parent) + if not refresh_parent: #TODO ~~~~ + self._repeats = 0 + for task in self.TASKS: + task.resetStatus() + + def _markSuccess(self): + if self.repeatMode != RepeatMode.ONCE: + if self.repeatMode == RepeatMode.WHILE or self.repeatMode == RepeatMode.FOR: + self._repeats += 1 + self._successful_repeats += 1 + if self._repeats < self._repeats_max: # if repeat limit not reached yet, mark everything as free + self.resetStatus(refresh_parent=True) + return + elif self._successful_repeats < self._repeats_max: + self.setStatus(TaskStatus.ERROR) + return + + self.setStatus(TaskStatus.SUCCESS) + + def refreshStatus(self): + # unblock or block tasks that need previous tasks + previous_task = self.TASKS[0] + for task in self.TASKS[1:]: + if task.getStatus() == TaskStatus.NEEDSPREVIOUS: + if previous_task.getStatus() == TaskStatus.SUCCESS: + task.setStatus(TaskStatus.FREE, refresh_parent = False) + if previous_task.getStatus() in [TaskStatus.BLOCKED, TaskStatus.ERROR]: + task.setStatus(TaskStatus.BLOCKED) + previous_task = task + + # Decides the status of the list based on the childs' statuses and the ExecutionMode XML setting. + child_statuses = [task.getStatus() for task in self.TASKS] + # CAUTION The order of conditions do count! + if TaskStatus.CRITICAL in child_statuses: + self.setStatus(TaskStatus.CRITICAL);return + + if self.executionMode == ExecutionMode.ONE: + if len([1 for c in child_statuses if c == TaskStatus.SUCCESS]) == 1: + self._markSuccess() + for task in self.TASKS: + if task.getStatus() in [TaskStatus.FREE, TaskStatus.PENDING]: + task.setStatus(TaskStatus.BLOCKED) + return + + if TaskStatus.PENDING in child_statuses: + self.setStatus(TaskStatus.PENDING);return + if TaskStatus.FREE in child_statuses: + self.setStatus(TaskStatus.PENDING);return + if TaskStatus.PAUSED in child_statuses: + self.setStatus(TaskStatus.PAUSED);return + + if self.executionMode == ExecutionMode.ALL: + if len([1 for c in child_statuses if c == TaskStatus.SUCCESS]) == len(child_statuses): + self._markSuccess();return + elif self.executionMode == ExecutionMode.ATLEASTONE: + if len([1 for c in child_statuses if c == TaskStatus.SUCCESS]) >= 1: + self._markSuccess();return + + if TaskStatus.ERROR in child_statuses: + if self.repeatMode == RepeatMode.WHILE or self.repeatMode == RepeatMode.FOR: + self._repeats += 1 + if self._repeats < self._repeats_max: # if repeat limit not reached yet, mark everything as free + self.resetStatus(refresh_parent=True) + self.setStatus(TaskStatus.PENDING) + return + elif self._successful_repeats < self._repeats_max: + self.setStatus(TaskStatus.ERROR) + return + else: + self.setStatus(TaskStatus.ERROR) + #TODO Block all dependent nodes + return + + if TaskStatus.BLOCKED in child_statuses: + self.setStatus(TaskStatus.BLOCKED) + + def prettyprint(self, indentlevel): + super(TaskList, self).prettyprint(indentlevel) + for task in self.TASKS: + task.prettyprint(indentlevel + 1) + + def __repr__(self): + c = Console();c.setstyle(Colors.BOLD);c.setstyle(Colors.RED) + c.addtext("[{} TaskList] {} ".format(self.getStatusEmoji(), self.Name)) + c.endstyle();c.setstyle(Colors.GRAY) + c.addtext("[{}{}{}{}{}]".format(ExecutionMode.toEmoji(self.executionMode), + " " + ExecutionOrder.toEmoji(self.executionOrder), + " {}/{}".format(str(self._repeats), str(self._repeats_max)) \ + + RepeatMode.toEmoji(self.repeatMode) if self.repeatMode != RepeatMode.ONCE else "", + ", {}⚡".format(self.getReward()) if self.getReward() else "", + ", ~{}⌛".format(int(self.getDuration())) if self.getDuration() else "")) + return c.getText() diff --git a/ros_ws/src/ai_scheduler/src/ai/timer_client.py b/ros_ws/src/ai_scheduler/src/ai/timer_client.py new file mode 100644 index 0000000..67e86d9 --- /dev/null +++ b/ros_ws/src/ai_scheduler/src/ai/timer_client.py @@ -0,0 +1,14 @@ +import rospy +from ai_game_manager.msg import GameTime + +class TimerClient(): + def __init__(self): + rospy.Subscriber("/ai/game_manager/time", GameTime, self.on_new_time) + self.is_active = False + self.game_duration = -1 + self.time_elapsed = -1 + + def on_new_time(self, msg): + self.is_active = msg.is_active + self.game_duration = msg.game_time_duration + self.time_elapsed = msg.game_elapsed_time diff --git a/ros_ws/src/ai_scheduler/src/scheduler_communication.py b/ros_ws/src/ai_scheduler/src/scheduler_communication.py new file mode 100644 index 0000000..d13343f --- /dev/null +++ b/ros_ws/src/ai_scheduler/src/scheduler_communication.py @@ -0,0 +1,166 @@ +# -*- coding: utf-8 -*- +import time +import rospy +import actionlib + +import ai_scheduler.msg +import ai_scheduler.srv + +import navigation_navigator.msg +import movement_actuators.msg +import movement_actuators.srv +import memory_map.srv +import ai_game_manager.srv +import ai_game_manager.srv +import drivers_ard_hmi.msg +import drivers_ard_asserv.srv +import drivers_ard_asserv.msg +import drivers_ax12.msg + +class RequestTypes(object): + PUB_MSG = 0 + SUB_MSG = 1 + SERVICE = 2 + ACTION = 3 + + SERVERS = None + + @staticmethod + def init(): + RequestTypes.SERVERS = { + "/ai/scheduler/score": (RequestTypes.PUB_MSG, ai_scheduler.msg.AIScore), + "/ai/game_manager/set_status": (RequestTypes.SERVICE, ai_game_manager.srv.SetStatus), + "/ai/game_manager/set_timer": (RequestTypes.SERVICE, ai_game_manager.srv.SetTimer), + "/ai/game_manager/delay": (RequestTypes.SERVICE, ai_game_manager.srv.Delay), + + "/memory/map/get": (RequestTypes.SERVICE, memory_map.srv.MapGet), + "/memory/map/set": (RequestTypes.SERVICE, memory_map.srv.MapSet), + "/memory/map/transfer": (RequestTypes.SERVICE, memory_map.srv.MapTransfer), + + "/navigation/navigator/goto_action": (RequestTypes.ACTION, navigation_navigator.msg.DoGotoAction, navigation_navigator.msg.DoGotoGoal), + "/navigation/navigator/gotowaypoint_action": (RequestTypes.ACTION, navigation_navigator.msg.DoGotoWaypointAction, navigation_navigator.msg.DoGotoWaypointGoal), + "/movement/actuators/dispatch": (RequestTypes.ACTION, movement_actuators.msg.DispatchAction, movement_actuators.msg.DispatchGoal), + "/movement/actuators/barrel": (RequestTypes.ACTION, movement_actuators.msg.BarrelAction, movement_actuators.msg.BarrelGoal), + "/movement/actuators/arm": (RequestTypes.ACTION, movement_actuators.msg.ArmAction, movement_actuators.msg.ArmGoal), + "/movement/actuators/activate_canon": (RequestTypes.SERVICE, movement_actuators.srv.ActivateCanon), + + "/drivers/ard_asserv/set_pos": (RequestTypes.SERVICE, drivers_ard_asserv.srv.SetPos), + "/drivers/ard_asserv/pwm": (RequestTypes.SERVICE, drivers_ard_asserv.srv.Pwm), + "/drivers/ard_asserv/goto_action": (RequestTypes.ACTION, drivers_ard_asserv.msg.DoGotoAction, drivers_ard_asserv.msg.DoGotoGoal), + "/drivers/ax12": (RequestTypes.ACTION, drivers_ax12.msg.Ax12CommandAction, drivers_ax12.msg.Ax12CommandGoal), + + "/feedback/ard_hmi/ros_event": (RequestTypes.PUB_MSG, drivers_ard_hmi.msg.ROSEvent), + "/feedback/ard_hmi/hmi_event": (RequestTypes.SUB_MSG, drivers_ard_hmi.msg.HMIEvent)} + + @staticmethod + def getRequestType(dest): + return RequestTypes.SERVERS[dest][0] + @staticmethod + def getRequestClass(dest): + return RequestTypes.SERVERS[dest][1] + @staticmethod + def getActionGoalClass(dest): + return RequestTypes.SERVERS[dest][2] + +class AICommunication(): + DEFAULT_SUB_MSG_TIMEOUT = 5 + DEFAULT_ACTION_TIMEOUT = 20 + + _sub_msg_res = None + + def __init__(self): + RequestTypes.init() + self._cached_publishers = {} + + def SendRequest(self, dest, params, timeout=None, callback=None): + start_time = time.time() + if dest in RequestTypes.SERVERS: + if RequestTypes.getRequestType(dest) == RequestTypes.PUB_MSG: + response = self._pub_msg(dest, RequestTypes.getRequestClass(dest), params) + elif RequestTypes.getRequestType(dest) == RequestTypes.SUB_MSG: + response = self._sub_msg(dest, RequestTypes.getRequestClass(dest), timeout) + elif RequestTypes.getRequestType(dest) == RequestTypes.SERVICE: + response = self._send_service(dest, RequestTypes.getRequestClass(dest), params) + elif RequestTypes.getRequestType(dest) == RequestTypes.ACTION: + response = self._send_blocking_action(dest, RequestTypes.getRequestClass(dest), + RequestTypes.getActionGoalClass(dest), params, timeout) + if callback is not None: + callback(response, time.time() - start_time) + else: + return response + else: + raise ValueError, "Message destination '{}' was not recognized. Has it been added to scheduler_communication.py definition dict, or mispelled ?".format(dest) + + def _pub_msg(self, dest, msg_class, params): + try: + if dest not in self._cached_publishers: + rospy.loginfo("Creating and caching new publisher to '{}'.".format(dest)) + self._cached_publishers[dest] = rospy.Publisher(dest, msg_class, queue_size=10) + time.sleep(0.05) + + pub = self._cached_publishers[dest] + pub.publish(**params) + rospy.loginfo("Published to topic '{}'.".format(dest)) + return True + except Exception as e: + rospy.logerr("Publishing to topic '{}' failed.".format(dest)) + return False + + def _sub_msg(self, dest, msg_class, timeout=None): + self._sub_msg_res = None + rospy.Subscriber(dest, msg_class, self._sub_msg_callback) + + if timeout is None: + timeout = AICommunication.DEFAULT_SUB_MSG_TIMEOUT + rospy.loginfo("Waiting for message on topic '{}' for {}s...".format(dest, timeout)) + + s = time.time() + while not self._sub_msg_res and (time.time() - s < timeout): #TODO continue to search until response condition matches + time.sleep(0.02) + if self._sub_msg_res: + rospy.loginfo("Got message from topic '{}'.".format(dest)) + else: + rospy.logerr("Didn't receive any message from '{}' in {} seconds.".format(dest, timeout)) + return self._sub_msg_res + def _sub_msg_callback(self, msg): + self._sub_msg_res = msg + + def _send_service(self, dest, srv_class, params): + try: # Handle a timeout in case one node doesn't respond + server_wait_timeout = 2 + rospy.logdebug("Waiting for service %s for %d seconds" % (dest, server_wait_timeout)) + rospy.wait_for_service(dest, timeout=server_wait_timeout) + except rospy.ROSException: + return False + + rospy.loginfo("Sending service request to '{}'...".format(dest)) + service = rospy.ServiceProxy(dest, srv_class) + response = service(srv_class._request_class(**params)) #TODO rospy can't handle timeout, solution? + if response is not None: + rospy.loginfo("Got service response from '{}'.".format(dest)) + else: + rospy.logerr("Service call response from '{}' is null.".format(dest)) + return response + + def _send_blocking_action(self, dest, action_class, goal_class, params, timeout=None): + client = actionlib.SimpleActionClient(dest, action_class) + SERVER_WAIT_TIMEOUT = 2 + rospy.loginfo("Waiting for action server on '{}'...".format(dest)) + if client.wait_for_server(timeout = rospy.Duration(SERVER_WAIT_TIMEOUT)): + rospy.loginfo("Action server available, sending action goal...") + client.send_goal(goal_class(**params)) + + if timeout is None: + timeout = AICommunication.DEFAULT_ACTION_TIMEOUT + + rospy.loginfo("Waiting for action result with {}s timeout...".format(timeout)) + client.wait_for_result(timeout = rospy.Duration(timeout)) + response = client.get_result() + if response is not None: + rospy.loginfo("Got action result.") + else: + rospy.logerr("Action response timeout reached!") + return response + else: + rospy.logerr("Action wait_for_server timeout reached!") + return False diff --git a/ros_ws/src/ai_scheduler/src/scheduler_node.py b/ros_ws/src/ai_scheduler/src/scheduler_node.py new file mode 100755 index 0000000..8b56694 --- /dev/null +++ b/ros_ws/src/ai_scheduler/src/scheduler_node.py @@ -0,0 +1,65 @@ +#!/usr/bin/python +import time +import rospy +from scheduler_communication import AICommunication +from ai import RobotAI, GameProperties + +from drivers_ard_hmi.msg import SetStrategies, SetTeams, HMIEvent +from ai_game_manager.srv import SetStatus +from ai_game_manager import StatusServices + + +class AINode(): + def __init__(self): + self.DepartmentName, self.PackageName = "ai", "scheduler" + + rospy.init_node(self.PackageName, log_level = rospy.INFO) + + self.AI = RobotAI() + self.AI.load_game_properties() # fetch available strategies and teams + + self._hmi_init = False + self._ai_start_request = False + + self._strat_publisher = rospy.Publisher("/feedback/ard_hmi/set_strategies", SetStrategies, queue_size=10) + self._teams_publisher = rospy.Publisher("/feedback/ard_hmi/set_teams", SetTeams, queue_size=10) + rospy.Subscriber("/feedback/ard_hmi/hmi_event", HMIEvent, self.on_hmi_event) + + # Sending init status to ai/game_manager, subscribing to game_status status pub. + status_services = StatusServices(self.DepartmentName, self.PackageName, self.on_arm) + status_services.ready(True) # Tell ai/game_manager the node initialized successfuly. + + r = rospy.Rate(5) + while not rospy.is_shutdown(): + if not self._hmi_init: + self.send_game_properties() + if self._ai_start_request: + self.AI.start(AICommunication()) + self._ai_start_request = False + r.sleep() + + def send_game_properties(self): # Happens from init until HMI gets detected. + self._strat_publisher.publish(GameProperties.AVAILABLE_STRATEGIES) + self._teams_publisher.publish(GameProperties.AVAILABLE_TEAMS) + + def on_hmi_event(self, req): # Happens immediately after user clicks on 'ARM'on hmi. + if req.event == req.EVENT_HMI_INITIALIZED: + time.sleep(0.5) + self._hmi_init = True + if req.event == req.EVENT_START: + GameProperties.CURRENT_STRATEGY = GameProperties.AVAILABLE_STRATEGIES[req.chosen_strategy_id] + GameProperties.CURRENT_TEAM = GameProperties.AVAILABLE_TEAMS[req.chosen_team_id] + rospy.set_param("/current_strategy", GameProperties.CURRENT_STRATEGY) + rospy.set_param("/current_team", GameProperties.CURRENT_TEAM) + rospy.logdebug("Strategy ({}) and team ({}) params set !".format(GameProperties.CURRENT_STRATEGY, GameProperties.CURRENT_TEAM)) + + def on_arm(self, req): # Happens when 'ai/game_manager' launches ARM event (when user clicks on ARM on hmi). + rospy.loginfo("[AI] Starting actions ! Strategy '{}' and team '{}'.".format(GameProperties.CURRENT_STRATEGY, + GameProperties.CURRENT_TEAM)) + self._ai_start_request = True # Start the strategy + +''' +PACKAGE STARTING POINT HERE +''' +if __name__ == "__main__": + node = AINode() diff --git a/ros_ws/src/ai_scheduler/srv/test.srv b/ros_ws/src/ai_scheduler/srv/test.srv new file mode 100644 index 0000000..3f75578 --- /dev/null +++ b/ros_ws/src/ai_scheduler/srv/test.srv @@ -0,0 +1,5 @@ +geometry_msgs/Pose2D position +float64 speed +string goal_type +--- +bool success diff --git a/ros_ws/src/drivers_ard_asserv/CMakeLists.txt b/ros_ws/src/drivers_ard_asserv/CMakeLists.txt new file mode 100644 index 0000000..a1ff79c --- /dev/null +++ b/ros_ws/src/drivers_ard_asserv/CMakeLists.txt @@ -0,0 +1,75 @@ +cmake_minimum_required(VERSION 2.8.3) +project(drivers_ard_asserv) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs + geometry_msgs + message_generation + genmsg + actionlib_msgs + actionlib + drivers_port_finder + ai_game_manager + memory_map + tf + tf2_ros +) + +## Generate messages +add_message_files( + FILES + RobotSpeed.msg +) + +## Generate services +add_service_files( + FILES + Parameters.srv + EmergencyStop.srv + Goto.srv + Management.srv + Pwm.srv + SetPos.srv + Speed.srv +) + +## Generate actions +add_action_files(DIRECTORY action FILES DoGoto.action) + +# Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs + geometry_msgs + actionlib_msgs +) + +catkin_package( + CATKIN_DEPENDS message_runtime +) + +include_directories( + ${catkin_INCLUDE_DIRS} +) + +############# +## Install ## +############# + +catkin_install_python( + PROGRAMS src/ard_asserv_node.py src/marker_server_node.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install ( + DIRECTORY src/asserv + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + PATTERN "*.pyc" EXCLUDE +) \ No newline at end of file diff --git a/ros_ws/src/drivers_ard_asserv/README.md b/ros_ws/src/drivers_ard_asserv/README.md new file mode 100644 index 0000000..b678415 --- /dev/null +++ b/ros_ws/src/drivers_ard_asserv/README.md @@ -0,0 +1,7 @@ +=== Installation === + +Install pip and pyserial + +sudo apt-get install python-pip + +pip install pyserial \ No newline at end of file diff --git a/ros_ws/src/drivers_ard_asserv/action/DoGoto.action b/ros_ws/src/drivers_ard_asserv/action/DoGoto.action new file mode 100644 index 0000000..c49b832 --- /dev/null +++ b/ros_ws/src/drivers_ard_asserv/action/DoGoto.action @@ -0,0 +1,20 @@ +# Goto action is to tell the asserv to move to some place. The result will be sent when the robot is arrived or if there is a problem. +# Define the goal +# GOTO command, x and y in m, a in radians +uint8 GOTO = 0 +uint8 GOTOA = 1 +uint8 ROT = 2 +uint8 ROTNOMODULO = 3 +uint8 mode + +uint8 DIRECTION_BACKWARD = 0 +uint8 DIRECTION_FORWARD = 1 +uint8 direction + +geometry_msgs/Pose2D position +string position_waypoint +--- +# Result : true if arrived, false if not +bool result +--- +# Feedback message : nothing (robot pose is obtained via topics) \ No newline at end of file diff --git a/ros_ws/src/drivers_ard_asserv/msg/RobotSpeed.msg b/ros_ws/src/drivers_ard_asserv/msg/RobotSpeed.msg new file mode 100644 index 0000000..19aab59 --- /dev/null +++ b/ros_ws/src/drivers_ard_asserv/msg/RobotSpeed.msg @@ -0,0 +1,6 @@ +# Speed of the wheels +float32 pwm_speed_left +float32 pwm_speed_right +float32 linear_speed +float32 wheel_speed_left +float32 wheel_speed_right \ No newline at end of file diff --git a/ros_ws/src/drivers_ard_asserv/package.xml b/ros_ws/src/drivers_ard_asserv/package.xml new file mode 100644 index 0000000..9a43c8f --- /dev/null +++ b/ros_ws/src/drivers_ard_asserv/package.xml @@ -0,0 +1,26 @@ + + + drivers_ard_asserv + 0.0.1 + + The package that manages a serial communication between the arduino in charge of the asserv and the ros system. It provides services that redirects commands to the arduino, and topics that convey information from it. + + GPLv3 + Thomas Fuhrmann + UTCoupe + catkin + message_generation + rospy + std_msgs + geometry_msgs + actionlib + actionlib_msgs + drivers_port_finder + ai_game_manager + memory_map + tf + tf2_ros + message_runtime + + + diff --git a/ros_ws/src/drivers_ard_asserv/src/ard_asserv_node.py b/ros_ws/src/drivers_ard_asserv/src/ard_asserv_node.py new file mode 100755 index 0000000..c9f21bf --- /dev/null +++ b/ros_ws/src/drivers_ard_asserv/src/ard_asserv_node.py @@ -0,0 +1,362 @@ +#!/usr/bin/env python + +import rospy +from geometry_msgs.msg import Pose2D, TransformStamped +import actionlib +from drivers_ard_asserv.srv import * +from drivers_ard_asserv.msg import * +from drivers_port_finder.srv import * +import asserv +from ai_game_manager import StatusServices +from ai_game_manager.msg import GameStatus +from memory_map.srv import FillWaypoint +from memory_map.msg import Waypoint +import tf +import tf2_ros + +__author__ = "Thomas Fuhrmann" +__date__ = 21/10/2017 + +NODE_NAME = "ard_asserv" +GET_PORT_SERVICE_NAME = "/drivers/port_finder/get_port" +GET_MAP_SERVICE_NAME = "/memory/map/fill_waypoint" +GET_PORT_SERVICE_TIMEOUT = 25 # in seconds +GET_MAP_SERVICE_TIMEOUT = 15 # in seconds + + +class Asserv: + """ + The Asserv class manages the driver node for communication with asserv (real using the Arduino or in simulation). + As this node is used as an interface, it gets orders to send on ROS services. The state of the robot is published on ROS topics. + This node handles an action (see actionlib) for the Goto movement. + The selection of asserv type (real or simu) is made using serial port data. If the Arduino asserv is found, the real asserv is used. + If no Arduino asserv is found, the node will launch a simulated asserv. + """ + def __init__(self): + rospy.logdebug("[ASSERV] Starting asserv_node.") + # This dictionary stores the goals received by the DoGoto action and which are currently in processing + self._goals_dictionary = {} + # This dictionary stores the goals received by the Goto service and which are currently in processing + self._goto_srv_dictionary = {} + # Unique ID given to the received goal to manage it + self._goal_id_counter = 0 + # Instance of the asserv object (simu or real) + self._asserv_instance = None + # Flag to know if the system has been halted (end of game) + self._is_halted = False + # Init ROS stuff + rospy.init_node(NODE_NAME, anonymous=False, log_level=rospy.INFO) + self._pub_robot_pose = rospy.Publisher("/drivers/" + NODE_NAME + "/pose2d", Pose2D, queue_size=5) + self._pub_robot_speed = rospy.Publisher("/drivers/" + NODE_NAME + "/speed", RobotSpeed, queue_size=5) + self._srv_goto = rospy.Service("/drivers/" + NODE_NAME + "/goto", Goto, self._callback_goto) + self._srv_pwm = rospy.Service("/drivers/" + NODE_NAME + "/pwm", Pwm, self._callback_pwm) + self._srv_speed = rospy.Service("/drivers/" + NODE_NAME + "/speed", Speed, self._callback_speed) + self._srv_set_pos = rospy.Service("/drivers/" + NODE_NAME + "/set_pos", SetPos, self._callback_set_pos) + self._srv_emergency_stop = rospy.Service("/drivers/" + NODE_NAME + "/emergency_stop", EmergencyStop, self._callback_emergency_stop) + self._srv_params = rospy.Service("/drivers/" + NODE_NAME + "/parameters", Parameters, self._callback_asserv_param) + self._srv_management = rospy.Service("/drivers/" + NODE_NAME + "/management", Management, self._callback_management) + self._act_goto = actionlib.ActionServer("/drivers/" + NODE_NAME + "/goto_action", DoGotoAction, self._callback_action_goto, auto_start=False) + self._pub_tf_odom = tf2_ros.TransformBroadcaster() + self._act_goto.start() + self._srv_client_map_fill_waypoints = None + try: + rospy.wait_for_service(GET_PORT_SERVICE_NAME, GET_PORT_SERVICE_TIMEOUT) + srv_client_get_port = rospy.ServiceProxy(GET_PORT_SERVICE_NAME, GetPort) + arduino_port = srv_client_get_port("ard_asserv").port + except rospy.ROSException as exc: + rospy.loginfo("Port_finder has not been launched...") + arduino_port = "" + rospy.loginfo("Port_finder returns value : " + arduino_port) + + is_simu = False + if arduino_port == "": + rospy.logwarn("[ASSERV] Creation of the simu asserv.") + self._asserv_instance = asserv.AsservSimu(self) + is_simu = True + else: + rospy.loginfo("[ASSERV] Creation of the real asserv.") + self._asserv_instance = asserv.AsservReal(self, arduino_port) + try: + rospy.wait_for_service(GET_MAP_SERVICE_NAME, GET_MAP_SERVICE_TIMEOUT) + self._srv_client_map_fill_waypoints = rospy.ServiceProxy(GET_MAP_SERVICE_NAME, FillWaypoint) + rospy.logdebug("Memory_map has been found.") + except rospy.ROSException as exc: + rospy.logwarn("Memory_map has not been launched...") + # Tell ai/game_manager the node initialized successfuly. + StatusServices("drivers", "ard_asserv", None, self._callback_game_status).ready(not is_simu) + + self._asserv_instance.start() + + # goal_id generated by node, reached to know if the gial as been reached + def goal_reached(self, goal_id, reached): + if reached: + rospy.loginfo("[ASSERV] Goal id {}, has been reached.".format(goal_id)) + else: + rospy.logwarn("[ASSERV] Goal id {}, has NOT been reached.".format(goal_id)) + result = DoGotoResult(reached) + if goal_id in self._goals_dictionary: + self._goals_dictionary[goal_id].set_succeeded(result) + del self._goals_dictionary[goal_id] + elif goal_id in self._goto_srv_dictionary: + del self._goto_srv_dictionary[goal_id] + + # robot_pose is a Pose2d structure + def send_robot_position(self, robot_pose): + self._pub_robot_pose.publish(robot_pose) + # Send the position using tf + t = TransformStamped() + t.header.stamp = rospy.Time.now() + t.header.frame_id = "map" + t.child_frame_id = "odom" + t.transform.translation.x = robot_pose.x + t.transform.translation.y = robot_pose.y + t.transform.translation.z = 0.0 + q = tf.transformations.quaternion_from_euler(0, 0, robot_pose.theta) + t.transform.rotation.x = q[0] + t.transform.rotation.y = q[1] + t.transform.rotation.z = q[2] + t.transform.rotation.w = q[3] + self._pub_tf_odom.sendTransform(t) + + # robot_speed is a RobotSpeed structure + def send_robot_speed(self, robot_speed): + self._pub_robot_speed.publish(robot_speed) + + def _callback_goto(self, request): + """ + Callback of the Goto service. + @param request: Service request + @type request: GotoRequest + @return: True if request has been processed, false otherwise + @rtype: GotoResponse + """ + rospy.logdebug("[ASSERV] Received a request (goto service).") + # TODO manage the direction + response = self._process_goto_order(self._goal_id_counter, request.mode, request.position.x, request.position.y, request.position.theta, 1) + if response: + self._goto_srv_dictionary[self._goal_id_counter] = "" + self._goal_id_counter += 1 + else: + rospy.logerr("[ASSERV] Service GOTO has failed... Mode probably does not exist.") + return GotoResponse(response) + + def _callback_set_pos(self, request): + """ + Callback of the SetPos service. + @param request: Service request + @type request: SetPosRequest + @return: True if request has been processed, false otherwise + @rtype: SetPosResponse + """ + set_position = Pose2D(0, 0, 0) + if request.position_waypoint == "": + rospy.logdebug("[ASSERV] Received a request (set_pos service).") + set_position = request.position + else: + rospy.logdebug("[ASSERV] Received a request (set_pos service), using waypoint") + if self._srv_client_map_fill_waypoints is not None: + wpt = Waypoint(name=request.position_waypoint) + wpt.has_angle = True + set_position = self._srv_client_map_fill_waypoints.call(wpt).filled_waypoint.pose + else: + rospy.logwarn("[ASSERV] Received a waypoint request but memory_map seems not to be launched...") + if self._asserv_instance: + ret_value = self._asserv_instance.set_pos(set_position.x, set_position.y, set_position.theta) + else: + ret_value = False + return SetPosResponse(ret_value) + + def _callback_pwm(self, request): + """ + Callback of the Pwm service. + @param request: Service request + @type request: PwmRequest + @return: True if request has been processed, false otherwise + @rtype: PwmResponse + """ + rospy.logdebug("[ASSERV] Received a request (pwm service).") + if self._asserv_instance: + ret_value = self._asserv_instance.pwm(request.left, request.right, request.duration) + else: + ret_value = False + return PwmResponse(ret_value) + + def _callback_speed(self, request): + """ + Callback of the Speed service. + @param request: Service request + @type request: SpeedRequest + @return: True if request has been processed, false otherwise + @rtype: SpeedResponse + """ + rospy.logdebug("[ASSERV] Received a request (speed service).") + if self._asserv_instance: + ret_value = self._asserv_instance.speed(request.linear, request.angular, request.duration) + else: + ret_value = False + return SpeedResponse(ret_value) + + def _callback_emergency_stop(self, request): + """ + Callback of the EmergencyStop service. + @param request: Service request + @type request: EmergencyStopRequest + @return: True if request has been processed, false otherwise + @rtype: EmergencyStopResponse + """ + rospy.logdebug("[ASSERV] Received a request (emergency_stop service).") + if self._asserv_instance: + ret_value = self._asserv_instance.set_emergency_stop(request.enable) + else: + ret_value = False + return EmergencyStopResponse(ret_value) + + def _callback_asserv_param(self, request): + """ + Callback of the Parameters service. + @param request: Service request + @type request: ParametersRequest + @return: True if request has been processed, false otherwise + @rtype: ParametersResponse + """ + rospy.logdebug("[ASSERV] Received a request (parameters service).") + response = True + if self._asserv_instance: + if request.mode == request.SPDMAX: + response = self._asserv_instance.set_max_speed(request.spd, request.spd_ratio) + elif request.mode == request.ACCMAX: + response = self._asserv_instance.set_max_accel(request.acc) + elif request.mode == request.PIDRIGHT: + # TODO manage left and right + response = self._asserv_instance.set_pid(request.p, request.i, request.d) + elif request.mode == request.PIDLEFT: + # TODO manage left and right + response = self._asserv_instance.set_pid(request.p, request.i, request.d) + elif request.mode == request.PIDALL: + response = self._asserv_instance.set_pid(request.p, request.i, request.d) + else: + response = False + rospy.logerr("[ASSERV] Parameter mode %d does not exists...", request.mode) + else: + response = False + return ParametersResponse(response) + + def _callback_management(self, request): + """ + Callback of the Management service. + @param request: Service request + @type request: ManagementRequest + @return: True if request has been processed, false otherwise + @rtype: ManagementResponse + """ + rospy.logdebug("[ASSERV] Received a request (management service).") + response = True + if self._asserv_instance: + if request.mode == request.KILLG: + response = self._asserv_instance.kill_goal() + elif request.mode == request.CLEANG: + response = self._asserv_instance.clean_goals() + # Delete all internal goals + for goal in self._goals_dictionary.values(): + goal.set_canceled() + self._goals_dictionary.clear() + elif request.mode == request.PAUSE: + response = self._asserv_instance.pause(True) + elif request.mode == request.RESUME: + response = self._asserv_instance.pause(False) + elif request.mode == request.RESET_ID: + response = self._asserv_instance.reset_id() + else: + response = False + rospy.logerr("[ASSERV] Management mode %d does not exists...", request.mode) + else: + response = False + return ManagementResponse(response) + + def _callback_action_goto(self, goal_handled): + """ + Callback of the DoGoto action. + @param goal_handled: Goal handler corresponding to the received action + @type goal_handled: ServerGoalHandle + """ + if not self._is_halted: + pos = Pose2D(0, 0, 0) + if goal_handled.get_goal().position_waypoint == "": + rospy.logdebug("[ASSERV] Received a request (dogoto action).") + pos = goal_handled.get_goal().position + else: + rospy.logdebug("[ASSERV] Received a request (dogoto action), using waypoint") + if self._srv_client_map_fill_waypoints is not None: + wpt = Waypoint(name=goal_handled.get_goal().position_waypoint) + wpt.has_angle = True + pos = self._srv_client_map_fill_waypoints.call(wpt).filled_waypoint.pose + else: + rospy.logwarn("[ASSERV] Received a waypoint request but memory_map seems not to be launched...") + goal_handled.set_rejected() + return + + if self._process_goto_order(self._goal_id_counter, goal_handled.get_goal().mode, + pos.x, pos.y, pos.theta, + goal_handled.get_goal().direction): + goal_handled.set_accepted() + self._goals_dictionary[self._goal_id_counter] = goal_handled + self._goal_id_counter += 1 + else: + rospy.logerr("[ASSERV] Action GOTO has failed... Mode probably does not exist.") + else: + goal_handled.set_rejected() + rospy.logwarn("[ASSERV] Action GOTO can not be accepted, asserv has been halted.") + + def _process_goto_order(self, goal_id, mode, x, y, a, direction): + """ + Processes the goto order, coming from service or action. + @param mode: Mode of the Goto order (see Goto.srv or DoGoto.action files) + @type mode: string + @param x: X coordinate (in meters) + @type x: float64 + @param y: Y coordinate (in meters) + @type y: float64 + @param a: Angle (in radians) + @type a: float64 + @return: True if order sent, false otherwise + @rtype: bool + """ + to_return = True + if self._asserv_instance: + if mode == GotoRequest.GOTO: + rospy.loginfo("[ASSERV] Accepting goal GOTO (id = {}, x = {}, y = {}).".format(goal_id, x, y)) + to_return = self._asserv_instance.goto(goal_id, x, y, direction) + elif mode == GotoRequest.GOTOA: + rospy.loginfo("[ASSERV] Accepting goal GOTOA (id = {}, x = {}, y = {}, a = {}).".format(goal_id, x, y, a)) + to_return = self._asserv_instance.gotoa(goal_id, x, y, a, direction) + elif mode == GotoRequest.ROT: + rospy.loginfo("[ASSERV] Accepting goal ROT (id = {}, a = {}).".format(goal_id, a)) + to_return = self._asserv_instance.rot(goal_id, a, False) + elif mode == GotoRequest.ROTNOMODULO: + rospy.loginfo("[ASSERV] Accepting goal ROT NOMODULO (id = {}, a = {}).".format(goal_id, a)) + to_return = self._asserv_instance.rot(goal_id, a, True) + else: + to_return = False + rospy.logerr("[ASSERV] GOTO mode %d does not exists...", mode) + else: + to_return = False + return to_return + + def _callback_game_status(self, msg): + if not self._is_halted and msg.game_status == GameStatus.STATUS_HALT: + self._is_halted = True + # Halt the system, emergency stop + clean all goals + if self._asserv_instance: + self._asserv_instance.set_emergency_stop(True) + management_msg = ManagementRequest() + management_msg.mode = ManagementRequest.CLEANG + self._callback_management(management_msg) + rospy.loginfo("Asserv successfuly stopped") + elif self._is_halted and msg.game_status != GameStatus.STATUS_HALT: + self._is_halted = False + if self._asserv_instance: + self._asserv_instance.set_emergency_stop(False) + + +if __name__ == "__main__": + Asserv() diff --git a/ros_ws/src/drivers_ard_asserv/src/asserv/__init__.py b/ros_ws/src/drivers_ard_asserv/src/asserv/__init__.py new file mode 100644 index 0000000..29e75de --- /dev/null +++ b/ros_ws/src/drivers_ard_asserv/src/asserv/__init__.py @@ -0,0 +1,7 @@ +#!/usr/bin/env python +# -*-coding:Utf-8 -* + +from asserv_abstract import * +from asserv_real import * +from asserv_simu import * +from protocol_parser import * diff --git a/ros_ws/src/drivers_ard_asserv/src/asserv/asserv_abstract.py b/ros_ws/src/drivers_ard_asserv/src/asserv/asserv_abstract.py new file mode 100644 index 0000000..3c3f386 --- /dev/null +++ b/ros_ws/src/drivers_ard_asserv/src/asserv/asserv_abstract.py @@ -0,0 +1,67 @@ +#!/usr/bin/env python + +import rospy + +__author__ = "Thomas Fuhrmann" +__date__ = 16/12/2017 + + +class AsservAbstract: + def __init__(self, asserv_node): + self._node = asserv_node + + def goto(self, goal_id, x, y, direction): + rospy.logerr("AsservAbstract is abstract !") + return False + + def gotoa(self, goal_id, x, y, a, direction): + rospy.logerr("AsservAbstract is abstract !") + return False + + def rot(self, goal_id, a, no_modulo): + rospy.logerr("AsservAbstract is abstract !") + return False + + def pwm(self, left, right, duration): + rospy.logerr("AsservAbstract is abstract !") + return False + + def speed(self, linear, angular, duration): + rospy.logerr("AsservAbstract is abstract !") + return False + + def set_emergency_stop(self, stop): + rospy.logerr("AsservAbstract is abstract !") + return False + + def kill_goal(self): + rospy.logerr("AsservAbstract is abstract !") + return False + + def clean_goals(self): + rospy.logerr("AsservAbstract is abstract !") + return False + + def pause(self, pause): + rospy.logerr("AsservAbstract is abstract !") + return False + + def reset_id(self): + rospy.logerr("AsservAbstract is abstract !") + return False + + def set_max_speed(self, speed, speed_ratio): + rospy.logerr("AsservAbstract is abstract !") + return False + + def set_max_accel(self, accel): + rospy.logerr("AsservAbstract is abstract !") + return False + + def set_pid(self, p, i, d): + rospy.logerr("AsservAbstract is abstract !") + return False + + def set_pos(self, x, y, a): + rospy.logerr("AsservAbstract is abstract !") + return False diff --git a/ros_ws/src/drivers_ard_asserv/src/asserv/asserv_real.py b/ros_ws/src/drivers_ard_asserv/src/asserv/asserv_real.py new file mode 100644 index 0000000..48c64e8 --- /dev/null +++ b/ros_ws/src/drivers_ard_asserv/src/asserv/asserv_real.py @@ -0,0 +1,324 @@ +#!/usr/bin/env python + +import Queue +import serial +import threading +import os +from math import pi +import rospy +from geometry_msgs.msg import Pose2D +from drivers_ard_asserv.msg import RobotSpeed +from asserv_abstract import * +import protocol_parser +import math + +__author__ = "Thomas Fuhrmann" +__date__ = 16/12/2017 + +ASSERV_ERROR_POSITION = 0.01 # in meters +ASSERV_ERROR_ANGLE = 0.15 # in radians +POSITION_REACHED_CHECK_DELAY = 0.25 # in seconds +GOTOA_POS_ERROR_MULTIPLIER = 5 + + +class AsservReal(AsservAbstract): + def __init__(self, asserv_node, port): + AsservAbstract.__init__(self, asserv_node) + rospy.loginfo("AsservReal") + # Dictionary containing the list of orders which are interpreted by the Arduino (do not modify this dictionary !) + self._orders_dictionary = protocol_parser.protocol_parse(os.environ['UTCOUPE_WORKSPACE'] + "/arduino/common/asserv/protocol.h") + # Queue to store the received information from the Arduino + self._reception_queue = Queue.Queue() + # A queue is used to send data to prevent to send data too fast, which will result to concatenate two sending and making the Arduino crash + self._sending_queue = Queue.Queue() + # The order_id is sent to the asserv to identify the orders, it must be unique + self._order_id = 0 + # This dictionary stores the order_id with th goal_id to retrieve which goal has been completed + self._orders_id_dictionary = {} + # This list stores the last received ack ID from Arduino (a list to avoid zapping an ack). This list is used by the timer callback which check that position has been reached + self._last_received_id_dictionary = {} + # This is kind of a ack because the GOTOA order is transformed in GOTO + ROT in the Arduino asserv and this makes some trouble for goal reach check... + self._gotoa_second_goal_received = False + # Store the current position of robot, this one is the raw value returned by the asserv + self._robot_raw_position = Pose2D(0, 0, 0) + # Timer for the send of serial data to avoid that sending are buffered in the internal OS buffer (make the arduino crash) + self._tmr_serial_send = rospy.Timer(rospy.Duration(0.1), self._callback_timer_serial_send) + # Init the serial communication + # Flag to tell that the connected Arduino is started : we can send it orders + self._arduino_started_flag = False + # Serial object + self._serial_com = None + # Thread dedicated to the reception from the serial line + self._serial_receiver_thread = None + self._start_serial_com_line(port) + + def start(self): + """ + Starts the processing : feeding the reception queue. + """ + rospy.logdebug("[ASSERV] Node has correctly started.") + while not rospy.is_shutdown(): + if not self._reception_queue.empty(): + try: + self._process_received_data(self._reception_queue.get()) + self._reception_queue.task_done() + except KeyboardInterrupt: + break + rospy.sleep(0.001) + # Force the arduino to halt + self._halt() + + def goto(self, goal_id, x, y, direction): + if self._check_reached_position(x, y, False): + self._oneshot_timer = rospy.Timer(rospy.Duration(0.25), lambda e: self._node.goal_reached(goal_id, True), oneshot=True) + return True + + if direction == 0: # bugfix: -1 = BACKWARD, 0 = ANY + direction = -1 + self._send_serial_data(self._orders_dictionary['GOTO'], [str(int(round(x * 1000))), str(int(round(y * 1000))), str(direction)]) + # TODO make it proper + self._orders_id_dictionary[self._order_id - 1] = [goal_id, x, y] + return True + + def gotoa(self, goal_id, x, y, a, direction): + if self._check_reached_angle(a, False) and self._check_reached_position(x, y, GOTOA_POS_ERROR_MULTIPLIER, False): + self._oneshot_timer = rospy.Timer(rospy.Duration(0.25), lambda e: self._node.goal_reached(goal_id, True), oneshot=True) + return True + + if direction == 0: # bugfix: -1 = BACKWARD, 0 = ANY + direction = -1 + self._send_serial_data(self._orders_dictionary['GOTOA'], [str(int(round(x * 1000))), str(int(round(y * 1000))), str(int(round(a * 1000))), str(direction)]) + # TODO make it proper + self._orders_id_dictionary[self._order_id - 1] = [goal_id, x, y, a] + return True + + def rot(self, goal_id, a, no_modulo): + if self._check_reached_angle(a, False): + self._oneshot_timer = rospy.Timer(rospy.Duration(0.25), lambda e: self._node.goal_reached(goal_id, True), oneshot=True) + return True + + if no_modulo: + self._send_serial_data(self._orders_dictionary['ROTNOMODULO'], [str(int(round(a * 1000)))]) + else: + self._send_serial_data(self._orders_dictionary['ROT'], [str(int(round(a * 1000)))]) + # TODO make it proper + self._orders_id_dictionary[self._order_id - 1] = [goal_id, a] + return True + + def pwm(self, left, right, duration): + self._send_serial_data(self._orders_dictionary['PWM'], [str(left), str(right), str(duration)]) + return True + + def speed(self, linear, angular, duration): + self._send_serial_data(self._orders_dictionary['SPD'], [str(linear), str(angular), str(duration)]) + return True + + def set_emergency_stop(self, stop): + self._send_serial_data(self._orders_dictionary['SETEMERGENCYSTOP'], [str(int(stop))]) + return True + + def kill_goal(self): + self._send_serial_data(self._orders_dictionary['KILLG'], []) + return True + + def clean_goals(self): + self._send_serial_data(self._orders_dictionary['CLEANG'], []) + return True + + def pause(self, pause): + if pause: + self._send_serial_data(self._orders_dictionary['PAUSE'], []) + else: + self._send_serial_data(self._orders_dictionary['RESUME'], []) + return True + + def reset_id(self): + self._send_serial_data(self._orders_dictionary['RESET_ID'], []) + return True + + def set_max_speed(self, speed, speed_ratio): + self._send_serial_data(self._orders_dictionary['SPDMAX'], [str(speed), str(speed_ratio)]) + return True + + def set_max_accel(self, accel): + self._send_serial_data(self._orders_dictionary['ACCMAX'], [str(accel)]) + return True + + def set_pid(self, p, i, d): + # TODO manage lef and right + self._send_serial_data(self._orders_dictionary['PIDALL'], [str(int(round(p * 1000))), str(int(round(i * 1000))), str(int(round(d * 1000)))]) + # rospy.loginfo("Set pid sending : P = {}, I = {}, D = {}.".format(str(int(round(p * 1000))), str(int(round(i * 1000))), str(int(round(d * 1000))))) + return True + + def set_pos(self, x, y, a): + self._send_serial_data(self._orders_dictionary['SET_POS'], [str(int(round(x * 1000))), str(int(round(y * 1000))), str(int(round(a * 1000.0)))]) + return True + + def _start_serial_com_line(self, port): + """ + Initiate the serial communication line. + @param port: The port where the Arduino is connected (ex : "/devttyUSB0") + @type port: string + """ + try: + self._serial_com = serial.Serial(port, 57600, timeout=0.5) + self._serial_receiver_thread = threading.Thread(target=self._data_receiver) + self._serial_receiver_thread.start() + rospy.logdebug("[ASSERV] Serial communication line has started.") + except serial.SerialException: + rospy.logerr("[ASSERV] Port : " + port + " is not available, make sure you have plugged the Arduino.") + + def _data_receiver(self): + """ + Receives data from serial communication line and put it in a queue. + This function has to be called within a thread. + """ + while not rospy.is_shutdown(): + try: + received_data = self._serial_com.readline() + if received_data != "": + self._reception_queue.put(received_data.replace('\r\n', '')) + except KeyboardInterrupt: + break + except serial.SerialException as ex: + rospy.logwarn("[ASSERV] Serial read problem : " + ex.message) + rospy.sleep(0.001) + + def _process_received_data(self, data): + """ + This function is in charge of processing all the received data from the Arduino. + The data received have different types : + * Identification of the Arduino (ex : "asserv_gr") + * Status data (robot position, speed...), starting with ~ + * Orders ack, tells that an order has finished + * Debug string + @param data: The data received from Arduino + @type data: string + """ + # At init, start the Arduino + # TODO split data by _ to check if pr or gr and of asserv or others + if (not self._arduino_started_flag) and data.find("asserv") != -1: + rospy.logdebug("[ASSERV] Asserv Arduino identified, start it.") + self._sending_queue.put(self._orders_dictionary['START'] + ";0;\n") + # Received status + elif data.find("~") != -1: + rospy.logdebug("[ASSERV] Received status data.") + receied_data_list = data.split(";") + try: + angle = float(receied_data_list[4]) / 1000.0 + angle %= 2.0 * pi + robot_position = Pose2D(float(receied_data_list[2]) / 1000.0, float(receied_data_list[3]) / 1000.0, angle) + self._robot_raw_position = robot_position + self._node.send_robot_position(robot_position) + self._node.send_robot_speed(RobotSpeed(float(receied_data_list[5]), float(receied_data_list[6]), float(receied_data_list[7]) / 1000.0, float(receied_data_list[8]), float(receied_data_list[9]))) + except ValueError: + rospy.logwarn("[ASSERV] Received bad position from the robot, drop it...") + # Received order ack + elif data.find(";") >= 1 and data.find(";") <= 3: + # Special order ack, the first one concern the Arduino activation + if data.find("0;") == 0: + rospy.loginfo("[ASSERV] Arduino started") + self._arduino_started_flag = True + self._order_id += 1 + else: + rospy.logdebug("[ASSERV] Received order ack : %s", data) + ack_data = data.split(";") + try: + ack_id = int(ack_data[0]) + except ValueError: + ack_id = -1 + if ack_id in self._orders_id_dictionary: + rospy.logdebug("[ASSERV] Found key %d in order_id dictionary !", ack_id) + # Check if received a GOTOA ack because it needs extra processing + if len(self._orders_id_dictionary[ack_id]) == 4: + if self._gotoa_second_goal_received: + # Normal operation + self._gotoa_second_goal_received = False + else: + # Drop the received ack_id, we are waiting for the second one + # TODO better solution for -1 ? + ack_id = -1 + self._gotoa_second_goal_received = True + if ack_id != -1: + self._last_received_id_dictionary[ack_id] = self._orders_id_dictionary[ack_id] + del self._orders_id_dictionary[ack_id] + self._check_reached_timer = rospy.Timer(rospy.Duration(POSITION_REACHED_CHECK_DELAY), self._callback_timer_check_reached, oneshot=True) + else: + # Do nothing, some IDs are returned but do not correspond to a value in the dictionary. + rospy.logdebug("Received ack id ({}) but dropping it.".format(ack_id)) + else: + rospy.loginfo("%s", data) + + def _send_serial_data(self, order_type, args_list): + """ + This function sends data to the Arduino, using the specific protocol. + @param order_type: Type of the order to send (see protocol.h in arduino/common/asserv folder) + @type order_type: string containing an int + @param args_list: List of arguments, depending on the order + @type args_list: List + """ + if self._serial_com is not None: + args_list.insert(0, str(self._order_id)) + self._order_id += 1 + # TODO check if \n is necessary + '\n' + self._sending_queue.put(order_type + ";" + ";".join(args_list) + ";\n") + else: + rospy.logerr("[ASSERV] Try to send data but serial line is not connected...") + + def _callback_timer_serial_send(self, event): + if not self._sending_queue.empty(): + data_to_send = self._sending_queue.get() + rospy.logdebug("Sending data : " + data_to_send) + self._serial_com.write(data_to_send) + self._sending_queue.task_done() + + def _check_reached_angle(self, a, log=True): + result = (self._robot_raw_position.theta % (2 * math.pi) + 2 * math.pi < a % (2 * math.pi) + 2 * math.pi + ASSERV_ERROR_ANGLE) and \ + (self._robot_raw_position.theta % (2 * math.pi) + 2 * math.pi > a % (2 * math.pi) + 2 * math.pi - ASSERV_ERROR_ANGLE) + if result and log: + rospy.loginfo("Angle reached, own angle = {}, check angle = {} (angle margin : {})".format(self._robot_raw_position.theta, a, ASSERV_ERROR_ANGLE)) + elif log: + rospy.logwarn("Angle not reached, own angle = {}, check angle = {} (angle margin : {})".format(self._robot_raw_position.theta, a, ASSERV_ERROR_ANGLE)) + return result + + def _check_reached_position(self, x, y, ratio=1, log=True): + position_error = math.sqrt(math.pow(self._robot_raw_position.x - x, 2) + math.pow(self._robot_raw_position.y - y, 2)) + result = position_error < ASSERV_ERROR_POSITION * ratio + if result and log: + rospy.loginfo("Position reached, own pos = {}, {}, check pos = {}, {} (pos margin : {})".format(self._robot_raw_position.x, self._robot_raw_position.y, x, y, ASSERV_ERROR_POSITION * ratio)) + elif log: + rospy.logwarn("Position not reached, own pos = {}, {}, check pos = {}, {} (pos margin : {})".format(self._robot_raw_position.x, self._robot_raw_position.y, x, y, ASSERV_ERROR_POSITION * ratio)) + return result + + def _callback_timer_check_reached(self, event): + rospy.logdebug("In check reached timer callback") + result = True + reached = False + if len(self._last_received_id_dictionary) > 0: + received_id = self._last_received_id_dictionary.popitem() + goal_data = received_id[1] + # Check if the robot is arrived, otherwise this will tell that the robot is blocked (default behaviour of the asserv) + if len(goal_data) == 2: + reached = self._check_reached_angle(goal_data[1]) + elif len(goal_data) == 3: + reached = self._check_reached_position(goal_data[1], goal_data[2]) + elif len(goal_data) == 4: + reached = self._check_reached_position(goal_data[1], goal_data[2], GOTOA_POS_ERROR_MULTIPLIER) + reached &= self._check_reached_angle(goal_data[3]) + else: + rospy.logwarn("Goal id ack but not corresponding goal data...") + if not reached: + rospy.logdebug("Goal has not been reached !") + result = False + self._node.goal_reached(goal_data[0], result) + else: + rospy.logwarn("Check reached dict empty...") + + def _halt(self): + """ + This function is kind of a hack, it forces to send the HALT order to the arduino to halt the processing. + The reason is that some Arduino does not stop when serial communication is shutdown and this make the port_finder not work properly. + """ + self._send_serial_data(self._orders_dictionary['HALT'], []) + self._callback_timer_serial_send(rospy.timer.TimerEvent(rospy.Time(), rospy.Time(), rospy.Time(), rospy.Time(), 0.0)) + rospy.logwarn("Send halt to arduino") diff --git a/ros_ws/src/drivers_ard_asserv/src/asserv/asserv_simu.py b/ros_ws/src/drivers_ard_asserv/src/asserv/asserv_simu.py new file mode 100644 index 0000000..b70673e --- /dev/null +++ b/ros_ws/src/drivers_ard_asserv/src/asserv/asserv_simu.py @@ -0,0 +1,323 @@ +#!/usr/bin/env python + +import math +import rospy +from asserv_abstract import * +from geometry_msgs.msg import Pose2D +from drivers_ard_asserv.msg import RobotSpeed + +__author__ = "Thomas Fuhrmann & milesial" +__date__ = 19/04/2018 + +# TODO adapt to have realistic behaviour of the robot +SEND_POSE_RATE = 0.1 # in ms +SEND_SPEED_RATE = 0.1 # in ms +ASSERV_RATE = 0.05 # in ms +# To be more accurate on position error, the speed has to be lower otherwise the goal reached check will fail +ASSERV_ERROR_POSITION = 0.005 # in meters +ASSERV_ERROR_ANGLE = 0.05 # in radians +ASSERV_MINIMAL_SPEED = 0.05 # in m/s + +class AsservGoal(): + def __init__(self, goal_id, pose, has_angle=False, direction=1): + self.goal_id = goal_id + self.pose = pose + self.has_angle = has_angle + self.direction = direction + +class AsservSimu(AsservAbstract): + def __init__(self, asserv_node): + AsservAbstract.__init__(self, asserv_node) + rospy.loginfo("AsservSimu") + # Asserv management stuff + # The pose is in meters and rad + self._current_pose = Pose2D(0, 0, 0) + self._current_linear_speed = 0 + self._current_angular_speed = 0 + self._emergency_stop = False + # First element of tuple is the goal_id (see ROS actionlib) and the second one is the goal position + # The last one is if the asserv should use the angle + self._current_goal = None + # Distance between the robot and the goal when the current goal is set + self._current_goal_initial_distance = 0 + # Angle between the robot and the goal when the current goal is set + self._current_goal_initial_angle = 0 + # List of Pose2d corresponding to the goals + self._goals_list = [] + # Parameters + # The acceleration is in m/s^2 + self._max_acceleration = 0.1 + # The speed is in m/s + self._max_linear_speed = 0.5 + # The angular speed is in rad/s + self._max_angular_speed = 1.0 + # ROS stuff + self._tmr_pose_send = rospy.Timer(rospy.Duration(SEND_POSE_RATE), self._callback_timer_pose_send) + self._tmr_speed_send = rospy.Timer(rospy.Duration(SEND_SPEED_RATE), self._callback_timer_speed_send) + self._tmr_asserv_computation = rospy.Timer(rospy.Duration(ASSERV_RATE), self._callback_timer_asserv_computation) + + self._states = StatesManager() + + def start(self): + rospy.logdebug("[ASSERV] Node has correctly started in simulation mode.") + rospy.spin() + + def goto(self, goal_id, x, y, direction): + #rospy.loginfo("[ASSERV] Accepting goal (x = " + str(x) + ", y = " + str(y) + ").") + self._start_trajectory(goal_id, x, y, 0, direction) + return True + + def gotoa(self, goal_id, x, y, a, direction): + #rospy.loginfo("[ASSERV] Accepting goal (x = " + str(x) + ", y = " + str(y) + ", a = " + str(a) + ").") + self._start_trajectory(goal_id, x, y, a, direction, has_angle=True) + return True + + def rot(self, goal_id, a, no_modulo): + #rospy.loginfo("[ASSERV] Accepting goal (a = " + str(a) + ").") + self._current_pose.theta = a + self._node.goal_reached(goal_id, True) + return True + + def pwm(self, left, right, duration): + rospy.logwarn("Pwm is not implemented in simu yet...") + return False + + def speed(self, linear, angular, duration): + rospy.logerr("This function has not been implemented yet...") + return False + + def set_emergency_stop(self, stop): + #rospy.loginfo("[ASSERV] Emergency stop called : " + str(stop)) + self._emergency_stop = stop + if stop: + self._states.stop_movement() + self._current_linear_speed = 0 + self._current_angular_speed = 0 + elif self._current_goal is not None: + self._states.start_movement() + + return True + + def kill_goal(self): + rospy.logerr("This function has not been implemented yet...") + return False + + def clean_goals(self): + self._goals_list = [] + # TODO check to make it proper + self._current_goal = None + return True + + def pause(self, pause): + rospy.logerr("This function has not been implemented yet...") + return False + + def reset_id(self): + return True + + def set_max_speed(self, speed, speed_ratio): + self._max_linear_speed = speed + self._max_angular_speed = speed * speed_ratio + return True + + def set_max_accel(self, accel): + self._max_acceleration = accel + return True + + def set_pid(self, p, i, d): + return True + + def set_pos(self, x, y, a): + return_value = True + if self._current_linear_speed > 0 or self._current_angular_speed > 0: + rospy.logwarn("Setting pose wile moving is not a good idea...") + return_value = False + else: + self._current_pose = Pose2D(x, y, a) + return return_value + + + + def _callback_timer_asserv_computation(self, event): + # First check if a goal has to be got from the list + if self._current_goal is None and len(self._goals_list) > 0: + self._current_goal = self._goals_list.pop(0) + rospy.loginfo('[ASSERV] Starting a new goal : x = %f y = %f a = %f' % + (self._current_goal.pose.x, self._current_goal.pose.y, self._current_goal.pose.theta)) + + self._current_goal_initial_distance = math.sqrt(((self._current_goal.pose.x - self._current_pose.x) ** 2 + + (self._current_goal.pose.y - self._current_pose.y) ** 2)) + + self._current_goal_initial_angle = math.atan2(self._current_goal.pose.y - self._current_pose.y, + self._current_goal.pose.x - self._current_pose.x) + + self._states.start_movement() + + + if self._current_goal is None: return + + if self._states.should_rotate_to_align_traj( + curr_pose=self._current_pose, + goal_pose=self._current_goal.pose, + direction=self._current_goal.direction): + + self._states.state = State.ROTATING_TO_ALIGN_TRAJ + + elif self._states.should_move_on_traj( + curr_pose=self._current_pose, + goal_pose=self._current_goal.pose, + direction=self._current_goal.direction): + + + self._states.state = State.MOVING_ON_TRAJ + + acc_mult = self._states.get_needed_linear_acceleration_multiplier( + curr_pose=self._current_pose, + goal_pose=self._current_goal.pose, + initial_distance=self._current_goal_initial_distance, + direction=self._current_goal.direction + ) + self._accelerate(acc_mult) + self._update_current_pose_pos() + + elif self._states.should_rotate_after_traj( + curr_pose=self._current_pose, + goal_pose=self._current_goal.pose, + has_angle=self._current_goal.has_angle): + + self._states.state = State.ROTATING_AFTER_TRAJ + + else: + rospy.loginfo('[ASSERV] Goal position has been reached !') + self._states.stop_movement() + self._node.goal_reached(self._current_goal.goal_id, True) + self._current_angular_speed = 0 + self._current_linear_speed = 0 + self._current_goal = None + return + + rot_mult = self._states.get_needed_angle_speed_multiplier( + curr_pose=self._current_pose, + goal_pose=self._current_goal.pose, + direction=self._current_goal.direction + ) + self._rotate(rot_mult) + + def _accelerate(self, acc_mult=1): + self._current_linear_speed += acc_mult * self._max_acceleration * ASSERV_RATE + + if self._current_linear_speed < ASSERV_MINIMAL_SPEED: + self._current_linear_speed = ASSERV_MINIMAL_SPEED + + elif self._current_linear_speed > self._max_linear_speed: + self._current_linear_speed = self._max_linear_speed + + def _update_current_pose_pos(self): + current_x = self._current_pose.x + self._current_linear_speed * ASSERV_RATE * math.cos( + self._current_goal_initial_angle) + current_y = self._current_pose.y + self._current_linear_speed * ASSERV_RATE * math.sin( + self._current_goal_initial_angle) + + self._current_pose = Pose2D(current_x, current_y, self._current_pose.theta) + + def _rotate(self, speed_mult): + self._current_angular_speed = speed_mult * self._max_angular_speed + self._current_pose.theta += self._current_angular_speed * ASSERV_RATE + + def _callback_timer_pose_send(self, event): + self._node.send_robot_position(self._current_pose) + + def _callback_timer_speed_send(self, event): + self._node.send_robot_speed(RobotSpeed(0, 0, self._current_linear_speed, 0, 0)) + + def _start_trajectory(self, goal_id, x, y, a=0, direction=1, has_angle=False): #direction=1 forward, 0 backward + self._goals_list.append(AsservGoal(goal_id, Pose2D(x, y, a), has_angle, direction)) + + +class State: + ROTATING_TO_ALIGN_TRAJ = 0 + MOVING_ON_TRAJ = 1 + ROTATING_AFTER_TRAJ = 2 + IDLE = 3 + + +class StatesManager(): + def __init__(self): + self.in_movement = False + self.emergency_stop = False + self.state = State.IDLE + + def start_movement(self): + self.in_movement = True + self.state = State.IDLE + + def stop_movement(self): + self.in_movement = False + self.state = State.IDLE + + def get_wanted_angle_to_align_traj(self, curr_pose, goal_pose, direction=1): + wanted_angle = math.atan2(goal_pose.y - curr_pose.y, + goal_pose.x - curr_pose.x) + wanted_angle += math.pi if direction == 0 else 0 + + wanted_angle %= 2* math.pi + return wanted_angle + + def should_move_on_traj(self, curr_pose, goal_pose, direction): + wanted_angle = self.get_wanted_angle_to_align_traj(curr_pose, goal_pose, direction) + + return self.in_movement and not self.emergency_stop \ + and not self.is_pos_in_margins(curr_pose, goal_pose) \ + and self.is_angle_in_margins(curr_pose.theta, wanted_angle) + + def should_rotate_to_align_traj(self, curr_pose, goal_pose, direction): + wanted_angle = self.get_wanted_angle_to_align_traj(curr_pose, goal_pose, direction) + return self.in_movement and not self.emergency_stop and \ + not self.is_angle_in_margins(curr_pose.theta, wanted_angle) and \ + not self.is_pos_in_margins(curr_pose, goal_pose) + + def should_rotate_after_traj(self, curr_pose, goal_pose, has_angle=False): + return self.in_movement and not self.emergency_stop and \ + has_angle and self.is_pos_in_margins(curr_pose, goal_pose) \ + and not self.is_angle_in_margins(curr_pose.theta, goal_pose.theta) + + def get_needed_angle_speed_multiplier(self, curr_pose, goal_pose, direction=1): + if self.state in [State.IDLE, State.MOVING_ON_TRAJ] or not self.in_movement: + return 0 + + if self.state == State.ROTATING_AFTER_TRAJ: + wanted_angle = goal_pose.theta + elif self.state == State.ROTATING_TO_ALIGN_TRAJ: + wanted_angle = self.get_wanted_angle_to_align_traj(curr_pose, goal_pose, direction) + + diff = (wanted_angle - curr_pose.theta) % (2 * math.pi) + if diff > math.pi: + return -1 + else: + return 1 + + def get_needed_linear_acceleration_multiplier(self, curr_pose, goal_pose, initial_distance, direction=1): + if self.state in [State.IDLE, State.ROTATING_TO_ALIGN_TRAJ, State.ROTATING_AFTER_TRAJ]\ + or not self.in_movement: + return 0 + + current_goal_distance = math.sqrt((goal_pose.x - curr_pose.x) ** 2 + + (goal_pose.y - curr_pose.y) ** 2) + + if current_goal_distance > initial_distance / 2.0: + return 1 + else: + return -1 + + def is_pos_in_margins(self, current_pose, goal_pose): + return (current_pose.x > goal_pose.x - ASSERV_ERROR_POSITION) and \ + (current_pose.x < goal_pose.x + ASSERV_ERROR_POSITION) and \ + (current_pose.y > goal_pose.y - ASSERV_ERROR_POSITION) and \ + (current_pose.y < goal_pose.y + ASSERV_ERROR_POSITION) + + def is_angle_in_margins(self, current_theta, goal_theta): + return (current_theta % (2 * math.pi) + 2 * math.pi < + goal_theta % (2 * math.pi) + 2 * math.pi + ASSERV_ERROR_ANGLE) and \ + (current_theta % (2 * math.pi) + 2 * math.pi > + goal_theta % (2 * math.pi) + 2 * math.pi - ASSERV_ERROR_ANGLE) + diff --git a/ros_ws/src/drivers_ard_asserv/src/asserv/protocol_parser.py b/ros_ws/src/drivers_ard_asserv/src/asserv/protocol_parser.py new file mode 100755 index 0000000..9c78591 --- /dev/null +++ b/ros_ws/src/drivers_ard_asserv/src/asserv/protocol_parser.py @@ -0,0 +1,28 @@ +#!/usr/bin/env python + +import re + +__author__ = "Thomas Fuhrmann" +__date__ = 24/10/2017 + +""" +Converts #define from a .h file to a dictionary : +#define VALUEA +#define VALUEB 'b' +#define VALUEC 42 +In : +['VALUEA':"", 'VALUEB':"b", 'VALUEC':"42"] +""" +def protocol_parse(filename): + parsed_dict = {} + with open(filename, "r") as f: + data = f.readlines() + for line in data: + if line.find("END_ORDERS") == -1: + result = re.search('#define[ \t]+(\S+)[ \t]+(\S+)', line) + if result is not None: + parsed_dict[result.group(1)] = result.group(2).replace("'", "") + else: + break + return parsed_dict + diff --git a/ros_ws/src/drivers_ard_asserv/src/marker_server_node.py b/ros_ws/src/drivers_ard_asserv/src/marker_server_node.py new file mode 100755 index 0000000..9ef635e --- /dev/null +++ b/ros_ws/src/drivers_ard_asserv/src/marker_server_node.py @@ -0,0 +1,115 @@ +#!/usr/bin/env python + +import rospy + +from interactive_markers.interactive_marker_server import * +from visualization_msgs.msg import * +from visualization_msgs.msg import Marker +from drivers_ard_asserv.srv import SetPos, SetPosRequest + +from tf.transformations import euler_from_quaternion +from tf.transformations import quaternion_from_euler + +from geometry_msgs.msg import Pose2D + +import tf2_ros + + +def processFeedback(feedback): + if feedback.event_type != InteractiveMarkerFeedback.MOUSE_UP: + return + + p = feedback.pose.position + try: + srv = rospy.ServiceProxy("/drivers/ard_asserv/set_pos", SetPos) + q = [[o.x, o.y, o.z, o.w] for o in [feedback.pose.orientation]][0] + + msg = SetPosRequest() + msg.position.x = feedback.pose.position.x + msg.position.y = feedback.pose.position.y + msg.position.theta = euler_from_quaternion(q)[2] + srv(msg) + except Exception as e: + rospy.logwarn(e) + +def poseCallback(msg): + pose = int_marker.pose + pose.position.x = msg.x + pose.position.y = msg.y + q = quaternion_from_euler(0, 0, msg.theta) + pose.orientation.x = q[0] + pose.orientation.y = q[1] + pose.orientation.z = q[2] + pose.orientation.w = q[3] + + server.setPose(int_marker.name, pose) + server.applyChanges() + + + +if __name__ == "__main__": + rospy.init_node("asserv_interactive_marker") + + server = InteractiveMarkerServer("asserv_marker") + + int_marker = InteractiveMarker() + int_marker.header.frame_id = "map" + int_marker.name = "robot_pos" + int_marker.scale = 0.5 + + int_marker.pose.position.z = 0.001 + + + box_marker = Marker() + box_marker.type = Marker.CUBE + box_marker.scale.x = 0.201 + box_marker.scale.y = 0.301 + box_marker.scale.z = 0.351 + + box_marker.pose.position.z = 0.30 / 2.0 + 0.025 + + box_marker.color.r = 191.0 / 255.0 + box_marker.color.g = 1.0 + box_marker.color.b = 244.0 / 255.0 + box_marker.color.a = 1.0 + + + + box_control = InteractiveMarkerControl() + box_control.name = "translate_plane" + box_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE + + box_control.orientation.w = 1 + box_control.orientation.x = 0 + box_control.orientation.y = 1 + box_control.orientation.z = 0 + + + + int_marker.controls.append(box_control) + box_control.markers.append(box_marker) + box_control.always_visible = True + + int_marker.controls.append(box_control) + + rotate_control = InteractiveMarkerControl() + rotate_control.name = "rotate_yaw" + rotate_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS + rotate_control.orientation.w = 1 + rotate_control.orientation.x = 0 + rotate_control.orientation.y = 1 + rotate_control.orientation.z = 0 + + int_marker.controls.append(rotate_control) + + # add the interactive marker to our collection & + # tell the server to call processFeedback() when feedback arrives for it + server.insert(int_marker, processFeedback) + + # 'commit' changes and send to all clients + server.applyChanges() + + rospy.Subscriber("/drivers/ard_asserv/pose2d", Pose2D, poseCallback) + + rospy.spin() + diff --git a/ros_ws/src/drivers_ard_asserv/srv/EmergencyStop.srv b/ros_ws/src/drivers_ard_asserv/srv/EmergencyStop.srv new file mode 100644 index 0000000..1dfe4b4 --- /dev/null +++ b/ros_ws/src/drivers_ard_asserv/srv/EmergencyStop.srv @@ -0,0 +1,4 @@ +# EmergencyStop service the the emergency stop flag. If 1, the robot stops right now. +bool enable +--- +bool response \ No newline at end of file diff --git a/ros_ws/src/drivers_ard_asserv/srv/Goto.srv b/ros_ws/src/drivers_ard_asserv/srv/Goto.srv new file mode 100644 index 0000000..e231818 --- /dev/null +++ b/ros_ws/src/drivers_ard_asserv/srv/Goto.srv @@ -0,0 +1,9 @@ +# GOTO command, x and y in mm, a in radians +uint8 GOTO = 0 +uint8 GOTOA = 1 +uint8 ROT = 2 +uint8 ROTNOMODULO = 3 +uint8 mode +geometry_msgs/Pose2D position +--- +bool response \ No newline at end of file diff --git a/ros_ws/src/drivers_ard_asserv/srv/Management.srv b/ros_ws/src/drivers_ard_asserv/srv/Management.srv new file mode 100644 index 0000000..032551b --- /dev/null +++ b/ros_ws/src/drivers_ard_asserv/srv/Management.srv @@ -0,0 +1,9 @@ +# Management is all the commands to manage the asserv +uint8 KILLG = 0 #go to next order +uint8 CLEANG = 1 #cancal all orders +uint8 PAUSE = 2 #pause control +uint8 RESUME = 3 #resume control +uint8 RESET_ID = 4 #reset last finished id to 0 +uint8 mode +--- +bool response \ No newline at end of file diff --git a/ros_ws/src/drivers_ard_asserv/srv/Parameters.srv b/ros_ws/src/drivers_ard_asserv/srv/Parameters.srv new file mode 100644 index 0000000..d48b443 --- /dev/null +++ b/ros_ws/src/drivers_ard_asserv/srv/Parameters.srv @@ -0,0 +1,16 @@ +# Parameters lists the dynamic parameters of the asserv +# For future, may be implemented using a parameter server +uint8 SPDMAX = 0 +uint8 ACCMAX = 1 +uint8 PIDRIGHT = 2 +uint8 PIDLEFT = 3 +uint8 PIDALL = 4 +uint8 mode +uint16 acc +uint16 spd +float32 spd_ratio +float32 p +float32 i +float32 d +--- +bool response \ No newline at end of file diff --git a/ros_ws/src/drivers_ard_asserv/srv/Pwm.srv b/ros_ws/src/drivers_ard_asserv/srv/Pwm.srv new file mode 100644 index 0000000..fb3a2e5 --- /dev/null +++ b/ros_ws/src/drivers_ard_asserv/srv/Pwm.srv @@ -0,0 +1,6 @@ +# Send a PWM command, left and right are the PWM applied on the wheels (limited to 255) and duration is in ms +int16 left +int16 right +uint16 duration +--- +bool response \ No newline at end of file diff --git a/ros_ws/src/drivers_ard_asserv/srv/SetPos.srv b/ros_ws/src/drivers_ard_asserv/srv/SetPos.srv new file mode 100644 index 0000000..4940a8c --- /dev/null +++ b/ros_ws/src/drivers_ard_asserv/srv/SetPos.srv @@ -0,0 +1,5 @@ +# SetPos service sets the position of the robot +geometry_msgs/Pose2D position +string position_waypoint +--- +bool response \ No newline at end of file diff --git a/ros_ws/src/drivers_ard_asserv/srv/Speed.srv b/ros_ws/src/drivers_ard_asserv/srv/Speed.srv new file mode 100644 index 0000000..4cd3906 --- /dev/null +++ b/ros_ws/src/drivers_ard_asserv/srv/Speed.srv @@ -0,0 +1,6 @@ +# Send a SPD (speed) command, linear and angular, the duration is in ms +int16 linear +int16 angular +uint16 duration +--- +bool response \ No newline at end of file diff --git a/ros_ws/src/drivers_ard_hmi/CMakeLists.txt b/ros_ws/src/drivers_ard_hmi/CMakeLists.txt new file mode 100644 index 0000000..b8b396a --- /dev/null +++ b/ros_ws/src/drivers_ard_hmi/CMakeLists.txt @@ -0,0 +1,47 @@ +cmake_minimum_required(VERSION 2.8.3) +project(drivers_ard_hmi) + +find_package(catkin REQUIRED) + +find_package(catkin REQUIRED COMPONENTS + message_generation + std_msgs +) + +add_message_files( + FILES + SetTeams.msg + SetStrategies.msg + HMIEvent.msg + ROSEvent.msg +) + +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs # Or other packages containing msgs +) + +catkin_package( + CATKIN_DEPENDS message_runtime +) + +########### +## Build ## +########### +include_directories( +# include +# ${catkin_INCLUDE_DIRS} +) \ No newline at end of file diff --git a/ros_ws/src/drivers_ard_hmi/README.md b/ros_ws/src/drivers_ard_hmi/README.md new file mode 100644 index 0000000..b0a52f2 --- /dev/null +++ b/ros_ws/src/drivers_ard_hmi/README.md @@ -0,0 +1 @@ +This package is nothing more than a placeholder for the HMI arduino messages. \ No newline at end of file diff --git a/ros_ws/src/drivers_ard_hmi/msg/HMIEvent.msg b/ros_ws/src/drivers_ard_hmi/msg/HMIEvent.msg new file mode 100644 index 0000000..f0b4bf2 --- /dev/null +++ b/ros_ws/src/drivers_ard_hmi/msg/HMIEvent.msg @@ -0,0 +1,8 @@ +uint8 EVENT_HMI_INITIALIZED = 0 # Sent to tell ai/game_manager HMI is alive. +uint8 EVENT_START = 1 # Sent to tell ai/scheduler to start. +uint8 EVENT_JACK_FIRED = 2 # Sent when the user activates the jack (needs to be armed beforehand). +uint8 EVENT_GAME_CANCEL = 3 # Sent when the user clicks clicks on 'previous' when in game. +uint8 event + +uint8 chosen_strategy_id # Used for event +uint8 chosen_team_id # start only. \ No newline at end of file diff --git a/ros_ws/src/drivers_ard_hmi/msg/ROSEvent.msg b/ros_ws/src/drivers_ard_hmi/msg/ROSEvent.msg new file mode 100644 index 0000000..b6bf521 --- /dev/null +++ b/ros_ws/src/drivers_ard_hmi/msg/ROSEvent.msg @@ -0,0 +1,3 @@ +uint8 EVENT_ASK_JACK = 0 # Sent by ai/scheduler when it strarts waiting for the jack. +uint8 EVENT_GAME_STOP = 1 # Sent by ai/game_manager when game_status changed to HALT. +uint8 event \ No newline at end of file diff --git a/ros_ws/src/drivers_ard_hmi/msg/SetStrategies.msg b/ros_ws/src/drivers_ard_hmi/msg/SetStrategies.msg new file mode 100644 index 0000000..e7bcfad --- /dev/null +++ b/ros_ws/src/drivers_ard_hmi/msg/SetStrategies.msg @@ -0,0 +1 @@ +string[] strategies_names \ No newline at end of file diff --git a/ros_ws/src/drivers_ard_hmi/msg/SetTeams.msg b/ros_ws/src/drivers_ard_hmi/msg/SetTeams.msg new file mode 100644 index 0000000..f0073b4 --- /dev/null +++ b/ros_ws/src/drivers_ard_hmi/msg/SetTeams.msg @@ -0,0 +1 @@ +string[] teams_names \ No newline at end of file diff --git a/ros_ws/src/drivers_ard_hmi/package.xml b/ros_ws/src/drivers_ard_hmi/package.xml new file mode 100644 index 0000000..6b082e4 --- /dev/null +++ b/ros_ws/src/drivers_ard_hmi/package.xml @@ -0,0 +1,53 @@ + + + drivers_ard_hmi + 0.0.0 + The drivers_ard_hmi package + + + + + UTCoupe + Pierre LACLAU + + + + + + GPLv3 + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + message_generation + message_runtime + + + + + + + + diff --git a/ros_ws/src/drivers_ard_others/CMakeLists.txt b/ros_ws/src/drivers_ard_others/CMakeLists.txt new file mode 100644 index 0000000..3a21d7c --- /dev/null +++ b/ros_ws/src/drivers_ard_others/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 2.8.3) +project(drivers_ard_others) + +find_package(catkin REQUIRED COMPONENTS + message_generation + geometry_msgs + std_msgs +) + +## Generate messages in the 'msg' folder +add_message_files( + FILES + BeltRange.msg + Move.msg + MoveResponse.msg + ActDigitalStates.msg + ActPWMStates.msg + ActServoStates.msg + Color.msg +) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + geometry_msgs # std_msgs +) + +catkin_package( + CATKIN_DEPENDS + geometry_msgs + std_msgs + message_runtime +) + +include_directories( + ${catkin_INCLUDE_DIRS} +) \ No newline at end of file diff --git a/ros_ws/src/drivers_ard_others/msg/ActDigitalStates.msg b/ros_ws/src/drivers_ard_others/msg/ActDigitalStates.msg new file mode 100644 index 0000000..7a437d3 --- /dev/null +++ b/ros_ws/src/drivers_ard_others/msg/ActDigitalStates.msg @@ -0,0 +1 @@ +bool[] states # ordered by ID, gives true/false for HIGH/LOW states. \ No newline at end of file diff --git a/ros_ws/src/drivers_ard_others/msg/ActPWMStates.msg b/ros_ws/src/drivers_ard_others/msg/ActPWMStates.msg new file mode 100644 index 0000000..9823dee --- /dev/null +++ b/ros_ws/src/drivers_ard_others/msg/ActPWMStates.msg @@ -0,0 +1 @@ +uint8[] states # ordered by ID, gives the current PWM signals. \ No newline at end of file diff --git a/ros_ws/src/drivers_ard_others/msg/ActServoStates.msg b/ros_ws/src/drivers_ard_others/msg/ActServoStates.msg new file mode 100644 index 0000000..d75e294 --- /dev/null +++ b/ros_ws/src/drivers_ard_others/msg/ActServoStates.msg @@ -0,0 +1 @@ +int16[] states # ordered by ID, gives the actual servo angles. \ No newline at end of file diff --git a/ros_ws/src/drivers_ard_others/msg/BeltRange.msg b/ros_ws/src/drivers_ard_others/msg/BeltRange.msg new file mode 100644 index 0000000..27014f6 --- /dev/null +++ b/ros_ws/src/drivers_ard_others/msg/BeltRange.msg @@ -0,0 +1,2 @@ +string sensor_id +float32 range # mm \ No newline at end of file diff --git a/ros_ws/src/drivers_ard_others/msg/Color.msg b/ros_ws/src/drivers_ard_others/msg/Color.msg new file mode 100644 index 0000000..1525be0 --- /dev/null +++ b/ros_ws/src/drivers_ard_others/msg/Color.msg @@ -0,0 +1,4 @@ +# This messages describes the HSL values of a color get from a color sensor +uint16 hue # 0 to 360 +uint16 saturation # 0 to 360 +uint16 lightness # 0 to 360 \ No newline at end of file diff --git a/ros_ws/src/drivers_ard_others/msg/Move.msg b/ros_ws/src/drivers_ard_others/msg/Move.msg new file mode 100644 index 0000000..4a3faf5 --- /dev/null +++ b/ros_ws/src/drivers_ard_others/msg/Move.msg @@ -0,0 +1,13 @@ +# Topic to move an actuator. Specify the type, actuator id and desired position. + +int16 order_nb # Optional : you can enter an order nb, arduino will reply through + # The topic /move_responses with your nb and whether the move succeeded. + +uint8 TYPE_DIGITAL = 0 # On/Off devices +uint8 TYPE_PWM = 1 # PWM regulated devices (DC motors...) +uint8 TYPE_SERVO = 2 # Real servos (drived with Servo.h in arduino) +uint8 TYPE_STEPPER = 3 # Stepper motors which take a number of steps +uint8 type # Actuator type. +uint8 id # Actuator ID number defined in the arduino code before flashing. + +int16 dest_value # Can be a position, speed, state... \ No newline at end of file diff --git a/ros_ws/src/drivers_ard_others/msg/MoveResponse.msg b/ros_ws/src/drivers_ard_others/msg/MoveResponse.msg new file mode 100644 index 0000000..3652ba3 --- /dev/null +++ b/ros_ws/src/drivers_ard_others/msg/MoveResponse.msg @@ -0,0 +1,4 @@ +# Arduino only replies if order_nb was != 0 at call. + +int16 order_nb # The Move order_nb you entered during call. +bool success \ No newline at end of file diff --git a/ros_ws/src/drivers_ard_others/package.xml b/ros_ws/src/drivers_ard_others/package.xml new file mode 100644 index 0000000..78aab9c --- /dev/null +++ b/ros_ws/src/drivers_ard_others/package.xml @@ -0,0 +1,57 @@ + + + drivers_ard_others + 0.0.0 + The drivers_ard_others package + + + + + jack + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + geometry_msgs + std_msgs + geometry_msgs + std_msgs + + message_generation + message_runtime + + + + + + + + diff --git a/ros_ws/src/drivers_ax12/CMakeLists.txt b/ros_ws/src/drivers_ax12/CMakeLists.txt new file mode 100644 index 0000000..c69a376 --- /dev/null +++ b/ros_ws/src/drivers_ax12/CMakeLists.txt @@ -0,0 +1,70 @@ +cmake_minimum_required(VERSION 2.8.3) +project(drivers_ax12) + +add_compile_options(-std=c++14) + +find_package(catkin REQUIRED COMPONENTS + actionlib + actionlib_msgs + message_generation + message_runtime + roscpp + std_msgs + drivers_port_finder + ai_game_manager + memory_definitions + dynamixel_sdk +) + +add_action_files( + DIRECTORY action + FILES + Ax12Command.action +) + +add_service_files( + FILES + SetAx12Param.srv +) + +generate_messages( + DEPENDENCIES + actionlib_msgs +) + + +catkin_package( + INCLUDE_DIRS include + LIBRARIES drivers_ax12 + CATKIN_DEPENDS actionlib actionlib_msgs roscpp std_msgs drivers_port_finder ai_game_manager +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_executable(ax12_node src/ax12_node.cpp src/ax12_server.cpp src/ax12_driver.cpp) + +add_dependencies(ax12_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +target_link_libraries( + ax12_node + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +install( + TARGETS ax12_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install( + DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +) diff --git a/ros_ws/src/drivers_ax12/README.md b/ros_ws/src/drivers_ax12/README.md new file mode 100644 index 0000000..4c9251d --- /dev/null +++ b/ros_ws/src/drivers_ax12/README.md @@ -0,0 +1,4 @@ +# drivers_ax12 + +This package mainly provides an action server to move an AX-12A to an angle, or to switch it in wheel mode. Both modes need to specify a speed in the action goal. +It comes also with a service to set specific registers of an AX-12A, such as the punch, etc. It uses the Dynamixel SDK 3 ros package. \ No newline at end of file diff --git a/ros_ws/src/drivers_ax12/action/Ax12Command.action b/ros_ws/src/drivers_ax12/action/Ax12Command.action new file mode 100644 index 0000000..7369b71 --- /dev/null +++ b/ros_ws/src/drivers_ax12/action/Ax12Command.action @@ -0,0 +1,15 @@ +# goal +uint8 JOINT = 0 +uint8 WHEEL = 1 + +uint8 motor_id +uint16 position # 0 to 1023 +uint16 speed # 1 to 1023 in joint mode, 0 to 2047 in wheel mode + +uint8 mode +--- +# result +bool success +--- +#feedback +uint16 position \ No newline at end of file diff --git a/ros_ws/src/drivers_ax12/include/ax12_driver.h b/ros_ws/src/drivers_ax12/include/ax12_driver.h new file mode 100644 index 0000000..2defe9a --- /dev/null +++ b/ros_ws/src/drivers_ax12/include/ax12_driver.h @@ -0,0 +1,47 @@ + +#ifndef DRIVERS_AX12_AX12_DRIVER_H +#define DRIVERS_AX12_AX12_DRIVER_H + +#include +#include +#include "ax12_table.h" + +class Ax12Driver { +public: + + const static uint8_t SCAN_RANGE = 10; // the scan pings motors with id 1 to SCAN_RANGE + const static uint8_t PING_PASS_NBR = 12; // number of times a motor is pinged to make sure it is connected + const static uint8_t BAUD_RATE_INDEX = 1; // baudrate = 2000000 / (index + 1) + const static uint16_t PING_SLEEP = 150; // microsec to sleep between pings + +protected: + std::vector motor_ids; + std::unique_ptr port_handler; + std::unique_ptr packet_handler; + +public: + bool initialize(const std::string &port_index); + + void scan_motors(); + + bool write_register(uint8_t motor_id, Ax12Table::Register reg, uint16_t value); + + bool read_register(uint8_t motor_id, Ax12Table::Register reg, uint16_t &value); + + bool motor_id_exists(uint8_t motor_id); + + bool motor_id_connected(uint8_t motor_id); + + bool toggle_torque(bool enable); + + bool joint_mode(uint8_t motor_id, uint16_t min_angle = 1, uint16_t max_angle = 1023); + + bool wheel_mode(uint8_t motor_id); + + const std::vector &get_motor_ids() { return motor_ids; }; + + Ax12Driver() {} + +}; + +#endif //DRIVERS_AX12_AX12_DRIVER_H diff --git a/ros_ws/src/drivers_ax12/include/ax12_server.h b/ros_ws/src/drivers_ax12/include/ax12_server.h new file mode 100644 index 0000000..c15f346 --- /dev/null +++ b/ros_ws/src/drivers_ax12/include/ax12_server.h @@ -0,0 +1,77 @@ + +#ifndef DRIVERS_AX12_AX12_SERVER_H +#define DRIVERS_AX12_AX12_SERVER_H + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ax12_driver.h" + + +const double MAX_STOP_TIME = 3; // timeout (secs) +const double MAIN_FREQUENCY = 15; +const uint8_t POSITION_MARGIN = 6; +const std::string PORT_FINDER_SERVICE = "/drivers/port_finder/get_port"; +const std::string GAME_STATUS_TOPIC = "/ai/game_manager/status"; + + +typedef actionlib::ServerGoalHandle GoalHandle; + + +class Ax12Server { + +protected: + ros::NodeHandle nh_; + actionlib::ActionServer as_; + ros::ServiceServer set_param_service; + ros::Subscriber game_status_sub_; + std::list joint_goals_; + + // create messages that are used to published feedback/result + drivers_ax12::Ax12CommandFeedback feedback_; + drivers_ax12::Ax12CommandResult result_; + + Ax12Driver driver_; + ros::Timer timer_; + + std::unique_ptr status_services_; + + bool is_halted; + +public: + void execute_goal_cb(GoalHandle goal_handle); + + void cancel_goal_cb(GoalHandle goal_handle); + + bool execute_set_service_cb(drivers_ax12::SetAx12Param::Request &req, drivers_ax12::SetAx12Param::Response &res); + + void game_status_cb(const ai_game_manager::GameStatusConstPtr &status); + + std::string fetch_port(const std::string &service_name); + + void init_driver(const std::string &port); + + void main_loop(const ros::TimerEvent &); + + bool handle_joint_goal(GoalHandle goal_handle); + + bool handle_wheel_goal(GoalHandle goal_handle); + + const Ax12Table::Register *service_param_to_register(uint8_t param); + + Ax12Server(const std::string &action_name, const std::string &service_name); + + ~Ax12Server(); +}; + +#endif //DRIVERS_AX12_AX12_SERVER_H diff --git a/ros_ws/src/drivers_ax12/include/ax12_table.h b/ros_ws/src/drivers_ax12/include/ax12_table.h new file mode 100644 index 0000000..20d3f2c --- /dev/null +++ b/ros_ws/src/drivers_ax12/include/ax12_table.h @@ -0,0 +1,69 @@ + +#ifndef DRIVERS_AX12_AX12_TABLE_H +#define DRIVERS_AX12_AX12_TABLE_H + +#include + + +namespace Ax12Table { + /* + * Defines the items of the register table. + * This is specific the the AX-12A motor. + * See http://support.robotis.com/en/techsupport_eng.htm#product/dynamixel/ax_series/dxl_ax_actuator.htm + */ + + struct Register { + uint8_t address; + uint8_t access; + uint8_t size; + }; + + // access + const uint8_t READ_WRITE = 1; + const uint8_t READ = 0; + + // size + const uint8_t WORD = 2; + const uint8_t BYTE = 1; + + // table + const Register + // EEPROM + MODEL_NUMBER = {0, READ, WORD}, + VERSION_OF_FIRMWARE = {2, READ, BYTE}, + ID = {3, READ_WRITE, BYTE}, + BAUD_RATE = {4, READ_WRITE, BYTE}, + RETURN_DELAY_TIME = {5, READ_WRITE, BYTE}, + CW_ANGLE_LIMIT = {6, READ_WRITE, WORD}, + CCW_ANGLE_LIMIT = {8, READ_WRITE, WORD}, + HIGHEST_TEMPERATURE = {11, READ_WRITE, BYTE}, + LOWEST_VOLTAGE = {12, READ_WRITE, BYTE}, + HIGHEST_VOLTAGE = {13, READ_WRITE, BYTE}, + MAX_TORQUE = {14, READ_WRITE, WORD}, + STATUS_RETURN_LEVEL = {16, READ_WRITE, BYTE}, + ALARM_LED = {17, READ_WRITE, BYTE}, + ALARM_SHUTDOWN = {18, READ_WRITE, BYTE}, + + // RAM + TORQUE_ENABLE = {24, READ_WRITE, BYTE}, + LED = {25, READ_WRITE, BYTE}, + CW_COMPLIANCE_MARGIN = {26, READ_WRITE, BYTE}, + CCW_COMPLIANCE_MARGIN = {27, READ_WRITE, BYTE}, + CW_COMPLIANCE_SLOPE = {28, READ_WRITE, BYTE}, + CCW_COMPLIANCE_SLOPE = {29, READ_WRITE, BYTE}, + GOAL_POSITION = {30, READ_WRITE, WORD}, + MOVING_SPEED = {32, READ_WRITE, WORD}, + TORQUE_LIMIT = {34, READ_WRITE, WORD}, + PRESENT_POSITION = {36, READ, WORD}, + PRESENT_SPEED = {38, READ, WORD}, + PRESENT_LOAD = {40, READ, WORD}, + PRESENT_VOLTAGE = {42, READ, BYTE}, + PRESENT_TEMPERATURE = {43, READ, BYTE}, + REGISTERED = {44, READ, BYTE}, + MOVING = {46, READ, BYTE}, + LOCK = {47, READ_WRITE, BYTE}, + PUNCH = {48, READ_WRITE, WORD}; +} + + +#endif //DRIVERS_AX12_AX12_TABLE_H diff --git a/ros_ws/src/drivers_ax12/package.xml b/ros_ws/src/drivers_ax12/package.xml new file mode 100644 index 0000000..60cbcba --- /dev/null +++ b/ros_ws/src/drivers_ax12/package.xml @@ -0,0 +1,31 @@ + + + drivers_ax12 + 0.0.0 + The drivers_ax12 package + + GPLv3 + milesial + UTCoupe + + catkin + actionlib + actionlib_msgs + message_generation + roscpp + std_msgs + drivers_port_finder + ai_game_manager + dynamixel_sdk + actionlib + actionlib_msgs + message_runtime + roscpp + std_msgs + drivers_port_finder + ai_game_manager + memory_definition + + + + diff --git a/ros_ws/src/drivers_ax12/src/ax12_driver.cpp b/ros_ws/src/drivers_ax12/src/ax12_driver.cpp new file mode 100644 index 0000000..ac90876 --- /dev/null +++ b/ros_ws/src/drivers_ax12/src/ax12_driver.cpp @@ -0,0 +1,139 @@ +#include "ax12_driver.h" +#include +#include + +using namespace Ax12Table; + +bool Ax12Driver::initialize(const std::string &port_name) { + port_handler = std::unique_ptr( + dynamixel::PortHandler::getPortHandler(port_name.c_str())); + packet_handler = std::unique_ptr( + dynamixel::PacketHandler::getPacketHandler(1.0)); + + if (!port_handler->openPort()) { + ROS_ERROR_STREAM("SDK could not open the port '" << port_name << "'"); + return false; + } + + int baud_rate = 2000000 / (BAUD_RATE_INDEX + 1); + if (!port_handler->setBaudRate(baud_rate)) { + ROS_ERROR_STREAM("SDK could not set the baud rate at " << baud_rate); + return false; + } + + ROS_INFO_STREAM("SDK successfully initialized on port '" << port_name + << "' with baud rate " << baud_rate); + + return true; +} + +void Ax12Driver::scan_motors() { + ROS_DEBUG_STREAM("Scanning motors with IDs from 1 to " << SCAN_RANGE + <<", pinging " << PING_PASS_NBR << " times each"); + + motor_ids.clear(); + + for (uint8_t id = 1; id <= SCAN_RANGE; id++) { + if (motor_id_connected(id)) { + ROS_INFO("Found motor with ID %d", id); + motor_ids.push_back(id); + } + } + + ROS_INFO_STREAM("Found " << motor_ids.size() << " motors while scanning"); +} + +bool Ax12Driver::write_register(uint8_t motor_id, const Register reg, uint16_t value) { + if (reg.access == READ) { + ROS_WARN_STREAM("Cannot write value " << value << " to a read-only register for motor " << static_cast(motor_id)); + return false; + } + + int result; + + if (reg.size == BYTE) + result = packet_handler->write1ByteTxRx(port_handler.get(), motor_id, reg.address, + static_cast(value)); + else + result = packet_handler->write2ByteTxRx(port_handler.get(), motor_id, reg.address, value); + + if (result != COMM_SUCCESS) { + ROS_WARN_STREAM("Could not write value " << value << " to register for motor " << static_cast(motor_id) + << " : " << packet_handler->getTxRxResult(result)); + return false; + } + + return true; +} + +bool Ax12Driver::read_register(uint8_t motor_id, const Register reg, uint16_t &value) { + int result; + + if (reg.size == BYTE) { + uint8_t ret_val; + result = packet_handler->read1ByteTxRx(port_handler.get(), motor_id, reg.address, &ret_val); + value = ret_val; + } else { + uint16_t ret_val; + result = packet_handler->read2ByteTxRx(port_handler.get(), motor_id, reg.address, &ret_val); + value = ret_val; + } + + if (result != COMM_SUCCESS) { + ROS_WARN_STREAM("Could not read register for motor " << static_cast(motor_id) << " : " << packet_handler->getTxRxResult(result)); + return false; + } + + return true; +} + +bool Ax12Driver::motor_id_exists(uint8_t motor_id) { + return std::find(motor_ids.begin(), motor_ids.end(), motor_id) != motor_ids.end(); +} + +bool Ax12Driver::motor_id_connected(uint8_t motor_id) { + int result; + uint8_t err; + + for (uint8_t j = 0; j < PING_PASS_NBR; j++) { + result = packet_handler->ping(port_handler.get(), motor_id, &err); + + if (err != 0) { + ROS_DEBUG_STREAM("Could not ping motor " << static_cast(motor_id) << " because of an hardware error : " << packet_handler->getRxPacketError(err)); + } + + if (result == COMM_SUCCESS) { + return true; + } + + usleep(PING_SLEEP); + } + + return false; +} + +bool Ax12Driver::toggle_torque(bool enable) { + ROS_DEBUG_STREAM("Toggling torque " << (enable ? "on" : "off") << " for all motors"); + bool success = true; + + for (uint8_t i : motor_ids) { + success &= write_register(i, TORQUE_ENABLE, static_cast(enable)); + } + + return success; +} + +bool Ax12Driver::joint_mode(uint8_t motor_id, uint16_t min_angle, uint16_t max_angle) { + bool success = true; + + success &= write_register(motor_id, TORQUE_ENABLE, 0); + success &= write_register(motor_id, CW_ANGLE_LIMIT, min_angle); + success &= write_register(motor_id, CCW_ANGLE_LIMIT, max_angle); + success &= write_register(motor_id, TORQUE_ENABLE, 1); + + return success; +} + +bool Ax12Driver::wheel_mode(uint8_t motor_id) { + return joint_mode(motor_id, 0, 0); // wheel mode is set with min = max = 0 +} diff --git a/ros_ws/src/drivers_ax12/src/ax12_node.cpp b/ros_ws/src/drivers_ax12/src/ax12_node.cpp new file mode 100644 index 0000000..cd7a2eb --- /dev/null +++ b/ros_ws/src/drivers_ax12/src/ax12_node.cpp @@ -0,0 +1,9 @@ +#include "ros/ros.h" +#include "ax12_server.h" + +int main(int argc, char **argv) { + ros::init(argc, argv, "ax12"); + Ax12Server server("/drivers/ax12", "/drivers/ax12/set_param"); + ros::spin(); + return 0; +} diff --git a/ros_ws/src/drivers_ax12/src/ax12_server.cpp b/ros_ws/src/drivers_ax12/src/ax12_server.cpp new file mode 100644 index 0000000..91c5624 --- /dev/null +++ b/ros_ws/src/drivers_ax12/src/ax12_server.cpp @@ -0,0 +1,401 @@ +#include "ax12_server.h" + +#include + +#include + +using namespace Ax12Table; + + +void Ax12Server::init_driver(const std::string &port) { + + if (!driver_.initialize(port)) { + ROS_FATAL("Unable to initialize the AX-12 SDK, shutting down !"); + status_services_->setReady(false); + ros::shutdown(); + return; + } + + driver_.scan_motors(); + + driver_.toggle_torque(true); +} + +void Ax12Server::cancel_goal_cb(GoalHandle goal_handle) { + uint8_t motor_id = goal_handle.getGoal()->motor_id; + + ROS_INFO_STREAM("Received a cancel request, stopping movement of motor " << static_cast(motor_id)); + + uint16_t moving; + + driver_.read_register(motor_id, MOVING, moving); + + if (!moving) { + ROS_WARN_STREAM("Received a cancel request but motor " << static_cast(motor_id) << " is not moving"); + return; + } + + uint16_t pos; + driver_.joint_mode(motor_id); + driver_.read_register(motor_id, PRESENT_POSITION, pos); + driver_.write_register(motor_id, GOAL_POSITION, pos); + + for (auto it = joint_goals_.begin(); it != joint_goals_.end(); it++) { + if (goal_handle.getGoalID().id == it->getGoalID().id) { + joint_goals_.erase(it); + ROS_DEBUG_STREAM("Erased joint goal of motor " << static_cast(motor_id) << " from the list"); + return; + } + } +} + +void Ax12Server::execute_goal_cb(GoalHandle goal_handle) { + /* + * Handles the goal requests of the action server. + * Checks if the goal is valid, then tells the motor + * to move and adds the goal to the list of current goals + */ + + auto goal = goal_handle.getGoal(); + uint8_t motor_id = goal->motor_id; + + if (is_halted) { + ROS_ERROR("AX-12 action server received a goal, but game_status said that the system is halted !"); + result_.success = 0; + goal_handle.setRejected(result_); + return; + } + + if (!goal_handle.isValid()) { + ROS_ERROR("AX-12 action server received an invalid goal !"); + result_.success = 0; + goal_handle.setRejected(result_); + return; + } + + if (!driver_.motor_id_exists(motor_id)) { + ROS_ERROR_STREAM("AX-12 action server received a goal for motor ID " << static_cast(motor_id) << ", but no such motor was detected"); + result_.success = 0; + goal_handle.setRejected(result_); + return; + } + + if (!driver_.motor_id_connected(motor_id)) { + ROS_ERROR_STREAM("AX-12 action server received a goal for motor ID " << static_cast(motor_id) << ", but the motor was disconnected"); + result_.success = 0; + goal_handle.setRejected(result_); + return; + } + + if (goal->mode == goal->JOINT) + handle_joint_goal(goal_handle); + else + handle_wheel_goal(goal_handle); +} + +bool Ax12Server::handle_joint_goal(GoalHandle goal_handle) { + auto goal = goal_handle.getGoal(); + uint8_t motor_id = goal->motor_id; + uint16_t position = goal->position; + + if (position <= 0 || position > 1023) { + ROS_ERROR_STREAM("AX-12 action server received a joint goal for motor ID " << static_cast(motor_id) + << ", but with an invalid position: " << position); + result_.success = 0; + goal_handle.setRejected(result_); + return false; + } + + uint16_t speed = goal->speed; + if (speed < 0 || speed > 1023) { + ROS_ERROR_STREAM("AX-12 action server received a joint goal for motor ID " << static_cast(motor_id) + << ", but with an invalid speed: " << speed); + result_.success = 0; + goal_handle.setRejected(result_); + return false; + } + + goal_handle.setAccepted(); + + for (auto it = joint_goals_.begin(); it != joint_goals_.end();) { + if (it->getGoal()->motor_id == motor_id) { + it->setCanceled(); + ROS_WARN_STREAM("AX-12 action server received a joint goal for motor ID " + << static_cast(motor_id) << " while another joint goal was running for that motor." + << " The old goal was canceled."); + + it = joint_goals_.erase(it); + } else { + it++; + } + } + + bool success = true; + + success &= driver_.joint_mode(motor_id); + + success &= driver_.write_register(motor_id, MOVING_SPEED, speed); + success &= driver_.write_register(motor_id, GOAL_POSITION, position); + + joint_goals_.push_back(goal_handle); + ROS_INFO_STREAM("Received a joint goal for motor " << static_cast(motor_id)); + + if(!success) + ROS_WARN("At least one of the register writes was not successful, be careful"); + + ROS_DEBUG_STREAM("Success " << success << " setting goal and speed for motor " << static_cast(motor_id) << ", adding the goal to the list"); + + return success; +} + +bool Ax12Server::handle_wheel_goal(GoalHandle goal_handle) { + auto goal = goal_handle.getGoal(); + uint8_t motor_id = goal->motor_id; + uint16_t speed = goal->speed; + + if (speed < 0 || speed > 2047) { + ROS_ERROR_STREAM("AX-12 action server received a joint goal for motor ID " << static_cast(motor_id) + << ", but with an invalid speed: " << speed); + result_.success = 0; + goal_handle.setRejected(result_); + return false; + } + + goal_handle.setAccepted(); + + for (auto it = joint_goals_.begin(); it != joint_goals_.end();) { + if (it->getGoal()->motor_id == motor_id) { + result_.success = 0; + it->setCanceled(result_); + ROS_WARN_STREAM("AX-12 action server received a wheel goal for motor ID " << static_cast(motor_id) + << " while another joint goal was running for that motor. " + << "The old goal was canceled."); + it = joint_goals_.erase(it); + } else { + it++; + } + } + + bool success = true; + success &= driver_.write_register(motor_id, MOVING_SPEED, speed); + success &= driver_.wheel_mode(motor_id); + + + result_.success = static_cast(success); + goal_handle.setSucceeded(result_); + + ROS_INFO_STREAM("Received a wheel goal for motor " << static_cast(motor_id)); + if(!success) + ROS_WARN("Returning a bad result, as one of the register writes was not successful"); + else + ROS_INFO("Setting the goal as succeeded, all register writes successful"); + + + return success; +} + +void Ax12Server::main_loop(const ros::TimerEvent &) { + /* + * Called on a Timer, checks the status of all current goals and updates them + * Feedback is published too + */ + + uint8_t motor_id = 0; + uint16_t curr_position = 0; + uint32_t goal_position = 0; + uint16_t curr_speed = 0; + + for (auto it = joint_goals_.begin(); it != joint_goals_.end();) { + motor_id = it->getGoal()->motor_id; + driver_.read_register(motor_id, PRESENT_POSITION, curr_position); + + ROS_DEBUG_STREAM("Motor " << static_cast(motor_id) << " : position " << curr_position); + + goal_position = it->getGoal()->position; + + if (curr_position >= (goal_position - POSITION_MARGIN) && curr_position <= (goal_position + POSITION_MARGIN)) { + ROS_DEBUG_STREAM("Motor has reached the goal position ! curr_pos : " << curr_position + << ", goal_pos : " << goal_position); + result_.success = 1; + it->setSucceeded(result_); + it = joint_goals_.erase(it); + ROS_INFO_STREAM("AX-12 position goal " << goal_position << " succeeded for motor ID " << static_cast(motor_id)); + + } else if (ros::Time::now().toSec() - it->getGoalID().stamp.toSec() > MAX_STOP_TIME && it->getGoal()->speed == 0) { + ROS_ERROR_STREAM("Motor has not reached the goal position, timeout reached ! curr_pos : " << curr_position + << ", goal_pos : " << goal_position); + + // reset the alarm + driver_.write_register(motor_id, TORQUE_ENABLE, 0); + driver_.write_register(motor_id, TORQUE_ENABLE, 1); + + result_.success = 0; + it->setAborted(result_); + it = joint_goals_.erase(it); + } else { + feedback_.position = curr_position; + it->publishFeedback(feedback_); + it++; + } + } +} + +std::string Ax12Server::fetch_port(const std::string &service_name) { + /* + * Asks the port_finder for the motor port + */ + + std::string port; + + if (!ros::service::waitForService(service_name, 25000)) { + ROS_ERROR("Failed to contact drivers_port_finder (service not up)"); + } else { + ros::ServiceClient client = nh_.serviceClient(service_name); + drivers_port_finder::GetPort srv; + srv.request.component = "usb2ax"; + + if (client.call(srv)) { + port = srv.response.port; + } else { + ROS_ERROR("Failed to fetch the AX-12 port from drivers_port_finder"); + } + } + + if (port.length() == 0) { + ROS_FATAL("The AX-12 port is not set, shutting down..."); + status_services_->setReady(false); + ros::shutdown(); + return ""; + } + + return port; +} + +const Ax12Table::Register *Ax12Server::service_param_to_register(uint8_t param) { + switch (param) { + case drivers_ax12::SetAx12ParamRequest::ID: + return &ID; + case drivers_ax12::SetAx12ParamRequest::BAUD_RATE: + return &BAUD_RATE; + case drivers_ax12::SetAx12ParamRequest::RETURN_DELAY_TIME: + return &RETURN_DELAY_TIME; + case drivers_ax12::SetAx12ParamRequest::CW_ANGLE_LIMIT: + return &CW_ANGLE_LIMIT; + case drivers_ax12::SetAx12ParamRequest::CCW_ANGLE_LIMIT: + return &CCW_ANGLE_LIMIT; + case drivers_ax12::SetAx12ParamRequest::HIGHEST_TEMPERATURE: + return &HIGHEST_TEMPERATURE; + case drivers_ax12::SetAx12ParamRequest::LOWEST_VOLTAGE: + return &LOWEST_VOLTAGE; + case drivers_ax12::SetAx12ParamRequest::HIGHEST_VOLTAGE: + return &HIGHEST_VOLTAGE; + case drivers_ax12::SetAx12ParamRequest::MAX_TORQUE: + return &MAX_TORQUE; + case drivers_ax12::SetAx12ParamRequest::STATUS_RETURN_LEVEL: + return &STATUS_RETURN_LEVEL; + case drivers_ax12::SetAx12ParamRequest::ALARM_LED: + return &ALARM_LED; + case drivers_ax12::SetAx12ParamRequest::ALARM_SHUTDOWN: + return &ALARM_SHUTDOWN; + case drivers_ax12::SetAx12ParamRequest::TORQUE_ENABLE: + return &TORQUE_ENABLE; + case drivers_ax12::SetAx12ParamRequest::LED: + return &LED; + case drivers_ax12::SetAx12ParamRequest::CW_COMPLIANCE_MARGIN: + return &CW_COMPLIANCE_MARGIN; + case drivers_ax12::SetAx12ParamRequest::CCW_COMPLIANCE_MARGIN: + return &CCW_COMPLIANCE_MARGIN; + case drivers_ax12::SetAx12ParamRequest::CW_COMPLIANCE_SLOPE: + return &CW_COMPLIANCE_SLOPE; + case drivers_ax12::SetAx12ParamRequest::CCW_COMPLIANCE_SLOPE: + return &CCW_COMPLIANCE_SLOPE; + case drivers_ax12::SetAx12ParamRequest::GOAL_POSITION: + return &GOAL_POSITION; + case drivers_ax12::SetAx12ParamRequest::MOVING_SPEED: + return &MOVING_SPEED; + case drivers_ax12::SetAx12ParamRequest::TORQUE_LIMIT: + return &TORQUE_LIMIT; + case drivers_ax12::SetAx12ParamRequest::LOCK: + return &LOCK; + case drivers_ax12::SetAx12ParamRequest::PUNCH: + return &PUNCH; + default: + return nullptr; + } +} + +bool Ax12Server::execute_set_service_cb(drivers_ax12::SetAx12Param::Request &req, + drivers_ax12::SetAx12Param::Response &res) { + + if (!driver_.motor_id_exists(req.motor_id)) { + ROS_ERROR_STREAM("AX-12 set_param service server received a request for motor ID " << req.motor_id + << ", but no such motor was detected"); + res.success = 0; + return true; + } + + if (!driver_.motor_id_connected(req.motor_id)) { + ROS_ERROR("AX-12 set_param service server received a request for motor ID %d, but the motor was disconnected", + req.motor_id); + res.success = 0; + return true; + } + + const Register *reg = service_param_to_register(req.param); + + res.success = (uint8_t) driver_.write_register(req.motor_id, *reg, static_cast(req.value)); + + ROS_INFO_STREAM("Successfully changed parameter " << req.param << " of motor with ID " << req.motor_id + << " to " << req.value); + + return true; +} + +void Ax12Server::game_status_cb(const ai_game_manager::GameStatusConstPtr &status) { + if (!is_halted && status->game_status == status->STATUS_HALT) { + is_halted = true; + driver_.toggle_torque(false); + ROS_WARN("Halt request, stopping the motors"); + } else if (is_halted && status->game_status != status->STATUS_HALT) { + is_halted = false; + driver_.toggle_torque(true); + ROS_INFO("Halt finished, running the motors"); + + } +} + +Ax12Server::Ax12Server(const std::string &action_name, const std::string &service_name) : + as_(nh_, action_name, boost::bind(&Ax12Server::execute_goal_cb, this, _1), + boost::bind(&Ax12Server::cancel_goal_cb, this, _1), false), + set_param_service(nh_.advertiseService(service_name, &Ax12Server::execute_set_service_cb, this)), + game_status_sub_(nh_.subscribe(GAME_STATUS_TOPIC, 30, &Ax12Server::game_status_cb, this)), + joint_goals_(), + feedback_(), + result_(), + driver_(), + is_halted(false) { + + as_.start(); + + status_services_ = std::make_unique( + "drivers", "ax12", [this](const ai_game_manager::ArmRequest::ConstPtr &){ + driver_.scan_motors(); // TODO: uncomment and test if arm can be called while in game ??? + // driver_.toggle_torque(true); + }); + + std::string port = fetch_port(PORT_FINDER_SERVICE); + + init_driver(port); + + ROS_INFO_STREAM("AX-12 action server initialized for port " << port << ", waiting for goals"); + + timer_ = nh_.createTimer(ros::Duration(1.0 / MAIN_FREQUENCY), &Ax12Server::main_loop, this); + + status_services_->setReady(true); +} + +Ax12Server::~Ax12Server() { + driver_.toggle_torque(false); + ros::shutdown(); +} + diff --git a/ros_ws/src/drivers_ax12/srv/SetAx12Param.srv b/ros_ws/src/drivers_ax12/srv/SetAx12Param.srv new file mode 100644 index 0000000..00edfdb --- /dev/null +++ b/ros_ws/src/drivers_ax12/srv/SetAx12Param.srv @@ -0,0 +1,29 @@ +uint8 ID = 0 +uint8 BAUD_RATE = 1 +uint8 RETURN_DELAY_TIME = 2 +uint8 CW_ANGLE_LIMIT = 3 +uint8 CCW_ANGLE_LIMIT = 4 +uint8 HIGHEST_TEMPERATURE = 5 +uint8 LOWEST_VOLTAGE = 6 +uint8 HIGHEST_VOLTAGE = 7 +uint8 MAX_TORQUE = 8 +uint8 STATUS_RETURN_LEVEL = 9 +uint8 ALARM_LED = 10 +uint8 ALARM_SHUTDOWN = 11 +uint8 TORQUE_ENABLE = 12 +uint8 LED = 13 +uint8 CW_COMPLIANCE_MARGIN = 14 +uint8 CCW_COMPLIANCE_MARGIN = 15 +uint8 CW_COMPLIANCE_SLOPE = 16 +uint8 CCW_COMPLIANCE_SLOPE = 17 +uint8 GOAL_POSITION = 18 +uint8 MOVING_SPEED = 19 +uint8 TORQUE_LIMIT = 20 +uint8 LOCK = 21 +uint8 PUNCH = 22 + +uint8 motor_id +uint8 param +int16 value +--- +bool success \ No newline at end of file diff --git a/ros_ws/src/drivers_port_finder/CMakeLists.txt b/ros_ws/src/drivers_port_finder/CMakeLists.txt new file mode 100644 index 0000000..45fc9c1 --- /dev/null +++ b/ros_ws/src/drivers_port_finder/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 2.8.3) +project(drivers_port_finder) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs + message_generation + genmsg +) + +## Generate services +add_service_files( + FILES + GetPort.srv +) + +# Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs +) + +catkin_package( + CATKIN_DEPENDS message_runtime +) + +include_directories( + ${catkin_INCLUDE_DIRS} +) + +############# +## Install ## +############# + +catkin_install_python( + PROGRAMS src/port_finder_node.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install( + DIRECTORY def/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/def +) diff --git a/ros_ws/src/drivers_port_finder/def/components_list.xml b/ros_ws/src/drivers_port_finder/def/components_list.xml new file mode 100644 index 0000000..e553380 --- /dev/null +++ b/ros_ws/src/drivers_port_finder/def/components_list.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/ros_ws/src/drivers_port_finder/package.xml b/ros_ws/src/drivers_port_finder/package.xml new file mode 100644 index 0000000..ad72bca --- /dev/null +++ b/ros_ws/src/drivers_port_finder/package.xml @@ -0,0 +1,18 @@ + + + drivers_port_finder + 0.0.1 + + Package which determines which serial component is connected on which port. + + GPLv3 + Thomas Fuhrmann + UTCoupe + catkin + message_generation + rospy + std_msgs> + message_runtime + + + diff --git a/ros_ws/src/drivers_port_finder/src/port_finder_node.py b/ros_ws/src/drivers_port_finder/src/port_finder_node.py new file mode 100755 index 0000000..88f3d6b --- /dev/null +++ b/ros_ws/src/drivers_port_finder/src/port_finder_node.py @@ -0,0 +1,235 @@ +#!/usr/bin/env python + +import re +import subprocess +import serial +import rospy +import rospkg +import xml.etree.ElementTree as ET +from drivers_port_finder.srv import * +from ai_game_manager import StatusServices + + +__author__ = "Thomas Fuhrmann" +__date__ = 18/01/2017 + +NODE_NAME = "port_finder" +PACKAGE_NAME = "drivers_" + NODE_NAME +ARDUINO_LIST = ("mega", "nano", "uno", "leo") +#TODO put it in xml file +ARDUINO_NODE_LIST = ("ard_asserv",) +SERIAL_READ_SIZE = 50 +# Because of the ugly process with multiple different baudrate, this value has to be >= 3 +ARDUINO_LOOP_MAX_TRY = 3 + + +class PortFinder: + def __init__(self): + rospy.logdebug("Starting the node.") + # List containing the content of the xml definition file + self._components_list = [] + # List containing the connected components and the corresponding port + self._connected_component_list = [] + # List containing the final processing information, matching the serial port and the serial component + self._associated_port_list = [] + # List of file descriptor for calls to rosserial + self._rosserial_call_list = [] + # Init ROS stuff + rospy.init_node(NODE_NAME, anonymous=False, log_level=rospy.INFO) + def_dir = rospkg.RosPack().get_path(PACKAGE_NAME) + "/def" + self._parse_xml_file(def_dir + "/components_list.xml") + self._parse_dmesg() + self._associate_port() + self._identify_arduino() + rospy.loginfo("Port_finder ready, found : " + str(self._associated_port_list)) + self._srv_goto = rospy.Service("/drivers/" + NODE_NAME + "/get_port", GetPort, self._callback_get_port) + + # TODO find an other way ? + # Launch urg_node directly from port_finder to have the correct port + hokuyo_subprocess = None + hokuyo_port = self.get_port("hokuyo") + if hokuyo_port is not "": + hokuyo_subprocess = subprocess.Popen(["rosrun", "urg_node", "urg_node", "_serial_port:=" + hokuyo_port, "__ns:=sensors", "scan:=/processing/lidar_objects/scan"]) + StatusServices("drivers", "port_finder").ready(True) + else: + rospy.logwarn("Couldn't start urg_node") + StatusServices("drivers", "port_finder").ready(False) + + rospy.spin() + for rosserial_fd in self._rosserial_call_list: + rosserial_fd.terminate() + if hokuyo_subprocess: + hokuyo_subprocess.terminate() + + def get_port(self, device_name): + port_name = "" + if device_name == "all": + port_name = str(self._associated_port_list) + elif device_name in dict(self._associated_port_list).keys(): + port_name = dict(self._associated_port_list)[device_name] + return port_name + + def _callback_get_port(self, request): + response = self.get_port(request.component) + return GetPortResponse(response) + + def _parse_xml_file(self, file): + rospy.logdebug("Parsing port_finder definition...") + try: + root = ET.parse(file).getroot() + # TODO catch a real exception instead of all of them... + except: + rospy.logerr("File {} not found...".format(file)) + root = None + components = [] + if root is not None: + for component in root.findall("component"): + if "name" not in component.attrib: + rospy.logerr("Can't parse component: a 'name' attribute is required. Skipping this component.") + continue + required = ["name", "vendor_id", "product_id", "port_type", "rosserialable"] + for param in required: + if component.attrib[param] is None: + rospy.logerr("Can't parse component definition: a '{}' element is required. Skipping this component.".format(p)) + components.append({ + "name": component.attrib["name"], + "vendor_id": component.attrib["vendor_id"], + "product_id": component.attrib["product_id"], + "port_type": component.attrib["port_type"], + "rosserialable": component.attrib["rosserialable"] + }) + if not components: + rospy.logwarn("No component found in component_list definition.") + rospy.logdebug("{} components found in component_list definition".format(len(components))) + self._components_list = components + + def _parse_dmesg(self): + dmesg_output = self._get_dmesg().split('\n') + id_dict = {} + tty_dict = {} + id_dict_filtered = {} + merged_filtered_id_tty_list = [] + vendor_id_regex = re.compile('usb (.*):.*idVendor=([a-z0-9]+).*idProduct=([a-z0-9]+)') + tty_regexp = re.compile(' ([0-9\-.]+):.*(ttyACM.|ttyUSB.)') + # Parse the whole file to extract a list of lines containing idVendor and an other list containing ttyX + for line in dmesg_output: + vendor_id_matched = vendor_id_regex.search(line) + tty_matched = tty_regexp.search(line) + if vendor_id_matched is not None: + id_dict[vendor_id_matched.group(1)] = (vendor_id_matched.group(2), vendor_id_matched.group(3)) + if tty_matched is not None: + tty_dict[tty_matched.group(1)] = tty_matched.group(2) + # Filter the idVendor list to keep only the idVendor we use + for element in id_dict: + for component in self._components_list: + if id_dict[element][0] == component["vendor_id"] and id_dict[element][1] == component["product_id"]: + id_dict_filtered[element] = (id_dict[element] + (component["name"],)) + # Merge the information of vendor_id and tty port to have a single tuple in list + for element in id_dict_filtered: + for tty_element in tty_dict: + if tty_element == element: + merged_filtered_id_tty_list.append((id_dict_filtered[element][0], id_dict_filtered[element][1], tty_dict[tty_element], id_dict_filtered[element][2])) + break + self._connected_component_list = merged_filtered_id_tty_list + rospy.logdebug("Connected components : " + str(self._connected_component_list)) + + def _get_dmesg(self): + """ + From https://gist.github.com/saghul/542780 + """ + try: + sub = subprocess.Popen("dmesg", stdout=subprocess.PIPE, stderr=subprocess.PIPE) + stdout, stderr = sub.communicate() + returncode = sub.returncode + except OSError, e: + if e.errno == 2: + raise RuntimeError('"dmesg" is not present on this system') + else: + raise + if returncode != 0: + raise RuntimeError('Got return value %d while executing "%s", stderr output was:\n%s' % ( + returncode, " ".join("dmesg"), stderr.rstrip("\n"))) + return stdout + + def _associate_port(self): + for element in self._connected_component_list: + associated_element = (element[3], "/dev/" + element[2]) + if associated_element not in self._associated_port_list: + self._associated_port_list.append(associated_element) + else: + rospy.logdebug("Port_finder double found : " + str(associated_element)) + rospy.logdebug("Associated components : " + str(self._associated_port_list)) + + def _identify_arduino(self): + # Temporary list used to store identified components + final_port_list = [] + for counter, element in enumerate(self._associated_port_list): + if self._check_rosseriable(element[0]): + read_data = "" + serial_port_disconnected = False + arduino_node_flag = False + teraranger_flag = False + loop_counter = 0 + try: + while (read_data == "") and (loop_counter < ARDUINO_LOOP_MAX_TRY): + # TODO find a better way than using loop_counter... + if loop_counter == 1 and element[0] in ("ard_nano", "teraranger"): + com_line = serial.Serial(element[1], 115200, timeout=2) + else: + com_line = serial.Serial(element[1], 57600, timeout=2) + read_data = com_line.read(SERIAL_READ_SIZE) + com_line.close() + # Received a null character, close and open again the port + if len(read_data) != 0 and read_data[0] == '\x00': + read_data = "" + # TODO fin a better way to do it... + if loop_counter == 1 and element[0] in ("ard_nano", "teraranger"): + # Check for the teraranger protocol header + if read_data.find('\x54') != -1: + rospy.loginfo("Found a teraranger sensor !") + teraranger_flag = True + else: + read_data = "" + rospy.logdebug("Try number {} for {}, data = {}".format(loop_counter, element[1], read_data)) + # Always skip the first try, this is because sometimes serial data are present, sometimes not + # Do not remove this because it's the ugly way found to start a serial line with an other baudrate and keep a "correct" detection + if loop_counter == 0: + read_data = "" + # rospy.loginfo(read_data.encode('hex')) + loop_counter += 1 + except serial.SerialException: + rospy.logerr("Try to open port {} but it fails...".format(element[1])) + serial_port_disconnected = True + if not serial_port_disconnected: + if teraranger_flag: + final_port_list.append(("teraranger", element[1])) + # Check if it's an arduino using UTCoupe protocol + for arduino_node in ARDUINO_NODE_LIST: + if read_data.find(arduino_node) != -1: + final_port_list.append((arduino_node, element[1])) + rospy.loginfo("Found a real arduino named " + arduino_node) + arduino_node_flag = True + # Otherwise, in any case, start rosserial + # TODO try to check with rosserial protocol (0xFF0xFE) + if not (arduino_node_flag or teraranger_flag): + rospy.loginfo("Found an arduino to start with rosserial : " + element[1] + ", start it.") + self._rosserial_call_list.append(subprocess.Popen(["rosrun", "rosserial_python", "serial_node.py", element[1], "__name:=serial_node_" + str(counter)])) + # Replace the tuple in list to keep a track that the port is used by rosserial + # Add an arbitrary id to rosserial to avoid having 2 components with the same name + final_port_list.append(("rosserial_node_" + str(counter), element[1])) + else: + final_port_list.append((element[0], element[1])) + self._associated_port_list = final_port_list + + def _check_rosseriable(self, component_name): + returned_value = False + if component_name != "": + for element in self._components_list: + if element["name"] == component_name: + if element["rosserialable"] == "true": + returned_value = True + return returned_value + + +if __name__ == "__main__": + PortFinder() diff --git a/ros_ws/src/drivers_port_finder/srv/GetPort.srv b/ros_ws/src/drivers_port_finder/srv/GetPort.srv new file mode 100644 index 0000000..be152b4 --- /dev/null +++ b/ros_ws/src/drivers_port_finder/srv/GetPort.srv @@ -0,0 +1,4 @@ +# Get the port associated with the desired component +string component +--- +string port \ No newline at end of file diff --git a/ros_ws/src/drivers_teraranger/CMakeLists.txt b/ros_ws/src/drivers_teraranger/CMakeLists.txt new file mode 100644 index 0000000..522ca27 --- /dev/null +++ b/ros_ws/src/drivers_teraranger/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 2.8.3) +project(drivers_teraranger) + +find_package(catkin REQUIRED COMPONENTS + drivers_port_finder + drivers_ard_others + ai_game_manager + sensor_msgs +) + +catkin_package( + CATKIN_DEPENDS + drivers_port_finder + drivers_ard_others + sensor_msgs +) + +include_directories( + ${catkin_INCLUDE_DIRS} +) + +############# +## Install ## +############# + +catkin_install_python( + PROGRAMS src/teraranger_node.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/ros_ws/src/drivers_teraranger/package.xml b/ros_ws/src/drivers_teraranger/package.xml new file mode 100644 index 0000000..3017402 --- /dev/null +++ b/ros_ws/src/drivers_teraranger/package.xml @@ -0,0 +1,19 @@ + + + drivers_teraranger + 0.0.1 + + Teraranger driver interface. + + GPLv3 + Thomas Fuhrmann + UTCoupe + catkin + rospy + sensor_msgs> + drivers_port_finder> + drivers_ard_others> + ai_game_manager> + + + diff --git a/ros_ws/src/drivers_teraranger/src/teraranger_node.py b/ros_ws/src/drivers_teraranger/src/teraranger_node.py new file mode 100755 index 0000000..efd3cdd --- /dev/null +++ b/ros_ws/src/drivers_teraranger/src/teraranger_node.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python + +import subprocess +import rospy +from drivers_ard_others.msg import * +from drivers_port_finder.srv import * +from ai_game_manager import StatusServices +from sensor_msgs.msg import Range +from numpy import inf + +__author__ = "Thomas Fuhrmann" +__date__ = 06/01/2018 + +NODE_NAME = "teraranger" +PUBLISH_INTERVAL = 0.02 # in ms +GET_PORT_SERVICE_NAME = "/drivers/port_finder/get_port" +GET_PORT_SERVICE_TIMEOUT = 15 # in seconds +WATCHDOG_PERIOD = 0.1 # in seconds + + +class Teraranger: + def __init__(self): + rospy.init_node(NODE_NAME, anonymous=False, log_level=rospy.INFO) + teraranger_port = "" + self._node_subprocess = None + self._connected = False + try: + rospy.wait_for_service(GET_PORT_SERVICE_NAME, GET_PORT_SERVICE_TIMEOUT) + self._src_client_get_port = rospy.ServiceProxy(GET_PORT_SERVICE_NAME, GetPort) + teraranger_port = self._src_client_get_port("teraranger").port + except rospy.ROSException: + rospy.logerr("Port_finder service does not exist, can't fetch the teraranger port...") + + status = StatusServices("drivers", "teraranger") + if teraranger_port != "": + self._node_subprocess = subprocess.Popen(["rosrun", "teraranger", "one", "_portname:=" + teraranger_port, "__ns:=/"]) + self._watchdog = rospy.Timer(rospy.Duration(WATCHDOG_PERIOD), self._check_subprocess) + self._connected = True + status.ready(True) + else: + rospy.logerr("Teraranger port has not been found, start the node but can't send real data...") + status.ready(False) + self._range_value = 0.0 + self._pub_belt_range = rospy.Publisher("/drivers/ard_others/belt_ranges", BeltRange, queue_size=1) + self._sub_terarange = rospy.Subscriber("/teraranger_one", Range, self._callback_teranranger_range) + while not rospy.is_shutdown(): + if self._connected: + self._pub_belt_range.publish(BeltRange("sensor_tera1", self._range_value)) + rospy.sleep(PUBLISH_INTERVAL) + if self._node_subprocess: + self._node_subprocess.terminate() + + def _callback_teranranger_range(self, data): + if data.range == -inf: + data.range = -1 + self._range_value = float(data.range) + + def _check_subprocess(self, event): + if self._node_subprocess.poll() is not None: + if self._connected: + rospy.logwarn("Teraranger node subprocess as quit, stop publishing data.") + self._connected = False + + +if __name__ == "__main__": + Teraranger() diff --git a/ros_ws/src/memory_definitions/CMakeLists.txt b/ros_ws/src/memory_definitions/CMakeLists.txt new file mode 100644 index 0000000..1107bb9 --- /dev/null +++ b/ros_ws/src/memory_definitions/CMakeLists.txt @@ -0,0 +1,49 @@ +cmake_minimum_required(VERSION 2.8.3) +project(memory_definitions) + +find_package(catkin REQUIRED COMPONENTS message_generation) + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +add_service_files( + FILES + GetDefinition.srv + ) + +generate_messages( + DEPENDENCIES + ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +catkin_package() + +########### +## Build ## +########### + +include_directories() + + +############# +## Install ## +############# + +catkin_install_python( + PROGRAMS src/definitions_node.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install( + DIRECTORY def/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/def +) + +install( + DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +) diff --git a/ros_ws/src/memory_definitions/def/ai/system_orders.xml b/ros_ws/src/memory_definitions/def/ai/system_orders.xml new file mode 100644 index 0000000..43f0e72 --- /dev/null +++ b/ros_ws/src/memory_definitions/def/ai/system_orders.xml @@ -0,0 +1,147 @@ + + + + + 0 + + + 1 + + + + + + 0 + + + + + + + 1 + + + + + + 0 + 100 + + + 1 + + + + + + 1 + + + 1 + + + + + + + /objects/cube_1 + /objects/cube_2 + + + 1 + + + + + + 1 + + + + 1 + + + + + + + + + + + + 1 + + + + + + + 1 + + + 1 + + + + + + 1 + + 0 + 1 + + + 1 + + + 0 + + + + + + 1 + + 1 + + + 1 + + + + + + + + + + + 1 + + + + + + + + + + + + 1 + + + + + + + + 0 + 0 + + + 1 + + + \ No newline at end of file diff --git a/ros_ws/src/memory_definitions/def/map/1_Config.yml b/ros_ws/src/memory_definitions/def/map/1_Config.yml new file mode 100644 index 0000000..8b4769a --- /dev/null +++ b/ros_ws/src/memory_definitions/def/map/1_Config.yml @@ -0,0 +1,18 @@ +config: + teams: + green: + default: True + orange: + default: False + transforms: + - x_mirror + - a_mirror + + colors: + light_gray: [0.95, 0.95, 0.95, 1 ] + green: [0.1 , 1.0 , 0.1 , 1.0 ] + blue: [0.1 , 0.1 , 1.0 , 1.0 ] + yellow: [1 , 1 , 0.1 , 1 ] + black: [0.1 , 0.1 , 0.1 , 1 ] + orange: [1 , 0.6 , 0.1 , 1 ] + purple: [0.3 , 0.3 , 0.5 , 0.95] diff --git a/ros_ws/src/memory_definitions/def/map/2_Terrain.yml b/ros_ws/src/memory_definitions/def/map/2_Terrain.yml new file mode 100644 index 0000000..309af0c --- /dev/null +++ b/ros_ws/src/memory_definitions/def/map/2_Terrain.yml @@ -0,0 +1,143 @@ +terrain: + shape: + type: rect + width: 3.0 + height: 2.0 + _marker: + ns: table + id: 0 + type: mesh + scale: [0.001, 0.001, 0.001] + z: 0.0 + orientation: [1.57079632679, 0, 0] + color: light_gray + mesh_path: "package://memory_definitions/def/map/robotcities2018_map.stl" + walls: + layer_ground: + wall_top: + position: + frame_id: /map + x: 1.5 + y: 2.011 + type: fixed + shape: + type: rect + width: 3.044 + height: 0.022 + wall_bottom: + position: + frame_id: /map + x: 1.5 + y: -0.011 + type: fixed + shape: + type: rect + width: 3.044 + height: 0.022 + wall_left: + position: + frame_id: /map + x: -0.011 + y: 1 + type: fixed + shape: + type: rect + width: 0.022 + height: 2.0 + wall_right: + position: + frame_id: /map + x: 3.011 + y: 1 + type: fixed + shape: + type: rect + width: 0.022 + height: 2.0 + stations: + position: + frame_id: /map + x: 1.5 + y: 0.125 + type: fixed + shape: + type: rect + width: 1.212 + height: 0.250 + + layer_belt: + castle_green: + position: + frame_id: /map + x: 0.180 + y: 2.180 + type: fixed + shape: + type: rect + width: 0.360 + height: 0.360 + castle_orange: + position: + frame_id: /map + x: 2.820 + y: 2.180 + type: fixed + shape: + type: rect + width: 0.360 + height: 0.360 + uni_distrib_green: + position: + frame_id: /map + x: 0.05 + y: 1.160 + type: fixed + shape: + type: rect + width: 0.1 + height: 0.08 + multi_distrib_green: + position: + frame_id: /map + x: 2.390 + y: 0.05 + type: fixed + shape: + type: rect + width: 0.08 + height: 0.1 + uni_distrib_orange: + position: + frame_id: /map + x: 2.95 + y: 1.160 + type: fixed + shape: + type: rect + width: 0.1 + height: 0.08 + multi_distrib_orange: + position: + frame_id: /map + x: 0.610 + y: 0.05 + type: fixed + shape: + type: rect + width: 0.08 + height: 0.1 + stations: + position: + frame_id: /map + x: 1.5 + y: 0.125 + type: fixed + shape: + type: rect + width: 1.212 + height: 0.250 + + layer_pathfinder: + _include: + - layer_ground + - layer_belt diff --git a/ros_ws/src/memory_definitions/def/map/3_Zones.yml b/ros_ws/src/memory_definitions/def/map/3_Zones.yml new file mode 100644 index 0000000..430e84a --- /dev/null +++ b/ros_ws/src/memory_definitions/def/map/3_Zones.yml @@ -0,0 +1,20 @@ +zones: + spawn_zone: + position: + x: 0.2 + y: 1.675 + type: fixed + frame_id: map + shape: + type: rect + width: 0.400 + height: 0.650 + _marker: + ns: zones + z_scale: 0.001 + z: 0.001 + orientation: [0, 0, 0] + color: green + + properties: + walkable: true \ No newline at end of file diff --git a/ros_ws/src/memory_definitions/def/map/4_Waypoints.yml b/ros_ws/src/memory_definitions/def/map/4_Waypoints.yml new file mode 100644 index 0000000..fc749b3 --- /dev/null +++ b/ros_ws/src/memory_definitions/def/map/4_Waypoints.yml @@ -0,0 +1,293 @@ +waypoints: + way_spawn_approach: + position: + frame_id: /map + x: 0.4 + y: 1.65 + a: 0.0 + type: fixed + way_spawn: + position: + frame_id: /map + x: 0.228 + y: 1.448 + a: -1.57079 + type: fixed + + way_spawn_gr: + position: + frame_id: /map + x: 0.088 + y: 1.800 + a: 0 + type: fixed + + way_spawn_pr: + position: + frame_id: /map + x: 0.161 + y: 1.515 + a: -1.5707 + type: fixed + + way_button_approach: + position: + frame_id: /map + x: 1.13 + y: 1.772 #1.65 PR + a: -1.57079 + type: fixed + way_button_approach_back: + position: + frame_id: /map + x: 1.13 + y: 1.72 #1.65 PR + a: -1.57079 + type: fixed + way_button_accost: # not used (pwm) + position: + frame_id: /map + x: 1.13 + y: 1.942 #1.895 PR + a: -1.57079 + type: fixed + + way_uni_distrib_approach: + position: + frame_id: /map + x: 0.4 + y: 1.16 + a: 0.0 + type: fixed + way_uni_distrib_accost: + position: + frame_id: /map + x: 0.115 + y: 1.16 + a: 0.0 + type: fixed + way_multi_distrib_approach: + position: + frame_id: /map + x: 2.390 + y: 0.400 + a: -1.57079 + type: fixed + way_multi_distrib_accost: + position: + frame_id: /map + x: 2.390 + y: 0.115 + a: -1.57079 + type: fixed + + way_fire: + position: + frame_id: /map + x: 0.4 + y: 1.16 + a: 4.952 + type: fixed + + + way_fire_station: + position: + frame_id: /map + x: 1.514 + y: 0.6 + a: 5.442 + type: fixed + + way_fire_unicolor: + position: + frame_id: /map + x: 0.4 + y: 1.16 + a: 1.75 + type: fixed + + way_station_approach: + position: + frame_id: /map + x: 1.514 + y: 0.6 + a: 1.570 + type: fixed + + # mesured with PR on 03/05 + way_station_accost: + position: + frame_id: /map + x: 1.514 + y: 0.373 + a: 1.570 + type: fixed + + way_bee_approach: + position: + frame_id: /map + x: 0.24 #0.22 + y: 0.4 + a: -1.57079 + type: fixed + way_bee_accost: + position: + frame_id: /map + x: 0.24 #0.22 + y: 0.188 + a: -1.57079 + type: fixed + + + # TODO ------------ + way_bee_approach_green: + position: + frame_id: /map + x: 0.2665 #0.2665 + y: 0.7 #0.5 + a: -1.57079 + type: fixed + way_bee_accost_green: + position: + frame_id: /map + x: 0.2665 + y: 0.25 + a: -1.57079 + type: fixed + + way_bee_approach_orange: + position: + frame_id: /map + x: 0.74 + y: 0.74 + a: -2.356 + type: fixed + way_bee_accost_orange: + position: + frame_id: /map + x: 0.285 + y: 0.285 + a: -2.356 + type: fixed + + # ------------------ + + way_grape_1_approach: + position: + frame_id: /map + x: 0.850 + y: 0.8 + a: -1.57079 + type: fixed + + way_grape_1_accost: + position: + frame_id: /map + x: 0.850 + y: 0.83 + a: -1.57079 + type: fixed + way_grape_1_pushed: + position: + frame_id: /map + x: 0.680 + y: 1.83 + a: -1.57079 + type: fixed + way_grape_1_released: + position: + frame_id: /map + x: 0.850 + y: 1.48 + a: -1.57079 + type: fixed + + way_grape_2_approach: + position: + frame_id: /map + x: 0.75 + y: 0.46 + a: 0.0 + type: fixed + way_grape_2_accost: + position: + frame_id: /map + x: 0.3 + y: 0.56 + a: -1.57079 + type: fixed + way_grape_2_pushed: + position: + frame_id: /map + x: 0.6 + y: 1.78 + a: -1.57079 + type: fixed + way_grape_2_released: + position: + frame_id: /map + x: 0.6 + y: 1.48 + a: -1.57079 + type: fixed + + + + way_cube_close_bee: + position: + frame_id: /map + x: 0.3 + y: 0.5 + a: 0 + type: fixed + + way_past_cube_1: + position: + frame_id: /map + x: 1.35 + y: 0.5 + a: 0 + type: fixed + + way_past_cube_2: + position: + frame_id: /map + x: 0.58 + y: 1.635 + a: 0 + type: fixed + + way_cube_2: + position: + frame_id: /map + x: 0.85 + y: 1.46 + a: 1.57079 + type: fixed + + way_cube_3: + position: + frame_id: /map + x: 0.13 + y: 0.86 + a: 0 + type: fixed + + way_past_cube_3: + position: + frame_id: /map + x: 0.65 + y: 0.86 + a: 0 + type: fixed + + way_next_cube_3: + position: + frame_id: /map + x: 0.65 + y: 1.55 + a: 1.57079 + type: fixed + + + diff --git a/ros_ws/src/memory_definitions/def/map/5_Entities.yml b/ros_ws/src/memory_definitions/def/map/5_Entities.yml new file mode 100644 index 0000000..be2bec1 --- /dev/null +++ b/ros_ws/src/memory_definitions/def/map/5_Entities.yml @@ -0,0 +1,47 @@ +entities: + gr: + position: + frame_id: /map + x: 0.1 + y: 0.6 + a: 0.002 + type: dynamic + shape: + type: rect + width: 0.26 + height: 0.333 + _marker: + ns: robots + id: 0 + type: cube + scale: [0.26, 0.333, 0.35] + z: 0.175 + orientation: [0, 0, 0] + color: purple + containers: [] + trajectory: + history: null + currentpath: null + pr: + position: + frame_id: /map + x: 1.243 + y: 0.42 + a: 3.1415 + type: dynamic + shape: + type: rect + width: 0.2 + height: 0.22 + _marker: + ns: robots + id: 1 + type: cube + scale: [0.2, 0.22, 0.35] + z: 0.175 + orientation: [0, 0, 0] + color: purple + containers: [] + trajectory: + history: null + currentpath: null \ No newline at end of file diff --git a/ros_ws/src/memory_definitions/def/map/6_Objects.yml b/ros_ws/src/memory_definitions/def/map/6_Objects.yml new file mode 100644 index 0000000..0f44af6 --- /dev/null +++ b/ros_ws/src/memory_definitions/def/map/6_Objects.yml @@ -0,0 +1,227 @@ +classes: + cube: + collision: true # is the object to be avoided in the pathfinder + position: + frame_id: /map + type: pickable + a: 0 + shape: + type: rect + width: 0.06 + height: 0.06 + _marker: + ns: cubes + z_scale: 0.06 + z: 0.03 + orientation: [0, 0, 0] + + ball: + collision: false + position: + frame_id: /map + type: pickable + a: 0 + shape: + type: circle + radius: 0.022 + color: green #default, overritten when orange + _marker: + ns: balls + type: sphere + z_scale: 0.044 + z: 1 #default + orientation: [0, 0, 0] + + +objects: + container_grape_1: + cube_1: + class: cube + position: + x: 0.850 + y: 1.520 + color: black + cube_2: + class: cube + position: + x: 0.790 + y: 1.460 + color: green + cube_3: + class: cube + position: + x: 0.850 + y: 1.460 + color: yellow + cube_4: + class: cube + position: + x: 0.910 + y: 1.460 + color: orange + cube_5: + class: cube + position: + x: 0.850 + y: 1.400 + color: blue + + container_grape_2: + cube_1: + class: cube + position: + x: 0.3 + y: 0.95 + color: black + cube_2: + class: cube + position: + x: 0.24 + y: 0.89 + color: green + cube_3: + class: cube + position: + x: 0.3 + y: 0.89 + color: yellow + cube_4: + class: cube + position: + x: 0.36 + y: 0.89 + color: orange + cube_5: + class: cube + position: + x: 0.3 + y: 0.83 + color: blue + + container_grape_3: + cube_1: + class: cube + position: + x: 1.1 + y: 0.56 + color: black + cube_2: + class: cube + position: + x: 1.04 + y: 0.5 + color: green + cube_3: + class: cube + position: + x: 1.1 + y: 0.5 + color: yellow + cube_4: + class: cube + position: + x: 1.16 + y: 0.5 + color: orange + cube_5: + class: cube + position: + x: 1.1 + y: 0.44 + color: blue + + container_grape_4: + cube_1: + class: cube + position: + x: 2.150 + y: 1.520 + color: black + cube_2: + class: cube + position: + x: 2.210 + y: 1.460 + color: green + cube_3: + class: cube + position: + x: 2.150 + y: 1.460 + color: yellow + cube_4: + class: cube + position: + x: 2.090 + y: 1.460 + color: orange + cube_5: + class: cube + position: + x: 2.150 + y: 1.400 + color: blue + + container_grape_5: + cube_1: + class: cube + position: + x: 2.7 + y: 0.95 + color: black + cube_2: + class: cube + position: + x: 2.76 + y: 0.89 + color: green + cube_3: + class: cube + position: + x: 2.7 + y: 0.89 + color: yellow + cube_4: + class: cube + position: + x: 2.64 + y: 0.89 + color: orange + cube_5: + class: cube + position: + x: 2.7 + y: 0.83 + color: blue + + container_grape_6: + cube_1: + class: cube + position: + x: 1.9 + y: 0.56 + color: black + cube_2: + class: cube + position: + x: 1.96 + y: 0.5 + color: green + cube_3: + class: cube + position: + x: 1.9 + y: 0.5 + color: yellow + cube_4: + class: cube + position: + x: 1.84 + y: 0.5 + color: orange + cube_5: + class: cube + position: + x: 1.9 + y: 0.44 + color: blue diff --git a/ros_ws/src/memory_definitions/def/map/alt/classes.xml b/ros_ws/src/memory_definitions/def/map/alt/classes.xml new file mode 100644 index 0000000..41757a0 --- /dev/null +++ b/ros_ws/src/memory_definitions/def/map/alt/classes.xml @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/ros_ws/src/memory_definitions/def/map/alt/config.xml b/ros_ws/src/memory_definitions/def/map/alt/config.xml new file mode 100644 index 0000000..6a564df --- /dev/null +++ b/ros_ws/src/memory_definitions/def/map/alt/config.xml @@ -0,0 +1,41 @@ + + + + + + + mesh + table + + + + + package://memory_definitions/def/map/robotcities2018_map.stl + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/ros_ws/src/memory_definitions/def/map/alt/objects.xml b/ros_ws/src/memory_definitions/def/map/alt/objects.xml new file mode 100644 index 0000000..a0083c7 --- /dev/null +++ b/ros_ws/src/memory_definitions/def/map/alt/objects.xml @@ -0,0 +1,103 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/ros_ws/src/memory_definitions/def/map/alt/robots.xml b/ros_ws/src/memory_definitions/def/map/alt/robots.xml new file mode 100644 index 0000000..54af2e1 --- /dev/null +++ b/ros_ws/src/memory_definitions/def/map/alt/robots.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/ros_ws/src/memory_definitions/def/map/robotcities2018_map.stl b/ros_ws/src/memory_definitions/def/map/robotcities2018_map.stl new file mode 100644 index 0000000..f496f57 Binary files /dev/null and b/ros_ws/src/memory_definitions/def/map/robotcities2018_map.stl differ diff --git a/ros_ws/src/memory_definitions/def/robots/gr/ai/1_strategies.xml b/ros_ws/src/memory_definitions/def/robots/gr/ai/1_strategies.xml new file mode 100644 index 0000000..d394089 --- /dev/null +++ b/ros_ws/src/memory_definitions/def/robots/gr/ai/1_strategies.xml @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros_ws/src/memory_definitions/def/robots/gr/ai/2_actions.xml b/ros_ws/src/memory_definitions/def/robots/gr/ai/2_actions.xml new file mode 100644 index 0000000..30c3e0f --- /dev/null +++ b/ros_ws/src/memory_definitions/def/robots/gr/ai/2_actions.xml @@ -0,0 +1,303 @@ + + + + + + + + + + + + + + + + + way_spawn_gr + + + + + + 98 + + + + + + + + + + + + + + + + + + + + + 3 + 620 + + + + + + + + + 3 + 120 + 170 + + + + + + + + + 3 + 610 + 180 + + + + + + + + + 3 + 120 + + + + + + + + + + + way_button_approach + 1 + + + + -100 + -100 + 1200 + + + + 1.2 + + + + way_button_approach_back + 1 + + + + + + + + + way_grape_1_approach + 0 + + + + way_grape_1_pushed + 0 + + + -200 + -200 + 500 + + + + 0.5 + + + + + + + + + way_grape_2_approach + 0 + + + + way_grape_2_accost + 0 + + + + way_grape_2_pushed + 0 + + + + way_grape_2_released + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + way_bee_approach_green + + + + + + way_bee_accost_green + + + + + + + way_bee_approach_orange + + + + way_bee_accost_orange + + + + + + 0.5 + + + + + + way_bee_approach_orange + 0 + + + + + + + + + + + + + + + + way_cube_close_bee + 0 + + + + way_past_cube_1 + 1 + + + + way_past_cube_2 + 1 + 0 + + + + + 200 + 200 + 1000 + + + + 1 + + + + way_cube_2 + 0 + 0 + + + + + + way_cube_3 + 1 + + + + way_past_cube_3 + 1 + + + + way_next_cube_3 + 1 + 0 + + + + + 200 + 200 + 1000 + + + + 1 + + + + + + diff --git a/ros_ws/src/memory_definitions/def/robots/gr/ai/3_orders.xml b/ros_ws/src/memory_definitions/def/robots/gr/ai/3_orders.xml new file mode 100644 index 0000000..36414a4 --- /dev/null +++ b/ros_ws/src/memory_definitions/def/robots/gr/ai/3_orders.xml @@ -0,0 +1,26 @@ + + + + + + + 0 + 0 + + + 1 + + + + + + + + + 10.0 + + + 1 + + + diff --git a/ros_ws/src/memory_definitions/def/robots/gr/ai/README.md b/ros_ws/src/memory_definitions/def/robots/gr/ai/README.md new file mode 100644 index 0000000..14f6e40 --- /dev/null +++ b/ros_ws/src/memory_definitions/def/robots/gr/ai/README.md @@ -0,0 +1,81 @@ +# AI Actions definition files + +(_TODO writing in progress_) +This is where the robot is told what to do. + +## 1. Files +- `1_strategies.xml` + +File basic structure : +```xml + + + ... actionrefs and orderrefs + + + ... actionrefs and orderrefs + + +``` + +- `2_actions.xml` + +File basic structure : +```xml + + + ... actionrefs and orderrefs + + + ... actionrefs and orderrefs + + +``` + + + + +## 2. AI Objects + +### 1. Strategies +The `1_Strategies.json` file is a JSON dictionary of several Strategy objects. When the whole system is launched, the user selects which strategy the robot has to follow. +This lets the user create several ways ("strategies") of achieving the final objective, and quickly execute the one he wants at each execution time. + + +### 2. ActionLists +Group tags are a list or actions. This list can be configured as : +- `linear` : Sorted (the robot follows the order of actions in the list given in the file) +- `mostreward` : The AI automatically picks the sequence that will give the most amount of reward points. +- `fastest`, : The AI automatically picks the sequence that will execute the quickest. +- `simultaneous`, fastest actions first. + +When a group is executed, the conditions to consider it is successful or not can be configured : +- `all` : All actions must be done correctly. +- `one` : At least one action must be done correctly. +- `+` : Do all actions if possible, but if one is done it will be considered successful. +- `last` : The last action must be executed correctly. + +Properties : +- `reward`: If the whole group execution is considered successful, this is the amount of reward points given. + +### 3. Actions +Actions are almost the same as ActionLists, except they are necessarily defined in the file `2_actions.xml` and can be referenced by other actions. +An Action object is a group of `Orders`, `Actions` and `ActionLists`. + + +### 4. Orders +Properties : +- `time`, obligatory : Approximate time needed to complete an action (seconds). +- `reward` : If there are any, the action gives reward points to the AI. + +Supported embedded actions that can be used in actions: +- The `wheels` group communicates with the ROS `robot_movement_wheels` package. This is how the AI asks for the robot to move from a point to another. Actions defined : + - `wheels_gotoxy`: Ask the robot to move to a certain position. angle doesn't matter. + - `wheels_gotoxya`: Go to a certain position and angle. + - `wheels_gotoa`: Ask the robot to turn toward a certain direction. + - `wheels_delay`: Stop the robot for a certain duration (seconds). +- The `actuators` group: + - `actuator_open`: Set the actuator to the `closed` position set in the `robot.json` description file. + - `actuator_close`: Set the actuator to the `open` position set in the `robot.json` description file. + - `actuator_toggle`: Toggle between open/close position. + - `actuator_setpos`: Manually set a position to the actuator. diff --git a/ros_ws/src/memory_definitions/def/robots/gr/memory/.empty b/ros_ws/src/memory_definitions/def/robots/gr/memory/.empty new file mode 100644 index 0000000..e69de29 diff --git a/ros_ws/src/memory_definitions/def/robots/gr/movement/actuators.xml b/ros_ws/src/memory_definitions/def/robots/gr/movement/actuators.xml new file mode 100644 index 0000000..44ba444 --- /dev/null +++ b/ros_ws/src/memory_definitions/def/robots/gr/movement/actuators.xml @@ -0,0 +1,84 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros_ws/src/memory_definitions/def/robots/gr/movement/arm.yml b/ros_ws/src/memory_definitions/def/robots/gr/movement/arm.yml new file mode 100644 index 0000000..917b76d --- /dev/null +++ b/ros_ws/src/memory_definitions/def/robots/gr/movement/arm.yml @@ -0,0 +1,20 @@ +# closest motor to the origin +motor1: + id: 3 # id of the ax12 + center: 455 # value at which the angle is 0 + min: 100 # min value that is reacheable + max: 900 # you get it + length: 0.091 # length of the arm attached to this motor + +# the other one +motor2: + id: 1 + center: 512 + min: 100 + max: 900 + length: 0.106 + +# origin of the arm relative to the robot +origin: + x: 0 + y: 0 diff --git a/ros_ws/src/memory_definitions/def/robots/gr/navigation/.empty b/ros_ws/src/memory_definitions/def/robots/gr/navigation/.empty new file mode 100644 index 0000000..e69de29 diff --git a/ros_ws/src/memory_definitions/def/robots/gr/processing/belt.xml b/ros_ws/src/memory_definitions/def/robots/gr/processing/belt.xml new file mode 100644 index 0000000..dd9cc5f --- /dev/null +++ b/ros_ws/src/memory_definitions/def/robots/gr/processing/belt.xml @@ -0,0 +1,33 @@ + + + + 2 + 0.44 + 0.05 + 1 + + + 5 + 0.05235988 + 0.03 + 0 + + + + + -0.06 + 0.091 + 3.14159 + + + -0.06 + -0.104 + 3.14159 + + + 0 + 0 + 0 + + + diff --git a/ros_ws/src/memory_definitions/def/robots/gr/processing/lidar_objects.launch b/ros_ws/src/memory_definitions/def/robots/gr/processing/lidar_objects.launch new file mode 100644 index 0000000..1f44781 --- /dev/null +++ b/ros_ws/src/memory_definitions/def/robots/gr/processing/lidar_objects.launch @@ -0,0 +1,117 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros_ws/src/memory_definitions/def/robots/gr/recognition/.empty b/ros_ws/src/memory_definitions/def/robots/gr/recognition/.empty new file mode 100644 index 0000000..e69de29 diff --git a/ros_ws/src/memory_definitions/def/robots/gr/specific.launch b/ros_ws/src/memory_definitions/def/robots/gr/specific.launch new file mode 100644 index 0000000..05aa8e9 --- /dev/null +++ b/ros_ws/src/memory_definitions/def/robots/gr/specific.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/ros_ws/src/memory_definitions/def/robots/pr/ai/1_strategies.xml b/ros_ws/src/memory_definitions/def/robots/pr/ai/1_strategies.xml new file mode 100644 index 0000000..2809baa --- /dev/null +++ b/ros_ws/src/memory_definitions/def/robots/pr/ai/1_strategies.xml @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros_ws/src/memory_definitions/def/robots/pr/ai/2_actions.xml b/ros_ws/src/memory_definitions/def/robots/pr/ai/2_actions.xml new file mode 100644 index 0000000..f6e29e3 --- /dev/null +++ b/ros_ws/src/memory_definitions/def/robots/pr/ai/2_actions.xml @@ -0,0 +1,238 @@ + + + + + + + + + + + + + + + + + way_spawn_pr + + + + + + 97 + + + + + + + + + + + + way_button_approach + + + + way_button_accost + 0 + + + + way_button_approach + + + + + + + + > + way_uni_distrib_approach + 1 + + + + servo_lock + RIGHT + + + + way_uni_distrib_accost + 0 + 1 + + + + servo_lock + LEFT + + + + 0.5 + + + + way_uni_distrib_approach + 0 + + + + + + + + + way_multi_distrib_approach + + + + servo_lock + RIGHT + + + + way_multi_distrib_accost + 0 + 1 + + + + servo_lock + LEFT + + + + 0.5 + + + + way_multi_distrib_approach + 0 + + + + + + + + + way_fire_unicolor + 1 + + + + 0.9 + + + + + 0 + + + 0.5 + + + + + -1 + + + + + + + + + + way_station_approach + + + + way_station_accost + 0 + + + + servo_front_lift + OPEN + + + + servo_front_lift + OPEN + + + + way_station_approach + + + + + + + + + way_station_approach + + + + way_station_accost + 1 + + + + + 1 + + + + + + + + + + way_bee_approach + + + + way_bee_accost + + + + servo_button + OPEN + + + + 1 + + + + servo_button + CLOSE + + + + way_bee_approach + 0 + + + + + + + + + + + way_spawn + + + + diff --git a/ros_ws/src/memory_definitions/def/robots/pr/ai/3_orders.xml b/ros_ws/src/memory_definitions/def/robots/pr/ai/3_orders.xml new file mode 100644 index 0000000..92adeb5 --- /dev/null +++ b/ros_ws/src/memory_definitions/def/robots/pr/ai/3_orders.xml @@ -0,0 +1,28 @@ + + + + + + + 0 + 0 + + + 1 + + + + + + + 1.2 + 1 + + + + + + + + + diff --git a/ros_ws/src/memory_definitions/def/robots/pr/ai/README.md b/ros_ws/src/memory_definitions/def/robots/pr/ai/README.md new file mode 100644 index 0000000..a8f39f8 --- /dev/null +++ b/ros_ws/src/memory_definitions/def/robots/pr/ai/README.md @@ -0,0 +1,106 @@ +# AI Actions definition files + +(_TODO writing in progress_) +This is where the robot is told what to do. + +## 1. Files +- `1_Strategies.xml` + +File basic structure : +```xml + + + ... + + + ... + + +``` + +- `2_Actions.xml` + +File basic structure : +```xml + + + ... + + + ... + + +``` + + +- `3_HardwareActions.xml` + + + + + +## 2. AI Objects + +### 1. Strategies +The `1_Strategies.json` file is a JSON dictionary of several Strategy objects. When the whole system is launched, the user selects which strategy the robot has to follow. +This lets the user create several ways ("strategies") of achieving the final objective, and quickly execute the one he wants at each execution time. + +Strategy basic structure: + +```xml + + + 90 + + + ... + + + ... + + +``` + + + +### 2. ActionLists +Group tags are a list or actions. This list can be configured as : +- `linear` : Sorted (the robot follows the order of actions in the list given in the file) +- `mostreward` : The AI automatically picks the sequence that will give the most amount of reward points. +- `fastest`, : The AI automatically picks the sequence that will execute the quickest. +- `simultaneous`, fastest actions first. + +When a group is executed, the conditions to consider it is successful or not can be configured : +- `all` : All actions must be done correctly. +- `one` : At least one action must be done correctly. +- `+` : Do all actions if possible, but if one is done it will be considered successful. +- `last` : The last action must be executed correctly. + +Properties : +- `reward`: If the whole group execution is considered successful, this is the amount of reward points given. + +### 2. Actions +Actiosn are almost the same as ActionLists, except they are necessarily defined in the file `2_Actions.xml` and can be referenced by other actions. +An Action object is a group of `Orders`, `Actions` and `ActionLists`. + + +### 4. Orders +Properties : +- `time`, obligatory : Approximate time needed to complete an action (seconds). +- `reward` : If there are any, the action gives reward points to the AI. + +Supported embedded actions that can be used in actions: +- The `wheels` group communicates with the ROS `robot_movement_wheels` package. This is how the AI asks for the robot to move from a point to another. Actions defined : + - `wheels_gotoxy`: Ask the robot to move to a certain position. angle doesn't matter. + - `wheels_gotoxya`: Go to a certain position and angle. + - `wheels_gotoa`: Ask the robot to turn toward a certain direction. + - `wheels_delay`: Stop the robot for a certain duration (seconds). +- The `actuators` group: + - `actuator_open`: Set the actuator to the `closed` position set in the `robot.json` description file. + - `actuator_close`: Set the actuator to the `open` position set in the `robot.json` description file. + - `actuator_toggle`: Toggle between open/close position. + - `actuator_setpos`: Manually set a position to the actuator. + + +### 4. Actions execution +The last layer, `HardareActions.json`, diff --git a/ros_ws/src/memory_definitions/def/robots/pr/memory/.empty b/ros_ws/src/memory_definitions/def/robots/pr/memory/.empty new file mode 100644 index 0000000..e69de29 diff --git a/ros_ws/src/memory_definitions/def/robots/pr/movement/actuators.xml b/ros_ws/src/memory_definitions/def/robots/pr/movement/actuators.xml new file mode 100644 index 0000000..dbe7d36 --- /dev/null +++ b/ros_ws/src/memory_definitions/def/robots/pr/movement/actuators.xml @@ -0,0 +1,78 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros_ws/src/memory_definitions/def/robots/pr/navigation/.empty b/ros_ws/src/memory_definitions/def/robots/pr/navigation/.empty new file mode 100644 index 0000000..e69de29 diff --git a/ros_ws/src/memory_definitions/def/robots/pr/processing/belt.xml b/ros_ws/src/memory_definitions/def/robots/pr/processing/belt.xml new file mode 100644 index 0000000..35182de --- /dev/null +++ b/ros_ws/src/memory_definitions/def/robots/pr/processing/belt.xml @@ -0,0 +1,28 @@ + + + + 2 + 0.44 + 0.05 + 1 + + + 5 + 0.05235988 + 0.03 + 0 + + + + + 0.125 + 0.073 + 3.1415 + + + 0.125 + -0.047 + 3.1415 + + + diff --git a/ros_ws/src/memory_definitions/def/robots/pr/processing/lidar_objects.launch b/ros_ws/src/memory_definitions/def/robots/pr/processing/lidar_objects.launch new file mode 100644 index 0000000..0ba5f98 --- /dev/null +++ b/ros_ws/src/memory_definitions/def/robots/pr/processing/lidar_objects.launch @@ -0,0 +1,117 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros_ws/src/memory_definitions/def/robots/pr/recognition/.empty b/ros_ws/src/memory_definitions/def/robots/pr/recognition/.empty new file mode 100644 index 0000000..e69de29 diff --git a/ros_ws/src/memory_definitions/def/robots/pr/specific.launch b/ros_ws/src/memory_definitions/def/robots/pr/specific.launch new file mode 100644 index 0000000..6db066e --- /dev/null +++ b/ros_ws/src/memory_definitions/def/robots/pr/specific.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/ros_ws/src/memory_definitions/def/rviz/general.rviz b/ros_ws/src/memory_definitions/def/rviz/general.rviz new file mode 100644 index 0000000..7ca4835 --- /dev/null +++ b/ros_ws/src/memory_definitions/def/rviz/general.rviz @@ -0,0 +1,264 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Grid1/Offset1 + - /Navigation Markers1/Namespaces1 + - /Marker2 + Splitter Ratio: 0.517756999 + Tree Height: 886 + - 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: "" + - Class: Obstacle Publisher + Name: Obstacle Publisher + - Class: Obstacle Extractor + Name: Obstacle Extractor +Visualization Manager: + Class: "" + Displays: + - Class: rviz/Axes + Enabled: false + Length: 0.100000001 + Name: Global Axis + Radius: 0.00999999978 + Reference Frame: map + Value: false + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + arm_origin: + Value: true + belt_sensor1: + Value: true + belt_sensor2: + Value: true + belt_sensor_tera1: + Value: true + laser: + Value: true + map: + Value: true + odom: + Value: true + robot: + Value: true + Marker Scale: 0.5 + Name: TF + Show Arrows: false + Show Axes: true + Show Names: false + Tree: + map: + odom: + {} + robot: + arm_origin: + {} + belt_sensor1: + {} + belt_sensor2: + {} + belt_sensor_tera1: + {} + laser: + {} + Update Interval: 0 + Value: true + - Alpha: 0.5 + Cell Size: 0.100000001 + Class: rviz/Grid + Color: 255; 0; 0 + Enabled: false + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 1.5 + Y: 1 + Z: 0 + Plane: XY + Plane Cell Count: 30 + Reference Frame: map + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /visualization_markers/world + Name: World Markers + Namespaces: + balls: true + cubes: true + robots: true + table: true + waypoints: true + zones: true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /visualization_markers/navigation + Name: Navigation Markers + Namespaces: + collisions_main: true + collisions_path: true + Queue Size: 100 + Value: true + - Alpha: 0.600000024 + 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; 0; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 170; 0; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.0299999993 + Style: Boxes + Topic: /sensors/scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Circles color: 170; 0; 0 + Class: Obstacles + Enabled: true + Margin color: 0; 170; 0 + Name: Lidar Obstacles + Opacity: 0.75 + Segments color: 170; 170; 0 + Segments thickness: 0.0299999993 + Topic: /processing/lidar_objects/obstacles + Unreliable: false + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /visualization_markers/objects + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: false + Name: InteractiveMarkers + Show Axes: true + Show Descriptions: true + Show Visual Aids: false + Update Topic: /markers_servers/asserv_marker/update + Value: false + - Class: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: false + Name: InteractiveMarkers + Show Axes: true + Show Descriptions: true + Show Visual Aids: false + Update Topic: /markers_servers/navigator_marker/update + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /visualization_markers/objects + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Angle: -3.11500025 + Class: rviz/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Scale: 286.180908 + Target Frame: + Value: TopDownOrtho (rviz) + X: 1.26353836 + Y: 1.15049589 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1028 + Hide Left Dock: false + Hide Right Dock: false + Obstacle Extractor: + collapsed: false + Obstacle Publisher: + collapsed: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000003b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003a000003b0000000c600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000020e000003b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000003a000003b00000009e00fffffffa000000020100000003fb00000024004f00620073007400610063006c006500200045007800740072006100630074006f00720000000000ffffffff0000011500fffffffb00000024004f00620073007400610063006c00650020005000750062006c006900730068006500720000000000ffffffff000000f700fffffffb0000000a0056006900650077007301000008880000020e0000010000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000053e0000005bfc0100000002fb0000000800540069006d006500000000000000053e0000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000003cf000003b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1855 + X: 65 + Y: 24 diff --git a/ros_ws/src/memory_definitions/launch/debug_station.launch b/ros_ws/src/memory_definitions/launch/debug_station.launch new file mode 100644 index 0000000..716ce21 --- /dev/null +++ b/ros_ws/src/memory_definitions/launch/debug_station.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/ros_ws/src/memory_definitions/launch/general.launch b/ros_ws/src/memory_definitions/launch/general.launch new file mode 100644 index 0000000..d870f55 --- /dev/null +++ b/ros_ws/src/memory_definitions/launch/general.launch @@ -0,0 +1,53 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros_ws/src/memory_definitions/launch/test_asserv.launch b/ros_ws/src/memory_definitions/launch/test_asserv.launch new file mode 100644 index 0000000..d67ecac --- /dev/null +++ b/ros_ws/src/memory_definitions/launch/test_asserv.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros_ws/src/memory_definitions/launch/test_collision.launch b/ros_ws/src/memory_definitions/launch/test_collision.launch new file mode 100644 index 0000000..c0f556e --- /dev/null +++ b/ros_ws/src/memory_definitions/launch/test_collision.launch @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros_ws/src/memory_definitions/package.xml b/ros_ws/src/memory_definitions/package.xml new file mode 100644 index 0000000..89b252c --- /dev/null +++ b/ros_ws/src/memory_definitions/package.xml @@ -0,0 +1,20 @@ + + + memory_definitions + 0.0.0 + + Package with all the definition files the others packages need. + A service returns a full absolute path to the requested file. + The service can dynamically adapt the path based on ROS params. + E.g. The name of the robot, which will lead to different description files. + + GPLv3 + milesial + UTCoupe + catkin + message_generation + rospy + message_runtime + + + diff --git a/ros_ws/src/memory_definitions/src/definitions_node.py b/ros_ws/src/memory_definitions/src/definitions_node.py new file mode 100755 index 0000000..f733562 --- /dev/null +++ b/ros_ws/src/memory_definitions/src/definitions_node.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python + +from memory_definitions.srv import * +from ai_game_manager import StatusServices +import rospy +import rospkg +import os + + +PACKAGE_NAME = "memory_definitions" + +def callback(req): + def_dir = rospkg.RosPack().get_path(PACKAGE_NAME) + "/def" + req_split = req.request.split('/') + + if req_split[0] in ["ai", "memory", "navigation", "movement", "processing", "recognition", "drivers"]: + if not rospy.has_param('/robot'): + rospy.logerr("Parameter '/robot' not set, cannot provide the definition file {}".format(req.request)) + req_final = "" + else: + req_final = "robots/{}/{}".format(rospy.get_param("/robot").lower(), req.request) + if req_split[0] == "ai" and req_split[1] == "system_orders.xml": # exception. + req_final = req.request + else: + req_final = req.request + + + path = os.path.join(def_dir, req_final) + if os.path.isfile(path): + return GetDefinitionResponse(path, True) + else: + rospy.logerr("Request failed, file {} does not exist !".format(path)) + return GetDefinitionResponse("", False) + + +def server(): + rospy.init_node('definitions') + s = rospy.Service('/memory/definitions/get', GetDefinition, callback) + rospy.logdebug("Definitions server ready") + + # Tell ai/game_manager the node initialized successfuly. + StatusServices("memory", "definitions").ready(True) + + rospy.spin() + + +if __name__ == "__main__": + server() diff --git a/ros_ws/src/memory_definitions/srv/GetDefinition.srv b/ros_ws/src/memory_definitions/srv/GetDefinition.srv new file mode 100644 index 0000000..77de4f9 --- /dev/null +++ b/ros_ws/src/memory_definitions/srv/GetDefinition.srv @@ -0,0 +1,4 @@ +string request # e.g. processing/Belt.xml +--- +string path +bool success diff --git a/ros_ws/src/memory_map/CMakeLists.txt b/ros_ws/src/memory_map/CMakeLists.txt new file mode 100644 index 0000000..2be2634 --- /dev/null +++ b/ros_ws/src/memory_map/CMakeLists.txt @@ -0,0 +1,81 @@ +cmake_minimum_required(VERSION 2.8.3) +project(memory_map) + +find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs + geometry_msgs + message_generation +) + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +# Generate messages in the 'msg' folder +add_message_files( + FILES + Waypoint.msg +) + +# Generate services in the 'srv' folder +add_service_files( + FILES + MapGet.srv + MapSet.srv + MapTransfer.srv + MapGetOccupancy.srv + MapGetObjects.srv + FillWaypoint.srv +) + +# Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs + geometry_msgs +) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +################################### +## catkin specific configuration ## +################################### + +catkin_package( + CATKIN_DEPENDS message_runtime +) + +########### +## Build ## +########### + +include_directories( + ${catkin_INCLUDE_DIRS} +) + +############# +## Install ## +############# + +catkin_install_python( + PROGRAMS src/map_node.py src/map_communication.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install( + DIRECTORY src/map_manager src/markers src/occupancy + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + PATTERN "*.pyc" EXCLUDE +) + +install( + DIRECTORY def + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# diff --git a/ros_ws/src/memory_map/README.md b/ros_ws/src/memory_map/README.md new file mode 100644 index 0000000..6e07cae --- /dev/null +++ b/ros_ws/src/memory_map/README.md @@ -0,0 +1,40 @@ +# Description + +This is the Map Manager package. It is essentially the database that holds all the information, characteristics and statuses of all the elements +in the robot's environment. This includes the objects' positions, containers and the robot's containers. + +This package temporarily holds the RViz config file. In order to use it, open the file from RViz __before__ launching the node itself + (`rosrun memory_map map_node.py`). If not launched, the node will not publish its markers. + +# Services + +The package is aimed to offer three services called `memory/map/get`, `memory/map/set` and `memory/map/conditions`: +- The `get` service will let other packages retrieve the statuses of all elements in the database. This includes position, number of sub-elements +in a container, current speed of the enemy and so on. +- The `set` service is the way to update the database. For instance, a recognizer from the `Perception` Namespace will repeatedly +update the statuses of its designated objects. +- The `conditions` service will let packages easily test pre-implemented conditions on objects, containers and entities. For instance, the AI could +use this service to check if there are enough cubes in the robot's container before unloading them on the table. + +### Getting a value + +The node can only return a full object's dict or a single attribute value for now. In order to use it, create a path string defining the path in the database +until the specified attribute or object. For instance, in order to get the X position of the `cube_1` object, create a `MapGet` service request with +`/objects/cube_1/position/x` as the `request_path`. Typing `/objects/cube_1/position` will return the full position dict. + +__NOTE__ : For now, you can not get a dict with other dicts inside. For instance, you cannot get a full JSON description of a Map object (i.e. its position, +shape, visual, etc). You must ask for a dict which only has simple values, not dicts (e.g. the position attribute). + +### Setting a value + +The SET service allows you to change multiple values at a time of a same dict (that dosn't have any sub-dict inside as a value). + +Use the same request path structure as the GET service, except you must put `=new_value_here` after each attribute in your path. An example of a valid path would be `/objects/cube_13/position/x=0.42,y=0.84`, wich will change the specified values and leave the others untouched. Trying to set a new value to an entire dict will return an error (for now, TODO needed ?). + +__Note__ : A protection has been set that does not allow you to create new keys in the dicts. TODO Remove it ? + +# Topics + +This package will soon publish an image feed of the OccupancyGrid. `navigation_pathfinder` will be the main subscriber package. (__TODO__ or publish +a dictionary dedicated to physical obstacles. This would reduce the amount of traffic and memory usage if `navigation_pathfinder` is the only +subscriber). diff --git a/ros_ws/src/memory_map/def/occupancy/layer_belt.bmp b/ros_ws/src/memory_map/def/occupancy/layer_belt.bmp new file mode 100644 index 0000000..4d71c39 Binary files /dev/null and b/ros_ws/src/memory_map/def/occupancy/layer_belt.bmp differ diff --git a/ros_ws/src/memory_map/def/occupancy/layer_ground.bmp b/ros_ws/src/memory_map/def/occupancy/layer_ground.bmp new file mode 100644 index 0000000..afa8590 Binary files /dev/null and b/ros_ws/src/memory_map/def/occupancy/layer_ground.bmp differ diff --git a/ros_ws/src/memory_map/def/occupancy/layer_pathfinder.bmp b/ros_ws/src/memory_map/def/occupancy/layer_pathfinder.bmp new file mode 100644 index 0000000..f135c3e Binary files /dev/null and b/ros_ws/src/memory_map/def/occupancy/layer_pathfinder.bmp differ diff --git a/ros_ws/src/memory_map/msg/Waypoint.msg b/ros_ws/src/memory_map/msg/Waypoint.msg new file mode 100644 index 0000000..6d15fea --- /dev/null +++ b/ros_ws/src/memory_map/msg/Waypoint.msg @@ -0,0 +1,5 @@ +string name # Waypoint name, referenced in the map definition dict. + +bool has_angle # say whether the 'theta' attribute in pose is important +string frame_id # Position tf frame +geometry_msgs/Pose2D pose # X, Y, THETA values corrsponding to the Waypoint name. \ No newline at end of file diff --git a/ros_ws/src/memory_map/package.xml b/ros_ws/src/memory_map/package.xml new file mode 100644 index 0000000..c234036 --- /dev/null +++ b/ros_ws/src/memory_map/package.xml @@ -0,0 +1,17 @@ + + + memory_map + 0.0.0 + The memory_map package + GLPv3 + MadeInPierre + UTCoupe + catkin + message_generation + rospy + std_msgs + geometry_msgs + message_runtime + + + diff --git a/ros_ws/src/memory_map/src/map_communication.py b/ros_ws/src/memory_map/src/map_communication.py new file mode 100644 index 0000000..7fc7203 --- /dev/null +++ b/ros_ws/src/memory_map/src/map_communication.py @@ -0,0 +1,150 @@ +#!/usr/bin/python +import json +import time + +import rospy +import memory_map.msg +import memory_map.srv +from map_manager import SetMode, Map, DictManager +from occupancy import OccupancyGenerator + + +class Servers(): + GET_SERV = "/memory/map/get" + SET_SERV = "/memory/map/set" + TRANSFER_SERV = "/memory/map/transfer" + OCCUPANCY_SERV = "/memory/map/get_occupancy" + OBJECTS_SERV = "/memory/map/get_objects" + FILLWP_SERV = "/memory/map/fill_waypoint" + +class MapServices(): + def __init__(self, occupancy_generator): + self.GetSERV = rospy.Service(Servers.GET_SERV, memory_map.srv.MapGet, self.on_get) + self.SetSERV = rospy.Service(Servers.SET_SERV, memory_map.srv.MapSet, self.on_set) + self.TransferSERV = rospy.Service(Servers.TRANSFER_SERV, memory_map.srv.MapTransfer, self.on_transfer) + self.OccupancySERV = rospy.Service(Servers.OCCUPANCY_SERV, memory_map.srv.MapGetOccupancy, self.on_get_occupancy) + self.ObjectsSERV = rospy.Service(Servers.OBJECTS_SERV, memory_map.srv.MapGetObjects, self.on_get_objects) + self.FillWPSERV = rospy.Service(Servers.FILLWP_SERV, memory_map.srv.FillWaypoint, self.on_fill_waypoint) + self.occupancy_generator = occupancy_generator + + def on_get(self, req): + s = time.time() * 1000 + rospy.loginfo("GET:" + str(req.request_path)) + + success = False + response = Map.get(req.request_path) + if isinstance(response, DictManager): + rospy.logerr(" GET Request failed : '^' dict operator not allowed in services.") + response = None + + if response != None: + success = True + + rospy.logdebug(" Responding: " + str(response)) + rospy.logdebug(" Process took {0:.2f}ms".format(time.time() * 1000 - s)) + return memory_map.srv.MapGetResponse(success, json.dumps(response)) + + def on_get_objects(self, req): + s = time.time() * 1000 + rospy.loginfo("GET_OBJECTS:collisions_only=" + str(req.collisions_only)) + + success = False + objects = Map.get_objects(collisions_only=req.collisions_only) + + if objects != None: + success = True + + rospy.logdebug(" Responding: {} object(s) found.".format(len(objects))) + rospy.logdebug(" Process took {0:.2f}ms".format(time.time() * 1000 - s)) + return memory_map.srv.MapGetObjectsResponse(success, objects) + + def on_set(self, req): + s = time.time() * 1000 + rospy.loginfo("SET:" + str(req.request_path)) + + success = False + # success = Map.set(req.request_path, req.mode) + try: + success = Map.set(req.request_path, req.mode) + except Exception as e: + rospy.logerr(" SET Request failed (python reason) : " + str(e)) + + if success: + Map.Dirty = True + + rospy.logdebug(" Responding: " + str(success)) + rospy.logdebug(" Process took {0:.2f}ms".format(time.time() * 1000 - s)) + return memory_map.srv.MapSetResponse(success) + + def on_transfer(self, req): + s = time.time() * 1000 + rospy.loginfo("TRANSFER:{} to {}".format(req.old_path, req.new_path)) + elem, elem_name = Map.get(req.old_path + "/^"), req.old_path.split('/')[-1] + if elem: + success = Map.set(req.old_path, SetMode.MODE_DELETE) and \ + Map.set(req.new_path + "/{}".format(elem_name), SetMode.MODE_ADD, instance = elem) + Map.Dirty = True + else: + rospy.logerr(" TRANSFER Request failed : could not find the object at old_path '{}'.".format(req.old_path)) + success = False + + rospy.logdebug(" Responding: " + str(success)) + rospy.logdebug(" Process took {0:.2f}ms".format(time.time() * 1000 - s)) + return memory_map.srv.MapTransferResponse(success) + + def on_get_occupancy(self, req): + s = time.time() * 1000 + rospy.loginfo("GET_OCCUPANCY:" + str(req.layer_name)) + + path = self.occupancy_generator.generateLayer(Map, req.layer_name, req.img_width, req.margin) + try: + path = self.occupancy_generator.generateLayer(Map, req.layer_name, req.img_width, req.margin) + except Exception as e: + rospy.logerr(" Request failed : " + str(e)) + + rospy.logdebug(" Responding: " + str(path)) + rospy.logdebug(" Process took {0:.2f}ms".format(time.time() * 1000 - s)) + return memory_map.srv.MapGetOccupancyResponse(path) + + def on_fill_waypoint(self, req): + s = time.time() * 1000 + + filled_waypoint, filled_waypoint_name = None, req.waypoint.name + if filled_waypoint_name is not None and filled_waypoint_name != '': # name was given, complete the rest + filled_waypoint = Map.get("/waypoints/{}/^".format(req.waypoint.name)) + else: # assume pose was given, find the waypoint name + waypoints = Map.get("/waypoints/^").Dict + for w in waypoints: + if waypoints[w].get("position/x") == round(req.waypoint.pose.x, 3) and \ + waypoints[w].get("position/y") == round(req.waypoint.pose.y, 3): + if req.waypoint.has_angle: + if waypoints[w].get("position/a") == round(req.waypoint.pose.theta, 3): + filled_waypoint = waypoints[w] + filled_waypoint_name = w + else: + filled_waypoint = waypoints[w] + filled_waypoint_name = w + if filled_waypoint is None: + rospy.logerr(" Request failed : could not find waypoint that corresponds to the given coordinates.") + + success, w = False, None + if filled_waypoint_name is not None and filled_waypoint is not None: + success = True + w = memory_map.msg.Waypoint() + w.name = filled_waypoint_name + w.frame_id = filled_waypoint.get("position/frame_id") + w.pose.x, w.pose.y = filled_waypoint.get("position/x"), filled_waypoint.get("position/y") + + w.has_angle = req.waypoint.has_angle + if req.waypoint.has_angle and "a" in filled_waypoint.Dict["position"].Dict: + w.pose.theta = filled_waypoint.get("position/a") + rospy.loginfo("FILL_WAYPOINT: {} ({}, {}{})".format(str(w.name), + w.pose.x, w.pose.y, + ", {}".format( + w.pose.theta) if w.has_angle else "")) + + rospy.logdebug(" Responding: {} ({}, {}{})".format(filled_waypoint_name, filled_waypoint.get("position/x"), filled_waypoint.get("position/y"), + ", {}".format(w.pose.theta) if w.has_angle else "")) + + rospy.logdebug(" Process took {0:.2f}ms".format(time.time() * 1000 - s)) + return memory_map.srv.FillWaypointResponse(success, w) diff --git a/ros_ws/src/memory_map/src/map_manager/__init__.py b/ros_ws/src/memory_map/src/map_manager/__init__.py new file mode 100644 index 0000000..4f83abb --- /dev/null +++ b/ros_ws/src/memory_map/src/map_manager/__init__.py @@ -0,0 +1,3 @@ +from map import Map +from map_bases import SetMode, MapElement, DictManager, RequestPath +import map_attributes diff --git a/ros_ws/src/memory_map/src/map_manager/map.py b/ros_ws/src/memory_map/src/map_manager/map.py new file mode 100644 index 0000000..af7d423 --- /dev/null +++ b/ros_ws/src/memory_map/src/map_manager/map.py @@ -0,0 +1,96 @@ +#!/usr/bin/python +import time +import rospy +from map_loader import MapLoader, LoadingHelpers +from map_bases import DictManager, RequestPath +from map_attributes import Color +from map_classes import Terrain, Zone, Waypoint, Entity, Container, Object +from map_teams import Team + +class Map(): + MAP_DICT = None + Dirty = True # Whether a service changed the dict since the last RViz Marker publish. + + # Internal global variables + Colors = [] + Teams = [] + CurrentTeam = '' + + @staticmethod + def load(): + starttime = time.time() * 1000 + initdict_config = MapLoader.loadFile("1_Config.yml")["config"] + initdict_terrain = MapLoader.loadFile("2_Terrain.yml")["terrain"] + initdict_zones = MapLoader.loadFile("3_Zones.yml")["zones"] + initdict_waypoints = MapLoader.loadFile("4_Waypoints.yml")["waypoints"] + initdict_entities = MapLoader.loadFile("5_Entities.yml")["entities"] + initdict_objects = MapLoader.loadFile("6_Objects.yml") + obj_classes = initdict_objects["classes"] + rospy.loginfo("Loaded files in {0:.2f}ms.".format(time.time() * 1000 - starttime)) + + # Setting current team to the default set one. + for team in initdict_config["teams"]: + if bool(initdict_config["teams"][team]["default"]) is True: + if Map.CurrentTeam != '': + raise ValueError("ERROR Two or more teams have been set to default.") + Map.CurrentTeam = team + Map.Teams.append(Team(team, initdict_config["teams"][team])) + + # Loading the color palette + for color in initdict_config["colors"]: + Map.Colors.append(Color(color, initdict_config["colors"][color])) + + # Instantiate objects before creating the map dict + for zone in initdict_zones: + initdict_zones[zone] = Zone(initdict_zones[zone]) + for waypoint in initdict_waypoints: + initdict_waypoints[waypoint] = Waypoint(initdict_waypoints[waypoint]) + for entity in initdict_entities: + initdict_entities[entity] = Entity(initdict_entities[entity], obj_classes) + + # Create main Map dict + Map.MAP_DICT = DictManager({ + "terrain": Terrain(initdict_terrain), + "zones": DictManager(initdict_zones), + "waypoints": DictManager(initdict_waypoints), + "entities": DictManager(initdict_entities), + "objects": Container(initdict_objects["objects"], obj_classes) + }) + rospy.loginfo("Loaded map in {0:.2f}ms.".format(time.time() * 1000 - starttime)) + + @staticmethod + def swap_team(team_name): + if team_name != Map.CurrentTeam: + for team in Map.Teams: + if team.name == team_name: + team.swap() + return + rospy.logerr("Found no team with name '{}', aborting team swap.".format(team_name)) + + @staticmethod + def is_dirty(): + dirty = Map.Dirty + Map.Dirty = False + return dirty + + @staticmethod + def get(requestpath): + if requestpath[0] != "/": + rospy.logerr(" GET Request failed : global search needs to start with '/'.") + return None + return Map.MAP_DICT.get(requestpath) + + @staticmethod + def get_objects(collisions_only = False): + return Map.MAP_DICT.Dict["objects"].get_objects(collisions_only) + + @staticmethod + def set(requestpath, mode, instance = None): + if requestpath[0] != "/": + rospy.logerr(" SET Request failed : global search needs to start with '/'.") + return None + return Map.MAP_DICT.set(requestpath, mode, instance) + + @staticmethod + def transform(codes): + return Map.MAP_DICT.transform(codes) diff --git a/ros_ws/src/memory_map/src/map_manager/map_attributes.py b/ros_ws/src/memory_map/src/map_manager/map_attributes.py new file mode 100644 index 0000000..fa20773 --- /dev/null +++ b/ros_ws/src/memory_map/src/map_manager/map_attributes.py @@ -0,0 +1,69 @@ +#!/usr/bin/python +import math +import map +from map_loader import LoadingHelpers +from map_bases import DictManager + + +class Color(DictManager): + def __init__(self, name, initdict): + if not (isinstance(initdict, list) and len(initdict) == 4): + raise ValueError("ERROR Color must be a list with 4 elements in it (RBGA).") + super(Color, self).__init__({ + "name": name, + "r": float(initdict[0]), + "g": float(initdict[1]), + "b": float(initdict[2]), + "a": float(initdict[3]) + }) + +class Position2D(DictManager): + def __init__(self, initdict): + LoadingHelpers.checkKeysExist(initdict, "frame_id", "x", "y", "type") + super(Position2D, self).__init__(initdict) + + def transform(self, codes): + if "x_mirror" in codes: + self.Dict["x"] = map.Map.get("/terrain/shape/^").Dict["width"] - self.Dict["x"] + if "a_mirror" in codes: + if "a" in self.Dict.keys(): + self.Dict["a"] = math.pi - self.Dict["a"] + return True + +class Shape2D(DictManager): + def __init__(self, initdict): + LoadingHelpers.checkKeysExist(initdict, "type") + # TODO : validate for each shape type + super(Shape2D, self).__init__(initdict) + + +class MarkerRViz(DictManager): + def __init__(self, initdict, shape = None, color = None): + LoadingHelpers.checkKeysExist(initdict, "ns", "orientation") + + # Autofill based on other info + if shape is not None: + LoadingHelpers.checkKeysExist(initdict, "z_scale") + if shape.Dict["type"] == "circle": + LoadingHelpers.checkKeysExist(initdict, "type") + initdict["scale"] = (float(shape.Dict["radius"]) * 2.0, float(shape.Dict["radius"]) * 2.0, initdict["z_scale"]) + elif shape.Dict["type"] == "rect": + initdict["type"] = "cube" + initdict["scale"] = (float(shape.Dict["width"]), float(shape.Dict["height"]), initdict["z_scale"]) + else: + raise KeyError("Marker could not be autofilled with shape '{}', not implemented.".format(shape.Dict["type"])) + else: + LoadingHelpers.checkKeysExist(initdict, "scale") + + if color is not None: + initdict["color"] = color + else: + LoadingHelpers.checkKeysExist(initdict, "color") + initdict["color"] = [c for c in map.Map.Colors if c.Dict["name"] == initdict["color"]][0] + + super(MarkerRViz, self).__init__(initdict) + + +class Trajectory(): # TODO Inherit + def __init__(self, initdict): + pass diff --git a/ros_ws/src/memory_map/src/map_manager/map_bases.py b/ros_ws/src/memory_map/src/map_manager/map_bases.py new file mode 100644 index 0000000..dfcf48c --- /dev/null +++ b/ros_ws/src/memory_map/src/map_manager/map_bases.py @@ -0,0 +1,212 @@ +#!/usr/bin/python +import rospy +import json + +class SetMode(): + MODE_ADD = 0 + MODE_REPLACE = 1 + MODE_DELETE = 2 + +class MapElement(object): + def get(self, requestpath): + raise NotImplementedError("This is the super class. Needs to be overwritten from the child class.") + def set(self, requestpath, mode): + raise NotImplementedError("This is the super class. Needs to be overwritten from the child class.") + + +# class ListManager(MapElement): # Not used for now +# def __init__(self, classdef, initdict): +# self.Elements = [] +# for k in initdict.keys(): +# self.Elements.append(classdef(initdict[k])) + + +class DictManager(MapElement): + def __init__(self, elemdict): + self.Dict = elemdict if elemdict is not None else {} + if self.Dict: + for k in self.Dict.keys(): + if isinstance(k, dict): + raise ValueError("Inner dicts as values NOT allowed. '{}' has a dict inside. Must be initialized beforehand.".format(k)) + + def toList(self): + return self.Dict.values() + + def toDict(self, hide_private = False, recursive = False): + if recursive: + d = {} + for item in self.Dict: + if isinstance(self.Dict[item], DictManager): + if (hide_private and item[0] != "_") or not hide_private: + d[item] = self.Dict[item].toDict(hide_private, recursive = True) + else: + if (hide_private and item[0] != "_") or not hide_private: + d[item] = self.Dict[item] + return d + else: + for k in self.Dict.values(): + if isinstance(k, DictManager): + rospy.logerr((" ERROR Trying to transform a DictManager that has other DictManagers inside." + " Use '*' at the end to get a recursively-constructed subdict.")) + return None + return self.Dict + + def get(self, requestpath): + if isinstance(requestpath, str): + requestpath = RequestPath(requestpath) + if not requestpath.Valid: return None + keyname = requestpath.getNextKey() + if requestpath.isLast(): + if "," in keyname: + keys = keyname.split(",") + d = {} + for k in keys: + if not isinstance(self.Dict[k], DictManager): + d[k] = self.Dict[k] + else: + rospy.logerr(" GET Request failed : Must include a '*' or '^' dict operator at the end to get a full dict json or object.") + return d + elif keyname in self.Dict.keys(): + if not isinstance(self.Dict[keyname], DictManager): + return self.Dict[keyname] + rospy.logerr(" GET Request failed : Must include a '*' or '^' dict operator at the end to get a full dict json or object.") + elif keyname == '^': + return self + # rospy.logerr(" GET Request failed : Asked to get a DictManager object with key operator '^' but '{}' points to a '{}' object.".format(keyname, type(self.Dict[keyname]))) + elif keyname == '*': + return self.toDict(hide_private = True, recursive = True) + else: + rospy.logerr(" GET Request failed : Unrecognized last key dict operator '{}'.".format(keyname)) + else: + if keyname in self.Dict.keys(): + if isinstance(self.Dict[keyname], DictManager): + return self.Dict[keyname].get(requestpath) + else: + return self.Dict[keyname] + elif keyname == "*": + d = {} + current_level = requestpath.Counter + for item in self.Dict: + if isinstance(self.Dict[item], DictManager): + d[item] = self.Dict[item].get(requestpath) + requestpath.Counter = current_level + else: + d[item] = self.Dict[item] + return d + else: + rospy.logerr(" GET Request failed : Couldn't recognize key or find element '{}'.".format(keyname)) + + def set(self, requestpath, mode, instance = None): + if isinstance(requestpath, str): + requestpath = RequestPath(requestpath) + if not requestpath.Valid: return None + keyname = requestpath.getNextKey() + + if requestpath.isLast(): + if mode == SetMode.MODE_ADD: + return self._set_add(keyname, instance) + elif mode == SetMode.MODE_REPLACE: + return self._set_replace(keyname) + elif mode == SetMode.MODE_DELETE: + return self._set_remove(keyname) + else: + if '=' in keyname: + rospy.logerr(" Request failed : '=' assign operator must only be applied on the last path key.") + return False + if keyname in self.Dict: + return self.Dict[keyname].set(requestpath, mode, instance) + + def _set_add(self, lastkey, instance = None): + # e.g. /objects/cube_42:MapObject={"type": "object", "etc": True} + if instance is None: + request, new_dict = lastkey.split('=', 1) # split at first '='. + keyname, class_type = request.split(':') + new_dict = json.loads(new_dict) + + if keyname in self.Dict: + rospy.logerr(" ADD Request failed : key '{}' already exists in dict.".format(keyname)) + return False + + sc = DictManager.__subclasses__() + for c in sc: + if class_type == c.__name__: + self.Dict[keyname] = DictManager(new_dict) + return True + rospy.logerr(" ADD Request failed : class type '{}' not recognized.".format(class_type)) + return False + else: # add a DictManager (intern only, e.g. coming from MapTransfer). + self.Dict[lastkey] = instance + return True + + + def _set_replace(self, lastkey): + # e.g. /objects/cube_14/position/x=0.42,y=0.84,a=3.14 + assignments = [] + for s in lastkey.split(','): + if s.count("=") != 1: + rospy.logerr(" SET Request failed : invalid key part '{}', must have one '=' operator.".format(s)) + return False + key, new_value = s.split("=") + if not key in self.Dict: + rospy.logerr(" SET Request failed : key '{}' does not already exist in dict.".format(key)) + return False + if not isinstance(self.Dict[key], DictManager): + try: + new_value = type(self.Dict[key])(new_value) + assignments.append((key, new_value)) + except (TypeError, ValueError): + rospy.logerr(" SET Request failed : new value '{}' not castable to the old value '{}''s type.".format(new_value, self.Dict[key])) + return False + except KeyError: + rospy.logerr(" SET Request failed : Couldn't find existing key '{}'.".format(key)) + return False + else: + raise ValueError(" SET Request failed : Can't SET a whole DictManager to a new value.") + + for assignment in assignments: + self.Dict[assignment[0]] = assignment[1] + return True + + def _set_remove(self, lastkey): + # e.g. /objects/cube_12 + if not lastkey in self.Dict: + rospy.logerr(" DEL Request failed : key '{}' does not already exist in dict.".format(lastkey)) + return False + del self.Dict[lastkey] + return True + + def transform(self, codes): + success = True + for elem in self.Dict: + if isinstance(self.Dict[elem], DictManager): + success = min(success, self.Dict[elem].transform(codes)) + return success + + + +class RequestPath(): + def __init__(self, pathstring): + self.pathstring = pathstring + + self.Valid = True + if not len(pathstring): + rospy.logerr("Invalid path : empty path.") + self.Valid = False + if pathstring[-1] == '/': + rospy.logerr("Invalid path : must not end with '/'.") + self.Valid = False + self.Keys = [p for p in pathstring.split('/') if p != ''] + self.Counter = -1 + + def getNextKey(self): + if self.Counter < len(self.Keys) - 1: + self.Counter += 1 + return self.Keys[self.Counter] + else: + raise ValueError("ERROR Not enough levels in path ! Can't reach farther than the last path key.") + + def isLast(self): + return self.Counter == len(self.Keys) - 1 + + def __repr__(self): + return self.pathstring diff --git a/ros_ws/src/memory_map/src/map_manager/map_classes.py b/ros_ws/src/memory_map/src/map_manager/map_classes.py new file mode 100644 index 0000000..ec22d43 --- /dev/null +++ b/ros_ws/src/memory_map/src/map_manager/map_classes.py @@ -0,0 +1,134 @@ +#!/usr/bin/python +import copy, json +import rospy +from map_loader import LoadingHelpers +from map_bases import DictManager +from map_attributes import Position2D, Shape2D, MarkerRViz, Trajectory +import map + + +class Terrain(DictManager): + def __init__(self, initdict): + LoadingHelpers.checkKeysExist(initdict, "shape", "_marker", "walls") + + # Instantiate the layers before creating the dict + for layer in initdict["walls"]: + initdict["walls"][layer] = Layer(initdict["walls"][layer]) + + super(Terrain, self).__init__({ + "shape": Shape2D(initdict["shape"]), + "_marker": MarkerRViz(initdict["_marker"]), + "walls": DictManager(initdict["walls"]), + }) + + +class Layer(DictManager): + def __init__(self, initdict): + self.includes = [] + if "_include" in initdict.keys(): + self.includes = [i for i in initdict["_include"]] + del initdict["_include"] + + # Instantiate the walls before creating the dict + for wall in initdict: + initdict[wall] = Wall(initdict[wall]) + + super(Layer, self).__init__(initdict) + + +class Wall(DictManager): + def __init__(self, initdict): + LoadingHelpers.checkKeysExist(initdict, "position", "shape") + super(Wall, self).__init__({ + "position": Position2D(initdict["position"]), + "shape": Shape2D(initdict["shape"]) + }) + + +class Zone(DictManager): + def __init__(self, initdict): + LoadingHelpers.checkKeysExist(initdict, "position", "shape", "_marker", "properties") + super(Zone, self).__init__({ + "position": Position2D(initdict["position"]), + "shape": Shape2D(initdict["shape"]), + "_marker": MarkerRViz(initdict["_marker"], shape = Shape2D(initdict["shape"])), + "properties": DictManager(initdict["properties"]) + }) + + +class Waypoint(DictManager): + def __init__(self, initdict): + LoadingHelpers.checkKeysExist(initdict, "position") + super(Waypoint, self).__init__({ + "position": Position2D(initdict["position"]) + }) + + +class Entity(DictManager): + def __init__(self, initdict, obj_classes): + LoadingHelpers.checkKeysExist(initdict, "position", "shape", "_marker", "containers", "trajectory") + + for container in initdict["containers"]: + initdict["containers"][container] = Container(initdict["containers"][container], obj_classes) + + super(Entity, self).__init__({ + "position": Position2D(initdict["position"]), + "shape": Shape2D(initdict["shape"]), + "_marker": MarkerRViz(initdict["_marker"]), + "chest": DictManager(initdict["containers"]), + "trajectory": Trajectory(initdict["trajectory"]) + }) + + +class Container(DictManager): + def __init__(self, initdict, obj_classes): + for obj in initdict: + if "container_" in obj: + initdict[obj] = Container(initdict[obj], obj_classes) + else: + initdict[obj] = Object(initdict[obj], obj_classes) + super(Container, self).__init__(initdict) + + def get_objects(self, collisions_only = False): + objects = [] + for o in self.Dict: + if isinstance(self.Dict[o], Container): + objects += self.Dict[o].get_objects(collisions_only) + elif isinstance(self.Dict[o], Object): + if self.Dict[o].Dict["collision"] == collisions_only or collisions_only is False: + objects.append(json.dumps(self.Dict[o].get("*"))) + else: + rospy.logwarn("Not recognized DictManager type found while retrieving map objects, passing.") + return objects + + +class Object(DictManager): + def __init__(self, initdict, obj_classes): + # Autofilling if class is available + if "class" in initdict: + obj_class = copy.deepcopy([obj_classes[d] for d in obj_classes if d == initdict["class"]][0]) + new_initdict = LoadingHelpers.mergeDicts(obj_class, initdict) + for field in ["position", "shape", "_marker"]: + if field in initdict and field in obj_class: + new_initdict[field] = LoadingHelpers.mergeDicts(obj_class[field], initdict[field]) + elif field in initdict: + new_initdict[field] = initdict[field] + elif field in obj_class: + new_initdict[field] = obj_class[field] + initdict = new_initdict + + LoadingHelpers.checkKeysExist(initdict, "collision", "position", "shape", "_marker") + + d = {} + if "collision" in initdict: + d["collision"] = initdict["collision"] + if "color" in initdict: + d["color"] = [c for c in map.Map.Colors if c.Dict["name"] == initdict["color"]][0] + if "properties" in initdict: + d["properties"] = DictManager(initdict["properties"]) + d["shape"] = Shape2D(initdict["shape"]) + + d["position"] = Position2D(initdict["position"]) + d["_marker"] = MarkerRViz(initdict["_marker"], shape = d["shape"] if "shape" in d else None, \ + color = d["color"] if "color" in d else None) + super(Object, self).__init__(d) diff --git a/ros_ws/src/memory_map/src/map_manager/map_loader.py b/ros_ws/src/memory_map/src/map_manager/map_loader.py new file mode 100644 index 0000000..b8b87d6 --- /dev/null +++ b/ros_ws/src/memory_map/src/map_manager/map_loader.py @@ -0,0 +1,89 @@ +#!/usr/bin/python +import os +import yaml +import rospy + +from memory_definitions.srv import GetDefinition + +class LoadingHelpers(): + ''' + Helpers static class. Provides validation methods for easier and + cleaner YML verification handling inside the classes. + ''' + + @staticmethod + def checkKeysExist(checkdict, *keys_required): + ''' + Checks if the dict given has all of the keys given in keys_required. + Pops a ROS error and stops the node if the condition is not verified. + ''' + for k in keys_required: + if k not in checkdict.keys(): + m = "Missing required '{}' element in Map YML description file. Couldn't load Map.".format(k) + rospy.logerr(m) + raise ValueError(m) + + @staticmethod + def checkValueValid(value, *values_required): + ''' + Checks if the the value given corresponds to one of the possibilities given in values_required. + Pops a ROS error and stops the node if the condition is not verified. + ''' + if value not in values_required: + m = "Element value '{}' not valid, must be in '{}'. Couldn't load Map.".format(value, values_required) + rospy.logerr(m) + raise ValueError(m) + + @staticmethod + def mergeDicts(a, b): # if a key exists on both dicts, will take b's value. + c = a.copy() # start with x's keys and values + c.update(b) # modifies z with y's keys and values & returns None + return c + + +class MapLoader(): + @staticmethod + def loadFile(filename): + ''' + Gets the YML Map description file from the specified method + Please change the method correspondingly to what is currently used in your package. + ''' + return MapLoader.loafYamlFromDescriptionModule(filename) + + @staticmethod + def loadYamlFromFile(filename): # DEPRECATED + ''' + Loads the description file simply by getting the file in disk. + The file MUST be in the package's directory to avoid any problems. + ''' + return MapLoader._json_from_file(os.path.dirname(__file__) + "/../../def" + filename) + + @staticmethod + def loafYamlFromDescriptionModule(filename): + ''' + Loads the description file by gettign it from the 'memory/definitions' ROS package. + ''' + get_def = rospy.ServiceProxy('/memory/definitions/get', GetDefinition) + try: + get_def.wait_for_service(timeout = 2) + except: + rospy.logerr("FATAL Could not contact definitions service, timeout reached. Aborting.") + raise Exception() + + try: + res = get_def('map/' + filename) + if not res.success: + rospy.logerr("Error when fetching '{}' definition file".format(filename)) + raise Exception() + return MapLoader._json_from_file(res.path) + except rospy.ServiceException as exc: + rospy.logerr("Unhandled error while getting def file: {}".format(str(exc))) + raise Exception() + + @staticmethod + def _json_from_file(path): + with open(path, 'r') as stream: + try: + return yaml.load(stream) + except yaml.YAMLError as exc: + rospy.logerr("Could not load map YML file : " + str(exc)) diff --git a/ros_ws/src/memory_map/src/map_manager/map_teams.py b/ros_ws/src/memory_map/src/map_manager/map_teams.py new file mode 100644 index 0000000..b8e52d8 --- /dev/null +++ b/ros_ws/src/memory_map/src/map_manager/map_teams.py @@ -0,0 +1,22 @@ +import rospy +from map_bases import DictManager +import map + + +class Team(object): + def __init__(self, name, initdict): + self.name = name + self.default = bool(initdict["default"]) + + self.transforms = [] + if "transforms" in initdict: + self.transforms = [t for t in initdict["transforms"]] + + def swap(self): + success = map.Map.transform(self.transforms) + + if success: + rospy.loginfo("Swapped map to team '{}' successfully.".format(self.name)) + map.Map.CurrentTeam = self.name + else: rospy.logerr("Couldn't swap map to team '{}'.".format(self.name)) + return success diff --git a/ros_ws/src/memory_map/src/map_node.py b/ros_ws/src/memory_map/src/map_node.py new file mode 100755 index 0000000..eb5211a --- /dev/null +++ b/ros_ws/src/memory_map/src/map_node.py @@ -0,0 +1,42 @@ +#!/usr/bin/python +import rospy +import map_manager +import map_communication +from markers import MarkersPublisher +from occupancy import OccupancyGenerator +from ai_game_manager import StatusServices + + +class MapNode(): + def __init__(self): + rospy.init_node("map", log_level=rospy.INFO) + rospy.logdebug("Started /memory/map node.") + + map_manager.Map.load() + + # Starting and publishing the table STL to RViz + self.markers = MarkersPublisher() + + # Generate static occupancy images for pathfinder, etc. + occupancy = OccupancyGenerator() + + # Starting service handlers (Get, Set, Transfer, GetOccupancy) + map_communication.MapServices(occupancy) + rospy.logdebug("[memory/map] Map request servers ready.") + + # Tell ai/game_manager the node initialized successfuly. + StatusServices("memory", "map").ready(True) + + self.run() + + def run(self): + r = rospy.Rate(5) + while not rospy.is_shutdown(): + if rospy.has_param("/current_team"): + map_manager.Map.swap_team(rospy.get_param("/current_team")) + self.markers.updateMarkers(map_manager.Map) + r.sleep() + + +if __name__ == "__main__": + MapNode() diff --git a/ros_ws/src/memory_map/src/markers/__init__.py b/ros_ws/src/memory_map/src/markers/__init__.py new file mode 100644 index 0000000..8e41fde --- /dev/null +++ b/ros_ws/src/memory_map/src/markers/__init__.py @@ -0,0 +1 @@ +from markers_publisher import MarkersPublisher diff --git a/ros_ws/src/memory_map/src/markers/markers_publisher.py b/ros_ws/src/memory_map/src/markers/markers_publisher.py new file mode 100644 index 0000000..f2c004c --- /dev/null +++ b/ros_ws/src/memory_map/src/markers/markers_publisher.py @@ -0,0 +1,135 @@ +#!/usr/bin/python +import math, time +import rospy +from map_manager import map_attributes, DictManager +from visualization_msgs.msg import Marker + + +class MarkersPublisher(): + def __init__(self): + self._prev_num_connections = 0 + self.MARKERS_TOPIC = "/visualization_markers/world" + self._pub = rospy.Publisher(self.MARKERS_TOPIC, Marker, queue_size=10) + + def _is_connected(self): + return bool(self._pub.get_num_connections()) + + def _connections_has_changed(self): + if self._prev_num_connections != self._pub.get_num_connections(): + time.sleep(0.3) + return True + else: + return False + + def updateMarkers(self, world): + if (self._is_connected() and self._connections_has_changed()) or world.is_dirty(): # Draw if new connection or new content + self._publish_table(world) # Table STL without colors + self._publish_robot_stl(world) # Simple rectangle representing the robot shape. + self._publish_zones(world) # Departure areas + self._publish_waypoints(world) # Navigation positions + self._publish_objects(world.get("/objects/^")) # game elements (balls, cubes...) + + self._prev_num_connections = self._pub.get_num_connections() + + def _publish_table(self, world): + pos = map_attributes.Position2D({ + "frame_id": "/map", + "x": -0.022, + "y": 0.022 + 2, + "type": "fixed" + }) + self._publish_marker(0, pos, world.get("/terrain/_marker/^")) + + def _publish_robot_stl(self, world): + pos = map_attributes.Position2D({ + "frame_id": "/robot", + "x": 0, + "y": 0, + "type": "fixed" + }) + self._publish_marker(0, pos, world.get("/entities/{}/_marker/^".format(rospy.get_param("/robot")))) + + def _publish_zones(self, world): + for i, z in enumerate(world.get("/zones/^").toList()): + self._publish_marker(i, z.get("position/^"), z.get("_marker/^")) + + def _publish_waypoints(self, world): + i = 0 + for z in world.get("/waypoints/^").toList(): + m = DictManager({ + "ns": "waypoints", + "type": "arrow", + "scale": [0.08, 0.03, 0.03], + "z": 0.08, + "orientation": [0, 1.57079, 0], + "color": map_attributes.Color("brown", [0.8, 0.5, 0.1, 0.95]) + }) + self._publish_marker(i, z.get("position/^"), m) + if "a" in z.get("position/^").Dict.keys(): + i += 1 + n = DictManager({ + "ns": "waypoints", + "type": "arrow", + "scale": [0.06, 0.015, 0.015], + "z": 0, + "orientation": [0, 0, z.get("position/^").Dict["a"]], + "color": map_attributes.Color("brown", [0.8, 0.5, 0.1, 0.95]) + }) + self._publish_marker(i, z.get("position/^"), n) + i += 1 + + def _publish_objects(self, objects_dictman, j = 0): # TODO containers inside containers + i = 0 + for e in objects_dictman.Dict.keys(): + time.sleep(0.005) + if "container_" in e: + i += self._publish_objects(objects_dictman.get("{}/^".format(e)), i) + else: + self._publish_marker(i + j, objects_dictman.Dict[e].get("position/^"), objects_dictman.Dict[e].get("_marker/^")) + i += 1 + return i + + def _publish_marker(self, marker_id, position, visual): + markertypes = { + "cube": Marker.CUBE, + "sphere": Marker.SPHERE, + "arrow": Marker.ARROW, + "mesh": Marker.MESH_RESOURCE + } + + marker = Marker() + marker.header.frame_id = position.get("frame_id") + marker.type = markertypes[visual.get("type")] + marker.ns = visual.get("ns") + marker.id = marker_id + marker.frame_locked = True + + marker.action = Marker.ADD + marker.scale.x = visual.get("scale")[0] + marker.scale.y = visual.get("scale")[1] + marker.scale.z = visual.get("scale")[2] + marker.color.r = visual.Dict["color"].Dict["r"] + marker.color.g = visual.get("color/^").get("g") + marker.color.b = visual.get("color/^").get("b") + marker.color.a = visual.get("color/^").get("a") + marker.pose.position.x = position.get("x") + marker.pose.position.y = position.get("y") + marker.pose.position.z = visual.get("z") + orientation = self._euler_to_quaternion(visual.get("orientation")) + marker.pose.orientation.x = orientation[0] + marker.pose.orientation.y = orientation[1] + marker.pose.orientation.z = orientation[2] + marker.pose.orientation.w = orientation[3] + + marker.mesh_resource = visual.get("mesh_path") if marker.type == Marker.MESH_RESOURCE else '' + + self._pub.publish(marker) + + def _euler_to_quaternion(self, xyz): + cr, sr = math.cos(xyz[0] * 0.5), math.sin(xyz[0] * 0.5) + cp, sp = math.cos(xyz[1] * 0.5), math.sin(xyz[1] * 0.5) + cy, sy = math.cos(xyz[2] * 0.5), math.sin(xyz[2] * 0.5) + return (cy * sr * cp - sy * cr * sp, # qx + cy * cr * sp + sy * sr * cp, # qy + sy * cr * cp - cy * sr * sp, # qz + cy * cr * cp + sy * sr * sp) # qw diff --git a/ros_ws/src/memory_map/src/occupancy/__init__.py b/ros_ws/src/memory_map/src/occupancy/__init__.py new file mode 100644 index 0000000..90a4bd9 --- /dev/null +++ b/ros_ws/src/memory_map/src/occupancy/__init__.py @@ -0,0 +1,2 @@ +#!/usr/bin/python +from generator import OccupancyGenerator \ No newline at end of file diff --git a/ros_ws/src/memory_map/src/occupancy/generator.py b/ros_ws/src/memory_map/src/occupancy/generator.py new file mode 100644 index 0000000..b3449f2 --- /dev/null +++ b/ros_ws/src/memory_map/src/occupancy/generator.py @@ -0,0 +1,58 @@ +#!/usr/bin/python +import rospkg +import time +import rospy +from PIL import Image, ImageDraw + +class OccupancyGenerator(): + def generateLayer(self, world, layer_name, img_width = 0, margin = 0.0): + def generateStaticOccupancy(layers, layer_name, img_size, world_size, margin = 0.0): # margin in m. + img = Image.new("RGB", img_size, (255, 255, 255)) + + draw = ImageDraw.Draw(img) + + walls = layers.Dict[layer_name].toList() + for i in layers.Dict[layer_name].includes: + if i in layers.Dict.keys(): + walls += layers.Dict[i].toList() + else: + rospy.logerr("Couldn't find layer to include named '{}', skipping.".format(i)) + + for wall in walls: + position, shape = wall.get("position/^").toDict(), wall.get("shape/^").toDict() + + pos = world_to_img_pos(img_size, world_size, (position["x"], position["y"])) + if shape["type"] == "rect": + w = world_to_img_scale(img_size, world_size, shape["width"] + margin * 2) + h = world_to_img_scale(img_size, world_size, shape["height"] + margin * 2) + draw.rectangle((pos[0] - w/2.0, pos[1] - h/2.0, pos[0] + w/2.0, pos[1] + h/2.0), fill=(0, 0, 0)) + elif shape["type"] == "circle": + r = world_to_img_scale(img_size, world_size, shape["radius"] + margin) + draw.ellipse((pos[0] - r, pos[1] - r, pos[0] + r, pos[1] + r), fill = (0, 0, 0)) + elif shape["type"] == "polygon": + rospy.logwarn("Occupancy generator: polygon drawing not implemented") + elif shape["type"] == "line": + rospy.logwarn("Occupancy generator: line drawing not implemented") + else: + rospy.logerr("Occupancy generator could not recognize shape type '{}'.".format(shape["type"])) + del draw + return img + + def world_to_img_scale(img_size, world_size, world_coord): + return world_coord * (img_size[0] / (world_size[0])) + def world_to_img_pos(img_size, world_size, world_pos): + return (world_to_img_scale(img_size, world_size, world_pos[0]), img_size[1] - world_to_img_scale(img_size, world_size, world_pos[1])) + + img_width = img_width if img_width != 0 else 500 # Default value + world_size = (float(world.get("/terrain/shape/width")), float(world.get("/terrain/shape/height"))) + img_size = (img_width, int(img_width * (world_size[1] / world_size[0]))) + + final_path = rospkg.RosPack().get_path("memory_map") + "/def/occupancy/" + layer_name + ".bmp" + layers = world.get("/terrain/walls/^") + if not layer_name in layers.Dict.keys(): + rospy.logerr(" Couldn't find layer '{}'. Aborting.".format(layer_name)) + return None + + img = generateStaticOccupancy(layers, layer_name, img_size, world_size, margin) + img.save(final_path) + return final_path diff --git a/ros_ws/src/memory_map/src_alt/map_communication.py b/ros_ws/src/memory_map/src_alt/map_communication.py new file mode 100644 index 0000000..30d6d1b --- /dev/null +++ b/ros_ws/src/memory_map/src_alt/map_communication.py @@ -0,0 +1,126 @@ +#!/usr/bin/python +import json +import time + +import rospy +import memory_map.msg +import memory_map.srv +from map_manager import SetMode, Map, DictManager +from occupancy import OccupancyGenerator + + +class Servers(): + GET_SERV = "/memory/map/get" + SET_SERV = "/memory/map/set" + TRANSFER_SERV = "/memory/map/transfer" + OCCUPANCY_SERV = "/memory/map/get_occupancy" + FILLWP_SERV = "/memory/map/fill_waypoint" + +class MapServices(): + def __init__(self, occupancy_generator): + self.GetSERV = rospy.Service(Servers.GET_SERV, memory_map.srv.MapGet, self.on_get) + self.SetSERV = rospy.Service(Servers.SET_SERV, memory_map.srv.MapSet, self.on_set) + self.TransferSERV = rospy.Service(Servers.TRANSFER_SERV, memory_map.srv.MapTransfer, self.on_transfer) + self.OccupancySERV = rospy.Service(Servers.OCCUPANCY_SERV, memory_map.srv.MapGetOccupancy, self.on_get_occupancy) + self.FillWPSERV = rospy.Service(Servers.FILLWP_SERV, memory_map.srv.FillWaypoint, self.on_fill_waypoint) + self.occupancy_generator = occupancy_generator + + def on_get(self, req): + s = time.time() * 1000 + rospy.loginfo("GET:" + str(req.request_path)) + + success = False + response = Map.get(req.request_path) + if isinstance(response, DictManager): + rospy.logerr(" GET Request failed : '^' dict operator not allowed in services.") + response = None + + if response != None: + success = True + + rospy.logdebug(" Responding: " + str(response)) + rospy.logdebug(" Process took {0:.2f}ms".format(time.time() * 1000 - s)) + return memory_map.srv.MapGetResponse(success, json.dumps(response)) + + def on_set(self, req): + s = time.time() * 1000 + rospy.loginfo("SET:" + str(req.request_path)) + + success = False + success = Map.set(req.request_path, req.mode) + try: + success = Map.set(req.request_path, req.mode) + except Exception as e: + rospy.logerr(" SET Request failed (python reason) : " + str(e)) + + rospy.logdebug(" Responding: " + str(success)) + rospy.logdebug(" Process took {0:.2f}ms".format(time.time() * 1000 - s)) + return memory_map.srv.MapSetResponse(success) + + def on_transfer(self, req): + s = time.time() * 1000 + rospy.loginfo("TRANSFER:{} to {}".format(req.old_path, req.new_path)) + elem, elem_name = Map.get(req.old_path + "/^"), req.old_path.split('/')[-1] + if elem: + success = Map.set(req.old_path, SetMode.MODE_DELETE) and \ + Map.set(req.new_path + "/{}".format(elem_name), SetMode.MODE_ADD, instance = elem) + else: + rospy.logerr(" TRANSFER Request failed : could not find the object at old_path '{}'.".format(req.old_path)) + success = False + + rospy.logdebug(" Responding: " + str(success)) + rospy.logdebug(" Process took {0:.2f}ms".format(time.time() * 1000 - s)) + return memory_map.srv.MapTransferResponse(success) + + def on_get_occupancy(self, req): + s = time.time() * 1000 + rospy.loginfo("GET_OCCUPANCY:" + str(req.layer_name)) + + try: + path = self.occupancy_generator.generateLayer(Map, req.layer_name, req.margin) + except Exception as e: + rospy.logerr(" Request failed : " + str(e)) + + rospy.logdebug(" Responding: " + str(path)) + rospy.logdebug(" Process took {0:.2f}ms".format(time.time() * 1000 - s)) + return memory_map.srv.MapGetOccupancyResponse(path) + + def on_fill_waypoint(self, req): + s = time.time() * 1000 + rospy.loginfo("FILL_WAYPOINT: {} ({}, {}{})".format(str(req.waypoint.name if req.waypoint.name else ""), + req.waypoint.pose.x, req.waypoint.pose.y, + ", {}".format(req.waypoint.pose.theta) if req.waypoint.has_angle else "")) + + filled_waypoint, filled_waypoint_name = None, req.waypoint.name + if filled_waypoint_name is not None and filled_waypoint_name != '': # name was given, complete the rest + filled_waypoint = Map.get("/waypoints/{}/^".format(req.waypoint.name)) + else: # assume pose was given, find the waypoint name + waypoints = Map.get("/waypoints/^").Dict + for w in waypoints: + if waypoints[w].get("position/x") == round(req.waypoint.pose.x, 3) and \ + waypoints[w].get("position/y") == round(req.waypoint.pose.y, 3): + if req.waypoint.has_angle: + print waypoints[w].get("position/a") + print round(req.waypoint.pose.theta, 3) + if waypoints[w].get("position/a") == round(req.waypoint.pose.theta, 3): + filled_waypoint = waypoints[w] + filled_waypoint_name = w + else: + filled_waypoint = waypoints[w] + filled_waypoint_name = w + if filled_waypoint is None: + rospy.logerr(" Request failed : could not find waypoint that corresponds to the given coordinates.") + + success, w = False, None + if filled_waypoint_name is not None and filled_waypoint is not None: + success = True + w = memory_map.msg.Waypoint() + w.name = filled_waypoint_name + w.frame_id = filled_waypoint.get("position/frame_id") + w.pose.x, w.pose.y = filled_waypoint.get("position/x"), filled_waypoint.get("position/y") + if req.waypoint.has_angle and "a" in filled_waypoint.Dict: + w.pose.theta = filled_waypoint.get("position/a") + rospy.logdebug(" Responding: {} ({}, {})".format(filled_waypoint_name, filled_waypoint.get("position/x"), filled_waypoint.get("position/y"))) + + rospy.logdebug(" Process took {0:.2f}ms".format(time.time() * 1000 - s)) + return memory_map.srv.FillWaypointResponse(success, w) diff --git a/ros_ws/src/memory_map/src_alt/map_manager/__init__.py b/ros_ws/src/memory_map/src_alt/map_manager/__init__.py new file mode 100644 index 0000000..b763e6f --- /dev/null +++ b/ros_ws/src/memory_map/src_alt/map_manager/__init__.py @@ -0,0 +1 @@ +from map import Map diff --git a/ros_ws/src/memory_map/src_alt/map_manager/bases.py b/ros_ws/src/memory_map/src_alt/map_manager/bases.py new file mode 100644 index 0000000..3eae6ef --- /dev/null +++ b/ros_ws/src/memory_map/src_alt/map_manager/bases.py @@ -0,0 +1,5 @@ +class Vector3(object): + def __init__(self, x, y, z): + self.x = float(x) if x is not None else None + self.y = float(y) if y is not None else None + self.z = float(z) if z is not None else None diff --git a/ros_ws/src/memory_map/src_alt/map_manager/map.py b/ros_ws/src/memory_map/src_alt/map_manager/map.py new file mode 100644 index 0000000..ee9b41f --- /dev/null +++ b/ros_ws/src/memory_map/src_alt/map_manager/map.py @@ -0,0 +1,48 @@ +from map_loader import MapLoader, LoadChecker +from map_config import Config +from robot import Robot +from map_feature import Feature +from map_classes import Object, Container, Zone + +class Map(object): + CONFIG = Config() + MAP_FEATURE = None + + ROBOTS = [] + WAYPOINTS = [] + ZONES = [] + OBJECTS = [] + + @staticmethod + def load(robot_name, team_name): + config_xml = MapLoader.loadFile(MapLoader.CONFIG_FILE) + robots_xml = MapLoader.loadFile(MapLoader.ROBOTS_FILE) + classes_xml = MapLoader.loadFile(MapLoader.CLASSES_FILE) + objects_xml = MapLoader.loadFile(MapLoader.OBJECTS_FILE) + + Map.CONFIG.load(config_xml) + LoadChecker.checkNodesExist(config_xml, "map") + Map.MAP_FEATURE = Feature(config_xml.find("map").find("feature")) + + for robot in robots_xml.findall("robot"): + r = Robot(robot) + if r.name == robot_name: r.active = True + Map.ROBOTS.append(r) + + Map._load_team_objects(classes_xml, objects_xml, team_name) + + @staticmethod + def _load_team_objects(classes_xml, objects_xml, team_name): + team = [team for team in objects_xml.findall("team") if team.attrib["name"] == team_name][0] + for z in classes_xml.find("zones").findall("zone"): + Map.ZONES.append(Zone(z)) + + classes = [] + for c in classes_xml.find("objects").findall("object"): + classes.append(Object(c, [])) + + for o in team.findall("object"): + Map.OBJECTS.append(Object(o, classes)) + + for c in team.findall("container"): + Map.OBJECTS.append(Container(c, classes)) diff --git a/ros_ws/src/memory_map/src_alt/map_manager/map_classes.py b/ros_ws/src/memory_map/src_alt/map_manager/map_classes.py new file mode 100644 index 0000000..aca6f3c --- /dev/null +++ b/ros_ws/src/memory_map/src_alt/map_manager/map_classes.py @@ -0,0 +1,54 @@ +from map_feature import Feature +from map_loader import LoadChecker + + +class Waypoint(object): + def __init__(self, xml): + self.feature = Feature(xml) + + +class Zone(object): + def __init__(self, xml): + self.feature = Feature(xml) + + +class Object(object): + def __init__(self, xml, classes): + self.name = xml.attrib["name"] if "name" in xml.attrib else xml.attrib["class"] + self.features = [] + self.waypoints = [] + + # Pre-loading class info if available + if "class" in xml.attrib: + base = [c for c in classes if c.name == xml.attrib["class"]] + if len(base) == 1: + self.features = base[0].features + self.waypoints = base[0].waypoints + + # Loading features list + if xml.find("features") is not None: + for f in xml.find("features").findall("feature"): + self.features.append(Feature(f)) + + # Loading waypoints list + if xml.find("waypoints") is not None: + for w in xml.find("waypoints").findall("waypoint"): + self.waypoints.append(Waypoint(w)) + + +class Container(Object): + def __init__(self, xml, classes): + super(Container, self).__init__(xml, classes) + + # Loading elements list + LoadChecker.checkNodesExist(xml, "elements") + LoadChecker.checkAttribsExist(xml.find("elements"), "min", "max") + self.min = xml.find("elements").attrib["min"] + self.max = xml.find("elements").attrib["max"] + + self.elements = [] + for o in xml.find("elements").findall("object"): + self.elements.append(Object(o, classes)) + + for c in xml.find("elements").findall("container"): + self.elements.append(Container(c, classes)) diff --git a/ros_ws/src/memory_map/src_alt/map_manager/map_config.py b/ros_ws/src/memory_map/src_alt/map_manager/map_config.py new file mode 100644 index 0000000..1f7d5dd --- /dev/null +++ b/ros_ws/src/memory_map/src_alt/map_manager/map_config.py @@ -0,0 +1,41 @@ +from map_loader import LoadChecker +from map_feature import Feature + + +class Layer(object): + def __init__(self, xml): + LoadChecker.checkAttribsExist(xml, "name") + self.name = xml.attrib["name"] + + self.includes = [] + for include in xml.findall("include"): + self.includes.append(include.attrib["name"]) + + +class Color(object): + def __init__(self, xml): + LoadChecker.checkAttribsExist(xml, "name", "r", "g", "b") + self.name = xml.attrib["name"] + self.r = xml.attrib["r"] + self.g = xml.attrib["g"] + self.b = xml.attrib["b"] + self.a = xml.attrib["a"] if "a" in xml.attrib else 1.0 + + +class Config(object): + CURRENT_TEAM = None + DEFAULT_FRAME_ID = "map" + + LAYERS = [] + COLORS = [] + MARKERS = [] + + @staticmethod + def load(xml): + LoadChecker.checkNodesExist(xml, "map", "layers", "colors") + + for layer in xml.find("layers").findall("layer"): + Config.LAYERS.append(Layer(layer)) + + for color in xml.find("colors").findall("color"): + Config.COLORS.append(Color(color)) diff --git a/ros_ws/src/memory_map/src_alt/map_manager/map_feature.py b/ros_ws/src/memory_map/src_alt/map_manager/map_feature.py new file mode 100644 index 0000000..b1981ca --- /dev/null +++ b/ros_ws/src/memory_map/src_alt/map_manager/map_feature.py @@ -0,0 +1,52 @@ +from map_loader import LoadChecker +from bases import Vector3 +import shapes + + +class Position(object): + def __init__(self, xml): + self.x = float(xml.attrib["x"]) + self.y = float(xml.attrib["y"]) + self.a = float(xml.attrib["a"]) if "a" in xml.attrib else None + + +class Marker(object): + def __init__(self, xml, super_position=None, super_shape=None): + self.type = xml.find("type").text if xml.find("type") is not None else None + self.ns = xml.find("ns").text if xml.find("ns") is not None else None + self.scale = Vector3(None, None, None) + self.position = Vector3(None, None, None) + self.orientation = Vector3(None, None, None) + self.color = None + self._auto_complete(super_position, super_shape) + + def _auto_complete(self, position = None, shape = None): + if position is not None: + self.position.x = position.x + self.position.y = position.y + if position.a is not None: + self.orientation.z = position.a + if shape is not None: + self.scale.x = shape.get_scale().x + self.scale.y = shape.get_scale().y + + +class Feature(object): + def __init__(self, xml): + self.layer = None + self.position = None + self.shape = None + self.marker = None + self.auto_complete(xml) + + def auto_complete(self, xml): + self.layer = xml.attrib["layer"] if "layer" in xml.attrib else None + + pos = xml.find("position") + self.position = Position(pos) if pos is not None else None + + shape = xml.find("shape") + self.shape = shapes.CreateShape(shape) if shape is not None else None + + marker = xml.find("marker") + self.marker = Marker(marker, self.position, self.shape) if marker is not None else None diff --git a/ros_ws/src/memory_map/src_alt/map_manager/map_loader.py b/ros_ws/src/memory_map/src_alt/map_manager/map_loader.py new file mode 100644 index 0000000..1f4380b --- /dev/null +++ b/ros_ws/src/memory_map/src_alt/map_manager/map_loader.py @@ -0,0 +1,60 @@ +#!/usr/bin/python +import rospy +import xml.etree.ElementTree as ET +import rospy + +from memory_definitions.srv import GetDefinition + +class LoadChecker(): + @staticmethod + def checkNodesExist(xml, *node_names): + for node in node_names: + if not len(xml.findall(node)) > 0: + rospy.logerr("ERROR While loading map : '{}' node required inside '{}' node.".format(node, xml.tag)) + raise KeyError() + + @staticmethod + def checkAttribsExist(xml, *attrib_names): + for attrib in attrib_names: + if not attrib in xml.attrib: + rospy.logerr("ERROR While loading map : '{}' attribute required inside '{}' node.".format(attrib, xml.tag)) + raise KeyError() + +class MapLoader(): + CONFIG_FILE = "config.xml" + CLASSES_FILE = "classes.xml" + OBJECTS_FILE = "objects.xml" + ROBOTS_FILE = "robots.xml" + + @staticmethod + def loadFile(filename): + return MapLoader.loafXMLFromDescriptionModule(filename) + + @staticmethod + def loafXMLFromDescriptionModule(filename): + ''' + Loads the description file by getting it from the 'memory/definitions' ROS package. + ''' + get_def = rospy.ServiceProxy('/memory/definitions/get', GetDefinition) + try: + get_def.wait_for_service(timeout = 2) + except: + rospy.logerr("FATAL Could not contact definitions service, timeout reached. Aborting.") + raise Exception() + + try: + res = get_def('map/' + filename) + if not res.success: + rospy.logerr("Error when fetching '{}' definition file".format(filename)) + raise Exception() + return MapLoader._xml_from_file(res.path) + except rospy.ServiceException as exc: + rospy.logerr("Unhandled error while getting def file: {}".format(str(exc))) + raise Exception() + + @staticmethod + def _xml_from_file(path): + try: + return ET.parse(path).getroot() + except Exception as exc: + rospy.logerr("Could not load map XML file from path '{}' : {}".format(path, str(exc))) diff --git a/ros_ws/src/memory_map/src_alt/map_manager/robot.py b/ros_ws/src/memory_map/src_alt/map_manager/robot.py new file mode 100644 index 0000000..1bc9bc6 --- /dev/null +++ b/ros_ws/src/memory_map/src_alt/map_manager/robot.py @@ -0,0 +1,4 @@ +class Robot(object): + def __init__(self, xml): + self.name = xml.attrib["name"] + self.active = False # set to True externally if this robot is the current one in the system diff --git a/ros_ws/src/memory_map/src_alt/map_manager/shapes.py b/ros_ws/src/memory_map/src_alt/map_manager/shapes.py new file mode 100644 index 0000000..a3ccee0 --- /dev/null +++ b/ros_ws/src/memory_map/src_alt/map_manager/shapes.py @@ -0,0 +1,47 @@ +from map_loader import LoadChecker +from bases import Vector3 + +class Shape(object): + TYPE = '' + + def __init__(self, xml): + self.type = xml.attrib["type"] + + def get_scale(self): + raise NotImplementedError("Method must be overritten by subclass.") + + +def CreateShape(xml): + if "type" not in xml.attrib: + raise KeyError("Shape definitions need a type.") + + for c in Shape.__subclasses__(): + if c.TYPE == xml.attrib["type"]: + return c(xml) + + raise ValueError("No shape class defined for type '{}'.".format(xml.attrib["type"])) + + +class Rect(Shape): + TYPE = 'rect' + + def __init__(self, xml): + super(Rect, self).__init__(xml) + LoadChecker.checkAttribsExist(xml, "w", "h") + self.w = float(xml.attrib["w"]) + self.h = float(xml.attrib["h"]) + + def get_scale(self): + return Vector3(self.w, self.h, None) + + +class Circle(Shape): + TYPE = 'circle' + + def __init__(self, xml): + super(Circle, self).__init__(xml) + LoadChecker.checkAttribsExist(xml, "r") + self.r = float(xml.attrib["r"]) + + def get_scale(self): + return Vector3(self.r * 2.0, self.r * 2.0, None) diff --git a/ros_ws/src/memory_map/src_alt/map_node.py b/ros_ws/src/memory_map/src_alt/map_node.py new file mode 100644 index 0000000..18b22c9 --- /dev/null +++ b/ros_ws/src/memory_map/src_alt/map_node.py @@ -0,0 +1,41 @@ +#!/usr/bin/python +import rospy +import map_manager +#import map_communication +#from markers import MarkersPublisher +#from occupancy import OccupancyGenerator + + +class MapNode(): + def __init__(self): + rospy.init_node("map", log_level=rospy.INFO) + + map_manager.Map.load("gr", "green") + print map_manager.Map.OBJECTS[0].elements[0].features + + + # Starting and publishing the table STL to RViz + #self.markers = MarkersPublisher() + + # Generate static occupancy images for pathfinder, etc. + #occupancy = OccupancyGenerator(map_manager.Map) + + # Starting service handlers (Get, Set, Transfer, GetOccupancy) + #map_communication.MapServices(occupancy) + print "loaded map" + + #self.run() + + def run(self): + r = rospy.Rate(10) + while not rospy.is_shutdown(): + if rospy.has_param("/current_team"): + map_manager.Map.CONFIG.CURRENT_TEAM = rospy.get_param("/current_team") + if rospy.has_param("/robot"): + map_manager.Map.CONFIG.CURRENT_ROBOT = rospy.get_param("/robot") + #self.markers.updateMarkers(map_manager.Map) + r.sleep() + + +if __name__ == "__main__": + MapNode() diff --git a/ros_ws/src/memory_map/src_alt/markers/__init__.py b/ros_ws/src/memory_map/src_alt/markers/__init__.py new file mode 100644 index 0000000..8e41fde --- /dev/null +++ b/ros_ws/src/memory_map/src_alt/markers/__init__.py @@ -0,0 +1 @@ +from markers_publisher import MarkersPublisher diff --git a/ros_ws/src/memory_map/src_alt/markers/markers_publisher.py b/ros_ws/src/memory_map/src_alt/markers/markers_publisher.py new file mode 100644 index 0000000..fbeccb6 --- /dev/null +++ b/ros_ws/src/memory_map/src_alt/markers/markers_publisher.py @@ -0,0 +1,92 @@ +#!/usr/bin/python +import math +import rospy +from map_manager import map_attributes +from visualization_msgs.msg import Marker + + +class MarkersPublisher(): + def __init__(self): + self.MARKERS_TOPIC = "/visualization_markers/world" + self.MarkersPUBL = rospy.Publisher(self.MARKERS_TOPIC, Marker, queue_size=10) + + def _is_connected(self): + return bool(self.MarkersPUBL.get_num_connections()) + + def updateMarkers(self, world): + if self._is_connected(): + self._publish_table(world) + self._publish_robot_stl(world) + self._update_zones(world) + self._publish_objects(world.get("/objects/^")) + + def _publish_table(self, world): + pos = map_attributes.Position2D({ + "frame_id": "/map", + "x": -0.022, + "y": 0.022 + 2, + "type": "fixed" + }) + self._publish_marker(pos, world.get("/terrain/marker/^")) + + def _publish_robot_stl(self, world): + pos = map_attributes.Position2D({ + "frame_id": "/robot", + "x": 0, + "y": 0, + "type": "fixed" + }) + self._publish_marker(pos, world.get("/entities/{}/marker/^".format(rospy.get_param("/robot")))) + + def _update_zones(self, world): + for z in world.get("/zones/^").toList(): + self._publish_marker(z.get("position/^"), z.get("marker/^")) + + def _publish_objects(self, objects_dictman): + for e in objects_dictman.Dict.keys(): + if "container_" in e: + self._publish_objects(objects_dictman.get("{}/^".format(e))) + else: + self._publish_marker(objects_dictman.Dict[e].get("position/^"), objects_dictman.Dict[e].get("marker/^")) + + def _publish_marker(self, position, visual): + markertypes = { + "cube": Marker.CUBE, + "sphere": Marker.SPHERE, + "mesh": Marker.MESH_RESOURCE + } + marker = Marker() + marker.header.frame_id = position.get("frame_id") + marker.type = markertypes[visual.get("type")] + marker.ns = visual.get("ns") + marker.id = visual.get("id") + + marker.action = Marker.ADD + marker.scale.x = visual.get("scale")[0] + marker.scale.y = visual.get("scale")[1] + marker.scale.z = visual.get("scale")[2] + marker.color.r = visual.get("color")[0] + marker.color.g = visual.get("color")[1] + marker.color.b = visual.get("color")[2] + marker.color.a = visual.get("color")[3] + marker.pose.position.x = position.get("x") + marker.pose.position.y = position.get("y") + marker.pose.position.z = visual.get("z") + orientation = self._euler_to_quaternion(visual.get("orientation")) + marker.pose.orientation.x = orientation[0] + marker.pose.orientation.y = orientation[1] + marker.pose.orientation.z = orientation[2] + marker.pose.orientation.w = orientation[3] + + marker.mesh_resource = visual.get("mesh_path") if marker.type == Marker.MESH_RESOURCE else '' + + self.MarkersPUBL.publish(marker) + + def _euler_to_quaternion(self, xyz): + cr, sr = math.cos(xyz[0] * 0.5), math.sin(xyz[0] * 0.5) + cp, sp = math.cos(xyz[1] * 0.5), math.sin(xyz[1] * 0.5) + cy, sy = math.cos(xyz[2] * 0.5), math.sin(xyz[2] * 0.5) + return (cy * sr * cp - sy * cr * sp, # qx + cy * cr * sp + sy * sr * cp, # qy + sy * cr * cp - cy * sr * sp, # qz + cy * cr * cp + sy * sr * sp) # qw diff --git a/ros_ws/src/memory_map/src_alt/occupancy/__init__.py b/ros_ws/src/memory_map/src_alt/occupancy/__init__.py new file mode 100644 index 0000000..90a4bd9 --- /dev/null +++ b/ros_ws/src/memory_map/src_alt/occupancy/__init__.py @@ -0,0 +1,2 @@ +#!/usr/bin/python +from generator import OccupancyGenerator \ No newline at end of file diff --git a/ros_ws/src/memory_map/src_alt/occupancy/generator.py b/ros_ws/src/memory_map/src_alt/occupancy/generator.py new file mode 100644 index 0000000..af164b0 --- /dev/null +++ b/ros_ws/src/memory_map/src_alt/occupancy/generator.py @@ -0,0 +1,50 @@ +#!/usr/bin/python +import os +import time +import rospy +from PIL import Image, ImageDraw + +class OccupancyGenerator(): + def __init__(self, world): + self.ImgWidth = 500 # Width of the generated images. Height will be calculated based on the map aspect ratio. + self.WorldSize = (float(world.get("/terrain/shape/width")), float(world.get("/terrain/shape/height"))) + self.ImgSize = (self.ImgWidth, int(self.ImgWidth * (self.WorldSize[1] / self.WorldSize[0]))) + + def generateLayer(self, world, layer_name, margin = 0.0): + final_path = os.path.dirname(__file__) + "/img/" + layer_name + ".bmp" + layers = world.get("/terrain/walls/^") + if not layer_name in layers.Dict.keys(): + rospy.logerr(" Tried to get layer '{}''s image, but it hasn't been generated. Aborting.".format(layer_name)) + return None + img = self.generateStaticOccupancy(layers.Dict[layer_name], margin) + img.save(final_path) + return final_path + + def generateStaticOccupancy(self, layer, margin = 0.0): # margin in m. + img = Image.new("RGB", self.ImgSize, (255, 255, 255)) + + draw = ImageDraw.Draw(img) + + for wall in layer.toList(): + position, shape = wall.get("position/^").toDict(), wall.get("shape/^").toDict() + + pos = self.world_to_img_pos((position["x"], position["y"])) + if shape["type"] == "rect": + w, h = self.world_to_img_scale(shape["width"] + margin * 2), self.world_to_img_scale(shape["height"] + margin * 2) + draw.rectangle((pos[0] - w/2.0, pos[1] - h/2.0, pos[0] + w/2.0, pos[1] + h/2.0), fill=(0, 0, 0)) + elif shape["type"] == "circle": + r = self.world_to_img_scale(shape["radius"] + margin) + draw.ellipse((pos[0] - r, pos[1] - r, pos[0] + r, pos[1] + r), fill = (0, 0, 0)) + elif shape["type"] == "polygon": + rospy.logwarn("Occupancy generator: polygon drawing not implemented") + elif shape["type"] == "line": + rospy.logwarn("Occupancy generator: line drawing not implemented") + else: + rospy.logerr("Occupancy generator could not recognize shape type '{}'.".format(shape["type"])) + del draw + return img + + def world_to_img_scale(self, world_coord): + return world_coord * (self.ImgSize[0] / (self.WorldSize[0])) + def world_to_img_pos(self, world_pos): + return (self.world_to_img_scale(world_pos[0]), self.ImgSize[1] - self.world_to_img_scale(world_pos[1])) diff --git a/ros_ws/src/memory_map/src_alt/occupancy/img/layer_ground.bmp b/ros_ws/src/memory_map/src_alt/occupancy/img/layer_ground.bmp new file mode 100644 index 0000000..5078c72 Binary files /dev/null and b/ros_ws/src/memory_map/src_alt/occupancy/img/layer_ground.bmp differ diff --git a/ros_ws/src/memory_map/srv/FillWaypoint.srv b/ros_ws/src/memory_map/srv/FillWaypoint.srv new file mode 100644 index 0000000..5fb6ad6 --- /dev/null +++ b/ros_ws/src/memory_map/srv/FillWaypoint.srv @@ -0,0 +1,9 @@ +# Service that takes an incomplete memory_map/Waypoint message +# and that responds with the same message with the rest of the fields +# completed if possible. +# E.g. Send a waypoint with only its name and you will get the same +# thing with the XY filled (if the waypoint exists). +memory_map/Waypoint waypoint +--- +bool success +memory_map/Waypoint filled_waypoint # WARNING frame_id is NOT FILLED for now, assuming it is /map. \ No newline at end of file diff --git a/ros_ws/src/memory_map/srv/MapGet.srv b/ros_ws/src/memory_map/srv/MapGet.srv new file mode 100644 index 0000000..e39165c --- /dev/null +++ b/ros_ws/src/memory_map/srv/MapGet.srv @@ -0,0 +1,9 @@ +# Service to get items, attributes and objects from the map. +# Please refer to the README file on how to request a specific datum. +string request_path +--- +# Response can either be a simple string object (with a string, int, bool etc. inside) or a JSON dictionary. +# An false success field most probably means the request path isn't valid. +# Try to refer to the map logs. A failed GET request will generate a ROS error log on /rosout. +bool success +string response \ No newline at end of file diff --git a/ros_ws/src/memory_map/srv/MapGetObjects.srv b/ros_ws/src/memory_map/srv/MapGetObjects.srv new file mode 100644 index 0000000..7cef60d --- /dev/null +++ b/ros_ws/src/memory_map/srv/MapGetObjects.srv @@ -0,0 +1,5 @@ +# This service will reply with a list of all the map objects available. +bool collisions_only # consider only objects that can collide with the robot +--- +bool success +string[] objects # JSON definition of each retrieved object. \ No newline at end of file diff --git a/ros_ws/src/memory_map/srv/MapGetOccupancy.srv b/ros_ws/src/memory_map/srv/MapGetOccupancy.srv new file mode 100644 index 0000000..08bd68d --- /dev/null +++ b/ros_ws/src/memory_map/srv/MapGetOccupancy.srv @@ -0,0 +1,5 @@ +uint16 img_width # img width in pixels. Height will be calculated based on the aspect ratio. +float32 margin # margin in meters the shapes are going to be expanded at. +string layer_name +--- +string img_path \ No newline at end of file diff --git a/ros_ws/src/memory_map/srv/MapSet.srv b/ros_ws/src/memory_map/srv/MapSet.srv new file mode 100644 index 0000000..2bd8dff --- /dev/null +++ b/ros_ws/src/memory_map/srv/MapSet.srv @@ -0,0 +1,17 @@ +# Service to override the desired value(s) in the map database. +# Please refer to the README file on how to override a specific datum. +uint8 MODE_ADD = 0 # Create a DictManager (only one at a time). + # Subdicts will be automagically created. +uint8 MODE_REPLACE = 1 # Change a set of values in a dict. +uint8 MODE_REMOVE = 2 # Delete a value or a full dict (with its subdicts) +uint8 mode +# TODO ADD can only create a Dict, not a series of dicts (e.g. /objects/new_1/new_2:Object={} not supported) + +string request_path # Path examples : + # To create a full dict : /objects/cube_42:MapObject={"type": "object", "etc": True} + # To change a value : /objects/cube_14/position/x=0.42,y=0.84,a=3.14 + # To remove a full dict : /objects/cube_12 +--- +# A false response most probably means the request path or new value isn't valid. +# Try to refer to the map logs. A failed SET request will generate a ROS error log on /rosout. +bool success \ No newline at end of file diff --git a/ros_ws/src/memory_map/srv/MapTransfer.srv b/ros_ws/src/memory_map/srv/MapTransfer.srv new file mode 100644 index 0000000..4466342 --- /dev/null +++ b/ros_ws/src/memory_map/srv/MapTransfer.srv @@ -0,0 +1,4 @@ +string old_path # e.g. /objects/container_grape1 +string new_path # e.g. /entities/robot/containers/container_tower1 +--- +bool success # e.g. result: /entities/robot/containers/container_tower1/container_grape_1/... \ No newline at end of file diff --git a/ros_ws/src/movement_actuators/CMakeLists.txt b/ros_ws/src/movement_actuators/CMakeLists.txt new file mode 100644 index 0000000..d8ee5fa --- /dev/null +++ b/ros_ws/src/movement_actuators/CMakeLists.txt @@ -0,0 +1,56 @@ +cmake_minimum_required(VERSION 2.8.3) +project(movement_actuators) + +find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs + actionlib_msgs + tf2_ros + geometry_msgs + actionlib + ai_game_manager +) + +# Generate services in the 'srv' folder +add_service_files( + FILES + ActivateCanon.srv +) + +## Generate actions in the 'action' folder +add_action_files(DIRECTORY action FILES + Dispatch.action + Arm.action + Barrel.action + CubePicker.action +) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + actionlib_msgs + std_msgs +) + +catkin_package( + CATKIN_DEPENDS std_msgs actionlib_msgs tf2_ros rospy actionlib +) + +include_directories( + ${catkin_INCLUDE_DIRS} +) + +############# +## Install ## +############# + +catkin_install_python( + PROGRAMS src/actuators_node.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install( + DIRECTORY src/actuators + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + PATTERN "*.pyc" EXCLUDE +) diff --git a/ros_ws/src/movement_actuators/action/Arm.action b/ros_ws/src/movement_actuators/action/Arm.action new file mode 100644 index 0000000..db6a3e9 --- /dev/null +++ b/ros_ws/src/movement_actuators/action/Arm.action @@ -0,0 +1,8 @@ +# Specific action file for complex action "arm" of the big robot, 2018 +string frame_id +float32 x +float32 y +float32 timeout # secs, 0 for no timeout +--- +bool success +--- diff --git a/ros_ws/src/movement_actuators/action/Barrel.action b/ros_ws/src/movement_actuators/action/Barrel.action new file mode 100644 index 0000000..4a4b4c3 --- /dev/null +++ b/ros_ws/src/movement_actuators/action/Barrel.action @@ -0,0 +1,7 @@ +# Specific action file for complex action "BarrelSort" of the big robot, 2018 +bool sort +float32 timeout +float32 pause +--- +bool success +--- \ No newline at end of file diff --git a/ros_ws/src/movement_actuators/action/CubePicker.action b/ros_ws/src/movement_actuators/action/CubePicker.action new file mode 100644 index 0000000..86c9a39 --- /dev/null +++ b/ros_ws/src/movement_actuators/action/CubePicker.action @@ -0,0 +1,4 @@ +# Specific action file for complex action "cubePicker" of the big robot, 2018 +--- +bool success +--- diff --git a/ros_ws/src/movement_actuators/action/Dispatch.action b/ros_ws/src/movement_actuators/action/Dispatch.action new file mode 100644 index 0000000..ae4b2e5 --- /dev/null +++ b/ros_ws/src/movement_actuators/action/Dispatch.action @@ -0,0 +1,16 @@ +# Action used to move any actuator connected to the robot. Enter the actuator name (defined +# in the node's XML) with movement params. + +string name # Actuator id. +int32 id # Can be directly used instead of the string name. Ignored if name is defined. + +int32 timeout # In ms. Replaced by a default timeout if negative or null. + +string order # Movement type passed in to the arduinos/AX12s (TODO TO BE DEPRECATED ?). JOINT or WHEEL + +string preset # Can be used to find a defined position in the XML. Ignored if param is defined. +string param # Manual destination value (position, speed...) if no preset exists for your value. + +--- +bool success +--- diff --git a/ros_ws/src/movement_actuators/package.xml b/ros_ws/src/movement_actuators/package.xml new file mode 100644 index 0000000..7d817ff --- /dev/null +++ b/ros_ws/src/movement_actuators/package.xml @@ -0,0 +1,19 @@ + + + movement_actuators + 0.0.2 + The actuators package + GPLv3 + P. Potiron + Thomas Fuhrmann + UTCoupe + catkin + message_generation + rospy + std_msgs + actionlib + geometry_msgs + tf2_ros + actionlib_msgs + ai_game_manager + \ No newline at end of file diff --git a/ros_ws/src/movement_actuators/src/actuators/__init__.py b/ros_ws/src/movement_actuators/src/actuators/__init__.py new file mode 100644 index 0000000..a08b627 --- /dev/null +++ b/ros_ws/src/movement_actuators/src/actuators/__init__.py @@ -0,0 +1,9 @@ +#!/usr/bin/env python +# -*-coding:Utf-8 -* + +from actuators_abstract import * +from actuators_dispatch import * +from actuators_arm import * +from actuators_barrel import * +from actuators_canon import * +from actuators_cube_picker import * diff --git a/ros_ws/src/movement_actuators/src/actuators/actuators_abstract.py b/ros_ws/src/movement_actuators/src/actuators/actuators_abstract.py new file mode 100644 index 0000000..53ed337 --- /dev/null +++ b/ros_ws/src/movement_actuators/src/actuators/actuators_abstract.py @@ -0,0 +1,50 @@ +#!/usr/bin/env python + +import rospy +import actionlib + +__author__ = "Thomas Fuhrmann" +__date__ = 9/04/2018 + +NODE_NAME = "actuators" + + +class ActuatorsAbstract: + def __init__(self, action_name, action_type): + # Dictionary to manage received goals (mandatory for actionlib) + self._goals_handler_dictionary = {} + self._isHalted = False + self._action_type = None + if action_name != "" and action_type is not None: + self._action_type = action_type + self._action_server = actionlib.ActionServer("/movement/" + NODE_NAME + "/" + action_name, action_type, self._callback_action, auto_start=False) + self._action_server.start() + else: + rospy.logerr("ActuatorsAbstract class can not be built without action_name and action_type, aborting.") + exit(1) + + def _action_reached(self, goal_id, reached, result): + # Result is passed as parameter to avoid retrieving the goal result type in abstract class, maybe a TODO ? + if reached: + rospy.loginfo("Goal id {}, has been reached.".format(goal_id.id)) + else: + rospy.logwarn("Goal id {}, has NOT been reached.".format(goal_id.id)) + if goal_id in self._goals_handler_dictionary: + self._goals_handler_dictionary[goal_id].set_succeeded(result) + del self._goals_handler_dictionary[goal_id] + + def _process_action(self, goal, goal_id): + rospy.logerr("ActuatorsAbstract is abstract !") + return False + + def _callback_action(self, goal_handle): + rospy.logdebug("Just received an action") + if self._process_action(goal_handle.get_goal(), goal_handle.get_goal_id()): + goal_handle.set_accepted() + self._goals_handler_dictionary[goal_handle.get_goal_id()] = goal_handle + else: + goal_handle.set_rejected() + + def setHalted(self, isHalted): + rospy.logerr("ActuatorsAbstract is abstract !") + pass diff --git a/ros_ws/src/movement_actuators/src/actuators/actuators_arm.py b/ros_ws/src/movement_actuators/src/actuators/actuators_arm.py new file mode 100644 index 0000000..963293d --- /dev/null +++ b/ros_ws/src/movement_actuators/src/actuators/actuators_arm.py @@ -0,0 +1,227 @@ +import math + +import actionlib +import rospy +import tf2_ros +from actionlib_msgs.msg import GoalStatus +from actuators_abstract import ActuatorsAbstract +from drivers_ax12.msg import Ax12CommandGoal, Ax12CommandAction +from geometry_msgs.msg import TransformStamped, PointStamped +from movement_actuators.msg import ArmAction, ArmResult +from memory_definitions.srv import GetDefinition +from functools import partial +import yaml + +# Do not remove, used to transform PointStamped +import tf2_geometry_msgs + +class ActuatorsArm(ActuatorsAbstract): + def __init__(self): + self.ORIGIN_FRAME = "arm_origin" + + self._broadcaster = tf2_ros.StaticTransformBroadcaster() + + self._buffer = tf2_ros.Buffer() + self._listener = tf2_ros.TransformListener(self._buffer) + + self._fetch_and_parse_config() + + self._max_range = self._motor1['length'] + self._motor2['length'] + + self._ax12_goal_handles = {} + self._ax12_timers = {} + + self._bad_statuses = [GoalStatus.LOST, + GoalStatus.RECALLED, + GoalStatus.REJECTED, + GoalStatus.ABORTED, + GoalStatus.PREEMPTED] + + self._pub_static_transform(self.ORIGIN_X, self.ORIGIN_Y) + + self._client = actionlib.ActionClient('/drivers/ax12', Ax12CommandAction) + self._client.wait_for_server(rospy.Duration(10)) + + rospy.logdebug('Ax12 server found') + + ActuatorsAbstract.__init__(self, + action_name='arm', + action_type=ArmAction) + + def _process_action(self, goal, goal_id): + if self._isHalted: + rospy.logerr("Arm actuator is turned off!") + return False + point = PointStamped() + point.header.frame_id = goal.frame_id + point.point.x = goal.x + point.point.y = goal.y + + try: + point = self._buffer.transform_full(point, + target_frame=self.ORIGIN_FRAME, + target_time=rospy.Time(), + fixed_frame=goal.frame_id) + + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: + rospy.logerr("Can't transform given position into origin frame : %s" % e) + return False + + self._buffer.transform(point, self.ORIGIN_FRAME) + + x_rel = point.point.x + y_rel = point.point.y + + if math.sqrt(math.pow(x_rel, 2) + math.pow(y_rel, 2)) > self._max_range: + rospy.logerr("Can't reach goal, max range of %s" % self._max_range) + return False + + q2_toacos = (math.pow(x_rel, 2) + math.pow(y_rel, 2) + - math.pow(self._motor1['length'], 2) + - math.pow(self._motor2['length'], 2)) / (2 * self._motor1['length'] * self._motor2['length']) + + if abs(q2_toacos) > 1: + rospy.logerr("Can't reach goal, not on possible trajectory") + return False + + q2 = math.acos(q2_toacos) + + if y_rel == 0: + q1 = 0 + elif x_rel == 0: + q1 = y_rel / abs(y_rel) * math.pi / 2 + else: + q1 = math.atan(y_rel / x_rel) + + q1 -= math.atan((self._motor2['length'] * math.sin(q2)) / + (self._motor1['length'] + self._motor2['length'] * math.cos(q2))) + + if q2 > math.pi: + q2 = -(2 * math.pi - q2) + + if q1 > math.pi: + q1 = -(2 * math.pi - q1) + + rospy.logdebug('Computed angles : %f and %f' % (q1, q2)) + + q2_value = self._motor2['center'] + int((q2 / (15.0 / 18.0 * math.pi)) * 512.0) + q1_value = self._motor1['center'] + int((q1 / (15.0 / 18.0 * math.pi)) * 512.0) + + if q2_value >= self._motor2['max'] or q2_value <= self._motor2['min']: + rospy.logerr("Value not in range for motor 2: %d" % q2_value) + + return False + + if q1_value >= self._motor1['max'] or q1_value <= self._motor1['min']: + rospy.logerr("Value not in range for motor 1: %d" % q1_value) + return False + + rospy.logdebug('Sending positions %d and %d' % (q1_value, q2_value)) + + self._ax12_goal_handles[goal_id] = [None, None] + + goal1 = Ax12CommandGoal() + goal1.mode = goal1.JOINT + goal1.motor_id = self._motor1['id'] + goal1.position = q1_value + + self._ax12_goal_handles[goal_id][0] = \ + self._client.send_goal(goal1, transition_cb=self._callback) + + goal2 = Ax12CommandGoal() + goal2.mode = goal2.JOINT + goal2.motor_id = self._motor2['id'] + goal2.position = q2_value + + self._ax12_goal_handles[goal_id][1] = \ + self._client.send_goal(goal2, transition_cb=self._callback) + + if goal.timeout > 0: + self._ax12_timers[goal_id] = rospy.Timer(period=rospy.Duration(goal.timeout), + callback=partial(self._trigger_timeout, goal_id=goal_id), + oneshot=True) + else: + rospy.logwarn('No timeout specified, the action could take forever') + + return True + + def _trigger_timeout(self, event, goal_id): + rospy.logerr('Timeout triggered for goal %s, cancelling' % goal_id.id) + if goal_id in self._ax12_goal_handles: + self._ax12_goal_handles[goal_id][0].cancel() + self._ax12_goal_handles[goal_id][1].cancel() + + self._quit_action(goal_id, False) + + def _callback(self, gh): + goal_id_ax12 = gh.comm_state_machine.action_goal.goal_id.id + + for goal_id, [gh1, gh2] in self._ax12_goal_handles.iteritems(): + if gh1.comm_state_machine.action_goal.goal_id.id != goal_id_ax12 \ + and gh2.comm_state_machine.action_goal.goal_id.id != goal_id_ax12: + continue + + gh1_status = gh1.get_goal_status() + gh2_status = gh2.get_goal_status() + + if gh1_status == GoalStatus.SUCCEEDED \ + and gh2_status == GoalStatus.SUCCEEDED: + self._quit_action(goal_id, True) + return + + elif gh1_status in self._bad_statuses \ + or gh2_status in self._bad_statuses: + self._quit_action(goal_id, False) + return + + def _quit_action(self, goal_id, success): + if goal_id in self._ax12_goal_handles: + del self._ax12_goal_handles[goal_id] + + if goal_id in self._ax12_timers: + self._ax12_timers[goal_id].shutdown() + del self._ax12_timers[goal_id] + + self._action_reached(goal_id, success, ArmResult(success=success)) + + + def _pub_static_transform(self, x, y): + rospy.logdebug("Publishing the static transform") + + tr = TransformStamped() + + tr.header.stamp = rospy.Time.now() + tr.header.frame_id = "/robot" + tr.child_frame_id = self.ORIGIN_FRAME + + tr.transform.translation.x = x + tr.transform.translation.y = y + tr.transform.translation.z = 0 + tr.transform.rotation.w = 1 + + self._broadcaster.sendTransform(tr) + + def _fetch_and_parse_config(self): + rospy.wait_for_service('/memory/definitions/get') + service = rospy.ServiceProxy('/memory/definitions/get', GetDefinition) + + try: + path = service('movement/arm.yml').path + except rospy.ServiceException as e: + rospy.logerr("Can't fetch arm config: %s" % e) + return + + with open(path, 'r') as f: + try: + conf = yaml.load(f) + self._motor1 = conf['motor1'] + self._motor2 = conf['motor2'] + + self.ORIGIN_X = conf['origin']['x'] + self.ORIGIN_Y = conf['origin']['y'] + except yaml.YAMLError as e: + rospy.logerr("Can't parse config file : %s" % e) + + def setHalted(self, isHalted): + self._isHalted = isHalted + self._client.cancel_all_goals() diff --git a/ros_ws/src/movement_actuators/src/actuators/actuators_barrel.py b/ros_ws/src/movement_actuators/src/actuators/actuators_barrel.py new file mode 100644 index 0000000..3a089cd --- /dev/null +++ b/ros_ws/src/movement_actuators/src/actuators/actuators_barrel.py @@ -0,0 +1,175 @@ + +import actionlib +import rospy +from actuators_abstract import ActuatorsAbstract +from movement_actuators.msg import BarrelAction, BarrelResult, DispatchAction, DispatchGoal +import drivers_ard_others.msg + +from actionlib.action_client import CommState + +import matplotlib.pyplot as plt + +class Color: + UNKNOWN = 0 + ORANGE = 1 + GREEN = 2 + +class ActuatorsBarrel(ActuatorsAbstract): + def __init__(self): + + self.COLOR_SENSOR_TOPIC = '/drivers/ard_others/color' + self.BARREL_NAME = 'barrel' # name in the dispatcher + self.PRESET_BIN = 'BIN' + self.PRESET_NORMAL = 'HIGH' + self.PRESET_CANON = 'CANON' + + self.ORANGE_HUE = 15 + self.GREEN_HUE = 120 + self.HUE_MARGIN = 15 + + self.SATURATION_TRESH = 90 + + self.BALL_COUNT = 8 + + self._client = actionlib.SimpleActionClient('/movement/actuators/dispatch', DispatchAction) + self._client.wait_for_server(rospy.Duration(10)) + + self._curr_color = Color.UNKNOWN + + self._color_client = rospy.Subscriber(self.COLOR_SENSOR_TOPIC, drivers_ard_others.msg.Color, self._color_callback) + + self._team_color = Color.ORANGE if rospy.get_param('/current_team', 'orange') == 'orange' else Color.GREEN + + ActuatorsAbstract.__init__(self, + action_name='barrel', + action_type=BarrelAction) + + self._is_running = False + self._doing_back = False + self._doing_forth = False + + self._curr_goal_id = None + self._curr_pause = 0 + + self._pause_timer = None + + self._timer = None + self._curr_timeout = 0 + + def _process_action(self, goal, goal_id): + if self._isHalted: + rospy.logerr("Barrel actuator is turned off!") + return False + if self._is_running: + rospy.logerr("Received a goal but another one is in process !") + return False + + self._is_running = True + self._curr_goal_id = goal_id + self._curr_pause = goal.pause + + if goal.timeout > 0: + self._curr_timeout = goal.timeout*1000 + else: + self._curr_timeout = 5000 + rospy.logwarn('No timeout is set, setting dispatcher timeout to 5s') + + if not goal.sort: + rospy.logdebug('Starting goal chain with no sort') + self._start_goal_chain(self.PRESET_CANON) + else: + rospy.logdebug('Starting goal chain for the bin') + self._start_goal_chain(self.PRESET_BIN) + + return True + + def _start_back(self, e=None): + self._timer = rospy.Timer(rospy.Duration(self._curr_timeout), lambda e: self._finish_action(False), oneshot=True) + + g = DispatchGoal() + g.name = self.BARREL_NAME + g.order = 'JOINT' + g.preset = self.PRESET_NORMAL + g.timeout = self._curr_timeout + + self._doing_back = True + self._doing_forth = False + self._client.send_goal(g, done_cb=self._back_done_cb) + + + def _forth_done_cb(self, state, result): + rospy.logdebug('forth_done, waiting %d seconds' % self._curr_pause) + if self._timer: + self._timer.shutdown() + self._timer = None + + if self._curr_pause > 0: + self._pause_timer = rospy.Timer(rospy.Duration(self._curr_pause), self._start_back, oneshot=True) + else: + self._start_back() + + def _back_done_cb(self, state, result): + rospy.logdebug('back_done') + self._finish_action(result.success) + + def _color_callback(self, msg): + + if msg.hue <= self.ORANGE_HUE + self.HUE_MARGIN \ + and msg.hue >= self.ORANGE_HUE - self.HUE_MARGIN\ + and msg.saturation > self.SATURATION_TRESH: + self._curr_color = Color.ORANGE + + elif msg.hue <= self.GREEN_HUE + self.HUE_MARGIN \ + and msg.hue >= self.GREEN_HUE - self.HUE_MARGIN \ + and msg.saturation > self.SATURATION_TRESH: + self._curr_color = Color.GREEN + + else: + self._curr_color = Color.UNKNOWN + + if self._is_running and \ + not self._doing_back and \ + not self._doing_forth \ + and self._curr_color != Color.UNKNOWN: + + if self._team_color != self._curr_color: + rospy.logfatal('Starting goal chain for ennemy color ball (h: %d, s: %d, v %d)' + % (msg.hue, msg.saturation, msg.lightness)) + self._start_goal_chain(self.PRESET_BIN) + else: + rospy.logfatal('Starting goal chain for ally color balll (h: %d, s: %d, v %d)' + % (msg.hue, msg.saturation, msg.lightness)) + self._start_goal_chain(self.PRESET_CANON) + + + def _start_goal_chain(self, preset): + self._timer = rospy.Timer(rospy.Duration(self._curr_timeout), self._start_back, oneshot=True) + + g = DispatchGoal() + g.name = self.BARREL_NAME + g.order = 'JOINT' + g.preset = preset + g.timeout = self._curr_timeout + + self._doing_forth = True + self._client.send_goal(g, done_cb=self._forth_done_cb) + + def _finish_action(self, success): + self._is_running = False + self._doing_forth = False + self._doing_back = False + + self._client.stop_tracking_goal() + + if self._timer: + self._timer.shutdown() + self._timer = None + + self._action_reached(self._curr_goal_id, success, BarrelResult(success=success)) + + self._curr_goal_id = None + + + def setHalted(self, isHalted): + self._isHalted = isHalted + self._client.cancel_all_goals() diff --git a/ros_ws/src/movement_actuators/src/actuators/actuators_canon.py b/ros_ws/src/movement_actuators/src/actuators/actuators_canon.py new file mode 100644 index 0000000..383f527 --- /dev/null +++ b/ros_ws/src/movement_actuators/src/actuators/actuators_canon.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python + +import rospy +import actionlib +from math import sqrt, pow +from movement_actuators.srv import * +from movement_actuators.msg import DispatchAction, DispatchGoal +from geometry_msgs.msg import Pose2D + +__author__ = "Thomas Fuhrmann" +__date__ = 1/05/2018 + +DISPATCH_SERVER_TIMEOUT = 10 # in seconds +FLIPPER_PERIOD = 1 # in seconds +FLIPPER_NAME_CANON = "servo_flipper_side_canon" +FLIPPER_NAME_BIN = "servo_flipper_side_bin" +CANON_NAME = "canon" +DEFAULT_ACTUATORS_TIMEOUT = 1000 # in ms + + +class ActuatorsCanon: + # We consider to be at 66% on the diagonal starting from the bottom corner located in (370, 2000). + WATER_TOWER_POSITION = Pose2D(0.125, 2.267, 0.0) + + def __init__(self): + self._client = actionlib.SimpleActionClient('/movement/actuators/dispatch', DispatchAction) + self._client.wait_for_server(rospy.Duration(DISPATCH_SERVER_TIMEOUT)) + self._tmr_flipper_activate = False + self._tmr_flipper = rospy.Timer(rospy.Duration(FLIPPER_PERIOD), self._callback_timer_flipper) + self._srv_canon = rospy.Service("/movement/actuators/activate_canon", ActivateCanon, self._callback_activate_canon) + # Subscribe to Pose2D for asserv in order to compute distance to the goal + self._sub_asserv = rospy.Subscriber("/drivers/ard_asserv/pose2d", Pose2D, self._callback_position) + self._data_asserv = None + # Boolean used for flipper toggle values, as they just flip on 2 values + self._flipper_state = False + self._is_halted = False + + def _callback_activate_canon(self, request): + response = True + if request.fire_distance == -1: + self._flipper_stop() + self._canon_stop() + elif request.fire_distance >= 0: + self._flipper_start() + self._canon_start(request.fire_distance) + else: + response = False + return ActivateCanonResponse(response) + + def _flipper_start(self): + self._tmr_flipper_activate = True + + def _flipper_stop(self): + self._tmr_flipper_activate = False + + def _canon_start(self, distance): + recv_distance = distance + if recv_distance == 0: + recv_distance = self._compute_water_tower_distance() + ref_value = 16*recv_distance + 32 + goal_canon = DispatchGoal() + goal_canon.name = CANON_NAME + goal_canon.param = str(int(ref_value)) + goal_canon.timeout = DEFAULT_ACTUATORS_TIMEOUT + self._client.send_goal(goal_canon) + + def _canon_stop(self): + goal_canon = DispatchGoal() + goal_canon.name = CANON_NAME + goal_canon.preset = "OFF" + goal_canon.timeout = DEFAULT_ACTUATORS_TIMEOUT + self._client.send_goal(goal_canon) + + def _callback_timer_flipper(self, event): + if self._tmr_flipper_activate: + if self._flipper_state: + preset = "MIN" + self._flipper_state = False + else: + preset = "MAX" + self._flipper_state = True + goal_flipper_canon = DispatchGoal() + goal_flipper_canon.name = FLIPPER_NAME_CANON + goal_flipper_canon.preset = preset + goal_flipper_canon.timeout = DEFAULT_ACTUATORS_TIMEOUT + self._client.send_goal(goal_flipper_canon) + # Flipper bin side has been physically removed... + # goal_flipper_bin = DispatchGoal() + # goal_flipper_bin.name = FLIPPER_NAME_BIN + # goal_flipper_bin.preset = preset + # goal_flipper_bin.timeout = DEFAULT_ACTUATORS_TIMEOUT + # self._client.send_goal(goal_flipper_bin) + + def set_halted(self, is_halted): + self._is_halted = is_halted + if is_halted: + self._canon_stop() + self._flipper_stop() + + def _callback_position(self, data): + self._data_asserv = data + + def _compute_water_tower_distance(self): + return sqrt(pow(self._data_asserv.x - self.WATER_TOWER_POSITION.x, 2) + pow(self._data_asserv.y - self.WATER_TOWER_POSITION.y, 2)) + diff --git a/ros_ws/src/movement_actuators/src/actuators/actuators_cube_picker.py b/ros_ws/src/movement_actuators/src/actuators/actuators_cube_picker.py new file mode 100644 index 0000000..85ed41c --- /dev/null +++ b/ros_ws/src/movement_actuators/src/actuators/actuators_cube_picker.py @@ -0,0 +1,108 @@ +#!/usr/bin/env python + +import rospy, time +import actionlib +from actuators_abstract import ActuatorsAbstract +from actionlib_msgs.msg import GoalStatus +from geometry_msgs.msg import Pose2D +from movement_actuators.srv import * +from recognition_cube.srv import * +from movement_actuators.msg import CubePickerAction, ArmGoal, ArmAction, CubePickerResult, DispatchAction, DispatchGoal +from geometry_msgs.msg import PointStamped + +STEPPER_ELEVATOR_NAME = 'arm_elevator' +DEFAULT_ELEVATOR_TIMEOUT = 1000 + +class ActuatorsCubePicker(ActuatorsAbstract): + def __init__(self): + self._client_arm = actionlib.SimpleActionClient('/movement/actuators/arm', ArmAction) + self._client_arm.wait_for_server() + self._client_act = actionlib.SimpleActionClient('/movement/actuators/dispatch', DispatchAction) + self._client_act.wait_for_server() + #self._srv_cube_picker = rospy.Service("/movement/actuators/pick_cube", CubePicker, self._callback_pick_cube) + ActuatorsAbstract.__init__(self, + action_name='cubePicker', + action_type=CubePickerAction) + self._goal_id = None + + + def _process_action(self, goal, goal_id): + self._goal_id = goal_id + if self._isHalted: + rospy.logerr("CubePicker actuator is turned off!") + return False + + # Get cube center position + try: + cube_p = self._get_cube_pos() + except Exception as e: + rospy.logerr(e) + self._quit_action(goal_id, False) + return False + + self._move_elevator('GO_UP') + # Move arm to the cube center position + self._move_arm(cube_p) + + # Elevator should move now + try: + # Wait for arm to finish movement + self._client_arm.wait_for_result(rospy.Duration(2)) + except rospy.ROSException as e: + rospy.logerr('Erreur : ' + str(e)) + + + self._move_elevator('GO_DOWN') + + #rospy.loginfo('Result : ' + str(self._client_arm.get_result())) + if self._client_arm.get_result().success == False: + rospy.logwarn('Arm cant reach cube') + self._quit_action(goal_id, False) + return False + + rospy.loginfo('Arm positioned on cube') + #Wait for elevator to finish action + + #Pick cube + + self._quit_action(goal_id, True) + return True + + + def _quit_action(self, goal_id, success): + self._action_reached(goal_id, success, CubePickerResult(success=success)) + + def _get_cube_pos(self): + cube_srv = rospy.ServiceProxy('/recognition/localizer/recognition_cube', Cube_localizer) + # rospy.loginfo('--------------------------- CubePicker wait for service') + # try : + # rospy.wait_for_service(cube_srv, 2) + # except rospy.ROSException : + # rospy.loginfo('Cant reach cube_localizer service') + try: + cube_p = cube_srv.call() + except rospy.ServiceException as e: + rospy.logerr("Cube recognition service unavailable") + self._quit_action(self.goal_id, False) + return False + + #rospy.loginfo(cube_p) + if cube_p.position.x == 0 and cube_p.position.y == 0: + raise Exception('Cant find any cube in range') + return cube_p + + def _move_arm(self, cube_p): + goal = ArmGoal() + goal.frame_id = 'arm_origin' # cube_p.frame_id + goal.x = cube_p.position.x + goal.y = cube_p.position.y + self._client_arm.send_goal(goal) + + def _move_elevator(self, direction): + goal_arm_elevator = DispatchGoal() + goal_arm_elevator.id = 0 + goal_arm_elevator.name = STEPPER_ELEVATOR_NAME + goal_arm_elevator.preset = direction + goal_arm_elevator.timeout = DEFAULT_ELEVATOR_TIMEOUT + self._client_act.send_goal(goal_arm_elevator) + diff --git a/ros_ws/src/movement_actuators/src/actuators/actuators_dispatch.py b/ros_ws/src/movement_actuators/src/actuators/actuators_dispatch.py new file mode 100644 index 0000000..b48200c --- /dev/null +++ b/ros_ws/src/movement_actuators/src/actuators/actuators_dispatch.py @@ -0,0 +1,177 @@ +#!/usr/bin/env python + +import rospy +import threading +from functools import partial +from actuators_abstract import * +import actuators_parser +from movement_actuators.msg import DispatchAction, DispatchResult +from drivers_ax12.msg import Ax12CommandGoal, Ax12CommandAction +from actionlib_msgs.msg import GoalStatus +import drivers_ard_others.msg +import drivers_ax12.msg + +__author__ = "Thomas Fuhrmann" +__date__ = 9/04/2018 + + +class ActuatorsDispatch(ActuatorsAbstract): + def __init__(self): + ActuatorsAbstract.__init__(self, "dispatch", DispatchAction) + self._lock = threading.RLock() + # goal_id, order_id for arduino and goal for ax12 + self._active_goals = {} + # goal_id and timer + self._active_goal_timers = {} + # This counter enables to keep a track of orders sent to arduino + self._order_id_counter = 0 + self._act_parser = actuators_parser.ActuatorsParser() + self._pub_ard_move = rospy.Publisher('/drivers/ard_others/move', drivers_ard_others.msg.Move, queue_size=3) + self._sub_ard_response = rospy.Subscriber('/drivers/ard_others/move_response', drivers_ard_others.msg.MoveResponse, self._callback_move_response) + self._act_cli_ax12 = actionlib.ActionClient('/drivers/ax12', Ax12CommandAction) + self._act_cli_ax12.wait_for_server(rospy.Duration(10)) + self._isHalted = False + + def _process_action(self, goal, goal_id): + if self._isHalted: + rospy.logerr("Actuators are turned off!") + return False + to_return = False + actuator_properties = self._act_parser.get_actuator_properties(goal.name, goal.id) + if actuator_properties is None: + return False + param = goal.param + if param == "": + if goal.preset != "": + param = actuator_properties.preset[goal.preset] + else: + rospy.logwarn("Received goal ({}) as no param nor preset, reject the goal.".format(goal_id)) + return False + if actuator_properties.family.lower() == 'arduino': + rospy.loginfo("Received an arduino goal") + result = self._send_to_arduino(actuator_properties.id, actuator_properties.type, param) + if result >= 0: + with self._lock: + self._active_goals[goal_id] = result + to_return = True + elif actuator_properties.family.lower() == 'ax12': + rospy.loginfo("Received an ax12 goal") + goal_handle = self._send_to_ax12(actuator_properties.id, goal.order, param) + if goal_handle is not None: + with self._lock: + self._active_goals[goal_id] = goal_handle + to_return = True + else: + to_return = False + if to_return: + timeout = goal.timeout + if goal.timeout == 0: + timeout = actuator_properties.default_timeout + rospy.logwarn("No timeout given to dispatch goal, use default xml value %d ms" % timeout) + if timeout == 0: + rospy.logerr("Goal {} has no timeout nor default timeout, reject it.".format(goal_id)) + return False + self._active_goal_timers[goal_id] = rospy.Timer(period=rospy.Duration(timeout / 1000.0), + callback=partial(self._timer_callback_timeout, goal_id=goal_id), + oneshot=True) + return to_return + + def _callback_move_response(self, msg): + if msg.order_nb in filter(lambda e: isinstance(e, int), self._active_goals.values()): + received_goal_id = None + for goal_id in self._active_goals.iterkeys(): + if isinstance(self._active_goals[goal_id], int) and self._active_goals[goal_id] == msg.order_nb: + received_goal_id = goal_id + if received_goal_id is not None: + self._goal_reached(received_goal_id, msg.success) + else: + rospy.logwarn('Unknow id received : {}'.format(msg.order_nb)) + + def _callback_ax12_client(self, goal_handle): + received_goal_id = goal_handle.comm_state_machine.action_goal.goal_id.id + if goal_handle in filter(lambda e: not isinstance(e, int), self._active_goals.values()): + for goal_id, goal in self._active_goals.iteritems(): + if not isinstance(goal, int) and goal.comm_state_machine.action_goal.goal_id.id == received_goal_id: + goal_status = goal_handle.get_goal_status() + + if goal_status == GoalStatus.SUCCEEDED: + success = True + elif goal_status in [GoalStatus.LOST, + GoalStatus.RECALLED, + GoalStatus.REJECTED, + GoalStatus.ABORTED, + GoalStatus.PREEMPTED]: + success = False + else: + return + + self._goal_reached(goal_id, success) + return + else: + rospy.logwarn("Unknow ax12 goal received : {}".format(received_goal_id)) + + def _send_to_arduino(self, ard_id, ard_type, param): + msg = drivers_ard_others.msg.Move() + msg.id = ard_id + msg.type = { + 'digital': msg.TYPE_DIGITAL, + 'pwm': msg.TYPE_PWM, + 'servo': msg.TYPE_SERVO, + 'stepper': msg.TYPE_STEPPER + }[ard_type] + msg.dest_value = int(param) + msg.order_nb = self._get_order_id() + self._pub_ard_move.publish(msg) + return msg.order_nb + + def _send_to_ax12(self, motor_id, order, param): + try: + goal_position = int(param) + except ValueError as ex: + rospy.logerr("Try to dispatch an ax12 order with invalid position... Reject it.") + return None + goal = drivers_ax12.msg.Ax12CommandGoal() + goal.motor_id = int(motor_id) + if order.lower() == "joint": + goal.mode = drivers_ax12.msg.Ax12CommandGoal.JOINT + goal.speed = 0 + goal.position = goal_position + elif order.lower() == "wheel": + goal.mode = drivers_ax12.msg.Ax12CommandGoal.WHEEL + goal.speed = goal_position + else: + rospy.logerr("Bad order: {}, expected joint or wheel".format(order)) + return None + return self._act_cli_ax12.send_goal(goal, transition_cb=self._callback_ax12_client) + + def _get_order_id(self): + with self._lock: + current_id = self._order_id_counter + self._order_id_counter += 1 + return current_id + + def _timer_callback_timeout(self, event, goal_id): + rospy.logerr('Timeout triggered for goal %s, cancelling' % goal_id.id) + self._goal_reached(goal_id, False) + + def _goal_reached(self, goal_id, reached): + if goal_id in self._active_goals.keys(): + with self._lock: + # Remove goal from active goals + del self._active_goals[goal_id] + # Remove goal timeout + if goal_id in self._active_goal_timers: + if self._active_goal_timers[goal_id]: + self._active_goal_timers[goal_id].shutdown() + del self._active_goal_timers[goal_id] + self._action_reached(goal_id, reached, DispatchResult(reached)) + + def setHalted(self, isHalted): + self._isHalted = isHalted + if isHalted: + for actuator in self._act_parser._actuators_dictionary.values(): + if "OFF" in actuator.preset.keys(): + if actuator.family == "arduino": + self._send_to_arduino(actuator.id, actuator.type, actuator.preset["OFF"]) + continue + # TODO stop AX12 ? diff --git a/ros_ws/src/movement_actuators/src/actuators/actuators_parser.py b/ros_ws/src/movement_actuators/src/actuators/actuators_parser.py new file mode 100644 index 0000000..2211fe7 --- /dev/null +++ b/ros_ws/src/movement_actuators/src/actuators/actuators_parser.py @@ -0,0 +1,85 @@ +#!/usr/bin/python2.7 +# -*-coding:Utf-8 -* + +import xml.etree.ElementTree as ET +import rospy +from memory_definitions.srv import GetDefinition + +__author__ = "Thomas Fuhrmann", "P. Potiron" +__date__ = 11/04/2018 + +XML_FILE = "movement/actuators.xml" +GET_DEFINITION_SERVICE_NAME = "/memory/definitions/get" +GET_DEFINITION_SERVICE_TIMEOUT = 15 + + +class ActuatorsProperties: + def __init__(self, xml_actuators): + self.id = int(xml_actuators.get('id')) + self.name = xml_actuators.get('name') + self.type = xml_actuators.get('type') + self.family = xml_actuators.get('family') + self.default_timeout = int( xml_actuators.find('timeout').get('value') ) + positions = xml_actuators.find('positions') + self.default_position = positions.get('default') + self.preset = {} + for preset in positions.findall('preset'): + self.preset[preset.get('name')] = int( preset.get('value') ) + + +class ActuatorsParser: + def __init__(self): + self._actuators_dictionary = {} + self.parse_xml_file(self.get_xml_path(XML_FILE)) + + # if no actuator_name, use actuator_id + def get_actuator_properties(self, actuator_name, actuator_id): + if actuator_name == "" and actuator_id < 0: + rospy.logerr("You ask actuators properties but not provide valid name or id...") + return None + actuator_properties = None + if actuator_name != "": + try: + actuator_properties = self._actuators_dictionary[actuator_name] + except KeyError as ex: + rospy.logdebug("Asked for {} properties but no actuator found. Searching by id.") + if actuator_properties is None: + for actuator_element in self._actuators_dictionary.itervalues(): + if actuator_id == actuator_element.id: + actuator_properties = actuator_element + break + return actuator_properties + + def get_xml_path(self, filename): + get_def = rospy.ServiceProxy(GET_DEFINITION_SERVICE_NAME, GetDefinition) + xml_path = "" + try: + get_def.wait_for_service(GET_DEFINITION_SERVICE_TIMEOUT) + res = get_def(filename) + if not res.success: + rospy.logerr("Error when fetching '{}' definition file".format(filename)) + else: + xml_path = res.path + except rospy.exceptions.ROSException as ex: + rospy.logerr("memory_defintion service can not be reached...") + except rospy.ServiceException as exc: + rospy.logerr("Unhandled error while getting def file: {}".format(str(exc))) + return xml_path + + def parse_xml_file(self, filepath): + if filepath != "": + try: + xml_root = ET.parse(filepath).getroot() + for child in xml_root.iter('act'): + self._parse_xml_element(child) + except IOError as err: + rospy.logerr("Actuators xml file does not exists : {}".format(filepath)) + else: + rospy.logerr("The path of file to parse is empty...") + + def _parse_xml_element(self, element): + self._actuators_dictionary[element.get("name")] = ActuatorsProperties(element) + + +if __name__ == '__main__': + ActuatorsParser() diff --git a/ros_ws/src/movement_actuators/src/actuators_node.py b/ros_ws/src/movement_actuators/src/actuators_node.py new file mode 100755 index 0000000..5c23fc4 --- /dev/null +++ b/ros_ws/src/movement_actuators/src/actuators_node.py @@ -0,0 +1,53 @@ +#!/usr/bin/python2.7 +# -*-coding:Utf-8 -* + +import rospy +import actuators +from ai_game_manager import StatusServices +from ai_game_manager.msg import GameStatus + +__author__ = "P. Potiron", "Thomas Fuhrmann" +__date__ = 9/04/2018 + +NODE_NAME = "actuators" + + +class ActuatorsNode: + def __init__(self): + rospy.init_node(NODE_NAME, log_level=rospy.INFO) + self.dispatch_instance = actuators.ActuatorsDispatch() + + self._robot = rospy.get_param('/robot') + if self._robot.lower() == "gr": + self.arm_instance = actuators.ActuatorsArm() + self.cubePicker_instance = actuators.ActuatorsCubePicker() + elif self._robot.lower() == "pr": + self.barrel_instance = actuators.ActuatorsBarrel() + self.canon_instance = actuators.ActuatorsCanon() + + self._isHalted = False + + rospy.loginfo("Movement actuators has correctly started.") + StatusServices("movement", "actuators", None, self._on_gameStatus).ready(True) + rospy.spin() + + def _on_gameStatus(self, msg): + stateChanged = False + if not self._isHalted and msg.game_status == GameStatus.STATUS_HALT: + self._isHalted = True + stateChanged = True + elif self._isHalted and msg.game_status != GameStatus.STATUS_HALT: + self._isHalted = False + stateChanged = True + + if stateChanged: + self.dispatch_instance.setHalted(self._isHalted) + if self._robot.lower() == "gr": + self.arm_instance.setHalted(self._isHalted) + elif self._robot.lower() == "pr": + self.barrel_instance.setHalted(self._isHalted) + self.canon_instance.set_halted(self._isHalted) + + +if __name__ == '__main__': + ActuatorsNode() diff --git a/ros_ws/src/movement_actuators/src/actuators_node_old.py b/ros_ws/src/movement_actuators/src/actuators_node_old.py new file mode 100755 index 0000000..f6c8081 --- /dev/null +++ b/ros_ws/src/movement_actuators/src/actuators_node_old.py @@ -0,0 +1,180 @@ +#!/usr/bin/python2.7 +# -*-coding:Utf-8 -* + +import rospy +import time +import random +import threading +import actionlib +import movement_actuators.msg +from src.movement_actuators.src.actuators import actuators_properties +import drivers_ard_others.msg +import drivers_ax12.msg +from ai_game_manager import StatusServices +from ai_game_manager.msg import GameStatus + + +def current_milli_time(): return int(round(time.time() * 1000)) + +class ActuatorsNode(): + """Dispatch commands from AI to the correct node""" + + _result = movement_actuators.msg.dispatchResult() + + def __init__(self): + self.is_halted = False + + rospy.init_node('actuators') + self._namespace = '/movement/actuators/' + self._action_name = '{}dispatch'.format(self._namespace) + self._lock = threading.RLock() + self._call_stack = {} + self._action_server = actionlib.SimpleActionServer( self._action_name, movement_actuators.msg.dispatchAction, execute_cb=self.dispatch, auto_start=False) + self._arduino_move = rospy.Publisher( '/drivers/ard_others/move', drivers_ard_others.msg.Move, queue_size=30) # TODO check the queue_size + self._arduino_response = rospy.Subscriber( '/drivers/ard_others/move_response', drivers_ard_others.msg.MoveResponse, self.ard_callback) + self._ax12_client = actionlib.SimpleActionClient('/drivers/ax12', drivers_ax12.msg.Ax12CommandAction) + self._action_server.start() + + + # Tell ai/game_manager the node initialized successfuly. + StatusServices("movement", "actuators", None, self.game_status_callback).ready(True) + + def dispatch(self, command): + + if self.is_halted: + rospy.logerr("Received dispatch command but game_status is halted") + self._action_server.set_aborted(False) + return + + #-----Actuator check + try: + actuator = actuators_properties.getActuatorsList()[command.name] + except KeyError as identifier: + rospy.logwarn('The action dispatch should be called with a name instead of an id.') + for actuator in actuators_properties.getActuatorsList().values(): + if actuator.id == command.id: + break + actuator = None + if actuator == None: + self._action_server.set_succeeded(False) + rospy.logerr('Couldn\'t find actuators with name "' + + command.name + '" or id "' + str(command.id)+'".') + return + #-----Param check + param = None + if command.param == '': + try: + param = actuator.preset[command.preset] + except KeyError as identifier: + self._action_server.set_succeeded(False) + rospy.logerr('Couldn\'t find preset with name "' + + command.preset + '" in actuator "' + actuator.name + '"') + return + else: + param = int(command.param) #TODO check param + #-----Timeout check + timeout = None + if command.timeout<=0: + timeout = actuator.default_timeout + else: + timeout = command.timeout + #-----Time to send ! + if actuator.family.lower() == 'arduino': + self._result.success = self.sendToArduino( actuator.id, actuator.type, command.order, param, timeout ) + self._action_server.set_succeeded(self._result) + return + elif actuator.family.lower() == 'ax12': + self._result.success = self.sendToAx12( actuator.id, command.order, param, timeout) + self._action_server.set_succeeded(self._result) + return + + self._action_server.set_succeeded(False) + + def sendToArduino(self, ard_id, ard_type, order, param, timeout): + msg = drivers_ard_others.msg.Move() + msg.id = ard_id + msg.type = { + 'digital': msg.TYPE_DIGITAL, + 'pwm': msg.TYPE_PWM, + 'servo': msg.TYPE_SERVO + }[ard_type] + #Mode is not implemented yet: + #msg.mode = order + msg.dest_value = param + + event = threading.Event() + event.clear() + msg.order_nb = self.generateId(event) + + self._arduino_move.publish(msg) + ts = current_milli_time() + event.wait(timeout / 1000.0) + ts = current_milli_time() - ts + if ts >= timeout: + rospy.loginfo('Timeout reached') + success = False + if type(self._call_stack[msg.order_nb])==bool : + success = self._call_stack[msg.order_nb] + with self._lock: + del self._call_stack[msg.order_nb] + return success + + def sendToAx12(self, id, order, param, timeout): + goal = drivers_ax12.msg.Ax12CommandGoal() + goal.motor_id = int(id) + if order.lower() == "joint": + goal.mode = drivers_ax12.msg.Ax12CommandGoal.JOINT + goal.speed = 0 + goal.position = int(param) + elif order.lower() == "wheel": + goal.mode = drivers_ax12.msg.Ax12CommandGoal.WHEEL + goal.speed = int(param) + else: + rospy.logerr("Bad order: {}, expected joint or wheel".format(order)) + return False + + self._ax12_client.send_goal(goal) + if self._ax12_client.wait_for_result(rospy.Duration(int(timeout))): + success = self._ax12_client.get_result().success + else: + rospy.loginfo('Timeout reached') + success = False + + return success + + def generateId(self, event): + with self._lock: + ard_id = random.randint(1, 10000) + while ard_id in self._call_stack: + ard_id = random.randint(1, 10000) + self._call_stack[ard_id] = event + return ard_id + + def ard_callback(self, msg): + with self._lock: + if msg.order_nb in self._call_stack: + event = self._call_stack[msg.order_nb] + self._call_stack[msg.order_nb] = msg.success + event.set() + else: + rospy.logwarn('Unknow id received : {}'.format(msg.order_nb)) + + def game_status_callback(self, msg): + if not self.is_halted and msg.game_status == GameStatus.STATUS_HALT: + self.is_halted = True + for actuator in actuators_properties.getActuatorsList().values(): + try: + param = actuator.preset["OFF"] + self.sendToArduino( actuator.id, actuator.type, actuator.type, param, actuator.default_timeout ) + rospy.loginfo("Turned off " + actuator.name) + except Exception: + rospy.logwarn("Can't turn off an actuator!") + + elif self.is_halted and msg.game_status != GameStatus.STATUS_HALT: + self.is_halted = False + +if __name__ == '__main__': + actuators_properties.initActuatorsList() + actuators_node = ActuatorsNode() + rospy.loginfo('Actuators node started') + rospy.spin() diff --git a/ros_ws/src/movement_actuators/srv/ActivateCanon.srv b/ros_ws/src/movement_actuators/srv/ActivateCanon.srv new file mode 100644 index 0000000..5bbfb70 --- /dev/null +++ b/ros_ws/src/movement_actuators/srv/ActivateCanon.srv @@ -0,0 +1,10 @@ +# Service to activate the canon on PR +# This will start the canon wheel and activate the flipper servo motors. +# This service does not activate the barrel. + +# If value = -1, deactivate the canon. +# If value = 0, let the server determine the fire distance. +# If not, value has to contain the fire distance, in meters. +float32 fire_distance +--- +bool success diff --git a/ros_ws/src/navigation_collisions/CMakeLists.txt b/ros_ws/src/navigation_collisions/CMakeLists.txt new file mode 100644 index 0000000..a6ed96d --- /dev/null +++ b/ros_ws/src/navigation_collisions/CMakeLists.txt @@ -0,0 +1,78 @@ +cmake_minimum_required(VERSION 2.8.3) +project(navigation_collisions) + +## Find catkin macros and libraries +find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs + geometry_msgs + memory_map + message_generation +) + +############## +## Messages ## +############## + +# Generate messages +add_message_files( + FILES + PredictedCollision.msg +) + +# Generate services +add_service_files( + FILES + ActivateCollisions.srv +) + +# Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs + geometry_msgs +) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + + +################################### +## catkin specific configuration ## +################################### + +catkin_package( + CATKIN_DEPENDS rospy std_msgs message_runtime +) + +########### +## Build ## +########### + +include_directories( + ${catkin_INCLUDE_DIRS} +) + +############# +## Install ## +############# + +catkin_install_python( + PROGRAMS + src/collisions_node.py + src/collisions_robot.py + src/collisions_subscriptions.py + src/obstacles_stack.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install( + DIRECTORY src/collisions_engine src/markers_publisher + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + PATTERN "*.pyc" EXCLUDE +) + +############# +## Testing ## +############# diff --git a/ros_ws/src/navigation_collisions/README.md b/ros_ws/src/navigation_collisions/README.md new file mode 100644 index 0000000..94e549b --- /dev/null +++ b/ros_ws/src/navigation_collisions/README.md @@ -0,0 +1,10 @@ +Inputs : + +- robot's current position and speed *from drivers/ard_asserv or navigation/navigator* +- robot's state (driving, stopped, etc) and current path (remaining waypoints to pass through *from memory/map or navigation/navigator* +- belt dangerous objects and static objects *from processing/qmsk* +- enemies positions *from recognition/enemy_finder* + +Outputs : + +- publishes on a topic when a collision is predicted. navigator will subscribe to it and stop the robot when this message is published. diff --git a/ros_ws/src/navigation_collisions/config.rviz b/ros_ws/src/navigation_collisions/config.rviz new file mode 100644 index 0000000..bd75883 --- /dev/null +++ b/ros_ws/src/navigation_collisions/config.rviz @@ -0,0 +1,191 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Grid1/Offset1 + - /World Markers1 + - /World Markers1/Namespaces1 + - /Navigation Markers1 + - /Navigation Markers1/Namespaces1 + - /LaserScan1/Status1 + Splitter Ratio: 0.517756999 + Tree Height: 1057 + - 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: + - Class: rviz/Axes + Enabled: false + Length: 0.100000001 + Name: Global Axis + Radius: 0.00999999978 + Reference Frame: map + Value: false + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + map: + Value: true + robot: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + robot: + {} + Update Interval: 0 + Value: true + - Alpha: 0.5 + Cell Size: 0.100000001 + Class: rviz/Grid + Color: 255; 0; 0 + Enabled: false + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 1.5 + Y: 1 + Z: 0 + Plane: XY + Plane Cell Count: 30 + Reference Frame: map + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /visualization_markers/world + Name: World Markers + Namespaces: + cubes: true + zones: false + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /visualization_markers/navigation + Name: Navigation Markers + Namespaces: + collisions_obstacles: true + collisions_path: true + Queue Size: 100 + 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; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.00999999978 + Style: Points + Topic: /scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + 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/Orbit + Distance: 4.3868618 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 1.43944943 + Y: 1.2554481 + Z: -0.457502961 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.79479748 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.27862597 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1343 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000022c0000046dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004b0000046d000000e200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000020e0000048ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004f0000048f000000c000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009e60000005bfc0100000002fb0000000800540069006d00650100000000000009e60000036b00fffffffb0000000800540069006d00650100000000000004500000000000000000000007b20000046d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 2534 + X: 10 + Y: 84 diff --git a/ros_ws/src/navigation_collisions/msg/PredictedCollision.msg b/ros_ws/src/navigation_collisions/msg/PredictedCollision.msg new file mode 100644 index 0000000..427e4c4 --- /dev/null +++ b/ros_ws/src/navigation_collisions/msg/PredictedCollision.msg @@ -0,0 +1,20 @@ +# Collision context +uint8 LEVEL_STOP = 0 # Collision will happen very soon, stop the robot immediately! +uint8 LEVEL_DANGER = 1 # Path needs to be modified (while driving is fine) or a collision will happen. +uint8 LEVEL_POTENTIAL = 2 # A dynamic object will probably pass through at some time, maybe not (replan optional). +uint8 danger_level + +# float32 distance_left # Gives how far away the collision is (supposing the robot will follow the current path). +# float32 time_left # Gives how much time's left before hitting the obstacle (predicted based on the current speed). + +# Collision obstacle description +uint8 TYPE_POINT = 0 +uint8 TYPE_RECT = 1 +uint8 TYPE_CIRCLE = 2 +uint8 obstacle_type + +geometry_msgs/Pose2D obstacle_pos # For all obstacle types. + +float32 obstacle_width # For Rects. +float32 obstacle_height # For Rects. +float32 obstacle_radius # For Circles. \ No newline at end of file diff --git a/ros_ws/src/navigation_collisions/package.xml b/ros_ws/src/navigation_collisions/package.xml new file mode 100644 index 0000000..7d5e842 --- /dev/null +++ b/ros_ws/src/navigation_collisions/package.xml @@ -0,0 +1,16 @@ + + + navigation_collisions + 0.0.0 + The navigation_collisions package + GLPv3 + MadeInPierre + UTCoupe + catkin + message_generation + rospy + std_msgs + message_runtime + + + diff --git a/ros_ws/src/navigation_collisions/src/collisions_engine/__init__.py b/ros_ws/src/navigation_collisions/src/collisions_engine/__init__.py new file mode 100644 index 0000000..0ac90e0 --- /dev/null +++ b/ros_ws/src/navigation_collisions/src/collisions_engine/__init__.py @@ -0,0 +1,4 @@ +from engine_shapes import SegmentObstacle, RectObstacle, CircleObstacle +from engine_shapes_attrib import Point, Position +from engine_check_zone import VelocityCheckZone, Velocity, PathCheckZone +from engine_constants import CollisionLevel \ No newline at end of file diff --git a/ros_ws/src/navigation_collisions/src/collisions_engine/engine.py b/ros_ws/src/navigation_collisions/src/collisions_engine/engine.py new file mode 100644 index 0000000..e55df36 --- /dev/null +++ b/ros_ws/src/navigation_collisions/src/collisions_engine/engine.py @@ -0,0 +1,112 @@ +#!/usr/bin/python +import math +import rospy + +from engine_constants import CollisionLevel +from engine_shapes import Position, RectObstacle, CircleObstacle + + +class Collision(object): + def __init__(self, collision_level, obstacle, approx_distance): + self.level = collision_level + self.obstacle = obstacle + self.approx_distance = approx_distance # Distance between the centers of the robot and obstacle (poor precision) + + +class CollisionsResolver(object): + @staticmethod + def find_collisions(robot_shapes, obstacles_shapes): + collisions = [] + for robot_shape in robot_shapes: + for obstacle_shape in obstacles_shapes: + intersecting = False + if CollisionsResolver.intersect(robot_shape, obstacle_shape): + collisions.append(obstacle_shape) + intersecting = True + + if obstacle_shape.velocity is not None and not intersecting: # if the obstacle has a velocity, check its velocity zone too. + for vel_shape in obstacle_shape.velocity.get_shapes(obstacle_shape.position): + if CollisionsResolver.intersect(robot_shape, vel_shape): + collisions.append(vel_shape) + return collisions + + @staticmethod + def intersect(obs1, obs2): + def _segments_intersect(segment1, segment2): + s1, s2 = (segment1.first, segment1.last), (segment2.first, segment2.last) + # https://stackoverflow.com/questions/3838329/how-can-i-check-if-two-segments-intersect + ccw = lambda a, b, c: (c.y-a.y) * (b.x-a.x) > (b.y-a.y) * (c.x-a.x) + return ccw(s1[0],s2[0],s2[1]) != ccw(s1[1],s2[0],s2[1]) and ccw(s1[0],s1[1],s2[0]) != ccw(s1[0],s1[1],s2[1]) + + def _rects_intersect(rect1, rect2): + if _point_in_rect(rect1.position, rect2) or _point_in_rect(rect2.position, rect1): + return True # Checks if a rect is completely inside the other one. + for s1 in rect1.segments(): # If not, check if segments intersect. + for s2 in rect2.segments(): + if _segments_intersect(s1, s2): + return True + return False + + def _segment_intersects_rect(segment, rect): + if _point_in_rect(segment.position, rect): + return True # Checks if the segment center is inside the rect. + for rect_s in rect.segments(): # If not, check if segments intersect. + if _segments_intersect(segment, rect_s): + return True + return False + + def _segment_intersects_circle(segment, circle): + new_rect = RectObstacle(segment.position, circle.radius * 2, segment.length + 2 * circle.radius) + return _rect_intersects_circle(new_rect, circle) + + def _point_in_rect(point, rect): # Calculs persos #PS22 + phi = math.atan2(point.y - rect.position.y, point.x - rect.position.x) + phi += 2 * math.pi if phi < 0 else 0 + a = rect.position.a #if rect.Position.a > 0 else rect.Position.a + 2 * math.pi + d = math.sqrt((point.x - rect.position.x) ** 2 + (point.y - rect.position.y) ** 2) + local_point = (d * math.cos(phi - a), d * math.sin(phi - a)) + return - rect.width / 2.0 <= local_point[0] <= rect.width / 2.0 and - rect.height / 2.0 <= local_point[1] <= rect.height / 2.0 + + def _point_in_circle(point, circle): + d = math.sqrt((point.x - circle.position.x) ** 2 + (point.y - circle.position.y) ** 2) + return d <= circle.radius + + def _rect_intersects_circle(rect, circle): + new_rect = RectObstacle(rect.position, rect.width + circle.radius * 2, rect.height + circle.radius * 2) + return _point_in_rect(circle.position, new_rect) + + def _circles_intersect(circle1, circle2): + d = math.sqrt((circle2.position.x - circle1.position.x) ** 2 + (circle2.position.y - circle1.position.y) ** 2) + return d <= circle1.radius + circle2.radius + + types = (str(obs1), str(obs2)) + if types[0] == 'rect' and types[1] == 'rect': + return _rects_intersect(obs1, obs2) + elif types[0] == 'circle' and types[1] == 'circle': + return _circles_intersect(obs1, obs2) + elif types[0] == 'segment' and types[1] == 'segment': + return _segments_intersect(obs1, obs2) + elif types[0] == 'point' and types[1] == 'point': + return False + elif "rect" in types and "circle" in types: + if types[0] == "rect": + return _rect_intersects_circle(obs1, obs2) + return _rect_intersects_circle(obs2, obs1) + elif "segment" in types and "rect" in types: + if types[0] == "segment": + return _segment_intersects_rect(obs1, obs2) + return _segment_intersects_rect(obs2, obs1) + elif "segment" in types and "circle" in types: + if types[0] == "segment": + return _segment_intersects_circle(obs1, obs2) + return _segment_intersects_circle(obs2, obs1) + elif "rect" in types and "point" in types: + if types[0] == "rect": + return _point_in_rect(obs1, obs2) + return _rect_intersects_circle(obs2, obs1) + elif "circle" in types and "point" in types: + if types[0] == "point": + return _point_in_rect(obs1, obs2) + return _rect_intersects_circle(obs2, obs1) + else: + rospy.logerr("Collisions couldn't check collision between objects of type '{}' and '{}'.".format(types[0], types[1])) diff --git a/ros_ws/src/navigation_collisions/src/collisions_engine/engine_check_zone.py b/ros_ws/src/navigation_collisions/src/collisions_engine/engine_check_zone.py new file mode 100644 index 0000000..8f05d96 --- /dev/null +++ b/ros_ws/src/navigation_collisions/src/collisions_engine/engine_check_zone.py @@ -0,0 +1,108 @@ +import math +import rospy +from engine_constants import CollisionThresholds +from engine_shapes import RectObstacle, CircleObstacle +from engine_shapes_attrib import Position +from engine import Collision, CollisionLevel, CollisionsResolver + + +class CheckZone(object): + def __init__(self, width, height, collision_level): + self._width = width + self._height = height + self.collision_level = collision_level + + def get_shapes(self, robot_pos): + return [] + + def check_collisions(self, robot_pos, obstacles): + raise NotImplementedError("Must be overwritten.") + + +class VelocityCheckZone(CheckZone): + def __init__(self, width, height, collision_level): + super(VelocityCheckZone, self).__init__(width, height, collision_level) + + def get_shapes(self, robot_pos, vel_linear, vel_angular, max_dist = -1): + if abs(vel_linear) < CollisionThresholds.VEL_MIN: # if the object isn't moving fast enough, don't create the rect. + return [] + + expansion_dist = CollisionThresholds.get_stop_distance(vel_linear) + if max_dist != -1: + expansion_dist = min(expansion_dist, max_dist) # If set, reduce the expansion to the provided limit. + w, h = self._height + expansion_dist, self._width + l = w / 2.0 - self._height / 2.0 + + side_a = math.pi if vel_linear < 0 else 0 + return [RectObstacle(Position(robot_pos.x + l * math.cos(robot_pos.a + side_a), + robot_pos.y + l * math.sin(robot_pos.a + side_a), + robot_pos.a), w, h)] + + def check_collisions(self, robot_pos, vel_linear, vel_angular, obstacles): + collisions = [] + for o in CollisionsResolver.find_collisions(self.get_shapes(robot_pos, vel_linear, vel_angular), obstacles): + approx_d = math.sqrt((robot_pos.x - o.position.x) ** 2 + \ + (robot_pos.y - o.position.y) ** 2) # Very approximate distance + collisions.append(Collision(CollisionLevel.LEVEL_STOP, o, approx_d)) + return collisions + + +class Velocity(object): + def __init__(self, width, height, linear = 0.0, angular = 0.0): + self.linear = linear + self.angular = angular + self._check_zone = VelocityCheckZone(width, height, CollisionLevel.LEVEL_STOP) + + def get_shapes(self, object_pos, max_dist = -1): + return self._check_zone.get_shapes(object_pos, self.linear, self.angular, max_dist) + + def check_collisions(self, object_pos, obstacles): # Used only for the robot itself, not obstacles. + return self._check_zone.check_collisions(object_pos, self.linear, self.angular, obstacles) + + +class PathCheckZone(CheckZone): + def __init__(self, width, height, collision_level): + super(PathCheckZone, self).__init__(width, height, collision_level) + self.waypoints = [] + + def update_waypoints(self, new_waypoints): + if isinstance(new_waypoints, list) and len(new_waypoints) > 0: + self.waypoints = new_waypoints + else: + rospy.logerr("Trying to update the robot path with an invalid variable type.") + + def _get_full_waypoints(self, robot_pos): + return [robot_pos] + self.waypoints + + def get_shapes(self, robot_pos): + if len(self.waypoints) >= 1: + shapes = [] + path = self._get_full_waypoints(robot_pos) + for i in range(1, len(path)): + # Creating a rectangle with the robot's width between each waypoint + p_w = robot_pos if i == 0 else path[i - 1] + w = path[i] + + if p_w.x != w.x and p_w.y != w.y: + d = math.sqrt( (w.x - p_w.x) ** 2 + (w.y - p_w.y) ** 2) + angle = math.atan((w.y - p_w.y) / (w.x - p_w.x)) + pos = Position((w.x + p_w.x) / 2.0, (w.y + p_w.y) / 2.0, angle = angle) + + shapes.append(RectObstacle(pos, d, self._width)) + if i == len(path) - 1: + shapes.append(RectObstacle(Position(w.x, w.y, angle), self._height, self._width)) + else: + r = math.sqrt(self._width ** 2 + self._height ** 2) / 2.0 + shapes.append(CircleObstacle(Position(w.x, w.y), r)) + return shapes + else: + return [] + + def check_collisions(self, robot_pos, obstacles): + collisions = [] + for o in CollisionsResolver.find_collisions(self.get_shapes(robot_pos), obstacles): + approx_d = math.sqrt((robot_pos.x - o.position.x) ** 2 + \ + (robot_pos.y - o.position.y) ** 2) # Very approximate distance + collisions.append(Collision(CollisionLevel.LEVEL_DANGER if approx_d < CollisionThresholds.DANGER_RADIUS else CollisionLevel.LEVEL_POTENTIAL, + o, approx_d)) + return collisions diff --git a/ros_ws/src/navigation_collisions/src/collisions_engine/engine_constants.py b/ros_ws/src/navigation_collisions/src/collisions_engine/engine_constants.py new file mode 100644 index 0000000..d263afa --- /dev/null +++ b/ros_ws/src/navigation_collisions/src/collisions_engine/engine_constants.py @@ -0,0 +1,22 @@ +class CollisionLevel(object): + LEVEL_STOP = 0 + LEVEL_DANGER = 1 + LEVEL_POTENTIAL = 2 + +class CollisionType(object): + TYPE_STATIC = 0 + TYPE_DYNAMIC = 1 + +class CollisionThresholds(object): + VEL_MIN = 0.05 # minimum linear speed (m/s) before creating a stop rect. + STOP_MIN = 0.1 # minimum distance when linear_speed != 0. + STOP_GAIN = 0.6 # distance in meters when the robot is at 1 m/sec (linear coefficient). + STOP_MAX = 1.0 # maximum distance when linear_speed != 0. + + DANGER_RADIUS = 0.8 # radius around the robot where obstacles are considered at LEVEL_DANGER vs. LEVEL_POTENTIAL. + + @staticmethod + def get_stop_distance(linear_speed): + if not linear_speed: + return 0.0 # If robot stopped, don't create stop rect distance. + return sorted((CollisionThresholds.STOP_MIN, CollisionThresholds.STOP_GAIN * linear_speed, CollisionThresholds.STOP_MAX))[1] # clamp value diff --git a/ros_ws/src/navigation_collisions/src/collisions_engine/engine_shapes.py b/ros_ws/src/navigation_collisions/src/collisions_engine/engine_shapes.py new file mode 100644 index 0000000..240684e --- /dev/null +++ b/ros_ws/src/navigation_collisions/src/collisions_engine/engine_shapes.py @@ -0,0 +1,60 @@ +import math +import time + +from engine_shapes_attrib import Point, Position + +class MapObstacle(object): + def __init__(self, position, velocity = None): + self.position = position + self.velocity = velocity + self.spawn_time = time.time() + + +class SegmentObstacle(MapObstacle): + def __init__(self, first_point, last_point, velocity = None): + self.first = first_point + self.last = last_point + self.length = math.sqrt((self.last.x - self.first.x) ** 2 + (self.last.y - self.first.y) ** 2) + center_pos = Position((self.first.x + self.last.x) / 2.0, + (self.first.y + self.last.y) / 2.0, + math.atan2(self.last.y - self.first.y, self.last.x - self.first.x)) + super(SegmentObstacle, self).__init__(center_pos, velocity) + + def segment(self): + return (self.first, self.last) + + def __repr__(self): + return "segment" + + +class CircleObstacle(MapObstacle): + def __init__(self, position, radius, velocity = None): + super(CircleObstacle, self).__init__(position, velocity) + self.radius = radius + + def __repr__(self): + return "circle" + + +class RectObstacle(MapObstacle): + def __init__(self, position, width, height, velocity = None): + super(RectObstacle, self).__init__(position, velocity) + self.width = width + self.height = height + + def corners(self): # Calculs persos, a verifier DEPRECATED + corners = [] + l = math.sqrt((self.width / 2.0) ** 2 + (self.height / 2.0) ** 2) + corner_phi = math.atan2(self.height, self.width) + for angle_phi in [0, math.pi]: + for i in range(2): + phi = angle_phi + ((-1) ** (i + 1)) * corner_phi + corners.append(Point(self.position.x + l * math.cos(phi + self.position.a), self.position.y + l * math.sin(phi + self.position.a))) + return corners + + def segments(self): + c = self.corners() + return [SegmentObstacle(c[i], c[(i + 1) % 4]) for i in range(len(c))] + + def __repr__(self): + return "rect" diff --git a/ros_ws/src/navigation_collisions/src/collisions_engine/engine_shapes_attrib.py b/ros_ws/src/navigation_collisions/src/collisions_engine/engine_shapes_attrib.py new file mode 100644 index 0000000..686dc43 --- /dev/null +++ b/ros_ws/src/navigation_collisions/src/collisions_engine/engine_shapes_attrib.py @@ -0,0 +1,16 @@ +class Point(object): + def __init__(self, x, y): + self.x = x + self.y = y + + def tuple2(self): + return (self.x, self.y) + + +class Position(Point): + def __init__(self, x, y, angle = 0.0): + super(Position, self).__init__(x, y) + self.a = angle + + def tuple3(self): + return (self.x, self.y, self.a) diff --git a/ros_ws/src/navigation_collisions/src/collisions_node.py b/ros_ws/src/navigation_collisions/src/collisions_node.py new file mode 100755 index 0000000..587bcbe --- /dev/null +++ b/ros_ws/src/navigation_collisions/src/collisions_node.py @@ -0,0 +1,82 @@ +#!/usr/bin/python +import rospy +from collisions_subscriptions import CollisionsSubscriptions +from collisions_engine import CollisionLevel, RectObstacle, CircleObstacle, Position +from obstacles_stack import Map, ObstaclesStack +from markers_publisher import MarkersPublisher + +from geometry_msgs.msg import Pose2D +from navigation_collisions.msg import PredictedCollision +from navigation_collisions.srv import ActivateCollisions, ActivateCollisionsResponse + +from ai_game_manager import StatusServices + + +class CollisionsNode(): + def __init__(self): + rospy.init_node("collisions", log_level=rospy.INFO) + self.active = False # navigation/navigator activates this node through a service. + + self.subscriptions = CollisionsSubscriptions() + Map.Robot = self.subscriptions.create_robot() + + rospy.Service("/navigation/collisions/set_active", ActivateCollisions, self.on_set_active) + self.pub = rospy.Publisher("/navigation/collisions/warner", PredictedCollision, queue_size=1) + + self.markers = MarkersPublisher() + self.subscriptions.send_init() + rospy.loginfo("navigation/collisions ready, waiting for activation.") + + self.run() + + def run(self): + r = rospy.Rate(20) + while not rospy.is_shutdown(): + #ObstaclesStack.updateBeltPoints([RectObstacle(Position(1.5, 0.5, 0.2), 0.3, 0.15)]) +# startTime = rospy.Time.now() + self.subscriptions.update_robot() + if self.active: + for c in Map.Robot.check_collisions(ObstaclesStack.toList()): + self.publish_collision(c) + self.markers.publishCheckZones(Map.Robot) + + self.markers.publishObstacles(ObstaclesStack.toList()) + + ObstaclesStack.garbageCollect() + +# duration = rospy.Time.now() - startTime +# if rospy.Time.now() - lastPub > rospy.Duration(1): +# lastPub = rospy.Time.now() +# rospy.loginfo("check done in " + str(duration.to_sec())) + + r.sleep() + + def publish_collision(self, collision): + m = PredictedCollision() + m.danger_level = collision.level + + if collision.level == CollisionLevel.LEVEL_STOP: + rospy.logwarn_throttle(0.2, "[COLLISION] Found freaking close collision, please stop !!") + elif collision.level == CollisionLevel.LEVEL_DANGER: + rospy.logwarn_throttle(0.5, "[COLLISION] Found close collision intersecting with the path.") + elif collision.level == CollisionLevel.LEVEL_POTENTIAL: + rospy.loginfo_throttle(1, "[COLLISION] Found far-off collision intersecting with the path.") + + obs = collision.obstacle + m.obstacle_pos = Pose2D(obs.position.x, obs.position.y, obs.position.a) + if isinstance(obs, RectObstacle): + m.obstacle_type = m.TYPE_RECT + m.obstacle_width, m.obstacle_height = obs.width, obs.height + elif isinstance(obs, CircleObstacle): + m.obstacle_type = m.TYPE_CIRCLE + m.obstacle_radius = obs.radius + + self.pub.publish(m) + + def on_set_active(self, msg): + self.active = msg.active + rospy.loginfo("{} collisions check.{}".format("Starting" if self.active else "Stopping", ".." if self.active else "")) + return ActivateCollisionsResponse(True) + +if __name__ == "__main__": + CollisionsNode() diff --git a/ros_ws/src/navigation_collisions/src/collisions_robot.py b/ros_ws/src/navigation_collisions/src/collisions_robot.py new file mode 100644 index 0000000..9f4ad24 --- /dev/null +++ b/ros_ws/src/navigation_collisions/src/collisions_robot.py @@ -0,0 +1,51 @@ +import math +from collisions_engine import Position, Velocity, CollisionLevel, PathCheckZone + + +class NavStatus(object): + STATUS_IDLE = 0 + STATUS_NAVIGATING = 1 + + +class Robot(object): #TODO Inherit from RectObstacle + def __init__(self, width, height): + self.width = width + self.height = height + self._position = Position(0, 0) + self._velocity = Velocity(self.width, self.height) + self._nav_status = NavStatus.STATUS_IDLE + + self._path_check_zone = PathCheckZone(self.width, self.height, CollisionLevel.LEVEL_DANGER) + + def update_position(self, tuple3): + self._position.x = tuple3[0] + self._position.y = tuple3[1] + self._position.a = tuple3[2] + + def update_velocity(self, linear, angular): + self._velocity.linear = linear + self._velocity.angular = angular + + def update_status(self, new_status): + self._nav_status = new_status + + def update_waypoints(self, new_waypoints): + self._path_check_zone.update_waypoints(new_waypoints) + + def get_main_shapes(self): + return self._velocity.get_shapes(self._position, self._get_max_main_dist()) + + def get_path_shapes(self): + return self._path_check_zone.get_shapes(self._position) + + def check_collisions(self, obstacles): + return self._velocity.check_collisions(self._position, obstacles) + \ + self._path_check_zone.check_collisions(self._position, obstacles) + # TODO remove duplicate collisions between the two + + def _get_max_main_dist(self): + if isinstance(self._path_check_zone.waypoints, list) and len(self._path_check_zone.waypoints) > 0: + w = self._path_check_zone.waypoints[0] + return math.sqrt((w.x - self._position.x) ** 2 + (w.y - self._position.y) ** 2) + else: + return -1 # invalid waypoints diff --git a/ros_ws/src/navigation_collisions/src/collisions_subscriptions.py b/ros_ws/src/navigation_collisions/src/collisions_subscriptions.py new file mode 100644 index 0000000..952f77d --- /dev/null +++ b/ros_ws/src/navigation_collisions/src/collisions_subscriptions.py @@ -0,0 +1,136 @@ +import math, json +import rospy +import tf2_ros, tf + +from obstacles_stack import Map, ObstaclesStack +from collisions_robot import Robot +from collisions_engine import Point, Position, Velocity, SegmentObstacle, RectObstacle, CircleObstacle + +from memory_map.srv import MapGet +from navigation_navigator.msg import Status +from drivers_ard_asserv.msg import RobotSpeed +from recognition_objects_classifier.msg import ClassifiedObjects +from ai_game_manager import StatusServices + + +class CollisionsSubscriptions(object): + def __init__(self): + # Preparing to get the robot's position, belt frame_id transform. + self._tf2_pos_buffer = tf2_ros.Buffer(cache_time=rospy.Duration(5.0)) + self._tf2_pos_listener = tf2_ros.TransformListener(self._tf2_pos_buffer) + self._transform_listener = tf.TransformListener() + + # Callback buffers + self._nav_status = 0 #STATUS_IDLE + self._robot_path_waypoints = [] + self._vel_linear = 0.0 + self._vel_angular = 0.0 + + # Subscribing to dependencies + rospy.Subscriber("/navigation/navigator/status", Status, self._on_nav_status) + rospy.Subscriber("/recognition/objects_classifier/objects", ClassifiedObjects, self._on_classifier) + rospy.Subscriber("/drivers/ard_asserv/speed", RobotSpeed, self.on_robot_speed) + + self.game_status = StatusServices("navigation", "collisions", None, self._on_game_status) + + def send_init(self, success = True): + self.game_status.ready(success) + + def create_robot(self): + try: # Getting the robot shape and creating the robot instance + robot_type = rospy.get_param("/robot").lower() + map_get_client = rospy.ServiceProxy("/memory/map/get", MapGet) + map_get_client.wait_for_service(2.0) + shape = json.loads(map_get_client("/entities/" + robot_type + "/shape/*").response) + if not shape["type"] == "rect": + raise ValueError("Robot shape type not supported here.") + except Exception as e: + rospy.logerr("ERROR Collisions couldn't get the robot's shape from map : " + str(e)) + shape = {"width": 0.4, "height": 0.25} + return Robot(shape["width"], shape["height"]) # Can create a rect or circle + + def update_robot(self): + new_pos = self._update_robot_pos() + if new_pos is not None: + Map.Robot.update_position(new_pos) + + if self._nav_status is not None: + Map.Robot.update_status(self._nav_status) + if self._robot_path_waypoints is not None and len(self._robot_path_waypoints) > 0: + Map.Robot.update_waypoints(self._robot_path_waypoints) + + Map.Robot.update_velocity(self._vel_linear, self._vel_angular) + + def _update_robot_pos(self): + try: + t = self._tf2_pos_buffer.lookup_transform("map", "robot", rospy.Time()) + tx, ty = t.transform.translation.x, t.transform.translation.y + rz = self._quaternion_to_euler_angle(t.transform.rotation)[2] + return (tx, ty, rz) + except Exception as e: + return None + + def _on_game_status(self, msg): + pass + + def _on_nav_status(self, msg): + self._nav_status = msg.status + self._robot_path_waypoints = [Point(point.x, point.y) for point in msg.currentPath] + + def _on_classifier(self, msg): + new_belt = [] + for rect in msg.unknown_rects: + if rect.header.frame_id not in ["map", "/map"]: + rospy.logwarn("Belt rect not in /map tf frame, skipping.") + continue + + new_belt.append(RectObstacle(Position(rect.x, rect.y,\ + rect.a),\ + rect.w, rect.h)) + + if len(new_belt) > 0: + ObstaclesStack.updateBeltPoints(new_belt) + + + new_lidar = [] + + for segment in msg.unknown_segments: + if segment.header.frame_id not in ["map", "/map"]: + rospy.logwarn("Lidar segment not in /map tf frame, skipping.") + continue + + new_lidar.append(SegmentObstacle(Position(segment.segment.first_point.x, segment.segment.first_point.y), + Position(segment.segment.last_point.x, segment.segment.last_point.y))) + for circle in msg.unknown_circles: + if circle.header.frame_id not in ["map", "/map"]: + rospy.logwarn("Lidar circle not in /map tf frame, skipping.") + continue + vel_d = math.sqrt(circle.circle.velocity.y ** 2 + circle.circle.velocity.x ** 2) + vel_a = math.atan2(circle.circle.velocity.y, circle.circle.velocity.x) + new_lidar.append(CircleObstacle(Position(circle.circle.center.x, circle.circle.center.y, angle = vel_a), + circle.circle.radius, velocity = Velocity(circle.circle.radius * 2, circle.circle.radius * math.sqrt(3.0) / 2.0, + vel_d, 0.0))) + + if len(new_lidar) > 0: + ObstaclesStack.updateLidarObjects(new_lidar) + + def on_robot_speed(self, msg): + self._vel_linear = msg.linear_speed + self._vel_angular = 0.0 + + def _quaternion_to_euler_angle(self, quaternion): + # https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles + w, x, y, z = quaternion.w, quaternion.x, quaternion.y, quaternion.z + t0 = +2.0 * (w * x + y * z) + t1 = +1.0 - 2.0 * (x * x + y ** 2) + X = math.degrees(math.atan2(t0, t1)) + + t2 = +2.0 * (w * y - z * x) + t2 = +1.0 if t2 > +1.0 else t2 + t2 = -1.0 if t2 < -1.0 else t2 + Y = math.degrees(math.asin(t2)) + + t3 = +2.0 * (w * z + x * y) + t4 = +1.0 - 2.0 * (y ** 2 + z * z) + Z = math.atan2(t3, t4) + return (X, Y, Z) diff --git a/ros_ws/src/navigation_collisions/src/markers_publisher/__init__.py b/ros_ws/src/navigation_collisions/src/markers_publisher/__init__.py new file mode 100644 index 0000000..d84bda1 --- /dev/null +++ b/ros_ws/src/navigation_collisions/src/markers_publisher/__init__.py @@ -0,0 +1 @@ +from markers_publisher import MarkersPublisher \ No newline at end of file diff --git a/ros_ws/src/navigation_collisions/src/markers_publisher/markers_publisher.py b/ros_ws/src/navigation_collisions/src/markers_publisher/markers_publisher.py new file mode 100644 index 0000000..7c8726d --- /dev/null +++ b/ros_ws/src/navigation_collisions/src/markers_publisher/markers_publisher.py @@ -0,0 +1,81 @@ +#!/usr/bin/python +import math +import rospy +from visualization_msgs.msg import Marker + + +class MarkersPublisher(object): + def __init__(self): + self.MARKERS_TOPIC = "/visualization_markers/navigation" + self.MarkersPUBL = rospy.Publisher(self.MARKERS_TOPIC, Marker, queue_size=10) + + def _is_connected(self): + return bool(self.MarkersPUBL.get_num_connections()) + + def publishCheckZones(self, robot): + if self._is_connected(): + for i, main_shape in enumerate(robot.get_main_shapes()): + self._publish_marker("collisions_main", i, main_shape, 0.02, 0.01, (1.0, 0.0, 0.0, 0.8)) + + for i, path_shape in enumerate(robot.get_path_shapes()): + self._publish_marker("collisions_path", i, path_shape, 0.02, 0.01, (1.0, 0.5, 0.1, 0.8)) + + def publishObstacles(self, obstacles): # Temporaire ? + if self._is_connected(): + counter = 0 + for i, obs in enumerate(obstacles): + self._publish_marker("collisions_obstacles", i + counter, obs, 0.35, 0.35 / 2.0, (1.0, 0.8, 0.3, 0.8)) + + if obs.velocity is not None: # if the bostacle has a velocity, draw its rect too. + for vel_shape in obs.velocity.get_shapes(obs.position): + counter += 1 + self._publish_marker("collisions_obstacles", i + counter, vel_shape, 0.02, 0.01, (1.0, 0.0, 0.0, 0.8)) + + def _publish_marker(self, ns, index, obj, z_scale, z_height, color): + markertypes = { + "segment": Marker.CUBE, + "rect": Marker.CUBE, + "circle": Marker.CYLINDER, + "mesh": Marker.MESH_RESOURCE + } + marker = Marker() + marker.header.frame_id = "/map" + marker.type = markertypes[str(obj)] # TODO + marker.ns = ns + marker.id = index + + marker.action = Marker.ADD + if str(obj) == "rect": + marker.scale.x = obj.width + marker.scale.y = obj.height + elif str(obj) == "circle": + marker.scale.x = obj.radius * 2.0 + marker.scale.y = obj.radius * 2.0 + elif str(obj) == "segment": + marker.scale.x = obj.length + marker.scale.y = 0.02 + marker.scale.z = z_scale + marker.color.r = color[0] + marker.color.g = color[1] + marker.color.b = color[2] + marker.color.a = color[3] + marker.pose.position.x = obj.position.x + marker.pose.position.y = obj.position.y + marker.pose.position.z = z_height + orientation = self._euler_to_quaternion([0, 0, obj.position.a]) + marker.pose.orientation.x = orientation[0] + marker.pose.orientation.y = orientation[1] + marker.pose.orientation.z = orientation[2] + marker.pose.orientation.w = orientation[3] + marker.lifetime = rospy.Duration(0.1) + + self.MarkersPUBL.publish(marker) + + def _euler_to_quaternion(self, xyz): + cr, sr = math.cos(xyz[0] * 0.5), math.sin(xyz[0] * 0.5) + cp, sp = math.cos(xyz[1] * 0.5), math.sin(xyz[1] * 0.5) + cy, sy = math.cos(xyz[2] * 0.5), math.sin(xyz[2] * 0.5) + return (cy * sr * cp - sy * cr * sp, # qx + cy * cr * sp + sy * sr * cp, # qy + sy * cr * cp - cy * sr * sp, # qz + cy * cr * cp + sy * sr * sp) # qw diff --git a/ros_ws/src/navigation_collisions/src/obstacles_stack.py b/ros_ws/src/navigation_collisions/src/obstacles_stack.py new file mode 100644 index 0000000..05cbd4f --- /dev/null +++ b/ros_ws/src/navigation_collisions/src/obstacles_stack.py @@ -0,0 +1,44 @@ +import time +import rospy + +class Map(object): + Robot = None + + +class ObstaclesStack(): + OBSTACLES_LIFESPAN = 0.3 # max time in seconds before being considered too old + + BeltPoints = [] + LidarObjects = [] + Enemies = [] + + @staticmethod + def toList(): + return ObstaclesStack.BeltPoints + ObstaclesStack.LidarObjects + ObstaclesStack.Enemies + + @staticmethod + def updateBeltPoints(new_obstacles): + ObstaclesStack.BeltPoints = new_obstacles + + @staticmethod + def updateLidarObjects(new_obstacles): + ObstaclesStack.LidarObjects = new_obstacles + + @staticmethod + def updateEnemies(new_obstacles): + ObstaclesStack.Enemies = new_obstacles + + @staticmethod + def garbageCollect(): + current_time = time.time() + for obstacle in ObstaclesStack.BeltPoints: + if current_time - obstacle.spawn_time > ObstaclesStack.OBSTACLES_LIFESPAN: + ObstaclesStack.BeltPoints.remove(obstacle) + + for obstacle in ObstaclesStack.LidarObjects: + if current_time - obstacle.spawn_time > ObstaclesStack.OBSTACLES_LIFESPAN: + ObstaclesStack.LidarObjects.remove(obstacle) + + for obstacle in ObstaclesStack.Enemies: + if current_time - obstacle.spawn_time > ObstaclesStack.OBSTACLES_LIFESPAN: + ObstaclesStack.Enemies.remove(obstacle) diff --git a/ros_ws/src/navigation_collisions/srv/ActivateCollisions.srv b/ros_ws/src/navigation_collisions/srv/ActivateCollisions.srv new file mode 100644 index 0000000..57d0f20 --- /dev/null +++ b/ros_ws/src/navigation_collisions/srv/ActivateCollisions.srv @@ -0,0 +1,3 @@ +bool active # True to start checking for collisions, False for standby mode. +--- +bool success \ No newline at end of file diff --git a/ros_ws/src/navigation_navigator/CMakeLists.txt b/ros_ws/src/navigation_navigator/CMakeLists.txt new file mode 100644 index 0000000..cbab420 --- /dev/null +++ b/ros_ws/src/navigation_navigator/CMakeLists.txt @@ -0,0 +1,84 @@ +cmake_minimum_required(VERSION 2.8.3) +project(navigation_navigator) + +find_package(catkin REQUIRED COMPONENTS + message_generation + rospy + std_msgs + geometry_msgs + visualization_msgs + actionlib + actionlib_msgs + navigation_pathfinder + drivers_ard_asserv + memory_map + tf + interactive_markers + ai_game_manager +) + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +add_message_files( + FILES + Status.msg +) + +## Generate services in the 'srv' folder +add_service_files( + FILES + Goto.srv +) + +## Generate actions in the 'action' folder +add_action_files( + FILES + DoGoto.action + DoGotoWaypoint.action +) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs + geometry_msgs + actionlib_msgs +) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +################################### +## catkin specific configuration ## +################################### + +catkin_package( +# CATKIN_DEPENDS rospy std_msgs message_runtime +) + +########### +## Build ## +########### + +############# +## Install ## +############# + +catkin_install_python( + PROGRAMS src/navigator_node.py src/marker_server_node.py src/planning.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install( + DIRECTORY src/asserv src/collisions src/localizer src/map src/pathfinder + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + PATTERN "*.pyc" EXCLUDE +) + +############# +## Testing ## +############# + diff --git a/ros_ws/src/navigation_navigator/action/DoGoto.action b/ros_ws/src/navigation_navigator/action/DoGoto.action new file mode 100644 index 0000000..7a8d511 --- /dev/null +++ b/ros_ws/src/navigation_navigator/action/DoGoto.action @@ -0,0 +1,14 @@ +# Takes 1 point and returns status +uint8 GOTO = 0 +uint8 GOTOA = 1 +uint8 mode +uint8 BACKWARD = 0 +uint8 FORWARD = 1 +uint8 AUTOMATIC = 2 +uint8 direction +geometry_msgs/Pose2D target_pos +bool disable_collisions +--- +bool success +--- +# No feedback \ No newline at end of file diff --git a/ros_ws/src/navigation_navigator/action/DoGotoWaypoint.action b/ros_ws/src/navigation_navigator/action/DoGotoWaypoint.action new file mode 100644 index 0000000..2a21298 --- /dev/null +++ b/ros_ws/src/navigation_navigator/action/DoGotoWaypoint.action @@ -0,0 +1,14 @@ +# Takes a waypoint name and returns status +uint8 GOTO = 0 +uint8 GOTOA = 1 +uint8 mode +uint8 BACKWARD = 0 +uint8 FORWARD = 1 +uint8 AUTOMATIC = 2 +uint8 direction +string waypoint_name +bool disable_collisions +--- +bool success +--- +# No feedback \ No newline at end of file diff --git a/ros_ws/src/navigation_navigator/msg/Status.msg b/ros_ws/src/navigation_navigator/msg/Status.msg new file mode 100644 index 0000000..aa69da1 --- /dev/null +++ b/ros_ws/src/navigation_navigator/msg/Status.msg @@ -0,0 +1,6 @@ +# Constants +uint8 NAV_IDLE = 0 +uint8 NAV_NAVIGATING = 1 + +uint8 status +geometry_msgs/Pose2D[] currentPath diff --git a/ros_ws/src/navigation_navigator/package.xml b/ros_ws/src/navigation_navigator/package.xml new file mode 100644 index 0000000..76e12ab --- /dev/null +++ b/ros_ws/src/navigation_navigator/package.xml @@ -0,0 +1,25 @@ + + + navigation_navigator + 0.0.2 + The movement/navigation/navigator package for coupe18 + GLPv3 + Gaëtan Blond + UTCoupe + catkin + message_generation + rospy + std_msgs + actionlib + tf + interactive_markers + actionlib_msgs + visualization_msgs + navigation_pathfinder + drivers_ard_asserv + memory_map + ai_game_manager + message_runtime + + + diff --git a/ros_ws/src/navigation_navigator/src/asserv/__init__.py b/ros_ws/src/navigation_navigator/src/asserv/__init__.py new file mode 100644 index 0000000..3b7b799 --- /dev/null +++ b/ros_ws/src/navigation_navigator/src/asserv/__init__.py @@ -0,0 +1,4 @@ +#!/usr/bin/env python +# -*-coding:Utf-8 -* + +from asserv_client import * \ No newline at end of file diff --git a/ros_ws/src/navigation_navigator/src/asserv/asserv_client.py b/ros_ws/src/navigation_navigator/src/asserv/asserv_client.py new file mode 100644 index 0000000..682e573 --- /dev/null +++ b/ros_ws/src/navigation_navigator/src/asserv/asserv_client.py @@ -0,0 +1,139 @@ +#!/usr/bin/env python +# -*-coding:Utf-8 -* + +import rospy +import actionlib +from actionlib_msgs.msg import GoalStatus +from actionlib.action_client import CommState + +from geometry_msgs.msg import Pose2D + +from drivers_ard_asserv.msg import * + +from drivers_ard_asserv.srv import * + +class AsservClient(object): + ASSERV_NODE_NAME = "/drivers/ard_asserv" + ASSERV_GOTO_SERVICE_NAME = ASSERV_NODE_NAME + "/goto" + ASSERV_POSE_TOPIC_NAME = ASSERV_NODE_NAME + "/pose2d" + ASSERV_GOTOACTION_NAME = ASSERV_NODE_NAME + "/goto_action" + ASSERV_MANAGE_SERVICE_NAME = ASSERV_NODE_NAME + "/management" + ASSERV_EMERSTP_SERVICE_NAME = ASSERV_NODE_NAME + "/emergency_stop" + + def __init__ (self): + self._asservGotoService = "" + self._asservGotoActionClient = "" + self._asservManageService = "" + self._asservEmergencyStopService = "" + + self.currentPose = Pose2D(0.0, 0.0, 0.0) + + self._callbacksDoGoto = {} + self._currentActions = {} + + self._connectToServers() + + def _connectToServers (self): + rospy.loginfo('Waiting for "' + self.ASSERV_GOTO_SERVICE_NAME + '"...') + rospy.wait_for_service(self.ASSERV_GOTO_SERVICE_NAME) + rospy.loginfo('Asserv found.') + + # Goto service + try: + self._asservGotoService = rospy.ServiceProxy(self.ASSERV_GOTO_SERVICE_NAME, Goto) + self._asservGotoActionClient = actionlib.ActionClient(self.ASSERV_GOTOACTION_NAME, DoGotoAction) + self._asservGotoActionClient.wait_for_server() + self._asservManageService = rospy.ServiceProxy(self.ASSERV_MANAGE_SERVICE_NAME, Management) + self._asservEmergencyStopService = rospy.ServiceProxy(self.ASSERV_EMERSTP_SERVICE_NAME, EmergencyStop) + except rospy.ServiceException, e: + error_str = "Error when trying to connect to " + error_str += self.ASSERV_GOTO_SERVICE_NAME + error_str += " : " + str(e) + rospy.logfatal (error_str) + + # Pose topic + try: + rospy.Subscriber(self.ASSERV_POSE_TOPIC_NAME, Pose2D, self._handleUpdatePose) + except: + pass + + def _handleUpdatePose (self, newPose): + self.currentPose = newPose + + def cancelGoal(self, idOrder): + if idOrder in self._currentActions: + self._currentActions[idOrder].cancel() + else: + rospy.logwarn("Trying to cancel an unknown goal") + + def goto (self, pos, hasAngle): + response = False + try: + if hasAngle: + response = self._asservGotoService(mode=GotoRequest.GOTOA, position=pos).response + else: + response = self._asservGotoService(mode=GotoRequest.GOTO, position=pos).response + except rospy.ServiceException, e: + error_str = "Error when trying to use " + error_str += self.ASSERV_GOTO_SERVICE_NAME + error_str += " : " + str(e) + rospy.logerr (error_str) + raise Exception + else: + if not response: + raise Exception("Path valid but can't reach a point.") + + def _getGoalId(self, clientDoGotoHandle): + str_id = clientDoGotoHandle.comm_state_machine.action_goal.goal_id.id + return str_id + + def doGoto (self, pos, direction, hasAngle=False, callback=None): + mode = DoGotoGoal.GOTO + if hasAngle: + mode = DoGotoGoal.GOTOA + goal = DoGotoGoal(mode=mode, position=pos, direction=direction) + goalHandle = self._asservGotoActionClient.send_goal(goal, transition_cb=self._handleDoGotoResult) + idAct = self._getGoalId(goalHandle) + + if callback: + self._callbacksDoGoto[idAct] = callback + + self._currentActions[idAct] = goalHandle + + return idAct + + def _handleDoGotoResult (self, clientDoGotoHandle): + idAct = self._getGoalId(clientDoGotoHandle) + isDone = clientDoGotoHandle.get_comm_state() == CommState.DONE + if (clientDoGotoHandle.get_goal_status() == GoalStatus.SUCCEEDED) and isDone: + if idAct in self._callbacksDoGoto: + callback = self._callbacksDoGoto[idAct] + del self._callbacksDoGoto[idAct] + del self._currentActions[idAct] + raw_result = clientDoGotoHandle.get_result() + if hasattr(raw_result, "result"): + rospy.logdebug("Finished: " + idAct) + callback(idAct, raw_result.result) + else: + rospy.logwarn("No results found for " + idAct + ", node may be in undefined behavior") + rospy.logwarn("Status was " + clientDoGotoHandle.get_goal_status_text()) + elif clientDoGotoHandle.get_goal_status() in [GoalStatus.ABORTED, GoalStatus.PREEMPTED]: + if idAct in self._callbacksDoGoto: + rospy.logdebug("Cancelled: " + idAct) + self._callbacksDoGoto[idAct](idAct, False) + del self._callbacksDoGoto[idAct] + del self._currentActions[idAct] + + def cancelAllGoals(self): + self._asservManageService.call(mode=ManagementRequest.CLEANG) + self._asservManageService.call(mode=ManagementRequest.KILLG) + + def stopAsserv (self): + rospy.loginfo("stop asserv") + self._asservEmergencyStopService.call(enable=True) + # self._asservManageService.call(mode=ManagementRequest.CLEANG) + # self._asservEmergencyStopService.call(enable=False) + + def resumeAsserv(self): + rospy.loginfo("resume asserv") + self._asservEmergencyStopService.call(enable=False) diff --git a/ros_ws/src/navigation_navigator/src/collisions/__init__.py b/ros_ws/src/navigation_navigator/src/collisions/__init__.py new file mode 100644 index 0000000..684bdca --- /dev/null +++ b/ros_ws/src/navigation_navigator/src/collisions/__init__.py @@ -0,0 +1,4 @@ +#!/usr/bin/env python +# -*-coding:Utf-8 -* + +from collisions_client import * \ No newline at end of file diff --git a/ros_ws/src/navigation_navigator/src/collisions/collisions_client.py b/ros_ws/src/navigation_navigator/src/collisions/collisions_client.py new file mode 100644 index 0000000..2a51aa1 --- /dev/null +++ b/ros_ws/src/navigation_navigator/src/collisions/collisions_client.py @@ -0,0 +1,56 @@ +#!/usr/bin/env python +# -*-coding:Utf-8 -* + +import rospy +from navigation_collisions.msg import PredictedCollision +from navigation_collisions.srv import ActivateCollisions + +__author__ = "Gaëtan Blond" +__date__ = 11/12/2017 + +COLLISIONS_WATCHDOG_TIME = 0.3 + + +class CollisionsClient(object): + def __init__ (self, callbackStop, callbackResume): + self.WARNER_TOPIC = "/navigation/collisions/warner" + self.ACTIVATE_COLLISIONS_SERVICE_NAME = "/navigation/collisions/set_active" + self._callbackStop = callbackStop + self._callbackResume = callbackResume + self._tmr_collisions_check = rospy.Timer(rospy.Duration(COLLISIONS_WATCHDOG_TIME), self._callback_timer_collisions_watchdog) + self._last_collision = False + self._collision_active = False + self._activateCollisionsSrv = "" + self._connectToServers() + + def _warnerCallback (self, message): + if (message.danger_level <= PredictedCollision.LEVEL_DANGER): + self._last_collision = True + if not self._collision_active: + self._collision_active = True + rospy.loginfo("Obstacle detected, stopping the robot") + self._callbackStop() + + def _connectToServers (self): + rospy.loginfo("Waiting for \"" + self.ACTIVATE_COLLISIONS_SERVICE_NAME + "\"") + rospy.wait_for_service(self.ACTIVATE_COLLISIONS_SERVICE_NAME) + rospy.loginfo("Collisions found") + + try: + rospy.Subscriber(self.WARNER_TOPIC, PredictedCollision, self._warnerCallback, queue_size=1) + self._activateCollisionsSrv = rospy.ServiceProxy(self.ACTIVATE_COLLISIONS_SERVICE_NAME, ActivateCollisions) + except rospy.ServiceException, e: + str_error = "Error when trying to connect to " + str_error += self.ACTIVATE_COLLISIONS_SERVICE_NAME + str_error += " : " + str(e) + rospy.logerr(str_error) + + def _callback_timer_collisions_watchdog(self, event): + if self._collision_active: + if not self._last_collision: + self._callbackResume() + self._collision_active = False + self._last_collision = False + + def setEnabled(self, isEnabled): + self._activateCollisionsSrv.call(active=isEnabled) diff --git a/ros_ws/src/navigation_navigator/src/localizer/__init__.py b/ros_ws/src/navigation_navigator/src/localizer/__init__.py new file mode 100644 index 0000000..4828acf --- /dev/null +++ b/ros_ws/src/navigation_navigator/src/localizer/__init__.py @@ -0,0 +1,4 @@ +#!/usr/bin/env python +# -*-coding:Utf-8 -* + +from localizer_client import * \ No newline at end of file diff --git a/ros_ws/src/navigation_navigator/src/localizer/localizer_client.py b/ros_ws/src/navigation_navigator/src/localizer/localizer_client.py new file mode 100644 index 0000000..293269a --- /dev/null +++ b/ros_ws/src/navigation_navigator/src/localizer/localizer_client.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python +# -*-coding:Utf-8 -* + +from threading import Thread + +import rospy +import tf2_ros + +from geometry_msgs.msg import Pose2D + +class LocalizerClient(Thread): + def __init__(self): + self._lastKnownPos = Pose2D() + self._tfBuffer = tf2_ros.Buffer() + self._posRobotListener = tf2_ros.TransformListener(self._tfBuffer) + + # Creates the thread + Thread.__init__(self) + self.start() + + def run(self): + rate = rospy.Rate(10.0) + rospy.logdebug("Start run") + while not rospy.is_shutdown(): + try: + tmpPos = self._tfBuffer.lookup_transform("map", "robot", rospy.Time()).transform.translation + newPose = Pose2D(x=tmpPos.x, y=tmpPos.y) + if newPose != self._lastKnownPos: + self._lastKnownPos = newPose + # rospy.logdebug("tf2 frame : " + str(self._lastKnownPos.x) + ", " + str(self._lastKnownPos.y)) + rate.sleep() + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): + rate.sleep() + continue + + def getLastKnownPos(self): + return self._lastKnownPos \ No newline at end of file diff --git a/ros_ws/src/navigation_navigator/src/map/__init__.py b/ros_ws/src/navigation_navigator/src/map/__init__.py new file mode 100644 index 0000000..6446829 --- /dev/null +++ b/ros_ws/src/navigation_navigator/src/map/__init__.py @@ -0,0 +1,4 @@ +#!/usr/bin/env python +# -*-coding:Utf-8 -* + +from map_client import * \ No newline at end of file diff --git a/ros_ws/src/navigation_navigator/src/map/map_client.py b/ros_ws/src/navigation_navigator/src/map/map_client.py new file mode 100644 index 0000000..fad86ac --- /dev/null +++ b/ros_ws/src/navigation_navigator/src/map/map_client.py @@ -0,0 +1,36 @@ +#!/usr/bin/env python +# -*-coding:Utf-8 -* + +import rospy + +from memory_map.srv import FillWaypoint +from memory_map.msg import Waypoint + + +__author__ = "Gaëtan Blond" +__date__ = 1/2/2018 + +class MapClient(object): + def __init__ (self): + self._fillWaypointSrv = "" + + self.FILLWAYPOINTS_SERVICE_NAME = "/memory/map/fill_waypoint" + + self._connectToServer() + + def getPosFromWaypoint(self, waypointName): + waypoint = Waypoint(name = waypointName, has_angle = True) + return self._fillWaypointSrv.call(waypoint = waypoint).filled_waypoint.pose + + def _connectToServer(self): + rospy.loginfo("Waiting for \"" + self.FILLWAYPOINTS_SERVICE_NAME + "\"") + rospy.wait_for_service(self.FILLWAYPOINTS_SERVICE_NAME) + rospy.loginfo("Map found") + + try: + self._fillWaypointSrv = rospy.ServiceProxy(self.FILLWAYPOINTS_SERVICE_NAME, FillWaypoint) + except rospy.ServiceException, e: + str_error = "Error when trying to connect to " + str_error += self.FILLWAYPOINTS_SERVICE_NAME + str_error += " : " + str(e) + rospy.logerr(str_error) diff --git a/ros_ws/src/navigation_navigator/src/marker_server_node.py b/ros_ws/src/navigation_navigator/src/marker_server_node.py new file mode 100755 index 0000000..56b419d --- /dev/null +++ b/ros_ws/src/navigation_navigator/src/marker_server_node.py @@ -0,0 +1,133 @@ +#!/usr/bin/env python + +import actionlib +from interactive_markers.interactive_marker_server import * +from interactive_markers.menu_handler import * +from navigation_navigator.msg import DoGotoAction, DoGotoGoal +from tf.transformations import euler_from_quaternion +from visualization_msgs.msg import * +from visualization_msgs.msg import Marker + + +def processFeedback(feedback): + pass + +def goal_done(self, result): + if result.success: + pose = int_marker.pose + pose.position.x = -1 + pose.position.y = 1 + + server.setPose(int_marker.name, pose) + server.applyChanges() + +def processMenu(feedback): + goal = DoGotoGoal() + goal.mode = goal.GOTOA + goal.direction = goal.AUTOMATIC + goal.disable_collisions = False + goal.target_pos.x = feedback.pose.position.x + goal.target_pos.y = feedback.pose.position.y + + q = [[o.x, o.y, o.z, o.w] for o in [feedback.pose.orientation]][0] + + goal.target_pos.theta = euler_from_quaternion(q)[2] + + client.send_goal(goal, goal_done) + + rospy.loginfo("Interactive marker sent goto goal !") + + +if __name__ == "__main__": + + rospy.init_node("navigator_interactive_marker") + + client = actionlib.SimpleActionClient('/navigation/navigator/goto_action', DoGotoAction) + client.wait_for_server() + + server = InteractiveMarkerServer("navigator_marker") + + int_marker = InteractiveMarker() + int_marker.header.frame_id = "map" + int_marker.name = "goto_pos" + + # to make the rotation controls smaller + int_marker.scale = 0.5 + + # to make the rotation controls slightly above the ground so no glitch + int_marker.pose.position.z = 0.001 + + # position the marker outside the map so it does not block the view + int_marker.pose.position.x = -1 + int_marker.pose.position.y = 1 + + + # 'real' marker that will be displayed : shape of the robot + box_marker = Marker() + box_marker.type = Marker.CUBE + box_marker.scale.x = 0.201 + box_marker.scale.y = 0.301 + box_marker.scale.z = 0.351 + + box_marker.pose.position.z = 0.30 / 2.0 + 0.025 + + # translucent purple + box_marker.color.r = 175.0 / 255.0 + box_marker.color.g = 0.0 + box_marker.color.b = 219.0 / 255.0 + box_marker.color.a = 0.3 + + + menu_handler = MenuHandler() + menu_handler.insert("Let's go !", callback=processMenu) + + + # control for panning + box_control = InteractiveMarkerControl() + box_control.name = "translate_plane" + box_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE + box_control.orientation.w = 1 + box_control.orientation.x = 0 + box_control.orientation.y = 1 + box_control.orientation.z = 0 + box_control.always_visible = True + + int_marker.controls.append(box_control) + + # so the robot can be clicked and dragged + box_control.markers.append(box_marker) + + + # control for z rotation + rotate_control = InteractiveMarkerControl() + rotate_control.name = "rotate_yaw" + rotate_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS + rotate_control.orientation.w = 1 + rotate_control.orientation.x = 0 + rotate_control.orientation.y = 1 + rotate_control.orientation.z = 0 + + # control for the context menu (right click) + menu_control = InteractiveMarkerControl() + menu_control.name = "menu" + menu_control.interaction_mode = InteractiveMarkerControl.MENU + # left- and right-clickable text + menu_control.description = "Goto destination" + menu_control.always_visible = True + + # so the controls are linked to the interactive marker + int_marker.controls.append(rotate_control) + int_marker.controls.append(menu_control) + + + # add the interactive marker to our collection & + # tell the server to call processFeedback() when feedback arrives for it + server.insert(int_marker, processFeedback) + + # 'commit' changes and send to all clients + server.applyChanges() + + menu_handler.apply(server, int_marker.name) + + rospy.spin() + diff --git a/ros_ws/src/navigation_navigator/src/navigator_node.py b/ros_ws/src/navigation_navigator/src/navigator_node.py new file mode 100755 index 0000000..db7fa08 --- /dev/null +++ b/ros_ws/src/navigation_navigator/src/navigator_node.py @@ -0,0 +1,198 @@ +#!/usr/bin/env python +# -*-coding:Utf-8 -* + +from functools import partial +import math + +import rospy +import actionlib + +from geometry_msgs.msg import Pose2D +from navigation_navigator.srv import Goto +from navigation_navigator.msg import Status, DoGotoResult, DoGotoGoal, DoGotoAction, DoGotoWaypointResult, DoGotoWaypointAction + +from pathfinder import PathfinderClient +from asserv import AsservClient +from localizer import LocalizerClient +from collisions import CollisionsClient +from map import MapClient +from planning import Plan +# + +from ai_game_manager import StatusServices + + +__author__ = "Gaëtan Blond" +__date__ = 17/10/2017 + +NODE_NAME = "navigator" +NODE_NAMESPACE = "navigation" +FULL_NODE_NAME = "/" + NODE_NAMESPACE + "/" + NODE_NAME + +NB_MAX_TRY = 7 +TIME_MAX_STOP = 1 # sec + +# Constants used for the status of goto requests +class GotoStatuses(object): + WAITING_FOR_RESULT = 0 + SUCCESS = 1 + FAILURE = 2 + +# Constants used for the status topic +class NavigatorStatuses(object): + NAV_IDLE = 0 + NAV_NAVIGATING = 1 + NAV_STOPPED = 2 + +class NavigatorNode(object): + """ + The NavigatorNode class is the link between the AI, the Pathfinder and the Asserv. + This node gets a movement order on ROS service. It can accept many at a time but is not design to. + The node will wait for the Pathfinder and the Asserv to advertize their services and actions before starting itself. + """ + def __init__ (self): + """ + Initialize the node. Does not start it. + """ + + self._actionSrv_Dogoto = "" + self._actionSrv_doGotoWaypoint = "" + self._statusPublisher = "" + + self._pathfinderClient = "" + self._asservClient = "" + self._localizerClient = "" + self._collisionsClient = "" + self._mapClient = "" + + self._currentStatus = NavigatorStatuses.NAV_IDLE + self._currentPlan = "" + self._currentGoal = "" + self._lastStopped = rospy.Time(0) + self._isCanceling = False + self._idCurrentTry = 0 + + def _planResultCallback (self, result): + self._isCanceling = False + if result == True or self._idCurrentTry == NB_MAX_TRY: + if self._idCurrentTry == NB_MAX_TRY and not result: + rospy.logerr("Something wrong happened with our goal, aborting") + else: + rospy.loginfo("Goal successful") + self._currentStatus = NavigatorStatuses.NAV_IDLE + self._collisionsClient.setEnabled(False) + self._updateStatus() + self._currentGoal.set_succeeded(DoGotoResult(result)) + else: + self._idCurrentTry += 1 + rospy.loginfo("Trying a new time to reach the goal : try number " + str(self._idCurrentTry)) + self._currentStatus = NavigatorStatuses.NAV_NAVIGATING + self._currentPlan.replan(self._localizerClient.getLastKnownPos()) + + def _handleDoGotoRequest (self, handledGoal): + """ + Callback for the navigator's goto action request. + The start position is the last received one from the localizer. + If there are no path between start and end positions, it will respond with success=False. + Else all waypoints are send to the asserv and the navigator will respond when the asserv + will have treated all points. + @param handledGoal: the received goal + """ + posStart = self._localizerClient.getLastKnownPos() + posEnd = handledGoal.get_goal().target_pos + hasAngle = False + if handledGoal.get_goal().mode == handledGoal.get_goal().GOTOA: + hasAngle = True + + if handledGoal.get_goal().disable_collisions: + rospy.loginfo("Collisions disabled") + self._executeGoto(posStart, posEnd, hasAngle, handledGoal) + + def _handleDoGotoWaypointRequest(self, handledGoal): + rospy.logdebug("request waypoint") + startPos = self._localizerClient.getLastKnownPos() + endPos = self._mapClient.getPosFromWaypoint(handledGoal.get_goal().waypoint_name) + hasAngle = False + if handledGoal.get_goal().mode == handledGoal.get_goal().GOTOA: + hasAngle = True + self._executeGoto(startPos, endPos, hasAngle, handledGoal) + + + def _executeGoto (self, startPos, endPos, hasAngle, handledGoal): + self._currentStatus = NavigatorStatuses.NAV_NAVIGATING + self._collisionsClient.setEnabled(not handledGoal.get_goal().disable_collisions) + handledGoal.set_accepted() + + if hasAngle: + rospy.loginfo("Received a request to (" + str(endPos.x) + ", " + str(endPos.y) + ", " + str(endPos.theta) + ")") + else: + rospy.loginfo("Received a request to (" + str(endPos.x) + ", " + str(endPos.y) + ")") + + self._currentGoal = handledGoal + self._idCurrentTry = 1 + rospy.loginfo("Try 1") + self._currentPlan.newPlan(startPos, endPos, hasAngle, handledGoal.get_goal().direction) + + def _callbackEmergencyStop (self): + """ + Ask the asserv to stop and update the status + """ + if self._currentStatus != NavigatorStatuses.NAV_STOPPED: + self._currentStatus = NavigatorStatuses.NAV_STOPPED + self._asservClient.stopAsserv() + self._lastStopped = rospy.Time.now() + self._updateStatus() + elif rospy.Time.now() - self._lastStopped > rospy.Duration(TIME_MAX_STOP) and not self._isCanceling: + self._isCanceling = True + rospy.loginfo("Something is blocking our way, cancelling all goals") + self._currentPlan.cancelAsservGoals() + + def _callbackAsservResume(self): + self._currentStatus = NavigatorStatuses.NAV_NAVIGATING + self._isCanceling = False + #self._collisionsClient.setEnabled(True) + self._asservClient.resumeAsserv() + self._updateStatus() + + def _updateStatus (self): + """ + Send the current status and waypoint list in the navigator's status topic. + """ + statusMsg = Status() + statusMsg.status = self._currentStatus + statusMsg.currentPath = self._currentPlan.getCurrentPath() + self._statusPublisher.publish(statusMsg) + + def startNode(self): + """ + Start the node and the clients. + """ + rospy.init_node (NODE_NAME, anonymous=False, log_level=rospy.INFO) + # Create the clients + self._pathfinderClient = PathfinderClient() + self._asservClient = AsservClient() + self._localizerClient = LocalizerClient() + self._collisionsClient = CollisionsClient(self._callbackEmergencyStop, self._callbackAsservResume) + self._mapClient = MapClient() + # Create action servers and topic publisher + self._actionSrv_Dogoto = actionlib.ActionServer(FULL_NODE_NAME + "/goto_action", DoGotoAction, self._handleDoGotoRequest, auto_start=False) + self._actionSrv_doGotoWaypoint = actionlib.ActionServer(FULL_NODE_NAME + "/gotowaypoint_action", DoGotoWaypointAction, self._handleDoGotoWaypointRequest, auto_start=False) + self._statusPublisher = rospy.Publisher(FULL_NODE_NAME + "/status", Status, queue_size=1) + # Launch the node + self._actionSrv_Dogoto.start() + self._actionSrv_doGotoWaypoint.start() + rospy.loginfo ("Ready to navigate!") + self._currentPlan = Plan(self._asservClient, self._pathfinderClient, self._planResultCallback, self._updateStatus) + self._updateStatus() + + # Tell ai/game_manager the node initialized successfuly. + StatusServices(NODE_NAMESPACE, NODE_NAME).ready(True) + + rospy.spin () + +if __name__ == "__main__": + try: + node = NavigatorNode () + node.startNode () + except rospy.ROSInterruptException: + pass diff --git a/ros_ws/src/navigation_navigator/src/pathfinder/__init__.py b/ros_ws/src/navigation_navigator/src/pathfinder/__init__.py new file mode 100644 index 0000000..2753fa9 --- /dev/null +++ b/ros_ws/src/navigation_navigator/src/pathfinder/__init__.py @@ -0,0 +1,4 @@ +#!/usr/bin/env python +# -*-coding:Utf-8 -* + +from pathfinder_client import * \ No newline at end of file diff --git a/ros_ws/src/navigation_navigator/src/pathfinder/pathfinder_client.py b/ros_ws/src/navigation_navigator/src/pathfinder/pathfinder_client.py new file mode 100644 index 0000000..db5757a --- /dev/null +++ b/ros_ws/src/navigation_navigator/src/pathfinder/pathfinder_client.py @@ -0,0 +1,40 @@ +#!/usr/bin/env python +# -*-coding:Utf-8 -* + +import rospy +from geometry_msgs.msg import Pose2D +from navigation_pathfinder.srv import * + +class PathfinderClient(object): + def __init__ (self): + self.PATHFINDER_FINDPATH_SERVICE_NAME = "/navigation/pathfinder/find_path" + self.pathfinderFindPathService = "" + self._ConnectToServer () + + def _ConnectToServer (self): + rospy.loginfo('Waiting for "' + self.PATHFINDER_FINDPATH_SERVICE_NAME + '"...') + rospy.wait_for_service (self.PATHFINDER_FINDPATH_SERVICE_NAME) + rospy.loginfo('Pathfinder found.') + try: + self.pathfinderFindPathService = rospy.ServiceProxy(self.PATHFINDER_FINDPATH_SERVICE_NAME, FindPath) + except rospy.ServiceException, e: + error_str = "Error when trying to connect to " + error_str += self.PATHFINDER_FINDPATH_SERVICE_NAME + error_str += " : " + str(e) + rospy.logfatal (error_str) + + def FindPath (self, startPos, endPos): + path = "" + try: + path = self.pathfinderFindPathService(startPos, endPos) + except rospy.ServiceException, e: + error_str = "Error when trying to use " + error_str += self.PATHFINDER_FINDPATH_SERVICE_NAME + error_str += " : " + str(e) + rospy.logerr (error_str) + raise Exception + else: + if (not path.success) or (len(path.path) == 0): + raise Exception("No path found.") + else: + return path.path \ No newline at end of file diff --git a/ros_ws/src/navigation_navigator/src/planning.py b/ros_ws/src/navigation_navigator/src/planning.py new file mode 100644 index 0000000..cdb2a45 --- /dev/null +++ b/ros_ws/src/navigation_navigator/src/planning.py @@ -0,0 +1,145 @@ +#!/usr/bin/env python +# -*-coding:Utf-8 -* + +__author__ = "Gaëtan Blond" +__date__ = 14/4/2018 + +import math +from collections import OrderedDict + +import rospy + +from asserv import AsservClient +from pathfinder import PathfinderClient + +class PlanStatuses: + IDLE = 0 + NAVIGATING = 1 + CANCELLING = 2 + +class Directions: + BACKWARD = 0 + FORWARD = 1 + AUTOMATIC = 2 + + +def pointToStr(point): + """ + Convert a geometry_msgs/Pose2D object (or any object with x an y properties) to a string + """ + return "(" + str(point.x) + "," + str(point.y) + ")" + +class Plan(object): + def __init__ (self, asservClient, pathfinderClient, resultCallback, updateCallback): + self._asservClient = asservClient + self._pathfinderClient = pathfinderClient + self._currentPath = OrderedDict() + self._resultCallback = resultCallback + self._updateCallback = updateCallback + self._status = PlanStatuses.IDLE + self._endPos = "" + self._hasAngle = False + self._direction = Directions.AUTOMATIC + + def newPlan(self, startPos, endPos, hasAngle, direction): + self._endPos = endPos + self._hasAngle = hasAngle + self._direction = direction + self.replan(startPos) + + def replan(self, startPos): + if len(self._currentPath) > 0: + self.cancelAsservGoals() + self._currentPath = OrderedDict() # needed ? + debugStr = "Asked to go from " + debugStr += pointToStr(startPos) + debugStr += " to " + pointToStr(self._endPos) + rospy.logdebug(debugStr) + try: + # sends a request to the pathfinder + path = self._pathfinderClient.FindPath(startPos, self._endPos) + self._printPath (path) + # then sends the path point per point to the arduino_asserv + path.pop(0) # Removes the first point (we are already on startPos) + path.pop() # Removes the last point + lastPoint = startPos + for point in path: + idOrder = self._asservClient.doGoto(point, self._getDirection(self._direction, point, lastPoint), False, self._asservGotoCallback) + self._currentPath[idOrder] = point + lastPoint = point + idOrder = self._asservClient.doGoto(self._endPos, self._getDirection(self._direction, self._endPos, lastPoint), self._hasAngle, self._asservGotoCallback) + self._currentPath[idOrder] = self._endPos + self._status = PlanStatuses.NAVIGATING + rospy.logdebug("Our path has " + str(len(self._currentPath)) + " points:") + for key in self._currentPath.keys(): + rospy.logdebug(key) + self._updateCallback() # needed ? + except Exception as e: + rospy.logerr("Navigation failed: " + e.message) + if len(self._currentPath) > 0: + self.cancelAsservGoals() + else: + self._status = PlanStatuses.IDLE + self._resultCallback(False) + + def _asservGotoCallback(self, idOrder, result): + rospy.logdebug("Callback for " + idOrder) + rospy.logdebug("Path size : " + str(len(self._currentPath))) + if idOrder in self._currentPath.keys(): + del self._currentPath[idOrder] + if len(self._currentPath) == 0: + if self._status == PlanStatuses.CANCELLING: + result = False + self._status = PlanStatuses.IDLE + self._resultCallback(result) + else: + rospy.logwarn("Trying to delete an unknown order...") + self._updateCallback() + + + def cancelAsservGoals(self): + self._status = PlanStatuses.CANCELLING + #for idGoal in self._currentPath.keys(): + # self._asservClient.cancelGoal(idGoal) + self._asservClient.cancelAllGoals() + + def getCurrentPath(self): + path = [] + for idOrder in self._currentPath.keys(): + rospy.logdebug("order " + idOrder) + path.append(self._currentPath[idOrder]) + return path + + def _getAngle(self, v1, v2): + prodScal = v1.x*v2.x + v1.y*v2.y + normeV1 = math.sqrt(pow(v1.x, 2) + pow(v1.y, 2)) + normeV2 = math.sqrt(pow(v2.x, 2) + pow(v2.y, 2)) + cosV1V2 = prodScal / (normeV1 * normeV2) + + # fix float precision + if cosV1V2 > 1.0: + cosV1V2 = 1.0 + elif cosV1V2 < -1.0: + cosV1V2 = -1.0 + + return math.acos(cosV1V2) + + def _getDirection(self, askedDirection, newPos, lastPos): + # TODO debug (last angle / current angle ?) + if askedDirection != Directions.AUTOMATIC: + return askedDirection + if abs(lastPos.theta - self._getAngle(lastPos, newPos)) > (math.pi / 2): + return Directions.BACKWARD + else: + return Directions.FORWARD + + def _printPath (self, path): + """ + Print the path in the debug log from ROS. + @param path: An array of Pose2D + """ + debugStr = "Received path: [" + for point in path: + debugStr += pointToStr(point) + "," + debugStr += "]" + rospy.logdebug (debugStr) \ No newline at end of file diff --git a/ros_ws/src/navigation_navigator/srv/Goto.srv b/ros_ws/src/navigation_navigator/srv/Goto.srv new file mode 100644 index 0000000..91fbb14 --- /dev/null +++ b/ros_ws/src/navigation_navigator/srv/Goto.srv @@ -0,0 +1,4 @@ +# Takes 1 point and returns status +geometry_msgs/Pose2D target_pos +--- +bool success \ No newline at end of file diff --git a/ros_ws/src/navigation_pathfinder/CMakeLists.txt b/ros_ws/src/navigation_pathfinder/CMakeLists.txt new file mode 100644 index 0000000..c2dbfd4 --- /dev/null +++ b/ros_ws/src/navigation_pathfinder/CMakeLists.txt @@ -0,0 +1,109 @@ +cmake_minimum_required(VERSION 2.8.3) +project(navigation_pathfinder) + +add_compile_options(-std=c++14) + +find_package(catkin REQUIRED COMPONENTS + roscpp + roslib + std_msgs + genmsg + geometry_msgs + dynamic_reconfigure + ai_game_manager + memory_map + recognition_objects_classifier +) + +set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules" ${CMAKE_MODULE_PATH}) # Please note the use of CMAKE_CURRENT_SOURCE_DIR +find_package(SFML 2 REQUIRED COMPONENTS + graphics +) + +set(NLOHMANN_JSON_HEADER_LOCATION "$ENV{UTCOUPE_WORKSPACE}/libs/json/single_include/") + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +add_service_files( + FILES + FindPath.srv +) + +generate_messages( + DEPENDENCIES + std_msgs + geometry_msgs +) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +generate_dynamic_reconfigure_options( + def/PathfinderNode.cfg +) + +################################### +## catkin specific configuration ## +################################### + +catkin_package( + CATKIN_DEPENDS message_runtime roslib +) + +########### +## Build ## +########### + +include_directories( + include + ${NLOHMANN_JSON_HEADER_LOCATION} + ${catkin_INCLUDE_DIRS} +) + + +file( + GLOB_RECURSE + source_files_pathfinder + src/pathfinder/* +) +add_executable(pathfinder_node ${source_files_pathfinder} src/pathfinder_node.cpp) + +target_link_libraries(pathfinder_node + ${catkin_LIBRARIES} + ${SFML_LIBRARIES} ${SFML_DEPENDENCIES} +) + +add_dependencies(pathfinder_node + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${PROJECT_NAME}_gencfg + ${catkin_EXPORTED_TARGETS} +) + +############# +## Install ## +############# + +install ( + TARGETS pathfinder_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install( + DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +) + +install( + DIRECTORY def/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/def +) + +############# +## Testing ## +############# + diff --git a/ros_ws/src/navigation_pathfinder/cmake_modules/FindSFML.cmake b/ros_ws/src/navigation_pathfinder/cmake_modules/FindSFML.cmake new file mode 100644 index 0000000..d2ec360 --- /dev/null +++ b/ros_ws/src/navigation_pathfinder/cmake_modules/FindSFML.cmake @@ -0,0 +1,364 @@ +# This script locates the SFML library +# ------------------------------------ +# +# Usage +# ----- +# +# When you try to locate the SFML libraries, you must specify which modules you want to use (system, window, graphics, network, audio, main). +# If none is given, the SFML_LIBRARIES variable will be empty and you'll end up linking to nothing. +# example: +# find_package(SFML COMPONENTS graphics window system) # find the graphics, window and system modules +# +# You can enforce a specific version, either MAJOR.MINOR or only MAJOR. +# If nothing is specified, the version won't be checked (i.e. any version will be accepted). +# example: +# find_package(SFML COMPONENTS ...) # no specific version required +# find_package(SFML 2 COMPONENTS ...) # any 2.x version +# find_package(SFML 2.4 COMPONENTS ...) # version 2.4 or greater +# +# By default, the dynamic libraries of SFML will be found. To find the static ones instead, +# you must set the SFML_STATIC_LIBRARIES variable to TRUE before calling find_package(SFML ...). +# Since you have to link yourself all the SFML dependencies when you link it statically, the following +# additional variables are defined: SFML_XXX_DEPENDENCIES and SFML_DEPENDENCIES (see their detailed +# description below). +# In case of static linking, the SFML_STATIC macro will also be defined by this script. +# example: +# set(SFML_STATIC_LIBRARIES TRUE) +# find_package(SFML 2 COMPONENTS network system) +# +# On Mac OS X if SFML_STATIC_LIBRARIES is not set to TRUE then by default CMake will search for frameworks unless +# CMAKE_FIND_FRAMEWORK is set to "NEVER" for example. Please refer to CMake documentation for more details. +# Moreover, keep in mind that SFML frameworks are only available as release libraries unlike dylibs which +# are available for both release and debug modes. +# +# If SFML is not installed in a standard path, you can use the SFML_ROOT CMake (or environment) variable +# to tell CMake where SFML is. +# +# Output +# ------ +# +# This script defines the following variables: +# - For each specified module XXX (system, window, graphics, network, audio, main): +# - SFML_XXX_LIBRARY_DEBUG: the name of the debug library of the xxx module (set to SFML_XXX_LIBRARY_RELEASE is no debug version is found) +# - SFML_XXX_LIBRARY_RELEASE: the name of the release library of the xxx module (set to SFML_XXX_LIBRARY_DEBUG is no release version is found) +# - SFML_XXX_LIBRARY: the name of the library to link to for the xxx module (includes both debug and optimized names if necessary) +# - SFML_XXX_FOUND: true if either the debug or release library of the xxx module is found +# - SFML_XXX_DEPENDENCIES: the list of libraries the module depends on, in case of static linking +# - SFML_LIBRARIES: the list of all libraries corresponding to the required modules +# - SFML_FOUND: true if all the required modules are found +# - SFML_INCLUDE_DIR: the path where SFML headers are located (the directory containing the SFML/Config.hpp file) +# - SFML_DEPENDENCIES: the list of libraries SFML depends on, in case of static linking +# +# example: +# find_package(SFML 2 COMPONENTS system window graphics audio REQUIRED) +# include_directories(${SFML_INCLUDE_DIR}) +# add_executable(myapp ...) +# target_link_libraries(myapp ${SFML_LIBRARIES}) + +# define the SFML_STATIC macro if static build was chosen +if(SFML_STATIC_LIBRARIES) + add_definitions(-DSFML_STATIC) +endif() + +# define the list of search paths for headers and libraries +set(FIND_SFML_PATHS + ${SFML_ROOT} + $ENV{SFML_ROOT} + ~/Library/Frameworks + /Library/Frameworks + /usr/local + /usr + /sw + /opt/local + /opt/csw + /opt) + +# find the SFML include directory +find_path(SFML_INCLUDE_DIR SFML/Config.hpp + PATH_SUFFIXES include + PATHS ${FIND_SFML_PATHS}) + +# check the version number +set(SFML_VERSION_OK TRUE) +if(SFML_FIND_VERSION AND SFML_INCLUDE_DIR) + # extract the major and minor version numbers from SFML/Config.hpp + # we have to handle framework a little bit differently: + if("${SFML_INCLUDE_DIR}" MATCHES "SFML.framework") + set(SFML_CONFIG_HPP_INPUT "${SFML_INCLUDE_DIR}/Headers/Config.hpp") + else() + set(SFML_CONFIG_HPP_INPUT "${SFML_INCLUDE_DIR}/SFML/Config.hpp") + endif() + FILE(READ "${SFML_CONFIG_HPP_INPUT}" SFML_CONFIG_HPP_CONTENTS) + STRING(REGEX REPLACE ".*#define SFML_VERSION_MAJOR ([0-9]+).*" "\\1" SFML_VERSION_MAJOR "${SFML_CONFIG_HPP_CONTENTS}") + STRING(REGEX REPLACE ".*#define SFML_VERSION_MINOR ([0-9]+).*" "\\1" SFML_VERSION_MINOR "${SFML_CONFIG_HPP_CONTENTS}") + STRING(REGEX REPLACE ".*#define SFML_VERSION_PATCH ([0-9]+).*" "\\1" SFML_VERSION_PATCH "${SFML_CONFIG_HPP_CONTENTS}") + if (NOT "${SFML_VERSION_PATCH}" MATCHES "^[0-9]+$") + set(SFML_VERSION_PATCH 0) + endif() + math(EXPR SFML_REQUESTED_VERSION "${SFML_FIND_VERSION_MAJOR} * 10000 + ${SFML_FIND_VERSION_MINOR} * 100 + ${SFML_FIND_VERSION_PATCH}") + + # if we could extract them, compare with the requested version number + if (SFML_VERSION_MAJOR) + # transform version numbers to an integer + math(EXPR SFML_VERSION "${SFML_VERSION_MAJOR} * 10000 + ${SFML_VERSION_MINOR} * 100 + ${SFML_VERSION_PATCH}") + + # compare them + if(SFML_VERSION LESS SFML_REQUESTED_VERSION) + set(SFML_VERSION_OK FALSE) + endif() + else() + # SFML version is < 2.0 + if (SFML_REQUESTED_VERSION GREATER 10900) + set(SFML_VERSION_OK FALSE) + set(SFML_VERSION_MAJOR 1) + set(SFML_VERSION_MINOR x) + set(SFML_VERSION_PATCH x) + endif() + endif() +endif() + +# find the requested modules +set(SFML_FOUND TRUE) # will be set to false if one of the required modules is not found +foreach(FIND_SFML_COMPONENT ${SFML_FIND_COMPONENTS}) + string(TOLOWER ${FIND_SFML_COMPONENT} FIND_SFML_COMPONENT_LOWER) + string(TOUPPER ${FIND_SFML_COMPONENT} FIND_SFML_COMPONENT_UPPER) + set(FIND_SFML_COMPONENT_NAME sfml-${FIND_SFML_COMPONENT_LOWER}) + + # no suffix for sfml-main, it is always a static library + if(FIND_SFML_COMPONENT_LOWER STREQUAL "main") + # release library + find_library(SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_RELEASE + NAMES ${FIND_SFML_COMPONENT_NAME} + PATH_SUFFIXES lib64 lib + PATHS ${FIND_SFML_PATHS}) + + # debug library + find_library(SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_DEBUG + NAMES ${FIND_SFML_COMPONENT_NAME}-d + PATH_SUFFIXES lib64 lib + PATHS ${FIND_SFML_PATHS}) + else() + # static release library + find_library(SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_STATIC_RELEASE + NAMES ${FIND_SFML_COMPONENT_NAME}-s + PATH_SUFFIXES lib64 lib + PATHS ${FIND_SFML_PATHS}) + + # static debug library + find_library(SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_STATIC_DEBUG + NAMES ${FIND_SFML_COMPONENT_NAME}-s-d + PATH_SUFFIXES lib64 lib + PATHS ${FIND_SFML_PATHS}) + + # dynamic release library + find_library(SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_DYNAMIC_RELEASE + NAMES ${FIND_SFML_COMPONENT_NAME} + PATH_SUFFIXES lib64 lib + PATHS ${FIND_SFML_PATHS}) + + # dynamic debug library + find_library(SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_DYNAMIC_DEBUG + NAMES ${FIND_SFML_COMPONENT_NAME}-d + PATH_SUFFIXES lib64 lib + PATHS ${FIND_SFML_PATHS}) + + # choose the entries that fit the requested link type + if(SFML_STATIC_LIBRARIES) + if(SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_STATIC_RELEASE) + set(SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_RELEASE ${SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_STATIC_RELEASE}) + endif() + if(SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_STATIC_DEBUG) + set(SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_DEBUG ${SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_STATIC_DEBUG}) + endif() + else() + if(SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_DYNAMIC_RELEASE) + set(SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_RELEASE ${SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_DYNAMIC_RELEASE}) + endif() + if(SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_DYNAMIC_DEBUG) + set(SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_DEBUG ${SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_DYNAMIC_DEBUG}) + endif() + endif() + endif() + + if (SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_DEBUG OR SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_RELEASE) + # library found + set(SFML_${FIND_SFML_COMPONENT_UPPER}_FOUND TRUE) + + # if both are found, set SFML_XXX_LIBRARY to contain both + if (SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_DEBUG AND SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_RELEASE) + set(SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY debug ${SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_DEBUG} + optimized ${SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_RELEASE}) + endif() + + # if only one debug/release variant is found, set the other to be equal to the found one + if (SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_DEBUG AND NOT SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_RELEASE) + # debug and not release + set(SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_RELEASE ${SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_DEBUG}) + set(SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY ${SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_DEBUG}) + endif() + if (SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_RELEASE AND NOT SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_DEBUG) + # release and not debug + set(SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_DEBUG ${SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_RELEASE}) + set(SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY ${SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_RELEASE}) + endif() + else() + # library not found + set(SFML_FOUND FALSE) + set(SFML_${FIND_SFML_COMPONENT_UPPER}_FOUND FALSE) + set(SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY "") + set(FIND_SFML_MISSING "${FIND_SFML_MISSING} SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY") + endif() + + # mark as advanced + MARK_AS_ADVANCED(SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY + SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_RELEASE + SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_DEBUG + SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_STATIC_RELEASE + SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_STATIC_DEBUG + SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_DYNAMIC_RELEASE + SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY_DYNAMIC_DEBUG) + + # add to the global list of libraries + set(SFML_LIBRARIES ${SFML_LIBRARIES} "${SFML_${FIND_SFML_COMPONENT_UPPER}_LIBRARY}") +endforeach() + +# in case of static linking, we must also define the list of all the dependencies of SFML libraries +if(SFML_STATIC_LIBRARIES) + + # detect the OS + if(${CMAKE_SYSTEM_NAME} MATCHES "Windows") + set(FIND_SFML_OS_WINDOWS 1) + elseif(${CMAKE_SYSTEM_NAME} MATCHES "Linux") + set(FIND_SFML_OS_LINUX 1) + elseif(${CMAKE_SYSTEM_NAME} MATCHES "FreeBSD") + set(FIND_SFML_OS_FREEBSD 1) + elseif(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") + set(FIND_SFML_OS_MACOSX 1) + endif() + + # start with an empty list + set(SFML_DEPENDENCIES) + set(FIND_SFML_DEPENDENCIES_NOTFOUND) + + # macro that searches for a 3rd-party library + macro(find_sfml_dependency output friendlyname) + # No lookup in environment variables (PATH on Windows), as they may contain wrong library versions + find_library(${output} NAMES ${ARGN} PATHS ${FIND_SFML_PATHS} PATH_SUFFIXES lib NO_SYSTEM_ENVIRONMENT_PATH) + if(${${output}} STREQUAL "${output}-NOTFOUND") + unset(output) + set(FIND_SFML_DEPENDENCIES_NOTFOUND "${FIND_SFML_DEPENDENCIES_NOTFOUND} ${friendlyname}") + endif() + endmacro() + + # sfml-system + list(FIND SFML_FIND_COMPONENTS "system" FIND_SFML_SYSTEM_COMPONENT) + if(NOT ${FIND_SFML_SYSTEM_COMPONENT} EQUAL -1) + + # update the list -- these are only system libraries, no need to find them + if(FIND_SFML_OS_LINUX OR FIND_SFML_OS_FREEBSD OR FIND_SFML_OS_MACOSX) + set(SFML_SYSTEM_DEPENDENCIES "pthread") + endif() + if(FIND_SFML_OS_LINUX) + set(SFML_SYSTEM_DEPENDENCIES ${SFML_SYSTEM_DEPENDENCIES} "rt") + endif() + if(FIND_SFML_OS_WINDOWS) + set(SFML_SYSTEM_DEPENDENCIES "winmm") + endif() + set(SFML_DEPENDENCIES ${SFML_SYSTEM_DEPENDENCIES} ${SFML_DEPENDENCIES}) + endif() + + # sfml-network + list(FIND SFML_FIND_COMPONENTS "network" FIND_SFML_NETWORK_COMPONENT) + if(NOT ${FIND_SFML_NETWORK_COMPONENT} EQUAL -1) + + # update the list -- these are only system libraries, no need to find them + if(FIND_SFML_OS_WINDOWS) + set(SFML_NETWORK_DEPENDENCIES "ws2_32") + endif() + set(SFML_DEPENDENCIES ${SFML_NETWORK_DEPENDENCIES} ${SFML_DEPENDENCIES}) + endif() + + # sfml-window + list(FIND SFML_FIND_COMPONENTS "window" FIND_SFML_WINDOW_COMPONENT) + if(NOT ${FIND_SFML_WINDOW_COMPONENT} EQUAL -1) + + # find libraries + if(FIND_SFML_OS_LINUX OR FIND_SFML_OS_FREEBSD) + find_sfml_dependency(X11_LIBRARY "X11" X11) + find_sfml_dependency(XRANDR_LIBRARY "Xrandr" Xrandr) + endif() + + if(FIND_SFML_OS_LINUX) + find_sfml_dependency(UDEV_LIBRARIES "UDev" udev libudev) + endif() + + # update the list + if(FIND_SFML_OS_WINDOWS) + set(SFML_WINDOW_DEPENDENCIES ${SFML_WINDOW_DEPENDENCIES} "opengl32" "winmm" "gdi32") + elseif(FIND_SFML_OS_LINUX) + set(SFML_WINDOW_DEPENDENCIES ${SFML_WINDOW_DEPENDENCIES} "GL" ${X11_LIBRARY} ${XRANDR_LIBRARY} ${UDEV_LIBRARIES}) + elseif(FIND_SFML_OS_FREEBSD) + set(SFML_WINDOW_DEPENDENCIES ${SFML_WINDOW_DEPENDENCIES} "GL" ${X11_LIBRARY} ${XRANDR_LIBRARY} "usbhid") + elseif(FIND_SFML_OS_MACOSX) + set(SFML_WINDOW_DEPENDENCIES ${SFML_WINDOW_DEPENDENCIES} "-framework OpenGL -framework Foundation -framework AppKit -framework IOKit -framework Carbon") + endif() + set(SFML_DEPENDENCIES ${SFML_WINDOW_DEPENDENCIES} ${SFML_DEPENDENCIES}) + endif() + + # sfml-graphics + list(FIND SFML_FIND_COMPONENTS "graphics" FIND_SFML_GRAPHICS_COMPONENT) + if(NOT ${FIND_SFML_GRAPHICS_COMPONENT} EQUAL -1) + + # find libraries + find_sfml_dependency(FREETYPE_LIBRARY "FreeType" freetype) + + # update the list + set(SFML_GRAPHICS_DEPENDENCIES ${FREETYPE_LIBRARY}) + set(SFML_DEPENDENCIES ${SFML_GRAPHICS_DEPENDENCIES} ${SFML_DEPENDENCIES}) + endif() + + # sfml-audio + list(FIND SFML_FIND_COMPONENTS "audio" FIND_SFML_AUDIO_COMPONENT) + if(NOT ${FIND_SFML_AUDIO_COMPONENT} EQUAL -1) + + # find libraries + find_sfml_dependency(OPENAL_LIBRARY "OpenAL" openal openal32) + find_sfml_dependency(OGG_LIBRARY "Ogg" ogg) + find_sfml_dependency(VORBIS_LIBRARY "Vorbis" vorbis) + find_sfml_dependency(VORBISFILE_LIBRARY "VorbisFile" vorbisfile) + find_sfml_dependency(VORBISENC_LIBRARY "VorbisEnc" vorbisenc) + find_sfml_dependency(FLAC_LIBRARY "FLAC" FLAC) + + # update the list + set(SFML_AUDIO_DEPENDENCIES ${OPENAL_LIBRARY} ${FLAC_LIBRARY} ${VORBISENC_LIBRARY} ${VORBISFILE_LIBRARY} ${VORBIS_LIBRARY} ${OGG_LIBRARY}) + set(SFML_DEPENDENCIES ${SFML_DEPENDENCIES} ${SFML_AUDIO_DEPENDENCIES}) + endif() + +endif() + +# handle errors +if(NOT SFML_VERSION_OK) + # SFML version not ok + set(FIND_SFML_ERROR "SFML found but version too low (requested: ${SFML_FIND_VERSION}, found: ${SFML_VERSION_MAJOR}.${SFML_VERSION_MINOR}.${SFML_VERSION_PATCH})") + set(SFML_FOUND FALSE) +elseif(SFML_STATIC_LIBRARIES AND FIND_SFML_DEPENDENCIES_NOTFOUND) + set(FIND_SFML_ERROR "SFML found but some of its dependencies are missing (${FIND_SFML_DEPENDENCIES_NOTFOUND})") + set(SFML_FOUND FALSE) +elseif(NOT SFML_FOUND) + # include directory or library not found + set(FIND_SFML_ERROR "Could NOT find SFML (missing: ${FIND_SFML_MISSING})") +endif() +if (NOT SFML_FOUND) + if(SFML_FIND_REQUIRED) + # fatal error + message(FATAL_ERROR ${FIND_SFML_ERROR}) + elseif(NOT SFML_FIND_QUIETLY) + # error but continue + message("${FIND_SFML_ERROR}") + endif() +endif() + +# handle success +if(SFML_FOUND AND NOT SFML_FIND_QUIETLY) + message(STATUS "Found SFML ${SFML_VERSION_MAJOR}.${SFML_VERSION_MINOR}.${SFML_VERSION_PATCH} in ${SFML_INCLUDE_DIR}") +endif() diff --git a/ros_ws/src/navigation_pathfinder/def/PathfinderNode.cfg b/ros_ws/src/navigation_pathfinder/def/PathfinderNode.cfg new file mode 100755 index 0000000..72ef3fe --- /dev/null +++ b/ros_ws/src/navigation_pathfinder/def/PathfinderNode.cfg @@ -0,0 +1,14 @@ +#!/usr/bin/env python +PACKAGE = "navigation_pathfinder" +NODE = "pathfinder_node" +CONFIG_NAME = "PathfinderNode" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("render", bool_t, 0, "Render the found path", False) +gen.add("renderFile", str_t, 0, "Path and file name to render the path (see SFML2 for compatible types)", "tmp.bmp") +gen.add("safetyMargin", double_t, 0, "Margin applied (in m) to dynamic obstacles", 0.15, 0, 0.5) + +exit(gen.generate(PACKAGE, NODE, CONFIG_NAME)) diff --git a/ros_ws/src/navigation_pathfinder/def/map.bmp b/ros_ws/src/navigation_pathfinder/def/map.bmp new file mode 100644 index 0000000..47737f3 Binary files /dev/null and b/ros_ws/src/navigation_pathfinder/def/map.bmp differ diff --git a/ros_ws/src/navigation_pathfinder/include/pathfinder/BarriersSubscribers/abstract_barriers_subscriber.h b/ros_ws/src/navigation_pathfinder/include/pathfinder/BarriersSubscribers/abstract_barriers_subscriber.h new file mode 100644 index 0000000..6d155b2 --- /dev/null +++ b/ros_ws/src/navigation_pathfinder/include/pathfinder/BarriersSubscribers/abstract_barriers_subscriber.h @@ -0,0 +1,61 @@ +#ifndef ABSTRACT_BARRIERS_SUBSCRIBER +#define ABSTRACT_BARRIERS_SUBSCRIBER + +#include + +#include "geometry_msgs/Pose2D.h" + +#include + +/** + * Class defining an abstract subscriber. + */ +class AbstractBarriersSubscriber +{ +public: + /** + * Initialize the safety margin. + * @param safetyMargin Margin to add to barriers. + */ + AbstractBarriersSubscriber(const double& safetyMargin) : _safetyMargin(safetyMargin) {}; + + /** + * Check if there are any obstacles at the given position. + * @param pos The position to check. + */ + virtual bool hasBarrier(const geometry_msgs::Pose2D& pos) = 0; + + /** + * Create internaly the ros subscriber. + * @param nodeHandle The node handle to use for the subscriber. + * @param sizeMaxQueue The size of the message queue (optional depending of the subscriber type). + * @param topic The name of the topic (or service) to subscribe. + */ + virtual void subscribe(ros::NodeHandle& nodeHandle, std::size_t sizeMaxQueue, std::string topic) = 0; + + /** + * Update the safety margin. + * @param safetyMargin The new safety margin. + */ + void setSafetyMargin(const double& safetyMargin) { _safetyMargin = safetyMargin; } + + /** + * Before starting its algorithm, the pathfinder can ask the subscriber to make a cache of all current barriers. + * For example, storing them into an array to have a constant access cost O(1). + * @param widthGrid The width of pathfinder internal map. + * @param heightGrid The height of pathfinder internal map. + */ + virtual void fetchOccupancyData(const uint& widthGrid, const uint& heightGrid) {}; + + /** + * Indicate if the base coordinates used in the subscriber are different from the pathfinder. + */ + const virtual bool needConversionBefore() const { return true; }; + +protected: + std::mutex g_mutex; + ros::Subscriber subscriber; + double _safetyMargin; +}; + +#endif // ABSTRACT_BARRIERS_SUBSCRIBER diff --git a/ros_ws/src/navigation_pathfinder/include/pathfinder/BarriersSubscribers/memory_map_subscriber.h b/ros_ws/src/navigation_pathfinder/include/pathfinder/BarriersSubscribers/memory_map_subscriber.h new file mode 100644 index 0000000..785d28c --- /dev/null +++ b/ros_ws/src/navigation_pathfinder/include/pathfinder/BarriersSubscribers/memory_map_subscriber.h @@ -0,0 +1,37 @@ +#ifndef MEMORY_MAP_SUBSCRIBER_H +#define MEMORY_MAP_SUBSCRIBER_H + +#include "pathfinder/BarriersSubscribers/abstract_barriers_subscriber.h" +#include "pathfinder/pos_convertor.h" + +#include "nlohmann/json.hpp" + +#include + +namespace Memory { + class MapSubscriber : public AbstractBarriersSubscriber + { + public: + MapSubscriber(const double& safetyMargin); + + bool hasBarrier(const geometry_msgs::Pose2D& pos) override; + void subscribe(ros::NodeHandle& nodeHandle, std::size_t sizeMaxQueue, std::string topic) override; + + void fetchOccupancyData(const uint& withGrid, const uint& heightGrid) override; + const bool needConversionBefore() const override { return false; } + + void setConvertor(std::shared_ptr convertor) { _convertor = convertor; }; + + private: + std::vector _lastReceivedJsons; + ros::ServiceClient _srvGetMapObjects; + std::vector< std::vector > _occupancyGrid; + + std::shared_ptr _convertor; + + void drawRectangle(const nlohmann::json& jsonRect); + void drawCircle(const nlohmann::json& jsonCircle); + }; +} + +#endif // MEMORY_MAP_SUBSCRIBER_H diff --git a/ros_ws/src/navigation_pathfinder/include/pathfinder/BarriersSubscribers/recognition_objects_classifier_subscriber.h b/ros_ws/src/navigation_pathfinder/include/pathfinder/BarriersSubscribers/recognition_objects_classifier_subscriber.h new file mode 100644 index 0000000..10a3917 --- /dev/null +++ b/ros_ws/src/navigation_pathfinder/include/pathfinder/BarriersSubscribers/recognition_objects_classifier_subscriber.h @@ -0,0 +1,44 @@ +#ifndef RECOGNITION_OBJECTS_CLASSIFIER_SUBSCRIBER +#define RECOGNITION_OBJECTS_CLASSIFIER_SUBSCRIBER + +#include "abstract_barriers_subscriber.h" + +#include "recognition_objects_classifier/ClassifiedObjects.h" + +#include + +namespace Recognition +{ + class ObjectsClassifierSubscriber : public AbstractBarriersSubscriber + { + public: + ObjectsClassifierSubscriber(const double& safetyMargin); + + bool hasBarrier(const geometry_msgs::Pose2D& pos) override; + void subscribe(ros::NodeHandle& nodeHandle, std::size_t sizeMaxQueue, std::string topic) override; + + private: + using Objects = recognition_objects_classifier::ClassifiedObjects; + using Rectangle = processing_belt_interpreter::RectangleStamped; + using Circle = recognition_objects_classifier::CircleObstacleStamped; + using Segment = recognition_objects_classifier::SegmentObstacleStamped; + + std::vector lastRects; + std::vector lastCircles; + std::vector lastSegments; + + void objectsCallback(const Objects::ConstPtr& msg); + void addRects(const std::vector& rects); + void addCircles(const std::vector& circs); + void addSegments(const std::vector& segs); + + bool isInsideRect(const Rectangle& rect, const geometry_msgs::Pose2D& pos) const; + bool isInsideCircle(const Circle& circ, const geometry_msgs::Pose2D& pos) const; + bool isCloseToSegment(const Segment& seg, const geometry_msgs::Pose2D& pos) const; + + double scalarProduct(std::pair vA, std::pair vB) const; + double vectorProduct(std::pair vA, std::pair vB) const; + }; +} // namespace Recognition + +#endif // RECOGNITION_OBJECTS_CLASSIFIER_SUBSCRIBER diff --git a/ros_ws/src/navigation_pathfinder/include/pathfinder/dynamic_barriers_manager.h b/ros_ws/src/navigation_pathfinder/include/pathfinder/dynamic_barriers_manager.h new file mode 100644 index 0000000..f344a23 --- /dev/null +++ b/ros_ws/src/navigation_pathfinder/include/pathfinder/dynamic_barriers_manager.h @@ -0,0 +1,69 @@ +#ifndef DYNAMIC_BARRIERS_MANAGER_H +#define DYNAMIC_BARRIERS_MANAGER_H + +#include "pathfinder/point.h" +#include "pathfinder/BarriersSubscribers/abstract_barriers_subscriber.h" +#include "pathfinder/pos_convertor.h" + +#include "geometry_msgs/Pose2D.h" + +#include +#include + +/** + * Class managing the barriers subscribers. + * It is mainly an interface between the pathfinder and the subscribers, converting the position when needed. + */ +class DynamicBarriersManager +{ +public: + using BarriersSubscriber = std::unique_ptr; + + DynamicBarriersManager() = default; + + /** + * Check if there is any dynamic obstacle on the given position. + * @param pos The position to check. + * @return True if there is at least one obstacle. + */ + bool hasBarriers(const geometry_msgs::Pose2D& pos); + /** + * Check if there is any dynamic obstacle on the given position. + * @param pos The position to check. + * @return True if there is at least one obstacle. + */ + bool hasBarriers(const Point& pos); + + /** + * Add an obstacle subscribers to the manager. + * The class must be derivated from AbstractBarriersSubscriber, and the object already initialized. + * @param subscriber The new subscriber to manage. + */ + void addBarrierSubscriber(BarriersSubscriber&& subscriber); + + /** + * Set the convertor to use with the subscribers. It must be already initialized. + * @param convertor The new convertor. + */ + void setConvertor(std::shared_ptr convertor); + + /** + * Update the safety margin used in all subscribers. + * @param newMargin The new margin to apply on barriers. + */ + void updateSafetyMargin(const double& newMargin); + void fetchOccupancyDatas(const uint& widthGrid, const uint& heightGrid) const; + +private: + std::vector< BarriersSubscriber > subscribers; + std::shared_ptr _convertor; + + /** + * Converts a position from the inside referential and type to the outside ones. + * @param pos The position in the inside referential and type. + * @return The position in the outside referential and type. + */ + geometry_msgs::Pose2D pointToPose2D(const Point& pos) const; +}; + +#endif // DYNAMIC_BARRIERS_MANAGER_H diff --git a/ros_ws/src/navigation_pathfinder/include/pathfinder/map_storage.h b/ros_ws/src/navigation_pathfinder/include/pathfinder/map_storage.h new file mode 100644 index 0000000..c9a9251 --- /dev/null +++ b/ros_ws/src/navigation_pathfinder/include/pathfinder/map_storage.h @@ -0,0 +1,52 @@ +#ifndef MAP_STORAGE_H +#define MAP_STORAGE_H + +#include + +#include "pathfinder/point.h" +#include "pathfinder/dynamic_barriers_manager.h" + +#include +#include + +#include + +/** + * Class used to load and save the pathfinder's datas in an image format. + */ +class MapStorage +{ +public: + using Vect2DBool = std::vector >; + + MapStorage() = default; + + /** + * Loads from an image the allowed positions. Supported format are bmp, png, tga, jpg, gif, psd, hdr and pic. + * @param filename The path to the file to load. + * @return A 2D boolean grid with the same size as the loaded image, true is meaning no obstacle in the case. + */ + Vect2DBool loadAllowedPositionsFromFile(const std::string& fileName); + + /** + * Saves the allowed position and the pathes to an image. Supported format are bmp, png, tga and jpg. + * @param filename The place and the filename where to create or replace the image. + * @param allowedPos A 2D grid with the same size than the image to create, true is meaning no obstacle in the case. + * @param dynBarriersMng The obstacle subscribers manager. + * @param path The raw path at the end of the pathfinder algorithm + * @param smoothPath The smoothed path that will be send as response. + */ + void saveMapToFile(const std::string& fileName, const Vect2DBool& allowedPos, std::shared_ptr dynBarriersMng, const std::vector& path, const std::vector& smoothPath); + +private: + const sf::Color ALLOWED_POS_COLOR = sf::Color::White; + const sf::Color NOT_ALLOWED_POS_COLOR = sf::Color::Black; + const sf::Color DYN_BARRIER_COLOR = sf::Color::Red; + const sf::Color PATH_COLOR = sf::Color::Blue; + const sf::Color SMOOTH_PATH_COLOR = sf::Color::Green; + + void drawPath(sf::Image& image, const Point& pA, const Point& pB, const sf::Color& color); +}; + + +#endif // MAP_STORAGE_H diff --git a/ros_ws/src/navigation_pathfinder/include/pathfinder/pathfinder.h b/ros_ws/src/navigation_pathfinder/include/pathfinder/pathfinder.h new file mode 100644 index 0000000..1554767 --- /dev/null +++ b/ros_ws/src/navigation_pathfinder/include/pathfinder/pathfinder.h @@ -0,0 +1,150 @@ +#ifndef PATHFINDER_H +#define PATHFINDER_H + +#include + +#include "navigation_pathfinder/FindPath.h" +#include "navigation_pathfinder/PathfinderNodeConfig.h" + +#include "pathfinder/map_storage.h" +#include "pathfinder/point.h" +#include "pathfinder/pos_convertor.h" +#include "pathfinder/dynamic_barriers_manager.h" + +#include +#include + + +/** + * Main class for the pathfinder algorithm. + * From arrays reprenting the static and dynamic barriers, it looks for a path between two positions and returns the shortest one if at least one exists. + */ +class Pathfinder +{ +public: + /** + * Shortcut for path in inside referential and type. + */ + typedef std::vector Path; + + /** + * Initialize the pathfinder by loading given image file containing static barriers positions, and the size of a rectangle containing all input positions (here the table). + * @param mapFileName The name of the image file to load. + * @param convertor The convertor shared with other parts of the programm. + * @param tableSize The reference size in the outside referential. + * @param dynBarriersMng The dynamic barriers manager already initialized. + * @param render Tells if an image must be generated to debug if a path was found. + * @param renderFile The image file name that will be generated to debug. + */ + Pathfinder(const std::string& mapFileName, std::shared_ptr convertor, const std::pair< double, double >& tableSize, std::shared_ptr dynBarriersMng, bool render = false, const std::string& renderFile = "tmp.bmp"); + + /** + * Try to find a path between the two given positions. The coordinates are directly used in inside referential. It returns false if no paths where found. + * @param startPos Start position. + * @param endPos End position. + * @param path Will contain the shortest path between startPos and endPos if a path was found. + */ + bool findPath(const Point& startPos, const Point& endPos, Path& path); + /** + * Callback for the ros FindPath service. Coordinates are converted between the outside and inside referential. + * @param req The request, containing the start and end positions. + * @param rep The response, will contain the shortest path if it exists. + */ + bool findPathCallback(navigation_pathfinder::FindPath::Request &req, navigation_pathfinder::FindPath::Response &rep); + + /** + * Callback for ros PathfinderNodeConfig dynamic reconfigure service. + * @param config The new configuration to use. + * @param level *Not used.* + */ + void reconfigureCallback(navigation_pathfinder::PathfinderNodeConfig &config, uint32_t level); + +private: + /** Shortcut to define a 2D array of short. **/ + typedef std::vector > Vect2DShort; + /** Shortcut to define a 2D array of bool. vector is a special type, different from vector **/ + typedef std::vector > Vect2DBool; + + /** Convertor object between inside and outside referentials **/ + std::shared_ptr _convertor; + + /** Manager for loading and saving image files **/ + MapStorage _mapStorage; + + /** Contains the positions of static barriers. **/ + Vect2DBool _allowedPositions; + /** Contains the positions of dynamic barriers. **/ + std::shared_ptr _dynBarriersMng; + + /** Tells if the map and the path must be saved after computing. **/ + bool _renderAfterComputing; + /** Name of the file that will be generated after computing. **/ + std::string _renderFile; + + /** + * From the end positions tries to reach the start position by increasing step-by-step the distance. For all intermedediate points it stores its distance to count it only one time and to be able after to retrieve the shortest path. Returns true if start position is reached, false else. + * @param distMap Used to store the distance for all seen position to the end position. + * @param startPos The start position to reach. + * @param endPos The end position. + * @return Tells if a path between start and end postions exists. + **/ + bool exploreGraph(Vect2DShort& distMap, const Point& startPos, const Point& endPos); + /** + * From the start position, find the shortest path to the end position by using the array containing all distances to end position. It returns the complete path from start to end position. + * @param distMap The 2D array containing distances to end position. + * @param startPos The start position. + * @param endPos The end position. + * @return The complete path between start and end positions. + */ + Path retrievePath(const Vect2DShort& distMap, const Point& startPos, const Point& endPos); + /** + * Removes unnecessary positions in the path by looking for a direct line between two points without meeting any barriers. It returns the cleaned path. + * @param rawPath The path to clean. + * @return The cleaned path. + */ + Path smoothPath(const Path& rawPath); + + /** + * Check if the given position is in the working referential, and if there is no barriers at the same place. + **/ + bool isValid(const Point& pos); + /** + * Check by "drawing" a line between two positinos if they can be directly connected. Returns true if there is no barriers, false else. + */ + bool canConnectWithLine(const Point& pA, const Point& pB); + + /** + * Defines all possible directions to move from any positions. May be implemented as constexpression in the future. + * @return The lists of all allowed movements. + */ + std::vector directions() const; + + // Convertors + /** + * Converts a position from the outside referential and type to the inside ones. + * @param pos The position in the outside referential and type. + * @return The position in the inside referential and type. + */ + Point pose2DToPoint(const geometry_msgs::Pose2D& pos) const; + /** + * Converts a position from the inside referential and type to the outside ones. + * @param pos The position in the inside referential and type. + * @return The position in the outside referential and type. + */ + geometry_msgs::Pose2D pointToPose2D(const Point& pos) const; + + /** + * Convert the path in the inside type to a string for debugging purposes. + * @param path The path in inside referential and type. + * @return The path in string format. + */ + std::string pathMapToStr(const Path& path); + /** + * Convert the path in the outside type to a string for debugging purposes. + * @param path The path in outside referential and type. + * @return The path in string format. + */ + std::string pathRosToStr(const std::vector& path); +}; + +#endif // PATHFINDER_H diff --git a/ros_ws/src/navigation_pathfinder/include/pathfinder/point.h b/ros_ws/src/navigation_pathfinder/include/pathfinder/point.h new file mode 100644 index 0000000..9e9b502 --- /dev/null +++ b/ros_ws/src/navigation_pathfinder/include/pathfinder/point.h @@ -0,0 +1,38 @@ +#ifndef POINT_H +#define POINT_H + +#include +#include + +#include "geometry_msgs/Pose2D.h" + +/** + * Class representing a position or a vector. + * Have somme basic operators. + */ +class Point +{ +public: + Point (int x, int y) : _x(x), _y(y) {} + Point () : Point(0,0) {} + + int norm1Dist (const Point& other) const; + double norm2Dist(const Point& other) const; + + // Operators + Point operator+ (const Point& other) const; + bool operator== (const Point& other) const; + bool operator!= (const Point& other) const; + friend std::ostream& operator<< (std::ostream& os, const Point& pos); + + // Getters & Setters + int getX () const { return _x; } + void setX (const int& x) { _x = x; } + int getY () const { return _y; } + void setY (const int& y) { _y = y; } + +private: + int _x, _y; +}; + +#endif // POINT_H diff --git a/ros_ws/src/navigation_pathfinder/include/pathfinder/pos_convertor.h b/ros_ws/src/navigation_pathfinder/include/pathfinder/pos_convertor.h new file mode 100644 index 0000000..17838f4 --- /dev/null +++ b/ros_ws/src/navigation_pathfinder/include/pathfinder/pos_convertor.h @@ -0,0 +1,45 @@ +#ifndef POS_CONVERTOR_H +#define POS_CONVERTOR_H + +#include + +/** + * @brief This class provide functions to convert coordinates between ROS system and the pathfinding one + */ +class PosConvertor +{ +public: + /** + * @brief Initialize the convertor. Do nothing yet. + */ + PosConvertor() = default; + + /** + * @brief Converts a coodinate from ROS system to pathfinding system using the scales. + * @param rosPos The coodinate in ROS system + * @return The coordinate in pathfinder system + */ + std::pair fromRosToMapPos (const std::pair& rosPos) const; + + /** + * @brief Converts a coodinate from pathfinding system to ROS system using the scales. + * @param mapPos The coodinate in pathfinder system + * @return The coordinate in ROS system + */ + std::pair fromMapToRosPos (const std::pair& mapPos) const; + + double fromMapToRosDistance (const double& dist) const; + + double fromRosToMapDistance (const double& dist) const; + + // Getters & Setters + void setSizes (std::pair sizeRos, std::pair sizeMap); + + void setInvertedY(bool invertedY) { _invertedY = invertedY; } + +private: + bool _invertedY; + std::pair _sizeRos, _sizeMap; +}; + +#endif // POS_CONVERTOR_H diff --git a/ros_ws/src/navigation_pathfinder/package.xml b/ros_ws/src/navigation_pathfinder/package.xml new file mode 100644 index 0000000..3406633 --- /dev/null +++ b/ros_ws/src/navigation_pathfinder/package.xml @@ -0,0 +1,22 @@ + + + navigation_pathfinder + 0.0.1 + The navigation_pathfinder package + UTCoupe Association + GPLv3 + Mindstan + catkin + roscpp + roslib + std_msgs + message_generation + geometry_msgs + dynamic_reconfigure + ai_game_manager + memory_map + recognition_objects_classifier + message_runtime + + + diff --git a/ros_ws/src/navigation_pathfinder/src/pathfinder/BarriersSubscribers/memory_map_subscriber.cpp b/ros_ws/src/navigation_pathfinder/src/pathfinder/BarriersSubscribers/memory_map_subscriber.cpp new file mode 100644 index 0000000..82600e3 --- /dev/null +++ b/ros_ws/src/navigation_pathfinder/src/pathfinder/BarriersSubscribers/memory_map_subscriber.cpp @@ -0,0 +1,115 @@ +#include "pathfinder/BarriersSubscribers/memory_map_subscriber.h" + +#include "memory_map/MapGetObjects.h" + +#include + +using namespace Memory; +using namespace std; +using json = nlohmann::json; + +inline double getNorme2Distance(const double& x1, const double& y1, const double& x2, const double& y2) +{ + double dX = x1 - x2; + double dY = y1 - y2; + return sqrt(dX*dX + dY*dY); +} + +MapSubscriber::MapSubscriber(const double& safetyMargin) + : AbstractBarriersSubscriber(safetyMargin) +{ +} + +bool MapSubscriber::hasBarrier(const geometry_msgs::Pose2D& pos) +{ + return _occupancyGrid[pos.y][pos.x]; +} + +void MapSubscriber::subscribe(ros::NodeHandle& nodeHandle, std::size_t sizeMaxQueue, std::string topic) +{ + _srvGetMapObjects = nodeHandle.serviceClient(topic); +} + +void MapSubscriber::fetchOccupancyData(const uint& widthGrid, const uint& heightGrid) +{ + if(_occupancyGrid.size() == heightGrid) + return; + + memory_map::MapGetObjects srv; + srv.request.collisions_only = true; + if (!_srvGetMapObjects.call(srv) || !srv.response.success) + { + ROS_ERROR("Error when trying to call memory_map/MapGetObjects"); + return; + } + _lastReceivedJsons.clear(); + + if (_occupancyGrid.size() != heightGrid || (heightGrid != 0 && _occupancyGrid.front().size() != widthGrid)) + _occupancyGrid = vector< vector > ( + heightGrid, + vector(widthGrid, false) + ); + else + for (unsigned row = 0; row < _occupancyGrid.size(); row++) + for (unsigned column = 0; column < _occupancyGrid.front().size(); column++) + _occupancyGrid[row][column] = false; + + for (auto&& object : srv.response.objects) + { + _lastReceivedJsons.push_back(json::parse(object)); +// ROS_DEBUG_STREAM("Received from map: " << _lastReceivedJsons.back().dump(4)); + string shapeType = _lastReceivedJsons.back()["shape"]["type"]; + if (shapeType == "rect") + drawRectangle(_lastReceivedJsons.back()); + else if (shapeType == "circle") + drawCircle(_lastReceivedJsons.back()); + } +} + +void Memory::MapSubscriber::drawRectangle(const nlohmann::json& jsonRect) +{ + double x, y, w, h; + x = jsonRect["position"]["x"]; + y = jsonRect["position"]["y"]; + w = jsonRect["shape"]["width"]; + h = jsonRect["shape"]["height"]; + + auto pos = _convertor->fromRosToMapPos(make_pair(x, y)); + w = _convertor->fromRosToMapDistance(w); + h = _convertor->fromRosToMapDistance(h); + + auto safeMarg = _convertor->fromRosToMapDistance(_safetyMargin); + + uint yMin = max(pos.second - (h/2) - safeMarg, 0.0); + uint yMax = min((double)_occupancyGrid.size(), pos.second + (h/2) + safeMarg + 0.5); + uint xMin = max(pos.first - (w/2) - safeMarg, 0.0); + uint xMax = min((double)_occupancyGrid.front().size(), pos.first + (w/2) + safeMarg + 0.5); + + for (uint row = yMin; row < yMax; row++) + for (uint column = xMin; column < xMax; column++) + _occupancyGrid[row][column] = true; +} + +void Memory::MapSubscriber::drawCircle(const nlohmann::json& jsonCircle) +{ + double x, y, r; + x = jsonCircle["position"]["x"]; + y = jsonCircle["position"]["y"]; + r = jsonCircle["shape"]["radius"]; + + auto pos = _convertor->fromRosToMapPos(make_pair(x, y)); + r = _convertor->fromRosToMapDistance(r); + + auto safeMarg = _convertor->fromRosToMapDistance(_safetyMargin); + + uint yMin = max(pos.second - r - safeMarg, 0.0); + uint yMax = min((double)_occupancyGrid.size(), pos.second + r + safeMarg + 0.5); + uint xMin = max(pos.first - r - safeMarg, 0.0); + uint xMax = min((double)_occupancyGrid.front().size(), pos.first + r + safeMarg + 0.5); + + for (uint row = yMin; row < yMax; row++) + for (uint column = xMin; column < xMax; column++) + if (getNorme2Distance(column, row, pos.first, pos.second) <= r) + _occupancyGrid[row][column] = true; +} + diff --git a/ros_ws/src/navigation_pathfinder/src/pathfinder/BarriersSubscribers/recognition_objects_subscriber.cpp b/ros_ws/src/navigation_pathfinder/src/pathfinder/BarriersSubscribers/recognition_objects_subscriber.cpp new file mode 100644 index 0000000..f72e219 --- /dev/null +++ b/ros_ws/src/navigation_pathfinder/src/pathfinder/BarriersSubscribers/recognition_objects_subscriber.cpp @@ -0,0 +1,128 @@ +#include "pathfinder/BarriersSubscribers/recognition_objects_classifier_subscriber.h" + +#include + +using namespace std; +using namespace Recognition; + +ObjectsClassifierSubscriber::ObjectsClassifierSubscriber(const double& safetyMargin) + : AbstractBarriersSubscriber(safetyMargin) +{ + // +} + +bool ObjectsClassifierSubscriber::hasBarrier(const geometry_msgs::Pose2D& pos) +{ + lock_guard lock(g_mutex); + + // Rectangles + for (const auto& rect : lastRects) + if (isInsideRect(rect, pos)) + return true; + + // Circles + for (const auto& circ : lastCircles) + if (isInsideCircle(circ, pos)) + return true; + + // Segments + for (const auto& seg : lastSegments) + if (isCloseToSegment(seg, pos)) + return true; + + return false; +} + +void ObjectsClassifierSubscriber::subscribe(ros::NodeHandle& nodeHandle, std::size_t sizeMaxQueue, std::string topic) +{ + subscriber = nodeHandle.subscribe( + topic, + sizeMaxQueue, + &Recognition::ObjectsClassifierSubscriber::objectsCallback, + this + ); +} + +void ObjectsClassifierSubscriber::objectsCallback(const Objects::ConstPtr& msg) +{ + lock_guard lock(g_mutex); + lastRects.clear(); + addRects(msg->unknown_rects); + lastCircles.clear(); + addCircles(msg->unknown_circles); + lastSegments.clear(); + addSegments(msg->unknown_segments); +} + +void ObjectsClassifierSubscriber::addRects(const std::vector& rects) +{ + lastRects.insert(lastRects.end(), rects.begin(), rects.end()); +} + +void ObjectsClassifierSubscriber::addCircles(const std::vector& circs) +{ + lastCircles.insert(lastCircles.end(), circs.begin(), circs.end()); +} + +void ObjectsClassifierSubscriber::addSegments(const std::vector& segs) +{ + lastSegments.insert(lastSegments.end(), segs.begin(), segs.end()); +} + +bool ObjectsClassifierSubscriber::isInsideRect(const Rectangle& rect, const geometry_msgs::Pose2D& pos) const +{ + if (rect.h == 0 || rect.w == 0) + return false; + double dx, dy; // we want the center of the rectangle as origin + dx = pos.x - rect.x; + dy = pos.y - rect.y; + double a, b; // (a,b) => coordinates of pos with the center of the rectangle as origin and its sides as vectors + a = -dx*cos(rect.a) - dy*sin(M_PI - rect.a) + rect.w/2; + b = dx*sin(rect.a) - dy*cos(rect.a); + // if a/rect.witdh is in [-1/2,1/2] and b/rect.height in [-1/2,1/2], then the pos is inside the rectangle + double da, db; + da = a/(rect.w + 2*_safetyMargin); + db = b/(rect.h + 2*_safetyMargin); + if (da > 1.0/2.0 || da < -1.0/2.0) + return false; + if (db > 1.0/2.0 || db < -1.0/2.0) + return false; + return true; +} + +bool ObjectsClassifierSubscriber::isInsideCircle(const Circle& circ, const geometry_msgs::Pose2D& pos) const +{ + processing_lidar_objects::CircleObstacle circle(circ.circle); + double distToCenter = sqrt(pow(pos.x - circle.center.x, 2) + pow(pos.y - circle.center.y, 2)); + return (distToCenter + _safetyMargin <= circle.radius); +} + +bool ObjectsClassifierSubscriber::isCloseToSegment(const Segment& seg, const geometry_msgs::Pose2D& pos) const +{ + if (seg.segment.first_point.x == seg.segment.last_point.x && seg.segment.first_point.y == seg.segment.last_point.y) + return false; + // A and B are the limits of the segment, M is our current position + pair vectAB(seg.segment.last_point.x - seg.segment.first_point.x, seg.segment.last_point.y - seg.segment.first_point.y); + pair vectAM(pos.x - seg.segment.first_point.x, pos.y - seg.segment.first_point.y); + pair vectBM(pos.x - seg.segment.last_point.x, pos.y - seg.segment.last_point.y); + double proj = scalarProduct(vectAB, vectAM); + + double distToSeg; + if (proj < 0) + distToSeg = sqrt(scalarProduct(vectAM, vectAM)); + else if (proj > scalarProduct(vectAB, vectAB)) + distToSeg = sqrt(scalarProduct(vectBM, vectBM)); + else + distToSeg = abs(vectorProduct(vectAB, vectAM))/(sqrt(scalarProduct(vectAB, vectAB))); + return distToSeg <= _safetyMargin; +} + +double ObjectsClassifierSubscriber::scalarProduct(pair vA, pair vB) const +{ + return vA.first*vB.first + vA.second*vB.second; +} + +double ObjectsClassifierSubscriber::vectorProduct(pair vA, pair vB) const +{ + return vA.first*vB.second - vA.second*vB.first; +} diff --git a/ros_ws/src/navigation_pathfinder/src/pathfinder/dynamic_barriers_manager.cpp b/ros_ws/src/navigation_pathfinder/src/pathfinder/dynamic_barriers_manager.cpp new file mode 100644 index 0000000..a94dcc0 --- /dev/null +++ b/ros_ws/src/navigation_pathfinder/src/pathfinder/dynamic_barriers_manager.cpp @@ -0,0 +1,63 @@ +#include "pathfinder/dynamic_barriers_manager.h" + +#include + +using namespace std; + +bool DynamicBarriersManager::hasBarriers(const geometry_msgs::Pose2D& pos) +{ + for (const auto& subscriber : subscribers) + if (subscriber->needConversionBefore() && subscriber->hasBarrier(pos)) + return true; + return false; +} + +bool DynamicBarriersManager::hasBarriers(const Point& pos) +{ + auto pose2dConvert = pointToPose2D(pos); + geometry_msgs::Pose2D pose2d; + pose2d.x = pos.getX(); + pose2d.y = pos.getY(); + + for (const auto& subscriber : subscribers) + { + if (subscriber->needConversionBefore() && subscriber->hasBarrier(pose2dConvert)) + return true; + else if (!subscriber->needConversionBefore() && subscriber->hasBarrier(pose2d)) + return true; + } + return false; +} + + +void DynamicBarriersManager::addBarrierSubscriber(BarriersSubscriber && subscriber) +{ + subscribers.push_back(std::move(subscriber)); +} + +void DynamicBarriersManager::setConvertor(std::shared_ptr convertor) +{ + _convertor = convertor; +} + +void DynamicBarriersManager::updateSafetyMargin(const double& newMargin) +{ + for (auto& subscriber : subscribers) + subscriber->setSafetyMargin(newMargin); +} + +void DynamicBarriersManager::fetchOccupancyDatas(const uint& widthGrid, const uint& heightGrid) const +{ + for (const auto& subscriber : subscribers) + subscriber->fetchOccupancyData(widthGrid, heightGrid); +} + +geometry_msgs::Pose2D DynamicBarriersManager::pointToPose2D(const Point& pos) const +{ + auto convertedPos = _convertor->fromMapToRosPos(pair(pos.getX(), pos.getY())); + geometry_msgs::Pose2D newPos; + newPos.x = convertedPos.first; + newPos.y = convertedPos.second; + return newPos; +} + diff --git a/ros_ws/src/navigation_pathfinder/src/pathfinder/map_storage.cpp b/ros_ws/src/navigation_pathfinder/src/pathfinder/map_storage.cpp new file mode 100644 index 0000000..5713f15 --- /dev/null +++ b/ros_ws/src/navigation_pathfinder/src/pathfinder/map_storage.cpp @@ -0,0 +1,109 @@ +#include "pathfinder/map_storage.h" + +#include + +using namespace std; + +MapStorage::Vect2DBool MapStorage::loadAllowedPositionsFromFile(const string& fileName) +{ + ROS_DEBUG_STREAM("MapStorage: loading " << fileName); + + sf::Image image; + image.loadFromFile(fileName); + + Vect2DBool allowedPos; + for (unsigned int line = 0; line < image.getSize().y; line++) + { + allowedPos.emplace_back(); + for (unsigned int column = 0; column < image.getSize().x; column++) + allowedPos[line].push_back(image.getPixel(column, line) == ALLOWED_POS_COLOR); + } + ROS_DEBUG_STREAM("MapStorage: Done, map size is " << allowedPos.front().size() << "x" << allowedPos.size()); + return allowedPos; +} + +void MapStorage::saveMapToFile(const string& fileName, const Vect2DBool& allowedPos, shared_ptr dynBarriersMng, const vector& path, const vector& smoothPath) +{ + ROS_DEBUG_STREAM("MapStorage: saving to " << fileName); + sf::Image image; + image.create(allowedPos.front().size(), allowedPos.size()); + + for (unsigned int line = 0; line < allowedPos.size(); line++) + { + for (unsigned int column = 0; column < allowedPos.front().size(); column++) + { + if (!allowedPos[line][column]) + image.setPixel(column, line, NOT_ALLOWED_POS_COLOR); + else if (dynBarriersMng->hasBarriers(Point(column, line))) + image.setPixel(column, line, DYN_BARRIER_COLOR); + else + image.setPixel(column, line, ALLOWED_POS_COLOR); + } + } + + Point lastPos = Point(-1, -1); + for (const Point& pos : path) + { + if (lastPos.getX() != -1) + drawPath(image, lastPos, pos, PATH_COLOR); + lastPos = pos; + } + lastPos = Point(-1, -1); + for (const Point& pos : smoothPath) + { + if (lastPos.getX() != -1) + drawPath(image, lastPos, pos, SMOOTH_PATH_COLOR); + lastPos = pos; + } + + image.saveToFile(fileName); + ROS_DEBUG("MapStorage: Done"); +} + +void MapStorage::drawPath(sf::Image& image, const Point& pA, const Point& pB, const sf::Color& color) +{ + Point drawPos = pA; + int dX, dY, stepX, stepY, error; + + dX = abs(pB.getX() - pA.getX()); + dY = abs(pB.getY() - pA.getY()); + + stepX = (pB.getX() > pA.getX()) ? 1 : -1; + stepY = (pB.getY() > pA.getY()) ? 1 : -1; + + + image.setPixel(pA.getX(), pA.getY(), color); + + if (dX > dY) + { + error = dX/2; + for (int i = 0; i < dX; i++) + { + drawPos = drawPos + Point(stepX, 0); + error += dY; + if (error > dX) + { + error -= dX; + drawPos = drawPos + Point(0, stepY); + } + image.setPixel(drawPos.getX(), drawPos.getY(), color); + } + } + else + { + error = dY/2; + for (int i = 0; i < dY; i++) + { + drawPos = drawPos + Point(0, stepY); + error += dX; + if (error > dY) + { + error -= dY; + drawPos = drawPos + Point(stepX, 0); + } + image.setPixel(drawPos.getX(), drawPos.getY(), color); + } + } + + image.setPixel(pB.getX(), pB.getY(), color); +} diff --git a/ros_ws/src/navigation_pathfinder/src/pathfinder/pathfinder.cpp b/ros_ws/src/navigation_pathfinder/src/pathfinder/pathfinder.cpp new file mode 100644 index 0000000..ac86c25 --- /dev/null +++ b/ros_ws/src/navigation_pathfinder/src/pathfinder/pathfinder.cpp @@ -0,0 +1,315 @@ +#include "pathfinder/pathfinder.h" + +#include +#include +#include +#include + +using namespace std; + +Pathfinder::Pathfinder(const string& mapFileName, shared_ptr convertor, const std::pair< double, double >& tableSize, + shared_ptr dynBarriersMng, bool render, const string& renderFile) +{ + _renderAfterComputing = render; + _renderFile = renderFile; + _dynBarriersMng = dynBarriersMng; + + _allowedPositions = _mapStorage.loadAllowedPositionsFromFile(mapFileName); + if (_allowedPositions.size() == 0) + ROS_FATAL("Allowed positions empty. Cannot define a scale. Please restart the node, it may crash soon."); + else + { + _convertor = convertor; + + if (_allowedPositions.front().size() * _allowedPositions.size() > 200000) { + ROS_WARN("Map image is big, the pathfinder may be very slow ! (150x100px works fine)"); + } + + _convertor->setSizes(tableSize, make_pair(_allowedPositions.front().size(), _allowedPositions.size())); + _dynBarriersMng->setConvertor(_convertor); + _dynBarriersMng->fetchOccupancyDatas(_allowedPositions.front().size(), _allowedPositions.size()); + } +} + + +bool Pathfinder::findPath(const Point& startPos, const Point& endPos, Path& path) +{ + ROS_DEBUG_STREAM("START: " << startPos); + ROS_DEBUG_STREAM("END: " << endPos); + + if (_allowedPositions.empty() || _allowedPositions.front().empty()) + { + ROS_ERROR("Allowed positions is empty. Did you load the file?"); + return false; + } + + _dynBarriersMng->fetchOccupancyDatas(_allowedPositions.front().size(), _allowedPositions.size()); + + auto startTime = chrono::high_resolution_clock::now(); + + // Creates a map filled with -1 + auto mapDist = Vect2DShort( + _allowedPositions.size(), vector(_allowedPositions.front().size(), -1) + ); + if (!exploreGraph(mapDist, startPos, endPos)) // endPos not found or no paths exist between startPos and endPos + { + if (_renderAfterComputing) + _mapStorage.saveMapToFile(_renderFile, _allowedPositions, _dynBarriersMng, Path(), Path()); + ROS_ERROR_STREAM("No path found !"); + return true; + } + + Path rawPath = retrievePath(mapDist, startPos, endPos); + path = smoothPath(rawPath); + + auto endTime = chrono::high_resolution_clock::now(); + chrono::duration elapsedSeconds = endTime - startTime; + + ROS_INFO_STREAM("Found a path with " << path.size() << " points (took " << elapsedSeconds.count() << " ms)"); + + if (_renderAfterComputing) + _mapStorage.saveMapToFile(_renderFile, _allowedPositions, _dynBarriersMng, rawPath, path); + + return true; +} + +bool Pathfinder::findPathCallback(navigation_pathfinder::FindPath::Request& req, navigation_pathfinder::FindPath::Response& rep) +{ + Path path; + + ROS_INFO_STREAM("Received request from (" << req.posStart.x << "," << req.posStart.y << ") to (" << req.posEnd.x << ", " << req.posEnd.y << ")"); + + auto startPos = pose2DToPoint(req.posStart); + auto endPos = pose2DToPoint(req.posEnd); + + bool no_error = findPath(startPos, endPos, path); + if (!no_error) + return false; + for (const Point& pos : path) + rep.path.push_back(pointToPose2D(pos)); + if (!path.empty()) + { + rep.success = true; + rep.path.front() = req.posStart; + rep.path.back() = req.posEnd; + } + else + rep.success = false; + + ROS_DEBUG_STREAM("Answering: " << pathRosToStr(rep.path) << ", " << (rep.success?"true":"false")); + + return true; +} + +void Pathfinder::reconfigureCallback(navigation_pathfinder::PathfinderNodeConfig& config, uint32_t level) +{ + ROS_INFO_STREAM ("Reconfigure request : " << config.render << " " << config.renderFile << " " << config.safetyMargin); + _renderAfterComputing = config.render; + _renderFile = config.renderFile; + // TODO detect env var and home + _dynBarriersMng->updateSafetyMargin(config.safetyMargin); +} + + + +bool Pathfinder::exploreGraph(Vect2DShort& distMap, const Point& startPos, const Point& endPos) +{ + vector previousPositions, nextPositions; + short distFromEnd = 0; + + if (!isValid(startPos) || !isValid(endPos)) + { + ROS_ERROR("Start or end position is not valid!"); + return false; + } + + previousPositions.push_back(endPos); + distMap[endPos.getY()][endPos.getX()] = distFromEnd; + distFromEnd++; + while (!previousPositions.empty()) + { + for (const Point& prevPos : previousPositions) + { + for (const Point& dir : directions()) + { + Point nextPos = prevPos + dir; + if (isValid(nextPos) && distMap[nextPos.getY()][nextPos.getX()] == -1) + { + distMap[nextPos.getY()][nextPos.getX()] = distFromEnd; + if (nextPos == startPos) + return true; + nextPositions.push_back(nextPos); + } + } + } + + previousPositions = std::move(nextPositions); // prevents use of temporary copy and nextPosition is now in undefined state + nextPositions.clear(); // Needed to make sure it is empty + distFromEnd++; + } + + return false; // if we reach this point, we haven't found start position +} + +Pathfinder::Path Pathfinder::retrievePath(const Vect2DShort& distMap, const Point& startPos, const Point& endPos) +{ + Path path; + path.push_back(startPos); + + Point lastPos = startPos; + + while (lastPos != endPos) + { + Point bestNextPos; + short bestDist = numeric_limits::max(); + for (const Point& dir : directions()) + { + Point nextPos = lastPos + dir; + if (isValid(nextPos)) + { + short posDist = distMap[nextPos.getY()][nextPos.getX()]; + if (posDist >= 0 && posDist < bestDist) + { + bestDist = posDist; + bestNextPos = nextPos; + } + } + } + lastPos = bestNextPos; + path.push_back(bestNextPos); + } + return path; +} + +Pathfinder::Path Pathfinder::smoothPath(const Path& rawPath) +{ + Path newPath; + + newPath.push_back(rawPath.front()); + unsigned posL = 0; + while (posL + 1 < rawPath.size()) + { + unsigned int posR = rawPath.size() - 1; // If size() = 0 we don't enter the loop + for (;posR > posL + 1; posR--) + if (canConnectWithLine(rawPath[posL], rawPath[posR])) + break; + posL = posR; // posR was >= posL + 1, so posL[t+1] >= posL[t] + 1 + newPath.push_back(rawPath[posL]); + } + + return newPath; +} + + +bool Pathfinder::isValid(const Point& pos) +{ + if (pos.getY() < 0 || pos.getY() >= _allowedPositions.size()) + return false; + if (pos.getX() < 0 || pos.getX() >= _allowedPositions.front().size()) + return false; + if (!_allowedPositions[pos.getY()][pos.getX()] || _dynBarriersMng->hasBarriers(pos)) + return false; + return true; +} + +bool Pathfinder::canConnectWithLine(const Point& pA, const Point& pB) +{ + Point testPos = pA; + int dX, dY, stepX, stepY, error; + + dX = abs(pB.getX() - pA.getX()); + dY = abs(pB.getY() - pA.getY()); + + stepX = (pB.getX() > pA.getX()) ? 1 : -1; + stepY = (pB.getY() > pA.getY()) ? 1 : -1; + + // We don't need to check start and end positions + + if (dX > dY) + { + error = dX/2; + for (int i = 0; i < dX; i++) + { + testPos = testPos + Point(stepX, 0); + error += dY; + if (error > dX) + { + error -= dX; + testPos = testPos + Point(0, stepY); + } + if (!isValid(testPos)) + return false; + } + } + else + { + error = dY/2; + for (int i = 0; i < dY; i++) + { + testPos = testPos + Point(0, stepY); + error += dX; + if (error > dY) + { + error -= dY; + testPos = testPos + Point(stepX, 0); + } + if (!isValid(testPos)) + return false; + } + } + return true; +} + + +std::vector< Point > Pathfinder::directions() const +{ + const vector dirs { + Point(0, 1), + Point(0, -1), + Point(1, 0), + Point(-1, 0) + // Add other directions if needed + }; + return dirs; // Should use move semantics with recent compilators +} + +Point Pathfinder::pose2DToPoint(const geometry_msgs::Pose2D& pos) const +{ + auto convertedPos = _convertor->fromRosToMapPos(pair(pos.x, pos.y)); + return Point(convertedPos.first, convertedPos.second); +} + +geometry_msgs::Pose2D Pathfinder::pointToPose2D(const Point& pos) const +{ + auto convertedPos = _convertor->fromMapToRosPos(pair(pos.getX(), pos.getY())); + geometry_msgs::Pose2D newPos; + newPos.x = convertedPos.first; + newPos.y = convertedPos.second; + return newPos; +} + +string Pathfinder::pathMapToStr(const Path& path) +{ + ostringstream os; + string str = "["; + for (const Point& pos : path) + os << pos << ", "; + str += os.str(); + if (str.length() > 2) + str.erase(str.end()-2, str.end()); + str += "]"; + return str; +} + +string Pathfinder::pathRosToStr(const vector& path) +{ + ostringstream os; + string str = "["; + for (const geometry_msgs::Pose2D& pos : path) + os << "(" << pos.x << ", " << pos.y << ")" << ", "; + str += os.str(); + if (str.length() > 2) + str.erase(str.end()-2, str.end()); + str += "]"; + return str; +} diff --git a/ros_ws/src/navigation_pathfinder/src/pathfinder/point.cpp b/ros_ws/src/navigation_pathfinder/src/pathfinder/point.cpp new file mode 100644 index 0000000..73c4c5e --- /dev/null +++ b/ros_ws/src/navigation_pathfinder/src/pathfinder/point.cpp @@ -0,0 +1,37 @@ +#include "pathfinder/point.h" + +using namespace std; + +int Point::norm1Dist (const Point& other) const +{ + return std::abs (_x - other.getX ()) + std::abs (_y - other.getY ()); +} + +double Point::norm2Dist(const Point& other) const +{ + double dX2 = pow(_x - other.getX (), 2); + double dY2 = pow(_y - other.getY (), 2); + return sqrt(dX2 + dY2); +} + +Point Point::operator+(const Point& other) const +{ + return Point (_x + other.getX (), _y + other.getY ()); +} + +bool Point::operator==(const Point& other) const +{ + return norm1Dist(other) == 0; +} + +bool Point::operator!=(const Point& other) const +{ + return norm1Dist(other) != 0; +} + +ostream & operator<<(ostream& os, const Point& pos) +{ + os << "(" << pos._x << "," << pos._y << ")"; + return os; +} + diff --git a/ros_ws/src/navigation_pathfinder/src/pathfinder/pos_convertor.cpp b/ros_ws/src/navigation_pathfinder/src/pathfinder/pos_convertor.cpp new file mode 100644 index 0000000..0f00f52 --- /dev/null +++ b/ros_ws/src/navigation_pathfinder/src/pathfinder/pos_convertor.cpp @@ -0,0 +1,53 @@ +#include "pathfinder/pos_convertor.h" + +using namespace std; + +pair PosConvertor::fromRosToMapPos (const pair& rosPos) const +{ + double x, y; + x = rosPos.first * (_sizeMap.first / _sizeRos.first); + y = rosPos.second * (_sizeMap.second / _sizeRos.second); + + if (_invertedY) + y = _sizeMap.second - y; + + return make_pair(x, y); +} + + +pair PosConvertor::fromMapToRosPos (const pair& mapPos) const +{ + double x, y; + x = mapPos.first * (_sizeRos.first / _sizeMap.first); + y = mapPos.second * (_sizeRos.second / _sizeMap.second); + + if (_invertedY) + y = _sizeRos.second - y; + + return make_pair(x, y); +} + +double PosConvertor::fromMapToRosDistance(const double& dist) const +{ + double xCoef = _sizeMap.first/_sizeRos.first; + double yCoef = _sizeMap.second/_sizeRos.second; + // We assume that the scale on x and y is the same, we take the linear average to have a better precision. + double coef = (xCoef + yCoef)/2; + return dist/coef; +} + +double PosConvertor::fromRosToMapDistance(const double& dist) const +{ + double xCoef = _sizeMap.first/_sizeRos.first; + double yCoef = _sizeMap.second/_sizeRos.second; + // We assume that the scale on x and y is the same, we take the linear average to have a better precision. + double coef = (xCoef + yCoef)/2; + return dist*coef; +} + + +void PosConvertor::setSizes(std::pair sizeRos, std::pair sizeMap) +{ + _sizeRos = sizeRos; + _sizeMap = sizeMap; +} diff --git a/ros_ws/src/navigation_pathfinder/src/pathfinder_node.cpp b/ros_ws/src/navigation_pathfinder/src/pathfinder_node.cpp new file mode 100644 index 0000000..5c2c6c3 --- /dev/null +++ b/ros_ws/src/navigation_pathfinder/src/pathfinder_node.cpp @@ -0,0 +1,102 @@ +#include +#include +#include + +#include "navigation_pathfinder/FindPath.h" +#include "navigation_pathfinder/PathfinderNodeConfig.h" + +#include "ai_game_manager/init_service.h" + +#include "pathfinder/map_storage.h" +#include "pathfinder/pathfinder.h" +#include "pathfinder/point.h" +#include "pathfinder/BarriersSubscribers/memory_map_subscriber.h" +#include "pathfinder/BarriersSubscribers/recognition_objects_classifier_subscriber.h" +#include "pathfinder/pos_convertor.h" + +#include + +using namespace std; +using namespace Memory; +using namespace Recognition; + +const string NAMESPACE_NAME = "navigation"; +const string NODE_NAME = "pathfinder"; + +const string FINDPATH_SERVICE_NAME = "/" + NAMESPACE_NAME + "/" + NODE_NAME + "/find_path"; +const pair TABLE_SIZE = {3.0, 2.0}; // Scale corresponding to messages received by the node +const string PR_MAP_FILE_NAME = "layer_ground.bmp"; +const string GR_MAP_FILE_NAME = "layer_pathfinder.bmp"; //"/ros_ws/src/navigation_pathfinder/def/map.bmp"; for debug purposes + +const size_t SIZE_MAX_QUEUE = 10; +const double SAFETY_MARGIN = 0.15; +const string MAP_GET_OBJECTS_SERVER = "/memory/map/get_objects"; +const string OBJECTS_CLASSIFIER_TOPIC = "/recognition/objects_classifier/objects"; + +template +unique_ptr constructSubscriber(ros::NodeHandle& nodeHandle, const string& topic); + +string fetchRobotName(ros::NodeHandle& nodeHandle); + +int main (int argc, char* argv[]) +{ + ros::init(argc, argv, "pathfinder_node"); + +// ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug); + ros::NodeHandle nodeHandle; + + auto robotName = fetchRobotName(nodeHandle); + string memoryMapPath = ros::package::getPath("memory_map") + "/def/occupancy/"; + string mapPath = memoryMapPath + GR_MAP_FILE_NAME; + if (robotName == "pr") + mapPath = PR_MAP_FILE_NAME; + + ROS_INFO_STREAM("Starting pathfinder with map \"" + mapPath + "\"..."); + + auto convertor = make_shared(); + convertor->setInvertedY(true); + + auto dynBarriersMng = make_shared(); + auto mapSubscriber = constructSubscriber(nodeHandle, MAP_GET_OBJECTS_SERVER); + mapSubscriber->setConvertor(convertor); + dynBarriersMng->addBarrierSubscriber(std::move(mapSubscriber)); + dynBarriersMng->addBarrierSubscriber(constructSubscriber(nodeHandle, OBJECTS_CLASSIFIER_TOPIC)); + + ros::service::waitForService(MAP_GET_OBJECTS_SERVER, 20000); + + Pathfinder pathfinder(mapPath, convertor, TABLE_SIZE, dynBarriersMng); + ros::ServiceServer findPathServer = nodeHandle.advertiseService(FINDPATH_SERVICE_NAME, &Pathfinder::findPathCallback, &pathfinder); + + dynamic_reconfigure::Server server; + dynamic_reconfigure::Server::CallbackType f; + + f = boost::bind(&Pathfinder::reconfigureCallback, &pathfinder, _1, _2); + server.setCallback(f); + + StatusServices gameStatusSrv(NAMESPACE_NAME, NODE_NAME); + gameStatusSrv.setReady(true); + + ros::spin(); + + return 0; +} + +template +unique_ptr constructSubscriber(ros::NodeHandle& nodeHandle, const string& topic) +{ + unique_ptr subscriber(new T(SAFETY_MARGIN)); + subscriber->subscribe(nodeHandle, SIZE_MAX_QUEUE, topic); + return subscriber; +} + + +string fetchRobotName(ros::NodeHandle& nodeHandle) +{ + string robot_name; // TODO use C++17 init in if statement + if (!nodeHandle.getParam("/robot", robot_name)) + { + ROS_WARN("Error when trying to get '/robot' param. Falling back to default name \"gr\"."); + return "gr"; + } + return robot_name; +} diff --git a/ros_ws/src/navigation_pathfinder/srv/FindPath.srv b/ros_ws/src/navigation_pathfinder/srv/FindPath.srv new file mode 100644 index 0000000..ebb2a36 --- /dev/null +++ b/ros_ws/src/navigation_pathfinder/srv/FindPath.srv @@ -0,0 +1,6 @@ +# Takes 2 points and returns a status and a vector of points +geometry_msgs/Pose2D posStart +geometry_msgs/Pose2D posEnd +--- +bool success +geometry_msgs/Pose2D[] path diff --git a/ros_ws/src/processing_belt_interpreter/CMakeLists.txt b/ros_ws/src/processing_belt_interpreter/CMakeLists.txt new file mode 100644 index 0000000..f73025b --- /dev/null +++ b/ros_ws/src/processing_belt_interpreter/CMakeLists.txt @@ -0,0 +1,55 @@ +cmake_minimum_required(VERSION 2.8.3) +project(processing_belt_interpreter) + +find_package(catkin REQUIRED COMPONENTS + message_generation + geometry_msgs + std_msgs + tf2 + tf2_ros + rospy + dynamic_reconfigure +) + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +add_message_files( + FILES + BeltRects.msg + RectangleStamped.msg +) + +generate_messages( + DEPENDENCIES + geometry_msgs + std_msgs +) + +generate_dynamic_reconfigure_options( + cfg/BeltInterpreter.cfg +) + +################################### +## catkin specific configuration ## +################################### + +catkin_package( + CATKIN_DEPENDS message_runtime +) + +############# +## Install ## +############# + +catkin_install_python( + PROGRAMS src/belt_interpreter_node.py src/belt_parser.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install( + DIRECTORY cfg + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + PATTERN "*.pyc" EXCLUDE +) diff --git a/ros_ws/src/processing_belt_interpreter/README.md b/ros_ws/src/processing_belt_interpreter/README.md new file mode 100644 index 0000000..96d97e1 --- /dev/null +++ b/ros_ws/src/processing_belt_interpreter/README.md @@ -0,0 +1,28 @@ +# belt_interpreter + +This node listen for sensor data from the belt, converts the ranges into rectangles taking in account the cone angle of the sensors and the range precision. It then splits the rectangles into two groups : those that belong to the map, and those that are unknown objects and may be dangerous. + +- Listens `/drivers/ard_others/belt_ranges` +- Publishes on `/processing/belt_interpreter/rects_filtered` +- Fetches definition in `processing/belt.xml` + +## Initialization + +At the start of the node, it fetches all map objects from its layer (`/terrain/walls/layer_belt/*`) thanks to the `memory_map` package.If it fails doing that, the node shuts down. + +It is also responsible for publishing the static transforms, one for each sensor, relative to the `/robot` transform. The transforms are computed from each sensor position and angle relative to the robot, all defined in the `belt.xml` definition file that get fetched thanks to the `memory_definition` package. + +## Processing the data +When the node receive data from the `drivers_ard_other` packages, it adds it to a stack. + +It then processes all data on the stack at a fixed rate. +If the range is greater than the max range defined in the definition file, the sensor get skipped. + +The rectangle's time stamp is the lastest common time between the `/map` tranform and the static transform of the sensor. +If this timestamp is too old (greater than `TF_TIME_THRESH` seconds), a warning is printed. + +The width and height of the rectangle are calculated base on the range, the range precision and the cone angle. + +Once we have a rectangle, to check if it is unknown or not, we check how much of it intersects elements of the map. If more than a certain percentage (`POINTS_PC_THRESHOLD`) of the rectangle overlaps map objects, it is considered static. + +To check this, the node turns the rectangle into a discrete number of points (with the `RESOLUTION_LONG` and `RESOLUTION_LONG` constants). However, there is a maximum number of points a rectangle can be turned into, defined with the constant `PRECISION_RANGE_THRESH`. If the range is greater than this constant, the rectangle will be turned into N points, where N is the number of points that would divide a rectangle at the `PRECISION_RANGE_THRESH` range. Thanks to this, higher ranges don't take too much time to compute. diff --git a/ros_ws/src/processing_belt_interpreter/cfg/BeltInterpreter.cfg b/ros_ws/src/processing_belt_interpreter/cfg/BeltInterpreter.cfg new file mode 100755 index 0000000..0fd84c8 --- /dev/null +++ b/ros_ws/src/processing_belt_interpreter/cfg/BeltInterpreter.cfg @@ -0,0 +1,11 @@ +#!/usr/bin/env python +PACKAGE = "processing_belt_interpreter" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("RECT_SCALE_WIDTH", double_t, 0, "The output rectangles X are scaled", 1, 0.1, 1.5) +gen.add("RECT_SCALE_HEIGHT", double_t, 0, "The output rectangles Y are scaled", 1, 0.1, 1.5) + +exit(gen.generate(PACKAGE, "belt_interpreter_node.py", "BeltInterpreter")) \ No newline at end of file diff --git a/ros_ws/src/processing_belt_interpreter/msg/BeltRects.msg b/ros_ws/src/processing_belt_interpreter/msg/BeltRects.msg new file mode 100644 index 0000000..9d7bfa2 --- /dev/null +++ b/ros_ws/src/processing_belt_interpreter/msg/BeltRects.msg @@ -0,0 +1 @@ +RectangleStamped[] rects \ No newline at end of file diff --git a/ros_ws/src/processing_belt_interpreter/msg/RectangleStamped.msg b/ros_ws/src/processing_belt_interpreter/msg/RectangleStamped.msg new file mode 100644 index 0000000..87b7ac6 --- /dev/null +++ b/ros_ws/src/processing_belt_interpreter/msg/RectangleStamped.msg @@ -0,0 +1,10 @@ +std_msgs/Header header + +# center (m, m) +float32 x +float32 y +# dims (m, m) +float32 w +float32 h +# ccw angle (rad) +float32 a diff --git a/ros_ws/src/processing_belt_interpreter/package.xml b/ros_ws/src/processing_belt_interpreter/package.xml new file mode 100644 index 0000000..e5386d0 --- /dev/null +++ b/ros_ws/src/processing_belt_interpreter/package.xml @@ -0,0 +1,22 @@ + + + processing_belt_interpreter + 0.0.1 + + Takes a list of ranges from the distance sensors surrounding the robot and filters out the points + pointing to an existing object in the map. The others may correspond to unknwown objects (enemies + for instance). + Publishes two lists in a topic corresponding with the categorized ranges. + + GPLv3 + milesial + UTCoupe + catkin + rospy + message_generation + tf2 + tf2_ros + message_runtime + + + diff --git a/ros_ws/src/processing_belt_interpreter/src/belt_interpreter_node.py b/ros_ws/src/processing_belt_interpreter/src/belt_interpreter_node.py new file mode 100755 index 0000000..185c8cc --- /dev/null +++ b/ros_ws/src/processing_belt_interpreter/src/belt_interpreter_node.py @@ -0,0 +1,245 @@ +#!/usr/bin/env python + +import rospy +from belt_parser import BeltParser +import tf +import tf2_ros +import math +import copy + +from memory_definitions.srv import GetDefinition +from processing_belt_interpreter.msg import * +from drivers_ard_others.msg import BeltRange +from geometry_msgs.msg import Pose2D, TransformStamped, PointStamped +from ai_game_manager import StatusServices +from dynamic_reconfigure.server import Server +from processing_belt_interpreter.cfg import BeltInterpreterConfig + +from multiprocessing import Lock + +class BeltInterpreter(object): + def __init__(self): + super(BeltInterpreter, self).__init__() + + rospy.init_node("belt_interpreter") + + rospy.loginfo("Belt interpreter is initializing...") + + # template for the sensor frame id, with '{}' being the sensor id + self.SENSOR_FRAME_ID = "belt_{}" + self.DEF_FILE = "processing/belt.xml" + self.TOPIC = "/processing/belt_interpreter/rects" + self.SENSORS_TOPIC = "/drivers/ard_others/belt_ranges" + + self.PUB_RATE = rospy.Rate(10) + + self.RECT_SCALE_WIDTH = 1.0 + self.RECT_SCALE_HEIGHT = 1.0 + + self.WATCHDOG_PERIOD_BELT = rospy.Duration(0.015) + self.WATCHDOG_PERIOD_TERA = rospy.Duration(0.05) + + self.PREVIOUS_DATA_SIZE = 2 + + filepath = self.fetch_definition() + + self._belt_parser = BeltParser(filepath) + self._pub = rospy.Publisher(self.TOPIC, BeltRects, queue_size=1) + self._broadcaster = tf2_ros.StaticTransformBroadcaster() + + self.pub_static_transforms() + + self._sensors_sub = rospy.Subscriber(self.SENSORS_TOPIC, BeltRange, + self.callback) + + self.syn_param_srv = Server(BeltInterpreterConfig, self.dyn_param_cb) + + self._mutex = Lock() + + self._watchdog = rospy.Timer(self.WATCHDOG_PERIOD_TERA, self.publish, oneshot=True) + + self._current_rects = {} + self._current_statuses = {} + self._data_to_process = [] + self._previous_rects = [] + self._previous_statuses = [] + + self._same_bad_value_counter = {s: 0 for s in self._belt_parser.Sensors.keys()} + self._last_bad_value = {s: 0 for s in self._belt_parser.Sensors.keys()} + + rospy.loginfo("Belt interpreter is ready. Listening for sensor data on '{}'.".format(self.SENSORS_TOPIC)) # TODO duplicate log with status_services.ready() + # Tell ai/game_manager the node initialized successfuly. + StatusServices("processing", "belt_interpreter").ready(True) + + rospy.spin() + + def dyn_param_cb(self, config, level): + self.RECT_SCALE_HEIGHT = config["RECT_SCALE_HEIGHT"] + self.RECT_SCALE_WIDTH = config["RECT_SCALE_WIDTH"] + rospy.loginfo("Set rect scale to (%f, %f)" % (self.RECT_SCALE_WIDTH, self.RECT_SCALE_HEIGHT)) + + return config + + def publish(self, event): + with self._mutex: + if self._current_rects.keys() == ["sensor_tera1"] or not self._current_rects: + if self._watchdog: + self._watchdog.shutdown() + self._watchdog = rospy.Timer(self.WATCHDOG_PERIOD_TERA, self.publish, oneshot=True) + if len(self._current_rects) > 0: + self._previous_rects.append(copy.deepcopy(self._current_rects)) + self._previous_statuses.append(copy.deepcopy(self._current_statuses)) + + if(len(self._previous_rects) > self.PREVIOUS_DATA_SIZE): + self._previous_rects.pop(0) + + if (len(self._previous_statuses) > self.PREVIOUS_DATA_SIZE): + self._previous_statuses.pop(0) + + self._pub.publish(self._current_rects.values()) + self._current_rects.clear() + self._current_statuses.clear() + + def process_range(self, data): + if data.sensor_id not in self._belt_parser.Sensors.keys(): + rospy.logerr("Received data from belt sensor '{}' but no such sensor is defined" + .format(data.sensor_id)) + return + + with self._mutex: + + params = self._belt_parser.Params[self._belt_parser.Sensors[data.sensor_id]["type"]] + + if data.range > params["max_range"] or data.range <= 0: + self._current_statuses.update({data.sensor_id: False}) + if data.range == self._last_bad_value[data.sensor_id]: + self._same_bad_value_counter[data.sensor_id] += 1 + else: + self._same_bad_value_counter[data.sensor_id] = 0 + self._last_bad_value[data.sensor_id] = data.range + + if self._same_bad_value_counter[data.sensor_id] > 100: + rospy.logwarn_throttle(1, "Sensor %s might be disconnected !" % data.sensor_id) + + # If we published this sensor most of the time and its bad, publish the last one we got + l = [data.sensor_id in d and d[data.sensor_id] for d in self._previous_statuses] + if sum(l) > math.ceil((self.PREVIOUS_DATA_SIZE + 1) / 2): + for d in reversed(self._previous_rects): + if data.sensor_id in d: + rospy.logdebug('Got bad data for sensor %s but publishing the last good data' % data.sensor_id) + r = d[data.sensor_id] + r.header.stamp = rospy.Time.now() + self._current_rects.update({data.sensor_id: d[data.sensor_id]}) + return + return + + self._same_bad_value_counter[data.sensor_id] = 0 + + if params["scale_responsive"]: + width = self.get_rect_width(data.range, params) * self.RECT_SCALE_WIDTH + height = self.get_rect_height(data.range, params) * self.RECT_SCALE_HEIGHT + else: + width = self.get_rect_width(data.range, params) + height = self.get_rect_height(data.range, params) + + rect = RectangleStamped() + rect.header.frame_id = self.SENSOR_FRAME_ID.format(data.sensor_id) + + rect.header.stamp = rospy.Time.now() + rect.x = self.get_rect_x(data.range, params) + rect.y = 0 + rect.w = width + rect.h = height + rect.a = 0 + + self._current_rects.update({data.sensor_id: rect}) + self._current_statuses.update({data.sensor_id: True}) + + + def get_rect_width(self, r, params): + prec = r * params["precision"] + angle = params["angle"] + + x_far = r + prec + x_close = math.cos(angle / 2) * (r - prec) + + # called width because along x axis, but it is the smaller side + width = abs(x_far - x_close) + return width + + def get_rect_height(self, r, params): + prec = r * params["precision"] + angle = params["angle"] + + return abs(2 * math.sin(angle / 2) * (r + prec)) + + + def get_rect_x(self, r, params): + prec = r * params["precision"] + angle = params["angle"] + + x_far = r + prec + x_close = math.cos(angle / 2) * (r - prec) + return (x_far + x_close) / 2 + + def callback(self, data): + publish_now = False + if data.sensor_id in self._current_rects and data.sensor_id != 'sensor_tera1': + publish_now = True + + self.process_range(data) + + if data.sensor_id != 'sensor_tera1' and not publish_now: + if self._watchdog: + self._watchdog.shutdown() + self._watchdog = rospy.Timer(self.WATCHDOG_PERIOD_BELT, self.publish, oneshot=True) + elif publish_now: + self.publish(None) + + + def pub_static_transforms(self): + tr_list = [] + for id, s in self._belt_parser.Sensors.items(): + tr = TransformStamped() + + tr.header.stamp = rospy.Time.now() + tr.header.frame_id = "robot" + tr.child_frame_id = self.SENSOR_FRAME_ID.format(id) + + tr.transform.translation.x = s["x"] + tr.transform.translation.y = s["y"] + tr.transform.translation.z = 0 + + quat = tf.transformations.quaternion_from_euler(0, 0, s["a"]) + tr.transform.rotation.x = quat[0] + tr.transform.rotation.y = quat[1] + tr.transform.rotation.z = quat[2] + tr.transform.rotation.w = quat[3] + + tr_list.append(tr) + + self._broadcaster.sendTransform(tr_list) + + def fetch_definition(self): + get_def = rospy.ServiceProxy('/memory/definitions/get', GetDefinition) + get_def.wait_for_service() + + try: + res = get_def(self.DEF_FILE) + + if not res.success: + msg = "Can't fetch belt definition file. Shutting down." + rospy.logfatal(msg) + raise rospy.ROSInitException(msg) + else: + rospy.logdebug("Belt definition file fetched.") + return res.path + + except rospy.ServiceException as exc: + msg = "Exception when fetching belt definition file. Shutting down.\n {}".format(str(exc)) + rospy.logfatal(msg) + raise rospy.ROSInitException(msg) + + +if __name__ == '__main__': + b = BeltInterpreter() diff --git a/ros_ws/src/processing_belt_interpreter/src/belt_parser.py b/ros_ws/src/processing_belt_interpreter/src/belt_parser.py new file mode 100644 index 0000000..1dfabcc --- /dev/null +++ b/ros_ws/src/processing_belt_interpreter/src/belt_parser.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python + +import xml.etree.ElementTree as ET +import rospy + + +class BeltParser(object): + """Class used to parse the definition XML""" + def __init__(self, file): + super(BeltParser, self).__init__() + rospy.logdebug("Parsing belt definition...") + + root = ET.parse(file).getroot() + + required = ["max_range", "angle", "precision", "scale_responsive"] + + # parse params + if required and root.find("params") is None: + msg = "Can't parse belt definition file: a 'params' element is required. Shutting down." + rospy.logfatal(msg) + raise rospy.ROSInitException(msg) + + self.Params = { p.attrib["type"] : {c.tag: float(c.text) for c in p} for p in root.find("params")} + + rospy.loginfo(self.Params) + + for p in required: + for s in self.Params: + if p not in self.Params[s]: + msg = "Can't parse belt definition: a '{}' element is required in the parameters. Shutting down."\ + .format(p) + rospy.logfatal(msg) + raise rospy.ROSInitException(msg) + + # parse sensors + if root.find("sensors") is None: + msg = "Can't parse belt definition: a 'sensors' element is required. Shutting down." + rospy.logfatal(msg) + raise rospy.ROSInitException(msg) + + sensors = [] + for sensor in root.find("sensors"): + if "id" not in sensor.attrib: + rospy.logerr("Can't parse sensor definition: a 'id' attribute is required. Skipping this sensor.") + continue + + required = ["x", "y", "a"] + for p in required: + if sensor.find(p) is None: + rospy.logerr("Can't parse sensor definition: a '{}' element is required. Skipping this sensor.".format(p)) + + if "type" not in sensor.attrib: + rospy.logerr("Can't parse sensor definition: a 'type' attribute is required. Skipping this sensor.") + continue + + if sensor.attrib["type"] not in self.Params: + rospy.logerr("Can't parse sensor definition: {} sensor type is not defined. Skipping this sensor." + .format(sensor.attrib["type"])) + continue + + sensors.append({ + "id": sensor.attrib["id"], + "x": float(sensor.find("x").text), + "y": float(sensor.find("y").text), + "a": float(sensor.find("a").text), + "type": sensor.attrib["type"] + }) + + + if not sensors: + rospy.logwarn("No sensor found in belt definition.") + + rospy.logdebug("{} sensors found in belt definition".format(len(sensors))) + + self.Sensors = {s["id"]: s for s in sensors} diff --git a/ros_ws/src/recognition_cube/CMakeLists.txt b/ros_ws/src/recognition_cube/CMakeLists.txt new file mode 100644 index 0000000..45c1e54 --- /dev/null +++ b/ros_ws/src/recognition_cube/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 2.8.3) +project(recognition_cube) + +## Find catkin macros and libraries +find_package(catkin REQUIRED COMPONENTS + rospy + tf2 + tf2_ros + message_generation) + +add_service_files( + FILES + Cube_localizer.srv +) + +generate_messages( + DEPENDENCIES + geometry_msgs + ) + +catkin_package( + +) + +include_directories( + +) + diff --git a/ros_ws/src/recognition_cube/package.xml b/ros_ws/src/recognition_cube/package.xml new file mode 100644 index 0000000..2bf44e9 --- /dev/null +++ b/ros_ws/src/recognition_cube/package.xml @@ -0,0 +1,18 @@ + + + recognition_cube + 0.0.0 + + The recognition_cube package find the center of the closest cube of the robot + + GPLv3 + milesial + UTCoupe + catkin + rospy + geometry_msgs + tf2 + tf2_ros + + + diff --git a/ros_ws/src/recognition_cube/src/cube_localizer_node.py b/ros_ws/src/recognition_cube/src/cube_localizer_node.py new file mode 100755 index 0000000..46a53af --- /dev/null +++ b/ros_ws/src/recognition_cube/src/cube_localizer_node.py @@ -0,0 +1,108 @@ +#!/usr/bin/env python +import rospy +import tf +import tf2_ros +import math +from geometry_msgs.msg import Pose2D, TransformStamped +from ai_game_manager import StatusServices +from processing_lidar_objects.msg import Obstacles +from recognition_cube.srv import * +from recognition_objects_classifier.msg import * + +class Localizer(object): + def __init__(self): + super(Localizer, self).__init__() + + rospy.init_node('cube_localizer_node', anonymous=False) + + self._br = tf2_ros.TransformBroadcaster() + + # TODO : subscribe to all sources of info + self._sub_asserv = rospy.Subscriber("/drivers/ard_asserv/pose2d", Pose2D, + self.callback_asserv) + self._lidar_sub = rospy.Subscriber("/processing/lidar_objects/raw_cube", Obstacles, self.lidar_callback) + self._srv_lidar_pos = rospy.Service("/recognition/localizer/recognition_cube", Cube_localizer, self.callback_lidar_pos) + self._data_asserv = None + + self._segments = {'map': [], 'unknown': []} + self._to_process = {'rects': [], 'circles': [], 'segments': []} + + StatusServices("recognition", "cube_localizer_node").ready(True) + + def lidar_callback(self, data): + self._to_process['segments'] = [] + self._to_process['segments'] = [SegmentObstacleStamped(data.header, s) for s in data.segments] + + def callback_asserv(self, data): + self._data_asserv = data + + def clear_objects(self): + self._segments['map'] = [] + self._segments['unknown'] = [] + + def callback_lidar_pos(self, info): + return self.process_segments() + + + def process_segments(self): + min = 2000 + closest = None + for segment in self._to_process['segments']: + seg = segment_properties(segment) + #rospy.loginfo('center_y : ' + str(seg.center_y) + ' norm : ' + str(seg.norm)) + if seg.distance < min and seg.distance < 0.4 and seg.norm > 0.015: + min = seg.distance + closest = seg + + if closest == None: + return ('laser', Pose2D(float(0), float(0), 0)) + + closest = self.calculate_cube_center(closest) + # rospy.loginfo( + # 'Segment le plus proche: ' + str(min) + ' X : ' + str(closest.center_x) + ' Y : ' + str(closest.center_y) + # + '\nFpoint X : ' + str(closest.segment.first_point.x) + ' Y : ' + str(closest.segment.first_point.y) + # + '\nLpoint X : ' + str(closest.segment.last_point.x) + ' Y : ' + str(closest.segment.last_point.y) + # + '\nCube X : ' + str(closest.orth_x) + ' Y : ' + str(closest.orth_y)) + #ret = str(min) + pos = Pose2D(float(closest.orth_x), float(closest.orth_y), 0) + return ('laser', pos) + + def run(self): + rospy.loginfo("Cube localizer node started") + rate = rospy.Rate(20) + while not rospy.is_shutdown(): + rate.sleep() + + # TODO : merge all data gathered + def calculate(self): + return self._data_asserv + + def calculate_cube_center(self, seg): + d = 0.03 + vect_orth_x = (-seg.y)/seg.norm + vect_orth_y = seg.x/seg.norm + if (seg.center_x*vect_orth_x + seg.center_y*vect_orth_y) < 0: + vect_orth_x = -vect_orth_x + vect_orth_y = -vect_orth_y + + seg.orth_x = seg.center_x + d*vect_orth_x + seg.orth_y = seg.center_y + d*vect_orth_y + return seg + +class segment_properties: + def __init__(self, seg): + segment = seg.segment + self.segment = seg.segment + self.x = segment.first_point.x - segment.last_point.x + self.y = segment.first_point.y - segment.last_point.y + self.vect = [self.x, self.y] + self.norm = math.sqrt(self.x * self.x + self.y * self.y) + self.center_x = (segment.first_point.x + segment.last_point.x) / 2 + self.center_y = (segment.first_point.y + segment.last_point.y) / 2 + self.distance = math.sqrt(self.center_x * self.center_x + self.center_y * self.center_y) + self.orth_x = None + self.orth_y = None + +if __name__ == '__main__': + loc = Localizer() + loc.run() diff --git a/ros_ws/src/recognition_cube/srv/Cube_localizer.srv b/ros_ws/src/recognition_cube/srv/Cube_localizer.srv new file mode 100644 index 0000000..c4a9972 --- /dev/null +++ b/ros_ws/src/recognition_cube/srv/Cube_localizer.srv @@ -0,0 +1,4 @@ +# No entry, if success return True, False if not +--- +string frame_id +geometry_msgs/Pose2D position \ No newline at end of file diff --git a/ros_ws/src/recognition_enemy_tracker/CMakeLists.txt b/ros_ws/src/recognition_enemy_tracker/CMakeLists.txt new file mode 100644 index 0000000..0370ea5 --- /dev/null +++ b/ros_ws/src/recognition_enemy_tracker/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 2.8.3) +project(recognition_enemy_tracker) + +find_package(catkin REQUIRED COMPONENTS + ai_game_manager + recognition_objects_classifier +) + +catkin_package( + CATKIN_DEPENDS + ai_game_manager + recognition_objects_classifier +) + +include_directories( + ${catkin_INCLUDE_DIRS} +) + +catkin_install_python( + PROGRAMS + src/enemy_tracker_node.py + src/enemy_tracker_properties.py + src/enemy_tracker_tracker.py + src/libtools.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install( + DIRECTORY src/Data + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + PATTERN "*.pyc" EXCLUDE +) + +install( + DIRECTORY def/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/def +) diff --git a/ros_ws/src/recognition_enemy_tracker/def/enemy_tracker_properties.xml b/ros_ws/src/recognition_enemy_tracker/def/enemy_tracker_properties.xml new file mode 100644 index 0000000..ada0ae8 --- /dev/null +++ b/ros_ws/src/recognition_enemy_tracker/def/enemy_tracker_properties.xml @@ -0,0 +1,5 @@ + + + + + \ No newline at end of file diff --git a/ros_ws/src/recognition_enemy_tracker/package.xml b/ros_ws/src/recognition_enemy_tracker/package.xml new file mode 100644 index 0000000..5eb9742 --- /dev/null +++ b/ros_ws/src/recognition_enemy_tracker/package.xml @@ -0,0 +1,16 @@ + + + recognition_enemy_tracker + 0.0.1 + + Detect enemies and calculate their speeds and trajectories. + + GPLv3 + P. Potiron + UTCoupe + catkin + rospy + std_msgs + ai_game_manager + recognition_objects_classifier + diff --git a/ros_ws/src/recognition_enemy_tracker/src/enemy_tracker_node.py b/ros_ws/src/recognition_enemy_tracker/src/enemy_tracker_node.py new file mode 100755 index 0000000..c9cb6c4 --- /dev/null +++ b/ros_ws/src/recognition_enemy_tracker/src/enemy_tracker_node.py @@ -0,0 +1,58 @@ +#!/usr/bin/python2.7 +# -*-coding:Utf-8 -* + +from recognition_objects_classifier.msg import ClassifiedObjects +from libtools import Rect +from enemy_tracker_tracker import EnemiesData +import enemy_tracker_properties +import rospy +from ai_game_manager import StatusServices + + +class EnemyTrackerNode(): + """Track enemies""" + + def __init__(self): + self._node = rospy.init_node('enemy_tracker') + self._namespace = '/recognition/enemy_tracker/' + self._belt_sub = rospy.Subscriber('/recognition/objects_classifier/objects', ClassifiedObjects, self.importPoint) + self.configure(None) + self.rect = [] + self.data = [] + + # Tell ai/game_manager the node initialized successfuly. + StatusServices("recognition", "enemy_tracker").ready(True) + + def importPoint(self, data): + rects = [] + for rect in data.unknown_rects: + # TODO check referentiel + rects.append(Rect(rect.x, rect.y, rect.w, rect.h, rect.header.stamp)) + self.saveRect(rects) + self.trackEnemies() + + def saveRect(self, rect): + if len(self.rect) >= self.maxRectHistory: + del self.rect[0] + self.rect.append(rect) + + def trackEnemies(self): + pass + + def updateData(auto_detect_change = False): + pass + + def configure(self, prop): + if prop is None: + #Default values + prop = {'maxRectHistory': '3','maxEnemiesVelocity': '0.5'} + self.maxRectHistory = int(prop['maxRectHistory']) + self.maxEnemiesVelocity = float(prop['maxEnemiesVelocity']) + + +if __name__ == '__main__': + enemy_tracker_node = EnemyTrackerNode() + rospy.loginfo('Enemy tracker node started') + #TODO Use rospy.get_param(name) instead + enemy_tracker_node.configure(enemy_tracker_properties.EnemyTrackerProperties()) + rospy.spin() diff --git a/ros_ws/src/recognition_enemy_tracker/src/enemy_tracker_properties.py b/ros_ws/src/recognition_enemy_tracker/src/enemy_tracker_properties.py new file mode 100755 index 0000000..67078c9 --- /dev/null +++ b/ros_ws/src/recognition_enemy_tracker/src/enemy_tracker_properties.py @@ -0,0 +1,33 @@ +#!/usr/bin/python2.7 +# -*-coding:Utf-8 -* + +import xml.etree.ElementTree as ET +import rospy +import rospkg + +_xml_path = rospkg.RosPack().get_path("recognition_enemy_tracker") + '/def/enemy_tracker_properties.xml' + +class EnemyTrackerProperties(): + """Properties of enemy_tracker_node""" + + def __init__(self, xml=_xml_path): + xml_root = ET.parse(xml).getroot() + self.properties = {} + for prop in xml_root.findall('prop'): + self.properties[prop.get('name')] = prop.get('value') + + def __getitem__(self, name): + return self.properties[name] + + +class RospyProperties(): + """Properties of enemy_tracker_node""" + + def __init__(self): + pass + + def __getitem__(self, name): + return rospy.get_param(name) + +if __name__ == '__main__': + print EnemyTrackerProperties().properties diff --git a/ros_ws/src/recognition_enemy_tracker/src/enemy_tracker_tracker.py b/ros_ws/src/recognition_enemy_tracker/src/enemy_tracker_tracker.py new file mode 100644 index 0000000..611949e --- /dev/null +++ b/ros_ws/src/recognition_enemy_tracker/src/enemy_tracker_tracker.py @@ -0,0 +1,30 @@ +#!/usr/bin/python2.7 +# -*-coding:Utf-8 -* + +from libtools import * + +class EnemiesData: + + def __init__(self, pos): + self.pos = pos + self.pos_history = [[], []] + + def __setattr__(self, name, value): + super.__setattr__(name, value) + if name == 'pos': + if len(self.pos_history[0]) >= self.maxPosHistory: + del self.pos_history[0][0] + del self.pos_history[1][0] + self.pos_history[0].append([value.toXPoint()]) + self.pos_history[1].append([value.toYPoint()]) + + def estimateSpeed(self): + return Point( + Polynome(self.pos_history[0]).derivative(), + Polynome(self.pos_history[1]).derivative() + ) + + def can_be_same_entity(self, rect, time): + p = Polynome(self.pos_history[0]) + p.P(time) + diff --git a/ros_ws/src/recognition_enemy_tracker/src/libtools.py b/ros_ws/src/recognition_enemy_tracker/src/libtools.py new file mode 100644 index 0000000..eb4b97d --- /dev/null +++ b/ros_ws/src/recognition_enemy_tracker/src/libtools.py @@ -0,0 +1,67 @@ +#!/usr/bin/python2.7 +# -*-coding:Utf-8 -* +from numpy import array +from numpy.linalg import solve +from math import sqrt + +class Rect: + def __init__(self,x=0,y=0,w=0,h=0,t=0): + self.x = x + self.y = y + self.w = w + self.h = h + self.t = t + + def toXPoint(self): + return Point(self.t, self.x + self.w/2) + + def toYPoint(self): + return Point(self.t, self.y + self.h/2) + + def diagonal(self): + return sqrt(self.w**2+self.h**2) + + def __repr__(self): + return 'Rect( x={}, y={}, w={}, h={}, t={} )'.format(self.x, self.y, self.w, self.h, self.t) +class Point: + def __init__(self, x=0, y=0): + self.x = x + self.y = y + + def __repr__(self): + return '({}, {})'.format(self.x, self.y) + + +class Polynome: + def __init__(self, point = []): + n = len(point) + if n > 0: + sys = [] + val = [] + for p in point: + eq = [] + for i in range(0, n): + eq.append(p.x**i) + val.append(p.y) + sys.append(eq) + self.coef = solve(array(sys),array(val)) + else: + self.coef = [0] + + def derivative(self): + p = Polynome() + del p.coef[0] + for i in range(1, len(self.coef)): + p.coef.append(self.coef[i]*i) + if len(p.coef) == 0: + p.coef.append(0) + return p + + def P(self,x): + y = 0 + for i in range(0,len(self.coef)): + y += self.coef * x**i + return y + + def __repr__(self): + return 'P{}'.format(self.coef) diff --git a/ros_ws/src/recognition_localizer/CMakeLists.txt b/ros_ws/src/recognition_localizer/CMakeLists.txt new file mode 100644 index 0000000..6d68d6e --- /dev/null +++ b/ros_ws/src/recognition_localizer/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 2.8.3) +project(recognition_localizer) + +## Find catkin macros and libraries +find_package(catkin REQUIRED COMPONENTS + rospy + tf2 + tf2_ros) + +catkin_package( + +) + +include_directories( + ${catkin_INCLUDE_DIRS} +) + +############# +## Install ## +############# + +catkin_install_python( + PROGRAMS src/localizer_node.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/ros_ws/src/recognition_localizer/package.xml b/ros_ws/src/recognition_localizer/package.xml new file mode 100644 index 0000000..0525905 --- /dev/null +++ b/ros_ws/src/recognition_localizer/package.xml @@ -0,0 +1,18 @@ + + + recognition_localizer + 0.0.0 + + The recognition_localizer package updates the tf /robot thanks to the odometry data from the asserv and other data like lidar. + + GPLv3 + milesial + UTCoupe + catkin + rospy + geometry_msgs + tf2 + tf2_ros + + + diff --git a/ros_ws/src/recognition_localizer/src/localizer_node.py b/ros_ws/src/recognition_localizer/src/localizer_node.py new file mode 100755 index 0000000..4e16226 --- /dev/null +++ b/ros_ws/src/recognition_localizer/src/localizer_node.py @@ -0,0 +1,59 @@ +#!/usr/bin/env python +import rospy +import tf +import tf2_ros +from geometry_msgs.msg import Pose2D, TransformStamped +from ai_game_manager import StatusServices + + +class Localizer(object): + def __init__(self): + super(Localizer, self).__init__() + + rospy.init_node('localizer_node', anonymous=False) + + self._br = tf2_ros.TransformBroadcaster() + + # TODO : subscribe to all sources of info + self._sub_asserv = rospy.Subscriber("/drivers/ard_asserv/pose2d", Pose2D, + self.callback_asserv) + + self._data_asserv = None + + # Tell ai/game_manager the node initialized successfuly. + StatusServices("recognition", "localizer").ready(True) + + def callback_asserv(self, data): + self._data_asserv = data + + def run(self): + rospy.loginfo("Localizer node started") + rate = rospy.Rate(20) + while not rospy.is_shutdown(): + data = self.calculate() + if data: + t = TransformStamped() + + t.header.stamp = rospy.Time.now() + t.header.frame_id = "map" + t.child_frame_id = "robot" + t.transform.translation.x = data.x + t.transform.translation.y = data.y + t.transform.translation.z = 0.0 + q = tf.transformations.quaternion_from_euler(0, 0, data.theta) + t.transform.rotation.x = q[0] + t.transform.rotation.y = q[1] + t.transform.rotation.z = q[2] + t.transform.rotation.w = q[3] + + self._br.sendTransform(t) + + rate.sleep() + + # TODO : merge all data gathered + def calculate(self): + return self._data_asserv + +if __name__ == '__main__': + loc = Localizer() + loc.run() diff --git a/ros_ws/src/recognition_objects_classifier/CMakeLists.txt b/ros_ws/src/recognition_objects_classifier/CMakeLists.txt new file mode 100644 index 0000000..6af7333 --- /dev/null +++ b/ros_ws/src/recognition_objects_classifier/CMakeLists.txt @@ -0,0 +1,92 @@ +cmake_minimum_required(VERSION 2.8.3) +project(recognition_objects_classifier) + +add_compile_options(-std=c++11) + +find_package(catkin REQUIRED COMPONENTS + roscpp + message_generation + geometry_msgs + ai_game_manager + processing_lidar_objects + processing_belt_interpreter + std_msgs + tf2 + tf2_ros + dynamic_reconfigure +) + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +add_message_files( + FILES + CircleObstacleStamped.msg + ClassifiedObjects.msg + SegmentObstacleStamped.msg +) + +generate_messages( + DEPENDENCIES + processing_lidar_objects + processing_belt_interpreter + geometry_msgs + std_msgs +) + +generate_dynamic_reconfigure_options( + cfg/ObjectsClassifier.cfg +) + +################################### +## catkin specific configuration ## +################################### + +catkin_package( + CATKIN_DEPENDS message_runtime roscpp + INCLUDE_DIRS include + #LIBRARIES ${PROJECT_NAME} +) + +set(NLOHMANN_JSON_HEADER_LOCATION "$ENV{UTCOUPE_WORKSPACE}/libs/json/single_include/") + +include_directories( + include + ${NLOHMANN_JSON_HEADER_LOCATION} + ${catkin_INCLUDE_DIRS} +) + + +add_executable(objects_classifier_node + src/objects_classifier_node.cpp src/objects_listener.cpp src/shapes.cpp src/processing_thread.cpp + src/main_thread.cpp src/map_objects.cpp src/markers_publisher.cpp) + +add_dependencies(objects_classifier_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) + + +target_link_libraries( + objects_classifier_node + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +install ( + TARGETS objects_classifier_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install( + DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +) + +install( + DIRECTORY cfg/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/cfg +) diff --git a/ros_ws/src/recognition_objects_classifier/cfg/ObjectsClassifier.cfg b/ros_ws/src/recognition_objects_classifier/cfg/ObjectsClassifier.cfg new file mode 100755 index 0000000..fb8774d --- /dev/null +++ b/ros_ws/src/recognition_objects_classifier/cfg/ObjectsClassifier.cfg @@ -0,0 +1,11 @@ +#!/usr/bin/env python +PACKAGE = "recognition_objects_classifier" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("MIN_MAP_FRAC", double_t, 0, "Minimum fraction of a rect to be in map for it to be considered static", 0.35, 0, 1) +gen.add("WALLS_MARGIN", double_t, 0, "Inside margins of the walls that are still considered in 'map' (m)", 0.1, 0, 0.2) + +exit(gen.generate(PACKAGE, "objects_classifier_node", "ObjectsClassifier")) \ No newline at end of file diff --git a/ros_ws/src/recognition_objects_classifier/include/main_thread.h b/ros_ws/src/recognition_objects_classifier/include/main_thread.h new file mode 100644 index 0000000..9ccfe7e --- /dev/null +++ b/ros_ws/src/recognition_objects_classifier/include/main_thread.h @@ -0,0 +1,107 @@ + +#ifndef RECOGNITION_OBJECTS_CLASSIFIER_MAIN_THREAD_H +#define RECOGNITION_OBJECTS_CLASSIFIER_MAIN_THREAD_H + +#include + +#include +#include + +#include +#include "processing_lidar_objects/Obstacles.h" +#include + +#include +#include "recognition_objects_classifier/ObjectsClassifierConfig.h" + +#include "map_objects.h" +#include "processing_thread.h" +#include "markers_publisher.h" + + +struct Point { + float x; + float y; + bool is_map; +}; + +const std::string PUB_TOPIC = "/recognition/objects_classifier/objects"; + +const int SENSORS_NBR = 6; + +// maximum number of points created per rect +const int MAX_POINTS = 500; + +const int THREADS_NBR = 6; + +// discretization steps (m) +const float STEP_X = 0.02; +const float STEP_Y = 0.02; + +// if the absolute time diff between the received time and the header time is +// greater than this (s), adjusts the header time (for rects) +const float TIME_DIFF_MAX = 0.05; + +// if a circle has a velocity above this value, it is considered unknown +const float CIRCLE_SPEED_MAX = 1.0; + +const double SECS_BETWEEN_RVIZ_PUB = 0.08; + +class MainThread { +protected: + // minimum fraction of a rect to be in map for it to be considered static + // not const because of dynamic reconfigure + float MIN_MAP_FRAC = 0.35; + + ros::NodeHandle &nh_; + + recognition_objects_classifier::ClassifiedObjects classified_objects_; + std::mutex lists_mutex_; + + ros::Publisher pub_; + + ros::Time last_rviz_rect_pub_; + ros::Time last_rviz_lidar_pub_; + + Point points_[SENSORS_NBR * MAX_POINTS]; + std::vector > rects_transforms_; + std::vector > threads_; + + MapObjects map_objects_; + + MarkersPublisher markers_publisher_; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tl_; + + dynamic_reconfigure::Server dyn_server_; + + void pub_loop(const ros::TimerEvent &); + + std::pair compute_division_steps( + const processing_belt_interpreter::RectangleStamped &rect); + + bool fetch_transform_and_adjust_stamp( + processing_belt_interpreter::RectangleStamped &rect, + geometry_msgs::TransformStamped &transform_out); + + void transform_rect( + processing_belt_interpreter::RectangleStamped &rect, + geometry_msgs::TransformStamped &transform); + + void notify_threads_and_wait(int num_points); + + void reconfigure_callback(recognition_objects_classifier::ObjectsClassifierConfig &config, uint32_t level); + +public: + MainThread(ros::NodeHandle &nh); + + ~MainThread(); + + void classify_and_publish_rects(processing_belt_interpreter::BeltRects &rects); + + void classify_and_publish_lidar_objects(processing_lidar_objects::Obstacles &obstacles); +}; + + +#endif //RECOGNITION_OBJECTS_CLASSIFIER_MAIN_THREAD_H diff --git a/ros_ws/src/recognition_objects_classifier/include/map_objects.h b/ros_ws/src/recognition_objects_classifier/include/map_objects.h new file mode 100644 index 0000000..5752ef7 --- /dev/null +++ b/ros_ws/src/recognition_objects_classifier/include/map_objects.h @@ -0,0 +1,33 @@ + +#ifndef PROJECT_MAP_OBJECTS_H +#define PROJECT_MAP_OBJECTS_H + +#include +#include + +#include + +#include "shapes.h" + + +const std::string MAP_GET_SERVICE = "/memory/map/get"; +const std::string MAP_OBJECTS = "/terrain/walls/layer_belt/*"; + +class MapObjects { +protected: + std::vector> map_shapes_; + ros::NodeHandle &nh_; + +public: + float WALLS_MARGIN = 0.03; + + void fetch_map_objects(); + + bool contains_point(float x, float y); + + MapObjects(ros::NodeHandle &nh) : + nh_(nh) {} + +}; + +#endif //PROJECT_MAP_OBJECTS_H diff --git a/ros_ws/src/recognition_objects_classifier/include/markers_publisher.h b/ros_ws/src/recognition_objects_classifier/include/markers_publisher.h new file mode 100644 index 0000000..588fd61 --- /dev/null +++ b/ros_ws/src/recognition_objects_classifier/include/markers_publisher.h @@ -0,0 +1,40 @@ + +#ifndef PROJECT_MARKERS_PUBLISHER_H +#define PROJECT_MARKERS_PUBLISHER_H + +#include +#include + +#include +#include + +#include "processing_belt_interpreter/RectangleStamped.h" +#include "recognition_objects_classifier/CircleObstacleStamped.h" +#include "recognition_objects_classifier/SegmentObstacleStamped.h" + +const std::string MARKERS_TOPIC = "/visualization_markers/objects"; +const float LIFETIME = 0.15; +const float Z_POS = 0.2; +const float Z_SCALE = 0.2; + +class MarkersPublisher { +protected: + ros::Publisher pub_; + +public: + void publish_rects(const std::vector &map_rects, + const std::vector &unknown_rects); + + void publish_circles(const std::vector &map_circles, + const std::vector &unknown_circles); + + void publish_segments(const std::vector &map_segments, + const std::vector &unknown_segments); + + bool is_connected(); + + MarkersPublisher(ros::NodeHandle &nh) : + pub_(nh.advertise(MARKERS_TOPIC, 6)) {} +}; + +#endif //PROJECT_MARKERS_PUBLISHER_H diff --git a/ros_ws/src/recognition_objects_classifier/include/objects_listener.h b/ros_ws/src/recognition_objects_classifier/include/objects_listener.h new file mode 100644 index 0000000..db2878c --- /dev/null +++ b/ros_ws/src/recognition_objects_classifier/include/objects_listener.h @@ -0,0 +1,40 @@ + +#ifndef RECOGNITION_OBJECTS_CLASSIFIER_OBJECTS_LISTENER_H +#define RECOGNITION_OBJECTS_CLASSIFIER_OBJECTS_LISTENER_H + +#include "ros/ros.h" +#include "main_thread.h" +#include "processing_belt_interpreter/BeltRects.h" +#include "processing_lidar_objects/Obstacles.h" + +#include + +using namespace processing_belt_interpreter; +using namespace processing_lidar_objects; + +const std::string BELT_TOPIC = "/processing/belt_interpreter/rects"; +const std::string LIDAR_TOPIC = "/processing/lidar_objects/obstacles"; + + +class ObjectsListener { +protected: + ros::Subscriber belt_sub_; + ros::Subscriber lidar_sub_; + MainThread &main_thread_; + + void on_belt_callback(const BeltRectsConstPtr &rects); + + void on_lidar_callback(const ObstaclesConstPtr &obstacles); + +public: + BeltRects rects; + Obstacles lidar_obstacles; + + ObjectsListener(ros::NodeHandle &nh, MainThread &main_thread) : + main_thread_(main_thread), + belt_sub_(nh.subscribe(BELT_TOPIC, 1, &ObjectsListener::on_belt_callback, this)), + lidar_sub_(nh.subscribe(LIDAR_TOPIC, 1, &ObjectsListener::on_lidar_callback, this)) {} + +}; + +#endif //RECOGNITION_OBJECTS_CLASSIFIER_OBJECTS_LISTENER_H diff --git a/ros_ws/src/recognition_objects_classifier/include/processing_thread.h b/ros_ws/src/recognition_objects_classifier/include/processing_thread.h new file mode 100644 index 0000000..0b53d13 --- /dev/null +++ b/ros_ws/src/recognition_objects_classifier/include/processing_thread.h @@ -0,0 +1,67 @@ + +#ifndef RECOGNITION_OBJECTS_CLASSIFIER_PROCESSING_THREAD_H +#define RECOGNITION_OBJECTS_CLASSIFIER_PROCESSING_THREAD_H + +#include +#include +#include + +#include + +#include "map_objects.h" + +struct Point; + +class ProcessingThread { +protected: + std::thread processing_thread_; + bool thread_stopped_; + + unsigned int start_idx_; + unsigned int length_; + Point *points_; + std::vector > &transforms_; + MapObjects &map_; + + // to pause the thread + std::mutex mutex_; + std::condition_variable cv_; + + // informs the processing_thread to start processing + bool ready_; + + // informs the main thread that the processing is finished + bool processed_; + + void classify_points(); + +public: + void start(); + + void stop(); + + void join(); + + void notify(unsigned int start_idx, unsigned int length); + + void wait_processing(); + + ProcessingThread(Point *points, std::vector > &transforms, + MapObjects &map) : + points_(points), + transforms_(transforms), + thread_stopped_(false), + ready_(false), + processed_(false), + map_(map) {} + + ProcessingThread(const ProcessingThread &other) : + points_(other.points_), + transforms_(other.transforms_), + thread_stopped_(other.thread_stopped_), + ready_(other.ready_), + processed_(other.processed_), + map_(other.map_) {} +}; + +#endif //RECOGNITION_OBJECTS_CLASSIFIER_PROCESSING_THREAD_H diff --git a/ros_ws/src/recognition_objects_classifier/include/shapes.h b/ros_ws/src/recognition_objects_classifier/include/shapes.h new file mode 100644 index 0000000..7983cc0 --- /dev/null +++ b/ros_ws/src/recognition_objects_classifier/include/shapes.h @@ -0,0 +1,36 @@ + +#ifndef RECOGNITION_OBJECTS_CLASSIFIER_SHAPES_H +#define RECOGNITION_OBJECTS_CLASSIFIER_SHAPES_H + +class Shape { +public: + virtual bool contains_point(float x, float y) const = 0; + + Shape() {} + + Shape(const Shape &other) {} +}; + +class Circle : public Shape { +protected: + float x_, y_, radius_; +public: + Circle(float x, float y, float radius) : + x_(x), y_(y), radius_(radius) {} + + bool contains_point(float x, float y) const; +}; + +class Rectangle : public Shape { +protected: + float x_, y_, width_, height_; +public: + Rectangle(float x, float y, float width, float height) : + x_(x), y_(y), width_(width), height_(height) {} + + bool contains_point(float x, float y) const; +}; + +// TODO: polygon + +#endif //RECOGNITION_OBJECTS_CLASSIFIER_SHAPES_H diff --git a/ros_ws/src/recognition_objects_classifier/msg/CircleObstacleStamped.msg b/ros_ws/src/recognition_objects_classifier/msg/CircleObstacleStamped.msg new file mode 100644 index 0000000..8b6f9a9 --- /dev/null +++ b/ros_ws/src/recognition_objects_classifier/msg/CircleObstacleStamped.msg @@ -0,0 +1,2 @@ +Header header +processing_lidar_objects/CircleObstacle circle \ No newline at end of file diff --git a/ros_ws/src/recognition_objects_classifier/msg/ClassifiedObjects.msg b/ros_ws/src/recognition_objects_classifier/msg/ClassifiedObjects.msg new file mode 100644 index 0000000..fa24d26 --- /dev/null +++ b/ros_ws/src/recognition_objects_classifier/msg/ClassifiedObjects.msg @@ -0,0 +1,8 @@ +processing_belt_interpreter/RectangleStamped[] map_rects +recognition_objects_classifier/CircleObstacleStamped[] map_circles +recognition_objects_classifier/SegmentObstacleStamped[] map_segments + +processing_belt_interpreter/RectangleStamped[] unknown_rects +recognition_objects_classifier/CircleObstacleStamped[] unknown_circles +recognition_objects_classifier/SegmentObstacleStamped[] unknown_segments + diff --git a/ros_ws/src/recognition_objects_classifier/msg/SegmentObstacleStamped.msg b/ros_ws/src/recognition_objects_classifier/msg/SegmentObstacleStamped.msg new file mode 100644 index 0000000..cbe9ead --- /dev/null +++ b/ros_ws/src/recognition_objects_classifier/msg/SegmentObstacleStamped.msg @@ -0,0 +1,2 @@ +Header header +processing_lidar_objects/SegmentObstacle segment \ No newline at end of file diff --git a/ros_ws/src/recognition_objects_classifier/package.xml b/ros_ws/src/recognition_objects_classifier/package.xml new file mode 100644 index 0000000..ff2bb29 --- /dev/null +++ b/ros_ws/src/recognition_objects_classifier/package.xml @@ -0,0 +1,25 @@ + + + recognition_objects_classifier + 0.0.1 + + Takes objects (rects, circles, segments) and classifies them as outside the map or as inside the map. + + GPLv3 + milesial + UTCoupe + catkin + message_generation + geometry_msgs + roscpp + std_msgs + tf2 + tf2_ros + processing_lidar_objects + processing_belt_interpreter + ai_game_manager + dynamic_reconfigure + message_runtime + + + diff --git a/ros_ws/src/recognition_objects_classifier/src/main_thread.cpp b/ros_ws/src/recognition_objects_classifier/src/main_thread.cpp new file mode 100644 index 0000000..a3cccb1 --- /dev/null +++ b/ros_ws/src/recognition_objects_classifier/src/main_thread.cpp @@ -0,0 +1,325 @@ +#include + +#include +#include +#include +#include + +#include +#include + +#include "main_thread.h" + + +void MainThread::classify_and_publish_rects(processing_belt_interpreter::BeltRects &rects) { + + { + std::lock_guard lk(lists_mutex_); + classified_objects_.map_rects.clear(); + classified_objects_.unknown_rects.clear(); + } + + if (rects.rects.empty()) + return; + + double time = ros::Time::now().toSec(); + double step_x, step_y; + + int end_idx[rects.rects.size()]; + + unsigned int rect_idx = 0; + unsigned int point_idx = 0; + + geometry_msgs::TransformStamped transform_stamped; + + rects_transforms_.clear(); + + for (auto &rect : rects.rects) { + + std::tie(step_x, step_y) = compute_division_steps(rect); + + if (!fetch_transform_and_adjust_stamp(rect, transform_stamped)) { + end_idx[rect_idx] = point_idx - 1; + rect_idx++; + continue; + } + + for (float x = rect.x - rect.w / 2; x <= rect.x + rect.w / 2; x += step_x) { + for (float y = rect.y - rect.h / 2; y <= rect.y + rect.h / 2; y += step_y) { + this->points_[point_idx].x = x; + this->points_[point_idx].y = y; + point_idx++; + } + } + + rects_transforms_.push_back({point_idx - 1, transform_stamped}); + + end_idx[rect_idx] = point_idx - 1; + rect_idx++; + + transform_rect(rect, transform_stamped); + } + + if (point_idx == 0) + return; + + notify_threads_and_wait(point_idx); + + std::lock_guard lk(lists_mutex_); + + // populate rect classified arrays + + int running_idx = 0; + unsigned int nbr_map = 0; + for (int r = 0; r < rects.rects.size(); r++) { + + if (end_idx[r] == -1 || (r > 0 && end_idx[r] == end_idx[r - 1])) + continue; + + nbr_map = 0; + for (int p = running_idx; p <= end_idx[r]; p++) { + if (points_[p].is_map) + nbr_map++; + } + + float frac_map = (float) nbr_map / (float) (end_idx[r] - running_idx); + running_idx = end_idx[r] + 1; + + if (frac_map >= MIN_MAP_FRAC) { + classified_objects_.map_rects.push_back(rects.rects[r]); + } else { + classified_objects_.unknown_rects.push_back(rects.rects[r]); + } + } + + if (!classified_objects_.unknown_rects.empty() || !classified_objects_.map_rects.empty()) { + pub_.publish(classified_objects_); + } + + if (ros::Time::now().toSec() - last_rviz_rect_pub_.toSec() > SECS_BETWEEN_RVIZ_PUB && + markers_publisher_.is_connected()) { + markers_publisher_.publish_rects(classified_objects_.map_rects, classified_objects_.unknown_rects); + last_rviz_rect_pub_ = ros::Time::now(); + } + + classified_objects_.map_rects.clear(); + classified_objects_.unknown_rects.clear(); + + time = ros::Time::now().toSec() - time; + + ROS_DEBUG_STREAM("Took " << time << " secs to process " << rects.rects.size() << " rects, " << point_idx << " points"); +} + +std::pair MainThread::compute_division_steps( + const processing_belt_interpreter::RectangleStamped &rect) { + + auto samples_x = static_cast(rect.w / STEP_X); + auto samples_y = static_cast(rect.h / STEP_Y); + + float step_x = STEP_X; + float step_y = STEP_Y; + + if (samples_x * samples_y > MAX_POINTS) { + step_x = static_cast(rect.w / sqrt(MAX_POINTS)); + step_y = static_cast(rect.h / sqrt(MAX_POINTS)); + + samples_x = static_cast(rect.w / step_x); + samples_y = static_cast(rect.h / step_y); + } + + if (samples_x < 2) { + step_x = rect.w; + } + if (samples_y < 2) { + step_y = rect.h; + } + + return {step_x, step_y}; +} + +bool MainThread::fetch_transform_and_adjust_stamp( + processing_belt_interpreter::RectangleStamped &rect, + geometry_msgs::TransformStamped &transform_out) { + + ros::Time common_time; + std::string err_msg; + + try { + tf_buffer_._getLatestCommonTime( + tf_buffer_._lookupFrameNumber("map"), + tf_buffer_._lookupFrameNumber(rect.header.frame_id), + common_time, &err_msg + ); + + // adjust the timestamp of the rect if it's a bit later than the last transform + if (rect.header.stamp > common_time && + fabs(rect.header.stamp.toSec() - common_time.toSec()) < TIME_DIFF_MAX) { + rect.header.stamp = common_time; + } + + transform_out = tf_buffer_.lookupTransform("map", rect.header.frame_id, rect.header.stamp); + + } catch (tf2::TransformException &ex) { + ROS_WARN_STREAM(ex.what()); + return false; + } + + return true; +} + + +void MainThread::transform_rect(processing_belt_interpreter::RectangleStamped &rect, + geometry_msgs::TransformStamped &transform) { + + geometry_msgs::PoseStamped pose_static_frame, pose_map_frame; + + pose_static_frame.header = rect.header; + pose_static_frame.pose.position.x = rect.x; + pose_static_frame.pose.position.y = rect.y; + pose_static_frame.pose.orientation = tf::createQuaternionMsgFromYaw(rect.a); + + tf2::doTransform(pose_static_frame, pose_map_frame, transform); + + rect.x = static_cast(pose_map_frame.pose.position.x); + rect.y = static_cast(pose_map_frame.pose.position.y); + rect.a = static_cast(tf::getYaw(pose_map_frame.pose.orientation)); + rect.header.frame_id = transform.header.frame_id; +} + +void MainThread::notify_threads_and_wait(int num_points) { + + auto size = static_cast(ceil((double) num_points / (double) THREADS_NBR)); + + unsigned int used_threads = 0; + for (int t = 0; t < THREADS_NBR; t++) { + // we finished the list + if (t * size >= num_points) + break; + + used_threads++; + + // end of the list + if (t * size + size >= num_points) + threads_[t]->notify(t * size, size - 1); + else + threads_[t]->notify(t * size, size); + } + + for (int t = 0; t < used_threads; t++) { + threads_[t]->wait_processing(); + } +} + +void MainThread::classify_and_publish_lidar_objects(processing_lidar_objects::Obstacles &obstacles) { + + double time = ros::Time::now().toSec(); + + std::lock_guard lk(lists_mutex_); + + classified_objects_.map_circles.clear(); + classified_objects_.unknown_circles.clear(); + classified_objects_.map_segments.clear(); + classified_objects_.unknown_segments.clear(); + + recognition_objects_classifier::CircleObstacleStamped circle_s; + circle_s.header = obstacles.header; + + recognition_objects_classifier::SegmentObstacleStamped segment_s; + segment_s.header = obstacles.header; + + // remove segments that are inside circles + for (auto &circle : obstacles.circles) { + + // remove segments that are inside circles + for (auto it = obstacles.segments.begin(); it != obstacles.segments.end();) { + if (pow(it->first_point.x - circle.center.x, 2) + pow(it->first_point.y - circle.center.y, 2) + <= pow(circle.true_radius, 2) && + pow(it->last_point.x - circle.center.x, 2) + pow(it->last_point.y - circle.center.y, 2) + <= pow(circle.true_radius, 2)) { + + it = obstacles.segments.erase(it); + } else { + ++it; + } + } + + // classify circle + circle_s.circle = circle; + if (pow(circle.velocity.x, 2) + pow(circle.velocity.y, 2) + pow(circle.velocity.z, 2) >= + pow(CIRCLE_SPEED_MAX, 2) || !map_objects_.contains_point(static_cast(circle.center.x), + static_cast(circle.center.y))) { + + classified_objects_.unknown_circles.push_back(circle_s); + } else { + classified_objects_.map_circles.push_back(circle_s); + } + } + + for (auto &segment : obstacles.segments) { + segment_s.segment = segment; + + if (map_objects_.contains_point(static_cast(segment.first_point.x), + static_cast(segment.first_point.y)) && + map_objects_.contains_point(static_cast(segment.last_point.x), + static_cast(segment.last_point.y))) { + classified_objects_.map_segments.push_back(segment_s); + } else { + classified_objects_.unknown_segments.push_back(segment_s); + } + } + if (!classified_objects_.unknown_segments.empty() || !classified_objects_.map_segments.empty() + || !classified_objects_.unknown_circles.empty() || !classified_objects_.map_circles.empty()) { + pub_.publish(classified_objects_); + } + + if (ros::Time::now().toSec() - last_rviz_lidar_pub_.toSec() > SECS_BETWEEN_RVIZ_PUB && + markers_publisher_.is_connected()) { + + markers_publisher_.publish_segments(classified_objects_.map_segments, classified_objects_.unknown_segments); + markers_publisher_.publish_circles(classified_objects_.map_circles, classified_objects_.unknown_circles); + last_rviz_lidar_pub_ = ros::Time::now(); + } + + classified_objects_.map_circles.clear(); + classified_objects_.unknown_circles.clear(); + classified_objects_.map_segments.clear(); + classified_objects_.unknown_segments.clear(); + + time = ros::Time::now().toSec() - time; + + ROS_DEBUG_STREAM("Took " << time << " secs to process lidar data"); + +} + +void MainThread::reconfigure_callback(recognition_objects_classifier::ObjectsClassifierConfig &config, uint32_t level) { + MIN_MAP_FRAC = config.MIN_MAP_FRAC; + map_objects_.WALLS_MARGIN = config.WALLS_MARGIN; + ROS_INFO_STREAM("Setting MIN_MAP_FRAC to " << MIN_MAP_FRAC << " and WALLS_MARGIN to " << map_objects_.WALLS_MARGIN); +} + +MainThread::MainThread(ros::NodeHandle &nh) : + nh_(nh), + pub_(nh.advertise(PUB_TOPIC, 1)), + tl_(tf_buffer_), + markers_publisher_(nh), + map_objects_(nh), + last_rviz_lidar_pub_(ros::Time::now()), + last_rviz_rect_pub_(ros::Time::now()) { + + map_objects_.fetch_map_objects(); + + dyn_server_.setCallback(boost::bind(&MainThread::reconfigure_callback, this, _1, _2)); + + for (int i = 0; i < THREADS_NBR; i++) { + threads_.push_back( + std::unique_ptr(new ProcessingThread(points_, rects_transforms_, map_objects_))); + threads_[i]->start(); + } +} + +MainThread::~MainThread() { + for (auto &thread : threads_) { + thread->stop(); + thread->join(); + } +} diff --git a/ros_ws/src/recognition_objects_classifier/src/map_objects.cpp b/ros_ws/src/recognition_objects_classifier/src/map_objects.cpp new file mode 100644 index 0000000..7b8d000 --- /dev/null +++ b/ros_ws/src/recognition_objects_classifier/src/map_objects.cpp @@ -0,0 +1,72 @@ +#include +#include + +#include + +#include "map_objects.h" + +using namespace nlohmann; + +void MapObjects::fetch_map_objects() { + ros::ServiceClient client = nh_.serviceClient(MAP_GET_SERVICE); + + client.waitForExistence(); + + memory_map::MapGet srv; + + srv.request.request_path = MAP_OBJECTS; + + if (client.call(srv) && srv.response.success) { + auto objects = json::parse(srv.response.response); + + map_shapes_.clear(); + for (auto &object : objects) { + + if (object["position"]["frame_id"] != "/map") { + ROS_ERROR("Map object not in /map !"); + continue; + } + + + std::string type = object["shape"]["type"]; + + float x = object["position"]["x"]; + float y = object["position"]["y"]; + + if (type == "rect") { + float height = object["shape"]["height"]; + float width = object["shape"]["width"]; + + map_shapes_.push_back(std::make_shared(x, y, width, height)); + + } else if (type == "circle") { + float radius = object["shape"]["radius"]; + + map_shapes_.push_back(std::make_shared(x, y, radius)); + + } else { + ROS_ERROR("Polygons from map not supported !"); + } + + } + + ROS_INFO_STREAM("Fetched " << map_shapes_.size() << " map shapes successfully"); + + } else { + ROS_ERROR("Failed to contact memory_map, static objects not fetched"); + } +} + +bool MapObjects::contains_point(float x, float y) { + // TODO: remove hardcoded values and fetch from map + + if (x > 3 - WALLS_MARGIN || x < WALLS_MARGIN || y < WALLS_MARGIN || y > 2 - WALLS_MARGIN) + return true; + + for (auto &map_shape : map_shapes_) { + if (map_shape->contains_point(x, y)) + return true; + } + + return false; +} diff --git a/ros_ws/src/recognition_objects_classifier/src/markers_publisher.cpp b/ros_ws/src/recognition_objects_classifier/src/markers_publisher.cpp new file mode 100644 index 0000000..81cf15e --- /dev/null +++ b/ros_ws/src/recognition_objects_classifier/src/markers_publisher.cpp @@ -0,0 +1,138 @@ + +#include + +#include "markers_publisher.h" + +void MarkersPublisher::publish_rects( + const std::vector &map_rects, + const std::vector &unknown_rects) { + + static unsigned int id_counter = 0; + + visualization_msgs::Marker marker; + marker.action = marker.ADD; + marker.type = marker.CUBE; + marker.color.a = 1.0; + marker.color.b = 0.0; + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.lifetime = ros::Duration(LIFETIME); + marker.pose.position.z = Z_POS; + marker.scale.z = Z_SCALE; + marker.ns = "rects"; + + const std::vector both_lists[2] = {std::move(map_rects), + std::move(unknown_rects)}; + + for (const auto &list : both_lists) { + for (auto &rect : list) { + marker.pose.position.x = rect.x; + marker.pose.position.y = rect.y; + marker.pose.orientation = tf::createQuaternionMsgFromYaw(rect.a); + marker.scale.x = rect.w; + marker.scale.y = rect.h; + marker.header = rect.header; + + marker.id = id_counter++; + pub_.publish(marker); + } + + marker.color.r = 1.0; + marker.color.g = 0.0; + } +} + +void MarkersPublisher::publish_circles( + const std::vector &map_circles, + const std::vector &unknown_circles) { + + static unsigned int id_counter = 0; + + visualization_msgs::Marker marker; + marker.action = marker.ADD; + marker.type = marker.CYLINDER; + marker.color.a = 1.0; + marker.color.b = 0.0; + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.lifetime = ros::Duration(LIFETIME); + marker.pose.position.z = Z_POS; + marker.scale.z = Z_SCALE; + marker.ns = "circles"; + + const std::vector both_lists[2] = {std::move(map_circles), + std::move(unknown_circles)}; + + for (const auto &list : both_lists) { + for (auto &circle : list) { + + marker.pose.position.x = circle.circle.center.x; + marker.pose.position.y = circle.circle.center.y; + marker.scale.x = circle.circle.true_radius * 2.0; + marker.scale.y = circle.circle.true_radius * 2.0; + marker.header = circle.header; + + marker.id = id_counter++; + pub_.publish(marker); + } + + marker.color.r = 1.0; + marker.color.g = 0.0; + } +} + +void MarkersPublisher::publish_segments( + const std::vector &map_segments, + const std::vector &unknown_segments) { + + + static unsigned int id_counter = 0; + + std::vector points; + + visualization_msgs::Marker marker; + marker.action = marker.ADD; + marker.type = marker.LINE_LIST; + marker.color.a = 1.0; + marker.color.b = 0.0; + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.lifetime = ros::Duration(LIFETIME); + marker.pose.position.z = Z_POS; + marker.ns = "segments"; + + geometry_msgs::Point point; + + const std::vector both_lists[2] = {std::move(map_segments), + std::move(unknown_segments)}; + + for (const auto &list : both_lists) { + points.clear(); + + + for (auto &segment : list) { + point.x = segment.segment.first_point.x; + point.y = segment.segment.first_point.y; + points.push_back(point); + + point.x = segment.segment.last_point.x; + point.y = segment.segment.last_point.y; + points.push_back(point); + } + + if (!list.empty()) { + marker.header = list.at(0).header; + marker.id = id_counter++; + pub_.publish(marker); + } + + marker.color.r = 1.0; + marker.color.g = 0.0; + } + + +} + +bool MarkersPublisher::is_connected() { + return pub_.getNumSubscribers() > 0; +} \ No newline at end of file diff --git a/ros_ws/src/recognition_objects_classifier/src/objects_classifier_node.cpp b/ros_ws/src/recognition_objects_classifier/src/objects_classifier_node.cpp new file mode 100644 index 0000000..04d4c3d --- /dev/null +++ b/ros_ws/src/recognition_objects_classifier/src/objects_classifier_node.cpp @@ -0,0 +1,19 @@ +#include +#include + +#include "main_thread.h" +#include "objects_listener.h" + +int main(int argc, char **argv) { + ros::init(argc, argv, "objects_classifier"); + + ros::NodeHandle node_handle; + + MainThread mt(node_handle); + ObjectsListener ol(node_handle, mt); + + StatusServices("recognition", "objects_classifier").setReady(true); + + ros::spin(); + return 0; +} diff --git a/ros_ws/src/recognition_objects_classifier/src/objects_listener.cpp b/ros_ws/src/recognition_objects_classifier/src/objects_listener.cpp new file mode 100644 index 0000000..734e5ca --- /dev/null +++ b/ros_ws/src/recognition_objects_classifier/src/objects_listener.cpp @@ -0,0 +1,11 @@ +#include "objects_listener.h" + +void ObjectsListener::on_belt_callback(const BeltRectsConstPtr &rects) { + this->rects = *rects; + main_thread_.classify_and_publish_rects(this->rects); +} + +void ObjectsListener::on_lidar_callback(const ObstaclesConstPtr &obstacles) { + this->lidar_obstacles = *obstacles; + main_thread_.classify_and_publish_lidar_objects(this->lidar_obstacles); +} \ No newline at end of file diff --git a/ros_ws/src/recognition_objects_classifier/src/processing_thread.cpp b/ros_ws/src/recognition_objects_classifier/src/processing_thread.cpp new file mode 100644 index 0000000..6b18939 --- /dev/null +++ b/ros_ws/src/recognition_objects_classifier/src/processing_thread.cpp @@ -0,0 +1,84 @@ +#include +#include +#include +#include +#include + + +#include "processing_thread.h" +#include "main_thread.h" + +void ProcessingThread::start() { + processing_thread_ = std::thread(&ProcessingThread::classify_points, this); +} + +void ProcessingThread::classify_points() { + + geometry_msgs::PointStamped point_static_frame, point_map_frame; + int rect_idx; + int point_idx; + int min_idx; + + while (!thread_stopped_) { + // wait until main thread notification + std::unique_lock lk(mutex_); + cv_.wait(lk, [this] { return ready_; }); + + // notif received + ready_ = false; + processed_ = false; + + point_idx = start_idx_; + + for (auto &pair : transforms_) { + if (pair.first < start_idx_) + continue; + + min_idx = pair.first < start_idx_ + length_ ? pair.first : start_idx_ + length_; + + for (; point_idx <= min_idx; point_idx++) { + + point_static_frame.point.x = points_[point_idx].x; + point_static_frame.point.y = points_[point_idx].y; + tf2::doTransform(point_static_frame, point_map_frame, pair.second); + + points_[point_idx].is_map = map_.contains_point(point_map_frame.point.x, point_map_frame.point.y); + } + + if (pair.first >= start_idx_ + length_) + break; + + } + + // unlocks guard and notifies main thread we finished + processed_ = true; + lk.unlock(); + cv_.notify_one(); + } +} + +void ProcessingThread::join() { + processing_thread_.join(); +} + +void ProcessingThread::notify(unsigned int start_idx, unsigned int length) { + // changes condition variable and notify so the thread can wake up + { + std::lock_guard lk(mutex_); + start_idx_ = start_idx; + length_ = length; + ready_ = true; + cv_.notify_one(); + } +} + + +void ProcessingThread::wait_processing() { + std::unique_lock lk(mutex_); + cv_.wait(lk, [this] { return processed_; }); +} + +void ProcessingThread::stop() { + thread_stopped_ = true; + notify(0, 0); +} \ No newline at end of file diff --git a/ros_ws/src/recognition_objects_classifier/src/shapes.cpp b/ros_ws/src/recognition_objects_classifier/src/shapes.cpp new file mode 100644 index 0000000..b2f3fec --- /dev/null +++ b/ros_ws/src/recognition_objects_classifier/src/shapes.cpp @@ -0,0 +1,15 @@ +#include "shapes.h" + +bool Circle::contains_point(float x, float y) const { + float diff_x = x - x_; + float diff_y = y - y; + float dist_sqr = diff_x * diff_x + diff_y * diff_y; + return dist_sqr < radius_ * radius_; +} + +bool Rectangle::contains_point(float x, float y) const { + return x >= x_ - width_ / 2 + and x <= x_ + width_ / 2 + and y >= y_ - height_ / 2 + and y <= y_ + height_ / 2; +} \ No newline at end of file diff --git a/scripts/install_external_nodes.sh b/scripts/install_external_nodes.sh new file mode 100755 index 0000000..f174021 --- /dev/null +++ b/scripts/install_external_nodes.sh @@ -0,0 +1,47 @@ +#!/bin/bash + +# The goal of this script is to compile and install external ROS nodes used by the UTCoupe nodes. This avoids to compile the external nodes each time the UTCoupe workspace is recompiled. + +ARCH=$(uname -m) +INSTALL_ROOT=/opt/ros/kinetic +TMP_ROOT=/tmp/utcoupe_ws +TMP_SRC=$TMP_ROOT/src + +# Fill this structure to add other external nodes +declare -A external_nodes +external_nodes=( + ["teraranger"]="https://github.com/Terabee/teraranger" + ["processing_lidar_objects"]="https://github.com/utcoupe/obstacle_detector.git" +) + +function fetch_missing_nodes() { + mkdir -p $TMP_SRC + cd $TMP_SRC + for node_name in "${!external_nodes[@]}"; do + if [ ! -d $INSTALL_ROOT/share/$node_name ]; then + echo "$node_name is not installed, fetch it." + git clone "${external_nodes[$node_name]}" + else + echo "$node_name is already installed installed." + fi + done +} + +function compile_and_install_nodes() { + if [ ! -z "$(ls -A $TMP_SRC)" ]; then + cd $TMP_ROOT + # Kind of ideal command to use, but catkin wont install direclty in ros base workspace because of _setup_util.py file... + #sudo -u $USER -H bash -c "source $INSTALL_ROOT/setup.bash; catkin_make install -DCMAKE_INSTALL_PREFIX=$INSTALL_ROOT -DCMAKE_BUILD_TYPE=Release" + #TODO improve using jobs directly (get the number of the current computer) + catkin_make install -DCMAKE_BUILD_TYPE=Release + sudo cp -ar $TMP_ROOT/install/include/* $INSTALL_ROOT/include + sudo cp -ar $TMP_ROOT/install/share/* $INSTALL_ROOT/share + sudo cp -ar $TMP_ROOT/install/lib/* $INSTALL_ROOT/lib + fi +} + +pushd . > /dev/null +fetch_missing_nodes +compile_and_install_nodes +popd > /dev/null + diff --git a/scripts/install_utcoupe_setup.sh b/scripts/install_utcoupe_setup.sh new file mode 100755 index 0000000..ad85d57 --- /dev/null +++ b/scripts/install_utcoupe_setup.sh @@ -0,0 +1,161 @@ +#!/bin/bash + +### The goal of this script is to install all UTCoupe specific packages to have a working setup. +### When you just cloned the UTCoupe repository, you have to run this script first ! + +function green_echo() { + echo -e "\033[32m$1\033[0m" +} + +function red_echo() { + echo -e "\033[31m$1\033[0m" +} + +# Globals +ARCH=$(uname -m) + +### Install the linux packages +function install_apt() { + green_echo "Install missing packages..." + sudo apt-get install git build-essential python python-pip cmake libboost-dev libsdl1.2-dev gcc-avr avrdude avr-libc libsfml-dev libarmadillo-dev libavcodec-dev libswscale-dev + + # Check if it's a PC or a raspi + if [ "$ARCH" = "x86_64" ]; then + green_echo "x86 architecture detected." + sudo apt-get install nodejs npm nodejs-legacy linux-headers-$(uname -r) + elif [ "$ARCH" = "armv7l" ]; then + green_echo "Raspberry Pi 3 system detected, remove previous npm installation to setup the used version." + sudo apt-get install raspberrypi-kernel-headers + sudo apt-get remove npm nodejs nodejs-legacy + curl -sL https://deb.nodesource.com/setup_4.x | sudo -E bash - + sudo npm install npm@3.5.2 -g + #TODO be sure that all is correct for raspi + elif [ "$ARCH" = "armv6l" ]; then + sudo apt-get install raspberrypi-kernel-headers + sudo apt-get remove npm nodejs nodejs-legacy + wget https://nodejs.org/dist/v4.8.1/node-v4.8.1-linux-armv6l.tar.gz + tar -xvf node-v4.8.1-linux-armv6l.tar.gz + cd node-v4.8.1-linux-armv6l + sudo cp -R * /usr/local/ + cd .. + sudo rm -rf node-v4.8.1-linux-armv6l + else + red_echo "ARCH of system not corresponding to a know arch... ($ARCH)" + fi +} + +### Install ros-desktop-full +function install_ros() { + if [ "$(lsb_release -sc)" = "xenial" ] || [ "$(lsb_release -sc)" = "willy" ]; then + sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' + sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116 + sudo apt-get update + # "Install" Arduino libs needed by us + if [ -d "/usr/share/arduino" ] && [ -d "$PWD/libs/arduino-libraries" ]; then + sudo cp -ar $PWD/libs/arduino-libraries/* /usr/share/arduino/libraries/ + else + red_echo "Unable to locate arduino folder or arduino-libraries extracted folder, please check your setup" + fi + sudo rosdep init + rosdep update + else + red_echo "Your OS is not Ubuntu Willy or Xenial, ROS will not been installed..." + fi +} + +function install_ros_depencies() { + if [ "$ARCH" = "x86_64" ]; then + sudo apt-get install ros-kinetic-desktop-full ros-kinetic-rosserial-arduino ros-kinetic-rosbridge-suite ros-kinetic-tf2-web-republisher ros-kinetic-serial ros-kinetic-dynamixel-sdk ros-kinetic-rosserial-python ros-kinetic-urg-c ros-kinetic-urg-node + elif [ "$ARCH" = "armv7l" ]; then + sudo apt-get install ros-kinetic-ros-base ros-kinetic-tf2 ros-kinetic-tf2-ros ros-kinetic-rviz ros-kinetic-diagnostic-updater ros-kinetic-roslint ros-kinetic-camera-info-manager ros-kinetic-rosserial-arduino ros-kinetic-rosbridge-suite ros-kinetic-tf2-web-republisher ros-kinetic-serial ros-kinetic-dynamixel-sdk ros-kinetic-rosserial-python ros-kinetic-urg-c ros-kinetic-urg-node + fi +} + +### Setup the variable environment to taget the UTCoupe main folder +### This function use $PWD instead of $UTCOUPE_WORKSPACE because if the env variable is not set before running the script, $UTCOUPE_WORKSPACE is unknown +function env_setup() { + # Add the UTCOUPE_WORKSPACE env variable, default consider as bash shell + if [ -z "$UTCOUPE_WORKSPACE" ]; then + green_echo "Env variable is not set." + if [ "$SHELL" = "/bin/zsh" ]; then + echo "export UTCOUPE_WORKSPACE=$PWD" >> $HOME/.zshrc + echo "export ROS_LANG_DISABLE=genlisp:geneus" >> $HOME/.zshrc + echo "export ROSCONSOLE_FORMAT='[\${node}:\${severity}] [\${time}] : \${message}'" >> $HOME/.zshrc + red_echo "Warning :\n" + red_echo "Please \"source ~/.zshrc\" and run again this script if necessary\n" + exit 1 + else + echo "export UTCOUPE_WORKSPACE=$PWD" >> $HOME/.bashrc + echo "export ROS_LANG_DISABLE=genlisp:geneus" >> $HOME/.bashrc + echo "export ROSCONSOLE_FORMAT='[\${node}:\${severity}] [\${time}] : \${message}'" >> $HOME/.bashrc + source $HOME/.bashrc + fi + fi + # Add the current user to the dialout group (to r/w in /dev files) + if ! id -Gn $USER | grep -qw "dialout"; then + sudo usermod -a -G dialout $USER + fi + # Untar all libraries (the copy is done in install_ros function) + for f in $PWD/libs/*; do + if [ ! -d $f ]; then + tar -C $PWD/libs -xzf $f + fi + done + # Add the Ethernet IP address of raspberry pi to have a shortcut + if ! grep "utcoupe" /etc/hosts > /dev/null; then + sudo sh -c "echo '#UTCoupe raspberry pi Ethernet IP when connected on the UTC network\n172.18.159.254 utcoupe_rpi31\n172.18.161.161 utcoupe_rpi32\n172.18.161.162 utcoupe_rpi33' >> /etc/hosts" + fi + # Update the submodules + git submodule update --init --recursive +} + +### Then install the UTCoupe ROS workspace +function install_ros_workspace() { + # Install the UTCoupe ROS specific packages + #TODO use the requirements system + pip install pyserial numpy scipy pymongo pyclipper pillow + printf "Download, compile and install all ROS external nodes needed by UTCoupe, this may take a while." + ./scripts/install_external_nodes.sh +} + +### Main install_script function, ask the user to install each main components +function launch_script() { + + env_setup + + printf "Install apt missing packets ? [Y/n]?" + read answer + if [ "$answer" = "" ] || [ "$answer" = "y" ] || [ "$answer" = "Y" ]; then + install_apt + if [ ! -d "/opt/ros" ]; then + printf "ROS has not been detected in /opt/ros, launch the installation process..." + install_ros + fi + install_ros_depencies + fi + printf "Install UTCoupe ROS workspace (ROS must to be installed) ? [Y/n]?" + read answer + if [ "$answer" = "" ] || [ "$answer" = "y" ] || [ "$answer" = "Y" ]; then + install_ros_workspace + fi +} + +# Check that the folder has been cloned from git and not downloaded, because submodules won't work... +if [ ! -d ".git" ]; then + red_echo "You have to clone this repository from git, not download it." + exit 1 +fi + +# Verify that the script is launched from the right place +if [ ! "${PWD##*/}" = "coupe18" ]; then + red_echo "You have to launch this script from UTCoupe main directory : ./script/${0##*/} or to rename this folder in coupe18." + exit 1 +fi + +# Ask the user if he wants launch the script +printf "Launch install script ? [Y/n]?" +read answer +if [ "$answer" = "" ] || [ "$answer" = "y" ] || [ "$answer" = "Y" ]; then + launch_script + echo "If you run the install script for the first time, please reboot your computer to apply all modifications." +fi diff --git a/scripts/roslaunch_start.sh b/scripts/roslaunch_start.sh new file mode 100755 index 0000000..99716cb --- /dev/null +++ b/scripts/roslaunch_start.sh @@ -0,0 +1,9 @@ +#!/bin/bash +# DO NOT DELETE : USED FOR INIT.D SERVICE +source ~/.bashrc +source /opt/ros/kinetic/setup.sh +source $UTCOUPE_WORKSPACE/ros_ws/devel/setup.sh + +/usr/bin/python /opt/ros/kinetic/bin/roslaunch memory_definitions general.launch robot:=$1 & +sleep 2 + diff --git a/webclient/.eslintignore b/webclient/.eslintignore new file mode 100644 index 0000000..1d68444 --- /dev/null +++ b/webclient/.eslintignore @@ -0,0 +1,3 @@ +app/simulateur/simulateur/* +assets/js/* +gulpfile.js diff --git a/webclient/.eslintrc.json b/webclient/.eslintrc.json new file mode 100644 index 0000000..7925ab8 --- /dev/null +++ b/webclient/.eslintrc.json @@ -0,0 +1,21 @@ +{ + "extends": ["eslint:recommended"], + "parserOptions": { + "ecmaVersion": 6 + }, + "env": { + "browser": true + }, + "globals": { + "angular": false, + "_": false, + "ROSLIB": false, + "Raphael": false, + "$": false, + "THREE": false + }, + "rules": { + "no-console": "off" + } + +} diff --git a/webclient/.gitattributes b/webclient/.gitattributes new file mode 100644 index 0000000..df111f7 --- /dev/null +++ b/webclient/.gitattributes @@ -0,0 +1 @@ +assets/** binary diff --git a/webclient/.gitignore b/webclient/.gitignore new file mode 100644 index 0000000..27bf3a0 --- /dev/null +++ b/webclient/.gitignore @@ -0,0 +1,4 @@ +# NPM +node_modules +*.log +assets/js/* \ No newline at end of file diff --git a/webclient/README.md b/webclient/README.md new file mode 100644 index 0000000..aa605c9 --- /dev/null +++ b/webclient/README.md @@ -0,0 +1,6 @@ +# Nouveau WebClient 2018 + +WebClient tout neuf pour 2018, facilitant le dialogue avec le backend ROS grâce à [roslibjs](https://github.com/RobotWebTools/roslibjs) et [rosbridge\_suite](http://wiki.ros.org/rosbridge_suite). + +Le client est une adaptation du très utile [ros-control-center](https://github.com/pantor/ros-control-center) et utilise la puissance d'[AngularJS](https://angularjs.org/). +Il permet donc d'envoyer des ordres et de recevoir des informations en temps réel en passant par les topics et services ROS. diff --git a/webclient/app/actions/action.component.js b/webclient/app/actions/action.component.js new file mode 100644 index 0000000..a22ee54 --- /dev/null +++ b/webclient/app/actions/action.component.js @@ -0,0 +1,64 @@ +class ServiceController { + constructor($scope, $http, $timeout, Ros) { + this.$scope = $scope; + this.$http = $http; + this.ros = Ros; + this.$timeout = $timeout; + this.status = -1; + } + + $onInit() { + + this.$scope.$watchGroup(['$ctrl.action.type', '$ctrl.action.active'], () => { + this.setFileName(); + this.client = new ROSLIB.ActionClient({ + ros : this.ros.ros, + serverName : this.action.name, + actionName : this.action.type + }); + }, () => {}); + } + + sendGoal(goalmsg) { + + + + var goal = new ROSLIB.Goal({ + actionClient : this.client, + goalMessage : goalmsg + }); + this.goal = goal; + goal.send(); + } + + setFileName() { + const path = 'app/actions/'; + + + this.fileName = `${path}default.html`; + + + if(!this.action.active) { + this.fileName = ''; + this.action.isOpen = false; + return; + } + else if (!this.action.type) { + this.fileName = `${path}default.html`; + return; + } + + const fileName = `${path}${this.action.type}.html`; + this.$http.get(fileName).then((result) => { + if (result.data) { + this.fileName = fileName; + } + }, () => {}); + } +} + +angular.module('roscc').component('ccAction', { + bindings: { action: '=' }, + template: ``, + controller: ServiceController, +}); diff --git a/webclient/app/actions/meta.html b/webclient/app/actions/meta.html new file mode 100644 index 0000000..b676162 --- /dev/null +++ b/webclient/app/actions/meta.html @@ -0,0 +1,19 @@ +
+ + {{ $ctrl.action.abbr }} +
{{ ['PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED', 'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED'][$ctrl.goal.status.status] }} ({{ $ctrl.action.type }})
+
+ + + + +
diff --git a/webclient/app/actions/movement_actuators/dispatchAction.html b/webclient/app/actions/movement_actuators/dispatchAction.html new file mode 100644 index 0000000..4c305a1 --- /dev/null +++ b/webclient/app/actions/movement_actuators/dispatchAction.html @@ -0,0 +1,74 @@ +
+
+ +
+ + +
+
+ +
+ +
+ + +
+
+ +
+ +
+ + +
+
+ +
+ +
+ + +
+
+ +
+ +
+ + +
+
+ +
+ +
+ + +
+
+
+
+Result : {{ $ctrl.goal.result.success }} +
diff --git a/webclient/app/actions/navigation_navigator/DoGotoAction.html b/webclient/app/actions/navigation_navigator/DoGotoAction.html new file mode 100644 index 0000000..893dcb9 --- /dev/null +++ b/webclient/app/actions/navigation_navigator/DoGotoAction.html @@ -0,0 +1,58 @@ +
+
+ +
+ +
+
+ +
+ +
+ +
+
+ +
+ +
+ + m +
+
+
+ + +
+ + m +
+
+ +
+ +
+ + rad +
+
+ +
+ +
+ Disable +
+
+ +
+ +
+Result : {{ $ctrl.goal.result.success }} +
diff --git a/webclient/app/actions/navigation_navigator/DoGotoWaypointAction.html b/webclient/app/actions/navigation_navigator/DoGotoWaypointAction.html new file mode 100644 index 0000000..a215e1a --- /dev/null +++ b/webclient/app/actions/navigation_navigator/DoGotoWaypointAction.html @@ -0,0 +1,44 @@ +
+
+ +
+ +
+
+ +
+ +
+ +
+
+ +
+ +
+ + +
+
+ +
+ +
+ Disable +
+
+
+ +
+Result : {{ $ctrl.goal.result.success }} +
diff --git a/webclient/app/app.js b/webclient/app/app.js new file mode 100755 index 0000000..5ddcc1a --- /dev/null +++ b/webclient/app/app.js @@ -0,0 +1,82 @@ +function ROSCCConfig($routeProvider, localStorageServiceProvider) { + $routeProvider + .when('/diagnostic', { template: '' }) + .when('/asserv', { template: '' }) + .when('/hokuyo', { template: '' }) + .when('/telecommande', { template: '' }) + .when('/settings', { template: '' }) + .otherwise({ redirectTo: '/diagnostic' }); + + localStorageServiceProvider + .setPrefix('roscc'); +} + +function run($rootScope) { + + $rootScope.domains = [ + { + name: 'drivers', + topics: ['ard_asserv/pose2d', + 'ard_asserv/speed'], + services: ['ard_asserv/emergency_stop', + 'ard_asserv/goto', + 'ard_asserv/set_pos', + 'ard_asserv/speed', + 'ard_asserv/pwm', + 'ard_asserv/management', + 'ard_asserv/parameters'], + actions: ['ard_asserv/goto_action'] + }, + { + name: 'ai', + topics: [], + services: [], + actions: [] + }, + { + name: "recognition", + topics: [], + services: [], + actions: [] + }, + { + name: "processing", + topics: [], + services: [], + actions: [] + }, + { + name: "sensors", + topics: [], + services: [], + actions: [] + }, + { + name: "movement", + topics: [], + services: [], + actions: [] + }, + { + name: "memory", + topics: [], + services: [], + actions: [] + }, + { + name: "feedback", + topics: [], + services: [], + actions: [] + }, + { + name: "navigation", + topics: [], + services: [], + actions: [] + } + ]; +} + + +angular.module('roscc', ['ngRoute', 'ui.bootstrap', 'LocalStorageModule', 'ngAnimate']).config(ROSCCConfig).run(run); diff --git a/webclient/app/asserv/asserv.component.js b/webclient/app/asserv/asserv.component.js new file mode 100644 index 0000000..5a08b68 --- /dev/null +++ b/webclient/app/asserv/asserv.component.js @@ -0,0 +1,64 @@ +class AsservController { + constructor (Ros, $interval) { + this.ros = Ros.ros; + this.$interval = $interval; + + + Ros.listen('/drivers/ard_asserv/pose2d', function (e) { + this.pushDataToChart(2, e.x); + this.pushDataToChart(3, e.theta); + this.pushDataToChart(4, e.y); + }.bind(this)) + + Ros.listen('/drivers/ard_asserv/speed', function (e) { + this.pushDataToChart(0, e.pwm_speed_left); + this.pushDataToChart(1, e.pwm_speed_right); + this.pushDataToChart(5, e.wheel_speed_left); + this.pushDataToChart(7, e.wheel_speed_right); + this.pushDataToChart(6, e.linear_speed); + + }.bind(this)) + + this.charts = [] + + for(let i = 0; i < 8; i++) { + let c = new SmoothieChart({grid:{verticalSections:4}, labels:{fontSize:20}, responsive: true, tooltip:true}); + let ts = new TimeSeries(); + + c.addTimeSeries(ts, { maxValueScale:1.32, minValueScale:1.32, strokeStyle: 'rgba(255, 0, 0, 1)', fillStyle: 'rgba(255, 0, 0, 0.2)', lineWidth: 4 }); + c.streamTo(document.getElementById("line"+(i+1)), 10); + + this.charts.push({chart: c, data: ts}); + + + + } + + + } + + pushDataToChart(i, e) { + this.charts[i].data.append(new Date().getTime(), e) + } + + + $onInit ($interval) { + var canvas = document.getElementsByTagName('canvas') + for (let c of canvas) { fitToContainer(c) } + + function fitToContainer (canvas) { + // Make it visually fill the positioned parent + canvas.style.width = '100%' + canvas.style.height = '100%' + // ...then set the internal size to match + canvas.width = canvas.offsetWidth + canvas.height = canvas.offsetHeight + } + + } +} + +angular.module('roscc').component('ccAsserv', { + templateUrl: 'app/asserv/asserv.html', + controller: AsservController +}) diff --git a/webclient/app/asserv/asserv.html b/webclient/app/asserv/asserv.html new file mode 100644 index 0000000..453d782 --- /dev/null +++ b/webclient/app/asserv/asserv.html @@ -0,0 +1,55 @@ +
+ +
+
PWM Speed Left
+ +
+ + +
+
PWM Speed Right
+ +
+ +
+
X Position
+ +
+ +
+
Orientation
+ +
+ +
+
Y Position
+ +
+ +
+ + + + +
+ +
+
Wheel Speed Right
+ +
+ +
+
Wheel Speed Right
+ +
+ +
+
Linear Speed
+ +
+
diff --git a/webclient/app/control/control.component.js b/webclient/app/control/control.component.js new file mode 100755 index 0000000..ea8c148 --- /dev/null +++ b/webclient/app/control/control.component.js @@ -0,0 +1,67 @@ + +class ControlController { + constructor($rootScope, $timeout, $interval, Settings, Domains, Ros, Console) { + this.$timeout = $timeout; + this.Domains = Domains; + this.domains = $rootScope.domains; + this.logs = Console.logs; + + this.ros = Ros; + this.setting = Settings.get(); + + if($rootScope.isConnected) { + this.$timeout(() => { this.onConnected(); }, 100); + } else { + $rootScope.$watch('isConnected', function(newValue) { + if(newValue) + this.$timeout(() => { this.onConnected(); }, 100); + }.bind(this)); + } + + } + + // The active domain shows further information in the center view + setActiveDomain(domain) { + this.activeDomain = domain; + } + + + onConnected() { + + if (!this.activeDomain) { + for(let d of this.domains) { + if(this.ros.getDomains().includes(d.name)) { + this.setActiveDomain(d.name); + return; + } + } + } + } + + + + refresh() { + this.ros.loadData(); + } + + isDomainActive(domain) { + return _.some(this.ros.getServicesForDomain(domain), (t) => t.active == true); + } + + collapseAll(domain) { + this.ros.getServicesForDomain(domain).map(function(item){ + item.isOpen = false; + }); + } + + expandAll(domain) { + this.ros.getServicesForDomain(domain).map(function(item){ + item.isOpen = true; + }); + } +} + +angular.module('roscc').component('ccControl', { + templateUrl: 'app/control/control.html', + controller: ControlController, +}); diff --git a/webclient/app/control/control.html b/webclient/app/control/control.html new file mode 100755 index 0000000..27380fc --- /dev/null +++ b/webclient/app/control/control.html @@ -0,0 +1,118 @@ +
+ + + + + +
+
+
+ +
+ +
+ +
+

{{ domain }}

+
+
+
+
+ + + + {{ $ctrl.showUnexpected ? ' Hide unexpected' : 'Show unexpected'}} + + +
+
+
+ +
+ +
+

{{ domain }}

+
+
+ +
+ + +
+ +
+
+

{{ domain }}

+
+ + +
+
+ + + +
+ +
+
+ + +
+ +
+
+ + + + +
+
+
+
+ + + +
+
diff --git a/webclient/app/diagnostic/diagnostic.component.js b/webclient/app/diagnostic/diagnostic.component.js new file mode 100644 index 0000000..b423959 --- /dev/null +++ b/webclient/app/diagnostic/diagnostic.component.js @@ -0,0 +1,67 @@ + +class DiagnosticController { + constructor($rootScope, $timeout, $interval, Settings, Domains, Ros, Console) { + this.$timeout = $timeout; + this.Domains = Domains; + this.domains = $rootScope.domains; + this.logs = Console.logs; + + this.ros = Ros; + this.setting = Settings.get(); + + + if($rootScope.isConnected) { + this.$timeout(() => { this.onConnected(); }, 1000); + } else { + $rootScope.$watch('isConnected', function(newValue) { + if(newValue) + this.$timeout(() => { this.onConnected(); }, 1000); + }.bind(this)); + } + + } + + // The active domain shows further information in the center view + setActiveDomain(domain) { + this.activeDomain = domain; + } + + + onConnected() { + if (!this.activeDomain) { + for(let d of this.domains) { + if(this.ros.getDomains().includes(d.name)) { + this.setActiveDomain(d.name); + } + } + } + } + + + + refresh() { + this.ros.loadData(); + } + + isDomainActive(domain) { + return _.some(this.ros.getTopicsForDomain(domain), (t) => t.active == true); + } + + collapseAll(domain) { + this.ros.getTopicsForDomain(domain).map(function(item){ + item.isOpen = false; + }); + } + + expandAll(domain) { + this.ros.getTopicsForDomain(domain).map(function(item){ + if(item.type) + item.isOpen = true; + }); + } +} + +angular.module('roscc').component('ccDiagnostic', { + templateUrl: 'app/diagnostic/diagnostic.html', + controller: DiagnosticController, +}); diff --git a/webclient/app/diagnostic/diagnostic.html b/webclient/app/diagnostic/diagnostic.html new file mode 100644 index 0000000..dac0391 --- /dev/null +++ b/webclient/app/diagnostic/diagnostic.html @@ -0,0 +1,114 @@ +
+ + + + + +
+
+
+ +
+ +
+ +
+

{{ domain }}

+
+
+
+
+ + + + {{ $ctrl.showUnexpected ? ' Hide unexpected' : 'Show unexpected'}} + + +
+
+ + + +
+ +
+

{{ domain }}

+ +
+
+ +
+ + + + + + +
+ +
+
+

{{ domain }}

+
+ + +
+
+ + + +
+ +
+
+ +
+
+
+
+ + +
+
diff --git a/webclient/app/helper/console.service.js b/webclient/app/helper/console.service.js new file mode 100644 index 0000000..5c1b7dd --- /dev/null +++ b/webclient/app/helper/console.service.js @@ -0,0 +1,47 @@ +class ConsoleService { + + constructor(Ros, Settings, $rootScope) { + this.ros = Ros; + this.setting = Settings.get(); + + this.logs = []; + $rootScope.$watch('isConnected', () => { + if($rootScope.isConnected) + this.setConsole(); + else if(this.consoleTopic) + this.consoleTopic.unsubscribe(); + }); + } + + // Setup of console (in the right sidebar) + setConsole() { + this.consoleTopic = new ROSLIB.Topic({ + ros: this.ros.ros, + name: this.setting.log, + messageType: 'rosgraph_msgs/Log', + }); + this.consoleTopic.subscribe((message) => { + const nameArray = message.name.split('/'); + const d = new Date((message.header.stamp.secs * 1E3) + (message.header.stamp.nsecs * 1E-6)); + + message.abbr = (nameArray.length > 1) ? nameArray[1] : message.name; + + // String formatting of message time and date + function addZero(i) { return i < 10 ? `0${i}` : `${i}`; } + message.dateString = `${addZero(d.getHours())}: + ${addZero(d.getMinutes())}: + ${addZero(d.getSeconds())}`; + + this.logs.unshift(message); + + if (this.logs.length > this.setting.maxConsoleEntries) { + this.logs.pop(); + } + + }); + } + + +} + +angular.module('roscc').service('Console', ConsoleService); diff --git a/webclient/app/helper/domains.service.js b/webclient/app/helper/domains.service.js new file mode 100755 index 0000000..d84a898 --- /dev/null +++ b/webclient/app/helper/domains.service.js @@ -0,0 +1,71 @@ +class DomainsService { + filterAdvanced(entry, advanced) { + if (advanced) { + return true; + } + + const entryArray = entry.split('/'); + if (!entry || _.isEmpty(entryArray)) { + return false; + } + + // Don't show the default nodes, params, topics and services + return (!_.contains([ + 'rosapi', + 'rosbridge_websocket', + 'rosout', + 'rosout_agg', + 'rosversion', + 'run_id', + 'rosdistro', + 'get_loggers', + 'set_logger_level', + ], _.last(entryArray))); + } + + getDomains(array) { + const result = []; + angular.forEach(array, (entry) => { + const nameArray = entry.name.split('/'); + if (nameArray.length > 1) { + result.push(nameArray[1]); + } + }); + return _.uniq(result).sort(); + } + + getGlobalParameters(array) { + const result = []; + angular.forEach(array, (entry) => { + const nameArray = entry.name.split('/'); + if (nameArray.length === 2) { + entry.abbr = _.last(nameArray); + result.push(entry); + } + }); + return result; + } + + getDataForDomain(array, domainName) { + const result = []; + angular.forEach(array, (entry) => { + const nameArray = entry.name.split('/'); + if ( + nameArray.length > 1 && + nameArray[1] === domainName && + (entry.fetched || !entry.active) && + !_.contains(nameArray, "get_loggers") && //TODO : filter nicely <3 (maybe put the rcc filter back) + !_.contains(nameArray, "set_logger_level") + ) { + entry.abbr = nameArray.slice(2).join('/'); + result.push(entry); + } + }); + return result; + } + + +} + +// Filter advanced topics, services, parameters by checking the beginning capital letter +angular.module('roscc').service('Domains', DomainsService); diff --git a/webclient/app/helper/quaternions.service.js b/webclient/app/helper/quaternions.service.js new file mode 100755 index 0000000..41067f5 --- /dev/null +++ b/webclient/app/helper/quaternions.service.js @@ -0,0 +1,32 @@ +class QuaternionsService { + getRoll(q) { + if (!q) { + return ''; + } + const rad = Math.atan2(2 * ((q.w * q.x) + (q.y * q.z)), 1 - (2 * ((q.x * q.x) + (q.y * q.y)))); + return (180 / Math.PI) * rad; + } + + getPitch(q) { + if (!q) { + return ''; + } + const rad = Math.asin(2 * ((q.w * q.y) - (q.z * q.x))); + return (180 / Math.PI) * rad; + } + + getYaw(q) { + if (!q) { + return ''; + } + const rad = Math.atan2(2 * ((q.w * q.z) + (q.x * q.y)), 1 - (2 * ((q.y * q.y) + (q.z * q.z)))); + return (180 / Math.PI) * rad; + } + + getInit() { + return { w: 1, x: 0, y: 0, z: 0 }; + } +} + +// Quaternions to Euler angles converter +angular.module('roscc').service('Quaternions', QuaternionsService); diff --git a/webclient/app/helper/ros.service.js b/webclient/app/helper/ros.service.js new file mode 100644 index 0000000..3cf106c --- /dev/null +++ b/webclient/app/helper/ros.service.js @@ -0,0 +1,358 @@ +class RosService { + constructor($rootScope, $log, $interval, $timeout, Settings, Domains) { + $rootScope.isConnected = false; + this.isConnected = $rootScope.isConnected; + + this.setting = Settings.get(); + this.$interval = $interval; + this.$timeout = $timeout; + this.$log = $log; + this.$rootScope = $rootScope; + this.ros = false; + this.Domains = Domains; + + this.resetData(); + this.newRosConnection(); + $interval(() => { + this.newRosConnection(); + }, 1000 / this.setting.refresh_rate); + + $interval(() => { + this.loadData(); + }, 1000 / this.setting.refresh_rate); + } + + newRosConnection(callback) { + if (this.$rootScope.isConnected || !this.setting) { + return; + } + + if (this.ros) { + this.ros.close(); // Close old connection + this.ros = false; + return; + } + + this.ros = new ROSLIB.Ros({ url: `ws://${this.setting.address}:${this.setting.port}` }); + + this.ros.on('connection', () => { + this.$rootScope.isConnected = true; + this.isConnected = this.$rootScope.isConnected; + this.resetData(); + this.loadData(); + this.$log.log('Successfully connected to server !'); + }); + + this.ros.on('error', () => { + this.$rootScope.isConnected = false; + this.isConnected = this.$rootScope.isConnected; + this.ros = false; + this.$log.log('Error trying to connect to server !'); + + }); + + this.ros.on('close', () => { + this.$rootScope.isConnected = false; + this.isConnected = this.$rootScope.isConnected; + this.$log.log('Connection to server closed !'); + }); + + + if(callback) { + this.$timeout(function() { + callback(); + }.bind(this), 1000); // [ms] + } + } + + + sendOrder(to, data, callback) { + if(!this.ros) return; + let service = new ROSLIB.Service({ ros: this.ros, name: to }); + let request = new ROSLIB.ServiceRequest(data); + service.callService(request, callback) + } + + publish(to, data) { + if(!this.ros) return; + let topic = new ROSLIB.Topic({ ros: this.ros, name: to }) + let msg = new ROSLIB.Message(data); + topic.publish(msg); + } + + listen(from, callback) { + if(!this.ros) return; + let topic = new ROSLIB.Topic({ ros: this.ros, name: from }) + topic.subscribe(callback); + return topic; + } + + getParam(name, callback) { + if(!this.ros) return; + let param = new ROSLIB.Param({ ros: this.ros, name: name }) + param.get(callback); + } + + setParam(name, value) { + if(!this.ros) return; + let param = new ROSLIB.Param({ ros: this.ros, name: name }) + param.set(value); + } + + + + resetData() { + this.data = { + rosout: [], + topics: [], + nodes: [], + parameters: [], + services: [], + actions: [] + }; + + //populate expected topics/services + angular.forEach(this.$rootScope.domains, (d) => { + + angular.forEach(d.topics, (t) => { + let name = '/'+d.name+'/'+t; + let newT = { + name: name, + abbr: t, + active: false, + expected: true, + isOpen: true + }; + this.data.topics.push(newT); + }); + + angular.forEach(d.services, (s) => { + let name = '/'+d.name+'/'+s; + let newS = { + name: name, + abbr: s, + active: false, + expected: true, + isOpen: true + }; + this.data.services.push(newS); + }); + + angular.forEach(d.actions, (a) => { + let name = '/'+d.name+'/'+a; + let newA = { + name: name, + abbr: a, + active: false, + expected: true, + isOpen: true + }; + this.data.actions.push(newA); + }); + }); + } + + // updates structure, all data, parameters, topics, services, nodes... + loadData() { + this.ros.getTopics((topics) => { // TODO: check if type is already returned here + angular.forEach(topics.topics, (name) => { + let topic = _.findWhere(this.data.topics, { name }); + + if(!topic && (name.match("(/feedback|/goal|/status|/cancel|/result)$"))) { + return; + } + if(topic) { //to update + topic.active = true; + } else { //to add + topic = { + name: name, + active: true, + isOpen: true + }; + this.data.topics.push(topic); + } + + if(!topic.fetched) { + this.ros.getTopicType(name, (type) => { + topic.type = type; + topic.fetched = true; + }); + } + }); + + for(let i = this.data.topics.length - 1; i >= 0; i--) { + let t = this.data.topics[i]; + let found = false; + for(let y = 0; y < topics.topics.length; y++) { + if(topics.topics[y] == t.name) + found = true; + } + + if(!found) { + if(!t.expected) { + this.data.topics.splice(i, 1); + } else { + t.active = false; + } + } + } + }); // end getTopics + + this.ros.getServices((services) => { + angular.forEach(services, (name) => { + let service = _.findWhere(this.data.services, { name }); + + if(service) { //to update + service.active = true; + } else { //to add + service = { + name: name, + active: true, + isOpen: true + }; + this.data.services.push(service); + } + if(!service.fetched) { + this.ros.getServiceType(name, (type) => { + service.type = type; + service.fetched = true; + }); + } + }); + + for(let i = this.data.services.length - 1; i >= 0; i--) { //angular foreach not working for this + let s = this.data.services[i]; + let found = false; + for(let y = 0; y < services.length; y++) { + if(services[y] == s.name) + found = true; + } + + if(!found) { + if(!s.expected) { + this.data.services.splice(i, 1); + } else { + s.active = false; + } + } + } + }); // end getServices + + + this.ros.getActionServers((actions) => { + angular.forEach(actions, (name) => { + let action = _.findWhere(this.data.actions, { name }); + + if(action) { //to update + action.active = true; + } else { //to add + action = { + name: name, + active: true, + isOpen: true + }; + this.data.actions.push(action); + } + if(!action.fetched) { + this.ros.getTopicType(name+'/goal', (type) => { + action.type = type.slice(0, -4); + action.fetched = true; + }); + } + }); + + for(let i = this.data.actions.length - 1; i >= 0; i--) { //angular foreach not working for this + let a = this.data.actions[i]; + let found = false; + for(let y = 0; y < actions.length; y++) { + if(actions[y] == a.name) + found = true; + } + + if(!found) { + if(!a.expected) { + this.data.actions.splice(i, 1); + } else { + a.active = false; + } + } + } + }); // end getActionServerss + + this.ros.getParams((params) => { //TODO : update like topics + angular.forEach(params, (name) => { + + let param = _.findWhere(this.data.parameters, { name }); + if(!param) { + param = { name } + this.data.parameters.push(param); + } + + if(!param.fetched) { + const rosparam = new ROSLIB.Param({ ros: this.ros, name }); + rosparam.get((value) => { + param.value = value; + param.fetched = true; + }); + } + }); + + for(let i = this.data.parameters.length - 1; i >= 0; i--) { //angular foreach not working for this + let p = this.data.parameters[i]; + + if(!_.contains(params, p.name)) { + this.data.parameters.splice(i, 1); + } + } + }); + + this.ros.getNodes((nodes) => { //TODO : update like topics + this.data.nodes = []; + angular.forEach(nodes, (name) => { + this.data.nodes.push({ name }); + }); + }); + } + + + getDomains() { + if(!this.data) + return; + const allData = this.data.topics.concat(this.data.services, this.data.nodes, this.data.parameters); + const domains = this.Domains.getDomains(allData); + + let expectedD = _.pluck(this.$rootScope.domains, 'name'); + + //set expected domains first in the list + return _.uniq(expectedD.concat(domains)); + } + + getExpectedDomains() { + return _.pluck(this.$rootScope.domains, 'name'); + } + + getUnexpectedDomains() { + return _.difference(this.getDomains(), this.getExpectedDomains()); + } + + getTopicsForDomain(domain) { + return this.Domains.getDataForDomain(this.data.topics, domain, false); + } + + getServicesForDomain(domain) { + return this.Domains.getDataForDomain(this.data.services, domain, false); + } + + getActionsForDomain(domain) { + return this.Domains.getDataForDomain(this.data.actions, domain, false); + } + + getGlobalParameters() { + return this.Domains.getGlobalParameters(this.data.parameters); + } + + + +} + +angular.module('roscc').service('Ros', RosService); diff --git a/webclient/app/hokuyo/hokuyo.component.js b/webclient/app/hokuyo/hokuyo.component.js new file mode 100644 index 0000000..c352b50 --- /dev/null +++ b/webclient/app/hokuyo/hokuyo.component.js @@ -0,0 +1,136 @@ +/* eslint-disable no-undef */ + +class HokuyoController { + constructor($rootScope, $scope, Hokuyo, Settings, Ros) { + $rootScope.act_page = 'hokuyo'; + this.$scope = $scope; + this.ros = Ros; + this.setting = Settings.get(); + + let changedTab = false; + if (Hokuyo.displays.one != null) { + changedTab = true; + } + + Hokuyo.displays.one = new HokuyoDisplay("hok1", "one", true); + Hokuyo.displays.two = new HokuyoDisplay("hok2", "two"); + Hokuyo.displays.main = new HokuyoDisplay("mainHokDisp", "main", false, Hokuyo.displays.one.dotColor, Hokuyo.displays.two.dotColor); + + if (changedTab) { + // Restore data + if (Hokuyo.lastData.one != null) { + Hokuyo.displays.one.updatePolarSpots(Hokuyo.lastData.one); + } + if (Hokuyo.lastData.two != null) { + Hokuyo.displays.two.updatePolarSpots(Hokuyo.lastData.two); + } + if (Hokuyo.lastData.main != null) { + Hokuyo.displays.main.updateAll(Hokuyo.lastData.main.hokuyos, Hokuyo.lastData.main.robotsSpots, Hokuyo.lastData.main.cartesianSpots); + } + } + + this.ros.listen(this.setting.hokuyo_1, function(msg) { + + this.$scope.name = "hokuyo.polar_raw_data"; + this.$scope.from = "hokuyo"; + this.$scope.data = { + hokuyo: "one", + polarSpots: Hokuyo.formatPolarPoints(msg) + }; + this.$scope.update(); + }.bind(this)); + + this.ros.listen(this.setting.hokuyo_2, function(msg) { + this.$scope.name = "hokuyo.polar_raw_data"; + this.$scope.from = "hokuyo"; + this.$scope.data = { + hokuyo: "two", + polarSpots: Hokuyo.formatPolarPoints(msg) + }; + + this.$scope.update(); + }.bind(this)); + + + +// $scope.name = "hokuyo.polar_raw_data"; +// $scope.from = "hokuyo"; +// $scope.data = '{\n\ +// "hokuyo": "one",\n\ +// "polarSpots": [\n\ +// [ -90, 350 ],\n\ +// [ -30, 235 ],\n\ +// [ -35, 230 ],\n\ +// [ -25, 230 ],\n\ +// [ -20, 100 ],\n\ +// [ -15, 105 ],\n\ +// [ -5, 120 ],\n\ +// [ 0, 100 ],\n\ +// [ 5, 90 ],\n\ +// [ 10, 95 ],\n\ +// [ 15, 100 ],\n\ +// [ 20, 100 ],\n\ +// [ 25, 230 ],\n\ +// [ 30, 235 ]\n\ +// ]\n\ +// }'; +// +// +// $scope.name = "lidar.all"; +// $scope.from = "lidar"; +// $scope.data = '{\n\ +// "hokuyos": [\n\ +// {\n\ +// "name": "one",\n\ +// "position": {\n\ +// "x": -6.2,\n\ +// "y": -6.2,\n\ +// "w": 0\n\ +// }\n\ +// },\n\ +// {\n\ +// "name": "two",\n\ +// "position": {\n\ +// "x": 306.2,\n\ +// "y": 100,\n\ +// "w": 180\n\ +// }\n\ +// }\n\ +// ],\n\ +// "cartesianSpots": [\n\ +// [ 20, 200 ],\n\ +// [ 30, 202 ],\n\ +// [ 40, 199 ],\n\ +// [ 148, 104 ],\n\ +// [ 145, 95 ],\n\ +// [ 153, 105 ],\n\ +// [ 155, 98 ],\n\ +// [ 95, 135 ],\n\ +// [ 100, 129 ],\n\ +// [ 101, 140 ],\n\ +// [ 105, 132 ],\n\ +// [ 104, 137 ],\n\ +// [ 230, 2 ],\n\ +// [ 234, 4 ]\n\ +// ],\n\ +// "robotsSpots": [\n\ +// [ 150, 100 ],\n\ +// [ 100, 135 ]\n\ +// ]\n\ +// }'; + + $scope.update = function() { + Hokuyo.onOrder($scope.from, $scope.name, $scope.data); + } + + $scope.update(); + } +} + + +angular.module('roscc').component('ccHokuyo', { + templateUrl: 'app/hokuyo/hokuyo.html', + controller: HokuyoController, +}); + +/* eslint-enable no-undef */ diff --git a/webclient/app/hokuyo/hokuyo.html b/webclient/app/hokuyo/hokuyo.html new file mode 100644 index 0000000..f0ca73d --- /dev/null +++ b/webclient/app/hokuyo/hokuyo.html @@ -0,0 +1,31 @@ +
+
+ + + + + +
+
+ +
+
+
+ +
+
+
+
+
diff --git a/webclient/app/hokuyo/hokuyo.service.js b/webclient/app/hokuyo/hokuyo.service.js new file mode 100644 index 0000000..4523dad --- /dev/null +++ b/webclient/app/hokuyo/hokuyo.service.js @@ -0,0 +1,83 @@ +angular.module('roscc').service('Hokuyo', ['$rootScope', '$sce', 'Ros', +function() { + this.displays = { + main: null, + one: null, + two: null + }; + + this.lastData = { + one: null, + two: null, + main: null + }; + + // format laserscan ros msg to a list of lists [theta, r], theta in degrees + // r in cm + this.formatPolarPoints = function(msg) { + let amin = msg.angle_min; + let ainc = msg.angle_increment; + + + let acurr = amin; + let out = []; + for(let r of msg.ranges) { + out.push([(acurr * 180 / Math.PI )%360, r * 100]); + acurr += ainc; + } + + return out; + } + + this.init = function () { + //Client.order(this.onOrder); //FIXME + }; + + this.onOrder = function (from, name, data) { + // if($rootScope.act_page == 'hokuyo') { + if (name == 'hokuyo.polar_raw_data') { + if (this.displays[data.hokuyo]) { + // Save for later + this.lastData[data.hokuyo] = data.polarSpots; + + // console.log("Received data from " + data.hokuyo); + + // Show + if (!this.displays[data.hokuyo].isBusy) { + this.displays[data.hokuyo].updatePolarSpots(data.polarSpots); + } + } + } else if (name == 'lidar.all' + && !!this.displays.main) { + // Save for later + this.lastData.main = {}; + this.lastData.main.hokuyos = data.hokuyos; + this.lastData.main.robotsSpots = data.robotsSpots; + this.lastData.main.cartesianSpots = data.cartesianSpots; + + // console.log("Received all"); + + // Show + if (!this.displays.main.isBusy) { + this.displays.main.updateAll(data.hokuyos, data.robotsSpots, data.cartesianSpots); + } + } else if (name == 'lidar.light' + && !!this.displays.main) { + // Save for later + this.lastData.main = {}; + this.lastData.main.hokuyos = data.hokuyos; + this.lastData.main.robotsSpots = data.robotsSpots; + + // console.log("Received all"); + + // Show + if (!this.displays.main.isBusy) { + this.displays.main.updateAll(data.hokuyos, data.robotsSpots, null); + } + } else if (name == 'lidar.robots' + && !!this.displays.main) { + // this.displays.main.updateRobots(data.robots); + } + // } + }.bind(this); +}]); diff --git a/webclient/app/hokuyo/hokuyoDisplay.class.js b/webclient/app/hokuyo/hokuyoDisplay.class.js new file mode 100644 index 0000000..023c475 --- /dev/null +++ b/webclient/app/hokuyo/hokuyoDisplay.class.js @@ -0,0 +1,289 @@ +"use strict"; + +/* eslint-disable */ + +class HokuyoDisplay { + constructor(parentId, mode, reinitColor = false, oneColor = null, twoColor = null) { + this.MAIN = "main"; + this.ONE = "one"; + this.TWO = "two"; + + this.dotRadius = 1; // cm + + this.parentId = parentId; + this.mode = mode; + this.div = $('#' + this.parentId); + + this.isBusy = false; // lock + + this.clearMainTimeout = null; // timeout in case we don't receive data for a while + + if (reinitColor) { + Raphael.getColor.reset(); + } + + this.onResize();// Math.max($('body').height() - $('#div_menu').outerHeight() - 2*$('#simu_before').outerHeight(), 200); + + // On window resize (doesn't seem to work) + // window.addEventListener('resize', this.onResize()); + // this.div.resize(this.onResize()); + // setInterval(this.onResize(), 1000); + + this.initChart(oneColor, twoColor); + } + + onResize() { + this.realW = 0; + this.realH = 0; + this.W = 0; + this.H = 0; + this.center = {}; + this.center.x = 0; + this.center.y = 0; + this.center.str = this.center.x + "," + this.center.y; // in the viewport frame + this.viewportScale = 1; + + if (this.mode == this.MAIN) { + var maxWidth = 1000; // px + + this.realW = 300; + this.realH = 200; + + // if (this.W != this.div.width()) { + this.W = 1000; //this.div.width(); + if (this.W > maxWidth) { + this.W = maxWidth; + } + this.H = ( this.realH / this.realW ) * this.W; + + // this.center.x = 0; // see initChart() + // this.center.y = 0; // see initChart() + + // } + + let scaleTable = 0.9; + + // Center + this.center.x = Math.round(((1 - scaleTable) * this.W) / 2); + this.center.y = Math.round(((1 - scaleTable) * this.H) / 2); + + // viewport scale : position (cm) * scale = position (pixel) + // we keep space for the outer towers + this.viewportScale = (scaleTable * this.W) / this.realW; + } else { + this.realW = 400; + this.realH = this.realW; + + this.W = 490; + this.H = this.W; + + this.center.x = this.W/2; + this.center.y = this.H/2; + + this.viewportScale = (this.W/2) / this.realW; + } + + this.center.str = this.center.x + "," + this.center.y; + + this.div.width(this.W); + this.div.height(this.H); + + // console.log(this.viewportScale); + // console.log(this.center.str); + // console.log(this.W); + // console.log(this.H); + } + + initChart(oneColor = null, twoColor = null) { + this.isBusy = true; + + this.r = Raphael(this.parentId, this.div.width(), this.div.height()); + + var grey = Raphael.color("#333"); + + if (this.mode != this.MAIN) { + + this.dotColor = Raphael.getColor(1); + + // Outmost circle + // Unit: cm + this.r.circle(0, 0, this.realW).attr({stroke: this.dotColor, fill: grey, transform: "t " + this.center.str + "s" + this.viewportScale, "fill-opacity": .1}); + + // Inner circles + // Unit: cm + for (var radius = 100; radius < this.realW; radius+=100) { + this.r.circle(0, 0, radius).attr({stroke: grey, fill: null, transform: "t " + this.center.str + "s" + this.viewportScale, "stroke-opacity": .4}); + } + + // Arraw + // Unit: pixel + this.r.path("M-7 10L0 -10L7 10L-7 10").attr({stroke: this.dotColor, fill: this.dotColor, transform: "t " + this.center.str, "fill-opacity": .4}); + + this.dots = new Map(); + } else { + // Init main + + if (oneColor == null + || twoColor == null) { + console.error("Main display must have the hokuyos Raphael colors"); + this.isBusy = false; + return; + } + + this.oneColor = oneColor; + this.twoColor = twoColor; + + this.dotColor = Raphael.color("#11B100"); + + // Field + // Paper.rect(x, y, width, height, [r]) in the REAL WORLD dimensions + // Transform : move to origin and then scale around origin + this.r.rect(0, 0, this.realW, this.realH).attr({stroke: grey, fill: grey, transform: "t " + this.center.str + "s" + this.viewportScale + "," + this.viewportScale + ",0,0", "fill-opacity": .1, "stroke-opacity": .4}); + + // Towers + // 8*8 sqare at the corner - 2.2cm of wood board + let xs = [ -10.2, 302.2 ]; + let ys = [ -10.2, 96, 202.2 ]; + let colors = [ Raphael.color("#2988D8"), Raphael.color("#FAFC08") ]; + let i = 0; + + // for each column (ie left & right) + for (let col = 0; col<=1; col++){ + // for each line (ie back, middle, front) + for (let line = 0; line<=2; line++){ + this.r.rect(xs[col], ys[line], 8, 8).attr({stroke: colors[i%2], fill: colors[i%2], transform: "t " + this.center.str + "s" + this.viewportScale + "," + this.viewportScale + ",0,0", "fill-opacity": .4, "stroke-opacity": .8}); + i++; + } + } + + // Hokuyos container + this.hokuyos = {}; + + // Dots container + this.objects = []; + } + + this.isBusy = false; + } + + updatePolarSpots(spots) { + if (this.mode == this.MAIN) { + console.error("Main display can't handle polar spot"); + return; + } + + this.isBusy = true; + + // console.log(spots); + // For each spots + spots.forEach(function(newSpot) { + var existingPlot = this.dots.get(newSpot[0]); + + if (!!existingPlot) { + // Dot already exist at this angle, let's update it + existingPlot.attr({cy: newSpot[1] * this.viewportScale }); + } else { + // This dot doesn't already exist, let's create it + var dot = this.r.circle(0, newSpot[1] * this.viewportScale, this.dotRadius).attr({ + stroke: this.dotColor, + fill: this.dotColor, + transform: "t," + this.center.str + "r180,0,0" + "r" + -newSpot[0] + ",0,0"}); + // + " 0 0" + + + this.dots.set(newSpot[0], dot); + } + }.bind(this)); + + this.isBusy = false; + } + + // updateRobots(robots) { + // if (this.mode != this.MAIN) { + // console.error("Single LiDAR displays can't handle global robot spot"); + // return; + // } + // } + + updateAll(hokuyos, robots, cartesianSpots) { + if (this.mode != this.MAIN) { + console.error("Single LiDAR displays can't handle global robot spot"); + return; + } + + this.isBusy = true; + + if (!!this.clearMainTimeout) { + clearTimeout(this.clearMainTimeout); + } + + // console.log(hokuyos); + + // For each hokuyo + hokuyos.forEach(function(hok) { + var existingHok = this.hokuyos[hok.name]; + + if (!!existingHok) { + existingHok.remove(); + } + + let color = hok.name == "one" ? this.oneColor : this.twoColor; + this.hokuyos[hok.name] = this.r.path("M-5 3L5 0L-5 -3L-5 3").attr({ + stroke: color, + fill: color, + transform: "t " + this.center.str + "s" + this.viewportScale + "," + this.viewportScale + ",0,0" + "t " + hok.position.x + "," + hok.position.y + "r" + hok.position.w, + "fill-opacity": .4}); + }.bind(this)); + + // For each object + for(let obj of this.objects) { + obj.remove(); + } + + this.objects = []; + + for(let robot of robots) { + this.objects.push( this.r.circle(robot[0], robot[1], this.dotRadius * 5 ).attr({ + stroke: this.dotColor, + fill: this.dotColor, + transform: "t," + this.center.str + "s" + this.viewportScale + "," + this.viewportScale + ",0,0", + "fill-opacity": .4}) ); + this.objects.push( this.r.text(robot[0], robot[1], "["+ parseInt(robot[0]) + "; " + parseInt(robot[1]) +"]").attr({ + fill: this.dotColor, + "font-size": "6px", + transform: "t,50,25" + "t," + this.center.str + "s" + this.viewportScale + "," + this.viewportScale + ",0,0"}) ); + + } + + // console.log(cartesianSpots); + if (!!cartesianSpots) { + for(let spot of cartesianSpots) { + this.objects.push( this.r.circle(spot[0], spot[1], this.dotRadius).attr({ + stroke: this.dotColor, + fill: this.dotColor, + transform: "t," + this.center.str + "s" + this.viewportScale + "," + this.viewportScale + ",0,0", + "fill-opacity": .4}) ); + } + } + + this.isBusy = false; + + this.clearMainTimeout = setTimeout(function() { + //this.clearMain(); + }.bind(this), 1000); + } + + clearMain() { + // For each object + for(let hokName in this.hokuyos) { + this.hokuyos[hokName].remove(); + } + + // For each object + for(let obj of this.objects) { + obj.remove(); + } + } + } + + + + /* eslint-enable */ diff --git a/webclient/app/navbar/navbar.component.js b/webclient/app/navbar/navbar.component.js new file mode 100755 index 0000000..b5a553b --- /dev/null +++ b/webclient/app/navbar/navbar.component.js @@ -0,0 +1,15 @@ +class NavbarController { + constructor($location, Ros) { + this.$location = $location; + this.ros = Ros; + } + + isPath(path) { + return this.$location.path() === path; + } +} + +angular.module('roscc').component('ccNavbar', { + templateUrl: 'app/navbar/navbar.html', + controller: NavbarController, +}); diff --git a/webclient/app/navbar/navbar.html b/webclient/app/navbar/navbar.html new file mode 100755 index 0000000..fbb50e5 --- /dev/null +++ b/webclient/app/navbar/navbar.html @@ -0,0 +1,26 @@ + diff --git a/webclient/app/parameters/parameters.component.js b/webclient/app/parameters/parameters.component.js new file mode 100755 index 0000000..16eaec7 --- /dev/null +++ b/webclient/app/parameters/parameters.component.js @@ -0,0 +1,22 @@ +class ParameterController { + constructor($timeout, Ros) { + this.ros = Ros; + this.$timeout = $timeout; + } + + $onInit() { + this.$timeout(function() { + this.param = new ROSLIB.Param({ ros: this.ros.ros, name: this.parameter.name }); + }.bind(this), 500); + } + + setValue(value) { + this.param.set(value); + } +} + +angular.module('roscc').component('ccParameter', { + bindings: { parameter: '=' }, + templateUrl: 'app/parameters/parameters.html', + controller: ParameterController, +}); diff --git a/webclient/app/parameters/parameters.html b/webclient/app/parameters/parameters.html new file mode 100755 index 0000000..fbc0fe4 --- /dev/null +++ b/webclient/app/parameters/parameters.html @@ -0,0 +1,14 @@ +
+
+

{{ $ctrl.parameter.abbr }}

+
+ +
+
+ + + + +
+
+
diff --git a/webclient/app/services/default.html b/webclient/app/services/default.html new file mode 100755 index 0000000..2928669 --- /dev/null +++ b/webclient/app/services/default.html @@ -0,0 +1,14 @@ + +
+
+
+ + + + +
+
+
+
+

Result: {{ $ctrl.result }}

+
diff --git a/webclient/app/services/drivers_ard_asserv/EmergencyStop.html b/webclient/app/services/drivers_ard_asserv/EmergencyStop.html new file mode 100644 index 0000000..50c036e --- /dev/null +++ b/webclient/app/services/drivers_ard_asserv/EmergencyStop.html @@ -0,0 +1,14 @@ + +
+ + +
diff --git a/webclient/app/services/drivers_ard_asserv/Goto.html b/webclient/app/services/drivers_ard_asserv/Goto.html new file mode 100644 index 0000000..815525d --- /dev/null +++ b/webclient/app/services/drivers_ard_asserv/Goto.html @@ -0,0 +1,51 @@ + +
+
+ + + +
+
+ +
+ +
+
+ +
+ +
+ + m +
+
+ +
+ +
+ + m +
+
+ +
+ +
+ + deg +
+
+
+ + diff --git a/webclient/app/services/drivers_ard_asserv/Management.html b/webclient/app/services/drivers_ard_asserv/Management.html new file mode 100644 index 0000000..8a59df0 --- /dev/null +++ b/webclient/app/services/drivers_ard_asserv/Management.html @@ -0,0 +1,13 @@ + +
+ +
+
+
+ +
+
+
diff --git a/webclient/app/services/drivers_ard_asserv/Parameters.html b/webclient/app/services/drivers_ard_asserv/Parameters.html new file mode 100644 index 0000000..ecc33ac --- /dev/null +++ b/webclient/app/services/drivers_ard_asserv/Parameters.html @@ -0,0 +1,64 @@ + + +
+
+ +
+
+ +
+ +
+
+ +
+ +
+ +
+
+ +
+ +
+ +
+
+ +
+ +
+ +
+
+ + +
+ +
+ + m/s +
+
+ +
+ +
+ + rad/m +
+
+ +
+ +
+ + m/s² +
+
+
+ diff --git a/webclient/app/services/drivers_ard_asserv/Pwm.html b/webclient/app/services/drivers_ard_asserv/Pwm.html new file mode 100644 index 0000000..c14d66d --- /dev/null +++ b/webclient/app/services/drivers_ard_asserv/Pwm.html @@ -0,0 +1,24 @@ +
+ +
+ + +
+ +
+ + +
+ +
+ +
+ + ms +
+
+ +
+ diff --git a/webclient/app/services/drivers_ard_asserv/SetPos.html b/webclient/app/services/drivers_ard_asserv/SetPos.html new file mode 100644 index 0000000..0e5ae19 --- /dev/null +++ b/webclient/app/services/drivers_ard_asserv/SetPos.html @@ -0,0 +1,37 @@ + +
+ +
+
+ +
+ + m +
+
+ +
+ +
+ + m +
+
+ +
+ +
+ + deg +
+
+
+ diff --git a/webclient/app/services/drivers_ard_asserv/Speed.html b/webclient/app/services/drivers_ard_asserv/Speed.html new file mode 100644 index 0000000..8dfd03b --- /dev/null +++ b/webclient/app/services/drivers_ard_asserv/Speed.html @@ -0,0 +1,29 @@ +
+
+ +
+ + m/s +
+
+ +
+ +
+ + rad/s +
+
+ +
+ +
+ + ms +
+
+
+ + diff --git a/webclient/app/services/drivers_port_finder/GetPort.html b/webclient/app/services/drivers_port_finder/GetPort.html new file mode 100644 index 0000000..ba10e62 --- /dev/null +++ b/webclient/app/services/drivers_port_finder/GetPort.html @@ -0,0 +1,20 @@ +
+
+ +
+ + +
+
+
+ +

Result: {{ $ctrl.result.port }}

+
+
+ + diff --git a/webclient/app/services/meta.html b/webclient/app/services/meta.html new file mode 100644 index 0000000..61c38cb --- /dev/null +++ b/webclient/app/services/meta.html @@ -0,0 +1,16 @@ +
+ + {{ $ctrl.service.abbr }} +
({{ $ctrl.service.type }})
+
+ + + +
diff --git a/webclient/app/services/service.component.js b/webclient/app/services/service.component.js new file mode 100755 index 0000000..3971595 --- /dev/null +++ b/webclient/app/services/service.component.js @@ -0,0 +1,86 @@ +class ServiceController { + constructor($scope, $http, $timeout, Ros) { + this.$scope = $scope; + this.$http = $http; + this.ros = Ros; + this.$timeout = $timeout; + this.flashState = -1; + } + + $onInit() { + + this.$scope.$watchGroup(['$ctrl.service.type', '$ctrl.service.active'], () => { + this.setFileName(); + }, () => {}); + } + + setFileName() { + const path = 'app/services/'; + + + this.fileName = `${path}default.html`; + + + if(!this.service.active) { + this.fileName = ''; + this.service.isOpen = false; + return; + } + else if (!this.service.type) { + this.fileName = `${path}default.html`; + return; + } + + const fileName = `${path}${this.service.type}.html`; + this.$http.get(fileName).then((result) => { + if (result.data) { + this.fileName = fileName; + } + }, () => {}); + } + + callService(input, isJSON) { + //(!this.service.active) + // return; + + const data = isJSON ? angular.fromJson(input) : input; + const ROSservice = new ROSLIB.Service({ + ros: this.ros.ros, + name: this.service.name, + serviceType: this.service.type, + }); + const request = new ROSLIB.ServiceRequest(data); + + this.flashState = -1; + + ROSservice.callService(request, (result) => { + this.result = result; + this.flashState = -1; + // -1 : no flash + // 0 : returned some object + // 1 : return some object with a success = true + // 2 : return some object with a success = false + + //assume this is the success state + if(_.keys(result).length == 1 && typeof result[_.keys(result)[0]] === 'boolean') { + let success = result[_.keys(result)[0]]; + if(success) + this.flashState = 1 + else + this.flashState = 2 + } else { + this.flashState = 0 + } + if(this.flashBackPromise) + this.$timeout.cancel(this.flashBackPromise); + + this.flashBackPromise = this.$timeout(() => { this.flashState = -1 }, 2000); + }); + } +} + +angular.module('roscc').component('ccService', { + bindings: { service: '=' }, + template: ``, + controller: ServiceController, +}); diff --git a/webclient/app/services/std_srvs/Empty.html b/webclient/app/services/std_srvs/Empty.html new file mode 100755 index 0000000..ead13f4 --- /dev/null +++ b/webclient/app/services/std_srvs/Empty.html @@ -0,0 +1,3 @@ +
+ +
diff --git a/webclient/app/services/std_srvs/SetBool.html b/webclient/app/services/std_srvs/SetBool.html new file mode 100755 index 0000000..87d9b18 --- /dev/null +++ b/webclient/app/services/std_srvs/SetBool.html @@ -0,0 +1,6 @@ +
+ +
diff --git a/webclient/app/services/std_srvs/Trigger.html b/webclient/app/services/std_srvs/Trigger.html new file mode 100755 index 0000000..6717325 --- /dev/null +++ b/webclient/app/services/std_srvs/Trigger.html @@ -0,0 +1,8 @@ +
+
+ +
+
+ {{ $ctrl.result.success }}: {{ $ctrl.result.message }} +
+
diff --git a/webclient/app/settings/settings.component.js b/webclient/app/settings/settings.component.js new file mode 100755 index 0000000..314da2d --- /dev/null +++ b/webclient/app/settings/settings.component.js @@ -0,0 +1,39 @@ +class SettingsController { + constructor(localStorageService, Settings) { + this.Settings = Settings; + + this.settings = Settings.getSettings() || [Settings.getDefaultSetting()]; + this.index = Settings.getIndex(); + + if (!this.index || this.index > this.settings.length) { + this.index = '0'; + } + + this.save(); // Save current setting again (if it's the first time) + } + + save() { + this.Settings.save(this.settings, this.index); + } + + add() { + this.settings.push(this.Settings.getDefaultSetting()); // Clone object + this.index = String(this.settings.length - 1); + this.save(); + } + + remove() { + this.settings.splice(this.index, 1); + this.index = '0'; + + if (!this.settings.length) { + this.add(); + } + this.save(); + } +} + +angular.module('roscc').component('ccSettings', { + templateUrl: 'app/settings/settings.html', + controller: SettingsController, +}); diff --git a/webclient/app/settings/settings.html b/webclient/app/settings/settings.html new file mode 100755 index 0000000..37b8b08 --- /dev/null +++ b/webclient/app/settings/settings.html @@ -0,0 +1,114 @@ +
+
+
+

Settings

+ +
+
+ +
+ +
+ + +
+
+ + +
+
+
+
+ +
+ +
+
+ +
+ +
+ +
+
+ +
+
+ +
+ +
+ +
+
+ +
+ +
+ +
+
+ +
+ +
+ +
+
+ +
+ +
+ +
+
+ +
+ +
+ +
+
+ +
+ +
+ +
+
+ + + + +
+
+
+ + + +
+
+
diff --git a/webclient/app/settings/settings.service.js b/webclient/app/settings/settings.service.js new file mode 100755 index 0000000..7a0fff5 --- /dev/null +++ b/webclient/app/settings/settings.service.js @@ -0,0 +1,68 @@ +class SettingsService { + constructor($location, localStorageService) { + this.$location = $location; + this.localStorageService = localStorageService; + } + + load() { + this.index = this.localStorageService.get('selectedSettingIndex'); + this.settings = this.localStorageService.get('settings'); + if (this.settings && this.index) { + this.setting = this.settings[this.index]; + } + + // If there are no saved settings, redirect to /settings for first setting input + if (!this.setting) { + this.$location.path('/settings').replace(); + } + } + + save(newSettings, newIndex) { + this.settings = newSettings; + this.index = newIndex; + this.localStorageService.set('selectedSettingIndex', newIndex); + this.localStorageService.set('settings', newSettings); + } + + get() { + if (!this.setting) { + this.load(); + } + + return this.setting; + } + + getIndex() { + if (!this.setting) { + this.load(); + } + + return this.index; + } + + getSettings() { + if (!this.setting) { + this.load(); + } + + return this.settings; + } + + getDefaultSetting() { + return { + name: 'Robot Name', + address: window.location.hostname, + port: 9090, // default port of rosbridge_server + log: '/rosout', + advanced: false, + hokuyo_1: '/sensors/hokuyo_1_raw', + hokuyo_2: '/sensors/hokuyo_2_raw', + maxConsoleEntries: 1000, + refresh_rate: 1, + log_level: 2 + }; + } +} + + +angular.module('roscc').service('Settings', SettingsService); diff --git a/webclient/app/topics/default.html b/webclient/app/topics/default.html new file mode 100755 index 0000000..973200f --- /dev/null +++ b/webclient/app/topics/default.html @@ -0,0 +1,16 @@ + +
+ +
+
+ + +
+
+ +
diff --git a/webclient/app/topics/drivers_ard_asserv/RobotSpeed.html b/webclient/app/topics/drivers_ard_asserv/RobotSpeed.html new file mode 100644 index 0000000..33e2e5f --- /dev/null +++ b/webclient/app/topics/drivers_ard_asserv/RobotSpeed.html @@ -0,0 +1,62 @@ +
+ +
+ +
+ + +
+
+ +
+ +
+ + +
+
+ +
+ +
+ + +
+
+ +
+ +
+ + +
+
+ +
+ +
+ + +
+
+
diff --git a/webclient/app/topics/drivers_ard_others/BeltRange.html b/webclient/app/topics/drivers_ard_others/BeltRange.html new file mode 100644 index 0000000..8a14e10 --- /dev/null +++ b/webclient/app/topics/drivers_ard_others/BeltRange.html @@ -0,0 +1,21 @@ +
+ +
+ +
+ + +
+
+
+ +
+ + m +
+
+
diff --git a/webclient/app/topics/geometry_msgs/Point.html b/webclient/app/topics/geometry_msgs/Point.html new file mode 100755 index 0000000..9be7cb1 --- /dev/null +++ b/webclient/app/topics/geometry_msgs/Point.html @@ -0,0 +1,18 @@ +
+ +
+ +
+ + m +
+
+
+ + +
+ + m +
+
+
diff --git a/webclient/app/topics/geometry_msgs/Pose.html b/webclient/app/topics/geometry_msgs/Pose.html new file mode 100755 index 0000000..2c7da5c --- /dev/null +++ b/webclient/app/topics/geometry_msgs/Pose.html @@ -0,0 +1,52 @@ +
+
+ +
+ + m +
+
+
+ + +
+ + m +
+
+ +
+ +
+ + m +
+
+
+
+ +
+ +
+ + ° +
+
+ +
+ +
+ + ° +
+
+ +
+ +
+ + rad +
+
+ +
diff --git a/webclient/app/topics/geometry_msgs/Pose2D.html b/webclient/app/topics/geometry_msgs/Pose2D.html new file mode 100755 index 0000000..b4f431a --- /dev/null +++ b/webclient/app/topics/geometry_msgs/Pose2D.html @@ -0,0 +1,27 @@ +
+ +
+ +
+ + m +
+
+
+ + +
+ + m +
+
+ +
+ +
+ + rad +
+
+ +
diff --git a/webclient/app/topics/geometry_msgs/PoseStamped.html b/webclient/app/topics/geometry_msgs/PoseStamped.html new file mode 100755 index 0000000..737a2a9 --- /dev/null +++ b/webclient/app/topics/geometry_msgs/PoseStamped.html @@ -0,0 +1,52 @@ +
+
+ +
+ + m +
+
+
+ + +
+ + m +
+
+ +
+ +
+ + m +
+
+
+
+ +
+ +
+ + ° +
+
+ +
+ +
+ + ° +
+
+ +
+ +
+ + ° +
+
+ +
diff --git a/webclient/app/topics/meta.html b/webclient/app/topics/meta.html new file mode 100644 index 0000000..62f660e --- /dev/null +++ b/webclient/app/topics/meta.html @@ -0,0 +1,43 @@ + +
+ + {{ $ctrl.topic.abbr }} + +
+ ({{ $ctrl.topic.active ? $ctrl.topic.type : 'Not running !' }}) +
+
+ + +
+
+ +
+
+
+
+ +
+
+
+ + +
diff --git a/webclient/app/topics/sensor_msgs/FluidPressure.html b/webclient/app/topics/sensor_msgs/FluidPressure.html new file mode 100755 index 0000000..7e8249d --- /dev/null +++ b/webclient/app/topics/sensor_msgs/FluidPressure.html @@ -0,0 +1,22 @@ +
+ + {{ $ctrl.topic.abbr }} + + +
({{ $ctrl.topic.type }})
+ +
+ + +
+
+
+ +
+ + Pa +
+
+
+
+
diff --git a/webclient/app/topics/sensor_msgs/Illuminance.html b/webclient/app/topics/sensor_msgs/Illuminance.html new file mode 100755 index 0000000..8b83a2f --- /dev/null +++ b/webclient/app/topics/sensor_msgs/Illuminance.html @@ -0,0 +1,22 @@ +
+ + {{ $ctrl.topic.abbr }} + + +
({{ $ctrl.topic.type }})
+ +
+ + +
+
+
+ +
+ + a.u. +
+
+
+
+
diff --git a/webclient/app/topics/sensor_msgs/Image.html b/webclient/app/topics/sensor_msgs/Image.html new file mode 100755 index 0000000..e4c3743 --- /dev/null +++ b/webclient/app/topics/sensor_msgs/Image.html @@ -0,0 +1,20 @@ +
+ + {{ $ctrl.topic.abbr }} + + +
({{ $ctrl.topic.type }})
+ +
+ + +
+
+
+
+ +
+
+
+
+
diff --git a/webclient/app/topics/sensor_msgs/Imu.html b/webclient/app/topics/sensor_msgs/Imu.html new file mode 100755 index 0000000..e2c05ba --- /dev/null +++ b/webclient/app/topics/sensor_msgs/Imu.html @@ -0,0 +1,70 @@ +
+ + {{ $ctrl.topic.abbr }} + + +
({{ $ctrl.topic.type }})
+ +
+ + +
+
+
+
+
+ +
+ + m/s2 +
+
+ +
+ +
+ + m/s2 +
+
+ +
+ +
+ + m/s2 +
+
+
+
+ +
+
+
+ +
+ + rad/s +
+
+ +
+ +
+ + rad/s +
+
+ +
+ +
+ + rad/s +
+
+
+
+
+
+
diff --git a/webclient/app/topics/sensor_msgs/Joy.html b/webclient/app/topics/sensor_msgs/Joy.html new file mode 100755 index 0000000..6eac6ac --- /dev/null +++ b/webclient/app/topics/sensor_msgs/Joy.html @@ -0,0 +1,115 @@ +
+ + {{ $ctrl.topic.abbr }} + + +
({{ $ctrl.topic.type }})
+ +
+ +
+
+
+
+ +
+ + +
+
+ +
+ +
+ + +
+
+ +
+ +
+ + +
+
+ +
+ +
+ + +
+
+ +
+ +
+ + +
+
+ +
+ +
+ + +
+
+
+ +
+
+ +
+ + +
+
+ +
+ +
+ + +
+
+ +
+ +
+ + +
+
+ + +
+ +
+ + +
+
+ + +
+ +
+ + +
+
+ +
+ +
+ + +
+
+
+
+
+
diff --git a/webclient/app/topics/sensor_msgs/MagneticField.html b/webclient/app/topics/sensor_msgs/MagneticField.html new file mode 100755 index 0000000..a33ea90 --- /dev/null +++ b/webclient/app/topics/sensor_msgs/MagneticField.html @@ -0,0 +1,42 @@ +
+ + {{ $ctrl.topic.abbr }} + + +
({{ $ctrl.topic.type }})
+ +
+ + +
+
+
+
+
+ +
+ + m/s2 +
+
+ +
+ +
+ + m/s2 +
+
+ +
+ +
+ + m/s2 +
+
+
+
+
+
+
diff --git a/webclient/app/topics/sensor_msgs/Range.html b/webclient/app/topics/sensor_msgs/Range.html new file mode 100755 index 0000000..bd6e7de --- /dev/null +++ b/webclient/app/topics/sensor_msgs/Range.html @@ -0,0 +1,22 @@ +
+ + {{ $ctrl.topic.abbr }} + + +
({{ $ctrl.topic.type }})
+ +
+ + +
+
+
+ +
+ + m +
+
+
+
+
diff --git a/webclient/app/topics/sensor_msgs/RelativeHumidity.html b/webclient/app/topics/sensor_msgs/RelativeHumidity.html new file mode 100755 index 0000000..8bc1c5c --- /dev/null +++ b/webclient/app/topics/sensor_msgs/RelativeHumidity.html @@ -0,0 +1,22 @@ +
+ + {{ $ctrl.topic.abbr }} + + +
({{ $ctrl.topic.type }})
+ +
+ + +
+
+
+ +
+ + % +
+
+
+
+
diff --git a/webclient/app/topics/sensor_msgs/Temperature.html b/webclient/app/topics/sensor_msgs/Temperature.html new file mode 100755 index 0000000..7ae7b23 --- /dev/null +++ b/webclient/app/topics/sensor_msgs/Temperature.html @@ -0,0 +1,22 @@ +
+ + {{ $ctrl.topic.abbr }} + + +
({{ $ctrl.topic.type }})
+ +
+ + +
+
+
+ +
+ + ° C +
+
+
+
+
diff --git a/webclient/app/topics/std_msgs/Float32.html b/webclient/app/topics/std_msgs/Float32.html new file mode 100755 index 0000000..e824ac9 --- /dev/null +++ b/webclient/app/topics/std_msgs/Float32.html @@ -0,0 +1,12 @@ +
+ +
+ + +
+ +
diff --git a/webclient/app/topics/std_msgs/Float64.html b/webclient/app/topics/std_msgs/Float64.html new file mode 100755 index 0000000..e824ac9 --- /dev/null +++ b/webclient/app/topics/std_msgs/Float64.html @@ -0,0 +1,12 @@ +
+ +
+ + +
+ +
diff --git a/webclient/app/topics/std_msgs/Int16.html b/webclient/app/topics/std_msgs/Int16.html new file mode 100755 index 0000000..cf5bc4f --- /dev/null +++ b/webclient/app/topics/std_msgs/Int16.html @@ -0,0 +1,14 @@ +
+ +
+ + +
+ +
diff --git a/webclient/app/topics/std_msgs/Int32.html b/webclient/app/topics/std_msgs/Int32.html new file mode 100755 index 0000000..e426a0c --- /dev/null +++ b/webclient/app/topics/std_msgs/Int32.html @@ -0,0 +1,14 @@ +
+ +
+ + +
+ +
diff --git a/webclient/app/topics/std_msgs/Int64.html b/webclient/app/topics/std_msgs/Int64.html new file mode 100755 index 0000000..829d993 --- /dev/null +++ b/webclient/app/topics/std_msgs/Int64.html @@ -0,0 +1,12 @@ +
+ +
+ + +
+ +
diff --git a/webclient/app/topics/std_msgs/Int8.html b/webclient/app/topics/std_msgs/Int8.html new file mode 100755 index 0000000..76e9ccb --- /dev/null +++ b/webclient/app/topics/std_msgs/Int8.html @@ -0,0 +1,15 @@ +
+ +
+ + +
+ +
diff --git a/webclient/app/topics/std_msgs/String.html b/webclient/app/topics/std_msgs/String.html new file mode 100755 index 0000000..df50202 --- /dev/null +++ b/webclient/app/topics/std_msgs/String.html @@ -0,0 +1,12 @@ +
+ +
+ + +
+ +
diff --git a/webclient/app/topics/std_msgs/UInt16.html b/webclient/app/topics/std_msgs/UInt16.html new file mode 100755 index 0000000..b2499b5 --- /dev/null +++ b/webclient/app/topics/std_msgs/UInt16.html @@ -0,0 +1,14 @@ +
+ +
+ + +
+ +
diff --git a/webclient/app/topics/std_msgs/UInt32.html b/webclient/app/topics/std_msgs/UInt32.html new file mode 100755 index 0000000..4e5caae --- /dev/null +++ b/webclient/app/topics/std_msgs/UInt32.html @@ -0,0 +1,14 @@ +
+ +
+ + +
+ +
diff --git a/webclient/app/topics/std_msgs/UInt64.html b/webclient/app/topics/std_msgs/UInt64.html new file mode 100755 index 0000000..7f73eb4 --- /dev/null +++ b/webclient/app/topics/std_msgs/UInt64.html @@ -0,0 +1,14 @@ +
+ +
+ + +
+ +
diff --git a/webclient/app/topics/std_msgs/UInt8.html b/webclient/app/topics/std_msgs/UInt8.html new file mode 100755 index 0000000..8817143 --- /dev/null +++ b/webclient/app/topics/std_msgs/UInt8.html @@ -0,0 +1,14 @@ +
+ +
+ + +
+ +
diff --git a/webclient/app/topics/topic.component.js b/webclient/app/topics/topic.component.js new file mode 100755 index 0000000..7d176f6 --- /dev/null +++ b/webclient/app/topics/topic.component.js @@ -0,0 +1,78 @@ +class TopicController { + constructor($scope, $http, Settings, Quaternions, Ros) { + this.$scope = $scope; + this.$http = $http; + this.setting = Settings.get(); + this.Quaternions = Quaternions; + this.ros = Ros; + this.isSubscribing = false; + this.toggle = true; + this.isOpen = true; + } + + $onInit() { + this.roslibTopic = new ROSLIB.Topic({ + ros : this.ros.ros, + name: this.topic.name, + messageType: this.topic.type, + }); + + const path = 'app/topics/'; + this.fileName = `${path}default.html`; + + this.$scope.$watchGroup(['$ctrl.topic.type', '$ctrl.topic.active'], () => { + this.isSubscribing = false; + this.toggle = false; + + if(!this.topic.active) { + this.fileName = ''; + this.topic.isOpen = false; + return; + } + + else if (!this.topic.type) { + this.fileName = `${path}default.html`; + //this.toggleSubscription(false); + return; + } + + const fileName = `${path}${this.topic.type}.html`; + this.$http.get(fileName).then((result) => { + if (result.data) { + this.fileName = fileName; + //this.toggleSubscription(false); + } + }, () => {}); + }); + } + + toggleSubscription(data) { + if(!this.topic.active) + return; + if (!data) { + this.roslibTopic.subscribe((message) => { + if(!this.fileName.includes('default.html')) + this.message = message; + else + this.message = JSON.stringify(message); + }); + } else { + this.roslibTopic.unsubscribe(); + } + this.isSubscribing = !data; + + } + + publishMessage(input) { + const data = !this.fileName.includes('default.html') ? angular.fromJson(input) : input; + const message = new ROSLIB.Message(data); + this.roslibTopic.publish(message); + } + +} + +angular.module('roscc').component('ccTopic', { + bindings: { topic: '=' }, + template: ``, + controller: TopicController +}); diff --git a/webclient/app/transforms/transforms.component.js b/webclient/app/transforms/transforms.component.js new file mode 100644 index 0000000..ac5362c --- /dev/null +++ b/webclient/app/transforms/transforms.component.js @@ -0,0 +1,38 @@ +class TransformController { + constructor(Ros) { + this.ros = Ros; + } + + $onInit() { + this.refresh(); + } + + refresh() { // relative to fixed + if(!this.transform || !this.transform.fixed || !this.transform.frame) + return; + + this.tfClient = new ROSLIB.TFClient({ + ros : this.ros.ros, + fixedFrame : this.transform.fixed, + angularThres : 0.001, + transThres : 0.001 + }); + + this.tfClient.subscribe(this.transform.frame, function(tf) { + let eulAngles = new THREE.Euler(); + + eulAngles.setFromQuaternion(new THREE.Quaternion( tf.rotation.x, tf.rotation.y, tf.rotation.z, tf.rotation.w )) + + this.transform.response = tf + this.transform.response.rotation = eulAngles; + + }.bind(this)); + } + + +} + +angular.module('roscc').component('ccTransform', { + templateUrl: 'app/transforms/transforms.html', + controller: TransformController, +}); diff --git a/webclient/app/transforms/transforms.html b/webclient/app/transforms/transforms.html new file mode 100644 index 0000000..08627ed --- /dev/null +++ b/webclient/app/transforms/transforms.html @@ -0,0 +1,59 @@ +
+
+
+
+ + +
+
+
+ + +
+
+
+ +
+
+
+
+ +
+ {{ $ctrl.transform.response.translation.x.toFixed(5) }} +
+
+ +
+ {{ $ctrl.transform.response.translation.y.toFixed(5) }} +
+
+ +
+ {{ $ctrl.transform.response.translation.z.toFixed(5) }} +
+
+
+
+
+ +
+ {{ $ctrl.transform.response.rotation.x.toFixed(5) }} +
+
+ +
+ {{ $ctrl.transform.response.rotation.y.toFixed(5) }} +
+
+ +
+ {{ $ctrl.transform.response.rotation.z.toFixed(5) }} +
+
+
+
+
diff --git a/webclient/assets/css/bootstrap.min.css b/webclient/assets/css/bootstrap.min.css new file mode 100755 index 0000000..4cf729e --- /dev/null +++ b/webclient/assets/css/bootstrap.min.css @@ -0,0 +1,6 @@ +/*! + * Bootstrap v3.3.6 (http://getbootstrap.com) + * Copyright 2011-2015 Twitter, Inc. + * Licensed under MIT (https://github.com/twbs/bootstrap/blob/master/LICENSE) + *//*! normalize.css v3.0.3 | MIT License | github.com/necolas/normalize.css */html{font-family:sans-serif;-webkit-text-size-adjust:100%;-ms-text-size-adjust:100%}body{margin:0}article,aside,details,figcaption,figure,footer,header,hgroup,main,menu,nav,section,summary{display:block}audio,canvas,progress,video{display:inline-block;vertical-align:baseline}audio:not([controls]){display:none;height:0}[hidden],template{display:none}a{background-color:transparent}a:active,a:hover{outline:0}abbr[title]{border-bottom:1px dotted}b,strong{font-weight:700}dfn{font-style:italic}h1{margin:.67em 0;font-size:2em}mark{color:#000;background:#ff0}small{font-size:80%}sub,sup{position:relative;font-size:75%;line-height:0;vertical-align:baseline}sup{top:-.5em}sub{bottom:-.25em}img{border:0}svg:not(:root){overflow:hidden}figure{margin:1em 40px}hr{height:0;-webkit-box-sizing:content-box;-moz-box-sizing:content-box;box-sizing:content-box}pre{overflow:auto}code,kbd,pre,samp{font-family:monospace,monospace;font-size:1em}button,input,optgroup,select,textarea{margin:0;font:inherit;color:inherit}button{overflow:visible}button,select{text-transform:none}button,html input[type=button],input[type=reset],input[type=submit]{-webkit-appearance:button;cursor:pointer}button[disabled],html input[disabled]{cursor:default}button::-moz-focus-inner,input::-moz-focus-inner{padding:0;border:0}input{line-height:normal}input[type=checkbox],input[type=radio]{-webkit-box-sizing:border-box;-moz-box-sizing:border-box;box-sizing:border-box;padding:0}input[type=number]::-webkit-inner-spin-button,input[type=number]::-webkit-outer-spin-button{height:auto}input[type=search]{-webkit-box-sizing:content-box;-moz-box-sizing:content-box;box-sizing:content-box;-webkit-appearance:textfield}input[type=search]::-webkit-search-cancel-button,input[type=search]::-webkit-search-decoration{-webkit-appearance:none}fieldset{padding:.35em .625em .75em;margin:0 2px;border:1px solid silver}legend{padding:0;border:0}textarea{overflow:auto}optgroup{font-weight:700}table{border-spacing:0;border-collapse:collapse}td,th{padding:0}/*! Source: https://github.com/h5bp/html5-boilerplate/blob/master/src/css/main.css */@media print{*,:after,:before{color:#000!important;text-shadow:none!important;background:0 0!important;-webkit-box-shadow:none!important;box-shadow:none!important}a,a:visited{text-decoration:underline}a[href]:after{content:" (" attr(href) ")"}abbr[title]:after{content:" (" attr(title) ")"}a[href^="javascript:"]:after,a[href^="#"]:after{content:""}blockquote,pre{border:1px solid #999;page-break-inside:avoid}thead{display:table-header-group}img,tr{page-break-inside:avoid}img{max-width:100%!important}h2,h3,p{orphans:3;widows:3}h2,h3{page-break-after:avoid}.navbar{display:none}.btn>.caret,.dropup>.btn>.caret{border-top-color:#000!important}.label{border:1px solid #000}.table{border-collapse:collapse!important}.table td,.table th{background-color:#fff!important}.table-bordered td,.table-bordered th{border:1px solid #ddd!important}}@font-face{font-family:'Glyphicons Halflings';src:url(../fonts/glyphicons-halflings-regular.eot);src:url(../fonts/glyphicons-halflings-regular.eot?#iefix) format('embedded-opentype'),url(../fonts/glyphicons-halflings-regular.woff2) format('woff2'),url(../fonts/glyphicons-halflings-regular.woff) format('woff'),url(../fonts/glyphicons-halflings-regular.ttf) format('truetype'),url(../fonts/glyphicons-halflings-regular.svg#glyphicons_halflingsregular) format('svg')}.glyphicon{position:relative;top:1px;display:inline-block;font-family:'Glyphicons Halflings';font-style:normal;font-weight:400;line-height:1;-webkit-font-smoothing:antialiased;-moz-osx-font-smoothing:grayscale}.glyphicon-asterisk:before{content:"\002a"}.glyphicon-plus:before{content:"\002b"}.glyphicon-eur:before,.glyphicon-euro:before{content:"\20ac"}.glyphicon-minus:before{content:"\2212"}.glyphicon-cloud:before{content:"\2601"}.glyphicon-envelope:before{content:"\2709"}.glyphicon-pencil:before{content:"\270f"}.glyphicon-glass:before{content:"\e001"}.glyphicon-music:before{content:"\e002"}.glyphicon-search:before{content:"\e003"}.glyphicon-heart:before{content:"\e005"}.glyphicon-star:before{content:"\e006"}.glyphicon-star-empty:before{content:"\e007"}.glyphicon-user:before{content:"\e008"}.glyphicon-film:before{content:"\e009"}.glyphicon-th-large:before{content:"\e010"}.glyphicon-th:before{content:"\e011"}.glyphicon-th-list:before{content:"\e012"}.glyphicon-ok:before{content:"\e013"}.glyphicon-remove:before{content:"\e014"}.glyphicon-zoom-in:before{content:"\e015"}.glyphicon-zoom-out:before{content:"\e016"}.glyphicon-off:before{content:"\e017"}.glyphicon-signal:before{content:"\e018"}.glyphicon-cog:before{content:"\e019"}.glyphicon-trash:before{content:"\e020"}.glyphicon-home:before{content:"\e021"}.glyphicon-file:before{content:"\e022"}.glyphicon-time:before{content:"\e023"}.glyphicon-road:before{content:"\e024"}.glyphicon-download-alt:before{content:"\e025"}.glyphicon-download:before{content:"\e026"}.glyphicon-upload:before{content:"\e027"}.glyphicon-inbox:before{content:"\e028"}.glyphicon-play-circle:before{content:"\e029"}.glyphicon-repeat:before{content:"\e030"}.glyphicon-refresh:before{content:"\e031"}.glyphicon-list-alt:before{content:"\e032"}.glyphicon-lock:before{content:"\e033"}.glyphicon-flag:before{content:"\e034"}.glyphicon-headphones:before{content:"\e035"}.glyphicon-volume-off:before{content:"\e036"}.glyphicon-volume-down:before{content:"\e037"}.glyphicon-volume-up:before{content:"\e038"}.glyphicon-qrcode:before{content:"\e039"}.glyphicon-barcode:before{content:"\e040"}.glyphicon-tag:before{content:"\e041"}.glyphicon-tags:before{content:"\e042"}.glyphicon-book:before{content:"\e043"}.glyphicon-bookmark:before{content:"\e044"}.glyphicon-print:before{content:"\e045"}.glyphicon-camera:before{content:"\e046"}.glyphicon-font:before{content:"\e047"}.glyphicon-bold:before{content:"\e048"}.glyphicon-italic:before{content:"\e049"}.glyphicon-text-height:before{content:"\e050"}.glyphicon-text-width:before{content:"\e051"}.glyphicon-align-left:before{content:"\e052"}.glyphicon-align-center:before{content:"\e053"}.glyphicon-align-right:before{content:"\e054"}.glyphicon-align-justify:before{content:"\e055"}.glyphicon-list:before{content:"\e056"}.glyphicon-indent-left:before{content:"\e057"}.glyphicon-indent-right:before{content:"\e058"}.glyphicon-facetime-video:before{content:"\e059"}.glyphicon-picture:before{content:"\e060"}.glyphicon-map-marker:before{content:"\e062"}.glyphicon-adjust:before{content:"\e063"}.glyphicon-tint:before{content:"\e064"}.glyphicon-edit:before{content:"\e065"}.glyphicon-share:before{content:"\e066"}.glyphicon-check:before{content:"\e067"}.glyphicon-move:before{content:"\e068"}.glyphicon-step-backward:before{content:"\e069"}.glyphicon-fast-backward:before{content:"\e070"}.glyphicon-backward:before{content:"\e071"}.glyphicon-play:before{content:"\e072"}.glyphicon-pause:before{content:"\e073"}.glyphicon-stop:before{content:"\e074"}.glyphicon-forward:before{content:"\e075"}.glyphicon-fast-forward:before{content:"\e076"}.glyphicon-step-forward:before{content:"\e077"}.glyphicon-eject:before{content:"\e078"}.glyphicon-chevron-left:before{content:"\e079"}.glyphicon-chevron-right:before{content:"\e080"}.glyphicon-plus-sign:before{content:"\e081"}.glyphicon-minus-sign:before{content:"\e082"}.glyphicon-remove-sign:before{content:"\e083"}.glyphicon-ok-sign:before{content:"\e084"}.glyphicon-question-sign:before{content:"\e085"}.glyphicon-info-sign:before{content:"\e086"}.glyphicon-screenshot:before{content:"\e087"}.glyphicon-remove-circle:before{content:"\e088"}.glyphicon-ok-circle:before{content:"\e089"}.glyphicon-ban-circle:before{content:"\e090"}.glyphicon-arrow-left:before{content:"\e091"}.glyphicon-arrow-right:before{content:"\e092"}.glyphicon-arrow-up:before{content:"\e093"}.glyphicon-arrow-down:before{content:"\e094"}.glyphicon-share-alt:before{content:"\e095"}.glyphicon-resize-full:before{content:"\e096"}.glyphicon-resize-small:before{content:"\e097"}.glyphicon-exclamation-sign:before{content:"\e101"}.glyphicon-gift:before{content:"\e102"}.glyphicon-leaf:before{content:"\e103"}.glyphicon-fire:before{content:"\e104"}.glyphicon-eye-open:before{content:"\e105"}.glyphicon-eye-close:before{content:"\e106"}.glyphicon-warning-sign:before{content:"\e107"}.glyphicon-plane:before{content:"\e108"}.glyphicon-calendar:before{content:"\e109"}.glyphicon-random:before{content:"\e110"}.glyphicon-comment:before{content:"\e111"}.glyphicon-magnet:before{content:"\e112"}.glyphicon-chevron-up:before{content:"\e113"}.glyphicon-chevron-down:before{content:"\e114"}.glyphicon-retweet:before{content:"\e115"}.glyphicon-shopping-cart:before{content:"\e116"}.glyphicon-folder-close:before{content:"\e117"}.glyphicon-folder-open:before{content:"\e118"}.glyphicon-resize-vertical:before{content:"\e119"}.glyphicon-resize-horizontal:before{content:"\e120"}.glyphicon-hdd:before{content:"\e121"}.glyphicon-bullhorn:before{content:"\e122"}.glyphicon-bell:before{content:"\e123"}.glyphicon-certificate:before{content:"\e124"}.glyphicon-thumbs-up:before{content:"\e125"}.glyphicon-thumbs-down:before{content:"\e126"}.glyphicon-hand-right:before{content:"\e127"}.glyphicon-hand-left:before{content:"\e128"}.glyphicon-hand-up:before{content:"\e129"}.glyphicon-hand-down:before{content:"\e130"}.glyphicon-circle-arrow-right:before{content:"\e131"}.glyphicon-circle-arrow-left:before{content:"\e132"}.glyphicon-circle-arrow-up:before{content:"\e133"}.glyphicon-circle-arrow-down:before{content:"\e134"}.glyphicon-globe:before{content:"\e135"}.glyphicon-wrench:before{content:"\e136"}.glyphicon-tasks:before{content:"\e137"}.glyphicon-filter:before{content:"\e138"}.glyphicon-briefcase:before{content:"\e139"}.glyphicon-fullscreen:before{content:"\e140"}.glyphicon-dashboard:before{content:"\e141"}.glyphicon-paperclip:before{content:"\e142"}.glyphicon-heart-empty:before{content:"\e143"}.glyphicon-link:before{content:"\e144"}.glyphicon-phone:before{content:"\e145"}.glyphicon-pushpin:before{content:"\e146"}.glyphicon-usd:before{content:"\e148"}.glyphicon-gbp:before{content:"\e149"}.glyphicon-sort:before{content:"\e150"}.glyphicon-sort-by-alphabet:before{content:"\e151"}.glyphicon-sort-by-alphabet-alt:before{content:"\e152"}.glyphicon-sort-by-order:before{content:"\e153"}.glyphicon-sort-by-order-alt:before{content:"\e154"}.glyphicon-sort-by-attributes:before{content:"\e155"}.glyphicon-sort-by-attributes-alt:before{content:"\e156"}.glyphicon-unchecked:before{content:"\e157"}.glyphicon-expand:before{content:"\e158"}.glyphicon-collapse-down:before{content:"\e159"}.glyphicon-collapse-up:before{content:"\e160"}.glyphicon-log-in:before{content:"\e161"}.glyphicon-flash:before{content:"\e162"}.glyphicon-log-out:before{content:"\e163"}.glyphicon-new-window:before{content:"\e164"}.glyphicon-record:before{content:"\e165"}.glyphicon-save:before{content:"\e166"}.glyphicon-open:before{content:"\e167"}.glyphicon-saved:before{content:"\e168"}.glyphicon-import:before{content:"\e169"}.glyphicon-export:before{content:"\e170"}.glyphicon-send:before{content:"\e171"}.glyphicon-floppy-disk:before{content:"\e172"}.glyphicon-floppy-saved:before{content:"\e173"}.glyphicon-floppy-remove:before{content:"\e174"}.glyphicon-floppy-save:before{content:"\e175"}.glyphicon-floppy-open:before{content:"\e176"}.glyphicon-credit-card:before{content:"\e177"}.glyphicon-transfer:before{content:"\e178"}.glyphicon-cutlery:before{content:"\e179"}.glyphicon-header:before{content:"\e180"}.glyphicon-compressed:before{content:"\e181"}.glyphicon-earphone:before{content:"\e182"}.glyphicon-phone-alt:before{content:"\e183"}.glyphicon-tower:before{content:"\e184"}.glyphicon-stats:before{content:"\e185"}.glyphicon-sd-video:before{content:"\e186"}.glyphicon-hd-video:before{content:"\e187"}.glyphicon-subtitles:before{content:"\e188"}.glyphicon-sound-stereo:before{content:"\e189"}.glyphicon-sound-dolby:before{content:"\e190"}.glyphicon-sound-5-1:before{content:"\e191"}.glyphicon-sound-6-1:before{content:"\e192"}.glyphicon-sound-7-1:before{content:"\e193"}.glyphicon-copyright-mark:before{content:"\e194"}.glyphicon-registration-mark:before{content:"\e195"}.glyphicon-cloud-download:before{content:"\e197"}.glyphicon-cloud-upload:before{content:"\e198"}.glyphicon-tree-conifer:before{content:"\e199"}.glyphicon-tree-deciduous:before{content:"\e200"}.glyphicon-cd:before{content:"\e201"}.glyphicon-save-file:before{content:"\e202"}.glyphicon-open-file:before{content:"\e203"}.glyphicon-level-up:before{content:"\e204"}.glyphicon-copy:before{content:"\e205"}.glyphicon-paste:before{content:"\e206"}.glyphicon-alert:before{content:"\e209"}.glyphicon-equalizer:before{content:"\e210"}.glyphicon-king:before{content:"\e211"}.glyphicon-queen:before{content:"\e212"}.glyphicon-pawn:before{content:"\e213"}.glyphicon-bishop:before{content:"\e214"}.glyphicon-knight:before{content:"\e215"}.glyphicon-baby-formula:before{content:"\e216"}.glyphicon-tent:before{content:"\26fa"}.glyphicon-blackboard:before{content:"\e218"}.glyphicon-bed:before{content:"\e219"}.glyphicon-apple:before{content:"\f8ff"}.glyphicon-erase:before{content:"\e221"}.glyphicon-hourglass:before{content:"\231b"}.glyphicon-lamp:before{content:"\e223"}.glyphicon-duplicate:before{content:"\e224"}.glyphicon-piggy-bank:before{content:"\e225"}.glyphicon-scissors:before{content:"\e226"}.glyphicon-bitcoin:before{content:"\e227"}.glyphicon-btc:before{content:"\e227"}.glyphicon-xbt:before{content:"\e227"}.glyphicon-yen:before{content:"\00a5"}.glyphicon-jpy:before{content:"\00a5"}.glyphicon-ruble:before{content:"\20bd"}.glyphicon-rub:before{content:"\20bd"}.glyphicon-scale:before{content:"\e230"}.glyphicon-ice-lolly:before{content:"\e231"}.glyphicon-ice-lolly-tasted:before{content:"\e232"}.glyphicon-education:before{content:"\e233"}.glyphicon-option-horizontal:before{content:"\e234"}.glyphicon-option-vertical:before{content:"\e235"}.glyphicon-menu-hamburger:before{content:"\e236"}.glyphicon-modal-window:before{content:"\e237"}.glyphicon-oil:before{content:"\e238"}.glyphicon-grain:before{content:"\e239"}.glyphicon-sunglasses:before{content:"\e240"}.glyphicon-text-size:before{content:"\e241"}.glyphicon-text-color:before{content:"\e242"}.glyphicon-text-background:before{content:"\e243"}.glyphicon-object-align-top:before{content:"\e244"}.glyphicon-object-align-bottom:before{content:"\e245"}.glyphicon-object-align-horizontal:before{content:"\e246"}.glyphicon-object-align-left:before{content:"\e247"}.glyphicon-object-align-vertical:before{content:"\e248"}.glyphicon-object-align-right:before{content:"\e249"}.glyphicon-triangle-right:before{content:"\e250"}.glyphicon-triangle-left:before{content:"\e251"}.glyphicon-triangle-bottom:before{content:"\e252"}.glyphicon-triangle-top:before{content:"\e253"}.glyphicon-console:before{content:"\e254"}.glyphicon-superscript:before{content:"\e255"}.glyphicon-subscript:before{content:"\e256"}.glyphicon-menu-left:before{content:"\e257"}.glyphicon-menu-right:before{content:"\e258"}.glyphicon-menu-down:before{content:"\e259"}.glyphicon-menu-up:before{content:"\e260"}*{-webkit-box-sizing:border-box;-moz-box-sizing:border-box;box-sizing:border-box}:after,:before{-webkit-box-sizing:border-box;-moz-box-sizing:border-box;box-sizing:border-box}html{font-size:10px;-webkit-tap-highlight-color:rgba(0,0,0,0)}body{font-family:"Helvetica Neue",Helvetica,Arial,sans-serif;font-size:14px;line-height:1.42857143;color:#333;background-color:#fff}button,input,select,textarea{font-family:inherit;font-size:inherit;line-height:inherit}a{color:#337ab7;text-decoration:none}a:focus,a:hover{color:#23527c;text-decoration:underline}a:focus{outline:thin dotted;outline:5px auto -webkit-focus-ring-color;outline-offset:-2px}figure{margin:0}img{vertical-align:middle}.carousel-inner>.item>a>img,.carousel-inner>.item>img,.img-responsive,.thumbnail a>img,.thumbnail>img{display:block;max-width:100%;height:auto}.img-rounded{border-radius:6px}.img-thumbnail{display:inline-block;max-width:100%;height:auto;padding:4px;line-height:1.42857143;background-color:#fff;border:1px solid #ddd;border-radius:4px;-webkit-transition:all .2s ease-in-out;-o-transition:all .2s ease-in-out;transition:all .2s ease-in-out}.img-circle{border-radius:50%}hr{margin-top:20px;margin-bottom:20px;border:0;border-top:1px solid #eee}.sr-only{position:absolute;width:1px;height:1px;padding:0;margin:-1px;overflow:hidden;clip:rect(0,0,0,0);border:0}.sr-only-focusable:active,.sr-only-focusable:focus{position:static;width:auto;height:auto;margin:0;overflow:visible;clip:auto}[role=button]{cursor:pointer}.h1,.h2,.h3,.h4,.h5,.h6,h1,h2,h3,h4,h5,h6{font-family:inherit;font-weight:500;line-height:1.1;color:inherit}.h1 .small,.h1 small,.h2 .small,.h2 small,.h3 .small,.h3 small,.h4 .small,.h4 small,.h5 .small,.h5 small,.h6 .small,.h6 small,h1 .small,h1 small,h2 .small,h2 small,h3 .small,h3 small,h4 .small,h4 small,h5 .small,h5 small,h6 .small,h6 small{font-weight:400;line-height:1;color:#777}.h1,.h2,.h3,h1,h2,h3{margin-top:20px;margin-bottom:10px}.h1 .small,.h1 small,.h2 .small,.h2 small,.h3 .small,.h3 small,h1 .small,h1 small,h2 .small,h2 small,h3 .small,h3 small{font-size:65%}.h4,.h5,.h6,h4,h5,h6{margin-top:10px;margin-bottom:10px}.h4 .small,.h4 small,.h5 .small,.h5 small,.h6 .small,.h6 small,h4 .small,h4 small,h5 .small,h5 small,h6 .small,h6 small{font-size:75%}.h1,h1{font-size:36px}.h2,h2{font-size:30px}.h3,h3{font-size:24px}.h4,h4{font-size:18px}.h5,h5{font-size:14px}.h6,h6{font-size:12px}p{margin:0 0 10px}.lead{margin-bottom:20px;font-size:16px;font-weight:300;line-height:1.4}@media (min-width:768px){.lead{font-size:21px}}.small,small{font-size:85%}.mark,mark{padding:.2em;background-color:#fcf8e3}.text-left{text-align:left}.text-right{text-align:right}.text-center{text-align:center}.text-justify{text-align:justify}.text-nowrap{white-space:nowrap}.text-lowercase{text-transform:lowercase}.text-uppercase{text-transform:uppercase}.text-capitalize{text-transform:capitalize}.text-muted{color:#777}.text-primary{color:#337ab7}a.text-primary:focus,a.text-primary:hover{color:#286090}.text-success{color:#3c763d}a.text-success:focus,a.text-success:hover{color:#2b542c}.text-info{color:#31708f}a.text-info:focus,a.text-info:hover{color:#245269}.text-warning{color:#8a6d3b}a.text-warning:focus,a.text-warning:hover{color:#66512c}.text-danger{color:#a94442}a.text-danger:focus,a.text-danger:hover{color:#843534}.bg-primary{color:#fff;background-color:#337ab7}a.bg-primary:focus,a.bg-primary:hover{background-color:#286090}.bg-success{background-color:#dff0d8}a.bg-success:focus,a.bg-success:hover{background-color:#c1e2b3}.bg-info{background-color:#d9edf7}a.bg-info:focus,a.bg-info:hover{background-color:#afd9ee}.bg-warning{background-color:#fcf8e3}a.bg-warning:focus,a.bg-warning:hover{background-color:#f7ecb5}.bg-danger{background-color:#f2dede}a.bg-danger:focus,a.bg-danger:hover{background-color:#e4b9b9}.page-header{padding-bottom:9px;margin:40px 0 20px;border-bottom:1px solid #eee}ol,ul{margin-top:0;margin-bottom:10px}ol ol,ol ul,ul ol,ul ul{margin-bottom:0}.list-unstyled{padding-left:0;list-style:none}.list-inline{padding-left:0;margin-left:-5px;list-style:none}.list-inline>li{display:inline-block;padding-right:5px;padding-left:5px}dl{margin-top:0;margin-bottom:20px}dd,dt{line-height:1.42857143}dt{font-weight:700}dd{margin-left:0}@media (min-width:768px){.dl-horizontal dt{float:left;width:160px;overflow:hidden;clear:left;text-align:right;text-overflow:ellipsis;white-space:nowrap}.dl-horizontal dd{margin-left:180px}}abbr[data-original-title],abbr[title]{cursor:help;border-bottom:1px dotted #777}.initialism{font-size:90%;text-transform:uppercase}blockquote{padding:10px 20px;margin:0 0 20px;font-size:17.5px;border-left:5px solid #eee}blockquote ol:last-child,blockquote p:last-child,blockquote ul:last-child{margin-bottom:0}blockquote .small,blockquote footer,blockquote small{display:block;font-size:80%;line-height:1.42857143;color:#777}blockquote .small:before,blockquote footer:before,blockquote small:before{content:'\2014 \00A0'}.blockquote-reverse,blockquote.pull-right{padding-right:15px;padding-left:0;text-align:right;border-right:5px solid #eee;border-left:0}.blockquote-reverse .small:before,.blockquote-reverse footer:before,.blockquote-reverse small:before,blockquote.pull-right .small:before,blockquote.pull-right footer:before,blockquote.pull-right small:before{content:''}.blockquote-reverse .small:after,.blockquote-reverse footer:after,.blockquote-reverse small:after,blockquote.pull-right .small:after,blockquote.pull-right footer:after,blockquote.pull-right small:after{content:'\00A0 \2014'}address{margin-bottom:20px;font-style:normal;line-height:1.42857143}code,kbd,pre,samp{font-family:Menlo,Monaco,Consolas,"Courier New",monospace}code{padding:2px 4px;font-size:90%;color:#c7254e;background-color:#f9f2f4;border-radius:4px}kbd{padding:2px 4px;font-size:90%;color:#fff;background-color:#333;border-radius:3px;-webkit-box-shadow:inset 0 -1px 0 rgba(0,0,0,.25);box-shadow:inset 0 -1px 0 rgba(0,0,0,.25)}kbd kbd{padding:0;font-size:100%;font-weight:700;-webkit-box-shadow:none;box-shadow:none}pre{display:block;padding:9.5px;margin:0 0 10px;font-size:13px;line-height:1.42857143;color:#333;word-break:break-all;word-wrap:break-word;background-color:#f5f5f5;border:1px solid #ccc;border-radius:4px}pre code{padding:0;font-size:inherit;color:inherit;white-space:pre-wrap;background-color:transparent;border-radius:0}.pre-scrollable{max-height:340px;overflow-y:scroll}.container{padding-right:15px;padding-left:15px;margin-right:auto;margin-left:auto}@media (min-width:768px){.container{width:750px}}@media (min-width:992px){.container{width:970px}}@media (min-width:1200px){.container{width:1170px}}.container-fluid{padding-right:15px;padding-left:15px;margin-right:auto;margin-left:auto}.row{margin-right:-15px;margin-left:-15px}.col-lg-1,.col-lg-10,.col-lg-11,.col-lg-12,.col-lg-2,.col-lg-3,.col-lg-4,.col-lg-5,.col-lg-6,.col-lg-7,.col-lg-8,.col-lg-9,.col-md-1,.col-md-10,.col-md-11,.col-md-12,.col-md-2,.col-md-3,.col-md-4,.col-md-5,.col-md-6,.col-md-7,.col-md-8,.col-md-9,.col-sm-1,.col-sm-10,.col-sm-11,.col-sm-12,.col-sm-2,.col-sm-3,.col-sm-4,.col-sm-5,.col-sm-6,.col-sm-7,.col-sm-8,.col-sm-9,.col-xs-1,.col-xs-10,.col-xs-11,.col-xs-12,.col-xs-2,.col-xs-3,.col-xs-4,.col-xs-5,.col-xs-6,.col-xs-7,.col-xs-8,.col-xs-9{position:relative;min-height:1px;padding-right:15px;padding-left:15px}.col-xs-1,.col-xs-10,.col-xs-11,.col-xs-12,.col-xs-2,.col-xs-3,.col-xs-4,.col-xs-5,.col-xs-6,.col-xs-7,.col-xs-8,.col-xs-9{float:left}.col-xs-12{width:100%}.col-xs-11{width:91.66666667%}.col-xs-10{width:83.33333333%}.col-xs-9{width:75%}.col-xs-8{width:66.66666667%}.col-xs-7{width:58.33333333%}.col-xs-6{width:50%}.col-xs-5{width:41.66666667%}.col-xs-4{width:33.33333333%}.col-xs-3{width:25%}.col-xs-2{width:16.66666667%}.col-xs-1{width:8.33333333%}.col-xs-pull-12{right:100%}.col-xs-pull-11{right:91.66666667%}.col-xs-pull-10{right:83.33333333%}.col-xs-pull-9{right:75%}.col-xs-pull-8{right:66.66666667%}.col-xs-pull-7{right:58.33333333%}.col-xs-pull-6{right:50%}.col-xs-pull-5{right:41.66666667%}.col-xs-pull-4{right:33.33333333%}.col-xs-pull-3{right:25%}.col-xs-pull-2{right:16.66666667%}.col-xs-pull-1{right:8.33333333%}.col-xs-pull-0{right:auto}.col-xs-push-12{left:100%}.col-xs-push-11{left:91.66666667%}.col-xs-push-10{left:83.33333333%}.col-xs-push-9{left:75%}.col-xs-push-8{left:66.66666667%}.col-xs-push-7{left:58.33333333%}.col-xs-push-6{left:50%}.col-xs-push-5{left:41.66666667%}.col-xs-push-4{left:33.33333333%}.col-xs-push-3{left:25%}.col-xs-push-2{left:16.66666667%}.col-xs-push-1{left:8.33333333%}.col-xs-push-0{left:auto}.col-xs-offset-12{margin-left:100%}.col-xs-offset-11{margin-left:91.66666667%}.col-xs-offset-10{margin-left:83.33333333%}.col-xs-offset-9{margin-left:75%}.col-xs-offset-8{margin-left:66.66666667%}.col-xs-offset-7{margin-left:58.33333333%}.col-xs-offset-6{margin-left:50%}.col-xs-offset-5{margin-left:41.66666667%}.col-xs-offset-4{margin-left:33.33333333%}.col-xs-offset-3{margin-left:25%}.col-xs-offset-2{margin-left:16.66666667%}.col-xs-offset-1{margin-left:8.33333333%}.col-xs-offset-0{margin-left:0}@media (min-width:768px){.col-sm-1,.col-sm-10,.col-sm-11,.col-sm-12,.col-sm-2,.col-sm-3,.col-sm-4,.col-sm-5,.col-sm-6,.col-sm-7,.col-sm-8,.col-sm-9{float:left}.col-sm-12{width:100%}.col-sm-11{width:91.66666667%}.col-sm-10{width:83.33333333%}.col-sm-9{width:75%}.col-sm-8{width:66.66666667%}.col-sm-7{width:58.33333333%}.col-sm-6{width:50%}.col-sm-5{width:41.66666667%}.col-sm-4{width:33.33333333%}.col-sm-3{width:25%}.col-sm-2{width:16.66666667%}.col-sm-1{width:8.33333333%}.col-sm-pull-12{right:100%}.col-sm-pull-11{right:91.66666667%}.col-sm-pull-10{right:83.33333333%}.col-sm-pull-9{right:75%}.col-sm-pull-8{right:66.66666667%}.col-sm-pull-7{right:58.33333333%}.col-sm-pull-6{right:50%}.col-sm-pull-5{right:41.66666667%}.col-sm-pull-4{right:33.33333333%}.col-sm-pull-3{right:25%}.col-sm-pull-2{right:16.66666667%}.col-sm-pull-1{right:8.33333333%}.col-sm-pull-0{right:auto}.col-sm-push-12{left:100%}.col-sm-push-11{left:91.66666667%}.col-sm-push-10{left:83.33333333%}.col-sm-push-9{left:75%}.col-sm-push-8{left:66.66666667%}.col-sm-push-7{left:58.33333333%}.col-sm-push-6{left:50%}.col-sm-push-5{left:41.66666667%}.col-sm-push-4{left:33.33333333%}.col-sm-push-3{left:25%}.col-sm-push-2{left:16.66666667%}.col-sm-push-1{left:8.33333333%}.col-sm-push-0{left:auto}.col-sm-offset-12{margin-left:100%}.col-sm-offset-11{margin-left:91.66666667%}.col-sm-offset-10{margin-left:83.33333333%}.col-sm-offset-9{margin-left:75%}.col-sm-offset-8{margin-left:66.66666667%}.col-sm-offset-7{margin-left:58.33333333%}.col-sm-offset-6{margin-left:50%}.col-sm-offset-5{margin-left:41.66666667%}.col-sm-offset-4{margin-left:33.33333333%}.col-sm-offset-3{margin-left:25%}.col-sm-offset-2{margin-left:16.66666667%}.col-sm-offset-1{margin-left:8.33333333%}.col-sm-offset-0{margin-left:0}}@media (min-width:992px){.col-md-1,.col-md-10,.col-md-11,.col-md-12,.col-md-2,.col-md-3,.col-md-4,.col-md-5,.col-md-6,.col-md-7,.col-md-8,.col-md-9{float:left}.col-md-12{width:100%}.col-md-11{width:91.66666667%}.col-md-10{width:83.33333333%}.col-md-9{width:75%}.col-md-8{width:66.66666667%}.col-md-7{width:58.33333333%}.col-md-6{width:50%}.col-md-5{width:41.66666667%}.col-md-4{width:33.33333333%}.col-md-3{width:25%}.col-md-2{width:16.66666667%}.col-md-1{width:8.33333333%}.col-md-pull-12{right:100%}.col-md-pull-11{right:91.66666667%}.col-md-pull-10{right:83.33333333%}.col-md-pull-9{right:75%}.col-md-pull-8{right:66.66666667%}.col-md-pull-7{right:58.33333333%}.col-md-pull-6{right:50%}.col-md-pull-5{right:41.66666667%}.col-md-pull-4{right:33.33333333%}.col-md-pull-3{right:25%}.col-md-pull-2{right:16.66666667%}.col-md-pull-1{right:8.33333333%}.col-md-pull-0{right:auto}.col-md-push-12{left:100%}.col-md-push-11{left:91.66666667%}.col-md-push-10{left:83.33333333%}.col-md-push-9{left:75%}.col-md-push-8{left:66.66666667%}.col-md-push-7{left:58.33333333%}.col-md-push-6{left:50%}.col-md-push-5{left:41.66666667%}.col-md-push-4{left:33.33333333%}.col-md-push-3{left:25%}.col-md-push-2{left:16.66666667%}.col-md-push-1{left:8.33333333%}.col-md-push-0{left:auto}.col-md-offset-12{margin-left:100%}.col-md-offset-11{margin-left:91.66666667%}.col-md-offset-10{margin-left:83.33333333%}.col-md-offset-9{margin-left:75%}.col-md-offset-8{margin-left:66.66666667%}.col-md-offset-7{margin-left:58.33333333%}.col-md-offset-6{margin-left:50%}.col-md-offset-5{margin-left:41.66666667%}.col-md-offset-4{margin-left:33.33333333%}.col-md-offset-3{margin-left:25%}.col-md-offset-2{margin-left:16.66666667%}.col-md-offset-1{margin-left:8.33333333%}.col-md-offset-0{margin-left:0}}@media (min-width:1200px){.col-lg-1,.col-lg-10,.col-lg-11,.col-lg-12,.col-lg-2,.col-lg-3,.col-lg-4,.col-lg-5,.col-lg-6,.col-lg-7,.col-lg-8,.col-lg-9{float:left}.col-lg-12{width:100%}.col-lg-11{width:91.66666667%}.col-lg-10{width:83.33333333%}.col-lg-9{width:75%}.col-lg-8{width:66.66666667%}.col-lg-7{width:58.33333333%}.col-lg-6{width:50%}.col-lg-5{width:41.66666667%}.col-lg-4{width:33.33333333%}.col-lg-3{width:25%}.col-lg-2{width:16.66666667%}.col-lg-1{width:8.33333333%}.col-lg-pull-12{right:100%}.col-lg-pull-11{right:91.66666667%}.col-lg-pull-10{right:83.33333333%}.col-lg-pull-9{right:75%}.col-lg-pull-8{right:66.66666667%}.col-lg-pull-7{right:58.33333333%}.col-lg-pull-6{right:50%}.col-lg-pull-5{right:41.66666667%}.col-lg-pull-4{right:33.33333333%}.col-lg-pull-3{right:25%}.col-lg-pull-2{right:16.66666667%}.col-lg-pull-1{right:8.33333333%}.col-lg-pull-0{right:auto}.col-lg-push-12{left:100%}.col-lg-push-11{left:91.66666667%}.col-lg-push-10{left:83.33333333%}.col-lg-push-9{left:75%}.col-lg-push-8{left:66.66666667%}.col-lg-push-7{left:58.33333333%}.col-lg-push-6{left:50%}.col-lg-push-5{left:41.66666667%}.col-lg-push-4{left:33.33333333%}.col-lg-push-3{left:25%}.col-lg-push-2{left:16.66666667%}.col-lg-push-1{left:8.33333333%}.col-lg-push-0{left:auto}.col-lg-offset-12{margin-left:100%}.col-lg-offset-11{margin-left:91.66666667%}.col-lg-offset-10{margin-left:83.33333333%}.col-lg-offset-9{margin-left:75%}.col-lg-offset-8{margin-left:66.66666667%}.col-lg-offset-7{margin-left:58.33333333%}.col-lg-offset-6{margin-left:50%}.col-lg-offset-5{margin-left:41.66666667%}.col-lg-offset-4{margin-left:33.33333333%}.col-lg-offset-3{margin-left:25%}.col-lg-offset-2{margin-left:16.66666667%}.col-lg-offset-1{margin-left:8.33333333%}.col-lg-offset-0{margin-left:0}}table{background-color:transparent}caption{padding-top:8px;padding-bottom:8px;color:#777;text-align:left}th{text-align:left}.table{width:100%;max-width:100%;margin-bottom:20px}.table>tbody>tr>td,.table>tbody>tr>th,.table>tfoot>tr>td,.table>tfoot>tr>th,.table>thead>tr>td,.table>thead>tr>th{padding:8px;line-height:1.42857143;vertical-align:top;border-top:1px solid #ddd}.table>thead>tr>th{vertical-align:bottom;border-bottom:2px solid #ddd}.table>caption+thead>tr:first-child>td,.table>caption+thead>tr:first-child>th,.table>colgroup+thead>tr:first-child>td,.table>colgroup+thead>tr:first-child>th,.table>thead:first-child>tr:first-child>td,.table>thead:first-child>tr:first-child>th{border-top:0}.table>tbody+tbody{border-top:2px solid #ddd}.table .table{background-color:#fff}.table-condensed>tbody>tr>td,.table-condensed>tbody>tr>th,.table-condensed>tfoot>tr>td,.table-condensed>tfoot>tr>th,.table-condensed>thead>tr>td,.table-condensed>thead>tr>th{padding:5px}.table-bordered{border:1px solid #ddd}.table-bordered>tbody>tr>td,.table-bordered>tbody>tr>th,.table-bordered>tfoot>tr>td,.table-bordered>tfoot>tr>th,.table-bordered>thead>tr>td,.table-bordered>thead>tr>th{border:1px solid #ddd}.table-bordered>thead>tr>td,.table-bordered>thead>tr>th{border-bottom-width:2px}.table-striped>tbody>tr:nth-of-type(odd){background-color:#f9f9f9}.table-hover>tbody>tr:hover{background-color:#f5f5f5}table col[class*=col-]{position:static;display:table-column;float:none}table td[class*=col-],table th[class*=col-]{position:static;display:table-cell;float:none}.table>tbody>tr.active>td,.table>tbody>tr.active>th,.table>tbody>tr>td.active,.table>tbody>tr>th.active,.table>tfoot>tr.active>td,.table>tfoot>tr.active>th,.table>tfoot>tr>td.active,.table>tfoot>tr>th.active,.table>thead>tr.active>td,.table>thead>tr.active>th,.table>thead>tr>td.active,.table>thead>tr>th.active{background-color:#f5f5f5}.table-hover>tbody>tr.active:hover>td,.table-hover>tbody>tr.active:hover>th,.table-hover>tbody>tr:hover>.active,.table-hover>tbody>tr>td.active:hover,.table-hover>tbody>tr>th.active:hover{background-color:#e8e8e8}.table>tbody>tr.success>td,.table>tbody>tr.success>th,.table>tbody>tr>td.success,.table>tbody>tr>th.success,.table>tfoot>tr.success>td,.table>tfoot>tr.success>th,.table>tfoot>tr>td.success,.table>tfoot>tr>th.success,.table>thead>tr.success>td,.table>thead>tr.success>th,.table>thead>tr>td.success,.table>thead>tr>th.success{background-color:#dff0d8}.table-hover>tbody>tr.success:hover>td,.table-hover>tbody>tr.success:hover>th,.table-hover>tbody>tr:hover>.success,.table-hover>tbody>tr>td.success:hover,.table-hover>tbody>tr>th.success:hover{background-color:#d0e9c6}.table>tbody>tr.info>td,.table>tbody>tr.info>th,.table>tbody>tr>td.info,.table>tbody>tr>th.info,.table>tfoot>tr.info>td,.table>tfoot>tr.info>th,.table>tfoot>tr>td.info,.table>tfoot>tr>th.info,.table>thead>tr.info>td,.table>thead>tr.info>th,.table>thead>tr>td.info,.table>thead>tr>th.info{background-color:#d9edf7}.table-hover>tbody>tr.info:hover>td,.table-hover>tbody>tr.info:hover>th,.table-hover>tbody>tr:hover>.info,.table-hover>tbody>tr>td.info:hover,.table-hover>tbody>tr>th.info:hover{background-color:#c4e3f3}.table>tbody>tr.warning>td,.table>tbody>tr.warning>th,.table>tbody>tr>td.warning,.table>tbody>tr>th.warning,.table>tfoot>tr.warning>td,.table>tfoot>tr.warning>th,.table>tfoot>tr>td.warning,.table>tfoot>tr>th.warning,.table>thead>tr.warning>td,.table>thead>tr.warning>th,.table>thead>tr>td.warning,.table>thead>tr>th.warning{background-color:#fcf8e3}.table-hover>tbody>tr.warning:hover>td,.table-hover>tbody>tr.warning:hover>th,.table-hover>tbody>tr:hover>.warning,.table-hover>tbody>tr>td.warning:hover,.table-hover>tbody>tr>th.warning:hover{background-color:#faf2cc}.table>tbody>tr.danger>td,.table>tbody>tr.danger>th,.table>tbody>tr>td.danger,.table>tbody>tr>th.danger,.table>tfoot>tr.danger>td,.table>tfoot>tr.danger>th,.table>tfoot>tr>td.danger,.table>tfoot>tr>th.danger,.table>thead>tr.danger>td,.table>thead>tr.danger>th,.table>thead>tr>td.danger,.table>thead>tr>th.danger{background-color:#f2dede}.table-hover>tbody>tr.danger:hover>td,.table-hover>tbody>tr.danger:hover>th,.table-hover>tbody>tr:hover>.danger,.table-hover>tbody>tr>td.danger:hover,.table-hover>tbody>tr>th.danger:hover{background-color:#ebcccc}.table-responsive{min-height:.01%;overflow-x:auto}@media screen and (max-width:767px){.table-responsive{width:100%;margin-bottom:15px;overflow-y:hidden;-ms-overflow-style:-ms-autohiding-scrollbar;border:1px solid #ddd}.table-responsive>.table{margin-bottom:0}.table-responsive>.table>tbody>tr>td,.table-responsive>.table>tbody>tr>th,.table-responsive>.table>tfoot>tr>td,.table-responsive>.table>tfoot>tr>th,.table-responsive>.table>thead>tr>td,.table-responsive>.table>thead>tr>th{white-space:nowrap}.table-responsive>.table-bordered{border:0}.table-responsive>.table-bordered>tbody>tr>td:first-child,.table-responsive>.table-bordered>tbody>tr>th:first-child,.table-responsive>.table-bordered>tfoot>tr>td:first-child,.table-responsive>.table-bordered>tfoot>tr>th:first-child,.table-responsive>.table-bordered>thead>tr>td:first-child,.table-responsive>.table-bordered>thead>tr>th:first-child{border-left:0}.table-responsive>.table-bordered>tbody>tr>td:last-child,.table-responsive>.table-bordered>tbody>tr>th:last-child,.table-responsive>.table-bordered>tfoot>tr>td:last-child,.table-responsive>.table-bordered>tfoot>tr>th:last-child,.table-responsive>.table-bordered>thead>tr>td:last-child,.table-responsive>.table-bordered>thead>tr>th:last-child{border-right:0}.table-responsive>.table-bordered>tbody>tr:last-child>td,.table-responsive>.table-bordered>tbody>tr:last-child>th,.table-responsive>.table-bordered>tfoot>tr:last-child>td,.table-responsive>.table-bordered>tfoot>tr:last-child>th{border-bottom:0}}fieldset{min-width:0;padding:0;margin:0;border:0}legend{display:block;width:100%;padding:0;margin-bottom:20px;font-size:21px;line-height:inherit;color:#333;border:0;border-bottom:1px solid #e5e5e5}label{display:inline-block;max-width:100%;margin-bottom:5px;font-weight:700}input[type=search]{-webkit-box-sizing:border-box;-moz-box-sizing:border-box;box-sizing:border-box}input[type=checkbox],input[type=radio]{margin:4px 0 0;margin-top:1px\9;line-height:normal}input[type=file]{display:block}input[type=range]{display:block;width:100%}select[multiple],select[size]{height:auto}input[type=file]:focus,input[type=checkbox]:focus,input[type=radio]:focus{outline:thin dotted;outline:5px auto -webkit-focus-ring-color;outline-offset:-2px}output{display:block;padding-top:7px;font-size:14px;line-height:1.42857143;color:#555}.form-control{display:block;width:100%;height:34px;padding:6px 12px;font-size:14px;line-height:1.42857143;color:#555;background-color:#fff;background-image:none;border:1px solid #ccc;border-radius:4px;-webkit-box-shadow:inset 0 1px 1px rgba(0,0,0,.075);box-shadow:inset 0 1px 1px rgba(0,0,0,.075);-webkit-transition:border-color ease-in-out .15s,-webkit-box-shadow ease-in-out .15s;-o-transition:border-color ease-in-out .15s,box-shadow ease-in-out .15s;transition:border-color ease-in-out .15s,box-shadow ease-in-out .15s}.form-control:focus{border-color:#66afe9;outline:0;-webkit-box-shadow:inset 0 1px 1px rgba(0,0,0,.075),0 0 8px rgba(102,175,233,.6);box-shadow:inset 0 1px 1px rgba(0,0,0,.075),0 0 8px rgba(102,175,233,.6)}.form-control::-moz-placeholder{color:#999;opacity:1}.form-control:-ms-input-placeholder{color:#999}.form-control::-webkit-input-placeholder{color:#999}.form-control::-ms-expand{background-color:transparent;border:0}.form-control[disabled],.form-control[readonly],fieldset[disabled] .form-control{background-color:#eee;opacity:1}.form-control[disabled],fieldset[disabled] .form-control{cursor:not-allowed}textarea.form-control{height:auto}input[type=search]{-webkit-appearance:none}@media screen and (-webkit-min-device-pixel-ratio:0){input[type=date].form-control,input[type=time].form-control,input[type=datetime-local].form-control,input[type=month].form-control{line-height:34px}.input-group-sm input[type=date],.input-group-sm input[type=time],.input-group-sm input[type=datetime-local],.input-group-sm input[type=month],input[type=date].input-sm,input[type=time].input-sm,input[type=datetime-local].input-sm,input[type=month].input-sm{line-height:30px}.input-group-lg input[type=date],.input-group-lg input[type=time],.input-group-lg input[type=datetime-local],.input-group-lg input[type=month],input[type=date].input-lg,input[type=time].input-lg,input[type=datetime-local].input-lg,input[type=month].input-lg{line-height:46px}}.form-group{margin-bottom:15px}.checkbox,.radio{position:relative;display:block;margin-top:10px;margin-bottom:10px}.checkbox label,.radio label{min-height:20px;padding-left:20px;margin-bottom:0;font-weight:400;cursor:pointer}.checkbox input[type=checkbox],.checkbox-inline input[type=checkbox],.radio input[type=radio],.radio-inline input[type=radio]{position:absolute;margin-top:4px\9;margin-left:-20px}.checkbox+.checkbox,.radio+.radio{margin-top:-5px}.checkbox-inline,.radio-inline{position:relative;display:inline-block;padding-left:20px;margin-bottom:0;font-weight:400;vertical-align:middle;cursor:pointer}.checkbox-inline+.checkbox-inline,.radio-inline+.radio-inline{margin-top:0;margin-left:10px}fieldset[disabled] input[type=checkbox],fieldset[disabled] input[type=radio],input[type=checkbox].disabled,input[type=checkbox][disabled],input[type=radio].disabled,input[type=radio][disabled]{cursor:not-allowed}.checkbox-inline.disabled,.radio-inline.disabled,fieldset[disabled] .checkbox-inline,fieldset[disabled] .radio-inline{cursor:not-allowed}.checkbox.disabled label,.radio.disabled label,fieldset[disabled] .checkbox label,fieldset[disabled] .radio label{cursor:not-allowed}.form-control-static{min-height:34px;padding-top:7px;padding-bottom:7px;margin-bottom:0}.form-control-static.input-lg,.form-control-static.input-sm{padding-right:0;padding-left:0}.input-sm{height:30px;padding:5px 10px;font-size:12px;line-height:1.5;border-radius:3px}select.input-sm{height:30px;line-height:30px}select[multiple].input-sm,textarea.input-sm{height:auto}.form-group-sm .form-control{height:30px;padding:5px 10px;font-size:12px;line-height:1.5;border-radius:3px}.form-group-sm select.form-control{height:30px;line-height:30px}.form-group-sm select[multiple].form-control,.form-group-sm textarea.form-control{height:auto}.form-group-sm .form-control-static{height:30px;min-height:32px;padding:6px 10px;font-size:12px;line-height:1.5}.input-lg{height:46px;padding:10px 16px;font-size:18px;line-height:1.3333333;border-radius:6px}select.input-lg{height:46px;line-height:46px}select[multiple].input-lg,textarea.input-lg{height:auto}.form-group-lg .form-control{height:46px;padding:10px 16px;font-size:18px;line-height:1.3333333;border-radius:6px}.form-group-lg select.form-control{height:46px;line-height:46px}.form-group-lg select[multiple].form-control,.form-group-lg textarea.form-control{height:auto}.form-group-lg .form-control-static{height:46px;min-height:38px;padding:11px 16px;font-size:18px;line-height:1.3333333}.has-feedback{position:relative}.has-feedback .form-control{padding-right:42.5px}.form-control-feedback{position:absolute;top:0;right:0;z-index:2;display:block;width:34px;height:34px;line-height:34px;text-align:center;pointer-events:none}.form-group-lg .form-control+.form-control-feedback,.input-group-lg+.form-control-feedback,.input-lg+.form-control-feedback{width:46px;height:46px;line-height:46px}.form-group-sm .form-control+.form-control-feedback,.input-group-sm+.form-control-feedback,.input-sm+.form-control-feedback{width:30px;height:30px;line-height:30px}.has-success .checkbox,.has-success .checkbox-inline,.has-success .control-label,.has-success .help-block,.has-success .radio,.has-success .radio-inline,.has-success.checkbox label,.has-success.checkbox-inline label,.has-success.radio label,.has-success.radio-inline label{color:#3c763d}.has-success .form-control{border-color:#3c763d;-webkit-box-shadow:inset 0 1px 1px rgba(0,0,0,.075);box-shadow:inset 0 1px 1px rgba(0,0,0,.075)}.has-success .form-control:focus{border-color:#2b542c;-webkit-box-shadow:inset 0 1px 1px rgba(0,0,0,.075),0 0 6px #67b168;box-shadow:inset 0 1px 1px rgba(0,0,0,.075),0 0 6px #67b168}.has-success .input-group-addon{color:#3c763d;background-color:#dff0d8;border-color:#3c763d}.has-success .form-control-feedback{color:#3c763d}.has-warning .checkbox,.has-warning .checkbox-inline,.has-warning .control-label,.has-warning .help-block,.has-warning .radio,.has-warning .radio-inline,.has-warning.checkbox label,.has-warning.checkbox-inline label,.has-warning.radio label,.has-warning.radio-inline label{color:#8a6d3b}.has-warning .form-control{border-color:#8a6d3b;-webkit-box-shadow:inset 0 1px 1px rgba(0,0,0,.075);box-shadow:inset 0 1px 1px rgba(0,0,0,.075)}.has-warning .form-control:focus{border-color:#66512c;-webkit-box-shadow:inset 0 1px 1px rgba(0,0,0,.075),0 0 6px #c0a16b;box-shadow:inset 0 1px 1px rgba(0,0,0,.075),0 0 6px #c0a16b}.has-warning .input-group-addon{color:#8a6d3b;background-color:#fcf8e3;border-color:#8a6d3b}.has-warning .form-control-feedback{color:#8a6d3b}.has-error .checkbox,.has-error .checkbox-inline,.has-error .control-label,.has-error .help-block,.has-error .radio,.has-error .radio-inline,.has-error.checkbox label,.has-error.checkbox-inline label,.has-error.radio label,.has-error.radio-inline label{color:#a94442}.has-error .form-control{border-color:#a94442;-webkit-box-shadow:inset 0 1px 1px rgba(0,0,0,.075);box-shadow:inset 0 1px 1px rgba(0,0,0,.075)}.has-error .form-control:focus{border-color:#843534;-webkit-box-shadow:inset 0 1px 1px rgba(0,0,0,.075),0 0 6px #ce8483;box-shadow:inset 0 1px 1px rgba(0,0,0,.075),0 0 6px #ce8483}.has-error .input-group-addon{color:#a94442;background-color:#f2dede;border-color:#a94442}.has-error .form-control-feedback{color:#a94442}.has-feedback label~.form-control-feedback{top:25px}.has-feedback label.sr-only~.form-control-feedback{top:0}.help-block{display:block;margin-top:5px;margin-bottom:10px;color:#737373}@media (min-width:768px){.form-inline .form-group{display:inline-block;margin-bottom:0;vertical-align:middle}.form-inline .form-control{display:inline-block;width:auto;vertical-align:middle}.form-inline .form-control-static{display:inline-block}.form-inline .input-group{display:inline-table;vertical-align:middle}.form-inline .input-group .form-control,.form-inline .input-group .input-group-addon,.form-inline .input-group .input-group-btn{width:auto}.form-inline .input-group>.form-control{width:100%}.form-inline .control-label{margin-bottom:0;vertical-align:middle}.form-inline .checkbox,.form-inline .radio{display:inline-block;margin-top:0;margin-bottom:0;vertical-align:middle}.form-inline .checkbox label,.form-inline .radio label{padding-left:0}.form-inline .checkbox input[type=checkbox],.form-inline .radio input[type=radio]{position:relative;margin-left:0}.form-inline .has-feedback .form-control-feedback{top:0}}.form-horizontal .checkbox,.form-horizontal .checkbox-inline,.form-horizontal .radio,.form-horizontal .radio-inline{padding-top:7px;margin-top:0;margin-bottom:0}.form-horizontal .checkbox,.form-horizontal .radio{min-height:27px}.form-horizontal .form-group{margin-right:-15px;margin-left:-15px}@media (min-width:768px){.form-horizontal .control-label{padding-top:7px;margin-bottom:0;text-align:right}}.form-horizontal .has-feedback .form-control-feedback{right:15px}@media (min-width:768px){.form-horizontal .form-group-lg .control-label{padding-top:11px;font-size:18px}}@media (min-width:768px){.form-horizontal .form-group-sm .control-label{padding-top:6px;font-size:12px}}.btn{display:inline-block;padding:6px 12px;margin-bottom:0;font-size:14px;font-weight:400;line-height:1.42857143;text-align:center;white-space:nowrap;vertical-align:middle;-ms-touch-action:manipulation;touch-action:manipulation;cursor:pointer;-webkit-user-select:none;-moz-user-select:none;-ms-user-select:none;user-select:none;background-image:none;border:1px solid transparent;border-radius:4px}.btn.active.focus,.btn.active:focus,.btn.focus,.btn:active.focus,.btn:active:focus,.btn:focus{outline:thin dotted;outline:5px auto -webkit-focus-ring-color;outline-offset:-2px}.btn.focus,.btn:focus,.btn:hover{color:#333;text-decoration:none}.btn.active,.btn:active{background-image:none;outline:0;-webkit-box-shadow:inset 0 3px 5px rgba(0,0,0,.125);box-shadow:inset 0 3px 5px rgba(0,0,0,.125)}.btn.disabled,.btn[disabled],fieldset[disabled] .btn{cursor:not-allowed;filter:alpha(opacity=65);-webkit-box-shadow:none;box-shadow:none;opacity:.65}a.btn.disabled,fieldset[disabled] a.btn{pointer-events:none}.btn-default{color:#333;background-color:#fff;border-color:#ccc}.btn-default.focus,.btn-default:focus{color:#333;background-color:#e6e6e6;border-color:#8c8c8c}.btn-default:hover{color:#333;background-color:#e6e6e6;border-color:#adadad}.btn-default.active,.btn-default:active,.open>.dropdown-toggle.btn-default{color:#333;background-color:#e6e6e6;border-color:#adadad}.btn-default.active.focus,.btn-default.active:focus,.btn-default.active:hover,.btn-default:active.focus,.btn-default:active:focus,.btn-default:active:hover,.open>.dropdown-toggle.btn-default.focus,.open>.dropdown-toggle.btn-default:focus,.open>.dropdown-toggle.btn-default:hover{color:#333;background-color:#d4d4d4;border-color:#8c8c8c}.btn-default.active,.btn-default:active,.open>.dropdown-toggle.btn-default{background-image:none}.btn-default.disabled.focus,.btn-default.disabled:focus,.btn-default.disabled:hover,.btn-default[disabled].focus,.btn-default[disabled]:focus,.btn-default[disabled]:hover,fieldset[disabled] .btn-default.focus,fieldset[disabled] .btn-default:focus,fieldset[disabled] .btn-default:hover{background-color:#fff;border-color:#ccc}.btn-default .badge{color:#fff;background-color:#333}.btn-primary{color:#fff;background-color:#337ab7;border-color:#2e6da4}.btn-primary.focus,.btn-primary:focus{color:#fff;background-color:#286090;border-color:#122b40}.btn-primary:hover{color:#fff;background-color:#286090;border-color:#204d74}.btn-primary.active,.btn-primary:active,.open>.dropdown-toggle.btn-primary{color:#fff;background-color:#286090;border-color:#204d74}.btn-primary.active.focus,.btn-primary.active:focus,.btn-primary.active:hover,.btn-primary:active.focus,.btn-primary:active:focus,.btn-primary:active:hover,.open>.dropdown-toggle.btn-primary.focus,.open>.dropdown-toggle.btn-primary:focus,.open>.dropdown-toggle.btn-primary:hover{color:#fff;background-color:#204d74;border-color:#122b40}.btn-primary.active,.btn-primary:active,.open>.dropdown-toggle.btn-primary{background-image:none}.btn-primary.disabled.focus,.btn-primary.disabled:focus,.btn-primary.disabled:hover,.btn-primary[disabled].focus,.btn-primary[disabled]:focus,.btn-primary[disabled]:hover,fieldset[disabled] .btn-primary.focus,fieldset[disabled] .btn-primary:focus,fieldset[disabled] .btn-primary:hover{background-color:#337ab7;border-color:#2e6da4}.btn-primary .badge{color:#337ab7;background-color:#fff}.btn-success{color:#fff;background-color:#5cb85c;border-color:#4cae4c}.btn-success.focus,.btn-success:focus{color:#fff;background-color:#449d44;border-color:#255625}.btn-success:hover{color:#fff;background-color:#449d44;border-color:#398439}.btn-success.active,.btn-success:active,.open>.dropdown-toggle.btn-success{color:#fff;background-color:#449d44;border-color:#398439}.btn-success.active.focus,.btn-success.active:focus,.btn-success.active:hover,.btn-success:active.focus,.btn-success:active:focus,.btn-success:active:hover,.open>.dropdown-toggle.btn-success.focus,.open>.dropdown-toggle.btn-success:focus,.open>.dropdown-toggle.btn-success:hover{color:#fff;background-color:#398439;border-color:#255625}.btn-success.active,.btn-success:active,.open>.dropdown-toggle.btn-success{background-image:none}.btn-success.disabled.focus,.btn-success.disabled:focus,.btn-success.disabled:hover,.btn-success[disabled].focus,.btn-success[disabled]:focus,.btn-success[disabled]:hover,fieldset[disabled] .btn-success.focus,fieldset[disabled] .btn-success:focus,fieldset[disabled] .btn-success:hover{background-color:#5cb85c;border-color:#4cae4c}.btn-success .badge{color:#5cb85c;background-color:#fff}.btn-info{color:#fff;background-color:#5bc0de;border-color:#46b8da}.btn-info.focus,.btn-info:focus{color:#fff;background-color:#31b0d5;border-color:#1b6d85}.btn-info:hover{color:#fff;background-color:#31b0d5;border-color:#269abc}.btn-info.active,.btn-info:active,.open>.dropdown-toggle.btn-info{color:#fff;background-color:#31b0d5;border-color:#269abc}.btn-info.active.focus,.btn-info.active:focus,.btn-info.active:hover,.btn-info:active.focus,.btn-info:active:focus,.btn-info:active:hover,.open>.dropdown-toggle.btn-info.focus,.open>.dropdown-toggle.btn-info:focus,.open>.dropdown-toggle.btn-info:hover{color:#fff;background-color:#269abc;border-color:#1b6d85}.btn-info.active,.btn-info:active,.open>.dropdown-toggle.btn-info{background-image:none}.btn-info.disabled.focus,.btn-info.disabled:focus,.btn-info.disabled:hover,.btn-info[disabled].focus,.btn-info[disabled]:focus,.btn-info[disabled]:hover,fieldset[disabled] .btn-info.focus,fieldset[disabled] .btn-info:focus,fieldset[disabled] .btn-info:hover{background-color:#5bc0de;border-color:#46b8da}.btn-info .badge{color:#5bc0de;background-color:#fff}.btn-warning{color:#fff;background-color:#f0ad4e;border-color:#eea236}.btn-warning.focus,.btn-warning:focus{color:#fff;background-color:#ec971f;border-color:#985f0d}.btn-warning:hover{color:#fff;background-color:#ec971f;border-color:#d58512}.btn-warning.active,.btn-warning:active,.open>.dropdown-toggle.btn-warning{color:#fff;background-color:#ec971f;border-color:#d58512}.btn-warning.active.focus,.btn-warning.active:focus,.btn-warning.active:hover,.btn-warning:active.focus,.btn-warning:active:focus,.btn-warning:active:hover,.open>.dropdown-toggle.btn-warning.focus,.open>.dropdown-toggle.btn-warning:focus,.open>.dropdown-toggle.btn-warning:hover{color:#fff;background-color:#d58512;border-color:#985f0d}.btn-warning.active,.btn-warning:active,.open>.dropdown-toggle.btn-warning{background-image:none}.btn-warning.disabled.focus,.btn-warning.disabled:focus,.btn-warning.disabled:hover,.btn-warning[disabled].focus,.btn-warning[disabled]:focus,.btn-warning[disabled]:hover,fieldset[disabled] .btn-warning.focus,fieldset[disabled] .btn-warning:focus,fieldset[disabled] .btn-warning:hover{background-color:#f0ad4e;border-color:#eea236}.btn-warning .badge{color:#f0ad4e;background-color:#fff}.btn-danger{color:#fff;background-color:#d9534f;border-color:#d43f3a}.btn-danger.focus,.btn-danger:focus{color:#fff;background-color:#c9302c;border-color:#761c19}.btn-danger:hover{color:#fff;background-color:#c9302c;border-color:#ac2925}.btn-danger.active,.btn-danger:active,.open>.dropdown-toggle.btn-danger{color:#fff;background-color:#c9302c;border-color:#ac2925}.btn-danger.active.focus,.btn-danger.active:focus,.btn-danger.active:hover,.btn-danger:active.focus,.btn-danger:active:focus,.btn-danger:active:hover,.open>.dropdown-toggle.btn-danger.focus,.open>.dropdown-toggle.btn-danger:focus,.open>.dropdown-toggle.btn-danger:hover{color:#fff;background-color:#ac2925;border-color:#761c19}.btn-danger.active,.btn-danger:active,.open>.dropdown-toggle.btn-danger{background-image:none}.btn-danger.disabled.focus,.btn-danger.disabled:focus,.btn-danger.disabled:hover,.btn-danger[disabled].focus,.btn-danger[disabled]:focus,.btn-danger[disabled]:hover,fieldset[disabled] .btn-danger.focus,fieldset[disabled] .btn-danger:focus,fieldset[disabled] .btn-danger:hover{background-color:#d9534f;border-color:#d43f3a}.btn-danger .badge{color:#d9534f;background-color:#fff}.btn-link{font-weight:400;color:#337ab7;border-radius:0}.btn-link,.btn-link.active,.btn-link:active,.btn-link[disabled],fieldset[disabled] .btn-link{background-color:transparent;-webkit-box-shadow:none;box-shadow:none}.btn-link,.btn-link:active,.btn-link:focus,.btn-link:hover{border-color:transparent}.btn-link:focus,.btn-link:hover{color:#23527c;text-decoration:underline;background-color:transparent}.btn-link[disabled]:focus,.btn-link[disabled]:hover,fieldset[disabled] .btn-link:focus,fieldset[disabled] .btn-link:hover{color:#777;text-decoration:none}.btn-group-lg>.btn,.btn-lg{padding:10px 16px;font-size:18px;line-height:1.3333333;border-radius:6px}.btn-group-sm>.btn,.btn-sm{padding:5px 10px;font-size:12px;line-height:1.5;border-radius:3px}.btn-group-xs>.btn,.btn-xs{padding:1px 5px;font-size:12px;line-height:1.5;border-radius:3px}.btn-block{display:block;width:100%}.btn-block+.btn-block{margin-top:5px}input[type=button].btn-block,input[type=reset].btn-block,input[type=submit].btn-block{width:100%}.fade{opacity:0;-webkit-transition:opacity .15s linear;-o-transition:opacity .15s linear;transition:opacity .15s linear}.fade.in{opacity:1}.collapse{display:none}.collapse.in{display:block}tr.collapse.in{display:table-row}tbody.collapse.in{display:table-row-group}.collapsing{position:relative;height:0;overflow:hidden;-webkit-transition-timing-function:ease;-o-transition-timing-function:ease;transition-timing-function:ease;-webkit-transition-duration:.35s;-o-transition-duration:.35s;transition-duration:.35s;-webkit-transition-property:height,visibility;-o-transition-property:height,visibility;transition-property:height,visibility}.caret{display:inline-block;width:0;height:0;margin-left:2px;vertical-align:middle;border-top:4px dashed;border-top:4px solid\9;border-right:4px solid transparent;border-left:4px solid transparent}.dropdown,.dropup{position:relative}.dropdown-toggle:focus{outline:0}.dropdown-menu{position:absolute;top:100%;left:0;z-index:1000;display:none;float:left;min-width:160px;padding:5px 0;margin:2px 0 0;font-size:14px;text-align:left;list-style:none;background-color:#fff;-webkit-background-clip:padding-box;background-clip:padding-box;border:1px solid #ccc;border:1px solid rgba(0,0,0,.15);border-radius:4px;-webkit-box-shadow:0 6px 12px rgba(0,0,0,.175);box-shadow:0 6px 12px rgba(0,0,0,.175)}.dropdown-menu.pull-right{right:0;left:auto}.dropdown-menu .divider{height:1px;margin:9px 0;overflow:hidden;background-color:#e5e5e5}.dropdown-menu>li>a{display:block;padding:3px 20px;clear:both;font-weight:400;line-height:1.42857143;color:#333;white-space:nowrap}.dropdown-menu>li>a:focus,.dropdown-menu>li>a:hover{color:#262626;text-decoration:none;background-color:#f5f5f5}.dropdown-menu>.active>a,.dropdown-menu>.active>a:focus,.dropdown-menu>.active>a:hover{color:#fff;text-decoration:none;background-color:#337ab7;outline:0}.dropdown-menu>.disabled>a,.dropdown-menu>.disabled>a:focus,.dropdown-menu>.disabled>a:hover{color:#777}.dropdown-menu>.disabled>a:focus,.dropdown-menu>.disabled>a:hover{text-decoration:none;cursor:not-allowed;background-color:transparent;background-image:none;filter:progid:DXImageTransform.Microsoft.gradient(enabled=false)}.open>.dropdown-menu{display:block}.open>a{outline:0}.dropdown-menu-right{right:0;left:auto}.dropdown-menu-left{right:auto;left:0}.dropdown-header{display:block;padding:3px 20px;font-size:12px;line-height:1.42857143;color:#777;white-space:nowrap}.dropdown-backdrop{position:fixed;top:0;right:0;bottom:0;left:0;z-index:990}.pull-right>.dropdown-menu{right:0;left:auto}.dropup .caret,.navbar-fixed-bottom .dropdown .caret{content:"";border-top:0;border-bottom:4px dashed;border-bottom:4px solid\9}.dropup .dropdown-menu,.navbar-fixed-bottom .dropdown .dropdown-menu{top:auto;bottom:100%;margin-bottom:2px}@media (min-width:768px){.navbar-right .dropdown-menu{right:0;left:auto}.navbar-right .dropdown-menu-left{right:auto;left:0}}.btn-group,.btn-group-vertical{position:relative;display:inline-block;vertical-align:middle}.btn-group-vertical>.btn,.btn-group>.btn{position:relative;float:left}.btn-group-vertical>.btn.active,.btn-group-vertical>.btn:active,.btn-group-vertical>.btn:focus,.btn-group-vertical>.btn:hover,.btn-group>.btn.active,.btn-group>.btn:active,.btn-group>.btn:focus,.btn-group>.btn:hover{z-index:2}.btn-group .btn+.btn,.btn-group .btn+.btn-group,.btn-group .btn-group+.btn,.btn-group .btn-group+.btn-group{margin-left:-1px}.btn-toolbar{margin-left:-5px}.btn-toolbar .btn,.btn-toolbar .btn-group,.btn-toolbar .input-group{float:left}.btn-toolbar>.btn,.btn-toolbar>.btn-group,.btn-toolbar>.input-group{margin-left:5px}.btn-group>.btn:not(:first-child):not(:last-child):not(.dropdown-toggle){border-radius:0}.btn-group>.btn:first-child{margin-left:0}.btn-group>.btn:first-child:not(:last-child):not(.dropdown-toggle){border-top-right-radius:0;border-bottom-right-radius:0}.btn-group>.btn:last-child:not(:first-child),.btn-group>.dropdown-toggle:not(:first-child){border-top-left-radius:0;border-bottom-left-radius:0}.btn-group>.btn-group{float:left}.btn-group>.btn-group:not(:first-child):not(:last-child)>.btn{border-radius:0}.btn-group>.btn-group:first-child:not(:last-child)>.btn:last-child,.btn-group>.btn-group:first-child:not(:last-child)>.dropdown-toggle{border-top-right-radius:0;border-bottom-right-radius:0}.btn-group>.btn-group:last-child:not(:first-child)>.btn:first-child{border-top-left-radius:0;border-bottom-left-radius:0}.btn-group .dropdown-toggle:active,.btn-group.open .dropdown-toggle{outline:0}.btn-group>.btn+.dropdown-toggle{padding-right:8px;padding-left:8px}.btn-group>.btn-lg+.dropdown-toggle{padding-right:12px;padding-left:12px}.btn-group.open .dropdown-toggle{-webkit-box-shadow:inset 0 3px 5px rgba(0,0,0,.125);box-shadow:inset 0 3px 5px rgba(0,0,0,.125)}.btn-group.open .dropdown-toggle.btn-link{-webkit-box-shadow:none;box-shadow:none}.btn .caret{margin-left:0}.btn-lg .caret{border-width:5px 5px 0;border-bottom-width:0}.dropup .btn-lg .caret{border-width:0 5px 5px}.btn-group-vertical>.btn,.btn-group-vertical>.btn-group,.btn-group-vertical>.btn-group>.btn{display:block;float:none;width:100%;max-width:100%}.btn-group-vertical>.btn-group>.btn{float:none}.btn-group-vertical>.btn+.btn,.btn-group-vertical>.btn+.btn-group,.btn-group-vertical>.btn-group+.btn,.btn-group-vertical>.btn-group+.btn-group{margin-top:-1px;margin-left:0}.btn-group-vertical>.btn:not(:first-child):not(:last-child){border-radius:0}.btn-group-vertical>.btn:first-child:not(:last-child){border-top-left-radius:4px;border-top-right-radius:4px;border-bottom-right-radius:0;border-bottom-left-radius:0}.btn-group-vertical>.btn:last-child:not(:first-child){border-top-left-radius:0;border-top-right-radius:0;border-bottom-right-radius:4px;border-bottom-left-radius:4px}.btn-group-vertical>.btn-group:not(:first-child):not(:last-child)>.btn{border-radius:0}.btn-group-vertical>.btn-group:first-child:not(:last-child)>.btn:last-child,.btn-group-vertical>.btn-group:first-child:not(:last-child)>.dropdown-toggle{border-bottom-right-radius:0;border-bottom-left-radius:0}.btn-group-vertical>.btn-group:last-child:not(:first-child)>.btn:first-child{border-top-left-radius:0;border-top-right-radius:0}.btn-group-justified{display:table;width:100%;table-layout:fixed;border-collapse:separate}.btn-group-justified>.btn,.btn-group-justified>.btn-group{display:table-cell;float:none;width:1%}.btn-group-justified>.btn-group .btn{width:100%}.btn-group-justified>.btn-group .dropdown-menu{left:auto}[data-toggle=buttons]>.btn input[type=checkbox],[data-toggle=buttons]>.btn input[type=radio],[data-toggle=buttons]>.btn-group>.btn input[type=checkbox],[data-toggle=buttons]>.btn-group>.btn input[type=radio]{position:absolute;clip:rect(0,0,0,0);pointer-events:none}.input-group{position:relative;display:table;border-collapse:separate}.input-group[class*=col-]{float:none;padding-right:0;padding-left:0}.input-group .form-control{position:relative;z-index:2;float:left;width:100%;margin-bottom:0}.input-group .form-control:focus{z-index:3}.input-group-lg>.form-control,.input-group-lg>.input-group-addon,.input-group-lg>.input-group-btn>.btn{height:46px;padding:10px 16px;font-size:18px;line-height:1.3333333;border-radius:6px}select.input-group-lg>.form-control,select.input-group-lg>.input-group-addon,select.input-group-lg>.input-group-btn>.btn{height:46px;line-height:46px}select[multiple].input-group-lg>.form-control,select[multiple].input-group-lg>.input-group-addon,select[multiple].input-group-lg>.input-group-btn>.btn,textarea.input-group-lg>.form-control,textarea.input-group-lg>.input-group-addon,textarea.input-group-lg>.input-group-btn>.btn{height:auto}.input-group-sm>.form-control,.input-group-sm>.input-group-addon,.input-group-sm>.input-group-btn>.btn{height:30px;padding:5px 10px;font-size:12px;line-height:1.5;border-radius:3px}select.input-group-sm>.form-control,select.input-group-sm>.input-group-addon,select.input-group-sm>.input-group-btn>.btn{height:30px;line-height:30px}select[multiple].input-group-sm>.form-control,select[multiple].input-group-sm>.input-group-addon,select[multiple].input-group-sm>.input-group-btn>.btn,textarea.input-group-sm>.form-control,textarea.input-group-sm>.input-group-addon,textarea.input-group-sm>.input-group-btn>.btn{height:auto}.input-group .form-control,.input-group-addon,.input-group-btn{display:table-cell}.input-group .form-control:not(:first-child):not(:last-child),.input-group-addon:not(:first-child):not(:last-child),.input-group-btn:not(:first-child):not(:last-child){border-radius:0}.input-group-addon,.input-group-btn{width:1%;white-space:nowrap;vertical-align:middle}.input-group-addon{padding:6px 12px;font-size:14px;font-weight:400;line-height:1;color:#555;text-align:center;background-color:#eee;border:1px solid #ccc;border-radius:4px}.input-group-addon.input-sm{padding:5px 10px;font-size:12px;border-radius:3px}.input-group-addon.input-lg{padding:10px 16px;font-size:18px;border-radius:6px}.input-group-addon input[type=checkbox],.input-group-addon input[type=radio]{margin-top:0}.input-group .form-control:first-child,.input-group-addon:first-child,.input-group-btn:first-child>.btn,.input-group-btn:first-child>.btn-group>.btn,.input-group-btn:first-child>.dropdown-toggle,.input-group-btn:last-child>.btn-group:not(:last-child)>.btn,.input-group-btn:last-child>.btn:not(:last-child):not(.dropdown-toggle){border-top-right-radius:0;border-bottom-right-radius:0}.input-group-addon:first-child{border-right:0}.input-group .form-control:last-child,.input-group-addon:last-child,.input-group-btn:first-child>.btn-group:not(:first-child)>.btn,.input-group-btn:first-child>.btn:not(:first-child),.input-group-btn:last-child>.btn,.input-group-btn:last-child>.btn-group>.btn,.input-group-btn:last-child>.dropdown-toggle{border-top-left-radius:0;border-bottom-left-radius:0}.input-group-addon:last-child{border-left:0}.input-group-btn{position:relative;font-size:0;white-space:nowrap}.input-group-btn>.btn{position:relative}.input-group-btn>.btn+.btn{margin-left:-1px}.input-group-btn>.btn:active,.input-group-btn>.btn:focus,.input-group-btn>.btn:hover{z-index:2}.input-group-btn:first-child>.btn,.input-group-btn:first-child>.btn-group{margin-right:-1px}.input-group-btn:last-child>.btn,.input-group-btn:last-child>.btn-group{z-index:2;margin-left:-1px}.nav{padding-left:0;margin-bottom:0;list-style:none}.nav>li{position:relative;display:block}.nav>li>a{position:relative;display:block;padding:10px 15px}.nav>li>a:focus,.nav>li>a:hover{text-decoration:none;background-color:#eee}.nav>li.disabled>a{color:#777}.nav>li.disabled>a:focus,.nav>li.disabled>a:hover{color:#777;text-decoration:none;cursor:not-allowed;background-color:transparent}.nav .open>a,.nav .open>a:focus,.nav .open>a:hover{background-color:#eee;border-color:#337ab7}.nav .nav-divider{height:1px;margin:9px 0;overflow:hidden;background-color:#e5e5e5}.nav>li>a>img{max-width:none}.nav-tabs{border-bottom:1px solid #ddd}.nav-tabs>li{float:left;margin-bottom:-1px}.nav-tabs>li>a{margin-right:2px;line-height:1.42857143;border:1px solid transparent;border-radius:4px 4px 0 0}.nav-tabs>li>a:hover{border-color:#eee #eee #ddd}.nav-tabs>li.active>a,.nav-tabs>li.active>a:focus,.nav-tabs>li.active>a:hover{color:#555;cursor:default;background-color:#fff;border:1px solid #ddd;border-bottom-color:transparent}.nav-tabs.nav-justified{width:100%;border-bottom:0}.nav-tabs.nav-justified>li{float:none}.nav-tabs.nav-justified>li>a{margin-bottom:5px;text-align:center}.nav-tabs.nav-justified>.dropdown .dropdown-menu{top:auto;left:auto}@media (min-width:768px){.nav-tabs.nav-justified>li{display:table-cell;width:1%}.nav-tabs.nav-justified>li>a{margin-bottom:0}}.nav-tabs.nav-justified>li>a{margin-right:0;border-radius:4px}.nav-tabs.nav-justified>.active>a,.nav-tabs.nav-justified>.active>a:focus,.nav-tabs.nav-justified>.active>a:hover{border:1px solid #ddd}@media (min-width:768px){.nav-tabs.nav-justified>li>a{border-bottom:1px solid #ddd;border-radius:4px 4px 0 0}.nav-tabs.nav-justified>.active>a,.nav-tabs.nav-justified>.active>a:focus,.nav-tabs.nav-justified>.active>a:hover{border-bottom-color:#fff}}.nav-pills>li{float:left}.nav-pills>li>a{border-radius:4px}.nav-pills>li+li{margin-left:2px}.nav-pills>li.active>a,.nav-pills>li.active>a:focus,.nav-pills>li.active>a:hover{color:#fff;background-color:#337ab7}.nav-stacked>li{float:none}.nav-stacked>li+li{margin-top:2px;margin-left:0}.nav-justified{width:100%}.nav-justified>li{float:none}.nav-justified>li>a{margin-bottom:5px;text-align:center}.nav-justified>.dropdown .dropdown-menu{top:auto;left:auto}@media (min-width:768px){.nav-justified>li{display:table-cell;width:1%}.nav-justified>li>a{margin-bottom:0}}.nav-tabs-justified{border-bottom:0}.nav-tabs-justified>li>a{margin-right:0;border-radius:4px}.nav-tabs-justified>.active>a,.nav-tabs-justified>.active>a:focus,.nav-tabs-justified>.active>a:hover{border:1px solid #ddd}@media (min-width:768px){.nav-tabs-justified>li>a{border-bottom:1px solid #ddd;border-radius:4px 4px 0 0}.nav-tabs-justified>.active>a,.nav-tabs-justified>.active>a:focus,.nav-tabs-justified>.active>a:hover{border-bottom-color:#fff}}.tab-content>.tab-pane{display:none}.tab-content>.active{display:block}.nav-tabs .dropdown-menu{margin-top:-1px;border-top-left-radius:0;border-top-right-radius:0}.navbar{position:relative;min-height:50px;margin-bottom:20px;border:1px solid transparent}@media (min-width:768px){.navbar{border-radius:4px}}@media (min-width:768px){.navbar-header{float:left}}.navbar-collapse{padding-right:15px;padding-left:15px;overflow-x:visible;-webkit-overflow-scrolling:touch;border-top:1px solid transparent;-webkit-box-shadow:inset 0 1px 0 rgba(255,255,255,.1);box-shadow:inset 0 1px 0 rgba(255,255,255,.1)}.navbar-collapse.in{overflow-y:auto}@media (min-width:768px){.navbar-collapse{width:auto;border-top:0;-webkit-box-shadow:none;box-shadow:none}.navbar-collapse.collapse{display:block!important;height:auto!important;padding-bottom:0;overflow:visible!important}.navbar-collapse.in{overflow-y:visible}.navbar-fixed-bottom .navbar-collapse,.navbar-fixed-top .navbar-collapse,.navbar-static-top .navbar-collapse{padding-right:0;padding-left:0}}.navbar-fixed-bottom .navbar-collapse,.navbar-fixed-top .navbar-collapse{max-height:340px}@media (max-device-width:480px) and (orientation:landscape){.navbar-fixed-bottom .navbar-collapse,.navbar-fixed-top .navbar-collapse{max-height:200px}}.container-fluid>.navbar-collapse,.container-fluid>.navbar-header,.container>.navbar-collapse,.container>.navbar-header{margin-right:-15px;margin-left:-15px}@media (min-width:768px){.container-fluid>.navbar-collapse,.container-fluid>.navbar-header,.container>.navbar-collapse,.container>.navbar-header{margin-right:0;margin-left:0}}.navbar-static-top{z-index:1000;border-width:0 0 1px}@media (min-width:768px){.navbar-static-top{border-radius:0}}.navbar-fixed-bottom,.navbar-fixed-top{position:fixed;right:0;left:0;z-index:1030}@media (min-width:768px){.navbar-fixed-bottom,.navbar-fixed-top{border-radius:0}}.navbar-fixed-top{top:0;border-width:0 0 1px}.navbar-fixed-bottom{bottom:0;margin-bottom:0;border-width:1px 0 0}.navbar-brand{float:left;height:50px;padding:15px 15px;font-size:18px;line-height:20px}.navbar-brand:focus,.navbar-brand:hover{text-decoration:none}.navbar-brand>img{display:block}@media (min-width:768px){.navbar>.container .navbar-brand,.navbar>.container-fluid .navbar-brand{margin-left:-15px}}.navbar-toggle{position:relative;float:right;padding:9px 10px;margin-top:8px;margin-right:15px;margin-bottom:8px;background-color:transparent;background-image:none;border:1px solid transparent;border-radius:4px}.navbar-toggle:focus{outline:0}.navbar-toggle .icon-bar{display:block;width:22px;height:2px;border-radius:1px}.navbar-toggle .icon-bar+.icon-bar{margin-top:4px}@media (min-width:768px){.navbar-toggle{display:none}}.navbar-nav{margin:7.5px -15px}.navbar-nav>li>a{padding-top:10px;padding-bottom:10px;line-height:20px}@media (max-width:767px){.navbar-nav .open .dropdown-menu{position:static;float:none;width:auto;margin-top:0;background-color:transparent;border:0;-webkit-box-shadow:none;box-shadow:none}.navbar-nav .open .dropdown-menu .dropdown-header,.navbar-nav .open .dropdown-menu>li>a{padding:5px 15px 5px 25px}.navbar-nav .open .dropdown-menu>li>a{line-height:20px}.navbar-nav .open .dropdown-menu>li>a:focus,.navbar-nav .open .dropdown-menu>li>a:hover{background-image:none}}@media (min-width:768px){.navbar-nav{float:left;margin:0}.navbar-nav>li{float:left}.navbar-nav>li>a{padding-top:15px;padding-bottom:15px}}.navbar-form{padding:10px 15px;margin-top:8px;margin-right:-15px;margin-bottom:8px;margin-left:-15px;border-top:1px solid transparent;border-bottom:1px solid transparent;-webkit-box-shadow:inset 0 1px 0 rgba(255,255,255,.1),0 1px 0 rgba(255,255,255,.1);box-shadow:inset 0 1px 0 rgba(255,255,255,.1),0 1px 0 rgba(255,255,255,.1)}@media (min-width:768px){.navbar-form .form-group{display:inline-block;margin-bottom:0;vertical-align:middle}.navbar-form .form-control{display:inline-block;width:auto;vertical-align:middle}.navbar-form .form-control-static{display:inline-block}.navbar-form .input-group{display:inline-table;vertical-align:middle}.navbar-form .input-group .form-control,.navbar-form .input-group .input-group-addon,.navbar-form .input-group .input-group-btn{width:auto}.navbar-form .input-group>.form-control{width:100%}.navbar-form .control-label{margin-bottom:0;vertical-align:middle}.navbar-form .checkbox,.navbar-form .radio{display:inline-block;margin-top:0;margin-bottom:0;vertical-align:middle}.navbar-form .checkbox label,.navbar-form .radio label{padding-left:0}.navbar-form .checkbox input[type=checkbox],.navbar-form .radio input[type=radio]{position:relative;margin-left:0}.navbar-form .has-feedback .form-control-feedback{top:0}}@media (max-width:767px){.navbar-form .form-group{margin-bottom:5px}.navbar-form .form-group:last-child{margin-bottom:0}}@media (min-width:768px){.navbar-form{width:auto;padding-top:0;padding-bottom:0;margin-right:0;margin-left:0;border:0;-webkit-box-shadow:none;box-shadow:none}}.navbar-nav>li>.dropdown-menu{margin-top:0;border-top-left-radius:0;border-top-right-radius:0}.navbar-fixed-bottom .navbar-nav>li>.dropdown-menu{margin-bottom:0;border-top-left-radius:4px;border-top-right-radius:4px;border-bottom-right-radius:0;border-bottom-left-radius:0}.navbar-btn{margin-top:8px;margin-bottom:8px}.navbar-btn.btn-sm{margin-top:10px;margin-bottom:10px}.navbar-btn.btn-xs{margin-top:14px;margin-bottom:14px}.navbar-text{margin-top:15px;margin-bottom:15px}@media (min-width:768px){.navbar-text{float:left;margin-right:15px;margin-left:15px}}@media (min-width:768px){.navbar-left{float:left!important}.navbar-right{float:right!important;margin-right:-15px}.navbar-right~.navbar-right{margin-right:0}}.navbar-default{background-color:#f8f8f8;border-color:#e7e7e7}.navbar-default .navbar-brand{color:#777}.navbar-default .navbar-brand:focus,.navbar-default .navbar-brand:hover{color:#5e5e5e;background-color:transparent}.navbar-default .navbar-text{color:#777}.navbar-default .navbar-nav>li>a{color:#777}.navbar-default .navbar-nav>li>a:focus,.navbar-default .navbar-nav>li>a:hover{color:#333;background-color:transparent}.navbar-default .navbar-nav>.active>a,.navbar-default .navbar-nav>.active>a:focus,.navbar-default .navbar-nav>.active>a:hover{color:#555;background-color:#e7e7e7}.navbar-default .navbar-nav>.disabled>a,.navbar-default .navbar-nav>.disabled>a:focus,.navbar-default .navbar-nav>.disabled>a:hover{color:#ccc;background-color:transparent}.navbar-default .navbar-toggle{border-color:#ddd}.navbar-default .navbar-toggle:focus,.navbar-default .navbar-toggle:hover{background-color:#ddd}.navbar-default .navbar-toggle .icon-bar{background-color:#888}.navbar-default .navbar-collapse,.navbar-default .navbar-form{border-color:#e7e7e7}.navbar-default .navbar-nav>.open>a,.navbar-default .navbar-nav>.open>a:focus,.navbar-default .navbar-nav>.open>a:hover{color:#555;background-color:#e7e7e7}@media (max-width:767px){.navbar-default .navbar-nav .open .dropdown-menu>li>a{color:#777}.navbar-default .navbar-nav .open .dropdown-menu>li>a:focus,.navbar-default .navbar-nav .open .dropdown-menu>li>a:hover{color:#333;background-color:transparent}.navbar-default .navbar-nav .open .dropdown-menu>.active>a,.navbar-default .navbar-nav .open .dropdown-menu>.active>a:focus,.navbar-default .navbar-nav .open .dropdown-menu>.active>a:hover{color:#555;background-color:#e7e7e7}.navbar-default .navbar-nav .open .dropdown-menu>.disabled>a,.navbar-default .navbar-nav .open .dropdown-menu>.disabled>a:focus,.navbar-default .navbar-nav .open .dropdown-menu>.disabled>a:hover{color:#ccc;background-color:transparent}}.navbar-default .navbar-link{color:#777}.navbar-default .navbar-link:hover{color:#333}.navbar-default .btn-link{color:#777}.navbar-default .btn-link:focus,.navbar-default .btn-link:hover{color:#333}.navbar-default .btn-link[disabled]:focus,.navbar-default .btn-link[disabled]:hover,fieldset[disabled] .navbar-default .btn-link:focus,fieldset[disabled] .navbar-default .btn-link:hover{color:#ccc}.navbar-inverse{background-color:#222;border-color:#080808}.navbar-inverse .navbar-brand{color:#9d9d9d}.navbar-inverse .navbar-brand:focus,.navbar-inverse .navbar-brand:hover{color:#fff;background-color:transparent}.navbar-inverse .navbar-text{color:#9d9d9d}.navbar-inverse .navbar-nav>li>a{color:#9d9d9d}.navbar-inverse .navbar-nav>li>a:focus,.navbar-inverse .navbar-nav>li>a:hover{color:#fff;background-color:transparent}.navbar-inverse .navbar-nav>.active>a,.navbar-inverse .navbar-nav>.active>a:focus,.navbar-inverse .navbar-nav>.active>a:hover{color:#fff;background-color:#080808}.navbar-inverse .navbar-nav>.disabled>a,.navbar-inverse .navbar-nav>.disabled>a:focus,.navbar-inverse .navbar-nav>.disabled>a:hover{color:#444;background-color:transparent}.navbar-inverse .navbar-toggle{border-color:#333}.navbar-inverse .navbar-toggle:focus,.navbar-inverse .navbar-toggle:hover{background-color:#333}.navbar-inverse .navbar-toggle .icon-bar{background-color:#fff}.navbar-inverse .navbar-collapse,.navbar-inverse .navbar-form{border-color:#101010}.navbar-inverse .navbar-nav>.open>a,.navbar-inverse .navbar-nav>.open>a:focus,.navbar-inverse .navbar-nav>.open>a:hover{color:#fff;background-color:#080808}@media (max-width:767px){.navbar-inverse .navbar-nav .open .dropdown-menu>.dropdown-header{border-color:#080808}.navbar-inverse .navbar-nav .open .dropdown-menu .divider{background-color:#080808}.navbar-inverse .navbar-nav .open .dropdown-menu>li>a{color:#9d9d9d}.navbar-inverse .navbar-nav .open .dropdown-menu>li>a:focus,.navbar-inverse .navbar-nav .open .dropdown-menu>li>a:hover{color:#fff;background-color:transparent}.navbar-inverse .navbar-nav .open .dropdown-menu>.active>a,.navbar-inverse .navbar-nav .open .dropdown-menu>.active>a:focus,.navbar-inverse .navbar-nav .open .dropdown-menu>.active>a:hover{color:#fff;background-color:#080808}.navbar-inverse .navbar-nav .open .dropdown-menu>.disabled>a,.navbar-inverse .navbar-nav .open .dropdown-menu>.disabled>a:focus,.navbar-inverse .navbar-nav .open .dropdown-menu>.disabled>a:hover{color:#444;background-color:transparent}}.navbar-inverse .navbar-link{color:#9d9d9d}.navbar-inverse .navbar-link:hover{color:#fff}.navbar-inverse .btn-link{color:#9d9d9d}.navbar-inverse .btn-link:focus,.navbar-inverse .btn-link:hover{color:#fff}.navbar-inverse .btn-link[disabled]:focus,.navbar-inverse .btn-link[disabled]:hover,fieldset[disabled] .navbar-inverse .btn-link:focus,fieldset[disabled] .navbar-inverse .btn-link:hover{color:#444}.breadcrumb{padding:8px 15px;margin-bottom:20px;list-style:none;background-color:#f5f5f5;border-radius:4px}.breadcrumb>li{display:inline-block}.breadcrumb>li+li:before{padding:0 5px;color:#ccc;content:"/\00a0"}.breadcrumb>.active{color:#777}.pagination{display:inline-block;padding-left:0;margin:20px 0;border-radius:4px}.pagination>li{display:inline}.pagination>li>a,.pagination>li>span{position:relative;float:left;padding:6px 12px;margin-left:-1px;line-height:1.42857143;color:#337ab7;text-decoration:none;background-color:#fff;border:1px solid #ddd}.pagination>li:first-child>a,.pagination>li:first-child>span{margin-left:0;border-top-left-radius:4px;border-bottom-left-radius:4px}.pagination>li:last-child>a,.pagination>li:last-child>span{border-top-right-radius:4px;border-bottom-right-radius:4px}.pagination>li>a:focus,.pagination>li>a:hover,.pagination>li>span:focus,.pagination>li>span:hover{z-index:2;color:#23527c;background-color:#eee;border-color:#ddd}.pagination>.active>a,.pagination>.active>a:focus,.pagination>.active>a:hover,.pagination>.active>span,.pagination>.active>span:focus,.pagination>.active>span:hover{z-index:3;color:#fff;cursor:default;background-color:#337ab7;border-color:#337ab7}.pagination>.disabled>a,.pagination>.disabled>a:focus,.pagination>.disabled>a:hover,.pagination>.disabled>span,.pagination>.disabled>span:focus,.pagination>.disabled>span:hover{color:#777;cursor:not-allowed;background-color:#fff;border-color:#ddd}.pagination-lg>li>a,.pagination-lg>li>span{padding:10px 16px;font-size:18px;line-height:1.3333333}.pagination-lg>li:first-child>a,.pagination-lg>li:first-child>span{border-top-left-radius:6px;border-bottom-left-radius:6px}.pagination-lg>li:last-child>a,.pagination-lg>li:last-child>span{border-top-right-radius:6px;border-bottom-right-radius:6px}.pagination-sm>li>a,.pagination-sm>li>span{padding:5px 10px;font-size:12px;line-height:1.5}.pagination-sm>li:first-child>a,.pagination-sm>li:first-child>span{border-top-left-radius:3px;border-bottom-left-radius:3px}.pagination-sm>li:last-child>a,.pagination-sm>li:last-child>span{border-top-right-radius:3px;border-bottom-right-radius:3px}.pager{padding-left:0;margin:20px 0;text-align:center;list-style:none}.pager li{display:inline}.pager li>a,.pager li>span{display:inline-block;padding:5px 14px;background-color:#fff;border:1px solid #ddd;border-radius:15px}.pager li>a:focus,.pager li>a:hover{text-decoration:none;background-color:#eee}.pager .next>a,.pager .next>span{float:right}.pager .previous>a,.pager .previous>span{float:left}.pager .disabled>a,.pager .disabled>a:focus,.pager .disabled>a:hover,.pager .disabled>span{color:#777;cursor:not-allowed;background-color:#fff}.label{display:inline;padding:.2em .6em .3em;font-size:75%;font-weight:700;line-height:1;color:#fff;text-align:center;white-space:nowrap;vertical-align:baseline;border-radius:.25em}a.label:focus,a.label:hover{color:#fff;text-decoration:none;cursor:pointer}.label:empty{display:none}.btn .label{position:relative;top:-1px}.label-default{background-color:#777}.label-default[href]:focus,.label-default[href]:hover{background-color:#5e5e5e}.label-primary{background-color:#337ab7}.label-primary[href]:focus,.label-primary[href]:hover{background-color:#286090}.label-success{background-color:#5cb85c}.label-success[href]:focus,.label-success[href]:hover{background-color:#449d44}.label-info{background-color:#5bc0de}.label-info[href]:focus,.label-info[href]:hover{background-color:#31b0d5}.label-warning{background-color:#f0ad4e}.label-warning[href]:focus,.label-warning[href]:hover{background-color:#ec971f}.label-danger{background-color:#d9534f}.label-danger[href]:focus,.label-danger[href]:hover{background-color:#c9302c}.badge{display:inline-block;min-width:10px;padding:3px 7px;font-size:12px;font-weight:700;line-height:1;color:#fff;text-align:center;white-space:nowrap;vertical-align:middle;background-color:#777;border-radius:10px}.badge:empty{display:none}.btn .badge{position:relative;top:-1px}.btn-group-xs>.btn .badge,.btn-xs .badge{top:0;padding:1px 5px}a.badge:focus,a.badge:hover{color:#fff;text-decoration:none;cursor:pointer}.list-group-item.active>.badge,.nav-pills>.active>a>.badge{color:#337ab7;background-color:#fff}.list-group-item>.badge{float:right}.list-group-item>.badge+.badge{margin-right:5px}.nav-pills>li>a>.badge{margin-left:3px}.jumbotron{padding-top:30px;padding-bottom:30px;margin-bottom:30px;color:inherit;background-color:#eee}.jumbotron .h1,.jumbotron h1{color:inherit}.jumbotron p{margin-bottom:15px;font-size:21px;font-weight:200}.jumbotron>hr{border-top-color:#d5d5d5}.container .jumbotron,.container-fluid .jumbotron{padding-right:15px;padding-left:15px;border-radius:6px}.jumbotron .container{max-width:100%}@media screen and (min-width:768px){.jumbotron{padding-top:48px;padding-bottom:48px}.container .jumbotron,.container-fluid .jumbotron{padding-right:60px;padding-left:60px}.jumbotron .h1,.jumbotron h1{font-size:63px}}.thumbnail{display:block;padding:4px;margin-bottom:20px;line-height:1.42857143;background-color:#fff;border:1px solid #ddd;border-radius:4px;-webkit-transition:border .2s ease-in-out;-o-transition:border .2s ease-in-out;transition:border .2s ease-in-out}.thumbnail a>img,.thumbnail>img{margin-right:auto;margin-left:auto}a.thumbnail.active,a.thumbnail:focus,a.thumbnail:hover{border-color:#337ab7}.thumbnail .caption{padding:9px;color:#333}.alert{padding:15px;margin-bottom:20px;border:1px solid transparent;border-radius:4px}.alert h4{margin-top:0;color:inherit}.alert .alert-link{font-weight:700}.alert>p,.alert>ul{margin-bottom:0}.alert>p+p{margin-top:5px}.alert-dismissable,.alert-dismissible{padding-right:35px}.alert-dismissable .close,.alert-dismissible .close{position:relative;top:-2px;right:-21px;color:inherit}.alert-success{color:#3c763d;background-color:#dff0d8;border-color:#d6e9c6}.alert-success hr{border-top-color:#c9e2b3}.alert-success .alert-link{color:#2b542c}.alert-info{color:#31708f;background-color:#d9edf7;border-color:#bce8f1}.alert-info hr{border-top-color:#a6e1ec}.alert-info .alert-link{color:#245269}.alert-warning{color:#8a6d3b;background-color:#fcf8e3;border-color:#faebcc}.alert-warning hr{border-top-color:#f7e1b5}.alert-warning .alert-link{color:#66512c}.alert-danger{color:#a94442;background-color:#f2dede;border-color:#ebccd1}.alert-danger hr{border-top-color:#e4b9c0}.alert-danger .alert-link{color:#843534}@-webkit-keyframes progress-bar-stripes{from{background-position:40px 0}to{background-position:0 0}}@-o-keyframes progress-bar-stripes{from{background-position:40px 0}to{background-position:0 0}}@keyframes progress-bar-stripes{from{background-position:40px 0}to{background-position:0 0}}.progress{height:20px;margin-bottom:20px;overflow:hidden;background-color:#f5f5f5;border-radius:4px;-webkit-box-shadow:inset 0 1px 2px rgba(0,0,0,.1);box-shadow:inset 0 1px 2px rgba(0,0,0,.1)}.progress-bar{float:left;width:0;height:100%;font-size:12px;line-height:20px;color:#fff;text-align:center;background-color:#337ab7;-webkit-box-shadow:inset 0 -1px 0 rgba(0,0,0,.15);box-shadow:inset 0 -1px 0 rgba(0,0,0,.15);-webkit-transition:width .6s ease;-o-transition:width .6s ease;transition:width .6s ease}.progress-bar-striped,.progress-striped .progress-bar{background-image:-webkit-linear-gradient(45deg,rgba(255,255,255,.15) 25%,transparent 25%,transparent 50%,rgba(255,255,255,.15) 50%,rgba(255,255,255,.15) 75%,transparent 75%,transparent);background-image:-o-linear-gradient(45deg,rgba(255,255,255,.15) 25%,transparent 25%,transparent 50%,rgba(255,255,255,.15) 50%,rgba(255,255,255,.15) 75%,transparent 75%,transparent);background-image:linear-gradient(45deg,rgba(255,255,255,.15) 25%,transparent 25%,transparent 50%,rgba(255,255,255,.15) 50%,rgba(255,255,255,.15) 75%,transparent 75%,transparent);-webkit-background-size:40px 40px;background-size:40px 40px}.progress-bar.active,.progress.active .progress-bar{-webkit-animation:progress-bar-stripes 2s linear infinite;-o-animation:progress-bar-stripes 2s linear infinite;animation:progress-bar-stripes 2s linear infinite}.progress-bar-success{background-color:#5cb85c}.progress-striped .progress-bar-success{background-image:-webkit-linear-gradient(45deg,rgba(255,255,255,.15) 25%,transparent 25%,transparent 50%,rgba(255,255,255,.15) 50%,rgba(255,255,255,.15) 75%,transparent 75%,transparent);background-image:-o-linear-gradient(45deg,rgba(255,255,255,.15) 25%,transparent 25%,transparent 50%,rgba(255,255,255,.15) 50%,rgba(255,255,255,.15) 75%,transparent 75%,transparent);background-image:linear-gradient(45deg,rgba(255,255,255,.15) 25%,transparent 25%,transparent 50%,rgba(255,255,255,.15) 50%,rgba(255,255,255,.15) 75%,transparent 75%,transparent)}.progress-bar-info{background-color:#5bc0de}.progress-striped .progress-bar-info{background-image:-webkit-linear-gradient(45deg,rgba(255,255,255,.15) 25%,transparent 25%,transparent 50%,rgba(255,255,255,.15) 50%,rgba(255,255,255,.15) 75%,transparent 75%,transparent);background-image:-o-linear-gradient(45deg,rgba(255,255,255,.15) 25%,transparent 25%,transparent 50%,rgba(255,255,255,.15) 50%,rgba(255,255,255,.15) 75%,transparent 75%,transparent);background-image:linear-gradient(45deg,rgba(255,255,255,.15) 25%,transparent 25%,transparent 50%,rgba(255,255,255,.15) 50%,rgba(255,255,255,.15) 75%,transparent 75%,transparent)}.progress-bar-warning{background-color:#f0ad4e}.progress-striped .progress-bar-warning{background-image:-webkit-linear-gradient(45deg,rgba(255,255,255,.15) 25%,transparent 25%,transparent 50%,rgba(255,255,255,.15) 50%,rgba(255,255,255,.15) 75%,transparent 75%,transparent);background-image:-o-linear-gradient(45deg,rgba(255,255,255,.15) 25%,transparent 25%,transparent 50%,rgba(255,255,255,.15) 50%,rgba(255,255,255,.15) 75%,transparent 75%,transparent);background-image:linear-gradient(45deg,rgba(255,255,255,.15) 25%,transparent 25%,transparent 50%,rgba(255,255,255,.15) 50%,rgba(255,255,255,.15) 75%,transparent 75%,transparent)}.progress-bar-danger{background-color:#d9534f}.progress-striped .progress-bar-danger{background-image:-webkit-linear-gradient(45deg,rgba(255,255,255,.15) 25%,transparent 25%,transparent 50%,rgba(255,255,255,.15) 50%,rgba(255,255,255,.15) 75%,transparent 75%,transparent);background-image:-o-linear-gradient(45deg,rgba(255,255,255,.15) 25%,transparent 25%,transparent 50%,rgba(255,255,255,.15) 50%,rgba(255,255,255,.15) 75%,transparent 75%,transparent);background-image:linear-gradient(45deg,rgba(255,255,255,.15) 25%,transparent 25%,transparent 50%,rgba(255,255,255,.15) 50%,rgba(255,255,255,.15) 75%,transparent 75%,transparent)}.media{margin-top:15px}.media:first-child{margin-top:0}.media,.media-body{overflow:hidden;zoom:1}.media-body{width:10000px}.media-object{display:block}.media-object.img-thumbnail{max-width:none}.media-right,.media>.pull-right{padding-left:10px}.media-left,.media>.pull-left{padding-right:10px}.media-body,.media-left,.media-right{display:table-cell;vertical-align:top}.media-middle{vertical-align:middle}.media-bottom{vertical-align:bottom}.media-heading{margin-top:0;margin-bottom:5px}.media-list{padding-left:0;list-style:none}.list-group{padding-left:0;margin-bottom:20px}.list-group-item{position:relative;display:block;padding:10px 15px;margin-bottom:-1px;background-color:#fff;border:1px solid #ddd}.list-group-item:first-child{border-top-left-radius:4px;border-top-right-radius:4px}.list-group-item:last-child{margin-bottom:0;border-bottom-right-radius:4px;border-bottom-left-radius:4px}a.list-group-item,button.list-group-item{color:#555}a.list-group-item .list-group-item-heading,button.list-group-item .list-group-item-heading{color:#333}a.list-group-item:focus,a.list-group-item:hover,button.list-group-item:focus,button.list-group-item:hover{color:#555;text-decoration:none;background-color:#f5f5f5}button.list-group-item{width:100%;text-align:left}.list-group-item.disabled,.list-group-item.disabled:focus,.list-group-item.disabled:hover{color:#777;cursor:not-allowed;background-color:#eee}.list-group-item.disabled .list-group-item-heading,.list-group-item.disabled:focus .list-group-item-heading,.list-group-item.disabled:hover .list-group-item-heading{color:inherit}.list-group-item.disabled .list-group-item-text,.list-group-item.disabled:focus .list-group-item-text,.list-group-item.disabled:hover .list-group-item-text{color:#777}.list-group-item.active,.list-group-item.active:focus,.list-group-item.active:hover{z-index:2;color:#fff;background-color:#337ab7;border-color:#337ab7}.list-group-item.active .list-group-item-heading,.list-group-item.active .list-group-item-heading>.small,.list-group-item.active .list-group-item-heading>small,.list-group-item.active:focus .list-group-item-heading,.list-group-item.active:focus .list-group-item-heading>.small,.list-group-item.active:focus .list-group-item-heading>small,.list-group-item.active:hover .list-group-item-heading,.list-group-item.active:hover .list-group-item-heading>.small,.list-group-item.active:hover .list-group-item-heading>small{color:inherit}.list-group-item.active .list-group-item-text,.list-group-item.active:focus .list-group-item-text,.list-group-item.active:hover .list-group-item-text{color:#c7ddef}.list-group-item-success{color:#3c763d;background-color:#dff0d8}a.list-group-item-success,button.list-group-item-success{color:#3c763d}a.list-group-item-success .list-group-item-heading,button.list-group-item-success .list-group-item-heading{color:inherit}a.list-group-item-success:focus,a.list-group-item-success:hover,button.list-group-item-success:focus,button.list-group-item-success:hover{color:#3c763d;background-color:#d0e9c6}a.list-group-item-success.active,a.list-group-item-success.active:focus,a.list-group-item-success.active:hover,button.list-group-item-success.active,button.list-group-item-success.active:focus,button.list-group-item-success.active:hover{color:#fff;background-color:#3c763d;border-color:#3c763d}.list-group-item-info{color:#31708f;background-color:#d9edf7}a.list-group-item-info,button.list-group-item-info{color:#31708f}a.list-group-item-info .list-group-item-heading,button.list-group-item-info .list-group-item-heading{color:inherit}a.list-group-item-info:focus,a.list-group-item-info:hover,button.list-group-item-info:focus,button.list-group-item-info:hover{color:#31708f;background-color:#c4e3f3}a.list-group-item-info.active,a.list-group-item-info.active:focus,a.list-group-item-info.active:hover,button.list-group-item-info.active,button.list-group-item-info.active:focus,button.list-group-item-info.active:hover{color:#fff;background-color:#31708f;border-color:#31708f}.list-group-item-warning{color:#8a6d3b;background-color:#fcf8e3}a.list-group-item-warning,button.list-group-item-warning{color:#8a6d3b}a.list-group-item-warning .list-group-item-heading,button.list-group-item-warning .list-group-item-heading{color:inherit}a.list-group-item-warning:focus,a.list-group-item-warning:hover,button.list-group-item-warning:focus,button.list-group-item-warning:hover{color:#8a6d3b;background-color:#faf2cc}a.list-group-item-warning.active,a.list-group-item-warning.active:focus,a.list-group-item-warning.active:hover,button.list-group-item-warning.active,button.list-group-item-warning.active:focus,button.list-group-item-warning.active:hover{color:#fff;background-color:#8a6d3b;border-color:#8a6d3b}.list-group-item-danger{color:#a94442;background-color:#f2dede}a.list-group-item-danger,button.list-group-item-danger{color:#a94442}a.list-group-item-danger .list-group-item-heading,button.list-group-item-danger .list-group-item-heading{color:inherit}a.list-group-item-danger:focus,a.list-group-item-danger:hover,button.list-group-item-danger:focus,button.list-group-item-danger:hover{color:#a94442;background-color:#ebcccc}a.list-group-item-danger.active,a.list-group-item-danger.active:focus,a.list-group-item-danger.active:hover,button.list-group-item-danger.active,button.list-group-item-danger.active:focus,button.list-group-item-danger.active:hover{color:#fff;background-color:#a94442;border-color:#a94442}.list-group-item-heading{margin-top:0;margin-bottom:5px}.list-group-item-text{margin-bottom:0;line-height:1.3}.panel{margin-bottom:20px;background-color:#fff;border:1px solid transparent;border-radius:4px;-webkit-box-shadow:0 1px 1px rgba(0,0,0,.05);box-shadow:0 1px 1px rgba(0,0,0,.05)}.panel-body{padding:15px}.panel-heading{padding:10px 15px;border-bottom:1px solid transparent;border-top-left-radius:3px;border-top-right-radius:3px}.panel-heading>.dropdown .dropdown-toggle{color:inherit}.panel-title{margin-top:0;margin-bottom:0;font-size:16px;color:inherit}.panel-title>.small,.panel-title>.small>a,.panel-title>a,.panel-title>small,.panel-title>small>a{color:inherit}.panel-footer{padding:10px 15px;background-color:#f5f5f5;border-top:1px solid #ddd;border-bottom-right-radius:3px;border-bottom-left-radius:3px}.panel>.list-group,.panel>.panel-collapse>.list-group{margin-bottom:0}.panel>.list-group .list-group-item,.panel>.panel-collapse>.list-group .list-group-item{border-width:1px 0;border-radius:0}.panel>.list-group:first-child .list-group-item:first-child,.panel>.panel-collapse>.list-group:first-child .list-group-item:first-child{border-top:0;border-top-left-radius:3px;border-top-right-radius:3px}.panel>.list-group:last-child .list-group-item:last-child,.panel>.panel-collapse>.list-group:last-child .list-group-item:last-child{border-bottom:0;border-bottom-right-radius:3px;border-bottom-left-radius:3px}.panel>.panel-heading+.panel-collapse>.list-group .list-group-item:first-child{border-top-left-radius:0;border-top-right-radius:0}.panel-heading+.list-group .list-group-item:first-child{border-top-width:0}.list-group+.panel-footer{border-top-width:0}.panel>.panel-collapse>.table,.panel>.table,.panel>.table-responsive>.table{margin-bottom:0}.panel>.panel-collapse>.table caption,.panel>.table caption,.panel>.table-responsive>.table caption{padding-right:15px;padding-left:15px}.panel>.table-responsive:first-child>.table:first-child,.panel>.table:first-child{border-top-left-radius:3px;border-top-right-radius:3px}.panel>.table-responsive:first-child>.table:first-child>tbody:first-child>tr:first-child,.panel>.table-responsive:first-child>.table:first-child>thead:first-child>tr:first-child,.panel>.table:first-child>tbody:first-child>tr:first-child,.panel>.table:first-child>thead:first-child>tr:first-child{border-top-left-radius:3px;border-top-right-radius:3px}.panel>.table-responsive:first-child>.table:first-child>tbody:first-child>tr:first-child td:first-child,.panel>.table-responsive:first-child>.table:first-child>tbody:first-child>tr:first-child th:first-child,.panel>.table-responsive:first-child>.table:first-child>thead:first-child>tr:first-child td:first-child,.panel>.table-responsive:first-child>.table:first-child>thead:first-child>tr:first-child th:first-child,.panel>.table:first-child>tbody:first-child>tr:first-child td:first-child,.panel>.table:first-child>tbody:first-child>tr:first-child th:first-child,.panel>.table:first-child>thead:first-child>tr:first-child td:first-child,.panel>.table:first-child>thead:first-child>tr:first-child th:first-child{border-top-left-radius:3px}.panel>.table-responsive:first-child>.table:first-child>tbody:first-child>tr:first-child td:last-child,.panel>.table-responsive:first-child>.table:first-child>tbody:first-child>tr:first-child th:last-child,.panel>.table-responsive:first-child>.table:first-child>thead:first-child>tr:first-child td:last-child,.panel>.table-responsive:first-child>.table:first-child>thead:first-child>tr:first-child th:last-child,.panel>.table:first-child>tbody:first-child>tr:first-child td:last-child,.panel>.table:first-child>tbody:first-child>tr:first-child th:last-child,.panel>.table:first-child>thead:first-child>tr:first-child td:last-child,.panel>.table:first-child>thead:first-child>tr:first-child th:last-child{border-top-right-radius:3px}.panel>.table-responsive:last-child>.table:last-child,.panel>.table:last-child{border-bottom-right-radius:3px;border-bottom-left-radius:3px}.panel>.table-responsive:last-child>.table:last-child>tbody:last-child>tr:last-child,.panel>.table-responsive:last-child>.table:last-child>tfoot:last-child>tr:last-child,.panel>.table:last-child>tbody:last-child>tr:last-child,.panel>.table:last-child>tfoot:last-child>tr:last-child{border-bottom-right-radius:3px;border-bottom-left-radius:3px}.panel>.table-responsive:last-child>.table:last-child>tbody:last-child>tr:last-child td:first-child,.panel>.table-responsive:last-child>.table:last-child>tbody:last-child>tr:last-child th:first-child,.panel>.table-responsive:last-child>.table:last-child>tfoot:last-child>tr:last-child td:first-child,.panel>.table-responsive:last-child>.table:last-child>tfoot:last-child>tr:last-child th:first-child,.panel>.table:last-child>tbody:last-child>tr:last-child td:first-child,.panel>.table:last-child>tbody:last-child>tr:last-child th:first-child,.panel>.table:last-child>tfoot:last-child>tr:last-child td:first-child,.panel>.table:last-child>tfoot:last-child>tr:last-child th:first-child{border-bottom-left-radius:3px}.panel>.table-responsive:last-child>.table:last-child>tbody:last-child>tr:last-child td:last-child,.panel>.table-responsive:last-child>.table:last-child>tbody:last-child>tr:last-child th:last-child,.panel>.table-responsive:last-child>.table:last-child>tfoot:last-child>tr:last-child td:last-child,.panel>.table-responsive:last-child>.table:last-child>tfoot:last-child>tr:last-child th:last-child,.panel>.table:last-child>tbody:last-child>tr:last-child td:last-child,.panel>.table:last-child>tbody:last-child>tr:last-child th:last-child,.panel>.table:last-child>tfoot:last-child>tr:last-child td:last-child,.panel>.table:last-child>tfoot:last-child>tr:last-child th:last-child{border-bottom-right-radius:3px}.panel>.panel-body+.table,.panel>.panel-body+.table-responsive,.panel>.table+.panel-body,.panel>.table-responsive+.panel-body{border-top:1px solid #ddd}.panel>.table>tbody:first-child>tr:first-child td,.panel>.table>tbody:first-child>tr:first-child th{border-top:0}.panel>.table-bordered,.panel>.table-responsive>.table-bordered{border:0}.panel>.table-bordered>tbody>tr>td:first-child,.panel>.table-bordered>tbody>tr>th:first-child,.panel>.table-bordered>tfoot>tr>td:first-child,.panel>.table-bordered>tfoot>tr>th:first-child,.panel>.table-bordered>thead>tr>td:first-child,.panel>.table-bordered>thead>tr>th:first-child,.panel>.table-responsive>.table-bordered>tbody>tr>td:first-child,.panel>.table-responsive>.table-bordered>tbody>tr>th:first-child,.panel>.table-responsive>.table-bordered>tfoot>tr>td:first-child,.panel>.table-responsive>.table-bordered>tfoot>tr>th:first-child,.panel>.table-responsive>.table-bordered>thead>tr>td:first-child,.panel>.table-responsive>.table-bordered>thead>tr>th:first-child{border-left:0}.panel>.table-bordered>tbody>tr>td:last-child,.panel>.table-bordered>tbody>tr>th:last-child,.panel>.table-bordered>tfoot>tr>td:last-child,.panel>.table-bordered>tfoot>tr>th:last-child,.panel>.table-bordered>thead>tr>td:last-child,.panel>.table-bordered>thead>tr>th:last-child,.panel>.table-responsive>.table-bordered>tbody>tr>td:last-child,.panel>.table-responsive>.table-bordered>tbody>tr>th:last-child,.panel>.table-responsive>.table-bordered>tfoot>tr>td:last-child,.panel>.table-responsive>.table-bordered>tfoot>tr>th:last-child,.panel>.table-responsive>.table-bordered>thead>tr>td:last-child,.panel>.table-responsive>.table-bordered>thead>tr>th:last-child{border-right:0}.panel>.table-bordered>tbody>tr:first-child>td,.panel>.table-bordered>tbody>tr:first-child>th,.panel>.table-bordered>thead>tr:first-child>td,.panel>.table-bordered>thead>tr:first-child>th,.panel>.table-responsive>.table-bordered>tbody>tr:first-child>td,.panel>.table-responsive>.table-bordered>tbody>tr:first-child>th,.panel>.table-responsive>.table-bordered>thead>tr:first-child>td,.panel>.table-responsive>.table-bordered>thead>tr:first-child>th{border-bottom:0}.panel>.table-bordered>tbody>tr:last-child>td,.panel>.table-bordered>tbody>tr:last-child>th,.panel>.table-bordered>tfoot>tr:last-child>td,.panel>.table-bordered>tfoot>tr:last-child>th,.panel>.table-responsive>.table-bordered>tbody>tr:last-child>td,.panel>.table-responsive>.table-bordered>tbody>tr:last-child>th,.panel>.table-responsive>.table-bordered>tfoot>tr:last-child>td,.panel>.table-responsive>.table-bordered>tfoot>tr:last-child>th{border-bottom:0}.panel>.table-responsive{margin-bottom:0;border:0}.panel-group{margin-bottom:20px}.panel-group .panel{margin-bottom:0;border-radius:4px}.panel-group .panel+.panel{margin-top:5px}.panel-group .panel-heading{border-bottom:0}.panel-group .panel-heading+.panel-collapse>.list-group,.panel-group .panel-heading+.panel-collapse>.panel-body{border-top:1px solid #ddd}.panel-group .panel-footer{border-top:0}.panel-group .panel-footer+.panel-collapse .panel-body{border-bottom:1px solid #ddd}.panel-default{border-color:#ddd}.panel-default>.panel-heading{color:#333;background-color:#f5f5f5;border-color:#ddd}.panel-default>.panel-heading+.panel-collapse>.panel-body{border-top-color:#ddd}.panel-default>.panel-heading .badge{color:#f5f5f5;background-color:#333}.panel-default>.panel-footer+.panel-collapse>.panel-body{border-bottom-color:#ddd}.panel-primary{border-color:#337ab7}.panel-primary>.panel-heading{color:#fff;background-color:#337ab7;border-color:#337ab7}.panel-primary>.panel-heading+.panel-collapse>.panel-body{border-top-color:#337ab7}.panel-primary>.panel-heading .badge{color:#337ab7;background-color:#fff}.panel-primary>.panel-footer+.panel-collapse>.panel-body{border-bottom-color:#337ab7}.panel-success{border-color:#d6e9c6}.panel-success>.panel-heading{color:#3c763d;background-color:#dff0d8;border-color:#d6e9c6}.panel-success>.panel-heading+.panel-collapse>.panel-body{border-top-color:#d6e9c6}.panel-success>.panel-heading .badge{color:#dff0d8;background-color:#3c763d}.panel-success>.panel-footer+.panel-collapse>.panel-body{border-bottom-color:#d6e9c6}.panel-info{border-color:#bce8f1}.panel-info>.panel-heading{color:#31708f;background-color:#d9edf7;border-color:#bce8f1}.panel-info>.panel-heading+.panel-collapse>.panel-body{border-top-color:#bce8f1}.panel-info>.panel-heading .badge{color:#d9edf7;background-color:#31708f}.panel-info>.panel-footer+.panel-collapse>.panel-body{border-bottom-color:#bce8f1}.panel-warning{border-color:#faebcc}.panel-warning>.panel-heading{color:#8a6d3b;background-color:#fcf8e3;border-color:#faebcc}.panel-warning>.panel-heading+.panel-collapse>.panel-body{border-top-color:#faebcc}.panel-warning>.panel-heading .badge{color:#fcf8e3;background-color:#8a6d3b}.panel-warning>.panel-footer+.panel-collapse>.panel-body{border-bottom-color:#faebcc}.panel-danger{border-color:#ebccd1}.panel-danger>.panel-heading{color:#a94442;background-color:#f2dede;border-color:#ebccd1}.panel-danger>.panel-heading+.panel-collapse>.panel-body{border-top-color:#ebccd1}.panel-danger>.panel-heading .badge{color:#f2dede;background-color:#a94442}.panel-danger>.panel-footer+.panel-collapse>.panel-body{border-bottom-color:#ebccd1}.embed-responsive{position:relative;display:block;height:0;padding:0;overflow:hidden}.embed-responsive .embed-responsive-item,.embed-responsive embed,.embed-responsive iframe,.embed-responsive object,.embed-responsive video{position:absolute;top:0;bottom:0;left:0;width:100%;height:100%;border:0}.embed-responsive-16by9{padding-bottom:56.25%}.embed-responsive-4by3{padding-bottom:75%}.well{min-height:20px;padding:19px;margin-bottom:20px;background-color:#f5f5f5;border:1px solid #e3e3e3;border-radius:4px;-webkit-box-shadow:inset 0 1px 1px rgba(0,0,0,.05);box-shadow:inset 0 1px 1px rgba(0,0,0,.05)}.well blockquote{border-color:#ddd;border-color:rgba(0,0,0,.15)}.well-lg{padding:24px;border-radius:6px}.well-sm{padding:9px;border-radius:3px}.close{float:right;font-size:21px;font-weight:700;line-height:1;color:#000;text-shadow:0 1px 0 #fff;filter:alpha(opacity=20);opacity:.2}.close:focus,.close:hover{color:#000;text-decoration:none;cursor:pointer;filter:alpha(opacity=50);opacity:.5}button.close{-webkit-appearance:none;padding:0;cursor:pointer;background:0 0;border:0}.modal-open{overflow:hidden}.modal{position:fixed;top:0;right:0;bottom:0;left:0;z-index:1050;display:none;overflow:hidden;-webkit-overflow-scrolling:touch;outline:0}.modal.fade .modal-dialog{-webkit-transition:-webkit-transform .3s ease-out;-o-transition:-o-transform .3s ease-out;transition:transform .3s ease-out;-webkit-transform:translate(0,-25%);-ms-transform:translate(0,-25%);-o-transform:translate(0,-25%);transform:translate(0,-25%)}.modal.in .modal-dialog{-webkit-transform:translate(0,0);-ms-transform:translate(0,0);-o-transform:translate(0,0);transform:translate(0,0)}.modal-open .modal{overflow-x:hidden;overflow-y:auto}.modal-dialog{position:relative;width:auto;margin:10px}.modal-content{position:relative;background-color:#fff;-webkit-background-clip:padding-box;background-clip:padding-box;border:1px solid #999;border:1px solid rgba(0,0,0,.2);border-radius:6px;outline:0;-webkit-box-shadow:0 3px 9px rgba(0,0,0,.5);box-shadow:0 3px 9px rgba(0,0,0,.5)}.modal-backdrop{position:fixed;top:0;right:0;bottom:0;left:0;z-index:1040;background-color:#000}.modal-backdrop.fade{filter:alpha(opacity=0);opacity:0}.modal-backdrop.in{filter:alpha(opacity=50);opacity:.5}.modal-header{padding:15px;border-bottom:1px solid #e5e5e5}.modal-header .close{margin-top:-2px}.modal-title{margin:0;line-height:1.42857143}.modal-body{position:relative;padding:15px}.modal-footer{padding:15px;text-align:right;border-top:1px solid #e5e5e5}.modal-footer .btn+.btn{margin-bottom:0;margin-left:5px}.modal-footer .btn-group .btn+.btn{margin-left:-1px}.modal-footer .btn-block+.btn-block{margin-left:0}.modal-scrollbar-measure{position:absolute;top:-9999px;width:50px;height:50px;overflow:scroll}@media (min-width:768px){.modal-dialog{width:600px;margin:30px auto}.modal-content{-webkit-box-shadow:0 5px 15px rgba(0,0,0,.5);box-shadow:0 5px 15px rgba(0,0,0,.5)}.modal-sm{width:300px}}@media (min-width:992px){.modal-lg{width:900px}}.tooltip{position:absolute;z-index:1070;display:block;font-family:"Helvetica Neue",Helvetica,Arial,sans-serif;font-size:12px;font-style:normal;font-weight:400;line-height:1.42857143;text-align:left;text-align:start;text-decoration:none;text-shadow:none;text-transform:none;letter-spacing:normal;word-break:normal;word-spacing:normal;word-wrap:normal;white-space:normal;filter:alpha(opacity=0);opacity:0;line-break:auto}.tooltip.in{filter:alpha(opacity=90);opacity:.9}.tooltip.top{padding:5px 0;margin-top:-3px}.tooltip.right{padding:0 5px;margin-left:3px}.tooltip.bottom{padding:5px 0;margin-top:3px}.tooltip.left{padding:0 5px;margin-left:-3px}.tooltip-inner{max-width:200px;padding:3px 8px;color:#fff;text-align:center;background-color:#000;border-radius:4px}.tooltip-arrow{position:absolute;width:0;height:0;border-color:transparent;border-style:solid}.tooltip.top .tooltip-arrow{bottom:0;left:50%;margin-left:-5px;border-width:5px 5px 0;border-top-color:#000}.tooltip.top-left .tooltip-arrow{right:5px;bottom:0;margin-bottom:-5px;border-width:5px 5px 0;border-top-color:#000}.tooltip.top-right .tooltip-arrow{bottom:0;left:5px;margin-bottom:-5px;border-width:5px 5px 0;border-top-color:#000}.tooltip.right .tooltip-arrow{top:50%;left:0;margin-top:-5px;border-width:5px 5px 5px 0;border-right-color:#000}.tooltip.left .tooltip-arrow{top:50%;right:0;margin-top:-5px;border-width:5px 0 5px 5px;border-left-color:#000}.tooltip.bottom .tooltip-arrow{top:0;left:50%;margin-left:-5px;border-width:0 5px 5px;border-bottom-color:#000}.tooltip.bottom-left .tooltip-arrow{top:0;right:5px;margin-top:-5px;border-width:0 5px 5px;border-bottom-color:#000}.tooltip.bottom-right .tooltip-arrow{top:0;left:5px;margin-top:-5px;border-width:0 5px 5px;border-bottom-color:#000}.popover{position:absolute;top:0;left:0;z-index:1060;display:none;max-width:276px;padding:1px;font-family:"Helvetica Neue",Helvetica,Arial,sans-serif;font-size:14px;font-style:normal;font-weight:400;line-height:1.42857143;text-align:left;text-align:start;text-decoration:none;text-shadow:none;text-transform:none;letter-spacing:normal;word-break:normal;word-spacing:normal;word-wrap:normal;white-space:normal;background-color:#fff;-webkit-background-clip:padding-box;background-clip:padding-box;border:1px solid #ccc;border:1px solid rgba(0,0,0,.2);border-radius:6px;-webkit-box-shadow:0 5px 10px rgba(0,0,0,.2);box-shadow:0 5px 10px rgba(0,0,0,.2);line-break:auto}.popover.top{margin-top:-10px}.popover.right{margin-left:10px}.popover.bottom{margin-top:10px}.popover.left{margin-left:-10px}.popover-title{padding:8px 14px;margin:0;font-size:14px;background-color:#f7f7f7;border-bottom:1px solid #ebebeb;border-radius:5px 5px 0 0}.popover-content{padding:9px 14px}.popover>.arrow,.popover>.arrow:after{position:absolute;display:block;width:0;height:0;border-color:transparent;border-style:solid}.popover>.arrow{border-width:11px}.popover>.arrow:after{content:"";border-width:10px}.popover.top>.arrow{bottom:-11px;left:50%;margin-left:-11px;border-top-color:#999;border-top-color:rgba(0,0,0,.25);border-bottom-width:0}.popover.top>.arrow:after{bottom:1px;margin-left:-10px;content:" ";border-top-color:#fff;border-bottom-width:0}.popover.right>.arrow{top:50%;left:-11px;margin-top:-11px;border-right-color:#999;border-right-color:rgba(0,0,0,.25);border-left-width:0}.popover.right>.arrow:after{bottom:-10px;left:1px;content:" ";border-right-color:#fff;border-left-width:0}.popover.bottom>.arrow{top:-11px;left:50%;margin-left:-11px;border-top-width:0;border-bottom-color:#999;border-bottom-color:rgba(0,0,0,.25)}.popover.bottom>.arrow:after{top:1px;margin-left:-10px;content:" ";border-top-width:0;border-bottom-color:#fff}.popover.left>.arrow{top:50%;right:-11px;margin-top:-11px;border-right-width:0;border-left-color:#999;border-left-color:rgba(0,0,0,.25)}.popover.left>.arrow:after{right:1px;bottom:-10px;content:" ";border-right-width:0;border-left-color:#fff}.carousel{position:relative}.carousel-inner{position:relative;width:100%;overflow:hidden}.carousel-inner>.item{position:relative;display:none;-webkit-transition:.6s ease-in-out left;-o-transition:.6s ease-in-out left;transition:.6s ease-in-out left}.carousel-inner>.item>a>img,.carousel-inner>.item>img{line-height:1}@media all and (transform-3d),(-webkit-transform-3d){.carousel-inner>.item{-webkit-transition:-webkit-transform .6s ease-in-out;-o-transition:-o-transform .6s ease-in-out;transition:transform .6s ease-in-out;-webkit-backface-visibility:hidden;backface-visibility:hidden;-webkit-perspective:1000px;perspective:1000px}.carousel-inner>.item.active.right,.carousel-inner>.item.next{left:0;-webkit-transform:translate3d(100%,0,0);transform:translate3d(100%,0,0)}.carousel-inner>.item.active.left,.carousel-inner>.item.prev{left:0;-webkit-transform:translate3d(-100%,0,0);transform:translate3d(-100%,0,0)}.carousel-inner>.item.active,.carousel-inner>.item.next.left,.carousel-inner>.item.prev.right{left:0;-webkit-transform:translate3d(0,0,0);transform:translate3d(0,0,0)}}.carousel-inner>.active,.carousel-inner>.next,.carousel-inner>.prev{display:block}.carousel-inner>.active{left:0}.carousel-inner>.next,.carousel-inner>.prev{position:absolute;top:0;width:100%}.carousel-inner>.next{left:100%}.carousel-inner>.prev{left:-100%}.carousel-inner>.next.left,.carousel-inner>.prev.right{left:0}.carousel-inner>.active.left{left:-100%}.carousel-inner>.active.right{left:100%}.carousel-control{position:absolute;top:0;bottom:0;left:0;width:15%;font-size:20px;color:#fff;text-align:center;text-shadow:0 1px 2px rgba(0,0,0,.6);background-color:rgba(0,0,0,0);filter:alpha(opacity=50);opacity:.5}.carousel-control.left{background-image:-webkit-linear-gradient(left,rgba(0,0,0,.5) 0,rgba(0,0,0,.0001) 100%);background-image:-o-linear-gradient(left,rgba(0,0,0,.5) 0,rgba(0,0,0,.0001) 100%);background-image:-webkit-gradient(linear,left top,right top,from(rgba(0,0,0,.5)),to(rgba(0,0,0,.0001)));background-image:linear-gradient(to right,rgba(0,0,0,.5) 0,rgba(0,0,0,.0001) 100%);filter:progid:DXImageTransform.Microsoft.gradient(startColorstr='#80000000', endColorstr='#00000000', GradientType=1);background-repeat:repeat-x}.carousel-control.right{right:0;left:auto;background-image:-webkit-linear-gradient(left,rgba(0,0,0,.0001) 0,rgba(0,0,0,.5) 100%);background-image:-o-linear-gradient(left,rgba(0,0,0,.0001) 0,rgba(0,0,0,.5) 100%);background-image:-webkit-gradient(linear,left top,right top,from(rgba(0,0,0,.0001)),to(rgba(0,0,0,.5)));background-image:linear-gradient(to right,rgba(0,0,0,.0001) 0,rgba(0,0,0,.5) 100%);filter:progid:DXImageTransform.Microsoft.gradient(startColorstr='#00000000', endColorstr='#80000000', GradientType=1);background-repeat:repeat-x}.carousel-control:focus,.carousel-control:hover{color:#fff;text-decoration:none;filter:alpha(opacity=90);outline:0;opacity:.9}.carousel-control .glyphicon-chevron-left,.carousel-control .glyphicon-chevron-right,.carousel-control .icon-next,.carousel-control .icon-prev{position:absolute;top:50%;z-index:5;display:inline-block;margin-top:-10px}.carousel-control .glyphicon-chevron-left,.carousel-control .icon-prev{left:50%;margin-left:-10px}.carousel-control .glyphicon-chevron-right,.carousel-control .icon-next{right:50%;margin-right:-10px}.carousel-control .icon-next,.carousel-control .icon-prev{width:20px;height:20px;font-family:serif;line-height:1}.carousel-control .icon-prev:before{content:'\2039'}.carousel-control .icon-next:before{content:'\203a'}.carousel-indicators{position:absolute;bottom:10px;left:50%;z-index:15;width:60%;padding-left:0;margin-left:-30%;text-align:center;list-style:none}.carousel-indicators li{display:inline-block;width:10px;height:10px;margin:1px;text-indent:-999px;cursor:pointer;background-color:#000\9;background-color:rgba(0,0,0,0);border:1px solid #fff;border-radius:10px}.carousel-indicators .active{width:12px;height:12px;margin:0;background-color:#fff}.carousel-caption{position:absolute;right:15%;bottom:20px;left:15%;z-index:10;padding-top:20px;padding-bottom:20px;color:#fff;text-align:center;text-shadow:0 1px 2px rgba(0,0,0,.6)}.carousel-caption .btn{text-shadow:none}@media screen and (min-width:768px){.carousel-control .glyphicon-chevron-left,.carousel-control .glyphicon-chevron-right,.carousel-control .icon-next,.carousel-control .icon-prev{width:30px;height:30px;margin-top:-10px;font-size:30px}.carousel-control .glyphicon-chevron-left,.carousel-control .icon-prev{margin-left:-10px}.carousel-control .glyphicon-chevron-right,.carousel-control .icon-next{margin-right:-10px}.carousel-caption{right:20%;left:20%;padding-bottom:30px}.carousel-indicators{bottom:20px}}.btn-group-vertical>.btn-group:after,.btn-group-vertical>.btn-group:before,.btn-toolbar:after,.btn-toolbar:before,.clearfix:after,.clearfix:before,.container-fluid:after,.container-fluid:before,.container:after,.container:before,.dl-horizontal dd:after,.dl-horizontal dd:before,.form-horizontal .form-group:after,.form-horizontal .form-group:before,.modal-footer:after,.modal-footer:before,.modal-header:after,.modal-header:before,.nav:after,.nav:before,.navbar-collapse:after,.navbar-collapse:before,.navbar-header:after,.navbar-header:before,.navbar:after,.navbar:before,.pager:after,.pager:before,.panel-body:after,.panel-body:before,.row:after,.row:before{display:table;content:" "}.btn-group-vertical>.btn-group:after,.btn-toolbar:after,.clearfix:after,.container-fluid:after,.container:after,.dl-horizontal dd:after,.form-horizontal .form-group:after,.modal-footer:after,.modal-header:after,.nav:after,.navbar-collapse:after,.navbar-header:after,.navbar:after,.pager:after,.panel-body:after,.row:after{clear:both}.center-block{display:block;margin-right:auto;margin-left:auto}.pull-right{float:right!important}.pull-left{float:left!important}.hide{display:none!important}.show{display:block!important}.invisible{visibility:hidden}.text-hide{font:0/0 a;color:transparent;text-shadow:none;background-color:transparent;border:0}.hidden{display:none!important}.affix{position:fixed}@-ms-viewport{width:device-width}.visible-lg,.visible-md,.visible-sm,.visible-xs{display:none!important}.visible-lg-block,.visible-lg-inline,.visible-lg-inline-block,.visible-md-block,.visible-md-inline,.visible-md-inline-block,.visible-sm-block,.visible-sm-inline,.visible-sm-inline-block,.visible-xs-block,.visible-xs-inline,.visible-xs-inline-block{display:none!important}@media (max-width:767px){.visible-xs{display:block!important}table.visible-xs{display:table!important}tr.visible-xs{display:table-row!important}td.visible-xs,th.visible-xs{display:table-cell!important}}@media (max-width:767px){.visible-xs-block{display:block!important}}@media (max-width:767px){.visible-xs-inline{display:inline!important}}@media (max-width:767px){.visible-xs-inline-block{display:inline-block!important}}@media (min-width:768px) and (max-width:991px){.visible-sm{display:block!important}table.visible-sm{display:table!important}tr.visible-sm{display:table-row!important}td.visible-sm,th.visible-sm{display:table-cell!important}}@media (min-width:768px) and (max-width:991px){.visible-sm-block{display:block!important}}@media (min-width:768px) and (max-width:991px){.visible-sm-inline{display:inline!important}}@media (min-width:768px) and (max-width:991px){.visible-sm-inline-block{display:inline-block!important}}@media (min-width:992px) and (max-width:1199px){.visible-md{display:block!important}table.visible-md{display:table!important}tr.visible-md{display:table-row!important}td.visible-md,th.visible-md{display:table-cell!important}}@media (min-width:992px) and (max-width:1199px){.visible-md-block{display:block!important}}@media (min-width:992px) and (max-width:1199px){.visible-md-inline{display:inline!important}}@media (min-width:992px) and (max-width:1199px){.visible-md-inline-block{display:inline-block!important}}@media (min-width:1200px){.visible-lg{display:block!important}table.visible-lg{display:table!important}tr.visible-lg{display:table-row!important}td.visible-lg,th.visible-lg{display:table-cell!important}}@media (min-width:1200px){.visible-lg-block{display:block!important}}@media (min-width:1200px){.visible-lg-inline{display:inline!important}}@media (min-width:1200px){.visible-lg-inline-block{display:inline-block!important}}@media (max-width:767px){.hidden-xs{display:none!important}}@media (min-width:768px) and (max-width:991px){.hidden-sm{display:none!important}}@media (min-width:992px) and (max-width:1199px){.hidden-md{display:none!important}}@media (min-width:1200px){.hidden-lg{display:none!important}}.visible-print{display:none!important}@media print{.visible-print{display:block!important}table.visible-print{display:table!important}tr.visible-print{display:table-row!important}td.visible-print,th.visible-print{display:table-cell!important}}.visible-print-block{display:none!important}@media print{.visible-print-block{display:block!important}}.visible-print-inline{display:none!important}@media print{.visible-print-inline{display:inline!important}}.visible-print-inline-block{display:none!important}@media print{.visible-print-inline-block{display:inline-block!important}}@media print{.hidden-print{display:none!important}} +/*# sourceMappingURL=bootstrap.min.css.map */ \ No newline at end of file diff --git a/webclient/assets/css/bootstrap.min.css.map b/webclient/assets/css/bootstrap.min.css.map new file mode 100755 index 0000000..5f49bb3 --- /dev/null +++ b/webclient/assets/css/bootstrap.min.css.map @@ -0,0 +1 @@ +{"version":3,"sources":["less/normalize.less","less/print.less","bootstrap.css","dist/css/bootstrap.css","less/glyphicons.less","less/scaffolding.less","less/mixins/vendor-prefixes.less","less/mixins/tab-focus.less","less/mixins/image.less","less/type.less","less/mixins/text-emphasis.less","less/mixins/background-variant.less","less/mixins/text-overflow.less","less/code.less","less/grid.less","less/mixins/grid.less","less/mixins/grid-framework.less","less/tables.less","less/mixins/table-row.less","less/forms.less","less/mixins/forms.less","less/buttons.less","less/mixins/buttons.less","less/mixins/opacity.less","less/component-animations.less","less/dropdowns.less","less/mixins/nav-divider.less","less/mixins/reset-filter.less","less/button-groups.less","less/mixins/border-radius.less","less/input-groups.less","less/navs.less","less/navbar.less","less/mixins/nav-vertical-align.less","less/utilities.less","less/breadcrumbs.less","less/pagination.less","less/mixins/pagination.less","less/pager.less","less/labels.less","less/mixins/labels.less","less/badges.less","less/jumbotron.less","less/thumbnails.less","less/alerts.less","less/mixins/alerts.less","less/progress-bars.less","less/mixins/gradients.less","less/mixins/progress-bar.less","less/media.less","less/list-group.less","less/mixins/list-group.less","less/panels.less","less/mixins/panels.less","less/responsive-embed.less","less/wells.less","less/close.less","less/modals.less","less/tooltip.less","less/mixins/reset-text.less","less/popovers.less","less/carousel.less","less/mixins/clearfix.less","less/mixins/center-block.less","less/mixins/hide-text.less","less/responsive-utilities.less","less/mixins/responsive-visibility.less"],"names":[],"mappings":";;;;4EAQA,KACE,YAAA,WACA,yBAAA,KACA,qBAAA,KAOF,KACE,OAAA,EAaF,QAAA,MAAA,QAAA,WAAA,OAAA,OAAA,OAAA,OAAA,KAAA,KAAA,IAAA,QAAA,QAaE,QAAA,MAQF,MAAA,OAAA,SAAA,MAIE,QAAA,aACA,eAAA,SAQF,sBACE,QAAA,KACA,OAAA,EAQF,SAAA,SAEE,QAAA,KAUF,EACE,iBAAA,YAQF,SAAA,QAEE,QAAA,EAUF,YACE,cAAA,IAAA,OAOF,EAAA,OAEE,YAAA,IAOF,IACE,WAAA,OAQF,GACE,OAAA,MAAA,EACA,UAAA,IAOF,KACE,MAAA,KACA,WAAA,KAOF,MACE,UAAA,IAOF,IAAA,IAEE,SAAA,SACA,UAAA,IACA,YAAA,EACA,eAAA,SAGF,IACE,IAAA,MAGF,IACE,OAAA,OAUF,IACE,OAAA,EAOF,eACE,SAAA,OAUF,OACE,OAAA,IAAA,KAOF,GACE,OAAA,EAAA,mBAAA,YAAA,gBAAA,YACA,WAAA,YAOF,IACE,SAAA,KAOF,KAAA,IAAA,IAAA,KAIE,YAAA,UAAA,UACA,UAAA,IAkBF,OAAA,MAAA,SAAA,OAAA,SAKE,OAAA,EACA,KAAA,QACA,MAAA,QAOF,OACE,SAAA,QAUF,OAAA,OAEE,eAAA,KAWF,OAAA,wBAAA,kBAAA,mBAIE,mBAAA,OACA,OAAA,QAOF,iBAAA,qBAEE,OAAA,QAOF,yBAAA,wBAEE,QAAA,EACA,OAAA,EAQF,MACE,YAAA,OAWF,qBAAA,kBAEE,mBAAA,WAAA,gBAAA,WAAA,WAAA,WACA,QAAA,EASF,8CAAA,8CAEE,OAAA,KAQF,mBACE,mBAAA,YACA,gBAAA,YAAA,WAAA,YAAA,mBAAA,UASF,iDAAA,8CAEE,mBAAA,KAOF,SACE,QAAA,MAAA,OAAA,MACA,OAAA,EAAA,IACA,OAAA,IAAA,MAAA,OAQF,OACE,QAAA,EACA,OAAA,EAOF,SACE,SAAA,KAQF,SACE,YAAA,IAUF,MACE,eAAA,EACA,gBAAA,SAGF,GAAA,GAEE,QAAA,uFCjUF,aA7FI,EAAA,OAAA,QAGI,MAAA,eACA,YAAA,eACA,WAAA,cAAA,mBAAA,eACA,WAAA,eAGJ,EAAA,UAEI,gBAAA,UAGJ,cACI,QAAA,KAAA,WAAA,IAGJ,kBACI,QAAA,KAAA,YAAA,IAKJ,6BAAA,mBAEI,QAAA,GAGJ,WAAA,IAEI,OAAA,IAAA,MAAA,KC4KL,kBAAA,MDvKK,MC0KL,QAAA,mBDrKK,IE8KN,GDLC,kBAAA,MDrKK,ICwKL,UAAA,eCUD,GF5KM,GE2KN,EF1KM,QAAA,ECuKL,OAAA,ECSD,GF3KM,GCsKL,iBAAA,MD/JK,QCkKL,QAAA,KCSD,YFtKU,oBCiKT,iBAAA,eD7JK,OCgKL,OAAA,IAAA,MAAA,KD5JK,OC+JL,gBAAA,mBCSD,UFpKU,UC+JT,iBAAA,eDzJS,mBEkKV,mBDLC,OAAA,IAAA,MAAA,gBEjPD,WACA,YAAA,uBFsPD,IAAA,+CE7OC,IAAK,sDAAuD,4BAA6B,iDAAkD,gBAAiB,gDAAiD,eAAgB,+CAAgD,mBAAoB,2EAA4E,cAE7W,WACA,SAAA,SACA,IAAA,IACA,QAAA,aACA,YAAA,uBACA,WAAA,OACA,YAAA,IACA,YAAA,EAIkC,uBAAA,YAAW,wBAAA,UACX,2BAAW,QAAA,QAEX,uBDuPlC,QAAS,QCtPyB,sBFiPnC,uBEjP8C,QAAA,QACX,wBAAW,QAAA,QACX,wBAAW,QAAA,QACX,2BAAW,QAAA,QACX,yBAAW,QAAA,QACX,wBAAW,QAAA,QACX,wBAAW,QAAA,QACX,yBAAW,QAAA,QACX,wBAAW,QAAA,QACX,uBAAW,QAAA,QACX,6BAAW,QAAA,QACX,uBAAW,QAAA,QACX,uBAAW,QAAA,QACX,2BAAW,QAAA,QACX,qBAAW,QAAA,QACX,0BAAW,QAAA,QACX,qBAAW,QAAA,QACX,yBAAW,QAAA,QACX,0BAAW,QAAA,QACX,2BAAW,QAAA,QACX,sBAAW,QAAA,QACX,yBAAW,QAAA,QACX,sBAAW,QAAA,QACX,wBAAW,QAAA,QACX,uBAAW,QAAA,QACX,uBAAW,QAAA,QACX,uBAAW,QAAA,QACX,uBAAW,QAAA,QACX,+BAAW,QAAA,QACX,2BAAW,QAAA,QACX,yBAAW,QAAA,QACX,wBAAW,QAAA,QACX,8BAAW,QAAA,QACX,yBAAW,QAAA,QACX,0BAAW,QAAA,QACX,2BAAW,QAAA,QACX,uBAAW,QAAA,QACX,uBAAW,QAAA,QACX,6BAAW,QAAA,QACX,6BAAW,QAAA,QACX,8BAAW,QAAA,QACX,4BAAW,QAAA,QACX,yBAAW,QAAA,QACX,0BAAW,QAAA,QACX,sBAAW,QAAA,QACX,uBAAW,QAAA,QACX,uBAAW,QAAA,QACX,2BAAW,QAAA,QACX,wBAAW,QAAA,QACX,yBAAW,QAAA,QACX,uBAAW,QAAA,QACX,uBAAW,QAAA,QACX,yBAAW,QAAA,QACX,8BAAW,QAAA,QACX,6BAAW,QAAA,QACX,6BAAW,QAAA,QACX,+BAAW,QAAA,QACX,8BAAW,QAAA,QACX,gCAAW,QAAA,QACX,uBAAW,QAAA,QACX,8BAAW,QAAA,QACX,+BAAW,QAAA,QACX,iCAAW,QAAA,QACX,0BAAW,QAAA,QACX,6BAAW,QAAA,QACX,yBAAW,QAAA,QACX,uBAAW,QAAA,QACX,uBAAW,QAAA,QACX,wBAAW,QAAA,QACX,wBAAW,QAAA,QACX,uBAAW,QAAA,QACX,gCAAW,QAAA,QACX,gCAAW,QAAA,QACX,2BAAW,QAAA,QACX,uBAAW,QAAA,QACX,wBAAW,QAAA,QACX,uBAAW,QAAA,QACX,0BAAW,QAAA,QACX,+BAAW,QAAA,QACX,+BAAW,QAAA,QACX,wBAAW,QAAA,QACX,+BAAW,QAAA,QACX,gCAAW,QAAA,QACX,4BAAW,QAAA,QACX,6BAAW,QAAA,QACX,8BAAW,QAAA,QACX,0BAAW,QAAA,QACX,gCAAW,QAAA,QACX,4BAAW,QAAA,QACX,6BAAW,QAAA,QACX,gCAAW,QAAA,QACX,4BAAW,QAAA,QACX,6BAAW,QAAA,QACX,6BAAW,QAAA,QACX,8BAAW,QAAA,QACX,2BAAW,QAAA,QACX,6BAAW,QAAA,QACX,4BAAW,QAAA,QACX,8BAAW,QAAA,QACX,+BAAW,QAAA,QACX,mCAAW,QAAA,QACX,uBAAW,QAAA,QACX,uBAAW,QAAA,QACX,uBAAW,QAAA,QACX,2BAAW,QAAA,QACX,4BAAW,QAAA,QACX,+BAAW,QAAA,QACX,wBAAW,QAAA,QACX,2BAAW,QAAA,QACX,yBAAW,QAAA,QACX,0BAAW,QAAA,QACX,yBAAW,QAAA,QACX,6BAAW,QAAA,QACX,+BAAW,QAAA,QACX,0BAAW,QAAA,QACX,gCAAW,QAAA,QACX,+BAAW,QAAA,QACX,8BAAW,QAAA,QACX,kCAAW,QAAA,QACX,oCAAW,QAAA,QACX,sBAAW,QAAA,QACX,2BAAW,QAAA,QACX,uBAAW,QAAA,QACX,8BAAW,QAAA,QACX,4BAAW,QAAA,QACX,8BAAW,QAAA,QACX,6BAAW,QAAA,QACX,4BAAW,QAAA,QACX,0BAAW,QAAA,QACX,4BAAW,QAAA,QACX,qCAAW,QAAA,QACX,oCAAW,QAAA,QACX,kCAAW,QAAA,QACX,oCAAW,QAAA,QACX,wBAAW,QAAA,QACX,yBAAW,QAAA,QACX,wBAAW,QAAA,QACX,yBAAW,QAAA,QACX,4BAAW,QAAA,QACX,6BAAW,QAAA,QACX,4BAAW,QAAA,QACX,4BAAW,QAAA,QACX,8BAAW,QAAA,QACX,uBAAW,QAAA,QACX,wBAAW,QAAA,QACX,0BAAW,QAAA,QACX,sBAAW,QAAA,QACX,sBAAW,QAAA,QACX,uBAAW,QAAA,QACX,mCAAW,QAAA,QACX,uCAAW,QAAA,QACX,gCAAW,QAAA,QACX,oCAAW,QAAA,QACX,qCAAW,QAAA,QACX,yCAAW,QAAA,QACX,4BAAW,QAAA,QACX,yBAAW,QAAA,QACX,gCAAW,QAAA,QACX,8BAAW,QAAA,QACX,yBAAW,QAAA,QACX,wBAAW,QAAA,QACX,0BAAW,QAAA,QACX,6BAAW,QAAA,QACX,yBAAW,QAAA,QACX,uBAAW,QAAA,QACX,uBAAW,QAAA,QACX,wBAAW,QAAA,QACX,yBAAW,QAAA,QACX,yBAAW,QAAA,QACX,uBAAW,QAAA,QACX,8BAAW,QAAA,QACX,+BAAW,QAAA,QACX,gCAAW,QAAA,QACX,8BAAW,QAAA,QACX,8BAAW,QAAA,QACX,8BAAW,QAAA,QACX,2BAAW,QAAA,QACX,0BAAW,QAAA,QACX,yBAAW,QAAA,QACX,6BAAW,QAAA,QACX,2BAAW,QAAA,QACX,4BAAW,QAAA,QACX,wBAAW,QAAA,QACX,wBAAW,QAAA,QACX,2BAAW,QAAA,QACX,2BAAW,QAAA,QACX,4BAAW,QAAA,QACX,+BAAW,QAAA,QACX,8BAAW,QAAA,QACX,4BAAW,QAAA,QACX,4BAAW,QAAA,QACX,4BAAW,QAAA,QACX,iCAAW,QAAA,QACX,oCAAW,QAAA,QACX,iCAAW,QAAA,QACX,+BAAW,QAAA,QACX,+BAAW,QAAA,QACX,iCAAW,QAAA,QACX,qBAAW,QAAA,QACX,4BAAW,QAAA,QACX,4BAAW,QAAA,QACX,2BAAW,QAAA,QACX,uBAAW,QAAA,QASX,wBAAW,QAAA,QACX,wBAAW,QAAA,QACX,4BAAW,QAAA,QACX,uBAAW,QAAA,QACX,wBAAW,QAAA,QACX,uBAAW,QAAA,QACX,yBAAW,QAAA,QACX,yBAAW,QAAA,QACX,+BAAW,QAAA,QACX,uBAAW,QAAA,QACX,6BAAW,QAAA,QACX,sBAAW,QAAA,QACX,wBAAW,QAAA,QACX,wBAAW,QAAA,QACX,4BAAW,QAAA,QACX,uBAAW,QAAA,QACX,4BAAW,QAAA,QACX,6BAAW,QAAA,QACX,2BAAW,QAAA,QACX,0BAAW,QAAA,QACX,sBAAW,QAAA,QACX,sBAAW,QAAA,QACX,sBAAW,QAAA,QACX,sBAAW,QAAA,QACX,wBAAW,QAAA,QACX,sBAAW,QAAA,QACX,wBAAW,QAAA,QACX,4BAAW,QAAA,QACX,mCAAW,QAAA,QACX,4BAAW,QAAA,QACX,oCAAW,QAAA,QACX,kCAAW,QAAA,QACX,iCAAW,QAAA,QACX,+BAAW,QAAA,QACX,sBAAW,QAAA,QACX,wBAAW,QAAA,QACX,6BAAW,QAAA,QACX,4BAAW,QAAA,QACX,6BAAW,QAAA,QACX,kCAAW,QAAA,QACX,mCAAW,QAAA,QACX,sCAAW,QAAA,QACX,0CAAW,QAAA,QACX,oCAAW,QAAA,QACX,wCAAW,QAAA,QACX,qCAAW,QAAA,QACX,iCAAW,QAAA,QACX,gCAAW,QAAA,QACX,kCAAW,QAAA,QACX,+BAAW,QAAA,QACX,0BAAW,QAAA,QACX,8BAAW,QAAA,QACX,4BAAW,QAAA,QACX,4BAAW,QAAA,QACX,6BAAW,QAAA,QACX,4BAAW,QAAA,QCtS/C,0BCgEE,QAAA,QHi+BF,EDNC,mBAAA,WGxhCI,gBAAiB,WFiiCZ,WAAY,WGl+BZ,OADL,QJg+BJ,mBAAA,WGthCI,gBAAiB,WACpB,WAAA,WHyhCD,KGrhCC,UAAW,KAEX,4BAAA,cAEA,KACA,YAAA,iBAAA,UAAA,MAAA,WHuhCD,UAAA,KGnhCC,YAAa,WF4hCb,MAAO,KACP,iBAAkB,KExhClB,OADA,MAEA,OHqhCD,SG/gCC,YAAa,QACb,UAAA,QACA,YAAA,QAEA,EFwhCA,MAAO,QEthCL,gBAAA,KAIF,QH8gCD,QKnkCC,MAAA,QAEA,gBAAA,ULskCD,QGxgCC,QAAS,KAAK,OACd,QAAA,IAAA,KAAA,yBH0gCD,eAAA,KGngCC,OHsgCD,OAAA,ECSD,IACE,eAAgB,ODDjB,4BMhlCC,0BLmlCF,gBKplCE,iBADA,eH4EA,QAAS,MACT,UAAA,KHwgCD,OAAA,KGjgCC,aACA,cAAA,IAEA,eACA,QAAA,aC6FA,UAAA,KACK,OAAA,KACG,QAAA,IEvLR,YAAA,WACA,iBAAA,KACA,OAAA,IAAA,MAAA,KNgmCD,cAAA,IGlgCC,mBAAoB,IAAI,IAAI,YAC5B,cAAA,IAAA,IAAA,YHogCD,WAAA,IAAA,IAAA,YG7/BC,YACA,cAAA,IAEA,GHggCD,WAAA,KGx/BC,cAAe,KACf,OAAA,EACA,WAAA,IAAA,MAAA,KAEA,SACA,SAAA,SACA,MAAA,IACA,OAAA,IACA,QAAA,EH0/BD,OAAA,KGl/BC,SAAA,OF2/BA,KAAM,cEz/BJ,OAAA,EAEA,0BACA,yBACA,SAAA,OACA,MAAA,KHo/BH,OAAA,KGz+BC,OAAQ,EACR,SAAA,QH2+BD,KAAA,KCSD,cACE,OAAQ,QAQV,IACA,IMnpCE,IACA,IACA,IACA,INyoCF,GACA,GACA,GACA,GACA,GACA,GDAC,YAAA,QOnpCC,YAAa,IN4pCb,YAAa,IACb,MAAO,QAoBT,WAZA,UAaA,WAZA,UM7pCI,WN8pCJ,UM7pCI,WN8pCJ,UM7pCI,WN8pCJ,UDMC,WCLD,UACA,UAZA,SAaA,UAZA,SAaA,UAZA,SAaA,UAZA,SAaA,UAZA,SAaA,UAZA,SMrpCE,YAAa,INyqCb,YAAa,EACb,MAAO,KAGT,IMzqCE,IAJF,IN4qCA,GAEA,GDLC,GCSC,WAAY,KACZ,cAAe,KASjB,WANA,UDCC,WCCD,UM7qCA,WN+qCA,UACA,UANA,SM7qCI,UN+qCJ,SM5qCA,UN8qCA,SAQE,UAAW,IAGb,IMrrCE,IAJF,INwrCA,GAEA,GDLC,GCSC,WAAY,KACZ,cAAe,KASjB,WANA,UDCC,WCCD,UMxrCA,WN0rCA,UACA,UANA,SMzrCI,UN2rCJ,SMvrCA,UNyrCA,SMzrCU,UAAA,IACV,IAAA,GAAU,UAAA,KACV,IAAA,GAAU,UAAA,KACV,IAAA,GAAU,UAAA,KACV,IAAA,GAAU,UAAA,KACV,IAAA,GAAU,UAAA,KAOR,IADF,GPusCC,UAAA,KCSD,EM1sCE,OAAA,EAAA,EAAA,KAEA,MPqsCD,cAAA,KOhsCC,UAAW,KAwOX,YAAa,IA1OX,YAAA,IPusCH,yBO9rCC,MNusCE,UAAW,MMlsCf,OAAA,MAEE,UAAA,IAKF,MP2rCC,KO3rCsB,QAAA,KP8rCtB,iBAAA,QO7rCsB,WPgsCtB,WAAA,KO/rCsB,YPksCtB,WAAA,MOjsCsB,aPosCtB,WAAA,OOnsCsB,cPssCtB,WAAA,QOnsCsB,aPssCtB,YAAA,OOrsCsB,gBPwsCtB,eAAA,UOvsCsB,gBP0sCtB,eAAA,UOtsCC,iBPysCD,eAAA,WQ5yCC,YR+yCD,MAAA,KCSD,cOrzCI,MAAA,QAHF,qBDwGF,qBP8sCC,MAAA,QCSD,cO5zCI,MAAA,QAHF,qBD2GF,qBPktCC,MAAA,QCSD,WOn0CI,MAAA,QAHF,kBD8GF,kBPstCC,MAAA,QCSD,cO10CI,MAAA,QAHF,qBDiHF,qBP0tCC,MAAA,QCSD,aOj1CI,MAAA,QDwHF,oBAHF,oBExHE,MAAA,QACA,YR21CA,MAAO,KQz1CL,iBAAA,QAHF,mBF8HF,mBP4tCC,iBAAA,QCSD,YQh2CI,iBAAA,QAHF,mBFiIF,mBPguCC,iBAAA,QCSD,SQv2CI,iBAAA,QAHF,gBFoIF,gBPouCC,iBAAA,QCSD,YQ92CI,iBAAA,QAHF,mBFuIF,mBPwuCC,iBAAA,QCSD,WQr3CI,iBAAA,QF6IF,kBADF,kBAEE,iBAAA,QPuuCD,aO9tCC,eAAgB,INuuChB,OAAQ,KAAK,EAAE,KMruCf,cAAA,IAAA,MAAA,KAFF,GPmuCC,GCSC,WAAY,EACZ,cAAe,KM/tCf,MP2tCD,MO5tCD,MAPI,MASF,cAAA,EAIF,eALE,aAAA,EACA,WAAA,KPmuCD,aO/tCC,aAAc,EAKZ,YAAA,KACA,WAAA,KP8tCH,gBOxtCC,QAAS,aACT,cAAA,IACA,aAAA,IAEF,GNiuCE,WAAY,EM/tCZ,cAAA,KAGA,GADF,GP2tCC,YAAA,WOvtCC,GP0tCD,YAAA,IOpnCD,GAvFM,YAAA,EAEA,yBACA,kBGtNJ,MAAA,KACA,MAAA,MACA,SAAA,OVs6CC,MAAA,KO9nCC,WAAY,MAhFV,cAAA,SPitCH,YAAA,OOvsCD,kBNitCE,YAAa,OM3sCjB,0BPusCC,YOtsCC,OAAA,KA9IqB,cAAA,IAAA,OAAA,KAmJvB,YACE,UAAA,IACA,eAAA,UAEA,WPusCD,QAAA,KAAA,KOlsCG,OAAA,EAAA,EAAA,KN2sCF,UAAW,OACX,YAAa,IAAI,MAAM,KMrtCzB,yBPgtCC,wBOhtCD,yBN0tCE,cAAe,EMpsCb,kBAFA,kBACA,iBPmsCH,QAAA,MOhsCG,UAAA,INysCF,YAAa,WACb,MAAO,KMjsCT,yBP4rCC,yBO5rCD,wBAEE,QAAA,cAEA,oBACA,sBACA,cAAA,KP8rCD,aAAA,EOxrCG,WAAA,MNisCF,aAAc,IAAI,MAAM,KACxB,YAAa,EMjsCX,kCNmsCJ,kCMpsCe,iCACX,oCNosCJ,oCDLC,mCCUC,QAAS,GMlsCX,iCNosCA,iCM1sCM,gCAOJ,mCNosCF,mCDLC,kCO9rCC,QAAA,cPmsCD,QWx+CC,cAAe,KVi/Cf,WAAY,OACZ,YAAa,WU9+Cb,KX0+CD,IWt+CD,IACE,KACA,YAAA,MAAA,OAAA,SAAA,cAAA,UAEA,KACA,QAAA,IAAA,IXw+CD,UAAA,IWp+CC,MAAO,QACP,iBAAA,QACA,cAAA,IAEA,IACA,QAAA,IAAA,IACA,UAAA,IV6+CA,MU7+CA,KXs+CD,iBAAA,KW5+CC,cAAe,IASb,mBAAA,MAAA,EAAA,KAAA,EAAA,gBACA,WAAA,MAAA,EAAA,KAAA,EAAA,gBAEA,QV8+CF,QU9+CE,EXs+CH,UAAA,KWj+CC,YAAa,IACb,mBAAA,KACA,WAAA,KAEA,IACA,QAAA,MACA,QAAA,MACA,OAAA,EAAA,EAAA,KACA,UAAA,KACA,YAAA,WACA,MAAA,KACA,WAAA,UXm+CD,UAAA,WW9+CC,iBAAkB,QAehB,OAAA,IAAA,MAAA,KACA,cAAA,IAEA,SACA,QAAA,EACA,UAAA,QXk+CH,MAAA,QW79CC,YAAa,SACb,iBAAA,YACA,cAAA,EC1DF,gBCHE,WAAA,MACA,WAAA,OAEA,Wb+hDD,cAAA,KYzhDC,aAAA,KAqEA,aAAc,KAvEZ,YAAA,KZgiDH,yBY3hDC,WAkEE,MAAO,OZ89CV,yBY7hDC,WA+DE,MAAO,OZm+CV,0BY1hDC,WCvBA,MAAA,QAGA,iBbojDD,cAAA,KYvhDC,aAAc,KCvBd,aAAA,KACA,YAAA,KCAE,KACE,aAAA,MAEA,YAAA,MAGA,UAAA,WAAA,WAAA,WAAA,UAAA,UAAA,UAAA,UAAA,UAAA,UAAA,UAAA,UAAA,UAAA,WAAA,WAAA,WAAA,UAAA,UAAA,UAAA,UAAA,UAAA,UAAA,UAAA,UAAA,UAAA,WAAA,WAAA,WAAA,UAAA,UAAA,UAAA,UAAA,UAAA,UAAA,UAAA,UAAA,UAAA,WAAA,WAAA,WAAA,UAAA,UAAA,UAAA,UAAA,UAAA,UAAA,UAAA,UdijDL,SAAA,ScjiDG,WAAA,IACE,cAAA,KdmiDL,aAAA,Kc3hDG,UAAA,WAAA,WAAA,WAAA,UAAA,UAAA,UAAA,UAAA,UAAA,UAAA,UAAA,Ud8hDH,MAAA,Kc9hDG,WdiiDH,MAAA,KcjiDG,WdoiDH,MAAA,acpiDG,WduiDH,MAAA,acviDG,Ud0iDH,MAAA,Ic1iDG,Ud6iDH,MAAA,ac7iDG,UdgjDH,MAAA,achjDG,UdmjDH,MAAA,IcnjDG,UdsjDH,MAAA,actjDG,UdyjDH,MAAA,aczjDG,Ud4jDH,MAAA,Ic5jDG,Ud+jDH,MAAA,achjDG,UdmjDH,MAAA,YcnjDG,gBdsjDH,MAAA,KctjDG,gBdyjDH,MAAA,aczjDG,gBd4jDH,MAAA,ac5jDG,ed+jDH,MAAA,Ic/jDG,edkkDH,MAAA,aclkDG,edqkDH,MAAA,acrkDG,edwkDH,MAAA,IcxkDG,ed2kDH,MAAA,ac3kDG,ed8kDH,MAAA,ac9kDG,edilDH,MAAA,IcjlDG,edolDH,MAAA,ac/kDG,edklDH,MAAA,YcjmDG,edomDH,MAAA,KcpmDG,gBdumDH,KAAA,KcvmDG,gBd0mDH,KAAA,ac1mDG,gBd6mDH,KAAA,ac7mDG,edgnDH,KAAA,IchnDG,edmnDH,KAAA,acnnDG,edsnDH,KAAA,actnDG,edynDH,KAAA,IcznDG,ed4nDH,KAAA,ac5nDG,ed+nDH,KAAA,ac/nDG,edkoDH,KAAA,IcloDG,edqoDH,KAAA,achoDG,edmoDH,KAAA,YcpnDG,edunDH,KAAA,KcvnDG,kBd0nDH,YAAA,Kc1nDG,kBd6nDH,YAAA,ac7nDG,kBdgoDH,YAAA,achoDG,iBdmoDH,YAAA,IcnoDG,iBdsoDH,YAAA,actoDG,iBdyoDH,YAAA,aczoDG,iBd4oDH,YAAA,Ic5oDG,iBd+oDH,YAAA,ac/oDG,iBdkpDH,YAAA,aclpDG,iBdqpDH,YAAA,IcrpDG,iBdwpDH,YAAA,acxpDG,iBd2pDH,YAAA,Yc7rDG,iBACE,YAAA,EAOJ,yBACE,UAAA,WAAA,WAAA,WAAA,UAAA,UAAA,UAAA,UAAA,UAAA,UAAA,UAAA,Ud2rDD,MAAA,Kc3rDC,Wd8rDD,MAAA,Kc9rDC,WdisDD,MAAA,acjsDC,WdosDD,MAAA,acpsDC,UdusDD,MAAA,IcvsDC,Ud0sDD,MAAA,ac1sDC,Ud6sDD,MAAA,ac7sDC,UdgtDD,MAAA,IchtDC,UdmtDD,MAAA,acntDC,UdstDD,MAAA,acttDC,UdytDD,MAAA,IcztDC,Ud4tDD,MAAA,ac7sDC,UdgtDD,MAAA,YchtDC,gBdmtDD,MAAA,KcntDC,gBdstDD,MAAA,acttDC,gBdytDD,MAAA,acztDC,ed4tDD,MAAA,Ic5tDC,ed+tDD,MAAA,ac/tDC,edkuDD,MAAA,acluDC,edquDD,MAAA,IcruDC,edwuDD,MAAA,acxuDC,ed2uDD,MAAA,ac3uDC,ed8uDD,MAAA,Ic9uDC,edivDD,MAAA,ac5uDC,ed+uDD,MAAA,Yc9vDC,ediwDD,MAAA,KcjwDC,gBdowDD,KAAA,KcpwDC,gBduwDD,KAAA,acvwDC,gBd0wDD,KAAA,ac1wDC,ed6wDD,KAAA,Ic7wDC,edgxDD,KAAA,achxDC,edmxDD,KAAA,acnxDC,edsxDD,KAAA,IctxDC,edyxDD,KAAA,aczxDC,ed4xDD,KAAA,ac5xDC,ed+xDD,KAAA,Ic/xDC,edkyDD,KAAA,ac7xDC,edgyDD,KAAA,YcjxDC,edoxDD,KAAA,KcpxDC,kBduxDD,YAAA,KcvxDC,kBd0xDD,YAAA,ac1xDC,kBd6xDD,YAAA,ac7xDC,iBdgyDD,YAAA,IchyDC,iBdmyDD,YAAA,acnyDC,iBdsyDD,YAAA,actyDC,iBdyyDD,YAAA,IczyDC,iBd4yDD,YAAA,ac5yDC,iBd+yDD,YAAA,ac/yDC,iBdkzDD,YAAA,IclzDC,iBdqzDD,YAAA,acrzDC,iBdwzDD,YAAA,YY/yDD,iBE3CE,YAAA,GAQF,yBACE,UAAA,WAAA,WAAA,WAAA,UAAA,UAAA,UAAA,UAAA,UAAA,UAAA,UAAA,Udy1DD,MAAA,Kcz1DC,Wd41DD,MAAA,Kc51DC,Wd+1DD,MAAA,ac/1DC,Wdk2DD,MAAA,acl2DC,Udq2DD,MAAA,Icr2DC,Udw2DD,MAAA,acx2DC,Ud22DD,MAAA,ac32DC,Ud82DD,MAAA,Ic92DC,Udi3DD,MAAA,acj3DC,Udo3DD,MAAA,acp3DC,Udu3DD,MAAA,Icv3DC,Ud03DD,MAAA,ac32DC,Ud82DD,MAAA,Yc92DC,gBdi3DD,MAAA,Kcj3DC,gBdo3DD,MAAA,acp3DC,gBdu3DD,MAAA,acv3DC,ed03DD,MAAA,Ic13DC,ed63DD,MAAA,ac73DC,edg4DD,MAAA,ach4DC,edm4DD,MAAA,Icn4DC,eds4DD,MAAA,act4DC,edy4DD,MAAA,acz4DC,ed44DD,MAAA,Ic54DC,ed+4DD,MAAA,ac14DC,ed64DD,MAAA,Yc55DC,ed+5DD,MAAA,Kc/5DC,gBdk6DD,KAAA,Kcl6DC,gBdq6DD,KAAA,acr6DC,gBdw6DD,KAAA,acx6DC,ed26DD,KAAA,Ic36DC,ed86DD,KAAA,ac96DC,edi7DD,KAAA,acj7DC,edo7DD,KAAA,Icp7DC,edu7DD,KAAA,acv7DC,ed07DD,KAAA,ac17DC,ed67DD,KAAA,Ic77DC,edg8DD,KAAA,ac37DC,ed87DD,KAAA,Yc/6DC,edk7DD,KAAA,Kcl7DC,kBdq7DD,YAAA,Kcr7DC,kBdw7DD,YAAA,acx7DC,kBd27DD,YAAA,ac37DC,iBd87DD,YAAA,Ic97DC,iBdi8DD,YAAA,acj8DC,iBdo8DD,YAAA,acp8DC,iBdu8DD,YAAA,Icv8DC,iBd08DD,YAAA,ac18DC,iBd68DD,YAAA,ac78DC,iBdg9DD,YAAA,Ich9DC,iBdm9DD,YAAA,acn9DC,iBds9DD,YAAA,YY18DD,iBE9CE,YAAA,GAQF,0BACE,UAAA,WAAA,WAAA,WAAA,UAAA,UAAA,UAAA,UAAA,UAAA,UAAA,UAAA,Udu/DD,MAAA,Kcv/DC,Wd0/DD,MAAA,Kc1/DC,Wd6/DD,MAAA,ac7/DC,WdggED,MAAA,achgEC,UdmgED,MAAA,IcngEC,UdsgED,MAAA,actgEC,UdygED,MAAA,aczgEC,Ud4gED,MAAA,Ic5gEC,Ud+gED,MAAA,ac/gEC,UdkhED,MAAA,aclhEC,UdqhED,MAAA,IcrhEC,UdwhED,MAAA,aczgEC,Ud4gED,MAAA,Yc5gEC,gBd+gED,MAAA,Kc/gEC,gBdkhED,MAAA,aclhEC,gBdqhED,MAAA,acrhEC,edwhED,MAAA,IcxhEC,ed2hED,MAAA,ac3hEC,ed8hED,MAAA,ac9hEC,ediiED,MAAA,IcjiEC,edoiED,MAAA,acpiEC,eduiED,MAAA,acviEC,ed0iED,MAAA,Ic1iEC,ed6iED,MAAA,acxiEC,ed2iED,MAAA,Yc1jEC,ed6jED,MAAA,Kc7jEC,gBdgkED,KAAA,KchkEC,gBdmkED,KAAA,acnkEC,gBdskED,KAAA,actkEC,edykED,KAAA,IczkEC,ed4kED,KAAA,ac5kEC,ed+kED,KAAA,ac/kEC,edklED,KAAA,IcllEC,edqlED,KAAA,acrlEC,edwlED,KAAA,acxlEC,ed2lED,KAAA,Ic3lEC,ed8lED,KAAA,aczlEC,ed4lED,KAAA,Yc7kEC,edglED,KAAA,KchlEC,kBdmlED,YAAA,KcnlEC,kBdslED,YAAA,actlEC,kBdylED,YAAA,aczlEC,iBd4lED,YAAA,Ic5lEC,iBd+lED,YAAA,ac/lEC,iBdkmED,YAAA,aclmEC,iBdqmED,YAAA,IcrmEC,iBdwmED,YAAA,acxmEC,iBd2mED,YAAA,ac3mEC,iBd8mED,YAAA,Ic9mEC,iBdinED,YAAA,acjnEC,iBdonED,YAAA,YevrED,iBACA,YAAA,GAGA,MACA,iBAAA,YAEA,Qf0rED,YAAA,IexrEC,eAAgB,IAChB,MAAA,Kf0rED,WAAA,KenrEC,GACA,WAAA,KfurED,OezrEC,MAAO,KdosEP,UAAW,KACX,cAAe,KcxrET,mBd2rER,mBc1rEQ,mBAHA,mBACA,mBd2rER,mBDHC,QAAA,IepsEC,YAAa,WAoBX,eAAA,IACA,WAAA,IAAA,MAAA,KArBJ,mBdmtEE,eAAgB,OAChB,cAAe,IAAI,MAAM,KDJ1B,uCCMD,uCcttEA,wCdutEA,wCcnrEI,2CANI,2CfqrEP,WAAA,Ee1qEG,mBf6qEH,WAAA,IAAA,MAAA,KCWD,cACE,iBAAkB,KchqEpB,6BdmqEA,6BclqEE,6BAZM,6BfuqEP,6BCMD,6BDHC,QAAA,ICWD,gBACE,OAAQ,IAAI,MAAM,Kc3qEpB,4Bd8qEA,4Bc9qEA,4BAQQ,4Bf+pEP,4BCMD,4Bc9pEM,OAAA,IAAA,MAAA,KAYF,4BAFJ,4BfqpEC,oBAAA,IexoEG,yCf2oEH,iBAAA,QejoEC,4BACA,iBAAA,QfqoED,uBe/nEG,SAAA,Od0oEF,QAAS,aczoEL,MAAA,KAEA,sBfkoEL,sBgB9wEC,SAAA,OfyxEA,QAAS,WACT,MAAO,KAST,0BetxEE,0BfgxEF,0BAGA,0BezxEM,0BAMJ,0BfixEF,0BAGA,0BACA,0BDNC,0BCAD,0BAGA,0BASE,iBAAkB,QDLnB,sCgBnyEC,sCAAA,oCf0yEF,sCevxEM,sCf4xEJ,iBAAkB,QASpB,2Be3yEE,2BfqyEF,2BAGA,2Be9yEM,2BAMJ,2BfsyEF,2BAGA,2BACA,2BDNC,2BCAD,2BAGA,2BASE,iBAAkB,QDLnB,uCgBxzEC,uCAAA,qCf+zEF,uCe5yEM,uCfizEJ,iBAAkB,QASpB,wBeh0EE,wBf0zEF,wBAGA,wBen0EM,wBAMJ,wBf2zEF,wBAGA,wBACA,wBDNC,wBCAD,wBAGA,wBASE,iBAAkB,QDLnB,oCgB70EC,oCAAA,kCfo1EF,oCej0EM,oCfs0EJ,iBAAkB,QASpB,2Ber1EE,2Bf+0EF,2BAGA,2Bex1EM,2BAMJ,2Bfg1EF,2BAGA,2BACA,2BDNC,2BCAD,2BAGA,2BASE,iBAAkB,QDLnB,uCgBl2EC,uCAAA,qCfy2EF,uCet1EM,uCf21EJ,iBAAkB,QASpB,0Be12EE,0Bfo2EF,0BAGA,0Be72EM,0BAMJ,0Bfq2EF,0BAGA,0BACA,0BDNC,0BCAD,0BAGA,0BASE,iBAAkB,QDLnB,sCejtEC,sCADF,oCdytEA,sCe32EM,sCDoJJ,iBAAA,QA6DF,kBACE,WAAY,KA3DV,WAAA,KAEA,oCACA,kBACA,MAAA,KfqtED,cAAA,Ke9pEC,WAAY,OAnDV,mBAAA,yBfotEH,OAAA,IAAA,MAAA,KCWD,yBACE,cAAe,Ec7qEjB,qCdgrEA,qCcltEI,qCARM,qCfmtET,qCCMD,qCDHC,YAAA,OCWD,kCACE,OAAQ,EcxrEV,0Dd2rEA,0Dc3rEA,0DAzBU,0Df6sET,0DCMD,0DAME,YAAa,EchsEf,yDdmsEA,yDcnsEA,yDArBU,yDfitET,yDCMD,yDAME,aAAc,EDLjB,yDe3sEW,yDEzNV,yDjBm6EC,yDiBl6ED,cAAA,GAMA,SjBm6ED,UAAA,EiBh6EC,QAAS,EACT,OAAA,EACA,OAAA,EAEA,OACA,QAAA,MACA,MAAA,KACA,QAAA,EACA,cAAA,KACA,UAAA,KjBk6ED,YAAA,QiB/5EC,MAAO,KACP,OAAA,EACA,cAAA,IAAA,MAAA,QAEA,MjBi6ED,QAAA,aiBt5EC,UAAW,Kb4BX,cAAA,IACG,YAAA,IJ83EJ,mBiBt5EC,mBAAoB,WhBi6EjB,gBAAiB,WgB/5EpB,WAAA,WjB05ED,qBiBx5EC,kBAGA,OAAQ,IAAI,EAAE,EACd,WAAA,MjBu5ED,YAAA,OiBl5EC,iBACA,QAAA,MAIF,kBhB45EE,QAAS,MgB15ET,MAAA,KAIF,iBAAA,ahB25EE,OAAQ,KIh+ER,uBL29ED,2BK19EC,wBY2EA,QAAS,KAAK,OACd,QAAA,IAAA,KAAA,yBACA,eAAA,KAEA,OACA,QAAA,MjBi5ED,YAAA,IiBv3EC,UAAW,KACX,YAAA,WACA,MAAA,KAEA,cACA,QAAA,MACA,MAAA,KACA,OAAA,KACA,QAAA,IAAA,KACA,UAAA,KACA,YAAA,WACA,MAAA,KbxDA,iBAAA,KACQ,iBAAA,KAyHR,OAAA,IAAA,MAAA,KACK,cAAA,IACG,mBAAA,MAAA,EAAA,IAAA,IAAA,iBJ0zET,WAAA,MAAA,EAAA,IAAA,IAAA,iBkBl8EC,mBAAA,aAAA,YAAA,KAAA,mBAAA,YAAA,KACE,cAAA,aAAA,YAAA,KAAA,WAAA,YAAA,KACA,WAAA,aAAA,YAAA,KAAA,WAAA,YAAA,KdWM,oBJ27ET,aAAA,QI15EC,QAAA,EACE,mBAAA,MAAA,EAAA,IAAA,IAAA,iBAAA,EAAA,EAAA,IAAA,qBACA,WAAA,MAAA,EAAA,IAAA,IAAA,iBAAA,EAAA,EAAA,IAAA,qBAEF,gCAA0B,MAAA,KJ65E3B,QAAA,EI55EiC,oCJ+5EjC,MAAA,KiBl4EG,yCACA,MAAA,KAQF,0BhBw4EA,iBAAkB,YAClB,OAAQ,EgBr4EN,wBjB+3EH,wBiB53EC,iChBu4EA,iBAAkB,KgBr4EhB,QAAA,EAIF,wBACE,iCjB43EH,OAAA,YiB/2EC,sBjBk3ED,OAAA,KiBh2EG,mBhB42EF,mBAAoB,KAEtB,qDgB72EM,8BjBs2EH,8BiBn2EC,wCAAA,+BhB+2EA,YAAa,KgB72EX,iCjB22EH,iCiBx2EC,2CAAA,kChB42EF,0BACA,0BACA,oCACA,2BAKE,YAAa,KgBl3EX,iCjBg3EH,iCACF,2CiBt2EC,kChBy2EA,0BACA,0BACA,oCACA,2BgB32EA,YAAA,MhBm3EF,YgBz2EE,cAAA,KAGA,UADA,OjBm2ED,SAAA,SiBv2EC,QAAS,MhBk3ET,WAAY,KgB12EV,cAAA,KAGA,gBADA,aAEA,WAAA,KjBm2EH,aAAA,KiBh2EC,cAAe,EhB22Ef,YAAa,IACb,OAAQ,QgBt2ER,+BjBk2ED,sCiBp2EC,yBACA,gCAIA,SAAU,ShB02EV,WAAY,MgBx2EZ,YAAA,MAIF,oBAAA,cAEE,WAAA,KAGA,iBADA,cAEA,SAAA,SACA,QAAA,aACA,aAAA,KjB+1ED,cAAA,EiB71EC,YAAa,IhBw2Eb,eAAgB,OgBt2EhB,OAAA,QAUA,kCjBs1ED,4BCWC,WAAY,EACZ,YAAa,KgBz1Eb,wCAAA,qCjBq1ED,8BCOD,+BgBl2EI,2BhBi2EJ,4BAME,OAAQ,YDNT,0BiBz1EG,uBAMF,oCAAA,iChB+1EA,OAAQ,YDNT,yBiBt1EK,sBAaJ,mCAFF,gCAGE,OAAA,YAGA,qBjB20ED,WAAA,KiBz0EC,YAAA,IhBo1EA,eAAgB,IgBl1Ed,cAAA,EjB40EH,8BiB9zED,8BCnQE,cAAA,EACA,aAAA,EAEA,UACA,OAAA,KlBokFD,QAAA,IAAA,KkBlkFC,UAAA,KACE,YAAA,IACA,cAAA,IAGF,gBjB4kFA,OAAQ,KiB1kFN,YAAA,KD2PA,0BAFJ,kBAGI,OAAA,KAEA,6BACA,OAAA,KjB20EH,QAAA,IAAA,KiBj1EC,UAAW,KAST,YAAA,IACA,cAAA,IAVJ,mChBg2EE,OAAQ,KgBl1EN,YAAA,KAGA,6CAjBJ,qCAkBI,OAAA,KAEA,oCACA,OAAA,KjB20EH,WAAA,KiBv0EC,QAAS,IAAI,KC/Rb,UAAA,KACA,YAAA,IAEA,UACA,OAAA,KlBymFD,QAAA,KAAA,KkBvmFC,UAAA,KACE,YAAA,UACA,cAAA,IAGF,gBjBinFA,OAAQ,KiB/mFN,YAAA,KDuRA,0BAFJ,kBAGI,OAAA,KAEA,6BACA,OAAA,KjBo1EH,QAAA,KAAA,KiB11EC,UAAW,KAST,YAAA,UACA,cAAA,IAVJ,mChBy2EE,OAAQ,KgB31EN,YAAA,KAGA,6CAjBJ,qCAkBI,OAAA,KAEA,oCACA,OAAA,KjBo1EH,WAAA,KiB30EC,QAAS,KAAK,KAEd,UAAA,KjB40ED,YAAA,UiBx0EG,cjB20EH,SAAA,SiBt0EC,4BACA,cAAA,OAEA,uBACA,SAAA,SACA,IAAA,EACA,MAAA,EACA,QAAA,EACA,QAAA,MACA,MAAA,KjBy0ED,OAAA,KiBv0EC,YAAa,KhBk1Eb,WAAY,OACZ,eAAgB,KDLjB,oDiBz0EC,uCADA,iCAGA,MAAO,KhBk1EP,OAAQ,KACR,YAAa,KDLd,oDiBz0EC,uCADA,iCAKA,MAAO,KhBg1EP,OAAQ,KACR,YAAa,KAKf,uBAEA,8BAJA,4BADA,yBAEA,oBAEA,2BDNC,4BkBvuFG,mCAJA,yBD0ZJ,gCbvWE,MAAA,QJ6rFD,2BkB1uFG,aAAA,QACE,mBAAA,MAAA,EAAA,IAAA,IAAA,iBd4CJ,WAAA,MAAA,EAAA,IAAA,IAAA,iBJksFD,iCiB31EC,aAAc,QC5YZ,mBAAA,MAAA,EAAA,IAAA,IAAA,iBAAA,EAAA,EAAA,IAAA,QACA,WAAA,MAAA,EAAA,IAAA,IAAA,iBAAA,EAAA,EAAA,IAAA,QlB2uFH,gCiBh2EC,MAAO,QCtYL,iBAAA,QlByuFH,aAAA,QCWD,oCACE,MAAO,QAKT,uBAEA,8BAJA,4BADA,yBAEA,oBAEA,2BDNC,4BkBrwFG,mCAJA,yBD6ZJ,gCb1WE,MAAA,QJ2tFD,2BkBxwFG,aAAA,QACE,mBAAA,MAAA,EAAA,IAAA,IAAA,iBd4CJ,WAAA,MAAA,EAAA,IAAA,IAAA,iBJguFD,iCiBt3EC,aAAc,QC/YZ,mBAAA,MAAA,EAAA,IAAA,IAAA,iBAAA,EAAA,EAAA,IAAA,QACA,WAAA,MAAA,EAAA,IAAA,IAAA,iBAAA,EAAA,EAAA,IAAA,QlBywFH,gCiB33EC,MAAO,QCzYL,iBAAA,QlBuwFH,aAAA,QCWD,oCACE,MAAO,QAKT,qBAEA,4BAJA,0BADA,uBAEA,kBAEA,yBDNC,0BkBnyFG,iCAJA,uBDgaJ,8Bb7WE,MAAA,QJyvFD,yBkBtyFG,aAAA,QACE,mBAAA,MAAA,EAAA,IAAA,IAAA,iBd4CJ,WAAA,MAAA,EAAA,IAAA,IAAA,iBJ8vFD,+BiBj5EC,aAAc,QClZZ,mBAAA,MAAA,EAAA,IAAA,IAAA,iBAAA,EAAA,EAAA,IAAA,QACA,WAAA,MAAA,EAAA,IAAA,IAAA,iBAAA,EAAA,EAAA,IAAA,QlBuyFH,8BiBt5EC,MAAO,QC5YL,iBAAA,QlBqyFH,aAAA,QiBj5EG,kCjBo5EH,MAAA,QiBj5EG,2CjBo5EH,IAAA,KiBz4EC,mDACA,IAAA,EAEA,YjB44ED,QAAA,MiBzzEC,WAAY,IAwEZ,cAAe,KAtIX,MAAA,QAEA,yBjB23EH,yBiBvvEC,QAAS,aA/HP,cAAA,EACA,eAAA,OjB03EH,2BiB5vEC,QAAS,aAxHP,MAAA,KjBu3EH,eAAA,OiBn3EG,kCACA,QAAA,aAmHJ,0BhB8wEE,QAAS,aACT,eAAgB,OgBv3Ed,wCjBg3EH,6CiBxwED,2CjB2wEC,MAAA,KiB/2EG,wCACA,MAAA,KAmGJ,4BhB0xEE,cAAe,EgBt3Eb,eAAA,OAGA,uBADA,oBjBg3EH,QAAA,aiBtxEC,WAAY,EhBiyEZ,cAAe,EgBv3EX,eAAA,OAsFN,6BAAA,0BAjFI,aAAA,EAiFJ,4CjB+xEC,sCiB12EG,SAAA,SjB62EH,YAAA,EiBl2ED,kDhB82EE,IAAK,GgBp2EL,2BjBi2EH,kCiBl2EG,wBAEA,+BAXF,YAAa,IhBs3Eb,WAAY,EgBr2EV,cAAA,EJviBF,2BIshBF,wBJrhBE,WAAA,KI4jBA,6BAyBA,aAAc,MAnCV,YAAA,MAEA,yBjB01EH,gCACF,YAAA,IiB13EG,cAAe,EAwCf,WAAA,OAwBJ,sDAdQ,MAAA,KjBg1EL,yBACF,+CiBr0EC,YAAA,KAEE,UAAW,MjBw0EZ,yBACF,+CmBt6FG,YAAa,IACf,UAAA,MAGA,KACA,QAAA,aACA,QAAA,IAAA,KAAA,cAAA,EACA,UAAA,KACA,YAAA,IACA,YAAA,WACA,WAAA,OC0CA,YAAA,OACA,eAAA,OACA,iBAAA,aACA,aAAA,ahB+JA,OAAA,QACG,oBAAA,KACC,iBAAA,KACI,gBAAA,KJiuFT,YAAA,KmBz6FG,iBAAA,KlBq7FF,OAAQ,IAAI,MAAM,YAClB,cAAe,IDHhB,kBKx8FC,kBAEA,WACA,kBJ28FF,kBADA,WkBl7FE,QAAA,KAAA,OlBy7FA,QAAS,IAAI,KAAK,yBAClB,eAAgB,KkBn7FhB,WnB46FD,WmB/6FG,WlB27FF,MAAO,KkBt7FL,gBAAA,Kf6BM,YADR,YJq5FD,iBAAA,KmB56FC,QAAA,ElBw7FA,mBAAoB,MAAM,EAAE,IAAI,IAAI,iBAC5B,WAAY,MAAM,EAAE,IAAI,IAAI,iBoBn+FpC,cAGA,ejB8DA,wBACQ,OAAA,YJ65FT,OAAA,kBmB56FG,mBAAA,KlBw7FM,WAAY,KkBt7FhB,QAAA,IASN,eC3DE,yBACA,eAAA,KpBo+FD,aoBj+FC,MAAA,KnB6+FA,iBAAkB,KmB3+FhB,aAAA,KpBq+FH,mBoBn+FO,mBAEN,MAAA,KACE,iBAAA,QACA,aAAA,QpBo+FH,mBoBj+FC,MAAA,KnB6+FA,iBAAkB,QAClB,aAAc,QmBz+FR,oBADJ,oBpBo+FH,mCoBj+FG,MAAA,KnB6+FF,iBAAkB,QAClB,aAAc,QmBz+FN,0BnB++FV,0BAHA,0BmB7+FM,0BnB++FN,0BAHA,0BDFC,yCoB3+FK,yCnB++FN,yCmB1+FE,MAAA,KnBk/FA,iBAAkB,QAClB,aAAc,QmB3+FZ,oBpBm+FH,oBoBn+FG,mCnBg/FF,iBAAkB,KmB5+FV,4BnBi/FV,4BAHA,4BDHC,6BCOD,6BAHA,6BkB99FA,sCClBM,sCnBi/FN,sCmB3+FI,iBAAA,KACA,aAAA,KDcJ,oBC9DE,MAAA,KACA,iBAAA,KpB6hGD,aoB1hGC,MAAA,KnBsiGA,iBAAkB,QmBpiGhB,aAAA,QpB8hGH,mBoB5hGO,mBAEN,MAAA,KACE,iBAAA,QACA,aAAA,QpB6hGH,mBoB1hGC,MAAA,KnBsiGA,iBAAkB,QAClB,aAAc,QmBliGR,oBADJ,oBpB6hGH,mCoB1hGG,MAAA,KnBsiGF,iBAAkB,QAClB,aAAc,QmBliGN,0BnBwiGV,0BAHA,0BmBtiGM,0BnBwiGN,0BAHA,0BDFC,yCoBpiGK,yCnBwiGN,yCmBniGE,MAAA,KnB2iGA,iBAAkB,QAClB,aAAc,QmBpiGZ,oBpB4hGH,oBoB5hGG,mCnByiGF,iBAAkB,KmBriGV,4BnB0iGV,4BAHA,4BDHC,6BCOD,6BAHA,6BkBphGA,sCCrBM,sCnB0iGN,sCmBpiGI,iBAAA,QACA,aAAA,QDkBJ,oBClEE,MAAA,QACA,iBAAA,KpBslGD,aoBnlGC,MAAA,KnB+lGA,iBAAkB,QmB7lGhB,aAAA,QpBulGH,mBoBrlGO,mBAEN,MAAA,KACE,iBAAA,QACA,aAAA,QpBslGH,mBoBnlGC,MAAA,KnB+lGA,iBAAkB,QAClB,aAAc,QmB3lGR,oBADJ,oBpBslGH,mCoBnlGG,MAAA,KnB+lGF,iBAAkB,QAClB,aAAc,QmB3lGN,0BnBimGV,0BAHA,0BmB/lGM,0BnBimGN,0BAHA,0BDFC,yCoB7lGK,yCnBimGN,yCmB5lGE,MAAA,KnBomGA,iBAAkB,QAClB,aAAc,QmB7lGZ,oBpBqlGH,oBoBrlGG,mCnBkmGF,iBAAkB,KmB9lGV,4BnBmmGV,4BAHA,4BDHC,6BCOD,6BAHA,6BkBzkGA,sCCzBM,sCnBmmGN,sCmB7lGI,iBAAA,QACA,aAAA,QDsBJ,oBCtEE,MAAA,QACA,iBAAA,KpB+oGD,UoB5oGC,MAAA,KnBwpGA,iBAAkB,QmBtpGhB,aAAA,QpBgpGH,gBoB9oGO,gBAEN,MAAA,KACE,iBAAA,QACA,aAAA,QpB+oGH,gBoB5oGC,MAAA,KnBwpGA,iBAAkB,QAClB,aAAc,QmBppGR,iBADJ,iBpB+oGH,gCoB5oGG,MAAA,KnBwpGF,iBAAkB,QAClB,aAAc,QmBppGN,uBnB0pGV,uBAHA,uBmBxpGM,uBnB0pGN,uBAHA,uBDFC,sCoBtpGK,sCnB0pGN,sCmBrpGE,MAAA,KnB6pGA,iBAAkB,QAClB,aAAc,QmBtpGZ,iBpB8oGH,iBoB9oGG,gCnB2pGF,iBAAkB,KmBvpGV,yBnB4pGV,yBAHA,yBDHC,0BCOD,0BAHA,0BkB9nGA,mCC7BM,mCnB4pGN,mCmBtpGI,iBAAA,QACA,aAAA,QD0BJ,iBC1EE,MAAA,QACA,iBAAA,KpBwsGD,aoBrsGC,MAAA,KnBitGA,iBAAkB,QmB/sGhB,aAAA,QpBysGH,mBoBvsGO,mBAEN,MAAA,KACE,iBAAA,QACA,aAAA,QpBwsGH,mBoBrsGC,MAAA,KnBitGA,iBAAkB,QAClB,aAAc,QmB7sGR,oBADJ,oBpBwsGH,mCoBrsGG,MAAA,KnBitGF,iBAAkB,QAClB,aAAc,QmB7sGN,0BnBmtGV,0BAHA,0BmBjtGM,0BnBmtGN,0BAHA,0BDFC,yCoB/sGK,yCnBmtGN,yCmB9sGE,MAAA,KnBstGA,iBAAkB,QAClB,aAAc,QmB/sGZ,oBpBusGH,oBoBvsGG,mCnBotGF,iBAAkB,KmBhtGV,4BnBqtGV,4BAHA,4BDHC,6BCOD,6BAHA,6BkBnrGA,sCCjCM,sCnBqtGN,sCmB/sGI,iBAAA,QACA,aAAA,QD8BJ,oBC9EE,MAAA,QACA,iBAAA,KpBiwGD,YoB9vGC,MAAA,KnB0wGA,iBAAkB,QmBxwGhB,aAAA,QpBkwGH,kBoBhwGO,kBAEN,MAAA,KACE,iBAAA,QACA,aAAA,QpBiwGH,kBoB9vGC,MAAA,KnB0wGA,iBAAkB,QAClB,aAAc,QmBtwGR,mBADJ,mBpBiwGH,kCoB9vGG,MAAA,KnB0wGF,iBAAkB,QAClB,aAAc,QmBtwGN,yBnB4wGV,yBAHA,yBmB1wGM,yBnB4wGN,yBAHA,yBDFC,wCoBxwGK,wCnB4wGN,wCmBvwGE,MAAA,KnB+wGA,iBAAkB,QAClB,aAAc,QmBxwGZ,mBpBgwGH,mBoBhwGG,kCnB6wGF,iBAAkB,KmBzwGV,2BnB8wGV,2BAHA,2BDHC,4BCOD,4BAHA,4BkBxuGA,qCCrCM,qCnB8wGN,qCmBxwGI,iBAAA,QACA,aAAA,QDuCJ,mBACE,MAAA,QACA,iBAAA,KnBkuGD,UmB/tGC,YAAA,IlB2uGA,MAAO,QACP,cAAe,EAEjB,UG5wGE,iBemCE,iBflCM,oBJqwGT,6BmBhuGC,iBAAA,YlB4uGA,mBAAoB,KACZ,WAAY,KkBzuGlB,UAEF,iBAAA,gBnBguGD,gBmB9tGG,aAAA,YnBouGH,gBmBluGG,gBAIA,MAAA,QlB0uGF,gBAAiB,UACjB,iBAAkB,YDNnB,0BmBnuGK,0BAUN,mCATM,mClB8uGJ,MAAO,KmB7yGP,gBAAA,KAGA,mBADA,QpBsyGD,QAAA,KAAA,KmB5tGC,UAAW,KlBwuGX,YAAa,UmBpzGb,cAAA,IAGA,mBADA,QpB6yGD,QAAA,IAAA,KmB/tGC,UAAW,KlB2uGX,YAAa,ImB3zGb,cAAA,IAGA,mBADA,QpBozGD,QAAA,IAAA,ImB9tGC,UAAW,KACX,YAAA,IACA,cAAA,IAIF,WACE,QAAA,MnB8tGD,MAAA,KCYD,sBACE,WAAY,IqB53GZ,6BADF,4BtBq3GC,6BIhsGC,MAAA,KAEQ,MJosGT,QAAA,EsBx3GC,mBAAA,QAAA,KAAA,OACE,cAAA,QAAA,KAAA,OtB03GH,WAAA,QAAA,KAAA,OsBr3GC,StBw3GD,QAAA,EsBt3Ga,UtBy3Gb,QAAA,KsBx3Ga,atB23Gb,QAAA,MsB13Ga,etB63Gb,QAAA,UsBz3GC,kBACA,QAAA,gBlBwKA,YACQ,SAAA,SAAA,OAAA,EAOR,SAAA,OACQ,mCAAA,KAAA,8BAAA,KAGR,2BAAA,KACQ,4BAAA,KAAA,uBAAA,KJ8sGT,oBAAA,KuBx5GC,4BAA6B,OAAQ,WACrC,uBAAA,OAAA,WACA,oBAAA,OAAA,WAEA,OACA,QAAA,aACA,MAAA,EACA,OAAA,EACA,YAAA,IACA,eAAA,OvB05GD,WAAA,IAAA,OuBt5GC,WAAY,IAAI,QtBq6GhB,aAAc,IAAI,MAAM,YsBn6GxB,YAAA,IAAA,MAAA,YAKA,UADF,QvBu5GC,SAAA,SuBj5GC,uBACA,QAAA,EAEA,eACA,SAAA,SACA,IAAA,KACA,KAAA,EACA,QAAA,KACA,QAAA,KACA,MAAA,KACA,UAAA,MACA,QAAA,IAAA,EACA,OAAA,IAAA,EAAA,EACA,UAAA,KACA,WAAA,KACA,WAAA,KnBsBA,iBAAA,KACQ,wBAAA,YmBrBR,gBAAA,YtBk6GA,OsBl6GA,IAAA,MAAA,KvBq5GD,OAAA,IAAA,MAAA,gBuBh5GC,cAAA,IACE,mBAAA,EAAA,IAAA,KAAA,iBACA,WAAA,EAAA,IAAA,KAAA,iBAzBJ,0BCzBE,MAAA,EACA,KAAA,KAEA,wBxBu8GD,OAAA,IuBj7GC,OAAQ,IAAI,EAmCV,SAAA,OACA,iBAAA,QAEA,oBACA,QAAA,MACA,QAAA,IAAA,KACA,MAAA,KvBi5GH,YAAA,IuB34GC,YAAA,WtB25GA,MAAO,KsBz5GL,YAAA,OvB+4GH,0BuB74GG,0BAMF,MAAA,QtBu5GA,gBAAiB,KACjB,iBAAkB,QsBp5GhB,yBAEA,+BADA,+BvB04GH,MAAA,KuBh4GC,gBAAA,KtBg5GA,iBAAkB,QAClB,QAAS,EDZV,2BuB93GC,iCAAA,iCAEE,MAAA,KEzGF,iCF2GE,iCAEA,gBAAA,KvBg4GH,OAAA,YuB33GC,iBAAkB,YAGhB,iBAAA,KvB23GH,OAAA,0DuBt3GG,qBvBy3GH,QAAA,MuBh3GC,QACA,QAAA,EAQF,qBACE,MAAA,EACA,KAAA,KAIF,oBACE,MAAA,KACA,KAAA,EAEA,iBACA,QAAA,MACA,QAAA,IAAA,KvB22GD,UAAA,KuBv2GC,YAAa,WACb,MAAA,KACA,YAAA,OAEA,mBACA,SAAA,MACA,IAAA,EvBy2GD,MAAA,EuBr2GC,OAAQ,EACR,KAAA,EACA,QAAA,IAQF,2BtB+2GE,MAAO,EsB32GL,KAAA,KAEA,eACA,sCvB+1GH,QAAA,GuBt2GC,WAAY,EtBs3GZ,cAAe,IAAI,OsB32GjB,cAAA,IAAA,QAEA,uBvB+1GH,8CuB10GC,IAAK,KAXL,OAAA,KApEA,cAAA,IvB85GC,yBuB11GD,6BA1DA,MAAA,EACA,KAAA,KvBw5GD,kC0BviHG,MAAO,KzBujHP,KAAM,GyBnjHR,W1ByiHD,oB0B7iHC,SAAU,SzB6jHV,QAAS,ayBvjHP,eAAA,OAGA,yB1ByiHH,gBCgBC,SAAU,SACV,MAAO,KyBhjHT,gC1ByiHC,gCCYD,+BAFA,+ByBnjHA,uBANM,uBzB0jHN,sBAFA,sBAQE,QAAS,EyBrjHP,qB1B0iHH,2B0BriHD,2BACE,iC1BuiHD,YAAA,KCgBD,aACE,YAAa,KDZd,kB0B7iHD,wBAAA,0BzB8jHE,MAAO,KDZR,kB0BliHD,wBACE,0B1BoiHD,YAAA,I0B/hHC,yE1BkiHD,cAAA,E2BnlHC,4BACG,YAAA,EDsDL,mEzBgjHE,wBAAyB,E0B/lHzB,2BAAA,E3BolHD,6C0B/hHD,8CACE,uBAAA,E1BiiHD,0BAAA,E0B9hHC,sB1BiiHD,MAAA,KCgBD,8D0BlnHE,cAAA,E3BumHD,mE0B9hHD,oECjEE,wBAAA,EACG,2BAAA,EDqEL,oEzB6iHE,uBAAwB,EyB3iHxB,0BAAA,EAiBF,mCACE,iCACA,QAAA,EAEF,iCACE,cAAA,IACA,aAAA,IAKF,oCtB/CE,cAAA,KACQ,aAAA,KsBkDR,iCtBnDA,mBAAA,MAAA,EAAA,IAAA,IAAA,iBACQ,WAAA,MAAA,EAAA,IAAA,IAAA,iBsByDV,0CACE,mBAAA,K1B0gHD,WAAA,K0BtgHC,YACA,YAAA,EAGF,eACE,aAAA,IAAA,IAAA,E1BwgHD,oBAAA,ECgBD,uBACE,aAAc,EAAE,IAAI,IyB7gHlB,yBACA,+BACA,oC1BkgHH,QAAA,M0BzgHC,MAAO,KAcH,MAAA,K1B8/GL,UAAA,KCgBD,oCACE,MAAO,KyBvgHL,8BACA,oC1B4/GH,oC0Bv/GC,0CACE,WAAA,K1By/GH,YAAA,E2BlqHC,4DACC,cAAA,EAQA,sD3B+pHF,uBAAA,I0Bz/GC,wBAAA,IC/KA,2BAAA,EACC,0BAAA,EAQA,sD3BqqHF,uBAAA,E0B1/GC,wBAAyB,EACzB,2BAAA,I1B4/GD,0BAAA,ICgBD,uE0BzrHE,cAAA,E3B8qHD,4E0Bz/GD,6EC7LE,2BAAA,EACC,0BAAA,EDoMH,6EACE,uBAAA,EACA,wBAAA,EAEA,qB1Bu/GD,QAAA,M0B3/GC,MAAO,KzB2gHP,aAAc,MyBpgHZ,gBAAA,SAEA,0B1Bw/GH,gC0BjgHC,QAAS,WAYP,MAAA,K1Bw/GH,MAAA,G0Bp/GG,qC1Bu/GH,MAAA,KCgBD,+CACE,KAAM,KyBh/GF,gDAFA,6C1By+GL,2D0Bx+GK,wDEzOJ,SAAU,SACV,KAAA,cACA,eAAA,K5BotHD,a4BhtHC,SAAA,SACE,QAAA,MACA,gBAAA,S5BmtHH,0B4B3tHC,MAAO,KAeL,cAAA,EACA,aAAA,EAOA,2BACA,SAAA,S5B0sHH,QAAA,E4BxsHG,MAAA,KACE,MAAA,K5B0sHL,cAAA,ECgBD,iCACE,QAAS,EiBtrHT,8BACA,mCACA,sCACA,OAAA,KlB2qHD,QAAA,KAAA,KkBzqHC,UAAA,KjByrHA,YAAa,UACb,cAAe,IiBxrHb,oClB6qHH,yCkB1qHC,4CjB0rHA,OAAQ,KACR,YAAa,KDTd,8C4BltHD,mDAAA,sD3B6tHA,sCACA,2CiB5rHI,8CjBisHF,OAAQ,KiB7sHR,8BACA,mCACA,sCACA,OAAA,KlBksHD,QAAA,IAAA,KkBhsHC,UAAA,KjBgtHA,YAAa,IACb,cAAe,IiB/sHb,oClBosHH,yCkBjsHC,4CjBitHA,OAAQ,KACR,YAAa,KDTd,8C4BhuHD,mDAAA,sD3B2uHA,sCACA,2CiBntHI,8CjBwtHF,OAAQ,K2B5uHR,2B5BguHD,mB4BhuHC,iB3BivHA,QAAS,W2B5uHX,8D5BguHC,sD4BhuHD,oDAEE,cAAA,EAEA,mB5BkuHD,iB4B7tHC,MAAO,GACP,YAAA,OACA,eAAA,OAEA,mBACA,QAAA,IAAA,KACA,UAAA,KACA,YAAA,IACA,YAAA,EACA,MAAA,K5B+tHD,WAAA,O4B5tHC,iBAAA,KACE,OAAA,IAAA,MAAA,KACA,cAAA,I5B+tHH,4B4B5tHC,QAAA,IAAA,KACE,UAAA,KACA,cAAA,I5B+tHH,4B4BlvHC,QAAS,KAAK,K3BkwHd,UAAW,K2BxuHT,cAAA,IAKJ,wCAAA,qC3BwuHE,WAAY,EAEd,uCACA,+BACA,kC0Bh1HE,6CACG,8CC4GL,6D5BwtHC,wE4BvtHC,wBAAA,E5B0tHD,2BAAA,ECgBD,+BACE,aAAc,EAEhB,sCACA,8B2BnuHA,+D5BytHC,oDCWD,iC0Br1HE,4CACG,6CCiHH,uBAAA,E5B2tHD,0BAAA,E4BrtHC,8BAGA,YAAA,E5ButHD,iB4B3tHC,SAAU,SAUR,UAAA,E5BotHH,YAAA,O4BltHK,sB5BqtHL,SAAA,SCgBD,2BACE,YAAa,K2B3tHb,6BAAA,4B5B+sHD,4B4B5sHK,QAAA,EAGJ,kCAAA,wCAGI,aAAA,K5B+sHL,iC6B72HD,uCACE,QAAA,EACA,YAAA,K7Bg3HD,K6Bl3HC,aAAc,EAOZ,cAAA,EACA,WAAA,KARJ,QAWM,SAAA,SACA,QAAA,M7B+2HL,U6B72HK,SAAA,S5B63HJ,QAAS,M4B33HH,QAAA,KAAA,KAMJ,gB7B02HH,gB6Bz2HK,gBAAA,K7B42HL,iBAAA,KCgBD,mB4Bx3HQ,MAAA,KAGA,yBADA,yB7B62HP,MAAA,K6Br2HG,gBAAA,K5Bq3HF,OAAQ,YACR,iBAAkB,Y4Bl3Hd,aAzCN,mB7Bg5HC,mBwBn5HC,iBAAA,KACA,aAAA,QAEA,kBxBs5HD,OAAA,I6Bt5HC,OAAQ,IAAI,EA0DV,SAAA,O7B+1HH,iBAAA,Q6Br1HC,c7Bw1HD,UAAA,K6Bt1HG,UAEA,cAAA,IAAA,MAAA,KALJ,aASM,MAAA,KACA,cAAA,KAEA,e7Bu1HL,aAAA,I6Bt1HK,YAAA,WACE,OAAA,IAAA,MAAA,Y7Bw1HP,cAAA,IAAA,IAAA,EAAA,ECgBD,qBACE,aAAc,KAAK,KAAK,K4B/1HlB,sBAEA,4BADA,4BAEA,MAAA,K7Bo1HP,OAAA,Q6B/0HC,iBAAA,KAqDA,OAAA,IAAA,MAAA,KA8BA,oBAAA,YAnFA,wBAwDE,MAAA,K7B8xHH,cAAA,E6B5xHK,2BACA,MAAA,KA3DJ,6BAgEE,cAAA,IACA,WAAA,OAYJ,iDA0DE,IAAK,KAjED,KAAA,K7B6xHH,yB6B5tHD,2BA9DM,QAAA,W7B6xHL,MAAA,G6Bt2HD,6BAuFE,cAAA,GAvFF,6B5B23HA,aAAc,EACd,cAAe,IDZhB,kC6BzuHD,wCA3BA,wCATM,OAAA,IAAA,MAAA,K7BkxHH,yB6B9uHD,6B5B8vHE,cAAe,IAAI,MAAM,KACzB,cAAe,IAAI,IAAI,EAAE,EDZ1B,kC6Bj3HD,wC7Bk3HD,wC6Bh3HG,oBAAA,MAIE,c7Bk3HL,MAAA,K6B/2HK,gB7Bk3HL,cAAA,ICgBD,iBACE,YAAa,I4B13HP,uBAQR,6B7Bu2HC,6B6Br2HG,MAAA,K7Bw2HH,iBAAA,Q6Bt2HK,gBACA,MAAA,KAYN,mBACE,WAAA,I7B+1HD,YAAA,E6B51HG,e7B+1HH,MAAA,K6B71HK,kBACA,MAAA,KAPN,oBAYI,cAAA,IACA,WAAA,OAYJ,wCA0DE,IAAK,KAjED,KAAA,K7B81HH,yB6B7xHD,kBA9DM,QAAA,W7B81HL,MAAA,G6Br1HD,oBACA,cAAA,GAIE,oBACA,cAAA,EANJ,yB5B62HE,aAAc,EACd,cAAe,IDZhB,8B6B7yHD,oCA3BA,oCATM,OAAA,IAAA,MAAA,K7Bs1HH,yB6BlzHD,yB5Bk0HE,cAAe,IAAI,MAAM,KACzB,cAAe,IAAI,IAAI,EAAE,EDZ1B,8B6B30HD,oC7B40HD,oC6B10HG,oBAAA,MAGA,uB7B60HH,QAAA,K6Bl0HC,qBF3OA,QAAA,M3BkjID,yB8B3iIC,WAAY,KACZ,uBAAA,EACA,wBAAA,EAEA,Q9B6iID,SAAA,S8BriIC,WAAY,KA8nBZ,cAAe,KAhoBb,OAAA,IAAA,MAAA,Y9B4iIH,yB8B5hIC,QAgnBE,cAAe,K9Bi7GlB,yB8BphIC,eACA,MAAA,MAGA,iBACA,cAAA,KAAA,aAAA,KAEA,WAAA,Q9BqhID,2BAAA,M8BnhIC,WAAA,IAAA,MAAA,YACE,mBAAA,MAAA,EAAA,IAAA,EAAA,qB9BqhIH,WAAA,MAAA,EAAA,IAAA,EAAA,qB8B57GD,oBArlBI,WAAA,KAEA,yBAAA,iB9BqhID,MAAA,K8BnhIC,WAAA,EACE,mBAAA,KACA,WAAA,KAEA,0B9BqhIH,QAAA,gB8BlhIC,OAAA,eACE,eAAA,E9BohIH,SAAA,kBCkBD,oBACE,WAAY,QDZf,sC8BlhIK,mC9BihIH,oC8B5gIC,cAAe,E7B+hIf,aAAc,G6Bp+GlB,sCAnjBE,mC7B4hIA,WAAY,MDdX,4D8BtgID,sC9BugID,mCCkBG,WAAY,O6B9gId,kCANE,gC9BygIH,4B8B1gIG,0BAuiBF,aAAc,M7Bs/Gd,YAAa,MAEf,yBDZC,kC8B9gIK,gC9B6gIH,4B8B9gIG,0BAcF,aAAc,EAChB,YAAA,GAMF,mBA8gBE,QAAS,KAhhBP,aAAA,EAAA,EAAA,I9BqgIH,yB8BhgIC,mB7BkhIE,cAAe,G6B7gIjB,qBADA,kB9BmgID,SAAA,M8B5/HC,MAAO,EAggBP,KAAM,E7B+gHN,QAAS,KDdR,yB8BhgID,qB9BigID,kB8BhgIC,cAAA,GAGF,kBACE,IAAA,EACA,aAAA,EAAA,EAAA,I9BogID,qB8B7/HC,OAAQ,EACR,cAAA,EACA,aAAA,IAAA,EAAA,EAEA,cACA,MAAA,K9B+/HD,OAAA,K8B7/HC,QAAA,KAAA,K7B+gIA,UAAW,K6B7gIT,YAAA,KAIA,oBAbJ,oB9B2gIC,gBAAA,K8B1/HG,kB7B6gIF,QAAS,MDdR,yBACF,iC8Bn/HC,uCACA,YAAA,OAGA,eC9LA,SAAA,SACA,MAAA,MD+LA,QAAA,IAAA,KACA,WAAA,IACA,aAAA,KACA,cAAA,I9Bs/HD,iBAAA,Y8Bl/HC,iBAAA,KACE,OAAA,IAAA,MAAA,Y9Bo/HH,cAAA,I8B/+HG,qBACA,QAAA,EAEA,yB9Bk/HH,QAAA,M8BxgIC,MAAO,KAyBL,OAAA,I9Bk/HH,cAAA,I8BvjHD,mCAvbI,WAAA,I9Bm/HH,yB8Bz+HC,eACA,QAAA,MAGE,YACA,OAAA,MAAA,M9B4+HH,iB8B/8HC,YAAA,KA2YA,eAAgB,KAjaZ,YAAA,KAEA,yBACA,iCACA,SAAA,OACA,MAAA,KACA,MAAA,KAAA,WAAA,E9By+HH,iBAAA,Y8B9kHC,OAAQ,E7BimHR,mBAAoB,K6Bz/HhB,WAAA,KAGA,kDAqZN,sC9BqlHC,QAAA,IAAA,KAAA,IAAA,KCmBD,sC6B1/HQ,YAAA,KAmBR,4C9By9HD,4C8B1lHG,iBAAkB,M9B+lHnB,yB8B/lHD,YAtYI,MAAA,K9Bw+HH,OAAA,E8Bt+HK,eACA,MAAA,K9B0+HP,iB8B99HG,YAAa,KACf,eAAA,MAGA,aACA,QAAA,KAAA,K1B9NA,WAAA,IACQ,aAAA,M2B/DR,cAAA,IACA,YAAA,M/B+vID,WAAA,IAAA,MAAA,YiBzuHC,cAAe,IAAI,MAAM,YAwEzB,mBAAoB,MAAM,EAAE,IAAI,EAAE,qBAAyB,EAAE,IAAI,EAAE,qBAtI/D,WAAA,MAAA,EAAA,IAAA,EAAA,qBAAA,EAAA,IAAA,EAAA,qBAEA,yBjB2yHH,yBiBvqHC,QAAS,aA/HP,cAAA,EACA,eAAA,OjB0yHH,2BiB5qHC,QAAS,aAxHP,MAAA,KjBuyHH,eAAA,OiBnyHG,kCACA,QAAA,aAmHJ,0BhBssHE,QAAS,aACT,eAAgB,OgB/yHd,wCjBgyHH,6CiBxrHD,2CjB2rHC,MAAA,KiB/xHG,wCACA,MAAA,KAmGJ,4BhBktHE,cAAe,EgB9yHb,eAAA,OAGA,uBADA,oBjBgyHH,QAAA,aiBtsHC,WAAY,EhBytHZ,cAAe,EgB/yHX,eAAA,OAsFN,6BAAA,0BAjFI,aAAA,EAiFJ,4CjB+sHC,sCiB1xHG,SAAA,SjB6xHH,YAAA,E8BtgID,kDAmWE,IAAK,GAvWH,yBACE,yB9BihIL,cAAA,I8B//HD,oCAoVE,cAAe,GA1Vf,yBACA,aACA,MAAA,KACA,YAAA,E1BzPF,eAAA,EACQ,aAAA,EJswIP,YAAA,EACF,OAAA,E8BtgIG,mBAAoB,KACtB,WAAA,M9B0gID,8B8BtgIC,WAAY,EACZ,uBAAA,EHzUA,wBAAA,EAQA,mDACC,cAAA,E3B40IF,uBAAA,I8BlgIC,wBAAyB,IChVzB,2BAAA,EACA,0BAAA,EDkVA,YCnVA,WAAA,IACA,cAAA,IDqVA,mBCtVA,WAAA,KACA,cAAA,KD+VF,mBChWE,WAAA,KACA,cAAA,KDuWF,aAsSE,WAAY,KA1SV,cAAA,KAEA,yB9BkgID,aACF,MAAA,K8Br+HG,aAAc,KAhBhB,YAAA,MACA,yBE5WA,aF8WE,MAAA,eAFF,cAKI,MAAA,gB9B0/HH,aAAA,M8Bh/HD,4BACA,aAAA,GADF,gBAKI,iBAAA,Q9Bm/HH,aAAA,QCmBD,8B6BngIM,MAAA,KARN,oC9B6/HC,oC8B/+HG,MAAA,Q9Bk/HH,iBAAA,Y8B7+HK,6B9Bg/HL,MAAA,KCmBD,iC6B//HQ,MAAA,KAKF,uC9B4+HL,uCCmBC,MAAO,KACP,iBAAkB,Y6B5/HZ,sCAIF,4C9B0+HL,4CCmBC,MAAO,KACP,iBAAkB,Q6B1/HZ,wCAxCR,8C9BohIC,8C8Bt+HG,MAAA,K9By+HH,iBAAA,YCmBD,+B6Bz/HM,aAAA,KAGA,qCApDN,qC9B8hIC,iBAAA,KCmBD,yC6Bv/HI,iBAAA,KAOE,iCAAA,6B7Bq/HJ,aAAc,Q6Bj/HR,oCAiCN,0C9Bk8HD,0C8B9xHC,MAAO,KA7LC,iBAAA,QACA,yB7Bi/HR,sD6B/+HU,MAAA,KAKF,4D9B49HP,4DCmBC,MAAO,KACP,iBAAkB,Y6B5+HV,2DAIF,iE9B09HP,iECmBC,MAAO,KACP,iBAAkB,Q6B1+HV,6D9B69HX,mEADE,mE8B7jIC,MAAO,KA8GP,iBAAA,aAEE,6B9Bo9HL,MAAA,K8B/8HG,mC9Bk9HH,MAAA,KCmBD,0B6Bl+HM,MAAA,KAIA,gCAAA,gC7Bm+HJ,MAAO,K6Bz9HT,0CARQ,0CASN,mD9B08HD,mD8Bz8HC,MAAA,KAFF,gBAKI,iBAAA,K9B68HH,aAAA,QCmBD,8B6B79HM,MAAA,QARN,oC9Bu9HC,oC8Bz8HG,MAAA,K9B48HH,iBAAA,Y8Bv8HK,6B9B08HL,MAAA,QCmBD,iC6Bz9HQ,MAAA,QAKF,uC9Bs8HL,uCCmBC,MAAO,KACP,iBAAkB,Y6Bt9HZ,sCAIF,4C9Bo8HL,4CCmBC,MAAO,KACP,iBAAkB,Q6Bp9HZ,wCAxCR,8C9B8+HC,8C8B/7HG,MAAA,K9Bk8HH,iBAAA,YCmBD,+B6Bl9HM,aAAA,KAGA,qCArDN,qC9Bw/HC,iBAAA,KCmBD,yC6Bh9HI,iBAAA,KAME,iCAAA,6B7B+8HJ,aAAc,Q6B38HR,oCAuCN,0C9Bs5HD,0C8B93HC,MAAO,KAvDC,iBAAA,QAuDV,yBApDU,kE9By7HP,aAAA,Q8Bt7HO,0D9By7HP,iBAAA,QCmBD,sD6Bz8HU,MAAA,QAKF,4D9Bs7HP,4DCmBC,MAAO,KACP,iBAAkB,Y6Bt8HV,2DAIF,iE9Bo7HP,iECmBC,MAAO,KACP,iBAAkB,Q6Bp8HV,6D9Bu7HX,mEADE,mE8B7hIC,MAAO,KA+GP,iBAAA,aAEE,6B9Bm7HL,MAAA,Q8B96HG,mC9Bi7HH,MAAA,KCmBD,0B6Bj8HM,MAAA,QAIA,gCAAA,gC7Bk8HJ,MAAO,KgC1kJT,0CH0oBQ,0CGzoBN,mDjC2jJD,mDiC1jJC,MAAA,KAEA,YACA,QAAA,IAAA,KjC8jJD,cAAA,KiCnkJC,WAAY,KAQV,iBAAA,QjC8jJH,cAAA,IiC3jJK,eACA,QAAA,ajC+jJL,yBiC3kJC,QAAS,EAAE,IAkBT,MAAA,KjC4jJH,QAAA,SkC/kJC,oBACA,MAAA,KAEA,YlCklJD,QAAA,akCtlJC,aAAc,EAOZ,OAAA,KAAA,ElCklJH,cAAA,ICmBD,eiClmJM,QAAA,OAEA,iBACA,oBACA,SAAA,SACA,MAAA,KACA,QAAA,IAAA,KACA,YAAA,KACA,YAAA,WlCmlJL,MAAA,QkCjlJG,gBAAA,KjComJF,iBAAkB,KiCjmJZ,OAAA,IAAA,MAAA,KPVH,6B3B8lJJ,gCkChlJG,YAAA,EjCmmJF,uBAAwB,I0B1nJxB,0BAAA,I3B4mJD,4BkC3kJG,+BjC8lJF,wBAAyB,IACzB,2BAA4B,IiC3lJxB,uBAFA,uBAGA,0BAFA,0BlCilJL,QAAA,EkCzkJG,MAAA,QjC4lJF,iBAAkB,KAClB,aAAc,KAEhB,sBiC1lJM,4BAFA,4BjC6lJN,yBiC1lJM,+BAFA,+BAGA,QAAA,ElC8kJL,MAAA,KkCroJC,OAAQ,QjCwpJR,iBAAkB,QAClB,aAAc,QiCtlJV,wBAEA,8BADA,8BjCulJN,2BiCzlJM,iCjC0lJN,iCDZC,MAAA,KkClkJC,OAAQ,YjCqlJR,iBAAkB,KkChqJd,aAAA,KAEA,oBnCipJL,uBmC/oJG,QAAA,KAAA,KlCkqJF,UAAW,K0B7pJX,YAAA,U3B+oJD,gCmC9oJG,mClCiqJF,uBAAwB,I0B1qJxB,0BAAA,I3B4pJD,+BkC7kJD,kCjCgmJE,wBAAyB,IkChrJrB,2BAAA,IAEA,oBnCiqJL,uBmC/pJG,QAAA,IAAA,KlCkrJF,UAAW,K0B7qJX,YAAA,I3B+pJD,gCmC9pJG,mClCirJF,uBAAwB,I0B1rJxB,0BAAA,I3B4qJD,+BoC9qJD,kCACE,wBAAA,IACA,2BAAA,IAEA,OpCgrJD,aAAA,EoCprJC,OAAQ,KAAK,EAOX,WAAA,OpCgrJH,WAAA,KCmBD,UmChsJM,QAAA,OAEA,YACA,eACA,QAAA,apCirJL,QAAA,IAAA,KoC/rJC,iBAAkB,KnCktJlB,OAAQ,IAAI,MAAM,KmC/rJd,cAAA,KAnBN,kBpCosJC,kBCmBC,gBAAiB,KmC5rJb,iBAAA,KA3BN,eAAA,kBAkCM,MAAA,MAlCN,mBAAA,sBnCguJE,MAAO,KmCrrJH,mBAEA,yBADA,yBpCwqJL,sBqCrtJC,MAAO,KACP,OAAA,YACA,iBAAA,KAEA,OACA,QAAA,OACA,QAAA,KAAA,KAAA,KACA,UAAA,IACA,YAAA,IACA,YAAA,EACA,MAAA,KrCutJD,WAAA,OqCntJG,YAAA,OpCsuJF,eAAgB,SoCpuJZ,cAAA,MrCutJL,cqCrtJK,cAKJ,MAAA,KACE,gBAAA,KrCktJH,OAAA,QqC7sJG,aACA,QAAA,KAOJ,YCtCE,SAAA,StCkvJD,IAAA,KCmBD,eqChwJM,iBAAA,KALJ,2BD0CF,2BrC+sJC,iBAAA,QCmBD,eqCvwJM,iBAAA,QALJ,2BD8CF,2BrCktJC,iBAAA,QCmBD,eqC9wJM,iBAAA,QALJ,2BDkDF,2BrCqtJC,iBAAA,QCmBD,YqCrxJM,iBAAA,QALJ,wBDsDF,wBrCwtJC,iBAAA,QCmBD,eqC5xJM,iBAAA,QALJ,2BD0DF,2BrC2tJC,iBAAA,QCmBD,cqCnyJM,iBAAA,QCDJ,0BADF,0BAEE,iBAAA,QAEA,OACA,QAAA,aACA,UAAA,KACA,QAAA,IAAA,IACA,UAAA,KACA,YAAA,IACA,YAAA,EACA,MAAA,KACA,WAAA,OvCwxJD,YAAA,OuCrxJC,eAAA,OACE,iBAAA,KvCuxJH,cAAA,KuClxJG,aACA,QAAA,KAGF,YtCqyJA,SAAU,SsCnyJR,IAAA,KAMA,0BvC+wJH,eCmBC,IAAK,EsChyJD,QAAA,IAAA,IvCmxJL,cuCjxJK,cAKJ,MAAA,KtC+xJA,gBAAiB,KsC7xJf,OAAA,QvC+wJH,+BuC3wJC,4BACE,MAAA,QvC6wJH,iBAAA,KuCzwJG,wBvC4wJH,MAAA,MuCxwJG,+BvC2wJH,aAAA,IwCp0JC,uBACA,YAAA,IAEA,WACA,YAAA,KxCu0JD,eAAA,KwC50JC,cAAe,KvC+1Jf,MAAO,QuCt1JL,iBAAA,KAIA,eAbJ,cAcI,MAAA,QxCu0JH,awCr1JC,cAAe,KAmBb,UAAA,KxCq0JH,YAAA,ICmBD,cuCn1JI,iBAAA,QAEA,sBxCo0JH,4BwC91JC,cAAe,KA8Bb,aAAA,KxCm0JH,cAAA,IwChzJD,sBAfI,UAAA,KxCo0JD,oCwCj0JC,WvCo1JA,YAAa,KuCl1JX,eAAA,KxCo0JH,sBwC1zJD,4BvC60JE,cAAe,KuCj1Jb,aAAA,KC5CJ,ezC+2JD,cyC92JC,UAAA,MAGA,WACA,QAAA,MACA,QAAA,IACA,cAAA,KrCiLA,YAAA,WACK,iBAAA,KACG,OAAA,IAAA,MAAA,KJisJT,cAAA,IyC33JC,mBAAoB,OAAO,IAAI,YxC84J1B,cAAe,OAAO,IAAI,YwCj4J7B,WAAA,OAAA,IAAA,YAKF,iBzC82JD,eCmBC,aAAc,KACd,YAAa,KwC13JX,mBA1BJ,kBzCq4JC,kByC12JG,aAAA,QCzBJ,oBACE,QAAA,IACA,MAAA,KAEA,O1Cy4JD,QAAA,K0C74JC,cAAe,KAQb,OAAA,IAAA,MAAA,YAEA,cAAA,IAVJ,UAeI,WAAA,E1Cq4JH,MAAA,QCmBD,mByCl5JI,YAAA,IArBJ,SAyBI,U1Ck4JH,cAAA,ECmBD,WyC34JE,WAAA,IAFF,mBAAA,mBAMI,cAAA,KAEA,0BACA,0B1C43JH,SAAA,S0Cp3JC,IAAK,KCvDL,MAAA,MACA,MAAA,Q3C+6JD,e0Cz3JC,MAAO,QClDL,iBAAA,Q3C86JH,aAAA,Q2C36JG,kB3C86JH,iBAAA,Q2Ct7JC,2BACA,MAAA,Q3C07JD,Y0Ch4JC,MAAO,QCtDL,iBAAA,Q3Cy7JH,aAAA,Q2Ct7JG,e3Cy7JH,iBAAA,Q2Cj8JC,wBACA,MAAA,Q3Cq8JD,e0Cv4JC,MAAO,QC1DL,iBAAA,Q3Co8JH,aAAA,Q2Cj8JG,kB3Co8JH,iBAAA,Q2C58JC,2BACA,MAAA,Q3Cg9JD,c0C94JC,MAAO,QC9DL,iBAAA,Q3C+8JH,aAAA,Q2C58JG,iB3C+8JH,iBAAA,Q4Ch9JC,0BAAQ,MAAA,QACR,wCAAQ,K5Cs9JP,oBAAA,KAAA,E4Cl9JD,GACA,oBAAA,EAAA,GACA,mCAAQ,K5Cw9JP,oBAAA,KAAA,E4C19JD,GACA,oBAAA,EAAA,GACA,gCAAQ,K5Cw9JP,oBAAA,KAAA,E4Ch9JD,GACA,oBAAA,EAAA,GAGA,UACA,OAAA,KxCsCA,cAAA,KACQ,SAAA,OJ86JT,iBAAA,Q4Ch9JC,cAAe,IACf,mBAAA,MAAA,EAAA,IAAA,IAAA,eACA,WAAA,MAAA,EAAA,IAAA,IAAA,eAEA,cACA,MAAA,KACA,MAAA,EACA,OAAA,KACA,UAAA,KxCyBA,YAAA,KACQ,MAAA,KAyHR,WAAA,OACK,iBAAA,QACG,mBAAA,MAAA,EAAA,KAAA,EAAA,gBJk0JT,WAAA,MAAA,EAAA,KAAA,EAAA,gB4C78JC,mBAAoB,MAAM,IAAI,K3Cw+JzB,cAAe,MAAM,IAAI,K4Cv+J5B,WAAA,MAAA,IAAA,KDEF,sBCAE,gCDAF,iBAAA,yK5Ci9JD,iBAAA,oK4C18JC,iBAAiB,iK3Cs+JjB,wBAAyB,KAAK,KGlhK9B,gBAAA,KAAA,KJ4/JD,qBI1/JS,+BwCmDR,kBAAmB,qBAAqB,GAAG,OAAO,SErElD,aAAA,qBAAA,GAAA,OAAA,S9C+gKD,UAAA,qBAAA,GAAA,OAAA,S6C59JG,sBACA,iBAAA,Q7Cg+JH,wC4C38JC,iBAAkB,yKEzElB,iBAAA,oK9CuhKD,iBAAA,iK6Cp+JG,mBACA,iBAAA,Q7Cw+JH,qC4C/8JC,iBAAkB,yKE7ElB,iBAAA,oK9C+hKD,iBAAA,iK6C5+JG,sBACA,iBAAA,Q7Cg/JH,wC4Cn9JC,iBAAkB,yKEjFlB,iBAAA,oK9CuiKD,iBAAA,iK6Cp/JG,qBACA,iBAAA,Q7Cw/JH,uC+C/iKC,iBAAkB,yKAElB,iBAAA,oK/CgjKD,iBAAA,iK+C7iKG,O/CgjKH,WAAA,KC4BD,mB8CtkKE,WAAA,E/C+iKD,O+C3iKD,YACE,SAAA,O/C6iKD,KAAA,E+CziKC,Y/C4iKD,MAAA,Q+CxiKG,c/C2iKH,QAAA,MC4BD,4B8CjkKE,UAAA,KAGF,aAAA,mBAEE,aAAA,KAGF,YAAA,kB9CkkKE,cAAe,K8C3jKjB,YAHE,Y/CuiKD,a+CniKC,QAAA,W/CsiKD,eAAA,I+CliKC,c/CqiKD,eAAA,O+ChiKC,cACA,eAAA,OAMF,eACE,WAAA,EACA,cAAA,ICvDF,YAEE,aAAA,EACA,WAAA,KAQF,YACE,aAAA,EACA,cAAA,KAGA,iBACA,SAAA,SACA,QAAA,MhDglKD,QAAA,KAAA,KgD7kKC,cAAA,KrB3BA,iBAAA,KACC,OAAA,IAAA,MAAA,KqB6BD,6BACE,uBAAA,IrBvBF,wBAAA,I3BymKD,4BgDvkKC,cAAe,E/CmmKf,2BAA4B,I+CjmK5B,0BAAA,IAFF,kBAAA,uBAKI,MAAA,KAIF,2CAAA,gD/CmmKA,MAAO,K+C/lKL,wBAFA,wBhD4kKH,6BgD3kKG,6BAKF,MAAO,KACP,gBAAA,KACA,iBAAA,QAKA,uB/C+lKA,MAAO,KACP,WAAY,K+C5lKV,0BhDskKH,gCgDrkKG,gCALF,MAAA,K/CsmKA,OAAQ,YACR,iBAAkB,KDxBnB,mDgD/kKC,yDAAA,yD/C4mKA,MAAO,QDxBR,gDgDnkKC,sDAAA,sD/CgmKA,MAAO,K+C5lKL,wBAEA,8BADA,8BhDskKH,QAAA,EgD3kKC,MAAA,K/CumKA,iBAAkB,QAClB,aAAc,QAEhB,iDDpBC,wDCuBD,uDADA,uD+C5mKE,8DAYI,6D/C+lKN,uD+C3mKE,8D/C8mKF,6DAKE,MAAO,QDxBR,8CiD7qKG,oDADF,oDAEE,MAAA,QAEA,yBhD0sKF,MAAO,QgDxsKH,iBAAA,QAFF,0BAAA,+BAKI,MAAA,QAGF,mDAAA,wDhD2sKJ,MAAO,QDtBR,gCiDnrKO,gCAGF,qCAFE,qChD8sKN,MAAO,QACP,iBAAkB,QAEpB,iCgD1sKQ,uCAFA,uChD6sKR,sCDtBC,4CiDtrKO,4CArBN,MAAA,KACE,iBAAA,QACA,aAAA,QAEA,sBhDuuKF,MAAO,QgDruKH,iBAAA,QAFF,uBAAA,4BAKI,MAAA,QAGF,gDAAA,qDhDwuKJ,MAAO,QDtBR,6BiDhtKO,6BAGF,kCAFE,kChD2uKN,MAAO,QACP,iBAAkB,QAEpB,8BgDvuKQ,oCAFA,oChD0uKR,mCDtBC,yCiDntKO,yCArBN,MAAA,KACE,iBAAA,QACA,aAAA,QAEA,yBhDowKF,MAAO,QgDlwKH,iBAAA,QAFF,0BAAA,+BAKI,MAAA,QAGF,mDAAA,wDhDqwKJ,MAAO,QDtBR,gCiD7uKO,gCAGF,qCAFE,qChDwwKN,MAAO,QACP,iBAAkB,QAEpB,iCgDpwKQ,uCAFA,uChDuwKR,sCDtBC,4CiDhvKO,4CArBN,MAAA,KACE,iBAAA,QACA,aAAA,QAEA,wBhDiyKF,MAAO,QgD/xKH,iBAAA,QAFF,yBAAA,8BAKI,MAAA,QAGF,kDAAA,uDhDkyKJ,MAAO,QDtBR,+BiD1wKO,+BAGF,oCAFE,oChDqyKN,MAAO,QACP,iBAAkB,QAEpB,gCgDjyKQ,sCAFA,sChDoyKR,qCDtBC,2CiD7wKO,2CDkGN,MAAO,KACP,iBAAA,QACA,aAAA,QAEF,yBACE,WAAA,EACA,cAAA,IE1HF,sBACE,cAAA,EACA,YAAA,IAEA,O9C0DA,cAAA,KACQ,iBAAA,KJgvKT,OAAA,IAAA,MAAA,YkDtyKC,cAAe,IACf,mBAAA,EAAA,IAAA,IAAA,gBlDwyKD,WAAA,EAAA,IAAA,IAAA,gBkDlyKC,YACA,QAAA,KvBnBC,e3B0zKF,QAAA,KAAA,KkDzyKC,cAAe,IAAI,MAAM,YAMvB,uBAAA,IlDsyKH,wBAAA,IkDhyKC,0CACA,MAAA,QAEA,alDmyKD,WAAA,EkDvyKC,cAAe,EjDm0Kf,UAAW,KACX,MAAO,QDtBR,oBkD7xKC,sBjDqzKF,eiD3zKI,mBAKJ,qBAEE,MAAA,QvBvCA,cACC,QAAA,KAAA,K3By0KF,iBAAA,QkDxxKC,WAAY,IAAI,MAAM,KjDozKtB,2BAA4B,IiDjzK1B,0BAAA,IAHJ,mBAAA,mCAMM,cAAA,ElD2xKL,oCkDtxKG,oDjDkzKF,aAAc,IAAI,EiDhzKZ,cAAA,EvBtEL,4D3Bg2KF,4EkDpxKG,WAAA,EjDgzKF,uBAAwB,IiD9yKlB,wBAAA,IvBtEL,0D3B81KF,0EkD7yKC,cAAe,EvB1Df,2BAAA,IACC,0BAAA,IuB0FH,+EAEI,uBAAA,ElDixKH,wBAAA,EkD7wKC,wDlDgxKD,iBAAA,EC4BD,0BACE,iBAAkB,EiDryKpB,8BlD6wKC,ckD7wKD,gCjD0yKE,cAAe,EiD1yKjB,sCAQM,sBlD2wKL,wCC4BC,cAAe,K0Bx5Kf,aAAA,KuByGF,wDlDwxKC,0BC4BC,uBAAwB,IACxB,wBAAyB,IiDrzK3B,yFAoBQ,yFlD2wKP,2DkD5wKO,2DjDwyKN,uBAAwB,IACxB,wBAAyB,IAK3B,wGiDj0KA,wGjD+zKA,wGDtBC,wGCuBD,0EiDh0KA,0EjD8zKA,0EiDtyKU,0EjD8yKR,uBAAwB,IAK1B,uGiD30KA,uGjDy0KA,uGDtBC,uGCuBD,yEiD10KA,yEjDw0KA,yEiD5yKU,yEvB7HR,wBAAA,IuBiGF,sDlDwzKC,yBC4BC,2BAA4B,IAC5B,0BAA2B,IiD3yKrB,qFA1CR,qFAyCQ,wDlDsxKP,wDC4BC,2BAA4B,IAC5B,0BAA2B,IAG7B,oGDtBC,oGCwBD,oGiDj2KA,oGjD81KA,uEiDhzKU,uEjDkzKV,uEiDh2KA,uEjDs2KE,0BAA2B,IAG7B,mGDtBC,mGCwBD,mGiD32KA,mGjDw2KA,sEiDtzKU,sEjDwzKV,sEiD12KA,sEjDg3KE,2BAA4B,IiDrzK1B,0BlD8xKH,qCkDz1KD,0BAAA,qCA+DI,WAAA,IAAA,MAAA,KA/DJ,kDAAA,kDAmEI,WAAA,EAnEJ,uBAAA,yCjD83KE,OAAQ,EiDpzKA,+CjDwzKV,+CiDl4KA,+CjDo4KA,+CAEA,+CANA,+CDjBC,iECoBD,iEiDn4KA,iEjDq4KA,iEAEA,iEANA,iEAWE,YAAa,EiD9zKL,8CjDk0KV,8CiDh5KA,8CjDk5KA,8CAEA,8CANA,8CDjBC,gECoBD,gEiDj5KA,gEjDm5KA,gEAEA,gEANA,gEAWE,aAAc,EAIhB,+CiD95KA,+CjD45KA,+CiDr0KU,+CjDw0KV,iEiD/5KA,iEjD65KA,iEDtBC,iEC6BC,cAAe,EAEjB,8CiDt0KU,8CjDw0KV,8CiDx6KA,8CjDu6KA,gEDtBC,gECwBD,gEiDn0KI,gEACA,cAAA,EAUJ,yBACE,cAAA,ElDsyKD,OAAA,EkDlyKG,aACA,cAAA,KANJ,oBASM,cAAA,ElDqyKL,cAAA,IkDhyKG,2BlDmyKH,WAAA,IC4BD,4BiD3zKM,cAAA,EAKF,wDAvBJ,wDlDwzKC,WAAA,IAAA,MAAA,KkD/xKK,2BlDkyKL,WAAA,EmDrhLC,uDnDwhLD,cAAA,IAAA,MAAA,KmDrhLG,eACA,aAAA,KnDyhLH,8BmD3hLC,MAAA,KAMI,iBAAA,QnDwhLL,aAAA,KmDrhLK,0DACA,iBAAA,KAGJ,qCAEI,MAAA,QnDshLL,iBAAA,KmDviLC,yDnD0iLD,oBAAA,KmDviLG,eACA,aAAA,QnD2iLH,8BmD7iLC,MAAA,KAMI,iBAAA,QnD0iLL,aAAA,QmDviLK,0DACA,iBAAA,QAGJ,qCAEI,MAAA,QnDwiLL,iBAAA,KmDzjLC,yDnD4jLD,oBAAA,QmDzjLG,eACA,aAAA,QnD6jLH,8BmD/jLC,MAAA,QAMI,iBAAA,QnD4jLL,aAAA,QmDzjLK,0DACA,iBAAA,QAGJ,qCAEI,MAAA,QnD0jLL,iBAAA,QmD3kLC,yDnD8kLD,oBAAA,QmD3kLG,YACA,aAAA,QnD+kLH,2BmDjlLC,MAAA,QAMI,iBAAA,QnD8kLL,aAAA,QmD3kLK,uDACA,iBAAA,QAGJ,kCAEI,MAAA,QnD4kLL,iBAAA,QmD7lLC,sDnDgmLD,oBAAA,QmD7lLG,eACA,aAAA,QnDimLH,8BmDnmLC,MAAA,QAMI,iBAAA,QnDgmLL,aAAA,QmD7lLK,0DACA,iBAAA,QAGJ,qCAEI,MAAA,QnD8lLL,iBAAA,QmD/mLC,yDnDknLD,oBAAA,QmD/mLG,cACA,aAAA,QnDmnLH,6BmDrnLC,MAAA,QAMI,iBAAA,QnDknLL,aAAA,QmD/mLK,yDACA,iBAAA,QAGJ,oCAEI,MAAA,QnDgnLL,iBAAA,QoD/nLC,wDACA,oBAAA,QAEA,kBACA,SAAA,SpDkoLD,QAAA,MoDvoLC,OAAQ,EnDmqLR,QAAS,EACT,SAAU,OAEZ,yCmDzpLI,wBADA,yBAEA,yBACA,wBACA,SAAA,SACA,IAAA,EACA,OAAA,EpDkoLH,KAAA,EoD7nLC,MAAO,KACP,OAAA,KpD+nLD,OAAA,EoD1nLC,wBpD6nLD,eAAA,OqDvpLC,uBACA,eAAA,IAEA,MACA,WAAA,KACA,QAAA,KjDwDA,cAAA,KACQ,iBAAA,QJmmLT,OAAA,IAAA,MAAA,QqDlqLC,cAAe,IASb,mBAAA,MAAA,EAAA,IAAA,IAAA,gBACA,WAAA,MAAA,EAAA,IAAA,IAAA,gBAKJ,iBACE,aAAA,KACA,aAAA,gBAEF,SACE,QAAA,KACA,cAAA,ICtBF,SACE,QAAA,IACA,cAAA,IAEA,OACA,MAAA,MACA,UAAA,KjCRA,YAAA,IAGA,YAAA,ErBwrLD,MAAA,KsDhrLC,YAAA,EAAA,IAAA,EAAA,KrD4sLA,OAAQ,kBqD1sLN,QAAA,GjCbF,aiCeE,ajCZF,MAAA,KrBgsLD,gBAAA,KsD5qLC,OAAA,QACE,OAAA,kBACA,QAAA,GAEA,aACA,mBAAA,KtD8qLH,QAAA,EuDnsLC,OAAQ,QACR,WAAA,IvDqsLD,OAAA,EuDhsLC,YACA,SAAA,OAEA,OACA,SAAA,MACA,IAAA,EACA,MAAA,EACA,OAAA,EACA,KAAA,EAIA,QAAA,KvDgsLD,QAAA,KuD7rLC,SAAA,OnD+GA,2BAAA,MACI,QAAA,EAEI,0BAkER,mBAAA,kBAAA,IAAA,SAEK,cAAA,aAAA,IAAA,SACG,WAAA,UAAA,IAAA,SJghLT,kBAAA,kBuDnsLC,cAAA,kBnD2GA,aAAA,kBACI,UAAA,kBAEI,wBJ2lLT,kBAAA,euDvsLK,cAAe,eACnB,aAAA,eACA,UAAA,eAIF,mBACE,WAAA,OACA,WAAA,KvDwsLD,cuDnsLC,SAAU,SACV,MAAA,KACA,OAAA,KAEA,eACA,SAAA,SnDaA,iBAAA,KACQ,wBAAA,YmDZR,gBAAA,YtD+tLA,OsD/tLA,IAAA,MAAA,KAEA,OAAA,IAAA,MAAA,evDqsLD,cAAA,IuDjsLC,QAAS,EACT,mBAAA,EAAA,IAAA,IAAA,eACA,WAAA,EAAA,IAAA,IAAA,eAEA,gBACA,SAAA,MACA,IAAA,EACA,MAAA,EvDmsLD,OAAA,EuDjsLC,KAAA,ElCrEA,QAAA,KAGA,iBAAA,KkCmEA,qBlCtEA,OAAA,iBAGA,QAAA,EkCwEF,mBACE,OAAA,kBACA,QAAA,GAIF,cACE,QAAA,KvDmsLD,cAAA,IAAA,MAAA,QuD9rLC,qBACA,WAAA,KAKF,aACE,OAAA,EACA,YAAA,WAIF,YACE,SAAA,SACA,QAAA,KvD6rLD,cuD/rLC,QAAS,KAQP,WAAA,MACA,WAAA,IAAA,MAAA,QATJ,wBAaI,cAAA,EvDyrLH,YAAA,IuDrrLG,mCvDwrLH,YAAA,KuDlrLC,oCACA,YAAA,EAEA,yBACA,SAAA,SvDqrLD,IAAA,QuDnqLC,MAAO,KAZP,OAAA,KACE,SAAA,OvDmrLD,yBuDhrLD,cnDvEA,MAAA,MACQ,OAAA,KAAA,KmD2ER,eAAY,mBAAA,EAAA,IAAA,KAAA,evDkrLX,WAAA,EAAA,IAAA,KAAA,euD5qLD,UAFA,MAAA,OvDorLD,yBwDl0LC,UACA,MAAA,OCNA,SAEA,SAAA,SACA,QAAA,KACA,QAAA,MACA,YAAA,iBAAA,UAAA,MAAA,WACA,UAAA,KACA,WAAA,OACA,YAAA,IACA,YAAA,WACA,WAAA,KACA,WAAA,MACA,gBAAA,KACA,YAAA,KACA,eAAA,KACA,eAAA,ODHA,WAAA,OnCVA,aAAA,OAGA,UAAA,OrBy1LD,YAAA,OwD90LC,OAAA,iBnCdA,QAAA,ErBg2LD,WAAA,KwDj1LY,YAAmB,OAAA,kBxDq1L/B,QAAA,GwDp1LY,aAAmB,QAAA,IAAA,ExDw1L/B,WAAA,KwDv1LY,eAAmB,QAAA,EAAA,IxD21L/B,YAAA,IwD11LY,gBAAmB,QAAA,IAAA,ExD81L/B,WAAA,IwDz1LC,cACA,QAAA,EAAA,IACA,YAAA,KAEA,eACA,UAAA,MxD41LD,QAAA,IAAA,IwDx1LC,MAAO,KACP,WAAA,OACA,iBAAA,KACA,cAAA,IAEA,exD01LD,SAAA,SwDt1LC,MAAA,EACE,OAAA,EACA,aAAA,YACA,aAAA,MAEA,4BxDw1LH,OAAA,EwDt1LC,KAAA,IACE,YAAA,KACA,aAAA,IAAA,IAAA,EACA,iBAAA,KAEA,iCxDw1LH,MAAA,IwDt1LC,OAAA,EACE,cAAA,KACA,aAAA,IAAA,IAAA,EACA,iBAAA,KAEA,kCxDw1LH,OAAA,EwDt1LC,KAAA,IACE,cAAA,KACA,aAAA,IAAA,IAAA,EACA,iBAAA,KAEA,8BxDw1LH,IAAA,IwDt1LC,KAAA,EACE,WAAA,KACA,aAAA,IAAA,IAAA,IAAA,EACA,mBAAA,KAEA,6BxDw1LH,IAAA,IwDt1LC,MAAA,EACE,WAAA,KACA,aAAA,IAAA,EAAA,IAAA,IACA,kBAAA,KAEA,+BxDw1LH,IAAA,EwDt1LC,KAAA,IACE,YAAA,KACA,aAAA,EAAA,IAAA,IACA,oBAAA,KAEA,oCxDw1LH,IAAA,EwDt1LC,MAAA,IACE,WAAA,KACA,aAAA,EAAA,IAAA,IACA,oBAAA,KAEA,qCxDw1LH,IAAA,E0Dr7LC,KAAM,IACN,WAAA,KACA,aAAA,EAAA,IAAA,IACA,oBAAA,KAEA,SACA,SAAA,SACA,IAAA,EDXA,KAAA,EAEA,QAAA,KACA,QAAA,KACA,UAAA,MACA,QAAA,IACA,YAAA,iBAAA,UAAA,MAAA,WACA,UAAA,KACA,WAAA,OACA,YAAA,IACA,YAAA,WACA,WAAA,KACA,WAAA,MACA,gBAAA,KACA,YAAA,KACA,eAAA,KCAA,eAAA,OAEA,WAAA,OACA,aAAA,OAAA,UAAA,OACA,YAAA,OACA,iBAAA,KACA,wBAAA,YtD8CA,gBAAA,YACQ,OAAA,IAAA,MAAA,KJq5LT,OAAA,IAAA,MAAA,e0Dh8LC,cAAA,IAAY,mBAAA,EAAA,IAAA,KAAA,e1Dm8Lb,WAAA,EAAA,IAAA,KAAA,e0Dl8La,WAAA,KACZ,aAAY,WAAA,MACZ,eAAY,YAAA,KAGd,gBACE,WAAA,KAEA,cACA,YAAA,MAEA,e1Dw8LD,QAAA,IAAA,K0Dr8LC,OAAQ,EACR,UAAA,K1Du8LD,iBAAA,Q0D/7LC,cAAA,IAAA,MAAA,QzD49LA,cAAe,IAAI,IAAI,EAAE,EyDz9LvB,iBACA,QAAA,IAAA,KAEA,gBACA,sB1Di8LH,SAAA,S0D97LC,QAAS,MACT,MAAA,E1Dg8LD,OAAA,E0D97LC,aAAc,YACd,aAAA,M1Di8LD,gB0D57LC,aAAA,KAEE,sBACA,QAAA,GACA,aAAA,KAEA,oB1D87LH,OAAA,M0D77LG,KAAA,IACE,YAAA,MACA,iBAAA,KACA,iBAAA,gBACA,oBAAA,E1Dg8LL,0B0D57LC,OAAA,IACE,YAAA,MACA,QAAA,IACA,iBAAA,KACA,oBAAA,EAEA,sB1D87LH,IAAA,I0D77LG,KAAA,MACE,WAAA,MACA,mBAAA,KACA,mBAAA,gBACA,kBAAA,E1Dg8LL,4B0D57LC,OAAA,MACE,KAAA,IACA,QAAA,IACA,mBAAA,KACA,kBAAA,EAEA,uB1D87LH,IAAA,M0D77LG,KAAA,IACE,YAAA,MACA,iBAAA,EACA,oBAAA,KACA,oBAAA,gB1Dg8LL,6B0D37LC,IAAA,IACE,YAAA,MACA,QAAA,IACA,iBAAA,EACA,oBAAA,KAEA,qB1D67LH,IAAA,I0D57LG,MAAA,MACE,WAAA,MACA,mBAAA,EACA,kBAAA,KACA,kBAAA,gB1D+7LL,2B2DvjMC,MAAO,IACP,OAAA,M3DyjMD,QAAA,I2DtjMC,mBAAoB,EACpB,kBAAA,KAEA,U3DwjMD,SAAA,S2DrjMG,gBACA,SAAA,SvD6KF,MAAA,KACK,SAAA,OJ64LN,sB2DlkMC,SAAU,S1D+lMV,QAAS,K0DjlML,mBAAA,IAAA,YAAA,K3DwjML,cAAA,IAAA,YAAA,K2D9hMC,WAAA,IAAA,YAAA,KvDmKK,4BAFL,0BAGQ,YAAA,EA3JA,qDA+GR,sBAEQ,mBAAA,kBAAA,IAAA,YJi7LP,cAAA,aAAA,IAAA,Y2D5jMG,WAAA,UAAA,IAAA,YvDmHJ,4BAAA,OACQ,oBAAA,OuDjHF,oBAAA,O3D+jML,YAAA,OI/8LD,mCHy+LA,2BGx+LQ,KAAA,EuD5GF,kBAAA,sB3DgkML,UAAA,sBC2BD,kCADA,2BG/+LA,KAAA,EACQ,kBAAA,uBuDtGF,UAAA,uBArCN,6B3DumMD,gC2DvmMC,iC1DkoME,KAAM,E0DrlMN,kBAAA,mB3D+jMH,UAAA,oBAGA,wB2D/mMD,sBAAA,sBAsDI,QAAA,MAEA,wB3D6jMH,KAAA,E2DzjMG,sB3D4jMH,sB2DxnMC,SAAU,SA+DR,IAAA,E3D4jMH,MAAA,KC0BD,sB0DllMI,KAAA,KAnEJ,sBAuEI,KAAA,MAvEJ,2BA0EI,4B3D2jMH,KAAA,E2DljMC,6BACA,KAAA,MAEA,8BACA,KAAA,KtC3FA,kBsC6FA,SAAA,SACA,IAAA,EACA,OAAA,EACA,KAAA,EACA,MAAA,I3DsjMD,UAAA,K2DjjMC,MAAA,KdnGE,WAAA,OACA,YAAA,EAAA,IAAA,IAAA,eACA,iBAAA,cAAA,OAAA,kBACA,QAAA,G7CwpMH,uB2DrjMC,iBAAA,sEACE,iBAAA,iEACA,iBAAA,uFdxGA,iBAAA,kEACA,OAAA,+GACA,kBAAA,SACA,wBACA,MAAA,E7CgqMH,KAAA,K2DvjMC,iBAAA,sE1DmlMA,iBAAiB,iE0DjlMf,iBAAA,uFACA,iBAAA,kEACA,OAAA,+GtCvHF,kBAAA,SsCyFF,wB3DylMC,wBC4BC,MAAO,KACP,gBAAiB,KACjB,OAAQ,kB0DhlMN,QAAA,EACA,QAAA,G3D2jMH,0C2DnmMD,2CA2CI,6BADA,6B1DqlMF,SAAU,S0DhlMR,IAAA,IACA,QAAA,E3DwjMH,QAAA,a2DxmMC,WAAY,MAqDV,0CADA,6B3DyjMH,KAAA,I2D7mMC,YAAa,MA0DX,2CADA,6BAEA,MAAA,IACA,aAAA,MAME,6BADF,6B3DsjMH,MAAA,K2DjjMG,OAAA,KACE,YAAA,M3DmjML,YAAA,E2DxiMC,oCACA,QAAA,QAEA,oCACA,QAAA,QAEA,qBACA,SAAA,SACA,OAAA,K3D2iMD,KAAA,I2DpjMC,QAAS,GAYP,MAAA,IACA,aAAA,EACA,YAAA,KACA,WAAA,OACA,WAAA,KAEA,wBACA,QAAA,aAWA,MAAA,KACA,OAAA,K3DiiMH,OAAA,I2DhkMC,YAAa,OAkCX,OAAA,QACA,iBAAA,OACA,iBAAA,cACA,OAAA,IAAA,MAAA,K3DiiMH,cAAA,K2DzhMC,6BACA,MAAA,KACA,OAAA,KACA,OAAA,EACA,iBAAA,KAEA,kBACA,SAAA,SACA,MAAA,IACA,OAAA,K3D4hMD,KAAA,I2D3hMC,QAAA,GACE,YAAA,K3D6hMH,eAAA,K2Dp/LC,MAAO,KAhCP,WAAA,O1DijMA,YAAa,EAAE,IAAI,IAAI,eAEzB,uB0D9iMM,YAAA,KAEA,oCACA,0C3DshMH,2C2D9hMD,6BAAA,6BAYI,MAAA,K3DshMH,OAAA,K2DliMD,WAAA,M1D8jME,UAAW,KDxBZ,0C2DjhMD,6BACE,YAAA,MAEA,2C3DmhMD,6B2D/gMD,aAAA,M3DkhMC,kBACF,MAAA,I4DhxMC,KAAA,I3D4yME,eAAgB,KAElB,qBACE,OAAQ,MAkBZ,qCADA,sCADA,mBADA,oBAXA,gBADA,iBAOA,uBADA,wBADA,iBADA,kBADA,wBADA,yBASA,mCADA,oC2DvzME,oBAAA,qBAAA,oBAAA,qB3D8zMF,WADA,YAOA,uBADA,wBADA,qBADA,sBADA,cADA,e2Dl0MI,a3Dw0MJ,cDvBC,kB4DhzMG,mB3DwzMJ,WADA,YAwBE,QAAS,MACT,QAAS,IASX,qCADA,mBANA,gBAGA,uBADA,iBADA,wBAIA,mCDhBC,oB6Dl1MC,oB5Dq2MF,W+B/1MA,uBhCu0MC,qB4D/zMG,cChBF,aACA,kB5Dk2MF,W+Bx1ME,MAAO,KhC40MR,cgCz0MC,QAAS,MACT,aAAA,KhC20MD,YAAA,KgCl0MC,YhCq0MD,MAAA,gBgCl0MC,WhCq0MD,MAAA,egCl0MC,MhCq0MD,QAAA,e8D51MC,MACA,QAAA,gBAEA,WACA,WAAA,O9B8BF,WACE,KAAA,EAAA,EAAA,EhCm0MD,MAAA,YgC5zMC,YAAa,KACb,iBAAA,YhC8zMD,OAAA,E+D91MC,Q/Di2MD,QAAA,eC4BD,OACE,SAAU,M+Dt4MV,chE+2MD,MAAA,aC+BD,YADA,YADA,YADA,YAIE,QAAS,e+Dv5MT,kBhEy4MC,mBgEx4MD,yBhEo4MD,kB+Dr1MD,mBA6IA,yB9D+tMA,kBACA,mB8Dp3ME,yB9Dg3MF,kBACA,mBACA,yB+D15MY,QAAA,eACV,yBAAU,YhE64MT,QAAA,gBC4BD,iB+Dv6MU,QAAA,gBhEg5MX,c+D/1MG,QAAS,oB/Dm2MV,c+Dr2MC,c/Ds2MH,QAAA,sB+Dj2MG,yB/Dq2MD,kBACF,QAAA,iB+Dj2MG,yB/Dq2MD,mBACF,QAAA,kBgEn6MC,yBhEu6MC,yBgEt6MD,QAAA,wBACA,+CAAU,YhE26MT,QAAA,gBC4BD,iB+Dr8MU,QAAA,gBhE86MX,c+Dx2MG,QAAS,oB/D42MV,c+D92MC,c/D+2MH,QAAA,sB+D12MG,+C/D82MD,kBACF,QAAA,iB+D12MG,+C/D82MD,mBACF,QAAA,kBgEj8MC,+ChEq8MC,yBgEp8MD,QAAA,wBACA,gDAAU,YhEy8MT,QAAA,gBC4BD,iB+Dn+MU,QAAA,gBhE48MX,c+Dj3MG,QAAS,oB/Dq3MV,c+Dv3MC,c/Dw3MH,QAAA,sB+Dn3MG,gD/Du3MD,kBACF,QAAA,iB+Dn3MG,gD/Du3MD,mBACF,QAAA,kBgE/9MC,gDhEm+MC,yBgEl+MD,QAAA,wBACA,0BAAU,YhEu+MT,QAAA,gBC4BD,iB+DjgNU,QAAA,gBhE0+MX,c+D13MG,QAAS,oB/D83MV,c+Dh4MC,c/Di4MH,QAAA,sB+D53MG,0B/Dg4MD,kBACF,QAAA,iB+D53MG,0B/Dg4MD,mBACF,QAAA,kBgEr/MC,0BhEy/MC,yBACF,QAAA,wBgE1/MC,yBhE8/MC,WACF,QAAA,gBgE//MC,+ChEmgNC,WACF,QAAA,gBgEpgNC,gDhEwgNC,WACF,QAAA,gBAGA,0B+Dn3MC,WA4BE,QAAS,gBC5LX,eAAU,QAAA,eACV,aAAU,ehE4hNT,QAAA,gBC4BD,oB+DtjNU,QAAA,gBhE+hNX,iB+Dj4MG,QAAS,oBAMX,iB/D83MD,iB+Dz2MG,QAAS,sB/D82MZ,qB+Dl4MC,QAAS,e/Dq4MV,a+D/3MC,qBAcE,QAAS,iB/Ds3MZ,sB+Dn4MC,QAAS,e/Ds4MV,a+Dh4MC,sBAOE,QAAS,kB/D83MZ,4B+D/3MC,QAAS,eCpLT,ahEujNC,4BACF,QAAA,wBC6BD,aACE,cACE,QAAS"} \ No newline at end of file diff --git a/webclient/assets/css/hokuyo.css b/webclient/assets/css/hokuyo.css new file mode 100644 index 0000000..f35cfbe --- /dev/null +++ b/webclient/assets/css/hokuyo.css @@ -0,0 +1,28 @@ +.hokuyo{ +} + +.test { + margin: auto; + margin-top: 1em; + margin-bottom: 1em; + width: 1000px; +} + +#mainHokDisp { + overflow-wrap: normal; + /*background-color: #222;*/ + margin: auto; +} + +#singleHokDisps { + margin: auto; + margin-top: 1em; + margin-bottom: 3em; + display: flex; + justify-content: space-between; + width: 1000px; +} + +.singleHokDisp { + /*background-color: #333;*/ +} \ No newline at end of file diff --git a/webclient/assets/css/roscc.css b/webclient/assets/css/roscc.css new file mode 100755 index 0000000..bd1f6fa --- /dev/null +++ b/webclient/assets/css/roscc.css @@ -0,0 +1,122 @@ +html, body { + height: 100%; +} + +.ng-scope { + height: 95%; +} + +#connected, #disconnected { + height: 20px; + width: 20px; + border-radius: 50%; + margin-top: 20px; +} +#disconnected { + background-color: Tomato; +} + +#connected { + background-color: MediumSeaGreen; +} + +.reload { + cursor: pointer; +} + +.panel { + margin-bottom: 5px; +} +.panel-domain { + cursor: pointer; +} +.panel-active { + border: none; +} +.panel-active.panel-default .panel-heading { + color: white; + background-color: #2E3D5F; + border: 1px #2E3D5F solid; + border-radius: 3px; +} +.panel-active.panel-danger .panel-heading { + color: white; + background-color: #8e3b3b; + border: 1px #8e3b3b solid; + border-radius: 3px; +} + + +.sidebar { + position: absolute; + top: 50px; + bottom: 0; + right: 0; + + padding: 0; + border-left: 1px solid #e7e7e7; + border-bottom: 1px solid #e7e7e7; +} +.console { + position: absolute; + top: 0; + bottom: 70px; + display: block; + width: 100%; + + overflow-x: hidden; + overflow-y: auto; +} +.console > table > tr > td { + border-top: none; + border-bottom: 1px solid #ddd; +} +.battery { + position: absolute; + bottom: 0; + padding: 15px 15px 0 15px; + width: 100%; + + background-color: white; + border-top: 1px solid #e7e7e7; +} + +.form-margin { + margin: 0 15px; +} + +.sub-btn { + margin-left: 20px; +} + +.topic-container, .service-container, .action-container { + display: grid; + grid-gap: 20px; + margin: 0px; + height: auto; + grid-row-gap: 0; + +} + +.topic-accord .panel-body { + margin-bottom: -15px; +} + +.col-autofit { + grid-template-columns: repeat(auto-fit, minmax(150px, 1fr)); +} + + +div.smoothie-chart-tooltip { + background: #fff; + padding: 0.5em; + margin-top: 20px; + font-family: consolas; + color: black; + font-size: 20px; + pointer-events: none; +} + +.chart-wrapper { + padding: 10px; +} diff --git a/webclient/assets/css/simulateur.css b/webclient/assets/css/simulateur.css new file mode 100644 index 0000000..f5d9b97 --- /dev/null +++ b/webclient/assets/css/simulateur.css @@ -0,0 +1,48 @@ +#simulateur_container li { + font-size: 18px; +} + +#simulateur_container .utcoupe { + color: red; +} +#simulateur_container .utcoupe.launched { + color: green; +} + +#simulateur_container .config { + color: #fff; + background: #000; + padding: 5px; + border-radius: 3px; +} +#simulateur_container .legend { + width: 200px; + display: inline-block; +} + +#simulateur_container .logger{ + background: black; + color: white; + padding: 3px; +} + +#simulateur_container .logger_head{ + min-width: 6em; + color: DeepPink; + display: inline-block; +} + +#simulateur_container body{ + margin: 0; + width: 100%; + height: 100%; +} + +#simulateur_container canvas { + width: 100%; + height: 100%; +} + +#simulateur_container { + height: 100%; +} diff --git a/webclient/assets/fonts/glyphicons-halflings-regular.eot b/webclient/assets/fonts/glyphicons-halflings-regular.eot new file mode 100755 index 0000000..b93a495 Binary files /dev/null and b/webclient/assets/fonts/glyphicons-halflings-regular.eot differ diff --git a/webclient/assets/fonts/glyphicons-halflings-regular.svg b/webclient/assets/fonts/glyphicons-halflings-regular.svg new file mode 100755 index 0000000..94fb549 --- /dev/null +++ b/webclient/assets/fonts/glyphicons-halflings-regular.svg @@ -0,0 +1,288 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/webclient/assets/fonts/glyphicons-halflings-regular.ttf b/webclient/assets/fonts/glyphicons-halflings-regular.ttf new file mode 100755 index 0000000..1413fc6 Binary files /dev/null and b/webclient/assets/fonts/glyphicons-halflings-regular.ttf differ diff --git a/webclient/assets/fonts/glyphicons-halflings-regular.woff b/webclient/assets/fonts/glyphicons-halflings-regular.woff new file mode 100755 index 0000000..9e61285 Binary files /dev/null and b/webclient/assets/fonts/glyphicons-halflings-regular.woff differ diff --git a/webclient/assets/fonts/glyphicons-halflings-regular.woff2 b/webclient/assets/fonts/glyphicons-halflings-regular.woff2 new file mode 100755 index 0000000..64539b5 Binary files /dev/null and b/webclient/assets/fonts/glyphicons-halflings-regular.woff2 differ diff --git a/webclient/assets/images/ros-logo.svg b/webclient/assets/images/ros-logo.svg new file mode 100755 index 0000000..f808ee4 --- /dev/null +++ b/webclient/assets/images/ros-logo.svg @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + diff --git a/webclient/assets/images/ros.svg b/webclient/assets/images/ros.svg new file mode 100755 index 0000000..41787fc --- /dev/null +++ b/webclient/assets/images/ros.svg @@ -0,0 +1,87 @@ + + + + + + + + + + image/svg+xml + + + + + + + + + + + + diff --git a/webclient/assets/images/screenshot.png b/webclient/assets/images/screenshot.png new file mode 100755 index 0000000..371da9a Binary files /dev/null and b/webclient/assets/images/screenshot.png differ diff --git a/webclient/assets/images/utcoupe-logo.png b/webclient/assets/images/utcoupe-logo.png new file mode 100644 index 0000000..41b9993 Binary files /dev/null and b/webclient/assets/images/utcoupe-logo.png differ diff --git a/webclient/gulpfile.js b/webclient/gulpfile.js new file mode 100755 index 0000000..b34e4a9 --- /dev/null +++ b/webclient/gulpfile.js @@ -0,0 +1,43 @@ +const gulp = require('gulp'); +const babel = require('gulp-babel'); +const concat = require('gulp-concat'); + +function swallowError(error) { + console.log(error.toString()); + + this.emit('end'); +} + +// Babel then concat all app js files into roscc.js +gulp.task('js', function () { + return gulp.src(['app/app.js', 'app/**/*.js']) + .pipe(babel({presets: ['env']})) + .pipe(concat('roscc.js')) + .pipe(gulp.dest('assets/js/')) + .on('error', swallowError); +}); + +// Concat all libs into vendor.js +gulp.task('js-vendor', function () { + return gulp.src([ + 'node_modules/underscore/underscore-min.js', + 'node_modules/angular/angular.min.js', + 'node_modules/angular-animate/angular-animate.min.js', + 'node_modules/angular-route/angular-route.min.js', + 'node_modules/angular-local-storage/dist/angular-local-storage.js', + 'node_modules/angular-ui-bootstrap/dist/ui-bootstrap-tpls.js', + 'node_modules/eventemitter2/lib/eventemitter2.js', + 'node_modules/jquery/dist/jquery.min.js', + 'node_modules/smoothie/smoothie.js', + 'node_modules/roslib/build/roslib.min.js' + ]) + .pipe(concat('vendor.js')) + .pipe(gulp.dest('assets/js/')); +}); + +// Changes will be detected automatically +gulp.task('watch', function () { + gulp.watch('app/**/*.js', ['js']); +}); + +gulp.task('default', ['js-vendor', 'js', 'watch']); diff --git a/webclient/index.html b/webclient/index.html new file mode 100755 index 0000000..1380ab0 --- /dev/null +++ b/webclient/index.html @@ -0,0 +1,24 @@ + + + + + UTCoupe WebClient + + + + + + + + + + + + +
+ + + + + + diff --git a/webclient/package.json b/webclient/package.json new file mode 100644 index 0000000..44d16a4 --- /dev/null +++ b/webclient/package.json @@ -0,0 +1,47 @@ +{ + "name": "utcoupe-webclient-18", + "version": "0.2.0", + "description": "The webclient used for coupe18.", + "repository": { + "type": "git", + "url": "git+https://github.com/utcoupe/coupe18.git" + }, + "author": "milesial", + "license": "BSD-3-Clause", + "dependencies": { + "angular": "^1.6.4", + "angular-animate": "^1.6.4", + "angular-local-storage": "^0.7.1", + "angular-route": "^1.6.4", + "angular-ui-bootstrap": "^2.5.0", + "babel-core": "^6.26.0", + "babel-preset-env": "^1.6.1", + "bootstrap": "^3.3.7", + "eventemitter2": "^5.0.1", + "gulp": "^3.9.1", + "gulp-babel": "^7.0.1", + "gulp-concat": "^2.6.1", + "http-server": "^0.11.1", + "jquery": "^3.0.0", + "roslib": "^0.20.0", + "smoothie": "^1.34.0", + "underscore": "^1.8.3" + }, + "devDependencies": { + "eslint": "^4.19.1", + "eslint-config-airbnb": "^16.1.0", + "eslint-config-angular": "^0.5.0", + "eslint-config-standard": "^11.0.0", + "eslint-plugin-angular": "^3.3.0", + "eslint-plugin-import": "^2.7.0", + "eslint-plugin-jsx-a11y": "^6.0.3", + "eslint-plugin-node": "^6.0.1", + "eslint-plugin-promise": "^3.5.0", + "eslint-plugin-react": "^7.0.1", + "eslint-plugin-standard": "^3.0.1" + }, + "scripts": { + "start": "gulp js && gulp js-vendor && http-server", + "dev": "gulp & http-server -c-1" + } +}