try new zoom cmd id

.envrc

@@ -1 +0,0 @@
-use flake

.gitignore

@@ -10,7 +10,4 @@ log/
 .vscode/
 
 #Pycache folder
-__pycache__/
-
-#Direnv
-.direnv/
+__pycache__/

.gitmodules

@@ -1,6 +1,3 @@
 [submodule "src/ros2_interfaces_pkg"]
 	path = src/ros2_interfaces_pkg
-	url = ../ros2_interfaces_pkg
-[submodule "src/astra_description"]
-	path = src/astra_descriptions
-	url = ../astra_descriptions
+	url = git@github.com:SHC-ASTRA/ros2_interfaces_pkg.git

LICENSE

@@ -1,661 +0,0 @@
-                    GNU AFFERO GENERAL PUBLIC LICENSE
-                       Version 3, 19 November 2007
-
- Copyright (C) 2007 Free Software Foundation, Inc. <https://fsf.org/>
- Everyone is permitted to copy and distribute verbatim copies
- of this license document, but changing it is not allowed.
-
-                            Preamble
-
-  The GNU Affero General Public License is a free, copyleft license for
-software and other kinds of works, specifically designed to ensure
-cooperation with the community in the case of network server software.
-
-  The licenses for most software and other practical works are designed
-to take away your freedom to share and change the works.  By contrast,
-our General Public Licenses are 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.
-
-  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.
-
-  Developers that use our General Public Licenses protect your rights
-with two steps: (1) assert copyright on the software, and (2) offer
-you this License which gives you legal permission to copy, distribute
-and/or modify the software.
-
-  A secondary benefit of defending all users' freedom is that
-improvements made in alternate versions of the program, if they
-receive widespread use, become available for other developers to
-incorporate.  Many developers of free software are heartened and
-encouraged by the resulting cooperation.  However, in the case of
-software used on network servers, this result may fail to come about.
-The GNU General Public License permits making a modified version and
-letting the public access it on a server without ever releasing its
-source code to the public.
-
-  The GNU Affero General Public License is designed specifically to
-ensure that, in such cases, the modified source code becomes available
-to the community.  It requires the operator of a network server to
-provide the source code of the modified version running there to the
-users of that server.  Therefore, public use of a modified version, on
-a publicly accessible server, gives the public access to the source
-code of the modified version.
-
-  An older license, called the Affero General Public License and
-published by Affero, was designed to accomplish similar goals.  This is
-a different license, not a version of the Affero GPL, but Affero has
-released a new version of the Affero GPL which permits relicensing under
-this license.
-
-  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 Affero 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. Remote Network Interaction; Use with the GNU General Public License.
-
-  Notwithstanding any other provision of this License, if you modify the
-Program, your modified version must prominently offer all users
-interacting with it remotely through a computer network (if your version
-supports such interaction) an opportunity to receive the Corresponding
-Source of your version by providing access to the Corresponding Source
-from a network server at no charge, through some standard or customary
-means of facilitating copying of software.  This Corresponding Source
-shall include the Corresponding Source for any work covered by version 3
-of the GNU General Public License that is incorporated pursuant to the
-following paragraph.
-
-  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 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 work with which it is combined will remain governed by version
-3 of the GNU General Public License.
-
-  14. Revised Versions of this License.
-
-  The Free Software Foundation may publish revised and/or new versions of
-the GNU Affero 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 Affero 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 Affero 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 Affero General Public License can be used, that proxy's
-public statement of acceptance of a version permanently authorizes you
-to choose that version for the Program.
-
-  Later license versions may give you additional or different
-permissions.  However, no additional obligations are imposed on any
-author or copyright holder as a result of your choosing to follow a
-later version.
-
-  15. Disclaimer of Warranty.
-
-  THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
-APPLICABLE LAW.  EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
-HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
-OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
-THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
-PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
-IS WITH YOU.  SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
-ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
-
-  16. Limitation of Liability.
-
-  IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
-WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
-THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
-GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
-USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
-DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
-PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
-EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
-SUCH DAMAGES.
-
-  17. Interpretation of Sections 15 and 16.
-
-  If the disclaimer of warranty and limitation of liability provided
-above cannot be given local legal effect according to their terms,
-reviewing courts shall apply local law that most closely approximates
-an absolute waiver of all civil liability in connection with the
-Program, unless a warranty or assumption of liability accompanies a
-copy of the Program in return for a fee.
-
-                     END OF TERMS AND CONDITIONS
-
-            How to Apply These Terms to Your New Programs
-
-  If you develop a new program, and you want it to be of the greatest
-possible use to the public, the best way to achieve this is to make it
-free software which everyone can redistribute and change under these terms.
-
-  To do so, attach the following notices to the program.  It is safest
-to attach them to the start of each source file to most effectively
-state the exclusion of warranty; and each file should have at least
-the "copyright" line and a pointer to where the full notice is found.
-
-    <one line to give the program's name and a brief idea of what it does.>
-    Copyright (C) <year>  <name of author>
-
-    This program is free software: you can redistribute it and/or modify
-    it under the terms of the GNU Affero 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 Affero General Public License for more details.
-
-    You should have received a copy of the GNU Affero General Public License
-    along with this program.  If not, see <https://www.gnu.org/licenses/>.
-
-Also add information on how to contact you by electronic and paper mail.
-
-  If your software can interact with users remotely through a computer
-network, you should also make sure that it provides a way for users to
-get its source.  For example, if your program is a web application, its
-interface could display a "Source" link that leads users to an archive
-of the code.  There are many ways you could offer source, and different
-solutions will be better for different programs; see section 13 for the
-specific requirements.
-
-  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 AGPL, see
-<https://www.gnu.org/licenses/>.

README.md

@@ -1,7 +1,5 @@
 # rover-ros2
 
-[![License: AGPL v3](https://img.shields.io/badge/License-AGPL_v3-blue.svg)](https://www.gnu.org/licenses/agpl-3.0)
-
 Submodule which includes all ros2 packages for the rover. These are centrally located for modular rover operation.
 
 You will use this package to launch any module-side ROS2 nodes.

auto_start/auto_start_anchor.sh

@@ -1,24 +0,0 @@
-#!/bin/bash
-
-# Wait for a network interface to be up (not necessarily online)
-while ! ip link show | grep -q "state UP"; do
-    echo "[INFO] Waiting for active network interface..."
-    sleep 1
-done
-
-echo "[INFO] Network interface is up!"
-
-# Your actual ROS node start command goes here
-echo "[INFO] Starting ROS node..."
-
-# Source ROS 2 Humble setup script
-source /opt/ros/humble/setup.bash
-
-# Source your workspace setup script
-source /home/clucky/rover-ros2/install/setup.bash
-
-# CD to directory
-cd /home/clucky/rover-ros2/
-
-# Launch the ROS 2 node with the desired mode
-ros2 launch anchor_pkg rover.launch.py mode:=anchor

auto_start/auto_start_core_headless.sh

@@ -1,24 +0,0 @@
-#!/bin/bash
-
-# Wait for a network interface to be up (not necessarily online)
-while ! ip link show | grep -q "state UP"; do
-    echo "[INFO] Waiting for active network interface..."
-    sleep 1
-done
-
-echo "[INFO] Network interface is up!"
-
-# Your actual ROS node start command goes here
-echo "[INFO] Starting ROS node..."
-
-# Source ROS 2 Humble setup script
-source /opt/ros/humble/setup.bash
-
-# Source your workspace setup script
-source /home/clucky/rover-ros2/install/setup.bash
-
-# CD to directory
-cd /home/clucky/rover-ros2/
-
-# Launch the ROS 2 node
-ros2 run core_pkg headless

auto_start/auto_start_headless_full.sh

@@ -1,25 +0,0 @@
-#!/bin/bash
-
-# Wait for a network interface to be up (not necessarily online)
-while ! ip link show | grep -q "state UP"; do
-    echo "[INFO] Waiting for active network interface..."
-    sleep 1
-done
-
-echo "[INFO] Network interface is up!"
-
-# Your actual ROS node start command goes here
-echo "[INFO] Starting ROS node..."
-
-# Source ROS 2 Humble setup script
-source /opt/ros/humble/setup.bash
-
-# Source your workspace setup script
-source /home/clucky/rover-ros2/install/setup.bash
-
-# CD to directory
-cd /home/clucky/rover-ros2/
-
-# Launch the ROS 2 node
-ros2 run headless_pkg headless_full
-

auto_start/start_rosbag.sh

@@ -1,22 +0,0 @@
-#!/bin/bash
-
-ANCHOR_WS="/home/clucky/rover-ros2"
-AUTONOMY_WS="/home/clucky/rover-Autonomy"
-BAG_LOCATION="/home/clucky/bags/autostart"
-
-# Wait for a network interface to be up (not necessarily online)
-while ! ip link show | grep -q "state UP"; do
-    echo "[INFO] Waiting for active network interface..."
-    sleep 1
-done
-
-echo "[INFO] Network interface is up!"
-
-
-source /opt/ros/humble/setup.bash
-source $ANCHOR_WS/install/setup.bash
-[ -f $AUTONOMY_WS/install/setup.bash ] && source $AUTONOMY_WS/install/setup.bash
-
-cd $BAG_LOCATION
-
-ros2 bag record -a

flake.lock

@@ -1,84 +0,0 @@
-{
-  "nodes": {
-    "flake-utils": {
-      "inputs": {
-        "systems": "systems"
-      },
-      "locked": {
-        "lastModified": 1731533236,
-        "narHash": "sha256-l0KFg5HjrsfsO/JpG+r7fRrqm12kzFHyUHqHCVpMMbI=",
-        "owner": "numtide",
-        "repo": "flake-utils",
-        "rev": "11707dc2f618dd54ca8739b309ec4fc024de578b",
-        "type": "github"
-      },
-      "original": {
-        "owner": "numtide",
-        "repo": "flake-utils",
-        "type": "github"
-      }
-    },
-    "nix-ros-overlay": {
-      "inputs": {
-        "flake-utils": "flake-utils",
-        "nixpkgs": "nixpkgs"
-      },
-      "locked": {
-        "lastModified": 1758094726,
-        "narHash": "sha256-agLnClczRtYY+kQFh5dv4wGNhtFNKK7KFOmypDhsWCs=",
-        "owner": "lopsided98",
-        "repo": "nix-ros-overlay",
-        "rev": "9d0557032aadb65df065b1972a632572b57234b5",
-        "type": "github"
-      },
-      "original": {
-        "owner": "lopsided98",
-        "ref": "master",
-        "repo": "nix-ros-overlay",
-        "type": "github"
-      }
-    },
-    "nixpkgs": {
-      "locked": {
-        "lastModified": 1744849697,
-        "narHash": "sha256-S9hqvanPSeRu6R4cw0OhvH1rJ+4/s9xIban9C4ocM/0=",
-        "owner": "lopsided98",
-        "repo": "nixpkgs",
-        "rev": "6318f538166fef9f5118d8d78b9b43a04bb049e4",
-        "type": "github"
-      },
-      "original": {
-        "owner": "lopsided98",
-        "ref": "nix-ros",
-        "repo": "nixpkgs",
-        "type": "github"
-      }
-    },
-    "root": {
-      "inputs": {
-        "nix-ros-overlay": "nix-ros-overlay",
-        "nixpkgs": [
-          "nix-ros-overlay",
-          "nixpkgs"
-        ]
-      }
-    },
-    "systems": {
-      "locked": {
-        "lastModified": 1681028828,
-        "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=",
-        "owner": "nix-systems",
-        "repo": "default",
-        "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e",
-        "type": "github"
-      },
-      "original": {
-        "owner": "nix-systems",
-        "repo": "default",
-        "type": "github"
-      }
-    }
-  },
-  "root": "root",
-  "version": 7
-}

flake.nix

@@ -1,74 +0,0 @@
-{
-  description = "Development environment for ASTRA Anchor";
-
-  inputs = {
-    nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/master";
-    nixpkgs.follows = "nix-ros-overlay/nixpkgs"; # IMPORTANT!!!
-  };
-
-  outputs =
-    {
-      self,
-      nix-ros-overlay,
-      nixpkgs,
-    }:
-    nix-ros-overlay.inputs.flake-utils.lib.eachDefaultSystem (
-      system:
-      let
-        pkgs = import nixpkgs {
-          inherit system;
-          overlays = [ nix-ros-overlay.overlays.default ];
-        };
-      in
-      {
-        devShells.default = pkgs.mkShell {
-          name = "ASTRA Anchor";
-          packages = with pkgs; [
-            colcon
-            (python312.withPackages (
-              p: with p; [
-                pyserial
-                pygame
-                scipy
-                crccheck
-                black
-              ]
-            ))
-            (
-              with rosPackages.humble;
-              buildEnv {
-                paths = [
-                  ros-core
-                  ros2cli
-                  ros2run
-                  ros2bag
-                  rviz2
-                  xacro
-                  ament-cmake-core
-                  python-cmake-module
-                  diff-drive-controller
-                  parameter-traits
-                  generate-parameter-library
-                  joint-state-publisher-gui
-                  robot-state-publisher
-                  ros2-control
-                  controller-manager
-                  # ros2-controllers nixpkg does not build :(
-                ];
-              }
-            )
-          ];
-          shellHook = ''
-            # Display stuff
-            export DISPLAY=''${DISPLAY:-:0}
-            export QT_X11_NO_MITSHM=1
-          '';
-        };
-      }
-    );
-
-  nixConfig = {
-    extra-substituters = [ "https://ros.cachix.org" ];
-    extra-trusted-public-keys = [ "ros.cachix.org-1:dSyZxI8geDCJrwgvCOHDoAfOm5sV1wCPjBkKL+38Rvo=" ];
-  };
-}

src/anchor_pkg/anchor_pkg/anchor_node.py

@@ -7,86 +7,22 @@ import time
 import atexit
 
 import serial
-import os
 import sys
 import threading
 import glob
 
-from std_msgs.msg import String, Header
-from ros2_interfaces_pkg.msg import VicCAN
+from std_msgs.msg import String
+from ros2_interfaces_pkg.msg import CoreFeedback
+from ros2_interfaces_pkg.msg import CoreControl
 
 serial_pub = None
 thread = None
 
-
-"""
-Publishers:
-  * /anchor/from_vic/debug
-    - Every string received from the MCU is published here for debugging
-  * /anchor/from_vic/core
-    - VicCAN messages for Core node
-  * /anchor/from_vic/arm
-    - VicCAN messages for Arm node
-  * /anchor/from_vic/bio
-    - VicCAN messages for Bio node
-
-Subscribers:
-  * /anchor/from_vic/mock_mcu
-    - For testing without an actual MCU, publish strings here as if they came from an MCU
-  * /anchor/to_vic/relay
-    - Core, Arm, and Bio publish VicCAN messages to this topic to send to the MCU
-  * /anchor/to_vic/relay_string
-    - Publish raw strings to this topic to send directly to the MCU for debugging
-"""
 class SerialRelay(Node):
     def __init__(self):
         # Initalize node with name
         super().__init__("anchor_node")#previously 'serial_publisher'
 
-        # Loop through all serial devices on the computer to check for the MCU
-        self.port = None
-        if port_override := os.getenv("PORT_OVERRIDE"):
-            self.port = port_override
-        ports = SerialRelay.list_serial_ports()
-        for i in range(4):
-            if self.port is not None:
-                break
-            for port in ports:
-                try:
-                    # connect and send a ping command
-                    ser = serial.Serial(port, 115200, timeout=1)
-                    #(f"Checking port {port}...")
-                    ser.write(b"ping\n")
-                    response = ser.read_until(bytes("\n", "utf8"))
-
-                    # if pong is in response, then we are talking with the MCU
-                    if b"pong" in response:
-                        self.port = port
-                        self.get_logger().info(f"Found MCU at {self.port}!")
-                        break
-                except:
-                    pass
-
-        if self.port is None:
-            self.get_logger().info("Unable to find MCU...")
-            time.sleep(1)
-            sys.exit(1)
-
-        self.ser = serial.Serial(self.port, 115200)
-        self.get_logger().info(f"Enabling Relay Mode")
-        self.ser.write(b"can_relay_mode,on\n")
-        atexit.register(self.cleanup)
-
-        # New pub/sub with VicCAN
-        self.fromvic_debug_pub_ = self.create_publisher(String, '/anchor/from_vic/debug', 20)
-        self.fromvic_core_pub_ = self.create_publisher(VicCAN, '/anchor/from_vic/core', 20)
-        self.fromvic_arm_pub_ = self.create_publisher(VicCAN, '/anchor/from_vic/arm', 20)
-        self.fromvic_bio_pub_ = self.create_publisher(VicCAN, '/anchor/from_vic/bio', 20)
-
-        self.mock_mcu_sub_ = self.create_subscription(String, '/anchor/from_vic/mock_mcu', self.on_mock_fromvic, 20)
-        self.tovic_sub_ = self.create_subscription(VicCAN, '/anchor/to_vic/relay', self.on_relay_tovic_viccan, 20)
-        self.tovic_debug_sub_ = self.create_subscription(String, '/anchor/to_vic/relay_string', self.on_relay_tovic_string, 20)
-
 
         # Create publishers 
         self.arm_pub = self.create_publisher(String, '/anchor/arm/feedback', 10)
@@ -94,9 +30,41 @@ class SerialRelay(Node):
         self.bio_pub = self.create_publisher(String, '/anchor/bio/feedback', 10)
 
         self.debug_pub = self.create_publisher(String, '/anchor/debug', 10)
-
+        
         # Create a subscriber 
-        self.relay_sub = self.create_subscription(String, '/anchor/relay', self.on_relay_tovic_string, 10)
+        self.relay_sub = self.create_subscription(String, '/anchor/relay', self.send_cmd, 10)
+
+        # Loop through all serial devices on the computer to check for the MCU
+        self.port = None
+        ports = SerialRelay.list_serial_ports()
+        for i in range(4):
+            for port in ports:
+                try:
+                    # connect and send a ping command
+                    ser = serial.Serial(port, 115200, timeout=1)
+                    #(f"Checking port {port}...")
+                    ser.write(b"ping\n")
+                    response = ser.read_until("\n")
+
+                    # if pong is in response, then we are talking with the MCU
+                    if b"pong" in response:
+                        self.port = port
+                        self.get_logger().info(f"Found MCU at {self.port}!")
+                        self.get_logger().info(f"Enabling Relay Mode")
+                        ser.write(b"can_relay_mode,on\n")
+                        break
+                except:
+                    pass
+            if self.port is not None:
+                break
+        
+        if self.port is None:
+            self.get_logger().info("Unable to find MCU...")
+            time.sleep(1)
+            sys.exit(1)
+        
+        self.ser = serial.Serial(self.port, 115200)
+        atexit.register(self.cleanup)
 
 
     def run(self):
@@ -112,22 +80,19 @@ class SerialRelay(Node):
             sys.exit(0)
 
     def read_MCU(self):
-        """ Check the USB serial port for new data from the MCU, and publish string to appropriate topics """
         try:
             output = str(self.ser.readline(), "utf8")
             
             if output:
-                self.relay_fromvic(output)
                 # All output over debug temporarily
                 #self.get_logger().info(f"[MCU] {output}")
                 msg = String()
                 msg.data = output
-                self.debug_pub.publish(msg)
                 if output.startswith("can_relay_fromvic,core"):
                     self.core_pub.publish(msg)
                 elif output.startswith("can_relay_fromvic,arm") or output.startswith("can_relay_fromvic,digit"):  # digit for voltage readings
                     self.arm_pub.publish(msg)
-                if output.startswith("can_relay_fromvic,citadel") or output.startswith("can_relay_fromvic,digit"):  # digit for SHT sensor
+                elif output.startswith("can_relay_fromvic,citadel") or output.startswith("can_relay_fromvic,digit"):  # digit for SHT sensor
                     self.bio_pub.publish(msg)
                 # msg = String()
                 # msg.data = output
@@ -138,89 +103,21 @@ class SerialRelay(Node):
             print("Closing serial port.")
             if self.ser.is_open:
                 self.ser.close()
-            exit(1)
+            self.exit(1)
         except TypeError as e:
             print(f"TypeError: {e}")
             print("Closing serial port.")
             if self.ser.is_open:
                 self.ser.close()
-            exit(1)
+            self.exit(1)
         except Exception as e:
             print(f"Exception: {e}")
-            # print("Closing serial port.")
-            # if self.ser.is_open:
-            #     self.ser.close()
-            # exit(1)
+            print("Closing serial port.")
+            if self.ser.is_open:
+                self.ser.close()
+            self.exit(1)
 
-
-    def on_mock_fromvic(self, msg: String):
-        """ For testing without an actual MCU, publish strings here as if they came from an MCU """
-        # self.get_logger().info(f"Got command from mock MCU: {msg}")
-        self.relay_fromvic(msg.data)
-
-
-    def on_relay_tovic_viccan(self, msg: VicCAN):
-        """ Relay a VicCAN message to the MCU """
-        output: str = f"can_relay_tovic,{msg.mcu_name},{msg.command_id}"
-        for num in msg.data:
-            output += f",{round(num, 7)}"  # limit to 7 decimal places
-        output += "\n"
-        # self.get_logger().info(f"VicCAN relay to MCU: {output}")
-        self.ser.write(bytes(output, "utf8"))
-
-    def relay_fromvic(self, msg: str):
-        """ Relay a string message from the MCU to the appropriate VicCAN topic """
-        self.fromvic_debug_pub_.publish(String(data=msg))
-        parts = msg.strip().split(",")
-        if len(parts) > 0 and parts[0] != "can_relay_fromvic":
-            self.get_logger().debug(f"Ignoring non-VicCAN message: '{msg.strip()}'")
-            return
-
-        # String validation
-        malformed: bool = False
-        malformed_reason: str = ""
-        if len(parts) < 3 or len(parts) > 7:
-            malformed = True
-            malformed_reason = f"invalid argument count (expected [3,7], got {len(parts)})"
-        elif parts[1] not in ["core", "arm", "digit", "citadel", "broadcast"]:
-            malformed = True
-            malformed_reason = f"invalid mcu_name '{parts[1]}'"
-        elif not(parts[2].isnumeric()) or int(parts[2]) < 0:
-            malformed = True
-            malformed_reason = f"command_id '{parts[2]}' is not a non-negative integer"
-        else:
-            for x in parts[3:]:
-                try:
-                    float(x)
-                except ValueError:
-                    malformed = True
-                    malformed_reason = f"data '{x}' is not a float"
-                    break
-
-        if malformed:
-            self.get_logger().warning(f"Ignoring malformed from_vic message: '{msg.strip()}'; reason: {malformed_reason}")
-            return
-
-        # Have valid VicCAN message
-
-        output = VicCAN()
-        output.mcu_name = parts[1]
-        output.command_id = int(parts[2])
-        if len(parts) > 3:
-            output.data = [float(x) for x in parts[3:]]
-        output.header = Header(stamp=self.get_clock().now().to_msg(), frame_id="from_vic")
-
-        # self.get_logger().info(f"Relaying from MCU: {output}")
-        if output.mcu_name == "core":
-            self.fromvic_core_pub_.publish(output)
-        elif output.mcu_name == "arm" or output.mcu_name == "digit":
-            self.fromvic_arm_pub_.publish(output)
-        elif output.mcu_name == "citadel" or output.mcu_name == "digit":
-            self.fromvic_bio_pub_.publish(output)
-
-
-    def on_relay_tovic_string(self, msg: String):
-        """ Relay a raw string message to the MCU for debugging """
+    def send_cmd(self, msg):
         message = msg.data
         #self.get_logger().info(f"Sending command to MCU: {msg}")
         self.ser.write(bytes(message, "utf8"))
@@ -228,7 +125,7 @@ class SerialRelay(Node):
     @staticmethod
     def list_serial_ports():
         return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
-
+    
     def cleanup(self):
         print("Cleaning up before terminating...")
         if self.ser.is_open:
@@ -250,6 +147,6 @@ def main(args=None):
     serial_pub.run()
 
 if __name__ == '__main__':
-    #signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0))  # Catch Ctrl+Z and exit cleanly
+    signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0))  # Catch Ctrl+Z and exit cleanly
     signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0))  # Catch termination signals and exit cleanly
     main()

src/anchor_pkg/package.xml

@@ -5,13 +5,9 @@
   <version>0.0.0</version>
   <description>TODO: Package description</description>
   <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
-  <license>AGPL-3.0-only</license>
+  <license>TODO: License declaration</license>
 
   <depend>rclpy</depend>
-  <depend>common_interfaces</depend>
-  <depend>python3-serial</depend>
-
-  <build_depend>black</build_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/anchor_pkg/setup.py

@@ -1,6 +1,4 @@
 from setuptools import find_packages, setup
-from os import path
-from glob import glob
 
 package_name = 'anchor_pkg'
 
@@ -11,8 +9,7 @@ setup(
     data_files=[
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
-        (path.join("share", package_name), ['package.xml']),
-        (path.join("share", package_name, "launch"), glob("launch/*"))
+        ('share/' + package_name, ['package.xml']),
     ],
     install_requires=['setuptools'],
     zip_safe=True,

src/arm_pkg/arm_pkg/arm_headless.py

@@ -42,7 +42,7 @@ class Headless(Node):
         # Depricated, kept temporarily for reference
         #self.subscriber = self.create_subscription(String, '/astra/arm/feedback', self.read_feedback, 10)
 
-        #self.debug_sub = self.create_subscription(String, '/arm/feedback/debug', self.read_feedback, 10)
+        self.debug_sub = self.create_subscription(String, '/arm/feedback/debug', self.read_feedback, 10)
 
 
         self.laser_status = 0
@@ -81,14 +81,14 @@ class Headless(Node):
             while rclpy.ok():
                 #Check the pico for updates
 
-                #self.read_feedback()
+                self.read_feedback()
                 if pygame.joystick.get_count() == 0: #if controller disconnected, wait for it to be reconnected
                     print(f"Gamepad disconnected: {self.gamepad.get_name()}")
                     
                     while pygame.joystick.get_count() == 0:
                         #self.send_controls() #depricated, kept for reference temporarily
                         self.send_manual()
-                        #self.read_feedback()
+                        self.read_feedback()
                     self.gamepad = pygame.joystick.Joystick(0)
                     self.gamepad.init() #re-initialized gamepad
                     print(f"Gamepad reconnected: {self.gamepad.get_name()}")
@@ -142,7 +142,7 @@ class Headless(Node):
                 input.axis0 = -1
 
             if self.gamepad.get_axis(0) > .15 or self.gamepad.get_axis(0) < -.15:
-                input.axis1 = round(self.gamepad.get_axis(0))
+                input.axis1 = -1 * round(self.gamepad.get_axis(0))
 
             if self.gamepad.get_axis(1) > .15 or self.gamepad.get_axis(1) < -.15:
                 input.axis2 = -1 * round(self.gamepad.get_axis(1))
@@ -268,6 +268,22 @@ class Headless(Node):
     #         pass
 
 
+            
+
+
+    def read_feedback(self, msg):
+        
+        # Create a string message object
+        #msg = String()
+
+        # Set message data
+        #msg.data = output
+
+        # Publish data
+        #self.publisher.publish(msg.data)
+        
+        print(f"[MCU] {msg.data}", end="")
+        #print(f"[Pico] Publishing: {msg}")
 
         
 

src/arm_pkg/arm_pkg/arm_node.py

@@ -1,6 +1,5 @@
 import rclpy
 from rclpy.node import Node
-from rclpy import qos
 import serial
 import sys
 import threading
@@ -14,34 +13,9 @@ from ros2_interfaces_pkg.msg import ArmIK
 from ros2_interfaces_pkg.msg import SocketFeedback
 from ros2_interfaces_pkg.msg import DigitFeedback
 
-
-# IK-Related imports
-import numpy as np
-import time, math, os
-from math import sin, cos, pi
-from ament_index_python.packages import get_package_share_directory
-# from ikpy.chain import Chain
-# from ikpy.link import OriginLink, URDFLink
-# #import pygame as pyg
-# from scipy.spatial.transform import Rotation as R
-
-from . import astra_arm
-
-# control_qos = qos.QoSProfile(
-#     history=qos.QoSHistoryPolicy.KEEP_LAST,
-#     depth=1,
-#     reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
-#     durability=qos.QoSDurabilityPolicy.VOLATILE,
-#     deadline=1000,
-#     lifespan=500,
-#     liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
-#     liveliness_lease_duration=5000
-# )
-
 serial_pub = None
 thread = None
 
-
 class SerialRelay(Node):
     def __init__(self):
         # Initialize node
@@ -56,20 +30,17 @@ class SerialRelay(Node):
         self.debug_pub = self.create_publisher(String, '/arm/feedback/debug', 10)
         self.socket_pub = self.create_publisher(SocketFeedback, '/arm/feedback/socket', 10)
         self.digit_pub = self.create_publisher(DigitFeedback, '/arm/feedback/digit', 10)
-        self.feedback_timer = self.create_timer(0.25, self.publish_feedback)
+        self.feedback_timer = self.create_timer(1.0, self.publish_feedback)
 
         # Create subscribers
-        self.ik_sub = self.create_subscription(ArmIK, '/arm/control/ik', self.send_ik, 10)
+        self.ik_sub = self.create_subscription(ArmIK, '/arm/control/ik', self.send_ik, 10) 
         self.man_sub = self.create_subscription(ArmManual, '/arm/control/manual', self.send_manual, 10)
 
-        self.ik_debug = self.create_publisher(String, '/arm/debug/ik', 10)
-
         # Topics used in anchor mode
         if self.launch_mode == 'anchor':
             self.anchor_sub = self.create_subscription(String, '/anchor/arm/feedback', self.anchor_feedback, 10)
             self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10)
 
-        self.arm = astra_arm.Arm('arm12.urdf')
         self.arm_feedback = SocketFeedback()
         self.digit_feedback = DigitFeedback()
 
@@ -85,7 +56,7 @@ class SerialRelay(Node):
                         ser = serial.Serial(port, 115200, timeout=1)
                         #print(f"Checking port {port}...")
                         ser.write(b"ping\n")
-                        response = ser.read_until("\n")  # type: ignore
+                        response = ser.read_until("\n")
 
                         # if pong is in response, then we are talking with the MCU
                         if b"pong" in response:
@@ -153,106 +124,48 @@ class SerialRelay(Node):
             pass
 
     def send_ik(self, msg):
-        # Convert Vector3 to a NumPy array
-        input_raw = np.array([-msg.movement_vector.x, msg.movement_vector.y, msg.movement_vector.z])  # Convert input to a NumPy array
-        # decrease input vector by 90%
-        input_raw = input_raw * 0.2
+        pass
 
-        if input_raw[0] == 0.0 and input_raw[1] == 0.0 and input_raw[2] == 0.0:
-            self.get_logger().info("No input, stopping arm.")
-            command = "can_relay_tovic,arm,39,0,0,0,0\n"
-            self.send_cmd(command)
-            return
-
-        # Debug output
-        tempMsg = String()
-        tempMsg.data = "From IK Control Got Vector: " + str(input_raw)
-        #self.debug_pub.publish(tempMsg)
-
-        # Target position is current position + input vector
-        current_position = self.arm.get_position_vector()
-        target_position = current_position + input_raw
-
-
-        #Print for IK DEBUG
-        tempMsg = String()
-        # tempMsg.data = "Current Position: " + str(current_position) + "\nInput Vector" + str(input_raw) + "\nTarget Position: " + str(target_position) + "\nAngles: " + str(self.arm.current_angles)
-        tempMsg.data = "Current Angles: " + str(math.degrees(self.arm.current_angles[2])) + ", " + str(math.degrees(self.arm.current_angles[4])) + ", " + str(math.degrees(self.arm.current_angles[6]))
-        self.ik_debug.publish(tempMsg)
-        self.get_logger().info(f"[IK] {tempMsg.data}")
-
-        # Debug output for current position
-        #tempMsg.data = "Current Position: " + str(current_position)
-        #self.debug_pub.publish(tempMsg)
-
-        # Debug output for target position
-        #tempMsg.data = "Target Position: " + str(target_position)
-        #self.debug_pub.publish(tempMsg)
-
-        # Perform IK with the target position
-        if self.arm.perform_ik(target_position, self.get_logger()):
-            # Send command to control
-            
-            #command = "can_relay_tovic,arm,32," + ",".join(map(str, self.arm.ik_angles[:4])) + "\n"
-            #self.send_cmd(command)
-            self.get_logger().info(f"IK Success: {target_position}")
-            self.get_logger().info(f"IK Angles: [{str(round(math.degrees(self.arm.ik_angles[2]), 2))}, {str(round(math.degrees(self.arm.ik_angles[4]), 2))}, {str(round(math.degrees(self.arm.ik_angles[6]), 2))}]")
-
-            # tempMsg = String()
-            # tempMsg.data = "IK Success: " + str(target_position)
-            # #self.debug_pub.publish(tempMsg)
-            # tempMsg.data = "Sending: " + str(command)
-            #self.debug_pub.publish(tempMsg)
-
-            # Send the IK angles to the MCU
-            command = "can_relay_tovic,arm,32," + f"{math.degrees(self.arm.ik_angles[0])*10},{math.degrees(self.arm.ik_angles[2])*10},{math.degrees(self.arm.ik_angles[4])*10},{math.degrees(self.arm.ik_angles[6])*10}" + "\n"
-            # self.send_cmd(command)
-
-            # Manual control for Wrist/Effector
-            command += "can_relay_tovic,digit,35," + str(msg.effector_roll) + "\n"
-            
-            command += "can_relay_tovic,digit,36,0," + str(msg.effector_yaw) + "\n"
-
-            command += "can_relay_tovic,digit,26," + str(msg.gripper) + "\n"
-
-            command += "can_relay_tovic,digit,28," + str(msg.laser) + "\n"
-
-            self.send_cmd(command)
-        else:
-            self.get_logger().info("IK Fail")
-            self.get_logger().info(f"IK Angles: [{str(math.degrees(self.arm.ik_angles[2]))}, {str(math.degrees(self.arm.ik_angles[4]))}, {str(math.degrees(self.arm.ik_angles[6]))}]")
-            # tempMsg = String()
-            # tempMsg.data = "IK Fail"
-            #self.debug_pub.publish(tempMsg)
-
-
-    def send_manual(self, msg: ArmManual):
+    def send_manual(self, msg):
         axis0 = msg.axis0
         axis1 = -1 * msg.axis1
         axis2 = msg.axis2
         axis3 = msg.axis3
 
         #Send controls for arm
-        command = "can_relay_tovic,arm,18," + str(int(msg.brake)) + "\n"
-        command += "can_relay_tovic,arm,39," + str(axis0) + "," + str(axis1) + "," + str(axis2) + "," + str(axis3) + "\n"
+        command = "can_relay_tovic,arm,39," + str(axis0) + "," + str(axis1) + "," + str(axis2) + "," + str(axis3) + "\n"
+        #self.send_cmd(command)
         
         #Send controls for end effector
-
-        # command += "can_relay_tovic,digit,35," + str(msg.effector_roll) + "\n"
-        # command += "can_relay_tovic,digit,36,0," + str(msg.effector_yaw) + "\n"
-        command += "can_relay_tovic,digit,39," + str(msg.effector_yaw) + "," + str(msg.effector_roll) + "\n"
+        command += "can_relay_tovic,digit,35," + str(msg.effector_roll) + "\n"
+        #self.send_cmd(command)
+        
+        command += "can_relay_tovic,digit,36,0," + str(msg.effector_yaw) + "\n"
+        #self.send_cmd(command)
 
         command += "can_relay_tovic,digit,26," + str(msg.gripper) + "\n"
+        #self.send_cmd(command)
 
         command += "can_relay_tovic,digit,28," + str(msg.laser) + "\n"
-
-        command += "can_relay_tovic,digit,34," + str(msg.linear_actuator) + "\n"
-
         self.send_cmd(command)
+        
+        
+        
+        #print(f"[Wrote] {command}", end="")
+
+    #Not yet finished, needs embedded implementation for new commands
+        # ef_roll = msg.effector_roll
+        # ef_yaw = msg.effector_yaw
+        # gripper = msg.gripper
+        # actuator = msg.linear_actuator
+        # laser = msg.laser
+        # #Send controls for digit
+
+        # command = "can_relay_tovic,digit," + str(ef_roll) + "," + str(ef_yaw) + "," + str(gripper) + "," + str(actuator) + "," + str(laser) + "\n"
 
         return
     
-    def send_cmd(self, msg: str):
+    def send_cmd(self, msg):
         if self.launch_mode == 'anchor': #if in anchor mode, send to anchor node to relay
             output = String()
             output.data = msg
@@ -261,7 +174,7 @@ class SerialRelay(Node):
             self.get_logger().info(f"[Arm to MCU] {msg}")
             self.ser.write(bytes(msg, "utf8"))
 
-    def anchor_feedback(self, msg: String):
+    def anchor_feedback(self, msg):
         output = msg.data
         if output.startswith("can_relay_fromvic,arm,55"):
             #pass
@@ -270,6 +183,7 @@ class SerialRelay(Node):
             #pass
             self.updateBusVoltage(output)
         elif output.startswith("can_relay_fromvic,arm,53"):
+            #pass
             self.updateMotorFeedback(output)
         elif output.startswith("can_relay_fromvic,digit,54"):
             parts = msg.data.split(",")
@@ -284,7 +198,6 @@ class SerialRelay(Node):
             parts = msg.data.split(",")
             if len(parts) >= 4:
                 self.digit_feedback.wrist_angle = float(parts[3])
-                # self.digit_feedback.wrist_roll = float(parts[4])
         else:
             return
 
@@ -292,17 +205,17 @@ class SerialRelay(Node):
         self.socket_pub.publish(self.arm_feedback)
         self.digit_pub.publish(self.digit_feedback)
 
-    def updateAngleFeedback(self, msg: str):
-        # Angle feedbacks,
-        # split the msg.data by commas
-        parts = msg.split(",")
+    def updateAngleFeedback(self, msg: String):
+                # Angle feedbacks,
+        #split the msg.data by commas
+        parts = msg.data.split(",")
         
         if len(parts) >= 7:
             # Extract the angles from the string
             angles_in = parts[3:7]
             # Convert the angles to floats divide by 10.0
             angles = [float(angle) / 10.0 for angle in angles_in]
-            # angles[0] = 0.0 #override axis0 to zero
+            #angles[0] = 0.0 #override axis0 to zero
             #
             #
             #THIS NEEDS TO BE REMOVED LATER
@@ -314,7 +227,7 @@ class SerialRelay(Node):
             #
             #
             # # Update the arm's current angles
-            self.arm.update_angles(angles)
+            #self.arm.update_angles(angles)
             self.arm_feedback.axis0_angle = angles[0]
             self.arm_feedback.axis1_angle = angles[1]
             self.arm_feedback.axis2_angle = angles[2]
@@ -327,9 +240,10 @@ class SerialRelay(Node):
         else:
             self.get_logger().info("Invalid angle feedback input format")
 
-    def updateBusVoltage(self, msg: str):
+
+    def updateBusVoltage(self, msg: String):
         # Bus Voltage feedbacks
-        parts = msg.split(",")
+        parts = msg.data.split(",")
         if len(parts) >= 7:
             # Extract the voltage from the string
             voltages_in = parts[3:7]
@@ -340,29 +254,25 @@ class SerialRelay(Node):
             self.arm_feedback.voltage_3 = float(voltages_in[3]) / 100.0
         else:
             self.get_logger().info("Invalid voltage feedback input format")
-    
-    def updateMotorFeedback(self, msg: str):
-        parts = str(msg.strip()).split(",")
-        motorId = round(float(parts[3]))
-        temp = float(parts[4]) / 10.0
-        voltage = float(parts[5]) / 10.0
-        current = float(parts[6]) / 10.0
-        if motorId == 1:
-            self.arm_feedback.axis1_temp = temp
-            self.arm_feedback.axis1_voltage = voltage
-            self.arm_feedback.axis1_current = current
-        elif motorId == 2:
-            self.arm_feedback.axis2_temp = temp
-            self.arm_feedback.axis2_voltage = voltage
-            self.arm_feedback.axis2_current = current
-        elif motorId == 3:
-            self.arm_feedback.axis3_temp = temp
-            self.arm_feedback.axis3_voltage = voltage
-            self.arm_feedback.axis3_current = current
-        elif motorId == 4:
-            self.arm_feedback.axis0_temp = temp
-            self.arm_feedback.axis0_voltage = voltage
-            self.arm_feedback.axis0_current = current
+
+
+    def updateMotorFeedback(self, msg):
+        # Motor voltage/current/temperature feedback
+        return
+        # parts = msg.data.split(",")
+        # if len(parts) >= 7:
+        #     # Extract the voltage/current/temperature from the string
+        #     values_in = parts[3:7]
+        #     # Convert the voltages to floats
+        #     for i in range(4):
+        #         #update arm_feedback's axisX_temp for each axis0_temp, axis1_temp, etc...
+        #         pass
+
+        #         # self.arm_feedback.updateJointVoltages(i, float(values_in[i]) / 10.0)
+        #         # self.arm_feedback.updateJointCurrents(i, float(values_in[i]) / 10.0)
+        #         # self.arm_feedback.updateJointTemperatures(i, float(values_in[i]) / 10.0)
+        # else:
+        #     self.get_logger().info("Invalid motor feedback input format")
 
 
     @staticmethod
@@ -372,11 +282,8 @@ class SerialRelay(Node):
 
     def cleanup(self):
         print("Cleaning up...")
-        try:
-            if self.ser.is_open:
-                self.ser.close()
-        except Exception as e:
-            exit(0)
+        if self.ser.is_open:
+            self.ser.close()
 
 def myexcepthook(type, value, tb):
     print("Uncaught exception:", type, value)
@@ -392,6 +299,6 @@ def main(args=None):
     serial_pub.run()
 
 if __name__ == '__main__':
-    #signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0))  # Catch Ctrl+Z and exit cleanly
+    signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0))  # Catch Ctrl+Z and exit cleanly
     signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0))  # Catch termination signals and exit cleanly
     main()

src/arm_pkg/arm_pkg/astra_arm.py

@@ -1,145 +0,0 @@
-import rclpy
-from rclpy.node import Node
-import numpy as np
-import time, math, os
-from math import sin, cos, pi
-from ament_index_python.packages import get_package_share_directory
-from ikpy.chain import Chain
-from ikpy.link import OriginLink, URDFLink
-#import pygame as pyg
-from scipy.spatial.transform import Rotation as R
-from geometry_msgs.msg import Vector3  
-
-
-# Misc
-degree = pi / 180.0
-
-
-def convert_angles(angles):
-    # Converts angles to the format used for the urdf (contains some dummy joints)
-    return [0.0, math.radians(angles[0]), math.radians(angles[1]), 0.0, math.radians(angles[2]), 0.0, math.radians(angles[3]), 0.0, math.radians(angles[4]), math.radians(angles[5]), 0.0]
-
-
-class Arm:
-    def __init__(self, urdf_name):
-        self.ik_tolerance = 1e-1 #Tolerance (in meters) to determine if solution is valid
-        # URDF file path
-        self.urdf = os.path.join(get_package_share_directory('arm_pkg'), 'urdf', urdf_name)
-        # IKpy Chain        
-        self.chain = Chain.from_urdf_file(self.urdf)
-
-
-        # Arrays for joint states
-        # Some links in the URDF are static (non-joints), these will remain zero for IK
-        # Indexes: Fixed_base, Ax_0, Ax_1, seg1, Ax_2, seg2, ax_3, seg3, continuous, wrist, Effector
-        self.zero_angles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
-        self.current_angles = self.zero_angles
-        self.last_angles = self.zero_angles
-        self.ik_angles = self.zero_angles
-
-        self.current_position: list[float] = []
-        self.target_position = [0.0, 0.0, 0.0] 
-        self.target_orientation: list = [] # Effector orientation desired at target position. 
-                                # Generally orientation for the effector is modified manually by the operator. 
-        
-        # Might not need, copied over from state_publisher.py in ik_test
-        #self.step = 0.03 # Max movement increment
-
-
-    def perform_ik(self, target_position, logger):
-        self.target_position = target_position
-        # Update the target orientation to the current orientation
-        self.update_orientation()
-        # print(f"[IK FOR] Target Position: {self.target_position}")
-        try:
-            # print(f"[TRY] Current Angles: {self.current_angles}")
-            # print(f"[TRY] Target Position: {self.target_position}")
-            # print(f"[TRY] Target Orientation: {self.target_orientation}")
-            self.ik_angles = self.chain.inverse_kinematics(
-                target_position=self.target_position,
-                target_orientation=self.target_orientation,
-                initial_position=self.current_angles,
-                orientation_mode="all"
-            )
-            # Check if the solution is within the tolerance
-            fk_matrix = self.chain.forward_kinematics(self.ik_angles)
-
-            fk_position = fk_matrix[:3, 3]  # type: ignore
-            
-            # print(f"[TRY] FK Position for Solution: {fk_position}")
-
-            error = np.linalg.norm(target_position - fk_position)
-            if error > self.ik_tolerance:
-                logger.info(f"No VALID IK Solution within tolerance. Error: {error}")
-                return False
-            else:
-                logger.info(f"IK Solution Found. Error: {error}")
-                return True
-        except Exception as e:
-            logger.info(f"IK failed for exception: {e}")
-            return False
-    
-    # # Given the FK_Matix for the arm's current pose, update the orientation array
-    # def update_orientation(self, fk_matrix):
-    #     self.target_orientation = fk_matrix[:3, :3]
-    #     return
-    
-    # def update_joints(self, ax_0, ax_1, ax_2, ax_3, wrist):
-    #     self.current_angles = [0.0, 0.0, ax_0, ax_1, ax_2, ax_3, wrist, 0.0]
-    #     return
-
-    # Get current orientation of the end effector and update target_orientation
-    def update_orientation(self):
-
-        # FK matrix for arm's current pose
-        fk_matrix = self.chain.forward_kinematics(self.current_angles)
-        
-        # Update target_orientation to the effector's current orientation
-        self.target_orientation = fk_matrix[:3, :3]  # type: ignore
-
-    # Update current angles to those provided
-    # Resetting last_angles to the new angles
-    #
-    # Use: First call, or when angles are changed manually.
-    def reset_angles(self, angles):
-        # Update angles to the new angles
-        self.current_angles = convert_angles(angles)
-        self.last_angles = self.current_angles
-
-    # Update current angles to those provided
-    # Maintain previous angles in last_angles
-    #
-    # Use: Repeated calls during IK operation
-    def update_angles(self, angles):
-        # Update angles to the new angles
-        self.last_angles = self.current_angles
-        self.current_angles = convert_angles(angles)
-    
-    # Get current X,Y,Z position of end effector
-    def get_position(self):
-        # FK matrix for arm's current pose
-        fk_matrix = self.chain.forward_kinematics(self.current_angles)
-        
-        # Get the position of the end effector from the FK matrix
-        position = fk_matrix[:3, 3]  # type: ignore
-        
-        return position
-
-    # Get current X,Y,Z position of end effector
-    def get_position_vector(self):
-        # FK matrix for arm's current pose
-        fk_matrix = self.chain.forward_kinematics(self.current_angles)
-        
-        # Get the position of the end effector from the FK matrix
-        position = fk_matrix[:3, 3]  # type: ignore
-
-        # Return position as a NumPy array
-        return np.array(position)
-
-    
-    def update_position(self):
-        # FK matrix for arm's current pose
-        fk_matrix = self.chain.forward_kinematics(self.current_angles)
-        
-        # Get the position of the end effector from the FK matrix and update current pos
-        self.current_position = fk_matrix[:3, 3]  # type: ignore

src/arm_pkg/package.xml

@@ -5,14 +5,10 @@
   <version>1.0.0</version>
   <description>Core arm package which handles ROS2 commnuication.</description>
   <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
-  <license>AGPL-3.0-only</license>
+  <license>All Rights Reserved</license>
 
   <depend>rclpy</depend>
-  <depend>common_interfaces</depend>
-  <depend>python3-numpy</depend>
   <depend>ros2_interfaces_pkg</depend>
-  <!-- TODO: remove after refactored out -->
-  <exec_depend>python3-ikpy-pip</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/arm_pkg/setup.py

@@ -1,6 +1,4 @@
 from setuptools import find_packages, setup
-import os
-from glob import glob
 
 package_name = 'arm_pkg'
 
@@ -12,8 +10,6 @@ setup(
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
-        (os.path.join('share', package_name, 'launch'), glob('launch/*')),
-  	    (os.path.join('share', package_name, 'urdf'), glob('urdf/*')),
     ],
     install_requires=['setuptools'],
     zip_safe=True,

src/arm_pkg/urdf/arm11.urdf

@@ -1,239 +0,0 @@
-<?xml version="1.0" ?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from omni_description/robots/omnipointer_arm_only.urdf.xacro | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
-<robot name="omnipointer" xmlns:xacro="http://ros.org/wiki/xacro">
-  <!--MX64trntbl + MX106:  ? + 0.153 -->
-  <!-- singleaxismount + 2.5girder + singleaxismount + base + MX106: 0.007370876 + 0.0226796 + 0.007370876 + ? + 0.153 -->
-  <!-- frame106 + singleaxismount + adjustgirder + singleaxismount + base + MX28: 0.0907185 + 0.00737088 + 0.110563 + 0.00737088 + ? + 0.077 -->
-  <!-- frame28 + singleaxismount + 5girder + singleaxismount + base + MX28: 0.0907185 + 0.00737088 + 0.0368544 + 0.00737088 + ? + 0.077 -->
-  <!-- frame28 + singleaxismount + 2.5girder + singleaxismount + tip: 0.0907185 + 0.00737088 + 0.0226796 + 0.00737088 + ? -->
-  <material name="omni/Blue">
-    <color rgba="0 0 0.8 1"/>
-  </material>
-  <material name="omni/Red">
-    <color rgba="1 0 0 1"/>
-  </material>
-  <material name="omni/Green">
-    <color rgba="0 1 0 1"/>
-  </material>
-  <material name="omni/Yellow">
-    <color rgba="1 1 0 1"/>
-  </material>
-  <material name="omni/LightGrey">
-    <color rgba="0.6 0.6 0.6 1"/>
-  </material>
-  <material name="omni/DarkGrey">
-    <color rgba="0.4 0.4 0.4 1"/>
-  </material>
-  <!-- Now we can start using the macros xacro:included above to define the actual omnipointer -->
-  <!-- The first use of a macro.  This one was defined in youbot_base/base.urdf.xacro above.
-       A macro like this will expand to a set of link and joint definitions, and to additional
-       Gazebo-related extensions (sensor plugins, etc).  The macro takes an argument, name,
-       that equals "base", and uses it to generate names for its component links and joints
-       (e.g., base_link).  The xacro:included origin block is also an argument to the macro.  By convention,
-       the origin block defines where the component is w.r.t its parent (in this case the parent
-       is the world frame). For more, see http://www.ros.org/wiki/xacro -->
-  <!-- foot for arm-->
-  <link name="base_link">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="10.0"/>
-      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
-    </inertial>
-  </link>
-  <!-- joint between base_link and arm_0_link -->
-  <joint name="arm_joint_0" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <parent link="base_link"/>
-    <child link="arm_link_0"/>
-  </joint>
-  <link name="arm_link_0">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.02725"/>
-      <geometry>
-        <box size="0.1143 0.1143 0.0545"/>
-      </geometry>
-      <material name="omni/LightGrey"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.02725"/>
-      <geometry>
-        <box size="0.1143 0.1143 0.0545"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.02725"/>
-      <mass value="0.2"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.000267245666667" ixy="0" ixz="0" iyy="0.000267245666667" iyz="0" izz="0.000435483"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_1" type="revolute">
-    <parent link="arm_link_0"/>
-    <child link="arm_link_1"/>
-    <dynamics damping="3.0" friction="0.3"/>
-    <limit effort="30.0" lower="-3.1415926535" upper="3.1415926535" velocity="5.0"/>
-    <origin rpy="0 0 0" xyz="0 0 0.0545"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-  <link name="arm_link_1">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.075"/>
-      <geometry>
-        <box size="0.0402 0.05 0.15"/>
-      </geometry>
-      <material name="omni/Blue"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.075"/>
-      <geometry>
-        <box size="0.0402 0.05 0.15"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.075"/>
-      <mass value="0.190421352"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.000279744834534" ixy="0" ixz="0" iyy="0.000265717763008" iyz="0" izz="6.53151584738e-05"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_2" type="revolute">
-    <parent link="arm_link_1"/>
-    <child link="arm_link_2"/>
-    <dynamics damping="3.0" friction="0.3"/>
-    <limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
-    <origin rpy="0 0 0" xyz="0 0 0.15"/>
-    <axis xyz="0 1 0"/>
-  </joint>
-  <link name="arm_link_2">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.2355"/>
-      <geometry>
-        <box size="0.0356 0.05 0.471"/>
-      </geometry>
-      <material name="omni/Red"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.2355"/>
-      <geometry>
-        <box size="0.0356 0.05 0.471"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.2355"/>
-      <mass value="0.29302326"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.00251484771035" ixy="0" ixz="0" iyy="0.00248474836108" iyz="0" izz="9.19936757328e-05"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_3" type="revolute">
-    <parent link="arm_link_2"/>
-    <child link="arm_link_3"/>
-    <dynamics damping="3.0" friction="0.3"/>
-    <limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
-    <origin rpy="0 0 0" xyz="0 0 0.471"/>
-    <axis xyz="0 1 0"/>
-  </joint>
-  <link name="arm_link_3">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.1885"/>
-      <geometry>
-        <box size="0.0356 0.05 0.377"/>
-      </geometry>
-      <material name="omni/Yellow"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.1885"/>
-      <geometry>
-        <box size="0.0356 0.05 0.377"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.1885"/>
-      <mass value="0.21931466"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.000791433503053" ixy="0" ixz="0" iyy="0.000768905501178" iyz="0" izz="6.88531064581e-05"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_4" type="revolute">
-    <parent link="arm_link_3"/>
-    <child link="arm_link_4"/>
-    <dynamics damping="3.0" friction="0.3"/>
-    <limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
-    <origin rpy="0 0 0" xyz="0 0 0.377"/>
-    <axis xyz="0 1 0"/>
-  </joint>
-  <link name="arm_link_4">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.066"/>
-      <geometry>
-        <box size="0.0356 0.05 0.132"/>
-      </geometry>
-      <material name="omni/Green"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.066"/>
-      <geometry>
-        <box size="0.0356 0.05 0.132"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.066"/>
-      <mass value="0.15813986"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.00037242266488" ixy="0" ixz="0" iyy="0.000356178538461" iyz="0" izz="4.96474819141e-05"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_5" type="revolute">
-    <parent link="arm_link_4"/>
-    <child link="arm_link_5"/>
-    <dynamics damping="3.0" friction="0.3"/>
-    <limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
-    <origin rpy="0 0 0" xyz="0 0 0.132"/>
-    <axis xyz="1 0 0"/>
-  </joint>
-  <link name="arm_link_5">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.124"/>
-      <geometry>
-        <box size="0.0356 0.05 0.248"/>
-      </geometry>
-      <material name="omni/Blue"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.124"/>
-      <geometry>
-        <box size="0.0356 0.05 0.248"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.124"/>
-      <mass value="0.15813986"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.00037242266488" ixy="0" ixz="0" iyy="0.000356178538461" iyz="0" izz="4.96474819141e-05"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_6" type="fixed">
-    <parent link="arm_link_5"/>
-    <child link="arm_link_6"/>
-    <origin rpy="0 0 0" xyz="0 0 0.248"/>
-  </joint>
-  <link name="arm_link_6">
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="1e-12"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="1e-12" ixy="0" ixz="0" iyy="1e-12" iyz="0" izz="1e-12"/>
-    </inertial>
-  </link>
-  <!-- END OF ARM LINKS/JOINTS -->
-</robot>

src/arm_pkg/urdf/arm12.urdf

@@ -1,307 +0,0 @@
-<robot name="robot">
-  <link name="base_footprint"></link>
-  <joint name="base_joint" type="fixed">
-    <parent link="base_footprint" />
-    <child link="base_link" />
-    <origin xyz="0 0 0.0004048057655643422" rpy="0 0 0" />
-  </joint>
-  <link name="base_link">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <box size="0.5 0.5 0.01" />
-      </geometry>
-      <material name="base_link-material">
-        <color rgba="0 0.6038273388475408 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <box size="0.5 0.5 0.01" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.17" ixy="0" ixz="0" iyy="0.17" iyz="0" izz="0.05" />
-    </inertial>
-  </link>
-  <joint name="Axis_0_Joint" type="revolute">
-    <parent link="base_link" />
-    <child link="Axis_0" />
-    <origin xyz="0 0 0.035" rpy="0 0 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-2.5" upper="2.5" velocity="0.5"/>  </joint>
-  <link name="Axis_0">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.15" length="0.059" />
-      </geometry>
-      <material name="Axis_0-material">
-        <color rgba="0.3515325994898463 0.4735314961384573 0.9301108583738498 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.15" length="0.059" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_1_Joint" type="revolute">
-    <parent link="Axis_0" />
-    <child link="Axis_1" />
-    <origin xyz="0 0 0.11189588647115647" rpy="1.5707963267948963 0 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-1.7" upper="1.7" velocity="0.5"/>  </joint>
-  <link name="Axis_1">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.082" length="0.1" />
-      </geometry>
-      <material name="Axis_1-material">
-        <color rgba="0.14702726648767014 0.14126329113044458 0.7304607400847158 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.082" length="0.1" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_1_to_Segment_1" type="fixed">
-    <parent link="Axis_1" />
-    <child link="Segment_1" />
-    <origin xyz="0 0.2350831500270899 0" rpy="-1.5707963267948963 0 0" />
-  </joint>
-  <link name="Segment_1">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.473" />
-      </geometry>
-      <material name="Segment_1-material">
-        <color rgba="0.09084171117479915 0.3231432091022285 0.1844749944900301 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.473" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_2_Joint" type="revolute">
-    <parent link="Segment_1" />
-    <child link="Axis_2" />
-    <origin xyz="0 -5.219894517229704e-17 0.2637568842473722" rpy="1.5707963267948963 0 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-2.7" upper="2.7" velocity="0.5"/>  </joint>
-  <link name="Axis_2">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.055" length="0.1" />
-      </geometry>
-      <material name="Axis_2-material">
-        <color rgba="0.14702726648767014 0.14126329113044458 0.7304607400847158 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.055" length="0.1" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_2_to_Segment_2" type="fixed">
-    <parent link="Axis_2" />
-    <child link="Segment_2" />
-    <origin xyz="0 0.19535682173790003 0" rpy="-1.5707963267948963 0 0" />
-  </joint>
-  <link name="Segment_2">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.393" />
-      </geometry>
-      <material name="Segment_2-material">
-        <color rgba="0.09084171117479915 0.3231432091022285 0.1844749944900301 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.393" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_3_Joint" type="revolute">
-    <parent link="Segment_2" />
-    <child link="Axis_3" />
-    <origin xyz="0 -4.337792830220178e-17 0.199625776257357" rpy="1.5707963267948963 0 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-1.6" upper="1.6" velocity="0.5"/>  </joint>
-  <link name="Axis_3">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.05" length="0.1" />
-      </geometry>
-      <material name="Axis_3-material">
-        <color rgba="0.14702726648767014 0.14126329113044458 0.7304607400847158 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.05" length="0.1" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_3_to_Segment_3" type="fixed">
-    <parent link="Axis_3" />
-    <child link="Segment_3" />
-    <origin xyz="0 0.06725724726912972 0" rpy="-1.5707963267948963 0 0" />
-  </joint>
-  <link name="Segment_3">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.135" />
-      </geometry>
-      <material name="Segment_3-material">
-        <color rgba="0.09084171117479915 0.3231432091022285 0.1844749944900301 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.135" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Wrist_Joint" type="revolute">
-    <parent link="Segment_3" />
-    <child link="Axis_4" />
-    <origin xyz="0 0 0.0655808825338593" rpy="0 1.5707963267948966 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-1.6" upper="1.6" velocity="0.5"/>  </joint>
-  <link name="Axis_4">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.03" length="0.075" />
-      </geometry>
-      <material name="Axis_4-material">
-        <color rgba="0.23455058215026167 0.9301108583738498 0.21952619971859377 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.03" length="0.075" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Continuous_Joint" type="revolute">
-    <parent link="Axis_4" />
-    <child link="Axis_4_C" />
-    <origin xyz="0.0009533507860803557 0 0" rpy="0 -1.5707963267948966 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-3.14" upper="3.14" velocity="0.5"/>
-  </joint>
-  <link name="Axis_4_C">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.02" length="0.01" />
-      </geometry>
-      <material name="Axis_4_C-material">
-        <color rgba="0.006048833020386069 0.407240211891531 0.15592646369776456 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.02" length="0.01" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_4_C_to_Effector" type="fixed">
-    <parent link="Axis_4_C" />
-    <child link="Effector" />
-    <origin xyz="0 0 0.06478774571448076" rpy="0 0 0" />
-  </joint>
-  <link name="Effector">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <box size="0.05 0.01 0.135" />
-      </geometry>
-      <material name="Effector-material">
-        <color rgba="0.2746773120495699 0.01680737574872402 0.5711248294565854 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <box size="0.01 0.01 0.135" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
-    </inertial>
-  </link>
-</robot>

src/bio_pkg/bio_pkg/bio_node.py

@@ -3,7 +3,6 @@ from rclpy.node import Node
 import serial
 import sys
 import threading
-import os
 import glob
 import time
 import atexit
@@ -145,11 +144,10 @@ class SerialRelay(Node):
 
         # LSS (SCYTHE)
         command = "can_relay_tovic,citadel,24," + str(msg.bio_arm) + "\n"
-        #self.send_cmd(command)
-        
+        self.send_cmd(command)
         # Vibration Motor
-        command += "can_relay_tovic,citadel,26," + str(msg.vibration_motor) + "\n"
-        #self.send_cmd(command)
+        command = "can_relay_tovic,citadel,26," + str(msg.vibration_motor) + "\n"
+        self.send_cmd(command)
         
 
         # FAERIE Control Commands 
@@ -158,15 +156,16 @@ class SerialRelay(Node):
         # To be reviewed before use#
 
         # Laser
-        command += "can_relay_tovic,digit,28," + str(msg.laser) + "\n"
-        #self.send_cmd(command)
+        command = "can_relay_tovic,digit,28," + str(msg.laser) + "\n"
+        self.send_cmd(command)
 
         # Drill (SCABBARD)
-        command += f"can_relay_tovic,digit,19,{msg.drill:.2f}\n"
-        #self.send_cmd(command)
+        command = f"can_relay_tovic,digit,19,{msg.drill:.2f}\n"
+        print(msg.drill)
+        self.send_cmd(command)
 
         # Bio linear actuator
-        command += "can_relay_tovic,digit,42," + str(msg.drill_arm) + "\n"
+        command = "can_relay_tovic,digit,42," + str(msg.drill_arm) + "\n"
         self.send_cmd(command)
 
 
@@ -183,7 +182,7 @@ class SerialRelay(Node):
     def anchor_feedback(self, msg: String):
         output = msg.data
         parts = str(output.strip()).split(",")
-        #self.get_logger().info(f"[Bio Anchor] {msg.data}")
+        self.get_logger().info(f"[Bio Anchor] {msg.data}")
 
         if output.startswith("can_relay_fromvic,citadel,54"):  # bat, 12, 5, Voltage readings * 100
             self.bio_feedback.bat_voltage = float(parts[3]) / 100.0
@@ -203,11 +202,8 @@ class SerialRelay(Node):
 
     def cleanup(self):
         print("Cleaning up...")
-        try:
-            if self.ser.is_open:
-                self.ser.close()
-        except Exception as e:
-            exit(0)
+        if self.ser.is_open:
+            self.ser.close()
 
 def myexcepthook(type, value, tb):
     print("Uncaught exception:", type, value)
@@ -223,6 +219,6 @@ def main(args=None):
     serial_pub.run()
 
 if __name__ == '__main__':
-    #signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0))  # Catch Ctrl+Z and exit cleanly
+    signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0))  # Catch Ctrl+Z and exit cleanly
     signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0))  # Catch termination signals and exit cleanly
     main()

src/bio_pkg/package.xml

@@ -5,10 +5,9 @@
   <version>0.0.0</version>
   <description>TODO: Package description</description>
   <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
-  <license>AGPL-3.0-only</license>
+  <license>TODO: License declaration</license>
 
   <depend>rclpy</depend>
-  <depend>common_interfaces</depend>
   <depend>ros2_interfaces_pkg</depend>
 
   <test_depend>ament_copyright</test_depend>

src/core_pkg/core_pkg/core_headless.py

@@ -20,86 +20,113 @@ os.environ["SDL_VIDEODRIVER"] = "dummy"  # Prevents pygame from trying to open a
 os.environ["SDL_AUDIODRIVER"] = "dummy"  # Force pygame to use a dummy audio driver before pygame.init()
 
 
-max_speed = 90 #Max speed as a duty cycle percentage (1-100)
+max_speed = 75 #Max speed as a duty cycle percentage (1-100)
 
 class Headless(Node):
     def __init__(self):
-        # Initialize pygame first
+        # Initalize node with name
+        super().__init__("core_headless")
+
+        self.create_timer(0.20, self.send_controls)
+
+
+        # Create a publisher to publish any output the pico sends
+        self.publisher = self.create_publisher(CoreControl, '/core/control', 10) 
+
+
+        self.lastMsg = String() #Used to ignore sending controls repeatedly when they do not change
+
         pygame.init()
+
+        # Initialize the gamepad module
         pygame.joystick.init()
-        
-        # Wait for a gamepad to be connected
-        self.gamepad = None
-        print("Waiting for gamepad connection...")
-        while pygame.joystick.get_count() == 0:
-            # Process any pygame events to keep it responsive
-            for event in pygame.event.get():
-                if event.type == pygame.QUIT:
-                    pygame.quit()
-                    sys.exit(0)
-            time.sleep(1.0)  # Check every second
-            print("No gamepad found. Waiting...")
-        
-        # Initialize the gamepad
+
+        # Check if any gamepad is connected
+        if pygame.joystick.get_count() == 0:
+            print("No gamepad found.")
+            pygame.quit()
+            exit()
+        for event in pygame.event.get():
+            if event.type == pygame.QUIT:
+                pygame.quit()
+                exit()
+
+        # Initialize the first gamepad, print name to terminal
         self.gamepad = pygame.joystick.Joystick(0)
         self.gamepad.init()
         print(f'Gamepad Found: {self.gamepad.get_name()}')
-        
-        # Now initialize the ROS2 node
-        super().__init__("core_headless")
-        self.create_timer(0.15, self.send_controls)
-        self.publisher = self.create_publisher(CoreControl, '/core/control', 10)
-        self.lastMsg = String()  # Used to ignore sending controls repeatedly when they do not change
+        #
+        #
+
 
     def run(self):
         # This thread makes all the update processes run in the background
         thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
         thread.start()
         
+
         try:
             while rclpy.ok():
+                #Check the pico for updates
                 self.send_controls()
-                time.sleep(0.1)  # Small delay to avoid CPU hogging
+
+                if pygame.joystick.get_count() == 0: #if controller disconnected, wait for it to be reconnected
+                    print(f"Gamepad disconnected: {self.gamepad.get_name()}")
+                    
+                    while pygame.joystick.get_count() == 0:
+                        self.send_controls()
+                    self.gamepad = pygame.joystick.Joystick(0)
+                    self.gamepad.init() #re-initialized gamepad
+                    print(f"Gamepad reconnected: {self.gamepad.get_name()}")
+                    
+
         except KeyboardInterrupt:
             sys.exit(0)
+        
 
     def send_controls(self):
+        
         for event in pygame.event.get():
             if event.type == pygame.QUIT:
                 pygame.quit()
-                sys.exit(0)
-        
-        # Check if controller is still connected
-        if pygame.joystick.get_count() == 0:
-            print("Gamepad disconnected. Exiting...")
-            # Send one last zero control message
-            input = CoreControl()
-            input.left_stick = 0
-            input.right_stick = 0
-            input.max_speed = 0
-            self.publisher.publish(input)
-            self.get_logger().info("Final stop command sent. Shutting down.")
-            # Clean up
-            pygame.quit()
-            sys.exit(0)
-            
+                exit()
         input = CoreControl()
+
         input.max_speed = max_speed
-        input.right_stick = -1 * round(self.gamepad.get_axis(4), 2)  # right y-axis
+
+        input.right_stick = round(self.gamepad.get_axis(4),2)#right y-axis
         if self.gamepad.get_axis(5) > 0:
             input.left_stick = input.right_stick
         else:
-            input.left_stick = -1 * round(self.gamepad.get_axis(1), 2)  # left y-axis
+            input.left_stick = round(self.gamepad.get_axis(1),2)#lext y-axis
 
-        output = f'L: {input.left_stick}, R: {input.right_stick}, M: {max_speed}'
-        self.get_logger().info(f"[Ctrl] {output}")
-        self.publisher.publish(input)
+
+
+        if pygame.joystick.get_count() != 0:
+            output = f'L: {input.left_stick}, R: {input.right_stick}, M: {max_speed}' #stop the rover if there is no controller connected
+            self.get_logger().info(f"[Ctrl] {output}")
+            self.publisher.publish(input)
+        else:
+            input.left_stick = 0
+            input.right_stick = 0
+            input.max_speed = 0
+            self.get_logger().info(f"[Ctrl] Stopping. No Gamepad Connected. ")
+            self.publisher.publish(input)
+
+
+        
 
 def main(args=None):
     rclpy.init(args=args)
+
     node = Headless()
+    
     rclpy.spin(node)
     rclpy.shutdown()
 
+    #tb_bs = BaseStation()
+    #node.run()
+
+
 if __name__ == '__main__':
     main()

src/core_pkg/core_pkg/core_node.py

@@ -1,529 +1,268 @@
-import rclpy
-from rclpy.node import Node
-from rclpy import qos
-from rclpy.duration import Duration
-from std_srvs.srv import Empty
-
-import signal
-import time
-import atexit
-
-import serial
-import os
-import sys
-import threading
-import glob
-from scipy.spatial.transform import Rotation
-from math import copysign, pi
-
-from std_msgs.msg import String, Header
-from sensor_msgs.msg import Imu, NavSatFix, NavSatStatus, JointState
-from geometry_msgs.msg import TwistStamped, Twist
-from ros2_interfaces_pkg.msg import CoreControl, CoreFeedback, RevMotorState
-from ros2_interfaces_pkg.msg import VicCAN, NewCoreFeedback, Barometer, CoreCtrlState
-
-
-serial_pub = None
-thread = None
-
-CORE_WHEELBASE = 0.836  # meters
-CORE_WHEEL_RADIUS = 0.171  # meters
-CORE_GEAR_RATIO = 100.0  # Clucky: 100:1, Testbed: 64:1
-
-control_qos = qos.QoSProfile(
-    history=qos.QoSHistoryPolicy.KEEP_LAST,
-    depth=2,
-    reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
-    durability=qos.QoSDurabilityPolicy.VOLATILE,
-    deadline=Duration(seconds=1),
-    lifespan=Duration(nanoseconds=500_000_000),  # 500ms
-    liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
-    liveliness_lease_duration=Duration(seconds=5)
-)
-
-# Used to verify the length of an incoming VicCAN feedback message
-# Key is VicCAN command_id, value is expected length of data list
-viccan_msg_len_dict = {
-    48: 1,
-    49: 1,
-    50: 2,
-    51: 4,
-    52: 4,
-    53: 4,
-    54: 4,
-    56: 4,  # really 3, but viccan
-    58: 4   # ditto
-}
-
-
-class SerialRelay(Node):
-    def __init__(self):
-        # Initalize node with name
-        super().__init__("core_node")
-
-        # Launch mode -- anchor vs core
-        self.declare_parameter('launch_mode', 'core')
-        self.launch_mode = self.get_parameter('launch_mode').value
-        self.get_logger().info(f"Core launch_mode is: {self.launch_mode}")
-
-
-        ##################################################
-        # Topics
-
-        # Anchor
-        if self.launch_mode == 'anchor':
-            self.anchor_fromvic_sub_ = self.create_subscription(VicCAN, '/anchor/from_vic/core', self.relay_fromvic, 20)
-            self.anchor_tovic_pub_ = self.create_publisher(VicCAN, '/anchor/to_vic/relay', 20)
-
-            self.anchor_sub = self.create_subscription(String, '/anchor/core/feedback', self.anchor_feedback, 10)
-            self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10)
-
-        # Control
-
-        # autonomy twist -- m/s and rad/s -- for autonomy, in particular Nav2
-        self.cmd_vel_sub_ = self.create_subscription(TwistStamped, '/cmd_vel', self.cmd_vel_callback, 1)
-        # manual twist -- [-1, 1] rather than real units
-        self.twist_man_sub_ = self.create_subscription(Twist, '/core/twist', self.twist_man_callback, qos_profile=control_qos)
-        # manual flags -- brake mode and max duty cycle
-        self.control_state_sub_ = self.create_subscription(CoreCtrlState, '/core/control/state', self.control_state_callback, qos_profile=control_qos)
-        self.twist_max_duty = 0.5  # max duty cycle for twist commands (0.0 - 1.0); walking speed is 0.5
-
-        # Feedback
-
-        # Consolidated and organized core feedback
-        self.feedback_new_pub_ = self.create_publisher(NewCoreFeedback, '/core/feedback_new', qos_profile=qos.qos_profile_sensor_data)
-        self.feedback_new_state = NewCoreFeedback()
-        self.feedback_new_state.fl_motor.id = 1
-        self.feedback_new_state.bl_motor.id = 2
-        self.feedback_new_state.fr_motor.id = 3
-        self.feedback_new_state.br_motor.id = 4
-        self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback)  # TODO: not sure about this
-        # Joint states for topic-based controller
-        self.joint_state_pub_ = self.create_publisher(JointState, '/core/joint_states', qos_profile=qos.qos_profile_sensor_data)
-        # IMU (embedded BNO-055)
-        self.imu_pub_ = self.create_publisher(Imu, '/core/imu', qos_profile=qos.qos_profile_sensor_data)
-        self.imu_state = Imu()
-        self.imu_state.header.frame_id = "core_bno055"
-        # GPS (embedded u-blox M9N)
-        self.gps_pub_ = self.create_publisher(NavSatFix, '/gps/fix', qos_profile=qos.qos_profile_sensor_data)
-        self.gps_state = NavSatFix()
-        self.gps_state.header.frame_id = "core_gps_antenna"
-        self.gps_state.status.service = NavSatStatus.SERVICE_GPS
-        self.gps_state.status.status = NavSatStatus.STATUS_NO_FIX
-        self.gps_state.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
-        # Barometer (embedded BMP-388)
-        self.baro_pub_ = self.create_publisher(Barometer, '/core/baro', qos_profile=qos.qos_profile_sensor_data)
-        self.baro_state = Barometer()
-        self.baro_state.header.frame_id = "core_bmp388"
-
-        # Old
-        
-        # /core/control
-        self.control_sub = self.create_subscription(CoreControl, '/core/control', self.send_controls, 10)  # old control method -- left_stick, right_stick, max_speed, brake, and some other random autonomy stuff
-        # /core/feedback
-        self.feedback_pub = self.create_publisher(CoreFeedback, '/core/feedback', 10)
-        self.core_feedback = CoreFeedback()
-        # Debug
-        self.debug_pub = self.create_publisher(String, '/core/debug', 10)
-        self.ping_service = self.create_service(Empty, '/astra/core/ping', self.ping_callback)
-
-
-        ##################################################
-        # Find microcontroller (Non-anchor only)
-
-        # Core (non-anchor) specific
-        if self.launch_mode == 'core':
-            # Loop through all serial devices on the computer to check for the MCU
-            self.port = None
-            ports = SerialRelay.list_serial_ports()
-            for i in range(2):
-                for port in ports:
-                    try:
-                        # connect and send a ping command
-                        ser = serial.Serial(port, 115200, timeout=1)
-                        #(f"Checking port {port}...")
-                        ser.write(b"ping\n")
-                        response = ser.read_until("\n")  # type: ignore
-
-                        # if pong is in response, then we are talking with the MCU
-                        if b"pong" in response:
-                            self.port = port
-                            self.get_logger().info(f"Found MCU at {self.port}!")
-                            self.get_logger().info(f"Enabling Relay Mode")
-                            ser.write(b"can_relay_mode,on\n")
-                            break
-                    except:
-                        pass
-                if self.port is not None:
-                    break
-        
-            if self.port is None:
-                self.get_logger().info("Unable to find MCU...")
-                time.sleep(1)
-                sys.exit(1)
-        
-            self.ser = serial.Serial(self.port, 115200)
-            atexit.register(self.cleanup)
-        # end __init__()
-
-
-    def run(self):
-        # This thread makes all the update processes run in the background
-        global thread
-        thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
-        thread.start()
-
-        try:
-            while rclpy.ok():
-                if self.launch_mode == 'core':
-                    self.read_MCU() # Check the MCU for updates
-        except KeyboardInterrupt:
-            sys.exit(0)
-
-    def read_MCU(self):  # NON-ANCHOR SPECIFIC
-        try:
-            output = str(self.ser.readline(), "utf8")
-            
-            if output:
-                # All output over debug temporarily
-                print(f"[MCU] {output}")
-                msg = String()
-                msg.data = output
-                self.debug_pub.publish(msg)
-                return
-        except serial.SerialException as e:
-            print(f"SerialException: {e}")
-            print("Closing serial port.")
-            if self.ser.is_open:
-                self.ser.close()
-            sys.exit(1)
-        except TypeError as e:
-            print(f"TypeError: {e}")
-            print("Closing serial port.")
-            if self.ser.is_open:
-                self.ser.close()
-            sys.exit(1)
-        except Exception as e:
-            print(f"Exception: {e}")
-            print("Closing serial port.")
-            if self.ser.is_open:
-                self.ser.close()
-            sys.exit(1)
-
-    def scale_duty(self, value: float, max_speed: float):
-        leftMin = -1
-        leftMax = 1
-        rightMin = -max_speed/100.0
-        rightMax = max_speed/100.0
-
-        # Figure out how 'wide' each range is
-        leftSpan = leftMax - leftMin
-        rightSpan = rightMax - rightMin
-
-        # Convert the left range into a 0-1 range (float)
-        valueScaled = float(value - leftMin) / float(leftSpan)
-
-        # Convert the 0-1 range into a value in the right range.
-        return str(rightMin + (valueScaled * rightSpan))
-
-    def send_controls(self, msg: CoreControl):
-        if(msg.turn_to_enable):
-            command = "can_relay_tovic,core,41," + str(msg.turn_to) + ',' + str(msg.turn_to_timeout) + '\n' 
-        else:
-            command = "can_relay_tovic,core,19," + self.scale_duty(msg.left_stick, msg.max_speed) + ',' + self.scale_duty(msg.right_stick, msg.max_speed) + '\n'
-        self.send_cmd(command)
-
-        # Brake mode
-        command = "can_relay_tovic,core,18," + str(int(msg.brake)) + '\n'
-        self.send_cmd(command)
-
-        #print(f"[Sys] Relaying: {command}")
-
-    def cmd_vel_callback(self, msg: TwistStamped):
-        linear = msg.twist.linear.x
-        angular = -msg.twist.angular.z
-
-        vel_left_rads = (linear - (angular * CORE_WHEELBASE / 2)) / CORE_WHEEL_RADIUS
-        vel_right_rads = (linear + (angular * CORE_WHEELBASE / 2)) / CORE_WHEEL_RADIUS
-
-        vel_left_rpm = round((vel_left_rads * 60) / (2 * 3.14159)) * CORE_GEAR_RATIO
-        vel_right_rpm = round((vel_right_rads * 60) / (2 * 3.14159)) * CORE_GEAR_RATIO
-        
-        self.send_viccan(20, [vel_left_rpm, vel_right_rpm])
-
-    def twist_man_callback(self, msg: Twist):
-        linear = msg.linear.x  # [-1 1] for forward/back from left stick y
-        angular = msg.angular.z  # [-1 1] for left/right from right stick x
-
-        if (linear < 0):  # reverse turning direction when going backwards (WIP)
-            angular *= -1
-
-        if abs(linear) > 1 or abs(angular) > 1:
-            # if speed is greater than 1, then there is a problem
-            # make it look like a problem and don't just run away lmao
-            linear = copysign(0.25, linear)  # 0.25 duty cycle in direction of control (hopefully slow)
-            angular = copysign(0.25, angular)
-
-        duty_left = linear - angular
-        duty_right = linear + angular
-        scale = max(1, abs(duty_left), abs(duty_right))
-        duty_left /= scale
-        duty_right /= scale
-
-        # Apply max duty cycle
-        # Joysticks provide values [-1, 1] rather than real units
-        duty_left = map_range(duty_left, -1, 1, -self.twist_max_duty, self.twist_max_duty)
-        duty_right = map_range(duty_right, -1, 1, -self.twist_max_duty, self.twist_max_duty)
-
-        self.send_viccan(19, [duty_left, duty_right])
-
-    def control_state_callback(self, msg: CoreCtrlState):
-        # Brake mode
-        self.send_viccan(18, [msg.brake_mode])
-
-        # Max duty cycle
-        self.twist_max_duty = msg.max_duty  # twist_man_callback will handle this
-
-    def send_cmd(self, msg: str):
-        if self.launch_mode == 'anchor':
-            #self.get_logger().info(f"[Core to Anchor Relay] {msg}")
-            output = String()#Convert to std_msg string
-            output.data = msg
-            self.anchor_pub.publish(output)
-        elif self.launch_mode == 'core':
-            self.get_logger().info(f"[Core to MCU] {msg}")
-            self.ser.write(bytes(msg, "utf8"))
-
-
-    def send_viccan(self, cmd_id: int, data: list[float]):
-        self.anchor_tovic_pub_.publish(VicCAN(
-            header=Header(stamp=self.get_clock().now().to_msg(), frame_id="to_vic"),
-            mcu_name="core",
-            command_id=cmd_id,
-            data=data
-        ))
-
-
-    def anchor_feedback(self, msg: String):
-        output = msg.data
-        parts = str(output.strip()).split(",")
-        # GNSS Latitude
-        if output.startswith("can_relay_fromvic,core,48"):
-            self.core_feedback.gps_lat = float(parts[3])
-        # GNSS Longitude
-        elif output.startswith("can_relay_fromvic,core,49"):
-            self.core_feedback.gps_long = float(parts[3])
-        # GNSS Satellite count and altitude
-        elif output.startswith("can_relay_fromvic,core,50"):
-            self.core_feedback.gps_sats = round(float(parts[3]))
-            self.core_feedback.gps_alt = round(float(parts[4]), 2)
-        # Gyro x, y, z, and imu calibration
-        elif output.startswith("can_relay_fromvic,core,51"):
-            self.core_feedback.bno_gyro.x = float(parts[3])
-            self.core_feedback.bno_gyro.y = float(parts[4])
-            self.core_feedback.bno_gyro.z = float(parts[5])
-            self.core_feedback.imu_calib = round(float(parts[6]))
-        # Accel x, y, z, heading
-        elif output.startswith("can_relay_fromvic,core,52"):
-            self.core_feedback.bno_accel.x = float(parts[3])
-            self.core_feedback.bno_accel.y = float(parts[4])
-            self.core_feedback.bno_accel.z = float(parts[5])
-            self.core_feedback.orientation = float(parts[6])
-        # REV Sparkmax feedback
-        elif output.startswith("can_relay_fromvic,core,53"):
-            motorId = round(float(parts[3]))
-            temp = float(parts[4]) / 10.0
-            voltage = float(parts[5]) / 10.0
-            current = float(parts[6]) / 10.0
-            if motorId == 1:
-                self.core_feedback.fl_temp = temp
-                self.core_feedback.fl_voltage = voltage
-                self.core_feedback.fl_current = current
-            elif motorId == 2:
-                self.core_feedback.bl_temp = temp
-                self.core_feedback.bl_voltage = voltage
-                self.core_feedback.bl_current = current
-            elif motorId == 3:
-                self.core_feedback.fr_temp = temp
-                self.core_feedback.fr_voltage = voltage
-                self.core_feedback.fr_current = current
-            elif motorId == 4:
-                self.core_feedback.br_temp = temp
-                self.core_feedback.br_voltage = voltage
-                self.core_feedback.br_current = current
-        # Voltages batt, 12, 5, 3, all * 100
-        elif output.startswith("can_relay_fromvic,core,54"):
-            self.core_feedback.bat_voltage = float(parts[3]) / 100.0
-            self.core_feedback.voltage_12 = float(parts[4]) / 100.0
-            self.core_feedback.voltage_5 = float(parts[5]) / 100.0
-            self.core_feedback.voltage_3 = float(parts[6]) / 100.0
-        # BMP temperature, altitude, pressure
-        elif output.startswith("can_relay_fromvic,core,56"):
-            self.core_feedback.bmp_temp = float(parts[3])
-            self.core_feedback.bmp_alt = float(parts[4])
-            self.core_feedback.bmp_pres = float(parts[5])
-        else:
-            return
-        self.feedback_new_state.header.stamp = self.get_clock().now().to_msg()
-        self.feedback_new_pub_.publish(self.feedback_new_state)
-        #self.get_logger().info(f"[Core Anchor] {msg}")
-
-    def relay_fromvic(self, msg: VicCAN):
-        # Assume that the message is coming from Core
-        # skill diff if not
-
-        # Check message len to prevent crashing on bad data
-        if msg.command_id in viccan_msg_len_dict:
-            expected_len = viccan_msg_len_dict[msg.command_id]
-            if len(msg.data) != expected_len:
-                self.get_logger().warning(f"Ignoring VicCAN message with id {msg.command_id} due to unexpected data length (expected {expected_len}, got {len(msg.data)})")
-                return
-
-        match msg.command_id:
-            # GNSS
-            case 48:  # GNSS Latitude
-                self.gps_state.latitude = float(msg.data[0])
-            case 49:  # GNSS Longitude
-                self.gps_state.longitude = float(msg.data[0])
-            case 50:  # GNSS Satellite count and altitude
-                self.gps_state.status.status = NavSatStatus.STATUS_FIX if int(msg.data[0]) >= 3 else NavSatStatus.STATUS_NO_FIX
-                self.gps_state.altitude = float(msg.data[1])
-                self.gps_state.header.stamp = msg.header.stamp
-                self.gps_pub_.publish(self.gps_state)
-            # IMU
-            case 51:  # Gyro x, y, z, and imu calibration
-                self.feedback_new_state.imu_calib = round(float(msg.data[3]))
-                self.imu_state.angular_velocity.x = float(msg.data[0])
-                self.imu_state.angular_velocity.y = float(msg.data[1])
-                self.imu_state.angular_velocity.z = float(msg.data[2])
-                self.imu_state.header.stamp = msg.header.stamp
-            case 52:  # Accel x, y, z, heading
-                self.imu_state.linear_acceleration.x = float(msg.data[0])
-                self.imu_state.linear_acceleration.y = float(msg.data[1])
-                self.imu_state.linear_acceleration.z = float(msg.data[2])
-                # Deal with quaternion
-                r = Rotation.from_euler('z', float(msg.data[3]), degrees=True)
-                q = r.as_quat()
-                self.imu_state.orientation.x = q[0]
-                self.imu_state.orientation.y = q[1]
-                self.imu_state.orientation.z = q[2]
-                self.imu_state.orientation.w = q[3]
-                self.imu_state.header.stamp = msg.header.stamp
-                self.imu_pub_.publish(self.imu_state)
-            # REV Motors
-            case 53:  # REV SPARK MAX feedback
-                motorId = round(float(msg.data[0]))
-                temp = float(msg.data[1]) / 10.0
-                voltage = float(msg.data[2]) / 10.0
-                current = float(msg.data[3]) / 10.0
-                motor: RevMotorState | None = None
-                match motorId:
-                    case 1:
-                        motor = self.feedback_new_state.fl_motor
-                    case 2:
-                        motor = self.feedback_new_state.bl_motor
-                    case 3:
-                        motor = self.feedback_new_state.fr_motor
-                    case 4:
-                        motor = self.feedback_new_state.br_motor
-                    case _:
-                        self.get_logger().warning(f"Ignoring REV motor feedback 53 with invalid motorId {motorId}")
-                        return
-
-                if motor:
-                    motor.temperature = temp
-                    motor.voltage = voltage
-                    motor.current = current
-                    motor.header.stamp = msg.header.stamp
-
-                self.feedback_new_pub_.publish(self.feedback_new_state)
-            # Board voltage
-            case 54:  # Voltages batt, 12, 5, 3, all * 100
-                self.feedback_new_state.board_voltage.vbatt = float(msg.data[0]) / 100.0
-                self.feedback_new_state.board_voltage.v12 = float(msg.data[1]) / 100.0
-                self.feedback_new_state.board_voltage.v5 = float(msg.data[2]) / 100.0
-                self.feedback_new_state.board_voltage.v3 = float(msg.data[3]) / 100.0
-            # Baro
-            case 56:  # BMP temperature, altitude, pressure
-                self.baro_state.temperature = float(msg.data[0])
-                self.baro_state.altitude = float(msg.data[1])
-                self.baro_state.pressure = float(msg.data[2])
-                self.baro_state.header.stamp = msg.header.stamp
-                self.baro_pub_.publish(self.baro_state)
-            # REV Motors (pos and vel)
-            case 58:  # REV position and velocity
-                motorId = round(float(msg.data[0]))
-                position = float(msg.data[1])
-                velocity = float(msg.data[2])
-                joint_state_msg = JointState()  # TODO: not sure if all motors should be in each message or not
-                joint_state_msg.position = [position * (2 * pi) / CORE_GEAR_RATIO]  # revolutions to radians
-                joint_state_msg.velocity = [velocity * (2 * pi / 60.0) / CORE_GEAR_RATIO]  # RPM to rad/s
-
-                motor: RevMotorState | None = None
-
-                match motorId:
-                    case 1:
-                        motor = self.feedback_new_state.fl_motor
-                        joint_state_msg.name = ["fl_motor_joint"]
-                    case 2:
-                        motor = self.feedback_new_state.bl_motor
-                        joint_state_msg.name = ["bl_motor_joint"]
-                    case 3:
-                        motor = self.feedback_new_state.fr_motor
-                        joint_state_msg.name = ["fr_motor_joint"]
-                    case 4:
-                        motor = self.feedback_new_state.br_motor
-                        joint_state_msg.name = ["br_motor_joint"]
-                    case _:
-                        self.get_logger().warning(f"Ignoring REV motor feedback 58 with invalid motorId {motorId}")
-                        return
-
-                joint_state_msg.header.stamp = msg.header.stamp
-                self.joint_state_pub_.publish(joint_state_msg)
-            case _:
-                return
-
-
-    def publish_feedback(self):
-        #self.get_logger().info(f"[Core] {self.core_feedback}")
-        self.feedback_pub.publish(self.core_feedback)
-
-    def ping_callback(self, request, response):
-        return response
-
-
-    @staticmethod
-    def list_serial_ports():
-        return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
-    
-    def cleanup(self):
-        print("Cleaning up before terminating...")
-        try:
-            if self.ser.is_open:
-                self.ser.close()
-        except Exception as e:
-            exit(0)
-
-
-def myexcepthook(type, value, tb):
-    print("Uncaught exception:", type, value)
-    if serial_pub:
-        serial_pub.cleanup()
-
-def map_range(value: float, in_min: float, in_max: float, out_min: float, out_max: float):
-    return (value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
-
-
-def main(args=None):
-   rclpy.init(args=args)
-   sys.excepthook = myexcepthook
-
-   global serial_pub
-
-   serial_pub = SerialRelay()
-   serial_pub.run()
-
-if __name__ == '__main__':
-    #signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0))  # Catch Ctrl+Z and exit cleanly
-    signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0))  # Catch termination signals and exit cleanly
-    main()
+import rclpy
+from rclpy.node import Node
+from std_srvs.srv import Empty
+
+import signal
+import time
+import atexit
+
+import serial
+import sys
+import threading
+import glob
+
+from std_msgs.msg import String
+from ros2_interfaces_pkg.msg import CoreFeedback
+from ros2_interfaces_pkg.msg import CoreControl
+
+serial_pub = None
+thread = None
+
+class SerialRelay(Node):
+    def __init__(self):
+        # Initalize node with name
+        super().__init__("core_node")#previously 'serial_publisher'
+
+        # Get launch mode parameter
+        self.declare_parameter('launch_mode', 'core')
+        self.launch_mode = self.get_parameter('launch_mode').value
+        self.get_logger().info(f"core launch_mode is: {self.launch_mode}")
+
+        # Create publishers 
+        self.debug_pub = self.create_publisher(String, '/core/debug', 10) 
+        self.feedback_pub = self.create_publisher(CoreFeedback, '/core/feedback', 10)
+        # Create a subscriber 
+        self.control_sub = self.create_subscription(CoreControl, '/core/control', self.send_controls, 10)
+        
+        # Create a publisher for telemetry
+        self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback)
+
+        # Create a service server for pinging the rover
+        self.ping_service = self.create_service(Empty, '/astra/core/ping', self.ping_callback)
+
+        if self.launch_mode == 'anchor':
+            self.anchor_sub = self.create_subscription(String, '/anchor/core/feedback', self.anchor_feedback, 10)
+            self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10)
+
+        self.core_feedback = CoreFeedback()
+
+
+        if self.launch_mode == 'core':
+            # Loop through all serial devices on the computer to check for the MCU
+            self.port = None
+            ports = SerialRelay.list_serial_ports()
+            for i in range(2):
+                for port in ports:
+                    try:
+                        # connect and send a ping command
+                        ser = serial.Serial(port, 115200, timeout=1)
+                        #(f"Checking port {port}...")
+                        ser.write(b"ping\n")
+                        response = ser.read_until("\n")
+
+                        # if pong is in response, then we are talking with the MCU
+                        if b"pong" in response:
+                            self.port = port
+                            self.get_logger().info(f"Found MCU at {self.port}!")
+                            self.get_logger().info(f"Enabling Relay Mode")
+                            ser.write(b"can_relay_mode,on\n")
+                            break
+                    except:
+                        pass
+                if self.port is not None:
+                    break
+        
+            if self.port is None:
+                self.get_logger().info("Unable to find MCU...")
+                time.sleep(1)
+                sys.exit(1)
+        
+            self.ser = serial.Serial(self.port, 115200)
+            atexit.register(self.cleanup)
+
+
+    def run(self):
+        # This thread makes all the update processes run in the background
+        global thread
+        thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
+        thread.start()
+        
+    
+        try:
+            while rclpy.ok():
+                if self.launch_mode == 'core':
+                    self.read_MCU() # Check the MCU for updates
+        except KeyboardInterrupt:
+            sys.exit(0)
+
+    def read_MCU(self):
+        try:
+            output = str(self.ser.readline(), "utf8")
+            
+            if output:
+                # All output over debug temporarily
+                print(f"[MCU] {output}")
+                msg = String()
+                msg.data = output
+                self.debug_pub.publish(msg)
+                return
+                # Temporary
+
+                # packet = output.strip().split(',')
+                
+                # if len(packet) >= 2 and packet[0] == "core" and packet[1] == "telemetry":
+                #     feedback = CoreFeedback()
+                #     feedback.gpslat = float(packet[2])
+                #     feedback.gpslon = float(packet[3])
+                #     feedback.gpssat = float(packet[4])
+                #     feedback.bnogyr.x = float(packet[5])
+                #     feedback.bnogyr.y = float(packet[6])
+                #     feedback.bnogyr.z = float(packet[7])
+                #     feedback.bnoacc.x = float(packet[8])
+                #     feedback.bnoacc.y = float(packet[9])
+                #     feedback.bnoacc.z = float(packet[10])
+                #     feedback.orient = float(packet[11])
+                #     feedback.bmptemp = float(packet[12])
+                #     feedback.bmppres = float(packet[13])
+                #     feedback.bmpalt = float(packet[14])
+
+                #     self.telemetry_publisher.publish(feedback)
+                # else:
+                #     # print(f"[MCU] {output}", end="")
+                #     # msg = String()
+                #     # msg.data = output
+                #     # self.debug_pub.publish(msg)
+                #     return
+        except serial.SerialException as e:
+            print(f"SerialException: {e}")
+            print("Closing serial port.")
+            if self.ser.is_open:
+                self.ser.close()
+            self.exit(1)
+        except TypeError as e:
+            print(f"TypeError: {e}")
+            print("Closing serial port.")
+            if self.ser.is_open:
+                self.ser.close()
+            self.exit(1)
+        except Exception as e:
+            print(f"Exception: {e}")
+            print("Closing serial port.")
+            if self.ser.is_open:
+                self.ser.close()
+            self.exit(1)
+                
+    def scale_duty(self, value: float, max_speed: float):
+        leftMin = -1
+        leftMax = 1
+        rightMin = -max_speed/100.0
+        rightMax = max_speed/100.0
+
+
+        # Figure out how 'wide' each range is
+        leftSpan = leftMax - leftMin
+        rightSpan = rightMax - rightMin
+
+        # Convert the left range into a 0-1 range (float)
+        valueScaled = float(value - leftMin) / float(leftSpan)
+
+        # Convert the 0-1 range into a value in the right range.
+        return str(rightMin + (valueScaled * rightSpan))
+
+    def send_controls(self, msg):
+        #can_relay_tovic,core,19, left_stick, right_stick 
+        if(msg.turn_to_enable):
+            command = "can_relay_tovic,core,41," + str(msg.turn_to) + ',' + str(msg.turn_to_timeout) + '\n' 
+        else:
+            command = "can_relay_tovic,core,19," + self.scale_duty(msg.left_stick, msg.max_speed) + ',' + self.scale_duty(msg.right_stick, msg.max_speed) + '\n'
+        self.send_cmd(command)
+
+        # Brake mode
+        command = "can_relay_tovic,core,18," + str(int(msg.brake)) + '\n'
+        self.send_cmd(command)
+
+        #print(f"[Sys] Relaying: {command}")
+    def send_cmd(self, msg):
+        if self.launch_mode == 'anchor':
+            #self.get_logger().info(f"[Core to Anchor Relay] {msg}")
+            output = String()#Convert to std_msg string
+            output.data = msg
+            self.anchor_pub.publish(output)
+        elif self.launch_mode == 'core':
+            self.get_logger().info(f"[Core to MCU] {msg}")
+            self.ser.write(bytes(msg, "utf8"))
+
+    def anchor_feedback(self, msg: String):
+        output = msg.data
+        parts = str(output.strip()).split(",")
+        #self.get_logger().info(f"[ANCHOR FEEDBACK parts] {parts}")
+        if output.startswith("can_relay_fromvic,core,48"): #GNSS Lattitude
+            self.core_feedback.gps_lat = float(parts[3])
+        elif output.startswith("can_relay_fromvic,core,49"):#GNSS Longitude
+            self.core_feedback.gps_long = float(parts[3])
+        elif output.startswith("can_relay_fromvic,core,50"):#GNSS Satellite Count
+            self.core_feedback.gps_sats = round(float(parts[3]))
+        elif output.startswith("can_relay_fromvic,core,51"):#Gyro x,y,z
+            self.core_feedback.bno_gyro.x = float(parts[3])
+            self.core_feedback.bno_gyro.y = float(parts[4])
+            self.core_feedback.bno_gyro.z = float(parts[5])
+            self.core_feedback.imu_calib = round(float(parts[6]))
+        elif output.startswith("can_relay_fromvic,core,52"):#Accel x,y,z, heading *10
+            self.core_feedback.bno_accel.x = float(parts[3])
+            self.core_feedback.bno_accel.y = float(parts[4])
+            self.core_feedback.bno_accel.z = float(parts[5])
+            self.core_feedback.orientation = float(parts[6]) / 10.0
+        elif output.startswith("can_relay_fromvic,core,53"):#Rev motor feedback
+            pass
+            #self.updateMotorFeedback(output)
+        elif output.startswith("can_relay_fromvic,core,54"):#bat, 12, 5, 3, Voltage readings * 100
+            self.core_feedback.bat_voltage = float(parts[3]) / 100.0
+            self.core_feedback.voltage_12 = float(parts[4]) / 100.0
+            self.core_feedback.voltage_5 = float(parts[5]) / 100.0
+            self.core_feedback.voltage_3 = float(parts[6]) / 100.0
+        elif output.startswith("can_relay_fromvic,core,56"):#BMP Temp, Altitude, Pressure
+            self.core_feedback.bmp_temp = float(parts[3])
+            self.core_feedback.bmp_alt = float(parts[4])
+            self.core_feedback.bmp_pres = float(parts[5])
+        else:
+            return
+        #self.get_logger().info(f"[Core Anchor] {msg}")
+
+    def publish_feedback(self):
+        #self.get_logger().info(f"[Core] {self.core_feedback}")
+        self.feedback_pub.publish(self.core_feedback)
+
+    def ping_callback(self, request, response):
+        return response
+
+
+    @staticmethod
+    def list_serial_ports():
+        return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
+    
+    def cleanup(self):
+        print("Cleaning up before terminating...")
+        if self.ser.is_open:
+            self.ser.close()
+
+def myexcepthook(type, value, tb):
+    print("Uncaught exception:", type, value)
+    if serial_pub:
+        serial_pub.cleanup()
+      
+
+
+def main(args=None):
+   rclpy.init(args=args)
+   sys.excepthook = myexcepthook
+
+   global serial_pub
+
+   serial_pub = SerialRelay()
+   serial_pub.run()
+
+if __name__ == '__main__':
+    signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0))  # Catch Ctrl+Z and exit cleanly
+    signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0))  # Catch termination signals and exit cleanly
+    main()
+

src/core_pkg/core_pkg/core_ptz.py

@@ -42,6 +42,13 @@ class PtzNode(Node):
         self.loop = None
         self.thread_pool = None
         
+        # Track current zoom level and zoom state
+        self.current_zoom_level = 1.0
+        self.max_zoom_level = 6.0  # A8 mini max zoom
+        self.zoom_step = 0.2       # Zoom step per command
+        self.zooming = 0           # Current zoom direction: -1=out, 0=stop, 1=in
+        self.zoom_timer = None
+        
         # Create publishers
         self.feedback_pub = self.create_publisher(PtzFeedback, '/ptz/feedback', 10)
         self.debug_pub = self.create_publisher(String, '/ptz/debug', 10)
@@ -130,7 +137,6 @@ class PtzNode(Node):
                     debug_str += str(data)
                 self.get_logger().debug(debug_str)
 
-
     def check_camera_connection(self):
         """Periodically check camera connection and attempt to reconnect if needed."""
         if not self.camera_connected and not self.shutdown_requested:
@@ -181,6 +187,10 @@ class PtzNode(Node):
             if msg.reset:
                 self.get_logger().info("Attempting to reset camera to center position")
                 await self.camera.send_attitude_angles_command(0.0, 0.0)
+                
+                # Also reset zoom to 1x when resetting camera
+                self.current_zoom_level = 1.0
+                await self.camera.send_absolute_zoom_command(1.0)
                 return
                 
             if msg.control_mode == 0:
@@ -201,10 +211,21 @@ class PtzNode(Node):
                 self.get_logger().debug(f"Attempting single axis: axis={axis.name}, angle={angle}")
                 await self.camera.send_single_axis_attitude_command(angle, axis)
 
-            elif msg.control_mode == 3: 
-                zoom_level = msg.zoom_level
-                self.get_logger().debug(f"Attempting absolute zoom: level={zoom_level}x")
-                await self.camera.send_absolute_zoom_command(zoom_level)
+            elif msg.control_mode == 3:
+                # Instead of absolute zoom, interpret zoom_level as zoom direction
+                zoom_direction = int(msg.zoom_level) if abs(msg.zoom_level) >= 0.5 else 0
+                zoom_direction = max(-1, min(1, zoom_direction))  # Restrict to -1, 0, 1
+                
+                if zoom_direction != self.zooming:
+                    self.zooming = zoom_direction
+                    self.get_logger().debug(f"Zoom direction changed to {zoom_direction}")
+                    
+                    if zoom_direction == 0:
+                        # Stop zooming
+                        self.get_logger().debug(f"Stopping zoom at level {self.current_zoom_level:.1f}x")
+                    else:
+                        # Start zooming in the specified direction
+                        await self.perform_zoom(zoom_direction)
                 
             if hasattr(msg, 'stream_type') and hasattr(msg, 'stream_freq'):
                 if msg.stream_type > 0 and msg.stream_freq >= 0: 
@@ -221,13 +242,52 @@ class PtzNode(Node):
         except RuntimeError as e: # Catch SDK's "not connected" errors
             self.get_logger().warning(f"SDK command failed (likely not connected): {e}")
             # self.camera_connected will be updated by health/connection checks
-            # self.feedback_msg.error_msg = f"Command failed: {str(e)}" # Already set by health check
-            # self.feedback_pub.publish(self.feedback_msg)
         except Exception as e:
             self.get_logger().error(f"Error processing control command: {e}")
             self.feedback_msg.error_msg = f"Control error: {str(e)}"
             self.feedback_pub.publish(self.feedback_msg) # Publish for other errors
 
+    async def perform_zoom(self, direction):
+        """Perform a zoom operation in the specified direction."""
+        if not self.camera or not self.camera.is_connected:
+            return
+            
+        if direction == 0:
+            return
+            
+        # Calculate new zoom level
+        new_zoom_level = self.current_zoom_level + (direction * self.zoom_step)
+        
+        # Clamp to valid range
+        new_zoom_level = max(1.0, min(self.max_zoom_level, new_zoom_level))
+        
+        # Skip if no change (already at min/max)
+        if new_zoom_level == self.current_zoom_level:
+            if direction > 0:
+                self.get_logger().debug(f"Already at maximum zoom level ({self.max_zoom_level:.1f}x)")
+            else:
+                self.get_logger().debug(f"Already at minimum zoom level (1.0x)")
+            return
+            
+        # Set the new zoom level
+        self.get_logger().debug(f"Zooming from {self.current_zoom_level:.1f}x to {new_zoom_level:.1f}x")
+        self.current_zoom_level = new_zoom_level
+        
+        try:
+            await self.camera.send_absolute_zoom_command(new_zoom_level)
+            
+            # If still zooming in same direction, schedule another zoom step
+            if self.zooming == direction:
+                # Schedule a new zoom step after a short delay if we're not at the limits
+                if (direction > 0 and new_zoom_level < self.max_zoom_level) or \
+                   (direction < 0 and new_zoom_level > 1.0):
+                    # Use create_timer for a non-blocking delay
+                    await asyncio.sleep(0.2)  # 200ms delay between zoom steps
+                    if self.zooming == direction:  # Check if direction hasn't changed
+                        await self.perform_zoom(direction)
+        except Exception as e:
+            self.get_logger().error(f"Error during zoom operation: {e}")
+
     def publish_debug(self, message_text):
         """Publish debug message."""
         msg = String()
@@ -250,7 +310,6 @@ class PtzNode(Node):
             self.get_logger().warning("Asyncio loop not running, cannot execute coroutine.")
             return None
 
-
     async def shutdown_node_async(self):
         """Perform clean shutdown of camera connection."""
         self.shutdown_requested = True

src/core_pkg/core_pkg/siyi_sdk/siyi.py

@@ -64,7 +64,7 @@ class CommandID(Enum):
     """
 
     ROTATION_CONTROL = 0x07
-    ABSOLUTE_ZOOM = 0x08
+    ABSOLUTE_ZOOM = 0x0C
     ATTITUDE_ANGLES = 0x0E
     SINGLE_AXIS_CONTROL = 0x41
     DATA_STREAM_REQUEST = 0x25

src/core_pkg/package.xml

@@ -5,12 +5,9 @@
   <version>1.0.0</version>
   <description>Core rover control package to handle command interpretation and embedded interfacing.</description>
   <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
-  <license>AGPL-3.0-only</license>
+  <license>All Rights Reserved</license>
 
   <depend>rclpy</depend>
-  <depend>common_interfaces</depend>
-  <depend>python3-scipy</depend>
-  <depend>python-crccheck-pip</depend>
   <depend>ros2_interfaces_pkg</depend>
 
   <test_depend>ament_copyright</test_depend>

src/headless_pkg/package.xml

@@ -1,23 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>headless_pkg</name>
-  <version>1.0.0</version>
-  <description>Headless rover control package to handle command interpretation and embedded interfacing.</description>
-  <maintainer email="ds0196@uah.edu">David Sharpe</maintainer>
-  <license>AGPL-3.0-only</license>
-
-  <depend>rclpy</depend>
-  <depend>common_interfaces</depend>
-  <depend>python3-pygame</depend>
-  <depend>ros2_interfaces_pkg</depend>
-
-  <test_depend>ament_copyright</test_depend>
-  <test_depend>ament_flake8</test_depend>
-  <test_depend>ament_pep257</test_depend>
-  <test_depend>python3-pytest</test_depend>
-
-  <export>
-    <build_type>ament_python</build_type>
-  </export>
-</package>

src/headless_pkg/setup.cfg

@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/headless_pkg
-[install]
-install_scripts=$base/lib/headless_pkg

src/headless_pkg/setup.py

@@ -1,24 +0,0 @@
-from setuptools import find_packages, setup
-
-package_name = 'headless_pkg'
-
-setup(
-    name=package_name,
-    version='1.0.0',
-    packages=find_packages(exclude=['test']),
-    data_files=[
-        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
-    ],
-    install_requires=['setuptools'],
-    zip_safe=True,
-    maintainer='David Sharpe',
-    maintainer_email='ds0196@uah.edu',
-    description='Headless rover control package to handle command interpretation and embedded interfacing.',
-    license='All Rights Reserved',
-    entry_points={
-        'console_scripts': [
-            "headless_full = src.headless_node:main",
-        ],
-    },
-)

src/headless_pkg/src/headless_node.py

@@ -1,300 +0,0 @@
-import rclpy
-from rclpy.node import Node
-from rclpy import qos
-from rclpy.duration import Duration
-
-import signal
-import time
-import atexit
-
-import os
-import sys
-import threading
-import glob
-from math import copysign
-
-from std_msgs.msg import String
-from geometry_msgs.msg import Twist
-from ros2_interfaces_pkg.msg import CoreControl, ArmManual, BioControl
-from ros2_interfaces_pkg.msg import CoreCtrlState
-
-import pygame
-
-os.environ["SDL_VIDEODRIVER"] = "dummy"  # Prevents pygame from trying to open a display
-os.environ["SDL_AUDIODRIVER"] = "dummy"  # Force pygame to use a dummy audio driver before pygame.init()
-
-
-CORE_STOP_MSG = CoreControl()  # All zeros by default
-CORE_STOP_TWIST_MSG = Twist()  # "
-ARM_STOP_MSG = ArmManual()     # "
-BIO_STOP_MSG = BioControl()    # "
-
-control_qos = qos.QoSProfile(
-    history=qos.QoSHistoryPolicy.KEEP_LAST,
-    depth=2,
-    reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
-    durability=qos.QoSDurabilityPolicy.VOLATILE,
-    deadline=Duration(seconds=1),
-    lifespan=Duration(nanoseconds=500_000_000),  # 500ms
-    liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
-    liveliness_lease_duration=Duration(seconds=5)
-)
-
-CORE_MODE = "twist"  # "twist" or "duty"
-
-
-class Headless(Node):
-    def __init__(self):
-        # Initialize pygame first
-        pygame.init()
-        pygame.joystick.init()
-        super().__init__("headless")
-
-        # Wait for anchor to start
-        pub_info = self.get_publishers_info_by_topic('/anchor/from_vic/debug')
-        while len(pub_info) == 0:
-            self.get_logger().info("Waiting for anchor to start...")
-            time.sleep(1.0)
-            pub_info = self.get_publishers_info_by_topic('/anchor/from_vic/debug')
-
-        # Wait for a gamepad to be connected
-        print("Waiting for gamepad connection...")
-        while pygame.joystick.get_count() == 0:
-            # Process any pygame events to keep it responsive
-            for event in pygame.event.get():
-                if event.type == pygame.QUIT:
-                    pygame.quit()
-                    sys.exit(0)
-            time.sleep(1.0)  # Check every second
-            print("No gamepad found. Waiting...")
-
-        # Initialize the gamepad
-        self.gamepad = pygame.joystick.Joystick(0)
-        self.gamepad.init()
-        print(f'Gamepad Found: {self.gamepad.get_name()}')
-
-        self.create_timer(0.15, self.send_controls)
-
-        self.core_publisher = self.create_publisher(CoreControl, '/core/control', 2)
-        self.arm_publisher = self.create_publisher(ArmManual, '/arm/control/manual', 2)
-        self.bio_publisher = self.create_publisher(BioControl, '/bio/control', 2)
-
-        self.core_twist_pub_ = self.create_publisher(Twist, '/core/twist', qos_profile=control_qos)
-        self.core_state_pub_ = self.create_publisher(CoreCtrlState, '/core/control/state', qos_profile=control_qos)
-
-
-        self.ctrl_mode = "core"  # Start in core mode
-        self.core_brake_mode = False
-        self.core_max_duty = 0.5  # Default max duty cycle (walking speed)
-
-        # Rumble when node is ready (returns False if rumble not supported)
-        self.gamepad.rumble(0.7, 0.8, 150)
-
-
-    def run(self):
-        # This thread makes all the update processes run in the background
-        thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
-        thread.start()
-        
-        try:
-            while rclpy.ok():
-                self.send_controls()
-                time.sleep(0.1)  # Small delay to avoid CPU hogging
-        except KeyboardInterrupt:
-            sys.exit(0)
-
-    def send_controls(self):
-        """ Read the gamepad state and publish control messages """
-        for event in pygame.event.get():
-            if event.type == pygame.QUIT:
-                pygame.quit()
-                sys.exit(0)
-        
-        # Check if controller is still connected
-        if pygame.joystick.get_count() == 0:
-            print("Gamepad disconnected. Exiting...")
-            # Send one last zero control message
-            self.core_publisher.publish(CORE_STOP_MSG)
-            self.arm_publisher.publish(ARM_STOP_MSG)
-            self.bio_publisher.publish(BIO_STOP_MSG)
-            self.get_logger().info("Final stop commands sent. Shutting down.")
-            # Clean up
-            pygame.quit()
-            sys.exit(0)
-
-
-        new_ctrl_mode = self.ctrl_mode  # if "" then inequality will always be true
-
-        # Check for control mode change
-        dpad_input = self.gamepad.get_hat(0)
-        if dpad_input[1] == 1:
-            new_ctrl_mode = "arm"
-        elif dpad_input[1] == -1:
-            new_ctrl_mode = "core"
-
-        if new_ctrl_mode != self.ctrl_mode:
-            self.gamepad.rumble(0.6, 0.7, 75)
-            self.ctrl_mode = new_ctrl_mode
-            self.get_logger().info(f"Switched to {self.ctrl_mode} control mode")
-
-
-        # CORE
-        if self.ctrl_mode == "core" and CORE_MODE == "duty":
-            input = CoreControl()
-            input.max_speed = 90
-
-            # Collect controller state
-            left_stick_y = deadzone(self.gamepad.get_axis(1))
-            right_stick_y = deadzone(self.gamepad.get_axis(4))
-            right_trigger = deadzone(self.gamepad.get_axis(5))
-
-
-            # Right wheels
-            input.right_stick = float(round(-1 * right_stick_y, 2))
-
-            # Left wheels
-            if right_trigger > 0:
-                input.left_stick = input.right_stick
-            else:
-                input.left_stick = float(round(-1 * left_stick_y, 2))
-
-
-            # Debug
-            output = f'L: {input.left_stick}, R: {input.right_stick}'
-            self.get_logger().info(f"[Ctrl] {output}")
-
-            self.core_publisher.publish(input)
-            self.arm_publisher.publish(ARM_STOP_MSG)
-            # self.bio_publisher.publish(BIO_STOP_MSG)
-        
-        elif self.ctrl_mode == "core" and CORE_MODE == "twist":
-            input = Twist()
-
-            # Collect controller state
-            left_stick_y = deadzone(self.gamepad.get_axis(1))
-            right_stick_x = deadzone(self.gamepad.get_axis(3))
-            button_a = self.gamepad.get_button(0)
-            left_bumper = self.gamepad.get_button(4)
-            right_bumper = self.gamepad.get_button(5)
-
-            # Forward/back and Turn
-            input.linear.x = -1.0 * left_stick_y
-            input.angular.z = -1.0 * copysign(right_stick_x ** 2, right_stick_x)  # Exponent for finer control (curve)
-
-            # Publish
-            self.core_twist_pub_.publish(input)
-            self.arm_publisher.publish(ARM_STOP_MSG)
-            # self.bio_publisher.publish(BIO_STOP_MSG)
-            self.get_logger().info(f"[Core Ctrl] Linear: {round(input.linear.x, 2)}, Angular: {round(input.angular.z, 2)}")
-
-            # Brake mode
-            new_brake_mode = button_a
-            # Max duty cycle
-            if left_bumper:
-                new_max_duty = 0.25
-            elif right_bumper:
-                new_max_duty = 0.9
-            else:
-                new_max_duty = 0.5
-
-            # Only publish if needed
-            if new_brake_mode != self.core_brake_mode or new_max_duty != self.core_max_duty:
-                self.core_brake_mode = new_brake_mode
-                self.core_max_duty = new_max_duty
-                state_msg = CoreCtrlState()
-                state_msg.brake_mode = bool(self.core_brake_mode)
-                state_msg.max_duty = float(self.core_max_duty)
-                self.core_state_pub_.publish(state_msg)
-                self.get_logger().info(f"[Core State] Brake: {self.core_brake_mode}, Max Duty: {self.core_max_duty}")
-
-
-        # ARM and BIO
-        if self.ctrl_mode == "arm":
-            arm_input = ArmManual()
-            
-            # Collect controller state
-            left_stick_x = deadzone(self.gamepad.get_axis(0))
-            left_stick_y = deadzone(self.gamepad.get_axis(1))
-            left_trigger = deadzone(self.gamepad.get_axis(2))
-            right_stick_x = deadzone(self.gamepad.get_axis(3))
-            right_stick_y = deadzone(self.gamepad.get_axis(4))
-            right_trigger = deadzone(self.gamepad.get_axis(5))
-            right_bumper = self.gamepad.get_button(5)
-            dpad_input = self.gamepad.get_hat(0)
-
-
-            # EF Grippers
-            if left_trigger > 0 and right_trigger > 0:
-                arm_input.gripper = 0
-            elif left_trigger > 0:
-                arm_input.gripper = -1
-            elif right_trigger > 0:
-                arm_input.gripper = 1
-
-            # Axis 0
-            if dpad_input[0] == 1:
-                arm_input.axis0 = 1
-            elif dpad_input[0] == -1:
-                arm_input.axis0 = -1
-
-
-            if right_bumper:  # Control end effector
-
-                # Effector yaw
-                if left_stick_x > 0:
-                    arm_input.effector_yaw = 1
-                elif left_stick_x < 0:
-                    arm_input.effector_yaw = -1
-
-                # Effector roll
-                if right_stick_x > 0:
-                    arm_input.effector_roll = 1
-                elif right_stick_x < 0:
-                    arm_input.effector_roll = -1
-
-            else:   # Control arm axis
-
-                # Axis 1
-                if abs(left_stick_x) > .15:
-                    arm_input.axis1 = round(left_stick_x)
-
-                # Axis 2
-                if abs(left_stick_y) > .15:
-                    arm_input.axis2 = -1 * round(left_stick_y)
-
-                # Axis 3
-                if abs(right_stick_y) > .15:
-                    arm_input.axis3 = -1 * round(right_stick_y)
-
-
-            # BIO
-            bio_input = BioControl(
-                bio_arm=int(left_stick_y * -100),
-                drill_arm=int(round(right_stick_y) * -100)
-            )
-
-            # Drill motor (FAERIE)
-            if deadzone(left_trigger) > 0 or deadzone(right_trigger) > 0:
-                bio_input.drill = int(30 * (right_trigger - left_trigger))  # Max duty cycle 30%
-
-            self.core_publisher.publish(CORE_STOP_MSG)
-            self.arm_publisher.publish(arm_input)
-            # self.bio_publisher.publish(bio_input)
-
-
-def deadzone(value: float, threshold=0.05) -> float:
-    """ Apply a deadzone to a joystick input so the motors don't sound angry """
-    if abs(value) < threshold:
-        return 0
-    return value
-
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = Headless()
-    rclpy.spin(node)
-    rclpy.shutdown()
-
-if __name__ == '__main__':
-    signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0))  # Catch termination signals and exit cleanly
-    main()

src/latency_tester/CMakeLists.txt

@@ -1,27 +0,0 @@
-cmake_minimum_required(VERSION 3.22)
-project(latency_tester)
-
-# Default to C++14
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
-endif()
-
-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)
-find_package(rclcpp REQUIRED)
-find_package(std_msgs REQUIRED)
-
-# Add embedded ping node
-add_executable(embedded_ping src/embedded_ping.cpp)
-ament_target_dependencies(embedded_ping rclcpp std_msgs)
-
-install(TARGETS
-  embedded_ping
-  DESTINATION lib/${PROJECT_NAME}
-)
-
-ament_package()

src/latency_tester/package.xml

@@ -1,20 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>latency_tester</name>
-  <version>1.0.0</version>
-  <description>Tests latency in ASTRA's comms.</description>
-  <maintainer email="ds0196@uah.edu">David</maintainer>
-  <license>AGPL-3.0-only</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-  <depend>rclcpp</depend>
-  <depend>common_interfaces</depend>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>

src/latency_tester/src/embedded_ping.cpp

@@ -1,104 +0,0 @@
-#include <chrono>
-#include <functional>
-#include <memory>
-#include <string>
-#include <iomanip>
-#include <locale>
-
-#include "rclcpp/rclcpp.hpp"
-#include "std_msgs/msg/string.hpp"
-
-using namespace std::chrono_literals;
-
-
-/**
- * `input` into `args` separated by `delim`; equivalent to Python's `.split`;
- * Example:  "ctrl,led,on" => `{"ctrl","led","on"}`
- * @param input String to be separated
- * @param delim char which separates parts of input
- * @author David Sharpe, for ASTRA
- * @deprecated Use function without delim parameter
- */
-std::vector<std::string> split(const std::string& input, const char delim = ',');
-
-
-class LatencyTester : public rclcpp::Node
-{
-public:
-    LatencyTester()
-    : Node("latency_tester"), count_(0), target_mcu_("core")
-    {
-        publisher_ = this->create_publisher<std_msgs::msg::String>("/anchor/relay", 10);
-        timer_ = this->create_wall_timer(
-        1000ms, std::bind(&LatencyTester::timer_callback, this));
-        subscriber_ = this->create_subscription<std_msgs::msg::String>(
-            "/anchor/debug",
-            10,
-            std::bind(&LatencyTester::response_callback, this, std::placeholders::_1));
-    }
-
-private:
-    void timer_callback()  // Send ping to embedded at 1 Hz
-    {
-        auto message = std_msgs::msg::String();
-        message.data = "can_relay_tovic," + target_mcu_ + ",1," + std::to_string(count_++) + '\n';
-        publisher_->publish(message);
-        RCLCPP_INFO(this->get_logger(), "Sending ping #%ld to %s", count_, target_mcu_.c_str());
-        last_send_stamp_ = this->get_clock()->now();
-    }
-
-    void response_callback(const std_msgs::msg::String::SharedPtr msg)
-    {
-        auto now = this->now();
-        std::vector<std::string> args = split(msg->data, ',');
-        if (args.size() < 3 || args[0] != "can_relay_fromvic" || args[2] != "1")
-            return;
-
-        if (args[1] != target_mcu_) {
-            RCLCPP_INFO(this->get_logger(), "Received pong from different MCU: %s", args[1].c_str());
-            return;
-        }
-
-        // TODO: add topic for this so we can plot with MATLAB while using core/arm
-        RCLCPP_INFO(this->get_logger(), "Received pong from %s after %lf ms", target_mcu_.c_str(),
-                    (now - last_send_stamp_).nanoseconds() / 1000000.0);
-    }
-
-    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
-    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscriber_;
-    rclcpp::TimerBase::SharedPtr timer_;
-    size_t count_;
-    rclcpp::Time last_send_stamp_;
-    std::string target_mcu_;
-};
-
-int main(int argc, char * argv[])
-{
-    rclcpp::init(argc, argv);
-    rclcpp::spin(std::make_shared<LatencyTester>());
-    rclcpp::shutdown();
-    return 0;
-}
-
-
-std::vector<std::string> split(const std::string& input, const char delim) {
-    // Modified from
-    // https://stackoverflow.com/questions/14265581/parse-split-a-string-in-c-using-string-delimiter-standard-c
-
-    std::vector<std::string> args;
-
-    // if empty input for some reason, don't do anything
-    if (input.length() == 0)
-        return args;
-
-    size_t last = 0;
-    size_t next = 0;
-    while ((next = input.find(delim, last)) != std::string::npos)
-    {
-        args.push_back(input.substr(last, next-last));
-        last = next + 1;
-    }
-    args.push_back(input.substr(last));
-
-    return args;
-}

systemd/anchor.service

@@ -1,14 +0,0 @@
-[Unit]
-Description=Anchor nodes for controlling the rover and its modules (rover-ros2)
-After=systemd-user-sessions.service
-Requires=systemd-user-sessions.service
-
-[Service]
-ExecStart=/home/clucky/rover-ros2/auto_start/auto_start_anchor.sh
-Restart=always
-RestartSec=5
-User=clucky
-Environment=PYTHONUNBUFFERED=1
-
-[Install]
-WantedBy=multi-user.target

systemd/astra_rosbag.service

@@ -1,14 +0,0 @@
-[Unit]
-Description=Record a rosbag on boot to /home/clucky/bags/
-After=systemd-user-sessions.service
-Requires=systemd-user-sessions.service
-
-[Service]
-ExecStart=/home/clucky/rover-ros2/auto_start/start_rosbag.sh
-Restart=always
-RestartSec=5
-User=clucky
-Environment=PYTHONUNBUFFERED=1
-
-[Install]
-WantedBy=multi-user.target

systemd/core_headless.service

@@ -1,14 +0,0 @@
-[Unit]
-Description=Autostart headless core node for controlling the rover without a base station
-After=systemd-user-sessions.service
-Requires=systemd-user-sessions.service
-
-[Service]
-ExecStart=/home/clucky/rover-ros2/auto_start/auto_start_core_headless.sh
-Restart=always
-RestartSec=10
-User=clucky
-Environment=PYTHONUNBUFFERED=1
-
-[Install]
-WantedBy=multi-user.target

systemd/headless_full.service

@@ -1,14 +0,0 @@
-[Unit]
-Description=Headless node to control Core and Arm
-After=systemd-user-sessions.service
-Requires=systemd-user-sessions.service
-
-[Service]
-ExecStart=/home/clucky/rover-ros2/auto_start/auto_start_headless_full.sh
-Restart=always
-RestartSec=10
-User=clucky
-Environment=PYTHONUNBUFFERED=1
-
-[Install]
-WantedBy=multi-user.target