From 4aae0913c14261699e706a17a10ee12da7ceb1f0 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Tue, 10 Jun 2025 14:39:19 +0200 Subject: [PATCH 01/46] chore: better directory structure --- cybership_controller/CMakeLists.txt | 11 + cybership_controller/LICENSE | 674 ++++++++++++++++++ cybership_controller/README.md | 2 + .../position/reference_filter.py | 0 .../velocity/reference_feedforward.py | 0 cybership_controller/package.xml | 22 + cybership_controller/requirements.txt | 1 + .../src/cybership_controller/__init__.py | 0 .../position/reference_filter.py | 170 +++++ .../src/cybership_controller/utils.py | 69 ++ .../velocity/reference_feedforward.py | 130 ++++ .../launch/force_controller.launch.py | 12 +- cybership_tests/CMakeLists.txt | 9 +- .../reference_filter_server.py | 97 +-- .../cybership_tests/velocity_control.py | 192 +---- 15 files changed, 1109 insertions(+), 280 deletions(-) create mode 100644 cybership_controller/CMakeLists.txt create mode 100644 cybership_controller/LICENSE create mode 100644 cybership_controller/README.md create mode 100644 cybership_controller/cybership_controller/position/reference_filter.py create mode 100644 cybership_controller/cybership_controller/velocity/reference_feedforward.py create mode 100644 cybership_controller/package.xml create mode 100644 cybership_controller/requirements.txt create mode 100644 cybership_controller/src/cybership_controller/__init__.py create mode 100644 cybership_controller/src/cybership_controller/position/reference_filter.py create mode 100644 cybership_controller/src/cybership_controller/utils.py create mode 100644 cybership_controller/src/cybership_controller/velocity/reference_feedforward.py diff --git a/cybership_controller/CMakeLists.txt b/cybership_controller/CMakeLists.txt new file mode 100644 index 0000000..a0c8a28 --- /dev/null +++ b/cybership_controller/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.10) +project(cybership_controller) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) + +ament_python_install_package(${PROJECT_NAME} + PACKAGE_DIR src/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/cybership_controller/LICENSE b/cybership_controller/LICENSE new file mode 100644 index 0000000..f288702 --- /dev/null +++ b/cybership_controller/LICENSE @@ -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. + + + Copyright (C) + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + +Also add information on how to contact you by electronic and paper mail. + + If the program does terminal interaction, make it output a short +notice like this when it starts in an interactive mode: + + Copyright (C) + This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, your program's commands +might be different; for a GUI interface, you would use an "about box". + + You should also get your employer (if you work as a programmer) or school, +if any, to sign a "copyright disclaimer" for the program, if necessary. +For more information on this, and how to apply and follow the GNU GPL, see +. + + The GNU General Public License does not permit incorporating your program +into proprietary programs. If your program is a subroutine library, you +may consider it more useful to permit linking proprietary applications with +the library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. But first, please read +. diff --git a/cybership_controller/README.md b/cybership_controller/README.md new file mode 100644 index 0000000..f674a07 --- /dev/null +++ b/cybership_controller/README.md @@ -0,0 +1,2 @@ +# Cybership Controller + diff --git a/cybership_controller/cybership_controller/position/reference_filter.py b/cybership_controller/cybership_controller/position/reference_filter.py new file mode 100644 index 0000000..e69de29 diff --git a/cybership_controller/cybership_controller/velocity/reference_feedforward.py b/cybership_controller/cybership_controller/velocity/reference_feedforward.py new file mode 100644 index 0000000..e69de29 diff --git a/cybership_controller/package.xml b/cybership_controller/package.xml new file mode 100644 index 0000000..f37b06d --- /dev/null +++ b/cybership_controller/package.xml @@ -0,0 +1,22 @@ + + + + cybership_controller + 0.0.0 + TODO: Package description + cem + GPL-3.0-only + + ament_cmake + ament_cmake_python + + rclpy + geometry_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/cybership_controller/requirements.txt b/cybership_controller/requirements.txt new file mode 100644 index 0000000..e92e81b --- /dev/null +++ b/cybership_controller/requirements.txt @@ -0,0 +1 @@ +skadipy @ git+https://github.com/incebellipipo/skadipy.git@master \ No newline at end of file diff --git a/cybership_controller/src/cybership_controller/__init__.py b/cybership_controller/src/cybership_controller/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/cybership_controller/src/cybership_controller/position/reference_filter.py b/cybership_controller/src/cybership_controller/position/reference_filter.py new file mode 100644 index 0000000..12fe3f2 --- /dev/null +++ b/cybership_controller/src/cybership_controller/position/reference_filter.py @@ -0,0 +1,170 @@ +import numpy as np + + +def wrap_to_pi(angle): + r"""Wrap an angle to [-pi, pi]. + + .. math:: + \mathrm{result} = (\,\mathtt{angle} + \pi\,) \bmod (2\pi) - \pi + + Args: + angle (float): Angle in radians. + + Returns: + float: Angle wrapped to the interval [-pi, pi]. + """ + return (angle + np.pi) % (2 * np.pi) - np.pi + + +def Rz(psi): + r"""Compute a 3DOF rotation matrix about the z-axis. + + .. math:: + R_z(\psi)= \begin{pmatrix} + \cos(\psi) & -\sin(\psi) & 0\\[1mm] + \sin(\psi) & \cos(\psi) & 0\\[1mm] + 0 & 0 & 1 + \end{pmatrix} + + Args: + psi (float): Yaw angle in radians. + + Returns: + numpy.ndarray: A 3x3 rotation matrix. + """ + return np.array( + [[np.cos(psi), -np.sin(psi), 0], + [np.sin(psi), np.cos(psi), 0], + [0, 0, 1]] + ) + + +class ThirdOrderReferenceFilter: + """Third-order reference filter for guidance.""" + + def __init__(self, dt, omega=[0.2, 0.2, 0.2], delta=[1.0, 1.0, 1.0], initial_eta=None): + """Initialize the third-order reference filter. + + Args: + dt (float): Time step size. + omega (list of float, optional): Frequency parameters. Defaults to [0.2, 0.2, 0.2]. + delta (list of float, optional): Damping parameters. Defaults to [1.0, 1.0, 1.0]. + initial_eta (list of float, optional): Initial state [x, y, yaw]. Defaults to None. + """ + self._dt = dt + # Keep everything as a float array + self.eta_d = ( + np.zeros(3) if initial_eta is None else np.array( + initial_eta, dtype=float) + ) # [ x, y, yaw ] + self.eta_d_dot = np.zeros(3) + self.eta_d_ddot = np.zeros(3) + self._eta_r = self.eta_d.copy() # reference target (unwrapped) + + # State vector of 9 elements: [ eta, eta_dot, eta_ddot ] + self._x = np.concatenate([self.eta_d, self.eta_d_dot, self.eta_d_ddot]) + + # Gains + self._delta = np.eye(3) + self._w = np.diag(omega) + O3x3 = np.zeros((3, 3)) + self.Ad = np.block( + [ + [O3x3, np.eye(3), O3x3], + [O3x3, O3x3, np.eye(3)], + [ + -self._w**3, + -(2 * self._delta + np.eye(3)) @ self._w**2, + -(2 * self._delta + np.eye(3)) @ self._w, + ], + ] + ) + self.Bd = np.block([[O3x3], [O3x3], [self._w**3]]) + + def get_eta_d(self): + r"""Retrieve the desired pose. + + Returns: + numpy.ndarray: Desired pose as [x, y, yaw]. + """ + return self.eta_d + + def get_eta_d_dot(self): + r"""Retrieve the desired velocity in the inertial frame. + + Returns: + numpy.ndarray: Desired velocity. + """ + return self.eta_d_dot + + def get_eta_d_ddot(self): + r"""Retrieve the desired acceleration in the inertial frame. + + Returns: + numpy.ndarray: Desired acceleration. + """ + return self.eta_d_ddot + + def get_nu_d(self): + r"""Retrieve the desired velocity in the body frame. + + Returns: + numpy.ndarray: Body frame velocity (u, v, r). + """ + psi = self.eta_d[2] + return Rz(psi).T @ self.eta_d_dot + + def set_eta_r(self, eta_r): + r"""Set the reference pose with smooth yaw transition. + + Args: + eta_r (list or numpy.ndarray): Reference pose [x, y, yaw]. + """ + old_yaw = self._eta_r[2] + new_yaw = eta_r[2] + + # Wrap new yaw to [-pi, pi] + new_yaw = wrap_to_pi(new_yaw) + + # Get minimal difference + diff = wrap_to_pi(new_yaw - old_yaw) + continuous_yaw = old_yaw + diff # small step only + + self._eta_r = np.array([eta_r[0], eta_r[1], continuous_yaw]) + + def update(self): + r"""Integrate the filter for one time step. + + The state is updated as follows: + + .. math:: + \dot{x} = A_d x + B_d \eta_r + + .. math:: + x_{\mathrm{new}} = x + dt \cdot \dot{x} + + Updates the internal state based on the reference pose. + """ + x_dot = self.Ad @ self._x + self.Bd @ self._eta_r + self._x += self._dt * x_dot + + # Extract the updated states + self.eta_d = self._x[:3] + self.eta_d_dot = self._x[3:6] + self.eta_d_ddot = self._x[6:] + + +def saturate(x, z): + r"""Apply a smooth saturation to the input. + + .. math:: + \mathrm{saturated\_value} = \frac{x}{\lvert x \rvert + z} + + Args: + x (numpy.ndarray or float): Input value. + z (numpy.ndarray or float): Saturation parameter. + + Returns: + numpy.ndarray or float: Saturated value between -1 and 1. + """ + return x / (np.abs(x) + z) diff --git a/cybership_controller/src/cybership_controller/utils.py b/cybership_controller/src/cybership_controller/utils.py new file mode 100644 index 0000000..24cd8e1 --- /dev/null +++ b/cybership_controller/src/cybership_controller/utils.py @@ -0,0 +1,69 @@ + +import numpy as np + +class ExponentialSmoothing(): + """ + Exponential smoothing class + """ + + def __init__(self, r: float = 0.7) -> None: + """ + Exponential smoothing class constructor + + :param r: A weighting factor in the set [0-1] + """ + self.r = r + self.x = None + self.x_p = None + self.dx_p = None + + def __call__(self, x: np.ndarray) -> np.ndarray: + """ + Exponential smoothing function + + :param x: current value of x + :return: dx + """ + if self.x_p is None or self.dx_p is None: + self.x_p = x + self.dx_p = x + return x + else: + self.dx_p = ExponentialSmoothing.__func(self.r, x, self.x_p, self.dx_p) + self.x_p = x + return self.dx_p + + def reset(self, x: np.ndarray) -> None: + """ + Reset the exponential smoothing + + :param x: current value of x + :return: + """ + self.x_p = x + self.dx_p = x + + @staticmethod + def __func( + r: float, x: np.ndarray, x_p: np.ndarray = None, dx_p: np.ndarray = None + ) -> np.ndarray: + r""" + Exponential smoothing function + + .. math:: + \begin{aligned} + s_{0}&=x_{0}\\ + s_{t}&=\alpha x_{t}+(1-\alpha )s_{t-1},\quad t>0 + \end{aligned} + + + :param r: A weighting factor in the set [0-1] + :param x: current value of x + :param x_p: previous value of x + :param dx_p: last computed filtered x value + :return: + """ + if x_p is None or dx_p is None: + return x + else: + return (1 - r) * dx_p + r * (x - x_p) \ No newline at end of file diff --git a/cybership_controller/src/cybership_controller/velocity/reference_feedforward.py b/cybership_controller/src/cybership_controller/velocity/reference_feedforward.py new file mode 100644 index 0000000..61b6d80 --- /dev/null +++ b/cybership_controller/src/cybership_controller/velocity/reference_feedforward.py @@ -0,0 +1,130 @@ +from typing import Optional, Dict, Any +import numpy as np +from cybership_controller.utils import ExponentialSmoothing + + +class RffVelocityController(): + """Inverse-dynamics velocity controller with PI action and acceleration limiting.""" + + def __init__(self, config: Optional[Dict[str, Any]] = None): + r"""Initialize the RffVelocityController. + + Args: + config (Optional[Dict[str, Any]]): Configuration parameters dictionary. + Defaults to None. Supported keys: + - "M" (array-like): Inertia matrix, defaults to 3x3 identity matrix + - "D" (array-like): Damping matrix, defaults to 3x3 zero matrix + - "Kp" (array-like): Proportional gain matrix, defaults to 3x3 identity matrix + - "Ki" (array-like): Integral gain matrix, defaults to 3x3 zero matrix + - "Kd" (array-like): Derivative gain matrix, defaults to 3x3 zero matrix + - "dt" (float): Time step size in seconds, defaults to 0.01 + - "a_max" (float): Hard acceleration limit, values <= 0 disable limit, defaults to -1.0 + - "I_max" (float): Integral wind-up cap, values <= 0 disable cap, defaults to -1.0 + - "smooth_limit" (bool): Enable smooth limiting behavior, defaults to False + + M = \\text{inertia matrix},\\quad D = \\text{damping matrix},\\quad K_p, K_i, K_d = \\text{gain matrices},\\\\ + dt = \\text{time step size}. + + + Equations: + .. math:: + M = \text{inertia matrix},\quad D = \text{damping matrix},\quad K_p, K_i, K_d = \text{gain matrices},\\ + dt = \text{time step size}. + """ + + self.config = config or {} + # Matrices and gains + self.M: np.ndarray = np.array( + self.config.get("M", np.eye(3)), dtype=float) + self.D: np.ndarray = np.array( + self.config.get("D", np.zeros((3, 3))), dtype=float) + self.Kp: np.ndarray = np.array( + self.config.get("Kp", np.eye(3)), dtype=float) + self.Ki: np.ndarray = np.array( + self.config.get("Ki", np.zeros((3, 3))), dtype=float) + self.Kd: np.ndarray = np.array( + self.config.get("Kd", np.zeros((3, 3))), dtype=float) + self.dt: float = float( + self.config.get("dt", 0.01)) # seconds + + # Pre-compute inverse inertia + self.M_inv = np.linalg.inv(self.M) + + # Limits and behaviours + # hard acc limit (<=0 => off) + self.a_max: float = float(self.config.get("a_max", -1.0)) + # integral wind-up cap (<=0 => off) + self.I_max: float = float(self.config.get("I_max", -1.0)) + self.smooth: bool = bool(self.config.get("smooth_limit", False)) + + # Internal state + # for desired-velocity derivative + self._prev_vd: Optional[np.ndarray] = None + self._int_e: np.ndarray = np.zeros(3) # integral of error dt + + self._prev_tau = None + + def update(self, current_velocity: np.ndarray, desired_velocity: np.ndarray, dt: float) -> np.ndarray: + r"""Compute the force/torque command tau. + + The controller computes tau based on the following equations: + + .. math:: + e = current\_velocity - desired\_velocity + + .. math:: + v_{d\_dot} = \text{smoothed derivative of desired\_velocity} + + .. math:: + e_{dot} = \text{smoothed derivative of } e,\quad \text{where } e = current\_velocity - desired\_velocity + + .. math:: + a_{des} = v_{d\_dot} - K_p \cdot e - K_i \cdot \int e\,dt - K_d \cdot e_{dot} + + .. math:: + \tau = M \cdot a_{des} + D \cdot desired\_velocity + + Args: + current_velocity (np.ndarray): Current measured velocity. + desired_velocity (np.ndarray): Desired velocity. + dt (float): Time step in seconds. + + Raises: + ValueError: If dt is not positive. + + Returns: + np.ndarray: Computed force/torque command tau. + """ + + if dt <= 0.0: + return self._prev_tau if self._prev_tau is not None else np.zeros(3) + + v = current_velocity.reshape(3) + vd = desired_velocity.reshape(3) + + if not hasattr(self, '_vd_dot_smoother'): + r = self.config.get("filter_alpha", 0.7) + self._vd_dot_smoother = ExponentialSmoothing(r=r) + + if not hasattr(self, '_e_dot_smoother'): + r = self.config.get("filter_alpha", 0.7) + self._e_dot_smoother = ExponentialSmoothing(r=r) + + raw_vd_dot = (vd - self._prev_vd) / \ + dt if self._prev_vd is not None else np.zeros(3) + vd_dot = self._vd_dot_smoother(raw_vd_dot) + self._prev_vd = vd.copy() + + e = v - vd + raw_e_dot = (v - self._prev_vd) / \ + dt if self._prev_vd is not None else np.zeros(3) + e_dot = self._e_dot_smoother(raw_e_dot) + + self._int_e = self._int_e + e * dt + self._int_e = np.clip(self._int_e, -self.I_max, self.I_max) + + a_des = vd_dot - self.Kp @ e - self.Ki @ self._int_e - self.Kd @ e_dot + + tau = self.M @ a_des + self.D @ vd + self._prev_tau = tau.copy() + return tau diff --git a/cybership_dp/launch/force_controller.launch.py b/cybership_dp/launch/force_controller.launch.py index 4178171..21fd2f1 100644 --- a/cybership_dp/launch/force_controller.launch.py +++ b/cybership_dp/launch/force_controller.launch.py @@ -44,6 +44,12 @@ def generate_launch_description(): "--vessel-name", launch.substitutions.LaunchConfiguration("vessel_name"), ], + remappings=[ + ( + "control/force/command", + "control/force/command/mux", + ), # Remap to the multiplexer output + ], output="screen", respawn=True, respawn_delay=5, @@ -56,8 +62,10 @@ def generate_launch_description(): executable="mux", name=f"force_mux_{anon()}", arguments=[ - "control/force/command", # Output topic - "control/force/mux", # Input topic to listen to + "control/force/command/mux", # Output topic + "control/force/command", # Input topic to listen to + "control/force/command/velocity", # Input topic to listen to + "control/force/command/position", # Input topic to listen to "--repeat-delay", "0.1" # Optional delay for repeated messages ], output="screen", diff --git a/cybership_tests/CMakeLists.txt b/cybership_tests/CMakeLists.txt index 8ddca9c..ddc1087 100644 --- a/cybership_tests/CMakeLists.txt +++ b/cybership_tests/CMakeLists.txt @@ -19,13 +19,12 @@ find_package(nav_msgs REQUIRED) ament_python_install_package(${PROJECT_NAME}) +# Find all python scripts under the "nodes" directory. +file(GLOB NODE_SCRIPTS ${CMAKE_CURRENT_SOURCE_DIR}/cybership_tests/*.py) + install( PROGRAMS - ${PROJECT_NAME}/four_corner_line_of_sight.py - ${PROJECT_NAME}/four_corner_client.py - ${PROJECT_NAME}/pid_position_server.py - ${PROJECT_NAME}/go_to_client.py - ${PROJECT_NAME}/reference_filter_server.py + ${NODE_SCRIPTS} DESTINATION lib/${PROJECT_NAME} ) diff --git a/cybership_tests/cybership_tests/reference_filter_server.py b/cybership_tests/cybership_tests/reference_filter_server.py index 95c8bc3..70095b2 100755 --- a/cybership_tests/cybership_tests/reference_filter_server.py +++ b/cybership_tests/cybership_tests/reference_filter_server.py @@ -28,6 +28,7 @@ from visualization_msgs.msg import Marker from std_msgs.msg import Float32MultiArray +from cybership_controller.position.reference_filter import ThirdOrderReferenceFilter try: from cybership_interfaces.msg import PerformanceMetrics @@ -62,88 +63,6 @@ def Rz(psi): ) -class ThrdOrderRefFilter: - """Third-order reference filter for guidance.""" - - def __init__(self, dt, omega=[0.2, 0.2, 0.2], delta=[1.0, 1.0, 1.0], initial_eta=None): - self._dt = dt - # Keep everything as a float array - self.eta_d = ( - np.zeros(3) if initial_eta is None else np.array( - initial_eta, dtype=float) - ) # [ x, y, yaw ] - self.eta_d_dot = np.zeros(3) - self.eta_d_ddot = np.zeros(3) - self._eta_r = self.eta_d.copy() # reference target (unwrapped) - - # State vector of 9 elements: [ eta, eta_dot, eta_ddot ] - self._x = np.concatenate([self.eta_d, self.eta_d_dot, self.eta_d_ddot]) - - # Gains - self._delta = np.eye(3) - self._w = np.diag(omega) - O3x3 = np.zeros((3, 3)) - self.Ad = np.block( - [ - [O3x3, np.eye(3), O3x3], - [O3x3, O3x3, np.eye(3)], - [ - -self._w**3, - -(2 * self._delta + np.eye(3)) @ self._w**2, - -(2 * self._delta + np.eye(3)) @ self._w, - ], - ] - ) - self.Bd = np.block([[O3x3], [O3x3], [self._w**3]]) - - def get_eta_d(self): - """Get desired pose: [ x, y, yaw ].""" - return self.eta_d - - def get_eta_d_dot(self): - """Get desired velocity in the inertial frame.""" - return self.eta_d_dot - - def get_eta_d_ddot(self): - """Get desired acceleration in the inertial frame.""" - return self.eta_d_ddot - - def get_nu_d(self): - """Get desired velocity in *body* frame (u, v, r).""" - psi = self.eta_d[2] - return Rz(psi).T @ self.eta_d_dot - - def set_eta_r(self, eta_r): - """ - Set the reference pose. We ensure the yaw changes by at most ±π - so the filter sees only small changes in yaw. - """ - old_yaw = self._eta_r[2] - new_yaw = eta_r[2] - - # Wrap new yaw to [-pi, pi] - new_yaw = wrap_to_pi(new_yaw) - - # Get minimal difference - diff = wrap_to_pi(new_yaw - old_yaw) - continuous_yaw = old_yaw + diff # small step only - - self._eta_r = np.array([eta_r[0], eta_r[1], continuous_yaw]) - - def update(self): - """ - Integrate filter for one time step. We do NOT forcibly wrap - 'self.eta_d[2]' so the filter remains continuous in yaw. - """ - x_dot = self.Ad @ self._x + self.Bd @ self._eta_r - self._x += self._dt * x_dot - - # Extract the updated states - self.eta_d = self._x[:3] - self.eta_d_dot = self._x[3:6] - self.eta_d_ddot = self._x[6:] - - def saturate(x, z): """ Saturation function: returns x / (|x| + z) @@ -169,13 +88,11 @@ def __init__(self): self.debug_error_vel_pub = self.create_publisher( TwistStamped, "control/pose/debug/tracking_error_velocity", 10) - self.debug_metrics_pub = None if PerformanceMetrics is not None: self.debug_metrics_pub = self.create_publisher( PerformanceMetrics, "control/pose/debug/performance_metrics", 10) - # self.create_subscription(PoseStamped, "/goal_pose", self.goal_pose_callback, 10) self.create_subscription( Odometry, "measurement/odom", self.odom_callback, 10) @@ -279,7 +196,7 @@ def odom_callback(self, msg: Odometry): msg.pose.pose.orientation.w, ] ).as_euler("xyz", degrees=False) - self.ref_filter = ThrdOrderRefFilter( + self.ref_filter = ThirdOrderReferenceFilter( dt=self.dt, omega=[0.15, 0.15, 0.15], delta=[0.8, 0.8, 0.8], @@ -378,7 +295,6 @@ def process_performance_metrics(self, desired_pose, desired_vel, error_pos, erro f"RMS: {metrics_msg.rms:.3f}m, Max: {metrics_msg.max:.3f}m" ) - def control_loop(self): """ Control loop that computes and publishes the control command. @@ -438,11 +354,13 @@ def control_loop(self): self.integral_error_yaw += error_yaw * self.dt # Apply saturation to the integral error - self.integral_error_pos = np.clip(self.integral_error_pos, -self.max_integral_error_pos, self.max_integral_error_pos) + self.integral_error_pos = np.clip( + self.integral_error_pos, -self.max_integral_error_pos, self.max_integral_error_pos) # self.integral_error_pos = self.max_integral_error_pos * saturate( # self.integral_error_pos, self.saturation_pos # ) - self.integral_error_yaw = np.clip(self.integral_error_yaw, -self.max_integral_error_yaw, self.max_integral_error_yaw) + self.integral_error_yaw = np.clip( + self.integral_error_yaw, -self.max_integral_error_yaw, self.max_integral_error_yaw) # self.integral_error_yaw = self.max_integral_error_yaw * saturate( # self.integral_error_yaw, self.saturation_yaw # ) @@ -469,7 +387,8 @@ def control_loop(self): ) # Process and publish performance metrics - self.process_performance_metrics(desired_pose, desired_vel, error_pos, error_vel, error_yaw) + self.process_performance_metrics( + desired_pose, desired_vel, error_pos, error_vel, error_yaw) control_x, control_y, control_yaw = Rz( current_yaw).T @ np.array([world_x, world_y, world_yaw]) diff --git a/cybership_tests/cybership_tests/velocity_control.py b/cybership_tests/cybership_tests/velocity_control.py index 9242891..85bdb1a 100755 --- a/cybership_tests/cybership_tests/velocity_control.py +++ b/cybership_tests/cybership_tests/velocity_control.py @@ -9,18 +9,13 @@ # --------------------------------------------------------------------------- import numpy as np -import math import rclpy from rclpy.node import Node -from geometry_msgs.msg import Twist, Wrench, PoseStamped, Pose2D +from geometry_msgs.msg import Twist, Wrench from nav_msgs.msg import Odometry -from typing import Tuple import shoeboxpy.model3dof as box -from visualization_msgs.msg import Marker -from scipy.spatial.transform import Rotation as R -import numpy as np -from typing import Dict, Any, Optional +from cybership_controller.velocity.reference_feedforward import RffVelocityController as VelocityController # Add import for performance metrics try: @@ -28,173 +23,7 @@ except ImportError: print("cybership_msgs not found. Skipping performance metrics.") -import numpy as np - -class ExponentialSmoothing(): - """ - Exponential smoothing class - """ - - def __init__(self, r: float = 0.7) -> None: - """ - Exponential smoothing class constructor - - :param r: A weighting factor in the set [0-1] - """ - self.r = r - self.x = None - self.x_p = None - self.dx_p = None - - def __call__(self, x: np.ndarray) -> np.ndarray: - """ - Exponential smoothing function - :param x: current value of x - :return: dx - """ - if self.x_p is None or self.dx_p is None: - self.x_p = x - self.dx_p = x - return x - else: - self.dx_p = ExponentialSmoothing.__func(self.r, x, self.x_p, self.dx_p) - self.x_p = x - return self.dx_p - - def reset(self, x: np.ndarray) -> None: - """ - Reset the exponential smoothing - - :param x: current value of x - :return: - """ - self.x_p = x - self.dx_p = x - - @staticmethod - def __func( - r: float, x: np.ndarray, x_p: np.ndarray = None, dx_p: np.ndarray = None - ) -> np.ndarray: - r""" - Exponential smoothing function - - .. math:: - \begin{aligned} - s_{0}&=x_{0}\\ - s_{t}&=\alpha x_{t}+(1-\alpha )s_{t-1},\quad t>0 - \end{aligned} - - - :param r: A weighting factor in the set [0-1] - :param x: current value of x - :param x_p: previous value of x - :param dx_p: last computed filtered x value - :return: - """ - if x_p is None or dx_p is None: - return x - else: - return (1 - r) * dx_p + r * (x - x_p) - -def _saturate(vec: np.ndarray, limit: float) -> np.ndarray: - """Scale `vec` so its l2-norm does not exceed `limit`. If `limit` <= 0, return vec unchanged.""" - nrm = np.linalg.norm(vec) - return vec if nrm <= limit or limit <= 0.0 else vec * (limit / nrm) - -# ---------------------------------------------------------------------- # -# Controller implementation # -# ---------------------------------------------------------------------- # - -class AccelLimitedInverseDynamicsPI(): - """Inverse-dynamics velocity controller with PI action and acceleration limiting.""" - - def __init__(self, config: Optional[Dict[str, Any]] = None): - - self.config = config or {} - # Matrices and gains - self.M: np.ndarray = np.array(self.config.get("M", np.eye(3)), dtype=float) - self.D: np.ndarray = np.array(self.config.get("D", np.zeros((3, 3))), dtype=float) - self.Kp: np.ndarray = np.array(self.config.get("Kp", np.eye(3)), dtype=float) - self.Ki: np.ndarray = np.array(self.config.get("Ki", np.zeros((3, 3))), dtype=float) - self.Kd: np.ndarray = np.array(self.config.get("Kd", np.zeros((3, 3))), dtype=float) - self.dt: float = float(self.config.get("dt", 0.01)) # seconds - - # Pre-compute inverse inertia - self.M_inv = np.linalg.inv(self.M) - - # Limits and behaviours - self.a_max: float = float(self.config.get("a_max", -1.0)) # hard acc limit (<=0 => off) - self.I_max: float = float(self.config.get("I_max", -1.0)) # integral wind-up cap (<=0 => off) - self.smooth: bool = bool(self.config.get("smooth_limit", False)) - - # Internal state - self._prev_vd: Optional[np.ndarray] = None # for desired-velocity derivative - self._int_e: np.ndarray = np.zeros(3) # integral of error dt - - # ------------------------------------------------------------------ # - # Public API # - # ------------------------------------------------------------------ # - def update( - self, - current_velocity: np.ndarray, - desired_velocity: np.ndarray, - dt: float, - ) -> np.ndarray: - """Compute the force/torque command tau.""" - - if dt <= 0.0: - raise ValueError("dt must be positive") - - v = current_velocity.reshape(3) - vd = desired_velocity.reshape(3) - - if not hasattr(self, '_vd_dot_smoother'): - r = self.config.get("filter_alpha", 0.7) - self._vd_dot_smoother = ExponentialSmoothing(r=r) - - if not hasattr(self, '_e_dot_smoother'): - r = self.config.get("filter_alpha", 0.7) - self._e_dot_smoother = ExponentialSmoothing(r=r) - - raw_vd_dot = (vd - self._prev_vd) / dt if self._prev_vd is not None else np.zeros(3) - vd_dot = self._vd_dot_smoother(raw_vd_dot) - self._prev_vd = vd.copy() - - e = v - vd - raw_e_dot = (v - self._prev_vd) / dt if self._prev_vd is not None else np.zeros(3) - e_dot = self._e_dot_smoother(raw_e_dot) - - self._int_e = self._int_e + e * dt - self._int_e = np.clip(self._int_e, -self.I_max, self.I_max) - - a_des = vd_dot - self.Kp @ e - self.Ki @ self._int_e - self.Kd @ e_dot - - tau = self.M @ a_des + self.D @ vd - return tau - - def reset(self): - """Clear stored derivative and integral information.""" - self._prev_vd = None - self._int_e = np.zeros(3) - - # ------------------------------------------------------------------ # - # Private helpers # - # ------------------------------------------------------------------ # - def _limit_acceleration(self, a_des: np.ndarray) -> tuple[np.ndarray, bool]: - """Return (a_cmd, saturated_flag) after applying the configured limiter.""" - if self.a_max <= 0.0: - return a_des, False # unlimited - - if self.smooth: - # Smooth limiter using tanh - a_cmd = self.a_max * np.tanh(a_des / self.a_max) - saturated = not np.allclose(a_cmd, a_des) - return a_cmd, saturated - else: - a_cmd = _saturate(a_des, self.a_max) - saturated = not np.allclose(a_cmd, a_des) - return a_cmd, saturated class VelocityControlNode(Node): """ @@ -206,7 +35,7 @@ def __init__(self): super().__init__("velocity_control_node", namespace="voyager") # Create a velocity controller - self.dt = 0.05 # seconds + self.dt = 0.1 # seconds self.vessel = box.Shoebox(L=1.0, B=0.3, T=0.05) # # Control gains for surge, sway, and yaw @@ -214,7 +43,7 @@ def __init__(self): self.k_i_gain = np.array([0.0, 0.0, 0.0]) self.k_d_gain = np.array([0.3, 0.3, 0.3]) - self.controller = AccelLimitedInverseDynamicsPI( + self.controller = VelocityController( config={ "M": self.vessel.M_eff, "D": self.vessel.D, @@ -248,13 +77,13 @@ def __init__(self): Odometry, 'measurement/odom', self.odom_callback, - 10 + 1 ) # Publisher for control commands self.control_pub = self.create_publisher( Wrench, - 'control/force/mux/velocity', + 'control/force/command/velocity', 10 ) @@ -281,7 +110,7 @@ def __init__(self): # --- Performance metrics --- # Performance metrics tracking with moving window - self.window_size = 200 # 2 seconds of data at 100Hz + self.window_size = 50 # 2 seconds of data at 100Hz self.error_window = [] # Store recent velocity errors self.start_time = None self.sample_count = 0 @@ -401,16 +230,11 @@ def control_loop(self): """ Periodic control loop to update reference filter and publish control commands. """ - # Update the reference filter - - now = self.get_clock().now().nanoseconds * 1e-9 - dt_rt = now - self._prev_t if hasattr(self, '_prev_t') else self.dt tau = self.controller.update( current_velocity=self.nu, desired_velocity=self.nu_cmd, - dt=dt_rt + dt=self.dt ) - self._prev_t = now # Calculate velocity error for metrics error_vel = self.nu_cmd - self.nu From 51e01b0be0a56e0dfc806b55dd4a920dcc012141 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Tue, 10 Jun 2025 15:11:18 +0200 Subject: [PATCH 02/46] chore: housekeeping --- .../config/voyager/controller_position.yaml | 55 ++ .../config/voyager/controller_velocity.yaml | 39 ++ cybership_controller/CMakeLists.txt | 8 + .../position/reference_filter.py | 0 .../velocity/reference_feedforward.py | 0 .../nodes/position_control.py | 656 ++++++++++++++++++ .../nodes/velocity_control.py | 383 ++++++++++ .../reference_filter_server.py | 199 ++++-- .../cybership_tests/velocity_control.py | 147 +++- 9 files changed, 1412 insertions(+), 75 deletions(-) create mode 100644 cybership_config/config/voyager/controller_position.yaml create mode 100644 cybership_config/config/voyager/controller_velocity.yaml delete mode 100644 cybership_controller/cybership_controller/position/reference_filter.py delete mode 100644 cybership_controller/cybership_controller/velocity/reference_feedforward.py create mode 100755 cybership_controller/nodes/position_control.py create mode 100755 cybership_controller/nodes/velocity_control.py diff --git a/cybership_config/config/voyager/controller_position.yaml b/cybership_config/config/voyager/controller_position.yaml new file mode 100644 index 0000000..4d7798e --- /dev/null +++ b/cybership_config/config/voyager/controller_position.yaml @@ -0,0 +1,55 @@ +/**: + ros__parameters: + # Vessel physical dimensions (meters) + vessel: + length: 1.0 # Length of vessel (m) + beam: 0.3 # Width of vessel (m) + draft: 0.02 # Draft/depth of vessel (m) + + # Controller parameters + control: + # Position control gains + p_gain: + pos: 4.0 # Proportional gain for position + vel: 0.7 # Proportional gain for velocity + yaw: 1.3 # Proportional gain for yaw + + # Integral gains + i_gain: + pos: 0.2 # Integral gain for position + vel: 0.1 # Integral gain for velocity + yaw: 0.2 # Integral gain for yaw + + # Derivative gains + d_gain: + pos: 0.2 # Derivative gain for position + vel: 0.5 # Derivative gain for velocity + yaw: 1.0 # Derivative gain for yaw + + # Integral error limits + max_integral_error: + pos: 1.0 # Maximum position error integration + yaw: 1.4 # Maximum yaw error integration + + # Saturation parameters + saturation: + pos: 0.1 # Position control saturation + yaw: 0.1 # Yaw control saturation + + # Target reaching tolerances + tolerance: + pos: 0.25 # Position reaching tolerance (m) + yaw: 0.1 # Yaw angle reaching tolerance (rad) + + # Reference filter parameters + filter: + omega: [0.15, 0.15, 0.15] # Natural frequency for reference filter + delta: [0.8, 0.8, 0.8] # Damping ratio for reference filter + + # Performance metrics configuration + metrics: + window_size: 200 # Number of samples in moving window + interval: 1.0 # Time between metrics calculations (seconds) + + # General parameters + dt: 0.01 # Control loop time step (seconds) diff --git a/cybership_config/config/voyager/controller_velocity.yaml b/cybership_config/config/voyager/controller_velocity.yaml new file mode 100644 index 0000000..00b3b00 --- /dev/null +++ b/cybership_config/config/voyager/controller_velocity.yaml @@ -0,0 +1,39 @@ +/**: + ros__parameters: + # Vessel physical dimensions (meters) + vessel: + length: 1.0 # Length of vessel + beam: 0.3 # Width of vessel + draft: 0.05 # Draft (depth) of vessel + + # Control parameters + control: + # Proportional gains for each DOF + p_gain: + surge: 5.0 # Forward/backward + sway: 5.0 # Left/right + yaw: 5.0 # Rotation + + # Integral gains for each DOF + i_gain: + surge: 0.0 + sway: 0.0 + yaw: 0.0 + + # Derivative gains for each DOF + d_gain: + surge: 0.3 + sway: 0.3 + yaw: 0.3 + + i_max: 20.0 # Maximum integral contribution + smooth_limit: true # Enable smooth limiting + filter_alpha: 0.1 # Smoothing factor for desired velocity (0-1) + + # Performance metrics configuration + metrics: + window_size: 50 # Number of samples in moving window + interval: 1.0 # Time between metrics calculations (seconds) + + # General parameters + dt: 0.1 # Control loop time step (seconds) diff --git a/cybership_controller/CMakeLists.txt b/cybership_controller/CMakeLists.txt index a0c8a28..ff75559 100644 --- a/cybership_controller/CMakeLists.txt +++ b/cybership_controller/CMakeLists.txt @@ -8,4 +8,12 @@ ament_python_install_package(${PROJECT_NAME} PACKAGE_DIR src/${PROJECT_NAME} ) +file(GLOB NODE_SCRIPTS ${CMAKE_CURRENT_SOURCE_DIR}/nodes/*.py) + +install( + PROGRAMS + ${NODE_SCRIPTS} + DESTINATION lib/${PROJECT_NAME} +) + ament_package() \ No newline at end of file diff --git a/cybership_controller/cybership_controller/position/reference_filter.py b/cybership_controller/cybership_controller/position/reference_filter.py deleted file mode 100644 index e69de29..0000000 diff --git a/cybership_controller/cybership_controller/velocity/reference_feedforward.py b/cybership_controller/cybership_controller/velocity/reference_feedforward.py deleted file mode 100644 index e69de29..0000000 diff --git a/cybership_controller/nodes/position_control.py b/cybership_controller/nodes/position_control.py new file mode 100755 index 0000000..bfccc34 --- /dev/null +++ b/cybership_controller/nodes/position_control.py @@ -0,0 +1,656 @@ +#!/usr/bin/env python3 + +# ---------------------------------------------------------------------------- +# This code is part of the MCSimPython toolbox and repository +# Created By: Jan-Erik Hygen +# Created Date: 2023-01-30, +# Revised: 2025-01-31 Kristian Magnus Roen Now fitting the MC-Gym for csad. +# Revised: 2025-03-26 Emir Cem Gezer Applied to the Cybership Software Suite +# Tested: +# Copyright (C) 2025: NTNU, Trondheim +# Licensed under GPL-3.0-or-later +# --------------------------------------------------------------------------- + +import time +import numpy as np +import rclpy +from rclpy.node import Node +from rclpy.parameter import Parameter +from geometry_msgs.msg import Wrench, Pose2D, PoseStamped, TwistStamped +from nav_msgs.msg import Odometry +import math +import numpy as np +from scipy.spatial.transform import Rotation as R +from nav2_msgs.action import NavigateToPose +from rclpy.action import ActionServer, GoalResponse, CancelResponse, ActionClient +from rclpy.executors import MultiThreadedExecutor +from cybership_tests.go_to_client import NavigateToPoseClient +from shoeboxpy.model3dof import Shoebox +from visualization_msgs.msg import Marker +from std_msgs.msg import Float32MultiArray + +from cybership_controller.position.reference_filter import ThirdOrderReferenceFilter + +try: + from cybership_interfaces.msg import PerformanceMetrics +except ImportError: + print("cybership_msgs not found. Skipping performance metrics.") + + +def wrap_to_pi(angle): + """ + Wrap an angle in radians to the interval [-pi, pi]. + """ + return (angle + math.pi) % (2 * math.pi) - math.pi + + +def Rz(psi): + """3DOF Rotation matrix about z-axis. + + Parameters + ---------- + psi : float + Yaw angle (rad) + + Returns + ------- + Rz : array_like + 3x3 rotation matrix. + + """ + return np.array( + [[np.cos(psi), -np.sin(psi), 0], + [np.sin(psi), np.cos(psi), 0], [0, 0, 1]] + ) + + +def saturate(x, z): + """ + Saturation function: returns x / (|x| + z) + This ensures a smooth saturation between -1 and 1. + """ + return x / (np.abs(x) + z) + + +class GotoPointController(Node): + def __init__(self): + super().__init__("goto_point_controller", namespace="voyager") + + # Declare parameters with default values + self.declare_parameters( + namespace='', + parameters=[ + # Controller gains + ('control.p_gain.pos', 4.0), + ('control.i_gain.pos', 0.2), + ('control.d_gain.pos', 0.2), + ('control.p_gain.vel', 0.7), + ('control.i_gain.vel', 0.1), + ('control.d_gain.vel', 0.5), + ('control.p_gain.yaw', 1.3), + ('control.i_gain.yaw', 0.2), + ('control.d_gain.yaw', 1.0), + + # Controller limits + ('control.max_integral_error.pos', 1.0), + ('control.max_integral_error.yaw', 1.4), + ('control.saturation.pos', 0.1), + ('control.saturation.yaw', 0.1), + + # Tolerances + ('control.tolerance.pos', 0.25), + ('control.tolerance.yaw', 0.1), + + # Vessel properties + ('vessel.length', 1.0), + ('vessel.beam', 0.3), + ('vessel.draft', 0.02), + + # Reference filter parameters + ('filter.omega', [0.15, 0.15, 0.15]), + ('filter.delta', [0.8, 0.8, 0.8]), + + # Performance metrics + ('metrics.window_size', 200), + ('metrics.interval', 1.0), + + # Time step + ('dt', 0.01), + ] + ) + + # Add parameter callback for runtime updates + self.add_on_set_parameters_callback(self.parameters_callback) + + # Publisher to send control commands (force and torque) + self.control_pub = self.create_publisher( + Wrench, "control/force/command", 10) + # Debug publishers for tracking performance metrics + self.debug_pose_pub = self.create_publisher( + PoseStamped, "control/pose/debug/reference_pose", 10) + self.debug_vel_pub = self.create_publisher( + TwistStamped, "control/pose/debug/reference_velocity", 10) + self.debug_error_pose_pub = self.create_publisher( + Pose2D, "control/pose/debug/tracking_error_position", 10) + self.debug_error_vel_pub = self.create_publisher( + TwistStamped, "control/pose/debug/tracking_error_velocity", 10) + + self.debug_metrics_pub = None + if PerformanceMetrics is not None: + self.debug_metrics_pub = self.create_publisher( + PerformanceMetrics, "control/pose/debug/performance_metrics", 10) + + # self.create_subscription(PoseStamped, "/goal_pose", self.goal_pose_callback, 10) + self.create_subscription( + Odometry, "measurement/odom", self.odom_callback, 10) + + self.marker_pub = self.create_publisher( + Marker, "visualization_marker", 10) + + # Get time step from parameters + self.dt = self.get_parameter('dt').value + + # Initialize controller parameters from ROS parameters + self.update_configuration() + + # Latest odometry message storage + self.latest_odom = None + + # --- Target state --- + # Set initial desired target position and orientation. + self.target_x = None # target x position (meters) + self.target_y = None # target y position (meters) + self.target_yaw = None # target yaw (radians) + + # Track integral error + self.integral_error_pos = np.zeros(2) + self.integral_error_yaw = 0.0 + + self.error_pos = np.zeros(2) + self.error_yaw = 0.0 + + # Initialize the 3rd order reference filter. + # Here, we treat [x, y, yaw] as the 3D pose to be smoothed. + self.ref_filter = None + + self.get_logger().info( + "Goto Point Controller (Reference Filter Version) Initialized with parameters." + ) + + # Timer for periodic control updates + self.timer = self.create_timer(self.dt, self.control_loop) + + # --- Action Server using nav2_msgs/NavigateToPose --- + self._action_server = ActionServer( + self, + NavigateToPose, + "navigate_to_pose", + execute_callback=self.execute_callback, + goal_callback=self.action_goal_callback, + cancel_callback=self.action_cancel_callback, + ) + + def update_configuration(self): + """Update controller configuration from parameters""" + # Get controller gains + self.Kp_pos = self.get_parameter('control.p_gain.pos').value + self.Ki_pos = self.get_parameter('control.i_gain.pos').value + self.Kd_pos = self.get_parameter('control.d_gain.pos').value + self.Kp_vel = self.get_parameter('control.p_gain.vel').value + self.Ki_vel = self.get_parameter('control.i_gain.vel').value + self.Kd_vel = self.get_parameter('control.d_gain.vel').value + self.Kp_yaw = self.get_parameter('control.p_gain.yaw').value + self.Ki_yaw = self.get_parameter('control.i_gain.yaw').value + self.Kd_yaw = self.get_parameter('control.d_gain.yaw').value + + # Get controller limits + self.max_integral_error_pos = self.get_parameter('control.max_integral_error.pos').value + self.max_integral_error_yaw = self.get_parameter('control.max_integral_error.yaw').value + self.saturation_pos = self.get_parameter('control.saturation.pos').value + self.saturation_yaw = self.get_parameter('control.saturation.yaw').value + + # Get tolerances + self.pos_tol = self.get_parameter('control.tolerance.pos').value + self.yaw_tol = self.get_parameter('control.tolerance.yaw').value + + # Get vessel properties + vessel_length = self.get_parameter('vessel.length').value + vessel_beam = self.get_parameter('vessel.beam').value + vessel_draft = self.get_parameter('vessel.draft').value + + # Create vessel model + self.shoebox = Shoebox( + L=vessel_length, + B=vessel_beam, + T=vessel_draft, + ) + + # Get performance metrics parameters + self.window_size = self.get_parameter('metrics.window_size').value + self.metrics_interval = self.get_parameter('metrics.interval').value + + # Reset reference filter if parameters change + if self.ref_filter is not None and self.latest_odom is not None: + # Get filter parameters + filter_omega = self.get_parameter('filter.omega').value + filter_delta = self.get_parameter('filter.delta').value + + # Save current state + current_eta = self.ref_filter.eta_d + + # Create new filter with updated parameters + self.ref_filter = ThirdOrderReferenceFilter( + dt=self.dt, + omega=filter_omega, + delta=filter_delta, + initial_eta=current_eta, + ) + self.ref_filter.eta_d = current_eta + + self.get_logger().info( + f"Updated configuration - vessel: [{vessel_length}, {vessel_beam}, {vessel_draft}], " + + f"POS gains: P={self.Kp_pos}, I={self.Ki_pos}, D={self.Kd_pos}, " + + f"YAW gains: P={self.Kp_yaw}, I={self.Ki_yaw}, D={self.Kd_yaw}" + ) + + def parameters_callback(self, params): + """Handle parameter updates""" + update_needed = False + + for param in params: + if param.name.startswith(('control.', 'vessel.', 'filter.', 'metrics.')): + update_needed = True + + if param.name == 'dt': + self.dt = param.value + # Recreate the timer with new dt + self.timer.cancel() + self.timer = self.create_timer(self.dt, self.control_loop) + update_needed = True + + # Update configuration if parameters changed + if update_needed: + self.update_configuration() + + return True # Accept all parameter changes + + def odom_callback(self, msg: Odometry): + """ + Callback to update the latest odometry measurement. + """ + if self.ref_filter is None: + print("Setting target position from odometry.") + self.target_x = msg.pose.pose.position.x + self.target_y = msg.pose.pose.position.y + _, _, self.target_yaw = R.from_quat( + [ + msg.pose.pose.orientation.x, + msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z, + msg.pose.pose.orientation.w, + ] + ).as_euler("xyz", degrees=False) + + # Get filter parameters from ROS parameters + filter_omega = self.get_parameter('filter.omega').value + filter_delta = self.get_parameter('filter.delta').value + + self.ref_filter = ThirdOrderReferenceFilter( + dt=self.dt, + omega=filter_omega, + delta=filter_delta, + initial_eta=[self.target_x, self.target_y, self.target_yaw], + ) + self.ref_filter.eta_d = np.array( + [self.target_x, self.target_y, self.target_yaw] + ) + + self.latest_odom = msg + + def process_performance_metrics(self, desired_pose, desired_vel, error_pos, error_vel, error_yaw): + """ + Process and publish debug information using a moving window approach. + """ + # Publish reference pose (from reference filter) + debug_ref_pose = PoseStamped() + debug_ref_pose.header.stamp = self.get_clock().now().to_msg() + debug_ref_pose.header.frame_id = "world" + debug_ref_pose.pose.position.x = desired_pose[0] + debug_ref_pose.pose.position.y = desired_pose[1] + debug_ref_pose.pose.position.z = 0.0 + # Convert yaw to quaternion + q = R.from_euler('z', desired_pose[2]).as_quat() + debug_ref_pose.pose.orientation.x = q[0] + debug_ref_pose.pose.orientation.y = q[1] + debug_ref_pose.pose.orientation.z = q[2] + debug_ref_pose.pose.orientation.w = q[3] + self.debug_pose_pub.publish(debug_ref_pose) + + debug_ref_vel = TwistStamped() + debug_ref_vel.header.stamp = self.get_clock().now().to_msg() + debug_ref_vel.header.frame_id = "base_link" + debug_ref_vel.twist.linear.x = desired_vel[0] + debug_ref_vel.twist.linear.y = desired_vel[1] + debug_ref_vel.twist.angular.z = desired_vel[2] + self.debug_vel_pub.publish(debug_ref_vel) + + debug_error_vel = TwistStamped() + debug_error_vel.header.stamp = self.get_clock().now().to_msg() + debug_error_vel.header.frame_id = "base_link" + debug_error_vel.twist.linear.x = error_vel[0] + debug_error_vel.twist.linear.y = error_vel[1] + debug_error_vel.twist.angular.z = error_yaw + self.debug_error_vel_pub.publish(debug_error_vel) + + # Publish error information + debug_error_pose = Pose2D() + debug_error_pose.x = error_pos[0] + debug_error_pose.y = error_pos[1] + debug_error_pose.theta = error_yaw + self.debug_error_pose_pub.publish(debug_error_pose) + + # Update metrics using moving window + error_norm = np.sqrt(error_pos[0]**2 + error_pos[1]**2) + + # Initialize start time if not already set + if self.start_time is None: + self.start_time = self.get_clock().now().nanoseconds / 1e9 + + # Add newest error to window + self.error_window.append(error_norm) + self.error_yaw_window.append(abs(error_yaw)) + + # Keep window at fixed size + if len(self.error_window) > self.window_size: + self.error_window.pop(0) + self.error_yaw_window.pop(0) + + self.sample_count += 1 + + current_time = self.get_clock().now().nanoseconds / 1e9 + if current_time - self.last_metrics_time >= self.metrics_interval and len(self.error_window) > 0: + self.last_metrics_time = current_time + + # Calculate window statistics + error_array = np.array(self.error_window) + + metrics_msg = PerformanceMetrics() + metrics_msg.header.stamp = self.get_clock().now().to_msg() + metrics_msg.header.frame_id = "voyager" + metrics_msg.message = "Position tracking error (meters)" + + # Calculate statistics over the window + metrics_msg.mean = np.mean(error_array) + metrics_msg.median = np.median(error_array) + metrics_msg.rms = np.sqrt(np.mean(np.square(error_array))) + metrics_msg.min = np.min(error_array) + metrics_msg.max = np.max(error_array) + metrics_msg.stddev = np.std(error_array) + + self.debug_metrics_pub.publish(metrics_msg) + + self.get_logger().info( + f"Position tracking metrics - Mean: {metrics_msg.mean:.3f}m, " + f"RMS: {metrics_msg.rms:.3f}m, Max: {metrics_msg.max:.3f}m" + ) + + def control_loop(self): + """ + Periodic control loop to update reference filter and publish control commands. + This version uses a 3rd order reference filter to generate a smooth desired trajectory. + """ + if self.latest_odom is None: + return + + if self.ref_filter is None: + return + + # Extract current position from odometry (global frame) + pos = self.latest_odom.pose.pose.position + current_x = pos.x + current_y = pos.y + + # Extract yaw (heading) from quaternion orientation + orientation = self.latest_odom.pose.pose.orientation + rot = R.from_quat([orientation.x, orientation.y, + orientation.z, orientation.w]) + _, _, current_yaw = rot.as_euler("xyz", degrees=False) + + # Also get current velocities from odometry for the derivative term. + current_vx = self.latest_odom.twist.twist.linear.x + current_vy = self.latest_odom.twist.twist.linear.y + current_yaw_rate = self.latest_odom.twist.twist.angular.z + + # Update the reference filter with the latest target pose. + # This "command" is smoothed by the filter. + ref = np.array([self.target_x, self.target_y, self.target_yaw]) + self.ref_filter.set_eta_r(ref) + self.ref_filter.update() + + # Retrieve filtered outputs: desired pose, velocity, and acceleration. + desired_pose = self.ref_filter.eta_d # [x, y, yaw] + desired_vel = self.ref_filter.eta_d_dot # velocity + # acceleration (feedforward term) + desired_acc = self.ref_filter.eta_d_ddot + + # Compute errors (position and yaw) between the filtered desired state and current state. + error_pos = np.array( + [desired_pose[0] - current_x, desired_pose[1] - current_y]) + error_yaw = wrap_to_pi(desired_pose[2] - current_yaw) + + self.error_pos = error_pos + self.error_yaw = error_yaw + + # Compute velocity errors + current_vel = np.array([current_vx, current_vy]) + error_vel = np.array( + [desired_vel[0] - current_vx, desired_vel[1] - current_vy]) + error_yaw_rate = desired_vel[2] - current_yaw_rate + + acc = self.shoebox.M_eff @ self.ref_filter.get_nu_d() + + self.integral_error_pos += error_pos * self.dt + self.integral_error_yaw += error_yaw * self.dt + + # Apply saturation to the integral error + self.integral_error_pos = np.clip( + self.integral_error_pos, -self.max_integral_error_pos, self.max_integral_error_pos) + self.integral_error_yaw = np.clip( + self.integral_error_yaw, -self.max_integral_error_yaw, self.max_integral_error_yaw) + + # Compute control commands. + # For position, we use feedforward desired acceleration plus a PID correction. + world_x = ( + acc[0] + + self.Kp_pos * error_pos[0] + + self.Kd_vel * error_vel[0] + + self.Ki_pos * self.integral_error_pos[0] + ) + world_y = ( + acc[1] + + self.Kp_pos * error_pos[1] + + self.Kd_vel * error_vel[1] + + self.Ki_pos * self.integral_error_pos[1] + ) + world_yaw = ( + acc[2] + + self.Kp_yaw * error_yaw + + self.Kd_yaw * error_yaw_rate + + self.Ki_yaw * self.integral_error_yaw + ) + + # Process and publish performance metrics + self.process_performance_metrics( + desired_pose, desired_vel, error_pos, error_vel, error_yaw) + + control_x, control_y, control_yaw = Rz( + current_yaw).T @ np.array([world_x, world_y, world_yaw]) + + # Optionally apply saturation. + # control_x = saturate(control_x, self.saturation_pos) + # control_y = saturate(control_y, self.saturation_pos) + # control_yaw = saturate(control_yaw, self.saturation_yaw) + + wrench_msg = Wrench() + + # Publish these instead + wrench_msg.force.x = control_x + wrench_msg.force.y = control_y + wrench_msg.force.z = 0.0 # No vertical force + wrench_msg.torque.x = 0.0 + wrench_msg.torque.y = 0.0 + wrench_msg.torque.z = control_yaw + + # Publish the control command. + self.control_pub.publish(wrench_msg) + + # Debug logging. + self.get_logger().debug( + f"Current: ({current_x:.2f}, {current_y:.2f}, {current_yaw:.2f}), " + f"Filtered desired: ({desired_pose[0]:.2f}, {desired_pose[1]:.2f}, {desired_pose[2]:.2f}), " + f"Control: ({control_x:.2f}, {control_y:.2f}, {control_yaw:.2f})" + ) + + def action_goal_callback(self, goal_request): + self.get_logger().info("Received NavigateToPose goal request") + return GoalResponse.ACCEPT + + def action_cancel_callback(self, goal_handle): + self.get_logger().info("Received cancel request for NavigateToPose") + return CancelResponse.ACCEPT + + def execute_callback(self, goal_handle): + self.get_logger().info("Executing NavigateToPose goal...") + + # Extract target pose from the goal message + target_pose: PoseStamped = goal_handle.request.pose + self.target_x = target_pose.pose.position.x + self.target_y = target_pose.pose.position.y + + self.get_logger().info( + f"Target position: ({self.target_x}, {self.target_y})") + + # Convert quaternion to yaw angle + orientation = target_pose.pose.orientation + rot = R.from_quat([ + orientation.x, + orientation.y, + orientation.z, + orientation.w] + ) + _, _, self.target_yaw = rot.as_euler("xyz", degrees=False) + + feedback_msg = NavigateToPose.Feedback() + current_feedback_pose = PoseStamped() + + self.publish_target_pose_marker(target_pose) + + # Loop until the robot reaches the target (within tolerance) or the goal is canceled. + while rclpy.ok(): + if goal_handle.is_cancel_requested: + goal_handle.canceled() + self.get_logger().info("NavigateToPose goal canceled") + result = NavigateToPose.Result() + return result + + if self.latest_odom is None: + time.sleep(1.0) + continue + + # Extract current pose from odometry + current_odom = self.latest_odom.pose.pose + current_feedback_pose.header.stamp = self.get_clock().now().to_msg() + current_feedback_pose.header.frame_id = "odom" + + current_feedback_pose.pose.position = current_odom.position + current_feedback_pose.pose.orientation = current_odom.orientation + + # Compute remaining distance + error_norm = np.sqrt( + (self.target_x - current_feedback_pose.pose.position.x) ** 2 + + (self.target_y - current_feedback_pose.pose.position.y) ** 2 + ) + _, _, current_yaw = R.from_quat( + [ + current_feedback_pose.pose.orientation.x, + current_feedback_pose.pose.orientation.y, + current_feedback_pose.pose.orientation.z, + current_feedback_pose.pose.orientation.w, + ] + ).as_euler("xyz", degrees=False) + error_yaw = wrap_to_pi(self.target_yaw - current_yaw) + + feedback_msg.current_pose = current_feedback_pose + feedback_msg.distance_remaining = float(error_norm) + + goal_handle.publish_feedback(feedback_msg) + self.get_logger().debug( + f"Distance remaining: {error_norm:.2f}, angle error: {error_yaw:.2f}" + ) + + # Check if target is reached (both position and yaw) + if error_norm < self.pos_tol and abs(error_yaw) < self.yaw_tol: + self.error_pos = np.array([np.inf, np.info]) + self.error_yaw = np.inf + break + + time.sleep(1.0) # Publish feedback at ~1Hz + + goal_handle.succeed() + result = NavigateToPose.Result() + self.get_logger().info("NavigateToPose goal succeeded") + return result + + def publish_target_pose_marker(self, pose_stamped: PoseStamped): + """Publish a simple marker (e.g., an arrow) in RViz to visualize the requested pose.""" + marker = Marker() + marker.header = pose_stamped.header + marker.header.frame_id = "world" # Adjust frame if necessary + marker.ns = "target_pose" + marker.id = 0 + marker.type = Marker.ARROW + marker.action = Marker.ADD + + # Pose (position/orientation) + marker.pose = pose_stamped.pose + + # Scale and color + marker.scale.x = 0.5 # Arrow length + marker.scale.y = 0.1 # Arrow width + marker.scale.z = 0.1 + marker.color.a = 1.0 + marker.color.r = 1.0 + marker.color.g = 0.0 + marker.color.b = 0.0 + + # Lifetime (0 = forever) + marker.lifetime.sec = 0 + marker.lifetime.nanosec = 0 + + self.marker_pub.publish(marker) + + +def main(args=None): + rclpy.init(args=args) + server_node = GotoPointController() + client_node = NavigateToPoseClient() + executor = ( + MultiThreadedExecutor() + ) # Allows processing multiple callbacks concurrently + executor.add_node(server_node) + executor.add_node(client_node) + try: + executor.spin() + except KeyboardInterrupt: + server_node.get_logger().info("Keyboard interrupt, shutting down...") + client_node.get_logger().info("Keyboard interrupt, shutting down...") + + finally: + server_node.destroy_node() + client_node.destroy_node() + executor.shutdown() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/cybership_controller/nodes/velocity_control.py b/cybership_controller/nodes/velocity_control.py new file mode 100755 index 0000000..d8d85db --- /dev/null +++ b/cybership_controller/nodes/velocity_control.py @@ -0,0 +1,383 @@ +#!/usr/bin/env python3 + +# ---------------------------------------------------------------------------- +# This code is part of the Cybership Software Suite +# Created By: Emir Cem Gezer +# Created Date: 2025-03-26 +# Copyright (C) 2025: NTNU, Trondheim +# Licensed under GPL-3.0-or-later +# --------------------------------------------------------------------------- + +import numpy as np +import rclpy +from rclpy.node import Node +from rclpy.parameter import Parameter +from geometry_msgs.msg import Twist, Wrench +from nav_msgs.msg import Odometry +import shoeboxpy.model3dof as box + +from cybership_controller.velocity.reference_feedforward import RffVelocityController as VelocityController + +# Add import for performance metrics +try: + from cybership_interfaces.msg import PerformanceMetrics +except ImportError: + print("cybership_msgs not found. Skipping performance metrics.") + + + +class VelocityControlNode(Node): + """ + ROS Node for velocity control using a reference filter. + Subscribes to cmd_vel for velocity commands and publishes control forces/moments. + """ + + def __init__(self): + super().__init__("velocity_control_node", namespace="voyager") + + # Declare parameters with default values + self.declare_parameters( + namespace='', + parameters=[ + # Vessel dimensions + ('vessel.length', 1.0), + ('vessel.beam', 0.3), + ('vessel.draft', 0.05), + + # Control gains + ('control.p_gain.surge', 5.0), + ('control.p_gain.sway', 5.0), + ('control.p_gain.yaw', 5.0), + + ('control.i_gain.surge', 0.0), + ('control.i_gain.sway', 0.0), + ('control.i_gain.yaw', 0.0), + + ('control.d_gain.surge', 0.3), + ('control.d_gain.sway', 0.3), + ('control.d_gain.yaw', 0.3), + + ('control.i_max', 20.0), + ('control.smooth_limit', True), + ('control.filter_alpha', 0.1), + + # Performance metrics + ('metrics.window_size', 50), + ('metrics.interval', 1.0), + + # Time step + ('dt', 0.1), + ] + ) + + # Add parameter callback for runtime updates + self.add_on_set_parameters_callback(self.parameters_callback) + + # Get current parameter values + self.dt = self.get_parameter('dt').value + + # Initialize vessel and controller + self.update_configuration() + + self.nu_prev = np.zeros(3) # [u, v, r] previous velocities + self.nu_cmd = np.zeros(3) # [u, v, r] desired velocities + self.nu_cmd_prev = np.zeros(3) # [u, v, r] desired velocities + + # Current velocity state + self.nu = np.zeros(3) # [u, v, r] + + # Subscribe to cmd_vel (velocity commands) + self.cmd_vel_sub = self.create_subscription( + Twist, + 'control/velocity/command', + self.cmd_vel_callback, + 10 + ) + + # Subscribe to odometry for current velocity + self.odom_sub = self.create_subscription( + Odometry, + 'measurement/odom', + self.odom_callback, + 1 + ) + + # Publisher for control commands + self.control_pub = self.create_publisher( + Wrench, + 'control/force/command/velocity', + 10 + ) + + # Add debug publishers for tracking performance + self.debug_vel_cmd_pub = self.create_publisher( + Twist, + 'control/velocity/debug/reference_velocity', + 10 + ) + self.debug_error_vel_pub = self.create_publisher( + Twist, + 'control/velocity/debug/tracking_error_velocity', + 10 + ) + + # Performance metrics publisher + self.debug_metrics_pub = None + if 'PerformanceMetrics' in globals(): + self.debug_metrics_pub = self.create_publisher( + PerformanceMetrics, + "control/velocity/debug/performance_metrics", + 10 + ) + + # --- Performance metrics --- + # Performance metrics tracking with moving window + self.window_size = self.get_parameter('metrics.window_size').value + self.error_window = [] # Store recent velocity errors + self.start_time = None + self.sample_count = 0 + + # Time between metrics calculations + self.metrics_interval = self.get_parameter('metrics.interval').value + self.last_metrics_time = 0.0 + + # Timer for control loop + self.timer = self.create_timer(self.dt, self.control_loop) + + self.get_logger().info("Velocity controller initialized with parameters") + + def update_configuration(self): + """Update vessel and controller configuration from parameters""" + # Get vessel dimensions + vessel_length = self.get_parameter('vessel.length').value + vessel_beam = self.get_parameter('vessel.beam').value + vessel_draft = self.get_parameter('vessel.draft').value + + # Create vessel model + self.vessel = box.Shoebox(L=vessel_length, B=vessel_beam, T=vessel_draft) + + # Get control gains + self.k_p_gain = np.array([ + self.get_parameter('control.p_gain.surge').value, + self.get_parameter('control.p_gain.sway').value, + self.get_parameter('control.p_gain.yaw').value + ]) + + self.k_i_gain = np.array([ + self.get_parameter('control.i_gain.surge').value, + self.get_parameter('control.i_gain.sway').value, + self.get_parameter('control.i_gain.yaw').value + ]) + + self.k_d_gain = np.array([ + self.get_parameter('control.d_gain.surge').value, + self.get_parameter('control.d_gain.sway').value, + self.get_parameter('control.d_gain.yaw').value + ]) + + # Create/update controller + self.controller = VelocityController( + config={ + "M": self.vessel.M_eff, + "D": self.vessel.D, + "Kp": np.diag(self.k_p_gain), + "Ki": np.diag(self.k_i_gain), + "Kd": np.diag(self.k_d_gain), + "I_max": self.get_parameter('control.i_max').value, + "smooth_limit": self.get_parameter('control.smooth_limit').value, + "filter_alpha": self.get_parameter('control.filter_alpha').value, + "dt": self.dt, + } + ) + + # Update metrics parameters + self.window_size = self.get_parameter('metrics.window_size').value + self.metrics_interval = self.get_parameter('metrics.interval').value + + self.get_logger().info(f"Updated configuration - vessel: [{vessel_length}, {vessel_beam}, {vessel_draft}], " + + f"P: {self.k_p_gain}, I: {self.k_i_gain}, D: {self.k_d_gain}") + + def parameters_callback(self, params): + """Handle parameter updates""" + update_needed = False + + for param in params: + if param.name in [ + 'vessel.length', 'vessel.beam', 'vessel.draft', + 'control.p_gain.surge', 'control.p_gain.sway', 'control.p_gain.yaw', + 'control.i_gain.surge', 'control.i_gain.sway', 'control.i_gain.yaw', + 'control.d_gain.surge', 'control.d_gain.sway', 'control.d_gain.yaw', + 'control.i_max', 'control.smooth_limit', 'control.filter_alpha', + 'metrics.window_size', 'metrics.interval' + ]: + update_needed = True + + if param.name == 'dt': + self.dt = param.value + # Recreate the timer with new dt + self.timer.cancel() + self.timer = self.create_timer(self.dt, self.control_loop) + update_needed = True + + # Update vessel and controller if parameters changed + if update_needed: + self.update_configuration() + + return True # Accept all parameter changes + + def cmd_vel_callback(self, msg: Twist): + """ + Process incoming velocity commands. + + Parameters: + ----------- + msg : Twist + Desired velocity command + """ + # Extract the desired velocities from the Twist message + # Linear velocities: x (surge), y (sway) + # Angular velocity: z (yaw rate) + self.nu_cmd = np.array([ + msg.linear.x, # surge velocity (u) + msg.linear.y, # sway velocity (v) + msg.angular.z # yaw rate (r) + ]) + + self.get_logger().debug( + f"Received cmd_vel: [{self.nu_cmd[0]:.2f}, {self.nu_cmd[1]:.2f}, {self.nu_cmd[2]:.2f}]") + + def odom_callback(self, msg: Odometry): + """ + Update current velocity from odometry. + + Parameters: + ----------- + msg : Odometry + Odometry message containing current state + """ + # Extract body-fixed velocities + self.nu = np.array([ + msg.twist.twist.linear.x, # surge velocity (u) + msg.twist.twist.linear.y, # sway velocity (v) + msg.twist.twist.angular.z # yaw rate (r) + ]) + + def process_performance_metrics(self, desired_vel, error_vel): + """ + Process and publish debug information using a moving window approach. + """ + # Publish command velocity for debugging + msg = Twist() + msg.linear.x = desired_vel[0] + msg.linear.y = desired_vel[1] + msg.angular.z = desired_vel[2] + + self.debug_vel_cmd_pub.publish(msg) + + # Publish velocity error for debugging + error_twist = Twist() + error_twist.linear.x = error_vel[0] + error_twist.linear.y = error_vel[1] + error_twist.linear.z = 0.0 + error_twist.angular.x = 0.0 + error_twist.angular.y = 0.0 + error_twist.angular.z = error_vel[2] + self.debug_error_vel_pub.publish(error_twist) + + # Update metrics using moving window + error_norm = np.sqrt( + error_vel[0]**2 + error_vel[1]**2 + error_vel[2]**2) + + # Initialize start time if not already set + if self.start_time is None: + self.start_time = self.get_clock().now().nanoseconds / 1e9 + + # Add newest error to window + self.error_window.append(error_norm) + + # Keep window at fixed size + if len(self.error_window) > self.window_size: + self.error_window.pop(0) + + self.sample_count += 1 + + current_time = self.get_clock().now().nanoseconds / 1e9 + if current_time - self.last_metrics_time >= self.metrics_interval and len(self.error_window) > 0: + self.last_metrics_time = current_time + + # Calculate window statistics + error_array = np.array(self.error_window) + + if self.debug_metrics_pub is not None: + metrics_msg = PerformanceMetrics() + metrics_msg.header.stamp = self.get_clock().now().to_msg() + metrics_msg.header.frame_id = "voyager" + metrics_msg.message = "Velocity tracking error" + + # Calculate statistics over the window + metrics_msg.mean = np.mean(error_array) + metrics_msg.median = np.median(error_array) + metrics_msg.rms = np.sqrt(np.mean(np.square(error_array))) + metrics_msg.min = np.min(error_array) + metrics_msg.max = np.max(error_array) + metrics_msg.stddev = np.std(error_array) + + self.debug_metrics_pub.publish(metrics_msg) + + self.get_logger().info( + f"Velocity tracking metrics - Mean: {metrics_msg.mean:.3f}, " + f"RMS: {metrics_msg.rms:.3f}, Max: {metrics_msg.max:.3f}" + ) + + def control_loop(self): + """ + Periodic control loop to update reference filter and publish control commands. + """ + tau = self.controller.update( + current_velocity=self.nu, + desired_velocity=self.nu_cmd, + dt=self.dt + ) + + # Calculate velocity error for metrics + error_vel = self.nu_cmd - self.nu + + # Process and publish performance metrics + self.process_performance_metrics(self.nu_cmd, error_vel) + + # Create and publish Wrench message + wrench_msg = Wrench() + wrench_msg.force.x = float(tau[0]) + wrench_msg.force.y = float(tau[1]) + wrench_msg.force.z = 0.0 + wrench_msg.torque.x = 0.0 + wrench_msg.torque.y = 0.0 + wrench_msg.torque.z = float(tau[2]) + + self.control_pub.publish(wrench_msg) + + +def main(args=None): + """ + Main function to initialize and run the velocity control node with a multithreaded executor. + """ + rclpy.init(args=args) + node = VelocityControlNode() + + # Create a multithreaded executor + executor = rclpy.executors.MultiThreadedExecutor() + executor.add_node(node) + + try: + # Spin the executor instead of the node directly + executor.spin() + except KeyboardInterrupt: + node.get_logger().info("Keyboard interrupt, shutting down...") + finally: + executor.shutdown() + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/cybership_tests/cybership_tests/reference_filter_server.py b/cybership_tests/cybership_tests/reference_filter_server.py index 70095b2..bfccc34 100755 --- a/cybership_tests/cybership_tests/reference_filter_server.py +++ b/cybership_tests/cybership_tests/reference_filter_server.py @@ -15,6 +15,7 @@ import numpy as np import rclpy from rclpy.node import Node +from rclpy.parameter import Parameter from geometry_msgs.msg import Wrench, Pose2D, PoseStamped, TwistStamped from nav_msgs.msg import Odometry import math @@ -75,6 +76,52 @@ class GotoPointController(Node): def __init__(self): super().__init__("goto_point_controller", namespace="voyager") + # Declare parameters with default values + self.declare_parameters( + namespace='', + parameters=[ + # Controller gains + ('control.p_gain.pos', 4.0), + ('control.i_gain.pos', 0.2), + ('control.d_gain.pos', 0.2), + ('control.p_gain.vel', 0.7), + ('control.i_gain.vel', 0.1), + ('control.d_gain.vel', 0.5), + ('control.p_gain.yaw', 1.3), + ('control.i_gain.yaw', 0.2), + ('control.d_gain.yaw', 1.0), + + # Controller limits + ('control.max_integral_error.pos', 1.0), + ('control.max_integral_error.yaw', 1.4), + ('control.saturation.pos', 0.1), + ('control.saturation.yaw', 0.1), + + # Tolerances + ('control.tolerance.pos', 0.25), + ('control.tolerance.yaw', 0.1), + + # Vessel properties + ('vessel.length', 1.0), + ('vessel.beam', 0.3), + ('vessel.draft', 0.02), + + # Reference filter parameters + ('filter.omega', [0.15, 0.15, 0.15]), + ('filter.delta', [0.8, 0.8, 0.8]), + + # Performance metrics + ('metrics.window_size', 200), + ('metrics.interval', 1.0), + + # Time step + ('dt', 0.01), + ] + ) + + # Add parameter callback for runtime updates + self.add_on_set_parameters_callback(self.parameters_callback) + # Publisher to send control commands (force and torque) self.control_pub = self.create_publisher( Wrench, "control/force/command", 10) @@ -100,7 +147,12 @@ def __init__(self): self.marker_pub = self.create_publisher( Marker, "visualization_marker", 10) - self.dt = 0.01 # seconds + # Get time step from parameters + self.dt = self.get_parameter('dt').value + + # Initialize controller parameters from ROS parameters + self.update_configuration() + # Latest odometry message storage self.latest_odom = None @@ -110,66 +162,24 @@ def __init__(self): self.target_y = None # target y position (meters) self.target_yaw = None # target yaw (radians) - # --- Controller gains (for the PD part) --- - # You can tune these gains as needed. - self.Kp_pos = 4.0 # proportional gain for position - self.Ki_pos = 0.2 # integral gain for position - self.Kd_pos = 0.2 # derivative gain for position - self.Kp_vel = 0.7 # proportional gain for velocity - self.Ki_vel = 0.1 # integral gain for velocity - self.Kd_vel = 0.5 # derivative gain for velocity - - self.Kp_yaw = 1.3 # proportional gain for yaw - self.Ki_yaw = 0.2 # integral gain for yaw - self.Kd_yaw = 1.0 # derivative gain for yaw - - # --- Performance metrics --- - # Performance metrics tracking with moving window - self.window_size = 200 # 2 seconds of data at 100Hz - self.error_window = [] # Store recent position errors - self.error_yaw_window = [] # Store recent yaw errors - self.start_time = None - self.sample_count = 0 - - # Time between metrics calculations (e.g., 1 second) - self.metrics_interval = 1.0 # seconds - self.last_metrics_time = 0.0 - # Track integral error - self.max_integral_error_pos = 1.0 - self.max_integral_error_yaw = 1.4 self.integral_error_pos = np.zeros(2) self.integral_error_yaw = 0.0 - # Saturation parameters (if needed) - self.saturation_pos = 0.1 - self.saturation_yaw = 0.1 - - # Tolerances for considering the target reached - self.pos_tol = 0.25 # meters - self.yaw_tol = 0.1 # radians - self.error_pos = np.zeros(2) self.error_yaw = 0.0 # Initialize the 3rd order reference filter. # Here, we treat [x, y, yaw] as the 3D pose to be smoothed. - initial_eta = [self.target_x, self.target_y, self.target_yaw] self.ref_filter = None self.get_logger().info( - "Goto Point Controller (Reference Filter Version) Initialized." + "Goto Point Controller (Reference Filter Version) Initialized with parameters." ) # Timer for periodic control updates self.timer = self.create_timer(self.dt, self.control_loop) - self.shoebox = Shoebox( - L=1.0, - B=0.3, - T=0.02, - ) - # --- Action Server using nav2_msgs/NavigateToPose --- self._action_server = ActionServer( self, @@ -180,6 +190,90 @@ def __init__(self): cancel_callback=self.action_cancel_callback, ) + def update_configuration(self): + """Update controller configuration from parameters""" + # Get controller gains + self.Kp_pos = self.get_parameter('control.p_gain.pos').value + self.Ki_pos = self.get_parameter('control.i_gain.pos').value + self.Kd_pos = self.get_parameter('control.d_gain.pos').value + self.Kp_vel = self.get_parameter('control.p_gain.vel').value + self.Ki_vel = self.get_parameter('control.i_gain.vel').value + self.Kd_vel = self.get_parameter('control.d_gain.vel').value + self.Kp_yaw = self.get_parameter('control.p_gain.yaw').value + self.Ki_yaw = self.get_parameter('control.i_gain.yaw').value + self.Kd_yaw = self.get_parameter('control.d_gain.yaw').value + + # Get controller limits + self.max_integral_error_pos = self.get_parameter('control.max_integral_error.pos').value + self.max_integral_error_yaw = self.get_parameter('control.max_integral_error.yaw').value + self.saturation_pos = self.get_parameter('control.saturation.pos').value + self.saturation_yaw = self.get_parameter('control.saturation.yaw').value + + # Get tolerances + self.pos_tol = self.get_parameter('control.tolerance.pos').value + self.yaw_tol = self.get_parameter('control.tolerance.yaw').value + + # Get vessel properties + vessel_length = self.get_parameter('vessel.length').value + vessel_beam = self.get_parameter('vessel.beam').value + vessel_draft = self.get_parameter('vessel.draft').value + + # Create vessel model + self.shoebox = Shoebox( + L=vessel_length, + B=vessel_beam, + T=vessel_draft, + ) + + # Get performance metrics parameters + self.window_size = self.get_parameter('metrics.window_size').value + self.metrics_interval = self.get_parameter('metrics.interval').value + + # Reset reference filter if parameters change + if self.ref_filter is not None and self.latest_odom is not None: + # Get filter parameters + filter_omega = self.get_parameter('filter.omega').value + filter_delta = self.get_parameter('filter.delta').value + + # Save current state + current_eta = self.ref_filter.eta_d + + # Create new filter with updated parameters + self.ref_filter = ThirdOrderReferenceFilter( + dt=self.dt, + omega=filter_omega, + delta=filter_delta, + initial_eta=current_eta, + ) + self.ref_filter.eta_d = current_eta + + self.get_logger().info( + f"Updated configuration - vessel: [{vessel_length}, {vessel_beam}, {vessel_draft}], " + + f"POS gains: P={self.Kp_pos}, I={self.Ki_pos}, D={self.Kd_pos}, " + + f"YAW gains: P={self.Kp_yaw}, I={self.Ki_yaw}, D={self.Kd_yaw}" + ) + + def parameters_callback(self, params): + """Handle parameter updates""" + update_needed = False + + for param in params: + if param.name.startswith(('control.', 'vessel.', 'filter.', 'metrics.')): + update_needed = True + + if param.name == 'dt': + self.dt = param.value + # Recreate the timer with new dt + self.timer.cancel() + self.timer = self.create_timer(self.dt, self.control_loop) + update_needed = True + + # Update configuration if parameters changed + if update_needed: + self.update_configuration() + + return True # Accept all parameter changes + def odom_callback(self, msg: Odometry): """ Callback to update the latest odometry measurement. @@ -196,10 +290,15 @@ def odom_callback(self, msg: Odometry): msg.pose.pose.orientation.w, ] ).as_euler("xyz", degrees=False) + + # Get filter parameters from ROS parameters + filter_omega = self.get_parameter('filter.omega').value + filter_delta = self.get_parameter('filter.delta').value + self.ref_filter = ThirdOrderReferenceFilter( dt=self.dt, - omega=[0.15, 0.15, 0.15], - delta=[0.8, 0.8, 0.8], + omega=filter_omega, + delta=filter_delta, initial_eta=[self.target_x, self.target_y, self.target_yaw], ) self.ref_filter.eta_d = np.array( @@ -297,7 +396,7 @@ def process_performance_metrics(self, desired_pose, desired_vel, error_pos, erro def control_loop(self): """ - Control loop that computes and publishes the control command. + Periodic control loop to update reference filter and publish control commands. This version uses a 3rd order reference filter to generate a smooth desired trajectory. """ if self.latest_odom is None: @@ -356,14 +455,8 @@ def control_loop(self): # Apply saturation to the integral error self.integral_error_pos = np.clip( self.integral_error_pos, -self.max_integral_error_pos, self.max_integral_error_pos) - # self.integral_error_pos = self.max_integral_error_pos * saturate( - # self.integral_error_pos, self.saturation_pos - # ) self.integral_error_yaw = np.clip( self.integral_error_yaw, -self.max_integral_error_yaw, self.max_integral_error_yaw) - # self.integral_error_yaw = self.max_integral_error_yaw * saturate( - # self.integral_error_yaw, self.saturation_yaw - # ) # Compute control commands. # For position, we use feedforward desired acceleration plus a PID correction. diff --git a/cybership_tests/cybership_tests/velocity_control.py b/cybership_tests/cybership_tests/velocity_control.py index 85bdb1a..d8d85db 100755 --- a/cybership_tests/cybership_tests/velocity_control.py +++ b/cybership_tests/cybership_tests/velocity_control.py @@ -11,6 +11,7 @@ import numpy as np import rclpy from rclpy.node import Node +from rclpy.parameter import Parameter from geometry_msgs.msg import Twist, Wrench from nav_msgs.msg import Odometry import shoeboxpy.model3dof as box @@ -34,28 +35,49 @@ class VelocityControlNode(Node): def __init__(self): super().__init__("velocity_control_node", namespace="voyager") - # Create a velocity controller - self.dt = 0.1 # seconds - self.vessel = box.Shoebox(L=1.0, B=0.3, T=0.05) + # Declare parameters with default values + self.declare_parameters( + namespace='', + parameters=[ + # Vessel dimensions + ('vessel.length', 1.0), + ('vessel.beam', 0.3), + ('vessel.draft', 0.05), + + # Control gains + ('control.p_gain.surge', 5.0), + ('control.p_gain.sway', 5.0), + ('control.p_gain.yaw', 5.0), + + ('control.i_gain.surge', 0.0), + ('control.i_gain.sway', 0.0), + ('control.i_gain.yaw', 0.0), + + ('control.d_gain.surge', 0.3), + ('control.d_gain.sway', 0.3), + ('control.d_gain.yaw', 0.3), + + ('control.i_max', 20.0), + ('control.smooth_limit', True), + ('control.filter_alpha', 0.1), + + # Performance metrics + ('metrics.window_size', 50), + ('metrics.interval', 1.0), + + # Time step + ('dt', 0.1), + ] + ) - # # Control gains for surge, sway, and yaw - self.k_p_gain = np.array([5.0, 5.0, 5.0]) - self.k_i_gain = np.array([0.0, 0.0, 0.0]) - self.k_d_gain = np.array([0.3, 0.3, 0.3]) + # Add parameter callback for runtime updates + self.add_on_set_parameters_callback(self.parameters_callback) - self.controller = VelocityController( - config={ - "M": self.vessel.M_eff, - "D": self.vessel.D, - "Kp": np.diag(self.k_p_gain), - "Ki": np.diag(self.k_i_gain), - "Kd": np.diag(self.k_d_gain), - "I_max": 20.0, # Nms - "smooth_limit": True, - "filter_alpha": 0.1, # Smoothing factor for desired velocity - "dt": self.dt, - } - ) + # Get current parameter values + self.dt = self.get_parameter('dt').value + + # Initialize vessel and controller + self.update_configuration() self.nu_prev = np.zeros(3) # [u, v, r] previous velocities self.nu_cmd = np.zeros(3) # [u, v, r] desired velocities @@ -110,18 +132,99 @@ def __init__(self): # --- Performance metrics --- # Performance metrics tracking with moving window - self.window_size = 50 # 2 seconds of data at 100Hz + self.window_size = self.get_parameter('metrics.window_size').value self.error_window = [] # Store recent velocity errors self.start_time = None self.sample_count = 0 # Time between metrics calculations - self.metrics_interval = 1.0 # seconds + self.metrics_interval = self.get_parameter('metrics.interval').value self.last_metrics_time = 0.0 # Timer for control loop self.timer = self.create_timer(self.dt, self.control_loop) + self.get_logger().info("Velocity controller initialized with parameters") + + def update_configuration(self): + """Update vessel and controller configuration from parameters""" + # Get vessel dimensions + vessel_length = self.get_parameter('vessel.length').value + vessel_beam = self.get_parameter('vessel.beam').value + vessel_draft = self.get_parameter('vessel.draft').value + + # Create vessel model + self.vessel = box.Shoebox(L=vessel_length, B=vessel_beam, T=vessel_draft) + + # Get control gains + self.k_p_gain = np.array([ + self.get_parameter('control.p_gain.surge').value, + self.get_parameter('control.p_gain.sway').value, + self.get_parameter('control.p_gain.yaw').value + ]) + + self.k_i_gain = np.array([ + self.get_parameter('control.i_gain.surge').value, + self.get_parameter('control.i_gain.sway').value, + self.get_parameter('control.i_gain.yaw').value + ]) + + self.k_d_gain = np.array([ + self.get_parameter('control.d_gain.surge').value, + self.get_parameter('control.d_gain.sway').value, + self.get_parameter('control.d_gain.yaw').value + ]) + + # Create/update controller + self.controller = VelocityController( + config={ + "M": self.vessel.M_eff, + "D": self.vessel.D, + "Kp": np.diag(self.k_p_gain), + "Ki": np.diag(self.k_i_gain), + "Kd": np.diag(self.k_d_gain), + "I_max": self.get_parameter('control.i_max').value, + "smooth_limit": self.get_parameter('control.smooth_limit').value, + "filter_alpha": self.get_parameter('control.filter_alpha').value, + "dt": self.dt, + } + ) + + # Update metrics parameters + self.window_size = self.get_parameter('metrics.window_size').value + self.metrics_interval = self.get_parameter('metrics.interval').value + + self.get_logger().info(f"Updated configuration - vessel: [{vessel_length}, {vessel_beam}, {vessel_draft}], " + + f"P: {self.k_p_gain}, I: {self.k_i_gain}, D: {self.k_d_gain}") + + def parameters_callback(self, params): + """Handle parameter updates""" + update_needed = False + + for param in params: + if param.name in [ + 'vessel.length', 'vessel.beam', 'vessel.draft', + 'control.p_gain.surge', 'control.p_gain.sway', 'control.p_gain.yaw', + 'control.i_gain.surge', 'control.i_gain.sway', 'control.i_gain.yaw', + 'control.d_gain.surge', 'control.d_gain.sway', 'control.d_gain.yaw', + 'control.i_max', 'control.smooth_limit', 'control.filter_alpha', + 'metrics.window_size', 'metrics.interval' + ]: + update_needed = True + + if param.name == 'dt': + self.dt = param.value + # Recreate the timer with new dt + self.timer.cancel() + self.timer = self.create_timer(self.dt, self.control_loop) + update_needed = True + + # Update vessel and controller if parameters changed + if update_needed: + self.update_configuration() + + return True # Accept all parameter changes + def cmd_vel_callback(self, msg: Twist): """ Process incoming velocity commands. From 6f8ad06cba54559402d7633a01873c8850b4c721 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Thu, 10 Jul 2025 15:01:13 +0200 Subject: [PATCH 03/46] fix: fix them all transformations, omg, there are so many! --- .../urdf/model/voyager/base.urdf.xacro | 13 +- .../cybership_dp/voyager/force_controller.py | 4 +- cybership_observer/CMakeLists.txt | 13 +- cybership_observer/nodes/enu_to_ned_odom.py | 113 +++++++----------- cybership_observer/nodes/ned_to_enu_odom.py | 0 cybership_observer/package.xml | 1 + .../src/cybership_simulator/base.py | 10 +- .../src/cybership_simulator/enterprise.py | 25 ++-- .../src/cybership_simulator/voyager.py | 56 +++++---- .../reference_filter_server.py | 14 ++- cybership_viz/config/voyager.rviz | 50 +++++--- 11 files changed, 167 insertions(+), 132 deletions(-) create mode 100644 cybership_observer/nodes/ned_to_enu_odom.py diff --git a/cybership_description/urdf/model/voyager/base.urdf.xacro b/cybership_description/urdf/model/voyager/base.urdf.xacro index 34c7b48..3eb5c36 100644 --- a/cybership_description/urdf/model/voyager/base.urdf.xacro +++ b/cybership_description/urdf/model/voyager/base.urdf.xacro @@ -21,6 +21,13 @@ + + + + + + + @@ -39,20 +46,20 @@ - + - + - + \ No newline at end of file diff --git a/cybership_dp/cybership_dp/voyager/force_controller.py b/cybership_dp/cybership_dp/voyager/force_controller.py index d3ef67e..03d347b 100644 --- a/cybership_dp/cybership_dp/voyager/force_controller.py +++ b/cybership_dp/cybership_dp/voyager/force_controller.py @@ -107,7 +107,7 @@ def _initialize_thrusters(self): extra_attributes={ "rate_limit": 0.1, "saturation_limit": 1.0, - "reference_angle": -np.pi / 2.0, + "reference_angle": np.pi / 2.0, "name": "port_azimuth", }, ) @@ -116,7 +116,7 @@ def _initialize_thrusters(self): extra_attributes={ "rate_limit": 0.1, "saturation_limit": 1.0, - "reference_angle": np.pi / 2.0, + "reference_angle": -np.pi / 2.0, "name": "starboard_azimuth", }, ) diff --git a/cybership_observer/CMakeLists.txt b/cybership_observer/CMakeLists.txt index 77edb1d..4fce58b 100644 --- a/cybership_observer/CMakeLists.txt +++ b/cybership_observer/CMakeLists.txt @@ -15,25 +15,26 @@ find_package(tf2_ros REQUIRED) include_directories(include) -add_executable(ned_to_enu_transformer src/ned_to_enu_transformer.cpp) -ament_target_dependencies(ned_to_enu_transformer +add_executable(ned_to_enu_transformer_cpp src/ned_to_enu_transformer.cpp) +ament_target_dependencies(ned_to_enu_transformer_cpp rclcpp std_msgs geometry_msgs tf2_ros ) + +file(GLOB NODE_SCRIPTS ${CMAKE_CURRENT_SOURCE_DIR}/nodes/*.py) + install( PROGRAMS - nodes/enu_to_ned_twist.py - nodes/enu_to_ned_odom.py - nodes/ned_to_enu_transformer.py + ${NODE_SCRIPTS} DESTINATION lib/${PROJECT_NAME} ) install( TARGETS - ned_to_enu_transformer + ned_to_enu_transformer_cpp DESTINATION lib/${PROJECT_NAME} ) diff --git a/cybership_observer/nodes/enu_to_ned_odom.py b/cybership_observer/nodes/enu_to_ned_odom.py index 708582a..d4c3dfe 100755 --- a/cybership_observer/nodes/enu_to_ned_odom.py +++ b/cybership_observer/nodes/enu_to_ned_odom.py @@ -2,90 +2,67 @@ import rclpy from rclpy.node import Node -from geometry_msgs.msg import PoseWithCovarianceStamped from nav_msgs.msg import Odometry -import tf_transformations -import numpy as np +from geometry_msgs.msg import Pose, PoseWithCovariance, TwistWithCovariance, Quaternion -class NEDToENUPublisher(Node): +from tf_transformations import euler_from_quaternion, quaternion_from_euler +class OdomEnuToNedNode(Node): def __init__(self): - super().__init__('ned_to_enu_publisher') - - # Declare and get the frame_id parameter - self.declare_parameter('frame_id', 'world') - self.frame_id = self.get_parameter( - 'frame_id').get_parameter_value().string_value - - # Subscriber to the NED frame PoseWithCovarianceStamped messages - self.ned_sub = self.create_subscription( + super().__init__('odom_enu_to_ned') + self.sub = self.create_subscription( Odometry, 'in_odom', - self.ned_callback, - 10 - ) - - # Publisher for the transformed ENU frame PoseWithCovarianceStamped messages - self.enu_pub = self.create_publisher( - Odometry, - 'out_odom', + self.odom_callback, 10 ) + self.declare_parameter('frame_id', 'world') + self.pub = self.create_publisher(Odometry, 'out_odom', 10) - def ned_callback(self, msg): - # Transform the pose from NED to ENU frame - enu_msg = Odometry() - enu_msg.header = msg.header - enu_msg.header.frame_id = self.frame_id # Set the frame_id parameter - - enu_msg.child_frame_id = msg.child_frame_id + def odom_callback(self, msg): + ned_msg = Odometry() + ned_msg.header = msg.header + ned_msg.child_frame_id = "base_link_ned" # Position transformation - enu_msg.pose.pose.position.x = - msg.pose.pose.position.y - enu_msg.pose.pose.position.y = - msg.pose.pose.position.x - enu_msg.pose.pose.position.z = - msg.pose.pose.position.z - - enu_msg.twist.twist.linear.x = msg.twist.twist.linear.y - enu_msg.twist.twist.linear.y = msg.twist.twist.linear.x - enu_msg.twist.twist.linear.z = - msg.twist.twist.linear.z - - enu_msg.twist.twist.angular.x = msg.twist.twist.angular.x - enu_msg.twist.twist.angular.y = - msg.twist.twist.angular.y - enu_msg.twist.twist.angular.z = - msg.twist.twist.angular.z - - - # Orientation transformation (quaternion) - q_ned = [ - msg.pose.pose.orientation.x, - msg.pose.pose.orientation.y, - msg.pose.pose.orientation.z, - msg.pose.pose.orientation.w - ] - - roll, pitch, yaw = tf_transformations.euler_from_quaternion( - q_ned, axes='sxyz') # Check the orientation in Euler angles - - q_enu = tf_transformations.quaternion_from_euler( - roll, pitch, -yaw - np.pi / 2.0, axes='sxyz') - - enu_msg.pose.pose.orientation.x = q_enu[0] - enu_msg.pose.pose.orientation.y = q_enu[1] - enu_msg.pose.pose.orientation.z = q_enu[2] - enu_msg.pose.pose.orientation.w = q_enu[3] - - # Copy the covariance matrix as it is (assuming it remains the same) - enu_msg.pose.covariance = msg.pose.covariance - - # Publish the transformed message - self.enu_pub.publish(enu_msg) - + ned_msg.pose.pose.position.x = msg.pose.pose.position.y + ned_msg.pose.pose.position.y = msg.pose.pose.position.x + ned_msg.pose.pose.position.z = -msg.pose.pose.position.z + + # Orientation transformation + # Convert quaternion to euler + q = msg.pose.pose.orientation + euler = euler_from_quaternion([q.x, q.y, q.z, q.w]) + + roll = euler[0] + pitch = -euler[1] + yaw = -euler[2] + + # Convert back to quaternion + new_q = quaternion_from_euler(roll, pitch, yaw) + ned_msg.pose.pose.orientation = Quaternion() + ned_msg.pose.pose.orientation.x = new_q[0] + ned_msg.pose.pose.orientation.y = new_q[1] + ned_msg.pose.pose.orientation.z = new_q[2] + ned_msg.pose.pose.orientation.w = new_q[3] + + # Velocity transformation + ned_msg.twist.twist.linear.x = msg.twist.twist.linear.y + ned_msg.twist.twist.linear.y = msg.twist.twist.linear.x + ned_msg.twist.twist.linear.z = -msg.twist.twist.linear.z + + ned_msg.twist.twist.angular.x = msg.twist.twist.angular.y + ned_msg.twist.twist.angular.y = msg.twist.twist.angular.x + ned_msg.twist.twist.angular.z = -msg.twist.twist.angular.z + + self.pub.publish(ned_msg) def main(args=None): rclpy.init(args=args) - node = NEDToENUPublisher() + node = OdomEnuToNedNode() rclpy.spin(node) + node.destroy_node() rclpy.shutdown() - if __name__ == '__main__': main() diff --git a/cybership_observer/nodes/ned_to_enu_odom.py b/cybership_observer/nodes/ned_to_enu_odom.py new file mode 100644 index 0000000..e69de29 diff --git a/cybership_observer/package.xml b/cybership_observer/package.xml index 4600177..18e8043 100644 --- a/cybership_observer/package.xml +++ b/cybership_observer/package.xml @@ -11,6 +11,7 @@ std_msgs rclpy robot_localization + tf_transformations ament_cmake ament_lint_auto diff --git a/cybership_simulator/src/cybership_simulator/base.py b/cybership_simulator/src/cybership_simulator/base.py index 0ce7a2a..38b3e8d 100644 --- a/cybership_simulator/src/cybership_simulator/base.py +++ b/cybership_simulator/src/cybership_simulator/base.py @@ -77,6 +77,8 @@ def __init__(self, node_name="base_simulator"): self.dt = 0.01 self.timer = self.create_timer(self.dt, self.iterate) + self.body_frame_id = "base_link_ned" + # ------------------------------------------------------------------------ # ABSTRACT METHODS (to be implemented in subclasses) # ------------------------------------------------------------------------ @@ -124,6 +126,8 @@ def _declare_parameters(self): ('yaw_rate', 0.0), ] ) + self.declare_parameter('frame_id', 'world') + self.declare_parameter('body_frame_id', 'base_link_ned') def _read_parameters(self): self.eta0[0] = self.get_parameter('initial_conditions.position.x').value @@ -195,7 +199,7 @@ def iterate(self): # # Transform to body fixed frame # F[0:3] = Rz.T @ F[0:3] - self.vessel.eta[2] = 0.0 + # self.vessel.eta[2] = 0.0 self.vessel.step(tau=tau, dt=self.dt) @@ -221,7 +225,7 @@ def publish_odom(self): self.odom.header.frame_id = "world" self.odom.header.stamp = self.get_clock().now().to_msg() - self.odom.child_frame_id = "base_link" + self.odom.child_frame_id = "base_link_ned" self.odom.pose.pose.position.x = eta[0].item() self.odom.pose.pose.position.y = eta[1].item() @@ -249,7 +253,7 @@ def publish_tf(self): t = TransformStamped() t.header.stamp = self.get_clock().now().to_msg() t.header.frame_id = "world" - t.child_frame_id = "base_link" + t.child_frame_id = "base_link_ned" t.transform.translation.x = self.odom.pose.pose.position.x t.transform.translation.y = self.odom.pose.pose.position.y diff --git a/cybership_simulator/src/cybership_simulator/enterprise.py b/cybership_simulator/src/cybership_simulator/enterprise.py index 7785ec1..ddbb62e 100755 --- a/cybership_simulator/src/cybership_simulator/enterprise.py +++ b/cybership_simulator/src/cybership_simulator/enterprise.py @@ -24,19 +24,30 @@ def _create_vessel(self): ) def _init_allocator(self): + + # Thruster definitions are in accordance with NED frame tunnel = skadipy.actuator.Fixed( position=skadipy.toolbox.Point([0.3875, 0.0, -0.01]), orientation=skadipy.toolbox.Quaternion( axis=(0.0, 0.0, 1.0), radians=np.pi / 2.0 ) ) + + # X = -0.4574, Y = -0.055, Z = -0.1 port_azimuth = skadipy.actuator.Azimuth( position=skadipy.toolbox.Point([-0.4574, -0.055, -0.1]), ) + # X = -0.4547, Y = 0.055, Z = -0.1 starboard_azimuth = skadipy.actuator.Azimuth( position=skadipy.toolbox.Point([-0.4547, 0.055, -0.1]), ) - actuators = [tunnel, port_azimuth, starboard_azimuth] + + # This order is important when unpacking the command vector + actuators = [ + tunnel, + port_azimuth, + starboard_azimuth + ] dofs = [ skadipy.allocator._base.ForceTorqueComponent.X, skadipy.allocator._base.ForceTorqueComponent.Y, @@ -86,25 +97,25 @@ def cb_tunnel_thruster(self, msg: geometry_msgs.msg.Wrench): issued.wrench.force.x = msg.force.x self.publisher_tunnel_thruster.publish(issued) - def cb_starboard_thruster(self, msg: geometry_msgs.msg.Wrench): + def cb_port_thruster(self, msg: geometry_msgs.msg.Wrench): self.u[1] = np.clip(msg.force.x, -1.0, 1.0) self.u[2] = np.clip(msg.force.y, -1.0, 1.0) issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "stern_starboard_thruster_link" + issued.header.frame_id = "stern_port_thruster_link" issued.header.stamp = self.get_clock().now().to_msg() issued.wrench.force.x = msg.force.x issued.wrench.force.y = msg.force.y - self.publisher_starboard_thruster.publish(issued) + self.publisher_port_thruster.publish(issued) - def cb_port_thruster(self, msg: geometry_msgs.msg.Wrench): + def cb_starboard_thruster(self, msg: geometry_msgs.msg.Wrench): self.u[3] = np.clip(msg.force.x, -1.0, 1.0) self.u[4] = np.clip(msg.force.y, -1.0, 1.0) issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "stern_port_thruster_link" + issued.header.frame_id = "stern_starboard_thruster_link" issued.header.stamp = self.get_clock().now().to_msg() issued.wrench.force.x = msg.force.x issued.wrench.force.y = msg.force.y - self.publisher_port_thruster.publish(issued) + self.publisher_starboard_thruster.publish(issued) # ---------------------------------------------------------------------------- # Main entry point diff --git a/cybership_simulator/src/cybership_simulator/voyager.py b/cybership_simulator/src/cybership_simulator/voyager.py index b6702c7..0c87a81 100755 --- a/cybership_simulator/src/cybership_simulator/voyager.py +++ b/cybership_simulator/src/cybership_simulator/voyager.py @@ -8,6 +8,7 @@ import rclpy import geometry_msgs.msg + class VoyagerSimulator(BaseSimulator): """ Concrete simulator for the Voyager vessel. @@ -36,7 +37,12 @@ def _init_allocator(self): starboard_azimuth = skadipy.actuator.Azimuth( position=skadipy.toolbox.Point([-0.4547, 0.055, -0.1]), ) - actuators = [tunnel, port_azimuth, starboard_azimuth] + # This order is important when unpacking the command vector + actuators = [ + tunnel, + port_azimuth, + starboard_azimuth + ] dofs = [ skadipy.allocator._base.ForceTorqueComponent.X, skadipy.allocator._base.ForceTorqueComponent.Y, @@ -90,41 +96,49 @@ def cb_tunnel_thruster(self, msg: geometry_msgs.msg.Wrench): issued.wrench.force.x = msg.force.x self.publisher_tunnel_thruster.publish(issued) - def cb_starboard_thruster(self, msg: geometry_msgs.msg.Wrench): - self.u[1] = np.clip(msg.force.x, -1.0, 1.0) - self.u[2] = np.clip(msg.force.y, -1.0, 1.0) + def cb_port_thruster(self, msg: geometry_msgs.msg.Wrench): + fx = np.clip(msg.force.x, -1.0, 1.0) + fy = np.clip(msg.force.y, -1.0, 1.0) # Apply a deadband to the thruster commands - if np.linalg.norm(self.u[1:3]) < 0.1: - self.u[1] = 0.0 - self.u[2] = 0.0 + if np.linalg.norm([fx, fy]) < 0.1: + fx = 0.0 + fy = 0.0 + + self.u[1] = fx + self.u[2] = fy issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "stern_starboard_thruster_link" + issued.header.frame_id = "stern_port_thruster_link" issued.header.stamp = self.get_clock().now().to_msg() - issued.wrench.force.x = msg.force.x - issued.wrench.force.y = msg.force.y - self.publisher_starboard_thruster.publish(issued) + issued.wrench.force.x = fx + issued.wrench.force.y = fy + self.publisher_port_thruster.publish(issued) - def cb_port_thruster(self, msg: geometry_msgs.msg.Wrench): - self.u[3] = np.clip(msg.force.x, -1.0, 1.0) - self.u[4] = np.clip(msg.force.y, -1.0, 1.0) + def cb_starboard_thruster(self, msg: geometry_msgs.msg.Wrench): + fx = np.clip(msg.force.x, -1.0, 1.0) + fy = np.clip(msg.force.y, -1.0, 1.0) - if np.linalg.norm(self.u[3:5]) < 0.1: - self.u[3] = 0.0 - self.u[4] = 0.0 + # Apply a deadband to the thruster commands + if np.linalg.norm([fx, fy]) < 0.1: + fx = 0.0 + fy = 0.0 + + self.u[3] = fx + self.u[4] = fy issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "stern_port_thruster_link" + issued.header.frame_id = "stern_starboard_thruster_link" issued.header.stamp = self.get_clock().now().to_msg() - issued.wrench.force.x = msg.force.x - issued.wrench.force.y = msg.force.y - self.publisher_port_thruster.publish(issued) + issued.wrench.force.x = fx + issued.wrench.force.y = fy + self.publisher_starboard_thruster.publish(issued) # ---------------------------------------------------------------------------- # Main entry point # ---------------------------------------------------------------------------- + def main(args=None): rclpy.init(args=args) diff --git a/cybership_tests/cybership_tests/reference_filter_server.py b/cybership_tests/cybership_tests/reference_filter_server.py index bfccc34..bf6af14 100755 --- a/cybership_tests/cybership_tests/reference_filter_server.py +++ b/cybership_tests/cybership_tests/reference_filter_server.py @@ -119,6 +119,17 @@ def __init__(self): ] ) + # Initialize the 3rd order reference filter. + # Here, we treat [x, y, yaw] as the 3D pose to be smoothed. + self.ref_filter = None + + self.start_time = None + + self.error_window = [] # For position error + self.error_yaw_window = [] # For yaw error + self.sample_count = 0 + self.last_metrics_time = 0.0 + # Add parameter callback for runtime updates self.add_on_set_parameters_callback(self.parameters_callback) @@ -169,9 +180,6 @@ def __init__(self): self.error_pos = np.zeros(2) self.error_yaw = 0.0 - # Initialize the 3rd order reference filter. - # Here, we treat [x, y, yaw] as the 3D pose to be smoothed. - self.ref_filter = None self.get_logger().info( "Goto Point Controller (Reference Filter Version) Initialized with parameters." diff --git a/cybership_viz/config/voyager.rviz b/cybership_viz/config/voyager.rviz index 976167b..035ddb9 100644 --- a/cybership_viz/config/voyager.rviz +++ b/cybership_viz/config/voyager.rviz @@ -6,6 +6,9 @@ Panels: Expanded: - /Global Options1 - /Status1 + - /TF1 + - /TF1/Frames1 + - /TF1/Tree1 - /Odometry1/Shape1 - /Wrench4/Topic1 - /RobotModel1/Mass Properties1 @@ -53,11 +56,15 @@ Visualization Manager: Value: true - Class: rviz_default_plugins/TF Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" Frame Timeout: 15 Frames: All Enabled: true base_link: Value: true + base_link_ned: + Value: true bow_tunnel_thruster_link: Value: true imu_link: @@ -77,17 +84,18 @@ Visualization Manager: Show Names: false Tree: world: - base_link: - bow_tunnel_thruster_link: - {} - imu_link: - {} - mocap_link: - {} - stern_port_thruster_link: - {} - stern_starboard_thruster_link: - {} + base_link_ned: + base_link: + bow_tunnel_thruster_link: + {} + imu_link: + {} + mocap_link: + {} + stern_port_thruster_link: + {} + stern_starboard_thruster_link: + {} Update Interval: 0 Value: true - Accept NaN Values: false @@ -199,7 +207,7 @@ Visualization Manager: Scale: 1 Value: true Value: true - Enabled: true + Enabled: false Keep: 100 Name: Odometry Position Tolerance: 0.10000000149011612 @@ -220,7 +228,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: measurement/odom - Value: true + Value: false - Accept NaN Values: false Alpha: 1 Arrow Width: 0.5 @@ -240,7 +248,7 @@ Visualization Manager: Torque Arrow Scale: 2 Torque Color: 204; 204; 51 Value: true - - Alpha: 1 + - Alpha: 0.4000000059604645 Class: rviz_default_plugins/RobotModel Collision Enabled: false Description File: "" @@ -263,6 +271,10 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + base_link_ned: + Alpha: 1 + Show Axes: false + Show Trail: false bow_tunnel_thruster_link: Alpha: 1 Show Axes: false @@ -337,7 +349,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 10 + Distance: 3.9361820220947266 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -349,7 +361,7 @@ Visualization Manager: Z: 0 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false + Invert Z Axis: true Name: Current View Near Clip Distance: 0.009999999776482582 Pitch: 0.7853981852531433 @@ -363,7 +375,7 @@ Window Geometry: Height: 846 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000018e000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000031c000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000018e000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000033100000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000031c000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -373,5 +385,5 @@ Window Geometry: Views: collapsed: true Width: 1200 - X: 2146 - Y: 537 + X: 1985 + Y: 914 From 468d8b5d2920da87405af32f8c57cce67d873490 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Fri, 11 Jul 2025 02:52:58 +0200 Subject: [PATCH 04/46] feat: wowhohohow so much have changed. absolutely no idea how to write them now. --- .../config/drillship/controller_position.yaml | 55 ++++ .../config/drillship/controller_velocity.yaml | 51 +++ .../enterprise/controller_position.yaml | 55 ++++ .../enterprise/controller_velocity.yaml | 51 +++ .../config/voyager/controller_velocity.yaml | 44 ++- .../config/voyager/mocap_transformer.yaml | 2 +- .../config/voyager/thruster_control.yaml | 6 +- cybership_description/README.md | 7 +- .../urdf/model/enterprise/base.urdf.xacro | 7 + .../urdf/model/voyager/base.urdf.xacro | 6 +- cybership_dp/CMakeLists.txt | 13 +- cybership_dp/config/enterprise/.gitkeep | 0 cybership_dp/config/force_controller.yaml | 3 - cybership_dp/config/velocity_controller.yaml | 18 -- cybership_dp/config/voyager/.gitkeep | 0 .../enterprise/force_controller.py | 158 +++++++++ .../cybership_dp/force_control/.gitkeep | 0 .../cybership_dp/position_control/.gitkeep | 0 .../cybership_dp/velocity_control/__init__.py | 0 .../velocity_control/backstepping.py | 37 --- .../voyager/velocity_controller.py | 306 ------------------ cybership_dp/launch/dp.launch.py | 53 +-- .../launch/position_controller.launch.py | 71 ++++ ....launch.py => thrust_allocation.launch.py} | 28 +- .../launch/velocity_controller.launch.py | 53 ++- .../nodes/position_control_node.py | 6 +- ...ontroller.py => thrust_allocation_node.py} | 0 .../nodes/velocity_control_node.py | 4 +- cybership_dp/nodes/velocity_controller.py | 22 ++ cybership_teleop/config/allocation/ps5.yaml | 6 +- cybership_teleop/config/simple/ps5.yaml | 4 +- .../four_corner_line_of_sight.py | 286 ---------------- cybership_thrusters/src/azimuth.cpp | 4 + 33 files changed, 617 insertions(+), 739 deletions(-) create mode 100644 cybership_config/config/drillship/controller_position.yaml create mode 100644 cybership_config/config/drillship/controller_velocity.yaml create mode 100644 cybership_config/config/enterprise/controller_position.yaml create mode 100644 cybership_config/config/enterprise/controller_velocity.yaml delete mode 100644 cybership_dp/config/enterprise/.gitkeep delete mode 100644 cybership_dp/config/force_controller.yaml delete mode 100644 cybership_dp/config/velocity_controller.yaml delete mode 100644 cybership_dp/config/voyager/.gitkeep create mode 100644 cybership_dp/cybership_dp/enterprise/force_controller.py delete mode 100644 cybership_dp/cybership_dp/force_control/.gitkeep delete mode 100644 cybership_dp/cybership_dp/position_control/.gitkeep delete mode 100644 cybership_dp/cybership_dp/velocity_control/__init__.py delete mode 100644 cybership_dp/cybership_dp/velocity_control/backstepping.py delete mode 100644 cybership_dp/cybership_dp/voyager/velocity_controller.py create mode 100644 cybership_dp/launch/position_controller.launch.py rename cybership_dp/launch/{force_controller.launch.py => thrust_allocation.launch.py} (69%) rename cybership_tests/cybership_tests/reference_filter_server.py => cybership_dp/nodes/position_control_node.py (99%) rename cybership_dp/nodes/{force_controller.py => thrust_allocation_node.py} (100%) rename cybership_tests/cybership_tests/velocity_control.py => cybership_dp/nodes/velocity_control_node.py (99%) delete mode 100755 cybership_tests/cybership_tests/four_corner_line_of_sight.py diff --git a/cybership_config/config/drillship/controller_position.yaml b/cybership_config/config/drillship/controller_position.yaml new file mode 100644 index 0000000..4d7798e --- /dev/null +++ b/cybership_config/config/drillship/controller_position.yaml @@ -0,0 +1,55 @@ +/**: + ros__parameters: + # Vessel physical dimensions (meters) + vessel: + length: 1.0 # Length of vessel (m) + beam: 0.3 # Width of vessel (m) + draft: 0.02 # Draft/depth of vessel (m) + + # Controller parameters + control: + # Position control gains + p_gain: + pos: 4.0 # Proportional gain for position + vel: 0.7 # Proportional gain for velocity + yaw: 1.3 # Proportional gain for yaw + + # Integral gains + i_gain: + pos: 0.2 # Integral gain for position + vel: 0.1 # Integral gain for velocity + yaw: 0.2 # Integral gain for yaw + + # Derivative gains + d_gain: + pos: 0.2 # Derivative gain for position + vel: 0.5 # Derivative gain for velocity + yaw: 1.0 # Derivative gain for yaw + + # Integral error limits + max_integral_error: + pos: 1.0 # Maximum position error integration + yaw: 1.4 # Maximum yaw error integration + + # Saturation parameters + saturation: + pos: 0.1 # Position control saturation + yaw: 0.1 # Yaw control saturation + + # Target reaching tolerances + tolerance: + pos: 0.25 # Position reaching tolerance (m) + yaw: 0.1 # Yaw angle reaching tolerance (rad) + + # Reference filter parameters + filter: + omega: [0.15, 0.15, 0.15] # Natural frequency for reference filter + delta: [0.8, 0.8, 0.8] # Damping ratio for reference filter + + # Performance metrics configuration + metrics: + window_size: 200 # Number of samples in moving window + interval: 1.0 # Time between metrics calculations (seconds) + + # General parameters + dt: 0.01 # Control loop time step (seconds) diff --git a/cybership_config/config/drillship/controller_velocity.yaml b/cybership_config/config/drillship/controller_velocity.yaml new file mode 100644 index 0000000..cebf2df --- /dev/null +++ b/cybership_config/config/drillship/controller_velocity.yaml @@ -0,0 +1,51 @@ +# Configuration for velocity_control.py node +# Node: /cybership/velocity_control_node +# This configuration supports the reference feedforward velocity controller + +/**: + ros__parameters: + # Vessel physical dimensions (meters) + # These parameters define the physical characteristics of the vessel + # and are used to calculate the vessel's dynamic model + vessel: + length: 1.0 # Length of vessel (L) in meters + beam: 0.3 # Width of vessel (B) in meters + draft: 0.05 # Draft (depth) of vessel (T) in meters + + # Control parameters for PID velocity controller + # The controller uses reference feedforward with PID feedback + control: + # Proportional gains for each degree of freedom (DOF) + # Higher values provide faster response but may cause oscillations + p_gain: + surge: 5.0 # Forward/backward motion (u) + sway: 5.0 # Left/right motion (v) + yaw: 5.0 # Rotational motion (r) + + # Integral gains for each DOF + # Helps eliminate steady-state errors + i_gain: + surge: 0.0 # Usually kept low or zero for velocity control + sway: 0.0 + yaw: 0.0 + + # Derivative gains for each DOF + # Provides damping and improves stability + d_gain: + surge: 0.3 # Helps reduce overshoot + sway: 0.3 + yaw: 0.3 + + # Control limits and filtering + i_max: 20.0 # Maximum integral contribution (anti-windup) + smooth_limit: true # Enable smooth limiting for control outputs + filter_alpha: 0.1 # Smoothing factor for desired velocity (0-1, lower = more smoothing) + + # Performance metrics configuration + # Used for monitoring and debugging controller performance + metrics: + window_size: 50 # Number of samples in moving window for statistics + interval: 1.0 # Time between metrics calculations (seconds) + + # Timing parameters + dt: 0.1 # Control loop time step (seconds) - affects timer frequency diff --git a/cybership_config/config/enterprise/controller_position.yaml b/cybership_config/config/enterprise/controller_position.yaml new file mode 100644 index 0000000..4d7798e --- /dev/null +++ b/cybership_config/config/enterprise/controller_position.yaml @@ -0,0 +1,55 @@ +/**: + ros__parameters: + # Vessel physical dimensions (meters) + vessel: + length: 1.0 # Length of vessel (m) + beam: 0.3 # Width of vessel (m) + draft: 0.02 # Draft/depth of vessel (m) + + # Controller parameters + control: + # Position control gains + p_gain: + pos: 4.0 # Proportional gain for position + vel: 0.7 # Proportional gain for velocity + yaw: 1.3 # Proportional gain for yaw + + # Integral gains + i_gain: + pos: 0.2 # Integral gain for position + vel: 0.1 # Integral gain for velocity + yaw: 0.2 # Integral gain for yaw + + # Derivative gains + d_gain: + pos: 0.2 # Derivative gain for position + vel: 0.5 # Derivative gain for velocity + yaw: 1.0 # Derivative gain for yaw + + # Integral error limits + max_integral_error: + pos: 1.0 # Maximum position error integration + yaw: 1.4 # Maximum yaw error integration + + # Saturation parameters + saturation: + pos: 0.1 # Position control saturation + yaw: 0.1 # Yaw control saturation + + # Target reaching tolerances + tolerance: + pos: 0.25 # Position reaching tolerance (m) + yaw: 0.1 # Yaw angle reaching tolerance (rad) + + # Reference filter parameters + filter: + omega: [0.15, 0.15, 0.15] # Natural frequency for reference filter + delta: [0.8, 0.8, 0.8] # Damping ratio for reference filter + + # Performance metrics configuration + metrics: + window_size: 200 # Number of samples in moving window + interval: 1.0 # Time between metrics calculations (seconds) + + # General parameters + dt: 0.01 # Control loop time step (seconds) diff --git a/cybership_config/config/enterprise/controller_velocity.yaml b/cybership_config/config/enterprise/controller_velocity.yaml new file mode 100644 index 0000000..cebf2df --- /dev/null +++ b/cybership_config/config/enterprise/controller_velocity.yaml @@ -0,0 +1,51 @@ +# Configuration for velocity_control.py node +# Node: /cybership/velocity_control_node +# This configuration supports the reference feedforward velocity controller + +/**: + ros__parameters: + # Vessel physical dimensions (meters) + # These parameters define the physical characteristics of the vessel + # and are used to calculate the vessel's dynamic model + vessel: + length: 1.0 # Length of vessel (L) in meters + beam: 0.3 # Width of vessel (B) in meters + draft: 0.05 # Draft (depth) of vessel (T) in meters + + # Control parameters for PID velocity controller + # The controller uses reference feedforward with PID feedback + control: + # Proportional gains for each degree of freedom (DOF) + # Higher values provide faster response but may cause oscillations + p_gain: + surge: 5.0 # Forward/backward motion (u) + sway: 5.0 # Left/right motion (v) + yaw: 5.0 # Rotational motion (r) + + # Integral gains for each DOF + # Helps eliminate steady-state errors + i_gain: + surge: 0.0 # Usually kept low or zero for velocity control + sway: 0.0 + yaw: 0.0 + + # Derivative gains for each DOF + # Provides damping and improves stability + d_gain: + surge: 0.3 # Helps reduce overshoot + sway: 0.3 + yaw: 0.3 + + # Control limits and filtering + i_max: 20.0 # Maximum integral contribution (anti-windup) + smooth_limit: true # Enable smooth limiting for control outputs + filter_alpha: 0.1 # Smoothing factor for desired velocity (0-1, lower = more smoothing) + + # Performance metrics configuration + # Used for monitoring and debugging controller performance + metrics: + window_size: 50 # Number of samples in moving window for statistics + interval: 1.0 # Time between metrics calculations (seconds) + + # Timing parameters + dt: 0.1 # Control loop time step (seconds) - affects timer frequency diff --git a/cybership_config/config/voyager/controller_velocity.yaml b/cybership_config/config/voyager/controller_velocity.yaml index 00b3b00..cebf2df 100644 --- a/cybership_config/config/voyager/controller_velocity.yaml +++ b/cybership_config/config/voyager/controller_velocity.yaml @@ -1,39 +1,51 @@ +# Configuration for velocity_control.py node +# Node: /cybership/velocity_control_node +# This configuration supports the reference feedforward velocity controller + /**: ros__parameters: # Vessel physical dimensions (meters) + # These parameters define the physical characteristics of the vessel + # and are used to calculate the vessel's dynamic model vessel: - length: 1.0 # Length of vessel - beam: 0.3 # Width of vessel - draft: 0.05 # Draft (depth) of vessel + length: 1.0 # Length of vessel (L) in meters + beam: 0.3 # Width of vessel (B) in meters + draft: 0.05 # Draft (depth) of vessel (T) in meters - # Control parameters + # Control parameters for PID velocity controller + # The controller uses reference feedforward with PID feedback control: - # Proportional gains for each DOF + # Proportional gains for each degree of freedom (DOF) + # Higher values provide faster response but may cause oscillations p_gain: - surge: 5.0 # Forward/backward - sway: 5.0 # Left/right - yaw: 5.0 # Rotation + surge: 5.0 # Forward/backward motion (u) + sway: 5.0 # Left/right motion (v) + yaw: 5.0 # Rotational motion (r) # Integral gains for each DOF + # Helps eliminate steady-state errors i_gain: - surge: 0.0 + surge: 0.0 # Usually kept low or zero for velocity control sway: 0.0 yaw: 0.0 # Derivative gains for each DOF + # Provides damping and improves stability d_gain: - surge: 0.3 + surge: 0.3 # Helps reduce overshoot sway: 0.3 yaw: 0.3 - i_max: 20.0 # Maximum integral contribution - smooth_limit: true # Enable smooth limiting - filter_alpha: 0.1 # Smoothing factor for desired velocity (0-1) + # Control limits and filtering + i_max: 20.0 # Maximum integral contribution (anti-windup) + smooth_limit: true # Enable smooth limiting for control outputs + filter_alpha: 0.1 # Smoothing factor for desired velocity (0-1, lower = more smoothing) # Performance metrics configuration + # Used for monitoring and debugging controller performance metrics: - window_size: 50 # Number of samples in moving window + window_size: 50 # Number of samples in moving window for statistics interval: 1.0 # Time between metrics calculations (seconds) - # General parameters - dt: 0.1 # Control loop time step (seconds) + # Timing parameters + dt: 0.1 # Control loop time step (seconds) - affects timer frequency diff --git a/cybership_config/config/voyager/mocap_transformer.yaml b/cybership_config/config/voyager/mocap_transformer.yaml index c35c0df..9f3196c 100644 --- a/cybership_config/config/voyager/mocap_transformer.yaml +++ b/cybership_config/config/voyager/mocap_transformer.yaml @@ -2,6 +2,6 @@ ros__parameters: world_frame: world_ned base_frame: mocap_link - rigid_body_name: "1" + rigid_body_name: "0" topic_name: measurement/pose publish_tf: false diff --git a/cybership_config/config/voyager/thruster_control.yaml b/cybership_config/config/voyager/thruster_control.yaml index 6b7d7f8..608861d 100644 --- a/cybership_config/config/voyager/thruster_control.yaml +++ b/cybership_config/config/voyager/thruster_control.yaml @@ -11,7 +11,7 @@ inverted: false angle: topic: hardware/joint/servo_2/angle - inverted: false + inverted: true azimuth_starboard: type: azimuth @@ -23,7 +23,7 @@ inverted: false angle: topic: hardware/joint/servo_1/angle - inverted: false + inverted: true tunnel: type: fixed @@ -32,4 +32,4 @@ force_topic: thruster/tunnel/command signal: topic: hardware/prop/esc_0/scaled - inverted: false \ No newline at end of file + inverted: true \ No newline at end of file diff --git a/cybership_description/README.md b/cybership_description/README.md index 5273366..fc668f1 100644 --- a/cybership_description/README.md +++ b/cybership_description/README.md @@ -19,7 +19,12 @@ This package depends on: - `xacro`: For URDF preprocessing - `urdf`: For URDF parsing -## Usage +## Frames + +> [!NOTE] +> The devices on the vehicle is first defined in the ENU coordinate frame, and then transformed to the NED coordinate frame using `base_link_ned` frame. + +## Usage ### Visualizing the Model diff --git a/cybership_description/urdf/model/enterprise/base.urdf.xacro b/cybership_description/urdf/model/enterprise/base.urdf.xacro index a284fda..36ab5a6 100644 --- a/cybership_description/urdf/model/enterprise/base.urdf.xacro +++ b/cybership_description/urdf/model/enterprise/base.urdf.xacro @@ -1,6 +1,13 @@ + + - - !-- diff --git a/cybership_dp/CMakeLists.txt b/cybership_dp/CMakeLists.txt index 9d53637..aab1b06 100644 --- a/cybership_dp/CMakeLists.txt +++ b/cybership_dp/CMakeLists.txt @@ -8,17 +8,22 @@ find_package(geometry_msgs REQUIRED) install( - DIRECTORY launch config - DESTINATION share/${PROJECT_NAME} + DIRECTORY + launch + DESTINATION + share/${PROJECT_NAME} ) +# Find all python scripts under the "nodes" directory. +file(GLOB NODE_SCRIPTS ${CMAKE_CURRENT_SOURCE_DIR}/nodes/*.py) + install( PROGRAMS - nodes/force_controller.py - nodes/velocity_controller.py + ${NODE_SCRIPTS} DESTINATION lib/${PROJECT_NAME} ) + ament_python_install_package(${PROJECT_NAME}) ament_package() \ No newline at end of file diff --git a/cybership_dp/config/enterprise/.gitkeep b/cybership_dp/config/enterprise/.gitkeep deleted file mode 100644 index e69de29..0000000 diff --git a/cybership_dp/config/force_controller.yaml b/cybership_dp/config/force_controller.yaml deleted file mode 100644 index b8ec1e6..0000000 --- a/cybership_dp/config/force_controller.yaml +++ /dev/null @@ -1,3 +0,0 @@ -/**: - ros__parameters: - frequency: 20.0 diff --git a/cybership_dp/config/velocity_controller.yaml b/cybership_dp/config/velocity_controller.yaml deleted file mode 100644 index a5d0037..0000000 --- a/cybership_dp/config/velocity_controller.yaml +++ /dev/null @@ -1,18 +0,0 @@ -/**: - ros__parameters: - frequency: 10.0 - surge: - p_gain: 5.0 - i_gain: 0.1 - i_limit: 0.1 - d_gain: 1.0 - sway: - p_gain: 5.0 - i_gain: 0.01 - i_limit: 0.1 - d_gain: 2.0 - yaw: - p_gain: 2.0 - i_gain: 0.1 - i_limit: 0.1 - d_gain: 1.0 diff --git a/cybership_dp/config/voyager/.gitkeep b/cybership_dp/config/voyager/.gitkeep deleted file mode 100644 index e69de29..0000000 diff --git a/cybership_dp/cybership_dp/enterprise/force_controller.py b/cybership_dp/cybership_dp/enterprise/force_controller.py new file mode 100644 index 0000000..a98e4d3 --- /dev/null +++ b/cybership_dp/cybership_dp/enterprise/force_controller.py @@ -0,0 +1,158 @@ +import rclpy +import rclpy.node + +import geometry_msgs.msg + +import numpy as np +import skadipy +import skadipy.allocator.reference_filters + + +class ForceControllerROS(rclpy.node.Node): + def __init__(self, *args, **kwargs): + super().__init__( + node_name="enterprise_thrust_allocator", + allow_undeclared_parameters=True, + *args, + **kwargs, + ) + + self.actuators: list[skadipy.actuator.ActuatorBase] = [] + self.allocator: skadipy.allocator.AllocatorBase = None + + self.tau_cmd = np.zeros((6, 1), dtype=np.float32) + self.subs = {} + self.subs["force"] = self.create_subscription( + geometry_msgs.msg.Wrench, "control/force/command", self.force_callback, 10 + ) + + self.pubs = {} + self.pubs["port_thruster"] = self.create_publisher( + geometry_msgs.msg.Wrench, "thruster/port/command", 10 + ) + self.pubs["starboard_thruster"] = self.create_publisher( + geometry_msgs.msg.Wrench, "thruster/starboard/command", 10 + ) + self.pubs["tunnel_thruster"] = self.create_publisher( + geometry_msgs.msg.Wrench, "thruster/tunnel/command", 10 + ) + + self.declare_parameter("frequency", rclpy.Parameter.Type.DOUBLE) + self.freq = ( + self.get_parameter_or("frequency", 1.0).get_parameter_value().double_value + ) + + self._initialize_thrusters() + + self._initialize_allocator() + + self.create_timer(1.0 / self.freq, self.timer_callback) + + def timer_callback(self): + + self.allocator.allocate(tau=self.tau_cmd) + + # Tunnel thruster + u0_f = self.actuators[0].force + msg = geometry_msgs.msg.Wrench() + msg.force.x = float(u0_f[0]) + + self.pubs["tunnel_thruster"].publish(msg) + + # Port azimuth thruster + u1_f = self.actuators[1].force + msg = geometry_msgs.msg.Wrench() + msg.force.x = float(u1_f[0]) + msg.force.y = float(u1_f[1]) + + self.pubs["port_thruster"].publish(msg) + + # Starboard azimuth thruster + u2_f = self.actuators[2].force + msg = geometry_msgs.msg.Wrench() + msg.force.x = float(u2_f[0]) + msg.force.y = float(u2_f[1]) + + self.pubs["starboard_thruster"].publish(msg) + + + def force_callback(self, msg: geometry_msgs.msg.Wrench): + + self.tau_cmd = np.array( + [ + msg.force.x, + msg.force.y, + msg.force.z, + msg.torque.x, + msg.torque.y, + msg.torque.z, + ], + dtype=np.float32, + ).reshape((6, 1)) + + def _initialize_thrusters(self): + tunnel = skadipy.actuator.Fixed( + position=skadipy.toolbox.Point([0.3875, 0.0, 0.0]), + orientation=skadipy.toolbox.Quaternion( + axis=(0.0, 0.0, 1.0), radians=np.pi / 2.0 + ), + extra_attributes={ + "rate_limit": 0.1, + "saturation_limit": 0.7, + "name": "tunnel", + }, + ) + port_azimuth = skadipy.actuator.Azimuth( + position=skadipy.toolbox.Point([-0.4574, -0.055, 0.0]), + extra_attributes={ + "rate_limit": 0.1, + "saturation_limit": 1.0, + "reference_angle": np.pi / 2.0, + "name": "port_azimuth", + }, + ) + starboard_azimuth = skadipy.actuator.Azimuth( + position=skadipy.toolbox.Point([-0.4547, 0.055, 0.0]), + extra_attributes={ + "rate_limit": 0.1, + "saturation_limit": 1.0, + "reference_angle": -np.pi / 2.0, + "name": "starboard_azimuth", + }, + ) + + # Put all actuators in a list and create the allocator object + self.actuators = [ + tunnel, + port_azimuth, + starboard_azimuth, + ] + + def _initialize_allocator(self): + dofs = [ + skadipy.allocator.ForceTorqueComponent.X, + skadipy.allocator.ForceTorqueComponent.Y, + skadipy.allocator.ForceTorqueComponent.N, + ] + self.allocator = skadipy.allocator.reference_filters.MinimumMagnitudeAndAzimuth( + actuators=self.actuators, + force_torque_components=dofs, + gamma=0.001, + mu=0.01, + rho=1, + time_step=(1.0 /self.freq ), + control_barrier_function=skadipy.safety.ControlBarrierFunctionType.SUMSQUARE, + ) + self.allocator.compute_configuration_matrix() + + +def main(args=None): + rclpy.init(args=args) + force_controller = ForceControllerROS() + rclpy.spin(force_controller) + force_controller.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/cybership_dp/cybership_dp/force_control/.gitkeep b/cybership_dp/cybership_dp/force_control/.gitkeep deleted file mode 100644 index e69de29..0000000 diff --git a/cybership_dp/cybership_dp/position_control/.gitkeep b/cybership_dp/cybership_dp/position_control/.gitkeep deleted file mode 100644 index e69de29..0000000 diff --git a/cybership_dp/cybership_dp/velocity_control/__init__.py b/cybership_dp/cybership_dp/velocity_control/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/cybership_dp/cybership_dp/velocity_control/backstepping.py b/cybership_dp/cybership_dp/velocity_control/backstepping.py deleted file mode 100644 index 443e1c8..0000000 --- a/cybership_dp/cybership_dp/velocity_control/backstepping.py +++ /dev/null @@ -1,37 +0,0 @@ -#!/usr/bin/env python3 - -import numpy as np - -class BacksteppingController: - def __init__(self, mass, damping, k_gain): - """ - Initialize the backstepping controller for surge, sway, and yaw. - - Parameters: - mass (array-like): [m_surge, m_sway, m_yaw] parameters. - damping (array-like): [d_surge, d_sway, d_yaw] damping coefficients. - k_gain (array-like): [k_surge, k_sway, k_yaw] control gains. - """ - self.mass = np.array(mass) - self.damping = np.array(damping) - self.k_gain = np.array(k_gain) - - def update(self, v, v_des, dv_des): - """ - Compute the control input vector for surge, sway, and yaw. - - Parameters: - v (np.array): Current velocity vector [surge, sway, yaw]. - v_des (np.array): Desired velocity vector [surge, sway, yaw]. - dv_des (np.array): Time derivative of the desired velocity vector. - - Returns: - u (np.array): Control input vector [u_surge, u_sway, u_yaw]. - """ - # Tracking error for each channel - e = v - v_des - - # Backstepping control law for each channel: - # u = damping * v + mass * (dv_des - k_gain * e) - u = self.damping @ v + self.mass @ (dv_des - self.k_gain * e) - return u diff --git a/cybership_dp/cybership_dp/voyager/velocity_controller.py b/cybership_dp/cybership_dp/voyager/velocity_controller.py deleted file mode 100644 index 2998eaa..0000000 --- a/cybership_dp/cybership_dp/voyager/velocity_controller.py +++ /dev/null @@ -1,306 +0,0 @@ -import rclpy -import rclpy.node - -import geometry_msgs.msg -import nav_msgs.msg -import rcl_interfaces.msg - -import numpy as np - -from cybership_dp.velocity_control.backstepping import BacksteppingController -import shoeboxpy.model6dof as box - - -class PIDVelocityControllerROS(rclpy.node.Node): - def __init__(self, *args, **kwargs): - super().__init__( - node_name="voyager_velocity_controller", - allow_undeclared_parameters=True, - automatically_declare_parameters_from_overrides=False, - ) - - # Declare and get parameters - self._declare_parameters() - self._update_parameters() - - # Initialize messages - self._vel_cmd_msg = geometry_msgs.msg.Twist() - self._odom_msg = nav_msgs.msg.Odometry() - - # Initialize variables - self._previous_error = np.zeros(3, dtype=np.float32) - self._k_pid = np.zeros((3, 3), dtype=np.float32) - - # Initialize subscribers and publishers - self._subs = {} - self._pubs = {} - self._subs["odometry"] = self.create_subscription( - nav_msgs.msg.Odometry, "measurement/odom", self._odom_callback, 10 - ) - self._subs["command"] = self.create_subscription( - geometry_msgs.msg.Twist, - "control/velocity/command", - self._vel_cmd_callback, - 10, - ) - self._pubs["force"] = self.create_publisher( - geometry_msgs.msg.Wrench, "control/force/command", 10 - ) - - # Initialize timer - self._dt = 1.0 / self._freq - self.create_timer(self._dt, self.timer_callback) - - def timer_callback(self): - - self._update_parameters() - - cmd_force = geometry_msgs.msg.Wrench() - - x = np.array( - [ - self._odom_msg.twist.twist.linear.x, - self._odom_msg.twist.twist.linear.y, - self._odom_msg.twist.twist.angular.z, - ], - dtype=np.float32, - ) - - u = np.array( - [ - self._vel_cmd_msg.linear.x, - self._vel_cmd_msg.linear.y, - self._vel_cmd_msg.angular.z, - ], - dtype=np.float32, - ) - - # Calculate error - e = u - x - - # Proportional term - self._k_pid[0] = self._K_P @ e - - # Integral term - self._k_pid[1] += self._K_I @ (e * self._dt) - - # Derivative term - self._k_pid[2] = self._K_D @ (e - self._previous_error) / self._dt - - # Calculate control force - cmd = np.array( - [ - self._k_pid[0][0] + self._k_pid[1][0] + self._k_pid[2][0], - self._k_pid[0][1] + self._k_pid[1][1] + self._k_pid[2][1], - self._k_pid[0][2] + self._k_pid[1][2] + self._k_pid[2][2], - ], - dtype=float, - ) - - cmd_force.force.x = cmd[0] - cmd_force.force.y = cmd[1] - cmd_force.torque.z = cmd[2] - - self._pubs["force"].publish(cmd_force) - self._previous_error = e - - def _vel_cmd_callback(self, msg: geometry_msgs.msg.Twist): - self._vel_cmd_msg = msg - - def _odom_callback(self, msg: geometry_msgs.msg.Wrench): - self._odom_msg = msg - - def _declare_parameters(self): - - self.declare_parameter("frequency", rclpy.Parameter.Type.DOUBLE) - - dofs = ["surge", "sway", "yaw"] - gains = ["p_gain", "i_gain", "i_limit", "d_gain"] - for dof in dofs: - for gain in gains: - self.declare_parameter( - f"{dof}.{gain}", - 0.0, - rcl_interfaces.msg.ParameterDescriptor( - description=f"{dof} {gain}", - type=rcl_interfaces.msg.ParameterType.PARAMETER_DOUBLE, - read_only=False, - ), - ) - - def _update_parameters(self): - self._freq = self.get_parameter("frequency").get_parameter_value().double_value - - self._surge_p_gain = ( - self.get_parameter("surge.p_gain").get_parameter_value().double_value - ) - self._surge_i_gain = ( - self.get_parameter("surge.i_gain").get_parameter_value().double_value - ) - self._surge_d_gain = ( - self.get_parameter("surge.d_gain").get_parameter_value().double_value - ) - self._surge_i_limit = ( - self.get_parameter("surge.i_limit").get_parameter_value().double_value - ) - - self._sway_p_gain = ( - self.get_parameter("sway.p_gain").get_parameter_value().double_value - ) - self._sway_i_gain = ( - self.get_parameter("sway.i_gain").get_parameter_value().double_value - ) - self._sway_d_gain = ( - self.get_parameter("sway.d_gain").get_parameter_value().double_value - ) - self._sway_i_limit = ( - self.get_parameter("sway.i_limit").get_parameter_value().double_value - ) - - self._yaw_p_gain = ( - self.get_parameter("yaw.p_gain").get_parameter_value().double_value - ) - self._yaw_i_gain = ( - self.get_parameter("yaw.i_gain").get_parameter_value().double_value - ) - self._yaw_d_gain = ( - self.get_parameter("yaw.d_gain").get_parameter_value().double_value - ) - self._yaw_i_limit = ( - self.get_parameter("yaw.i_limit").get_parameter_value().double_value - ) - - self._K_P = np.array( - [ - [self._surge_p_gain, 0, 0], - [0, self._sway_p_gain, 0], - [0, 0, self._yaw_p_gain], - ], - dtype=np.float32, - ) - - self._K_I = np.array( - [ - [self._surge_i_gain, 0, 0], - [0, self._sway_i_gain, 0], - [0, 0, self._yaw_i_gain], - ], - dtype=np.float32, - ) - - self._K_D = np.array( - [ - [self._surge_d_gain, 0, 0], - [0, self._sway_d_gain, 0], - [0, 0, self._yaw_d_gain], - ], - dtype=np.float32, - ) - - -class BacksteppingVelocityControllerROS(rclpy.node.Node): - - def __init__(self, *args, **kwargs): - super().__init__( - node_name="voyager_velocity_controller", - allow_undeclared_parameters=True, - automatically_declare_parameters_from_overrides=False, - ) - # Define the reference model matrices (example values) - self.vessel = box.Shoebox(L=1.0, B=0.3, T=0.05) - - # Create an instance of the controller - self.controller = BacksteppingController( - mass=self.vessel.M_eff, - damping=self.vessel.D, - k_gain=np.array([10.0, 15.0, 0.0, 0.0, 0.0, 2.0]), - ) - - self._freq = 10.0 - - # Initialize messages - self._vel_cmd_msg = geometry_msgs.msg.Twist() - self._odom_msg = nav_msgs.msg.Odometry() - - # Initialize subscribers and publishers - self._subs = {} - self._pubs = {} - self._subs["odometry"] = self.create_subscription( - nav_msgs.msg.Odometry, "measurement/odom", self._odom_callback, 10 - ) - self._subs["command"] = self.create_subscription( - geometry_msgs.msg.Twist, - "control/velocity/command", - self._vel_cmd_callback, - 10, - ) - self._pubs["force"] = self.create_publisher( - geometry_msgs.msg.Wrench, "control/force/command", 10 - ) - - # Initialize timer - self._dt = 1.0 / self._freq - self.create_timer(self._dt, self.timer_callback) - - # self.v_prev = np.zeros(6, dtype=np.float32) - # self.v_des_prev = np.zeros(6, dtype=np.float32) - # self.dv_des = np.zeros(6, dtype=np.float32) - - def timer_callback(self): - - v=np.array( - [ - self._odom_msg.twist.twist.linear.x, - self._odom_msg.twist.twist.linear.y, - 0.0, - 0.0, - 0.0, - self._odom_msg.twist.twist.angular.z, - ], - dtype=np.float32, - ) - v_des=np.array( - [ - self._vel_cmd_msg.linear.x, - self._vel_cmd_msg.linear.y, - 0.0, - 0.0, - 0.0, - self._vel_cmd_msg.angular.z, - ], - dtype=np.float32, - ) - dv_des = (v_des - v) / self._dt - tau = self.controller.update( - v=v, - v_des=v_des, - dv_des=dv_des, - ) - - # self.v_prev = v - # self.v_des_prev = v_des - - - cmd_force = geometry_msgs.msg.Wrench() - cmd_force.force.x = tau[0] - cmd_force.force.y = tau[1] - cmd_force.torque.z = tau[5] - - self._pubs["force"].publish(cmd_force) - - def _vel_cmd_callback(self, msg: geometry_msgs.msg.Twist): - self._vel_cmd_msg = msg - - def _odom_callback(self, msg: geometry_msgs.msg.Wrench): - self._odom_msg = msg - -def main(args=None): - rclpy.init(args=args) - velocity_controller = BacksteppingVelocityControllerROS() - rclpy.spin(velocity_controller) - velocity_controller.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/cybership_dp/launch/dp.launch.py b/cybership_dp/launch/dp.launch.py index ef147e3..349bc8b 100644 --- a/cybership_dp/launch/dp.launch.py +++ b/cybership_dp/launch/dp.launch.py @@ -18,23 +18,13 @@ def generate_launch_description(): [ launch_ros.substitutions.FindPackageShare("cybership_dp"), "launch", - "force_controller.launch.py", + "thrust_allocation.launch.py", ] ) ), launch_arguments=[ ("vessel_name", launch.substitutions.LaunchConfiguration("vessel_name")), ("vessel_model", launch.substitutions.LaunchConfiguration("vessel_model")), - ( - "param_file", - launch.substitutions.PathJoinSubstitution( - [ - launch_ros.substitutions.FindPackageShare("cybership_dp"), - "config", - "force_controller.yaml", - ] - ), - ), ], ) @@ -51,20 +41,41 @@ def generate_launch_description(): launch_arguments=[ ("vessel_name", launch.substitutions.LaunchConfiguration("vessel_name")), ("vessel_model", launch.substitutions.LaunchConfiguration("vessel_model")), - ( - "param_file", - launch.substitutions.PathJoinSubstitution( - [ - launch_ros.substitutions.FindPackageShare("cybership_dp"), - "config", - "velocity_controller.yaml", - ] - ), - ), ], ) + include_position_controller = launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + launch.substitutions.PathJoinSubstitution( + [ + launch_ros.substitutions.FindPackageShare("cybership_dp"), + "launch", + "position_controller.launch.py", + ] + ) + ), + launch_arguments=[ + ("vessel_name", launch.substitutions.LaunchConfiguration("vessel_name")), + ("vessel_model", launch.substitutions.LaunchConfiguration("vessel_model")), + ], + ) + + # Add service call to switch force multiplexer to velocity controller output + # This will switch the force_mux to use control/force/command/velocity + service_call_switch_mux = launch.actions.ExecuteProcess( + cmd=[ + 'ros2', 'service', 'call', + [launch.substitutions.LaunchConfiguration("vessel_name"), '/force_mux/select'], + 'topic_tools_interfaces/srv/MuxSelect', + '{topic: "control/force/command/position"}' + ], + output='screen', + condition=launch.conditions.LaunchConfigurationEquals('auto_switch_mux', 'true') + ) + ld.add_action(service_call_switch_mux) + ld.add_action(include_force_controller) ld.add_action(include_velocity_controller) + ld.add_action(include_position_controller) return ld diff --git a/cybership_dp/launch/position_controller.launch.py b/cybership_dp/launch/position_controller.launch.py new file mode 100644 index 0000000..5b3fa62 --- /dev/null +++ b/cybership_dp/launch/position_controller.launch.py @@ -0,0 +1,71 @@ +import launch +import launch.actions +import launch.substitutions +import launch_ros.actions +from cybership_utilities.launch import anon +from cybership_utilities.launch import COMMON_ARGUMENTS as ARGUMENTS + + +def generate_launch_description(): + + ld = launch.LaunchDescription() + arg_param_file = launch.actions.DeclareLaunchArgument( + name="param_file", + default_value=launch.substitutions.PathJoinSubstitution( + [ + launch_ros.substitutions.FindPackageShare("cybership_config"), + "config", + launch.substitutions.LaunchConfiguration("vessel_model"), + "controller_position.yaml", + ] + ), + ) + ld.add_action(arg_param_file) + + arguments = list(filter(lambda a: a.name not in ["param_file", "vessel_model"], ARGUMENTS)) + for arg in arguments: + ld.add_action(arg) + + node_position_controller = launch_ros.actions.Node( + namespace=launch.substitutions.LaunchConfiguration("vessel_name"), + package="cybership_dp", + executable="position_control_node.py", + name=f"position_controller", + parameters=[ + launch.substitutions.LaunchConfiguration("param_file"), + { + "vessel_model": launch.substitutions.LaunchConfiguration( + "vessel_model" + ), + }, + ], + arguments=[ + "--vessel-model", + launch.substitutions.LaunchConfiguration("vessel_model"), + "--vessel-name", + launch.substitutions.LaunchConfiguration("vessel_name"), + ], + remappings=[ + ( + "control/force/command", + "control/force/command/position", + ), # Remap force command to position controller output + ], + output="screen", + respawn=True, + respawn_delay=5, + ) + + ld.add_action(node_position_controller) + + # Add argument to control whether to auto-switch the mux + arg_auto_switch = launch.actions.DeclareLaunchArgument( + name="auto_switch_mux", + default_value="true", + description="Automatically switch force mux to velocity controller output", + ) + ld.add_action(arg_auto_switch) + + + + return ld diff --git a/cybership_dp/launch/force_controller.launch.py b/cybership_dp/launch/thrust_allocation.launch.py similarity index 69% rename from cybership_dp/launch/force_controller.launch.py rename to cybership_dp/launch/thrust_allocation.launch.py index 21fd2f1..50390ab 100644 --- a/cybership_dp/launch/force_controller.launch.py +++ b/cybership_dp/launch/thrust_allocation.launch.py @@ -9,34 +9,19 @@ def generate_launch_description(): ld = launch.LaunchDescription() - arg_param_file = launch.actions.DeclareLaunchArgument( - name="param_file", - default_value=launch.substitutions.PathJoinSubstitution( - [ - launch_ros.substitutions.FindPackageShare("cybership_dp"), - "config", - "force_controller.yaml", - ] - ), - ) - ld.add_action(arg_param_file) - - arguments = list(filter(lambda a: a.name != "param_file", ARGUMENTS)) - for arg in arguments: - ld.add_action(arg) - node_force_controller = launch_ros.actions.Node( + node_thrust_allocation = launch_ros.actions.Node( namespace=launch.substitutions.LaunchConfiguration("vessel_name"), package="cybership_dp", - executable="force_controller.py", - name=f"force_controller_{anon()}", + executable="thrust_allocation_node.py", + name=f"thrust_allocation_node", parameters=[ - launch.substitutions.LaunchConfiguration("param_file"), { "vessel_model": launch.substitutions.LaunchConfiguration( "vessel_model" ), }, + {"frequency": 20.0, } ], arguments=[ "--vessel-model", @@ -60,20 +45,21 @@ def generate_launch_description(): namespace=launch.substitutions.LaunchConfiguration("vessel_name"), package="topic_tools", executable="mux", - name=f"force_mux_{anon()}", + name=f"force_mux", arguments=[ "control/force/command/mux", # Output topic "control/force/command", # Input topic to listen to "control/force/command/velocity", # Input topic to listen to "control/force/command/position", # Input topic to listen to "--repeat-delay", "0.1" # Optional delay for repeated messages + "--initial_topic", "control/force/command" # Initial topic to publish ], output="screen", respawn=True, respawn_delay=5, ) - ld.add_action(node_force_controller) + ld.add_action(node_thrust_allocation) ld.add_action(node_topic_mux) return ld diff --git a/cybership_dp/launch/velocity_controller.launch.py b/cybership_dp/launch/velocity_controller.launch.py index 3518db4..02325b2 100644 --- a/cybership_dp/launch/velocity_controller.launch.py +++ b/cybership_dp/launch/velocity_controller.launch.py @@ -13,23 +13,24 @@ def generate_launch_description(): name="param_file", default_value=launch.substitutions.PathJoinSubstitution( [ - launch_ros.substitutions.FindPackageShare("cybership_dp"), + launch_ros.substitutions.FindPackageShare("cybership_config"), "config", - "velocity_controller.yaml", + launch.substitutions.LaunchConfiguration("vessel_model"), + "controller_velocity.yaml", ] ), ) ld.add_action(arg_param_file) - arguments = list(filter(lambda a: a.name != "param_file", ARGUMENTS)) + arguments = list(filter(lambda a: a.name not in ["param_file", "vessel_model"], ARGUMENTS)) for arg in arguments: ld.add_action(arg) - node_force_controller = launch_ros.actions.Node( + node_velocity_controller = launch_ros.actions.Node( namespace=launch.substitutions.LaunchConfiguration("vessel_name"), package="cybership_dp", - executable="velocity_controller.py", - name=f"velocity_controller_{anon()}", + executable="velocity_control_node.py", + name=f"velocity_controller", parameters=[ launch.substitutions.LaunchConfiguration("param_file"), { @@ -44,11 +45,49 @@ def generate_launch_description(): "--vessel-name", launch.substitutions.LaunchConfiguration("vessel_name"), ], + remappings=[ + ( + "control/velocity/command", + "control/velocity/command/mux", + ), # Remap to the multiplexer output + ( + "control/force/command", + "control/force/command/velocity", + ), # Remap force command to velocity controller output + ], output="screen", respawn=True, respawn_delay=5, ) - ld.add_action(node_force_controller) + ld.add_action(node_velocity_controller) + + # Add topic multiplexer node for velocity commands + node_topic_mux = launch_ros.actions.Node( + namespace=launch.substitutions.LaunchConfiguration("vessel_name"), + package="topic_tools", + executable="mux", + name=f"velocity_mux", + arguments=[ + "control/velocity/command/mux", # Output topic + "control/velocity/command", # Input topic to listen to + "--repeat-delay", "0.1", # Optional delay for repeated messages + "--initial-topic", "control/velocity/command" # Initial topic to publish + ], + output="screen", + respawn=True, + respawn_delay=5, + ) + ld.add_action(node_topic_mux) + + # Add argument to control whether to auto-switch the mux + arg_auto_switch = launch.actions.DeclareLaunchArgument( + name="auto_switch_mux", + default_value="true", + description="Automatically switch force mux to velocity controller output", + ) + ld.add_action(arg_auto_switch) + + return ld diff --git a/cybership_tests/cybership_tests/reference_filter_server.py b/cybership_dp/nodes/position_control_node.py similarity index 99% rename from cybership_tests/cybership_tests/reference_filter_server.py rename to cybership_dp/nodes/position_control_node.py index bf6af14..9bacbb2 100755 --- a/cybership_tests/cybership_tests/reference_filter_server.py +++ b/cybership_dp/nodes/position_control_node.py @@ -72,9 +72,9 @@ def saturate(x, z): return x / (np.abs(x) + z) -class GotoPointController(Node): +class PositionController(Node): def __init__(self): - super().__init__("goto_point_controller", namespace="voyager") + super().__init__("position_controller", namespace="cybership") # Declare parameters with default values self.declare_parameters( @@ -640,7 +640,7 @@ def publish_target_pose_marker(self, pose_stamped: PoseStamped): def main(args=None): rclpy.init(args=args) - server_node = GotoPointController() + server_node = PositionController() client_node = NavigateToPoseClient() executor = ( MultiThreadedExecutor() diff --git a/cybership_dp/nodes/force_controller.py b/cybership_dp/nodes/thrust_allocation_node.py similarity index 100% rename from cybership_dp/nodes/force_controller.py rename to cybership_dp/nodes/thrust_allocation_node.py diff --git a/cybership_tests/cybership_tests/velocity_control.py b/cybership_dp/nodes/velocity_control_node.py similarity index 99% rename from cybership_tests/cybership_tests/velocity_control.py rename to cybership_dp/nodes/velocity_control_node.py index d8d85db..34c6f06 100755 --- a/cybership_tests/cybership_tests/velocity_control.py +++ b/cybership_dp/nodes/velocity_control_node.py @@ -33,7 +33,7 @@ class VelocityControlNode(Node): """ def __init__(self): - super().__init__("velocity_control_node", namespace="voyager") + super().__init__("velocity_control_node", namespace="cybership") # Declare parameters with default values self.declare_parameters( @@ -105,7 +105,7 @@ def __init__(self): # Publisher for control commands self.control_pub = self.create_publisher( Wrench, - 'control/force/command/velocity', + 'control/force/command', 10 ) diff --git a/cybership_dp/nodes/velocity_controller.py b/cybership_dp/nodes/velocity_controller.py index bd539d4..544d409 100755 --- a/cybership_dp/nodes/velocity_controller.py +++ b/cybership_dp/nodes/velocity_controller.py @@ -12,6 +12,7 @@ import os import sys import argparse +import warnings import numpy as np @@ -21,6 +22,18 @@ import cybership_dp.voyager import cybership_dp.enterprise +# DEPRECATION WARNING +warnings.warn( + "This velocity_controller.py file is deprecated and may be removed in future versions. " + "Please use the updated velocity control implementation.", + DeprecationWarning, + stacklevel=2 +) + +print("DEPRECATED: This velocity controller node is deprecated and may be removed in future versions.") +print(" Please migrate to the updated velocity control implementation.") +print(" For more information, consult the documentation or contact the development team.") +print() class VelocityControllerManager(): @@ -47,6 +60,15 @@ def initialize(self) -> rclpy.node.Node: def main(args=None): + # Print deprecation warning + print("=" * 80) + print("DEPRECATION WARNING") + print("This velocity_controller.py node is DEPRECATED!") + print("Please migrate to the updated velocity control implementation.") + print("This file may be removed in future versions.") + print("=" * 80) + print() + rclpy.init(args=args) manager = VelocityControllerManager() diff --git a/cybership_teleop/config/allocation/ps5.yaml b/cybership_teleop/config/allocation/ps5.yaml index 9990e72..29ba61c 100644 --- a/cybership_teleop/config/allocation/ps5.yaml +++ b/cybership_teleop/config/allocation/ps5.yaml @@ -8,15 +8,15 @@ axis_mappings: force-x: axis: 3 - scale: 1.0 + scale: 2.0 offset: 0.0 force-y: axis: 2 - scale: 1.0 + scale: -2.0 offset: 0.0 torque-z: axis: 0 - scale: 1.0 + scale: -1.0 offset: 0.0 enable: diff --git a/cybership_teleop/config/simple/ps5.yaml b/cybership_teleop/config/simple/ps5.yaml index 84d9e05..d59d41e 100644 --- a/cybership_teleop/config/simple/ps5.yaml +++ b/cybership_teleop/config/simple/ps5.yaml @@ -12,7 +12,7 @@ offset: 0.0 force-y: axis: 0 - scale: 1.0 + scale: -1.0 offset: 0.0 starboard_thruster: @@ -27,7 +27,7 @@ offset: 0.0 force-y: axis: 3 - scale: 1.0 + scale: -1.0 offset: 0.0 enable: diff --git a/cybership_tests/cybership_tests/four_corner_line_of_sight.py b/cybership_tests/cybership_tests/four_corner_line_of_sight.py deleted file mode 100755 index b549716..0000000 --- a/cybership_tests/cybership_tests/four_corner_line_of_sight.py +++ /dev/null @@ -1,286 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from rclpy.node import Node -from geometry_msgs.msg import Wrench -from nav_msgs.msg import Odometry -import math -import numpy as np -from scipy.spatial.transform import Rotation as R - - -def wrap_to_pi(angle): - """ - Wrap an angle in radians to the interval [-pi, pi]. - """ - return (angle + math.pi) % (2 * math.pi) - math.pi - - -class SquareMoveController(Node): - def __init__(self): - super().__init__("square_move_controller", namespace="voyager") - - # Publisher to send control commands (force and torque) - self.control_pub = self.create_publisher(Wrench, "control/force/command", 10) - - # Subscriber to receive odometry measurements - self.create_subscription(Odometry, "measurement/odom", self.odom_callback, 10) - - # Timer for periodic control update (dt = 0.01 s) - self.dt = 0.01 - self.timer = self.create_timer(self.dt, self.control_loop) - - # Latest odometry message storage - self.latest_odom = None - - # Square path parameters (in meters) - # (Example: a 3×3 square centered at (0,0).) - self.side_length = 3.0 - self.corners = [ - (0.0, 0.0), - (self.side_length, 0.0), - (self.side_length, self.side_length), - (0.0, self.side_length), - (0.0, 0.0), # Close the loop - ] - # Shift corners so the square is centered at (-1.5, -1.5): - for i in range(len(self.corners)): - self.corners[i] = (self.corners[i][0] - 1.5, self.corners[i][1] - 1.5) - - # Desired headings at each corner (radians): - self.corner_headings = [ - 0.0, # corner 0 - 0.0, # corner 1 - math.pi / 4, # corner 2 - 0.0, # corner 3 - 0.0, - ] - - # --------------------------------------- - # Controller gains (PID-like) - # --------------------------------------- - # Proportional gains - self.kp_xy = 1.0 # cross-track & along-track - self.kp_yaw = 0.6 # heading - self.kp_speed = 0.6 # forward speed - - # Integral gains - self.ki_xy = 0.1 # cross-track integrator - self.ki_yaw = 0.0 # heading integrator (set to 0 if not desired) - self.ki_speed = 0.0 # forward speed integrator (set to 0 if not desired) - - # Desired forward speed along the path - self.desired_speed = 1.0 # m/s - - # Index to track current segment start corner - self.current_waypoint_index = 0 - - # Threshold to decide when we have "arrived" near the next corner - self.segment_arrival_threshold = 0.25 - - # --------------------------------------- - # Integral error states - # --------------------------------------- - # We'll track x- and y-components of cross-track error - self.cross_x_int = 0.2 - self.cross_y_int = 0.2 - - # Integral of yaw error - self.yaw_err_int = 0.0 - - # Integral of speed error - self.speed_err_int = 0.0 - - # Maximum allowed velocities (in m/s) for the vessel - self.max_surge_velocity = 0.05 - self.max_sway_velocity = 0.1 - - self.get_logger().info( - "Square Move Controller with cross-track integral initialized." - ) - - def reset_integrators(self): - """ - Resets integral states to zero. - Typically called when you switch to a new segment. - """ - self.cross_x_int = 0.0 - self.cross_y_int = 0.0 - self.yaw_err_int = 0.0 - - def odom_callback(self, msg: Odometry): - """ - Callback to update the latest odometry measurement. - """ - self.latest_odom = msg - - def control_loop(self): - """ - Control loop that computes and publishes the control command. - Now includes integral control for cross-track, heading, and (optionally) speed. - """ - if self.latest_odom is None: - return - - # Check if the loop is complete - if self.current_waypoint_index == len(self.corners) - 1: - self.get_logger().info("Completed square path.") - # Stop the control loop - self.destroy_timer(self.timer) - return - - # Extract current position - pos = self.latest_odom.pose.pose.position - current_x = pos.x - current_y = pos.y - - # Extract current yaw - orientation = self.latest_odom.pose.pose.orientation - rot = R.from_quat([orientation.x, orientation.y, orientation.z, orientation.w]) - _, _, current_yaw = rot.as_euler("xyz", degrees=False) - - # Determine current segment from corners[i] to corners[i+1] - p1_index = self.current_waypoint_index - p2_index = (p1_index + 1) % len(self.corners) - p1 = self.corners[p1_index] - p2 = self.corners[p2_index] - - # Segment vector - dx = p2[0] - p1[0] - dy = p2[1] - p1[1] - segment_length = math.sqrt(dx * dx + dy * dy) - - # Handle degenerate case - if segment_length < 1e-6: - # Move to next segment - self.current_waypoint_index = p2_index - self.reset_integrators() - return - - # Normalized direction along the segment - ux = dx / segment_length - uy = dy / segment_length - - # Vector from p1 to current position - ex = current_x - p1[0] - ey = current_y - p1[1] - - # Along-track distance via dot product - along_track_dist = ex * ux + ey * uy - - # Check if we're close enough to the end of the segment - if along_track_dist >= segment_length - self.segment_arrival_threshold: - self.current_waypoint_index = p2_index - self.reset_integrators() - return - - # Clamp if below 0 - if along_track_dist < 0.0: - along_track_dist = 0.0 - - # Nearest point on the segment - nearest_x = p1[0] + along_track_dist * ux - nearest_y = p1[1] + along_track_dist * uy - - # Cross-track error (vector from nearest point to current position) - cross_x = current_x - nearest_x - cross_y = current_y - nearest_y - - # --------------------------------------- - # Update integrators - # --------------------------------------- - # Integrate cross-track error in x and y - self.cross_x_int += cross_x * self.dt - self.cross_y_int += cross_y * self.dt - - # Heading - desired_yaw = self.corner_headings[p1_index] - yaw_error = wrap_to_pi(desired_yaw - current_yaw) - self.yaw_err_int += yaw_error * self.dt - - # Speed integrator - # velocity in global frame - vx = self.latest_odom.twist.twist.linear.x - vy = self.latest_odom.twist.twist.linear.y - # along-track velocity - v_along = vx * ux + vy * uy - speed_err = self.desired_speed - v_along - self.speed_err_int += speed_err * self.dt - - # --------------------------------------- - # Compute Control (P + I) - # --------------------------------------- - # Cross-track control: push back toward the path - # tau_ct = -(kp*error + ki*integral_of_error) - tau_ct_x = -(self.kp_xy * cross_x + self.ki_xy * self.cross_x_int) - tau_ct_y = -(self.kp_xy * cross_y + self.ki_xy * self.cross_y_int) - # Along-track control for speed - # (kp*speed_err + ki*integral_of_speed_err) in direction of segment - speed_control = self.kp_speed * speed_err + self.ki_speed * self.speed_err_int - tau_along_x = speed_control * ux - tau_along_y = speed_control * uy - - # Combine cross-track + along-track - tau_x = tau_ct_x + tau_along_x - tau_y = tau_ct_y + tau_along_y - - # Heading control - # tau_yaw = kp_yaw * yaw_err + ki_yaw * yaw_err_int - tau_yaw = self.kp_yaw * yaw_error + self.ki_yaw * self.yaw_err_int - - # --- Velocity Limiting --- - # Assume the odometry twist is expressed in the vessel's body frame. - current_surge_velocity = self.latest_odom.twist.twist.linear.x - current_sway_velocity = self.latest_odom.twist.twist.linear.y - - # Limit surge force - if abs(current_surge_velocity) > self.max_surge_velocity: - scaling_factor = self.max_surge_velocity / abs(current_surge_velocity) - self.get_logger().debug( - f"Surge velocity ({current_surge_velocity:.2f} m/s) exceeds max. Scaling force_x by {scaling_factor:.2f}" - ) - tau_x *= scaling_factor - - # Limit sway force - if abs(current_sway_velocity) > self.max_sway_velocity: - scaling_factor = self.max_sway_velocity / abs(current_sway_velocity) - self.get_logger().debug( - f"Sway velocity ({current_sway_velocity:.2f} m/s) exceeds max. Scaling force_y by {scaling_factor:.2f}" - ) - tau_y *= scaling_factor - - wrench_msg = Wrench() - wrench_msg.force.x = float(tau_x) - wrench_msg.force.y = float(tau_y) - wrench_msg.force.z = 0.0 - wrench_msg.torque.x = 0.0 - wrench_msg.torque.y = 0.0 - wrench_msg.torque.z = float(tau_yaw) - self.control_pub.publish(wrench_msg) - - cross_track_error_mag = math.sqrt(cross_x**2 + cross_y**2) - self.get_logger().info( - f"Pos=({current_x:.2f}, {current_y:.2f}), " - f"Yaw={math.degrees(current_yaw):.1f} deg, " - f"dYaw={(math.degrees(yaw_error)):.1f} deg, " - f"SegDist={along_track_dist:.2f}/{segment_length:.2f}, " - f"XTE={cross_track_error_mag:.3f}, " - f"Fx={tau_x:.2f}, Fy={tau_y:.2f}, Tz={tau_yaw:.2f}" - f"SurgeVel={current_surge_velocity:.2f}, SwayVel={current_sway_velocity:.2f}, " - ) - - -def main(args=None): - rclpy.init(args=args) - node = SquareMoveController() - try: - rclpy.spin(node) - except KeyboardInterrupt: - node.get_logger().info("Square Move Controller stopped by user.") - finally: - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/cybership_thrusters/src/azimuth.cpp b/cybership_thrusters/src/azimuth.cpp index 19709c4..782b64c 100644 --- a/cybership_thrusters/src/azimuth.cpp +++ b/cybership_thrusters/src/azimuth.cpp @@ -62,6 +62,10 @@ void Azimuth::f_force_callback(const geometry_msgs::msg::Wrench::SharedPtr msg) auto angle = std::atan2(msg->force.y, msg->force.x); auto rpm = std::sqrt(std::pow(msg->force.y,2) + std::pow(msg->force.x,2)); + if (m_config.angle_inverted) { + angle = -angle; + } + angle_msg.data = angle; rpm_msg.data = rpm; From 9aa552a51a6ded11170f1cc1cd926c14a6a1c3ad Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Mon, 14 Jul 2025 11:38:44 +0200 Subject: [PATCH 05/46] fix: remove redundancy --- cybership_controller/CMakeLists.txt | 8 - .../nodes/position_control.py | 656 ------------------ .../nodes/velocity_control.py | 383 ---------- .../velocity/reference_feedforward.py | 16 +- 4 files changed, 13 insertions(+), 1050 deletions(-) delete mode 100755 cybership_controller/nodes/position_control.py delete mode 100755 cybership_controller/nodes/velocity_control.py diff --git a/cybership_controller/CMakeLists.txt b/cybership_controller/CMakeLists.txt index ff75559..a0c8a28 100644 --- a/cybership_controller/CMakeLists.txt +++ b/cybership_controller/CMakeLists.txt @@ -8,12 +8,4 @@ ament_python_install_package(${PROJECT_NAME} PACKAGE_DIR src/${PROJECT_NAME} ) -file(GLOB NODE_SCRIPTS ${CMAKE_CURRENT_SOURCE_DIR}/nodes/*.py) - -install( - PROGRAMS - ${NODE_SCRIPTS} - DESTINATION lib/${PROJECT_NAME} -) - ament_package() \ No newline at end of file diff --git a/cybership_controller/nodes/position_control.py b/cybership_controller/nodes/position_control.py deleted file mode 100755 index bfccc34..0000000 --- a/cybership_controller/nodes/position_control.py +++ /dev/null @@ -1,656 +0,0 @@ -#!/usr/bin/env python3 - -# ---------------------------------------------------------------------------- -# This code is part of the MCSimPython toolbox and repository -# Created By: Jan-Erik Hygen -# Created Date: 2023-01-30, -# Revised: 2025-01-31 Kristian Magnus Roen Now fitting the MC-Gym for csad. -# Revised: 2025-03-26 Emir Cem Gezer Applied to the Cybership Software Suite -# Tested: -# Copyright (C) 2025: NTNU, Trondheim -# Licensed under GPL-3.0-or-later -# --------------------------------------------------------------------------- - -import time -import numpy as np -import rclpy -from rclpy.node import Node -from rclpy.parameter import Parameter -from geometry_msgs.msg import Wrench, Pose2D, PoseStamped, TwistStamped -from nav_msgs.msg import Odometry -import math -import numpy as np -from scipy.spatial.transform import Rotation as R -from nav2_msgs.action import NavigateToPose -from rclpy.action import ActionServer, GoalResponse, CancelResponse, ActionClient -from rclpy.executors import MultiThreadedExecutor -from cybership_tests.go_to_client import NavigateToPoseClient -from shoeboxpy.model3dof import Shoebox -from visualization_msgs.msg import Marker -from std_msgs.msg import Float32MultiArray - -from cybership_controller.position.reference_filter import ThirdOrderReferenceFilter - -try: - from cybership_interfaces.msg import PerformanceMetrics -except ImportError: - print("cybership_msgs not found. Skipping performance metrics.") - - -def wrap_to_pi(angle): - """ - Wrap an angle in radians to the interval [-pi, pi]. - """ - return (angle + math.pi) % (2 * math.pi) - math.pi - - -def Rz(psi): - """3DOF Rotation matrix about z-axis. - - Parameters - ---------- - psi : float - Yaw angle (rad) - - Returns - ------- - Rz : array_like - 3x3 rotation matrix. - - """ - return np.array( - [[np.cos(psi), -np.sin(psi), 0], - [np.sin(psi), np.cos(psi), 0], [0, 0, 1]] - ) - - -def saturate(x, z): - """ - Saturation function: returns x / (|x| + z) - This ensures a smooth saturation between -1 and 1. - """ - return x / (np.abs(x) + z) - - -class GotoPointController(Node): - def __init__(self): - super().__init__("goto_point_controller", namespace="voyager") - - # Declare parameters with default values - self.declare_parameters( - namespace='', - parameters=[ - # Controller gains - ('control.p_gain.pos', 4.0), - ('control.i_gain.pos', 0.2), - ('control.d_gain.pos', 0.2), - ('control.p_gain.vel', 0.7), - ('control.i_gain.vel', 0.1), - ('control.d_gain.vel', 0.5), - ('control.p_gain.yaw', 1.3), - ('control.i_gain.yaw', 0.2), - ('control.d_gain.yaw', 1.0), - - # Controller limits - ('control.max_integral_error.pos', 1.0), - ('control.max_integral_error.yaw', 1.4), - ('control.saturation.pos', 0.1), - ('control.saturation.yaw', 0.1), - - # Tolerances - ('control.tolerance.pos', 0.25), - ('control.tolerance.yaw', 0.1), - - # Vessel properties - ('vessel.length', 1.0), - ('vessel.beam', 0.3), - ('vessel.draft', 0.02), - - # Reference filter parameters - ('filter.omega', [0.15, 0.15, 0.15]), - ('filter.delta', [0.8, 0.8, 0.8]), - - # Performance metrics - ('metrics.window_size', 200), - ('metrics.interval', 1.0), - - # Time step - ('dt', 0.01), - ] - ) - - # Add parameter callback for runtime updates - self.add_on_set_parameters_callback(self.parameters_callback) - - # Publisher to send control commands (force and torque) - self.control_pub = self.create_publisher( - Wrench, "control/force/command", 10) - # Debug publishers for tracking performance metrics - self.debug_pose_pub = self.create_publisher( - PoseStamped, "control/pose/debug/reference_pose", 10) - self.debug_vel_pub = self.create_publisher( - TwistStamped, "control/pose/debug/reference_velocity", 10) - self.debug_error_pose_pub = self.create_publisher( - Pose2D, "control/pose/debug/tracking_error_position", 10) - self.debug_error_vel_pub = self.create_publisher( - TwistStamped, "control/pose/debug/tracking_error_velocity", 10) - - self.debug_metrics_pub = None - if PerformanceMetrics is not None: - self.debug_metrics_pub = self.create_publisher( - PerformanceMetrics, "control/pose/debug/performance_metrics", 10) - - # self.create_subscription(PoseStamped, "/goal_pose", self.goal_pose_callback, 10) - self.create_subscription( - Odometry, "measurement/odom", self.odom_callback, 10) - - self.marker_pub = self.create_publisher( - Marker, "visualization_marker", 10) - - # Get time step from parameters - self.dt = self.get_parameter('dt').value - - # Initialize controller parameters from ROS parameters - self.update_configuration() - - # Latest odometry message storage - self.latest_odom = None - - # --- Target state --- - # Set initial desired target position and orientation. - self.target_x = None # target x position (meters) - self.target_y = None # target y position (meters) - self.target_yaw = None # target yaw (radians) - - # Track integral error - self.integral_error_pos = np.zeros(2) - self.integral_error_yaw = 0.0 - - self.error_pos = np.zeros(2) - self.error_yaw = 0.0 - - # Initialize the 3rd order reference filter. - # Here, we treat [x, y, yaw] as the 3D pose to be smoothed. - self.ref_filter = None - - self.get_logger().info( - "Goto Point Controller (Reference Filter Version) Initialized with parameters." - ) - - # Timer for periodic control updates - self.timer = self.create_timer(self.dt, self.control_loop) - - # --- Action Server using nav2_msgs/NavigateToPose --- - self._action_server = ActionServer( - self, - NavigateToPose, - "navigate_to_pose", - execute_callback=self.execute_callback, - goal_callback=self.action_goal_callback, - cancel_callback=self.action_cancel_callback, - ) - - def update_configuration(self): - """Update controller configuration from parameters""" - # Get controller gains - self.Kp_pos = self.get_parameter('control.p_gain.pos').value - self.Ki_pos = self.get_parameter('control.i_gain.pos').value - self.Kd_pos = self.get_parameter('control.d_gain.pos').value - self.Kp_vel = self.get_parameter('control.p_gain.vel').value - self.Ki_vel = self.get_parameter('control.i_gain.vel').value - self.Kd_vel = self.get_parameter('control.d_gain.vel').value - self.Kp_yaw = self.get_parameter('control.p_gain.yaw').value - self.Ki_yaw = self.get_parameter('control.i_gain.yaw').value - self.Kd_yaw = self.get_parameter('control.d_gain.yaw').value - - # Get controller limits - self.max_integral_error_pos = self.get_parameter('control.max_integral_error.pos').value - self.max_integral_error_yaw = self.get_parameter('control.max_integral_error.yaw').value - self.saturation_pos = self.get_parameter('control.saturation.pos').value - self.saturation_yaw = self.get_parameter('control.saturation.yaw').value - - # Get tolerances - self.pos_tol = self.get_parameter('control.tolerance.pos').value - self.yaw_tol = self.get_parameter('control.tolerance.yaw').value - - # Get vessel properties - vessel_length = self.get_parameter('vessel.length').value - vessel_beam = self.get_parameter('vessel.beam').value - vessel_draft = self.get_parameter('vessel.draft').value - - # Create vessel model - self.shoebox = Shoebox( - L=vessel_length, - B=vessel_beam, - T=vessel_draft, - ) - - # Get performance metrics parameters - self.window_size = self.get_parameter('metrics.window_size').value - self.metrics_interval = self.get_parameter('metrics.interval').value - - # Reset reference filter if parameters change - if self.ref_filter is not None and self.latest_odom is not None: - # Get filter parameters - filter_omega = self.get_parameter('filter.omega').value - filter_delta = self.get_parameter('filter.delta').value - - # Save current state - current_eta = self.ref_filter.eta_d - - # Create new filter with updated parameters - self.ref_filter = ThirdOrderReferenceFilter( - dt=self.dt, - omega=filter_omega, - delta=filter_delta, - initial_eta=current_eta, - ) - self.ref_filter.eta_d = current_eta - - self.get_logger().info( - f"Updated configuration - vessel: [{vessel_length}, {vessel_beam}, {vessel_draft}], " + - f"POS gains: P={self.Kp_pos}, I={self.Ki_pos}, D={self.Kd_pos}, " + - f"YAW gains: P={self.Kp_yaw}, I={self.Ki_yaw}, D={self.Kd_yaw}" - ) - - def parameters_callback(self, params): - """Handle parameter updates""" - update_needed = False - - for param in params: - if param.name.startswith(('control.', 'vessel.', 'filter.', 'metrics.')): - update_needed = True - - if param.name == 'dt': - self.dt = param.value - # Recreate the timer with new dt - self.timer.cancel() - self.timer = self.create_timer(self.dt, self.control_loop) - update_needed = True - - # Update configuration if parameters changed - if update_needed: - self.update_configuration() - - return True # Accept all parameter changes - - def odom_callback(self, msg: Odometry): - """ - Callback to update the latest odometry measurement. - """ - if self.ref_filter is None: - print("Setting target position from odometry.") - self.target_x = msg.pose.pose.position.x - self.target_y = msg.pose.pose.position.y - _, _, self.target_yaw = R.from_quat( - [ - msg.pose.pose.orientation.x, - msg.pose.pose.orientation.y, - msg.pose.pose.orientation.z, - msg.pose.pose.orientation.w, - ] - ).as_euler("xyz", degrees=False) - - # Get filter parameters from ROS parameters - filter_omega = self.get_parameter('filter.omega').value - filter_delta = self.get_parameter('filter.delta').value - - self.ref_filter = ThirdOrderReferenceFilter( - dt=self.dt, - omega=filter_omega, - delta=filter_delta, - initial_eta=[self.target_x, self.target_y, self.target_yaw], - ) - self.ref_filter.eta_d = np.array( - [self.target_x, self.target_y, self.target_yaw] - ) - - self.latest_odom = msg - - def process_performance_metrics(self, desired_pose, desired_vel, error_pos, error_vel, error_yaw): - """ - Process and publish debug information using a moving window approach. - """ - # Publish reference pose (from reference filter) - debug_ref_pose = PoseStamped() - debug_ref_pose.header.stamp = self.get_clock().now().to_msg() - debug_ref_pose.header.frame_id = "world" - debug_ref_pose.pose.position.x = desired_pose[0] - debug_ref_pose.pose.position.y = desired_pose[1] - debug_ref_pose.pose.position.z = 0.0 - # Convert yaw to quaternion - q = R.from_euler('z', desired_pose[2]).as_quat() - debug_ref_pose.pose.orientation.x = q[0] - debug_ref_pose.pose.orientation.y = q[1] - debug_ref_pose.pose.orientation.z = q[2] - debug_ref_pose.pose.orientation.w = q[3] - self.debug_pose_pub.publish(debug_ref_pose) - - debug_ref_vel = TwistStamped() - debug_ref_vel.header.stamp = self.get_clock().now().to_msg() - debug_ref_vel.header.frame_id = "base_link" - debug_ref_vel.twist.linear.x = desired_vel[0] - debug_ref_vel.twist.linear.y = desired_vel[1] - debug_ref_vel.twist.angular.z = desired_vel[2] - self.debug_vel_pub.publish(debug_ref_vel) - - debug_error_vel = TwistStamped() - debug_error_vel.header.stamp = self.get_clock().now().to_msg() - debug_error_vel.header.frame_id = "base_link" - debug_error_vel.twist.linear.x = error_vel[0] - debug_error_vel.twist.linear.y = error_vel[1] - debug_error_vel.twist.angular.z = error_yaw - self.debug_error_vel_pub.publish(debug_error_vel) - - # Publish error information - debug_error_pose = Pose2D() - debug_error_pose.x = error_pos[0] - debug_error_pose.y = error_pos[1] - debug_error_pose.theta = error_yaw - self.debug_error_pose_pub.publish(debug_error_pose) - - # Update metrics using moving window - error_norm = np.sqrt(error_pos[0]**2 + error_pos[1]**2) - - # Initialize start time if not already set - if self.start_time is None: - self.start_time = self.get_clock().now().nanoseconds / 1e9 - - # Add newest error to window - self.error_window.append(error_norm) - self.error_yaw_window.append(abs(error_yaw)) - - # Keep window at fixed size - if len(self.error_window) > self.window_size: - self.error_window.pop(0) - self.error_yaw_window.pop(0) - - self.sample_count += 1 - - current_time = self.get_clock().now().nanoseconds / 1e9 - if current_time - self.last_metrics_time >= self.metrics_interval and len(self.error_window) > 0: - self.last_metrics_time = current_time - - # Calculate window statistics - error_array = np.array(self.error_window) - - metrics_msg = PerformanceMetrics() - metrics_msg.header.stamp = self.get_clock().now().to_msg() - metrics_msg.header.frame_id = "voyager" - metrics_msg.message = "Position tracking error (meters)" - - # Calculate statistics over the window - metrics_msg.mean = np.mean(error_array) - metrics_msg.median = np.median(error_array) - metrics_msg.rms = np.sqrt(np.mean(np.square(error_array))) - metrics_msg.min = np.min(error_array) - metrics_msg.max = np.max(error_array) - metrics_msg.stddev = np.std(error_array) - - self.debug_metrics_pub.publish(metrics_msg) - - self.get_logger().info( - f"Position tracking metrics - Mean: {metrics_msg.mean:.3f}m, " - f"RMS: {metrics_msg.rms:.3f}m, Max: {metrics_msg.max:.3f}m" - ) - - def control_loop(self): - """ - Periodic control loop to update reference filter and publish control commands. - This version uses a 3rd order reference filter to generate a smooth desired trajectory. - """ - if self.latest_odom is None: - return - - if self.ref_filter is None: - return - - # Extract current position from odometry (global frame) - pos = self.latest_odom.pose.pose.position - current_x = pos.x - current_y = pos.y - - # Extract yaw (heading) from quaternion orientation - orientation = self.latest_odom.pose.pose.orientation - rot = R.from_quat([orientation.x, orientation.y, - orientation.z, orientation.w]) - _, _, current_yaw = rot.as_euler("xyz", degrees=False) - - # Also get current velocities from odometry for the derivative term. - current_vx = self.latest_odom.twist.twist.linear.x - current_vy = self.latest_odom.twist.twist.linear.y - current_yaw_rate = self.latest_odom.twist.twist.angular.z - - # Update the reference filter with the latest target pose. - # This "command" is smoothed by the filter. - ref = np.array([self.target_x, self.target_y, self.target_yaw]) - self.ref_filter.set_eta_r(ref) - self.ref_filter.update() - - # Retrieve filtered outputs: desired pose, velocity, and acceleration. - desired_pose = self.ref_filter.eta_d # [x, y, yaw] - desired_vel = self.ref_filter.eta_d_dot # velocity - # acceleration (feedforward term) - desired_acc = self.ref_filter.eta_d_ddot - - # Compute errors (position and yaw) between the filtered desired state and current state. - error_pos = np.array( - [desired_pose[0] - current_x, desired_pose[1] - current_y]) - error_yaw = wrap_to_pi(desired_pose[2] - current_yaw) - - self.error_pos = error_pos - self.error_yaw = error_yaw - - # Compute velocity errors - current_vel = np.array([current_vx, current_vy]) - error_vel = np.array( - [desired_vel[0] - current_vx, desired_vel[1] - current_vy]) - error_yaw_rate = desired_vel[2] - current_yaw_rate - - acc = self.shoebox.M_eff @ self.ref_filter.get_nu_d() - - self.integral_error_pos += error_pos * self.dt - self.integral_error_yaw += error_yaw * self.dt - - # Apply saturation to the integral error - self.integral_error_pos = np.clip( - self.integral_error_pos, -self.max_integral_error_pos, self.max_integral_error_pos) - self.integral_error_yaw = np.clip( - self.integral_error_yaw, -self.max_integral_error_yaw, self.max_integral_error_yaw) - - # Compute control commands. - # For position, we use feedforward desired acceleration plus a PID correction. - world_x = ( - acc[0] - + self.Kp_pos * error_pos[0] - + self.Kd_vel * error_vel[0] - + self.Ki_pos * self.integral_error_pos[0] - ) - world_y = ( - acc[1] - + self.Kp_pos * error_pos[1] - + self.Kd_vel * error_vel[1] - + self.Ki_pos * self.integral_error_pos[1] - ) - world_yaw = ( - acc[2] - + self.Kp_yaw * error_yaw - + self.Kd_yaw * error_yaw_rate - + self.Ki_yaw * self.integral_error_yaw - ) - - # Process and publish performance metrics - self.process_performance_metrics( - desired_pose, desired_vel, error_pos, error_vel, error_yaw) - - control_x, control_y, control_yaw = Rz( - current_yaw).T @ np.array([world_x, world_y, world_yaw]) - - # Optionally apply saturation. - # control_x = saturate(control_x, self.saturation_pos) - # control_y = saturate(control_y, self.saturation_pos) - # control_yaw = saturate(control_yaw, self.saturation_yaw) - - wrench_msg = Wrench() - - # Publish these instead - wrench_msg.force.x = control_x - wrench_msg.force.y = control_y - wrench_msg.force.z = 0.0 # No vertical force - wrench_msg.torque.x = 0.0 - wrench_msg.torque.y = 0.0 - wrench_msg.torque.z = control_yaw - - # Publish the control command. - self.control_pub.publish(wrench_msg) - - # Debug logging. - self.get_logger().debug( - f"Current: ({current_x:.2f}, {current_y:.2f}, {current_yaw:.2f}), " - f"Filtered desired: ({desired_pose[0]:.2f}, {desired_pose[1]:.2f}, {desired_pose[2]:.2f}), " - f"Control: ({control_x:.2f}, {control_y:.2f}, {control_yaw:.2f})" - ) - - def action_goal_callback(self, goal_request): - self.get_logger().info("Received NavigateToPose goal request") - return GoalResponse.ACCEPT - - def action_cancel_callback(self, goal_handle): - self.get_logger().info("Received cancel request for NavigateToPose") - return CancelResponse.ACCEPT - - def execute_callback(self, goal_handle): - self.get_logger().info("Executing NavigateToPose goal...") - - # Extract target pose from the goal message - target_pose: PoseStamped = goal_handle.request.pose - self.target_x = target_pose.pose.position.x - self.target_y = target_pose.pose.position.y - - self.get_logger().info( - f"Target position: ({self.target_x}, {self.target_y})") - - # Convert quaternion to yaw angle - orientation = target_pose.pose.orientation - rot = R.from_quat([ - orientation.x, - orientation.y, - orientation.z, - orientation.w] - ) - _, _, self.target_yaw = rot.as_euler("xyz", degrees=False) - - feedback_msg = NavigateToPose.Feedback() - current_feedback_pose = PoseStamped() - - self.publish_target_pose_marker(target_pose) - - # Loop until the robot reaches the target (within tolerance) or the goal is canceled. - while rclpy.ok(): - if goal_handle.is_cancel_requested: - goal_handle.canceled() - self.get_logger().info("NavigateToPose goal canceled") - result = NavigateToPose.Result() - return result - - if self.latest_odom is None: - time.sleep(1.0) - continue - - # Extract current pose from odometry - current_odom = self.latest_odom.pose.pose - current_feedback_pose.header.stamp = self.get_clock().now().to_msg() - current_feedback_pose.header.frame_id = "odom" - - current_feedback_pose.pose.position = current_odom.position - current_feedback_pose.pose.orientation = current_odom.orientation - - # Compute remaining distance - error_norm = np.sqrt( - (self.target_x - current_feedback_pose.pose.position.x) ** 2 - + (self.target_y - current_feedback_pose.pose.position.y) ** 2 - ) - _, _, current_yaw = R.from_quat( - [ - current_feedback_pose.pose.orientation.x, - current_feedback_pose.pose.orientation.y, - current_feedback_pose.pose.orientation.z, - current_feedback_pose.pose.orientation.w, - ] - ).as_euler("xyz", degrees=False) - error_yaw = wrap_to_pi(self.target_yaw - current_yaw) - - feedback_msg.current_pose = current_feedback_pose - feedback_msg.distance_remaining = float(error_norm) - - goal_handle.publish_feedback(feedback_msg) - self.get_logger().debug( - f"Distance remaining: {error_norm:.2f}, angle error: {error_yaw:.2f}" - ) - - # Check if target is reached (both position and yaw) - if error_norm < self.pos_tol and abs(error_yaw) < self.yaw_tol: - self.error_pos = np.array([np.inf, np.info]) - self.error_yaw = np.inf - break - - time.sleep(1.0) # Publish feedback at ~1Hz - - goal_handle.succeed() - result = NavigateToPose.Result() - self.get_logger().info("NavigateToPose goal succeeded") - return result - - def publish_target_pose_marker(self, pose_stamped: PoseStamped): - """Publish a simple marker (e.g., an arrow) in RViz to visualize the requested pose.""" - marker = Marker() - marker.header = pose_stamped.header - marker.header.frame_id = "world" # Adjust frame if necessary - marker.ns = "target_pose" - marker.id = 0 - marker.type = Marker.ARROW - marker.action = Marker.ADD - - # Pose (position/orientation) - marker.pose = pose_stamped.pose - - # Scale and color - marker.scale.x = 0.5 # Arrow length - marker.scale.y = 0.1 # Arrow width - marker.scale.z = 0.1 - marker.color.a = 1.0 - marker.color.r = 1.0 - marker.color.g = 0.0 - marker.color.b = 0.0 - - # Lifetime (0 = forever) - marker.lifetime.sec = 0 - marker.lifetime.nanosec = 0 - - self.marker_pub.publish(marker) - - -def main(args=None): - rclpy.init(args=args) - server_node = GotoPointController() - client_node = NavigateToPoseClient() - executor = ( - MultiThreadedExecutor() - ) # Allows processing multiple callbacks concurrently - executor.add_node(server_node) - executor.add_node(client_node) - try: - executor.spin() - except KeyboardInterrupt: - server_node.get_logger().info("Keyboard interrupt, shutting down...") - client_node.get_logger().info("Keyboard interrupt, shutting down...") - - finally: - server_node.destroy_node() - client_node.destroy_node() - executor.shutdown() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/cybership_controller/nodes/velocity_control.py b/cybership_controller/nodes/velocity_control.py deleted file mode 100755 index d8d85db..0000000 --- a/cybership_controller/nodes/velocity_control.py +++ /dev/null @@ -1,383 +0,0 @@ -#!/usr/bin/env python3 - -# ---------------------------------------------------------------------------- -# This code is part of the Cybership Software Suite -# Created By: Emir Cem Gezer -# Created Date: 2025-03-26 -# Copyright (C) 2025: NTNU, Trondheim -# Licensed under GPL-3.0-or-later -# --------------------------------------------------------------------------- - -import numpy as np -import rclpy -from rclpy.node import Node -from rclpy.parameter import Parameter -from geometry_msgs.msg import Twist, Wrench -from nav_msgs.msg import Odometry -import shoeboxpy.model3dof as box - -from cybership_controller.velocity.reference_feedforward import RffVelocityController as VelocityController - -# Add import for performance metrics -try: - from cybership_interfaces.msg import PerformanceMetrics -except ImportError: - print("cybership_msgs not found. Skipping performance metrics.") - - - -class VelocityControlNode(Node): - """ - ROS Node for velocity control using a reference filter. - Subscribes to cmd_vel for velocity commands and publishes control forces/moments. - """ - - def __init__(self): - super().__init__("velocity_control_node", namespace="voyager") - - # Declare parameters with default values - self.declare_parameters( - namespace='', - parameters=[ - # Vessel dimensions - ('vessel.length', 1.0), - ('vessel.beam', 0.3), - ('vessel.draft', 0.05), - - # Control gains - ('control.p_gain.surge', 5.0), - ('control.p_gain.sway', 5.0), - ('control.p_gain.yaw', 5.0), - - ('control.i_gain.surge', 0.0), - ('control.i_gain.sway', 0.0), - ('control.i_gain.yaw', 0.0), - - ('control.d_gain.surge', 0.3), - ('control.d_gain.sway', 0.3), - ('control.d_gain.yaw', 0.3), - - ('control.i_max', 20.0), - ('control.smooth_limit', True), - ('control.filter_alpha', 0.1), - - # Performance metrics - ('metrics.window_size', 50), - ('metrics.interval', 1.0), - - # Time step - ('dt', 0.1), - ] - ) - - # Add parameter callback for runtime updates - self.add_on_set_parameters_callback(self.parameters_callback) - - # Get current parameter values - self.dt = self.get_parameter('dt').value - - # Initialize vessel and controller - self.update_configuration() - - self.nu_prev = np.zeros(3) # [u, v, r] previous velocities - self.nu_cmd = np.zeros(3) # [u, v, r] desired velocities - self.nu_cmd_prev = np.zeros(3) # [u, v, r] desired velocities - - # Current velocity state - self.nu = np.zeros(3) # [u, v, r] - - # Subscribe to cmd_vel (velocity commands) - self.cmd_vel_sub = self.create_subscription( - Twist, - 'control/velocity/command', - self.cmd_vel_callback, - 10 - ) - - # Subscribe to odometry for current velocity - self.odom_sub = self.create_subscription( - Odometry, - 'measurement/odom', - self.odom_callback, - 1 - ) - - # Publisher for control commands - self.control_pub = self.create_publisher( - Wrench, - 'control/force/command/velocity', - 10 - ) - - # Add debug publishers for tracking performance - self.debug_vel_cmd_pub = self.create_publisher( - Twist, - 'control/velocity/debug/reference_velocity', - 10 - ) - self.debug_error_vel_pub = self.create_publisher( - Twist, - 'control/velocity/debug/tracking_error_velocity', - 10 - ) - - # Performance metrics publisher - self.debug_metrics_pub = None - if 'PerformanceMetrics' in globals(): - self.debug_metrics_pub = self.create_publisher( - PerformanceMetrics, - "control/velocity/debug/performance_metrics", - 10 - ) - - # --- Performance metrics --- - # Performance metrics tracking with moving window - self.window_size = self.get_parameter('metrics.window_size').value - self.error_window = [] # Store recent velocity errors - self.start_time = None - self.sample_count = 0 - - # Time between metrics calculations - self.metrics_interval = self.get_parameter('metrics.interval').value - self.last_metrics_time = 0.0 - - # Timer for control loop - self.timer = self.create_timer(self.dt, self.control_loop) - - self.get_logger().info("Velocity controller initialized with parameters") - - def update_configuration(self): - """Update vessel and controller configuration from parameters""" - # Get vessel dimensions - vessel_length = self.get_parameter('vessel.length').value - vessel_beam = self.get_parameter('vessel.beam').value - vessel_draft = self.get_parameter('vessel.draft').value - - # Create vessel model - self.vessel = box.Shoebox(L=vessel_length, B=vessel_beam, T=vessel_draft) - - # Get control gains - self.k_p_gain = np.array([ - self.get_parameter('control.p_gain.surge').value, - self.get_parameter('control.p_gain.sway').value, - self.get_parameter('control.p_gain.yaw').value - ]) - - self.k_i_gain = np.array([ - self.get_parameter('control.i_gain.surge').value, - self.get_parameter('control.i_gain.sway').value, - self.get_parameter('control.i_gain.yaw').value - ]) - - self.k_d_gain = np.array([ - self.get_parameter('control.d_gain.surge').value, - self.get_parameter('control.d_gain.sway').value, - self.get_parameter('control.d_gain.yaw').value - ]) - - # Create/update controller - self.controller = VelocityController( - config={ - "M": self.vessel.M_eff, - "D": self.vessel.D, - "Kp": np.diag(self.k_p_gain), - "Ki": np.diag(self.k_i_gain), - "Kd": np.diag(self.k_d_gain), - "I_max": self.get_parameter('control.i_max').value, - "smooth_limit": self.get_parameter('control.smooth_limit').value, - "filter_alpha": self.get_parameter('control.filter_alpha').value, - "dt": self.dt, - } - ) - - # Update metrics parameters - self.window_size = self.get_parameter('metrics.window_size').value - self.metrics_interval = self.get_parameter('metrics.interval').value - - self.get_logger().info(f"Updated configuration - vessel: [{vessel_length}, {vessel_beam}, {vessel_draft}], " + - f"P: {self.k_p_gain}, I: {self.k_i_gain}, D: {self.k_d_gain}") - - def parameters_callback(self, params): - """Handle parameter updates""" - update_needed = False - - for param in params: - if param.name in [ - 'vessel.length', 'vessel.beam', 'vessel.draft', - 'control.p_gain.surge', 'control.p_gain.sway', 'control.p_gain.yaw', - 'control.i_gain.surge', 'control.i_gain.sway', 'control.i_gain.yaw', - 'control.d_gain.surge', 'control.d_gain.sway', 'control.d_gain.yaw', - 'control.i_max', 'control.smooth_limit', 'control.filter_alpha', - 'metrics.window_size', 'metrics.interval' - ]: - update_needed = True - - if param.name == 'dt': - self.dt = param.value - # Recreate the timer with new dt - self.timer.cancel() - self.timer = self.create_timer(self.dt, self.control_loop) - update_needed = True - - # Update vessel and controller if parameters changed - if update_needed: - self.update_configuration() - - return True # Accept all parameter changes - - def cmd_vel_callback(self, msg: Twist): - """ - Process incoming velocity commands. - - Parameters: - ----------- - msg : Twist - Desired velocity command - """ - # Extract the desired velocities from the Twist message - # Linear velocities: x (surge), y (sway) - # Angular velocity: z (yaw rate) - self.nu_cmd = np.array([ - msg.linear.x, # surge velocity (u) - msg.linear.y, # sway velocity (v) - msg.angular.z # yaw rate (r) - ]) - - self.get_logger().debug( - f"Received cmd_vel: [{self.nu_cmd[0]:.2f}, {self.nu_cmd[1]:.2f}, {self.nu_cmd[2]:.2f}]") - - def odom_callback(self, msg: Odometry): - """ - Update current velocity from odometry. - - Parameters: - ----------- - msg : Odometry - Odometry message containing current state - """ - # Extract body-fixed velocities - self.nu = np.array([ - msg.twist.twist.linear.x, # surge velocity (u) - msg.twist.twist.linear.y, # sway velocity (v) - msg.twist.twist.angular.z # yaw rate (r) - ]) - - def process_performance_metrics(self, desired_vel, error_vel): - """ - Process and publish debug information using a moving window approach. - """ - # Publish command velocity for debugging - msg = Twist() - msg.linear.x = desired_vel[0] - msg.linear.y = desired_vel[1] - msg.angular.z = desired_vel[2] - - self.debug_vel_cmd_pub.publish(msg) - - # Publish velocity error for debugging - error_twist = Twist() - error_twist.linear.x = error_vel[0] - error_twist.linear.y = error_vel[1] - error_twist.linear.z = 0.0 - error_twist.angular.x = 0.0 - error_twist.angular.y = 0.0 - error_twist.angular.z = error_vel[2] - self.debug_error_vel_pub.publish(error_twist) - - # Update metrics using moving window - error_norm = np.sqrt( - error_vel[0]**2 + error_vel[1]**2 + error_vel[2]**2) - - # Initialize start time if not already set - if self.start_time is None: - self.start_time = self.get_clock().now().nanoseconds / 1e9 - - # Add newest error to window - self.error_window.append(error_norm) - - # Keep window at fixed size - if len(self.error_window) > self.window_size: - self.error_window.pop(0) - - self.sample_count += 1 - - current_time = self.get_clock().now().nanoseconds / 1e9 - if current_time - self.last_metrics_time >= self.metrics_interval and len(self.error_window) > 0: - self.last_metrics_time = current_time - - # Calculate window statistics - error_array = np.array(self.error_window) - - if self.debug_metrics_pub is not None: - metrics_msg = PerformanceMetrics() - metrics_msg.header.stamp = self.get_clock().now().to_msg() - metrics_msg.header.frame_id = "voyager" - metrics_msg.message = "Velocity tracking error" - - # Calculate statistics over the window - metrics_msg.mean = np.mean(error_array) - metrics_msg.median = np.median(error_array) - metrics_msg.rms = np.sqrt(np.mean(np.square(error_array))) - metrics_msg.min = np.min(error_array) - metrics_msg.max = np.max(error_array) - metrics_msg.stddev = np.std(error_array) - - self.debug_metrics_pub.publish(metrics_msg) - - self.get_logger().info( - f"Velocity tracking metrics - Mean: {metrics_msg.mean:.3f}, " - f"RMS: {metrics_msg.rms:.3f}, Max: {metrics_msg.max:.3f}" - ) - - def control_loop(self): - """ - Periodic control loop to update reference filter and publish control commands. - """ - tau = self.controller.update( - current_velocity=self.nu, - desired_velocity=self.nu_cmd, - dt=self.dt - ) - - # Calculate velocity error for metrics - error_vel = self.nu_cmd - self.nu - - # Process and publish performance metrics - self.process_performance_metrics(self.nu_cmd, error_vel) - - # Create and publish Wrench message - wrench_msg = Wrench() - wrench_msg.force.x = float(tau[0]) - wrench_msg.force.y = float(tau[1]) - wrench_msg.force.z = 0.0 - wrench_msg.torque.x = 0.0 - wrench_msg.torque.y = 0.0 - wrench_msg.torque.z = float(tau[2]) - - self.control_pub.publish(wrench_msg) - - -def main(args=None): - """ - Main function to initialize and run the velocity control node with a multithreaded executor. - """ - rclpy.init(args=args) - node = VelocityControlNode() - - # Create a multithreaded executor - executor = rclpy.executors.MultiThreadedExecutor() - executor.add_node(node) - - try: - # Spin the executor instead of the node directly - executor.spin() - except KeyboardInterrupt: - node.get_logger().info("Keyboard interrupt, shutting down...") - finally: - executor.shutdown() - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/cybership_controller/src/cybership_controller/velocity/reference_feedforward.py b/cybership_controller/src/cybership_controller/velocity/reference_feedforward.py index 61b6d80..d7cfb5c 100644 --- a/cybership_controller/src/cybership_controller/velocity/reference_feedforward.py +++ b/cybership_controller/src/cybership_controller/velocity/reference_feedforward.py @@ -59,10 +59,10 @@ def __init__(self, config: Optional[Dict[str, Any]] = None): # Internal state # for desired-velocity derivative - self._prev_vd: Optional[np.ndarray] = None + self._prev_vd: Optional[np.ndarray] = np.zeros(3) # previous desired velocity self._int_e: np.ndarray = np.zeros(3) # integral of error dt - self._prev_tau = None + self._prev_tau = np.zeros(3) # previous tau command def update(self, current_velocity: np.ndarray, desired_velocity: np.ndarray, dt: float) -> np.ndarray: r"""Compute the force/torque command tau. @@ -97,7 +97,7 @@ def update(self, current_velocity: np.ndarray, desired_velocity: np.ndarray, dt: """ if dt <= 0.0: - return self._prev_tau if self._prev_tau is not None else np.zeros(3) + return self._prev_tau v = current_velocity.reshape(3) vd = desired_velocity.reshape(3) @@ -128,3 +128,13 @@ def update(self, current_velocity: np.ndarray, desired_velocity: np.ndarray, dt: tau = self.M @ a_des + self.D @ vd self._prev_tau = tau.copy() return tau + + def reset(self): + """Reset the controller state.""" + self._prev_vd = np.zeros(3) + self._int_e = np.zeros(3) + self._prev_tau = np.zeros(3) + if hasattr(self, '_vd_dot_smoother'): + self._vd_dot_smoother.reset() + if hasattr(self, '_e_dot_smoother'): + self._e_dot_smoother.reset() \ No newline at end of file From dd2cd17e84119c296eb00c99812baf4c4d0ce9e6 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Mon, 14 Jul 2025 12:12:33 +0200 Subject: [PATCH 06/46] perf: much needed cleanup --- cybership_dp/launch/dp.launch.py | 1 - .../launch/position_controller.launch.py | 21 ------------------- .../launch/velocity_controller.launch.py | 21 ------------------- cybership_dp/nodes/position_control_node.py | 2 +- 4 files changed, 1 insertion(+), 44 deletions(-) diff --git a/cybership_dp/launch/dp.launch.py b/cybership_dp/launch/dp.launch.py index 349bc8b..16d4a21 100644 --- a/cybership_dp/launch/dp.launch.py +++ b/cybership_dp/launch/dp.launch.py @@ -70,7 +70,6 @@ def generate_launch_description(): '{topic: "control/force/command/position"}' ], output='screen', - condition=launch.conditions.LaunchConfigurationEquals('auto_switch_mux', 'true') ) ld.add_action(service_call_switch_mux) diff --git a/cybership_dp/launch/position_controller.launch.py b/cybership_dp/launch/position_controller.launch.py index 5b3fa62..f816705 100644 --- a/cybership_dp/launch/position_controller.launch.py +++ b/cybership_dp/launch/position_controller.launch.py @@ -33,17 +33,6 @@ def generate_launch_description(): name=f"position_controller", parameters=[ launch.substitutions.LaunchConfiguration("param_file"), - { - "vessel_model": launch.substitutions.LaunchConfiguration( - "vessel_model" - ), - }, - ], - arguments=[ - "--vessel-model", - launch.substitutions.LaunchConfiguration("vessel_model"), - "--vessel-name", - launch.substitutions.LaunchConfiguration("vessel_name"), ], remappings=[ ( @@ -58,14 +47,4 @@ def generate_launch_description(): ld.add_action(node_position_controller) - # Add argument to control whether to auto-switch the mux - arg_auto_switch = launch.actions.DeclareLaunchArgument( - name="auto_switch_mux", - default_value="true", - description="Automatically switch force mux to velocity controller output", - ) - ld.add_action(arg_auto_switch) - - - return ld diff --git a/cybership_dp/launch/velocity_controller.launch.py b/cybership_dp/launch/velocity_controller.launch.py index 02325b2..839bca8 100644 --- a/cybership_dp/launch/velocity_controller.launch.py +++ b/cybership_dp/launch/velocity_controller.launch.py @@ -33,17 +33,6 @@ def generate_launch_description(): name=f"velocity_controller", parameters=[ launch.substitutions.LaunchConfiguration("param_file"), - { - "vessel_model": launch.substitutions.LaunchConfiguration( - "vessel_model" - ), - }, - ], - arguments=[ - "--vessel-model", - launch.substitutions.LaunchConfiguration("vessel_model"), - "--vessel-name", - launch.substitutions.LaunchConfiguration("vessel_name"), ], remappings=[ ( @@ -80,14 +69,4 @@ def generate_launch_description(): ) ld.add_action(node_topic_mux) - # Add argument to control whether to auto-switch the mux - arg_auto_switch = launch.actions.DeclareLaunchArgument( - name="auto_switch_mux", - default_value="true", - description="Automatically switch force mux to velocity controller output", - ) - ld.add_action(arg_auto_switch) - - - return ld diff --git a/cybership_dp/nodes/position_control_node.py b/cybership_dp/nodes/position_control_node.py index 9bacbb2..1196c1a 100755 --- a/cybership_dp/nodes/position_control_node.py +++ b/cybership_dp/nodes/position_control_node.py @@ -28,7 +28,7 @@ from shoeboxpy.model3dof import Shoebox from visualization_msgs.msg import Marker from std_msgs.msg import Float32MultiArray - +from std_srvs.srv import SetBool, Trigger, Empty from cybership_controller.position.reference_filter import ThirdOrderReferenceFilter try: From d72bd31fce6f1284d3af784e2061c3cd08373f51 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Mon, 14 Jul 2025 13:15:30 +0200 Subject: [PATCH 07/46] feat: add mission controller --- .../config/voyager/controller_position.yaml | 8 +- cybership_dp/nodes/position_control_node.py | 55 ++++- cybership_dp/nodes/velocity_control_node.py | 49 ++++ cybership_tests/cybership_tests/mission.py | 230 ++++++++++++++++++ 4 files changed, 332 insertions(+), 10 deletions(-) create mode 100755 cybership_tests/cybership_tests/mission.py diff --git a/cybership_config/config/voyager/controller_position.yaml b/cybership_config/config/voyager/controller_position.yaml index 4d7798e..e0686d0 100644 --- a/cybership_config/config/voyager/controller_position.yaml +++ b/cybership_config/config/voyager/controller_position.yaml @@ -10,15 +10,15 @@ control: # Position control gains p_gain: - pos: 4.0 # Proportional gain for position + pos: 2.0 # Proportional gain for position vel: 0.7 # Proportional gain for velocity - yaw: 1.3 # Proportional gain for yaw + yaw: 0.9 # Proportional gain for yaw # Integral gains i_gain: pos: 0.2 # Integral gain for position vel: 0.1 # Integral gain for velocity - yaw: 0.2 # Integral gain for yaw + yaw: 0.1 # Integral gain for yaw # Derivative gains d_gain: @@ -29,7 +29,7 @@ # Integral error limits max_integral_error: pos: 1.0 # Maximum position error integration - yaw: 1.4 # Maximum yaw error integration + yaw: 0.6 # Maximum yaw error integration # Saturation parameters saturation: diff --git a/cybership_dp/nodes/position_control_node.py b/cybership_dp/nodes/position_control_node.py index 1196c1a..f2192d6 100755 --- a/cybership_dp/nodes/position_control_node.py +++ b/cybership_dp/nodes/position_control_node.py @@ -180,7 +180,6 @@ def __init__(self): self.error_pos = np.zeros(2) self.error_yaw = 0.0 - self.get_logger().info( "Goto Point Controller (Reference Filter Version) Initialized with parameters." ) @@ -197,6 +196,13 @@ def __init__(self): goal_callback=self.action_goal_callback, cancel_callback=self.action_cancel_callback, ) + # Service to enable/disable the controller + self.enabled = True + self.state_service = self.create_service( + SetBool, + f"{self.get_name()}/change_state", + self.change_state_callback + ) def update_configuration(self): """Update controller configuration from parameters""" @@ -212,10 +218,14 @@ def update_configuration(self): self.Kd_yaw = self.get_parameter('control.d_gain.yaw').value # Get controller limits - self.max_integral_error_pos = self.get_parameter('control.max_integral_error.pos').value - self.max_integral_error_yaw = self.get_parameter('control.max_integral_error.yaw').value - self.saturation_pos = self.get_parameter('control.saturation.pos').value - self.saturation_yaw = self.get_parameter('control.saturation.yaw').value + self.max_integral_error_pos = self.get_parameter( + 'control.max_integral_error.pos').value + self.max_integral_error_yaw = self.get_parameter( + 'control.max_integral_error.yaw').value + self.saturation_pos = self.get_parameter( + 'control.saturation.pos').value + self.saturation_yaw = self.get_parameter( + 'control.saturation.yaw').value # Get tolerances self.pos_tol = self.get_parameter('control.tolerance.pos').value @@ -407,6 +417,10 @@ def control_loop(self): Periodic control loop to update reference filter and publish control commands. This version uses a 3rd order reference filter to generate a smooth desired trajectory. """ + # Skip control loop if controller is disabled + if not self.enabled: + return + if self.latest_odom is None: return @@ -527,6 +541,34 @@ def action_cancel_callback(self, goal_handle): self.get_logger().info("Received cancel request for NavigateToPose") return CancelResponse.ACCEPT + def change_state_callback(self, request, response): + """Service callback to enable/disable the controller.""" + if request.data: + self.enabled = True + # Reset controller state + self.start_time = None + self.error_window.clear() + self.error_yaw_window.clear() + self.sample_count = 0 + self.last_metrics_time = 0.0 + self.integral_error_pos = np.zeros(2) + self.integral_error_yaw = 0.0 + self.error_pos = np.zeros(2) + self.error_yaw = 0.0 + self.ref_filter = None + + response.success = True + response.message = "Controller enabled and reset." + else: + self.enabled = False + # Publish zero forces and torques to reset the controller + zero_wrench = Wrench() + self.control_pub.publish(zero_wrench) + + response.success = True + response.message = "Controller disabled." + return response + def execute_callback(self, goal_handle): self.get_logger().info("Executing NavigateToPose goal...") @@ -554,13 +596,14 @@ def execute_callback(self, goal_handle): self.publish_target_pose_marker(target_pose) # Loop until the robot reaches the target (within tolerance) or the goal is canceled. - while rclpy.ok(): + while rclpy.ok(): # Loop until the robot reaches the target or goal is canceled if goal_handle.is_cancel_requested: goal_handle.canceled() self.get_logger().info("NavigateToPose goal canceled") result = NavigateToPose.Result() return result + # Wait for odometry if self.latest_odom is None: time.sleep(1.0) continue diff --git a/cybership_dp/nodes/velocity_control_node.py b/cybership_dp/nodes/velocity_control_node.py index 34c6f06..ffc10ec 100755 --- a/cybership_dp/nodes/velocity_control_node.py +++ b/cybership_dp/nodes/velocity_control_node.py @@ -16,6 +16,8 @@ from nav_msgs.msg import Odometry import shoeboxpy.model3dof as box +from std_srvs.srv import SetBool + from cybership_controller.velocity.reference_feedforward import RffVelocityController as VelocityController # Add import for performance metrics @@ -144,6 +146,14 @@ def __init__(self): # Timer for control loop self.timer = self.create_timer(self.dt, self.control_loop) + # Service to enable/disable the velocity controller + self.enabled = True + self.state_service = self.create_service( + SetBool, + f"{self.get_name()}/change_state", + self.change_state_callback + ) + self.get_logger().info("Velocity controller initialized with parameters") def update_configuration(self): @@ -225,6 +235,23 @@ def parameters_callback(self, params): return True # Accept all parameter changes + def change_state_callback(self, request, response): + """Service callback to enable/disable the velocity controller.""" + if request.data: + self.enabled = True + # Reset performance metrics state + self.start_time = None + self.error_window.clear() + self.sample_count = 0 + self.last_metrics_time = 0.0 + response.success = True + response.message = "Velocity controller enabled and reset." + else: + self.enabled = False + response.success = True + response.message = "Velocity controller disabled." + return response + def cmd_vel_callback(self, msg: Twist): """ Process incoming velocity commands. @@ -333,6 +360,9 @@ def control_loop(self): """ Periodic control loop to update reference filter and publish control commands. """ + # Skip control loop if controller is disabled + if not self.enabled: + return tau = self.controller.update( current_velocity=self.nu, desired_velocity=self.nu_cmd, @@ -356,6 +386,25 @@ def control_loop(self): self.control_pub.publish(wrench_msg) + def change_state_callback(self, request, response): + """ + Callback for the change_state service to enable/disable the controller. + """ + self.enabled = request.data + + if self.enabled: + response.message = "Velocity controller enabled" + self.get_logger().info(response.message) + else: + # Publish zero forces and torques to reset the controller + zero_wrench = Wrench() + self.control_pub.publish(zero_wrench) + response.message = "Velocity controller disabled" + self.get_logger().info(response.message) + + response.success = True + return response + def main(args=None): """ diff --git a/cybership_tests/cybership_tests/mission.py b/cybership_tests/cybership_tests/mission.py new file mode 100755 index 0000000..6fd52a0 --- /dev/null +++ b/cybership_tests/cybership_tests/mission.py @@ -0,0 +1,230 @@ +#!/usr/bin/env python3 + + +import rclpy +from rclpy.node import Node +from std_srvs.srv import SetBool +from topic_tools_interfaces.srv import MuxSelect +from geometry_msgs.msg import PoseStamped, Twist +from nav_msgs.msg import Odometry +import math +import time +from rclpy.action import ActionClient +from nav2_msgs.action import NavigateToPose + + +class ActionRunner(Node): + def __init__(self): + # Node namespace set at initialization; service and topic names are relative + super().__init__('action_runner', namespace='cybership') + # Define the mission actions + self.actions = [ + { + 'action': 'pose', + 'parameters': {'x': 0.0, 'y': 0.0, 'yaw': 0.0}, + 'duration': 2.0, + }, + { + 'action': 'velocity', + 'parameters': {'linear_x': 0.5, 'linear_y': 0.0, 'angular_z': 0.0}, + 'duration': 20.0, + }, + { + 'action': 'pose', + 'parameters': {'x': 0.0, 'y': 0.0, 'yaw': 0.0}, + 'duration': 2.0, + }, + { + 'action': 'velocity', + 'parameters': {'linear_x': 0.5, 'linear_y': 0.0, 'angular_z': 0.0}, + 'duration': 10.0, + }, + ] + self.repeat = 5 + + # Publisher for velocity controller input + self.vel_pub = self.create_publisher( + Twist, 'control/velocity/command', 10) + + # Mux select service client (relative to node namespace) + self.mux_client = self.create_client(MuxSelect, 'force_mux/select') + self.get_logger().info('Waiting for mux select service...') + self.mux_client.wait_for_service() + # Initialize odometry storage + self.current_odom = None + # Subscribe to odometry to monitor vehicle pose + self.create_subscription( + Odometry, 'measurement/odom', self.odom_callback, 10) + self.get_logger().info('Subscribed to measurement/odom') + # Action client for pose navigation (relative to node namespace) + self.pose_action_client = ActionClient( + self, NavigateToPose, 'navigate_to_pose') + self.get_logger().info('Waiting for NavigateToPose action server...') + self.pose_action_client.wait_for_server() + + # Enable/disable service clients (service names to be set later) + POSE_ENABLE_SERVICE = 'position_controller/change_state' + VELOCITY_ENABLE_SERVICE = 'velocity_controller/change_state' + + if POSE_ENABLE_SERVICE: + self.pose_enable_client = self.create_client( + SetBool, POSE_ENABLE_SERVICE) + self.pose_enable_client.wait_for_service() + else: + self.pose_enable_client = None + + if VELOCITY_ENABLE_SERVICE: + self.vel_enable_client = self.create_client( + SetBool, VELOCITY_ENABLE_SERVICE) + self.vel_enable_client.wait_for_service() + else: + self.vel_enable_client = None + + def call_mux(self, topic: str): + req = MuxSelect.Request() + req.topic = topic + future = self.mux_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result(): + self.get_logger().info(f'Mux switched to {topic}') + else: + self.get_logger().error(f'Failed to switch mux to {topic}') + + def set_enable(self, client, enable: bool, name: str): + if client is None: + return + req = SetBool.Request() + req.data = enable + future = client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result().success: + self.get_logger().info(f'Service {name} set enable={enable}') + else: + self.get_logger().error(f'Failed to set {name} enable={enable}') + + def send_pose_goal(self, x: float, y: float, yaw: float): + """ + Send a NavigateToPose action goal with given x, y, yaw. + """ + # Build goal message + msg = PoseStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = 'map' + msg.pose.position.x = x + msg.pose.position.y = y + msg.pose.position.z = 0.0 + # Orientation quaternion + qz = math.sin(yaw / 2.0) + qw = math.cos(yaw / 2.0) + msg.pose.orientation.z = qz + msg.pose.orientation.w = qw + + goal_msg = NavigateToPose.Goal() + goal_msg.pose = msg + self.get_logger().info( + f'Sending NavigateToPose goal: x={x}, y={y}, yaw={yaw}') + # Send goal + goal_future = self.pose_action_client.send_goal_async(goal_msg) + rclpy.spin_until_future_complete(self, goal_future) + goal_handle = goal_future.result() + if not goal_handle.accepted: + self.get_logger().error('NavigateToPose goal was rejected') + return + # Wait for result + result_future = goal_handle.get_result_async() + rclpy.spin_until_future_complete(self, result_future) + result = result_future.result().result + self.get_logger().info('NavigateToPose action completed') + + def run_pose_action(self, params, duration): + """Execute a pose action in a loop, monitoring odometry and abort conditions.""" + # Activate pose controller and send goal + self.call_mux('control/force/command/position') + self.set_enable(self.pose_enable_client, True, 'pose') + self.send_pose_goal(params['x'], params['y'], params['yaw']) + # Monitor until duration expires + start = self.get_clock().now().nanoseconds / 1e9 + while rclpy.ok() and (self.get_clock().now().nanoseconds / 1e9 - start) < duration: + if self.current_odom: + pos = self.current_odom.pose.pose.position + self.get_logger().debug( + f'Pose action - current pos: ({pos.x:.2f}, {pos.y:.2f})') + rclpy.spin_once(self, timeout_sec=0.1) + # Deactivate pose controller + self.set_enable(self.pose_enable_client, False, 'pose') + + def run_velocity_action(self, params, duration): + """Execute a velocity action in a loop, sending velocity commands continuously.""" + # Activate velocity controller + self.call_mux('control/force/command/velocity') + self.set_enable(self.vel_enable_client, True, 'velocity') + # Prepare command + msg = Twist() + msg.linear.x = params['linear_x'] + msg.linear.y = params['linear_y'] + msg.angular.z = params['angular_z'] + # Loop and publish + start = self.get_clock().now().nanoseconds / 1e9 + while rclpy.ok() and (self.get_clock().now().nanoseconds / 1e9 - start) < duration: + self.vel_pub.publish(msg) + if self.current_odom: + pos = self.current_odom.pose.pose.position + self.get_logger().debug( + f'Velocity action - current pos: ({pos.x:.2f}, {pos.y:.2f})') + rclpy.spin_once(self, timeout_sec=0.1) + # Deactivate velocity controller + self.set_enable(self.vel_enable_client, False, 'velocity') + + def run_wait_action(self, duration): + """Simple wait loop, still processing callbacks.""" + start = self.get_clock().now().nanoseconds / 1e9 + while rclpy.ok() and (self.get_clock().now().nanoseconds / 1e9 - start) < duration: + rclpy.spin_once(self, timeout_sec=0.1) + + def execute_actions(self): + + for _ in range(self.repeat): + + for action in self.actions: + act = action.get('action') + params = action.get('parameters', {}) + duration = action.get('duration', 0.0) + # Disable both controllers + self.set_enable(self.pose_enable_client, False, 'pose') + self.set_enable(self.vel_enable_client, False, 'velocity') + # Execute action-specific loop + if act == 'pose': + self.get_logger().info(f'Starting pose action for {duration}s') + self.run_pose_action(params, duration) + elif act == 'velocity': + self.get_logger().info( + f'Starting velocity action for {duration}s') + self.run_velocity_action(params, duration) + elif act == 'wait': + self.get_logger().info(f'Running wait for {duration}s') + self.run_wait_action(duration) + else: + self.get_logger().warn(f'Unknown action: {act}') + + def odom_callback(self, msg): + # Store and log the current vehicle pose from odometry data + self.current_odom = msg + pos = msg.pose.pose.position + ori = msg.pose.pose.orientation + self.get_logger().info( + f'Current pose - Position: ({pos.x:.2f}, {pos.y:.2f}, {pos.z:.2f}), ' + f'Orientation: ({ori.x:.2f}, {ori.y:.2f}, {ori.z:.2f}, {ori.w:.2f})' + ) + + +def main(args=None): + rclpy.init(args=args) + node = ActionRunner() + node.execute_actions() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() From ae0a285d5ccd454e382696ea8e52ae65b9d86453 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Tue, 15 Jul 2025 00:54:47 +0200 Subject: [PATCH 08/46] feat: add guidance --- cybership_controller/CMakeLists.txt | 13 ++ .../nodes/los_guidance_client.py | 90 +++++++++ .../nodes/los_guidance_server.py | 172 ++++++++++++++++++ cybership_controller/package.xml | 3 + .../src/cybership_controller/guidance/los.py | 92 ++++++++++ cybership_interfaces/CMakeLists.txt | 6 + .../action/LOSGuidance.action | 10 + 7 files changed, 386 insertions(+) create mode 100755 cybership_controller/nodes/los_guidance_client.py create mode 100755 cybership_controller/nodes/los_guidance_server.py create mode 100644 cybership_controller/src/cybership_controller/guidance/los.py create mode 100644 cybership_interfaces/action/LOSGuidance.action diff --git a/cybership_controller/CMakeLists.txt b/cybership_controller/CMakeLists.txt index a0c8a28..f1111f6 100644 --- a/cybership_controller/CMakeLists.txt +++ b/cybership_controller/CMakeLists.txt @@ -3,9 +3,22 @@ project(cybership_controller) find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(cybership_interfaces REQUIRED) ament_python_install_package(${PROJECT_NAME} PACKAGE_DIR src/${PROJECT_NAME} ) +# Install LOSGuidance action server node +file(GLOB LOS_SCRIPTS "${CMAKE_CURRENT_SOURCE_DIR}/nodes/*.py") +install( + PROGRAMS + ${LOS_SCRIPTS} + DESTINATION lib/${PROJECT_NAME} +) + ament_package() \ No newline at end of file diff --git a/cybership_controller/nodes/los_guidance_client.py b/cybership_controller/nodes/los_guidance_client.py new file mode 100755 index 0000000..4ca3c42 --- /dev/null +++ b/cybership_controller/nodes/los_guidance_client.py @@ -0,0 +1,90 @@ +#!/usr/bin/env python3 +""" +ROS2 Action Client for LOS Guidance +""" +import rclpy +from rclpy.node import Node +from rclpy.action import ActionClient +from rclpy.executors import MultiThreadedExecutor + +from nav_msgs.msg import Path +from geometry_msgs.msg import PoseStamped +from cybership_interfaces.action import LOSGuidance + +class LOSGuidanceClient(Node): + def __init__(self): + super().__init__('los_guidance_client') + self._action_client = ActionClient(self, LOSGuidance, 'los_guidance') + + def send_goal(self, waypoints): + goal_msg = LOSGuidance.Goal() + path_msg = Path() + path_msg.header.frame_id = 'world' + path_msg.header.stamp = self.get_clock().now().to_msg() + for x, y in waypoints: + pose = PoseStamped() + pose.header.frame_id = 'world' + pose.header.stamp = path_msg.header.stamp + pose.pose.position.x = x + pose.pose.position.y = y + pose.pose.position.z = 0.0 + pose.pose.orientation.w = 1.0 + path_msg.poses.append(pose) + goal_msg.path = path_msg + + self.get_logger().info('Waiting for action server...') + if not self._action_client.wait_for_server(timeout_sec=5.0): + self.get_logger().error('Action server not available, shutting down') + rclpy.shutdown() + return + + self.get_logger().info('Sending goal...') + send_goal_future = self._action_client.send_goal_async( + goal_msg, feedback_callback=self.feedback_callback) + send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info('Goal rejected :(') + return + self.get_logger().info('Goal accepted :)') + result_future = goal_handle.get_result_async() + result_future.add_done_callback(self.get_result_callback) + + def feedback_callback(self, feedback_msg): + feedback = feedback_msg.feedback + heading_deg = feedback.heading * 180.0 / 3.141592653589793 + vel = feedback.vel_cmd + self.get_logger().info( + f'Feedback: Heading={heading_deg:.1f}°, Vel=[{vel.x:.2f}, {vel.y:.2f}, {vel.z:.2f}]') + + def get_result_callback(self, future): + result = future.result().result + success = result.success + self.get_logger().info(f'Result: success={success}') + rclpy.shutdown() + + +def main(args=None): + rclpy.init(args=args) + client = LOSGuidanceClient() + # Example waypoints for testing + example_waypoints = [ + (5.0, 5.0), + (-5.0, 5.0), + (-5.0, -5.0), + (5.0, -5.0) + ] + client.send_goal(example_waypoints) + # Use MultiThreadedExecutor to handle feedback continuously + executor = MultiThreadedExecutor() + executor.add_node(client) + executor.spin() + executor.shutdown() + client.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/cybership_controller/nodes/los_guidance_server.py b/cybership_controller/nodes/los_guidance_server.py new file mode 100755 index 0000000..b2159fb --- /dev/null +++ b/cybership_controller/nodes/los_guidance_server.py @@ -0,0 +1,172 @@ +#!/usr/bin/env python3 +""" +ROS2 Action Server for LOS Guidance using cybership_interfaces/LOSGuidance +""" +import math +import rclpy +from rclpy.node import Node +from rclpy.action import ActionServer +from rclpy.action import CancelResponse, GoalResponse +from rclpy.executors import MultiThreadedExecutor +from rclpy.qos import QoSProfile, DurabilityPolicy + +from nav_msgs.msg import Path, Odometry +from geometry_msgs.msg import Twist, Vector3, Point +from visualization_msgs.msg import Marker +from tf_transformations import euler_from_quaternion + +from cybership_interfaces.action import LOSGuidance + +# Import underlying LOS guidance implementation +from cybership_controller.guidance.los import LOSGuidance as BaseLOSGuidance + +class LOSGuidanceROS(Node): + def __init__(self): + super().__init__('los_guidance_server', namespace="cybership") + # Parameters + self.declare_parameter('desired_speed', 0.3) + self.declare_parameter('lookahead', 1.0) + # Heading control gain + self.declare_parameter('heading_gain', 2.0) + # Publishers and Subscribers + self._cmd_pub = self.create_publisher(Twist, 'control/velocity/command', 10) + # Publish markers with transient local durability so RViZ receives past markers + self._marker_pub = self.create_publisher(Marker, 'visualization_marker', 10) + self._odom_sub = self.create_subscription(Odometry, 'measurement/odom', self.odom_callback, 10) + # Vehicle pose + self._position = None + self._yaw = None + # Action Server + self._action_server = ActionServer( + self, + LOSGuidance, + 'los_guidance', + execute_callback=self.execute_callback, + goal_callback=self.goal_callback, + cancel_callback=self.cancel_callback) + self.get_logger().info('LOS Guidance Action Server started') + + def odom_callback(self, msg: Odometry): + # Update position and heading + self._position = (msg.pose.pose.position.x, msg.pose.pose.position.y) + q = msg.pose.pose.orientation + # Convert quaternion to yaw using tf_transformations + quat = [q.x, q.y, q.z, q.w] + _, _, yaw = euler_from_quaternion(quat) + self._yaw = yaw + + def goal_callback(self, goal_request): + # Accept all goals + return GoalResponse.ACCEPT + + def cancel_callback(self, goal_handle): + return CancelResponse.ACCEPT + + def execute_callback(self, goal_handle): + # Extract waypoints from Path and include current vehicle position as initial point + path_msg: Path = goal_handle.request.path + # Build waypoints list, prepending vehicle's initial position if available + waypoints = [] + if self._position is not None: + waypoints.append(self._position) + + + waypoints.extend([(pose.pose.position.x, pose.pose.position.y) + for pose in path_msg.poses]) + # Initialize guidance + V_d = float(self.get_parameter('desired_speed').value) + delta = float(self.get_parameter('lookahead').value) + # Heading control gain + k_h = float(self.get_parameter('heading_gain').value) + guidance = BaseLOSGuidance(waypoints, V_d=V_d, delta=delta) + last_wp = waypoints[-1] + # Define rate for loop (10 Hz) + rate = self.create_rate(10) + + # Publish path marker + + while rclpy.ok(): + if goal_handle.is_cancel_requested: + goal_handle.canceled() + return LOSGuidance.Result(success=False) + if self._position is None or self._yaw is None: + rate.sleep() + continue + x, y = self._position + chi_d, vel_cmd = guidance.guidance(x, y) + twist = Twist() + # Compute forward velocity in body frame using current heading + v_forward = vel_cmd[0] * math.cos(self._yaw) + vel_cmd[1] * math.sin(self._yaw) + # Use computed forward speed (ensuring non-negative speed) + twist.linear.x = max(v_forward, 0.0) + twist.linear.y = 0.0 + twist.linear.z = 0.0 + # Compute and normalize heading error + heading_error = math.atan2(math.sin(chi_d - self._yaw), math.cos(chi_d - self._yaw)) + twist.angular.z = k_h * heading_error + self._cmd_pub.publish(twist) + # Feedback + feedback = LOSGuidance.Feedback() + feedback.heading = chi_d + feedback.vel_cmd = Vector3(x=vel_cmd[0], y=vel_cmd[1], z=0.0) + goal_handle.publish_feedback(feedback) + self.publish_path_marker(path_msg) + self.publish_arrow_marker(x, y, vel_cmd) + # Check completion + if math.hypot(x - last_wp[0], y - last_wp[1]) < delta: + break + rate.sleep() + + goal_handle.succeed() + result = LOSGuidance.Result() + result.success = True + return result + + def publish_path_marker(self, path_msg: Path): + marker = Marker() + marker.header = path_msg.header + marker.ns = 'los_path' + marker.id = 0 + marker.type = Marker.LINE_STRIP + marker.action = Marker.ADD + marker.scale.x = 0.05 + marker.color.r = 1.0; marker.color.g = 0.0; marker.color.b = 0.0; marker.color.a = 1.0 + marker.points = [Point(x=pose.pose.position.x, + y=pose.pose.position.y, + z=0.0) + for pose in path_msg.poses] + self._marker_pub.publish(marker) + + def publish_arrow_marker(self, x, y, vel_cmd): + marker = Marker() + marker.header.frame_id = 'world' + marker.header.stamp = self.get_clock().now().to_msg() + marker.ns = 'los_arrow' + marker.id = 1 + marker.type = Marker.ARROW + marker.action = Marker.ADD + marker.scale.x = 0.1 + marker.scale.y = 0.1 + marker.scale.z = 0.1 + # Use a copy of vel_cmd for scaling to avoid in-place modification + vel_arrow = vel_cmd * 3 + marker.color.r = 0.0; marker.color.g = 1.0; marker.color.b = 0.5; marker.color.a = 1.0 + start = Point(x=x, y=y, z=0.0) + end = Point(x=x + vel_arrow[0], y=y + vel_arrow[1], z=0.0) + marker.points = [start, end] + self._marker_pub.publish(marker) + + +def main(args=None): + rclpy.init(args=args) + node = LOSGuidanceROS() + executor = MultiThreadedExecutor() + executor.add_node(node) + executor.spin() + executor.shutdown() + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/cybership_controller/package.xml b/cybership_controller/package.xml index f37b06d..c56a997 100644 --- a/cybership_controller/package.xml +++ b/cybership_controller/package.xml @@ -12,6 +12,9 @@ rclpy geometry_msgs + nav_msgs + visualization_msgs + cybership_interfaces ament_lint_auto ament_lint_common diff --git a/cybership_controller/src/cybership_controller/guidance/los.py b/cybership_controller/src/cybership_controller/guidance/los.py new file mode 100644 index 0000000..af62c96 --- /dev/null +++ b/cybership_controller/src/cybership_controller/guidance/los.py @@ -0,0 +1,92 @@ +import numpy as np +from scipy.interpolate import splprep, splev +from scipy.optimize import minimize_scalar + +class LOSGuidance: + def __init__(self, pts, V_d, delta): + """ + Initialize the LOS guidance module. + + :param pts: array-like of shape (N, 2), waypoints in NED coordinates + :param V_d: desired speed (m/s) + :param delta: look-ahead distance (m) + """ + pts = np.asarray(pts) + # Fit cubic B-spline to the waypoints + self.tck, _ = splprep([pts[:, 0], pts[:, 1]], s=0, k=3) + self.V_d = V_d + self.delta = delta + + # Approximate total path length + samples = np.array(splev(np.linspace(0, 1, 500), self.tck)).T + diffs = np.diff(samples, axis=0) + self.path_len = np.sum(np.hypot(diffs[:, 0], diffs[:, 1])) + + def _project(self, x, y): + """ + Project the point (x, y) onto the spline to find the parameter s. + """ + def cost(u): + px, py = splev(u, self.tck) + return (px - x)**2 + (py - y)**2 + + res = minimize_scalar(cost, bounds=(0, 1), method='bounded') + return res.x + + def guidance(self, x, y): + """ + Compute the LOS guidance command. + + :param x: current NED x-position (m) + :param y: current NED y-position (m) + :return: (chi_d, vel_cmd) where chi_d is desired heading (rad), + and vel_cmd is a 2-element array [Vx, Vy] in NED. + """ + # 1) Project onto spline + s0 = self._project(x, y) + + # 2) Compute look-ahead parameter increment + ds = self.delta / self.path_len + s_la = min(s0 + ds, 1.0) + + # 3) Evaluate look-ahead point + x_la, y_la = splev(s_la, self.tck) + + # 4) Compute desired heading + dx, dy = x_la - x, y_la - y + chi_d = np.arctan2(dy, dx) + + # 5) Compute velocity command in NED + Vx = self.V_d * np.cos(chi_d) + Vy = self.V_d * np.sin(chi_d) + + return chi_d, np.array([Vx, Vy]) + + +if __name__ == "__main__": + # Example waypoints in NED (north-east) + waypoints = np.array([ + [0.0, 0.0], + [10.0, 5.0], + [20.0, 0.0], + [30.0, -5.0] + ]) + + # Initialize LOS guidance + desired_speed = 1.5 # m/s + lookahead = 5.0 # meters + los = LOSGuidance(waypoints, V_d=desired_speed, delta=lookahead) + + # Simulate evaluation at several positions + test_positions = [ + (2.0, 0.5), + (8.0, 3.0), + (15.0, -1.0), + (25.0, -4.0) + ] + + print("LOS Guidance Commands:") + for pos in test_positions: + chi_d, vel_cmd = los.guidance(*pos) + print(f"Position {pos} -> Heading: {np.degrees(chi_d):.1f}°, Vel_CMD: [{vel_cmd[0]:.2f}, {vel_cmd[1]:.2f}]") + diff --git a/cybership_interfaces/CMakeLists.txt b/cybership_interfaces/CMakeLists.txt index e753157..77ce1bd 100644 --- a/cybership_interfaces/CMakeLists.txt +++ b/cybership_interfaces/CMakeLists.txt @@ -7,6 +7,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(rosidl_default_generators REQUIRED) @@ -16,12 +17,17 @@ set(msg_files set(srv_files srv/ResetSimulator.srv ) +set(action_files + action/LOSGuidance.action +) rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} ${srv_files} + ${action_files} DEPENDENCIES geometry_msgs + nav_msgs builtin_interfaces ) diff --git a/cybership_interfaces/action/LOSGuidance.action b/cybership_interfaces/action/LOSGuidance.action new file mode 100644 index 0000000..164f9d0 --- /dev/null +++ b/cybership_interfaces/action/LOSGuidance.action @@ -0,0 +1,10 @@ +# Goal definition: path to follow +nav_msgs/Path path +# Result: final reached? not used +bool success +--- +# Empty result currently +--- +# Feedback: current heading and velocity command +float64 heading +geometry_msgs/Vector3 vel_cmd From dd7e915b47e38a37674c61627a3387aa68538b81 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Tue, 15 Jul 2025 01:15:35 +0200 Subject: [PATCH 09/46] feat: add marker visualizations --- .../nodes/los_guidance_client.py | 20 +++++++++++++----- .../nodes/los_guidance_server.py | 21 ++++++++++++++++++- .../src/cybership_controller/guidance/los.py | 15 +++++++++++++ 3 files changed, 50 insertions(+), 6 deletions(-) diff --git a/cybership_controller/nodes/los_guidance_client.py b/cybership_controller/nodes/los_guidance_client.py index 4ca3c42..088853c 100755 --- a/cybership_controller/nodes/los_guidance_client.py +++ b/cybership_controller/nodes/los_guidance_client.py @@ -71,11 +71,21 @@ def main(args=None): client = LOSGuidanceClient() # Example waypoints for testing example_waypoints = [ - (5.0, 5.0), - (-5.0, 5.0), - (-5.0, -5.0), - (5.0, -5.0) - ] + (0.0, 0.0), + (6.0, 0.0), + (6.0, 1.0), + (0.0, 1.0), + (0.0, 2.0), + (6.0, 2.0), + (6.0, 3.0), + (0.0, 3.0), + (0.0, 4.0), + (6.0, 4.0), + (6.0, 5.0), + (0.0, 5.0), + (0.0, 6.0), + (6.0, 6.0) + ] * 2 client.send_goal(example_waypoints) # Use MultiThreadedExecutor to handle feedback continuously executor = MultiThreadedExecutor() diff --git a/cybership_controller/nodes/los_guidance_server.py b/cybership_controller/nodes/los_guidance_server.py index b2159fb..239aa8f 100755 --- a/cybership_controller/nodes/los_guidance_server.py +++ b/cybership_controller/nodes/los_guidance_server.py @@ -19,6 +19,8 @@ # Import underlying LOS guidance implementation from cybership_controller.guidance.los import LOSGuidance as BaseLOSGuidance +import numpy as np +from scipy.interpolate import splev class LOSGuidanceROS(Node): def __init__(self): @@ -80,6 +82,8 @@ def execute_callback(self, goal_handle): k_h = float(self.get_parameter('heading_gain').value) guidance = BaseLOSGuidance(waypoints, V_d=V_d, delta=delta) last_wp = waypoints[-1] + # Visualize spline path + self.publish_spline_marker(guidance) # Define rate for loop (10 Hz) rate = self.create_rate(10) @@ -119,7 +123,6 @@ def execute_callback(self, goal_handle): goal_handle.succeed() result = LOSGuidance.Result() - result.success = True return result def publish_path_marker(self, path_msg: Path): @@ -156,6 +159,22 @@ def publish_arrow_marker(self, x, y, vel_cmd): marker.points = [start, end] self._marker_pub.publish(marker) + def publish_spline_marker(self, guidance): + marker = Marker() + marker.header.frame_id = 'world' + marker.header.stamp = self.get_clock().now().to_msg() + marker.ns = 'los_spline' + marker.id = 2 + marker.type = Marker.LINE_STRIP + marker.action = Marker.ADD + marker.scale.x = 0.05 + marker.color.r = 0.0; marker.color.g = 0.0; marker.color.b = 1.0; marker.color.a = 1.0 + # Sample spline path points + ts = np.linspace(0, 1, 100) + spline_pts = np.array(splev(ts, guidance.tck)).T + marker.points = [Point(x=pt[0], y=pt[1], z=0.0) for pt in spline_pts] + self._marker_pub.publish(marker) + def main(args=None): rclpy.init(args=args) diff --git a/cybership_controller/src/cybership_controller/guidance/los.py b/cybership_controller/src/cybership_controller/guidance/los.py index af62c96..3cc8230 100644 --- a/cybership_controller/src/cybership_controller/guidance/los.py +++ b/cybership_controller/src/cybership_controller/guidance/los.py @@ -12,6 +12,21 @@ def __init__(self, pts, V_d, delta): :param delta: look-ahead distance (m) """ pts = np.asarray(pts) + + # Add three intermediate points between each consecutive waypoint pair + expanded_pts = [pts[0]] # Start with first point + for i in range(len(pts) - 1): + start_pt = pts[i] + end_pt = pts[i + 1] + # Add 3 intermediate points + for j in range(1, 4): + alpha = j / 4.0 + intermediate_pt = start_pt + alpha * (end_pt - start_pt) + expanded_pts.append(intermediate_pt) + expanded_pts.append(end_pt) # Add the end point + + pts = np.array(expanded_pts) + # Fit cubic B-spline to the waypoints self.tck, _ = splprep([pts[:, 0], pts[:, 1]], s=0, k=3) self.V_d = V_d From aea128539192352096f91ff544e6fb7e48425de4 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Tue, 15 Jul 2025 11:32:45 +0200 Subject: [PATCH 10/46] feat: add simple mission planner --- .devcontainer/devcontainer.json | 2 +- README.md | 6 ++-- cybership_config/README.md | 15 +------- .../config/voyager/controller_position.yaml | 4 +-- .../src/cybership_controller/guidance/los.py | 4 +++ cybership_dp/README.md | 25 -------------- .../launch/position_controller.launch.py | 8 ++--- cybership_mocap/README.md | 2 +- cybership_tests/cybership_tests/mission.py | 34 ++++++++++++------- cybership_thrusters/README.md | 2 +- 10 files changed, 39 insertions(+), 63 deletions(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 13bd4ad..78eedd0 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -1,7 +1,7 @@ { "image": "incebellipipo/devcontainer:humble", "customizations": { - "terminalPrompt": "🐳 [container] ${containerName} ${folder}", + "terminalPrompt": "[container] ${containerName} ${folder}", "settings": { "terminal.integrated.shell.linux": "/bin/bash" }, diff --git a/README.md b/README.md index c76dced..421bd63 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,8 @@ # cybership_common -This repository is the home of the CyberShip Software Suite – a collection of ROS 2 packages that together provide the tools to simulate, visualize, and control autonomous maritime vessels. The suite covers everything from the digital twin (URDF models and visualization) to real-time sensor integration (IMU, motion capture, etc.) and advanced control (dynamic positioning and thrust allocation). +This repository is the home of the CyberShip Software Suite - a collection of ROS 2 packages that together provide the tools to simulate, visualize, and control autonomous maritime vessels. The suite covers everything from the digital twin (URDF models and visualization) to real-time sensor integration (IMU, motion capture, etc.) and advanced control (dynamic positioning and thrust allocation). -## What’s Included +## What's Included - **cybership_bringup**: Launch files to initialize all the necessary nodes for bringing up a vessel in a ROS 2 environment. This includes hardware drivers, localization, and sensor integration. - **cybership_simulator**: Simple Python scripts for vessel simulation using basic physics. Use these scripts to test vessel behavior in simulation. @@ -84,7 +84,7 @@ For Simulation: Use the cybership_simulator package. This runs simplified physics scripts: ### 3. Visualization -Visualize your vessel using the RViz visualization tools provided in the suite. The cybership_viz package sets up RViz with a preconfigured scene showing your vessel’s URDF model and sensor data overlays. To launch visualization: +Visualize your vessel using the RViz visualization tools provided in the suite. The cybership_viz package sets up RViz with a preconfigured scene showing your vessel's URDF model and sensor data overlays. To launch visualization: Additionally, the cybership_description package can publish the URDF model via: diff --git a/cybership_config/README.md b/cybership_config/README.md index b962ed8..a7f1d62 100644 --- a/cybership_config/README.md +++ b/cybership_config/README.md @@ -12,17 +12,4 @@ This package contains: ## Dependencies The package has minimal dependencies: -- `ament_cmake` (build dependency) - -## Structure - -The package uses a simple structure: -``` -cybership_config/ -├── CMakeLists.txt # Build configuration -├── LICENSE # GPL-3.0 license file -├── package.xml # Package metadata -├── README.md # This file -└── config/ # Configuration files directory - └── [various config files] -``` \ No newline at end of file +- `ament_cmake` (build dependency) \ No newline at end of file diff --git a/cybership_config/config/voyager/controller_position.yaml b/cybership_config/config/voyager/controller_position.yaml index e0686d0..fc9e485 100644 --- a/cybership_config/config/voyager/controller_position.yaml +++ b/cybership_config/config/voyager/controller_position.yaml @@ -29,7 +29,7 @@ # Integral error limits max_integral_error: pos: 1.0 # Maximum position error integration - yaw: 0.6 # Maximum yaw error integration + yaw: 0.1 # Maximum yaw error integration # Saturation parameters saturation: @@ -44,7 +44,7 @@ # Reference filter parameters filter: omega: [0.15, 0.15, 0.15] # Natural frequency for reference filter - delta: [0.8, 0.8, 0.8] # Damping ratio for reference filter + delta: [1.8, 1.8, 1.8] # Damping ratio for reference filter # Performance metrics configuration metrics: diff --git a/cybership_controller/src/cybership_controller/guidance/los.py b/cybership_controller/src/cybership_controller/guidance/los.py index 3cc8230..e60496f 100644 --- a/cybership_controller/src/cybership_controller/guidance/los.py +++ b/cybership_controller/src/cybership_controller/guidance/los.py @@ -7,6 +7,10 @@ def __init__(self, pts, V_d, delta): """ Initialize the LOS guidance module. + # TODO: This works as long as the waypoints are far apart. Path + # parameterization should be added between waypoints to ensure + # optimum doesn't jump between segments. + :param pts: array-like of shape (N, 2), waypoints in NED coordinates :param V_d: desired speed (m/s) :param delta: look-ahead distance (m) diff --git a/cybership_dp/README.md b/cybership_dp/README.md index 4279d9d..cb89256 100644 --- a/cybership_dp/README.md +++ b/cybership_dp/README.md @@ -11,31 +11,6 @@ The `cybership_dp` package implements control algorithms for dynamic positioning Both controllers are implemented as ROS 2 nodes and can be configured for different vessel models. -## Structure - -``` -cybership_dp/ -├── CMakeLists.txt # Build configuration -├── package.xml # Package metadata and dependencies -├── requirements.txt # Python dependencies -├── config/ # Controller configurations -│ ├── force_controller.yaml -│ ├── velocity_controller.yaml -│ └── [vessel_models]/ # Vessel-specific configs -├── cybership_dp/ # Python module -│ ├── __init__.py -│ ├── drillship/ # Drillship-specific implementations -│ ├── enterprise/ # Enterprise-specific implementations -│ └── voyager/ # Voyager-specific implementations -├── launch/ # Launch files -│ ├── dp.launch.py # Combined DP system launch -│ ├── force_controller.launch.py -│ └── velocity_controller.launch.py -└── nodes/ # ROS executable nodes - ├── force_controller.py - └── velocity_controller.py -``` - ## Usage ### Launch Files diff --git a/cybership_dp/launch/position_controller.launch.py b/cybership_dp/launch/position_controller.launch.py index f816705..1fc2810 100644 --- a/cybership_dp/launch/position_controller.launch.py +++ b/cybership_dp/launch/position_controller.launch.py @@ -10,7 +10,7 @@ def generate_launch_description(): ld = launch.LaunchDescription() arg_param_file = launch.actions.DeclareLaunchArgument( - name="param_file", + name="param", default_value=launch.substitutions.PathJoinSubstitution( [ launch_ros.substitutions.FindPackageShare("cybership_config"), @@ -22,8 +22,8 @@ def generate_launch_description(): ) ld.add_action(arg_param_file) - arguments = list(filter(lambda a: a.name not in ["param_file", "vessel_model"], ARGUMENTS)) - for arg in arguments: + # arguments = list(filter(lambda a: a.name not in ["param_file", "vessel_model"], ARGUMENTS)) + for arg in ARGUMENTS: ld.add_action(arg) node_position_controller = launch_ros.actions.Node( @@ -32,7 +32,7 @@ def generate_launch_description(): executable="position_control_node.py", name=f"position_controller", parameters=[ - launch.substitutions.LaunchConfiguration("param_file"), + launch.substitutions.LaunchConfiguration("param"), ], remappings=[ ( diff --git a/cybership_mocap/README.md b/cybership_mocap/README.md index e9e475b..a1f89bf 100644 --- a/cybership_mocap/README.md +++ b/cybership_mocap/README.md @@ -66,5 +66,5 @@ The node behavior can be configured via parameters in the `config.yaml` file: ### Published Transforms (if enabled) -- `mocap` → `cybership/base_link` - Transform from the world frame to the ship's base frame +- `mocap` -> `cybership/base_link` - Transform from the world frame to the ship's base frame diff --git a/cybership_tests/cybership_tests/mission.py b/cybership_tests/cybership_tests/mission.py index 6fd52a0..a8dfde7 100755 --- a/cybership_tests/cybership_tests/mission.py +++ b/cybership_tests/cybership_tests/mission.py @@ -11,7 +11,8 @@ import time from rclpy.action import ActionClient from nav2_msgs.action import NavigateToPose - +from tf_transformations import quaternion_from_euler +import numpy as np class ActionRunner(Node): def __init__(self): @@ -21,24 +22,32 @@ def __init__(self): self.actions = [ { 'action': 'pose', - 'parameters': {'x': 0.0, 'y': 0.0, 'yaw': 0.0}, - 'duration': 2.0, + 'parameters': {'x': -2.0, 'y': -2.0, 'yaw': np.pi / 4}, + 'duration': 10.0, }, { 'action': 'velocity', 'parameters': {'linear_x': 0.5, 'linear_y': 0.0, 'angular_z': 0.0}, - 'duration': 20.0, + 'duration': 30.0, + }, + { + 'action': 'wait', + 'duration': 5.0, }, { 'action': 'pose', - 'parameters': {'x': 0.0, 'y': 0.0, 'yaw': 0.0}, - 'duration': 2.0, + 'parameters': {'x': 2.0, 'y': 2.0, 'yaw': 5 * np.pi / 4}, + 'duration': 10.0, }, { 'action': 'velocity', 'parameters': {'linear_x': 0.5, 'linear_y': 0.0, 'angular_z': 0.0}, - 'duration': 10.0, + 'duration': 30.0, }, + { + 'action': 'wait', + 'duration': 5.0, + } ] self.repeat = 5 @@ -109,15 +118,16 @@ def send_pose_goal(self, x: float, y: float, yaw: float): # Build goal message msg = PoseStamped() msg.header.stamp = self.get_clock().now().to_msg() - msg.header.frame_id = 'map' + msg.header.frame_id = 'world' msg.pose.position.x = x msg.pose.position.y = y msg.pose.position.z = 0.0 # Orientation quaternion - qz = math.sin(yaw / 2.0) - qw = math.cos(yaw / 2.0) - msg.pose.orientation.z = qz - msg.pose.orientation.w = qw + quat = quaternion_from_euler(0.0, 0.0, yaw) + msg.pose.orientation.x = quat[0] + msg.pose.orientation.y = quat[1] + msg.pose.orientation.z = quat[2] + msg.pose.orientation.w = quat[3] goal_msg = NavigateToPose.Goal() goal_msg.pose = msg diff --git a/cybership_thrusters/README.md b/cybership_thrusters/README.md index c90b18a..1159cfa 100644 --- a/cybership_thrusters/README.md +++ b/cybership_thrusters/README.md @@ -119,7 +119,7 @@ thrusters: signal_inverted: true ``` -You can load this configuration via a YAML file and pass it as an argument to your node or use ROS2’s parameter mechanisms to set these values dynamically. +You can load this configuration via a YAML file and pass it as an argument to your node or use ROS2's parameter mechanisms to set these values dynamically. ## Additional Information From 532974d2ccba10a6fbf4530095ad692307e7cbe3 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Wed, 16 Jul 2025 10:42:22 +0200 Subject: [PATCH 11/46] feat: add random sampling for experiments --- cybership_tests/cybership_tests/mission.py | 49 +++++++++++++++------- 1 file changed, 33 insertions(+), 16 deletions(-) diff --git a/cybership_tests/cybership_tests/mission.py b/cybership_tests/cybership_tests/mission.py index a8dfde7..6afa4a6 100755 --- a/cybership_tests/cybership_tests/mission.py +++ b/cybership_tests/cybership_tests/mission.py @@ -14,11 +14,15 @@ from tf_transformations import quaternion_from_euler import numpy as np + class ActionRunner(Node): def __init__(self): # Node namespace set at initialization; service and topic names are relative super().__init__('action_runner', namespace='cybership') # Define the mission actions + + self.rng = np.random.default_rng() + self.actions = [ { 'action': 'pose', @@ -27,7 +31,12 @@ def __init__(self): }, { 'action': 'velocity', - 'parameters': {'linear_x': 0.5, 'linear_y': 0.0, 'angular_z': 0.0}, + 'parameters': { + # Sample linear and angular components each publish + 'linear_x': lambda: self.rng.uniform(0.4, 0.6), + 'linear_y': lambda: self.rng.uniform(-0.1, 0.1), + 'angular_z': lambda: self.rng.uniform(-0.1, 0.1) + }, 'duration': 30.0, }, { @@ -41,7 +50,12 @@ def __init__(self): }, { 'action': 'velocity', - 'parameters': {'linear_x': 0.5, 'linear_y': 0.0, 'angular_z': 0.0}, + 'parameters': { + # Sample linear and angular components each publish + 'linear_x': lambda: self.rng.uniform(0.4, 0.6), + 'linear_y': lambda: self.rng.uniform(-0.1, 0.1), + 'angular_z': lambda: self.rng.uniform(-0.1, 0.1) + }, 'duration': 30.0, }, { @@ -168,19 +182,27 @@ def run_velocity_action(self, params, duration): # Activate velocity controller self.call_mux('control/force/command/velocity') self.set_enable(self.vel_enable_client, True, 'velocity') - # Prepare command + # Loop and publish, sampling parameters if callable msg = Twist() - msg.linear.x = params['linear_x'] - msg.linear.y = params['linear_y'] - msg.angular.z = params['angular_z'] - # Loop and publish + lx = params['linear_x']() if callable(params.get( + 'linear_x')) else params.get('linear_x', 0.0) + ly = params['linear_y']() if callable(params.get( + 'linear_y')) else params.get('linear_y', 0.0) + az = params['angular_z']() if callable(params.get( + 'angular_z')) else params.get('angular_z', 0.0) + self.get_logger().info( + f'Velocity action (u={lx:.2f}, v={ly:.2f}, r={az:.2f})') start = self.get_clock().now().nanoseconds / 1e9 while rclpy.ok() and (self.get_clock().now().nanoseconds / 1e9 - start) < duration: + # Sample velocity parameters if provided as callables + + msg.linear.x = lx + msg.linear.y = ly + msg.angular.z = az self.vel_pub.publish(msg) if self.current_odom: pos = self.current_odom.pose.pose.position - self.get_logger().debug( - f'Velocity action - current pos: ({pos.x:.2f}, {pos.y:.2f})') + rclpy.spin_once(self, timeout_sec=0.1) # Deactivate velocity controller self.set_enable(self.vel_enable_client, False, 'velocity') @@ -204,7 +226,8 @@ def execute_actions(self): self.set_enable(self.vel_enable_client, False, 'velocity') # Execute action-specific loop if act == 'pose': - self.get_logger().info(f'Starting pose action for {duration}s') + self.get_logger().info( + f'Starting pose action for {duration}s') self.run_pose_action(params, duration) elif act == 'velocity': self.get_logger().info( @@ -219,12 +242,6 @@ def execute_actions(self): def odom_callback(self, msg): # Store and log the current vehicle pose from odometry data self.current_odom = msg - pos = msg.pose.pose.position - ori = msg.pose.pose.orientation - self.get_logger().info( - f'Current pose - Position: ({pos.x:.2f}, {pos.y:.2f}, {pos.z:.2f}), ' - f'Orientation: ({ori.x:.2f}, {ori.y:.2f}, {ori.z:.2f}, {ori.w:.2f})' - ) def main(args=None): From 13893f9266ccc74337fff901634d09e5b41de24e Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Sat, 19 Jul 2025 13:15:37 +0200 Subject: [PATCH 12/46] fix: mux topics and infinite abort action on mission --- .../launch/thrust_allocation.launch.py | 2 - .../launch/velocity_controller.launch.py | 2 - cybership_tests/cybership_tests/mission.py | 509 +++++++++++++----- cybership_viz/config/voyager.rviz | 41 +- 4 files changed, 418 insertions(+), 136 deletions(-) diff --git a/cybership_dp/launch/thrust_allocation.launch.py b/cybership_dp/launch/thrust_allocation.launch.py index 50390ab..358fd10 100644 --- a/cybership_dp/launch/thrust_allocation.launch.py +++ b/cybership_dp/launch/thrust_allocation.launch.py @@ -51,8 +51,6 @@ def generate_launch_description(): "control/force/command", # Input topic to listen to "control/force/command/velocity", # Input topic to listen to "control/force/command/position", # Input topic to listen to - "--repeat-delay", "0.1" # Optional delay for repeated messages - "--initial_topic", "control/force/command" # Initial topic to publish ], output="screen", respawn=True, diff --git a/cybership_dp/launch/velocity_controller.launch.py b/cybership_dp/launch/velocity_controller.launch.py index 839bca8..92c89c4 100644 --- a/cybership_dp/launch/velocity_controller.launch.py +++ b/cybership_dp/launch/velocity_controller.launch.py @@ -60,8 +60,6 @@ def generate_launch_description(): arguments=[ "control/velocity/command/mux", # Output topic "control/velocity/command", # Input topic to listen to - "--repeat-delay", "0.1", # Optional delay for repeated messages - "--initial-topic", "control/velocity/command" # Initial topic to publish ], output="screen", respawn=True, diff --git a/cybership_tests/cybership_tests/mission.py b/cybership_tests/cybership_tests/mission.py index 6afa4a6..58bd049 100755 --- a/cybership_tests/cybership_tests/mission.py +++ b/cybership_tests/cybership_tests/mission.py @@ -4,8 +4,8 @@ import rclpy from rclpy.node import Node from std_srvs.srv import SetBool -from topic_tools_interfaces.srv import MuxSelect -from geometry_msgs.msg import PoseStamped, Twist +from topic_tools_interfaces.srv import MuxSelect, MuxAdd +from geometry_msgs.msg import PoseStamped, Twist, Wrench from nav_msgs.msg import Odometry import math import time @@ -21,64 +21,24 @@ def __init__(self): super().__init__('action_runner', namespace='cybership') # Define the mission actions - self.rng = np.random.default_rng() + self.add_mux('control/force/command/constant') - self.actions = [ - { - 'action': 'pose', - 'parameters': {'x': -2.0, 'y': -2.0, 'yaw': np.pi / 4}, - 'duration': 10.0, - }, - { - 'action': 'velocity', - 'parameters': { - # Sample linear and angular components each publish - 'linear_x': lambda: self.rng.uniform(0.4, 0.6), - 'linear_y': lambda: self.rng.uniform(-0.1, 0.1), - 'angular_z': lambda: self.rng.uniform(-0.1, 0.1) - }, - 'duration': 30.0, - }, - { - 'action': 'wait', - 'duration': 5.0, - }, - { - 'action': 'pose', - 'parameters': {'x': 2.0, 'y': 2.0, 'yaw': 5 * np.pi / 4}, - 'duration': 10.0, - }, - { - 'action': 'velocity', - 'parameters': { - # Sample linear and angular components each publish - 'linear_x': lambda: self.rng.uniform(0.4, 0.6), - 'linear_y': lambda: self.rng.uniform(-0.1, 0.1), - 'angular_z': lambda: self.rng.uniform(-0.1, 0.1) - }, - 'duration': 30.0, - }, - { - 'action': 'wait', - 'duration': 5.0, - } - ] self.repeat = 5 # Publisher for velocity controller input self.vel_pub = self.create_publisher( Twist, 'control/velocity/command', 10) + # Publisher for constant force command + self.force_pub = self.create_publisher( + Wrench, 'control/force/command/constant', 10) - # Mux select service client (relative to node namespace) - self.mux_client = self.create_client(MuxSelect, 'force_mux/select') - self.get_logger().info('Waiting for mux select service...') - self.mux_client.wait_for_service() # Initialize odometry storage self.current_odom = None # Subscribe to odometry to monitor vehicle pose self.create_subscription( Odometry, 'measurement/odom', self.odom_callback, 10) self.get_logger().info('Subscribed to measurement/odom') + # Action client for pose navigation (relative to node namespace) self.pose_action_client = ActionClient( self, NavigateToPose, 'navigate_to_pose') @@ -103,10 +63,171 @@ def __init__(self): else: self.vel_enable_client = None + self.rng = np.random.default_rng() + + self.wait_until_ready() + + # Default abort lands back at origin after timeout + self.default_abort_action = { + 'name': 'default_abort', + 'action': self.run_pose_action, + 'parameters': {'x': 0.0, 'y': 0.0, 'yaw': 0.0, 'duration': 60.0, 'grace': 15.0}, + } + + self.actions = [ + { + 'name': 'pose_neg_diag', + 'action': self.run_pose_action, + 'parameters': { + 'x': -2.0, + 'y': -2.0, + 'yaw': np.pi / 4, + 'duration': 60.0 + }, + }, + { + 'name': 'velocity_random_1', + 'action': self.run_velocity_action, + 'parameters': { + # Sample linear and angular components each publish + 'linear_x': lambda: self.rng.uniform(0.2, 0.4), + 'linear_y': lambda: self.rng.uniform(-0.1, 0.1), + 'angular_z': lambda: self.rng.uniform(-0.1, 0.1), + 'duration': 30.0, + 'margin': 0.1 # Allow some margin for early success + }, + 'abort_action': { + 'name': 'velocity_random_1_abort', + 'action': self.run_pose_action, + 'parameters': { + 'x': 2.0, + 'y': 2.0, + 'yaw': 5 * np.pi / 4, + 'duration': 60.0, + 'grace': 15.0 + } + } + }, + { + 'name': 'force_random_1', + 'action': self.run_force_action, + 'parameters': { + 'force_x': lambda: self.rng.uniform(0.2, 0.8), + 'force_y': lambda: self.rng.uniform(-0.5, 0.5), + 'torque_z': lambda: self.rng.uniform(-0.1, 0.1), + 'duration': 30.0, + }, + 'abort_action': { + 'name': 'force_random_1_abort', + 'action': self.run_pose_action, + 'parameters': { + 'x': 2.0, + 'y': 2.0, + 'yaw': 5 * np.pi / 4, + 'duration': 60.0, + 'grace': 15.0 + } + } + }, + { + 'name': 'pose_pos_diag', + 'action': self.run_pose_action, + 'parameters': { + 'x': 2.0, + 'y': 2.0, + 'yaw': 5 * np.pi / 4, + 'duration': 60.0 + }, + }, + { + 'name': 'velocity_random_2', + 'action': self.run_velocity_action, + 'parameters': { + # Sample linear and angular components each publish + 'linear_x': lambda: self.rng.uniform(0.2, 0.4), + 'linear_y': lambda: self.rng.uniform(-0.1, 0.1), + 'angular_z': lambda: self.rng.uniform(-0.1, 0.1), + 'duration': 30.0, + 'margin': 0.1 # Allow some margin for early success + }, + 'abort_action': { + 'name': 'velocity_random_2_abort', + 'action': self.run_pose_action, + 'parameters': { + 'x': -2.0, + 'y': -2.0, + 'yaw': np.pi / 4, + 'duration': 60.0, + 'grace': 15.0, + } + } + }, + { + 'name': 'force_random_2', + 'action': self.run_force_action, + 'parameters': { + 'force_x': lambda: self.rng.uniform(0.2, 0.8), + 'force_y': lambda: self.rng.uniform(-0.5, 0.5), + 'torque_z': lambda: self.rng.uniform(-0.1, 0.1), + 'duration': 30.0, + }, + 'abort_action': { + 'name': 'force_random_2_abort', + 'action': self.run_pose_action, + 'parameters': { + 'x': -2.0, + 'y': -2.0, + 'yaw': np.pi / 4, + 'duration': 60.0, + 'grace': 15.0 + } + } + }, + { + 'name': 'wait', + 'action': self.run_wait_action, + 'parameters': {'duration': 10.0} + } + ] + + def wait_until_ready(self): + """ + Wait until the odometry has been read at least once. + This is useful to ensure the vehicle has a valid pose before starting actions. + """ + while rclpy.ok() and self.current_odom is None: + self.get_logger().info('Waiting for initial odometry data...') + rclpy.spin_once(self, timeout_sec=0.1) + self.get_logger().info('Initial odometry data received.') + + def add_mux(self, topic: str): + """Add a topic to the mux via MuxAdd service.""" + req = MuxAdd.Request() + mux_add_client = self.create_client(MuxAdd, 'force_mux/add') + req.topic = topic + future = mux_add_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + # MuxAdd returns a result with a success flag + if hasattr(future.result(), 'success') and future.result().success: + self.get_logger().info(f'Mux added topic {topic}') + else: + self.get_logger().error(f'Failed to add mux topic {topic}') + + def abort_condition(self): + """ + Check if the abort condition is met based on the current odometry. + This can be overridden in derived classes to implement specific conditions. + """ + return abs(self.current_odom.pose.pose.position.x) > 2.5 or abs(self.current_odom.pose.pose.position.y) > 2.5 + def call_mux(self, topic: str): + + mux_client = self.create_client(MuxSelect, 'force_mux/select') + mux_client.wait_for_service() + self.get_logger().info(f'Calling mux select for topic {topic}') req = MuxSelect.Request() req.topic = topic - future = self.mux_client.call_async(req) + future = mux_client.call_async(req) rclpy.spin_until_future_complete(self, future) if future.result(): self.get_logger().info(f'Mux switched to {topic}') @@ -125,119 +246,257 @@ def set_enable(self, client, enable: bool, name: str): else: self.get_logger().error(f'Failed to set {name} enable={enable}') - def send_pose_goal(self, x: float, y: float, yaw: float): - """ - Send a NavigateToPose action goal with given x, y, yaw. - """ + def _pose_feedback_callback(self, feedback_msg): + """Log distance remaining from NavigateToPose feedback.""" + fb = feedback_msg.feedback + # nav2 NavigateToPose feedback has distance_remaining + dist = getattr(fb, 'distance_remaining', None) + if dist is not None: + self.get_logger().info(f' NavigateToPose feedback: distance_remaining={dist:.2f}') + else: + self.get_logger().info(' NavigateToPose feedback received') + + def run_pose_action(self, **params): + """Unified pose action: send NavigateToPose goal and monitor abort (including while waiting).""" + # Activate pose controller via mux & service + self.call_mux('control/force/command/position') + self.set_enable(self.pose_enable_client, True, 'pose') + + # Extract parameters + x = params.get('x', 0.0) + y = params.get('y', 0.0) + yaw = params.get('yaw', 0.0) + duration = params.get('duration', 0.0) + grace = params.get('grace', 0.0) + self.get_logger().info( + f'Pose action start (x={x:.2f}, y={y:.2f}, yaw={yaw:.2f}), ' + f'duration={duration:.1f}s, grace={grace:.1f}s' + ) + # Build goal message msg = PoseStamped() msg.header.stamp = self.get_clock().now().to_msg() msg.header.frame_id = 'world' msg.pose.position.x = x msg.pose.position.y = y - msg.pose.position.z = 0.0 - # Orientation quaternion - quat = quaternion_from_euler(0.0, 0.0, yaw) - msg.pose.orientation.x = quat[0] - msg.pose.orientation.y = quat[1] - msg.pose.orientation.z = quat[2] - msg.pose.orientation.w = quat[3] - - goal_msg = NavigateToPose.Goal() - goal_msg.pose = msg - self.get_logger().info( - f'Sending NavigateToPose goal: x={x}, y={y}, yaw={yaw}') - # Send goal - goal_future = self.pose_action_client.send_goal_async(goal_msg) - rclpy.spin_until_future_complete(self, goal_future) - goal_handle = goal_future.result() - if not goal_handle.accepted: - self.get_logger().error('NavigateToPose goal was rejected') + q = quaternion_from_euler(0.0, 0.0, yaw) + msg.pose.orientation.x = q[0] + msg.pose.orientation.y = q[1] + msg.pose.orientation.z = q[2] + msg.pose.orientation.w = q[3] + goal = NavigateToPose.Goal() + goal.pose = msg + + # Send goal with feedback callback + goal_fut = self.pose_action_client.send_goal_async( + goal, feedback_callback=self._pose_feedback_callback + ) + rclpy.spin_until_future_complete(self, goal_fut) + handle = goal_fut.result() + if not handle.accepted: + self.get_logger().error('NavigateToPose goal rejected') + self.set_enable(self.pose_enable_client, False, 'pose') return - # Wait for result - result_future = goal_handle.get_result_async() - rclpy.spin_until_future_complete(self, result_future) - result = result_future.result().result - self.get_logger().info('NavigateToPose action completed') - - def run_pose_action(self, params, duration): - """Execute a pose action in a loop, monitoring odometry and abort conditions.""" - # Activate pose controller and send goal - self.call_mux('control/force/command/position') - self.set_enable(self.pose_enable_client, True, 'pose') - self.send_pose_goal(params['x'], params['y'], params['yaw']) - # Monitor until duration expires + + self.get_logger().info('NavigateToPose goal accepted, monitoring execution') + + # Wait for result, checking abort & timeout + result_fut = handle.get_result_async() start = self.get_clock().now().nanoseconds / 1e9 - while rclpy.ok() and (self.get_clock().now().nanoseconds / 1e9 - start) < duration: - if self.current_odom: - pos = self.current_odom.pose.pose.position - self.get_logger().debug( - f'Pose action - current pos: ({pos.x:.2f}, {pos.y:.2f})') - rclpy.spin_once(self, timeout_sec=0.1) + elapsed = 0.0 + while rclpy.ok(): + elapsed = (self.get_clock().now().nanoseconds / 1e9) - start + + if elapsed >= duration: + self.get_logger().info('NavigateToPose action completed due to duration') + cancel_fut = handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, cancel_fut) + self.set_enable(self.pose_enable_client, False, 'pose') + break + + if result_fut.done(): + self.get_logger().info('NavigateToPose action completed') + break + + if elapsed >= grace and self.abort_condition(): + + cancel_fut = handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, cancel_fut) + self.set_enable(self.pose_enable_client, False, 'pose') + raise RuntimeError('Abort condition met during pose action, aborting') + + rclpy.spin_once(self, timeout_sec=0.5) + # Deactivate pose controller self.set_enable(self.pose_enable_client, False, 'pose') - def run_velocity_action(self, params, duration): + def run_velocity_action(self, **params): """Execute a velocity action in a loop, sending velocity commands continuously.""" # Activate velocity controller - self.call_mux('control/force/command/velocity') + self.call_mux("control/force/command/velocity") self.set_enable(self.vel_enable_client, True, 'velocity') - # Loop and publish, sampling parameters if callable + + # Prepare and log command msg = Twist() - lx = params['linear_x']() if callable(params.get( - 'linear_x')) else params.get('linear_x', 0.0) - ly = params['linear_y']() if callable(params.get( - 'linear_y')) else params.get('linear_y', 0.0) - az = params['angular_z']() if callable(params.get( - 'angular_z')) else params.get('angular_z', 0.0) - self.get_logger().info( - f'Velocity action (u={lx:.2f}, v={ly:.2f}, r={az:.2f})') + lx = params.get('linear_x', 0.0) + ly = params.get('linear_y', 0.0) + az = params.get('angular_z', 0.0) + margin = params.get('margin', None) + self.get_logger().info(' ' + f'Velocity action (u={lx:.4f}, v={ly:.4f}, r={az:.4f})' + + (f', margin={margin:.4f}' if margin is not None else '')) + + # Extract duration, grace period and start time + duration = params.get('duration', 0.0) + grace = params.get('grace', 0.0) start = self.get_clock().now().nanoseconds / 1e9 - while rclpy.ok() and (self.get_clock().now().nanoseconds / 1e9 - start) < duration: - # Sample velocity parameters if provided as callables + elapsed = 0.0 + while rclpy.ok() and elapsed < duration: + elapsed = self.get_clock().now().nanoseconds / 1e9 - start + # Publish the commanded velocity msg.linear.x = lx msg.linear.y = ly msg.angular.z = az self.vel_pub.publish(msg) + + # Feedback: log current odometry state if self.current_odom: pos = self.current_odom.pose.pose.position + vel = self.current_odom.twist.twist.linear + self.get_logger().info( + ' ' + f'Velocity feedback: pos=({pos.x:.2f}, {pos.y:.2f}), ' + f'vel=({vel.x:.2f}, {vel.y:.2f})' + ) + + # margin-based early success + if margin is not None: + act = self.current_odom.twist.twist + if (abs(act.linear.x - lx) < margin and + abs(act.linear.y - ly) < margin): + self.get_logger().info('Velocity within margin, stopping early') + break + + # Abort check after grace period + if self.current_odom and elapsed >= grace and self.abort_condition(): + raise RuntimeError('Abort condition met during velocity action, aborting') + + rclpy.spin_once(self, timeout_sec=0.5) - rclpy.spin_once(self, timeout_sec=0.1) # Deactivate velocity controller self.set_enable(self.vel_enable_client, False, 'velocity') - def run_wait_action(self, duration): + def run_wait_action(self, **params): """Simple wait loop, still processing callbacks.""" + # Extract duration, optional grace period, and start time + duration = params.get('duration', 0.0) + grace = params.get('grace', 0.0) start = self.get_clock().now().nanoseconds / 1e9 - while rclpy.ok() and (self.get_clock().now().nanoseconds / 1e9 - start) < duration: + + self.get_logger().info(f'Waiting for {duration:.4f} seconds') + + elapsed = 0.0 + while rclpy.ok() and elapsed < duration: + # abort check after grace period + elapsed = self.get_clock().now().nanoseconds / 1e9 - start + + if elapsed >= grace and self.abort_condition(): + raise RuntimeError('Abort condition met during wait action, aborting') + rclpy.spin_once(self, timeout_sec=0.1) + # Feedback: log current odometry state + if self.current_odom: + pos = self.current_odom.pose.pose.position + self.get_logger().info( + f'Wait feedback: pos=({pos.x:.2f}, {pos.y:.2f}), elapsed={elapsed:.2f}s' + ) + + def run_force_action(self, **params): + """Execute a constant force action in a loop.""" + # Activate force topic on mux + self.call_mux("control/force/command/constant") + msg = Wrench() + + self.get_logger().info( + f'Force action (' + f'fx={params.get("force_x", 0.0):.4f}, ' + f'fy={params.get("force_y", 0.0):.4f}, ' + f'tz={params.get("torque_z", 0.0):.4f})' + ) + + # Extract duration, optional grace period, and start time + duration = params.get('duration', 0.0) + grace = params.get('grace', 0.0) + + start = self.get_clock().now().nanoseconds / 1e9 + elapsed = 0.0 + while rclpy.ok() and elapsed < duration: + elapsed = self.get_clock().now().nanoseconds / 1e9 - start + + msg.force.x = params.get('force_x', 0.0) + msg.force.y = params.get('force_y', 0.0) + msg.torque.z = params.get('torque_z', 0.0) + self.force_pub.publish(msg) + + # Feedback: log current odometry state + if self.current_odom: + pos = self.current_odom.pose.pose.position + self.get_logger().info( + f'Force feedback: pos=({pos.x:.2f}, {pos.y:.2f})' + ) + + # abort check after grace period + if elapsed >= grace and self.abort_condition(): + raise RuntimeError('Abort condition met during force action, aborting') + + rclpy.spin_once(self, timeout_sec=0.1) + + def execute_action(self, action): + """ + Execute a single action, handling any exceptions and aborts. + This is a wrapper to allow for dynamic action execution. + """ + if not isinstance(action, dict): + self.get_logger().error(f'Invalid action format: {action}') + return + + runner = action.get('action') + params = action.get('parameters', {}) + + self.set_enable(self.pose_enable_client, False, 'pose') + self.set_enable(self.vel_enable_client, False, 'velocity') + + if not callable(runner): + self.get_logger().warn(f'Unknown action: {runner}') + return + + # Resolve any callable parameters before running + resolved_params = {k: v() if callable(v) else v for k, v in params.items()} + + runner(**resolved_params) def execute_actions(self): for _ in range(self.repeat): for action in self.actions: - act = action.get('action') - params = action.get('parameters', {}) - duration = action.get('duration', 0.0) - # Disable both controllers - self.set_enable(self.pose_enable_client, False, 'pose') - self.set_enable(self.vel_enable_client, False, 'velocity') - # Execute action-specific loop - if act == 'pose': - self.get_logger().info( - f'Starting pose action for {duration}s') - self.run_pose_action(params, duration) - elif act == 'velocity': - self.get_logger().info( - f'Starting velocity action for {duration}s') - self.run_velocity_action(params, duration) - elif act == 'wait': - self.get_logger().info(f'Running wait for {duration}s') - self.run_wait_action(duration) - else: - self.get_logger().warn(f'Unknown action: {act}') + + try: + self.execute_action(action) + except RuntimeError as e: + abort_action = action.get('abort_action', self.default_abort_action) + self.get_logger().warn(f'Error executing action {action["name"]}: {e}') + self.get_logger().warn(f'Executing abort action: {abort_action["name"]}') + try: + self.execute_action(abort_action) + except RuntimeError as abort_e: + self.get_logger().error(f'Abort action failed too!: {abort_e}') + self.set_enable(self.pose_enable_client, False, 'pose') + self.set_enable(self.vel_enable_client, False, 'velocity') + self.call_mux('control/force/command') + exit(1) + def odom_callback(self, msg): # Store and log the current vehicle pose from odometry data diff --git a/cybership_viz/config/voyager.rviz b/cybership_viz/config/voyager.rviz index 035ddb9..56da955 100644 --- a/cybership_viz/config/voyager.rviz +++ b/cybership_viz/config/voyager.rviz @@ -13,6 +13,8 @@ Panels: - /Wrench4/Topic1 - /RobotModel1/Mass Properties1 - /RobotModel1/Description Topic1 + - /Marker1 + - /MarkerArray1 Splitter Ratio: 0.5 Tree Height: 555 - Class: rviz_common/Selection @@ -207,7 +209,7 @@ Visualization Manager: Scale: 1 Value: true Value: true - Enabled: false + Enabled: true Keep: 100 Name: Odometry Position Tolerance: 0.10000000149011612 @@ -228,7 +230,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: measurement/odom - Value: false + Value: true - Accept NaN Values: false Alpha: 1 Arrow Width: 0.5 @@ -303,6 +305,31 @@ Visualization Manager: Update Interval: 0 Value: true Visual Enabled: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Marker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /voyager/visualization_marker + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /voyager/visualization_marker_array + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -349,7 +376,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 3.9361820220947266 + Distance: 2.566887617111206 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -364,10 +391,10 @@ Visualization Manager: Invert Z Axis: true Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.7853981852531433 + Pitch: 0.7303981781005859 Target Frame: base_link Value: Orbit (rviz) - Yaw: 0.7853981852531433 + Yaw: 1.440398097038269 Saved: ~ Window Geometry: Displays: @@ -385,5 +412,5 @@ Window Geometry: Views: collapsed: true Width: 1200 - X: 1985 - Y: 914 + X: 1921 + Y: 850 From 5f3995be9ddb92dca752e06614617d21688bf68d Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Sat, 19 Jul 2025 13:44:58 +0200 Subject: [PATCH 13/46] fix: wait times --- cybership_tests/cybership_tests/mission.py | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/cybership_tests/cybership_tests/mission.py b/cybership_tests/cybership_tests/mission.py index 58bd049..9096061 100755 --- a/cybership_tests/cybership_tests/mission.py +++ b/cybership_tests/cybership_tests/mission.py @@ -23,7 +23,7 @@ def __init__(self): self.add_mux('control/force/command/constant') - self.repeat = 5 + self.repeat = 500 # Publisher for velocity controller input self.vel_pub = self.create_publisher( @@ -197,7 +197,7 @@ def wait_until_ready(self): """ while rclpy.ok() and self.current_odom is None: self.get_logger().info('Waiting for initial odometry data...') - rclpy.spin_once(self, timeout_sec=0.1) + rclpy.spin_once(self, timeout_sec=0.5) self.get_logger().info('Initial odometry data received.') def add_mux(self, topic: str): @@ -383,6 +383,8 @@ def run_velocity_action(self, **params): raise RuntimeError('Abort condition met during velocity action, aborting') rclpy.spin_once(self, timeout_sec=0.5) + # actual sleep to enforce loop period + time.sleep(0.5) # Deactivate velocity controller self.set_enable(self.vel_enable_client, False, 'velocity') @@ -404,7 +406,9 @@ def run_wait_action(self, **params): if elapsed >= grace and self.abort_condition(): raise RuntimeError('Abort condition met during wait action, aborting') - rclpy.spin_once(self, timeout_sec=0.1) + rclpy.spin_once(self, timeout_sec=0.5) + # actual sleep to enforce loop period + time.sleep(0.5) # Feedback: log current odometry state if self.current_odom: pos = self.current_odom.pose.pose.position @@ -419,7 +423,7 @@ def run_force_action(self, **params): msg = Wrench() self.get_logger().info( - f'Force action (' + f' Force action (' f'fx={params.get("force_x", 0.0):.4f}, ' f'fy={params.get("force_y", 0.0):.4f}, ' f'tz={params.get("torque_z", 0.0):.4f})' @@ -450,7 +454,9 @@ def run_force_action(self, **params): if elapsed >= grace and self.abort_condition(): raise RuntimeError('Abort condition met during force action, aborting') - rclpy.spin_once(self, timeout_sec=0.1) + rclpy.spin_once(self, timeout_sec=0.5) + # actual sleep to enforce loop period + time.sleep(0.5) def execute_action(self, action): """ From f88a101ac8482f83bad3b907c5dd223916713a77 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Sat, 19 Jul 2025 13:57:23 +0200 Subject: [PATCH 14/46] feat: add documentation --- cybership_tests/cybership_tests/mission.py | 241 +++++++++++++++------ 1 file changed, 175 insertions(+), 66 deletions(-) diff --git a/cybership_tests/cybership_tests/mission.py b/cybership_tests/cybership_tests/mission.py index 9096061..e062669 100755 --- a/cybership_tests/cybership_tests/mission.py +++ b/cybership_tests/cybership_tests/mission.py @@ -13,41 +13,70 @@ from nav2_msgs.action import NavigateToPose from tf_transformations import quaternion_from_euler import numpy as np +from typing import Any, Dict, Optional, List + +# Topic and service name constants +TOPIC_FORCE_CONSTANT: str = 'control/force/command/constant' +TOPIC_VELOCITY_COMMAND: str = 'control/velocity/command' +TOPIC_ODOM: str = 'measurement/odom' +TOPIC_POSE_GOAL: str = 'navigate_to_pose' +TOPIC_FORCE_POSITION: str = 'control/force/command/position' +TOPIC_FORCE_VELOCITY: str = 'control/force/command/velocity' +TOPIC_FORCE_BASE: str = 'control/force/command' + +SERVICE_MUX_ADD: str = 'force_mux/add' +SERVICE_MUX_SELECT: str = 'force_mux/select' +SERVICE_POSE_ENABLE: str = 'position_controller/change_state' +SERVICE_VEL_ENABLE: str = 'velocity_controller/change_state' class ActionRunner(Node): + """ + ActionRunner executes a predefined sequence of navigation, velocity, force, and wait actions + on a ROS2 node, monitoring odometry for abort conditions. + + Attributes: + repeat (int): Number of times to repeat the full action sequence. + current_odom (Odometry): Latest odometry message for pose feedback. + """ def __init__(self): - # Node namespace set at initialization; service and topic names are relative + """ + Initialize the ActionRunner node, including publishers, subscribers, action and service clients. + + Sets up: + - Mux for force control topics + - Publishers for velocity (Twist) and force (Wrench) commands + - Subscriber for odometry (Odometry) + - ActionClient for NavigateToPose + - Service clients for enabling/disabling pose and velocity controllers + """ super().__init__('action_runner', namespace='cybership') - # Define the mission actions - - self.add_mux('control/force/command/constant') + # Define the list of mission actions + self.add_mux(TOPIC_FORCE_CONSTANT) self.repeat = 500 - # Publisher for velocity controller input self.vel_pub = self.create_publisher( - Twist, 'control/velocity/command', 10) - # Publisher for constant force command + Twist, TOPIC_VELOCITY_COMMAND, 10) self.force_pub = self.create_publisher( - Wrench, 'control/force/command/constant', 10) + Wrench, TOPIC_FORCE_CONSTANT, 10) - # Initialize odometry storage + # Initialize storage for odometry messages self.current_odom = None - # Subscribe to odometry to monitor vehicle pose + # Subscribe to '/measurement/odom' topic for odometry feedback self.create_subscription( - Odometry, 'measurement/odom', self.odom_callback, 10) + Odometry, TOPIC_ODOM, self.odom_callback, 10) self.get_logger().info('Subscribed to measurement/odom') # Action client for pose navigation (relative to node namespace) self.pose_action_client = ActionClient( - self, NavigateToPose, 'navigate_to_pose') + self, NavigateToPose, TOPIC_POSE_GOAL) self.get_logger().info('Waiting for NavigateToPose action server...') self.pose_action_client.wait_for_server() # Enable/disable service clients (service names to be set later) - POSE_ENABLE_SERVICE = 'position_controller/change_state' - VELOCITY_ENABLE_SERVICE = 'velocity_controller/change_state' + POSE_ENABLE_SERVICE = SERVICE_POSE_ENABLE + VELOCITY_ENABLE_SERVICE = SERVICE_VEL_ENABLE if POSE_ENABLE_SERVICE: self.pose_enable_client = self.create_client( @@ -190,20 +219,26 @@ def __init__(self): } ] - def wait_until_ready(self): + def wait_until_ready(self) -> None: """ - Wait until the odometry has been read at least once. - This is useful to ensure the vehicle has a valid pose before starting actions. + Block until the first odometry message is received. + + Ensures the vehicle has a valid initial pose before performing actions. """ while rclpy.ok() and self.current_odom is None: self.get_logger().info('Waiting for initial odometry data...') rclpy.spin_once(self, timeout_sec=0.5) self.get_logger().info('Initial odometry data received.') - def add_mux(self, topic: str): - """Add a topic to the mux via MuxAdd service.""" + def add_mux(self, topic: str) -> None: + """ + Add a topic to the force multiplexer using MuxAdd service. + + Args: + topic (str): ROS topic name to add to the mux. + """ req = MuxAdd.Request() - mux_add_client = self.create_client(MuxAdd, 'force_mux/add') + mux_add_client = self.create_client(MuxAdd, SERVICE_MUX_ADD) req.topic = topic future = mux_add_client.call_async(req) rclpy.spin_until_future_complete(self, future) @@ -213,16 +248,23 @@ def add_mux(self, topic: str): else: self.get_logger().error(f'Failed to add mux topic {topic}') - def abort_condition(self): + def abort_condition(self) -> bool: """ - Check if the abort condition is met based on the current odometry. - This can be overridden in derived classes to implement specific conditions. + Determine if the mission should abort based on odometry limits. + + Returns: + bool: True if |x| or |y| position exceeds 2.5 meters, False otherwise. """ return abs(self.current_odom.pose.pose.position.x) > 2.5 or abs(self.current_odom.pose.pose.position.y) > 2.5 - def call_mux(self, topic: str): + def call_mux(self, topic: str) -> None: + """ + Select the active force control topic on the mux. - mux_client = self.create_client(MuxSelect, 'force_mux/select') + Args: + topic (str): ROS topic name to select for force commands. + """ + mux_client = self.create_client(MuxSelect, SERVICE_MUX_SELECT) mux_client.wait_for_service() self.get_logger().info(f'Calling mux select for topic {topic}') req = MuxSelect.Request() @@ -234,7 +276,15 @@ def call_mux(self, topic: str): else: self.get_logger().error(f'Failed to switch mux to {topic}') - def set_enable(self, client, enable: bool, name: str): + def set_enable(self, client: Optional[Any], enable: bool, name: str) -> None: + """ + Enable or disable a controller via SetBool service. + + Args: + client (rclpy.client.Client): Service client for SetBool. + enable (bool): True to enable, False to disable. + name (str): Human-readable name of the controller. + """ if client is None: return req = SetBool.Request() @@ -246,8 +296,13 @@ def set_enable(self, client, enable: bool, name: str): else: self.get_logger().error(f'Failed to set {name} enable={enable}') - def _pose_feedback_callback(self, feedback_msg): - """Log distance remaining from NavigateToPose feedback.""" + def _pose_feedback_callback(self, feedback_msg: Any) -> None: + """ + Callback to log remaining distance from a NavigateToPose action. + + Args: + feedback_msg: Action feedback message containing distance_remaining (float, meters). + """ fb = feedback_msg.feedback # nav2 NavigateToPose feedback has distance_remaining dist = getattr(fb, 'distance_remaining', None) @@ -256,10 +311,24 @@ def _pose_feedback_callback(self, feedback_msg): else: self.get_logger().info(' NavigateToPose feedback received') - def run_pose_action(self, **params): - """Unified pose action: send NavigateToPose goal and monitor abort (including while waiting).""" + def run_pose_action(self, **params: Any) -> None: + """ + Navigate to a target pose and monitor for timeouts or abort conditions. + + Sends a NavigateToPose goal and cancels if duration elapsed or abort condition met. + + Args: + x (float): Target x-position in meters. + y (float): Target y-position in meters. + yaw (float): Target yaw angle in radians. + duration (float): Max navigation time in seconds. + grace (float): Grace period before starting abort checks in seconds. + + Raises: + RuntimeError: If abort_condition() returns True after grace period. + """ # Activate pose controller via mux & service - self.call_mux('control/force/command/position') + self.call_mux(TOPIC_FORCE_POSITION) self.set_enable(self.pose_enable_client, True, 'pose') # Extract parameters @@ -330,10 +399,23 @@ def run_pose_action(self, **params): # Deactivate pose controller self.set_enable(self.pose_enable_client, False, 'pose') - def run_velocity_action(self, **params): - """Execute a velocity action in a loop, sending velocity commands continuously.""" + def run_velocity_action(self, **params: Any) -> None: + """ + Publish constant velocity commands for a given duration, with optional early stop. + + Args: + linear_x (float): Desired linear velocity in x (m/s). + linear_y (float): Desired linear velocity in y (m/s). + angular_z (float): Desired angular velocity around z (rad/s). + duration (float): Total time to publish commands (seconds). + margin (float, optional): Early stop threshold for actual vs commanded velocity (m/s). + grace (float): Grace period before starting abort checks (seconds). + + Raises: + RuntimeError: If abort_condition() returns True after grace period. + """ # Activate velocity controller - self.call_mux("control/force/command/velocity") + self.call_mux(TOPIC_FORCE_VELOCITY) self.set_enable(self.vel_enable_client, True, 'velocity') # Prepare and log command @@ -389,47 +471,63 @@ def run_velocity_action(self, **params): # Deactivate velocity controller self.set_enable(self.vel_enable_client, False, 'velocity') - def run_wait_action(self, **params): - """Simple wait loop, still processing callbacks.""" - # Extract duration, optional grace period, and start time + def run_wait_action(self, **params: Any) -> None: + """ + Wait for a specified duration while processing ROS callbacks. + + This loop periodically checks for an abort condition after an optional grace period + and logs odometry feedback during the wait. + + Args: + duration (float): Total wait time in seconds. + grace (float): Grace period before starting abort checks in seconds. + + Raises: + RuntimeError: If abort_condition() returns True after grace period. + """ duration = params.get('duration', 0.0) grace = params.get('grace', 0.0) start = self.get_clock().now().nanoseconds / 1e9 self.get_logger().info(f'Waiting for {duration:.4f} seconds') - elapsed = 0.0 while rclpy.ok() and elapsed < duration: - # abort check after grace period elapsed = self.get_clock().now().nanoseconds / 1e9 - start if elapsed >= grace and self.abort_condition(): - raise RuntimeError('Abort condition met during wait action, aborting') + raise RuntimeError( + 'Abort condition met during wait action, aborting') rclpy.spin_once(self, timeout_sec=0.5) - # actual sleep to enforce loop period time.sleep(0.5) - # Feedback: log current odometry state + if self.current_odom: pos = self.current_odom.pose.pose.position self.get_logger().info( - f'Wait feedback: pos=({pos.x:.2f}, {pos.y:.2f}), elapsed={elapsed:.2f}s' + f'Wait feedback: pos=({pos.x:.2f}, {pos.y:.2f}), ' + f'elapsed={elapsed:.2f}s' ) - def run_force_action(self, **params): - """Execute a constant force action in a loop.""" - # Activate force topic on mux - self.call_mux("control/force/command/constant") - msg = Wrench() + def run_force_action(self, **params: Any) -> None: + """ + Apply a constant force to the vehicle for a specified duration. - self.get_logger().info( - f' Force action (' - f'fx={params.get("force_x", 0.0):.4f}, ' - f'fy={params.get("force_y", 0.0):.4f}, ' - f'tz={params.get("torque_z", 0.0):.4f})' - ) + This loop publishes a Wrench message at fixed intervals and checks for abort + conditions after an optional grace period. Odometry feedback is logged each cycle. + + Args: + force_x (float): Force in x-direction in Newtons. + force_y (float): Force in y-direction in Newtons. + torque_z (float): Torque about z-axis in Newton-meters. + duration (float): Total time to apply force in seconds. + grace (float): Grace period before starting abort checks in seconds. + + Raises: + RuntimeError: If abort_condition() returns True after grace period. + """ + self.call_mux(TOPIC_FORCE_CONSTANT) + msg = Wrench() - # Extract duration, optional grace period, and start time duration = params.get('duration', 0.0) grace = params.get('grace', 0.0) @@ -443,25 +541,27 @@ def run_force_action(self, **params): msg.torque.z = params.get('torque_z', 0.0) self.force_pub.publish(msg) - # Feedback: log current odometry state if self.current_odom: pos = self.current_odom.pose.pose.position self.get_logger().info( f'Force feedback: pos=({pos.x:.2f}, {pos.y:.2f})' ) - # abort check after grace period if elapsed >= grace and self.abort_condition(): - raise RuntimeError('Abort condition met during force action, aborting') + raise RuntimeError( + 'Abort condition met during force action, aborting') rclpy.spin_once(self, timeout_sec=0.5) - # actual sleep to enforce loop period time.sleep(0.5) - def execute_action(self, action): + def execute_action(self, action: Dict[str, Any]) -> None: """ - Execute a single action, handling any exceptions and aborts. - This is a wrapper to allow for dynamic action execution. + Execute one mission action and resolve its parameters. + + Normalizes callable parameters, disables other controllers, and invokes the action. + + Args: + action (dict): Action descriptor with keys 'action' (callable) and 'parameters' (dict). """ if not isinstance(action, dict): self.get_logger().error(f'Invalid action format: {action}') @@ -482,8 +582,12 @@ def execute_action(self, action): runner(**resolved_params) - def execute_actions(self): + def execute_actions(self) -> None: + """ + Loop through the action sequence `repeat` times, handling aborts. + For each action, calls execute_action and on RuntimeError runs an abort_action. + """ for _ in range(self.repeat): for action in self.actions: @@ -500,16 +604,21 @@ def execute_actions(self): self.get_logger().error(f'Abort action failed too!: {abort_e}') self.set_enable(self.pose_enable_client, False, 'pose') self.set_enable(self.vel_enable_client, False, 'velocity') - self.call_mux('control/force/command') + self.call_mux(TOPIC_FORCE_BASE) exit(1) def odom_callback(self, msg): - # Store and log the current vehicle pose from odometry data + """ + Callback for Odometry subscription; stores the latest message. + + Args: + msg (Odometry): Received odometry message containing pose and twist. + """ self.current_odom = msg -def main(args=None): +def main(args: Optional[List[str]] = None) -> None: rclpy.init(args=args) node = ActionRunner() node.execute_actions() From 993c4876ffd06e6f592c5a5a1758704d64cc0578 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Mon, 21 Jul 2025 10:34:12 +0200 Subject: [PATCH 15/46] feat: add experiment reporting --- cybership_tests/cybership_tests/mission.py | 516 ++++++++++++--------- 1 file changed, 296 insertions(+), 220 deletions(-) diff --git a/cybership_tests/cybership_tests/mission.py b/cybership_tests/cybership_tests/mission.py index e062669..1d7d0fd 100755 --- a/cybership_tests/cybership_tests/mission.py +++ b/cybership_tests/cybership_tests/mission.py @@ -2,32 +2,38 @@ import rclpy +import rclpy.client from rclpy.node import Node from std_srvs.srv import SetBool from topic_tools_interfaces.srv import MuxSelect, MuxAdd from geometry_msgs.msg import PoseStamped, Twist, Wrench +from std_msgs.msg import Bool from nav_msgs.msg import Odometry -import math -import time from rclpy.action import ActionClient from nav2_msgs.action import NavigateToPose from tf_transformations import quaternion_from_euler import numpy as np from typing import Any, Dict, Optional, List +from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor + # Topic and service name constants -TOPIC_FORCE_CONSTANT: str = 'control/force/command/constant' -TOPIC_VELOCITY_COMMAND: str = 'control/velocity/command' -TOPIC_ODOM: str = 'measurement/odom' -TOPIC_POSE_GOAL: str = 'navigate_to_pose' -TOPIC_FORCE_POSITION: str = 'control/force/command/position' -TOPIC_FORCE_VELOCITY: str = 'control/force/command/velocity' -TOPIC_FORCE_BASE: str = 'control/force/command' +TOPIC_FORCE_CONSTANT: str = "control/force/command/constant" +TOPIC_VELOCITY_COMMAND: str = "control/velocity/command" +TOPIC_ODOM: str = "measurement/odom" +TOPIC_POSE_GOAL: str = "navigate_to_pose" +TOPIC_FORCE_POSITION: str = "control/force/command/position" +TOPIC_FORCE_VELOCITY: str = "control/force/command/velocity" +TOPIC_FORCE_BASE: str = "control/force/command" + +SERVICE_MUX_ADD: str = "force_mux/add" +SERVICE_MUX_SELECT: str = "force_mux/select" +SERVICE_POSE_ENABLE: str = "position_controller/change_state" +SERVICE_VEL_ENABLE: str = "velocity_controller/change_state" -SERVICE_MUX_ADD: str = 'force_mux/add' -SERVICE_MUX_SELECT: str = 'force_mux/select' -SERVICE_POSE_ENABLE: str = 'position_controller/change_state' -SERVICE_VEL_ENABLE: str = 'velocity_controller/change_state' +TOPIC_EXPERIMENT_ODOM: str = "experiment/odom" +TOPIC_EXPERIMENT_FORCE: str = "experiment/force" +TOPIC_EXPERIMENT_FLAG: str = "experiment/flag" class ActionRunner(Node): @@ -39,6 +45,7 @@ class ActionRunner(Node): repeat (int): Number of times to repeat the full action sequence. current_odom (Odometry): Latest odometry message for pose feedback. """ + def __init__(self): """ Initialize the ActionRunner node, including publishers, subscribers, action and service clients. @@ -50,173 +57,186 @@ def __init__(self): - ActionClient for NavigateToPose - Service clients for enabling/disabling pose and velocity controllers """ - super().__init__('action_runner', namespace='cybership') + super().__init__("action_runner", namespace="voyager") + # Private executor for service calls + # self._executor.add_node(self) # Define the list of mission actions self.add_mux(TOPIC_FORCE_CONSTANT) self.repeat = 500 - self.vel_pub = self.create_publisher( - Twist, TOPIC_VELOCITY_COMMAND, 10) - self.force_pub = self.create_publisher( - Wrench, TOPIC_FORCE_CONSTANT, 10) + self.vel_pub = self.create_publisher(Twist, TOPIC_VELOCITY_COMMAND, 10) + self.force_pub = self.create_publisher(Wrench, TOPIC_FORCE_CONSTANT, 10) + + self.experiment_odom_pub = self.create_publisher( + Odometry, TOPIC_EXPERIMENT_ODOM, 10 + ) + self.experiment_force_pub = self.create_publisher( + Wrench, TOPIC_EXPERIMENT_FORCE, 10 + ) + self.experiment_flag_pub = self.create_publisher( + Bool, TOPIC_EXPERIMENT_FLAG, 10 + ) - # Initialize storage for odometry messages self.current_odom = None - # Subscribe to '/measurement/odom' topic for odometry feedback - self.create_subscription( - Odometry, TOPIC_ODOM, self.odom_callback, 10) - self.get_logger().info('Subscribed to measurement/odom') + self.create_subscription(Odometry, TOPIC_ODOM, self.odom_callback, 10) # Action client for pose navigation (relative to node namespace) - self.pose_action_client = ActionClient( - self, NavigateToPose, TOPIC_POSE_GOAL) - self.get_logger().info('Waiting for NavigateToPose action server...') + self.pose_action_client = ActionClient(self, NavigateToPose, TOPIC_POSE_GOAL) + self.get_logger().info(f"Waiting for {self.pose_action_client._action_name} action server...") self.pose_action_client.wait_for_server() - # Enable/disable service clients (service names to be set later) - POSE_ENABLE_SERVICE = SERVICE_POSE_ENABLE - VELOCITY_ENABLE_SERVICE = SERVICE_VEL_ENABLE + self.pose_enable_client = self.create_client(SetBool, SERVICE_POSE_ENABLE) + self.get_logger().info(f"Waiting for server {self.pose_enable_client.service_name}...") + self.pose_enable_client.wait_for_service() - if POSE_ENABLE_SERVICE: - self.pose_enable_client = self.create_client( - SetBool, POSE_ENABLE_SERVICE) - self.pose_enable_client.wait_for_service() - else: - self.pose_enable_client = None - - if VELOCITY_ENABLE_SERVICE: - self.vel_enable_client = self.create_client( - SetBool, VELOCITY_ENABLE_SERVICE) - self.vel_enable_client.wait_for_service() - else: - self.vel_enable_client = None + self.vel_enable_client = self.create_client(SetBool, SERVICE_VEL_ENABLE) + self.get_logger().info(f"Waiting for server {self.vel_enable_client.service_name}...") + self.vel_enable_client.wait_for_service() self.rng = np.random.default_rng() self.wait_until_ready() + # Experiment publishing state + self.experiment_enabled: bool = False + self._last_force: Optional[Wrench] = None + self.create_timer(0.1, self._experiment_publisher) + + # Default abort lands back at origin after timeout self.default_abort_action = { - 'name': 'default_abort', - 'action': self.run_pose_action, - 'parameters': {'x': 0.0, 'y': 0.0, 'yaw': 0.0, 'duration': 60.0, 'grace': 15.0}, + "name": "default_abort", + "action": self.run_pose_action, + "parameters": { + "x": 0.0, + "y": 0.0, + "yaw": 0.0, + "duration": 60.0, + "grace": 15.0, + }, } self.actions = [ { - 'name': 'pose_neg_diag', - 'action': self.run_pose_action, - 'parameters': { - 'x': -2.0, - 'y': -2.0, - 'yaw': np.pi / 4, - 'duration': 60.0 + "name": "pose_neg_diag", + "action": self.run_pose_action, + "parameters": { + "x": -2.0, + "y": -2.0, + "yaw": np.pi / 4, + "duration": 60.0, }, }, { - 'name': 'velocity_random_1', - 'action': self.run_velocity_action, - 'parameters': { + "name": "velocity_random_1", + "action": self.run_velocity_action, + "parameters": { # Sample linear and angular components each publish - 'linear_x': lambda: self.rng.uniform(0.2, 0.4), - 'linear_y': lambda: self.rng.uniform(-0.1, 0.1), - 'angular_z': lambda: self.rng.uniform(-0.1, 0.1), - 'duration': 30.0, - 'margin': 0.1 # Allow some margin for early success + "linear_x": lambda: self.rng.uniform(0.4, 0.8), + "linear_y": lambda: self.rng.uniform(-0.1, 0.1), + "angular_z": lambda: self.rng.uniform(-0.01, 0.01), + "duration": 30.0, + "margin": 0.1, # Allow some margin for early success + }, + "abort_action": { + "name": "velocity_random_1_abort", + "action": self.run_pose_action, + "parameters": { + "x": 2.0, + "y": 2.0, + "yaw": 5 * np.pi / 4, + "duration": 60.0, + "grace": 15.0, + }, }, - 'abort_action': { - 'name': 'velocity_random_1_abort', - 'action': self.run_pose_action, - 'parameters': { - 'x': 2.0, - 'y': 2.0, - 'yaw': 5 * np.pi / 4, - 'duration': 60.0, - 'grace': 15.0 - } - } }, { - 'name': 'force_random_1', - 'action': self.run_force_action, - 'parameters': { - 'force_x': lambda: self.rng.uniform(0.2, 0.8), - 'force_y': lambda: self.rng.uniform(-0.5, 0.5), - 'torque_z': lambda: self.rng.uniform(-0.1, 0.1), - 'duration': 30.0, + "name": "force_random_1", + "action": self.run_force_action, + "parameters": { + "force_x": lambda: self.rng.uniform(0.2, 0.8), + "force_y": lambda: self.rng.uniform(-0.5, 0.5), + "torque_z": lambda: self.rng.uniform(-0.1, 0.1), + "duration": 30.0, + "experiment": True, # Enable experiment flag + "continuous_sampling": True, # Sample force continuously + "frequency": 1.0, # Hz + }, + "abort_action": { + "name": "force_random_1_abort", + "action": self.run_pose_action, + "parameters": { + "x": 2.0, + "y": 2.0, + "yaw": 5 * np.pi / 4, + "duration": 60.0, + "grace": 15.0, + }, }, - 'abort_action': { - 'name': 'force_random_1_abort', - 'action': self.run_pose_action, - 'parameters': { - 'x': 2.0, - 'y': 2.0, - 'yaw': 5 * np.pi / 4, - 'duration': 60.0, - 'grace': 15.0 - } - } }, { - 'name': 'pose_pos_diag', - 'action': self.run_pose_action, - 'parameters': { - 'x': 2.0, - 'y': 2.0, - 'yaw': 5 * np.pi / 4, - 'duration': 60.0 + "name": "pose_pos_diag", + "action": self.run_pose_action, + "parameters": { + "x": 2.0, + "y": 2.0, + "yaw": 5 * np.pi / 4, + "duration": 60.0, }, }, { - 'name': 'velocity_random_2', - 'action': self.run_velocity_action, - 'parameters': { + "name": "velocity_random_2", + "action": self.run_velocity_action, + "parameters": { # Sample linear and angular components each publish - 'linear_x': lambda: self.rng.uniform(0.2, 0.4), - 'linear_y': lambda: self.rng.uniform(-0.1, 0.1), - 'angular_z': lambda: self.rng.uniform(-0.1, 0.1), - 'duration': 30.0, - 'margin': 0.1 # Allow some margin for early success + "linear_x": lambda: self.rng.uniform(0.2, 0.8), + "linear_y": lambda: self.rng.uniform(-0.1, 0.1), + "angular_z": lambda: self.rng.uniform(-0.01, 0.01), + "duration": 30.0, + "margin": 0.1, # Allow some margin for early success + }, + "abort_action": { + "name": "velocity_random_2_abort", + "action": self.run_pose_action, + "parameters": { + "x": -2.0, + "y": -2.0, + "yaw": np.pi / 4, + "duration": 60.0, + "grace": 15.0, + }, }, - 'abort_action': { - 'name': 'velocity_random_2_abort', - 'action': self.run_pose_action, - 'parameters': { - 'x': -2.0, - 'y': -2.0, - 'yaw': np.pi / 4, - 'duration': 60.0, - 'grace': 15.0, - } - } }, { - 'name': 'force_random_2', - 'action': self.run_force_action, - 'parameters': { - 'force_x': lambda: self.rng.uniform(0.2, 0.8), - 'force_y': lambda: self.rng.uniform(-0.5, 0.5), - 'torque_z': lambda: self.rng.uniform(-0.1, 0.1), - 'duration': 30.0, + "name": "force_random_2", + "action": self.run_force_action, + "parameters": { + "force_x": lambda: self.rng.uniform(0.2, 0.8), + "force_y": lambda: self.rng.uniform(-0.5, 0.5), + "torque_z": lambda: self.rng.uniform(-0.1, 0.1), + "duration": 30.0, + "experiment": True, # Enable experiment flag + "continuous_sampling": True, # Sample force continuously + "frequency": 1.0, # Hz + }, + "abort_action": { + "name": "force_random_2_abort", + "action": self.run_pose_action, + "parameters": { + "x": -2.0, + "y": -2.0, + "yaw": np.pi / 4, + "duration": 60.0, + "grace": 15.0, + }, }, - 'abort_action': { - 'name': 'force_random_2_abort', - 'action': self.run_pose_action, - 'parameters': { - 'x': -2.0, - 'y': -2.0, - 'yaw': np.pi / 4, - 'duration': 60.0, - 'grace': 15.0 - } - } }, { - 'name': 'wait', - 'action': self.run_wait_action, - 'parameters': {'duration': 10.0} - } + "name": "wait", + "action": self.run_wait_action, + "parameters": {"duration": 10.0}, + }, ] def wait_until_ready(self) -> None: @@ -226,9 +246,24 @@ def wait_until_ready(self) -> None: Ensures the vehicle has a valid initial pose before performing actions. """ while rclpy.ok() and self.current_odom is None: - self.get_logger().info('Waiting for initial odometry data...') - rclpy.spin_once(self, timeout_sec=0.5) - self.get_logger().info('Initial odometry data received.') + self.get_logger().info("Waiting for initial odometry data...") + # process callbacks without blocking executor + rclpy.spin_once(self, timeout_sec=0.1) + self.get_logger().info("Initial odometry data received.") + + def _experiment_publisher(self) -> None: + """ + Background thread publishes experiment flag, and when enabled, odometry and force data. + """ + flag_msg = Bool() + flag_msg.data = self.experiment_enabled + self.experiment_flag_pub.publish(flag_msg) + if self.experiment_enabled: + if self.current_odom: + self.experiment_odom_pub.publish(self.current_odom) + if self._last_force: + self.experiment_force_pub.publish(self._last_force) + def add_mux(self, topic: str) -> None: """ @@ -243,10 +278,10 @@ def add_mux(self, topic: str) -> None: future = mux_add_client.call_async(req) rclpy.spin_until_future_complete(self, future) # MuxAdd returns a result with a success flag - if hasattr(future.result(), 'success') and future.result().success: - self.get_logger().info(f'Mux added topic {topic}') + if future.result().success: + self.get_logger().info(f"Mux added topic {topic}") else: - self.get_logger().error(f'Failed to add mux topic {topic}') + self.get_logger().error(f"Failed to add mux topic {topic}") def abort_condition(self) -> bool: """ @@ -255,7 +290,10 @@ def abort_condition(self) -> bool: Returns: bool: True if |x| or |y| position exceeds 2.5 meters, False otherwise. """ - return abs(self.current_odom.pose.pose.position.x) > 2.5 or abs(self.current_odom.pose.pose.position.y) > 2.5 + return ( + abs(self.current_odom.pose.pose.position.x) > 2.5 + or abs(self.current_odom.pose.pose.position.y) > 2.5 + ) def call_mux(self, topic: str) -> None: """ @@ -272,29 +310,27 @@ def call_mux(self, topic: str) -> None: future = mux_client.call_async(req) rclpy.spin_until_future_complete(self, future) if future.result(): - self.get_logger().info(f'Mux switched to {topic}') + self.get_logger().info(f"Mux switched to {topic}") else: - self.get_logger().error(f'Failed to switch mux to {topic}') + self.get_logger().error(f"Failed to switch mux to {topic}") - def set_enable(self, client: Optional[Any], enable: bool, name: str) -> None: + def set_enable(self, client: rclpy.client.Client, enable: bool) -> None: """ Enable or disable a controller via SetBool service. Args: client (rclpy.client.Client): Service client for SetBool. enable (bool): True to enable, False to disable. - name (str): Human-readable name of the controller. """ - if client is None: - return + self.get_logger().info(f"Setting {client.service_name} enable={enable}") req = SetBool.Request() req.data = enable future = client.call_async(req) rclpy.spin_until_future_complete(self, future) if future.result().success: - self.get_logger().info(f'Service {name} set enable={enable}') + self.get_logger().info(f'Service {client.service_name} set enable={enable}') else: - self.get_logger().error(f'Failed to set {name} enable={enable}') + self.get_logger().error(f'Failed to set {client.service_name} enable={enable}') def _pose_feedback_callback(self, feedback_msg: Any) -> None: """ @@ -305,11 +341,13 @@ def _pose_feedback_callback(self, feedback_msg: Any) -> None: """ fb = feedback_msg.feedback # nav2 NavigateToPose feedback has distance_remaining - dist = getattr(fb, 'distance_remaining', None) + dist = getattr(fb, "distance_remaining", None) if dist is not None: - self.get_logger().info(f' NavigateToPose feedback: distance_remaining={dist:.2f}') + self.get_logger().info( + f" NavigateToPose feedback: distance_remaining={dist:.2f}" + ) else: - self.get_logger().info(' NavigateToPose feedback received') + self.get_logger().info(" NavigateToPose feedback received") def run_pose_action(self, **params: Any) -> None: """ @@ -327,25 +365,27 @@ def run_pose_action(self, **params: Any) -> None: Raises: RuntimeError: If abort_condition() returns True after grace period. """ + resolved_params = {k: v() if callable(v) else v for k, v in params.items()} # Activate pose controller via mux & service self.call_mux(TOPIC_FORCE_POSITION) - self.set_enable(self.pose_enable_client, True, 'pose') + + self.set_enable(self.pose_enable_client, True) # Extract parameters - x = params.get('x', 0.0) - y = params.get('y', 0.0) - yaw = params.get('yaw', 0.0) - duration = params.get('duration', 0.0) - grace = params.get('grace', 0.0) + x = resolved_params.get("x", 0.0) + y = resolved_params.get("y", 0.0) + yaw = resolved_params.get("yaw", 0.0) + duration = resolved_params.get("duration", 0.0) + grace = resolved_params.get("grace", 0.0) self.get_logger().info( - f'Pose action start (x={x:.2f}, y={y:.2f}, yaw={yaw:.2f}), ' - f'duration={duration:.1f}s, grace={grace:.1f}s' + f"Pose action start (x={x:.2f}, y={y:.2f}, yaw={yaw:.2f}), " + f"duration={duration:.1f}s, grace={grace:.1f}s" ) # Build goal message msg = PoseStamped() msg.header.stamp = self.get_clock().now().to_msg() - msg.header.frame_id = 'world' + msg.header.frame_id = "world" msg.pose.position.x = x msg.pose.position.y = y q = quaternion_from_euler(0.0, 0.0, yaw) @@ -363,11 +403,11 @@ def run_pose_action(self, **params: Any) -> None: rclpy.spin_until_future_complete(self, goal_fut) handle = goal_fut.result() if not handle.accepted: - self.get_logger().error('NavigateToPose goal rejected') - self.set_enable(self.pose_enable_client, False, 'pose') + self.get_logger().error("NavigateToPose goal rejected") + self.set_enable(self.pose_enable_client, False) return - self.get_logger().info('NavigateToPose goal accepted, monitoring execution') + self.get_logger().info("NavigateToPose goal accepted, monitoring execution") # Wait for result, checking abort & timeout result_fut = handle.get_result_async() @@ -377,27 +417,29 @@ def run_pose_action(self, **params: Any) -> None: elapsed = (self.get_clock().now().nanoseconds / 1e9) - start if elapsed >= duration: - self.get_logger().info('NavigateToPose action completed due to duration') + self.get_logger().info( + "NavigateToPose action completed due to duration" + ) cancel_fut = handle.cancel_goal_async() rclpy.spin_until_future_complete(self, cancel_fut) - self.set_enable(self.pose_enable_client, False, 'pose') + self.set_enable(self.pose_enable_client, False) break if result_fut.done(): - self.get_logger().info('NavigateToPose action completed') + self.get_logger().info("NavigateToPose action completed") break if elapsed >= grace and self.abort_condition(): cancel_fut = handle.cancel_goal_async() rclpy.spin_until_future_complete(self, cancel_fut) - self.set_enable(self.pose_enable_client, False, 'pose') + self.set_enable(self.pose_enable_client, False) raise RuntimeError('Abort condition met during pose action, aborting') rclpy.spin_once(self, timeout_sec=0.5) # Deactivate pose controller - self.set_enable(self.pose_enable_client, False, 'pose') + self.set_enable(self.pose_enable_client, False) def run_velocity_action(self, **params: Any) -> None: """ @@ -414,25 +456,30 @@ def run_velocity_action(self, **params: Any) -> None: Raises: RuntimeError: If abort_condition() returns True after grace period. """ + resolved_params = {k: v() if callable(v) else v for k, v in params.items()} # Activate velocity controller self.call_mux(TOPIC_FORCE_VELOCITY) - self.set_enable(self.vel_enable_client, True, 'velocity') + self.set_enable(self.vel_enable_client, True) # Prepare and log command msg = Twist() - lx = params.get('linear_x', 0.0) - ly = params.get('linear_y', 0.0) - az = params.get('angular_z', 0.0) - margin = params.get('margin', None) - self.get_logger().info(' ' - f'Velocity action (u={lx:.4f}, v={ly:.4f}, r={az:.4f})' - + (f', margin={margin:.4f}' if margin is not None else '')) + lx = resolved_params.get("linear_x", 0.0) + ly = resolved_params.get("linear_y", 0.0) + az = resolved_params.get("angular_z", 0.0) + margin = resolved_params.get("margin", None) + self.get_logger().info( + " " + f"Velocity action (u={lx:.4f}, v={ly:.4f}, r={az:.4f})" + + (f", margin={margin:.4f}" if margin is not None else "") + ) # Extract duration, grace period and start time - duration = params.get('duration', 0.0) - grace = params.get('grace', 0.0) + duration = resolved_params.get("duration", 0.0) + grace = resolved_params.get("grace", 0.0) start = self.get_clock().now().nanoseconds / 1e9 elapsed = 0.0 + # Loop at ~10 Hz, processing callbacks without blocking + period = 0.1 while rclpy.ok() and elapsed < duration: elapsed = self.get_clock().now().nanoseconds / 1e9 - start @@ -445,31 +492,33 @@ def run_velocity_action(self, **params: Any) -> None: # Feedback: log current odometry state if self.current_odom: pos = self.current_odom.pose.pose.position - vel = self.current_odom.twist.twist.linear + lvel = self.current_odom.twist.twist.linear + avel = self.current_odom.twist.twist.angular self.get_logger().info( - ' ' - f'Velocity feedback: pos=({pos.x:.2f}, {pos.y:.2f}), ' - f'vel=({vel.x:.2f}, {vel.y:.2f})' + " " + f"Velocity feedback: " + f"vel=(vx={lvel.x:.2f}, vy={lvel.y:.2f}, vr={avel.z:.2f}), " + f"cmd=(u={lx:.2f}, v={ly:.2f}, r={az:.2f})" ) # margin-based early success if margin is not None: act = self.current_odom.twist.twist - if (abs(act.linear.x - lx) < margin and - abs(act.linear.y - ly) < margin): - self.get_logger().info('Velocity within margin, stopping early') + if abs(act.linear.x - lx) < margin and abs(act.linear.y - ly) < margin: + self.get_logger().info("Velocity within margin, stopping early") break # Abort check after grace period if self.current_odom and elapsed >= grace and self.abort_condition(): - raise RuntimeError('Abort condition met during velocity action, aborting') + raise RuntimeError( + "Abort condition met during velocity action, aborting" + ) - rclpy.spin_once(self, timeout_sec=0.5) - # actual sleep to enforce loop period - time.sleep(0.5) + # enforce loop rate and allow callbacks + rclpy.spin_once(self, timeout_sec=period) # Deactivate velocity controller - self.set_enable(self.vel_enable_client, False, 'velocity') + self.set_enable(self.vel_enable_client, False) def run_wait_action(self, **params: Any) -> None: """ @@ -485,27 +534,29 @@ def run_wait_action(self, **params: Any) -> None: Raises: RuntimeError: If abort_condition() returns True after grace period. """ - duration = params.get('duration', 0.0) - grace = params.get('grace', 0.0) + resolved_params = {k: v() if callable(v) else v for k, v in params.items()} + duration = resolved_params.get("duration", 0.0) + grace = resolved_params.get("grace", 0.0) start = self.get_clock().now().nanoseconds / 1e9 - self.get_logger().info(f'Waiting for {duration:.4f} seconds') + self.get_logger().info(f"Waiting for {duration:.4f} seconds") elapsed = 0.0 + # Loop at ~2 Hz, processing callbacks without blocking + period = 0.5 while rclpy.ok() and elapsed < duration: elapsed = self.get_clock().now().nanoseconds / 1e9 - start if elapsed >= grace and self.abort_condition(): - raise RuntimeError( - 'Abort condition met during wait action, aborting') + raise RuntimeError("Abort condition met during wait action, aborting") - rclpy.spin_once(self, timeout_sec=0.5) - time.sleep(0.5) + # allow callbacks and enforce loop timing + rclpy.spin_once(self, timeout_sec=period) if self.current_odom: pos = self.current_odom.pose.pose.position self.get_logger().info( - f'Wait feedback: pos=({pos.x:.2f}, {pos.y:.2f}), ' - f'elapsed={elapsed:.2f}s' + f"Wait feedback: pos=({pos.x:.2f}, {pos.y:.2f}), " + f"elapsed={elapsed:.2f}s" ) def run_force_action(self, **params: Any) -> None: @@ -525,34 +576,56 @@ def run_force_action(self, **params: Any) -> None: Raises: RuntimeError: If abort_condition() returns True after grace period. """ + + resolved_params = {k: v() if callable(v) else v for k, v in params.items()} + self.call_mux(TOPIC_FORCE_CONSTANT) + msg = Wrench() - duration = params.get('duration', 0.0) - grace = params.get('grace', 0.0) + duration = params.get("duration", 0.0) + grace = params.get("grace", 0.0) + + continuous_sampling = params.get("continuous_sampling", False) + frequency = params.get("frequency", 1.0) # Hz start = self.get_clock().now().nanoseconds / 1e9 elapsed = 0.0 + + # Loop at specified frequency (or 2 Hz), processing callbacks without blocking + period = 1.0 / (frequency if continuous_sampling else 2.0) + while rclpy.ok() and elapsed < duration: elapsed = self.get_clock().now().nanoseconds / 1e9 - start - msg.force.x = params.get('force_x', 0.0) - msg.force.y = params.get('force_y', 0.0) - msg.torque.z = params.get('torque_z', 0.0) + msg.force.x = resolved_params.get("force_x", 0.0) + msg.force.y = resolved_params.get("force_y", 0.0) + msg.torque.z = resolved_params.get("torque_z", 0.0) + + if continuous_sampling: + # Sample force parameters at each iteration + msg.force.x = params.get("force_x", 0.0)() + msg.force.y = params.get("force_y", 0.0)() + msg.torque.z = params.get("torque_z", 0.0)() + + # record last force for experiment publishing + self._last_force = msg self.force_pub.publish(msg) if self.current_odom: pos = self.current_odom.pose.pose.position self.get_logger().info( - f'Force feedback: pos=({pos.x:.2f}, {pos.y:.2f})' + f"Force feedback: (fx={msg.force.x:.2f}, " + f"fy={msg.force.y:.2f}, tz={msg.torque.z:.2f})," + f" pos=({pos.x:.2f}, {pos.y:.2f}), " + f"elapsed={elapsed:.2f}s" ) if elapsed >= grace and self.abort_condition(): - raise RuntimeError( - 'Abort condition met during force action, aborting') + raise RuntimeError("Abort condition met during force action, aborting") - rclpy.spin_once(self, timeout_sec=0.5) - time.sleep(0.5) + # allow callbacks and enforce publish rate + rclpy.spin_once(self) def execute_action(self, action: Dict[str, Any]) -> None: """ @@ -570,17 +643,21 @@ def execute_action(self, action: Dict[str, Any]) -> None: runner = action.get('action') params = action.get('parameters', {}) - self.set_enable(self.pose_enable_client, False, 'pose') - self.set_enable(self.vel_enable_client, False, 'velocity') + self.set_enable(self.pose_enable_client, False) + self.set_enable(self.vel_enable_client, False) if not callable(runner): self.get_logger().warn(f'Unknown action: {runner}') return - # Resolve any callable parameters before running - resolved_params = {k: v() if callable(v) else v for k, v in params.items()} + self.get_logger().info(f"Executing action: {action['name']}") + + # Handle experiment flag without resolving other parameters + self.experiment_enabled = bool(params.get("experiment", False)) + runner(**params) - runner(**resolved_params) + # Disable experiment after action completes + self.experiment_enabled = False def execute_actions(self) -> None: """ @@ -626,6 +703,5 @@ def main(args: Optional[List[str]] = None) -> None: node.destroy_node() rclpy.shutdown() - -if __name__ == '__main__': +if __name__ == "__main__": main() From 5759df5d4749620423aa982a0b2b29c49f3aaa52 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Mon, 21 Jul 2025 11:55:31 +0200 Subject: [PATCH 16/46] fix: bugs hanging the service calls --- cybership_tests/cybership_tests/mission.py | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/cybership_tests/cybership_tests/mission.py b/cybership_tests/cybership_tests/mission.py index 1d7d0fd..cb78477 100755 --- a/cybership_tests/cybership_tests/mission.py +++ b/cybership_tests/cybership_tests/mission.py @@ -15,6 +15,8 @@ import numpy as np from typing import Any, Dict, Optional, List from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor +from abc import ABC, abstractmethod +import time # Topic and service name constants @@ -245,10 +247,12 @@ def wait_until_ready(self) -> None: Ensures the vehicle has a valid initial pose before performing actions. """ + while rclpy.ok() and self.current_odom is None: self.get_logger().info("Waiting for initial odometry data...") # process callbacks without blocking executor rclpy.spin_once(self, timeout_sec=0.1) + time.sleep(0.5) self.get_logger().info("Initial odometry data received.") def _experiment_publisher(self) -> None: @@ -481,6 +485,7 @@ def run_velocity_action(self, **params: Any) -> None: # Loop at ~10 Hz, processing callbacks without blocking period = 0.1 + while rclpy.ok() and elapsed < duration: elapsed = self.get_clock().now().nanoseconds / 1e9 - start # Publish the commanded velocity @@ -513,8 +518,7 @@ def run_velocity_action(self, **params: Any) -> None: raise RuntimeError( "Abort condition met during velocity action, aborting" ) - - # enforce loop rate and allow callbacks + time.sleep(0.1) rclpy.spin_once(self, timeout_sec=period) # Deactivate velocity controller @@ -624,6 +628,7 @@ def run_force_action(self, **params: Any) -> None: if elapsed >= grace and self.abort_condition(): raise RuntimeError("Abort condition met during force action, aborting") + time.sleep(period) # Sleep for the period to control frequency # allow callbacks and enforce publish rate rclpy.spin_once(self) @@ -698,10 +703,13 @@ def odom_callback(self, msg): def main(args: Optional[List[str]] = None) -> None: rclpy.init(args=args) node = ActionRunner() + + executor = MultiThreadedExecutor() + executor.add_node(node) + node.execute_actions() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() + executor.spin() + executor.shutdown() if __name__ == "__main__": main() From 6066c0975418162dcc5abe4dc18eb9b5364fd2bf Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Fri, 1 Aug 2025 11:14:02 +0200 Subject: [PATCH 17/46] fix: localization issues --- cybership_config/config/voyager/mocap_transformer.yaml | 2 +- cybership_config/config/voyager/robot_localization.yaml | 3 +-- cybership_description/urdf/model/voyager/base.urdf.xacro | 5 +++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/cybership_config/config/voyager/mocap_transformer.yaml b/cybership_config/config/voyager/mocap_transformer.yaml index 9f3196c..0990b5e 100644 --- a/cybership_config/config/voyager/mocap_transformer.yaml +++ b/cybership_config/config/voyager/mocap_transformer.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - world_frame: world_ned + world_frame: world base_frame: mocap_link rigid_body_name: "0" topic_name: measurement/pose diff --git a/cybership_config/config/voyager/robot_localization.yaml b/cybership_config/config/voyager/robot_localization.yaml index 24c819d..cea5b65 100644 --- a/cybership_config/config/voyager/robot_localization.yaml +++ b/cybership_config/config/voyager/robot_localization.yaml @@ -1,4 +1,3 @@ -### ekf config file ### /**: ros__parameters: # The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin @@ -68,7 +67,7 @@ # estimation node from robot_localization! However, that instance should *not* fuse the global data. map_frame: map # Defaults to "map" if unspecified odom_frame: world # Defaults to "odom" if unspecified - base_link_frame: base_link # Defaults to "base_link" if unspecified + base_link_frame: base_link_ned # Defaults to "base_link" if unspecified world_frame: world # Defaults to the value of odom_frame if unspecified # The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry, diff --git a/cybership_description/urdf/model/voyager/base.urdf.xacro b/cybership_description/urdf/model/voyager/base.urdf.xacro index 0982010..523b553 100644 --- a/cybership_description/urdf/model/voyager/base.urdf.xacro +++ b/cybership_description/urdf/model/voyager/base.urdf.xacro @@ -19,15 +19,16 @@ - + - + + From 849c8c5dea9f5ea08d56cc05132a0e6055cb3ecb Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Fri, 1 Aug 2025 11:14:15 +0200 Subject: [PATCH 18/46] perf: increase position keeping performance --- .../config/voyager/controller_position.yaml | 4 +- cybership_tests/cybership_tests/mission.py | 2 +- cybership_tests/cybership_tests/mission_1.py | 799 ++++++++++++++++++ 3 files changed, 802 insertions(+), 3 deletions(-) create mode 100644 cybership_tests/cybership_tests/mission_1.py diff --git a/cybership_config/config/voyager/controller_position.yaml b/cybership_config/config/voyager/controller_position.yaml index fc9e485..2d6b245 100644 --- a/cybership_config/config/voyager/controller_position.yaml +++ b/cybership_config/config/voyager/controller_position.yaml @@ -43,8 +43,8 @@ # Reference filter parameters filter: - omega: [0.15, 0.15, 0.15] # Natural frequency for reference filter - delta: [1.8, 1.8, 1.8] # Damping ratio for reference filter + omega: [0.25, 0.25, 0.60] # Natural frequency for reference filter + delta: [1.00, 1.00, 1.00] # Damping ratio for reference filter # Performance metrics configuration metrics: diff --git a/cybership_tests/cybership_tests/mission.py b/cybership_tests/cybership_tests/mission.py index cb78477..2a07fcb 100755 --- a/cybership_tests/cybership_tests/mission.py +++ b/cybership_tests/cybership_tests/mission.py @@ -704,7 +704,7 @@ def main(args: Optional[List[str]] = None) -> None: rclpy.init(args=args) node = ActionRunner() - executor = MultiThreadedExecutor() + executor = SingleThreadedExecutor() executor.add_node(node) node.execute_actions() diff --git a/cybership_tests/cybership_tests/mission_1.py b/cybership_tests/cybership_tests/mission_1.py new file mode 100644 index 0000000..7be152e --- /dev/null +++ b/cybership_tests/cybership_tests/mission_1.py @@ -0,0 +1,799 @@ +#!/usr/bin/env python3 + +import rclpy +import rclpy.client +from rclpy.node import Node +from std_srvs.srv import SetBool +from topic_tools_interfaces.srv import MuxSelect, MuxAdd +from geometry_msgs.msg import PoseStamped, Twist, Wrench +from std_msgs.msg import Bool +from nav_msgs.msg import Odometry +from rclpy.action import ActionClient +from nav2_msgs.action import NavigateToPose +from tf_transformations import quaternion_from_euler +import numpy as np +from typing import Any, Dict, Optional, List +from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor +from abc import ABC, abstractmethod +import time + + +# Topic and service name constants +TOPIC_FORCE_CONSTANT: str = "control/force/command/constant" +TOPIC_VELOCITY_COMMAND: str = "control/velocity/command" +TOPIC_ODOM: str = "measurement/odom" +TOPIC_POSE_GOAL: str = "navigate_to_pose" +TOPIC_FORCE_POSITION: str = "control/force/command/position" +TOPIC_FORCE_VELOCITY: str = "control/force/command/velocity" +TOPIC_FORCE_BASE: str = "control/force/command" + +SERVICE_MUX_ADD: str = "force_mux/add" +SERVICE_MUX_SELECT: str = "force_mux/select" +SERVICE_POSE_ENABLE: str = "position_controller/change_state" +SERVICE_VEL_ENABLE: str = "velocity_controller/change_state" + +TOPIC_EXPERIMENT_ODOM: str = "experiment/odom" +TOPIC_EXPERIMENT_FORCE: str = "experiment/force" +TOPIC_EXPERIMENT_FLAG: str = "experiment/flag" + + +class ActionRunner(Node): + def __init__(self): + super().__init__("action_runner", namespace="voyager") + # Private executor for service calls + # self._executor.add_node(self) + # Define the list of mission actions + self.repeat = 500 + + self.mux_add_client = self.create_client(MuxAdd, SERVICE_MUX_ADD) + + self.mux_select_client = self.create_client( + MuxSelect, SERVICE_MUX_SELECT) + + self.pose_action_client = ActionClient( + self, NavigateToPose, TOPIC_POSE_GOAL) + + self.pose_enable_client = self.create_client( + SetBool, SERVICE_POSE_ENABLE) + + self.vel_enable_client = self.create_client( + SetBool, SERVICE_VEL_ENABLE) + + self.vel_pub = self.create_publisher(Twist, TOPIC_VELOCITY_COMMAND, 10) + self.force_pub = self.create_publisher( + Wrench, TOPIC_FORCE_CONSTANT, 10) + + self.experiment_odom_pub = self.create_publisher( + Odometry, TOPIC_EXPERIMENT_ODOM, 10 + ) + self.experiment_force_pub = self.create_publisher( + Wrench, TOPIC_EXPERIMENT_FORCE, 10 + ) + self.experiment_flag_pub = self.create_publisher( + Bool, TOPIC_EXPERIMENT_FLAG, 10 + ) + + self.current_odom = None + self.create_subscription(Odometry, TOPIC_ODOM, self.odom_callback, 10) + + self.rng = np.random.default_rng() + + # Experiment publishing state + self.experiment_enabled: bool = False + self._last_force: Optional[Wrench] = None + self.create_timer(0.05, self._experiment_publisher) + + # Default abort lands back at origin after timeout + self.default_abort_action = { + "name": "default_abort", + "action": self.run_pose_action, + "parameters": { + "x": -4.0, + "y": 0.5, + "yaw": 0.0, + "duration": 50.0, + "grace": 50.0, + }, + } + + self.actions = [ + # i want boat in same starting position each time. + { + "name": "pose_neg_diag", + "action": self.run_pose_action, + "parameters": { + "x": -4.0, + "y": 0.5, + "yaw": 0.0, + "duration": 60.0, + }, + }, + # command constant forces + # { + # "name": "command_constant_force", + # "action": self.run_force_action, + # "parameters": { + # "force_x": lambda: self.rng.uniform(0.7, 1.3), + # "force_y": lambda: self.rng.uniform(-0.0, 0.0), + # "torque_z": lambda: self.rng.uniform(-0.0, 0.0), + # "duration": 20.0, + # "experiment": True, # Enable experiment flag + # "continuous_sampling": True, # Sample force continuously + # "frequency": 0.25, # Hz + # } + # }, + # { + # "name": "pose_pos_diag", + # "action": self.run_pose_action, + # "parameters": { + # "x": 3.0, + # "y": 0.0, + # "yaw": np.pi, + # "duration": 60.0, + # }, + # }, + # command constant forces + { + "name": "command_constant_force", + "action": self.run_force_action, + "parameters": { + "force_x": lambda: self.rng.uniform(0.7, 1.2), + "force_y": lambda: self.rng.uniform(-0.1, 0.1), + "torque_z": lambda: self.rng.uniform(-0.1, 0.1), + "duration": 20.0, + "experiment": True, # Enable experiment flag + "continuous_sampling": True, # Sample force continuously + "frequency": 0.25, # Hz + } + } + # { + # "name": "pose_neg_diag", + # "action": self.run_pose_action, + # "parameters": { + # "x": -2.0, + # "y": -2.0, + # "yaw": np.pi / 4, + # "duration": 60.0, + # }, + # }, + # { + # "name": "velocity_random_1", + # "action": self.run_velocity_action, + # "parameters": { + # # Sample linear and angular components each publish + # "linear_x": lambda: self.rng.uniform(0, 0.3), + # "linear_y": lambda: self.rng.uniform(-0.1, 0.1), + # "angular_z": lambda: self.rng.uniform(-0.1, 0.1), + # "duration": 15.0, + # "margin": 0.01, # Allow some margin for early success + # }, + # "abort_action": { + # "name": "velocity_random_1_abort", + # "action": self.run_pose_action, + # "parameters": { + # "x": 2.0, + # "y": 2.0, + # "yaw": 5 * np.pi / 4, + # "duration": 60.0, + # "grace": 15.0, + # }, + # }, + # }, + # { + # "name": "force_random_1", + # "action": self.run_force_action, + # "parameters": { + # "force_x": lambda: self.rng.uniform(0.5, 1.5), + # "force_y": lambda: self.rng.uniform(-1.5, 1.5), + # "torque_z": lambda: self.rng.uniform(-1.5, 1.5), + # "duration": 20.0, + # "experiment": True, # Enable experiment flag + # "continuous_sampling": True, # Sample force continuously + # "frequency": 1.0, # Hz + # }, + # "abort_action": { + # "name": "force_random_1_abort", + # "action": self.run_pose_action, + # "parameters": { + # "x": 2.0, + # "y": 2.0, + # "yaw": 5 * np.pi / 4, + # "duration": 60.0, + # "grace": 15.0, + # }, + # }, + # }, + # { + # "name": "pose_pos_diag", + # "action": self.run_pose_action, + # "parameters": { + # "x": 2.0, + # "y": 2.0, + # "yaw": 5 * np.pi / 4, + # "duration": 60.0, + # }, + # }, + # { + # "name": "velocity_random_2", + # "action": self.run_velocity_action, + # "parameters": { + # # Sample linear and angular components each publish + # "linear_x": lambda: self.rng.uniform(0, 0.3), + # "linear_y": lambda: self.rng.uniform(-0.1, 0.1), + # "angular_z": lambda: self.rng.uniform(-0.1, 0.1), + # "duration": 15.0, + # "margin": 0.01, # Allow some margin for early success + # }, + # "abort_action": { + # "name": "velocity_random_2_abort", + # "action": self.run_pose_action, + # "parameters": { + # "x": -2.0, + # "y": -2.0, + # "yaw": np.pi / 4, + # "duration": 60.0, + # "grace": 15.0, + # }, + # }, + # }, + # { + # "name": "force_random_2", + # "action": self.run_force_action, + # "parameters": { + # "force_x": lambda: self.rng.uniform(0, 0.2), + # "force_y": lambda: self.rng.uniform(0, 0.05), + # "torque_z": lambda: self.rng.uniform(0, 0.05), + # "duration": 20.0, + # "experiment": True, # Enable experiment flag + # "continuous_sampling": True, # Sample force continuously + # "frequency": 1.0, # Hz + # }, + # "abort_action": { + # "name": "force_random_2_abort", + # "action": self.run_pose_action, + # "parameters": { + # "x": -2.0, + # "y": -2.0, + # "yaw": np.pi / 4, + # "duration": 60.0, + # "grace": 15.0, + # }, + # }, + # }, + # { + # "name": "wait", + # "action": self.run_wait_action, + # "parameters": {"duration": 10.0}, + # }, + ] + + def call_init_services(self) -> None: + # Private executor for service calls + # self._executor.add_node(self) + # Define the list of mission actions + + # Action client for pose navigation (relative to node namespace) + self.get_logger().info(f"Waiting for pose action server...") + self.pose_action_client.wait_for_server() + + self.get_logger().info(f"Waiting for pose enable server ...") + self.pose_enable_client.wait_for_service() + + self.get_logger().info(f"Waiting for vel enables server ...") + self.vel_enable_client.wait_for_service() + + self.get_logger().info(f"Waiting for Force mux server...") + self.mux_add_client.wait_for_service() + + self.get_logger().info(f"Waiting for Force mux server...") + self.mux_select_client.wait_for_service() + + self.add_mux(TOPIC_FORCE_CONSTANT) + + self.wait_until_ready() + + def wait_until_ready(self) -> None: + """ + Block until the first odometry message is received. + + Ensures the vehicle has a valid initial pose before performing actions. + """ + + while rclpy.ok() and self.current_odom is None: + self.get_logger().info("Waiting for initial odometry data...") + # process callbacks without blocking executor + rclpy.spin_once(self, timeout_sec=0.1) + time.sleep(0.5) + self.get_logger().info("Initial odometry data received.") + + def _experiment_publisher(self) -> None: + """ + Background thread publishes experiment flag, and when enabled, odometry and force data. + """ + flag_msg = Bool() + flag_msg.data = self.experiment_enabled + self.experiment_flag_pub.publish(flag_msg) + if self.experiment_enabled: + if self.current_odom: + self.experiment_odom_pub.publish(self.current_odom) + if self._last_force: + self.experiment_force_pub.publish(self._last_force) + + def add_mux(self, topic: str) -> None: + """ + Add a topic to the force multiplexer using MuxAdd service. + + Args: + topic (str): ROS topic name to add to the mux. + """ + req = MuxAdd.Request() + req.topic = topic + future = self.mux_add_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + # MuxAdd returns a result with a success flag + if future.result().success: + self.get_logger().info(f"Mux added topic {topic}") + else: + self.get_logger().error(f"Failed to add mux topic {topic}") + + def abort_condition(self) -> bool: + """ + Determine if the mission should abort based on odometry limits. + + Returns: + bool: True if |x| or |y| position exceeds 2.5 meters, False otherwise. + """ + + return ( + not( + (-5.0 < self.current_odom.pose.pose.position.x < 4.0) + and (-3.0 < self.current_odom.pose.pose.position.y < 2.0) + ) + ) + + def call_mux(self, topic: str) -> None: + """ + Select the active force control topic on the mux. + + Args: + topic (str): ROS topic name to select for force commands. + """ + self.get_logger().info(f'Calling mux select for topic {topic}') + req = MuxSelect.Request() + req.topic = topic + future = self.mux_select_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result(): + self.get_logger().info(f"Mux switched to {topic}") + else: + self.get_logger().error(f"Failed to switch mux to {topic}") + + def set_enable(self, client: rclpy.client.Client, enable: bool) -> None: + """ + Enable or disable a controller via SetBool service. + + Args: + client (rclpy.client.Client): Service client for SetBool. + enable (bool): True to enable, False to disable. + """ + self.get_logger().info(f"Setting {client} enable={enable}") + req = SetBool.Request() + req.data = enable + future = client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result().success: + self.get_logger().info(f'Service {client} set enable={enable}') + else: + self.get_logger().error(f'Failed to set {client} enable={enable}') + + def _pose_feedback_callback(self, feedback_msg: Any) -> None: + """ + Callback to log remaining distance from a NavigateToPose action. + + Args: + feedback_msg: Action feedback message containing distance_remaining (float, meters). + """ + fb = feedback_msg.feedback + # nav2 NavigateToPose feedback has distance_remaining + dist = getattr(fb, "distance_remaining", None) + if dist is not None: + self.get_logger().info( + f" NavigateToPose feedback: distance_remaining={dist:.2f}" + ) + else: + self.get_logger().info(" NavigateToPose feedback received") + + def run_pose_action(self, **params: Any) -> None: + """ + Navigate to a target pose and monitor for timeouts or abort conditions. + + Sends a NavigateToPose goal and cancels if duration elapsed or abort condition met. + + Args: + x (float): Target x-position in meters. + y (float): Target y-position in meters. + yaw (float): Target yaw angle in radians. + duration (float): Max navigation time in seconds. + grace (float): Grace period before starting abort checks in seconds. + + Raises: + RuntimeError: If abort_condition() returns True after grace period. + """ + resolved_params = {k: v() if callable( + v) else v for k, v in params.items()} + # Activate pose controller via mux & service + self.call_mux(TOPIC_FORCE_POSITION) + + self.set_enable(self.pose_enable_client, True) + + # Extract parameters + x = resolved_params.get("x", 0.0) + y = resolved_params.get("y", 0.0) + yaw = resolved_params.get("yaw", 0.0) + duration = resolved_params.get("duration", 0.0) + grace = resolved_params.get("grace", 0.0) + self.get_logger().info( + f"Pose action start (x={x:.2f}, y={y:.2f}, yaw={yaw:.2f}), " + f"duration={duration:.1f}s, grace={grace:.1f}s" + ) + + # Build goal message + msg = PoseStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = "world" + msg.pose.position.x = x + msg.pose.position.y = y + q = quaternion_from_euler(0.0, 0.0, yaw) + msg.pose.orientation.x = q[0] + msg.pose.orientation.y = q[1] + msg.pose.orientation.z = q[2] + msg.pose.orientation.w = q[3] + goal = NavigateToPose.Goal() + goal.pose = msg + + # Send goal with feedback callback + goal_fut = self.pose_action_client.send_goal_async( + goal, feedback_callback=self._pose_feedback_callback + ) + rclpy.spin_until_future_complete(self, goal_fut) + handle = goal_fut.result() + if not handle.accepted: + self.get_logger().error("NavigateToPose goal rejected") + self.set_enable(self.pose_enable_client, False) + return + + self.get_logger().info("NavigateToPose goal accepted, monitoring execution") + + # Wait for result, checking abort & timeout + result_fut = handle.get_result_async() + start = self.get_clock().now().nanoseconds / 1e9 + elapsed = 0.0 + while rclpy.ok(): + elapsed = (self.get_clock().now().nanoseconds / 1e9) - start + + if elapsed >= duration: + self.get_logger().info( + "NavigateToPose action completed due to duration" + ) + cancel_fut = handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, cancel_fut) + self.set_enable(self.pose_enable_client, False) + break + + if result_fut.done(): + self.get_logger().info("NavigateToPose action completed") + break + + if elapsed >= grace and self.abort_condition(): + + cancel_fut = handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, cancel_fut) + self.set_enable(self.pose_enable_client, False) + raise RuntimeError( + 'Abort condition met during pose action, aborting') + + rclpy.spin_once(self, timeout_sec=0.5) + + # Deactivate pose controller + self.set_enable(self.pose_enable_client, False) + + def run_velocity_action(self, **params: Any) -> None: + """ + Publish constant velocity commands for a given duration, with optional early stop. + + Args: + linear_x (float): Desired linear velocity in x (m/s). + linear_y (float): Desired linear velocity in y (m/s). + angular_z (float): Desired angular velocity around z (rad/s). + duration (float): Total time to publish commands (seconds). + margin (float, optional): Early stop threshold for actual vs commanded velocity (m/s). + grace (float): Grace period before starting abort checks (seconds). + + Raises: + RuntimeError: If abort_condition() returns True after grace period. + """ + resolved_params = {k: v() if callable( + v) else v for k, v in params.items()} + # Activate velocity controller + self.call_mux(TOPIC_FORCE_VELOCITY) + self.set_enable(self.vel_enable_client, True) + + # Prepare and log command + msg = Twist() + lx = resolved_params.get("linear_x", 0.0) + ly = resolved_params.get("linear_y", 0.0) + az = resolved_params.get("angular_z", 0.0) + margin = resolved_params.get("margin", None) + self.get_logger().info( + " " + f"Velocity action (u={lx:.4f}, v={ly:.4f}, r={az:.4f})" + + (f", margin={margin:.4f}" if margin is not None else "") + ) + + # Extract duration, grace period and start time + duration = resolved_params.get("duration", 0.0) + grace = resolved_params.get("grace", 0.0) + start = self.get_clock().now().nanoseconds / 1e9 + elapsed = 0.0 + # Loop at ~10 Hz, processing callbacks without blocking + period = 0.1 + + while rclpy.ok() and elapsed < duration: + elapsed = self.get_clock().now().nanoseconds / 1e9 - start + # Publish the commanded velocity + msg.linear.x = lx + msg.linear.y = ly + msg.angular.z = az + self.vel_pub.publish(msg) + + # Feedback: log current odometry state + if self.current_odom: + pos = self.current_odom.pose.pose.position + lvel = self.current_odom.twist.twist.linear + avel = self.current_odom.twist.twist.angular + self.get_logger().info( + " " + f"Velocity feedback: " + f"vel=(vx={lvel.x:.2f}, vy={lvel.y:.2f}, vr={avel.z:.2f}), " + f"cmd=(u={lx:.2f}, v={ly:.2f}, r={az:.2f})" + ) + + # margin-based early success + if margin is not None: + act = self.current_odom.twist.twist + if abs(act.linear.x - lx) < margin and abs(act.linear.y - ly) < margin: + self.get_logger().info("Velocity within margin, stopping early") + break + + # Abort check after grace period + if self.current_odom and elapsed >= grace and self.abort_condition(): + raise RuntimeError( + "Abort condition met during velocity action, aborting" + ) + time.sleep(0.1) + rclpy.spin_once(self, timeout_sec=period) + + # Deactivate velocity controller + self.set_enable(self.vel_enable_client, False) + + def run_wait_action(self, **params: Any) -> None: + """ + Wait for a specified duration while processing ROS callbacks. + + This loop periodically checks for an abort condition after an optional grace period + and logs odometry feedback during the wait. + + Args: + duration (float): Total wait time in seconds. + grace (float): Grace period before starting abort checks in seconds. + + Raises: + RuntimeError: If abort_condition() returns True after grace period. + """ + resolved_params = {k: v() if callable( + v) else v for k, v in params.items()} + duration = resolved_params.get("duration", 0.0) + grace = resolved_params.get("grace", 0.0) + start = self.get_clock().now().nanoseconds / 1e9 + + self.get_logger().info(f"Waiting for {duration:.4f} seconds") + elapsed = 0.0 + # Loop at ~2 Hz, processing callbacks without blocking + period = 0.5 + while rclpy.ok() and elapsed < duration: + elapsed = self.get_clock().now().nanoseconds / 1e9 - start + + if elapsed >= grace and self.abort_condition(): + raise RuntimeError( + "Abort condition met during wait action, aborting") + + # allow callbacks and enforce loop timing + rclpy.spin_once(self, timeout_sec=period) + + if self.current_odom: + pos = self.current_odom.pose.pose.position + self.get_logger().info( + f"Wait feedback: pos=({pos.x:.2f}, {pos.y:.2f}), " + f"elapsed={elapsed:.2f}s" + ) + + def run_force_action(self, **params: Any) -> None: + """ + Apply a constant force to the vehicle for a specified duration. + + This loop publishes a Wrench message at fixed intervals and checks for abort + conditions after an optional grace period. Odometry feedback is logged each cycle. + + Args: + force_x (float): Force in x-direction in Newtons. + force_y (float): Force in y-direction in Newtons. + torque_z (float): Torque about z-axis in Newton-meters. + duration (float): Total time to apply force in seconds. + grace (float): Grace period before starting abort checks in seconds. + + Raises: + RuntimeError: If abort_condition() returns True after grace period. + """ + + resolved_params = {k: v() if callable( + v) else v for k, v in params.items()} + + self.call_mux(TOPIC_FORCE_CONSTANT) + + msg = Wrench() + + duration = params.get("duration", 0.0) + grace = params.get("grace", 0.0) + + continuous_sampling = params.get("continuous_sampling", False) + frequency = params.get("frequency", 1.0) # Hz + + start = self.get_clock().now().nanoseconds / 1e9 + elapsed = 0.0 + + # Loop at specified frequency (or 2 Hz), processing callbacks without blocking + period = 1.0 / (frequency if continuous_sampling else 2.0) + + last_update_elapsed = 0.0 + while rclpy.ok() and elapsed < duration: + + elapsed = self.get_clock().now().nanoseconds / 1e9 - start + + # allow callbacks and enforce publish rate + rclpy.spin_once(self, timeout_sec=0.5) + + # if np.mod(elapsed, period) > 0.01: + # # Skip this iteration if not at the right frequency + # continue + + if abs(last_update_elapsed - elapsed) < period: + # Skip if not enough time has passed since last update + continue + + last_update_elapsed = elapsed + + msg.force.x = resolved_params.get("force_x", 0.0) + msg.force.y = resolved_params.get("force_y", 0.0) + msg.torque.z = resolved_params.get("torque_z", 0.0) + + if continuous_sampling: + # Sample force parameters at each iteration + msg.force.x = params.get("force_x", 0.0)() + msg.force.y = params.get("force_y", 0.0)() + msg.torque.z = params.get("torque_z", 0.0)() + + # record last force for experiment publishing + self._last_force = msg + self.force_pub.publish(msg) + + if self.current_odom: + pos = self.current_odom.pose.pose.position + self.get_logger().info( + f"Force feedback: (fx={msg.force.x:.2f}, " + f"fy={msg.force.y:.2f}, tz={msg.torque.z:.2f})," + f" pos=({pos.x:.2f}, {pos.y:.2f}), " + f"elapsed={elapsed:.2f}s" + ) + + if elapsed >= grace and self.abort_condition(): + raise RuntimeError( + "Abort condition met during force action, aborting") + + # time.sleep(period) # Sleep for the period to control frequency + + def execute_action(self, action: Dict[str, Any]) -> None: + """ + Execute one mission action and resolve its parameters. + + Normalizes callable parameters, disables other controllers, and invokes the action. + + Args: + action (dict): Action descriptor with keys 'action' (callable) and 'parameters' (dict). + """ + if not isinstance(action, dict): + self.get_logger().error(f'Invalid action format: {action}') + return + + runner = action.get('action') + params = action.get('parameters', {}) + + self.set_enable(self.pose_enable_client, False) + self.set_enable(self.vel_enable_client, False) + + if not callable(runner): + self.get_logger().warn(f'Unknown action: {runner}') + return + + self.get_logger().info(f"Executing action: {action['name']}") + + # Handle experiment flag without resolving other parameters + self.experiment_enabled = bool(params.get("experiment", False)) + runner(**params) + + # Disable experiment after action completes + self.experiment_enabled = False + + def execute_actions(self) -> None: + """ + Loop through the action sequence `repeat` times, handling aborts. + + For each action, calls execute_action and on RuntimeError runs an abort_action. + """ + print("executing actions...") + for _ in range(self.repeat): + + for action in self.actions: + + try: + self.execute_action(action) + except RuntimeError as e: + abort_action = action.get( + 'abort_action', self.default_abort_action) + self.get_logger().warn( + f'Error executing action {action["name"]}: {e}') + self.get_logger().warn( + f'Executing abort action: {abort_action["name"]}') + try: + self.execute_action(abort_action) + except RuntimeError as abort_e: + self.get_logger().error( + f'Abort action failed too!: {abort_e}') + self.set_enable(self.pose_enable_client, False) + self.set_enable(self.vel_enable_client, False) + self.call_mux(TOPIC_FORCE_BASE) + exit(1) + + def odom_callback(self, msg): + """ + Callback for Odometry subscription; stores the latest message. + + Args: + msg (Odometry): Received odometry message containing pose and twist. + """ + self.current_odom = msg + + # if self.experiment_enabled: + # # Publish odometry for experiment if enabled + # self.experiment_odom_pub.publish(msg) + # self.experiment_flag_pub.publish(Bool(data=True)) + # self.experiment_force_pub.publish(self._last_force) + + +def main(args: Optional[List[str]] = None) -> None: + + rclpy.init(args=args) + node = ActionRunner() + + node.call_init_services() + node.wait_until_ready() + + executor = MultiThreadedExecutor() + executor.add_node(node) + + node.execute_actions() + executor.spin() + executor.shutdown() + + +if __name__ == "__main__": + main() From 94809328d1b8a07a2d8a5f80ec464a4f25a8f921 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Thu, 7 Aug 2025 15:48:00 +0200 Subject: [PATCH 19/46] fix: launch path substitution --- cybership_bringup/launch/drillship.launch.py | 4 +--- cybership_bringup/launch/enterprise.launch.py | 4 +--- cybership_bringup/launch/localization.launch.py | 4 +--- cybership_bringup/launch/voyager.launch.py | 4 +--- 4 files changed, 4 insertions(+), 12 deletions(-) diff --git a/cybership_bringup/launch/drillship.launch.py b/cybership_bringup/launch/drillship.launch.py index 0dc6666..be9de5e 100644 --- a/cybership_bringup/launch/drillship.launch.py +++ b/cybership_bringup/launch/drillship.launch.py @@ -26,9 +26,7 @@ def include_launch_action_with_config( ( 'param_file', launch.substitutions.PathJoinSubstitution( - launch.substitutions.PathJoinSubstitution( - [config_pkg_dir, 'config', vessel_model, param_file] - ) + [config_pkg_dir, 'config', vessel_model, config_file] ) ) ) diff --git a/cybership_bringup/launch/enterprise.launch.py b/cybership_bringup/launch/enterprise.launch.py index de6c384..bfafaf9 100644 --- a/cybership_bringup/launch/enterprise.launch.py +++ b/cybership_bringup/launch/enterprise.launch.py @@ -26,9 +26,7 @@ def include_launch_action_with_config( ( 'param_file', launch.substitutions.PathJoinSubstitution( - launch.substitutions.PathJoinSubstitution( - [config_pkg_dir, 'config', vessel_model, config_file] - ) + [config_pkg_dir, 'config', vessel_model, config_file] ) ) ) diff --git a/cybership_bringup/launch/localization.launch.py b/cybership_bringup/launch/localization.launch.py index 8ba0190..95cfe3d 100644 --- a/cybership_bringup/launch/localization.launch.py +++ b/cybership_bringup/launch/localization.launch.py @@ -26,9 +26,7 @@ def include_launch_action_with_config( ( 'param_file', launch.substitutions.PathJoinSubstitution( - launch.substitutions.PathJoinSubstitution( - [config_pkg_dir, 'config', vessel_model, config_file] - ) + [config_pkg_dir, 'config', vessel_model, config_file] ) ) ) diff --git a/cybership_bringup/launch/voyager.launch.py b/cybership_bringup/launch/voyager.launch.py index 7eb6b06..ce11d9a 100644 --- a/cybership_bringup/launch/voyager.launch.py +++ b/cybership_bringup/launch/voyager.launch.py @@ -26,9 +26,7 @@ def include_launch_action_with_config( ( 'param_file', launch.substitutions.PathJoinSubstitution( - launch.substitutions.PathJoinSubstitution( - [config_pkg_dir, 'config', vessel_model, param_file] - ) + [config_pkg_dir, 'config', vessel_model, config_file] ) ) ) From 364e6c06643b652aa42a6490f1bc19379c674457 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Thu, 7 Aug 2025 15:48:36 +0200 Subject: [PATCH 20/46] ci: change --- .github/workflows/build-and-push.yml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.github/workflows/build-and-push.yml b/.github/workflows/build-and-push.yml index 9b6369a..4b87482 100644 --- a/.github/workflows/build-and-push.yml +++ b/.github/workflows/build-and-push.yml @@ -22,8 +22,7 @@ jobs: - ros_distro: 'jazzy' arch: 'arm64' - # runs-on: ubuntu-latest - runs-on: ubuntu-22.04 + runs-on: ubuntu-latest steps: - name: Checkout From 555ae8f0c0b31d5fca4e0b65489751071ccf625f Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Tue, 19 Aug 2025 12:50:10 +0200 Subject: [PATCH 21/46] feat: add drillship --- cybership_bringup/launch/drillship.launch.py | 4 +- cybership_bringup/launch/voyager.launch.py | 2 +- .../config/drillship/azimuth_controller.yaml | 14 ++--- .../config/drillship/servo_driver.yaml | 56 +++++++++--------- .../config/drillship/thruster_control.yaml | 42 +++++++------- .../urdf/model/drillship/base.urdf.xacro | 9 ++- .../drillship/force_controller.py | 58 ++++++++++++------- 7 files changed, 105 insertions(+), 80 deletions(-) diff --git a/cybership_bringup/launch/drillship.launch.py b/cybership_bringup/launch/drillship.launch.py index be9de5e..bfc1f5e 100644 --- a/cybership_bringup/launch/drillship.launch.py +++ b/cybership_bringup/launch/drillship.launch.py @@ -9,7 +9,7 @@ def include_launch_action_with_config( vessel_model, vessel_name, launch_file, - param_file=""): + config_file=""): bringup_pkg_dir = launch_ros.substitutions.FindPackageShare( 'cybership_bringup') @@ -21,7 +21,7 @@ def include_launch_action_with_config( ('vessel_model', vessel_model) ] - if len(param_file) != 0: + if len(config_file) != 0: launch_arguments.append( ( 'param_file', diff --git a/cybership_bringup/launch/voyager.launch.py b/cybership_bringup/launch/voyager.launch.py index ce11d9a..54d97b3 100644 --- a/cybership_bringup/launch/voyager.launch.py +++ b/cybership_bringup/launch/voyager.launch.py @@ -9,7 +9,7 @@ def include_launch_action_with_config( vessel_model, vessel_name, launch_file, - param_file=""): + config_file=""): bringup_pkg_dir = launch_ros.substitutions.FindPackageShare( 'cybership_bringup') diff --git a/cybership_config/config/drillship/azimuth_controller.yaml b/cybership_config/config/drillship/azimuth_controller.yaml index 19c8cd2..f2d4101 100644 --- a/cybership_config/config/drillship/azimuth_controller.yaml +++ b/cybership_config/config/drillship/azimuth_controller.yaml @@ -11,7 +11,7 @@ device_id: 3 gain: 100.0 delta: 45.0 - offset: -2.6 + offset: 1.6 servo_2: frame_id: servo_2_link @@ -19,7 +19,7 @@ device_id: 1 gain: 100.0 delta: 45.0 - offset: -0.18 + offset: 2.2 servo_3: frame_id: servo_3_link @@ -27,7 +27,7 @@ device_id: 2 gain: 100.0 delta: 45.0 - offset: -2.6 + offset: -1.0 servo_4: frame_id: servo_4_link @@ -35,7 +35,7 @@ device_id: 6 gain: 100.0 delta: 45.0 - offset: -0.18 + offset: 2.4 servo_5: frame_id: servo_5_link @@ -43,12 +43,12 @@ device_id: 4 gain: 100.0 delta: 45.0 - offset: -2.6 + offset: -2.0 servo_6: frame_id: servo_6_link - control_topic: hardware/joint/servo_6_/angle + control_topic: hardware/joint/servo_6/angle device_id: 5 gain: 100.0 delta: 45.0 - offset: -0.18 \ No newline at end of file + offset: 0.2 \ No newline at end of file diff --git a/cybership_config/config/drillship/servo_driver.yaml b/cybership_config/config/drillship/servo_driver.yaml index 11442e8..dc5a5ee 100644 --- a/cybership_config/config/drillship/servo_driver.yaml +++ b/cybership_config/config/drillship/servo_driver.yaml @@ -6,24 +6,24 @@ esc_1: pwm_topic: hardware/prop/esc_1/pwm scaled_topic: hardware/prop/esc_1/scaled - channel: 0 + channel: 5 pulse_duration: - center: 1.575 - max: 1.675 - min: 1.475 + center: 1.50001 + max: 1.7 + min: 1.5 scale: center: 0.0 - min: -1.3 - max: 1.3 + min: -1.0 + max: 1.0 esc_2: pwm_topic: hardware/prop/esc_2/pwm scaled_topic: hardware/prop/esc_2/scaled - channel: 1 + channel: 7 pulse_duration: - center: 1.575 - max: 1.675 - min: 1.475 + center: 1.50001 + max: 1.7 + min: 1.5 scale: center: 0.0 min: -1.0 @@ -32,11 +32,11 @@ esc_3: pwm_topic: hardware/prop/esc_3/pwm scaled_topic: hardware/prop/esc_3/scaled - channel: 3 + channel: 9 pulse_duration: - center: 1.5 - min: 1.25 - max: 1.75 + center: 1.50001 + max: 1.7 + min: 1.5 scale: center: 0.0 min: -1.0 @@ -45,24 +45,24 @@ esc_4: pwm_topic: hardware/prop/esc_4/pwm scaled_topic: hardware/prop/esc_4/scaled - channel: 5 + channel: 11 pulse_duration: - center: 1.575 - max: 1.675 - min: 1.475 + center: 1.50001 + max: 1.7 + min: 1.5 scale: center: 0.0 - min: -1.3 - max: 1.3 + min: -1.0 + max: 1.0 esc_5: pwm_topic: hardware/prop/esc_5/pwm scaled_topic: hardware/prop/esc_5/scaled - channel: 4 + channel: 13 pulse_duration: - center: 1.575 - max: 1.675 - min: 1.475 + center: 1.50001 + max: 1.7 + min: 1.5 scale: center: 0.0 min: -1.0 @@ -71,11 +71,11 @@ esc_6: pwm_topic: hardware/prop/esc_6/pwm scaled_topic: hardware/prop/esc_6/scaled - channel: 3 + channel: 15 pulse_duration: - center: 1.5 - min: 1.25 - max: 1.75 + center: 1.50001 + max: 1.7 + min: 1.5 scale: center: 0.0 min: -1.0 diff --git a/cybership_config/config/drillship/thruster_control.yaml b/cybership_config/config/drillship/thruster_control.yaml index 302621d..0c5c520 100644 --- a/cybership_config/config/drillship/thruster_control.yaml +++ b/cybership_config/config/drillship/thruster_control.yaml @@ -2,11 +2,11 @@ ros__parameters: thrusters: - azimuth_1: + bow_port: type: azimuth - force_topic: thruster/azimuth_1/command + force_topic: thruster/bow_port/command force_max: 1.0 - force_min: -1.0 + force_min: 0.0 rpm: topic: hardware/prop/esc_1/scaled inverted: false @@ -14,11 +14,11 @@ topic: hardware/joint/servo_1/angle inverted: false - azimuth_2: + bow_center: type: azimuth - force_topic: thruster/azimuth_2/command + force_topic: thruster/bow_center/command force_max: 1.0 - force_min: -1.0 + force_min: 0.0 rpm: topic: hardware/prop/esc_2/scaled inverted: false @@ -26,11 +26,11 @@ topic: hardware/joint/servo_2/angle inverted: false - azimuth_3: + bow_starboard: type: azimuth - force_topic: thruster/azimuth_3/command + force_topic: thruster/bow_starboard/command force_max: 1.0 - force_min: -1.0 + force_min: 0.0 rpm: topic: hardware/prop/esc_3/scaled inverted: false @@ -38,38 +38,38 @@ topic: hardware/joint/servo_3/angle inverted: false - azimuth_4: + stern_port: type: azimuth - force_topic: thruster/azimuth_4/command + force_topic: thruster/stern_port/command force_max: 1.0 - force_min: -1.0 + force_min: 0.0 rpm: topic: hardware/prop/esc_4/scaled inverted: false angle: - topic: hardware/joint/servo_4/angle + topic: hardware/joint/servo_5/angle inverted: false - azimuth_5: + stern_center: type: azimuth - force_topic: thruster/azimuth_5/command + force_topic: thruster/stern_center/command force_max: 1.0 - force_min: -1.0 + force_min: 0.0 rpm: topic: hardware/prop/esc_5/scaled inverted: false angle: - topic: hardware/joint/servo_5/angle + topic: hardware/joint/servo_6/angle inverted: false - azimuth_6: + stern_starboard: type: azimuth - force_topic: thruster/azimuth_6/command + force_topic: thruster/stern_starboard/command force_max: 1.0 - force_min: -1.0 + force_min: 0.0 rpm: topic: hardware/prop/esc_6/scaled inverted: false angle: - topic: hardware/joint/servo_6/angle + topic: hardware/joint/servo_4/angle inverted: false diff --git a/cybership_description/urdf/model/drillship/base.urdf.xacro b/cybership_description/urdf/model/drillship/base.urdf.xacro index 5588a08..76a6230 100644 --- a/cybership_description/urdf/model/drillship/base.urdf.xacro +++ b/cybership_description/urdf/model/drillship/base.urdf.xacro @@ -5,9 +5,16 @@ + + + + + + + - + diff --git a/cybership_dp/cybership_dp/drillship/force_controller.py b/cybership_dp/cybership_dp/drillship/force_controller.py index 1286381..82f596a 100644 --- a/cybership_dp/cybership_dp/drillship/force_controller.py +++ b/cybership_dp/cybership_dp/drillship/force_controller.py @@ -36,14 +36,14 @@ def __init__(self, *args, **kwargs): self.pubs["bow_starboard_azimuth"] = self.create_publisher( geometry_msgs.msg.Wrench, "thruster/bow_starboard/command", 10 ) - self.pubs["aft_port_azimuth"] = self.create_publisher( - geometry_msgs.msg.Wrench, "thruster/aft_port/command", 10 + self.pubs["stern_port_azimuth"] = self.create_publisher( + geometry_msgs.msg.Wrench, "thruster/stern_port/command", 10 ) - self.pubs["aft_center_azimuth"] = self.create_publisher( - geometry_msgs.msg.Wrench, "thruster/aft_center/command", 10 + self.pubs["stern_center_azimuth"] = self.create_publisher( + geometry_msgs.msg.Wrench, "thruster/stern_center/command", 10 ) - self.pubs["aft_starboard_azimuth"] = self.create_publisher( - geometry_msgs.msg.Wrench, "thruster/aft_starboard/command", 10 + self.pubs["stern_starboard_azimuth"] = self.create_publisher( + geometry_msgs.msg.Wrench, "thruster/stern_starboard/command", 10 ) self.declare_parameter("frequency", rclpy.Parameter.Type.DOUBLE) @@ -84,19 +84,19 @@ def timer_callback(self): msg = geometry_msgs.msg.Wrench() msg.force.x = float(u3_f[0]) msg.force.y = float(u3_f[1]) - self.pubs["aft_port_azimuth"].publish(msg) + self.pubs["stern_port_azimuth"].publish(msg) # Aft center azimuth thruster u4_f = self.actuators[4].force msg = geometry_msgs.msg.Wrench() msg.force.x = float(u4_f[0]) msg.force.y = float(u4_f[1]) - self.pubs["aft_center_azimuth"].publish(msg) + self.pubs["stern_center_azimuth"].publish(msg) # Aft starboard azimuth thruster u5_f = self.actuators[5].force msg = geometry_msgs.msg.Wrench() msg.force.x = float(u5_f[0]) msg.force.y = float(u5_f[1]) - self.pubs["aft_starboard_azimuth"].publish(msg) + self.pubs["stern_starboard_azimuth"].publish(msg) def force_callback(self, msg: geometry_msgs.msg.Wrench): @@ -116,47 +116,65 @@ def force_callback(self, msg: geometry_msgs.msg.Wrench): def _initialize_thrusters(self): bow_port_azimuth = skadipy.actuator.Azimuth( position=skadipy.toolbox.Point([0.9344, -0.11, 0.0]), + orientation=skadipy.toolbox.Quaternion( + axis=(0.0, 0.0, 1.0), radians=np.pi / 2.0 + ), extra_attributes={ "rate_limit": 0.1, "saturation_limit": 0.7, - "reference_angle": 3 * np.pi / 4.0, + "reference_angle": 3* np.pi / 4.0, } ) bow_center_azimuth = skadipy.actuator.Azimuth( position=skadipy.toolbox.Point([1.0678, 0.0, 0.0]), + orientation=skadipy.toolbox.Quaternion( + axis=(0.0, 0.0, 1.0), radians=np.pi / 2.0 + ), extra_attributes={ "rate_limit": 0.1, "saturation_limit": 0.7, - "reference_angle": 4 * np.pi / 4.0, + "reference_angle": -np.pi, } ) bow_starboard_azimuth = skadipy.actuator.Azimuth( position=skadipy.toolbox.Point([0.9344, 0.11, 0.0]), + orientation=skadipy.toolbox.Quaternion( + axis=(0.0, 0.0, 1.0), radians=np.pi / 2.0 + ), extra_attributes={ "rate_limit": 0.1, "saturation_limit": 0.7, - "reference_angle": 5 * np.pi / 4.0, + "reference_angle": -3*np.pi / 4.0, } ) - aft_port_azimuth = skadipy.actuator.Azimuth( + stern_port_azimuth = skadipy.actuator.Azimuth( position=skadipy.toolbox.Point([-0.9911, -0.1644, -0.1]), + orientation=skadipy.toolbox.Quaternion( + axis=(0.0, 0.0, 1.0), radians=np.pi / 2.0 + ), extra_attributes={ "rate_limit": 0.1, "saturation_limit": 0.7, "reference_angle": np.pi / 4.0, } ) - aft_center_azimuth = skadipy.actuator.Azimuth( + stern_center_azimuth = skadipy.actuator.Azimuth( position=skadipy.toolbox.Point([-1.1644, 0.0, 0.0]), + orientation=skadipy.toolbox.Quaternion( + axis=(0.0, 0.0, 1.0), radians=np.pi / 2.0 + ), extra_attributes={ "rate_limit": 0.1, "saturation_limit": 0.7, "reference_angle": 0.0, } ) - aft_starboard_azimuth = skadipy.actuator.Azimuth( + stern_starboard_azimuth = skadipy.actuator.Azimuth( position=skadipy.toolbox.Point([-0.9911, 0.1644, 0.0]), + orientation=skadipy.toolbox.Quaternion( + axis=(0.0, 0.0, 1.0), radians=np.pi / 2.0 + ), extra_attributes={ "rate_limit": 0.1, "saturation_limit": 0.7, @@ -169,9 +187,9 @@ def _initialize_thrusters(self): bow_port_azimuth, bow_center_azimuth, bow_starboard_azimuth, - aft_port_azimuth, - aft_center_azimuth, - aft_starboard_azimuth + stern_port_azimuth, + stern_center_azimuth, + stern_starboard_azimuth ] def _initialize_allocator(self): @@ -183,8 +201,8 @@ def _initialize_allocator(self): self.allocator = skadipy.allocator.reference_filters.MinimumMagnitudeAndAzimuth( actuators=self.actuators, force_torque_components=dofs, - gamma=0.001, - mu=0.01, + gamma=0.01, + mu=0.1, rho=1, time_step=(1.0 /self.freq ), control_barrier_function=skadipy.safety.ControlBarrierFunctionType.SUMSQUARE, From a99e0b7a6e7bf4eaedb400e0215ac4e1d4fd823a Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Tue, 19 Aug 2025 14:13:29 +0200 Subject: [PATCH 22/46] ci: get multi arch build working --- .github/workflows/build-and-push-arm64.yml | 4 ---- .github/workflows/build-and-push.yml | 10 +++------- 2 files changed, 3 insertions(+), 11 deletions(-) diff --git a/.github/workflows/build-and-push-arm64.yml b/.github/workflows/build-and-push-arm64.yml index 6446d08..5dd2f1b 100644 --- a/.github/workflows/build-and-push-arm64.yml +++ b/.github/workflows/build-and-push-arm64.yml @@ -10,9 +10,6 @@ on: jobs: docker: - # Disable this job for now, as we are not using arm64 images yet. - if: false - strategy: fail-fast: false matrix: @@ -21,7 +18,6 @@ jobs: - ros_distro: 'jazzy' runs-on: ubuntu-24.04-arm64 - # runs-on: ubuntu-22.04 steps: - name: Checkout diff --git a/.github/workflows/build-and-push.yml b/.github/workflows/build-and-push.yml index 4b87482..9694d09 100644 --- a/.github/workflows/build-and-push.yml +++ b/.github/workflows/build-and-push.yml @@ -14,13 +14,7 @@ jobs: matrix: configurations: - ros_distro: 'humble' - arch: 'amd64' - ros_distro: 'jazzy' - arch: 'amd64' - - ros_distro: 'humble' - arch: 'arm64' - - ros_distro: 'jazzy' - arch: 'arm64' runs-on: ubuntu-latest @@ -48,5 +42,7 @@ jobs: build-args: | ROS_DISTRO=${{ matrix.configurations.ros_distro }} push: true - platforms: linux/${{ matrix.configurations.arch }} + platforms: + - linux/amd64 + - linux/arm64 tags: incebellipipo/cybership:${{ matrix.configurations.ros_distro }} From ad3d8c34405e2e3abe8affa617b7d182297865b7 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Tue, 19 Aug 2025 14:23:12 +0200 Subject: [PATCH 23/46] ci: fix platforms syntax --- .github/workflows/build-and-push.yml | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/.github/workflows/build-and-push.yml b/.github/workflows/build-and-push.yml index 9694d09..adf8852 100644 --- a/.github/workflows/build-and-push.yml +++ b/.github/workflows/build-and-push.yml @@ -42,7 +42,5 @@ jobs: build-args: | ROS_DISTRO=${{ matrix.configurations.ros_distro }} push: true - platforms: - - linux/amd64 - - linux/arm64 + platforms: linux/amd64,linux/arm64 tags: incebellipipo/cybership:${{ matrix.configurations.ros_distro }} From d605ce70771a9a073d7524b8220df7117342b756 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Tue, 19 Aug 2025 14:49:02 +0200 Subject: [PATCH 24/46] ci: fix missing build dependencies --- cybership_interfaces/package.xml | 1 + dockerfile | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/cybership_interfaces/package.xml b/cybership_interfaces/package.xml index 8075157..ce0bc1f 100644 --- a/cybership_interfaces/package.xml +++ b/cybership_interfaces/package.xml @@ -14,6 +14,7 @@ rosidl_default_generators geometry_msgs builtin_interfaces + nav_msgs rosidl_default_runtime rosidl_interface_packages diff --git a/dockerfile b/dockerfile index 7bb0319..79a8734 100644 --- a/dockerfile +++ b/dockerfile @@ -2,7 +2,7 @@ ARG ROS_DISTRO=jazzy ARG UID=1000 ARG GID=1000 -FROM ros:$ROS_DISTRO +FROM ros:${ROS_DISTRO}-ros-base ARG ROS_DISTRO ARG UID From 426de0eb8c943e3f8f51991ffd22878bb605603c Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Tue, 19 Aug 2025 17:08:22 +0200 Subject: [PATCH 25/46] ci: fix build --- cybership_thrusters/package.xml | 1 + dockerfile | 51 +++++++++++++++++---------------- 2 files changed, 27 insertions(+), 25 deletions(-) diff --git a/cybership_thrusters/package.xml b/cybership_thrusters/package.xml index ba07fe9..eb07405 100644 --- a/cybership_thrusters/package.xml +++ b/cybership_thrusters/package.xml @@ -12,6 +12,7 @@ geometry_msgs std_msgs std_srvs + rclcpp ament_lint_auto ament_lint_common diff --git a/dockerfile b/dockerfile index 79a8734..ceeef84 100644 --- a/dockerfile +++ b/dockerfile @@ -12,45 +12,46 @@ ENV ROS_DISTRO=${ROS_DISTRO} ENV DEBIAN_FRONTEND=noninteractive ENV DEBIAN_FRONTEND=teletype -RUN apt-get update && apt-get upgrade -y && apt-get install -y sudo +RUN sudo apt-get update && \ + sudo apt-get install -y git ros-dev-tools python3-venv python3-pip && \ + rosdep update -RUN uid=${UID} && \ +RUN \ + # Remove existing user with the specified UID if it exists + uid=${UID} && \ if getent passwd $uid > /dev/null; then \ username=$(getent passwd $uid | cut -d: -f1); \ userdel -r $username; \ - fi - -RUN groupadd --gid "${GID}" ros_user \ + fi \ + && \ + # Add a new user with the specified UID and GID + groupadd --gid "${GID}" ros_user \ && useradd --uid "${UID}" --gid "${GID}" -m ros_user \ && usermod -aG sudo ros_user \ && echo "ros_user ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers -RUN mkdir -p /ros/src - -WORKDIR /ros - -COPY . src/cybership_common -COPY .git/ src/cybership_common/.git/ +RUN mkdir -p /ros/src && chown -R ros_user:ros_user /ros -RUN chown -R ros_user:ros_user /ros +COPY . /ros/src/cybership +COPY .git/ /ros/src/cybership/.git/ USER ros_user -RUN sudo apt-get update && \ - sudo apt-get install -y git ros-dev-tools && \ - sudo apt-cache search ${ROS_DISTRO}-rmw \ - | grep -v -E "dbgsym|connextdds" \ - | awk '{print $1}' \ - | xargs sudo apt-get install -y - -RUN cd src/cybership_common && git submodule update --init --recursive +RUN sudo chown -R ros_user:ros_user /ros && \ + git -C /ros/src/cybership submodule update --init --recursive && \ + rosdep update && \ + rosdep install --from-paths /ros/src --ignore-src -r -y -RUN rosdep update && \ - rosdep install --from-paths src --ignore-src -r -y +RUN . /opt/ros/${ROS_DISTRO}/setup.sh && \ + python3 -m venv /ros/venv --system-site-packages --symlinks && \ + touch /ros/venv/COLCON_IGNORE && \ + . /ros/venv/bin/activate && \ + find /ros/src -name "requirements*txt" -exec pip install -r {} \; -RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.sh && colcon build" +RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.sh && cd /ros && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release && \ + echo 'source /ros/install/setup.sh' >> /home/ros_user/.bashrc && \ + echo 'source /ros/venv/bin/activate' >> /home/ros_user/.bashrc" SHELL ["/bin/bash"] - -ENTRYPOINT ["/bin/bash", "-c", "source /ros/install/setup.sh && exec \"${@}\"", "--"] +ENTRYPOINT ["/bin/bash", "-c", "source /ros/install/setup.sh && source /ros/venv/bin/activate && exec \"${@}\"", "--"] CMD ["bash"] From 9ccf00683ae75f126c0709bd48605e3c843f3f91 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Wed, 20 Aug 2025 18:29:20 +0200 Subject: [PATCH 26/46] chore: build chain clean up --- cybership_dp/CMakeLists.txt | 3 --- cybership_teleop/CMakeLists.txt | 14 ------------- .../cybership_tests/mission_action.py | 0 .../cybership_tests/mission_manager.py | 0 cybership_utilities/CMakeLists.txt | 5 ----- cybership_utilities/package.xml | 5 ----- cybership_viz/CMakeLists.txt | 20 ------------------- 7 files changed, 47 deletions(-) create mode 100644 cybership_tests/cybership_tests/mission_action.py create mode 100644 cybership_tests/cybership_tests/mission_manager.py diff --git a/cybership_dp/CMakeLists.txt b/cybership_dp/CMakeLists.txt index aab1b06..eca0167 100644 --- a/cybership_dp/CMakeLists.txt +++ b/cybership_dp/CMakeLists.txt @@ -3,9 +3,6 @@ project(cybership_dp) find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) -find_package(rclpy REQUIRED) -find_package(geometry_msgs REQUIRED) - install( DIRECTORY diff --git a/cybership_teleop/CMakeLists.txt b/cybership_teleop/CMakeLists.txt index 39e6265..7056823 100644 --- a/cybership_teleop/CMakeLists.txt +++ b/cybership_teleop/CMakeLists.txt @@ -7,21 +7,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() install( DIRECTORY diff --git a/cybership_tests/cybership_tests/mission_action.py b/cybership_tests/cybership_tests/mission_action.py new file mode 100644 index 0000000..e69de29 diff --git a/cybership_tests/cybership_tests/mission_manager.py b/cybership_tests/cybership_tests/mission_manager.py new file mode 100644 index 0000000..e69de29 diff --git a/cybership_utilities/CMakeLists.txt b/cybership_utilities/CMakeLists.txt index 76f588e..c861c03 100644 --- a/cybership_utilities/CMakeLists.txt +++ b/cybership_utilities/CMakeLists.txt @@ -8,11 +8,6 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) -find_package(joy REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(rclpy REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(tf2_ros REQUIRED) ament_python_install_package(${PROJECT_NAME}) diff --git a/cybership_utilities/package.xml b/cybership_utilities/package.xml index df00ab2..4ae4237 100644 --- a/cybership_utilities/package.xml +++ b/cybership_utilities/package.xml @@ -9,12 +9,7 @@ ament_cmake - joy - sensor_msgs rclpy - geometry_msgs - tf2_ros - ros_base ament_cmake_python diff --git a/cybership_viz/CMakeLists.txt b/cybership_viz/CMakeLists.txt index cee71cb..40dcd98 100644 --- a/cybership_viz/CMakeLists.txt +++ b/cybership_viz/CMakeLists.txt @@ -1,27 +1,7 @@ cmake_minimum_required(VERSION 3.10) project(cybership_viz) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() install( DIRECTORY launch config From 4a8fdbe78154b60d14bf6527825035d6b4160192 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Fri, 22 Aug 2025 12:03:45 +0200 Subject: [PATCH 27/46] fix: position controller and localization for drillship --- .../launch/localization.launch.py | 4 +- .../config/drillship/azimuth_controller.yaml | 2 +- .../config/drillship/controller_position.yaml | 20 +- .../config/drillship/mocap_transformer.yaml | 2 +- .../config/drillship/robot_localization.yaml | 3 +- .../urdf/model/drillship/base.urdf.xacro | 1 + .../drillship/force_controller.py | 30 +- cybership_dp/nodes/position_control_node.py | 103 +- cybership_tests/cybership_tests/mission_1.py | 1603 +++++++++-------- 9 files changed, 910 insertions(+), 858 deletions(-) mode change 100644 => 100755 cybership_tests/cybership_tests/mission_1.py diff --git a/cybership_bringup/launch/localization.launch.py b/cybership_bringup/launch/localization.launch.py index 95cfe3d..84b93be 100644 --- a/cybership_bringup/launch/localization.launch.py +++ b/cybership_bringup/launch/localization.launch.py @@ -43,9 +43,9 @@ def include_launch_action_with_config( def generate_launch_description(): - vessel_name = 'voyager' + vessel_name = 'drillship' - vessel_model = 'voyager' + vessel_model = 'drillship' ld = launch.LaunchDescription() diff --git a/cybership_config/config/drillship/azimuth_controller.yaml b/cybership_config/config/drillship/azimuth_controller.yaml index f2d4101..8dce357 100644 --- a/cybership_config/config/drillship/azimuth_controller.yaml +++ b/cybership_config/config/drillship/azimuth_controller.yaml @@ -51,4 +51,4 @@ device_id: 5 gain: 100.0 delta: 45.0 - offset: 0.2 \ No newline at end of file + offset: -1.0 \ No newline at end of file diff --git a/cybership_config/config/drillship/controller_position.yaml b/cybership_config/config/drillship/controller_position.yaml index 4d7798e..26edb7f 100644 --- a/cybership_config/config/drillship/controller_position.yaml +++ b/cybership_config/config/drillship/controller_position.yaml @@ -2,29 +2,29 @@ ros__parameters: # Vessel physical dimensions (meters) vessel: - length: 1.0 # Length of vessel (m) - beam: 0.3 # Width of vessel (m) + length: 2.0 # Length of vessel (m) + beam: 0.5 # Width of vessel (m) draft: 0.02 # Draft/depth of vessel (m) # Controller parameters control: # Position control gains p_gain: - pos: 4.0 # Proportional gain for position - vel: 0.7 # Proportional gain for velocity - yaw: 1.3 # Proportional gain for yaw + pos: 0.6 # Proportional gain for position + vel: 0.2 # Proportional gain for velocity + yaw: 0.7 # Proportional gain for yaw # Integral gains i_gain: - pos: 0.2 # Integral gain for position - vel: 0.1 # Integral gain for velocity - yaw: 0.2 # Integral gain for yaw + pos: 0.05 # Integral gain for position + vel: 0.0 # Integral gain for velocity + yaw: 0.1 # Integral gain for yaw # Derivative gains d_gain: - pos: 0.2 # Derivative gain for position + pos: 0.5 # Derivative gain for position vel: 0.5 # Derivative gain for velocity - yaw: 1.0 # Derivative gain for yaw + yaw: 1.5 # Derivative gain for yaw # Integral error limits max_integral_error: diff --git a/cybership_config/config/drillship/mocap_transformer.yaml b/cybership_config/config/drillship/mocap_transformer.yaml index 228394e..0eff1c7 100644 --- a/cybership_config/config/drillship/mocap_transformer.yaml +++ b/cybership_config/config/drillship/mocap_transformer.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - world_frame: world_ned + world_frame: world base_frame: mocap_link rigid_body_name: "2" topic_name: measurement/pose diff --git a/cybership_config/config/drillship/robot_localization.yaml b/cybership_config/config/drillship/robot_localization.yaml index 24c819d..cea5b65 100644 --- a/cybership_config/config/drillship/robot_localization.yaml +++ b/cybership_config/config/drillship/robot_localization.yaml @@ -1,4 +1,3 @@ -### ekf config file ### /**: ros__parameters: # The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin @@ -68,7 +67,7 @@ # estimation node from robot_localization! However, that instance should *not* fuse the global data. map_frame: map # Defaults to "map" if unspecified odom_frame: world # Defaults to "odom" if unspecified - base_link_frame: base_link # Defaults to "base_link" if unspecified + base_link_frame: base_link_ned # Defaults to "base_link" if unspecified world_frame: world # Defaults to the value of odom_frame if unspecified # The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry, diff --git a/cybership_description/urdf/model/drillship/base.urdf.xacro b/cybership_description/urdf/model/drillship/base.urdf.xacro index 76a6230..443bd3c 100644 --- a/cybership_description/urdf/model/drillship/base.urdf.xacro +++ b/cybership_description/urdf/model/drillship/base.urdf.xacro @@ -16,6 +16,7 @@ + diff --git a/cybership_dp/cybership_dp/drillship/force_controller.py b/cybership_dp/cybership_dp/drillship/force_controller.py index 82f596a..75b436b 100644 --- a/cybership_dp/cybership_dp/drillship/force_controller.py +++ b/cybership_dp/cybership_dp/drillship/force_controller.py @@ -68,31 +68,31 @@ def timer_callback(self): msg.force.y = float(u0_f[1]) self.pubs["bow_port_azimuth"].publish(msg) # Bow center azimuth thruster - u1_f = self.actuators[1].force - msg = geometry_msgs.msg.Wrench() - msg.force.x = float(u1_f[0]) - msg.force.y = float(u1_f[1]) - self.pubs["bow_center_azimuth"].publish(msg) + # u1_f = self.actuators[1].force + # msg = geometry_msgs.msg.Wrench() + # msg.force.x = float(u1_f[0]) + # msg.force.y = float(u1_f[1]) + # self.pubs["bow_center_azimuth"].publish(msg) # Bow starboard azimuth thruster - u2_f = self.actuators[2].force + u2_f = self.actuators[1].force msg = geometry_msgs.msg.Wrench() msg.force.x = float(u2_f[0]) msg.force.y = float(u2_f[1]) self.pubs["bow_starboard_azimuth"].publish(msg) # Aft port azimuth thruster - u3_f = self.actuators[3].force + u3_f = self.actuators[2].force msg = geometry_msgs.msg.Wrench() msg.force.x = float(u3_f[0]) msg.force.y = float(u3_f[1]) self.pubs["stern_port_azimuth"].publish(msg) # Aft center azimuth thruster - u4_f = self.actuators[4].force - msg = geometry_msgs.msg.Wrench() - msg.force.x = float(u4_f[0]) - msg.force.y = float(u4_f[1]) - self.pubs["stern_center_azimuth"].publish(msg) + # u4_f = self.actuators[4].force + # msg = geometry_msgs.msg.Wrench() + # msg.force.x = float(u4_f[0]) + # msg.force.y = float(u4_f[1]) + # self.pubs["stern_center_azimuth"].publish(msg) # Aft starboard azimuth thruster - u5_f = self.actuators[5].force + u5_f = self.actuators[3].force msg = geometry_msgs.msg.Wrench() msg.force.x = float(u5_f[0]) msg.force.y = float(u5_f[1]) @@ -185,10 +185,10 @@ def _initialize_thrusters(self): # Put all actuators in a list and create the allocator object self.actuators = [ bow_port_azimuth, - bow_center_azimuth, + # bow_center_azimuth, bow_starboard_azimuth, stern_port_azimuth, - stern_center_azimuth, + # stern_center_azimuth, stern_starboard_azimuth ] diff --git a/cybership_dp/nodes/position_control_node.py b/cybership_dp/nodes/position_control_node.py index f2192d6..c70db0b 100755 --- a/cybership_dp/nodes/position_control_node.py +++ b/cybership_dp/nodes/position_control_node.py @@ -100,6 +100,8 @@ def __init__(self): # Tolerances ('control.tolerance.pos', 0.25), ('control.tolerance.yaw', 0.1), + # Require being within tolerance for this long before success + ('control.success_hold_time', 1.0), # Vessel properties ('vessel.length', 1.0), @@ -230,6 +232,7 @@ def update_configuration(self): # Get tolerances self.pos_tol = self.get_parameter('control.tolerance.pos').value self.yaw_tol = self.get_parameter('control.tolerance.yaw').value + self.success_hold_time = self.get_parameter('control.success_hold_time').value # Get vessel properties vessel_length = self.get_parameter('vessel.length').value @@ -292,38 +295,69 @@ def parameters_callback(self, params): return True # Accept all parameter changes + # def odom_callback(self, msg: Odometry): + # """ + # Callback to update the latest odometry measurement. + # """ + # if self.ref_filter is None: + # print("Setting target position from odometry.") + # self.target_x = msg.pose.pose.position.x + # self.target_y = msg.pose.pose.position.y + # _, _, self.target_yaw = R.from_quat( + # [ + # msg.pose.pose.orientation.x, + # msg.pose.pose.orientation.y, + # msg.pose.pose.orientation.z, + # msg.pose.pose.orientation.w, + # ] + # ).as_euler("xyz", degrees=False) + + # # Get filter parameters from ROS parameters + # filter_omega = self.get_parameter('filter.omega').value + # filter_delta = self.get_parameter('filter.delta').value + + # self.ref_filter = ThirdOrderReferenceFilter( + # dt=self.dt, + # omega=filter_omega, + # delta=filter_delta, + # initial_eta=[self.target_x, self.target_y, self.target_yaw], + # ) + # self.ref_filter.eta_d = np.array( + # [self.target_x, self.target_y, self.target_yaw] + # ) + + # self.latest_odom = msg + def odom_callback(self, msg: Odometry): - """ - Callback to update the latest odometry measurement. - """ - if self.ref_filter is None: - print("Setting target position from odometry.") - self.target_x = msg.pose.pose.position.x - self.target_y = msg.pose.pose.position.y - _, _, self.target_yaw = R.from_quat( - [ - msg.pose.pose.orientation.x, - msg.pose.pose.orientation.y, - msg.pose.pose.orientation.z, - msg.pose.pose.orientation.w, - ] - ).as_euler("xyz", degrees=False) + current_x = msg.pose.pose.position.x + current_y = msg.pose.pose.position.y + _, _, current_yaw = R.from_quat([ + msg.pose.pose.orientation.x, + msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z, + msg.pose.pose.orientation.w + ]).as_euler("xyz", degrees=False) + + # Always store latest odom + self.latest_odom = msg - # Get filter parameters from ROS parameters + if self.ref_filter is None: + # Initialize the filter to CURRENT pose (not the target) filter_omega = self.get_parameter('filter.omega').value filter_delta = self.get_parameter('filter.delta').value - self.ref_filter = ThirdOrderReferenceFilter( dt=self.dt, omega=filter_omega, delta=filter_delta, - initial_eta=[self.target_x, self.target_y, self.target_yaw], - ) - self.ref_filter.eta_d = np.array( - [self.target_x, self.target_y, self.target_yaw] + initial_eta=[current_x, current_y, current_yaw], ) + self.ref_filter.eta_d = np.array([current_x, current_y, current_yaw]) - self.latest_odom = msg + # Only seed a target from odom if no goal has ever been set + if self.target_x is None: + self.target_x = current_x + self.target_y = current_y + self.target_yaw = current_yaw def process_performance_metrics(self, desired_pose, desired_vel, error_pos, error_vel, error_yaw): """ @@ -574,6 +608,11 @@ def execute_callback(self, goal_handle): # Extract target pose from the goal message target_pose: PoseStamped = goal_handle.request.pose + # Warn if frame differs from odom, since we compute distances in odom frame + if target_pose.header.frame_id and target_pose.header.frame_id not in ("odom", "/odom"): + self.get_logger().warn( + f"Goal frame_id '{target_pose.header.frame_id}' differs from 'odom'; no TF transform is applied." + ) self.target_x = target_pose.pose.position.x self.target_y = target_pose.pose.position.y @@ -595,8 +634,9 @@ def execute_callback(self, goal_handle): self.publish_target_pose_marker(target_pose) - # Loop until the robot reaches the target (within tolerance) or the goal is canceled. - while rclpy.ok(): # Loop until the robot reaches the target or goal is canceled + # Loop until the robot reaches the target (within tolerance for a hold time) or the goal is canceled. + within_since = None + while rclpy.ok(): # Loop until target reached or goal is canceled if goal_handle.is_cancel_requested: goal_handle.canceled() self.get_logger().info("NavigateToPose goal canceled") @@ -639,11 +679,18 @@ def execute_callback(self, goal_handle): f"Distance remaining: {error_norm:.2f}, angle error: {error_yaw:.2f}" ) - # Check if target is reached (both position and yaw) + # Check if target is reached (both position and yaw) and held + now_t = self.get_clock().now().nanoseconds / 1e9 if error_norm < self.pos_tol and abs(error_yaw) < self.yaw_tol: - self.error_pos = np.array([np.inf, np.info]) - self.error_yaw = np.inf - break + if within_since is None: + within_since = now_t + if (now_t - within_since) >= self.success_hold_time: + # Mark large errors to avoid lingering metrics effects + self.error_pos = np.array([np.inf, np.inf]) + self.error_yaw = np.inf + break + else: + within_since = None time.sleep(1.0) # Publish feedback at ~1Hz diff --git a/cybership_tests/cybership_tests/mission_1.py b/cybership_tests/cybership_tests/mission_1.py old mode 100644 new mode 100755 index 7be152e..cfa963a --- a/cybership_tests/cybership_tests/mission_1.py +++ b/cybership_tests/cybership_tests/mission_1.py @@ -1,799 +1,804 @@ -#!/usr/bin/env python3 - -import rclpy -import rclpy.client -from rclpy.node import Node -from std_srvs.srv import SetBool -from topic_tools_interfaces.srv import MuxSelect, MuxAdd -from geometry_msgs.msg import PoseStamped, Twist, Wrench -from std_msgs.msg import Bool -from nav_msgs.msg import Odometry -from rclpy.action import ActionClient -from nav2_msgs.action import NavigateToPose -from tf_transformations import quaternion_from_euler -import numpy as np -from typing import Any, Dict, Optional, List -from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor -from abc import ABC, abstractmethod -import time - - -# Topic and service name constants -TOPIC_FORCE_CONSTANT: str = "control/force/command/constant" -TOPIC_VELOCITY_COMMAND: str = "control/velocity/command" -TOPIC_ODOM: str = "measurement/odom" -TOPIC_POSE_GOAL: str = "navigate_to_pose" -TOPIC_FORCE_POSITION: str = "control/force/command/position" -TOPIC_FORCE_VELOCITY: str = "control/force/command/velocity" -TOPIC_FORCE_BASE: str = "control/force/command" - -SERVICE_MUX_ADD: str = "force_mux/add" -SERVICE_MUX_SELECT: str = "force_mux/select" -SERVICE_POSE_ENABLE: str = "position_controller/change_state" -SERVICE_VEL_ENABLE: str = "velocity_controller/change_state" - -TOPIC_EXPERIMENT_ODOM: str = "experiment/odom" -TOPIC_EXPERIMENT_FORCE: str = "experiment/force" -TOPIC_EXPERIMENT_FLAG: str = "experiment/flag" - - -class ActionRunner(Node): - def __init__(self): - super().__init__("action_runner", namespace="voyager") - # Private executor for service calls - # self._executor.add_node(self) - # Define the list of mission actions - self.repeat = 500 - - self.mux_add_client = self.create_client(MuxAdd, SERVICE_MUX_ADD) - - self.mux_select_client = self.create_client( - MuxSelect, SERVICE_MUX_SELECT) - - self.pose_action_client = ActionClient( - self, NavigateToPose, TOPIC_POSE_GOAL) - - self.pose_enable_client = self.create_client( - SetBool, SERVICE_POSE_ENABLE) - - self.vel_enable_client = self.create_client( - SetBool, SERVICE_VEL_ENABLE) - - self.vel_pub = self.create_publisher(Twist, TOPIC_VELOCITY_COMMAND, 10) - self.force_pub = self.create_publisher( - Wrench, TOPIC_FORCE_CONSTANT, 10) - - self.experiment_odom_pub = self.create_publisher( - Odometry, TOPIC_EXPERIMENT_ODOM, 10 - ) - self.experiment_force_pub = self.create_publisher( - Wrench, TOPIC_EXPERIMENT_FORCE, 10 - ) - self.experiment_flag_pub = self.create_publisher( - Bool, TOPIC_EXPERIMENT_FLAG, 10 - ) - - self.current_odom = None - self.create_subscription(Odometry, TOPIC_ODOM, self.odom_callback, 10) - - self.rng = np.random.default_rng() - - # Experiment publishing state - self.experiment_enabled: bool = False - self._last_force: Optional[Wrench] = None - self.create_timer(0.05, self._experiment_publisher) - - # Default abort lands back at origin after timeout - self.default_abort_action = { - "name": "default_abort", - "action": self.run_pose_action, - "parameters": { - "x": -4.0, - "y": 0.5, - "yaw": 0.0, - "duration": 50.0, - "grace": 50.0, - }, - } - - self.actions = [ - # i want boat in same starting position each time. - { - "name": "pose_neg_diag", - "action": self.run_pose_action, - "parameters": { - "x": -4.0, - "y": 0.5, - "yaw": 0.0, - "duration": 60.0, - }, - }, - # command constant forces - # { - # "name": "command_constant_force", - # "action": self.run_force_action, - # "parameters": { - # "force_x": lambda: self.rng.uniform(0.7, 1.3), - # "force_y": lambda: self.rng.uniform(-0.0, 0.0), - # "torque_z": lambda: self.rng.uniform(-0.0, 0.0), - # "duration": 20.0, - # "experiment": True, # Enable experiment flag - # "continuous_sampling": True, # Sample force continuously - # "frequency": 0.25, # Hz - # } - # }, - # { - # "name": "pose_pos_diag", - # "action": self.run_pose_action, - # "parameters": { - # "x": 3.0, - # "y": 0.0, - # "yaw": np.pi, - # "duration": 60.0, - # }, - # }, - # command constant forces - { - "name": "command_constant_force", - "action": self.run_force_action, - "parameters": { - "force_x": lambda: self.rng.uniform(0.7, 1.2), - "force_y": lambda: self.rng.uniform(-0.1, 0.1), - "torque_z": lambda: self.rng.uniform(-0.1, 0.1), - "duration": 20.0, - "experiment": True, # Enable experiment flag - "continuous_sampling": True, # Sample force continuously - "frequency": 0.25, # Hz - } - } - # { - # "name": "pose_neg_diag", - # "action": self.run_pose_action, - # "parameters": { - # "x": -2.0, - # "y": -2.0, - # "yaw": np.pi / 4, - # "duration": 60.0, - # }, - # }, - # { - # "name": "velocity_random_1", - # "action": self.run_velocity_action, - # "parameters": { - # # Sample linear and angular components each publish - # "linear_x": lambda: self.rng.uniform(0, 0.3), - # "linear_y": lambda: self.rng.uniform(-0.1, 0.1), - # "angular_z": lambda: self.rng.uniform(-0.1, 0.1), - # "duration": 15.0, - # "margin": 0.01, # Allow some margin for early success - # }, - # "abort_action": { - # "name": "velocity_random_1_abort", - # "action": self.run_pose_action, - # "parameters": { - # "x": 2.0, - # "y": 2.0, - # "yaw": 5 * np.pi / 4, - # "duration": 60.0, - # "grace": 15.0, - # }, - # }, - # }, - # { - # "name": "force_random_1", - # "action": self.run_force_action, - # "parameters": { - # "force_x": lambda: self.rng.uniform(0.5, 1.5), - # "force_y": lambda: self.rng.uniform(-1.5, 1.5), - # "torque_z": lambda: self.rng.uniform(-1.5, 1.5), - # "duration": 20.0, - # "experiment": True, # Enable experiment flag - # "continuous_sampling": True, # Sample force continuously - # "frequency": 1.0, # Hz - # }, - # "abort_action": { - # "name": "force_random_1_abort", - # "action": self.run_pose_action, - # "parameters": { - # "x": 2.0, - # "y": 2.0, - # "yaw": 5 * np.pi / 4, - # "duration": 60.0, - # "grace": 15.0, - # }, - # }, - # }, - # { - # "name": "pose_pos_diag", - # "action": self.run_pose_action, - # "parameters": { - # "x": 2.0, - # "y": 2.0, - # "yaw": 5 * np.pi / 4, - # "duration": 60.0, - # }, - # }, - # { - # "name": "velocity_random_2", - # "action": self.run_velocity_action, - # "parameters": { - # # Sample linear and angular components each publish - # "linear_x": lambda: self.rng.uniform(0, 0.3), - # "linear_y": lambda: self.rng.uniform(-0.1, 0.1), - # "angular_z": lambda: self.rng.uniform(-0.1, 0.1), - # "duration": 15.0, - # "margin": 0.01, # Allow some margin for early success - # }, - # "abort_action": { - # "name": "velocity_random_2_abort", - # "action": self.run_pose_action, - # "parameters": { - # "x": -2.0, - # "y": -2.0, - # "yaw": np.pi / 4, - # "duration": 60.0, - # "grace": 15.0, - # }, - # }, - # }, - # { - # "name": "force_random_2", - # "action": self.run_force_action, - # "parameters": { - # "force_x": lambda: self.rng.uniform(0, 0.2), - # "force_y": lambda: self.rng.uniform(0, 0.05), - # "torque_z": lambda: self.rng.uniform(0, 0.05), - # "duration": 20.0, - # "experiment": True, # Enable experiment flag - # "continuous_sampling": True, # Sample force continuously - # "frequency": 1.0, # Hz - # }, - # "abort_action": { - # "name": "force_random_2_abort", - # "action": self.run_pose_action, - # "parameters": { - # "x": -2.0, - # "y": -2.0, - # "yaw": np.pi / 4, - # "duration": 60.0, - # "grace": 15.0, - # }, - # }, - # }, - # { - # "name": "wait", - # "action": self.run_wait_action, - # "parameters": {"duration": 10.0}, - # }, - ] - - def call_init_services(self) -> None: - # Private executor for service calls - # self._executor.add_node(self) - # Define the list of mission actions - - # Action client for pose navigation (relative to node namespace) - self.get_logger().info(f"Waiting for pose action server...") - self.pose_action_client.wait_for_server() - - self.get_logger().info(f"Waiting for pose enable server ...") - self.pose_enable_client.wait_for_service() - - self.get_logger().info(f"Waiting for vel enables server ...") - self.vel_enable_client.wait_for_service() - - self.get_logger().info(f"Waiting for Force mux server...") - self.mux_add_client.wait_for_service() - - self.get_logger().info(f"Waiting for Force mux server...") - self.mux_select_client.wait_for_service() - - self.add_mux(TOPIC_FORCE_CONSTANT) - - self.wait_until_ready() - - def wait_until_ready(self) -> None: - """ - Block until the first odometry message is received. - - Ensures the vehicle has a valid initial pose before performing actions. - """ - - while rclpy.ok() and self.current_odom is None: - self.get_logger().info("Waiting for initial odometry data...") - # process callbacks without blocking executor - rclpy.spin_once(self, timeout_sec=0.1) - time.sleep(0.5) - self.get_logger().info("Initial odometry data received.") - - def _experiment_publisher(self) -> None: - """ - Background thread publishes experiment flag, and when enabled, odometry and force data. - """ - flag_msg = Bool() - flag_msg.data = self.experiment_enabled - self.experiment_flag_pub.publish(flag_msg) - if self.experiment_enabled: - if self.current_odom: - self.experiment_odom_pub.publish(self.current_odom) - if self._last_force: - self.experiment_force_pub.publish(self._last_force) - - def add_mux(self, topic: str) -> None: - """ - Add a topic to the force multiplexer using MuxAdd service. - - Args: - topic (str): ROS topic name to add to the mux. - """ - req = MuxAdd.Request() - req.topic = topic - future = self.mux_add_client.call_async(req) - rclpy.spin_until_future_complete(self, future) - # MuxAdd returns a result with a success flag - if future.result().success: - self.get_logger().info(f"Mux added topic {topic}") - else: - self.get_logger().error(f"Failed to add mux topic {topic}") - - def abort_condition(self) -> bool: - """ - Determine if the mission should abort based on odometry limits. - - Returns: - bool: True if |x| or |y| position exceeds 2.5 meters, False otherwise. - """ - - return ( - not( - (-5.0 < self.current_odom.pose.pose.position.x < 4.0) - and (-3.0 < self.current_odom.pose.pose.position.y < 2.0) - ) - ) - - def call_mux(self, topic: str) -> None: - """ - Select the active force control topic on the mux. - - Args: - topic (str): ROS topic name to select for force commands. - """ - self.get_logger().info(f'Calling mux select for topic {topic}') - req = MuxSelect.Request() - req.topic = topic - future = self.mux_select_client.call_async(req) - rclpy.spin_until_future_complete(self, future) - if future.result(): - self.get_logger().info(f"Mux switched to {topic}") - else: - self.get_logger().error(f"Failed to switch mux to {topic}") - - def set_enable(self, client: rclpy.client.Client, enable: bool) -> None: - """ - Enable or disable a controller via SetBool service. - - Args: - client (rclpy.client.Client): Service client for SetBool. - enable (bool): True to enable, False to disable. - """ - self.get_logger().info(f"Setting {client} enable={enable}") - req = SetBool.Request() - req.data = enable - future = client.call_async(req) - rclpy.spin_until_future_complete(self, future) - if future.result().success: - self.get_logger().info(f'Service {client} set enable={enable}') - else: - self.get_logger().error(f'Failed to set {client} enable={enable}') - - def _pose_feedback_callback(self, feedback_msg: Any) -> None: - """ - Callback to log remaining distance from a NavigateToPose action. - - Args: - feedback_msg: Action feedback message containing distance_remaining (float, meters). - """ - fb = feedback_msg.feedback - # nav2 NavigateToPose feedback has distance_remaining - dist = getattr(fb, "distance_remaining", None) - if dist is not None: - self.get_logger().info( - f" NavigateToPose feedback: distance_remaining={dist:.2f}" - ) - else: - self.get_logger().info(" NavigateToPose feedback received") - - def run_pose_action(self, **params: Any) -> None: - """ - Navigate to a target pose and monitor for timeouts or abort conditions. - - Sends a NavigateToPose goal and cancels if duration elapsed or abort condition met. - - Args: - x (float): Target x-position in meters. - y (float): Target y-position in meters. - yaw (float): Target yaw angle in radians. - duration (float): Max navigation time in seconds. - grace (float): Grace period before starting abort checks in seconds. - - Raises: - RuntimeError: If abort_condition() returns True after grace period. - """ - resolved_params = {k: v() if callable( - v) else v for k, v in params.items()} - # Activate pose controller via mux & service - self.call_mux(TOPIC_FORCE_POSITION) - - self.set_enable(self.pose_enable_client, True) - - # Extract parameters - x = resolved_params.get("x", 0.0) - y = resolved_params.get("y", 0.0) - yaw = resolved_params.get("yaw", 0.0) - duration = resolved_params.get("duration", 0.0) - grace = resolved_params.get("grace", 0.0) - self.get_logger().info( - f"Pose action start (x={x:.2f}, y={y:.2f}, yaw={yaw:.2f}), " - f"duration={duration:.1f}s, grace={grace:.1f}s" - ) - - # Build goal message - msg = PoseStamped() - msg.header.stamp = self.get_clock().now().to_msg() - msg.header.frame_id = "world" - msg.pose.position.x = x - msg.pose.position.y = y - q = quaternion_from_euler(0.0, 0.0, yaw) - msg.pose.orientation.x = q[0] - msg.pose.orientation.y = q[1] - msg.pose.orientation.z = q[2] - msg.pose.orientation.w = q[3] - goal = NavigateToPose.Goal() - goal.pose = msg - - # Send goal with feedback callback - goal_fut = self.pose_action_client.send_goal_async( - goal, feedback_callback=self._pose_feedback_callback - ) - rclpy.spin_until_future_complete(self, goal_fut) - handle = goal_fut.result() - if not handle.accepted: - self.get_logger().error("NavigateToPose goal rejected") - self.set_enable(self.pose_enable_client, False) - return - - self.get_logger().info("NavigateToPose goal accepted, monitoring execution") - - # Wait for result, checking abort & timeout - result_fut = handle.get_result_async() - start = self.get_clock().now().nanoseconds / 1e9 - elapsed = 0.0 - while rclpy.ok(): - elapsed = (self.get_clock().now().nanoseconds / 1e9) - start - - if elapsed >= duration: - self.get_logger().info( - "NavigateToPose action completed due to duration" - ) - cancel_fut = handle.cancel_goal_async() - rclpy.spin_until_future_complete(self, cancel_fut) - self.set_enable(self.pose_enable_client, False) - break - - if result_fut.done(): - self.get_logger().info("NavigateToPose action completed") - break - - if elapsed >= grace and self.abort_condition(): - - cancel_fut = handle.cancel_goal_async() - rclpy.spin_until_future_complete(self, cancel_fut) - self.set_enable(self.pose_enable_client, False) - raise RuntimeError( - 'Abort condition met during pose action, aborting') - - rclpy.spin_once(self, timeout_sec=0.5) - - # Deactivate pose controller - self.set_enable(self.pose_enable_client, False) - - def run_velocity_action(self, **params: Any) -> None: - """ - Publish constant velocity commands for a given duration, with optional early stop. - - Args: - linear_x (float): Desired linear velocity in x (m/s). - linear_y (float): Desired linear velocity in y (m/s). - angular_z (float): Desired angular velocity around z (rad/s). - duration (float): Total time to publish commands (seconds). - margin (float, optional): Early stop threshold for actual vs commanded velocity (m/s). - grace (float): Grace period before starting abort checks (seconds). - - Raises: - RuntimeError: If abort_condition() returns True after grace period. - """ - resolved_params = {k: v() if callable( - v) else v for k, v in params.items()} - # Activate velocity controller - self.call_mux(TOPIC_FORCE_VELOCITY) - self.set_enable(self.vel_enable_client, True) - - # Prepare and log command - msg = Twist() - lx = resolved_params.get("linear_x", 0.0) - ly = resolved_params.get("linear_y", 0.0) - az = resolved_params.get("angular_z", 0.0) - margin = resolved_params.get("margin", None) - self.get_logger().info( - " " - f"Velocity action (u={lx:.4f}, v={ly:.4f}, r={az:.4f})" - + (f", margin={margin:.4f}" if margin is not None else "") - ) - - # Extract duration, grace period and start time - duration = resolved_params.get("duration", 0.0) - grace = resolved_params.get("grace", 0.0) - start = self.get_clock().now().nanoseconds / 1e9 - elapsed = 0.0 - # Loop at ~10 Hz, processing callbacks without blocking - period = 0.1 - - while rclpy.ok() and elapsed < duration: - elapsed = self.get_clock().now().nanoseconds / 1e9 - start - # Publish the commanded velocity - msg.linear.x = lx - msg.linear.y = ly - msg.angular.z = az - self.vel_pub.publish(msg) - - # Feedback: log current odometry state - if self.current_odom: - pos = self.current_odom.pose.pose.position - lvel = self.current_odom.twist.twist.linear - avel = self.current_odom.twist.twist.angular - self.get_logger().info( - " " - f"Velocity feedback: " - f"vel=(vx={lvel.x:.2f}, vy={lvel.y:.2f}, vr={avel.z:.2f}), " - f"cmd=(u={lx:.2f}, v={ly:.2f}, r={az:.2f})" - ) - - # margin-based early success - if margin is not None: - act = self.current_odom.twist.twist - if abs(act.linear.x - lx) < margin and abs(act.linear.y - ly) < margin: - self.get_logger().info("Velocity within margin, stopping early") - break - - # Abort check after grace period - if self.current_odom and elapsed >= grace and self.abort_condition(): - raise RuntimeError( - "Abort condition met during velocity action, aborting" - ) - time.sleep(0.1) - rclpy.spin_once(self, timeout_sec=period) - - # Deactivate velocity controller - self.set_enable(self.vel_enable_client, False) - - def run_wait_action(self, **params: Any) -> None: - """ - Wait for a specified duration while processing ROS callbacks. - - This loop periodically checks for an abort condition after an optional grace period - and logs odometry feedback during the wait. - - Args: - duration (float): Total wait time in seconds. - grace (float): Grace period before starting abort checks in seconds. - - Raises: - RuntimeError: If abort_condition() returns True after grace period. - """ - resolved_params = {k: v() if callable( - v) else v for k, v in params.items()} - duration = resolved_params.get("duration", 0.0) - grace = resolved_params.get("grace", 0.0) - start = self.get_clock().now().nanoseconds / 1e9 - - self.get_logger().info(f"Waiting for {duration:.4f} seconds") - elapsed = 0.0 - # Loop at ~2 Hz, processing callbacks without blocking - period = 0.5 - while rclpy.ok() and elapsed < duration: - elapsed = self.get_clock().now().nanoseconds / 1e9 - start - - if elapsed >= grace and self.abort_condition(): - raise RuntimeError( - "Abort condition met during wait action, aborting") - - # allow callbacks and enforce loop timing - rclpy.spin_once(self, timeout_sec=period) - - if self.current_odom: - pos = self.current_odom.pose.pose.position - self.get_logger().info( - f"Wait feedback: pos=({pos.x:.2f}, {pos.y:.2f}), " - f"elapsed={elapsed:.2f}s" - ) - - def run_force_action(self, **params: Any) -> None: - """ - Apply a constant force to the vehicle for a specified duration. - - This loop publishes a Wrench message at fixed intervals and checks for abort - conditions after an optional grace period. Odometry feedback is logged each cycle. - - Args: - force_x (float): Force in x-direction in Newtons. - force_y (float): Force in y-direction in Newtons. - torque_z (float): Torque about z-axis in Newton-meters. - duration (float): Total time to apply force in seconds. - grace (float): Grace period before starting abort checks in seconds. - - Raises: - RuntimeError: If abort_condition() returns True after grace period. - """ - - resolved_params = {k: v() if callable( - v) else v for k, v in params.items()} - - self.call_mux(TOPIC_FORCE_CONSTANT) - - msg = Wrench() - - duration = params.get("duration", 0.0) - grace = params.get("grace", 0.0) - - continuous_sampling = params.get("continuous_sampling", False) - frequency = params.get("frequency", 1.0) # Hz - - start = self.get_clock().now().nanoseconds / 1e9 - elapsed = 0.0 - - # Loop at specified frequency (or 2 Hz), processing callbacks without blocking - period = 1.0 / (frequency if continuous_sampling else 2.0) - - last_update_elapsed = 0.0 - while rclpy.ok() and elapsed < duration: - - elapsed = self.get_clock().now().nanoseconds / 1e9 - start - - # allow callbacks and enforce publish rate - rclpy.spin_once(self, timeout_sec=0.5) - - # if np.mod(elapsed, period) > 0.01: - # # Skip this iteration if not at the right frequency - # continue - - if abs(last_update_elapsed - elapsed) < period: - # Skip if not enough time has passed since last update - continue - - last_update_elapsed = elapsed - - msg.force.x = resolved_params.get("force_x", 0.0) - msg.force.y = resolved_params.get("force_y", 0.0) - msg.torque.z = resolved_params.get("torque_z", 0.0) - - if continuous_sampling: - # Sample force parameters at each iteration - msg.force.x = params.get("force_x", 0.0)() - msg.force.y = params.get("force_y", 0.0)() - msg.torque.z = params.get("torque_z", 0.0)() - - # record last force for experiment publishing - self._last_force = msg - self.force_pub.publish(msg) - - if self.current_odom: - pos = self.current_odom.pose.pose.position - self.get_logger().info( - f"Force feedback: (fx={msg.force.x:.2f}, " - f"fy={msg.force.y:.2f}, tz={msg.torque.z:.2f})," - f" pos=({pos.x:.2f}, {pos.y:.2f}), " - f"elapsed={elapsed:.2f}s" - ) - - if elapsed >= grace and self.abort_condition(): - raise RuntimeError( - "Abort condition met during force action, aborting") - - # time.sleep(period) # Sleep for the period to control frequency - - def execute_action(self, action: Dict[str, Any]) -> None: - """ - Execute one mission action and resolve its parameters. - - Normalizes callable parameters, disables other controllers, and invokes the action. - - Args: - action (dict): Action descriptor with keys 'action' (callable) and 'parameters' (dict). - """ - if not isinstance(action, dict): - self.get_logger().error(f'Invalid action format: {action}') - return - - runner = action.get('action') - params = action.get('parameters', {}) - - self.set_enable(self.pose_enable_client, False) - self.set_enable(self.vel_enable_client, False) - - if not callable(runner): - self.get_logger().warn(f'Unknown action: {runner}') - return - - self.get_logger().info(f"Executing action: {action['name']}") - - # Handle experiment flag without resolving other parameters - self.experiment_enabled = bool(params.get("experiment", False)) - runner(**params) - - # Disable experiment after action completes - self.experiment_enabled = False - - def execute_actions(self) -> None: - """ - Loop through the action sequence `repeat` times, handling aborts. - - For each action, calls execute_action and on RuntimeError runs an abort_action. - """ - print("executing actions...") - for _ in range(self.repeat): - - for action in self.actions: - - try: - self.execute_action(action) - except RuntimeError as e: - abort_action = action.get( - 'abort_action', self.default_abort_action) - self.get_logger().warn( - f'Error executing action {action["name"]}: {e}') - self.get_logger().warn( - f'Executing abort action: {abort_action["name"]}') - try: - self.execute_action(abort_action) - except RuntimeError as abort_e: - self.get_logger().error( - f'Abort action failed too!: {abort_e}') - self.set_enable(self.pose_enable_client, False) - self.set_enable(self.vel_enable_client, False) - self.call_mux(TOPIC_FORCE_BASE) - exit(1) - - def odom_callback(self, msg): - """ - Callback for Odometry subscription; stores the latest message. - - Args: - msg (Odometry): Received odometry message containing pose and twist. - """ - self.current_odom = msg - - # if self.experiment_enabled: - # # Publish odometry for experiment if enabled - # self.experiment_odom_pub.publish(msg) - # self.experiment_flag_pub.publish(Bool(data=True)) - # self.experiment_force_pub.publish(self._last_force) - - -def main(args: Optional[List[str]] = None) -> None: - - rclpy.init(args=args) - node = ActionRunner() - - node.call_init_services() - node.wait_until_ready() - - executor = MultiThreadedExecutor() - executor.add_node(node) - - node.execute_actions() - executor.spin() - executor.shutdown() - - -if __name__ == "__main__": - main() +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + + +import rclpy +import rclpy.client +from rclpy.node import Node +from std_srvs.srv import SetBool +from topic_tools_interfaces.srv import MuxSelect, MuxAdd +from geometry_msgs.msg import PoseStamped, Twist, Wrench +from std_msgs.msg import Bool +from nav_msgs.msg import Odometry +from rclpy.action import ActionClient +from nav2_msgs.action import NavigateToPose +from tf_transformations import quaternion_from_euler +import numpy as np +from typing import Any, Dict, Optional, List +from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor +from abc import ABC, abstractmethod +import time + + +# Topic and service name constants +TOPIC_FORCE_CONSTANT: str = "control/force/command/constant" +TOPIC_VELOCITY_COMMAND: str = "control/velocity/command" +TOPIC_ODOM: str = "measurement/odom" +TOPIC_POSE_GOAL: str = "navigate_to_pose" +TOPIC_FORCE_POSITION: str = "control/force/command/position" +TOPIC_FORCE_VELOCITY: str = "control/force/command/velocity" +TOPIC_FORCE_BASE: str = "control/force/command" + +SERVICE_MUX_ADD: str = "force_mux/add" +SERVICE_MUX_SELECT: str = "force_mux/select" +SERVICE_POSE_ENABLE: str = "position_controller/change_state" +SERVICE_VEL_ENABLE: str = "velocity_controller/change_state" + +TOPIC_EXPERIMENT_ODOM: str = "experiment/odom" +TOPIC_EXPERIMENT_FORCE: str = "experiment/force" +TOPIC_EXPERIMENT_FLAG: str = "experiment/flag" + + +class ActionRunner(Node): + def __init__(self): + super().__init__("action_runner", namespace="voyager") + # Private executor for service calls + # self._executor.add_node(self) + # Define the list of mission actions + self.repeat = 500 + + self.mux_add_client = self.create_client(MuxAdd, SERVICE_MUX_ADD) + + self.mux_select_client = self.create_client( + MuxSelect, SERVICE_MUX_SELECT) + + self.pose_action_client = ActionClient( + self, NavigateToPose, TOPIC_POSE_GOAL) + + self.pose_enable_client = self.create_client( + SetBool, SERVICE_POSE_ENABLE) + + self.vel_enable_client = self.create_client( + SetBool, SERVICE_VEL_ENABLE) + + self.vel_pub = self.create_publisher(Twist, TOPIC_VELOCITY_COMMAND, 10) + self.force_pub = self.create_publisher( + Wrench, TOPIC_FORCE_CONSTANT, 10) + + self.experiment_odom_pub = self.create_publisher( + Odometry, TOPIC_EXPERIMENT_ODOM, 10 + ) + self.experiment_force_pub = self.create_publisher( + Wrench, TOPIC_EXPERIMENT_FORCE, 10 + ) + self.experiment_flag_pub = self.create_publisher( + Bool, TOPIC_EXPERIMENT_FLAG, 10 + ) + + self.current_odom = None + self.create_subscription(Odometry, TOPIC_ODOM, self.odom_callback, 10) + + self.rng = np.random.default_rng() + + # Experiment publishing state + self.experiment_enabled: bool = False + self._last_force: Optional[Wrench] = None + self.create_timer(0.05, self._experiment_publisher) + + # Default abort lands back at origin after timeout + self.default_abort_action = { + "name": "default_abort", + "action": self.run_pose_action, + "parameters": { + "x": -2.0, + "y": 0.0, + "yaw": 0.0, + "duration": 50.0, + "grace": 50.0, + }, + } + + self.actions = [ + # i want boat in same starting position each time. + { + "name": "pose_neg_diag", + "action": self.run_pose_action, + "parameters": { + "x": -3.0, + "y": 0.5, + "yaw": 0.0, + "duration": 60.0, + }, + }, + # command constant forces + # { + # "name": "command_constant_force", + # "action": self.run_force_action, + # "parameters": { + # "force_x": lambda: self.rng.uniform(0.7, 1.3), + # "force_y": lambda: self.rng.uniform(-0.0, 0.0), + # "torque_z": lambda: self.rng.uniform(-0.0, 0.0), + # "duration": 20.0, + # "experiment": True, # Enable experiment flag + # "continuous_sampling": True, # Sample force continuously + # "frequency": 0.25, # Hz + # } + # }, + # { + # "name": "pose_pos_diag", + # "action": self.run_pose_action, + # "parameters": { + # "x": 3.0, + # "y": 0.0, + # "yaw": np.pi, + # "duration": 60.0, + # }, + # }, + # command constant forces + { + "name": "command_constant_force", + "action": self.run_force_action, + "parameters": { + "force_x": lambda: self.rng.uniform(0.7, 1.0), + "force_y": lambda: self.rng.uniform(-0.05, 0.05), + "torque_z": lambda: self.rng.uniform(-0.05, 0.05), + "duration": 20.0, + "experiment": True, # Enable experiment flag + "continuous_sampling": False, # Sample force continuously + "frequency": 0.25, # Hz + } + } + # { + # "name": "pose_neg_diag", + # "action": self.run_pose_action, + # "parameters": { + # "x": -2.0, + # "y": -2.0, + # "yaw": np.pi / 4, + # "duration": 60.0, + # }, + # }, + # { + # "name": "velocity_random_1", + # "action": self.run_velocity_action, + # "parameters": { + # # Sample linear and angular components each publish + # "linear_x": lambda: self.rng.uniform(0, 0.3), + # "linear_y": lambda: self.rng.uniform(-0.1, 0.1), + # "angular_z": lambda: self.rng.uniform(-0.1, 0.1), + # "duration": 15.0, + # "margin": 0.01, # Allow some margin for early success + # }, + # "abort_action": { + # "name": "velocity_random_1_abort", + # "action": self.run_pose_action, + # "parameters": { + # "x": 2.0, + # "y": 2.0, + # "yaw": 5 * np.pi / 4, + # "duration": 60.0, + # "grace": 15.0, + # }, + # }, + # }, + # { + # "name": "force_random_1", + # "action": self.run_force_action, + # "parameters": { + # "force_x": lambda: self.rng.uniform(0.5, 1.5), + # "force_y": lambda: self.rng.uniform(-1.5, 1.5), + # "torque_z": lambda: self.rng.uniform(-1.5, 1.5), + # "duration": 20.0, + # "experiment": True, # Enable experiment flag + # "continuous_sampling": True, # Sample force continuously + # "frequency": 1.0, # Hz + # }, + # "abort_action": { + # "name": "force_random_1_abort", + # "action": self.run_pose_action, + # "parameters": { + # "x": 2.0, + # "y": 2.0, + # "yaw": 5 * np.pi / 4, + # "duration": 60.0, + # "grace": 15.0, + # }, + # }, + # }, + # { + # "name": "pose_pos_diag", + # "action": self.run_pose_action, + # "parameters": { + # "x": 2.0, + # "y": 2.0, + # "yaw": 5 * np.pi / 4, + # "duration": 60.0, + # }, + # }, + # { + # "name": "velocity_random_2", + # "action": self.run_velocity_action, + # "parameters": { + # # Sample linear and angular components each publish + # "linear_x": lambda: self.rng.uniform(0, 0.3), + # "linear_y": lambda: self.rng.uniform(-0.1, 0.1), + # "angular_z": lambda: self.rng.uniform(-0.1, 0.1), + # "duration": 15.0, + # "margin": 0.01, # Allow some margin for early success + # }, + # "abort_action": { + # "name": "velocity_random_2_abort", + # "action": self.run_pose_action, + # "parameters": { + # "x": -2.0, + # "y": -2.0, + # "yaw": np.pi / 4, + # "duration": 60.0, + # "grace": 15.0, + # }, + # }, + # }, + # { + # "name": "force_random_2", + # "action": self.run_force_action, + # "parameters": { + # "force_x": lambda: self.rng.uniform(0, 0.2), + # "force_y": lambda: self.rng.uniform(0, 0.05), + # "torque_z": lambda: self.rng.uniform(0, 0.05), + # "duration": 20.0, + # "experiment": True, # Enable experiment flag + # "continuous_sampling": True, # Sample force continuously + # "frequency": 1.0, # Hz + # }, + # "abort_action": { + # "name": "force_random_2_abort", + # "action": self.run_pose_action, + # "parameters": { + # "x": -2.0, + # "y": -2.0, + # "yaw": np.pi / 4, + # "duration": 60.0, + # "grace": 15.0, + # }, + # }, + # }, + # { + # "name": "wait", + # "action": self.run_wait_action, + # "parameters": {"duration": 10.0}, + # }, + ] + + def call_init_services(self) -> None: + # Private executor for service calls + # self._executor.add_node(self) + # Define the list of mission actions + + # Action client for pose navigation (relative to node namespace) + self.get_logger().info(f"Waiting for pose action server...") + self.pose_action_client.wait_for_server() + + self.get_logger().info(f"Waiting for pose enable server ...") + self.pose_enable_client.wait_for_service() + + self.get_logger().info(f"Waiting for vel enables server ...") + self.vel_enable_client.wait_for_service() + + self.get_logger().info(f"Waiting for Force mux server...") + self.mux_add_client.wait_for_service() + + self.get_logger().info(f"Waiting for Force mux server...") + self.mux_select_client.wait_for_service() + + self.add_mux(TOPIC_FORCE_CONSTANT) + + self.wait_until_ready() + + def wait_until_ready(self) -> None: + """ + Block until the first odometry message is received. + + Ensures the vehicle has a valid initial pose before performing actions. + """ + + while rclpy.ok() and self.current_odom is None: + self.get_logger().info("Waiting for initial odometry data...") + # process callbacks without blocking executor + rclpy.spin_once(self, timeout_sec=0.1) + time.sleep(0.5) + self.get_logger().info("Initial odometry data received.") + + def _experiment_publisher(self) -> None: + """ + Background thread publishes experiment flag, and when enabled, odometry and force data. + """ + flag_msg = Bool() + flag_msg.data = self.experiment_enabled + self.experiment_flag_pub.publish(flag_msg) + if self.experiment_enabled: + if self.current_odom: + self.experiment_odom_pub.publish(self.current_odom) + if self._last_force: + self.experiment_force_pub.publish(self._last_force) + + def add_mux(self, topic: str) -> None: + """ + Add a topic to the force multiplexer using MuxAdd service. + + Args: + topic (str): ROS topic name to add to the mux. + """ + req = MuxAdd.Request() + req.topic = topic + future = self.mux_add_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + # MuxAdd returns a result with a success flag + if future.result().success: + self.get_logger().info(f"Mux added topic {topic}") + else: + self.get_logger().error(f"Failed to add mux topic {topic}") + + def abort_condition(self) -> bool: + """ + Determine if the mission should abort based on odometry limits. + + Returns: + bool: True if |x| or |y| position exceeds 2.5 meters, False otherwise. + """ + + return ( + not( + (-5.0 < self.current_odom.pose.pose.position.x < 4.0) + and (-3.0 < self.current_odom.pose.pose.position.y < 2.0) + ) + ) + + def call_mux(self, topic: str) -> None: + """ + Select the active force control topic on the mux. + + Args: + topic (str): ROS topic name to select for force commands. + """ + self.get_logger().info(f'Calling mux select for topic {topic}') + req = MuxSelect.Request() + req.topic = topic + future = self.mux_select_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result(): + self.get_logger().info(f"Mux switched to {topic}") + else: + self.get_logger().error(f"Failed to switch mux to {topic}") + + def set_enable(self, client: rclpy.client.Client, enable: bool) -> None: + """ + Enable or disable a controller via SetBool service. + + Args: + client (rclpy.client.Client): Service client for SetBool. + enable (bool): True to enable, False to disable. + """ + self.get_logger().info(f"Setting {client} enable={enable}") + req = SetBool.Request() + req.data = enable + future = client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result().success: + self.get_logger().info(f'Service {client} set enable={enable}') + else: + self.get_logger().error(f'Failed to set {client} enable={enable}') + + def _pose_feedback_callback(self, feedback_msg: Any) -> None: + """ + Callback to log remaining distance from a NavigateToPose action. + + Args: + feedback_msg: Action feedback message containing distance_remaining (float, meters). + """ + fb = feedback_msg.feedback + # nav2 NavigateToPose feedback has distance_remaining + dist = getattr(fb, "distance_remaining", None) + if dist is not None: + self.get_logger().info( + f" NavigateToPose feedback: distance_remaining={dist:.2f}" + ) + else: + self.get_logger().info(" NavigateToPose feedback received") + + def run_pose_action(self, **params: Any) -> None: + """ + Navigate to a target pose and monitor for timeouts or abort conditions. + + Sends a NavigateToPose goal and cancels if duration elapsed or abort condition met. + + Args: + x (float): Target x-position in meters. + y (float): Target y-position in meters. + yaw (float): Target yaw angle in radians. + duration (float): Max navigation time in seconds. + grace (float): Grace period before starting abort checks in seconds. + + Raises: + RuntimeError: If abort_condition() returns True after grace period. + """ + resolved_params = {k: v() if callable( + v) else v for k, v in params.items()} + # Activate pose controller via mux & service + self.call_mux(TOPIC_FORCE_POSITION) + + self.set_enable(self.pose_enable_client, True) + + # Extract parameters + x = resolved_params.get("x", 0.0) + y = resolved_params.get("y", 0.0) + yaw = resolved_params.get("yaw", 0.0) + duration = resolved_params.get("duration", 0.0) + grace = resolved_params.get("grace", 0.0) + self.get_logger().info( + f"Pose action start (x={x:.2f}, y={y:.2f}, yaw={yaw:.2f}), " + f"duration={duration:.1f}s, grace={grace:.1f}s" + ) + + # Build goal message + msg = PoseStamped() + msg.header.stamp = self.get_clock().now().to_msg() + # Use the same frame as odometry to avoid frame mismatch in the server's + # distance_remaining calculation. If your stack uses TF, you can send in + # any frame and transform on the server side instead. + msg.header.frame_id = "odom" + msg.pose.position.x = x + msg.pose.position.y = y + q = quaternion_from_euler(0.0, 0.0, yaw) + msg.pose.orientation.x = q[0] + msg.pose.orientation.y = q[1] + msg.pose.orientation.z = q[2] + msg.pose.orientation.w = q[3] + goal = NavigateToPose.Goal() + goal.pose = msg + + # Send goal with feedback callback + goal_fut = self.pose_action_client.send_goal_async( + goal, feedback_callback=self._pose_feedback_callback + ) + rclpy.spin_until_future_complete(self, goal_fut) + handle = goal_fut.result() + if not handle.accepted: + self.get_logger().error("NavigateToPose goal rejected") + self.set_enable(self.pose_enable_client, False) + return + + self.get_logger().info("NavigateToPose goal accepted, monitoring execution") + + # Wait for result, checking abort & timeout + result_fut = handle.get_result_async() + start = self.get_clock().now().nanoseconds / 1e9 + elapsed = 0.0 + while rclpy.ok(): + elapsed = (self.get_clock().now().nanoseconds / 1e9) - start + + if elapsed >= duration: + self.get_logger().info( + "NavigateToPose action completed due to duration" + ) + cancel_fut = handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, cancel_fut) + self.set_enable(self.pose_enable_client, False) + break + + if result_fut.done(): + self.get_logger().info("NavigateToPose action completed") + break + + if elapsed >= grace and self.abort_condition(): + + cancel_fut = handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, cancel_fut) + self.set_enable(self.pose_enable_client, False) + raise RuntimeError( + 'Abort condition met during pose action, aborting') + + rclpy.spin_once(self, timeout_sec=0.5) + + # Deactivate pose controller + self.set_enable(self.pose_enable_client, False) + + def run_velocity_action(self, **params: Any) -> None: + """ + Publish constant velocity commands for a given duration, with optional early stop. + + Args: + linear_x (float): Desired linear velocity in x (m/s). + linear_y (float): Desired linear velocity in y (m/s). + angular_z (float): Desired angular velocity around z (rad/s). + duration (float): Total time to publish commands (seconds). + margin (float, optional): Early stop threshold for actual vs commanded velocity (m/s). + grace (float): Grace period before starting abort checks (seconds). + + Raises: + RuntimeError: If abort_condition() returns True after grace period. + """ + resolved_params = {k: v() if callable( + v) else v for k, v in params.items()} + # Activate velocity controller + self.call_mux(TOPIC_FORCE_VELOCITY) + self.set_enable(self.vel_enable_client, True) + + # Prepare and log command + msg = Twist() + lx = resolved_params.get("linear_x", 0.0) + ly = resolved_params.get("linear_y", 0.0) + az = resolved_params.get("angular_z", 0.0) + margin = resolved_params.get("margin", None) + self.get_logger().info( + " " + f"Velocity action (u={lx:.4f}, v={ly:.4f}, r={az:.4f})" + + (f", margin={margin:.4f}" if margin is not None else "") + ) + + # Extract duration, grace period and start time + duration = resolved_params.get("duration", 0.0) + grace = resolved_params.get("grace", 0.0) + start = self.get_clock().now().nanoseconds / 1e9 + elapsed = 0.0 + # Loop at ~10 Hz, processing callbacks without blocking + period = 0.1 + + while rclpy.ok() and elapsed < duration: + elapsed = self.get_clock().now().nanoseconds / 1e9 - start + # Publish the commanded velocity + msg.linear.x = lx + msg.linear.y = ly + msg.angular.z = az + self.vel_pub.publish(msg) + + # Feedback: log current odometry state + if self.current_odom: + pos = self.current_odom.pose.pose.position + lvel = self.current_odom.twist.twist.linear + avel = self.current_odom.twist.twist.angular + self.get_logger().info( + " " + f"Velocity feedback: " + f"vel=(vx={lvel.x:.2f}, vy={lvel.y:.2f}, vr={avel.z:.2f}), " + f"cmd=(u={lx:.2f}, v={ly:.2f}, r={az:.2f})" + ) + + # margin-based early success + if margin is not None: + act = self.current_odom.twist.twist + if abs(act.linear.x - lx) < margin and abs(act.linear.y - ly) < margin: + self.get_logger().info("Velocity within margin, stopping early") + break + + # Abort check after grace period + if self.current_odom and elapsed >= grace and self.abort_condition(): + raise RuntimeError( + "Abort condition met during velocity action, aborting" + ) + time.sleep(0.1) + rclpy.spin_once(self, timeout_sec=period) + + # Deactivate velocity controller + self.set_enable(self.vel_enable_client, False) + + def run_wait_action(self, **params: Any) -> None: + """ + Wait for a specified duration while processing ROS callbacks. + + This loop periodically checks for an abort condition after an optional grace period + and logs odometry feedback during the wait. + + Args: + duration (float): Total wait time in seconds. + grace (float): Grace period before starting abort checks in seconds. + + Raises: + RuntimeError: If abort_condition() returns True after grace period. + """ + resolved_params = {k: v() if callable( + v) else v for k, v in params.items()} + duration = resolved_params.get("duration", 0.0) + grace = resolved_params.get("grace", 0.0) + start = self.get_clock().now().nanoseconds / 1e9 + + self.get_logger().info(f"Waiting for {duration:.4f} seconds") + elapsed = 0.0 + # Loop at ~2 Hz, processing callbacks without blocking + period = 0.5 + while rclpy.ok() and elapsed < duration: + elapsed = self.get_clock().now().nanoseconds / 1e9 - start + + if elapsed >= grace and self.abort_condition(): + raise RuntimeError( + "Abort condition met during wait action, aborting") + + # allow callbacks and enforce loop timing + rclpy.spin_once(self, timeout_sec=period) + + if self.current_odom: + pos = self.current_odom.pose.pose.position + self.get_logger().info( + f"Wait feedback: pos=({pos.x:.2f}, {pos.y:.2f}), " + f"elapsed={elapsed:.2f}s" + ) + + def run_force_action(self, **params: Any) -> None: + """ + Apply a constant force to the vehicle for a specified duration. + + This loop publishes a Wrench message at fixed intervals and checks for abort + conditions after an optional grace period. Odometry feedback is logged each cycle. + + Args: + force_x (float): Force in x-direction in Newtons. + force_y (float): Force in y-direction in Newtons. + torque_z (float): Torque about z-axis in Newton-meters. + duration (float): Total time to apply force in seconds. + grace (float): Grace period before starting abort checks in seconds. + + Raises: + RuntimeError: If abort_condition() returns True after grace period. + """ + + resolved_params = {k: v() if callable( + v) else v for k, v in params.items()} + + self.call_mux(TOPIC_FORCE_CONSTANT) + + msg = Wrench() + + duration = params.get("duration", 0.0) + grace = params.get("grace", 0.0) + + continuous_sampling = params.get("continuous_sampling", False) + frequency = params.get("frequency", 1.0) # Hz + + start = self.get_clock().now().nanoseconds / 1e9 + elapsed = 0.0 + + # Loop at specified frequency (or 2 Hz), processing callbacks without blocking + period = 1.0 / (frequency if continuous_sampling else 2.0) + + last_update_elapsed = 0.0 + while rclpy.ok() and elapsed < duration: + + elapsed = self.get_clock().now().nanoseconds / 1e9 - start + + # allow callbacks and enforce publish rate + rclpy.spin_once(self, timeout_sec=0.5) + + # if np.mod(elapsed, period) > 0.01: + # # Skip this iteration if not at the right frequency + # continue + + if abs(last_update_elapsed - elapsed) < period: + # Skip if not enough time has passed since last update + continue + + last_update_elapsed = elapsed + + msg.force.x = resolved_params.get("force_x", 0.0) + msg.force.y = resolved_params.get("force_y", 0.0) + msg.torque.z = resolved_params.get("torque_z", 0.0) + + if continuous_sampling: + # Sample force parameters at each iteration + msg.force.x = params.get("force_x", 0.0)() + msg.force.y = params.get("force_y", 0.0)() + msg.torque.z = params.get("torque_z", 0.0)() + + # record last force for experiment publishing + self._last_force = msg + self.force_pub.publish(msg) + + if self.current_odom: + pos = self.current_odom.pose.pose.position + self.get_logger().info( + f"Force feedback: (fx={msg.force.x:.2f}, " + f"fy={msg.force.y:.2f}, tz={msg.torque.z:.2f})," + f" pos=({pos.x:.2f}, {pos.y:.2f}), " + f"elapsed={elapsed:.2f}s" + ) + + if elapsed >= grace and self.abort_condition(): + raise RuntimeError( + "Abort condition met during force action, aborting") + + # time.sleep(period) # Sleep for the period to control frequency + + def execute_action(self, action: Dict[str, Any]) -> None: + """ + Execute one mission action and resolve its parameters. + + Normalizes callable parameters, disables other controllers, and invokes the action. + + Args: + action (dict): Action descriptor with keys 'action' (callable) and 'parameters' (dict). + """ + if not isinstance(action, dict): + self.get_logger().error(f'Invalid action format: {action}') + return + + runner = action.get('action') + params = action.get('parameters', {}) + + self.set_enable(self.pose_enable_client, False) + self.set_enable(self.vel_enable_client, False) + + if not callable(runner): + self.get_logger().warn(f'Unknown action: {runner}') + return + + self.get_logger().info(f"Executing action: {action['name']}") + + # Handle experiment flag without resolving other parameters + self.experiment_enabled = bool(params.get("experiment", False)) + runner(**params) + + # Disable experiment after action completes + self.experiment_enabled = False + + def execute_actions(self) -> None: + """ + Loop through the action sequence `repeat` times, handling aborts. + + For each action, calls execute_action and on RuntimeError runs an abort_action. + """ + print("executing actions...") + for _ in range(self.repeat): + + for action in self.actions: + + try: + self.execute_action(action) + except RuntimeError as e: + abort_action = action.get( + 'abort_action', self.default_abort_action) + self.get_logger().warn( + f'Error executing action {action["name"]}: {e}') + self.get_logger().warn( + f'Executing abort action: {abort_action["name"]}') + try: + self.execute_action(abort_action) + except RuntimeError as abort_e: + self.get_logger().error( + f'Abort action failed too!: {abort_e}') + self.set_enable(self.pose_enable_client, False) + self.set_enable(self.vel_enable_client, False) + self.call_mux(TOPIC_FORCE_BASE) + exit(1) + + def odom_callback(self, msg): + """ + Callback for Odometry subscription; stores the latest message. + + Args: + msg (Odometry): Received odometry message containing pose and twist. + """ + self.current_odom = msg + + # if self.experiment_enabled: + # # Publish odometry for experiment if enabled + # self.experiment_odom_pub.publish(msg) + # self.experiment_flag_pub.publish(Bool(data=True)) + # self.experiment_force_pub.publish(self._last_force) + + +def main(args: Optional[List[str]] = None) -> None: + + rclpy.init(args=args) + node = ActionRunner() + + node.call_init_services() + node.wait_until_ready() + + executor = MultiThreadedExecutor() + executor.add_node(node) + + node.execute_actions() + executor.spin() + executor.shutdown() + + +if __name__ == "__main__": + main() From 6f550c24410e7d67c90aa489ae76162f5952123e Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Sun, 24 Aug 2025 08:52:57 +0200 Subject: [PATCH 28/46] Add new ROS distributions to build matrix --- .github/workflows/build-and-push.yml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.github/workflows/build-and-push.yml b/.github/workflows/build-and-push.yml index adf8852..4244eb8 100644 --- a/.github/workflows/build-and-push.yml +++ b/.github/workflows/build-and-push.yml @@ -15,7 +15,9 @@ jobs: configurations: - ros_distro: 'humble' - ros_distro: 'jazzy' - + - ros_distro: 'iron' + - ros_distro: 'galactic' + runs-on: ubuntu-latest steps: From 8419b55bd99feee177e9323f00478b22a405f0a7 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Sun, 24 Aug 2025 17:01:42 +0200 Subject: [PATCH 29/46] fix: update line of sight --- .../nodes/los_guidance_client.py | 15 +- .../nodes/los_guidance_server.py | 93 +++++++-- .../src/cybership_controller/guidance/los.py | 189 ++++++++++++------ .../action/LOSGuidance.action | 1 + cybership_tests/CMakeLists.txt | 7 - 5 files changed, 209 insertions(+), 96 deletions(-) diff --git a/cybership_controller/nodes/los_guidance_client.py b/cybership_controller/nodes/los_guidance_client.py index 088853c..f6ec9c9 100755 --- a/cybership_controller/nodes/los_guidance_client.py +++ b/cybership_controller/nodes/los_guidance_client.py @@ -73,19 +73,10 @@ def main(args=None): example_waypoints = [ (0.0, 0.0), (6.0, 0.0), - (6.0, 1.0), - (0.0, 1.0), - (0.0, 2.0), - (6.0, 2.0), - (6.0, 3.0), - (0.0, 3.0), - (0.0, 4.0), - (6.0, 4.0), - (6.0, 5.0), - (0.0, 5.0), + (6.0, 6.0), (0.0, 6.0), - (6.0, 6.0) - ] * 2 + (0.0, 0.0), + ] client.send_goal(example_waypoints) # Use MultiThreadedExecutor to handle feedback continuously executor = MultiThreadedExecutor() diff --git a/cybership_controller/nodes/los_guidance_server.py b/cybership_controller/nodes/los_guidance_server.py index 239aa8f..64571f5 100755 --- a/cybership_controller/nodes/los_guidance_server.py +++ b/cybership_controller/nodes/los_guidance_server.py @@ -27,9 +27,9 @@ def __init__(self): super().__init__('los_guidance_server', namespace="cybership") # Parameters self.declare_parameter('desired_speed', 0.3) - self.declare_parameter('lookahead', 1.0) + self.declare_parameter('lookahead', 0.4) # Heading control gain - self.declare_parameter('heading_gain', 2.0) + self.declare_parameter('heading_gain', 4.0) # Publishers and Subscribers self._cmd_pub = self.create_publisher(Twist, 'control/velocity/command', 10) # Publish markers with transient local durability so RViZ receives past markers @@ -46,6 +46,8 @@ def __init__(self): execute_callback=self.execute_callback, goal_callback=self.goal_callback, cancel_callback=self.cancel_callback) + self.current_goal = None + self.get_logger().info('LOS Guidance Action Server started') def odom_callback(self, msg: Odometry): @@ -57,14 +59,21 @@ def odom_callback(self, msg: Odometry): _, _, yaw = euler_from_quaternion(quat) self._yaw = yaw - def goal_callback(self, goal_request): - # Accept all goals + def goal_callback(self, goal_request: LOSGuidance.Goal) -> GoalResponse: + if len(goal_request.path.poses) < 2: + self.get_logger().warn("Path has fewer than 2 poses — rejecting goal.") + return GoalResponse.REJECT + if self.current_goal is not None: + self.get_logger().info("Cancelling previous active goal.") + self.current_goal.abort() return GoalResponse.ACCEPT def cancel_callback(self, goal_handle): return CancelResponse.ACCEPT def execute_callback(self, goal_handle): + self.current_goal = goal_handle + self.get_logger().info("Goal accepted: starting LOS guidance.") # Extract waypoints from Path and include current vehicle position as initial point path_msg: Path = goal_handle.request.path # Build waypoints list, prepending vehicle's initial position if available @@ -75,7 +84,28 @@ def execute_callback(self, goal_handle): waypoints.extend([(pose.pose.position.x, pose.pose.position.y) for pose in path_msg.poses]) + + # add intermediate points + n_intermediate = 2 # Number of intermediate points between each waypoint + extended_waypoints = [] + for i in range(len(waypoints) - 1): + p1 = waypoints[i] + p2 = waypoints[i + 1] + extended_waypoints.append(p1) + # Add intermediate points + for j in range(1, n_intermediate + 1): + mid_point = ( + p1[0] + (p2[0] - p1[0]) * j / (n_intermediate + 1), + p1[1] + (p2[1] - p1[1]) * j / (n_intermediate + 1) + ) + extended_waypoints.append(mid_point) + + extended_waypoints.append(waypoints[-1]) + waypoints = np.array(extended_waypoints) + # Initialize guidance + hz = 10 + rate = self.create_rate(hz) V_d = float(self.get_parameter('desired_speed').value) delta = float(self.get_parameter('lookahead').value) # Heading control gain @@ -84,20 +114,20 @@ def execute_callback(self, goal_handle): last_wp = waypoints[-1] # Visualize spline path self.publish_spline_marker(guidance) - # Define rate for loop (10 Hz) - rate = self.create_rate(10) - # Publish path marker + dt = 1.0 / hz + canceled = False while rclpy.ok(): if goal_handle.is_cancel_requested: goal_handle.canceled() - return LOSGuidance.Result(success=False) + canceled = True + break if self._position is None or self._yaw is None: rate.sleep() continue x, y = self._position - chi_d, vel_cmd = guidance.guidance(x, y) + chi_d, vel_cmd, extras = guidance.guidance(x, y, dt=dt) twist = Twist() # Compute forward velocity in body frame using current heading v_forward = vel_cmd[0] * math.cos(self._yaw) + vel_cmd[1] * math.sin(self._yaw) @@ -116,14 +146,20 @@ def execute_callback(self, goal_handle): goal_handle.publish_feedback(feedback) self.publish_path_marker(path_msg) self.publish_arrow_marker(x, y, vel_cmd) + # Look-ahead visualization + # x_la, y_la, _, _ = guidance.lookahead_point(x, y) + self.publish_lookahead_marker(extras["lookahead_point"][0], extras["lookahead_point"][1]) + # self.publish_lookahead_line(x, y, x_la, y_la) # Check completion if math.hypot(x - last_wp[0], y - last_wp[1]) < delta: break rate.sleep() - goal_handle.succeed() - result = LOSGuidance.Result() - return result + if not canceled: + goal_handle.succeed() + self.get_logger().info("LOS guidance finished (result empty).") + self.current_goal = None + return LOSGuidance.Result() def publish_path_marker(self, path_msg: Path): marker = Marker() @@ -170,11 +206,42 @@ def publish_spline_marker(self, guidance): marker.scale.x = 0.05 marker.color.r = 0.0; marker.color.g = 0.0; marker.color.b = 1.0; marker.color.a = 1.0 # Sample spline path points - ts = np.linspace(0, 1, 100) + ts = np.linspace(0, 1, 1000) spline_pts = np.array(splev(ts, guidance.tck)).T marker.points = [Point(x=pt[0], y=pt[1], z=0.0) for pt in spline_pts] self._marker_pub.publish(marker) + def publish_lookahead_marker(self, x_la, y_la): + m = Marker() + m.header.frame_id = 'world' + m.header.stamp = self.get_clock().now().to_msg() + m.ns = 'los_lookahead' + m.id = 3 + m.type = Marker.SPHERE + m.action = Marker.ADD + m.scale.x = 0.25 # diameter in meters + m.scale.y = 0.25 + m.scale.z = 0.25 + m.color.r = 1.0; m.color.g = 0.85; m.color.b = 0.0; m.color.a = 1.0 # golden/yellow + m.pose.position.x = float(x_la) + m.pose.position.y = float(y_la) + m.pose.position.z = 0.0 + self._marker_pub.publish(m) + + def publish_lookahead_line(self, x, y, x_la, y_la): + m = Marker() + m.header.frame_id = 'world' + m.header.stamp = self.get_clock().now().to_msg() + m.ns = 'los_lookahead' + m.id = 4 + m.type = Marker.LINE_STRIP + m.action = Marker.ADD + m.scale.x = 0.03 + m.color.r = 1.0; m.color.g = 0.85; m.color.b = 0.0; m.color.a = 0.9 + start = Point(x=float(x), y=float(y), z=0.0) + end = Point(x=float(x_la), y=float(y_la), z=0.0) + m.points = [start, end] + self._marker_pub.publish(m) def main(args=None): rclpy.init(args=args) diff --git a/cybership_controller/src/cybership_controller/guidance/los.py b/cybership_controller/src/cybership_controller/guidance/los.py index e60496f..2878ddc 100644 --- a/cybership_controller/src/cybership_controller/guidance/los.py +++ b/cybership_controller/src/cybership_controller/guidance/los.py @@ -3,84 +3,145 @@ from scipy.optimize import minimize_scalar class LOSGuidance: - def __init__(self, pts, V_d, delta): + def __init__(self, pts, V_d, delta, window_len=None, mu=0.0): """ - Initialize the LOS guidance module. - - # TODO: This works as long as the waypoints are far apart. Path - # parameterization should be added between waypoints to ensure - # optimum doesn't jump between segments. - - :param pts: array-like of shape (N, 2), waypoints in NED coordinates - :param V_d: desired speed (m/s) - :param delta: look-ahead distance (m) + pts: (N,2) waypoints (NED) + V_d: desired speed [m/s] + delta: look-ahead distance [m] (arc-length) + window_len: local search window [m] around s_prev (default: 4*delta) + mu: small gradient gain for along-track stabilization [0..0.5], 0 to disable """ pts = np.asarray(pts) - - # Add three intermediate points between each consecutive waypoint pair - expanded_pts = [pts[0]] # Start with first point - for i in range(len(pts) - 1): - start_pt = pts[i] - end_pt = pts[i + 1] - # Add 3 intermediate points - for j in range(1, 4): - alpha = j / 4.0 - intermediate_pt = start_pt + alpha * (end_pt - start_pt) - expanded_pts.append(intermediate_pt) - expanded_pts.append(end_pt) # Add the end point - - pts = np.array(expanded_pts) - - # Fit cubic B-spline to the waypoints - self.tck, _ = splprep([pts[:, 0], pts[:, 1]], s=0, k=3) - self.V_d = V_d - self.delta = delta - - # Approximate total path length - samples = np.array(splev(np.linspace(0, 1, 500), self.tck)).T - diffs = np.diff(samples, axis=0) - self.path_len = np.sum(np.hypot(diffs[:, 0], diffs[:, 1])) - - def _project(self, x, y): + self.tck, self.u_wp = splprep([pts[:,0], pts[:,1]], s=0, k=min(3, len(pts)-1)) + + # Precompute dense arc-length lookup: u_grid <-> s_grid + self.u_grid = np.linspace(0.0, 1.0, 2000) + xy = np.array(splev(self.u_grid, self.tck)).T + dxy = np.diff(xy, axis=0) + seg = np.hypot(dxy[:,0], dxy[:,1]) + s_grid = np.concatenate(([0.0], np.cumsum(seg))) + self.s_grid = s_grid + self.path_len = float(s_grid[-1]) + + self.V_d = float(V_d) + self.delta = float(delta) + self.window_len = 4.0*self.delta if window_len is None else float(window_len) + self.mu = float(mu) # small gradient to reduce along-track error + + # Persistent arc-length parameter s (initialize lazily on first call) + self.s = None + + # ---- helpers: arc-length <-> u ------------------------------------------------- + def _u_from_s(self, s): + s = np.clip(s, 0.0, self.path_len) + i = np.searchsorted(self.s_grid, s) + if i == 0: return self.u_grid[0] + if i >= len(self.s_grid): return self.u_grid[-1] + s0, s1 = self.s_grid[i-1], self.s_grid[i] + u0, u1 = self.u_grid[i-1], self.u_grid[i] + t = 0.0 if s1==s0 else (s - s0)/(s1 - s0) + return u0 + t*(u1 - u0) + + def _pos_from_s(self, s): + u = self._u_from_s(s) + x, y = splev(u, self.tck) + return np.array([x, y]) + + def _tangent_from_s(self, s): + u = self._u_from_s(s) + dx, dy = splev(u, self.tck, der=1) + t = np.array([dx, dy]) + nrm = np.linalg.norm(t) + return t / (nrm + 1e-12), nrm # unit tangent, |p'(u)| (scaled) + + # ---- local projection around s_prev (bounded window + progress bias) ---------- + def _project_local(self, x, y, s_guess, ds_ff): """ - Project the point (x, y) onto the spline to find the parameter s. + Project (x,y) to arc-length s within a local window around s_guess. + Adds a quadratic bias toward s_guess + ds_ff to prefer forward progress. """ - def cost(u): + w = self.window_len + a = max(0.0, s_guess - 0.5*w) + b = min(self.path_len, s_guess + 0.5*w) + if b <= a: # fallback + a, b = 0.0, self.path_len + + target = np.array([x, y]) + s_target = s_guess + ds_ff + + def cost(s): + p = self._pos_from_s(s) + d = p - target + # distance term + progress bias term (lambda weights) + return d@d + 1e-3*(s - s_target)**2 + + res = minimize_scalar(cost, bounds=(a, b), method='bounded', options={'xatol':1e-6}) + return float(res.x) + + # ---- public API ---------------------------------------------------------------- + def reset_to_nearest(self, x, y): + """Call once before the loop to initialize s to the global nearest point.""" + target = np.array([x, y]) + def cost_u(u): px, py = splev(u, self.tck) - return (px - x)**2 + (py - y)**2 - - res = minimize_scalar(cost, bounds=(0, 1), method='bounded') - return res.x - - def guidance(self, x, y): + d = np.array([px, py]) - target + return d@d + res = minimize_scalar(cost_u, bounds=(0.0, 1.0), method='bounded') + u0 = float(res.x) + # convert to arc length + i = np.searchsorted(self.u_grid, u0) + if i == 0: self.s = 0.0 + elif i >= len(self.u_grid): self.s = self.path_len + else: + u0a, u0b = self.u_grid[i-1], self.u_grid[i] + s0a, s0b = self.s_grid[i-1], self.s_grid[i] + t = (u0 - u0a) / (u0b - u0a + 1e-12) + self.s = s0a + t*(s0b - s0a) + + def guidance(self, x, y, dt=None): """ - Compute the LOS guidance command. - - :param x: current NED x-position (m) - :param y: current NED y-position (m) - :return: (chi_d, vel_cmd) where chi_d is desired heading (rad), - and vel_cmd is a 2-element array [Vx, Vy] in NED. + Compute heading + NED velocity command. + If dt is given, advance by V_d*dt along the curve; otherwise use delta lookahead. + Returns (chi_d [rad], vel_cmd [2], extras dict) """ - # 1) Project onto spline - s0 = self._project(x, y) - - # 2) Compute look-ahead parameter increment - ds = self.delta / self.path_len - s_la = min(s0 + ds, 1.0) - - # 3) Evaluate look-ahead point - x_la, y_la = splev(s_la, self.tck) - - # 4) Compute desired heading - dx, dy = x_la - x, y_la - y + if self.s is None: + self.reset_to_nearest(x, y) + + # feedforward advance in arc length + ds_ff = (self.V_d * (dt if dt is not None else 0.0)) + + # optional gradient correction to reduce along-track error (small mu) + # ṡ_c = -mu * t_hat^T * e where e = p(s) - current position + # discretize as ds_grad ~ ṡ_c * dt + ds_grad = 0.0 + if dt is not None and self.mu > 0.0: + p_s = self._pos_from_s(self.s) + t_hat, _ = self._tangent_from_s(self.s) + e = p_s - np.array([x, y]) + ds_grad = - self.mu * float(t_hat.dot(e)) * dt + + # predict prior to projection + s_pred = np.clip(self.s + ds_ff + ds_grad, 0.0, self.path_len) + + # local projection (prevents jumping at crossings) + s0 = self._project_local(x, y, s_pred, ds_ff) + + # choose look-ahead by arc length + s_la = min(s0 + self.delta, self.path_len) + p_la = self._pos_from_s(s_la) + + # heading toward lookahead + dx, dy = p_la[0] - x, p_la[1] - y chi_d = np.arctan2(dy, dx) - # 5) Compute velocity command in NED + # velocity command in NED Vx = self.V_d * np.cos(chi_d) Vy = self.V_d * np.sin(chi_d) - return chi_d, np.array([Vx, Vy]) + # commit parameter for next call + self.s = s0 + return chi_d, np.array([Vx, Vy]), {"s": self.s, "s_la": s_la, "path_len": self.path_len, "lookahead_point": p_la} if __name__ == "__main__": # Example waypoints in NED (north-east) diff --git a/cybership_interfaces/action/LOSGuidance.action b/cybership_interfaces/action/LOSGuidance.action index 164f9d0..cb0c1a5 100644 --- a/cybership_interfaces/action/LOSGuidance.action +++ b/cybership_interfaces/action/LOSGuidance.action @@ -4,6 +4,7 @@ nav_msgs/Path path bool success --- # Empty result currently +bool success --- # Feedback: current heading and velocity command float64 heading diff --git a/cybership_tests/CMakeLists.txt b/cybership_tests/CMakeLists.txt index ddc1087..76ed859 100644 --- a/cybership_tests/CMakeLists.txt +++ b/cybership_tests/CMakeLists.txt @@ -8,14 +8,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) -find_package(joy REQUIRED) -find_package(sensor_msgs REQUIRED) find_package(rclpy REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(nav2_msgs REQUIRED) -find_package(nav_msgs REQUIRED) - ament_python_install_package(${PROJECT_NAME}) From 67a09a7bed5b6450a88c44d5094b2043959b829f Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Thu, 28 Aug 2025 08:16:05 +0200 Subject: [PATCH 30/46] feat: add kilted, remove others, remove arm64 build --- .github/workflows/build-and-push-arm64.yml | 47 ---------------------- .github/workflows/build-and-push.yml | 6 +-- 2 files changed, 2 insertions(+), 51 deletions(-) delete mode 100644 .github/workflows/build-and-push-arm64.yml diff --git a/.github/workflows/build-and-push-arm64.yml b/.github/workflows/build-and-push-arm64.yml deleted file mode 100644 index 5dd2f1b..0000000 --- a/.github/workflows/build-and-push-arm64.yml +++ /dev/null @@ -1,47 +0,0 @@ -name: Docker Hub for ARM64 - -on: - push: - branches: - - 'master' - schedule: - - cron: "0 0 * * 0" - -jobs: - docker: - - strategy: - fail-fast: false - matrix: - configurations: - - ros_distro: 'humble' - - ros_distro: 'jazzy' - - runs-on: ubuntu-24.04-arm64 - - steps: - - name: Checkout - uses: actions/checkout@v3 - - - name: Set up QEMU - uses: docker/setup-qemu-action@v3 - - - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v3 - - - name: Login to Docker Hub - uses: docker/login-action@v3 - with: - username: ${{ secrets.DOCKERHUB_USERNAME }} - password: ${{ secrets.DOCKERHUB_TOKEN }} - - - name: Build and push - uses: docker/build-push-action@v6 - with: - context: . - file: dockerfile - build-args: | - ROS_DISTRO=${{ matrix.configurations.ros_distro }} - push: true - platforms: linux/arm64 - tags: incebellipipo/cybership:${{ matrix.configurations.ros_distro }} diff --git a/.github/workflows/build-and-push.yml b/.github/workflows/build-and-push.yml index 4244eb8..79f3c3e 100644 --- a/.github/workflows/build-and-push.yml +++ b/.github/workflows/build-and-push.yml @@ -13,11 +13,9 @@ jobs: fail-fast: false matrix: configurations: - - ros_distro: 'humble' + - ros_distro: 'kilted' - ros_distro: 'jazzy' - - ros_distro: 'iron' - - ros_distro: 'galactic' - + runs-on: ubuntu-latest steps: From ae8dfa29bfbac7e377cde39b77bc055a231f05b0 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Thu, 28 Aug 2025 11:28:25 +0200 Subject: [PATCH 31/46] feat: add gui --- cybership_utilities/CMakeLists.txt | 6 + cybership_utilities/README.md | 12 +- .../cybership_utilities/launch.py | 35 ++ cybership_utilities/nodes/force_mux_gui.py | 368 ++++++++++++++++++ cybership_utilities/package.xml | 1 + cybership_utilities/requirements.txt | 1 + 6 files changed, 422 insertions(+), 1 deletion(-) create mode 100755 cybership_utilities/nodes/force_mux_gui.py diff --git a/cybership_utilities/CMakeLists.txt b/cybership_utilities/CMakeLists.txt index c861c03..0026363 100644 --- a/cybership_utilities/CMakeLists.txt +++ b/cybership_utilities/CMakeLists.txt @@ -11,4 +11,10 @@ find_package(ament_cmake_python REQUIRED) ament_python_install_package(${PROJECT_NAME}) +# Install Python executables +install(PROGRAMS + nodes/force_mux_gui.py + DESTINATION lib/${PROJECT_NAME} +) + ament_package() \ No newline at end of file diff --git a/cybership_utilities/README.md b/cybership_utilities/README.md index 9d1d8b4..26617cd 100644 --- a/cybership_utilities/README.md +++ b/cybership_utilities/README.md @@ -20,4 +20,14 @@ You can use the provided utility functions and launch arguments in your ROS 2 la from cybership_utilities import launch ``` -Then use `sanitize_hostname_for_ros2` or `anon` in your launch configurations, and include `COMMON_ARGUMENTS` to standardize launch argument handling. \ No newline at end of file +Then use `sanitize_hostname_for_ros2` or `anon` in your launch configurations, and include `COMMON_ARGUMENTS` to standardize launch argument handling. + +### Force Mux GUI + +The script `nodes/force_mux_gui.py` provides a small PyQt GUI to inspect and switch the `topic_tools` force mux source for a selected vehicle namespace. + +- Lists available input topics via `MuxList` +- Switches active topic via `MuxSelect` +- Namespace can be typed (e.g., `/enterprise`) or set with `VEHICLE_NS` env var. + +Run it with your preferred Python, ensuring your ROS 2 environment is sourced so service types are discoverable. \ No newline at end of file diff --git a/cybership_utilities/cybership_utilities/launch.py b/cybership_utilities/cybership_utilities/launch.py index aecc9fd..9f2572d 100644 --- a/cybership_utilities/cybership_utilities/launch.py +++ b/cybership_utilities/cybership_utilities/launch.py @@ -71,3 +71,38 @@ def anon(host: bool = True) -> str: description="Use simulation clock if true", ), ] + +def include_launch_action_with_config( + vessel_model, + vessel_name, + launch_file, + config_file=""): + + bringup_pkg_dir = launch_ros.substitutions.FindPackageShare( + 'cybership_bringup') + config_pkg_dir = launch_ros.substitutions.FindPackageShare( + 'cybership_config') + + launch_arguments = [ + ('vessel_name', vessel_name), + ('vessel_model', vessel_model) + ] + + if len(config_file) != 0: + launch_arguments.append( + ( + 'param_file', + launch.substitutions.PathJoinSubstitution( + [config_pkg_dir, 'config', vessel_model, config_file] + ) + ) + ) + + return launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + launch.substitutions.PathJoinSubstitution( + [bringup_pkg_dir, 'launch', 'include', launch_file] + ) + ), + launch_arguments=launch_arguments + ) \ No newline at end of file diff --git a/cybership_utilities/nodes/force_mux_gui.py b/cybership_utilities/nodes/force_mux_gui.py new file mode 100755 index 0000000..5484a97 --- /dev/null +++ b/cybership_utilities/nodes/force_mux_gui.py @@ -0,0 +1,368 @@ +#!/usr/bin/env python3 + +""" +PyQt GUI to control topic_tools multiplexers for force or velocity commands. + +Features +- Enter/select a vehicle namespace (e.g., /enterprise) +- Discover mux services and list input topics via MuxList +- Switch the active source using MuxSelect +""" + +import os +import sys +from typing import Optional, List + +from PyQt5 import QtWidgets, QtCore + +import rclpy +from rclpy.node import Node + +try: + from topic_tools_interfaces.srv import MuxList, MuxSelect + from std_srvs.srv import Empty +except Exception: + print("Missing dependency topic_tools_interfaces. Please build/install it.") + raise + + +DEFAULT_MUX_NAME = os.environ.get("MUX_NAME", "force_mux") + + +class ForceMuxClient(Node): + def __init__(self, namespace: str, mux_name: str = DEFAULT_MUX_NAME) -> None: + super().__init__("force_mux_client", namespace=namespace) + self.mux_name = mux_name + self._list_cli = self.create_client(MuxList, f"{mux_name}/list") + self._select_cli = self.create_client(MuxSelect, f"{mux_name}/select") + + def discover_mux_bases(self) -> List[str]: + bases: List[str] = [] + ns_prefix = self.get_namespace().rstrip("/") + for name, types in self.get_service_names_and_types(): + flat_types: List[str] = [] + for t in types: + if isinstance(t, str): + flat_types.append(t) + else: + flat_types.extend(t) + if not any("topic_tools_interfaces/srv/MuxList" in t for t in flat_types): + continue + if ns_prefix and not name.startswith(ns_prefix + "/"): + continue + if not name.endswith("/list"): + continue + base = name.split("/")[-2] + bases.append(base) + return sorted(set(bases)) + + def wait_for_services(self, timeout_sec: float = 2.0) -> bool: + ok1 = self._list_cli.wait_for_service(timeout_sec=timeout_sec) + ok2 = self._select_cli.wait_for_service(timeout_sec=timeout_sec) + return bool(ok1 and ok2) + + def list_topics(self): + req = MuxList.Request() + return self._list_cli.call_async(req) + + def select_topic(self, topic: str): + req = MuxSelect.Request() + req.topic = topic + return self._select_cli.call_async(req) + + +class MuxPanel(QtWidgets.QGroupBox): + def __init__(self, kind: str, get_namespace, ensure_rcl) -> None: + title = "Force Mux" if kind == "force" else "Velocity Mux" + super().__init__(title) + self.kind = kind # 'force' or 'velocity' + self._get_namespace = get_namespace + self._ensure_rcl = ensure_rcl + self._node = None # type: Optional[ForceMuxClient] + + self.topic_combo = QtWidgets.QComboBox() + self.topic_combo.setEditable(False) + self.find_btn = QtWidgets.QPushButton("Find") + self.refresh_btn = QtWidgets.QPushButton("Refresh") + self.select_btn = QtWidgets.QPushButton("Select") + self.status_lbl = QtWidgets.QLabel("Idle") + + row = QtWidgets.QHBoxLayout() + row.addWidget(self.topic_combo, 1) + row.addWidget(self.find_btn) + row.addWidget(self.refresh_btn) + row.addWidget(self.select_btn) + + lay = QtWidgets.QVBoxLayout(self) + lay.addLayout(row) + lay.addWidget(self.status_lbl) + + self.find_btn.clicked.connect(self.on_find) + self.refresh_btn.clicked.connect(self.on_refresh) + self.select_btn.clicked.connect(self.on_select) + + def _prefix(self) -> str: + return "force_mux" if self.kind == "force" else "velocity_mux" + + def _desired_default_mux_name(self) -> str: + # Allow per-kind override, else global, else default + per_kind = os.environ.get("FORCE_MUX_NAME" if self.kind == "force" else "VELOCITY_MUX_NAME") + if per_kind: + return per_kind + env_override = os.environ.get("MUX_NAME") + if env_override: + return env_override + return self._prefix() + + def _namespace(self) -> str: + ns = (self._get_namespace() or "").strip() + if ns and not ns.startswith("/"): + ns = "/" + ns + return ns + + def _recreate_node(self, mux_name: Optional[str] = None) -> Optional[ForceMuxClient]: + ns = self._namespace() + if self._node is not None: + try: + self._node.destroy_node() + except Exception: + pass + self._node = None + self._ensure_rcl() + try: + self._node = ForceMuxClient(namespace=ns, mux_name=mux_name or self._desired_default_mux_name()) + except Exception as e: + self.status_lbl.setText(f"Node error: {e}") + self._node = None + return self._node + + def spin_once(self) -> None: + if self._node is not None and rclpy.ok(): + rclpy.spin_once(self._node, timeout_sec=0.0) + + def on_find(self) -> None: + node = self._recreate_node() + if node is None: + return + self.status_lbl.setText("Discovering mux…") + bases = node.discover_mux_bases() + if not bases: + self.status_lbl.setText("No mux found") + return + preferred = [b for b in bases if b.startswith(self._prefix())] + chosen = preferred[0] if preferred else bases[0] + # Bind to chosen mux + self._recreate_node(mux_name=chosen) + self.status_lbl.setText(f"Using '{chosen}'") + + def on_refresh(self) -> None: + node = self._recreate_node() + if node is None: + return + self.status_lbl.setText("Waiting for services…") + if not node.wait_for_services(timeout_sec=2.0): + bases = node.discover_mux_bases() + if not bases: + self.status_lbl.setText("Mux services unavailable") + return + chosen = (next((b for b in bases if b.startswith(self._prefix())), None) or bases[0]) + node = self._recreate_node(mux_name=chosen) + if node is None or not node.wait_for_services(timeout_sec=2.0): + self.status_lbl.setText("Mux services unavailable") + return + self.status_lbl.setText(f"Connected '{chosen}'") + + self.status_lbl.setText("Listing…") + fut = node.list_topics() + + def on_done(): + if fut.cancelled() or fut.exception() is not None: + self.status_lbl.setText(f"List failed: {fut.exception()}") + return + resp = fut.result() + topics = list(getattr(resp, "topics", [])) + self.topic_combo.clear() + if topics: + self.topic_combo.addItems(topics) + self.status_lbl.setText(f"{len(topics)} topic(s)") + else: + self.status_lbl.setText("No topics in mux") + + fut.add_done_callback(lambda _: QtCore.QTimer.singleShot(0, on_done)) + + def on_select(self) -> None: + if self._node is None: + self.status_lbl.setText("Find/Refresh first") + return + topic = self.topic_combo.currentText().strip() + if not topic: + self.status_lbl.setText("Pick a topic") + return + self.status_lbl.setText(f"Selecting {topic}…") + fut = self._node.select_topic(topic) + + def on_done(): + if fut.cancelled() or fut.exception() is not None: + self.status_lbl.setText(f"Select failed: {fut.exception()}") + return + resp = fut.result() + ok = bool(getattr(resp, "success", False)) + self.status_lbl.setText("Selected" if ok else "Select rejected") + + fut.add_done_callback(lambda _: QtCore.QTimer.singleShot(0, on_done)) + + +class BothMuxGUI(QtWidgets.QWidget): + def __init__(self) -> None: + super().__init__() + self.setWindowTitle("Mux Controller (Force + Velocity)") + self.setMinimumWidth(640) + + # rcl state + self._rcl_inited = False + + # Top controls + self.ns_edit = QtWidgets.QLineEdit() + self.ns_edit.setPlaceholderText("/vehicle_namespace (e.g., /enterprise)") + self.ns_edit.setText(os.environ.get("VEHICLE_NS", "")) + + form = QtWidgets.QFormLayout() + form.addRow("Namespace", self.ns_edit) + + # Panels (vertical stack) + self.force_panel = MuxPanel("force", get_namespace=lambda: self.ns_edit.text(), ensure_rcl=self._ensure_rcl) + self.velocity_panel = MuxPanel("velocity", get_namespace=lambda: self.ns_edit.text(), ensure_rcl=self._ensure_rcl) + + panels = QtWidgets.QVBoxLayout() + panels.addWidget(self.force_panel) + panels.addWidget(self.velocity_panel) + + # Thruster control + thruster_box = QtWidgets.QGroupBox("Thruster") + self.thr_activate_btn = QtWidgets.QPushButton("Activate") + self.thr_disable_btn = QtWidgets.QPushButton("Deactivate") + self.thr_status_lbl = QtWidgets.QLabel("Idle") + thr_buttons = QtWidgets.QHBoxLayout() + thr_buttons.addWidget(self.thr_activate_btn) + thr_buttons.addWidget(self.thr_disable_btn) + thr_lay = QtWidgets.QVBoxLayout(thruster_box) + thr_lay.addLayout(thr_buttons) + thr_lay.addWidget(self.thr_status_lbl) + + root = QtWidgets.QVBoxLayout(self) + root.addLayout(form) + root.addWidget(thruster_box) + root.addLayout(panels) + + # Timer to spin both nodes + self._spin_timer = QtCore.QTimer(self) + self._spin_timer.timeout.connect(self._spin_once) + self._spin_timer.start(20) + + # Thruster node/state + self._thruster_node = None + self._thruster_ns = None + self.thr_activate_btn.clicked.connect(self.on_thr_activate) + self.thr_disable_btn.clicked.connect(self.on_thr_disable) + + def _ensure_rcl(self) -> None: + if not self._rcl_inited: + rclpy.init(args=None) + self._rcl_inited = True + + def _spin_once(self) -> None: + self.force_panel.spin_once() + self.velocity_panel.spin_once() + # Spin thruster node too + if self._thruster_node is not None and rclpy.ok(): + rclpy.spin_once(self._thruster_node, timeout_sec=0.0) + + def _namespace(self) -> str: + ns = (self.ns_edit.text() or "").strip() + if ns and not ns.startswith("/"): + ns = "/" + ns + return ns + + def _ensure_thruster_node(self): + # Create once per namespace; reuse to avoid handle invalidation + ns = self._namespace() + if self._thruster_node is None or self._thruster_ns != ns: + # Dispose old (if any) before replacing + if self._thruster_node is not None: + try: + self._thruster_node.destroy_node() + except Exception: + pass + self._thruster_node = None + self._ensure_rcl() + self._thruster_node = rclpy.create_node("thruster_client", namespace=ns) + self._thruster_ns = ns + return self._thruster_node + + def _discover_thruster_services(self, name_candidates, node) -> list: + found = [] + empty_type_names = {"std_srvs/srv/Empty", "std_srvs/srv/Empty.srv"} + services = node.get_service_names_and_types() + ns = self._namespace() or "/" + ns_prefix = ns.rstrip("/") + "/" if ns != "/" else "/" + for cand in name_candidates: + for name, types in services: + flat_types = [] + for t in types: + flat_types.append(t if isinstance(t, str) else t[0]) + if not any(t in empty_type_names for t in flat_types): + continue + if not name.startswith(ns_prefix): + continue + if name.endswith("/" + cand): + found.append(name) + # Unique preserve order + seen = set() + ordered = [] + for n in found: + if n not in seen: + seen.add(n) + ordered.append(n) + return ordered + + def _call_empty_service(self, name_candidates) -> bool: + node = self._ensure_thruster_node() + # Try discovered fully-qualified matches first (using same node) + fqns = self._discover_thruster_services(name_candidates, node) + try_list = fqns + name_candidates + for svc_name in try_list: + cli = node.create_client(Empty, svc_name) + if not cli.wait_for_service(timeout_sec=1.5): + continue + fut = cli.call_async(Empty.Request()) + for _ in range(30): + rclpy.spin_once(node, timeout_sec=0.02) + if fut.done(): + break + if fut.done() and fut.exception() is None: + return True + return False + + def on_thr_activate(self) -> None: + self.thr_status_lbl.setText("Activating…") + ok = self._call_empty_service(["thruster/activate", "thruster/enable"]) # fallback to enable + self.thr_status_lbl.setText("Activated" if ok else "Activate failed") + + def on_thr_disable(self) -> None: + self.thr_status_lbl.setText("Disabling…") + ok = self._call_empty_service(["thruster/disable"]) # strict name per request + self.thr_status_lbl.setText("Disabled" if ok else "Disable failed") + + +def main() -> None: + app = QtWidgets.QApplication(sys.argv) + gui = BothMuxGUI() + gui.show() + rc = app.exec_() + if rclpy.ok(): + rclpy.shutdown() + sys.exit(rc) + + +if __name__ == "__main__": + main() diff --git a/cybership_utilities/package.xml b/cybership_utilities/package.xml index 4ae4237..6b1badc 100644 --- a/cybership_utilities/package.xml +++ b/cybership_utilities/package.xml @@ -10,6 +10,7 @@ ament_cmake rclpy + topic_tools_interfaces ament_cmake_python diff --git a/cybership_utilities/requirements.txt b/cybership_utilities/requirements.txt index e69de29..dcd21e8 100644 --- a/cybership_utilities/requirements.txt +++ b/cybership_utilities/requirements.txt @@ -0,0 +1 @@ +PyQt5>=5.15 From 515e7e9a8db863e94fefaf6937f9765bddf0c485 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Thu, 28 Aug 2025 11:28:44 +0200 Subject: [PATCH 32/46] fix: line of sight --- cybership_bringup/launch/drillship.launch.py | 36 +---- cybership_bringup/launch/enterprise.launch.py | 34 +--- .../launch/localization.launch.py | 35 +--- cybership_bringup/launch/voyager.launch.py | 35 +--- .../config/voyager/controller_velocity.yaml | 6 +- .../nodes/los_guidance_client.py | 9 +- .../nodes/los_guidance_server.py | 149 ++++++++++++------ .../src/cybership_controller/guidance/los.py | 32 ++-- 8 files changed, 132 insertions(+), 204 deletions(-) diff --git a/cybership_bringup/launch/drillship.launch.py b/cybership_bringup/launch/drillship.launch.py index bfc1f5e..52ea28d 100644 --- a/cybership_bringup/launch/drillship.launch.py +++ b/cybership_bringup/launch/drillship.launch.py @@ -5,40 +5,10 @@ import launch.launch_description_sources -def include_launch_action_with_config( - vessel_model, - vessel_name, - launch_file, - config_file=""): - - bringup_pkg_dir = launch_ros.substitutions.FindPackageShare( - 'cybership_bringup') - config_pkg_dir = launch_ros.substitutions.FindPackageShare( - 'cybership_config') - - launch_arguments = [ - ('vessel_name', vessel_name), - ('vessel_model', vessel_model) - ] - - if len(config_file) != 0: - launch_arguments.append( - ( - 'param_file', - launch.substitutions.PathJoinSubstitution( - [config_pkg_dir, 'config', vessel_model, config_file] - ) - ) - ) - return launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - launch.substitutions.PathJoinSubstitution( - [bringup_pkg_dir, 'launch', 'include', launch_file] - ) - ), - launch_arguments=launch_arguments - ) + +from cybership_utilities.launch import include_launch_action_with_config + def generate_launch_description(): diff --git a/cybership_bringup/launch/enterprise.launch.py b/cybership_bringup/launch/enterprise.launch.py index bfafaf9..1008618 100644 --- a/cybership_bringup/launch/enterprise.launch.py +++ b/cybership_bringup/launch/enterprise.launch.py @@ -5,40 +5,8 @@ import launch.launch_description_sources -def include_launch_action_with_config( - vessel_model, - vessel_name, - launch_file, - config_file=""): +from cybership_utilities.launch import include_launch_action_with_config - bringup_pkg_dir = launch_ros.substitutions.FindPackageShare( - 'cybership_bringup') - config_pkg_dir = launch_ros.substitutions.FindPackageShare( - 'cybership_config') - - launch_arguments = [ - ('vessel_name', vessel_name), - ('vessel_model', vessel_model) - ] - - if len(config_file) != 0: - launch_arguments.append( - ( - 'param_file', - launch.substitutions.PathJoinSubstitution( - [config_pkg_dir, 'config', vessel_model, config_file] - ) - ) - ) - - return launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - launch.substitutions.PathJoinSubstitution( - [bringup_pkg_dir, 'launch', 'include', launch_file] - ) - ), - launch_arguments=launch_arguments - ) def generate_launch_description(): diff --git a/cybership_bringup/launch/localization.launch.py b/cybership_bringup/launch/localization.launch.py index 84b93be..3510d4a 100644 --- a/cybership_bringup/launch/localization.launch.py +++ b/cybership_bringup/launch/localization.launch.py @@ -4,41 +4,8 @@ import launch.substitutions import launch.launch_description_sources +from cybership_utilities.launch import include_launch_action_with_config -def include_launch_action_with_config( - vessel_model, - vessel_name, - launch_file, - config_file=""): - - bringup_pkg_dir = launch_ros.substitutions.FindPackageShare( - 'cybership_bringup') - config_pkg_dir = launch_ros.substitutions.FindPackageShare( - 'cybership_config') - - launch_arguments = [ - ('vessel_name', vessel_name), - ('vessel_model', vessel_model) - ] - - if len(config_file) != 0: - launch_arguments.append( - ( - 'param_file', - launch.substitutions.PathJoinSubstitution( - [config_pkg_dir, 'config', vessel_model, config_file] - ) - ) - ) - - return launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - launch.substitutions.PathJoinSubstitution( - [bringup_pkg_dir, 'launch', 'include', launch_file] - ) - ), - launch_arguments=launch_arguments - ) def generate_launch_description(): diff --git a/cybership_bringup/launch/voyager.launch.py b/cybership_bringup/launch/voyager.launch.py index 54d97b3..c60534d 100644 --- a/cybership_bringup/launch/voyager.launch.py +++ b/cybership_bringup/launch/voyager.launch.py @@ -4,41 +4,8 @@ import launch.substitutions import launch.launch_description_sources +from cybership_utilities.launch import include_launch_action_with_config -def include_launch_action_with_config( - vessel_model, - vessel_name, - launch_file, - config_file=""): - - bringup_pkg_dir = launch_ros.substitutions.FindPackageShare( - 'cybership_bringup') - config_pkg_dir = launch_ros.substitutions.FindPackageShare( - 'cybership_config') - - launch_arguments = [ - ('vessel_name', vessel_name), - ('vessel_model', vessel_model) - ] - - if len(param_file) != 0: - launch_arguments.append( - ( - 'param_file', - launch.substitutions.PathJoinSubstitution( - [config_pkg_dir, 'config', vessel_model, config_file] - ) - ) - ) - - return launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - launch.substitutions.PathJoinSubstitution( - [bringup_pkg_dir, 'launch', 'include', launch_file] - ) - ), - launch_arguments=launch_arguments - ) def generate_launch_description(): diff --git a/cybership_config/config/voyager/controller_velocity.yaml b/cybership_config/config/voyager/controller_velocity.yaml index cebf2df..b888004 100644 --- a/cybership_config/config/voyager/controller_velocity.yaml +++ b/cybership_config/config/voyager/controller_velocity.yaml @@ -44,8 +44,8 @@ # Performance metrics configuration # Used for monitoring and debugging controller performance metrics: - window_size: 50 # Number of samples in moving window for statistics - interval: 1.0 # Time between metrics calculations (seconds) + window_size: 10 # Number of samples in moving window for statistics + interval: 5.0 # Time between metrics calculations (seconds) # Timing parameters - dt: 0.1 # Control loop time step (seconds) - affects timer frequency + dt: 0.5 # Control loop time step (seconds) - affects timer frequency diff --git a/cybership_controller/nodes/los_guidance_client.py b/cybership_controller/nodes/los_guidance_client.py index f6ec9c9..4d661d3 100755 --- a/cybership_controller/nodes/los_guidance_client.py +++ b/cybership_controller/nodes/los_guidance_client.py @@ -7,6 +7,8 @@ from rclpy.action import ActionClient from rclpy.executors import MultiThreadedExecutor +import numpy as np + from nav_msgs.msg import Path from geometry_msgs.msg import PoseStamped from cybership_interfaces.action import LOSGuidance @@ -72,10 +74,9 @@ def main(args=None): # Example waypoints for testing example_waypoints = [ (0.0, 0.0), - (6.0, 0.0), - (6.0, 6.0), - (0.0, 6.0), - (0.0, 0.0), + (np.random.normal(6.0, 1.0), 0.0), + (np.random.normal(6.0, 1.0),np.random.normal(6.0, 1.0)), + (0.0, np.random.normal(6.0, 1.0)), ] client.send_goal(example_waypoints) # Use MultiThreadedExecutor to handle feedback continuously diff --git a/cybership_controller/nodes/los_guidance_server.py b/cybership_controller/nodes/los_guidance_server.py index 64571f5..ac676f4 100755 --- a/cybership_controller/nodes/los_guidance_server.py +++ b/cybership_controller/nodes/los_guidance_server.py @@ -5,9 +5,10 @@ import math import rclpy from rclpy.node import Node -from rclpy.action import ActionServer +from rclpy.action import ActionServer, ActionClient from rclpy.action import CancelResponse, GoalResponse -from rclpy.executors import MultiThreadedExecutor +from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup from rclpy.qos import QoSProfile, DurabilityPolicy from nav_msgs.msg import Path, Odometry @@ -20,21 +21,26 @@ # Import underlying LOS guidance implementation from cybership_controller.guidance.los import LOSGuidance as BaseLOSGuidance import numpy as np +import typing from scipy.interpolate import splev + class LOSGuidanceROS(Node): def __init__(self): super().__init__('los_guidance_server', namespace="cybership") # Parameters - self.declare_parameter('desired_speed', 0.3) + self.declare_parameter('desired_speed', 0.5) self.declare_parameter('lookahead', 0.4) # Heading control gain - self.declare_parameter('heading_gain', 4.0) + self.declare_parameter('heading_gain', 1.0) # Publishers and Subscribers - self._cmd_pub = self.create_publisher(Twist, 'control/velocity/command', 10) + self._cmd_pub = self.create_publisher( + Twist, 'control/velocity/command', 10) # Publish markers with transient local durability so RViZ receives past markers - self._marker_pub = self.create_publisher(Marker, 'visualization_marker', 10) - self._odom_sub = self.create_subscription(Odometry, 'measurement/odom', self.odom_callback, 10) + self._marker_pub = self.create_publisher( + Marker, 'visualization_marker', 10) + self._odom_sub = self.create_subscription( + Odometry, 'measurement/odom', self.odom_callback, 10) # Vehicle pose self._position = None self._yaw = None @@ -45,8 +51,10 @@ def __init__(self): 'los_guidance', execute_callback=self.execute_callback, goal_callback=self.goal_callback, - cancel_callback=self.cancel_callback) - self.current_goal = None + cancel_callback=self.cancel_callback, + callback_group=ReentrantCallbackGroup()) + + self.goal_uuids: typing.List = [] self.get_logger().info('LOS Guidance Action Server started') @@ -63,48 +71,55 @@ def goal_callback(self, goal_request: LOSGuidance.Goal) -> GoalResponse: if len(goal_request.path.poses) < 2: self.get_logger().warn("Path has fewer than 2 poses — rejecting goal.") return GoalResponse.REJECT - if self.current_goal is not None: - self.get_logger().info("Cancelling previous active goal.") - self.current_goal.abort() + return GoalResponse.ACCEPT - def cancel_callback(self, goal_handle): + def cancel_callback(self, goal_handle) -> CancelResponse: + return CancelResponse.ACCEPT - def execute_callback(self, goal_handle): - self.current_goal = goal_handle - self.get_logger().info("Goal accepted: starting LOS guidance.") - # Extract waypoints from Path and include current vehicle position as initial point - path_msg: Path = goal_handle.request.path - # Build waypoints list, prepending vehicle's initial position if available - waypoints = [] - if self._position is not None: - waypoints.append(self._position) + def prepare_waypoints(self, path_msg: Path, extend: bool = True, n_intermediate: int = 2) -> np.ndarray: + waypoints = [(pose.pose.position.x, pose.pose.position.y) + for pose in path_msg.poses] + + if extend: + # Add intermediate points + extended_waypoints = [] + for i in range(len(waypoints) - 1): + p1 = waypoints[i] + p2 = waypoints[i + 1] + extended_waypoints.append(p1) + # Add intermediate points + for j in range(1, n_intermediate + 1): + mid_point = ( + p1[0] + (p2[0] - p1[0]) * j / (n_intermediate + 1), + p1[1] + (p2[1] - p1[1]) * j / (n_intermediate + 1) + ) + extended_waypoints.append(mid_point) + extended_waypoints.append(waypoints[-1]) + waypoints = extended_waypoints + + return np.array(waypoints) + + async def execute_callback(self, goal_handle: LOSGuidance.Goal) -> LOSGuidance.Result: + + if len(self.goal_uuids) > 0: + self.goal_uuids.pop(0) + self.goal_uuids.append(tuple(goal_handle.goal_id.uuid)) + path_msg: Path = goal_handle.request.path + waypoints = [] waypoints.extend([(pose.pose.position.x, pose.pose.position.y) for pose in path_msg.poses]) + if self._position is not None: + waypoints.append(self._position) - # add intermediate points - n_intermediate = 2 # Number of intermediate points between each waypoint - extended_waypoints = [] - for i in range(len(waypoints) - 1): - p1 = waypoints[i] - p2 = waypoints[i + 1] - extended_waypoints.append(p1) - # Add intermediate points - for j in range(1, n_intermediate + 1): - mid_point = ( - p1[0] + (p2[0] - p1[0]) * j / (n_intermediate + 1), - p1[1] + (p2[1] - p1[1]) * j / (n_intermediate + 1) - ) - extended_waypoints.append(mid_point) - - extended_waypoints.append(waypoints[-1]) - waypoints = np.array(extended_waypoints) + waypoints = self.prepare_waypoints( + path_msg, extend=True, n_intermediate=2) # Initialize guidance - hz = 10 + hz = 2 rate = self.create_rate(hz) V_d = float(self.get_parameter('desired_speed').value) delta = float(self.get_parameter('lookahead').value) @@ -119,9 +134,21 @@ def execute_callback(self, goal_handle): canceled = False while rclpy.ok(): + self.get_logger().info(f"{self.goal_uuids}") + if tuple(goal_handle.goal_id.uuid) not in self.goal_uuids: + self.get_logger().info( + f"LOS guidance goal no longer current. {goal_handle.goal_id.uuid}") + canceled = True + break + if goal_handle.is_cancel_requested: goal_handle.canceled() - canceled = True + self.get_logger().info( + f"LOS guidance canceled. {goal_handle.goal_id.uuid}") + break + if not goal_handle.is_active: + self.get_logger().info( + f"LOS guidance no longer active. {goal_handle.goal_id.uuid}") break if self._position is None or self._yaw is None: rate.sleep() @@ -130,13 +157,15 @@ def execute_callback(self, goal_handle): chi_d, vel_cmd, extras = guidance.guidance(x, y, dt=dt) twist = Twist() # Compute forward velocity in body frame using current heading - v_forward = vel_cmd[0] * math.cos(self._yaw) + vel_cmd[1] * math.sin(self._yaw) + v_forward = vel_cmd[0] * \ + math.cos(self._yaw) + vel_cmd[1] * math.sin(self._yaw) # Use computed forward speed (ensuring non-negative speed) twist.linear.x = max(v_forward, 0.0) twist.linear.y = 0.0 twist.linear.z = 0.0 # Compute and normalize heading error - heading_error = math.atan2(math.sin(chi_d - self._yaw), math.cos(chi_d - self._yaw)) + heading_error = math.atan2( + math.sin(chi_d - self._yaw), math.cos(chi_d - self._yaw)) twist.angular.z = k_h * heading_error self._cmd_pub.publish(twist) # Feedback @@ -148,7 +177,8 @@ def execute_callback(self, goal_handle): self.publish_arrow_marker(x, y, vel_cmd) # Look-ahead visualization # x_la, y_la, _, _ = guidance.lookahead_point(x, y) - self.publish_lookahead_marker(extras["lookahead_point"][0], extras["lookahead_point"][1]) + self.publish_lookahead_marker( + extras["lookahead_point"][0], extras["lookahead_point"][1]) # self.publish_lookahead_line(x, y, x_la, y_la) # Check completion if math.hypot(x - last_wp[0], y - last_wp[1]) < delta: @@ -158,7 +188,6 @@ def execute_callback(self, goal_handle): if not canceled: goal_handle.succeed() self.get_logger().info("LOS guidance finished (result empty).") - self.current_goal = None return LOSGuidance.Result() def publish_path_marker(self, path_msg: Path): @@ -169,7 +198,10 @@ def publish_path_marker(self, path_msg: Path): marker.type = Marker.LINE_STRIP marker.action = Marker.ADD marker.scale.x = 0.05 - marker.color.r = 1.0; marker.color.g = 0.0; marker.color.b = 0.0; marker.color.a = 1.0 + marker.color.r = 1.0 + marker.color.g = 0.0 + marker.color.b = 0.0 + marker.color.a = 1.0 marker.points = [Point(x=pose.pose.position.x, y=pose.pose.position.y, z=0.0) @@ -189,7 +221,10 @@ def publish_arrow_marker(self, x, y, vel_cmd): marker.scale.z = 0.1 # Use a copy of vel_cmd for scaling to avoid in-place modification vel_arrow = vel_cmd * 3 - marker.color.r = 0.0; marker.color.g = 1.0; marker.color.b = 0.5; marker.color.a = 1.0 + marker.color.r = 0.0 + marker.color.g = 1.0 + marker.color.b = 0.5 + marker.color.a = 1.0 start = Point(x=x, y=y, z=0.0) end = Point(x=x + vel_arrow[0], y=y + vel_arrow[1], z=0.0) marker.points = [start, end] @@ -204,7 +239,10 @@ def publish_spline_marker(self, guidance): marker.type = Marker.LINE_STRIP marker.action = Marker.ADD marker.scale.x = 0.05 - marker.color.r = 0.0; marker.color.g = 0.0; marker.color.b = 1.0; marker.color.a = 1.0 + marker.color.r = 0.0 + marker.color.g = 0.0 + marker.color.b = 1.0 + marker.color.a = 1.0 # Sample spline path points ts = np.linspace(0, 1, 1000) spline_pts = np.array(splev(ts, guidance.tck)).T @@ -222,7 +260,10 @@ def publish_lookahead_marker(self, x_la, y_la): m.scale.x = 0.25 # diameter in meters m.scale.y = 0.25 m.scale.z = 0.25 - m.color.r = 1.0; m.color.g = 0.85; m.color.b = 0.0; m.color.a = 1.0 # golden/yellow + m.color.r = 1.0 + m.color.g = 0.85 + m.color.b = 0.0 + m.color.a = 1.0 # golden/yellow m.pose.position.x = float(x_la) m.pose.position.y = float(y_la) m.pose.position.z = 0.0 @@ -237,12 +278,16 @@ def publish_lookahead_line(self, x, y, x_la, y_la): m.type = Marker.LINE_STRIP m.action = Marker.ADD m.scale.x = 0.03 - m.color.r = 1.0; m.color.g = 0.85; m.color.b = 0.0; m.color.a = 0.9 + m.color.r = 1.0 + m.color.g = 0.85 + m.color.b = 0.0 + m.color.a = 0.9 start = Point(x=float(x), y=float(y), z=0.0) - end = Point(x=float(x_la), y=float(y_la), z=0.0) + end = Point(x=float(x_la), y=float(y_la), z=0.0) m.points = [start, end] self._marker_pub.publish(m) + def main(args=None): rclpy.init(args=args) node = LOSGuidanceROS() @@ -255,4 +300,4 @@ def main(args=None): if __name__ == '__main__': - main() \ No newline at end of file + main() diff --git a/cybership_controller/src/cybership_controller/guidance/los.py b/cybership_controller/src/cybership_controller/guidance/los.py index 2878ddc..2bc2a4b 100644 --- a/cybership_controller/src/cybership_controller/guidance/los.py +++ b/cybership_controller/src/cybership_controller/guidance/los.py @@ -2,6 +2,7 @@ from scipy.interpolate import splprep, splev from scipy.optimize import minimize_scalar + class LOSGuidance: def __init__(self, pts, V_d, delta, window_len=None, mu=0.0): """ @@ -12,20 +13,22 @@ def __init__(self, pts, V_d, delta, window_len=None, mu=0.0): mu: small gradient gain for along-track stabilization [0..0.5], 0 to disable """ pts = np.asarray(pts) - self.tck, self.u_wp = splprep([pts[:,0], pts[:,1]], s=0, k=min(3, len(pts)-1)) + self.tck, self.u_wp = splprep( + [pts[:, 0], pts[:, 1]], s=0, k=min(3, len(pts)-1)) # Precompute dense arc-length lookup: u_grid <-> s_grid self.u_grid = np.linspace(0.0, 1.0, 2000) xy = np.array(splev(self.u_grid, self.tck)).T dxy = np.diff(xy, axis=0) - seg = np.hypot(dxy[:,0], dxy[:,1]) + seg = np.hypot(dxy[:, 0], dxy[:, 1]) s_grid = np.concatenate(([0.0], np.cumsum(seg))) self.s_grid = s_grid self.path_len = float(s_grid[-1]) self.V_d = float(V_d) self.delta = float(delta) - self.window_len = 4.0*self.delta if window_len is None else float(window_len) + self.window_len = 4.0 * \ + self.delta if window_len is None else float(window_len) self.mu = float(mu) # small gradient to reduce along-track error # Persistent arc-length parameter s (initialize lazily on first call) @@ -35,11 +38,13 @@ def __init__(self, pts, V_d, delta, window_len=None, mu=0.0): def _u_from_s(self, s): s = np.clip(s, 0.0, self.path_len) i = np.searchsorted(self.s_grid, s) - if i == 0: return self.u_grid[0] - if i >= len(self.s_grid): return self.u_grid[-1] + if i == 0: + return self.u_grid[0] + if i >= len(self.s_grid): + return self.u_grid[-1] s0, s1 = self.s_grid[i-1], self.s_grid[i] u0, u1 = self.u_grid[i-1], self.u_grid[i] - t = 0.0 if s1==s0 else (s - s0)/(s1 - s0) + t = 0.0 if s1 == s0 else (s - s0)/(s1 - s0) return u0 + t*(u1 - u0) def _pos_from_s(self, s): @@ -75,13 +80,15 @@ def cost(s): # distance term + progress bias term (lambda weights) return d@d + 1e-3*(s - s_target)**2 - res = minimize_scalar(cost, bounds=(a, b), method='bounded', options={'xatol':1e-6}) + res = minimize_scalar(cost, bounds=( + a, b), method='bounded', options={'xatol': 1e-6}) return float(res.x) # ---- public API ---------------------------------------------------------------- def reset_to_nearest(self, x, y): """Call once before the loop to initialize s to the global nearest point.""" target = np.array([x, y]) + def cost_u(u): px, py = splev(u, self.tck) d = np.array([px, py]) - target @@ -90,8 +97,10 @@ def cost_u(u): u0 = float(res.x) # convert to arc length i = np.searchsorted(self.u_grid, u0) - if i == 0: self.s = 0.0 - elif i >= len(self.u_grid): self.s = self.path_len + if i == 0: + self.s = 0.0 + elif i >= len(self.u_grid): + self.s = self.path_len else: u0a, u0b = self.u_grid[i-1], self.u_grid[i] s0a, s0b = self.s_grid[i-1], self.s_grid[i] @@ -143,6 +152,7 @@ def guidance(self, x, y, dt=None): return chi_d, np.array([Vx, Vy]), {"s": self.s, "s_la": s_la, "path_len": self.path_len, "lookahead_point": p_la} + if __name__ == "__main__": # Example waypoints in NED (north-east) waypoints = np.array([ @@ -168,5 +178,5 @@ def guidance(self, x, y, dt=None): print("LOS Guidance Commands:") for pos in test_positions: chi_d, vel_cmd = los.guidance(*pos) - print(f"Position {pos} -> Heading: {np.degrees(chi_d):.1f}°, Vel_CMD: [{vel_cmd[0]:.2f}, {vel_cmd[1]:.2f}]") - + print( + f"Position {pos} -> Heading: {np.degrees(chi_d):.1f}°, Vel_CMD: [{vel_cmd[0]:.2f}, {vel_cmd[1]:.2f}]") From 303eb01e411b6720da1398abbb9e5d95fa91bb41 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Thu, 28 Aug 2025 14:46:09 +0200 Subject: [PATCH 33/46] feat: add multi vehicle sim support --- .../config/drillship/azimuth_controller.yaml | 12 +- .../config/drillship/imu_bno055.yaml | 2 +- .../config/drillship/mocap_transformer.yaml | 2 +- .../config/drillship/robot_localization.yaml | 2 +- .../config/voyager/azimuth_controller.yaml | 4 +- .../config/voyager/imu_bno055.yaml | 2 +- .../config/voyager/mocap_transformer.yaml | 2 +- .../config/voyager/robot_localization.yaml | 2 +- .../launch/description.launch.py | 3 +- .../meshes/drillship/model.stl | Bin 0 -> 139184 bytes .../urdf/model/drillship/base.urdf.xacro | 12 + .../config/multi_vessel_simulation.yaml | 30 + cybership_simulator/config/simulation.yaml | 2 +- .../launch/multi_vessel.launch.py | 100 +++ .../src/cybership_simulator/base.py | 36 +- .../src/cybership_simulator/drillship.py | 14 +- .../src/cybership_simulator/enterprise.py | 9 +- .../src/cybership_simulator/voyager.py | 16 +- cybership_utilities/html/index.html | 243 +++++++ cybership_viz/config/drillship.rviz | 146 +++- cybership_viz/config/multi.rviz | 651 ++++++++++++++++++ cybership_viz/config/voyager.rviz | 244 ++++--- cybership_viz/launch/multi.launch.py | 41 ++ 23 files changed, 1383 insertions(+), 192 deletions(-) create mode 100644 cybership_description/meshes/drillship/model.stl create mode 100644 cybership_simulator/config/multi_vessel_simulation.yaml create mode 100644 cybership_simulator/launch/multi_vessel.launch.py create mode 100644 cybership_utilities/html/index.html create mode 100644 cybership_viz/config/multi.rviz create mode 100644 cybership_viz/launch/multi.launch.py diff --git a/cybership_config/config/drillship/azimuth_controller.yaml b/cybership_config/config/drillship/azimuth_controller.yaml index 8dce357..e91fa61 100644 --- a/cybership_config/config/drillship/azimuth_controller.yaml +++ b/cybership_config/config/drillship/azimuth_controller.yaml @@ -6,7 +6,7 @@ servos: servo_1: - frame_id: servo_1_link + frame_id: drillship/servo_1_link control_topic: hardware/joint/servo_1/angle device_id: 3 gain: 100.0 @@ -14,7 +14,7 @@ offset: 1.6 servo_2: - frame_id: servo_2_link + frame_id: drillship/servo_2_link control_topic: hardware/joint/servo_2/angle device_id: 1 gain: 100.0 @@ -22,7 +22,7 @@ offset: 2.2 servo_3: - frame_id: servo_3_link + frame_id: drillship/servo_3_link control_topic: hardware/joint/servo_3/angle device_id: 2 gain: 100.0 @@ -30,7 +30,7 @@ offset: -1.0 servo_4: - frame_id: servo_4_link + frame_id: drillship/servo_4_link control_topic: hardware/joint/servo_4/angle device_id: 6 gain: 100.0 @@ -38,7 +38,7 @@ offset: 2.4 servo_5: - frame_id: servo_5_link + frame_id: drillship/servo_5_link control_topic: hardware/joint/servo_5/angle device_id: 4 gain: 100.0 @@ -46,7 +46,7 @@ offset: -2.0 servo_6: - frame_id: servo_6_link + frame_id: drillship/servo_6_link control_topic: hardware/joint/servo_6/angle device_id: 5 gain: 100.0 diff --git a/cybership_config/config/drillship/imu_bno055.yaml b/cybership_config/config/drillship/imu_bno055.yaml index f30feaf..836789d 100644 --- a/cybership_config/config/drillship/imu_bno055.yaml +++ b/cybership_config/config/drillship/imu_bno055.yaml @@ -35,7 +35,7 @@ i2c_addr: 0x28 data_query_frequency: 100 calib_status_frequency: 0.1 - frame_id: "imu_link" + frame_id: "drillship/imu_link" operation_mode: 0x0C placement_axis_remap: "P2" acc_factor: 100.0 diff --git a/cybership_config/config/drillship/mocap_transformer.yaml b/cybership_config/config/drillship/mocap_transformer.yaml index 0eff1c7..6827ce1 100644 --- a/cybership_config/config/drillship/mocap_transformer.yaml +++ b/cybership_config/config/drillship/mocap_transformer.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: world_frame: world - base_frame: mocap_link + base_frame: drillship/mocap_link rigid_body_name: "2" topic_name: measurement/pose publish_tf: false \ No newline at end of file diff --git a/cybership_config/config/drillship/robot_localization.yaml b/cybership_config/config/drillship/robot_localization.yaml index cea5b65..2b63e9b 100644 --- a/cybership_config/config/drillship/robot_localization.yaml +++ b/cybership_config/config/drillship/robot_localization.yaml @@ -67,7 +67,7 @@ # estimation node from robot_localization! However, that instance should *not* fuse the global data. map_frame: map # Defaults to "map" if unspecified odom_frame: world # Defaults to "odom" if unspecified - base_link_frame: base_link_ned # Defaults to "base_link" if unspecified + base_link_frame: drillship/base_link_ned # Defaults to "base_link" if unspecified world_frame: world # Defaults to the value of odom_frame if unspecified # The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry, diff --git a/cybership_config/config/voyager/azimuth_controller.yaml b/cybership_config/config/voyager/azimuth_controller.yaml index ef7ebdc..e5c9a6b 100644 --- a/cybership_config/config/voyager/azimuth_controller.yaml +++ b/cybership_config/config/voyager/azimuth_controller.yaml @@ -6,7 +6,7 @@ servos: # Starboard servo_1: - frame_id: starboard_azimuth_link + frame_id: voyager/starboard_azimuth_link control_topic: hardware/joint/servo_1/angle device_id: 1 gain: 100.0 @@ -14,7 +14,7 @@ offset: -2.6 # Port servo_2: - frame_id: port_azimuth_link + frame_id: voyager/port_azimuth_link control_topic: hardware/joint/servo_2/angle device_id: 2 gain: 100.0 diff --git a/cybership_config/config/voyager/imu_bno055.yaml b/cybership_config/config/voyager/imu_bno055.yaml index f30feaf..fbf2039 100644 --- a/cybership_config/config/voyager/imu_bno055.yaml +++ b/cybership_config/config/voyager/imu_bno055.yaml @@ -35,7 +35,7 @@ i2c_addr: 0x28 data_query_frequency: 100 calib_status_frequency: 0.1 - frame_id: "imu_link" + frame_id: "voyager/imu_link" operation_mode: 0x0C placement_axis_remap: "P2" acc_factor: 100.0 diff --git a/cybership_config/config/voyager/mocap_transformer.yaml b/cybership_config/config/voyager/mocap_transformer.yaml index 0990b5e..507b8dc 100644 --- a/cybership_config/config/voyager/mocap_transformer.yaml +++ b/cybership_config/config/voyager/mocap_transformer.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: world_frame: world - base_frame: mocap_link + base_frame: voyager/mocap_link rigid_body_name: "0" topic_name: measurement/pose publish_tf: false diff --git a/cybership_config/config/voyager/robot_localization.yaml b/cybership_config/config/voyager/robot_localization.yaml index cea5b65..86d4941 100644 --- a/cybership_config/config/voyager/robot_localization.yaml +++ b/cybership_config/config/voyager/robot_localization.yaml @@ -67,7 +67,7 @@ # estimation node from robot_localization! However, that instance should *not* fuse the global data. map_frame: map # Defaults to "map" if unspecified odom_frame: world # Defaults to "odom" if unspecified - base_link_frame: base_link_ned # Defaults to "base_link" if unspecified + base_link_frame: voyager/base_link_ned # Defaults to "base_link" if unspecified world_frame: world # Defaults to the value of odom_frame if unspecified # The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry, diff --git a/cybership_description/launch/description.launch.py b/cybership_description/launch/description.launch.py index 7dd221f..f94a5c4 100755 --- a/cybership_description/launch/description.launch.py +++ b/cybership_description/launch/description.launch.py @@ -31,7 +31,8 @@ def generate_launch_description(): {'robot_description': launch.substitutions.Command([ 'xacro', ' ', xacro_file, ' ', 'gazebo:=ignition', ' ', - 'namespace:=', launch.substitutions.LaunchConfiguration('vessel_name')])} + 'namespace:=', launch.substitutions.LaunchConfiguration('vessel_name')])}, + {'frame_prefix': [launch.substitutions.LaunchConfiguration('vessel_name'), '/']}, ], remappings=[ # ('/tf', 'tf'), diff --git a/cybership_description/meshes/drillship/model.stl b/cybership_description/meshes/drillship/model.stl new file mode 100644 index 0000000000000000000000000000000000000000..bb5995929da1daf60136fddcecf085f96138248c GIT binary patch literal 139184 zcmb@P2Xqy;*6#%v+nC;K=)IZ__8G}^(|ZlQhu*8P>AiQ;N$4T;?lTgY9!Nq9B@jvi zp_h;l2=z(ZlD%bL?)Sa*9&6njtuy-RzejVnG$Z}bn2`U!|854z_WDGyZgZwN-#$!I z{`T3k?$+ZQcb&_7-AVK8d-q|r_Fh#cviz?s7b@+~V~7Tlcs9}3B54!(K!#O6nn0R!owupXJfiFC9(4q-BIqjUM#={8Kuj{YDnCMoqj@WU%0uS%^y^Co= z!VYY8+GdPk6&giH;aWpl*B6I*d7k*%fp|pY2N#{2bHmyCmpeST7N(z+XeiF?DaWsL zxFFBt@QwVm*19m3|GVm5e5CmKMk4K~vi#A@{Vr~aka~Yqr(-LPX3u_^5R7YK`hB0q zqUehut!I=2v_jY3X?99+ErU|)QyfS+k9mqb`S{8+C zon6*m%pH`0`*z&p+QeA@>g>+S{Osu3N*-Jb)4fx56i+=V^?BS*T${a`n3IjFcrXed zDcz-`$lN12KlE_7i(4Wj)UPGm^&o&bl%H8&7YLvpY zOgUB9Zr3IV87ow6`EP8u;5kqZ>3A<#mCv^db#FgN>rv%HKQwi2pUvQbimKLFL*QBi zs-JYvMR)u>K~!6~R@!tIoQ@^?eV%}*FfpacBe&J^sX!RoiV8Gvt#pFWG$mJlo&XI@ z9GIPwhps#bgrO5^;9B`BXQFHBoc}xl8kp#MIWO;d_&pGYPN;!vjUH2&&Kw)ys6PCU z1ZZI5aq1F$;g1=CFmyrHHwE#D z#jAgwAW8$*O8yj^2g1;n21JEx{qt2_z9reF&l3<8u66xcU3z2mw$Brwfr+zc>+#~ZPXJ-)*r-$i zG;l3hrk^FoN&qzC5nSDusOw~C)pG1u603o0;j+?Gg!&gl&E0x~tF~0FnSlY&Zr}Uw zT=Tgq$JGB%M5AZvfVk1Bbzv!EU9=fz5y*b zA%-xteE!(D-t$w2ao(Zvh(n7$@z83`>D)_x9Mgo8;k9U% zd%n=SaeY?qOZ7^*pK3PjB~nv-1QYd+ zB^K?LH>RUD`N`f5_Pljk3>qO8+-Sz}k(*Wir2}Gm z6w`!kP1icuk%V77!dvnU@0}-xz%(KMXu?+#xE8(!_7$xEQ$_ck8v##uLz+D2elxzL za}UH^XEidIYi>pfLVnHumD}@91-5!d8S$S8HBv8Y_7@*Axefi4U5!^azD&lOrR&Oa z6u(KWBNJcqj44~w72o=C+!7)8rTw zSCIWD0$o8Kdj+Y!g67)B9ErI$99==shAT+*6=eDhp_aaaKr z!(7YtTx$_}jN)jn1sdjBuIE~gqtY`cM{_OEFxPTD*P3w`j$lI1k34p+b@g0J@ew^< zaWvOz4LR3xJ=Zc!=vkDbxfW=cYq_3l8LoxDC0LITIoEPM*D_p7&zv01wOT{YwOr4& z47Y^|JwNi;xz?-aT8q##XKdEWxmG{1$hp?lb1lPdVFGKzT+8)bE8<^gIoEPD*XnC7 z=UT4kS~or+XG@4g&$U3qTpOk5T62E|S4z*8v4o*vu8q=jtwrd$4+u=dTwD z7UAfLC1wm(Pb^;j#3FDld`|jlg2z6w9MVrL0w2MI>MQjviJ7D16HAmDujLbqz(>rt zUDd)Z!Pq`l{=fX+Sc_0SDxW#aC_>SyW{id^#r#W1m}`J(LUxR+#5Zj1Evj|e$!AxZ zDy~WsE&8$drvnA7m3S1aN`r|Y|R>(p$R$j za+X`|^=MIVQ9(=N(f;uw)4_UtM6FI{jZ8mnx5&D{4XPX_W~R!;_uU>VX0EBl4~(7W z!35SOEwip%y1w z_INRYwF#;HdVzbw9W6d?ENT&Lx-=BCPS@k!-059R%RQEAU%LhGj}ZH7b>g#LH5Co! z*5aM>GQhs=Pq7eD zV|p#l|6K240&5deKIe7!pKC)!#vdkFM3%jZXXI$_a-T>JK=+a$vPaXrmO-Yj=% zF>O;#{&??Y7ZX^U5TBl}+$+roh@YFUw}>%6%j>b{ihF2GZ*jKw5sQeLkRLQMb-HZPaufMplsn>!o}$yq3%uCJ ze8L@BgKwGt+QkIcCZuntOYYH~Jw)pg7cC;BaW2rP@bhhpCS-7rQHn7}O&^40jt{6(p*BJl7fi@>7+rU}XYa|wPdU1#y@%Uk?I zvjHN-`Vf9>i|}FsYZH=UOC|oSUkC9=j~2KV)>9cg@@ftKU2FBMHf7ly_uJ-&d7AlsECSb3<+#yj=(G!bW%e{;)%1!Y z@zUwyaf8o9ahL9 zaBgE-dU3k+2ck~1lE>gV!t(f&Z%hLTC}$aoZFa|IoPN-|2E??p7%u`5#79` zs8+i)@80N{iwUev$kY0Zc%{R&`GD2^Edu8@rU}{7wmh#=BNy*{dW2|DAiv1jF_32% z_QAyj)+XfRyq4VCccSaRf22j=+{QE^JIfRPHp?{ki=Jb|v1yq_`Cg?t2}sH@fwc+Q zcsm)t|HWx<=n|X2d5&qhdj9J;H}Rrd-VI$Mtg!^^sXV{`C*$dosddp7%ci-1Zhh=d z+dsx4a4kGa5VHQrOP;*@ZQd-bn%F*Nh8S@#KVLJbJB$owG*DUVS(}$vdiqW}YkX>5 z?``NtYgnFoCrRX&$znmtLP* ze0?XxB54bFe!a!7Hg79huI?_<>@2|dy>ISf0&C0Br{HY<^NiEHYs-!nfio4;axL7Q z!f*7QSF(c^}lKPx#lS#&rNQtti2i)z^}TyWS$=_{F;9=?*w0w zq_#!iOtnXb+ueTR$?_iIxym;e!){F#Rio4Kkc$(eFoCt@{E_A)pEBWlKFh}@aHe9K zkRI<>^ATt5iwUev$X^}S@g4tc<wmF>dpTdh2?qd-+Q!Sc^;R|?^^)vX`Prby0 z`AtRV-RXFA<_#_;ur?v9KP};Z{x*tFt=Y#SaHe8f?qK~ilW$=?`KXfp#guo!Vn))m zeA2L+E+(+Hyp!p)h*wAz$amZtU=cV|F-=Io`ucoWTVLL0@(9traG>~iZ)$$8^Gg>K zSeuZfyE^jkPDZ;S^KAlWDy9iZ`ynGAU3IfNu}zq0{yv{L;-8xPhY^kmtWC)N?fH2~ zbPab{JDb3nifMW8Q{|%jP5ZiTfyUt?TdmAu+OMhkq60}eCa^XkH3J{IyK~I<)~{<5 zIBPL2pE)zub>D4X>s|GAgf*66J(aaP+Gf&uULZ|7xe_41Uu$dv*TOkZNb&=jMe_AQ zy!^FLvHIprL3h1zbC+BKPFa)NDr^0#-*j{T9xn5I+t>i{vC&lCwsJj-z?o`~3^Q8Y z-qA-EA32Bn;GT(V+IuCha6F5^bO-TB|-|+7*_u<~^ zy~U5OCx{I<-nz#E4|*|ywdIV{ZUY}Ry)*x!t4-ic#kAZvs5_Sz%;E6;egnnQKK(_J zL2unGg_^jSz}kdVsW_MWu5oxhADgf;6^%;rzNEoIzS~=kAI>yDj6dB%G|Bna-RQHy z#e}6z3MO8{yX-5?OU5#P9ZWJ~=cWpY&;vp!*&1X~SzbK-_XMfwc+wrT<)B z@BM4H`!|Cv0%t0w3Arv;(I%(h?w;hM#G8wOV$XwD?!S%xb}@mq328c`5np_HzU%xm z(jsuCVw#YV^HcIATgSUUB^e{glYHWp|0}mw&qN#(SX-{WKF!1*ZYb$~d>v*HI8!lA zNZ0nRdt^%)cj=~Zk-2zgQ6c9)?(eyLIVP|+A*Fg=aBu%P(>rXrP2jAxXtFO=S+~@H zCEon|BCN3l>#3|ASkPDJ`I;P6AkTB}w+UPe=e%4)?v+RM>U6-J`(+IgSa+6qx9qe# zc1vQ&bCcUDYlpw;=-zIeMdo?7e=~{siOTZ5S87`X&QyD3s4pJyhk-?S@-!VqBHtO} zwfnt$IMc%@Okiz!N7nQXCj-j!MqhTa2%M>yCS+5g_1u4PI^KFpf6-^#B+=lMAi4_cT#!W zOoPXW;M@5`ndp=5s?v!$Ca|`=Lr$KGkIL)s{@TeVaHe8f{;s6xw{F1I0Qbq?;i7xC z%;Is?Q?B1gUycc^E!Rcwdfm4Zx_Qg~W)nDTF)i0JT4#4t<{Iebrz5Pf1na4+9g*^( z8?!&S??v92JU?p_xE9WNIiL5*BR>7?<9?i2Q}{*A5+Bdjaf$D%gE+TU`Jc1tR;ypc z5YqpvFGT8fo7|+QLM;MkDy9iZIr>kY^43Z>`c_8~?lV&izgy1@Y4C3pCa|{LQOxy- zzrC`-{b_G!i@=$RX*uIm+rpE)4tE>gmus)fr--UwHgadqTkFLH)+VHQstx?#!qeSl zGX`1&&QwgxtW6fl>z*CtR=PM?yuCbB^pekQAypf>n84a{^&`(h-lX12_j%PJ7J)Mr z(}XO%J)aMJvE02iY=}7V@=FmswZ0oNV1tVZtS#q{j*EDb4Aa~a)ocQ1DyHRLvRoYu zZadVyeQKB}Ju*bJFD>sI7u<0%fwkqXO!PFKq-q{_Xd9cbG8K(VguL)~__%$4d2@t~ z7E6CCEf(Fa?e?4U+Qo#WO{(0k!Ydv+?d?;?CUB-=TFy}ue0WsFz24)e#)yKK^NFLk zL)~QAlWOOsjDoEPeTb<3Ay z0&5eJ`qLKo(Bt<}<+IuZ&RR?pvb5PN@5srCymM|xSYrv+Q&~Iz)(V~H)vEYIo=07` z30w>3yj+thP*{9ltGL_58zPFOnJtq2GkHHLGUgD@J3G(6n3_}+Yn0wySE-Ga=a>#W zJ5wxN*=*nK2lu4+Vcf_+d6MlRZqDAF1U`cE4(mxLd5S2$qjEBLXu*C~o@4soXVXOe zWXUVF9XZZRh&Ol(&vV)1t}ZZ8;3GKiupS}waU|am9pI)-G{nktOc(W;EP~E7*xz&N zIjPaX6UkH6PT}s!K1ARnIPb8Y+^Mb<$!`awaL44ZiNrgGi$(W)9Q?a$0~gcsE@Rj{ z{_105cX*?r*2sW~q+wmep)pwwyvV&oUhQ%n7x2abLj^v9Gu75uadA3dw{fg@ zQpaJ|Sc2)oXBvopJZGgZjvbd8KKCc`B)1-TOXnOe@DZG;SdWk&ANJuH@9qq~n_{Fj zmSDQiU5}_dTo(NK(%?y~3=q1k<$_ z7Zr&nq}e~P7Ln`J|IG8@C4xG8i(MEa@DZG;SdWl-|MuW6*Tb!`1k=|)W)&>? z)(U&($a$U|oF_+y9Ob-Q>qH2A1ZOSQlXo%|FM2y)@>H(y$Q~Imt+Mv(Vu_2!%;)sf zdHH=LWPCgV*TOjuejlp+K2+~vQ{?xd$nQgA{XQP;_i^GA(vd=u-v?;$`#9R~V`*sj z4n=++pyj?n)&-9C`%t~>O_ARR2&@f$A4mIrEJFKlDDwLN4SpX-`+cb1^`^-00|eFv zzmKE+J{FXupp|X!j09ejlL0@8f8{57oQg6#0FCz}n#VakSsZBD8ylBEJvN;P-K~ z--qg5Z;JdrKwxd~`#9R~V-ecDLy_MHXz=?u+V4a4t~W(~A0V(c_t8~i?w_WM|b_Qp`;_W>IGK92VLFkB1ksXUiE?lji#6RiC{ z*6&0ytqAG&p|O4+s{K9;x24L`$wQIf2O@1w^n>Gz@9??d%IIYoXS80~ON;P;{0?_&{oG{Ab` z_o3SFL-js6MSdR`yRbI+eW>>PSOgvoupan*sP_9Xy-!Y&-v`DntPOr2s{K9|fky+Z z2Yw%_{XR_ZlT+mPfw2o~gWrd0zmG-W(E#g#--l|y57YbP6#0E%?84gM_o3SFV-a{X zzIGK2-aCECT1YJ(ftn z57mAjrgzpU^83I@grfz&57mAji@>>!^}z2#wcm&7opp-*J}?qtZSeb0?f0<=oZDCr z{619seVE=^r^xREBN5gHzYo=ZAB(`bjrG9qL$%+B>78|o{5~)eVQujHQ0@1z2%Otk z5Bxq<`+b<+S*OVF10xaE2EPy0ejkg#xsCO}??biUhv}Vliu^t>5@Bud`%vxou?U>o zSP%R@RQr9H-dU%}?*k(d)&{>1)qWp~z`2d}!0$t~--qd)e2V-&FcM*H@cU5h_pu0^ z=U5N?K92VLSYrv+Q+Y1^J~YlFEY zU|hl4;P;{0?_&`-Q?VZSeW>>PFuk)*k>3Z#6|4<@AFBO67J)Mr>w({gYQGQDJL?qr zePCR{+Ti!0+V5i#I8(76_78|o{63J|Dr==DfX4cLsP_9<1kO}@WRQLzs{KAp@2peg_knQ*YlGj1YQK*~;7rAO z;P;{0@5A)IJVkyV7+0`1_lFEYU|hl4;P;{0?_&`- zQ?VZSeW>>PFuk)*k>3Z#6|4<@AFBO67J)Mr>w({gYQGQDJL?qrePCR{+Ti!0+V5i# zI8(76__WLlsv+f|j59GGWTImUJV*Nf$`+Y0|XR19iNWTx&ejlcH)*a;c zfpG0SP%R@RQr9H-dT5$ z-v`DOtPOr2s{K9|VP$HJ-v?;$`%vxoVR~oXL4F??S1fJi_o3SFV-Yx0Ej=RrK2-aC znBG};klzQ!6|4<@AFBO67J)Mr>w({gYQGQDJL?Ye`@pz@wZZR0wcp1gaHe8C@cU5h z_hEWx-9dgI7+0`1_0_Q)XpK2-aCnBG};klzQ!6|4<@AFBO67J)Mr>w({gYQGQDJL?Ye`^1bZFq(b)$tqO)eVE=^ zcaYx)#ucm$ejlp+J{Ey973+cDhibnM(>v=9^83KJg0;c#L$%+>B5tf#V8`hA>OzfVk_pN+5xTnpzs_G#PM>-S;W?_&`-Q|*yK`hBSO`!Ky1?;yVqj4N0h{619seJlcx z5?BxXK2-aCnBI$bklzP#8*78#hibo%Mc_=udf@k=+V8{k&bovAJ}|CeZSeb0?f0<= zoT*q3{619seVE=^caYx)#ucm$ejlp+J{Ey973+cDhibnM(>v=9^83KJg0;c#L$%+> zBCJe}@%sP`ejlp+K1}bdJILs=K2-aCnBG};klzQ!6|4<@ zAFBO67J)Mr>w(|L(S9GMck&(N_knQ*YlGj%(S9F`z*&p+!0+SHejjUOzxOlx1xfmpu})qWp_kKnw+df@k=+V5lKIi|I%r(&$%hibnM!$)x5 zVLkBsQ0@1z2<_gfjQl<@Vu0U=YQK*)GGIdcaw^68eW>>PFnk1ODvnUD>0g{qwcp1Y z88EGVIR|3>K2-aC7(RkC73+cDhibo%H8NmY`*JGA`hBSO`!IY2XDZeMzYo=ZA8Ta5 zwD#pxjP?6a?e}5$2+mZj2Yw$%`+cmD0n^%-voF@~<7mGR!$)w|Vm~=}J{Ey%;hdMdWW(#SyT`LJZ_ifljjxxoJ{1poE{*ugDL5;Vt=u`z zld8lnD5vV*3O*;~KwxXOWONetGfTo1(R|Qvv9%1LJ}=8oc|JZ}BFCL^dmFOUXEU&0 z1H&y1TnpLKsIwx%z2dGImmM&cc6>gRpp;9J`Yx(%lGe2VXSuHB5d5LVW44% z_z}iMmG^Qf=FYCnf{L~QV(pa%o&vt(K*O}FH2Qukg4O4Q^xIj9{a7)OMF%gl2&G~4 z?5G;-$;+naslmQI*#(H9*9&_dyxR;Krd>sJZ8WhYtZ3z#|5}xO%2u=K)jAP4B3g>Rb`W#gk(5ZjJ3GnVQt$l zwFsqwBaDlxK|pr){mnWo`rQH`UX zcN~a|0TUo=al5K)az(z=sdakRt7k*@wBYX+p)_!WaZ#nYnuMjk+L|Q_z6ivnv7J5t zjyeI~aMP~RFu5YHM}`!vL3ZrD3v5x(&A`Wgpr$XD`cF1`R_f4WnmA6<*3=x1Seb!9yRrH_|R29Hme?83BM-|b(6o%}iz+wt`XAUXthf*e$}453>3vqxR$ z*ddjJK|?+4NZZ1Utw)R`sje>|WI^K(x&M267O$tIFn^FaOTZ?A)x^wHoX}LHX^I?HWRrafESE ztsIt>)yQ3!ed>4xi2f~ZLk{A0RoQ&=3Avp$2djHFl=-H3Xc1Ts)AF8SPa4)bs4=Tp z;2sc}A034p#OzA4x>e7&HI}&ISO2g>cQS~_Zfc|*7J=@m53J}xEO!0iX`ks~Z ziZGc*NXrLF+3q=w*xq7eEkbGF2;-tk6j7R0t6G*V^OWRac^9yUyJp19gOV^gp=gtv za@@U9ktN-dpB1_{B>`cwOA&^aV~JCawb+@LoecgOh~ewj#ms}UmLZgOaPuWjz0oQM zSBxvomOn1Ziq`67Y2aE)!_e{yKF`m(-UwkGZu$c8$D{6$gE)3oHs5?g7Og4Gigzu` zmfk305%GH=?#vt1$;E=ct;zDNzu>A`4OTXW9K`LaviauA_r1Q#&nEsJ!o1N*LBkM= z#u3IvRj61F_PAzkc4X0M(8%|+x})w=al5K)GEI7l-sNO@Yu9A&cf7KQ_z}iM)sAIl zV_wKvZR%qnPG_#@sJm3$t}2^vzVtfJ%FdRK4rQdbADo9Flm?D4E~+^v(z5KW8nW;1 zqy}QphkTB@OU3P~viar{a&=2)cJ^v0>)$fiB9w;F!?av`UGb5wtK6Dx%U2nQ6~+9V z8W%5GIj;!w&6lfc2U4;;3u?3R^1TD9T|=lcjxa8&>KlKcJF<3U9S%$bB69H$p0T4| zf`(~VX_(xU_q%W2({FyN&Q{<0$s(|x9aX`4IoY+7l$DIXpnrNxl|x5XY{{=9@3QQF(H+Lmxue zk5}tkMEqWeJM+`4v$AiG*Jj5aCkG<^;6;#wxLs8?nI;`{Uu9<{{;0+B-OFSVN&`n2 z7gg<+Sy;D5a*lfZ&Q-M*b(;-2h}%_V^UWvZ_`0mD!r9tv^!otNFodFUgmF>r-J6m5 z9<9foG>9R_^_mPhh}%_V^UWt@+Sbf$e4jdO%+W7E!w~T!jEic2L~0f=tN}~5Fa(Il z;X@$@al5K)zWH*;BqTj6+`TqCAJ)wxl!np6v|OS8<_&GIq$&H(od-mxOtqW>sjtJh zV%k-N$uxP-P}ZOQ_^KM?4G&s`(!dcKTK*Qf$Z^UZHDcX2-2h@=rc4gY{1!A!yGq04 zrd&gw{0F_*vn*S&E~QZI8bXzEgoY;M_4mnHj-;XNU+*$LDD4b(V*XReK}DFHP-T;w z@|p8_YIf&FW#)hI`vio^E=&`0dA>h8)x8Fr_;RO3C_TTgeW-diRD_|WpP_szMz+;w zLpyy5#Ci8TWE74>mCg59{#}{;soDOY>am})a);MJNp%VO&%zKBZz6t2Soua-{*{#AFT`h1*qSlQ(ka zte%?PeBOvPuaVv&lm?D4E~-!7lq@}K%=Tr-2gIdZ`yiumyQ*xy$8yJ{XDXJfTO*eG zV^NEMzCsbkMfLV(GL~_0Lw3AVA0P(L-V7Oq+f`-rJtky%!Q`w=-}%Obx&3`EM7Gn_wq--40Iw5v4C_gJo;C-!0Q*VJU6(jT%ka3o4ES(6pc zMfvMF>43Q4%MSG|#|pOo%hJHLFfHFF)5S}({T#$@j!iFAt(*l~JG~;`LsTXbMVL$@ zWSM)0{>I35}z8di%So2pK_B}wIyk(T9Z+P$q!XFxk*Un zT#smy^NrZG-g_-#Yw4-%)TFeKQJ5y=?CqbZ=esW%f40UVl-}bjIcOT0gP11dr@f!( z{G=@y+tw3^p?~;L^>l|LQDyVZmoB~9Z)iZn*6irumKG8JoZ`-7Zkxoc0Byzg{81Q) zvkgB&4&ruI*?jZmQ|j78EZxUeEWCbRi%=Rk!nml`d`!$9FK@;6ewPJ^bv0i@4&ruI z+2p2NTRM`MRejis^+=Y{B9sP>FfOW9{z=%={w-O(FfOX|+Y_$Hj3-{qRI$|*)z zgwntf#zhtN@eK`}SBH%}=0bj$%uxi^i_-}B;}xB9r#5S_{8$2_i2o%gOPSLyrlp^H z)&trrv%|{xf3k8=)iQ)S52G#DsV82h?ZPUsMDJ2VMwu*9M9@&-EUxi8Xc$`V@D|)c z{Rt%M<$I*Y>RW$x=XEcLJz-g2e5rvdJ#FJJ#(X z9aXv`+kGIfMJNp%VO&&?JWuJ_!>!rvDd9kbo%W&X2@FfOXVvUg~yJq_6I`ln%JFqxwWtQV)zCFwo6Gulx8x-B{Z@ok~EPWwyWyO@@1 z$o@al;qNQ5ltt58`JrkVLY;@vmj0`)rzmL(9KSJ}iRz z4o<{I_h~9ryT8mo^VRM{r9^1J}YdAz##5Oqc)Gi?OcLc-LD) z+04>`kmrgpS*yw>|K&Zy)^T*dXAt{4pn*jw`o_TuG(gVhn3lg2uCRxS?!8!_M>T=) zZ(A7h9Ji~=CcC7ksQofJs@6bu?c4Mg5kJDXsQR2fOz(X7ie;Rb4TwVv^FyBFc2(JA z7a?gT?4u{U_GM)0LszwH2vx=r#zj>#_#|z2wI>@nFe7Lb4af_5j@wmblU>q>ERN8V zg?qE~17b7`f%PygpYF1qp|dLWV52V-0*xuxb3&fuc2(JAm%M`QPSS^;da^p_(pZGj zFnXAlzm{$ABi+%XGb>njAP^&qXM;S)?W(fLM?&6BI8Af+>c+mk+1MhK297W;sx1|c zQvcuV%jT@a`cBQ3fao*JkAA!PK4=)4kYib!)5;AV(l>M33Ds`F zxxYK5M-Yh62$R8zFf<`~uFa&sd`!ifGzkU5M51aL zLTTS@{LU$R`jyP{9COFhdtENj;psYB8n_mw3AyR>C9S)B2)m!E5pU+{%Njqe2zjmu zleMaB@?WlO&#y$sun{a;6t;-!<@&M{#T>ZP$F%&NSLhe?M5mE#)z?KVVo1%dEcHSM z^e`>&$i7ZYKTI9Xs!a>vxRxs4|FH^a*u?XZ)9LorgISsE8F^sIzHFmY2F4N`yDFOu zmg{hB+tZmXMzB(IZn~I=-wSbPp5kMDDqow;8kfFh5h|*HWtBk>)AC(NNpsN|dBfQI z%`0473!jrsJS(=8PFpsJeRbfOOY-$)A=692c!gtEWiu+tmAk<+Xu6g|+1^9jEdrkt zrsb-^Z(ZpZ(?+o7J$G7!ifX{_ilB#SLhfB{K*!TjY=}D?&coEg=VTKRJy+2piwCma z=byVPiu7gmhLwOZ5y!5|W+ak*HF+tGDms{Dt{-g?@p~ce%-8K6Kyxl0&gM-!0Z|2J zs{k69z}oV=o2eTucVqS?~K9c-I1bS>UCjFz({mRoRSo zG6x&3qJ#PmWZ|9Ox|qO`U|PO4VqrKP_;4sow)i7NRX9mG(69)kEq{-*c>oQWF`WI9 z`Fo2{=V5vQ)ABDu2W+7oyY^-?TeRdI3-)D^Ckn#Yj$>ElxKVUl^>63}zkV!dDUVH{ zNH8s*4XRC{{VEP(Wv4TXP*G*u5g4O~XgRA59Yfn)8p2+^&Ix&LYAM3>0`5aX63tvo zM}OUcwLRp*jAKUR_~#UN9=*2or++Mzs|ID>TLg{@)3SHpZlZhNe#LI@iJ1qDh9ZJ< zl%zRcy3p$;TKU}#h@{QG@4$Bac{G8>&9K7s#M&!B7+PjjubFhnJP%t{J*PFcD-A=a z-Zk2UTxc|o_O0fy=rg&5sui9iH|;+1F|=!l{AK&H>4!7Wis29CY_NLJH}t{p^;!KU z#L~dEupS|WGZd%w#$2Y^2XqCE{y9_9;&%z0lbK~yRA#o54(vNMXys>V*`t2-5)fv- zQiP%9ir1;ybWxW_RK0ClsT&O(l_CsH$cI)z^yA+jY0cEFEduufrsbVXmc35Cm0|45 zJjwYm-jF4KQ5|L(MVR?RmCfuU_gEUtbxL*#XTyf?axtOGyVur&83)sZbQ+tIb~+Wt z{L-#;LtC_A<%%+x0dTvjY-R@ez1pi#Ih~S@sXK;EEf{DK@y{vl zJUSc|PUf}YY;pdw7NH`U@k15R!?g5SUfAyReH6~NANPgcHMJCB&ci0&E+|BMoEpt8 z1g&(Rb!@|W?eM_Zj$>D4Gup}BrN`N628{MuAds;de#&#ULDx1-c zkRx9gqRVcMW<7seU=i_qA@0oo==sD+x?&8=wRoXLsHoZ%2mw7z6OuQ_CFiT=;cR-1 zs&F2r7Cv*EC>vUv(xb!KxLdjTq2_H^|D%;)Jjb!Ca@?3mT9l_Bii~1DSDw3=h~Eow zXTG;sa{Be9(d^y*7Z#zSI=Pa99;W3RXgWV}N-P?~E=^nl=V5B$Gq;HzzgMNB?)GO( zt}TKYr*Vd2G>P;&;t1{YINRrwb0$jHWt zrzu1S)J_DwU}!~{SwfDx*9*|FZAsbc)-5a|zJ{R*Sy?|bT{|NK^G{aaBJeq3TK-Zm zG|2hmNH}|RaF;u(LNL3rBNXNjMVQ$@mCd{&e*yYOQs?)v5o`!)Z4ruY)T9o~ADEVR zGV?k*eY-`lS;dP0ak^Qp1mn3P%=k!1$g?14pw~b(@ z68V8fhrepV=#1M{WiuWVvdiu0R9zauo~*6|8iv4nn3n$C#?zc7p%E;2c#KBX^)+GS z#O(4Ee>sM=^)7MK%&5U;U8)Kr4sKVK%@{*S zM5Rc_aU$3+QxwJi4VyUnRZ?0(-cz@q@Ep#=w59ZZoXMz9rdL3lmI%qz=cV)T{%AHwzQG>X z!tD-gQ-kfj7(~mSQ2QUN{IgS%xF%aM_X`*qV#h&L3)AxVgY8n&H~TWM>(%RvrITu~ zDVqZ6xsiz=!dP0Kxf#dgx=-n(v~t^AtjpXQHW5$5(DDtKzuk4BC+A|W?8+*gfI8`Hyv%Rf?1+HcFf<=5P*O(ou_ko^jzXX2AQnE&JC&!zUj`IKG z-?12XynGYeisme7#A}*k#uST~7#PW}>`LUE%X{6TrB7z|D7JTBBf5Kv*Bigq=+X;4 z35l|-8~eBIJi2MjcF+i)SsB{Jv~n(+Xyq?if3ME|4o=2GO6~%$bBe{4ouK#837q}L zVJx2}+SX>TcKWd|OMabzFzqUhxHBI!w=(-_#w~i~D}RC2mE+Ussq&-}ZJa(W&N=dp zC&Pl+ty8yXrHp<8A5pY9C)|>}3tv>6ohvqzUR+)ZG)yf_SIx7Or5e}?dW4WCxAU{Y zYzKY)TS?HkJ#jo7!L=|ge{WEt0;}*bfK94>7d)xGCQoqIls)aJTBd)LA1=N+Yn_WdT>OIe3(752TUg`B($3Dd%NKw!G$9!eXJysA z=jGkLJab7jS-cj`Lq%l>+>(4R4*NtCZ#qmXKB<*}Fuj0j`7Vpz1zC@ZnOL5{*Wd*F z%0>YdVrsaE5os?|K^!M~}vEo9tb#&Z0 zI1e1V`rK$s-)r56G{e}3wC(O{2?)~*n3iv1E1aGcs9umAzVZ?LSBWz`bQ(pUba3p- z`w{0`I`>5?HnMMi)}gi^_^&WwBEd8vWd|f-3$|ruIUeQ&;$g=Ra2_~zrD0l(luq- z(M`z`5GE2#OV|C+hiM_6k6oQnECFH8$v%%R3s2C4*BY~nC$GScV*d_>AS&!aQM+Vj zpPP`$D<9Jj1)8wv0^eB#jvdo-&#U}iIyOsHR=jiyD=JLzGE>=%{5e5euHr15PyY#_ z?9X%AU=Pwng==A2`hD7Lp-(b&Vx<>sgZ+p-*~-uhvUhRpY8SxlHZUtXayAGLqQCz8` zYV28>EUElU0?m>nrh~ezr>ip#01b0Q<@SK>3q1D<`hkXNN#4mc`NQeGq91J;I@}^u zyTfZ$$*+D*jA{9nwC!&_?<-w)){G1X0@q3&l+2TmP`^Wt`8O8w{Q3Q5P)li?TOLqQ z5h`m{otR%Q>+149W0^W)2o*^Rt$}HI9{nTf)oJP2+Ah_guUOURo^OsMhIWmQNYQal zz>X`Y&_o;k*htzS0r9?DL+9TgK7k|2(9(x|wg}zo9-|A!_Dnz=-<;kVTgeyn3@x8= zJ{6%4x*n&;+w_DpH?>qGCPG!N_RVI`o5AXDLEL{9=!AWF*EwD!95l=k)eC>>ep6-B zl5{{MD(3t%KRMmrd5lGGhgoB1TqM~{^F*&^PLX}NR^0(oY z&UiMhiFERmjj#yZKbVgD9?EYb?1tZ3PoZ5!sH5upszz#4{_h07f^o#BR>f$R>l>(F zxqjA}t9H$vIqqpfX0-h1JiRxc-Yq&f0g3OPsT^uW+9z!t?^B30cxMfF9dEpE~CUL0_46)eK;EN$@$z9mNWF zolz$z(e6WrCLnsX*zf!j?hEsyq2>Ny_GHfMpO!ec3r1MiLunYojw*WK4bS8$)11&| z35oT8PK2w8Y5NMQBc_&WckA&&`P4PjkEEAQEB$hC=d_LcK-KlbA5^XhaC=jN9RJ&#k5nmhm9F^*}GpG3!x&E1uh`uiW z)Iy{$L}h5XCUfwKXWpb~&c;R&R#ZyE5O(iAig@gqo@<(Org1{ToGqr~&RgZ6nuSaB z{{(%aYMHZDpPRBxs9LyPQ)}VL)~r(W@Bg89u?DV%^`ry)`W|Q5sws5F`Ju4BYtB~D zX2nAhaecMrhdWM@wae)Eo`Vt)oxK@p%X7YPhhu0$8Wz6h*_&sYbGdm05av9Th9Q)O z(I&)gx5MLqF2cDwAj0Y^OyF~p{=`a8Jr(ZEb*hHg8n_mw?cP=QQFVSu4{r-pwM_4- z&rR7TR4sD_F|jXW4VJ!pa{b=HIA$!Y;usBF3)AwuyJ(ryHt%wpvg}|ubJIWZSEk~6 z_w3|GPScQSv{3C~7NKG{YfG4xE8F4=`t{pSv_|nZ35cJzl&AThCWiY*L(AXFq?qU2 zid;%R?69L!8ZG`PM7MbSfH1Utb`D+c8E`Se2^nHvK}@JKH|HmxKSmeuh@MrQv%4a! zzQP3VH@VJ`Zjh(OZ*85Ck+ue|g=u-e%OcprW6PbF=`wrUABtpU%a#o^YgSb~(>;$O zPr!bKes6+l`OC0-5p2`RjGpg${^G&0V*Nh>PiwULixN)UY=3PIAmiImv z!dc4l$J`}tUhIfZm~+Ckd=vEKF|6ocDXIVDlTrAHip0D<2S-TA%L8GoN%MR(;AHD< zm{4W;Eh%N*4rFNgE7M(r*|Nzi=r^C1dQd5-7e?>E|%H;Oi9?2#Q&j)WXQtg^|`lxaoaidsE)?jJ? zn^W?d7Za5v-DPQ#;I6y5^*$l#tT{M_-B09Cdv&}Ug$Wf^G94ABrM^xZjb+w3o zn2C?OKF}jL+rbNHAeXHQO(Ytr=5_{(Ug3MeNua$^J?C zA?Ju=XI)Io9g`GoS;H>xXr>Hld5aa1tWJTkL3_w$D-xU77c_v?>%NVS>(kGTPaK?{ zC3tr3pDkMYUi0-~l_L+(bw^Sr(CF3eWYFTi*MKmzTpfHS{idZEJuw7J>W6zJe37O1D_43Q_!pq1OMdn9e zmKwv{(X7eZfzFASr8qu<`v>dE_j3A;X373a>1nbq)Vdy+?m91$Jr5dMxK*xSq(*nS z?z-;UQ;$#Cb{rqUJ&N@{-@BiqKTlu{Ov`)G=9yT==khIS{;ymgx$l_Ow@vWU0Qqhi z^!_77$7St_6#lH(ie{|HkMdWI@rcG%_XJOV`^ciDpP^k=Hn(LZHnQsO1VoqKLBSb{ z{R4!d<@)Z!;_QCzJnYngW4uinIn)38GRQq7-;pDuG6dF^>(r-1S(9c?G|3$VS$kO1ouEDyo-@WJiblXK5TJ-_|5G41rsczw=tsn2nrx$MLV8 zCIRtJiE%mV?TFDZwDbsXF3$S2tIUcVN)I_Wcz{pvl#nY{Pusn_?|4&oW)We_9*nj6 zO4Ty7D*N&MK_58?wS4K4bz76U5N14%Z4hB<=!4eQB!>U9HINYOCo#|#@RLbZGJ!};uLWWcm^ z%7#6m0jt`xDf`R7PW9~Gdx8)A`3T+!WZnXVzm;;YsOx^3u39JQ>sy_GDEE~&c%t}V z(ehq&!yUT!#|A8Ahf}aqZEC4@jh-s6uO1wnA^$(}%#Stsi;nGEfSvd?3+y7BBWgFa z`i?rmQ_d%{T9SWfG;aVK(Cl9ta-kILBAZ&cT~+Rv_*78Z^lzlbt2}8~isZfNds-JX z%n_xLeBrVJPoolphG|LO`;5%Mp5Dk!JFcx~5vtv)XZjXU-$YEy{n{}v=(N9EvQ#xE zL0grZy$_z`eFg7&G4FR#gz0JdZU1vOJ@;dCc5%_w1jNh^Sv*-fCbHhcB!8t8|k-s)! zW7D62TA`t(J?naWbk$yg*@;oR$fn<a{#l567g~beb#?Drglbpz?gG@iKtrB$o##}2t7YCr zp*)8kRiDc|_o{auLY{M-=Lraz!CsW-`g+Jb=Q_{f3YuD2PnBiXM#bhi*Le=-A@eQ@ zvI;W}*E3E6Le3>oXvWdKD`y<8XB_BVQw!^u)sMzHU5QSzO z(1RIA=oyFMT9{U4Ijoh4PhDDMrU(48gpwB5V%&Qj<*ORKLit;+KL@lfoYrOS*zLZOca}omSz5h|+f`+G|9B|&&Qj<*OVBV!aKB+axU&@c&e9@OyYkL*Il8li zUWYXRq1OPYeu_JU)&St!r=K7XsZ|R3c9@_6LazZh@d^19ABENc^mkXT0SLVYkU&E| zlOIBB06@bUfY56IR6q9}LTdm(U~O0f5PA(D0U@9051};xpkWO_=rsW7t9Z}kKw$0v z{sf6NOiTZrFtyaRF*K|J2)zd2=x6fHXbk|afnEdH61xT<^csLg;Qq0%pj-nGdJVw3 z9#~I5t8b270}y%*fZ`*#f3O~`0SLVYU|kPP>!s9E3EhV-u)c? zc>-%-nh?3xC-hpMqu1M_%&MaqofR$DFQa1KlrGo$gkI~j2z(6`VQ5(E6MC%=_E_Yq zV=1-jhzYDM|GKN3V-UnH5?SyNXJ#y~5}6%!{=Y zdhOMN2^@)i=5p;-=(X2iOz5@Ol1dNLu=XnS+G`-LrB^(m7i>bVQwzOL4ZAXO)w!fu z|2DC!sN`CBl=>XjsfAvrhW#i*C=Izz4N=)=E?4MpO7o`C6ykgVgEzu{SS}cn}|aDAF&b2{SUADT;3tS z>o4^FhecpL`#j|ShtT^U!B|5@B6nrBt9CIRa~^vC1FoQ{r3ktI0q0>8a<53}y`o@M zOYT8!H+xVfb`_P}kMgR|VXsK&y`qwsz)>l}(6CD;^e&l4?_fo#ojBaCD$89mulij6 z6^u?}gx)1{FoE?j4ZCDQ?~(<^C*&?!plTP>u+J&w& z?o?-J&K48ubGgqM7`x92grpz`jRvlTY1j=HdN&;MT<(VNK)c~yv>WbKpTlmr(7WLY z2-Bmgcjaz4(6AdW^lms;kr)lVpB;N`3=O;CLhpt{b{azOX9Iz0*bNtYHyn1!459Q) zFJKy67Or+#FzrI%%D19@65Ra$N4hMy`t728x8dYQu69{igz^_ip9ELG;K4MwEVy=A zBp{?uf+Lp&(BQIgwaX#_Azc<6xh#MNmxWimEP#-X6jwdPYu5u1`24_S;ngk+rfNw? z3Xe}{{{(yvE(@=ASy&qXhrRQT)~aR}{-*aLRRQTp6={NS_D<-%N$(dP!-p}3%B2q*Uq$nzah!h1C1qG#ilR1-z2?xIS{&m;)t@X0*>)q>Qewpl@ zWRg5dX1EmBhckeLq+NtW>^Pwt}ilvnDAU*MCbaF5pZ@4Ym?J>t}mo>eMrD*L;Q+H$`V{R zoaK!FC8TqG$q4=xEbV?PlNZ-vzo^k?Y4=-kf}h4Tgr)DTgU)i|sY!l<^4>ZO0cSb! zD{vamy(&8QDoqNUdyUKqTwVv}?{FG7GXC%I+-pSVUXu}UN)v07(|GPRqI0iE*pe72 zOK{zA*5H}%LOS=FjIdvt)1C+CkQJRnmUI?4GUv2;+36f|WISWyIpm1WAtxj3SKu_B zLyqViG7@kOIrc>@!F7W#*4f{MbPhQgVZVa==1_+TN%ffEy!rp=%yu#Y&TNOq&T|^i z&?`DaFHH)ZP>aE4yd8G47{UzEks^dX(0cL#x|@-VfV&bAyPJVD-px>S zHv`|pO7Rk!XSjh8#_ncBbT~;+jMhD!LNk-${B}I3aaQ_4D5{2yU64rsy2Rml8D+70z6y04) zM!;R7klkIP8icz`itaAqn_DT9uhnOs;f7KeySt?5?h@*-Dds)k?o!C^E}=fWyA;vg zC5JHi!mXsm=38+Z?=FROcL@nDW$VKw`W6fm=fRoLK`k4_b;x>A!8t-!|y3Z+0ij|@pnW)1h;6A6K`kqr;5IaHm!o z&-E?=cV89VeN9Hd-B)F9=W-hF@G82)D@=-&GNTOKKSmuc0e5&6-Qi6}z#U$ETTVli z%GY-)y6-GZij|@p%UB;S0r#C1-FJ2fW3}O~HWHl1`_78)JL8B0H@&eWoS>V`R3C8P zSVUiYn(pf3_+j%h1l-k+jK=~_<6V7CclD95 zCE*0ujd%4G-PKPwxNh{FpvZSZG6H-j;CpZyyIe?`Q+Um%U%FEn|-z+HW=1AJLT%=-n>=>0;I_X{L!*-fb|;l2lWzfk1;A{hbR zFYwJdjXqK|`AETCeehWlF&`=T8yWu!=p#jwkCbEt_(+M2rx>%B4n9(l2H)z_t84O+ zf;;Y3%H(VHnP>2k62*L^X!4PQ=KySq$rrqzB-J21+nmrxiY6Z^sKZL}e98L>((oI= z$>N%Pq@WJ_eYg~-VLe#oaZP@4@XUnO&VL1V-yHnnX!48W5aW{k;$TTQjec=7`Ni?? zl!CiN`0J@lfL|OrhKSV4K8J7PTMCo!F1P7E)*d7iN~>F=I*2Hv~` z5R)^f!ctj+>)^EKwW}yY@lU**e|%pYmog<;x2y^3b4&GkqheurJzlmW9ro+;5^@^$ z&-Z5xKY&t=9kXz=k!tm1Q)TvUc-hT!X?TyMy`p{a<}HDEH}lK*9+u!bIPH0d8Wsy5 zg|FMV<4JrE`{t%3Z|-k``MRareqXv!fsUi3!xFrNoQB=-#i_$@fEYKe0@lZ+o=uh6 z6XInz&qrZKp0roAmfn18;>zkKUrTTuoQBl|lxhTg-GgiP<9paQHzj%0OUl6GmtuW9@>El0_J?@c&GVj3Nq>hpCh+FR;LU@T z2k|{D!F6yNevO*HDe+0rQE>fad=LBPrX+bEOUl0yz*WteTv0YUXKLi~qIz>qE zlGwGHTdFz>cW+$??~!F*k`7B49af(qEDdY5>1)HYzk!a-FUOe_YctH+Y3IU5XiLoV zzNwAKmXut%A{=ljUQU0oQw>g-XKQ^p?RlF&S`qI2Jzn~QW}yx))op7d?B^~q_{OU6 z4JcK%Bl{e}=&+W-5SE731Q6Lk$E1CWkl<2p9Bzbt&?SxySQn~a<7KzzCmq7*u)Wq0 zmImLSK#T{Xmj62vT&h;q#@O#%V(j$Q;Rtw-R!jCfgwbJplp!qbdHJTU4k!H_FPo+9 zfoo?jRrHg_*jMf#YL5$#{}nG&ubzxLEMaum)@}$(!+8GaxbW8ZQ0Mzjpbjo&*XJ(r zS+ne6SFj8%Iz*_$5=Mt@ONOvC%uD{M8&zEqFRRyx_f3jzKW0@7J{qxInP>2ANbRcN z*=kYGC|(x(*NOM$OZY*#MPZGN8U{K0tHr^xT9!xBb^ZA*r*wC5d}l_z>P zgi)>4G1S4Oz{@JOE0<`R=clmf$#_}d#*+?VblA3J2upk3=sl;y_LJh}==2@&7ZxsM zf0J>E58j*_s^jtU+};zY!xBb^ZA*r*H2Q&5QmvFBz-Oj1J~Lec{EJ59Ulf1E z0q>oP`4`1LVM+!5MX7&)f6<8ii#mkSVS5qz7bODxi$>&MG%$Ht9fknkpNjbxB?A15 zM&w@vO`C-C8(KqZitUywD~iYz=Z6{qIKPe;0p&V*cBS0RQcZ{I@650shn#^WTm%`fpd{za8J) zN|~}-edZaww5yo^b~P%=e>hr!h`LL~$DMJ#5(xVJ)XiK->&PaWev*V#*GYFVKqecfzh>sIdd>njpOPGA^m|+M@ zV=N&>v4qf;*m1(_j6pP@$V99&&*0~dPHaIeAw{u-k`WLMNZBZSmd02@ied>l1VsZ< zCJvI(VQGvdq$rk`>i12l%j}Jjt(xxX^cyzC@z`c-^7#zVyfY2;C@|*OQt9; znM))^7K;& zDvH||@Dx*ah`fiR0e>fq+ovdQpF>a_y%=F>&x5#qisJTRsccC&!F6NYK1Ff+9KyUu zY+Oo;mWcX158@~)ilZ3td`;OQ1|*IK{GBk4qM|s84nc7ZV}zwKj-sMCiVnd`#c7PA zs3?vizK1QlA?)ju?QCv&GQ%u<*G9`{5{GBkarJ}f&0VgQVWQ?#hoHo4KOi^4* zhv22+G}L*TLW<&A;(OS#8^VriE&*{y6~!49JjImVp5)-~1a&_1F-37k9fF_t;55b= zRTO8`A$X}cjd4a5#TmtZZp&^6JCeBs#C=s1_Z2Oi9lgw142awsnRv_Q`S!V2i2JH2 z?rSmvqPRvjhP0(I?yI7>uZ|Tp`C7uf`R#EK;=U@1`|9Z6Qk;g<>Qz5c6o(g2i$E;g z$VALGgdNEY0daU~#DO@xisJAlBkY*LX^g|GC=M^4Fy&>pgwbJjV;o*Zad?vvcDyn= zER8XX6~!#ZagdiCq7I`yci#hI7AuNb><}hjJ7yTd(inSLQS4>6MoaAy5PMmj zPl~9oG67!qOPWT~Qo&Y>Bo%oAXc*i9LFK;vbQD z24CKEE*#>xD~jWujDV=@(d>FZB4KHa|S+ToZx4Ec87#+568^Y2shg*MMof_R;-Y@bY!KFTLaWgV| z|1Pm$^;z|M-zViG%deim#a*$WYA&d^& zw+&%wn77Z_p$1*F%!sKgPKBqme-{6ncP?b-P zn_Ssx?OdOsZ(D}2-Z<5@SPJ>X&#jQKR)$M)8crwNys9e9Xe>wUh=kFeq18Llp0_;e9FeYw^aJ#)9q!`Pb(n7rFKqG*w0-e^MaJRe2Nb8$o6eyo^Rjac`MqhPQ6`3rte?IAxueZ%jdM`4Q}&?TKjzy`Q~TaoE~MQei`!| z_EeV;DO2m~^Xto7IkzHV`!gquKHLA{H?l9&>aByS$cH-q>FD56oc6qbzR#d@4{aeo zeR5zj9b@}Xz&_{_Z$FetR~pnz4k|p_A&d^&YYky(IA() zLs%MadOw;~dn=pC=br401ebcYV~BmwCE|)_)0@6%DvvDd<`DR;sMi|8(%^I8vrM`` znuhXP_!bhjcN(c;t@lKy8qCJi>bHrMzh%=ud{(P4Y7 zAuJ8?Amg*?jL$ch8D6i61ee;Aw;J{+muOrwr!Li}kvurMw?h~mwig+~(y&XmA-7KZ zNmF^Dc}66-RI!V7u}8VY#1(nA&d^&iwt3D@afe4e%-re6M5^Gf3Y=i zspj(@#~$SppDuntSN*(^9CbaZRF*J0Y%el|r9E%Ydk^U4pEi;^e#wNT;!`^YUvPfaQ^+rv( z|LbNBVRYC&VF*iuPwJ)x^|4(-J~}=g2`&}ac23lE)EvCcaho_jsF;4UeFgdO*Q=5d z5ScP|)56m5TVKux^~&KD<&_y9BVl`vDTyUa32hICGpTj+>Fkq)?A~{lqk~Iv8gA+> z*{4=N&|N;Uc_QBCus0;lt%Rn}E(tgH*lYc=lE=ldSFfu@U(}WFo%q5bOun|)a@zBL zdFQ5DT)m@QS*$t|Tq^I+pM+**>JlY4XVFDwHIgrVHOV224%_bxVQJ5MZRdSDe}@+G zqaU&%!KI#VcQQ09NSF9}Szi75*A3;}fc~X^Bl+-&?Qz}DXz!;SW|}E z)8Knp!sxL5&JdRNyji~$*S)^0DWCX0EfQR+R-YQ!2VLTg`4x4!UX|tC?@Bv_(P4X$ zAuJ7hckvZ zGQNy`&?UAG&Zf8aY#~>ET)`oX4%_bxVQJ5+wm!S=SmSY7t;A3G-MQ4h;csFebctEt z-LHGLX)KTT$&5NIVRYDjX9!EfnWE<((De$#7}fnN)WM~Ge`FQ*L6apK32df=m4!zYV>8xkSThg?0a9_2jvSb~=R7Vf&pS zEDb)k4i?s*m8&nO{QMpgTq;B3W9W&?CAxlHT7UmTARlg%bid9LMu+WphOo5f4f?&b z{`7hvzn&hU4lb3mLR##DE-|EZpr8J^wCwi&MTamtY`-&vr9E%VBY{5oTWJ~h+%HIQ zsqtNNL^V#&30$Je!A82`gxoT?P!H>z6ZWQ_AuJ7ZpQr2TT?_Nb)eDCsVf&qtvV(9P#EVutV3kfcjy3woH2VJ6X;X*p~pa!ze8%rI+=&=3H5S9k7Ttf=!bO#&A z8b>=L!KF$*@+S5{m&kRXtZw(EFS~S`>kvkV?RSQ-G&@$w6?Y$f2p|qa@JypFgk3%GlZq#Z0+&d`d01|vQo;aNN}mja(8(1B8-C$@n*lr z_4E^2<@|TEV!Psmz4d7bOM71F&dqh&BiUsCo&t$jzXK^tm=fOZweJt8qx)PcDhKZP z!6_A&GCC~nd1L35*6B|R*>KzRV0)=I<@?juCl>$b0Q!OCJ~MB7^1OfM{rb-*n#rnD zvpR&y*Y;X-*V59Sm$GmQ{X&t>^5Ted$q3uGISpU8#{>GuE)bREosswzdA{5O=4~Cj zWl`Pb^}2G_JIWzUzP6{DQdt^W`}(4KQjvOc`L3Qwa4G9q(xQ({hI} zI&4ohgr(uM;n7EQG_AUT*`WjbctQBNZqJZX*qWH^A2Hj*q&+#OM^fC&!yhktF$chz_0$DQr3s1 zOH?~vUw?PFuq^OL1=L~lHMH%ihOjhvr#w+#?>JgmZjMiS4=!bWKf1)W@lEvk1Nr1b zn`)sBOBfxtry9c2aN6*NCi?WJ`DECy0}@=ydO&oE%@sQ8Iipg^BQh1<6Sh6aNLj+@ zyWMNYZfK#ieV$7muQ0~Z!KFBzlf(M%0G`)^eI+b!ZssV*w_ z%5=&++rQtNFf=FZGZG0V@+mweOkeSx0)Lk19m3p8G|xt#eNOTh=|e$fXJr|(Vlj(ZJ>awm5Noc_;*sb@~# zl@LaX>Oh!!rW(ZOdlOU#g4N)EAgB%mt3jmit%K@7uo^`A-UQWwU^R&Jy$PxV!D_a>+g1gk-$|1-gA5NTMSZ=MtOz8o)y&hCu&IoB0yqL!9A9+`W0_NJzJ zF7Rq2^-|4E&};KgSsRu(A1|-g8-{mAEn#ke@;k$phCP;iS)%Ccc)4Xv=eYV4%gUuC z8mR|2uR|R+U#<^MA&oB+HT^hVJ~yg!_@6xzyE;lu8WrAvd&0w-4MT5M-BLGI{TKX&-Vf}1n0K=8=XtMIsS}>M8ZWoy z&gu~6PPOqrV16UxUjbHLyYCOzfM0`;^SFeuoYsp2r{P@2inqgDP#=vte7n`8*iu%6KEK3Bndd*!*S$>` zEWuc7pAp{rN^x@sevZBr6Y6yH)BpMzKBJDi5?pHA)?@xtqds*BqegH0{PWfX9>hksSX@o9X!0gIi~Ou{?AI~WS+dA%ADvwEV-A35{gP8_v zeY_-#2kaO5|83;9UBHt1$B)gQ!gh7?aOJ4StW8*w!tli>-dX3#2rg9%L^ZehmTJtsydBa9CCUxMZ-rqA1Vx@)PD5nSpH-Jl@ppR0r0 z60XlmoByAn@079@=WayOJki=KGh?;S_p(<=Bv>g!7=2cUeKvoG7z%49`%ULwiuRWq zqpc-NSs}@)`NtZlRAD-`KIciX(QBfr>^DTleb2_d++<4hX76~NQ0=6sKBS)7_Q-8J zXlR#MyWoKT)5vMjnI{ajMs;U9ugVmHB_H~ z4zAB7ZhX+n&-uhB;ia=K;Yc0XDbn8PpuQ_;Z2OH{O=~QBcX?u-Tk-nx>Yd`5ubQaI zP)w}21hv0cr5g6q=X!oG`hM0#=`YhMPVk(-|8AWz z!JDT(QAKYIkenE@=bR`}y`d_nTpCWXK69$Zy6ls}xqroLE@fK2ZS7nil#2c1Qv8|c z#Khr|OY!HL>?{1gO(dZIE89lzY<zj_PZxY^L^|*s_wVf zMeqEzV{@&12Q#3z*xT|(50^UEEUmo0A@~mjmnwWHpB#L?+*!Z0n^pref)#l^p#o~k}YRsRhk+8H&%$>U<*tTzs{-A!@ zkf#(bbX@Gd+DxT7o65-vJTC5B5Vxt@Xq{qvS;2|Su;TAQHyiXM9La~GVNGHvPNi;dO%vT2?Ahg05JmPd!qrqC}=?X2%?CDtZ# z+Vf8Ds2ogrprk%Iua-j$Z+AunA2(FJ64Rj$OM|zC>luSt3(M%5i-qPj#R;w(e4bS; zAqEudsl%Gbow`lG@q|cKwuuU1{NOai$Q{&Ad~>C*zV!JoI>l)? z*LUq&@Lkbi`so8xoSNbUFD*p8T{>3GuhmnR`DTaUb-T3wS7OYn7HZMPbc)k3zZx?^ zw11l)?(O7-FXqx5;y|wHShq#(!zxbz0TXk$Ld|gI^ zx9OdmgN>`(=o7OuJFSEhytJOTzDzlB>ez$2{pq?6u{8f4@z2@z%3G68aT;!bX3QZ{ z|5;q;YhTkLI^^CV<`?RuMy`VQU^GM-8#y+3Dnn7d{*`)?OYxTPw%z|)xc^$XnB`=@ z%Db)PzZUMl7S6U3ZlnHxuyA?LOb?fSut%J(GeEXdY2@(o_0)S{InDU`)|v+DF%77({)?BqKIutgBw5wYsG}ulTuxi8G4s4*uEU62CuvQw;sM zmeNF68czKF`B3=#zNta8C&23>E7iuM>168_zB&yAr{NZz|5|wSnRkNctH#Ucf5{`O zW)oWEZde!sj*2dh+0!^pXAT6lThir~x4@p4(V zQgT;{s;b>fX%#29Zn$@M`n52Bm1RNkoGvl@MroO~Jo>p)uNGI| zE>DNQz1y_`*A1)f97n?pnM%rPCu_;oP#=%gXs8PPnFd>lZ5gJ;oZ8-4ojGoz%U^81 zKV0(s4N;_BH;3SLUKIj9vQK=RslVL*ZbiB1-#C>OBGz$&>-M~2uXK%4_1P;Pp5PL^4mb_| zMYE@iN=)A?wCeA)60XnGf86{kYRK?ZP#@`<*N=9W8!N{3d%+>NlzFyo!Sj~B>P64y z?<3RfKOu7IH1f|ZjaBtOlU8@Oo=pwDRJW=6bw^6j@x+3x(d%P6$))+yNKWutb6ZLO zNzX>#`<>*eE2(9DsPjxI8>)#{u1B2UIl-)N^UKkt3a#YSD)%`AuQg74UfQ=tMe}L^ z5qF;)m!*`v@_s#4=8ww}C%A5i`Eqeu)Z}yn`S5QA9fH>yr#)}&k_l1nKN`r5{}hzj z=T($FCe%?qEBq00g6sA?y?cJtXO@)p=9O{?UW1&5Rq8jbqt$1mOn;%2)2_HaQ-jlA z@YTX9CVo_fZBwIDwX@6YQ7ebwQoR1b?hZO1R+cZyiDz?&g;3|0{3fdP)TGk_ww_H5 z9{i-KD!S6d^E%xnYc#me2sx#8FNff@=C+cCTPj5DA0Huqt<+0&JD*1ue!Q_NpXPkT z39cI=rvF$e%2Z~AJhQ!zL-1PTH2A>&Rzxd{jF8#4_7N8{m6F>EHc|^xeidxM`I zkICrm0wd&+%zYh#*BYlGcFm|hQIi|P<(yyohzW01l>6UrpdMRzAmRkq4Zohwh>tE8 z9xnHVLmh(GAg7_VpX(m2^oPsU-whR9it95qSoTIOrEXk+`uMKHtSINA=CbO|+)lgV zQoR1*H_IR22n<*d`bE`vZsG6b-62RTt7<#b0!6J)0V|zvy`0qAX{_yc=JXufLi` zaDvyG+e%K2ND-aAIYu6-_hG=LIGy8II{ECYO;qtTzruU$>Jf!=>%_}zyJiJEh1VL- z32t*_N({^QikA(_jSsjKr&IoPzwGc!V^!|&1EAyU`@Ra#wgErepH2*T3a>SuljoiM z@N78a^LUw1{YRfmaeCXIrRDwI8mTLbHh_+=GM0{RE{vCFf5;H<6kdZoCpZr`_tB{6 z(s(I;`Nro`oIX6Qk}Pz&f$Ep?oS+Y$neNlfQqw3b*vwL} znWc!$EaPZqnT&wBp9)@pt+%7GfSj#X6e(+(jjQ3C)msqX`EU5G_%w+ z(hD}TM1sE`&MbYJSvmyG^aPt(B8@XkpJtYtMtZ?!mPl~jIJ5L=X6X<#(-UlFi8RhE zeVSQnnpp@ovqXaH#+jv0GfRgsbq@1HVSZ!h`fz3$(#%pDDVR+NHnT*6>&BU-PcutR z)*MT8mW=ZV|W|jfXEH#bWg3Tqx3pqZsZ@H*i7aAq0M%u>@lL9m%6wivD(XO;oYEFFT^ z0oR8!%YbHV>~CESq3z-bO>H+Tp!LX1DaWC8n*?TSz;UGx^ZS1 z(9F^yc&%}LII|3BW~pfgAlS?j+Zfl4Gs}QxmJY#dkn6*lWk54ar&*M~F9fM%A8OL2Xs24Q9?Vl&HtW|mI7;!?c+ac1e$%rc~zo?tUe ztY=e$FteodqA;`cX=bT7!E4QJB`~uLXl5C4DNfT2P{d}IKFus6p2BO5=Y%s$pJtX3 zm*OC?omI2KyBc8%*kmrOm%YbHi-eP_Erh$)<)y1K<8uy(e+IN<`EpdYD#(if+`_6(Bv~w)jzBB5>eP>1c&JIEQz=G{NBaQpc z5$!wM9bT*{PH^40@2qIwS@BjvJI8|UJEK0_cUH9T>=3kbEZDv?(zx%eXy4f(Xpcp( zeP^U`-#MavXQ!2Lf|nNeofYjnI|S_<3%2i!`e5YDm_yONvqR91vS9npNaMb9MElN~ zOYxTPw%z|)IM{douZ2spoJsr6Xb0U^@?Q%F`_BKhaM()zf3t8&Ck^qmp*`h|zYfs} zN_;kBEu2p*9y`lPP=e-Cbb|6f5L}8*Q2qykOVJ6+|3GjlGkdkak=^?}xRiN@bC$)- z+un_EPO;(~ht9xaspu3dKI3OfxZ~U_5?snYC(V}Nly`K;DR03}c_-6{r@SLNHdd9a4Bvjz*pFi?VFQoK0%rkxMz+wgX?n%lE(WV+-BHk z)6bI;31EE;VekATvmcg7e(L_td~FFWk!&=Obbl;no#Dn|Noz&n{Qhm5XkNvX%ptsu zcn>nbeCu}eZ(jT?>Dfq|e+kJ5CS`6P_J*5+$xB5y8Dk}}rQ)T<{OLal(%dqv1bLgh zIDJ={l9kFS3EYFs1osJ@QY9liCS~p`z6Jkt2rQMki&z@o5=+HO_&+O^lP{Dg0G*DG z2l?!*O<0oda1V0Sopqj!;8Nx; zVXwQkUBK3V4)=KR4Ot2AT`F@I@y6|*6Ql1+B$)T$Dg2%8TB>9Om%2kYWDezrf36N= zKddd``jSNPzd+n61$Pv&4qO86hWm6kJfxf6!rVVMV=dh7#@{c^-{EFCKI7eRpYDcZ zq;a?#9wY4U-<*aBV3S7qbjRJ%LATT~U%KNi%&mXC~_C+3A_f-&3k$exZNMn?S7x`=L>ebKSsmtej?y@e?YhUG48AVeV5a&4)AN>lV5{G z{)Sx2JcIuMEFta;N4(Txt9uX%>ZtN3@% zgQ(L!MV)pCu8-3gJv*T2*+@VvY+<4~@O;fPM3BY5dmcp34k&tdqRH11=9!o9cBvo= zctBCWQ3u5I7RIM2&(}OdBwYMEoU*7?D4;0dsKXLmAEz;zc|g(3Q3u2(7sfX#&(}Od z3}E~__@?SvCZK5MsKXLQhtac{-VfAqu zqs#jgT^@TB#EQrEV+h+y%(HDho(Iw8{jN#T<+0aV!t_CiE|2Ys({R>c>S}+|&+)QZ z+8(F_V%}r>;rW_pTbn@C9_JtbD_*8vJsEXa!sxL2I1Q0v2dwkQ{~9lA`M)Fa=Kdzw zet5p-*|r|=k~V#{KjJqycfMr5Ll_-aAEzOf>5Wx>HtL#Be$pX~4qn3BrP}<_3V$c)@CVI8V*A7<*nW7v=GoRJ*mfw@8}J_4j_h*? zql1_5cB#IczSchrM2gHWBLSXN(1!DT&9iMi;7xbs3V*<*csc#SPFqdBmN3t}gttp| zY*&@I4{w5wPEn#6mmYa4sa+Yuwi5GfYZLsDE!{CFwKI|Aa|2=y>l+-0>Una?RUGIxN9U$Z7BdJ%3Z&lc1yE z`pHnGe|Rj@Uoj{ z+q)n(N26kX2sPL)b1HlfOK=^WhA1&ms`gN-U58!`Jfps z?F`A-L^~#WUiZr9<2KX>yIbYWWW@Y$o8WAc(-0}PQFg!em3X;2!&n>#i$dQvb55SG z`Bv6Gg5TOz&%{msB3_QzUI1GLCu|8h4Swk7?XFS#TD;7^<|$ko%-EZxgXe3$g0*~L zukycE8WQuxggy3+MMW>LtS;=rTxO1zbuM9=l*dKtct4l z87?LrdqKCz@M6S?Eg$yM>ldyFyXAN{Mi?4mdhh5Y&g2=XU#Stdl}m9t`=EY0O1mp; zyKxc_a~>-yYN=5=-_Z>bPucSB0R7+xUxtMX&5d|YaHi;15mES)QTp5;>mx43=~XBD z>;5~Bg#YXr2RbUu+aBC{Y>ckvO>+n?^~39f^(O<2G}S8olDcut;o{j;J$!LLW_ z(HZh9F135=Gy0t`et@$~v+f|$Wu6n1`+k(paQPv{r8r&q@H0AXg}=k$Z}f#yT`4nr z>(sWR^}=Q49D++7%s5m(J^xggVdo4WmNsk=jHok8U(XvTp2CU4U54t}Gp>h2Qg%1r zszLsse3wyrdP$*p%GTe8>T4ZuhHJ}qjd)I;_xP|>L4nSr^tvumaj6;Ahv{yWZiNL^ z=R1gF?_cmAYd=b7sa8{QsZBkG>C8)Rg-!qHbO-U=(&PT$tw-q>w%1f#YTlS(dgYA*37s=$I7@=LCwcU>7O31=@7dn4bzWi z_M+WoJ4KxKyo!TA3Ll+4Qm<;?QgNv{rH1OCLC4G$GwvW}&YT)|J?$u6ZeK&yZ`v@O zHM>dFgGg`=04Qav`Jh#WGczOKGMN5E4!F=O^H?fG6*;rEU}EO@SaxN^Zr zy|8FY#Zx9!9ICHFzP~P>5%HYhw0e&A;gH26b@#tpC@#h6RF#M7zKc$UOBb0^{W$3n zSzv8-eeSd81D?W(-`*Ui`wjAsEao|1~EaN_lMhUrw#dC}|x zrc{@Idp7)F_(;9HU~7lqQp0Kt)&BdZ!Y1>Kj%-67i{AKTgzocgXT?)Ev10x(-T!$n z8W|cL-M$?YKK}ejog!%M5L~KD{h@ltl2c)USw=_7v8kf>^N-Z6F11%Yg%dfJ57Q|| zdr{4aM#rjD6T@=Pj@0o(TRQ}os@H0$KK9|M@ZzgRN5iL9CtiAgr2e*QOT|+-(R9Nw z{nmJB?d^!HQGiv|%&uAku!Y!C&+DNPS>?L&c>yz2MJby5cb}T3n=qDOJtA;i=Lib=zOs zE1uG6&rm%RYAsi>84=IP^UCxt??1eHq&`xkh2m12KKh%pfDW1ZK3iF2RxhGRaBY&+2M3L5O zqFNQ7*Hs@LuXxH!2`}gk&wLlozA`o9Il-E<+B;FYNzdyZwZ|zg#pyh?N9*jxQb%QU zWzaEgQAL&ZvBCO8f%g?p8QpKRe)#5&u;(WeBc7AzJu)SudhLy;^{&@GP+W@B&y9%J zGaILj+AS#tI=(M6Qf+&zkG`>Si$id!orlKga?RF-TfpK%kE&E%&Ck(a4~gHRcnT-F z_86-lK6pLcSUe{XS>tA@G=sY7jcxZk1eaR-c7o3P*}QP!2jhXbZ^2XQ@_?TDr(1g! zPvOK56<*e>pZqOMojL^&wKgtNXRfu=Pp&%Y5L_zd<74%;Y_El*hnV@*kEN%n(S8>_ z>C88Zr*Pu!wBz-pPrnUUS3e#?jFELq)a>_K>#yqmta!?EuT9V+j(r*qoS;IUljprr zd6jyxX-l1J-tUS_4Nv`&{`}&L;mg6xkxP8B?_+f-XsEyMc0+L~PXC(sRh?n+hvDvQ z^PyC^cWqOJ_cqk$`(9T(vpITEEoSQKPh)OL4k>t%>@z zzovx;3Qq(b`+xmP{g_JVYLBPUJS9uwIGyX=j}t$y^kT$w0$;_!CAFziMSc2mCe5Wd z-DkigU1U|)ux8<&prh?KX>_0bh4k7ExiwE|uy3*+|3%91_f923o)gTen_f~w%T&=D z^Q6~Y>Ts$Fy5fzSMKkUn9=XJ}bh&gxomuZ_nNM>mPXBuUR6Vq8j>Losn-gJeP_KY) z(CV^!?`RQ+;8FvAc|||Js(jfUe+JO5eyLMHSMPjTr9Vy?nywXr96et`BBQP^z*}s?2wkikICbVCAJ~<&{7yA)l?he75rPV=FI3 zE3bePv=Z{!$_r^+c_~_X)!J-Qd7(aBc_~_X1zd{Lv_kS@ zE3b%FUJk*fXjSCLR$hu$UV^7^g4S1lY~>Zv$}8Y0v?}u1%8PWs$}6Ikm*7&grh@y) zv6UAQu=0v%6ae75o;0#;r= zt-J)6qP3IHR$fHF$}6IkmqXA>C}1luq;cit)5=S5DOw@;3>4G^4ZFZbim5Xrk}LOK>Sp)B4Jft-K;yc?q7v30ffqv6Yuk zD=&xOQnbGEV=J$KR$hXqaDrAyL2Tva)5^;sxD>6g{MgDXpp}>4DV(4cQV?5t`LyzK z2rfmdB0skBildd6;3=G-^;HmCd4;s{5g%h+w3Suj-fL30Dr_f5sXDct#0V}V7R$hWjahg_0L2TtEXyqk%3azPpw(>%K zxbhOT@)BH%)3ibgVk<8}D=&xOQnbGEV=FH~D=)!QI6*5UKeqA`wDNKYE=B7rKeqA` zwDJ->g%h+w@?$G6K`SqZ;8L{4@nb75K`Sr8Q#e8ED?hgK614IXJcU+8K3jQ_4p?~! zT6qaBMJpknt-OeUm6xEEm*7&IrWKMOTX_juc?q6ED0iP#>DOz7ui>t_X%F8JgFS|=D`n#{1S7eyncDk|X-?f?c zEB_lTYa2!9A8MpO9g|W#aN*Oae$%Ep|0_3x;d4HLHD{`m+oAy#2FVU%w+f!pddcIu zPsYo^;+yLto)g4g-da+1ef$O4sDD$3DEDFuonhm(ph=C7BTjqX${cS+AMbru4q7o! z@DxrA-qcE8IrUdi;i!UARUh0ay0&qcO#j&Hf~Rcm+)md!{%f$U$%l97nAYt;+0Y zH7`gp@+o<)!G6J|INdl!N8PmY-@(j>w}Ou6f7%r^e!ZWpo%Vp>DX*98s%PiA5KL|d zJHJdH?6`L-Cw9;1CZ~LL(II}!+FhUDcOlqce`dsK&#PKDtq3mnmP5856{?cW|Kd=!oY8_i^Go zi!!;Nk~?#55L}AWjWhPpH7*#f$7ZPJm&cB0*hf>|_o?7OpR8VrMS}S`av2o^{{|mvT&YkXq#IJRy z-9hj-=TiJ_`Bwn{aDVp|WUmxinTXjd#q3p-WUp$R+!hQ5dsTnzRz>y-Q(A$&>YMR$ z^ucX=g*ka%`>iEKC$LwO`!{t6vObZqSDf~|4|BW~D6m&AteB^G3Ma^}L`n84_25Rq zKVYv?KK8oeDP%t)1@?;P1nsKZfy9MiuhLA}sJIlT$*x36_Uf4vsiNXwuX=UctawWG zB>NHFciUc}KF|BT)x5|9d-X?y{fbL*nyf^WWUm(ev@5C)_Nr;x1B$1R#fgl);yIzc zQe>|jg6v1c>=n{zuN2uU#ih7DvMW)Ny=qx#i25At)r@o36i+esqeklAqDHsv73zZ% zriCi0f?%(jy>U!&DNd7JiIVKqBt1`!1AFz+(3^^`D}~ zSBmVF<|$-9B4)2pAKEKL_DXRnPLst@F?*%SUTK~}{Vrnm3iYAA(qykR*(=5D6%y2g zRm@&#vR94{vP6p6E2Pn0#Rx7%mPj#sMFiL@P4-H2DY8V0*{iz{T#77_V)lv%uvf7+ z=TiJ_`FDdzv4;x=5A+(X{S^<1N$)q(sissFI|}cQ{>s`&@4mm97&v=()cnJyx>nYz zqC?yLVB6n%wnQ{5=LntVr+yBx;#e&`AgCrP6p9=gW=koKg;$P^($CF))FEmNsHyX> zt}3cb{5ayY=gph3DSCZMTiv&LCdo^6d440ks??*RP?s+wPJ@o8o{I`(?yIM?`$ll7 ztX~N|^QS7}VEbKn5HHm`5iR^Gvp#aEwdBOrHyi0KnJSB|#g0UrhV!D;-iS`ruc;sV zx1{7!)dmK--m;2f;HZ6f5Thn8ioSU`ukQ1DGl$r|(bvTemKVeFAB;E+=NXPqi{9*V zRBhke*CE!`_jUckWyK>k4@aDadCAU7(dBH8!34TC$e9vufMrkUObTcC=i(+pApU-v@}|hH(q`}+tq$8Edttt>bEkd?^WwBiyj~}zsC#O(uGM~1Fl<&cowom@;Kvn>!&};DtQY3qMYU$;4~f6 z!3q9%*hL=na!~)Nwz}lgnH?Sci*owjI(WXEzBj?8IPH1Y+70mwUdyc4ENSbMiq{9H z@2!Iqu8!T$9!SiTvyjU6<7lUqa4BA!_twD)t`A3ph(-fNty;0sK(Wz4#YTgGMgxZ+ zd#>1MfHaN24FVbsG%pqP3B^VOq;WI|Xf#k!qX7{x8U!>N zXiiWcRBSXr8b^bGMgz^I$et@U8V~`aK|rH{Ly$dJY&1X`M}vSy1BW0xuh?jSG>!%V zjRu-a@e-2NRRcLg0~p1 z4@ZN5MgvWKO|j7c+b-9Qqd`EUfkW{6;QDYh@M$z~S_#)jmVs&zMgyNl1Bc*JZhdfT zLiPjm;&s6Pj-x?Dqk-lr{EKp$Ylw{osDl$cCmanT8VwvB{EKq>-a2@`oW3`~r8teF zK}e&4Qz~8`oW8dXPPjT?G>D_oz-c91ir417b#Q{~gOlV(CdQThGhRN^>s^1+`TDwd zhianKxUJC(73=7a>Qxhs2W^SI?_682Yfw!zTbl?ova*$5iaP-@*0Xi{(jkOwpueeD zP1JmHr9;E-qNfuRKe+%mkq&Khi1g>0=$*e;6{n6bh&T%-Ij>co;UZAf1)$1UX(?24=05azgBOdzg|&A4DYfT-eW}R$4*^`hLD*CoWxxTpI{dkmx7Syel@Z_9G&**rounQr(p%;}zd`Yo5;HpM0-sh8 zwJ(f{cuwFy@zv$3*Okt4$hA)um*Vt<(%tlcw;mQM?=a9&zSg&@`y`pxsuPn0^Pp5fG3$Lesw|_BlMh=L0PO!#NS@e`!_e=lT zT$)R9dS=6}y5X${#F|a!H{_$$Gw7@{3(8eTa%i5iWJfRkz|;pt(K7ubo)i3X_e5^p z?$r$Pv-9~im*Vt_Ioud8WJWo+p#Yc*gur-Q(c`y2{*3qWqslHBYJ6uAkn~BC}8h z21Gn3xCgnsn9etJmsq*6wB}Nr=D$Y6w;EGaZ~tts*gCbe{oMzDCFj3#^PJEoCX!7I z$@(Z}6BVO-3- z$R-Ld#c8rcDrOS}*+jup$l57p6Hy=9L_s!Da4AlcZAZ`97*x!9HFHYA#)>`LoRKeAO1W(~ajg!rFmC^r1Nvmxm3jc$D z36c|B>WvER^k4hS-9-ma;Y31()_U*cOs#1XF`Ng{%bT%X4zyoJ(<<`uSZ-#Zx#z{r@hd;wjX(F<+jOTPjXa zKgWDI9V76~xfJzt)Nxk=&;-q;I8A-<_Ip77=3jy)3xN4@IvIiDhTtjG&ynUiVdk;_ zaH)IIF=2e*1b^oA|32}}nGyO-_CsOSZd)V%Jh=iMUI-4?9wxW< zo~n2XCzhTVrt4q)A$;da^IOKj85_c{clVUTdz}|N<>xNX=o}kQg!}K`81bAuZ~1+T z!-`+V%eNY*2)God$LxMuk12a1+|gw%=$QZ8u4sJCp)$pUIfAEDZ8=b>bK%UWRo0S1efA;kBVLNTRrN84HK8Zd$fM-ebudD zFPXaFLWkf|i|2LH={n~M^YmQ~1pmid18-OSnahJWGuB4A6n_TKcptP1rcG}v2T#r{ zIgzpN^ZLpB--S)5ZE;#YQ1l za5jYL19z>NOR5iZw30WgWtP1CaDwZGQ%I#ssx#j_E?Z7?3Ej4@?)vxguweR)5vRcq zOViFOW4ab{r_3yQ`{4xF4PI6HbXG$THGS6yOC&93^9Pl|`X&si678oVJEdR@J9q@nE9KeOag zT%X%^SBr7#y?g~^jW4A`R4vs(&$!eq@qG3*(ccet)Z*01t)223oA~asSt`@uMlxikY@V zoCc4i1BAM;H?^DyI(Ylx1lJ9}?=}`HRmarw9nisv)RVjFF&hsi#;eT{r#-KK-sUPt zrd0CZy=^6z;Tt4|6U1aqKR+@3XAg=OVWz=}{cDEl@rw>3 z!D-JM-(qnX)R-drmwI0E7RL#m6Znq4Z+=++(c0obvr!JQVahYQdc9q!gVV5s_0)G^ z+OvU3_kv4IC^1AY{dgM^oQ7SQ5;>zX{o9C|bw*0wMmfPt>v`kSWsUwBQA6aOKFT4A zJ~mJfp5J z9iHDOTJ~O5v1*h{%p3{cLk3qug41x{x$KZ=VAnYDbB$4ww`xxC(!wwF)y74C_NgR( zoa7SaFFv8ub`KNH{F>7+7dUH;PZBv>6g9ugTESQ`n*rwi=f$( zpR}6T^2&uMYrSD|cDt#9r*PWU@xb&Aeyu${<*^>;6;GiVlFwEXq$5Y##eM}?P4sP< z!ma^Oiqo&bYT~`JC;WrBni&1tu3#msCQ^@|qj(C)c=@>`|WoKt1MQW%)KoJn5R6(jrLb5c2SL_eiyR6s}q0kSg}0bePJ zbZJ340t%M*6%-V{bMM|+{&&j<-^=qD=h=IH=bT$+=KOE6GiTkq=p6rMTfU2^(8vwWNcZ5T1mZFr=EW(F$p) z6Li0tFr=DL2(*$_lpa=1h}D4W3XgH+`qjjJ2vQT4R1*x>uB={kP!r-@5o$u0YJ%Z< zMFKquHK9v2p%Aiq(Lqg!ctTCMrJ7*4UXj4qLQS}(notN?z38AOL_DD;45=mR1*pzs}~*Agor29gdx=g!}W>;#ujSAkZM99Wc8whnh^1XnlPlAV7L;Iz}P}f z7*b6rgsfzAP!l4aP!on!6AY~|p1O8PO&C&5D1@x^U7#jf`PGCW)dWLdQYW}TO^6JI znlPlAU}%LgFkAKf!1an}5gz|H1gQzHR1*wi$a+TyH6hO^QWI{eCK#@8B+!#k6K<&{ z6hc-nI;aT|PpApER1*wWI1(6Js0p`J6AB@#7ai1uh$qy9TdE0$D;x=oE!2ctstJXV zwTKRCLc|kl!jNi$;R;6rV+%E5NHw7lvKG-nO^A3xO&C&5C~Fr9j4jlJA=QLJ$hyP@ zYC^;lYQm6ef}s_zSE&=^`9W&JkZM99&`Q=mt-`8_pw$II9o=_8RsV0=wjs(UQYQinm zghFtgaP0Pn>7XV=JfSAsQcWhm#S+b%07unV%)`$I7`HL^d+}%KmzHIyDcFdT)cDdOvDnzYveOaHfy31&z()4_| zTV3p=h7Ysrw|kgq#qVkhe~S}$PK-xyYaVynZpMb%KjsfGu?303!~3zGahqCoEYAPJ z^xA;>_KRl{?CJRvOl4`L-yJ;Oa7vl*gsqu-IQ&8Sk*zBwlw@_L%^KhN!Vfk z@oQ(JSlJ;8aqdhXcCOApTW4f$u#l$r(Wj@`y?0+VHXZ4!5OM~^-Ye4dJpI9E?d!>d zjk@2ebAI^Me(cr1vbPp4m~A0V-%zOTw!5_$X0&JHl^DsJo@XmkPi!fey2V18zFo7c zv0dU~E$`)4Ny-c&f$NRd)vk}N$U~7vjpx-lU%T)Hw&K}+oA)=~Y9UQ;K{#FA+SaZ# z+xSy2q3E4?M&iNd90{Dw2b*)GgNb708#UsqFO^o}p!Y4Y5_t^gbCuv$&Bnz0t%{Uy zwAP`W&jsP3pY)%{ESKS7@0VQe7)OU2%V`{;)p9LYk_+%q{k<8KcaZ z&(HB<3lckxDeTDd`TF{K{Q1=8z}@!Bz9Y;5mAmQK^5Ll|tjv2;MONsEo>TP0JbT%f z6U;s-PkYe{>4-7OtX;w;y>gM66yx8YzHgtJI^Nu|xt9xDn&c+4UuI>B80d-awf}Uq zEB-mjEZ(GP4idO3k=C?g#ai1V`X`$1m7SFsJ7>JbSngrHcdOaX@90H5O*>h4lXc{F zvRUQSupIP@MB`t_vm+OVcjpWmKW^sF^>c(NIlK3&i2Igvdv>$HAuMz-QtC01pB z;O$IrG8<9rF)y~HP94XVuRW)y4_siOCr#V%tlnx45r1C((u-D=`;KE-Ul1`dkf_}L zEmwDX>)RJwbG&GEzV$d(?z3}x^{Iixt}S1=y3#kpD*b1N7p*$Bpct#p>EFK{Nc>cA zx2wfZDdy2fcX`q3@22C};Fag}j9G!io|{KpE9fnJm1FjJ(dyaAacs?sb9&v`fy9)Y z6RzR>y?uB0c+u)Az4@XW5f_4pxS{7AED-)TRB8-5D_W{w{nPhdgG89BNV}{9HP<3 zfkdda%dH&ZR7fIl&bgICJpD;vj8Iu|D~H$+M1&eEZsib9)0>*qo`)j1l|!rzB0^=w ztsJ5ry=6*`5sKhe4l)0M2yW#Njp;pD_m9D?9OC5a2O+qXL+JDdFf~TaLx;U{sV=EK zvuh4o9oRCK#Vk0dulXt~fS7o^mHX+O6thO-cU)+N^o#ehpR*9Gu z6^)FD$!1M3W^wpIb<) z#E=`KjejqE)BJ4YGj1f%YIy8Gc6!+qz3JkQDMo=eQ;qp^Cz`WU{cm&>e}#awO7y;1 z(Ck&fr|y0{o61V0tjW2sHpx+RV&6wc{w(b{j2T7-5e1tAvg%Qagzl=t>jz+7SlgvW1nF|T2!UTad{nm1HOXH&*iRQXj zR^=d3vgKPW{>ksXc}aeCCumJOQ!3B>^TuRz-Hi`i=og8V%_g!FyH0x-l$l5UnDyr= z_vOjSW}jgbw#u{93+hhzRf+lae%GHnpJMK5FrXE-M3Bb$gx;wa@u~kj7oIe2^Nz#% z96HZ84eRB?7)Z#o*~`y)O^fdSn6>UeiaD*s%Q@JB1Zy~&C7eFxJ@6<$YgZ zm}%vgxv)i^wO)QTKu`3Y>Yw&oGd@o?i=0f$K`W$p)E>bqX7Bednz4jpeAME+)sB?# zsn?iGo~d5AGKd%|aqGzjcJ+iLGjGsCUL??Jc+x9Oub<(qKY*S^s#W{4rJeD4qB;J? zNnM_)Ubr%dcq-Am+!(vVrZ>&nt&cm`5<#HVor?q6-!HuFomYDW#i&wqhFzfM1T(wn z4L7#%zbSIw`--tOt=^Tz#*+SH&61}I+w1FeVingFu&bQeZ)Mc$%trLl>;dWftcH`? zv!jy>+Y{WoDMss!(~O_{4>$V^{YWA3JEKU;4K?{cjDZ9m|2IU{J=2Zq{W_b4Un#+| zV>+>i=N7b|SaHZgzZhGM5spCmzafrK>|*SjRmk)#?#-6Obz-;96|&EaIih5RvDFyi zi11nc{|WRJj@Go>g$fvT4=p!h=zX=}h~eGi*s1@yYXyFPknYs*EoD4biIoLb8H0Zq zVXl1ZC40uh&$7sUMeKEF_F6b=>Pozu_^B}`ssENyQ|d)>hO6l2ozM5D*T zyGGs3@eEsVbTOW$#qXGGB&;i7o*mjtStUsSll}~A5LM0|`^-U#F=p}0#!D^F8b{aw zR-;NAc5!N1d)Ac07RFGC4qbM;>o;#|EI2YriGlRlLv300Q)O-63=S-@%$<9qn(=`< zNg;;b?Z7T}C?>4b8Qi(ziu>~L&PJyuZ!z?R1ZGIziOU^qY(2Q&h}b@uVZLXKSoXw! zD%!jA4q2EX)js(RjQMfz88PL@C_Pt*9}V{DzpL62e;=l?8Zv&Dd#|g7@%g*-Mp_uF z8eoOlYFe3;&F*XMYa5?0PGo2`d6S#fJP~2<*?h!Oi8uC?cYmGwy1PL_3PUS=mVrv_ z+nMVc_1GisiN{lvC*vW3p6K~L%^Mr@+9rwLz|3vapB1_8`@J|`O#+)T+|vrDuou3~ccB&1hok$jjKR)#^(U|L=+E6uVcB=naS_=Z)n zr)8lhs(n_(Syx|5Vf|ld>qaZ2`~Is3`)%?~YtKwZbN=XA-Fx|b3adIX--|6sBwp>w z9^U0-b*1JNYjf9R_SlWFI=0~T3O!Mkaju{}=F()ARXwgHT5Uhym8HFL%liGxuPv3h zKC!#~9=&yX;I`McpcT@ux9rSzwb$&~JN8jOru80XXP$T zKmw2d8-gkTTUG$3yuKPx0f?+Hwi+WG5k9L2Bd7wfWd&f$Yo7rXfXE6xsab_1!e{mW zC(u_oT2uhGtN_e#guEMwKB@gc0*_UqP{CDpZK?pqCB0sRteJl{#ZY%oZ+hgYA=4-E=?7{ zbvD41cQpel0I|kYf+_$@Rsc#2q~$$IeAA!qI3<=@vI0;Dc|R9c>I_l^V95%=L|;hY zC}>(*?qGW$RRFEF4>mF1vs3|8`%gt<*S!ir^pD=bli$ETP8C3%@?(^qD}?XfZulzz zOI831A@A+N3bWO;=TkOY?K;#pRxeI8(JGNDfQBa`jNE$_fE=qS-rnD>?DM*UISl_zVe zPsd-mpp>`#8!6_tYSY|ET;17`ZT}<2SoE*J=W>3Vw%^;16#b}#Q#nYy(!3K}e=^2M z9<|3pTGNs?ud(`9px+O6-|j-I@%`f1HoKWo@c8BcqQ~V!)(`WO&DFoJ_o5ZjKR4>e z_GU#JM+fqHcUyifyZx#pv)0wkQ2O(ac``kd7@QzwgDibcq!6jGpMJTs=3~Gun(cf1I}3jV(x2Z15b5 zKh{vLk`Ci$+l7ivG#hLwW?{=~zs0j3b~QF4&Tg{M6I~f@UA0?xc-j1R<0EdgLi(*2 z`>~q3@2UBkHt66n`{?N*X1n<5UTjG^(3hQhr;hRW>5Ue8qQ4iNUXnf3qqq4`!ZjBX zRVu&0DolCO__M=C3u#R|d!-Uf(7KupzhC4;KD z%Uo!cyKDfPKlV|h2tAQfB|OF2va=u7GIxB^!i!c&kD#|tUDHd-ew-=Tk^PZc-JH~X ziwj$tygZ0i8dBWYdL`3BPny=DC{@3gA2E-`pUFW2pLl{ay9v~rGl zt*#QJ4eS7IAnw9QILAJ1kiDse-Tq&RtX}D%URkRP;@MpTRA+1DiQnb-FAS;P{bhr{GZ2MzEd+7?DfUyTk4uo zAjJitD%9H7}S`K|8c-lVv%*Za<;}O2#!C4`JR-U#4ytl9I2|;YzdR%vWVTR$e^0XPCJHC((B0O|n(SIK0ti=q& zXGLVzcXVCkJ;ZoHG02erqvxcmJ@nH-ghv2GB%GdLmPpmb)e^1bvA70n+H0GQW>)(0 zP0$a~B585FE6-zQRcUG;q--^48Vu#OQed z^z>F}k;euf1xbrz(#~ri-&DWGW7(X*78%drnPC4kZBFN|Sy!9A+msX7g1yCTHEm(U z+tFoe{f{891vA9hnzppJ(KO|Fizprg3ADo4qTe3zlZdfp-!V^*EH8S9J;!)HTZh0F zNsAHf`{q3J%AkL&;PV4pl4>k8cTCwYtn$k)HhV7qBm{virPAIv&!kQjR;{00VwM{7 zQV0TD3_)Ov?31vLjF**!g%OO7?Xeg&tvAVSYjs6ZYjnG=L2b3=o<@%}Um|B8IFJ z#W8wP30X%AD_N0=WAvmFyjCYYAgp9%DUQ*TpYZS*Lt`iopce16AcHVus-lVv*OjVlgWCjGM7=!8=MsqXcbEpUUKP|@uGYp>~u7$T7hT6;CoF z{y+7aNFVt9^2#~%SYZoS6HdIaF_@lp**@QO*k^T`6tnm9y`H_Fh+}>R|3!Bk{y8K? z-vC7dt(-GBm>5Jec!_3^x12c~Wer}KEsoKX8iURWu@X6fba(<=FcZ4k@tp|~PmO^D zTJbZu5oH|Q4}MPYs|;_!4AE2g80eRukFBYX!7;D}eUKZ9aK&Zm&Y(tfesAx!|NcqlQ|039GiwTM zBeC7@L-X1X|(TXj;_+W6Z}(rQ3a)Uyqh&k(*!p=0iM{m_S5@ z66yA^%D-hHfmRYDzSu!#$R2u7dNPsi3D2VY--xf z8u8|)E~VL=#jP?UX>Of;IHXmgB@y-GO0&twk3|O(&g#RpplK)S#+o13uE*}CZ;#&Q zyE}`qLRuw`60w4aJsZDO2(+TAM(p+ZR$|fG^~^!_Vwv7#eKcR;&Ym8|Q;D`j)F-0E z+=Wp{pcS4gnpUK1X>;|H@vP(4)lvN1c6R8HR*C*Zj3naaku$Oa3Fj)|folx?MdR9w zM$Oy=wmSZL(?N9oPNa%kTubQK+22D?^rogSE*f7E@yD1RN(}TH3)e3C{rS3!M&{lG z_W6+k&CnNGVQfu1QX<`mKQM-6U%1`WL-$u_AC6o3vx02z5qlODhzcZ}KJm{JNQ3xh z@X-o!M$G<6?AuQYMrrNt?E)gL5*{KViD-9eN@gJ8_e-Asq5VW(jPK{Ax$llhVP>)E znRKD^@4X?-kM9u~bJN@fMy9Z5PN&jtkxJaN(z0@C56~B5{QNZQABs_PY0)gcBY4M& z4!ewnn3C^U+(o-lD5dbc0| zNQWj4Oy3(sIDWY&s?_9IH6Z_Vm19Xw)@S_s_`Tn;U_6yLTQ`<1CgP>ryRrfazhA0u z_+7#`AOEIaze{(6%s=z9w($|9apCifw5AZh67fC}OW(ez5NIWpw~Wz^X0JER`#0bH zk=316g7Z5sj2BFFC!*$=-xLC^29c-c{`%_5{xK}3bh`ca$9J*@!EfR)o=RLIq7xBp zRI!anpcUUcso<|HAELAAV&ynnyHIQ+RTlo=j3TWPwTPHrG0v`>zdRBNwBnyP_1C+P z(sks{^fc?r!4ezzPTH_GIYQUqt{Qrxzm6uN@QgI;m)O#gNT8J(qdXDKW~8}m94gU- zw>Wz;7y~_N8YgNJG5Kw_A&_t~e?hSgR1*`yG{^~d%8yyCt?^8t14tf z2NI4Izxphf%95VeLsy>2da-7ehVMs5z&;hyDlwagpQ%=Swcj(DNT3z}oT@>~H|Kll z%9HnGy!p0X;cgmXW4 z2(;pNXa2cwtg6wKryeOIK~SCG?7cWv&i(uzv5$xdBDTlW617ht!Ts|3p&02kUySeP zq*;YY6ISS0T2y3w56Rc!xwr2FvFWd==B8N%iRjVkDK!TA%@Ea+^wfv0hZTt^btz>d zkM3lpDOSFHCSUCFRQ2%~TRN7G!o4M`g#202sEA`9apavecTpmG4w#Xp5@1D@MQf=#?P{1Vb4mT6+*8RXH+Gk(WN^XNbri)@ykCS>Yu@_#nX*cn)53cZf6Xl z`rN-e2=P>+0af~Ah}b^5adaTz_@xyiJo$P)imp|~Xzfnxwkpd*TE^M&8brs={v~>% zZ(*;wX!IeXq%m5J0e*46jJ^{`1XTg%(PQxnfmRq>(|)I`TPB^g)#z36v2+!4_IEKW zmFP-ZZ9EZtwIhL6&gZ_K&!DT@Z*=t`z_Z?`M#XvVSn>12Irb5gh`2?>uhTnch9bBX z|J>Paqvvds^+!YqWe;?&oiEE + + + + + + diff --git a/cybership_simulator/config/multi_vessel_simulation.yaml b/cybership_simulator/config/multi_vessel_simulation.yaml new file mode 100644 index 0000000..bbaf424 --- /dev/null +++ b/cybership_simulator/config/multi_vessel_simulation.yaml @@ -0,0 +1,30 @@ + +/voyager/voyager_simulation: + ros__parameters: + initial_conditions: + position: + x: 0.0 + y: 0.0 + yaw: 0.0 + velocity: + surge: 0.0 + sway: 0.0 + yaw_rate: 0.0 + tf_prefix: voyager + body_frame_id: base_link_ned + world_frame_id: world + +/drillship/drillship_simulation: + ros__parameters: + initial_conditions: + position: + x: 4.0 + y: 0.0 + yaw: 0.0 + velocity: + surge: 0.0 + sway: 0.0 + yaw_rate: 0.0 + tf_prefix: drillship + body_frame_id: base_link_ned + world_frame_id: world \ No newline at end of file diff --git a/cybership_simulator/config/simulation.yaml b/cybership_simulator/config/simulation.yaml index eff9db8..f64acde 100644 --- a/cybership_simulator/config/simulation.yaml +++ b/cybership_simulator/config/simulation.yaml @@ -8,4 +8,4 @@ velocity: surge: 0.0 sway: 0.0 - yaw_rate: 0.0 + yaw_rate: 0.0 \ No newline at end of file diff --git a/cybership_simulator/launch/multi_vessel.launch.py b/cybership_simulator/launch/multi_vessel.launch.py new file mode 100644 index 0000000..4eb81c5 --- /dev/null +++ b/cybership_simulator/launch/multi_vessel.launch.py @@ -0,0 +1,100 @@ +import os + +import launch +import launch.actions +import launch.conditions +import launch.substitutions +import launch_ros +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description() -> LaunchDescription: + ld = launch.LaunchDescription() + + # Launch args + arg_use_gui = launch.actions.DeclareLaunchArgument( + 'use_gui', default_value='true', description='Launch visualization for both vessels' + ) + + ld.add_action(arg_use_gui) + + # Resolve parameter file path + sim_share = get_package_share_directory('cybership_simulator') + multi_yaml_path = os.path.join( + sim_share, 'config', 'multi_vessel_simulation.yaml') + + # Include robot descriptions for both vessels + description_launch_voyager = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + launch_ros.substitutions.FindPackageShare( + 'cybership_description'), + '/launch/description.launch.py', + ] + ), + launch_arguments=[ + ('vessel_model', 'voyager'), + ('vessel_name', 'voyager'), + ], + ) + + description_launch_drillship = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + launch_ros.substitutions.FindPackageShare( + 'cybership_description'), + '/launch/description.launch.py', + ] + ), + launch_arguments=[ + ('vessel_model', 'drillship'), + ('vessel_name', 'drillship'), + ], + ) + + # Simulator nodes (names fixed to match keys in YAML; topics live under per-vessel namespace) + voyager_node = Node( + namespace='voyager', + package='cybership_simulator', + executable='voyager.py', + name='voyager_simulation', + parameters=[multi_yaml_path] + ) + + drillship_node = Node( + namespace='drillship', + package='cybership_simulator', + executable='drillship.py', + name='drillship_simulation', + parameters=[multi_yaml_path] + ) + + # Optional visualization for both (conditioned by use_gui) + viz = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + launch_ros.substitutions.FindPackageShare('cybership_viz'), + '/launch/multi.launch.py', + ] + ) + ) + + group_gui = launch.actions.GroupAction( + condition=launch.conditions.IfCondition( + launch.substitutions.LaunchConfiguration('use_gui', default='true')), + actions=[viz], + ) + + # Order: viz (optional) -> descriptions -> simulation nodes + ld.add_action(group_gui) + ld.add_action(description_launch_voyager) + ld.add_action(description_launch_drillship) + ld.add_action(voyager_node) + ld.add_action(drillship_node) + + return ld diff --git a/cybership_simulator/src/cybership_simulator/base.py b/cybership_simulator/src/cybership_simulator/base.py index 38b3e8d..ec3c606 100644 --- a/cybership_simulator/src/cybership_simulator/base.py +++ b/cybership_simulator/src/cybership_simulator/base.py @@ -46,15 +46,16 @@ def __init__(self, node_name="base_simulator"): # Thruster command vector (u) will be defined by the subclass since its dimension can vary. self.u = None + # Declare and read parameters for initial conditions + self._declare_parameters() + self._read_parameters() + self.add_on_set_parameters_callback(self._on_parameter_change) + # Create vessel and allocator (subclasses provide concrete implementations) self.vessel = self._create_vessel() self.allocator = self._init_allocator() self.allocator.compute_configuration_matrix() - # Declare and read parameters for initial conditions - self._declare_parameters() - self._read_parameters() - self.add_on_set_parameters_callback(self._on_parameter_change) # Common publishers for odometry and TF self.odom = Odometry() @@ -77,8 +78,6 @@ def __init__(self, node_name="base_simulator"): self.dt = 0.01 self.timer = self.create_timer(self.dt, self.iterate) - self.body_frame_id = "base_link_ned" - # ------------------------------------------------------------------------ # ABSTRACT METHODS (to be implemented in subclasses) # ------------------------------------------------------------------------ @@ -126,8 +125,10 @@ def _declare_parameters(self): ('yaw_rate', 0.0), ] ) - self.declare_parameter('frame_id', 'world') + self.declare_parameter('world_frame_id', 'world') self.declare_parameter('body_frame_id', 'base_link_ned') + self.declare_parameter('tf_prefix', '') + def _read_parameters(self): self.eta0[0] = self.get_parameter('initial_conditions.position.x').value @@ -138,6 +139,12 @@ def _read_parameters(self): self.nu0[1] = self.get_parameter('initial_conditions.velocity.sway').value self.nu0[5] = self.get_parameter('initial_conditions.velocity.yaw_rate').value + self.world_frame_id = self.get_parameter('world_frame_id').value + self.body_frame_id = self.get_parameter('body_frame_id').value + self.tf_prefix = self.get_parameter('tf_prefix').value + + self.get_logger().info(f"Initial conditions set: eta0={self.eta0.flatten()}, nu0={self.nu0.flatten()}") + def _on_parameter_change(self, params): successful = True new_eta0 = self.eta0.copy() @@ -216,16 +223,16 @@ def publish_clock(self): """ self.clock_msg.clock.sec = int(self.sim_time) self.clock_msg.clock.nanosec = int((self.sim_time - int(self.sim_time)) * 1e9) - self.publisher_clock.publish(self.clock_msg) + # self.publisher_clock.publish(self.clock_msg) def publish_odom(self): eta, nu = self.vessel.get_states() rot = R.from_euler('xyz', eta[3:6], degrees=False) quat = rot.as_quat() # [x, y, z, w] - self.odom.header.frame_id = "world" self.odom.header.stamp = self.get_clock().now().to_msg() - self.odom.child_frame_id = "base_link_ned" + self.odom.header.frame_id = self.world_frame_id + self.odom.child_frame_id = self._frame(self.body_frame_id) self.odom.pose.pose.position.x = eta[0].item() self.odom.pose.pose.position.y = eta[1].item() @@ -252,8 +259,8 @@ def publish_odom(self): def publish_tf(self): t = TransformStamped() t.header.stamp = self.get_clock().now().to_msg() - t.header.frame_id = "world" - t.child_frame_id = "base_link_ned" + t.header.frame_id = self.world_frame_id + t.child_frame_id = self._frame(self.body_frame_id) t.transform.translation.x = self.odom.pose.pose.position.x t.transform.translation.y = self.odom.pose.pose.position.y @@ -265,3 +272,8 @@ def publish_tf(self): self.tf_broadcaster.sendTransform(t) + def _frame(self, link: str) -> str: + """Return namespaced frame id if body_frame_id is set, else the link itself.""" + if isinstance(self.tf_prefix, str) and self.tf_prefix.strip(): + return f"{self.tf_prefix.rstrip('/')}/{link}" + return link \ No newline at end of file diff --git a/cybership_simulator/src/cybership_simulator/drillship.py b/cybership_simulator/src/cybership_simulator/drillship.py index 7f5c598..c0ce97b 100755 --- a/cybership_simulator/src/cybership_simulator/drillship.py +++ b/cybership_simulator/src/cybership_simulator/drillship.py @@ -20,7 +20,7 @@ def __init__(self): def _create_vessel(self): return shoeboxpy.model6dof.Shoebox( L=3.0, B=0.4, T=0.10, GM_theta=0.02, GM_phi=0.02, - eta0=np.array([0.0, 0.0, 0.0, 0.2, 0.2, 0.0]), + eta0=self.eta0.flatten(), ) def _init_allocator(self): @@ -116,7 +116,7 @@ def cb_bow_port_thruster(self, msg: geometry_msgs.msg.Wrench): self.u[0] = np.clip(msg.force.x, -1.0, 1.0) self.u[1] = np.clip(msg.force.y, -1.0, 1.0) issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "bow_port_thruster_link" + issued.header.frame_id = self._frame("bow_port_thruster_link") issued.header.stamp = self.get_clock().now().to_msg() issued.wrench.force.x = msg.force.x issued.wrench.force.y = msg.force.y @@ -126,7 +126,7 @@ def cb_bow_center_thruster(self, msg: geometry_msgs.msg.Wrench): self.u[2] = np.clip(msg.force.x, -1.0, 1.0) self.u[3] = np.clip(msg.force.y, -1.0, 1.0) issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "bow_center_thruster_link" + issued.header.frame_id = self._frame("bow_center_thruster_link") issued.header.stamp = self.get_clock().now().to_msg() issued.wrench.force.x = msg.force.x issued.wrench.force.y = msg.force.y @@ -136,7 +136,7 @@ def cb_bow_starboard_thruster(self, msg: geometry_msgs.msg.Wrench): self.u[4] = np.clip(msg.force.x, -1.0, 1.0) self.u[5] = np.clip(msg.force.y, -1.0, 1.0) issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "bow_starboard_thruster_link" + issued.header.frame_id = self._frame("bow_starboard_thruster_link") issued.header.stamp = self.get_clock().now().to_msg() issued.wrench.force.x = msg.force.x issued.wrench.force.y = msg.force.y @@ -146,7 +146,7 @@ def cb_aft_port_thruster(self, msg: geometry_msgs.msg.Wrench): self.u[6] = np.clip(msg.force.x, -1.0, 1.0) self.u[7] = np.clip(msg.force.y, -1.0, 1.0) issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "aft_port_thruster_link" + issued.header.frame_id = self._frame("aft_port_thruster_link") issued.header.stamp = self.get_clock().now().to_msg() issued.wrench.force.x = msg.force.x issued.wrench.force.y = msg.force.y @@ -156,7 +156,7 @@ def cb_aft_center_thruster(self, msg: geometry_msgs.msg.Wrench): self.u[8] = np.clip(msg.force.x, -1.0, 1.0) self.u[9] = np.clip(msg.force.y, -1.0, 1.0) issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "aft_center_thruster_link" + issued.header.frame_id = self._frame("aft_center_thruster_link") issued.header.stamp = self.get_clock().now().to_msg() issued.wrench.force.x = msg.force.x issued.wrench.force.y = msg.force.y @@ -166,7 +166,7 @@ def cb_aft_starboard_thruster(self, msg: geometry_msgs.msg.Wrench): self.u[10] = np.clip(msg.force.x, -1.0, 1.0) self.u[11] = np.clip(msg.force.y, -1.0, 1.0) issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "aft_starboard_thruster_link" + issued.header.frame_id = self._frame("aft_starboard_thruster_link") issued.header.stamp = self.get_clock().now().to_msg() issued.wrench.force.x = msg.force.x issued.wrench.force.y = msg.force.y diff --git a/cybership_simulator/src/cybership_simulator/enterprise.py b/cybership_simulator/src/cybership_simulator/enterprise.py index ddbb62e..826b59c 100755 --- a/cybership_simulator/src/cybership_simulator/enterprise.py +++ b/cybership_simulator/src/cybership_simulator/enterprise.py @@ -18,9 +18,10 @@ def __init__(self): super().__init__(node_name="enterprise_simulator") def _create_vessel(self): + return shoeboxpy.model6dof.Shoebox( L=1.0, B=0.3, T=0.08, GM_theta=0.02, GM_phi=0.02, - eta0=np.array([0.0, 0.0, 0.0, 0.2, 0.2, 0.0]), + eta0=self.eta0.flatten(), ) def _init_allocator(self): @@ -92,7 +93,7 @@ def setup_thrusters(self): def cb_tunnel_thruster(self, msg: geometry_msgs.msg.Wrench): self.u[0] = msg.force.x issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "bow_tunnel_thruster_link" + issued.header.frame_id = self._frame("bow_tunnel_thruster_link") issued.header.stamp = self.get_clock().now().to_msg() issued.wrench.force.x = msg.force.x self.publisher_tunnel_thruster.publish(issued) @@ -101,7 +102,7 @@ def cb_port_thruster(self, msg: geometry_msgs.msg.Wrench): self.u[1] = np.clip(msg.force.x, -1.0, 1.0) self.u[2] = np.clip(msg.force.y, -1.0, 1.0) issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "stern_port_thruster_link" + issued.header.frame_id = self._frame("stern_port_thruster_link") issued.header.stamp = self.get_clock().now().to_msg() issued.wrench.force.x = msg.force.x issued.wrench.force.y = msg.force.y @@ -111,7 +112,7 @@ def cb_starboard_thruster(self, msg: geometry_msgs.msg.Wrench): self.u[3] = np.clip(msg.force.x, -1.0, 1.0) self.u[4] = np.clip(msg.force.y, -1.0, 1.0) issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "stern_starboard_thruster_link" + issued.header.frame_id = self._frame("stern_starboard_thruster_link") issued.header.stamp = self.get_clock().now().to_msg() issued.wrench.force.x = msg.force.x issued.wrench.force.y = msg.force.y diff --git a/cybership_simulator/src/cybership_simulator/voyager.py b/cybership_simulator/src/cybership_simulator/voyager.py index 0c87a81..bafd2d5 100755 --- a/cybership_simulator/src/cybership_simulator/voyager.py +++ b/cybership_simulator/src/cybership_simulator/voyager.py @@ -21,7 +21,7 @@ def __init__(self): def _create_vessel(self): return shoeboxpy.model6dof.Shoebox( L=1.0, B=0.3, T=0.08, GM_theta=0.02, GM_phi=0.02, - eta0=np.array([0.0, 0.0, 0.0, 0.2, 0.2, 0.0]), + eta0=self.eta0.flatten(), ) def _init_allocator(self): @@ -86,12 +86,10 @@ def setup_thrusters(self): # Thruster command callbacks def cb_tunnel_thruster(self, msg: geometry_msgs.msg.Wrench): self.u[0] = msg.force.x - if np.linalg.norm(self.u[0]) < 0.05: self.u[0] = 0.0 - issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "bow_tunnel_thruster_link" + issued.header.frame_id = self._frame("bow_tunnel_thruster_link") issued.header.stamp = self.get_clock().now().to_msg() issued.wrench.force.x = msg.force.x self.publisher_tunnel_thruster.publish(issued) @@ -99,17 +97,14 @@ def cb_tunnel_thruster(self, msg: geometry_msgs.msg.Wrench): def cb_port_thruster(self, msg: geometry_msgs.msg.Wrench): fx = np.clip(msg.force.x, -1.0, 1.0) fy = np.clip(msg.force.y, -1.0, 1.0) - # Apply a deadband to the thruster commands if np.linalg.norm([fx, fy]) < 0.1: fx = 0.0 fy = 0.0 - self.u[1] = fx self.u[2] = fy - issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "stern_port_thruster_link" + issued.header.frame_id = self._frame("stern_port_thruster_link") issued.header.stamp = self.get_clock().now().to_msg() issued.wrench.force.x = fx issued.wrench.force.y = fy @@ -118,17 +113,14 @@ def cb_port_thruster(self, msg: geometry_msgs.msg.Wrench): def cb_starboard_thruster(self, msg: geometry_msgs.msg.Wrench): fx = np.clip(msg.force.x, -1.0, 1.0) fy = np.clip(msg.force.y, -1.0, 1.0) - # Apply a deadband to the thruster commands if np.linalg.norm([fx, fy]) < 0.1: fx = 0.0 fy = 0.0 - self.u[3] = fx self.u[4] = fy - issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "stern_starboard_thruster_link" + issued.header.frame_id = self._frame("stern_starboard_thruster_link") issued.header.stamp = self.get_clock().now().to_msg() issued.wrench.force.x = fx issued.wrench.force.y = fy diff --git a/cybership_utilities/html/index.html b/cybership_utilities/html/index.html new file mode 100644 index 0000000..85c1f39 --- /dev/null +++ b/cybership_utilities/html/index.html @@ -0,0 +1,243 @@ + + + + + Mux Controller (Foxglove roslibjs) + + + + + + +

Mux Controller foxglove_bridge + @foxglove/roslibjs

+ +
+ Connection +
+ + + + Disconnected +
+
+ +
+ Thruster +
+ + + Idle +
+
+ +
+ Force Mux +
+ + + + +
+
Idle
+
+ +
+ Velocity Mux +
+ + + + +
+
Idle
+
+ + + + diff --git a/cybership_viz/config/drillship.rviz b/cybership_viz/config/drillship.rviz index 74f0a34..6fb8622 100644 --- a/cybership_viz/config/drillship.rviz +++ b/cybership_viz/config/drillship.rviz @@ -6,7 +6,13 @@ Panels: Expanded: - /Global Options1 - /Status1 + - /TF1 + - /TF1/Frames1 + - /TF1/Tree1 - /Odometry1/Shape1 + - /RobotModel1 + - /RobotModel1/Status1 + - /RobotModel1/Description Topic1 Splitter Ratio: 0.5596868991851807 Tree Height: 1132 - Class: rviz_common/Selection @@ -50,26 +56,30 @@ Visualization Manager: Value: true - Class: rviz_default_plugins/TF Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" Frame Timeout: 15 Frames: All Enabled: true - aft_center_thruster_link: + drillship/aft_center_thruster_link: Value: true - aft_port_thruster_link: + drillship/aft_port_thruster_link: Value: true - aft_starboard_thruster_link: + drillship/aft_starboard_thruster_link: Value: true - base_link: + drillship/base_link: Value: true - bow_center_thruster_link: + drillship/base_link_ned: Value: true - bow_port_thruster_link: + drillship/bow_center_thruster_link: Value: true - bow_starboard_thruster_link: + drillship/bow_port_thruster_link: Value: true - imu_link: + drillship/bow_starboard_thruster_link: Value: true - mocap_link: + drillship/imu_link: + Value: true + drillship/mocap_link: Value: true world: Value: true @@ -80,22 +90,23 @@ Visualization Manager: Show Names: true Tree: world: - base_link: - aft_center_thruster_link: - {} - aft_port_thruster_link: - {} - aft_starboard_thruster_link: - {} - bow_center_thruster_link: - {} - bow_port_thruster_link: - {} - bow_starboard_thruster_link: - {} - imu_link: - {} - mocap_link: + drillship/base_link_ned: + drillship/base_link: + drillship/aft_center_thruster_link: + {} + drillship/aft_port_thruster_link: + {} + drillship/aft_starboard_thruster_link: + {} + drillship/bow_center_thruster_link: + {} + drillship/bow_port_thruster_link: + {} + drillship/bow_starboard_thruster_link: + {} + drillship/imu_link: + {} + drillship/mocap_link: {} Update Interval: 0 Value: true @@ -287,6 +298,73 @@ Visualization Manager: Reliability Policy: Reliable Value: measurement/odom Value: true + - Alpha: 0.5 + Class: rviz_default_plugins/RobotModel + Collision Enabled: true + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /drillship/robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + aft_center_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + aft_port_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + aft_starboard_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link_ned: + Alpha: 1 + Show Axes: false + Show Trail: false + bow_center_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + bow_port_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + bow_starboard_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + mocap_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: drillship + Update Interval: 0 + Value: true + Visual Enabled: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -333,7 +411,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 8.380566596984863 + Distance: 4.215280055999756 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -345,21 +423,21 @@ Visualization Manager: Z: 0 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false + Invert Z Axis: true Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.6697965264320374 + Pitch: 0.7303979992866516 Target Frame: base_link Value: Orbit (rviz) - Yaw: 2.2503936290740967 + Yaw: 0.4803977608680725 Saved: ~ Window Geometry: Displays: collapsed: false Height: 1423 Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000201000004f5fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000004f5000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000008330000003efc0100000002fb0000000800540069006d00650100000000000008330000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000062c000004f500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000304000004f5fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000004f5000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000004f5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000004f5000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000008330000003efc0100000002fb0000000800540069006d00650100000000000008330000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000423000004f500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -367,7 +445,7 @@ Window Geometry: Tool Properties: collapsed: false Views: - collapsed: true + collapsed: false Width: 2099 - X: 4319 - Y: 224 + X: 4255 + Y: 160 diff --git a/cybership_viz/config/multi.rviz b/cybership_viz/config/multi.rviz new file mode 100644 index 0000000..a8fa91f --- /dev/null +++ b/cybership_viz/config/multi.rviz @@ -0,0 +1,651 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1 + - /TF1/Frames1 + - /TF1/Tree1 + - /Odometry1 + - /Odometry1/Shape1 + - /RobotModel1 + - /RobotModel1/Mass Properties1 + - /RobotModel1/Description Topic1 + - /Marker1 + - /MarkerArray1 + - /RobotModel2 + - /Wrench1 + - /Voyager1 + - /Drillship1 + - /Drillship1/Wrench1 + - /Drillship1/Wrench2 + - /Drillship1/Wrench3 + Splitter Ratio: 0.5 + Tree Height: 709 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: true + drillship/aft_center_thruster_link: + Value: true + drillship/aft_port_thruster_link: + Value: true + drillship/aft_starboard_thruster_link: + Value: true + drillship/base_link: + Value: true + drillship/base_link_ned: + Value: true + drillship/bow_center_thruster_link: + Value: true + drillship/bow_port_thruster_link: + Value: true + drillship/bow_starboard_thruster_link: + Value: true + drillship/imu_link: + Value: true + drillship/mocap_link: + Value: true + voyager/base_link: + Value: true + voyager/base_link_ned: + Value: true + voyager/bow_tunnel_thruster_link: + Value: true + voyager/imu_link: + Value: true + voyager/mocap_link: + Value: true + voyager/stern_port_thruster_link: + Value: true + voyager/stern_starboard_thruster_link: + Value: true + world: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + world: + drillship/base_link_ned: + drillship/base_link: + drillship/aft_center_thruster_link: + {} + drillship/aft_port_thruster_link: + {} + drillship/aft_starboard_thruster_link: + {} + drillship/bow_center_thruster_link: + {} + drillship/bow_port_thruster_link: + {} + drillship/bow_starboard_thruster_link: + {} + drillship/imu_link: + {} + drillship/mocap_link: + {} + voyager/base_link_ned: + voyager/base_link: + voyager/bow_tunnel_thruster_link: + {} + voyager/imu_link: + {} + voyager/stern_port_thruster_link: + {} + voyager/stern_starboard_thruster_link: + {} + voyager/mocap_link: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 255; 25; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: PoseWithCovariance + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: measurement/pose + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 0.25 + Axes Radius: 0.02500000037252903 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: measurement/odom + Value: true + - Alpha: 0.4000000059604645 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /drillship/robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + aft_center_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + aft_port_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + aft_starboard_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link_ned: + Alpha: 1 + Show Axes: false + Show Trail: false + bow_center_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + bow_port_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + bow_starboard_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + mocap_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: drillship + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Marker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /voyager/visualization_marker + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /voyager/visualization_marker_array + Value: true + - Alpha: 0.4000000059604645 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /voyager/robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link_ned: + Alpha: 1 + Show Axes: false + Show Trail: false + bow_tunnel_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + mocap_link: + Alpha: 1 + Show Axes: false + Show Trail: false + stern_port_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + stern_starboard_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: voyager + Update Interval: 0 + Value: true + Visual Enabled: true + - Accept NaN Values: false + Alpha: 1 + Arrow Width: 0.5 + Class: rviz_default_plugins/Wrench + Enabled: true + Force Arrow Scale: 2 + Force Color: 204; 51; 51 + History Length: 1 + Name: Wrench + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /drillship/thruster/aft_center/issued + Torque Arrow Scale: 2 + Torque Color: 204; 204; 51 + Value: true + - Class: rviz_common/Group + Displays: + - Accept NaN Values: false + Alpha: 1 + Arrow Width: 0.5 + Class: rviz_default_plugins/Wrench + Enabled: true + Force Arrow Scale: 2 + Force Color: 204; 51; 51 + History Length: 1 + Name: Wrench + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /voyager/thruster/port/issued + Torque Arrow Scale: 2 + Torque Color: 204; 204; 51 + Value: true + - Accept NaN Values: false + Alpha: 1 + Arrow Width: 0.5 + Class: rviz_default_plugins/Wrench + Enabled: true + Force Arrow Scale: 2 + Force Color: 204; 51; 51 + History Length: 1 + Name: Wrench + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /voyager/thruster/starboard/issued + Torque Arrow Scale: 2 + Torque Color: 204; 204; 51 + Value: true + - Accept NaN Values: false + Alpha: 1 + Arrow Width: 0.5 + Class: rviz_default_plugins/Wrench + Enabled: true + Force Arrow Scale: 2 + Force Color: 204; 51; 51 + History Length: 1 + Name: Wrench + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /voyager/thruster/tunnel/issued + Torque Arrow Scale: 2 + Torque Color: 204; 204; 51 + Value: true + Enabled: true + Name: Voyager + - Class: rviz_common/Group + Displays: + - Accept NaN Values: false + Alpha: 1 + Arrow Width: 0.5 + Class: rviz_default_plugins/Wrench + Enabled: true + Force Arrow Scale: 2 + Force Color: 204; 51; 51 + History Length: 1 + Name: Wrench + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /drillship/thruster/aft_center/issued + Torque Arrow Scale: 2 + Torque Color: 204; 204; 51 + Value: true + - Accept NaN Values: false + Alpha: 1 + Arrow Width: 0.5 + Class: rviz_default_plugins/Wrench + Enabled: true + Force Arrow Scale: 2 + Force Color: 204; 51; 51 + History Length: 1 + Name: Wrench + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /drillship/thruster/aft_port/issued + Torque Arrow Scale: 2 + Torque Color: 204; 204; 51 + Value: true + - Accept NaN Values: false + Alpha: 1 + Arrow Width: 0.5 + Class: rviz_default_plugins/Wrench + Enabled: true + Force Arrow Scale: 2 + Force Color: 204; 51; 51 + History Length: 1 + Name: Wrench + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /drillship/thruster/aft_starboard/issued + Torque Arrow Scale: 2 + Torque Color: 204; 204; 51 + Value: true + - Accept NaN Values: false + Alpha: 1 + Arrow Width: 0.5 + Class: rviz_default_plugins/Wrench + Enabled: true + Force Arrow Scale: 2 + Force Color: 204; 51; 51 + History Length: 1 + Name: Wrench + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /drillship/thruster/bow_center/issued + Torque Arrow Scale: 2 + Torque Color: 204; 204; 51 + Value: true + - Accept NaN Values: false + Alpha: 1 + Arrow Width: 0.5 + Class: rviz_default_plugins/Wrench + Enabled: true + Force Arrow Scale: 2 + Force Color: 204; 51; 51 + History Length: 1 + Name: Wrench + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /drillship/thruster/bow_port/issued + Torque Arrow Scale: 2 + Torque Color: 204; 204; 51 + Value: true + - Accept NaN Values: false + Alpha: 1 + Arrow Width: 0.5 + Class: rviz_default_plugins/Wrench + Enabled: true + Force Arrow Scale: 2 + Force Color: 204; 51; 51 + History Length: 1 + Name: Wrench + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /drillship/thruster/bow_starboard/issued + Torque Arrow Scale: 2 + Torque Color: 204; 204; 51 + Value: true + Enabled: true + Name: Drillship + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: true + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.8403981328010559 + Target Frame: base_link + Value: Orbit (rviz) + Yaw: 0.7353982329368591 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1000 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000002ab0000034efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000033100000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000034e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001000000034efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000034e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006810000003efc0100000002fb0000000800540069006d00650100000000000006810000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000003d00000034e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1665 + X: 1148 + Y: 580 diff --git a/cybership_viz/config/voyager.rviz b/cybership_viz/config/voyager.rviz index 56da955..070057d 100644 --- a/cybership_viz/config/voyager.rviz +++ b/cybership_viz/config/voyager.rviz @@ -6,17 +6,18 @@ Panels: Expanded: - /Global Options1 - /Status1 - - /TF1 - /TF1/Frames1 - /TF1/Tree1 + - /Odometry1 - /Odometry1/Shape1 - - /Wrench4/Topic1 + - /RobotModel1 - /RobotModel1/Mass Properties1 - /RobotModel1/Description Topic1 - /Marker1 - /MarkerArray1 + - /RobotModel2 Splitter Ratio: 0.5 - Tree Height: 555 + Tree Height: 1764 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -63,19 +64,39 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: true - base_link: + drillship/aft_center_thruster_link: Value: true - base_link_ned: + drillship/aft_port_thruster_link: Value: true - bow_tunnel_thruster_link: + drillship/aft_starboard_thruster_link: Value: true - imu_link: + drillship/base_link: Value: true - mocap_link: + drillship/base_link_ned: Value: true - stern_port_thruster_link: + drillship/bow_center_thruster_link: Value: true - stern_starboard_thruster_link: + drillship/bow_port_thruster_link: + Value: true + drillship/bow_starboard_thruster_link: + Value: true + drillship/imu_link: + Value: true + drillship/mocap_link: + Value: true + voyager/base_link: + Value: true + voyager/base_link_ned: + Value: true + voyager/bow_tunnel_thruster_link: + Value: true + voyager/imu_link: + Value: true + voyager/mocap_link: + Value: true + voyager/stern_port_thruster_link: + Value: true + voyager/stern_starboard_thruster_link: Value: true world: Value: true @@ -86,77 +107,38 @@ Visualization Manager: Show Names: false Tree: world: - base_link_ned: - base_link: - bow_tunnel_thruster_link: + drillship/base_link_ned: + drillship/base_link: + drillship/aft_center_thruster_link: + {} + drillship/aft_port_thruster_link: + {} + drillship/aft_starboard_thruster_link: + {} + drillship/bow_center_thruster_link: {} - imu_link: + drillship/bow_port_thruster_link: {} - mocap_link: + drillship/bow_starboard_thruster_link: {} - stern_port_thruster_link: + drillship/imu_link: {} - stern_starboard_thruster_link: + drillship/mocap_link: + {} + voyager/base_link_ned: + voyager/base_link: + voyager/bow_tunnel_thruster_link: {} + voyager/imu_link: + {} + voyager/stern_port_thruster_link: + {} + voyager/stern_starboard_thruster_link: + {} + voyager/mocap_link: + {} Update Interval: 0 Value: true - - Accept NaN Values: false - Alpha: 1 - Arrow Width: 0.5 - Class: rviz_default_plugins/Wrench - Enabled: true - Force Arrow Scale: 2 - Force Color: 85; 255; 255 - History Length: 1 - Name: Wrench - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: thruster/port/issued - Torque Arrow Scale: 2 - Torque Color: 204; 204; 51 - Value: true - - Accept NaN Values: false - Alpha: 1 - Arrow Width: 0.5 - Class: rviz_default_plugins/Wrench - Enabled: true - Force Arrow Scale: 2 - Force Color: 85; 255; 255 - History Length: 1 - Name: Wrench - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: thruster/starboard/issued - Torque Arrow Scale: 2 - Torque Color: 204; 204; 51 - Value: true - - Accept NaN Values: false - Alpha: 1 - Arrow Width: 0.5 - Class: rviz_default_plugins/Wrench - Enabled: true - Force Arrow Scale: 2 - Force Color: 85; 255; 255 - History Length: 1 - Name: Wrench - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: thruster/tunnel/issued - Torque Arrow Scale: 2 - Torque Color: 204; 204; 51 - Value: true - Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 @@ -231,25 +213,6 @@ Visualization Manager: Reliability Policy: Reliable Value: measurement/odom Value: true - - Accept NaN Values: false - Alpha: 1 - Arrow Width: 0.5 - Class: rviz_default_plugins/Wrench - Enabled: true - Force Arrow Scale: 2 - Force Color: 0; 170; 255 - History Length: 1 - Name: Wrench - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: allocated - Torque Arrow Scale: 2 - Torque Color: 204; 204; 51 - Value: true - Alpha: 0.4000000059604645 Class: rviz_default_plugins/RobotModel Collision Enabled: false @@ -260,7 +223,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /voyager/robot_description + Value: /drillship/robot_description Enabled: true Links: All Links Enabled: true @@ -268,6 +231,18 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + aft_center_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + aft_port_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + aft_starboard_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false base_link: Alpha: 1 Show Axes: false @@ -277,23 +252,23 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - bow_tunnel_thruster_link: + bow_center_thruster_link: Alpha: 1 Show Axes: false Show Trail: false - imu_link: + bow_port_thruster_link: Alpha: 1 Show Axes: false Show Trail: false - mocap_link: + bow_starboard_thruster_link: Alpha: 1 Show Axes: false Show Trail: false - stern_port_thruster_link: + imu_link: Alpha: 1 Show Axes: false Show Trail: false - stern_starboard_thruster_link: + mocap_link: Alpha: 1 Show Axes: false Show Trail: false @@ -301,7 +276,7 @@ Visualization Manager: Inertia: false Mass: false Name: RobotModel - TF Prefix: "" + TF Prefix: drillship Update Interval: 0 Value: true Visual Enabled: true @@ -330,6 +305,61 @@ Visualization Manager: Reliability Policy: Reliable Value: /voyager/visualization_marker_array Value: true + - Alpha: 0.4000000059604645 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /voyager/robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link_ned: + Alpha: 1 + Show Axes: false + Show Trail: false + bow_tunnel_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + mocap_link: + Alpha: 1 + Show Axes: false + Show Trail: false + stern_port_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + stern_starboard_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: voyager + Update Interval: 0 + Value: true + Visual Enabled: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -391,18 +421,18 @@ Visualization Manager: Invert Z Axis: true Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.7303981781005859 + Pitch: 0.4803982079029083 Target Frame: base_link Value: Orbit (rviz) - Yaw: 1.440398097038269 + Yaw: 5.228579521179199 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 846 + Height: 2055 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000018e000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000033100000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000031c000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000018e0000076dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000033100000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000076d000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000ef40000003efc0100000002fb0000000800540069006d0065010000000000000ef40000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000d600000076d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -411,6 +441,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1200 - X: 1921 - Y: 850 + Width: 3828 + X: 0 + Y: 0 diff --git a/cybership_viz/launch/multi.launch.py b/cybership_viz/launch/multi.launch.py new file mode 100644 index 0000000..076781c --- /dev/null +++ b/cybership_viz/launch/multi.launch.py @@ -0,0 +1,41 @@ +import launch +import launch_ros + +import cybership_utilities.launch + +# import os +# from ament_index_python.packages import get_package_share_directory +# from launch import LaunchDescription +# from launch.actions import * +# from launch.substitutions import LaunchConfiguration +# from launch_ros.actions import Node +# from launch.launch_description_sources import * + + +def generate_launch_description(): + + ld = launch.LaunchDescription() + + for argument in cybership_utilities.launch.COMMON_ARGUMENTS: + ld.add_action(argument) + + subsitution_rviz_config_path = launch.substitutions.PathJoinSubstitution( + [ + launch_ros.substitutions.FindPackageShare("cybership_viz"), + "config", + "multi.rviz", + ] + ) + + node_rviz = launch_ros.actions.Node( + package="rviz2", + executable="rviz2", + name=f"rviz_{cybership_utilities.launch.anon()}", + namespace=launch.substitutions.LaunchConfiguration("vessel_name"), + output="screen", + arguments=["-d", subsitution_rviz_config_path], + ) + + ld.add_action(node_rviz) + + return ld From 7def400d4987af65f6e6f694d64dc12fd0dcad30 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Thu, 28 Aug 2025 17:00:36 +0200 Subject: [PATCH 34/46] fix: add missing tf prefixes --- .../nodes/los_guidance_client.py | 74 +---- .../nodes/los_guidance_server.py | 283 +--------------- .../guidance/ros_action_client.py | 98 ++++++ .../guidance/ros_action_server.py | 303 ++++++++++++++++++ .../launch/drillship.launch.py | 3 +- .../launch/enterprise.launch.py | 3 +- cybership_simulator/launch/voyager.launch.py | 3 +- cybership_viz/config/voyager.rviz | 102 +----- 8 files changed, 428 insertions(+), 441 deletions(-) create mode 100755 cybership_controller/src/cybership_controller/guidance/ros_action_client.py create mode 100755 cybership_controller/src/cybership_controller/guidance/ros_action_server.py diff --git a/cybership_controller/nodes/los_guidance_client.py b/cybership_controller/nodes/los_guidance_client.py index 4d661d3..27fe98a 100755 --- a/cybership_controller/nodes/los_guidance_client.py +++ b/cybership_controller/nodes/los_guidance_client.py @@ -12,73 +12,23 @@ from nav_msgs.msg import Path from geometry_msgs.msg import PoseStamped from cybership_interfaces.action import LOSGuidance - -class LOSGuidanceClient(Node): - def __init__(self): - super().__init__('los_guidance_client') - self._action_client = ActionClient(self, LOSGuidance, 'los_guidance') - - def send_goal(self, waypoints): - goal_msg = LOSGuidance.Goal() - path_msg = Path() - path_msg.header.frame_id = 'world' - path_msg.header.stamp = self.get_clock().now().to_msg() - for x, y in waypoints: - pose = PoseStamped() - pose.header.frame_id = 'world' - pose.header.stamp = path_msg.header.stamp - pose.pose.position.x = x - pose.pose.position.y = y - pose.pose.position.z = 0.0 - pose.pose.orientation.w = 1.0 - path_msg.poses.append(pose) - goal_msg.path = path_msg - - self.get_logger().info('Waiting for action server...') - if not self._action_client.wait_for_server(timeout_sec=5.0): - self.get_logger().error('Action server not available, shutting down') - rclpy.shutdown() - return - - self.get_logger().info('Sending goal...') - send_goal_future = self._action_client.send_goal_async( - goal_msg, feedback_callback=self.feedback_callback) - send_goal_future.add_done_callback(self.goal_response_callback) - - def goal_response_callback(self, future): - goal_handle = future.result() - if not goal_handle.accepted: - self.get_logger().info('Goal rejected :(') - return - self.get_logger().info('Goal accepted :)') - result_future = goal_handle.get_result_async() - result_future.add_done_callback(self.get_result_callback) - - def feedback_callback(self, feedback_msg): - feedback = feedback_msg.feedback - heading_deg = feedback.heading * 180.0 / 3.141592653589793 - vel = feedback.vel_cmd - self.get_logger().info( - f'Feedback: Heading={heading_deg:.1f}°, Vel=[{vel.x:.2f}, {vel.y:.2f}, {vel.z:.2f}]') - - def get_result_callback(self, future): - result = future.result().result - success = result.success - self.get_logger().info(f'Result: success={success}') - rclpy.shutdown() - +from cybership_controller.guidance.ros_action_client import LOSGuidanceClient +import time def main(args=None): rclpy.init(args=args) client = LOSGuidanceClient() # Example waypoints for testing - example_waypoints = [ - (0.0, 0.0), - (np.random.normal(6.0, 1.0), 0.0), - (np.random.normal(6.0, 1.0),np.random.normal(6.0, 1.0)), - (0.0, np.random.normal(6.0, 1.0)), - ] - client.send_goal(example_waypoints) + while rclpy.ok(): + example_waypoints = [ + (0.0, 0.0), + (np.random.normal(6.0, 0.5), 0.0), + (np.random.normal(6.0, 0.5),np.random.normal(6.0, 0.5)), + (0.0, np.random.normal(6.0, 0.5)), + ] + client.send_goal(example_waypoints) + time.sleep(5.0) + client.get_logger().info('Requesting goal cancellation...') # Use MultiThreadedExecutor to handle feedback continuously executor = MultiThreadedExecutor() executor.add_node(client) diff --git a/cybership_controller/nodes/los_guidance_server.py b/cybership_controller/nodes/los_guidance_server.py index ac676f4..97d3ae0 100755 --- a/cybership_controller/nodes/los_guidance_server.py +++ b/cybership_controller/nodes/los_guidance_server.py @@ -4,288 +4,9 @@ """ import math import rclpy -from rclpy.node import Node -from rclpy.action import ActionServer, ActionClient -from rclpy.action import CancelResponse, GoalResponse -from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor -from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup -from rclpy.qos import QoSProfile, DurabilityPolicy - -from nav_msgs.msg import Path, Odometry -from geometry_msgs.msg import Twist, Vector3, Point -from visualization_msgs.msg import Marker -from tf_transformations import euler_from_quaternion - -from cybership_interfaces.action import LOSGuidance - -# Import underlying LOS guidance implementation -from cybership_controller.guidance.los import LOSGuidance as BaseLOSGuidance -import numpy as np -import typing -from scipy.interpolate import splev - - -class LOSGuidanceROS(Node): - def __init__(self): - super().__init__('los_guidance_server', namespace="cybership") - # Parameters - self.declare_parameter('desired_speed', 0.5) - self.declare_parameter('lookahead', 0.4) - # Heading control gain - self.declare_parameter('heading_gain', 1.0) - # Publishers and Subscribers - self._cmd_pub = self.create_publisher( - Twist, 'control/velocity/command', 10) - # Publish markers with transient local durability so RViZ receives past markers - self._marker_pub = self.create_publisher( - Marker, 'visualization_marker', 10) - self._odom_sub = self.create_subscription( - Odometry, 'measurement/odom', self.odom_callback, 10) - # Vehicle pose - self._position = None - self._yaw = None - # Action Server - self._action_server = ActionServer( - self, - LOSGuidance, - 'los_guidance', - execute_callback=self.execute_callback, - goal_callback=self.goal_callback, - cancel_callback=self.cancel_callback, - callback_group=ReentrantCallbackGroup()) - - self.goal_uuids: typing.List = [] - - self.get_logger().info('LOS Guidance Action Server started') - - def odom_callback(self, msg: Odometry): - # Update position and heading - self._position = (msg.pose.pose.position.x, msg.pose.pose.position.y) - q = msg.pose.pose.orientation - # Convert quaternion to yaw using tf_transformations - quat = [q.x, q.y, q.z, q.w] - _, _, yaw = euler_from_quaternion(quat) - self._yaw = yaw - - def goal_callback(self, goal_request: LOSGuidance.Goal) -> GoalResponse: - if len(goal_request.path.poses) < 2: - self.get_logger().warn("Path has fewer than 2 poses — rejecting goal.") - return GoalResponse.REJECT - - return GoalResponse.ACCEPT - - def cancel_callback(self, goal_handle) -> CancelResponse: - - return CancelResponse.ACCEPT - - def prepare_waypoints(self, path_msg: Path, extend: bool = True, n_intermediate: int = 2) -> np.ndarray: - waypoints = [(pose.pose.position.x, pose.pose.position.y) - for pose in path_msg.poses] - - if extend: - # Add intermediate points - extended_waypoints = [] - for i in range(len(waypoints) - 1): - p1 = waypoints[i] - p2 = waypoints[i + 1] - extended_waypoints.append(p1) - # Add intermediate points - for j in range(1, n_intermediate + 1): - mid_point = ( - p1[0] + (p2[0] - p1[0]) * j / (n_intermediate + 1), - p1[1] + (p2[1] - p1[1]) * j / (n_intermediate + 1) - ) - extended_waypoints.append(mid_point) - extended_waypoints.append(waypoints[-1]) - waypoints = extended_waypoints - - return np.array(waypoints) - async def execute_callback(self, goal_handle: LOSGuidance.Goal) -> LOSGuidance.Result: - - if len(self.goal_uuids) > 0: - self.goal_uuids.pop(0) - - self.goal_uuids.append(tuple(goal_handle.goal_id.uuid)) - - path_msg: Path = goal_handle.request.path - waypoints = [] - waypoints.extend([(pose.pose.position.x, pose.pose.position.y) - for pose in path_msg.poses]) - if self._position is not None: - waypoints.append(self._position) - - waypoints = self.prepare_waypoints( - path_msg, extend=True, n_intermediate=2) - - # Initialize guidance - hz = 2 - rate = self.create_rate(hz) - V_d = float(self.get_parameter('desired_speed').value) - delta = float(self.get_parameter('lookahead').value) - # Heading control gain - k_h = float(self.get_parameter('heading_gain').value) - guidance = BaseLOSGuidance(waypoints, V_d=V_d, delta=delta) - last_wp = waypoints[-1] - # Visualize spline path - self.publish_spline_marker(guidance) - - dt = 1.0 / hz - - canceled = False - while rclpy.ok(): - self.get_logger().info(f"{self.goal_uuids}") - if tuple(goal_handle.goal_id.uuid) not in self.goal_uuids: - self.get_logger().info( - f"LOS guidance goal no longer current. {goal_handle.goal_id.uuid}") - canceled = True - break - - if goal_handle.is_cancel_requested: - goal_handle.canceled() - self.get_logger().info( - f"LOS guidance canceled. {goal_handle.goal_id.uuid}") - break - if not goal_handle.is_active: - self.get_logger().info( - f"LOS guidance no longer active. {goal_handle.goal_id.uuid}") - break - if self._position is None or self._yaw is None: - rate.sleep() - continue - x, y = self._position - chi_d, vel_cmd, extras = guidance.guidance(x, y, dt=dt) - twist = Twist() - # Compute forward velocity in body frame using current heading - v_forward = vel_cmd[0] * \ - math.cos(self._yaw) + vel_cmd[1] * math.sin(self._yaw) - # Use computed forward speed (ensuring non-negative speed) - twist.linear.x = max(v_forward, 0.0) - twist.linear.y = 0.0 - twist.linear.z = 0.0 - # Compute and normalize heading error - heading_error = math.atan2( - math.sin(chi_d - self._yaw), math.cos(chi_d - self._yaw)) - twist.angular.z = k_h * heading_error - self._cmd_pub.publish(twist) - # Feedback - feedback = LOSGuidance.Feedback() - feedback.heading = chi_d - feedback.vel_cmd = Vector3(x=vel_cmd[0], y=vel_cmd[1], z=0.0) - goal_handle.publish_feedback(feedback) - self.publish_path_marker(path_msg) - self.publish_arrow_marker(x, y, vel_cmd) - # Look-ahead visualization - # x_la, y_la, _, _ = guidance.lookahead_point(x, y) - self.publish_lookahead_marker( - extras["lookahead_point"][0], extras["lookahead_point"][1]) - # self.publish_lookahead_line(x, y, x_la, y_la) - # Check completion - if math.hypot(x - last_wp[0], y - last_wp[1]) < delta: - break - rate.sleep() - - if not canceled: - goal_handle.succeed() - self.get_logger().info("LOS guidance finished (result empty).") - return LOSGuidance.Result() - - def publish_path_marker(self, path_msg: Path): - marker = Marker() - marker.header = path_msg.header - marker.ns = 'los_path' - marker.id = 0 - marker.type = Marker.LINE_STRIP - marker.action = Marker.ADD - marker.scale.x = 0.05 - marker.color.r = 1.0 - marker.color.g = 0.0 - marker.color.b = 0.0 - marker.color.a = 1.0 - marker.points = [Point(x=pose.pose.position.x, - y=pose.pose.position.y, - z=0.0) - for pose in path_msg.poses] - self._marker_pub.publish(marker) - - def publish_arrow_marker(self, x, y, vel_cmd): - marker = Marker() - marker.header.frame_id = 'world' - marker.header.stamp = self.get_clock().now().to_msg() - marker.ns = 'los_arrow' - marker.id = 1 - marker.type = Marker.ARROW - marker.action = Marker.ADD - marker.scale.x = 0.1 - marker.scale.y = 0.1 - marker.scale.z = 0.1 - # Use a copy of vel_cmd for scaling to avoid in-place modification - vel_arrow = vel_cmd * 3 - marker.color.r = 0.0 - marker.color.g = 1.0 - marker.color.b = 0.5 - marker.color.a = 1.0 - start = Point(x=x, y=y, z=0.0) - end = Point(x=x + vel_arrow[0], y=y + vel_arrow[1], z=0.0) - marker.points = [start, end] - self._marker_pub.publish(marker) - - def publish_spline_marker(self, guidance): - marker = Marker() - marker.header.frame_id = 'world' - marker.header.stamp = self.get_clock().now().to_msg() - marker.ns = 'los_spline' - marker.id = 2 - marker.type = Marker.LINE_STRIP - marker.action = Marker.ADD - marker.scale.x = 0.05 - marker.color.r = 0.0 - marker.color.g = 0.0 - marker.color.b = 1.0 - marker.color.a = 1.0 - # Sample spline path points - ts = np.linspace(0, 1, 1000) - spline_pts = np.array(splev(ts, guidance.tck)).T - marker.points = [Point(x=pt[0], y=pt[1], z=0.0) for pt in spline_pts] - self._marker_pub.publish(marker) - - def publish_lookahead_marker(self, x_la, y_la): - m = Marker() - m.header.frame_id = 'world' - m.header.stamp = self.get_clock().now().to_msg() - m.ns = 'los_lookahead' - m.id = 3 - m.type = Marker.SPHERE - m.action = Marker.ADD - m.scale.x = 0.25 # diameter in meters - m.scale.y = 0.25 - m.scale.z = 0.25 - m.color.r = 1.0 - m.color.g = 0.85 - m.color.b = 0.0 - m.color.a = 1.0 # golden/yellow - m.pose.position.x = float(x_la) - m.pose.position.y = float(y_la) - m.pose.position.z = 0.0 - self._marker_pub.publish(m) - - def publish_lookahead_line(self, x, y, x_la, y_la): - m = Marker() - m.header.frame_id = 'world' - m.header.stamp = self.get_clock().now().to_msg() - m.ns = 'los_lookahead' - m.id = 4 - m.type = Marker.LINE_STRIP - m.action = Marker.ADD - m.scale.x = 0.03 - m.color.r = 1.0 - m.color.g = 0.85 - m.color.b = 0.0 - m.color.a = 0.9 - start = Point(x=float(x), y=float(y), z=0.0) - end = Point(x=float(x_la), y=float(y_la), z=0.0) - m.points = [start, end] - self._marker_pub.publish(m) +from cybership_controller.guidance.ros_action_server import LOSGuidanceROS +from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor def main(args=None): diff --git a/cybership_controller/src/cybership_controller/guidance/ros_action_client.py b/cybership_controller/src/cybership_controller/guidance/ros_action_client.py new file mode 100755 index 0000000..fd4364b --- /dev/null +++ b/cybership_controller/src/cybership_controller/guidance/ros_action_client.py @@ -0,0 +1,98 @@ +#!/usr/bin/env python3 +""" +ROS2 Action Client for LOS Guidance +""" +import rclpy +from rclpy.node import Node +from rclpy.action import ActionClient +from rclpy.executors import MultiThreadedExecutor + +import numpy as np + +from nav_msgs.msg import Path +from geometry_msgs.msg import PoseStamped +from cybership_interfaces.action import LOSGuidance + +class LOSGuidanceClient(Node): + def __init__(self): + super().__init__('los_guidance_client') + self._action_client = ActionClient(self, LOSGuidance, 'los_guidance') + + def send_goal(self, waypoints): + goal_msg = LOSGuidance.Goal() + path_msg = Path() + path_msg.header.frame_id = 'world' + path_msg.header.stamp = self.get_clock().now().to_msg() + for x, y in waypoints: + pose = PoseStamped() + pose.header.frame_id = 'world' + pose.header.stamp = path_msg.header.stamp + pose.pose.position.x = x + pose.pose.position.y = y + pose.pose.position.z = 0.0 + pose.pose.orientation.w = 1.0 + path_msg.poses.append(pose) + goal_msg.path = path_msg + + self.get_logger().info('Waiting for action server...') + if not self._action_client.wait_for_server(timeout_sec=5.0): + self.get_logger().error('Action server not available, shutting down') + rclpy.shutdown() + return + + self.get_logger().info('Sending goal...') + send_goal_future = self._action_client.send_goal_async( + goal_msg, feedback_callback=self.feedback_callback) + send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info('Goal rejected :(') + return + self.get_logger().info('Goal accepted :)') + result_future = goal_handle.get_result_async() + result_future.add_done_callback(self.get_result_callback) + + def feedback_callback(self, feedback_msg): + feedback = feedback_msg.feedback + heading_deg = feedback.heading * 180.0 / 3.141592653589793 + vel = feedback.vel_cmd + self.get_logger().info( + f'Feedback: Heading={heading_deg:.1f}°, Vel=[{vel.x:.2f}, {vel.y:.2f}, {vel.z:.2f}]') + + def get_result_callback(self, future): + result = future.result().result + success = result.success + self.get_logger().info(f'Result: success={success}') + rclpy.shutdown() + + def cancel_goal(self): + if self._action_client.server_is_ready(): + self.get_logger().info('Cancelling goal...') + cancel_future = self._action_client.cancel_all_goals_async() + cancel_future.add_done_callback(self.cancel_done) + + +def main(args=None): + rclpy.init(args=args) + client = LOSGuidanceClient() + # Example waypoints for testing + example_waypoints = [ + (0.0, 0.0), + (np.random.normal(6.0, 1.0), 0.0), + (np.random.normal(6.0, 1.0),np.random.normal(6.0, 1.0)), + (0.0, np.random.normal(6.0, 1.0)), + ] + client.send_goal(example_waypoints) + # Use MultiThreadedExecutor to handle feedback continuously + executor = MultiThreadedExecutor() + executor.add_node(client) + executor.spin() + executor.shutdown() + client.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/cybership_controller/src/cybership_controller/guidance/ros_action_server.py b/cybership_controller/src/cybership_controller/guidance/ros_action_server.py new file mode 100755 index 0000000..ac676f4 --- /dev/null +++ b/cybership_controller/src/cybership_controller/guidance/ros_action_server.py @@ -0,0 +1,303 @@ +#!/usr/bin/env python3 +""" +ROS2 Action Server for LOS Guidance using cybership_interfaces/LOSGuidance +""" +import math +import rclpy +from rclpy.node import Node +from rclpy.action import ActionServer, ActionClient +from rclpy.action import CancelResponse, GoalResponse +from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup +from rclpy.qos import QoSProfile, DurabilityPolicy + +from nav_msgs.msg import Path, Odometry +from geometry_msgs.msg import Twist, Vector3, Point +from visualization_msgs.msg import Marker +from tf_transformations import euler_from_quaternion + +from cybership_interfaces.action import LOSGuidance + +# Import underlying LOS guidance implementation +from cybership_controller.guidance.los import LOSGuidance as BaseLOSGuidance +import numpy as np +import typing +from scipy.interpolate import splev + + +class LOSGuidanceROS(Node): + def __init__(self): + super().__init__('los_guidance_server', namespace="cybership") + # Parameters + self.declare_parameter('desired_speed', 0.5) + self.declare_parameter('lookahead', 0.4) + # Heading control gain + self.declare_parameter('heading_gain', 1.0) + # Publishers and Subscribers + self._cmd_pub = self.create_publisher( + Twist, 'control/velocity/command', 10) + # Publish markers with transient local durability so RViZ receives past markers + self._marker_pub = self.create_publisher( + Marker, 'visualization_marker', 10) + self._odom_sub = self.create_subscription( + Odometry, 'measurement/odom', self.odom_callback, 10) + # Vehicle pose + self._position = None + self._yaw = None + # Action Server + self._action_server = ActionServer( + self, + LOSGuidance, + 'los_guidance', + execute_callback=self.execute_callback, + goal_callback=self.goal_callback, + cancel_callback=self.cancel_callback, + callback_group=ReentrantCallbackGroup()) + + self.goal_uuids: typing.List = [] + + self.get_logger().info('LOS Guidance Action Server started') + + def odom_callback(self, msg: Odometry): + # Update position and heading + self._position = (msg.pose.pose.position.x, msg.pose.pose.position.y) + q = msg.pose.pose.orientation + # Convert quaternion to yaw using tf_transformations + quat = [q.x, q.y, q.z, q.w] + _, _, yaw = euler_from_quaternion(quat) + self._yaw = yaw + + def goal_callback(self, goal_request: LOSGuidance.Goal) -> GoalResponse: + if len(goal_request.path.poses) < 2: + self.get_logger().warn("Path has fewer than 2 poses — rejecting goal.") + return GoalResponse.REJECT + + return GoalResponse.ACCEPT + + def cancel_callback(self, goal_handle) -> CancelResponse: + + return CancelResponse.ACCEPT + + def prepare_waypoints(self, path_msg: Path, extend: bool = True, n_intermediate: int = 2) -> np.ndarray: + waypoints = [(pose.pose.position.x, pose.pose.position.y) + for pose in path_msg.poses] + + if extend: + # Add intermediate points + extended_waypoints = [] + for i in range(len(waypoints) - 1): + p1 = waypoints[i] + p2 = waypoints[i + 1] + extended_waypoints.append(p1) + # Add intermediate points + for j in range(1, n_intermediate + 1): + mid_point = ( + p1[0] + (p2[0] - p1[0]) * j / (n_intermediate + 1), + p1[1] + (p2[1] - p1[1]) * j / (n_intermediate + 1) + ) + extended_waypoints.append(mid_point) + extended_waypoints.append(waypoints[-1]) + waypoints = extended_waypoints + + return np.array(waypoints) + + async def execute_callback(self, goal_handle: LOSGuidance.Goal) -> LOSGuidance.Result: + + if len(self.goal_uuids) > 0: + self.goal_uuids.pop(0) + + self.goal_uuids.append(tuple(goal_handle.goal_id.uuid)) + + path_msg: Path = goal_handle.request.path + waypoints = [] + waypoints.extend([(pose.pose.position.x, pose.pose.position.y) + for pose in path_msg.poses]) + if self._position is not None: + waypoints.append(self._position) + + waypoints = self.prepare_waypoints( + path_msg, extend=True, n_intermediate=2) + + # Initialize guidance + hz = 2 + rate = self.create_rate(hz) + V_d = float(self.get_parameter('desired_speed').value) + delta = float(self.get_parameter('lookahead').value) + # Heading control gain + k_h = float(self.get_parameter('heading_gain').value) + guidance = BaseLOSGuidance(waypoints, V_d=V_d, delta=delta) + last_wp = waypoints[-1] + # Visualize spline path + self.publish_spline_marker(guidance) + + dt = 1.0 / hz + + canceled = False + while rclpy.ok(): + self.get_logger().info(f"{self.goal_uuids}") + if tuple(goal_handle.goal_id.uuid) not in self.goal_uuids: + self.get_logger().info( + f"LOS guidance goal no longer current. {goal_handle.goal_id.uuid}") + canceled = True + break + + if goal_handle.is_cancel_requested: + goal_handle.canceled() + self.get_logger().info( + f"LOS guidance canceled. {goal_handle.goal_id.uuid}") + break + if not goal_handle.is_active: + self.get_logger().info( + f"LOS guidance no longer active. {goal_handle.goal_id.uuid}") + break + if self._position is None or self._yaw is None: + rate.sleep() + continue + x, y = self._position + chi_d, vel_cmd, extras = guidance.guidance(x, y, dt=dt) + twist = Twist() + # Compute forward velocity in body frame using current heading + v_forward = vel_cmd[0] * \ + math.cos(self._yaw) + vel_cmd[1] * math.sin(self._yaw) + # Use computed forward speed (ensuring non-negative speed) + twist.linear.x = max(v_forward, 0.0) + twist.linear.y = 0.0 + twist.linear.z = 0.0 + # Compute and normalize heading error + heading_error = math.atan2( + math.sin(chi_d - self._yaw), math.cos(chi_d - self._yaw)) + twist.angular.z = k_h * heading_error + self._cmd_pub.publish(twist) + # Feedback + feedback = LOSGuidance.Feedback() + feedback.heading = chi_d + feedback.vel_cmd = Vector3(x=vel_cmd[0], y=vel_cmd[1], z=0.0) + goal_handle.publish_feedback(feedback) + self.publish_path_marker(path_msg) + self.publish_arrow_marker(x, y, vel_cmd) + # Look-ahead visualization + # x_la, y_la, _, _ = guidance.lookahead_point(x, y) + self.publish_lookahead_marker( + extras["lookahead_point"][0], extras["lookahead_point"][1]) + # self.publish_lookahead_line(x, y, x_la, y_la) + # Check completion + if math.hypot(x - last_wp[0], y - last_wp[1]) < delta: + break + rate.sleep() + + if not canceled: + goal_handle.succeed() + self.get_logger().info("LOS guidance finished (result empty).") + return LOSGuidance.Result() + + def publish_path_marker(self, path_msg: Path): + marker = Marker() + marker.header = path_msg.header + marker.ns = 'los_path' + marker.id = 0 + marker.type = Marker.LINE_STRIP + marker.action = Marker.ADD + marker.scale.x = 0.05 + marker.color.r = 1.0 + marker.color.g = 0.0 + marker.color.b = 0.0 + marker.color.a = 1.0 + marker.points = [Point(x=pose.pose.position.x, + y=pose.pose.position.y, + z=0.0) + for pose in path_msg.poses] + self._marker_pub.publish(marker) + + def publish_arrow_marker(self, x, y, vel_cmd): + marker = Marker() + marker.header.frame_id = 'world' + marker.header.stamp = self.get_clock().now().to_msg() + marker.ns = 'los_arrow' + marker.id = 1 + marker.type = Marker.ARROW + marker.action = Marker.ADD + marker.scale.x = 0.1 + marker.scale.y = 0.1 + marker.scale.z = 0.1 + # Use a copy of vel_cmd for scaling to avoid in-place modification + vel_arrow = vel_cmd * 3 + marker.color.r = 0.0 + marker.color.g = 1.0 + marker.color.b = 0.5 + marker.color.a = 1.0 + start = Point(x=x, y=y, z=0.0) + end = Point(x=x + vel_arrow[0], y=y + vel_arrow[1], z=0.0) + marker.points = [start, end] + self._marker_pub.publish(marker) + + def publish_spline_marker(self, guidance): + marker = Marker() + marker.header.frame_id = 'world' + marker.header.stamp = self.get_clock().now().to_msg() + marker.ns = 'los_spline' + marker.id = 2 + marker.type = Marker.LINE_STRIP + marker.action = Marker.ADD + marker.scale.x = 0.05 + marker.color.r = 0.0 + marker.color.g = 0.0 + marker.color.b = 1.0 + marker.color.a = 1.0 + # Sample spline path points + ts = np.linspace(0, 1, 1000) + spline_pts = np.array(splev(ts, guidance.tck)).T + marker.points = [Point(x=pt[0], y=pt[1], z=0.0) for pt in spline_pts] + self._marker_pub.publish(marker) + + def publish_lookahead_marker(self, x_la, y_la): + m = Marker() + m.header.frame_id = 'world' + m.header.stamp = self.get_clock().now().to_msg() + m.ns = 'los_lookahead' + m.id = 3 + m.type = Marker.SPHERE + m.action = Marker.ADD + m.scale.x = 0.25 # diameter in meters + m.scale.y = 0.25 + m.scale.z = 0.25 + m.color.r = 1.0 + m.color.g = 0.85 + m.color.b = 0.0 + m.color.a = 1.0 # golden/yellow + m.pose.position.x = float(x_la) + m.pose.position.y = float(y_la) + m.pose.position.z = 0.0 + self._marker_pub.publish(m) + + def publish_lookahead_line(self, x, y, x_la, y_la): + m = Marker() + m.header.frame_id = 'world' + m.header.stamp = self.get_clock().now().to_msg() + m.ns = 'los_lookahead' + m.id = 4 + m.type = Marker.LINE_STRIP + m.action = Marker.ADD + m.scale.x = 0.03 + m.color.r = 1.0 + m.color.g = 0.85 + m.color.b = 0.0 + m.color.a = 0.9 + start = Point(x=float(x), y=float(y), z=0.0) + end = Point(x=float(x_la), y=float(y_la), z=0.0) + m.points = [start, end] + self._marker_pub.publish(m) + + +def main(args=None): + rclpy.init(args=args) + node = LOSGuidanceROS() + executor = MultiThreadedExecutor() + executor.add_node(node) + executor.spin() + executor.shutdown() + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/cybership_simulator/launch/drillship.launch.py b/cybership_simulator/launch/drillship.launch.py index 3d73101..8aeb77a 100644 --- a/cybership_simulator/launch/drillship.launch.py +++ b/cybership_simulator/launch/drillship.launch.py @@ -45,7 +45,8 @@ def generate_launch_description(): "config", "simulation.yaml", ] - ) + ), + {"tf_prefix": launch.substitutions.LaunchConfiguration("vessel_name")}, ], name=f"sim_{anon()}", ) diff --git a/cybership_simulator/launch/enterprise.launch.py b/cybership_simulator/launch/enterprise.launch.py index 5d38ca4..8ebdd00 100644 --- a/cybership_simulator/launch/enterprise.launch.py +++ b/cybership_simulator/launch/enterprise.launch.py @@ -45,7 +45,8 @@ def generate_launch_description(): "config", "simulation.yaml", ] - ) + ), + {"tf_prefix": launch.substitutions.LaunchConfiguration("vessel_name")}, ], name=f"sim_{anon()}", ) diff --git a/cybership_simulator/launch/voyager.launch.py b/cybership_simulator/launch/voyager.launch.py index 1c5d714..84acbb3 100644 --- a/cybership_simulator/launch/voyager.launch.py +++ b/cybership_simulator/launch/voyager.launch.py @@ -45,7 +45,8 @@ def generate_launch_description(): "config", "simulation.yaml", ] - ) + ), + {"tf_prefix": launch.substitutions.LaunchConfiguration("vessel_name")}, ], name=f"sim_{anon()}", ) diff --git a/cybership_viz/config/voyager.rviz b/cybership_viz/config/voyager.rviz index 070057d..b272b25 100644 --- a/cybership_viz/config/voyager.rviz +++ b/cybership_viz/config/voyager.rviz @@ -10,12 +10,9 @@ Panels: - /TF1/Tree1 - /Odometry1 - /Odometry1/Shape1 - - /RobotModel1 - - /RobotModel1/Mass Properties1 - - /RobotModel1/Description Topic1 - /Marker1 - /MarkerArray1 - - /RobotModel2 + - /RobotModel1 Splitter Ratio: 0.5 Tree Height: 1764 - Class: rviz_common/Selection @@ -107,24 +104,6 @@ Visualization Manager: Show Names: false Tree: world: - drillship/base_link_ned: - drillship/base_link: - drillship/aft_center_thruster_link: - {} - drillship/aft_port_thruster_link: - {} - drillship/aft_starboard_thruster_link: - {} - drillship/bow_center_thruster_link: - {} - drillship/bow_port_thruster_link: - {} - drillship/bow_starboard_thruster_link: - {} - drillship/imu_link: - {} - drillship/mocap_link: - {} voyager/base_link_ned: voyager/base_link: voyager/bow_tunnel_thruster_link: @@ -213,73 +192,6 @@ Visualization Manager: Reliability Policy: Reliable Value: measurement/odom Value: true - - Alpha: 0.4000000059604645 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /drillship/robot_description - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - aft_center_thruster_link: - Alpha: 1 - Show Axes: false - Show Trail: false - aft_port_thruster_link: - Alpha: 1 - Show Axes: false - Show Trail: false - aft_starboard_thruster_link: - Alpha: 1 - Show Axes: false - Show Trail: false - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - base_link_ned: - Alpha: 1 - Show Axes: false - Show Trail: false - bow_center_thruster_link: - Alpha: 1 - Show Axes: false - Show Trail: false - bow_port_thruster_link: - Alpha: 1 - Show Axes: false - Show Trail: false - bow_starboard_thruster_link: - Alpha: 1 - Show Axes: false - Show Trail: false - imu_link: - Alpha: 1 - Show Axes: false - Show Trail: false - mocap_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Mass Properties: - Inertia: false - Mass: false - Name: RobotModel - TF Prefix: drillship - Update Interval: 0 - Value: true - Visual Enabled: true - Class: rviz_default_plugins/Marker Enabled: true Name: Marker @@ -406,7 +318,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 2.566887617111206 + Distance: 10 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -421,10 +333,10 @@ Visualization Manager: Invert Z Axis: true Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.4803982079029083 + Pitch: 0.7853981852531433 Target Frame: base_link Value: Orbit (rviz) - Yaw: 5.228579521179199 + Yaw: 0.7853981852531433 Saved: ~ Window Geometry: Displays: @@ -432,7 +344,7 @@ Window Geometry: Height: 2055 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000018e0000076dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000033100000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000076d000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000ef40000003efc0100000002fb0000000800540069006d0065010000000000000ef40000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000d600000076d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000018e0000076dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000033100000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000076d000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001000000076dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000076d000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000ef40000003efc0100000002fb0000000800540069006d0065010000000000000ef40000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000d600000076d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -442,5 +354,5 @@ Window Geometry: Views: collapsed: true Width: 3828 - X: 0 - Y: 0 + X: -32 + Y: -32 From bb51bc85b07cd43db7aa7ce6c9ee87805775d4a4 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Fri, 29 Aug 2025 16:54:31 +0200 Subject: [PATCH 35/46] feat: add mux web ui --- .../src/cybership_controller/guidance/ros_action_client.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cybership_controller/src/cybership_controller/guidance/ros_action_client.py b/cybership_controller/src/cybership_controller/guidance/ros_action_client.py index fd4364b..42bf305 100755 --- a/cybership_controller/src/cybership_controller/guidance/ros_action_client.py +++ b/cybership_controller/src/cybership_controller/guidance/ros_action_client.py @@ -14,8 +14,8 @@ from cybership_interfaces.action import LOSGuidance class LOSGuidanceClient(Node): - def __init__(self): - super().__init__('los_guidance_client') + def __init__(self, **kwargs): + super().__init__('los_guidance_client', **kwargs) self._action_client = ActionClient(self, LOSGuidance, 'los_guidance') def send_goal(self, waypoints): From 8fa02e7e5a28cee1afff41250ac043453d770ec2 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Fri, 29 Aug 2025 16:54:40 +0200 Subject: [PATCH 36/46] feat: add web ui for mux --- cybership_utilities/CMakeLists.txt | 7 + cybership_utilities/html/index.html | 564 ++++++++++++++---------- cybership_utilities/launch/ui.launch.py | 49 ++ 3 files changed, 385 insertions(+), 235 deletions(-) create mode 100644 cybership_utilities/launch/ui.launch.py diff --git a/cybership_utilities/CMakeLists.txt b/cybership_utilities/CMakeLists.txt index 0026363..01693b1 100644 --- a/cybership_utilities/CMakeLists.txt +++ b/cybership_utilities/CMakeLists.txt @@ -12,6 +12,13 @@ find_package(ament_cmake_python REQUIRED) ament_python_install_package(${PROJECT_NAME}) # Install Python executables +install( + DIRECTORY + launch html + DESTINATION + share/${PROJECT_NAME} +) + install(PROGRAMS nodes/force_mux_gui.py DESTINATION lib/${PROJECT_NAME} diff --git a/cybership_utilities/html/index.html b/cybership_utilities/html/index.html index 85c1f39..3384a0e 100644 --- a/cybership_utilities/html/index.html +++ b/cybership_utilities/html/index.html @@ -1,243 +1,337 @@ - - - - Mux Controller (Foxglove roslibjs) - - - - - - -

Mux Controller foxglove_bridge + @foxglove/roslibjs

- -
- Connection -
- - - - Disconnected -
-
- -
- Thruster -
- - - Idle -
-
- -
- Force Mux -
- - - - -
-
Idle
-
- -
- Velocity Mux -
- - - - -
-
Idle
-
- - - - + return { ok: false }; + } + + el("thrActivate").onclick = async () => { + el('thrResult').textContent = 'Activating…'; + // Primary per request, with fallbacks similar to Python GUI + const res = await callFirstEmpty(['/thrusters/activate', '/thruster/activate', '/thruster/enable']); + el('thrResult').textContent = res.ok ? `Activated via ${res.name}` : `Service not found: ${withNs('/thrusters/activate')}`; + }; + + el("thrDeactivate").onclick = async () => { + el('thrResult').textContent = 'Deactivating…'; + const res = await callFirstEmpty(['/thrusters/deactivate', '/thruster/disable']); + el('thrResult').textContent = res.ok ? `Deactivated via ${res.name}` : `Service not found: ${withNs('/thrusters/deactivate')}`; + }; + + + \ No newline at end of file diff --git a/cybership_utilities/launch/ui.launch.py b/cybership_utilities/launch/ui.launch.py new file mode 100644 index 0000000..69fc79c --- /dev/null +++ b/cybership_utilities/launch/ui.launch.py @@ -0,0 +1,49 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, LogInfo +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, TextSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description() -> LaunchDescription: + http_port = LaunchConfiguration("http_port") + ws_port = LaunchConfiguration("ws_port") + + html_dir = PathJoinSubstitution([FindPackageShare("cybership_utilities"), "html"]) + + # Simple static file server for the Web UI (serves index2.html, etc.) + http_server = ExecuteProcess( + cmd=["python3", "-m", "http.server", http_port], + cwd=html_dir, + output="screen", + ) + + # Foxglove WebSocket bridge (ROS 2) for service/topic access from the Web UI + foxglove = Node( + package="foxglove_bridge", + executable="foxglove_bridge", + name="foxglove_bridge", + output="screen", + arguments=["--port", ws_port], + ) + + return LaunchDescription( + [ + DeclareLaunchArgument( + "http_port", default_value="8000", description="Port for the static HTTP server" + ), + DeclareLaunchArgument( + "ws_port", default_value="8765", description="WebSocket port for foxglove_bridge" + ), + LogInfo( + msg=[ + TextSubstitution(text="Serving UI at http://localhost:"), + http_port, + TextSubstitution(text="/index.html | Foxglove WS ws://localhost:"), + ws_port, + ] + ), + http_server, + foxglove, + ] + ) From aed740873678af1ee9014b05af9b2a13f8af0444 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Wed, 3 Sep 2025 10:18:20 +0200 Subject: [PATCH 37/46] misc: increase distance between vessels --- cybership_simulator/config/multi_vessel_simulation.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cybership_simulator/config/multi_vessel_simulation.yaml b/cybership_simulator/config/multi_vessel_simulation.yaml index bbaf424..72514a4 100644 --- a/cybership_simulator/config/multi_vessel_simulation.yaml +++ b/cybership_simulator/config/multi_vessel_simulation.yaml @@ -3,9 +3,9 @@ ros__parameters: initial_conditions: position: - x: 0.0 + x: 25.0 y: 0.0 - yaw: 0.0 + yaw: 3.14159 velocity: surge: 0.0 sway: 0.0 @@ -18,7 +18,7 @@ ros__parameters: initial_conditions: position: - x: 4.0 + x: 10.0 y: 0.0 yaw: 0.0 velocity: From 2d39f157119cd4499673170deaf14cc6fe71a824 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Thu, 4 Sep 2025 18:00:22 +0200 Subject: [PATCH 38/46] perf: positoin and ui --- cybership_config/config/drillship/controller_position.yaml | 6 +++--- cybership_utilities/launch/ui.launch.py | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/cybership_config/config/drillship/controller_position.yaml b/cybership_config/config/drillship/controller_position.yaml index 26edb7f..ffdc751 100644 --- a/cybership_config/config/drillship/controller_position.yaml +++ b/cybership_config/config/drillship/controller_position.yaml @@ -10,9 +10,9 @@ control: # Position control gains p_gain: - pos: 0.6 # Proportional gain for position + pos: 1.2 # Proportional gain for position vel: 0.2 # Proportional gain for velocity - yaw: 0.7 # Proportional gain for yaw + yaw: 1.0 # Proportional gain for yaw # Integral gains i_gain: @@ -24,7 +24,7 @@ d_gain: pos: 0.5 # Derivative gain for position vel: 0.5 # Derivative gain for velocity - yaw: 1.5 # Derivative gain for yaw + yaw: 2.5 # Derivative gain for yaw # Integral error limits max_integral_error: diff --git a/cybership_utilities/launch/ui.launch.py b/cybership_utilities/launch/ui.launch.py index 69fc79c..a69e04c 100644 --- a/cybership_utilities/launch/ui.launch.py +++ b/cybership_utilities/launch/ui.launch.py @@ -30,7 +30,7 @@ def generate_launch_description() -> LaunchDescription: return LaunchDescription( [ DeclareLaunchArgument( - "http_port", default_value="8000", description="Port for the static HTTP server" + "http_port", default_value="8001", description="Port for the static HTTP server" ), DeclareLaunchArgument( "ws_port", default_value="8765", description="WebSocket port for foxglove_bridge" From 3566062010365b6f0d677d36f1871cbba4b76fe7 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Thu, 4 Sep 2025 18:02:16 +0200 Subject: [PATCH 39/46] feat: add multiple joy support --- cybership_teleop/launch/allocation.launch.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/cybership_teleop/launch/allocation.launch.py b/cybership_teleop/launch/allocation.launch.py index 238d156..db9edc9 100644 --- a/cybership_teleop/launch/allocation.launch.py +++ b/cybership_teleop/launch/allocation.launch.py @@ -29,11 +29,20 @@ def generate_launch_description(): description="Joystick configuration file", ) + arg_joy_id = launch.actions.DeclareLaunchArgument( + "joy_id", + default_value="0", + description="Joystick id", + ) + node_joy = Node( namespace=launch.substitutions.LaunchConfiguration("vessel_name"), package="joy", executable="game_controller_node", name=f"vessel_joy_node_{anon()}", + parameters=[ + {"device_id": launch.substitutions.LaunchConfiguration("joy_id")} + ], output="screen", ) @@ -46,6 +55,7 @@ def generate_launch_description(): parameters=[launch.substitutions.LaunchConfiguration("joy_config")], ) + ld.add_action(arg_joy_id) ld.add_action(arg_joy_config) ld.add_action(node_joy) ld.add_action(node_joy_teleop) From 8cc538ca49a7d6f3475a1ad0f355db434fe81610 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Thu, 4 Sep 2025 18:03:43 +0200 Subject: [PATCH 40/46] perf: improve performance --- cybership_bringup/launch/localization.launch.py | 4 ++-- .../config/voyager/controller_position.yaml | 4 ++-- .../config/voyager/controller_velocity.yaml | 10 +++++----- .../cybership_controller/guidance/ros_action_server.py | 4 ++-- 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/cybership_bringup/launch/localization.launch.py b/cybership_bringup/launch/localization.launch.py index 3510d4a..4c5e628 100644 --- a/cybership_bringup/launch/localization.launch.py +++ b/cybership_bringup/launch/localization.launch.py @@ -10,9 +10,9 @@ def generate_launch_description(): - vessel_name = 'drillship' + vessel_name = 'voyager' - vessel_model = 'drillship' + vessel_model = 'voyager' ld = launch.LaunchDescription() diff --git a/cybership_config/config/voyager/controller_position.yaml b/cybership_config/config/voyager/controller_position.yaml index 2d6b245..17bff47 100644 --- a/cybership_config/config/voyager/controller_position.yaml +++ b/cybership_config/config/voyager/controller_position.yaml @@ -10,13 +10,13 @@ control: # Position control gains p_gain: - pos: 2.0 # Proportional gain for position + pos: 1.0 # Proportional gain for position vel: 0.7 # Proportional gain for velocity yaw: 0.9 # Proportional gain for yaw # Integral gains i_gain: - pos: 0.2 # Integral gain for position + pos: 0.1 # Integral gain for position vel: 0.1 # Integral gain for velocity yaw: 0.1 # Integral gain for yaw diff --git a/cybership_config/config/voyager/controller_velocity.yaml b/cybership_config/config/voyager/controller_velocity.yaml index b888004..1a701de 100644 --- a/cybership_config/config/voyager/controller_velocity.yaml +++ b/cybership_config/config/voyager/controller_velocity.yaml @@ -18,9 +18,9 @@ # Proportional gains for each degree of freedom (DOF) # Higher values provide faster response but may cause oscillations p_gain: - surge: 5.0 # Forward/backward motion (u) - sway: 5.0 # Left/right motion (v) - yaw: 5.0 # Rotational motion (r) + surge: 0.5 # Forward/backward motion (u) + sway: 0.3 # Left/right motion (v) + yaw: 0.3 # Rotational motion (r) # Integral gains for each DOF # Helps eliminate steady-state errors @@ -37,7 +37,7 @@ yaw: 0.3 # Control limits and filtering - i_max: 20.0 # Maximum integral contribution (anti-windup) + i_max: 0.0 # Maximum integral contribution (anti-windup) smooth_limit: true # Enable smooth limiting for control outputs filter_alpha: 0.1 # Smoothing factor for desired velocity (0-1, lower = more smoothing) @@ -48,4 +48,4 @@ interval: 5.0 # Time between metrics calculations (seconds) # Timing parameters - dt: 0.5 # Control loop time step (seconds) - affects timer frequency + dt: 0.2 # Control loop time step (seconds) - affects timer frequency diff --git a/cybership_controller/src/cybership_controller/guidance/ros_action_server.py b/cybership_controller/src/cybership_controller/guidance/ros_action_server.py index ac676f4..4fdfdcd 100755 --- a/cybership_controller/src/cybership_controller/guidance/ros_action_server.py +++ b/cybership_controller/src/cybership_controller/guidance/ros_action_server.py @@ -29,10 +29,10 @@ class LOSGuidanceROS(Node): def __init__(self): super().__init__('los_guidance_server', namespace="cybership") # Parameters - self.declare_parameter('desired_speed', 0.5) + self.declare_parameter('desired_speed', 0.3) self.declare_parameter('lookahead', 0.4) # Heading control gain - self.declare_parameter('heading_gain', 1.0) + self.declare_parameter('heading_gain', 1.1) # Publishers and Subscribers self._cmd_pub = self.create_publisher( Twist, 'control/velocity/command', 10) From 682c83116ac103ef7a127541cf707a34d94fb247 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Fri, 5 Sep 2025 17:29:20 +0200 Subject: [PATCH 41/46] feat: add service call for mux on teleop --- .../guidance/ros_action_server.py | 2 +- cybership_teleop/config/allocation/ps5.yaml | 19 ++++++++++++++++--- 2 files changed, 17 insertions(+), 4 deletions(-) diff --git a/cybership_controller/src/cybership_controller/guidance/ros_action_server.py b/cybership_controller/src/cybership_controller/guidance/ros_action_server.py index ac676f4..68ad130 100755 --- a/cybership_controller/src/cybership_controller/guidance/ros_action_server.py +++ b/cybership_controller/src/cybership_controller/guidance/ros_action_server.py @@ -30,7 +30,7 @@ def __init__(self): super().__init__('los_guidance_server', namespace="cybership") # Parameters self.declare_parameter('desired_speed', 0.5) - self.declare_parameter('lookahead', 0.4) + self.declare_parameter('lookahead', 1.0) # Heading control gain self.declare_parameter('heading_gain', 1.0) # Publishers and Subscribers diff --git a/cybership_teleop/config/allocation/ps5.yaml b/cybership_teleop/config/allocation/ps5.yaml index 29ba61c..4f8035f 100644 --- a/cybership_teleop/config/allocation/ps5.yaml +++ b/cybership_teleop/config/allocation/ps5.yaml @@ -8,11 +8,11 @@ axis_mappings: force-x: axis: 3 - scale: 2.0 + scale: 3.0 offset: 0.0 force-y: axis: 2 - scale: -2.0 + scale: -3.0 offset: 0.0 torque-z: axis: 0 @@ -25,9 +25,22 @@ service_name: thruster/enable buttons: [0] - disable: type: service interface_type: std_srvs/srv/Empty service_name: thruster/disable buttons: [1] + + joy_force_mux: + type: service + interface_type: topic_tools_interfaces/srv/MuxSelect + service_name: force_mux/select + buttons: [2] + request: {"topic": "control/force/command"} + + position_force_mux: + type: service + interface_type: topic_tools_interfaces/srv/MuxSelect + service_name: force_mux/select + buttons: [3] + request: {"topic": "control/force/command/position"} From 1fb8d15b26eaab9132c91738f982d80cd9e54699 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Fri, 5 Sep 2025 17:32:14 +0200 Subject: [PATCH 42/46] perf: better vessel positions --- .../config/multi_vessel_simulation.yaml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/cybership_simulator/config/multi_vessel_simulation.yaml b/cybership_simulator/config/multi_vessel_simulation.yaml index 72514a4..9dc2a0c 100644 --- a/cybership_simulator/config/multi_vessel_simulation.yaml +++ b/cybership_simulator/config/multi_vessel_simulation.yaml @@ -3,8 +3,8 @@ ros__parameters: initial_conditions: position: - x: 25.0 - y: 0.0 + x: 5.0 + y: 1.0 yaw: 3.14159 velocity: surge: 0.0 @@ -18,8 +18,8 @@ ros__parameters: initial_conditions: position: - x: 10.0 - y: 0.0 + x: -2.5 + y: 0.5 yaw: 0.0 velocity: surge: 0.0 @@ -27,4 +27,4 @@ yaw_rate: 0.0 tf_prefix: drillship body_frame_id: base_link_ned - world_frame_id: world \ No newline at end of file + world_frame_id: world From 79305eccf2e7b8fc842ed34c986f63f998c82b12 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Fri, 19 Sep 2025 15:33:16 +0200 Subject: [PATCH 43/46] feat: add all launch --- cybership_bringup/launch/all.launch.py | 106 ++++++++++++++++++ cybership_bringup/launch/drillship.launch.py | 14 --- cybership_bringup/launch/enterprise.launch.py | 7 -- .../launch/localization.launch.py | 16 ++- cybership_bringup/launch/voyager.launch.py | 6 - .../config/enterprise/robot_localization.yaml | 48 ++++---- .../launch/thrust_allocation.launch.py | 1 + .../launch/velocity_controller.launch.py | 2 + cybership_teleop/launch/joy.launch.py | 45 ++++++++ .../scripts/install_discovery_service.sh | 71 ++++++++++++ .../service/fastdds_discovery.service | 14 +++ 11 files changed, 273 insertions(+), 57 deletions(-) create mode 100644 cybership_bringup/launch/all.launch.py create mode 100644 cybership_teleop/launch/joy.launch.py create mode 100755 cybership_utilities/scripts/install_discovery_service.sh create mode 100644 cybership_utilities/service/fastdds_discovery.service diff --git a/cybership_bringup/launch/all.launch.py b/cybership_bringup/launch/all.launch.py new file mode 100644 index 0000000..f280fec --- /dev/null +++ b/cybership_bringup/launch/all.launch.py @@ -0,0 +1,106 @@ +import launch +import launch.actions +import launch.conditions +import launch.launch_description_sources +import launch.substitutions +import launch_ros.substitutions + + +def generate_launch_description(): + + ld = launch.LaunchDescription() + + vessel_model = launch.substitutions.LaunchConfiguration("vessel_model") + vessel_name = vessel_model + + ld.add_action( + launch.actions.DeclareLaunchArgument( + "vessel_model", + description="Vessel model to bring up (drillship|enterprise|voyager)", + ) + ) + + include_drillship = launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + launch.substitutions.PathJoinSubstitution( + [ + launch_ros.substitutions.FindPackageShare("cybership_bringup"), + "launch", + "drillship.launch.py", + ] + ) + ), + condition=launch.conditions.IfCondition( + launch.substitutions.PythonExpression(["'", vessel_model, "' == 'drillship'"]) + ), + ) + + include_enterprise = launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + launch.substitutions.PathJoinSubstitution( + [ + launch_ros.substitutions.FindPackageShare("cybership_bringup"), + "launch", + "enterprise.launch.py", + ] + ) + ), + condition=launch.conditions.IfCondition( + launch.substitutions.PythonExpression(["'", vessel_model, "' == 'enterprise'"]) + ), + ) + + include_voyager = launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + launch.substitutions.PathJoinSubstitution( + [ + launch_ros.substitutions.FindPackageShare("cybership_bringup"), + "launch", + "voyager.launch.py", + ] + ) + ), + condition=launch.conditions.IfCondition( + launch.substitutions.PythonExpression(["'", vessel_model, "' == 'voyager'"]) + ), + ) + + include_localization = launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + launch.substitutions.PathJoinSubstitution( + [ + launch_ros.substitutions.FindPackageShare("cybership_bringup"), + "launch", + "localization.launch.py", + ] + ) + ), + launch_arguments={ + "vessel_model": vessel_model, + "vessel_name": vessel_name, + }.items(), + ) + + include_dp = launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + launch.substitutions.PathJoinSubstitution( + [ + launch_ros.substitutions.FindPackageShare("cybership_dp"), + "launch", + "dp.launch.py", + ] + ) + ), + launch_arguments={ + "vessel_model": vessel_model, + "vessel_name": vessel_name, + }.items(), + ) + + ld.add_action(include_drillship) + ld.add_action(include_enterprise) + ld.add_action(include_voyager) + ld.add_action(include_localization) + ld.add_action(include_dp) + + return ld diff --git a/cybership_bringup/launch/drillship.launch.py b/cybership_bringup/launch/drillship.launch.py index 52ea28d..9d5aca5 100644 --- a/cybership_bringup/launch/drillship.launch.py +++ b/cybership_bringup/launch/drillship.launch.py @@ -40,13 +40,6 @@ def generate_launch_description(): ) ) - # ld.add_action( - # include_launch_action_with_config( - # vessel_model, vessel_name, - # 'robot_localization.launch.py', 'robot_localization.yaml' - # ) - # ) - ld.add_action( include_launch_action_with_config( vessel_model, vessel_name, @@ -68,11 +61,4 @@ def generate_launch_description(): ) ) - # ld.add_action( - # include_launch_action_with_config( - # vessel_model, vessel_name, - # 'urdf_description.launch.py', 'empty.yaml' - # ) - # ) - return ld diff --git a/cybership_bringup/launch/enterprise.launch.py b/cybership_bringup/launch/enterprise.launch.py index 1008618..aad8987 100644 --- a/cybership_bringup/launch/enterprise.launch.py +++ b/cybership_bringup/launch/enterprise.launch.py @@ -31,13 +31,6 @@ def generate_launch_description(): ) ) - # ld.add_action( - # include_launch_action_with_config( - # vessel_model, vessel_name, - # 'robot_localization.launch.py', 'robot_localization.yaml' - # ) - # ) - ld.add_action( include_launch_action_with_config( vessel_model, vessel_name, diff --git a/cybership_bringup/launch/localization.launch.py b/cybership_bringup/launch/localization.launch.py index 4c5e628..6b92208 100644 --- a/cybership_bringup/launch/localization.launch.py +++ b/cybership_bringup/launch/localization.launch.py @@ -3,29 +3,33 @@ import launch.actions import launch.substitutions import launch.launch_description_sources +import launch.launch_description from cybership_utilities.launch import include_launch_action_with_config - +from cybership_utilities.launch import anon +from cybership_utilities.launch import COMMON_ARGUMENTS as ARGUMENTS def generate_launch_description(): - vessel_name = 'voyager' - - vessel_model = 'voyager' ld = launch.LaunchDescription() + for argument in ARGUMENTS: + ld.add_action(argument) + ld.add_action( include_launch_action_with_config( - vessel_model, vessel_name, + launch.substitutions.LaunchConfiguration("vessel_model"), + launch.substitutions.LaunchConfiguration("vessel_name"), 'robot_localization.launch.py', 'robot_localization.yaml' ) ) ld.add_action( include_launch_action_with_config( - vessel_model, vessel_name, + launch.substitutions.LaunchConfiguration("vessel_model"), + launch.substitutions.LaunchConfiguration("vessel_name"), 'urdf_description.launch.py' ) ) diff --git a/cybership_bringup/launch/voyager.launch.py b/cybership_bringup/launch/voyager.launch.py index c60534d..5c034dd 100644 --- a/cybership_bringup/launch/voyager.launch.py +++ b/cybership_bringup/launch/voyager.launch.py @@ -43,12 +43,6 @@ def generate_launch_description(): 'force_multiplexer.launch.py', 'force_multiplexer.yaml' ) ) - # ld.add_action( - # include_launch_action_with_config( - # vessel_model, vessel_name, - # 'robot_localization.launch.py', 'robot_localization.yaml' - # ) - # ) ld.add_action( include_launch_action_with_config( diff --git a/cybership_config/config/enterprise/robot_localization.yaml b/cybership_config/config/enterprise/robot_localization.yaml index f8191c4..ba645b6 100644 --- a/cybership_config/config/enterprise/robot_localization.yaml +++ b/cybership_config/config/enterprise/robot_localization.yaml @@ -4,7 +4,7 @@ # The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin # computation until it receives at least one message from one of the inputs. It will then run continuously at the # frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified. - frequency: 30.0 + frequency: 10.0 # The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict # cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the @@ -15,7 +15,7 @@ # set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar # environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected # by, for example, an IMU. Defaults to false if unspecified. - two_d_mode: false + two_d_mode: true # Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for # future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if @@ -33,7 +33,7 @@ # Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by # debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious # effects on the performance of the node. Defaults to false if unspecified. - debug: false + debug: true # Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path. # debug_out_file: /path/to/debug/file.txt @@ -67,9 +67,9 @@ # 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state # estimation node from robot_localization! However, that instance should *not* fuse the global data. map_frame: map # Defaults to "map" if unspecified - odom_frame: odom # Defaults to "odom" if unspecified - base_link_frame: base_link # Defaults to "base_link" if unspecified - world_frame: odom # Defaults to the value of odom_frame if unspecified + odom_frame: world # Defaults to "odom" if unspecified + base_link_frame: enterprise/base_link_ned # Defaults to "base_link" if unspecified + world_frame: world # Defaults to the value of odom_frame if unspecified # The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry, # geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped, @@ -140,15 +140,15 @@ # odom1_pose_rejection_threshold: 2.0 # odom1_twist_rejection_threshold: 0.2 - pose0: /enterprise/measurement/pose - pose0_config: [true, true, true, - true, true, true, + pose0: measurement/pose + pose0_config: [ true, true, true, + true, true, true, false, false, false, false, false, false, false, false, false] - pose0_differential: true + pose0_differential: false pose0_relative: false - pose0_queue_size: 5 + pose0_queue_size: 10 pose0_rejection_threshold: 2.0 # Note the difference in parameter name # twist0: example/twist @@ -160,16 +160,16 @@ # twist0_queue_size: 3 # twist0_rejection_threshold: 2.0 - # imu0: example/imu + # imu0: measurement/imu # imu0_config: [false, false, false, - # true, true, true, + # false, false, false, # false, false, false, # true, true, true, # true, true, true] # imu0_differential: false - # imu0_relative: true - # imu0_queue_size: 5 - # imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names + # imu0_relative: false + # imu0_queue_size: 10 + # # imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names # imu0_twist_rejection_threshold: 0.8 # # imu0_linear_acceleration_rejection_threshold: 0.8 # @@ -193,27 +193,27 @@ # Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to # false. - stamped_control: false + # stamped_control: false # The last issued control command will be used in prediction for this period. Defaults to 0.2. - control_timeout: 0.2 + # control_timeout: 0.2 # Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw. - control_config: [true, false, false, false, false, true] + # control_config: [true, true, false, false, false, true] # Places limits on how large the acceleration term will be. Should match your robot's kinematics. - acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4] + # acceleration_limits: [1.3, 1.3, 0.0, 0.0, 0.0, 3.4] # Acceleration and deceleration limits are not always the same for robots. - deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5] + # deceleration_limits: [1.3, 1.3, 0.0, 0.0, 0.0, 4.5] # If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these # gains - acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9] + # acceleration_gains: [0.8, 0.8, 0.0, 0.0, 0.0, 0.9] # If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these # gains - deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0] + # deceleration_gains: [1.0, 1.0, 0.5, 0.0, 0.0, 1.0] # [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is # exposed as a configuration parameter. This matrix represents the noise we add to the total error after each @@ -246,4 +246,4 @@ # question. Users should take care not to use large values for variables that will not be measured directly. The values # are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the diagonal values below # if unspecified. In this example, we specify only the diagonal of the matrix. - initial_estimate_covariance: [1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9] \ No newline at end of file + # initial_estimate_covariance: [1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9] \ No newline at end of file diff --git a/cybership_dp/launch/thrust_allocation.launch.py b/cybership_dp/launch/thrust_allocation.launch.py index 358fd10..9569535 100644 --- a/cybership_dp/launch/thrust_allocation.launch.py +++ b/cybership_dp/launch/thrust_allocation.launch.py @@ -49,6 +49,7 @@ def generate_launch_description(): arguments=[ "control/force/command/mux", # Output topic "control/force/command", # Input topic to listen to + "control/force/command/manual", # Input topic to listen to "control/force/command/velocity", # Input topic to listen to "control/force/command/position", # Input topic to listen to ], diff --git a/cybership_dp/launch/velocity_controller.launch.py b/cybership_dp/launch/velocity_controller.launch.py index 92c89c4..4b43cf0 100644 --- a/cybership_dp/launch/velocity_controller.launch.py +++ b/cybership_dp/launch/velocity_controller.launch.py @@ -60,6 +60,8 @@ def generate_launch_description(): arguments=[ "control/velocity/command/mux", # Output topic "control/velocity/command", # Input topic to listen to + "control/velocity/command/los", # Input topic to listen to + "control/velocity/command/manual", # Input topic to listen to ], output="screen", respawn=True, diff --git a/cybership_teleop/launch/joy.launch.py b/cybership_teleop/launch/joy.launch.py new file mode 100644 index 0000000..efa865e --- /dev/null +++ b/cybership_teleop/launch/joy.launch.py @@ -0,0 +1,45 @@ +import launch +import launch_ros + +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os +from cybership_utilities.launch import anon +from cybership_utilities.launch import COMMON_ARGUMENTS as ARGUMENTS + + +def generate_launch_description(): + + ld = LaunchDescription() + + for argument in ARGUMENTS: + ld.add_action(argument) + + + + arg_joy_id = launch.actions.DeclareLaunchArgument( + "joy_id", + default_value="0", + description="Joystick id", + ) + + node_joy = Node( + namespace=launch.substitutions.LaunchConfiguration("vessel_name"), + package="joy", + executable="game_controller_node", + name=f"vessel_joy_node_{anon()}", + parameters=[ + {"device_id": launch.substitutions.LaunchConfiguration("joy_id")}, + {"deadzone": 0.1}, + {"autorepeat_rate": 20.0}, + {"coalesce_interval": 0.01}, + ], + output="screen", + ) + + + ld.add_action(arg_joy_id) + ld.add_action(node_joy) + + return ld diff --git a/cybership_utilities/scripts/install_discovery_service.sh b/cybership_utilities/scripts/install_discovery_service.sh new file mode 100755 index 0000000..06cf3e5 --- /dev/null +++ b/cybership_utilities/scripts/install_discovery_service.sh @@ -0,0 +1,71 @@ +#!/usr/bin/env bash + +set -Eeuo pipefail + +SCRIPT_DIR=$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" >/dev/null 2>&1 && pwd) +SERVICE_SRC="$SCRIPT_DIR/../service/fastdds_discovery.service" +SYSTEMD_USER_DIR="${XDG_CONFIG_HOME:-$HOME/.config}/systemd/user" +SERVICE_DEST="$SYSTEMD_USER_DIR/fastdds_discovery.service" +FASTDDS_ENV_FILE="$HOME/.config/fastdds" +ROSPROFILE_FILE="$HOME/.rosrc" + +echo "Installing Fast DDS Discovery Server user service..." + +if [[ ! -f "$SERVICE_SRC" ]]; then + echo "Error: Service file not found at $SERVICE_SRC" >&2 + exit 1 +fi + +# Optional sanity checks and friendly warnings +if ! command -v fastdds >/dev/null 2>&1; then + echo "Warning: 'fastdds' command is not in PATH. The service may fail to start until it's installed." >&2 +fi + +if [[ ! -f "$FASTDDS_ENV_FILE" ]]; then + echo "Creating default Fast DDS env file at $FASTDDS_ENV_FILE" + mkdir -p "$HOME/.config" + cat > "$FASTDDS_ENV_FILE" <<'EOF' +# Fast DDS Discovery Server environment +# Required: Unique discovery server ID (0-255). Choose per host. +export FASTDDS_DISCOVERY_ID="1" + +# Optional: Discovery server port (defaults to 11811 if unset) +export FASTDDS_DISCOVERY_PORT="11811" +EOF +else + echo "Found existing env file at $FASTDDS_ENV_FILE" +fi + +# Ensure ~/.rosrc exists to satisfy ConditionPathExists from the unit +if [[ ! -f "$ROSPROFILE_FILE" ]]; then + echo "Creating $ROSPROFILE_FILE (empty placeholder for service condition)" + : > "$ROSPROFILE_FILE" +fi + +mkdir -p "$SYSTEMD_USER_DIR" +install -m 0644 "$SERVICE_SRC" "$SERVICE_DEST" +echo "Installed unit to $SERVICE_DEST" + +# Enable lingering so the user service can run without a login session +echo "Enabling lingering for user '$USER' (so the service runs at boot)..." +if ! loginctl enable-linger "$USER" >/dev/null 2>&1; then + if command -v sudo >/dev/null 2>&1; then + echo "Retrying with sudo to enable lingering..." + sudo loginctl enable-linger "$USER" + else + echo "Warning: Could not enable lingering automatically. Run this manually:" >&2 + echo " sudo loginctl enable-linger $USER" >&2 + fi +fi + +# Reload, enable and start the user service +echo "Reloading systemd user units..." +systemctl --user daemon-reload + +echo "Enabling and starting 'fastdds_discovery.service'..." +systemctl --user enable --now fastdds_discovery.service + +echo "Service status (short):" +systemctl --user --no-pager --full status fastdds_discovery.service || true + +echo "Done. The Fast DDS Discovery Server should now be active." diff --git a/cybership_utilities/service/fastdds_discovery.service b/cybership_utilities/service/fastdds_discovery.service new file mode 100644 index 0000000..3307f7f --- /dev/null +++ b/cybership_utilities/service/fastdds_discovery.service @@ -0,0 +1,14 @@ +[Unit] +Description=Fast DDS Discovery Server (reads ~/.config/fastdds for configuration) +After=network-online.target +Wants=network-online.target +ConditionPathExists=%h/.config/fastdds + +[Service] +Type=simple +ExecStart=/bin/bash -lc 'source "$HOME/.config/fastdds"; : "${FASTDDS_DISCOVERY_ID:?Set FASTDDS_DISCOVERY_ID in ~/.config/fastdds}"; exec fastdds discovery -i "$FASTDDS_DISCOVERY_ID" -p "${FASTDDS_DISCOVERY_PORT:-11811}"' +Restart=always +RestartSec=5 + +[Install] +WantedBy=default.target \ No newline at end of file From 35fbb663c0f28788bfd78cb4c93dbfbdd8ad2c43 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Fri, 19 Sep 2025 15:33:25 +0200 Subject: [PATCH 44/46] feat: add safety controller --- cybership_thrusters/README.md | 22 ++++++++++++++++ .../cybership_thrusters.hpp | 21 ++++++++++++++++ cybership_thrusters/src/azimuth.cpp | 24 ++++++++++++++++++ cybership_thrusters/src/fixed.cpp | 23 +++++++++++++++++ cybership_thrusters/src/voith_schneider.cpp | 25 +++++++++++++++++++ 5 files changed, 115 insertions(+) diff --git a/cybership_thrusters/README.md b/cybership_thrusters/README.md index 1159cfa..0723069 100644 --- a/cybership_thrusters/README.md +++ b/cybership_thrusters/README.md @@ -78,6 +78,28 @@ The ROS2 parameters for configuring each thruster are defined under the `thruste - **`signal_inverted`**: A boolean flag indicating whether the output signal should be inverted. +## Safety Watchdog + +- All thruster controllers implement a safety watchdog. If no command message is received on the configured `force_topic` for more than `2.0` seconds, the thruster automatically publishes zero commands on its output topics (e.g., `signal`, `rpm`, `angle`, `arm_x`, `arm_y`). +- The timeout is configurable per thruster via `thrusters..safety_timeout` (seconds). Default: `2.0`. + +Example YAML: + +```yaml +thrusters: + thruster1: + type: "fixed" + force_topic: "/thruster1/force" + signal_topic: "/thruster1/signal" + force_max: 5.0 + force_min: -5.0 + safety_timeout: 2.0 # seconds +``` + +Behavior notes: +- When a thruster is disabled via service, it immediately publishes zero commands as well. +- The watchdog runs at 10 Hz and only publishes when a timeout occurs. + ## Usage The node will read parameters from the ROS2 parameter server and create instances of thrusters based on their configurations. diff --git a/cybership_thrusters/include/cybership_thrusters/cybership_thrusters.hpp b/cybership_thrusters/include/cybership_thrusters/cybership_thrusters.hpp index 348f7c0..383b868 100644 --- a/cybership_thrusters/include/cybership_thrusters/cybership_thrusters.hpp +++ b/cybership_thrusters/include/cybership_thrusters/cybership_thrusters.hpp @@ -102,6 +102,13 @@ class VoithSchneider : public ThrusterBase { void f_force_callback(const geometry_msgs::msg::Wrench::SharedPtr msg); + // Safety watchdog + rclcpp::TimerBase::SharedPtr m_watchdog_timer; + rclcpp::Time m_last_cmd_time; + double m_safety_timeout_sec = 2.0; + void f_watchdog_check(); + void f_publish_zero(); + public: VoithSchneider(rclcpp::Node::SharedPtr node, std::string thruster_name); @@ -138,6 +145,13 @@ class Fixed : public ThrusterBase { void f_force_callback(const geometry_msgs::msg::Wrench::SharedPtr msg); + // Safety watchdog + rclcpp::TimerBase::SharedPtr m_watchdog_timer; + rclcpp::Time m_last_cmd_time; + double m_safety_timeout_sec = 2.0; + void f_watchdog_check(); + void f_publish_zero(); + rclcpp::Service::SharedPtr m_enable_service; rclcpp::Service::SharedPtr m_disable_service; @@ -200,6 +214,13 @@ class Azimuth : public ThrusterBase { void f_force_callback(const geometry_msgs::msg::Wrench::SharedPtr msg); + // Safety watchdog + rclcpp::TimerBase::SharedPtr m_watchdog_timer; + rclcpp::Time m_last_cmd_time; + double m_safety_timeout_sec = 2.0; + void f_watchdog_check(); + void f_publish_zero(); + public: Azimuth(rclcpp::Node::SharedPtr node, std::string thruster_name); diff --git a/cybership_thrusters/src/azimuth.cpp b/cybership_thrusters/src/azimuth.cpp index 782b64c..81e93b3 100644 --- a/cybership_thrusters/src/azimuth.cpp +++ b/cybership_thrusters/src/azimuth.cpp @@ -25,6 +25,11 @@ Azimuth::Azimuth(rclcpp::Node::SharedPtr node, std::string name) : ThrusterBase( m_disable_service = m_node->create_service("thruster/disable", std::bind(&Azimuth::f_disable_callback, this, std::placeholders::_1, std::placeholders::_2)); + // Safety watchdog setup + m_node->get_parameter_or("thrusters." + m_config.name + ".safety_timeout", m_safety_timeout_sec, 2.0); + m_last_cmd_time = m_node->now(); + m_watchdog_timer = m_node->create_wall_timer(std::chrono::milliseconds(500), std::bind(&Azimuth::f_watchdog_check, this)); + } void Azimuth::f_enable_callback(const std::shared_ptr request, @@ -41,10 +46,12 @@ void Azimuth::f_disable_callback(const std::shared_ptrnow(); if (!m_enabled) { std_msgs::msg::Float32 zero_msg; @@ -77,6 +84,23 @@ void Azimuth::f_force_callback(const geometry_msgs::msg::Wrench::SharedPtr msg) } +void Azimuth::f_watchdog_check() +{ + auto now = m_node->now(); + auto elapsed = (now - m_last_cmd_time).seconds(); + if (elapsed > m_safety_timeout_sec) { + f_publish_zero(); + } +} + +void Azimuth::f_publish_zero() +{ + std_msgs::msg::Float32 zero_msg; + zero_msg.data = 0.0f; + m_angle_pub->publish(zero_msg); + m_rpm_pub->publish(zero_msg); +} + void Azimuth::Config::declare(rclcpp::Node::SharedPtr node) { // node->declare_parameter("thrusters." + name +".force_topic", rclcpp::PARAMETER_STRING); diff --git a/cybership_thrusters/src/fixed.cpp b/cybership_thrusters/src/fixed.cpp index 18b4c77..6e7bdeb 100644 --- a/cybership_thrusters/src/fixed.cpp +++ b/cybership_thrusters/src/fixed.cpp @@ -22,6 +22,11 @@ Fixed::Fixed(rclcpp::Node::SharedPtr node, std::string name) : ThrusterBase(node m_disable_service = m_node->create_service("thruster/disable", std::bind(&Fixed::f_disable_callback, this, std::placeholders::_1, std::placeholders::_2)); + // Safety watchdog setup + m_node->get_parameter_or("thrusters." + m_config.name + ".safety_timeout", m_safety_timeout_sec, 2.0); + m_last_cmd_time = m_node->now(); + m_watchdog_timer = m_node->create_wall_timer(std::chrono::milliseconds(500), std::bind(&Fixed::f_watchdog_check, this)); + } void Fixed::f_enable_callback(const std::shared_ptr request, @@ -38,10 +43,12 @@ void Fixed::f_disable_callback(const std::shared_ptrnow(); if (!m_enabled) { std_msgs::msg::Float32 zero_msg; @@ -79,6 +86,22 @@ void Fixed::f_force_callback(const geometry_msgs::msg::Wrench::SharedPtr msg) } +void Fixed::f_watchdog_check() +{ + auto now = m_node->now(); + auto elapsed = (now - m_last_cmd_time).seconds(); + if (elapsed > m_safety_timeout_sec) { + f_publish_zero(); + } +} + +void Fixed::f_publish_zero() +{ + std_msgs::msg::Float32 zero_msg; + zero_msg.data = 0.0f; + m_signal_pub->publish(zero_msg); +} + void Fixed::Config::declare(rclcpp::Node::SharedPtr node) { diff --git a/cybership_thrusters/src/voith_schneider.cpp b/cybership_thrusters/src/voith_schneider.cpp index 4118466..9b3b66b 100644 --- a/cybership_thrusters/src/voith_schneider.cpp +++ b/cybership_thrusters/src/voith_schneider.cpp @@ -27,6 +27,11 @@ VoithSchneider::VoithSchneider(rclcpp::Node::SharedPtr node, std::string name) : m_disable_service = m_node->create_service("thruster/disable", std::bind(&VoithSchneider::f_disable_callback, this, std::placeholders::_1, std::placeholders::_2)); + // Safety watchdog setup + m_node->get_parameter_or("thrusters." + m_config.name + ".safety_timeout", m_safety_timeout_sec, 2.0); + m_last_cmd_time = m_node->now(); + m_watchdog_timer = m_node->create_wall_timer(std::chrono::milliseconds(500), std::bind(&VoithSchneider::f_watchdog_check, this)); + } void VoithSchneider::f_enable_callback(const std::shared_ptr request, @@ -43,10 +48,12 @@ void VoithSchneider::f_disable_callback(const std::shared_ptrnow(); if (!m_enabled) { std_msgs::msg::Float32 zero_msg; @@ -103,6 +110,24 @@ void VoithSchneider::f_force_callback(const geometry_msgs::msg::Wrench::SharedPt } +void VoithSchneider::f_watchdog_check() +{ + auto now = m_node->now(); + auto elapsed = (now - m_last_cmd_time).seconds(); + if (elapsed > m_safety_timeout_sec) { + f_publish_zero(); + } +} + +void VoithSchneider::f_publish_zero() +{ + std_msgs::msg::Float32 zero_msg; + zero_msg.data = 0.0f; + m_arm_x_pub->publish(zero_msg); + m_arm_y_pub->publish(zero_msg); + m_rpm_pub->publish(zero_msg); +} + void VoithSchneider::Config::declare(rclcpp::Node::SharedPtr node) { // node->declare_parameter("thrusters." + name +".force_topic", rclcpp::PARAMETER_STRING); From 452861dfe789c34d0f1987e3ec89cdede4213f62 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Tue, 23 Sep 2025 16:42:17 +0200 Subject: [PATCH 45/46] fix: parameter update --- .../config/voyager/controller_position.yaml | 16 ++++++++-------- .../config/voyager/controller_velocity.yaml | 4 ++-- .../guidance/ros_action_server.py | 4 ++-- .../cybership_dp/voyager/force_controller.py | 4 ++-- cybership_dp/nodes/velocity_control_node.py | 10 ++++++---- 5 files changed, 20 insertions(+), 18 deletions(-) diff --git a/cybership_config/config/voyager/controller_position.yaml b/cybership_config/config/voyager/controller_position.yaml index 17bff47..a81565d 100644 --- a/cybership_config/config/voyager/controller_position.yaml +++ b/cybership_config/config/voyager/controller_position.yaml @@ -10,26 +10,26 @@ control: # Position control gains p_gain: - pos: 1.0 # Proportional gain for position + pos: 1.6 # Proportional gain for position vel: 0.7 # Proportional gain for velocity - yaw: 0.9 # Proportional gain for yaw + yaw: 1.1 # Proportional gain for yaw # Integral gains i_gain: - pos: 0.1 # Integral gain for position + pos: 0.2 # Integral gain for position vel: 0.1 # Integral gain for velocity - yaw: 0.1 # Integral gain for yaw + yaw: 0.2 # Integral gain for yaw # Derivative gains d_gain: - pos: 0.2 # Derivative gain for position + pos: 0.5 # Derivative gain for position vel: 0.5 # Derivative gain for velocity - yaw: 1.0 # Derivative gain for yaw + yaw: 0.5 # Derivative gain for yaw # Integral error limits max_integral_error: pos: 1.0 # Maximum position error integration - yaw: 0.1 # Maximum yaw error integration + yaw: 0.3 # Maximum yaw error integration # Saturation parameters saturation: @@ -52,4 +52,4 @@ interval: 1.0 # Time between metrics calculations (seconds) # General parameters - dt: 0.01 # Control loop time step (seconds) + dt: 0.1 # Control loop time step (seconds) diff --git a/cybership_config/config/voyager/controller_velocity.yaml b/cybership_config/config/voyager/controller_velocity.yaml index 1a701de..29c6d9a 100644 --- a/cybership_config/config/voyager/controller_velocity.yaml +++ b/cybership_config/config/voyager/controller_velocity.yaml @@ -20,7 +20,7 @@ p_gain: surge: 0.5 # Forward/backward motion (u) sway: 0.3 # Left/right motion (v) - yaw: 0.3 # Rotational motion (r) + yaw: 0.7 # Rotational motion (r) # Integral gains for each DOF # Helps eliminate steady-state errors @@ -48,4 +48,4 @@ interval: 5.0 # Time between metrics calculations (seconds) # Timing parameters - dt: 0.2 # Control loop time step (seconds) - affects timer frequency + dt: 0.05 # Control loop time step (seconds) - affects timer frequency diff --git a/cybership_controller/src/cybership_controller/guidance/ros_action_server.py b/cybership_controller/src/cybership_controller/guidance/ros_action_server.py index 4fdfdcd..606d8ff 100755 --- a/cybership_controller/src/cybership_controller/guidance/ros_action_server.py +++ b/cybership_controller/src/cybership_controller/guidance/ros_action_server.py @@ -30,9 +30,9 @@ def __init__(self): super().__init__('los_guidance_server', namespace="cybership") # Parameters self.declare_parameter('desired_speed', 0.3) - self.declare_parameter('lookahead', 0.4) + self.declare_parameter('lookahead', 1.4) # Heading control gain - self.declare_parameter('heading_gain', 1.1) + self.declare_parameter('heading_gain', 1.0) # Publishers and Subscribers self._cmd_pub = self.create_publisher( Twist, 'control/velocity/command', 10) diff --git a/cybership_dp/cybership_dp/voyager/force_controller.py b/cybership_dp/cybership_dp/voyager/force_controller.py index 03d347b..971c778 100644 --- a/cybership_dp/cybership_dp/voyager/force_controller.py +++ b/cybership_dp/cybership_dp/voyager/force_controller.py @@ -137,8 +137,8 @@ def _initialize_allocator(self): self.allocator = skadipy.allocator.reference_filters.MinimumMagnitudeAndAzimuth( actuators=self.actuators, force_torque_components=dofs, - gamma=0.001, - mu=0.01, + gamma=0.01, + mu=0.1, rho=1, time_step=(1.0 /self.freq ), control_barrier_function=skadipy.safety.ControlBarrierFunctionType.SUMSQUARE, diff --git a/cybership_dp/nodes/velocity_control_node.py b/cybership_dp/nodes/velocity_control_node.py index ffc10ec..4ffc2f5 100755 --- a/cybership_dp/nodes/velocity_control_node.py +++ b/cybership_dp/nodes/velocity_control_node.py @@ -47,9 +47,9 @@ def __init__(self): ('vessel.draft', 0.05), # Control gains - ('control.p_gain.surge', 5.0), - ('control.p_gain.sway', 5.0), - ('control.p_gain.yaw', 5.0), + ('control.p_gain.surge', 1.0), + ('control.p_gain.sway', 1.0), + ('control.p_gain.yaw', 1.0), ('control.i_gain.surge', 0.0), ('control.i_gain.sway', 0.0), @@ -59,7 +59,7 @@ def __init__(self): ('control.d_gain.sway', 0.3), ('control.d_gain.yaw', 0.3), - ('control.i_max', 20.0), + ('control.i_max', 1.0), ('control.smooth_limit', True), ('control.filter_alpha', 0.1), @@ -211,6 +211,8 @@ def parameters_callback(self, params): """Handle parameter updates""" update_needed = False + # Log parameter changes + for param in params: if param.name in [ 'vessel.length', 'vessel.beam', 'vessel.draft', From 441e7fd5be504ba25340091c4715843ce8561d13 Mon Sep 17 00:00:00 2001 From: Emir Cem Gezer Date: Tue, 30 Sep 2025 12:17:38 +0200 Subject: [PATCH 46/46] feat: improve multi visualization --- cybership_viz/config/multi.rviz | 453 +++++++++++++++----------------- 1 file changed, 213 insertions(+), 240 deletions(-) diff --git a/cybership_viz/config/multi.rviz b/cybership_viz/config/multi.rviz index a8fa91f..e2e07a3 100644 --- a/cybership_viz/config/multi.rviz +++ b/cybership_viz/config/multi.rviz @@ -6,25 +6,10 @@ Panels: Expanded: - /Global Options1 - /Status1 - - /TF1 - /TF1/Frames1 - /TF1/Tree1 - - /Odometry1 - - /Odometry1/Shape1 - - /RobotModel1 - - /RobotModel1/Mass Properties1 - - /RobotModel1/Description Topic1 - - /Marker1 - - /MarkerArray1 - - /RobotModel2 - - /Wrench1 - - /Voyager1 - - /Drillship1 - - /Drillship1/Wrench1 - - /Drillship1/Wrench2 - - /Drillship1/Wrench3 Splitter Ratio: 0.5 - Tree Height: 709 + Tree Height: 1228 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -146,152 +131,14 @@ Visualization Manager: {} Update Interval: 0 Value: true - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 255; 25; 0 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Name: PoseWithCovariance - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: measurement/pose - Value: true - - Angle Tolerance: 0.10000000149011612 - Class: rviz_default_plugins/Odometry - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: true - Keep: 100 - Name: Odometry - Position Tolerance: 0.10000000149011612 - Shape: - Alpha: 1 - Axes Length: 0.25 - Axes Radius: 0.02500000037252903 - Color: 255; 25; 0 - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Value: Axes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: measurement/odom - Value: true - - Alpha: 0.4000000059604645 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /drillship/robot_description - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - aft_center_thruster_link: - Alpha: 1 - Show Axes: false - Show Trail: false - aft_port_thruster_link: - Alpha: 1 - Show Axes: false - Show Trail: false - aft_starboard_thruster_link: - Alpha: 1 - Show Axes: false - Show Trail: false - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - base_link_ned: - Alpha: 1 - Show Axes: false - Show Trail: false - bow_center_thruster_link: - Alpha: 1 - Show Axes: false - Show Trail: false - bow_port_thruster_link: - Alpha: 1 - Show Axes: false - Show Trail: false - bow_starboard_thruster_link: - Alpha: 1 - Show Axes: false - Show Trail: false - imu_link: - Alpha: 1 - Show Axes: false - Show Trail: false - mocap_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Mass Properties: - Inertia: false - Mass: false - Name: RobotModel - TF Prefix: drillship - Update Interval: 0 - Value: true - Visual Enabled: true - Class: rviz_default_plugins/Marker Enabled: true Name: Marker Namespaces: - {} + los_arrow: true + los_lookahead: true + los_path: true + los_spline: true Topic: Depth: 5 Durability Policy: Volatile @@ -312,80 +159,6 @@ Visualization Manager: Reliability Policy: Reliable Value: /voyager/visualization_marker_array Value: true - - Alpha: 0.4000000059604645 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /voyager/robot_description - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - base_link_ned: - Alpha: 1 - Show Axes: false - Show Trail: false - bow_tunnel_thruster_link: - Alpha: 1 - Show Axes: false - Show Trail: false - imu_link: - Alpha: 1 - Show Axes: false - Show Trail: false - mocap_link: - Alpha: 1 - Show Axes: false - Show Trail: false - stern_port_thruster_link: - Alpha: 1 - Show Axes: false - Show Trail: false - stern_starboard_thruster_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Mass Properties: - Inertia: false - Mass: false - Name: RobotModel - TF Prefix: voyager - Update Interval: 0 - Value: true - Visual Enabled: true - - Accept NaN Values: false - Alpha: 1 - Arrow Width: 0.5 - Class: rviz_default_plugins/Wrench - Enabled: true - Force Arrow Scale: 2 - Force Color: 204; 51; 51 - History Length: 1 - Name: Wrench - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /drillship/thruster/aft_center/issued - Torque Arrow Scale: 2 - Torque Color: 204; 204; 51 - Value: true - Class: rviz_common/Group Displays: - Accept NaN Values: false @@ -445,6 +218,100 @@ Visualization Manager: Torque Arrow Scale: 2 Torque Color: 204; 204; 51 Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 0.25 + Axes Radius: 0.02500000037252903 + Color: 255; 255; 0 + Head Length: 0.10000000149011612 + Head Radius: 0.10000000149011612 + Shaft Length: 0.5 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /voyager/measurement/odom + Value: true + - Alpha: 0.4000000059604645 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /voyager/robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link_ned: + Alpha: 1 + Show Axes: false + Show Trail: false + bow_tunnel_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + mocap_link: + Alpha: 1 + Show Axes: false + Show Trail: false + stern_port_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + stern_starboard_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: voyager + Update Interval: 0 + Value: true + Visual Enabled: true Enabled: true Name: Voyager - Class: rviz_common/Group @@ -563,6 +430,112 @@ Visualization Manager: Torque Arrow Scale: 2 Torque Color: 204; 204; 51 Value: true + - Alpha: 0.4000000059604645 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /drillship/robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + aft_center_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + aft_port_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + aft_starboard_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link_ned: + Alpha: 1 + Show Axes: false + Show Trail: false + bow_center_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + bow_port_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + bow_starboard_thruster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + mocap_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: drillship + Update Interval: 0 + Value: true + Visual Enabled: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 0.25 + Axes Radius: 0.02500000037252903 + Color: 255; 255; 0 + Head Length: 0.10000000149011612 + Head Radius: 0.10000000149011612 + Shaft Length: 0.5 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /drillship/measurement/odom + Value: true Enabled: true Name: Drillship Enabled: true @@ -611,7 +584,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 10 + Distance: 16.33730697631836 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -626,18 +599,18 @@ Visualization Manager: Invert Z Axis: true Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.8403981328010559 + Pitch: 0.7853981852531433 Target Frame: base_link Value: Orbit (rviz) - Yaw: 0.7353982329368591 + Yaw: 0.7853981852531433 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1000 + Height: 1519 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000002ab0000034efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000033100000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000034e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001000000034efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000034e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006810000003efc0100000002fb0000000800540069006d00650100000000000006810000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000003d00000034e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000024500000555fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000033100000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000555000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010000000555fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000555000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007fd0000003efc0100000002fb0000000800540069006d00650100000000000007fd0000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005b20000055500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -646,6 +619,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1665 - X: 1148 - Y: 580 + Width: 2045 + X: 1313 + Y: 217