one more attempt at manual zoom

merge main into ptz-zoom

.envrc

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

.gitignore

@@ -11,7 +11,3 @@ log/
 
 #Pycache folder
 __pycache__/
-
-#Direnv
-.direnv/
-.venv

.gitmodules

@@ -1,6 +1,3 @@
-[submodule "src/astra_description"]
-	path = src/astra_descriptions
-	url = ../astra_descriptions
-[submodule "src/astra_msgs"]
-	path = src/astra_msgs
-	url = git@github.com:SHC-ASTRA/astra_msgs.git
+[submodule "src/ros2_interfaces_pkg"]
+	path = src/ros2_interfaces_pkg
+	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,233 +1,87 @@
-# ASTRA Rover ROS2 Packages
+# 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.
 
-Includes all main 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.
+<br>
 
-You will use these packages to launch all rover-side ROS2 nodes.
+## Software Pre-reqs
 
-## Table of Contents
+An acting base station computer will need several things:
 
-- [Software Prerequisites](#software-prerequisites)
-  - [Nix](#nix)
-  - [ROS2 Humble + rosdep](#ros2-humble--rosdep)
-- [Running](#running)
-  - [Testing Serial](#testing-serial)
-  - [Connecting the GuliKit Controller](#connecting-the-gulikit-controller)
-- [Common Problems/Toubleshooting](#common-problemstroubleshooting)
-- [Packages](#packages)
-- [Graphs](#graphs)
-  - [Full System](#full-system)
-  - [Individual Nodes](#individual-nodes)
-- [Maintainers](#maintainers)
+* ROS2 Humble
+    * Follow the standard ROS2 humble install process. Linux recommended.
+        * https://docs.ros.org/en/humble/Installation.html
+* Colcon
+    * `$ sudo apt update`
+    * `$ sudo apt install python3-colcon-common-extensions`
+* Configured Static IP for Ubiquiti bullet (Process varies by OS)
+    * IP Address: 192.168.1.x
+        * This can be just about anything not already in use. I recommend something 30-39
+    * Net Mask: 255.255.255.0
+    * Gateway: 192.168.1.0
 
-## Software Prerequisites
+## Launching with ANCHOR
 
-You need either [ROS2 Humble](https://docs.ros.org/en/humble/index.html)
-with [rosdep](https://docs.ros.org/en/humble/Tutorials/Intermediate/Rosdep.html#rosdep-installation)
-or [Nix](https://nixos.org/download/#nix-install-linux) installed. We recommend
-using Nix.
+ANCHOR (Active Node Controller Hub and Operational Relay)
+Allows for launching all nodes on the rover simulataneously. Additionally, all controls will run through the core's NUC and MCU.
+<br>
+1. SSH to core
+    * Core1: `ssh clucky@192.168.1.69`
+    * Core2: `ssh clucky@192.168.1.70`
+    * Password: \<can be found in the rover-Docs repo under documentation>
+2. Navigate to rover-ros2 workspace
+    * `cd rover-ros2`
+3. Source the workspace
+    * `source install/setup.bash`
+4. Launch ANCHOR
+    * `ros2 launch rover_launch.py mode:=anchor`
 
-### Nix
+## Launching as Standalone
 
-With Nix, all you have to do is enter the development shell:
+For use when running independent modules through their respective computers (pi/NUC) without ANCHOR.
 
-```bash
-$ cd path/to/rover-ros2
-$ nix develop
-```
+1. SSH to the the module's computer
+    * Core1: `ssh clucky@192.168.1.69`
+    * Core2: `ssh clucky@192.168.1.70`
+    * Arm: `ssh arm@192.168.1.70`
+    * Bio: \<TBD>
+    * Password: \<can be found in the rover-Docs repo under documentation>
+2. Run the main node (this sends commands to the MCU)
+    * Navigate to the rover-ros2 workspace (location may vary)
+        * `cd rover-ros2`
+    * Source the workspace
+        * `source install/setup.bash`
+    * Start the node
+        * ARM: `ros2 launch rover_launch.py mode:=arm`
+        * CORE: `ros2 launch rover_launch.py mode:=core`
+        * BIO: `ros2 launch rover_launch.py mode:=bio`
 
-### ROS2 Humble + rosdep
+## Running Headless
 
-With ROS2 Humble, start by using rosdep to install dependencies:
+Headless control nodes (for ARM and CORE) allow running of the module on-rover without the operator having ROS2 installed on their machine. You will need a laptop to connect to the pi/NUC in order to launch headless but it can be disconnected after the nodes are spun up.
+<br>
+1. SSH to the the module's computer
+    * Core1: `ssh clucky@192.168.1.69`
+    * Core2: `ssh clucky@192.168.1.70`
+    * Arm: `ssh arm@192.168.1.70`
+    * Password: \<can be found in the rover-Docs repo under documentation>
+2. Run the  headless node
+    * You must have ANCHOR or the module's Standalone node running
+    * Open a new terminal (SSH'd to the module)
+    * Navigate to rover-ros2 workspace
+        * `cd rover-ros2`
+    * Source the workspace
+        * `source install/setup.bash`
+    * Run the node (ensure controller is connected and on x-input mode)
+        * CORE: `ros2 run core_pkg headless`
+        * ARM: `ros2 run arm_pkg headless`
 
-```bash
-  # Setup rosdep
-$ sudo rosdep init  # only run if you haven't already
-$ rosdep update
-  # Install dependencies
-$ cd path/to/rover-ros2
-$ rosdep install --from-paths src -y --ignore-src
-```
+## Connecting the GuliKit Controller
 
-## Running
+Connecting the GuliKit Controller (Recommended)
 
-```bash
-$ colcon build
-$ source install/setup.bash
-  # main launch files:
-$ ros2 launch anchor_pkg rover.launch.py  # Must be run on a computer connected to a MCU on the rover.
-$ ros2 run headless_pkg headless_full  # Optionally run in a separate shell on the same or different computer.
-```
-
-### Using the Mock Connector
-
-Anchor provides a mock connector meant for testing and scripting purposes. You can select the mock connector by running anchor with this command:
-
-```bash
-$ ros2 launch anchor_pkg rover.launch.py connector:="mock"
-```
-
-To see all data that would be sent over the CAN network (and thus to the microcontrollers), use this command:
-
-```bash
-$ ros2 topic echo /anchor/to_vic/debug
-```
-
-To send data to the mock connector (as if you were a ROS2 node), use the normal relay topic:
-
-```bash
-$ ros2 topic pub /anchor/to_vic/relay astra_msgs/msg/VicCAN '{mcu_name: "core", command_id: 50, data: [0.0, 2.0, 0.0, 1.0]}'
-
-```
-
-To send data to the mock connector (as if you were a microcontroller), publish to the dedicated topic:
-
-```bash
-$ ros2 topic pub /anchor/from_vic/mock_mcu astra_msgs/msg/VicCAN '{mcu_name: "arm", command_id: 55, data: [0.0, 450.0, 900.0, 0.0]}'
-```
-
-### Testing Serial
-
-You can fake the presence of a Serial device (i.e., MCU) by using the following command:
-
-```bash
-$ socat -dd -v pty,rawer,crnl,link=/tmp/ttyACM9 pty,rawer,crnl,link=/tmp/ttyOUT
-```
-
-When you go to run anchor, use the `serial_override` ROS2 parameter to point it to the fake serial port, like so:
-
-```bash
-$ ros2 launch anchor_pkg rover.launch.py connector:=serial serial_override:=/tmp/ttyACM9
-```
-
-### Testing CAN
-
-You can create a virtual CAN network by using the following commands to create and then enable it:
-
-```bash
-sudo ip link add dev vcan0 type vcan
-sudo ip link set vcan0 up
-```
-
-When you go to run anchor, use the `can_override` ROS2 parameter to point it to the virtual CAN network, like so:
-
-```bash
-$ ros2 launch anchor_pkg rover.launch.py connector:=can can_override:=vcan0
-```
-
-Once you're done, you should delete the virtual network so that anchor doesn't get confused if you plug in a real CAN adapter:
-
-```bash
-$ sudo ip link delete vcan0
-```
-
-### Connecting the GuliKit Controller
-
-These instructions apply to the black XBox-style GuliKit controller, primarily used for controlling Arm through Basestation.
-
-* Connect the controller to your PC with a USB-C cable
+* Connect controller to pc with USB-C
 * Select the "X-Input" control mode (Windows logo) on the controller.
 * Hold the button next to the symbols (windows, android, switch, etc...)
 * You'll need to release the button and press down again to cycle to the next mode
-
-## Common Problems/Troubleshooting
-
-**Q**: When I try to launch the nodes, I receive a `package '' not found` error.
-
-A: Make sure you have sourced the workspace in the current shell:
-
-```bash
-$ source install/setup.bash  # or setup.zsh if using ZSH
-```
-
-**Q**: When I try to launch the nodes, I receive several `FileNotFoundError: [Errno 2]` errors.
-
-A: Sometimes the install files get messed up by running `colcon build` in different shells or updating packages. Try running the following commands to clean up your local build files:
-
-```bash
-$ rm -rf build/ install/
-$ colcon build
-```
-
-**Q**: When I run `colcon build` after the above suggestion, I receive several of the following errors:
-
-```bash
-[0.557s] WARNING:colcon.colcon_ros.prefix_path.ament:The path '' in the environment variable AMENT_PREFIX_PATH doesn't exist
-```
-
-A: Don't worry about it. If you had the workspace sourced, ROS2 will complain about the workspace install files not existing anymore after you deleted them. They will be re-created by `colcon build`, after which you can run `source install/setup.bash` to source the new install files.
-
-**Q**: When I try to launch Anchor, I receive the following errors:
-
-```bash
-[anchor-5] [INFO] [1762239452.937881841] [anchor]: Unable to find MCU...
-...
-[ERROR] [anchor-5]: process has died [pid 101820, exit code 1, cmd '.../rover-ros2/install/anchor_pkg/lib/anchor_pkg/anchor --ros-args -r __node:=anchor --params-file /tmp/launch_params_nmv6tpw4'].
-[INFO] [launch]: process[anchor-5] was required: shutting down launched system
-[INFO] [bio-4]: sending signal 'SIGINT' to process[bio-4]
-[INFO] [ptz-3]: sending signal 'SIGINT' to process[ptz-3]
-[INFO] [core-2]: sending signal 'SIGINT' to process[core-2]
-[INFO] [arm-1]: sending signal 'SIGINT' to process[arm-1]
-...
-```
-
-A: To find a microcontroller to talk to, Anchor sends a ping to every Serial port on your computer. If it does not receive a 'pong' in less than one second, then it aborts. There are a few possible fixes:
-
-- Keep trying to run it until it works
-- Run `lsusb` to see if the microcontroller is detected by your computer.
-- Run `ls /dev/tty*0` to see if there is a valid Serial port enumerated for the microcontroller.
-- Check if you are in the `dialout` group (or whatever group shows up by running `ls -l /dev/tty*`).
-
-## Packages
-
-- [anchor\_pkg](./src/anchor_pkg) - Handles Serial communication between the various other packages here and the microcontroller.
-- [arm\_pkg](./src/arm_pkg) - Relays controls and sensor data for the arm (socket and digit) between anchor and basestation/headless.
-- [astra\_descriptions](./src/astra_descriptions) - Submodule with URDF-related packages.
-- [bio\_pkg](./src/bio_pkg) - Like arm_pkg, but for CITADEL and FAERIE
-- [core\_pkg](./src/core_pkg) - Like arm_pkg, but for Core
-- [headless\_pkg](./src/headless_pkg) - Simple, non-graphical controller node to work in place of basestation when controlling the rover by itself. This is autostarted with anchor to allow for setup-less control of the rover.
-- [latency\_tester](./src/latency_tester) - A temporary node to test comms latency over ROS2, Serial, and CAN.
-- [ros2\_interfaces\_pkg](./src/ros2_interfaces_pkg) - Contains custom message types for communication between basestation and the rover over ROS2. (being renamed to `astra_msgs`).
-- [servo\_arm\_twist\_pkg](./src/servo_arm_twist_pkg) - A temporary node to translate controller state from `ros2_joy` to `Twist` messages to control the Arm via IK.
-
-## Graphs
-
-### Full System
-
-> **Anchor stand-alone** (`ros2 launch anchor_pkg rover.launch.py`)
->
-> ![rqt_graph of Anchor by itself, ran with command: ros2 launch anchor_pkg rover.launch.py](./docs-resources/graph-anchor-standalone.png)
-
-> **Anchor with [basestation-classic](https://github.com/SHC-ASTRA/basestation-classic)**
->
-> ![rqt_graph of Anchor ran with the same command as above, talking to basestation-classic](./docs-resources/graph-anchor-w-basestation-classic.png)
-
-> **Anchor with Headless** (`ros2 run headless_pkg headless_full`)
->
-> ![rqt_graph of Anchor ran with Headless](./docs-resources/graph-anchor-w-headless.png)
-
-### Individual Nodes
-
-> **Anchor** (`ros2 run anchor_pkg anchor`)
->
-> ![rqt_graph of Anchor node running by itself](./docs-resources/graph-anchor-anchor-standalone.png)
-
-> **Core** (`ros2 run core_pkg core --ros-args -p launch_mode:=anchor`)
->
-> ![rqt_graph of Core node running by itself](./docs-resources/graph-anchor-core-standalone.png)
-
-> **Arm** (`ros2 run arm_pkg arm --ros-args -p launch_mode:=anchor`)
->
-> ![rqt_graph of Arm node running by itself](./docs-resources/graph-anchor-arm-standalone.png)
-
-> **Bio** (`ros2 run bio_pkg bio --ros-args -p launch_mode:=anchor`)
->
-> ![rqt_graph of Bio node running by itself](./docs-resources/graph-anchor-bio-standalone.png)
-
-## Maintainers
-
-| Name | Email | Discord |
-| ---- | ----- | ------- |
-| David Sharpe | <ds0196@uah.edu> | `@ddavdd` |
-| Riley McLain | <rjm0037@uah.edu> | `@ryleu` |

auto_start/auto_start_anchor.sh

@@ -1,28 +0,0 @@
-#!/usr/bin/env bash
-set -e
-
-SCRIPT_DIR=$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" &>/dev/null && pwd)
-
-# 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
-if command -v nixos-rebuild; then
-  echo "[INFO] running on NixOS"
-else
-  source /opt/ros/humble/setup.bash
-fi
-
-# Source your workspace setup script
-source $SCRIPT_DIR/../install/setup.bash
-
-# Launch the ROS 2 node with the desired mode
-ros2 launch anchor_pkg rover.launch.py mode:=anchor

auto_start/auto_start_headless_full.sh

@@ -1,28 +0,0 @@
-#!/usr/bin/env bash
-set -e
-
-SCRIPT_DIR=$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" &>/dev/null && pwd)
-
-# 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
-if command -v nixos-rebuild; then
-  echo "[INFO] running on NixOS"
-else
-  source /opt/ros/humble/setup.bash
-fi
-
-# Source your workspace setup script
-source $SCRIPT_DIR/../install/setup.bash
-
-# Launch the ROS 2 node
-ros2 run headless_pkg headless_full

auto_start/start_rosbag.sh

@@ -1,29 +0,0 @@
-#!/usr/bin/env bash
-set -e
-
-SCRIPT_DIR=$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" &>/dev/null && pwd)
-
-[[ -z $ANCHOR_WS ]] && ANCHOR_WS="$SCRIPT_DIR/.."
-[[ -z $AUTONOMY_WS ]] && AUTONOMY_WS="$HOME/rover-Autonomy"
-BAG_LOCATION="$HOME/bags/autostart"
-[[ ! -d $BAG_LOCATION ]] && mkdir -p "$BAG_LOCATION"
-
-# 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!"
-
-if command -v nixos-rebuild; then
-  echo "[INFO] running on NixOS"
-else
-  source /opt/ros/humble/setup.bash
-fi
-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,105 +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": 1775216071,
-        "narHash": "sha256-PrPW70Fh1uLx3JxNV/NLeXjUhgfrZmi7ac8LJOlS0q4=",
-        "owner": "lopsided98",
-        "repo": "nix-ros-overlay",
-        "rev": "197a2b55c4ed24f8b885a5b20b65f426fb6d57ca",
-        "type": "github"
-      },
-      "original": {
-        "owner": "lopsided98",
-        "ref": "master",
-        "repo": "nix-ros-overlay",
-        "type": "github"
-      }
-    },
-    "nixpkgs": {
-      "locked": {
-        "lastModified": 1759381078,
-        "narHash": "sha256-gTrEEp5gEspIcCOx9PD8kMaF1iEmfBcTbO0Jag2QhQs=",
-        "owner": "NixOS",
-        "repo": "nixpkgs",
-        "rev": "7df7ff7d8e00218376575f0acdcc5d66741351ee",
-        "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"
-        ],
-        "treefmt-nix": "treefmt-nix"
-      }
-    },
-    "systems": {
-      "locked": {
-        "lastModified": 1681028828,
-        "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=",
-        "owner": "nix-systems",
-        "repo": "default",
-        "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e",
-        "type": "github"
-      },
-      "original": {
-        "owner": "nix-systems",
-        "repo": "default",
-        "type": "github"
-      }
-    },
-    "treefmt-nix": {
-      "inputs": {
-        "nixpkgs": [
-          "nixpkgs"
-        ]
-      },
-      "locked": {
-        "lastModified": 1773297127,
-        "narHash": "sha256-6E/yhXP7Oy/NbXtf1ktzmU8SdVqJQ09HC/48ebEGBpk=",
-        "owner": "numtide",
-        "repo": "treefmt-nix",
-        "rev": "71b125cd05fbfd78cab3e070b73544abe24c5016",
-        "type": "github"
-      },
-      "original": {
-        "owner": "numtide",
-        "repo": "treefmt-nix",
-        "type": "github"
-      }
-    }
-  },
-  "root": "root",
-  "version": 7
-}

flake.nix

@@ -1,105 +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!!!
-
-    treefmt-nix = {
-      url = "github:numtide/treefmt-nix";
-      inputs.nixpkgs.follows = "nixpkgs";
-    };
-  };
-
-  outputs =
-    {
-      self,
-      nix-ros-overlay,
-      nixpkgs,
-      ...
-    }@inputs:
-    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
-            socat
-            can-utils
-            (python313.withPackages (
-              p: with p; [
-                pyserial
-                python-can
-                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
-                  control-msgs
-                  control-toolbox
-                  moveit-core
-                  moveit-planners
-                  moveit-common
-                  moveit-msgs
-                  moveit-ros-planning
-                  moveit-ros-planning-interface
-                  moveit-ros-visualization
-                  moveit-configs-utils
-                  moveit-ros-move-group
-                  moveit-servo
-                  moveit-simple-controller-manager
-                  topic-based-ros2-control
-                  pilz-industrial-motion-planner
-                  pick-ik
-                  ompl
-                  joy
-                  ros2-controllers
-                  chomp-motion-planner
-                ];
-              }
-            )
-          ];
-          shellHook = ''
-            # Display stuff
-            export DISPLAY=''${DISPLAY:-:0}
-            export QT_X11_NO_MITSHM=1
-          '';
-        };
-
-        formatter = (inputs.treefmt-nix.lib.evalModule pkgs ./treefmt.nix).config.build.wrapper;
-      }
-    );
-
-  nixConfig = {
-    # Cache to pull ros packages from
-    extra-substituters = [ "https://ros.cachix.org" "https://attic.iid.ciirc.cvut.cz/ros" ];
-    extra-trusted-public-keys = [ "ros.cachix.org-1:dSyZxI8geDCJrwgvCOHDoAfOm5sV1wCPjBkKL+38Rvo=" "ros:JR95vUYsShSqfA1VTYoFt1Nz6uXasm5QrcOsGry9f6Q=" ];
-  };
-}

rover_launch.py

@@ -0,0 +1,132 @@
+#!/usr/bin/env python3
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+
+#Prevent making __pycache__ directories
+from sys import dont_write_bytecode
+dont_write_bytecode = True
+
+def launch_setup(context, *args, **kwargs):
+    # Retrieve the resolved value of the launch argument 'mode'
+    mode = LaunchConfiguration('mode').perform(context)
+    nodes = []
+
+    if mode == 'anchor':
+        # Launch every node and pass "anchor" as the parameter
+        nodes.append(
+            Node(
+                package='arm_pkg',
+                executable='arm',  # change as needed
+                name='arm',
+                output='both',
+                parameters=[{'launch_mode': mode}],
+                on_exit=Shutdown()
+            )
+        )
+        nodes.append(
+            Node(
+                package='core_pkg',
+                executable='core',  # change as needed
+                name='core',
+                output='both',
+                parameters=[{'launch_mode': mode}],
+                on_exit=Shutdown()
+            )
+        )
+        nodes.append(
+            Node(
+                package='core_pkg',
+                executable='ptz',  # change as needed
+                name='ptz',
+                output='both'
+                # Currently don't shutdown all nodes if the PTZ node fails, as it is not critical
+                # on_exit=Shutdown()  # Uncomment if you want to shutdown on PTZ failure
+            )
+        )
+        nodes.append(
+            Node(
+                package='bio_pkg',
+                executable='bio',  # change as needed
+                name='bio',
+                output='both',
+                parameters=[{'launch_mode': mode}],
+                on_exit=Shutdown()
+            )
+        )
+        nodes.append(
+            Node(
+                package='anchor_pkg',
+                executable='anchor',  # change as needed
+                name='anchor',
+                output='both',
+                parameters=[{'launch_mode': mode}],
+                on_exit=Shutdown()
+            )
+        )
+    elif mode in ['arm', 'core', 'bio', 'ptz']:
+        # Only launch the node corresponding to the provided mode.
+        if mode == 'arm':
+            nodes.append(
+                Node(
+                    package='arm_pkg',
+                    executable='arm',
+                    name='arm',
+                    output='both',
+                    parameters=[{'launch_mode': mode}],
+                    on_exit=Shutdown()
+                )
+            )
+        elif mode == 'core':
+            nodes.append(
+                Node(
+                    package='core_pkg',
+                    executable='core',
+                    name='core',
+                    output='both',
+                    parameters=[{'launch_mode': mode}],
+                    on_exit=Shutdown()
+                )
+            )
+        elif mode == 'bio':
+            nodes.append(
+                Node(
+                    package='bio_pkg',
+                    executable='bio',
+                    name='bio',
+                    output='both',
+                    parameters=[{'launch_mode': mode}],
+                    on_exit=Shutdown()
+                )
+            )
+        elif mode == 'ptz':
+            nodes.append(
+                Node(
+                    package='core_pkg',
+                    executable='ptz',
+                    name='ptz',
+                    output='both',
+                    on_exit=Shutdown(), #on fail, shutdown if this was the only node to be launched
+                )
+            )
+    else:
+        # If an invalid mode is provided, print an error.
+        print("Invalid mode provided. Choose one of: arm, core, bio, anchor, ptz.")
+
+    return nodes
+
+def generate_launch_description():
+    declare_arg = DeclareLaunchArgument(
+        'mode',
+        default_value='anchor',
+        description='Launch mode: arm, core, bio, anchor, or ptz'
+    )
+
+    return LaunchDescription([
+        declare_arg,
+        OpaqueFunction(function=launch_setup)
+    ])

scripts/reset-repo.bash

@@ -1,38 +0,0 @@
-#!/usr/bin/env bash
-
-repo_root="$(git rev-parse --show-toplevel)"
-
-if [[ -z $repo_root ]]; then
-  echo "script must be run from within the rover-ros2 repo" >&2
-  exit 1
-fi
-
-cd $repo_root
-
-echo "this will nuke all of your current un-commited git changes, including any changes to submodules and any gitignored files. is this okay? (y/N)"
-read okay
-
-if [[ $okay != "y" ]]; then
-  echo "you didn't say exactly 'y'. aborting." >&2
-  exit 2
-fi
-
-echo
-
-echo "ok say goodbye to everything in this repo"
-git submodule deinit --all -f && echo "- submodules gone"
-git clean -fdx && echo "- gitignored changes gone"
-git add -A
-git reset HEAD --hard && echo "- everything else gone"
-git submodule update --init --recursive && echo "- brought the submodules back"
-echo
-
-echo "in theory that should've done it. let's make sure"
-status=$(git status --porcelain)
-echo $status
-if [[ -z $status ]]; then
-  echo "nice, all clean!"
-else
-  echo "uhh that's not supposed to be there. this is probably a bug in this script. good luck!" >&2
-  exit 3
-fi

scripts/test-connectors.bash

@@ -1,463 +0,0 @@
-#!/usr/bin/env bash
-
-# test script for anchor connectors (mock, serial, CAN)
-
-set -o pipefail
-
-repo_root="$(git rev-parse --show-toplevel)"
-
-if [[ -z $repo_root ]]; then
-  echo "script must be run from within the rover-ros2 repo" >&2
-  exit 1
-fi
-
-cd "$repo_root"
-
-# colors
-BOLD='\033[1m'
-RED='\033[1;31m'
-GREEN='\033[1;32m'
-YELLOW='\033[1;33m'
-NC='\033[0m'
-
-TESTS_PASSED=0
-TESTS_FAILED=0
-
-log() {
-  echo -e "${BOLD}${YELLOW}info:${NC} ${1}"
-}
-
-pass() {
-  echo -e "${BOLD}${GREEN}pass:${NC} ${1}"
-  TESTS_PASSED=$((TESTS_PASSED + 1))
-}
-
-fail() {
-  echo -e "${BOLD}${RED}fail:${NC} ${1}"
-  TESTS_FAILED=$((TESTS_FAILED + 1))
-}
-
-cleanup() {
-  log "cleaning up"
-  if [[ -n $ANCHOR_PID ]]; then
-    kill -INT -- -"$ANCHOR_PID" 2>/dev/null || true
-    wait "$ANCHOR_PID" 2>/dev/null || true
-  fi
-  if [[ -n $SOCAT_PID ]]; then
-    kill -INT "$SOCAT_PID" 2>/dev/null || true
-    wait "$SOCAT_PID" 2>/dev/null || true
-  fi
-  rm -f /tmp/ttyACM9 /tmp/ttyOUT 2>/dev/null || true
-}
-
-trap cleanup EXIT
-
-source_ros2() {
-  source install/setup.bash
-}
-
-wait_for_topic() {
-  local topic="$1"
-  local timeout="${2:-5}"
-  local count=0
-  while ! ros2 topic list 2>/dev/null | grep -q "^${topic}$"; do
-    sleep 0.5
-    count=$((count + 1))
-    if [[ $count -ge $((timeout * 2)) ]]; then
-      return 1
-    fi
-  done
-  return 0
-}
-
-# run a ROS pub/echo test
-# usage: ros_pubsub_test <echo_topic> <pub_topic> <msg_type> <msg_data>
-# returns the echo output via stdout
-ros_pubsub_test() {
-  local echo_topic="$1"
-  local pub_topic="$2"
-  local msg_type="$3"
-  local msg_data="$4"
-
-  timeout 5 bash -c "
-    ros2 topic echo --once $echo_topic &
-    ECHO_PID=\$!
-    sleep 0.5
-    ros2 topic pub --once $pub_topic $msg_type \"$msg_data\" >/dev/null 2>&1
-    wait \$ECHO_PID
-  " 2>/dev/null || true
-}
-
-test_mock_connector() {
-  log "testing mock connector"
-
-  log "starting anchor with mock connector"
-  setsid ros2 run anchor_pkg anchor --ros-args -p connector:=mock &
-  ANCHOR_PID=$!
-  sleep 2
-
-  if ! kill -0 "$ANCHOR_PID" 2>/dev/null; then
-    fail "mock connector: anchor failed to start"
-    return 1
-  fi
-
-  if ! wait_for_topic "/anchor/to_vic/relay" 10; then
-    fail "mock connector: topics not available"
-    kill -INT -- -"$ANCHOR_PID" 2>/dev/null || true
-    return 1
-  fi
-
-  log "anchor started successfully"
-
-  # test: relay -> debug
-  log "testing relay -> debug"
-  local output
-  output=$(ros_pubsub_test "/anchor/to_vic/debug" "/anchor/to_vic/relay" \
-    "astra_msgs/msg/VicCAN" '{mcu_name: \"core\", command_id: 50, data: [1.0, 2.0, 3.0, 4.0]}')
-
-  if [[ -n $output ]] && echo "$output" | grep -q "can_relay_tovic,core,50"; then
-    pass "mock connector: relay -> debug"
-  else
-    fail "mock connector: relay -> debug"
-  fi
-
-  # test: mock_mcu -> from_vic/core
-  log "testing mock_mcu (core) -> from_vic/core"
-  output=$(ros_pubsub_test "/anchor/from_vic/core" "/anchor/from_vic/mock_mcu" \
-    "astra_msgs/msg/VicCAN" '{mcu_name: \"core\", command_id: 10, data: [100.0, 200.0]}')
-
-  if [[ -n $output ]] && echo "$output" | grep -q "mcu_name: core" && echo "$output" | grep -q "command_id: 10"; then
-    pass "mock connector: mock_mcu -> from_vic/core"
-  else
-    fail "mock connector: mock_mcu -> from_vic/core"
-  fi
-
-  # test: mock_mcu -> from_vic/arm
-  log "testing mock_mcu (arm) -> from_vic/arm"
-  output=$(ros_pubsub_test "/anchor/from_vic/arm" "/anchor/from_vic/mock_mcu" \
-    "astra_msgs/msg/VicCAN" '{mcu_name: \"arm\", command_id: 55, data: [0.0, 450.0, 900.0, 0.0]}')
-
-  if [[ -n $output ]] && echo "$output" | grep -q "mcu_name: arm" && echo "$output" | grep -q "command_id: 55"; then
-    pass "mock connector: mock_mcu -> from_vic/arm"
-  else
-    fail "mock connector: mock_mcu -> from_vic/arm"
-  fi
-
-  # test: mock_mcu -> from_vic/bio
-  log "testing mock_mcu (citadel) -> from_vic/bio"
-  output=$(ros_pubsub_test "/anchor/from_vic/bio" "/anchor/from_vic/mock_mcu" \
-    "astra_msgs/msg/VicCAN" '{mcu_name: \"citadel\", command_id: 20, data: [5.0]}')
-
-  if echo "$output" | grep -q "mcu_name: citadel" && echo "$output" | grep -q "command_id: 20"; then
-    pass "mock connector: mock_mcu -> from_vic/bio"
-  else
-    fail "mock connector: mock_mcu -> from_vic/bio"
-  fi
-
-  # test: relay_string -> debug
-  log "testing relay_string -> debug"
-  output=$(ros_pubsub_test "/anchor/to_vic/debug" "/anchor/to_vic/relay_string" \
-    "std_msgs/msg/String" '{data: \"test_raw_string_data\"}')
-
-  if [[ -n $output ]] && echo "$output" | grep -q "test_raw_string_data"; then
-    pass "mock connector: relay_string -> debug"
-  else
-    fail "mock connector: relay_string -> debug"
-  fi
-
-  kill -INT -- -"$ANCHOR_PID" 2>/dev/null || true
-  wait "$ANCHOR_PID" 2>/dev/null || true
-  ANCHOR_PID=""
-}
-
-test_serial_connector() {
-  log "testing serial connector"
-
-  log "creating virtual serial ports with socat"
-  socat pty,raw,echo=0,link=/tmp/ttyACM9 pty,raw,echo=0,link=/tmp/ttyOUT 2>/dev/null &
-  SOCAT_PID=$!
-  sleep 2
-
-  if ! kill -0 "$SOCAT_PID" 2>/dev/null; then
-    fail "serial connector: failed to create virtual serial ports"
-    return 1
-  fi
-
-  log "starting anchor with serial connector (override: /tmp/ttyACM9)"
-  setsid ros2 run anchor_pkg anchor --ros-args -p connector:=serial -p serial_override:=/tmp/ttyACM9 &
-  ANCHOR_PID=$!
-  sleep 2
-
-  if ! kill -0 "$ANCHOR_PID" 2>/dev/null; then
-    fail "serial connector: anchor failed to start"
-    kill -INT "$SOCAT_PID" 2>/dev/null || true
-    return 1
-  fi
-
-  if ! wait_for_topic "/anchor/to_vic/relay" 10; then
-    fail "serial connector: topics not available"
-    kill -INT -- -"$ANCHOR_PID" 2>/dev/null || true
-    kill -INT "$SOCAT_PID" 2>/dev/null || true
-    return 1
-  fi
-
-  pass "serial connector: anchor starts with virtual serial"
-
-  # test: relay -> serial output (VicCAN encoding)
-  log "testing relay -> serial output"
-
-  local serial_out_file
-  serial_out_file=$(mktemp)
-
-  # Start head first (blocks waiting for input), then publish
-  timeout 5 head -n1 /tmp/ttyOUT >"$serial_out_file" &
-  local head_pid=$!
-  sleep 0.3
-  ros2 topic pub --once /anchor/to_vic/relay astra_msgs/msg/VicCAN \
-    '{mcu_name: "core", command_id: 30, data: [1.0, 2.0, 3.0, 4.0]}' >/dev/null 2>&1
-  wait $head_pid 2>/dev/null || true
-
-  local serial_out
-  serial_out=$(cat "$serial_out_file")
-  rm -f "$serial_out_file"
-
-  if [[ -n $serial_out ]] && echo "$serial_out" | grep -q "can_relay_tovic,core,30"; then
-    pass "serial connector: relay -> serial output"
-  else
-    fail "serial connector: relay -> serial output (got: $serial_out)"
-  fi
-
-  # test: serial input -> from_vic/core
-  log "testing serial input -> from_vic/core"
-
-  local output
-  output=$(timeout 5 bash -c '
-    ros2 topic echo --once /anchor/from_vic/core &
-    ECHO_PID=$!
-    sleep 0.5
-    echo "can_relay_fromvic,core,15,10.0,20.0,30.0,40.0" > /tmp/ttyOUT
-    sleep 0.5
-    echo "can_relay_fromvic,core,15,10.0,20.0,30.0,40.0" > /tmp/ttyOUT
-    wait $ECHO_PID
-  ' 2>/dev/null) || true
-
-  if echo "$output" | grep -q "mcu_name: core" && echo "$output" | grep -q "command_id: 15"; then
-    pass "serial connector: serial input -> from_vic/core"
-  else
-    fail "serial connector: serial input -> from_vic/core (got: $output)"
-  fi
-
-  # test: relay_string -> debug
-  log "testing relay_string -> debug"
-  output=$(ros_pubsub_test "/anchor/to_vic/debug" "/anchor/to_vic/relay_string" \
-    "std_msgs/msg/String" '{data: \"serial_test_string\"}')
-
-  if [[ -n $output ]] && echo "$output" | grep -q "serial_test_string"; then
-    pass "serial connector: relay_string -> debug"
-  else
-    fail "serial connector: relay_string -> debug"
-  fi
-
-  kill -INT -- -"$ANCHOR_PID" 2>/dev/null || true
-  wait "$ANCHOR_PID" 2>/dev/null || true
-  ANCHOR_PID=""
-  kill -INT "$SOCAT_PID" 2>/dev/null || true
-  wait "$SOCAT_PID" 2>/dev/null || true
-  SOCAT_PID=""
-}
-
-test_can_connector() {
-  log "testing CAN connector"
-
-  log "starting anchor with CAN connector (override: vcan0)"
-  setsid ros2 run anchor_pkg anchor --ros-args -p connector:=can -p can_override:=vcan0 &
-  ANCHOR_PID=$!
-  sleep 2
-
-  if ! kill -0 "$ANCHOR_PID" 2>/dev/null; then
-    fail "CAN connector: anchor failed to start"
-    return 1
-  fi
-
-  if ! wait_for_topic "/anchor/to_vic/relay" 10; then
-    fail "CAN connector: topics not available"
-    kill -INT -- -"$ANCHOR_PID" 2>/dev/null || true
-    return 1
-  fi
-
-  log "anchor started successfully"
-  sleep 1
-
-  # test: relay -> CAN bus
-  # core=1, int16x4=2, cmd=30 -> id = (1<<8)|(2<<6)|30 = 0x19E
-  log "testing relay -> CAN bus"
-
-  local output
-  output=$(timeout 8 bash -c '
-    candump -n 1 vcan0 &
-    DUMP_PID=$!
-    sleep 1
-    ros2 topic pub --once /anchor/to_vic/relay astra_msgs/msg/VicCAN "{mcu_name: \"core\", command_id: 30, data: [1, 2, 3, 4]}" >/dev/null 2>&1
-    sleep 0.5
-    ros2 topic pub --once /anchor/to_vic/relay astra_msgs/msg/VicCAN "{mcu_name: \"core\", command_id: 30, data: [1, 2, 3, 4]}" >/dev/null 2>&1
-    wait $DUMP_PID
-  ' 2>/dev/null) || true
-
-  if echo "$output" | grep -qi "19E"; then
-    pass "CAN connector: relay -> CAN bus"
-  else
-    fail "CAN connector: relay -> CAN bus (got: $output)"
-  fi
-
-  # test: CAN -> from_vic/core
-  log "testing CAN bus -> from_vic/core"
-
-  output=$(timeout 5 bash -c '
-    ros2 topic echo --once /anchor/from_vic/core &
-    ECHO_PID=$!
-    sleep 1
-    cansend vcan0 18F#000A0014001E0028
-    sleep 0.5
-    cansend vcan0 18F#000A0014001E0028
-    wait $ECHO_PID
-  ' 2>/dev/null) || true
-
-  if echo "$output" | grep -q "mcu_name: core" && echo "$output" | grep -q "command_id: 15"; then
-    pass "CAN connector: CAN -> from_vic/core"
-  else
-    fail "CAN connector: CAN -> from_vic/core"
-  fi
-
-  # test: CAN -> from_vic/arm
-  log "testing CAN bus -> from_vic/arm"
-
-  output=$(timeout 5 bash -c '
-    ros2 topic echo --once /anchor/from_vic/arm &
-    ECHO_PID=$!
-    sleep 1
-    cansend vcan0 294#00640096012C01F4
-    sleep 0.5
-    cansend vcan0 294#00640096012C01F4
-    wait $ECHO_PID
-  ' 2>/dev/null) || true
-
-  if echo "$output" | grep -q "mcu_name: arm" && echo "$output" | grep -q "command_id: 20"; then
-    pass "CAN connector: CAN -> from_vic/arm"
-  else
-    fail "CAN connector: CAN -> from_vic/arm"
-  fi
-
-  # test: CAN double data type (data_type_key=0)
-  log "testing CAN double data type"
-
-  output=$(timeout 8 bash -c '
-    ros2 topic echo --once /anchor/from_vic/core &
-    ECHO_PID=$!
-    sleep 1
-    cansend vcan0 105#3FF0000000000000
-    sleep 0.5
-    cansend vcan0 105#3FF0000000000000
-    sleep 0.5
-    cansend vcan0 105#3FF0000000000000
-    wait $ECHO_PID
-  ' 2>/dev/null) || true
-
-  if echo "$output" | grep -q "mcu_name: core" && echo "$output" | grep -q "command_id: 5"; then
-    pass "CAN connector: double data type"
-  else
-    fail "CAN connector: double data type"
-  fi
-
-  # test: CAN float32x2 data type (data_type_key=1)
-  log "testing CAN float32x2 data type"
-
-  output=$(timeout 8 bash -c '
-    ros2 topic echo --once /anchor/from_vic/core &
-    ECHO_PID=$!
-    sleep 1
-    cansend vcan0 14A#3F80000040000000
-    sleep 0.5
-    cansend vcan0 14A#3F80000040000000
-    sleep 0.5
-    cansend vcan0 14A#3F80000040000000
-    wait $ECHO_PID
-  ' 2>/dev/null) || true
-
-  if echo "$output" | grep -q "mcu_name: core" && echo "$output" | grep -q "command_id: 10"; then
-    pass "CAN connector: float32x2 data type"
-  else
-    fail "CAN connector: float32x2 data type"
-  fi
-
-  kill -INT -- -"$ANCHOR_PID" 2>/dev/null || true
-  wait "$ANCHOR_PID" 2>/dev/null || true
-  ANCHOR_PID=""
-}
-
-check_prerequisites() {
-  log "checking prerequisites"
-  local missing=0
-
-  if [[ ! -f install/setup.bash ]]; then
-    fail "install/setup.bash not found; run 'colcon build --symlink-install' first"
-    missing=1
-  fi
-
-  if ! command -v socat &>/dev/null; then
-    fail "socat not found; install it or use 'nix develop'"
-    missing=1
-  fi
-
-  if ! command -v cansend &>/dev/null || ! command -v candump &>/dev/null; then
-    fail "can-utils (cansend/candump) not found; install it or use 'nix develop'"
-    missing=1
-  fi
-
-  if ! ip link show vcan0 &>/dev/null; then
-    fail "vcan0 interface not found"
-    log "  create it with:"
-    log "    sudo ip link add dev vcan0 type vcan"
-    log "    sudo ip link set vcan0 up"
-    missing=1
-  elif ! ip link show vcan0 | grep -q ",UP"; then
-    fail "vcan0 exists but is not UP"
-    log "  enable it with: sudo ip link set vcan0 up"
-    missing=1
-  fi
-
-  if [[ $missing -eq 1 ]]; then
-    echo ""
-    log "prerequisites not met"
-    exit 1
-  fi
-
-  log "all prerequisites met"
-}
-
-main() {
-  echo ""
-  log "anchor connector test suite"
-  echo ""
-
-  check_prerequisites
-
-  log "sourcing ROS2 workspace"
-  source_ros2
-
-  test_mock_connector
-  test_serial_connector
-  test_can_connector
-
-  echo ""
-  log "test summary"
-  echo -e "${BOLD}${GREEN}passed:${NC} $TESTS_PASSED"
-  echo -e "${BOLD}${RED}failed:${NC} $TESTS_FAILED"
-  echo ""
-
-  if [[ $TESTS_FAILED -gt 0 ]]; then
-    exit 1
-  fi
-  exit 0
-}
-
-main "$@"

src/anchor_pkg/anchor_pkg/anchor_node.py

@@ -1,255 +1,152 @@
-from warnings import deprecated
 import rclpy
 from rclpy.node import Node
-from rclpy.executors import ExternalShutdownException, SingleThreadedExecutor
-from rcl_interfaces.msg import ParameterDescriptor, ParameterType
+from std_srvs.srv import Empty
 
-from .connector import (
-    Connector,
-    MockConnector,
-    SerialConnector,
-    CANConnector,
-    NoValidDeviceException,
-    NoWorkingDeviceException,
-)
-from .convert import string_to_viccan, viccan_to_string
+import signal
+import time
+import atexit
+
+import serial
+import sys
+import threading
+import glob
 
-from astra_msgs.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
 
-class Anchor(Node):
-    """
-    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
-    * /anchor/to_vic/debug
-        - A string copy of the messages published to ./relay are published here
-
-    Subscribers:
-    * /anchor/from_vic/mock_mcu
-        - For testing without an actual MCU, publish VicCAN messages 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
-        - Send raw strings to connectors. Does not work for connectors that require conversion (like CANConnector)
-    * /anchor/relay
-        - Legacy method for talking to connectors. Takes String as input, but does not send the raw strings to connectors.
-          Instead, it converts them to VicCAN messages first.
-    """
-
-    connector: Connector
-
+class SerialRelay(Node):
     def __init__(self):
-        super().__init__("anchor_node")
+        # Initalize node with name
+        super().__init__("anchor_node")#previously 'serial_publisher'
 
-        logger = self.get_logger()
 
-        # ROS2 Parameter Setup
+        # Create publishers 
+        self.arm_pub = self.create_publisher(String, '/anchor/arm/feedback', 10)
+        self.core_pub = self.create_publisher(String, '/anchor/core/feedback', 10)
+        self.bio_pub = self.create_publisher(String, '/anchor/bio/feedback', 10)
 
-        self.declare_parameter(
-            "connector",
-            "auto",
-            ParameterDescriptor(
-                name="connector",
-                description="Declares which MCU connector should be used. Defaults to 'auto'.",
-                type=ParameterType.PARAMETER_STRING,
-                additional_constraints="Must be 'serial', 'can', 'mock', or 'auto'.",
-            ),
-        )
+        self.debug_pub = self.create_publisher(String, '/anchor/debug', 10)
         
-        self.declare_parameter(
-            "can_override",
-            "",
-            ParameterDescriptor(
-                name="can_override",
-                description="Overrides which CAN channel will be used. Defaults to ''.",
-                type=ParameterType.PARAMETER_STRING,
-                additional_constraints="Must be a valid CAN network that shows up in `ip link show`.",
-            ),
-        )
+        # Create a subscriber 
+        self.relay_sub = self.create_subscription(String, '/anchor/relay', self.send_cmd, 10)
 
-        self.declare_parameter(
-            "serial_override",
-            "",
-            ParameterDescriptor(
-                name="serial_override",
-                description="Overrides which serial port will be used. Defaults to ''.",
-                type=ParameterType.PARAMETER_STRING,
-                additional_constraints="Must be a valid path to a serial device file that shows up in `ls /dev/tty*`.",
-            ),
-        )
-
-        # Determine which connector to use. Options are Mock, Serial, and CAN
-        connector_select = (
-            self.get_parameter("connector").get_parameter_value().string_value
-        )
-        can_override = (
-            self.get_parameter("can_override").get_parameter_value().string_value
-        )
-        serial_override = (
-            self.get_parameter("serial_override").get_parameter_value().string_value
-        )
-        match connector_select:
-            case "serial":
-                logger.info("using serial connector")
-                self.connector = SerialConnector(
-                    logger, self.get_clock(), serial_override
-                )
-            case "can":
-                logger.info("using CAN connector")
-                self.connector = CANConnector(logger, self.get_clock(), can_override)
-            case "mock":
-                logger.info("using mock connector")
-                self.connector = MockConnector(logger, self.get_clock())
-            case "auto":
-                logger.info("automatically determining connector")
+        # 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:
-                    logger.info("trying CAN connector")
-                    self.connector = CANConnector(
-                        logger, self.get_clock(), can_override
-                    )
-                except (NoValidDeviceException, NoWorkingDeviceException, TypeError):
-                    logger.info("CAN connector failed, trying serial connector")
-                    self.connector = SerialConnector(
-                        logger, self.get_clock(), serial_override
-                    )
-            case _:
-                logger.fatal(
-                    f"invalid value for connector parameter: {connector_select}"
-                )
-                exit(1)
+                    # 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")
 
-        # ROS2 Topic Setup
+                    # 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
         
-        # Publishers
-        self.fromvic_debug_pub_ = self.create_publisher(  # only used by serial
-            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,
-        )
-        # Debug publisher
-        self.tovic_debug_pub_ = self.create_publisher(
-            String,
-            "/anchor/to_vic/debug",
-            20,
-        )
+        if self.port is None:
+            self.get_logger().info("Unable to find MCU...")
+            time.sleep(1)
+            sys.exit(1)
         
-        # Subscribers
-        self.tovic_sub_ = self.create_subscription(
-            VicCAN,
-            "/anchor/to_vic/relay",
-            self.write_connector,
-            20,
-        )
-        self.tovic_sub_legacy_ = self.create_subscription(
-            String,
-            "/anchor/relay",
-            self.write_connector_legacy,
-            20,
-        )
-        self.mock_mcu_sub_ = self.create_subscription(
-            VicCAN,
-            "/anchor/from_vic/mock_mcu",
-            self.relay_fromvic,
-            20,
-        )
-        self.tovic_string_sub_ = self.create_subscription(
-            String,
-            "/anchor/to_vic/relay_string",
-            self.write_connector_raw,
-            20,
-        )
+        self.ser = serial.Serial(self.port, 115200)
+        atexit.register(self.cleanup)
 
-        # poll at 100Hz for incoming data
-        self.read_timer_ = self.create_timer(0.01, self.read_connector)
 
-    def destroy_node(self):
-        self.get_logger().info("closing connector")
-        self.connector.cleanup()
-        super().destroy_node()
+    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()
         
-    def read_connector(self):
-        """Check the connector for new data from the MCU, and publish string to appropriate topics"""
-        viccan, raw = self.connector.read()
+        try:
+            while rclpy.ok():
+                self.read_MCU() # Check the MCU for updates
+        except KeyboardInterrupt:
+            sys.exit(0)
 
-        if raw:
-            self.fromvic_debug_pub_.publish(String(data=raw))
+    def read_MCU(self):
+        try:
+            output = str(self.ser.readline(), "utf8")
             
-        if viccan:
-            self.relay_fromvic(viccan)
+            if output:
+                # All output over debug temporarily
+                #self.get_logger().info(f"[MCU] {output}")
+                msg = String()
+                msg.data = output
+                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
+                    self.bio_pub.publish(msg)
+                # 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 write_connector(self, msg: VicCAN):
-        """Write to the connector and send a copy to /anchor/to_vic/debug"""
-        self.connector.write(msg)
-        self.tovic_debug_pub_.publish(String(data=viccan_to_string(msg)))
+    def send_cmd(self, msg):
+        message = msg.data
+        #self.get_logger().info(f"Sending command to MCU: {msg}")
+        self.ser.write(bytes(message, "utf8"))
 
-    def write_connector_raw(self, msg: String):
-        """Write raw string to the connector and send a copy to /anchor/to_vic/debug"""
-        self.connector.write_raw(msg)
-        self.tovic_debug_pub_.publish(msg)
+    @staticmethod
+    def list_serial_ports():
+        return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
     
-    @deprecated(
-        "Use /anchor/to_vic/relay or /anchor/to_vic/relay_string instead of /anchor/relay"
-    )
-    def write_connector_legacy(self, msg: String):
-        """Write to the connector by first attempting to convert String to VicCAN"""
-        # please do not reference this code. ~riley
-        for cmd in msg.data.split("\n"):
-            viccan = string_to_viccan(
-                cmd,
-                "anchor",
-                self.get_logger(),
-                self.get_clock().now().to_msg(),
-            )
-            if viccan:
-                self.write_connector(viccan)
+    def cleanup(self):
+        print("Cleaning up before terminating...")
+        if self.ser.is_open:
+            self.ser.close()
 
-    def relay_fromvic(self, msg: VicCAN):
-        """Relay a message from the MCU to the appropriate VicCAN topic"""
-        if msg.mcu_name == "core":
-            self.fromvic_core_pub_.publish(msg)
-        if msg.mcu_name == "arm" or msg.mcu_name == "digit":
-            self.fromvic_arm_pub_.publish(msg)
-        if msg.mcu_name == "citadel" or msg.mcu_name == "digit":
-            self.fromvic_bio_pub_.publish(msg)
+def myexcepthook(type, value, tb):
+    print("Uncaught exception:", type, value)
+    if serial_pub:
+        serial_pub.cleanup()
 
 
 def main(args=None):
     rclpy.init(args=args)
-    anchor_node = Anchor()
-    executor = SingleThreadedExecutor()
-    executor.add_node(anchor_node)
+    sys.excepthook = myexcepthook
 
-    try:
-        executor.spin()
-    except (KeyboardInterrupt, ExternalShutdownException):
-        pass
-    finally:
-        # don't accept any more jobs
-        executor.shutdown()
-        # make the node quit processing things
-        anchor_node.destroy_node()
-        # shut down everything else
-        rclpy.try_shutdown()
+    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/anchor_pkg/anchor_pkg/connector.py

@@ -1,441 +0,0 @@
-from abc import ABC, abstractmethod
-from astra_msgs.msg import VicCAN
-from std_msgs.msg import String
-from rclpy.clock import Clock
-from rclpy.impl.rcutils_logger import RcutilsLogger
-from .convert import string_to_viccan as _string_to_viccan, viccan_to_string
-
-# CAN
-import can
-import can.interfaces.socketcan
-import struct
-
-# Serial
-import serial
-import serial.tools.list_ports
-
-KNOWN_USBS = [
-    (0x2E8A, 0x00C0),  # Raspberry Pi Pico
-    (0x1A86, 0x55D4),  # Adafruit Feather ESP32 V2
-    (0x10C4, 0xEA60),  # DOIT ESP32 Devkit V1
-    (0x1A86, 0x55D3),  # ESP32 S3 Development Board
-]
-BAUD_RATE = 115200
-
-MCU_IDS = [
-    "broadcast",
-    "core",
-    "arm",
-    "digit",
-    "faerie",
-    "citadel",
-    "libs",
-]
-
-
-class NoValidDeviceException(Exception):
-    pass
-
-
-class NoWorkingDeviceException(Exception):
-    pass
-
-
-class MultipleValidDevicesException(Exception):
-    pass
-
-
-class DeviceClosedException(Exception):
-    pass
-
-
-class Connector(ABC):
-    logger: RcutilsLogger
-    clock: Clock
-
-    def string_to_viccan(self, msg: str, mcu_name: str):
-        """function currying so that we do not need to pass logger and clock every time"""
-        return _string_to_viccan(
-            msg,
-            mcu_name,
-            self.logger,
-            self.clock.now().to_msg(),
-        )
-
-    def __init__(self, logger: RcutilsLogger, clock: Clock):
-        self.logger = logger
-        self.clock = clock
-
-    @abstractmethod
-    def read(self) -> tuple[VicCAN | None, str | None]:
-        """
-        Must return a tuple of (VicCAN, debug message or string repr of VicCAN)
-        """
-        pass
-
-    @abstractmethod
-    def write(self, msg: VicCAN):
-        pass
-
-    @abstractmethod
-    def write_raw(self, msg: String):
-        pass
-
-    def cleanup(self):
-        pass
-
-
-class SerialConnector(Connector):
-    port: str
-    mcu_name: str
-    serial_interface: serial.Serial
-
-    def __init__(self, logger: RcutilsLogger, clock: Clock, serial_override: str = ""):
-        super().__init__(logger, clock)
-
-        ports = self._find_ports()
-        mcu_name: str | None = None
-
-        if serial_override:
-            logger.warn(
-                f"using serial_override: `{serial_override}`! this will bypass several checks."
-            )
-            ports = [serial_override]
-            mcu_name = "override"
-
-        if len(ports) <= 0:
-            raise NoValidDeviceException("no valid serial device found")
-        if (l := len(ports)) > 1:
-            raise MultipleValidDevicesException(
-                f"too many ({l}) valid serial devices found"
-            )
-
-        # check each of our ports to make sure one of them is responding
-        port = ports[0]
-        # we might already have a name by now if we overrode earlier
-        mcu_name = mcu_name or self._get_name(port)
-        if not mcu_name:
-            raise NoWorkingDeviceException(
-                f"found {port}, but it did not respond with its name"
-            )
-
-        self.port = port
-        self.mcu_name = mcu_name
-
-        # if we fail at this point, it should crash because we've already tested the port
-        self.serial_interface = serial.Serial(self.port, BAUD_RATE, timeout=1)
-
-    def _find_ports(self) -> list[str]:
-        """
-        Finds all valid ports but does not test them
-
-        returns: all valid ports
-        """
-        comports = serial.tools.list_ports.comports()
-        valid_ports = list(
-            map(  # get just device strings
-                lambda p: p.device,
-                filter(  # make sure we have a known device
-                    lambda p: (p.vid, p.pid) in KNOWN_USBS and p.device is not None,
-                    comports,
-                ),
-            )
-        )
-        self.logger.info(f"found valid MCU ports: [ {', '.join(valid_ports)} ]")
-        return valid_ports
-
-    def _get_name(self, port: str) -> str | None:
-        """
-        Get the name of the MCU (if it works)
-
-        returns: str name of the MCU, None if it doesn't work
-        """
-        # attempt to open the serial port
-        serial_interface: serial.Serial
-        try:
-            self.logger.info(f"asking {port} for its name")
-            serial_interface = serial.Serial(port, BAUD_RATE, timeout=1)
-
-            serial_interface.write(b"can_relay_mode,on\n")
-
-            for i in range(4):
-                self.logger.debug(f"attempt {i + 1} of 4 asking {port} for its name")
-                response = serial_interface.read_until(bytes("\n", "utf8"))
-                try:
-                    if b"can_relay_ready" in response:
-                        args: list[str] = response.decode("utf8").strip().split(",")
-                        if len(args) == 2:
-                            self.logger.info(f"we are talking to {args[1]}")
-                            return args[1]
-                        break
-                except UnicodeDecodeError as e:
-                    self.logger.info(
-                        f"ignoring UnicodeDecodeError when asking for MCU name: {e}"
-                    )
-
-            if serial_interface.is_open:
-                # turn relay mode off if it failed to respond with its name
-                serial_interface.write(b"can_relay_mode,off\n")
-                serial_interface.close()
-        except serial.SerialException as e:
-            self.logger.error(f"SerialException when asking for MCU name: {e}")
-
-        return None
-
-    def read(self) -> tuple[VicCAN | None, str | None]:
-        try:
-            raw = str(self.serial_interface.readline(), "utf8")
-
-            if not raw:
-                return (None, None)
-
-            return (
-                self.string_to_viccan(raw, self.mcu_name),
-                raw,
-            )
-        except serial.SerialException as e:
-            self.logger.error(f"SerialException: {e}")
-            raise DeviceClosedException(f"serial port {self.port} closed unexpectedly")
-        except Exception:
-            return (None, None)  # pretty much no other error matters
-
-    def write(self, msg: VicCAN):
-        self.write_raw(String(data=viccan_to_string(msg)))
-
-    def write_raw(self, msg: String):
-        self.serial_interface.write(bytes(msg.data, "utf8"))
-
-    def cleanup(self):
-        self.logger.info(f"closing serial port if open {self.port}")
-        try:
-            if self.serial_interface.is_open:
-                self.serial_interface.close()
-        except Exception as e:
-            self.logger.error(e)
-
-
-class CANConnector(Connector):
-    def __init__(self, logger: RcutilsLogger, clock: Clock, can_override: str):
-        super().__init__(logger, clock)
-
-        self.can_channel: str | None = None
-        self.can_bus: can.BusABC | None = None
-
-        avail = can.interfaces.socketcan.SocketcanBus._detect_available_configs()
-
-        if len(avail) == 0:
-            raise NoValidDeviceException("no CAN interfaces found")
-
-        # filter to busses whose channel matches the can_override
-        if can_override:
-            self.logger.info(f"overrode can interface with {can_override}")
-            avail = list(
-                filter(
-                    lambda b: b.get("channel") == can_override,
-                    avail,
-                )
-            )
-
-        if (l := len(avail)) > 1:
-            channels = ", ".join(str(b.get("channel")) for b in avail)
-            raise MultipleValidDevicesException(
-                f"too many ({l}) CAN interfaces found: [{channels}]"
-            )
-
-        bus = avail[0]
-        self.can_channel = str(bus.get("channel"))
-        self.logger.info(f"found CAN interface '{self.can_channel}'")
-
-        try:
-            self.can_bus = can.Bus(
-                interface="socketcan",
-                channel=self.can_channel,
-                bitrate=1_000_000,
-            )
-        except can.CanError as e:
-            raise NoWorkingDeviceException(
-                f"could not open CAN channel '{self.can_channel}': {e}"
-            )
-
-        if self.can_channel and self.can_channel.startswith("v"):
-            self.logger.warn("CAN interface is likely virtual")
-
-    def read(self) -> tuple[VicCAN | None, str | None]:
-        if not self.can_bus:
-            raise DeviceClosedException("CAN bus not initialized")
-
-        try:
-            message = self.can_bus.recv(timeout=0.0)
-        except can.CanError as e:
-            self.logger.error(f"CAN error while receiving: {e}")
-            raise DeviceClosedException("CAN bus closed unexpectedly")
-
-        if message is None:
-            return (None, None)
-
-        arbitration_id = message.arbitration_id & 0x7FF
-        data_bytes = bytes(message.data)
-
-        mcu_key = (arbitration_id >> 8) & 0b111
-        data_type_key = (arbitration_id >> 6) & 0b11
-        command = arbitration_id & 0x3F
-
-        try:
-            mcu_name = MCU_IDS[mcu_key]
-        except IndexError:
-            self.logger.warn(
-                f"received CAN frame with unknown MCU key {mcu_key}; id=0x{arbitration_id:X}"
-            )
-            return (None, None)
-
-        data: list[float] = []
-
-        try:
-            if data_type_key == 3:
-                data = []
-            elif data_type_key == 0:
-                if len(data_bytes) < 8:
-                    self.logger.warn(
-                        f"received double payload with insufficient length {len(data_bytes)}; dropping frame"
-                    )
-                    return (None, None)
-                (value,) = struct.unpack(">d", data_bytes[:8])
-                data = [float(value)]
-            elif data_type_key == 1:
-                if len(data_bytes) < 8:
-                    self.logger.warn(
-                        f"received float32x2 payload with insufficient length {len(data_bytes)}; dropping frame"
-                    )
-                    return (None, None)
-                v1, v2 = struct.unpack(">ff", data_bytes[:8])
-                data = [float(v1), float(v2)]
-            elif data_type_key == 2:
-                if len(data_bytes) < 8:
-                    self.logger.warn(
-                        f"received int16x4 payload with insufficient length {len(data_bytes)}; dropping frame"
-                    )
-                    return (None, None)
-                i1, i2, i3, i4 = struct.unpack(">hhhh", data_bytes[:8])
-                data = [float(i1), float(i2), float(i3), float(i4)]
-            else:
-                self.logger.warn(
-                    f"received CAN frame with unknown data_type_key {data_type_key}; id=0x{arbitration_id:X}"
-                )
-                return (None, None)
-        except struct.error as e:
-            self.logger.error(f"error unpacking CAN payload: {e}")
-            return (None, None)
-
-        viccan = VicCAN(
-            mcu_name=mcu_name,
-            command_id=int(command),
-            data=data,
-        )
-
-        self.logger.debug(
-            f"received CAN frame id=0x{message.arbitration_id:X}, "
-            f"decoded as VicCAN(mcu_name={viccan.mcu_name}, command_id={viccan.command_id}, data={viccan.data})"
-        )
-
-        return (
-            viccan,
-            f"{viccan.mcu_name},{viccan.command_id},"
-            + ",".join(map(str, list(viccan.data))),
-        )
-
-    def write(self, msg: VicCAN):
-        if not self.can_bus:
-            raise DeviceClosedException("CAN bus not initialized")
-
-        # build 11-bit arbitration ID according to VicCAN spec:
-        # bits 10..8: targeted MCU key
-        # bits 7..6:  data type key
-        # bits 5..0:  command
-
-        # map MCU name to 3-bit key.
-        try:
-            mcu_id = MCU_IDS.index((msg.mcu_name or "").lower())
-        except ValueError:
-            self.logger.error(
-                f"unknown VicCAN mcu_name '{msg.mcu_name}' for CAN frame; dropping message"
-            )
-            return
-
-        # determine data type from length:
-        # 0: double x1, 1: float32 x2, 2: int16 x4, 3: empty
-        match data_len := len(msg.data):
-            case 0:
-                data_type = 3
-                data = bytes()
-            case 1:
-                data_type = 0
-                data = struct.pack(">d", *msg.data)
-            case 2:
-                data_type = 1
-                data = struct.pack(">ff", *msg.data)
-            case 3 | 4:  # 3 gets treated as 4
-                data_type = 2
-                if data_len == 3:
-                    msg.data.append(0)
-                data = struct.pack(">hhhh", *[int(x) for x in msg.data])
-            case _:
-                self.logger.error(
-                    f"unexpected VicCAN data length: {data_len}; dropping message"
-                )
-                return
-
-        # command is limited to 6 bits.
-        command = int(msg.command_id)
-        if command < 0 or command > 0x3F:
-            self.logger.error(
-                f"invalid command_id for CAN frame: {command}; dropping message"
-            )
-            return
-
-        try:
-            can_message = can.Message(
-                arbitration_id=(mcu_id << 8) | (data_type << 6) | command,
-                data=data,
-                is_extended_id=False,
-            )
-        except Exception as e:
-            self.logger.error(f"failed to construct CAN message: {e}")
-            return
-
-        try:
-            self.can_bus.send(can_message)
-            self.logger.debug(
-                f"sent CAN frame id=0x{can_message.arbitration_id:X}, "
-                f"data={list(can_message.data)}"
-            )
-        except can.CanError as e:
-            self.logger.error(f"CAN error while sending: {e}")
-            raise DeviceClosedException("CAN bus closed unexpectedly")
-
-    def write_raw(self, msg: String):
-        self.logger.warn(
-            f"write_raw is not supported for CANConnector. msg: {msg.data}"
-        )
-
-    def cleanup(self):
-        try:
-            if self.can_bus is not None:
-                self.logger.info("shutting down CAN bus")
-                self.can_bus.shutdown()
-        except Exception as e:
-            self.logger.error(e)
-
-
-class MockConnector(Connector):
-    def __init__(self, logger: RcutilsLogger, clock: Clock):
-        super().__init__(logger, clock)
-        # No hardware interface for MockConnector. Publish to `/anchor/from_vic/mock_mcu` instead.
-
-    def read(self) -> tuple[VicCAN | None, str | None]:
-        return (None, None)
-
-    def write(self, msg: VicCAN):
-        pass
-
-    def write_raw(self, msg: String):
-        pass

src/anchor_pkg/anchor_pkg/convert.py

@@ -1,65 +0,0 @@
-from astra_msgs.msg import VicCAN
-from std_msgs.msg import Header
-from builtin_interfaces.msg import Time
-from rclpy.impl.rcutils_logger import RcutilsLogger
-
-
-def string_to_viccan(
-    msg: str, mcu_name: str, logger: RcutilsLogger, time: Time
-) -> VicCAN | None:
-    """
-    Converts the serial string VicCAN format to a ROS2 VicCAN message.
-    Does not fill out the Header of the message.
-    On a failure, it will log at a debug level why it failed and return None.
-    """
-
-    parts: list[str] = msg.strip().split(",")
-
-    # don't need an extra check because len of .split output is always >= 1
-    if not parts[0].startswith("can_relay_"):
-        logger.debug(f"got non-CAN data from {mcu_name}: {msg}")
-        return None
-    elif len(parts) < 3:
-        logger.debug(f"got garbage (not enough parts) CAN data from {mcu_name}: {msg}")
-        return None
-    elif len(parts) > 7:
-        logger.debug(f"got garbage (too many parts) CAN data from {mcu_name}: {msg}")
-        return None
-
-    try:
-        command_id = int(parts[2])
-    except ValueError:
-        logger.debug(
-            f"got garbage (non-integer command id) CAN data from {mcu_name}: {msg}"
-        )
-        return None
-
-    if command_id not in range(64):
-        logger.debug(
-            f"got garbage (wrong command id {command_id}) CAN data from {mcu_name}: {msg}"
-        )
-        return None
-
-    try:
-        return VicCAN(
-            header=Header(
-                stamp=time,
-                frame_id="from_vic",
-            ),
-            mcu_name=parts[1],
-            command_id=command_id,
-            data=[float(x) for x in parts[3:]],
-        )
-    except ValueError:
-        logger.debug(f"got garbage (non-numerical) CAN data from {mcu_name}: {msg}")
-        return None
-
-
-def viccan_to_string(viccan: VicCAN) -> str:
-    """Converts a ROS2 VicCAN message to the serial string VicCAN format."""
-    # make sure we accept 3 digits and treat it as 4
-    if len(viccan.data) == 3:
-        viccan.data.append(0)
-    # go from [ w, x, y, z ] -> ",w,x,y,z" & round to 7 digits max
-    data = "".join([f",{round(val,7)}" for val in viccan.data])
-    return f"can_relay_tovic,{viccan.mcu_name},{viccan.command_id}{data}\n"

src/anchor_pkg/launch/rover.launch.py

@@ -1,111 +0,0 @@
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, Shutdown
-from launch.conditions import IfCondition
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
-
-
-def generate_launch_description():
-    connector = LaunchConfiguration("connector")
-    serial_override = LaunchConfiguration("serial_override")
-    can_override = LaunchConfiguration("can_override")
-    use_ptz = LaunchConfiguration("use_ptz")
-
-    ld = LaunchDescription()
-
-    # arguments
-    ld.add_action(
-        DeclareLaunchArgument(
-            "connector",
-            default_value="auto",
-            description="Connector parameter for anchor node (default: 'auto')",
-        )
-    )
-
-    ld.add_action(
-        DeclareLaunchArgument(
-            "serial_override",
-            default_value="",
-            description="Serial port override parameter for anchor node (default: '')",
-        )
-    )
-
-    ld.add_action(
-        DeclareLaunchArgument(
-            "can_override",
-            default_value="",
-            description="CAN network override parameter for anchor node (default: '')",
-        )
-    )
-
-    ld.add_action(
-        DeclareLaunchArgument(
-            "use_ptz",
-            default_value="true",  # must be string for launch system
-            description="Whether to launch PTZ node (default: true)",
-        )
-    )
-
-    # nodes
-    ld.add_action(
-        Node(
-            package="arm_pkg",
-            executable="arm",
-            name="arm",
-            output="both",
-            parameters=[{"launch_mode": "anchor"}],
-            on_exit=Shutdown(),
-        )
-    )
-
-    ld.add_action(
-        Node(
-            package="core_pkg",
-            executable="core",
-            name="core",
-            output="both",
-            parameters=[{"launch_mode": "anchor"}],
-            on_exit=Shutdown(),
-        )
-    )
-
-    ld.add_action(
-        Node(
-            package="core_pkg",
-            executable="ptz",
-            name="ptz",
-            output="both",
-            condition=IfCondition(use_ptz),
-        )
-    )
-
-    ld.add_action(
-        Node(
-            package="bio_pkg",
-            executable="bio",
-            name="bio",
-            output="both",
-            parameters=[{"launch_mode": "anchor"}],
-            on_exit=Shutdown(),
-        )
-    )
-
-    ld.add_action(
-        Node(
-            package="anchor_pkg",
-            executable="anchor",
-            name="anchor",
-            output="both",
-            parameters=[
-                {
-                    "launch_mode": "anchor",
-                    "connector": connector,
-                    "serial_override": serial_override,
-                    "can_override": can_override,
-                }
-            ],
-            on_exit=Shutdown(),
-        )
-    )
-
-    return ld

src/anchor_pkg/package.xml

@@ -3,16 +3,11 @@
 <package format="3">
   <name>anchor_pkg</name>
   <version>0.0.0</version>
-  <description>Anchor -- ROS and CAN relay node</description>
-  <maintainer email="rjm0037@uah.edu">Riley</maintainer>
-  <license>AGPL-3.0-only</license>
+  <description>TODO: Package description</description>
+  <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
+  <license>TODO: License declaration</license>
 
   <depend>rclpy</depend>
-  <depend>common_interfaces</depend>
-  <depend>python3-serial</depend>
-  <depend>python3-can</depend>
-
-  <build_depend>black</build_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/anchor_pkg/setup.py

@@ -1,25 +1,25 @@
 from setuptools import find_packages, setup
-from os import path
-from glob import glob
 
-package_name = "anchor_pkg"
+package_name = 'anchor_pkg'
 
 setup(
     name=package_name,
-    version="0.0.0",
-    packages=find_packages(exclude=["test"]),
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
     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/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
     ],
-    install_requires=["setuptools"],
+    install_requires=['setuptools'],
     zip_safe=True,
-    maintainer="tristan",
-    maintainer_email="tristanmcginnis26@gmail.com",
-    description="Anchor node used to run all modules through a single modules MCU/Computer. Commands to all modules will be relayed through CAN",
-    license="All Rights Reserved",
+    maintainer='tristan',
+    maintainer_email='tristanmcginnis26@gmail.com',
+    description='Anchor node used to run all modules through a single modules MCU/Computer. Commands to all modules will be relayed through CAN',
+    license='All Rights Reserved',
     entry_points={
-        "console_scripts": ["anchor = anchor_pkg.anchor_node:main"],
+        'console_scripts': [
+            "anchor = anchor_pkg.anchor_node:main"
+        ],
     },
 )

src/arm_pkg/arm_pkg/arm_headless.py

@@ -0,0 +1,303 @@
+import rclpy
+from rclpy.node import Node
+
+import pygame
+
+import time
+
+import serial
+import sys
+import threading
+import glob
+import os
+
+from std_msgs.msg import String
+from ros2_interfaces_pkg.msg import ControllerState
+from ros2_interfaces_pkg.msg import ArmManual
+from ros2_interfaces_pkg.msg import ArmIK
+
+
+os.environ["SDL_AUDIODRIVER"] = "dummy"  # Force pygame to use a dummy audio driver before pygame.init()
+os.environ["SDL_VIDEODRIVER"] = "dummy"  # Prevents pygame from trying to open a display
+
+class Headless(Node):
+    def __init__(self):
+        # Initalize node with name
+        super().__init__("arm_headless")
+
+        # Depricated, kept temporarily for reference
+        # self.create_timer(0.20, self.send_controls)#read and send controls
+
+        self.create_timer(0.1, self.send_manual)
+
+
+        # Create a publisher to publish any output the pico sends
+
+        # Depricated, kept temporarily for reference
+        #self.publisher = self.create_publisher(ControllerState, '/astra/arm/control', 10) 
+        self.manual_pub = self.create_publisher(ArmManual, '/arm/control/manual', 10)
+
+        # Create a subscriber to listen to any commands sent for the pico
+        
+        # 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.laser_status = 0
+
+        # Initialize pygame
+        pygame.init()
+
+        # Initialize the gamepad module
+        pygame.joystick.init()
+
+        # 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()}')
+        #
+        #
+
+
+    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.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.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_manual(self):
+        for event in pygame.event.get():
+            if event.type == pygame.QUIT:
+                pygame.quit()
+                exit()
+        input = ArmManual()
+
+
+        # Triggers for gripper control
+        if self.gamepad.get_axis(2) > 0:#left trigger
+            input.gripper = -1
+        elif self.gamepad.get_axis(5) > 0:#right trigger
+            input.gripper = 1
+
+        # Toggle Laser
+        if self.gamepad.get_button(7):#Start
+            self.laser_status = 1
+        elif self.gamepad.get_button(6):#Back
+            self.laser_status = 0
+        input.laser = self.laser_status
+
+        if self.gamepad.get_button(5):#right bumper, control effector
+
+            # Left stick X-axis for effector yaw
+            if self.gamepad.get_axis(0) > 0:
+                input.effector_yaw = 1
+            elif self.gamepad.get_axis(0) < 0:
+                input.effector_yaw = -1
+
+            # Right stick X-axis for effector roll
+            if self.gamepad.get_axis(3) > 0:
+                input.effector_roll = 1
+            elif self.gamepad.get_axis(3) < 0:
+                input.effector_roll = -1
+
+        else:   # Control arm axis
+            dpad_input = self.gamepad.get_hat(0)
+            input.axis0 = 0
+            if dpad_input[0] == 1:
+                input.axis0 = 1
+            elif dpad_input[0] == -1:
+                input.axis0 = -1
+
+            if self.gamepad.get_axis(0) > .15 or self.gamepad.get_axis(0) < -.15:
+                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))
+
+            if self.gamepad.get_axis(4) > .15 or self.gamepad.get_axis(4) < -.15:
+                input.axis3 = -1 * round(self.gamepad.get_axis(4))
+
+            # input.axis1 = -1 * round(self.gamepad.get_axis(0))#left x-axis
+            # input.axis2 = -1 * round(self.gamepad.get_axis(1))#left y-axis
+            # input.axis3 = -1 * round(self.gamepad.get_axis(4))#right y-axis
+
+
+        #Button Mappings
+        #axis2 -> LT
+        #axis5 -> RT
+        #Buttons0 -> A
+        #Buttons1 -> B
+        #Buttons2 -> X
+        #Buttons3 -> Y
+        #Buttons4 -> LB
+        #Buttons5 -> RB
+        #Buttons6 -> Back
+        #Buttons7 -> Start
+
+        input.linear_actuator = 0
+
+
+        if pygame.joystick.get_count() != 0:
+        
+            self.get_logger().info(f"[Ctrl] {input.axis0}, {input.axis1}, {input.axis2}, {input.axis3}\n")
+            self.manual_pub.publish(input)
+        else:
+            pass
+
+        pass
+        
+    # Depricated, kept temporarily for reference
+    # def send_controls(self):
+
+    #     for event in pygame.event.get():
+    #         if event.type == pygame.QUIT:
+    #             pygame.quit()
+    #             exit()
+    #     input = ControllerState()
+
+    #     input.lt = self.gamepad.get_axis(2)#left trigger
+    #     input.rt = self.gamepad.get_axis(5)#right trigger
+        
+    #     #input.lb = self.gamepad.get_button(9)#Value must be converted to bool
+    #     if(self.gamepad.get_button(4)):#left bumper
+    #         input.lb = True
+    #     else:
+    #         input.lb = False
+
+    #     #input.rb = self.gamepad.get_button(10)#Value must be converted to bool
+    #     if(self.gamepad.get_button(5)):#right bumper
+    #         input.rb = True
+    #     else:
+    #         input.rb = False
+        
+    #     #input.plus = self.gamepad.get_button(6)#plus button
+    #     if(self.gamepad.get_button(7)):#plus button
+    #         input.plus = True
+    #     else:
+    #         input.plus = False
+
+    #     #input.minus = self.gamepad.get_button(4)#minus button
+    #     if(self.gamepad.get_button(6)):#minus button
+    #         input.minus = True
+    #     else:
+    #         input.minus = False
+
+    #     input.ls_x = round(self.gamepad.get_axis(0),2)#left x-axis
+    #     input.ls_y = round(self.gamepad.get_axis(1),2)#left y-axis
+    #     input.rs_x = round(self.gamepad.get_axis(3),2)#right x-axis
+    #     input.rs_y = round(self.gamepad.get_axis(4),2)#right y-axis  
+
+    #     #input.a = self.gamepad.get_button(1)#A button
+    #     if(self.gamepad.get_button(0)):#A button
+    #         input.a = True
+    #     else:
+    #         input.a = False
+    #     #input.b = self.gamepad.get_button(0)#B button
+    #     if(self.gamepad.get_button(1)):#B button
+    #         input.b = True
+    #     else:
+    #         input.b = False
+    #     #input.x = self.gamepad.get_button(3)#X button
+    #     if(self.gamepad.get_button(2)):#X button
+    #         input.x = True
+    #     else:
+    #         input.x = False
+    #     #input.y = self.gamepad.get_button(2)#Y button
+    #     if(self.gamepad.get_button(3)):#Y button
+    #         input.y = True
+    #     else:
+    #         input.y = False
+
+
+    #     dpad_input = self.gamepad.get_hat(0)#D-pad input
+
+    #     #not using up/down on DPad
+    #     input.d_up = False
+    #     input.d_down = False
+
+
+    #     if(dpad_input[0] == 1):#D-pad right
+    #         input.d_right = True
+    #     else:
+    #         input.d_right = False
+    #     if(dpad_input[0] == -1):#D-pad left
+    #         input.d_left = True
+    #     else:
+    #         input.d_left = False
+       
+
+    #     if pygame.joystick.get_count() != 0:
+        
+    #         self.get_logger().info(f"[Ctrl] Updated Controller State\n")
+
+    #         self.publisher.publish(input)
+    #     else:
+    #         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}")
+
+        
+
+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/arm_pkg/arm_pkg/arm_node.py

@@ -1,258 +1,189 @@
-import sys
-import threading
-import signal
-import math
-from warnings import deprecated
-
 import rclpy
 from rclpy.node import Node
-from rclpy.executors import ExternalShutdownException
-from rclpy import qos
-
-from std_msgs.msg import String, Header
-from sensor_msgs.msg import JointState
-from control_msgs.msg import JointJog
-from astra_msgs.msg import SocketFeedback, DigitFeedback, ArmManual  # TODO: Old topics
-from astra_msgs.msg import ArmFeedback, VicCAN, RevMotorState
-
-control_qos = qos.QoSProfile(
-    history=qos.QoSHistoryPolicy.KEEP_LAST,
-    depth=2,
-    reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,  # Best Effort subscribers are still compatible with Reliable publishers
-    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),
-)
+import serial
+import sys
+import threading
+import glob
+import time
+import atexit
+import signal
+from std_msgs.msg import String
+from ros2_interfaces_pkg.msg import ArmManual
+from ros2_interfaces_pkg.msg import ArmIK
+from ros2_interfaces_pkg.msg import SocketFeedback
+from ros2_interfaces_pkg.msg import DigitFeedback
 
+serial_pub = None
 thread = None
 
-
-class ArmNode(Node):
-    """Relay between Anchor and Basestation/Headless/Moveit2 for Arm related topics."""
-
-    # Every non-fixed joint defined in Arm's URDF
-    # Used for JointState and JointJog messsages
-    all_joint_names = [
-        "axis_0_joint",
-        "axis_1_joint",
-        "axis_2_joint",
-        "axis_3_joint",
-        "wrist_yaw_joint",
-        "wrist_roll_joint",
-        "ef_gripper_left_joint",
-    ]
-
-    # Used to verify the length of an incoming VicCAN feedback message
-    # Key is VicCAN command_id, value is expected length of data list
-    viccan_socket_msg_len_dict = {
-        53: 4,
-        54: 4,
-        55: 4,
-        58: 4,
-        59: 4,
-    }
-
-    viccan_digit_msg_len_dict = {
-        54: 4,
-        55: 2,
-        59: 2,
-    }
-
+class SerialRelay(Node):
     def __init__(self):
+        # Initialize node
         super().__init__("arm_node")
 
-        self.get_logger().info(f"arm launch_mode is: anchor")  # Hey I like the output
-
-        ##################################################
-        # Parameters
-
-        self.declare_parameter("use_old_topics", True)
-        self.use_old_topics = (
-            self.get_parameter("use_old_topics").get_parameter_value().bool_value
-        )
-
-        ##################################################
-        # Old topics
-
-        if self.use_old_topics:
-            # Anchor topics
-            self.anchor_sub = self.create_subscription(
-                String, "/anchor/arm/feedback", self.anchor_feedback, 10
-            )
-            self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)
+        # Get launch mode parameter
+        self.declare_parameter('launch_mode', 'arm')
+        self.launch_mode = self.get_parameter('launch_mode').value
+        self.get_logger().info(f"arm launch_mode is: {self.launch_mode}")
 
         # Create publishers
-            self.socket_pub = self.create_publisher(
-                SocketFeedback, "/arm/feedback/socket", 10
-            )
-            self.arm_feedback = SocketFeedback()
-            self.digit_pub = self.create_publisher(
-                DigitFeedback, "/arm/feedback/digit", 10
-            )
-            self.digit_feedback = DigitFeedback()
-            self.feedback_timer = self.create_timer(0.25, self.publish_feedback)
+        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(1.0, self.publish_feedback)
 
         # Create subscribers
-            self.man_sub = self.create_subscription(
-                ArmManual, "/arm/control/manual", self.send_manual, 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)
 
-        ###################################################
-        # New topics
+        # 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)
 
-        # Anchor topics
+        self.arm_feedback = SocketFeedback()
+        self.digit_feedback = DigitFeedback()
 
-        # from_vic
-        self.anchor_fromvic_sub_ = self.create_subscription(
-            VicCAN, "/anchor/from_vic/arm", self.relay_fromvic, 20
-        )
-        # to_vic
-        self.anchor_tovic_pub_ = self.create_publisher(
-            VicCAN, "/anchor/to_vic/relay", 20
-        )
+        # Search for ports IF in 'arm' (standalone) and not 'anchor' mode
+        if self.launch_mode == 'arm':
+            # 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)
+                        #print(f"Checking port {port}...")
+                        ser.write(b"ping\n")
+                        response = ser.read_until("\n")
 
-        # Control
+                        # 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 not None:
+                    break
             
-        # Manual: /arm/manual/joint_jog is published by Basestation or Headless
-        self.man_jointjog_sub_ = self.create_subscription(
-            JointJog,
-            "/arm/manual/joint_jog",
-            self.jointjog_callback,
-            qos_profile=control_qos,
-        )
-        # IK: /joint_commands is published by JointTrajectoryController via topic_based_control
-        self.joint_command_sub_ = self.create_subscription(
-            JointState,
-            "/joint_commands",
-            self.joint_command_callback,
-            qos_profile=control_qos,
-        )
+            if self.port is None:
+                self.get_logger().info("Unable to find MCU... please make sure it is connected.")
+                time.sleep(1)
+                sys.exit(1)
             
-        # Feedback
+            self.ser = serial.Serial(self.port, 115200)
+            atexit.register(self.cleanup)
 
-        # Combined Socket and Digit feedback
-        self.arm_feedback_pub_ = self.create_publisher(
-            ArmFeedback,
-            "/arm/feedback",
-            qos_profile=qos.qos_profile_sensor_data,
-        )
-        # IK arm pose: /joint_states is published from here to topic_based_control
-        self.joint_state_pub_ = self.create_publisher(
-            JointState, "/joint_states", qos_profile=qos.qos_profile_sensor_data
-        )
+    def run(self):
+        global thread
+        thread = threading.Thread(target=rclpy.spin, args=(self,), daemon=True)
+        thread.start()
         
-        ###################################################
-        # Saved state
+        #if in arm mode, will need to read from the MCU 
 
-        # Combined Socket and Digit feedback
-        self.arm_feedback_new = ArmFeedback()
+        try:
+            while rclpy.ok():
+                if self.launch_mode == 'arm':
+                    if self.ser.in_waiting:
+                        self.read_mcu()
+                    else:
+                        time.sleep(0.1)
+        except KeyboardInterrupt:
+            pass
+        finally:
+            self.cleanup()
 
-        # IK Arm pose
-        self.saved_joint_state = JointState()
-        self.saved_joint_state.name = self.all_joint_names
-        # ... initialize with zeros
-        self.saved_joint_state.position = [0.0] * len(self.saved_joint_state.name)
-        self.saved_joint_state.velocity = [0.0] * len(self.saved_joint_state.name)
 
-    def jointjog_callback(self, msg: JointJog):
-        if len(msg.joint_names) != len(msg.velocities):
-            self.get_logger().debug("Ignoring malformed /arm/manual/joint_jog message.")
-            return
+    #Currently will just spit out all values over the /arm/feedback/debug topic as strings
+    def read_mcu(self):
+        try:
+            output = str(self.ser.readline(), "utf8")
+            if output:
+                #self.get_logger().info(f"[MCU] {output}")
+                msg = String()
+                msg.data = output
+                self.debug_pub.publish(msg)
+        except serial.SerialException:
+            self.get_logger().info("SerialException caught... closing serial port.")
+            if self.ser.is_open:
+                self.ser.close()
+            pass
+        except TypeError as e:
+            self.get_logger().info(f"TypeError: {e}")
+            print("Closing serial port.")
+            if self.ser.is_open:
+                self.ser.close()
+            pass
+        except Exception as e:
+            print(f"Exception: {e}")
+            print("Closing serial port.")
+            if self.ser.is_open:
+                self.ser.close()
+            pass
 
-        # Grab velocities from message
-        velocities = [
-            (
-                msg.velocities[msg.joint_names.index(joint_name)]  # type: ignore
-                if joint_name in msg.joint_names
-                else 0.0
-            )
-            for joint_name in self.all_joint_names
-        ]
-        # Deadzone
-        velocities = [vel if abs(vel) > 0.05 else 0.0 for vel in velocities]
+    def send_ik(self, msg):
+        pass
 
-        self.send_velocities(velocities, msg.header)
-
-        # TODO: use msg.duration
-
-    def joint_command_callback(self, msg: JointState):
-        if len(msg.position) < 7 and len(msg.velocity) < 7:
-            self.get_logger().debug("Ignoring malformed /joint_command message.")
-            return  # command needs either position or velocity for all 7 joints
-
-        # Grab velocities from message
-        velocities = [
-            (
-                msg.velocity[msg.name.index(joint_name)]  # type: ignore
-                if joint_name in msg.name
-                else 0.0
-            )
-            for joint_name in self.all_joint_names
-        ]
-
-        self.send_velocities(velocities, msg.header)
-
-    def send_velocities(self, velocities: list[float], header: Header):
-        # ROS2's rad/s to VicCAN's deg/s*10; don't convert gripper's m/s
-        velocities = [
-            math.degrees(vel) * 10 if i < 6 else vel for i, vel in enumerate(velocities)
-        ]
-
-        # Send Axis 0-3
-        self.anchor_tovic_pub_.publish(
-            VicCAN(mcu_name="arm", command_id=43, data=velocities[0:3], header=header)
-        )
-        # Send Wrist yaw and roll
-        # TODO: Verify embedded
-        self.anchor_tovic_pub_.publish(
-            VicCAN(mcu_name="digit", command_id=43, data=velocities[4:5], header=header)
-        )
-        # Send End Effector Gripper
-        # TODO: Verify m/s received correctly by embedded
-        self.anchor_tovic_pub_.publish(
-            VicCAN(mcu_name="digit", command_id=26, data=[velocities[6]], header=header)
-        )
-
-    @deprecated("Uses an old message type. Will be removed at some point.")
-    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 = f"can_relay_tovic,arm,18,{int(msg.brake)}\n"
-        command += f"can_relay_tovic,arm,39,{axis0},{axis1},{axis2},{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"
+        #self.send_cmd(command)
         
-        command += f"can_relay_tovic,digit,39,{msg.effector_yaw},{msg.effector_roll}\n"
+        command += "can_relay_tovic,digit,36,0," + str(msg.effector_yaw) + "\n"
+        #self.send_cmd(command)
 
-        command += f"can_relay_tovic,digit,26,{msg.gripper}\n"  # no hardware rn
-
-        command += f"can_relay_tovic,digit,28,{msg.laser}\n"
-
-        command += f"can_relay_tovic,digit,34,{msg.linear_actuator}\n"
+        command += "can_relay_tovic,digit,26," + str(msg.gripper) + "\n"
+        #self.send_cmd(command)
 
+        command += "can_relay_tovic,digit,28," + str(msg.laser) + "\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
     
-    @deprecated("Uses an old message type. Will be removed at some point.")
     def send_cmd(self, msg: str):
-        output = String(data=msg)
+        if self.launch_mode == 'anchor': #if in anchor mode, send to anchor node to relay
+            output = String()
+            output.data = msg
             self.anchor_pub.publish(output)
+        elif self.launch_mode == 'arm': #if in standalone mode, send to MCU directly
+            self.get_logger().info(f"[Arm to MCU] {msg}")
+            self.ser.write(bytes(msg, "utf8"))
 
-    @deprecated("Uses an old message type. Will be removed at some point.")
     def anchor_feedback(self, msg: String):
         output = msg.data
         if output.startswith("can_relay_fromvic,arm,55"):
+            #pass
             self.updateAngleFeedback(output)
         elif output.startswith("can_relay_fromvic,arm,54"):
+            #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(",")
@@ -267,134 +198,13 @@ class ArmNode(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
 
-    def relay_fromvic(self, msg: VicCAN):
-        # Code for socket and digit are broken out for cleaner code
-        if msg.mcu_name == "arm":
-            self.process_fromvic_arm(msg)
-        elif msg.mcu_name == "digit":
-            self.process_fromvic_digit(msg)
-
-    def process_fromvic_arm(self, msg: VicCAN):
-        assert msg.mcu_name == "arm"
-
-        # Check message len to prevent crashing on bad data
-        if msg.command_id in self.viccan_socket_msg_len_dict:
-            expected_len = self.viccan_socket_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:
-            case 53:  # REV SPARK MAX feedback
-                motorId = round(msg.data[0])
-                motor: RevMotorState | None = None
-                match motorId:
-                    case 1:
-                        motor = self.arm_feedback_new.axis1_motor
-                    case 2:
-                        motor = self.arm_feedback_new.axis2_motor
-                    case 3:
-                        motor = self.arm_feedback_new.axis3_motor
-                    case 4:
-                        motor = self.arm_feedback_new.axis0_motor
-
-                if motor:
-                    motor.temperature = float(msg.data[1]) / 10.0
-                    motor.voltage = float(msg.data[2]) / 10.0
-                    motor.current = float(msg.data[3]) / 10.0
-                    motor.header.stamp = msg.header.stamp
-
-                self.arm_feedback_pub_.publish(self.arm_feedback_new)
-            case 54:  # Board voltages
-                self.arm_feedback_new.socket_voltage.vbatt = float(msg.data[0]) / 100.0
-                self.arm_feedback_new.socket_voltage.v12 = float(msg.data[1]) / 100.0
-                self.arm_feedback_new.socket_voltage.v5 = float(msg.data[2]) / 100.0
-                self.arm_feedback_new.socket_voltage.v3 = float(msg.data[3]) / 100.0
-                self.arm_feedback_new.socket_voltage.header.stamp = msg.header.stamp
-            case 55:  # Arm joint positions
-                angles = [angle / 10.0 for angle in msg.data]  # VicCAN sends deg*10
-                # Joint state publisher for URDF visualization
-                self.saved_joint_state.position[0] = math.radians(angles[0])  # Axis 0
-                self.saved_joint_state.position[1] = math.radians(angles[1])  # Axis 1
-                self.saved_joint_state.position[2] = math.radians(angles[2])  # Axis 2
-                self.saved_joint_state.position[3] = math.radians(angles[3])  # Axis 3
-                # Wrist is handled by digit feedback
-                self.saved_joint_state.header.stamp = msg.header.stamp
-                self.joint_state_pub_.publish(self.saved_joint_state)
-            case 58:  # REV SPARK MAX position and velocity feedback
-                motorId = round(msg.data[0])
-                motor: RevMotorState | None = None
-                match motorId:
-                    case 1:
-                        motor = self.arm_feedback_new.axis1_motor
-                    case 2:
-                        motor = self.arm_feedback_new.axis2_motor
-                    case 3:
-                        motor = self.arm_feedback_new.axis3_motor
-                    case 4:
-                        motor = self.arm_feedback_new.axis0_motor
-
-                if motor:
-                    motor.position = float(msg.data[1])
-                    motor.velocity = float(msg.data[2])
-                    motor.header.stamp = msg.header.stamp
-
-                self.arm_feedback_pub_.publish(self.arm_feedback_new)
-            case 59:  # Arm joint velocities
-                velocities = [vel / 100.0 for vel in msg.data]  # VicCAN sends deg/s*100
-                self.saved_joint_state.velocity[0] = math.radians(
-                    velocities[0]
-                )  # Axis 0
-                self.saved_joint_state.velocity[1] = math.radians(
-                    velocities[1]
-                )  # Axis 1
-                self.saved_joint_state.velocity[2] = math.radians(
-                    velocities[2]
-                )  # Axis 2
-                self.saved_joint_state.velocity[3] = math.radians(
-                    velocities[3]
-                )  # Axis 3
-                # Wrist is handled by digit feedback
-                self.saved_joint_state.header.stamp = msg.header.stamp
-                self.joint_state_pub_.publish(self.saved_joint_state)
-
-    def process_fromvic_digit(self, msg: VicCAN):
-        assert msg.mcu_name == "digit"
-
-        # Check message len to prevent crashing on bad data
-        if msg.command_id in self.viccan_digit_msg_len_dict:
-            expected_len = self.viccan_digit_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:
-            case 54:  # Board voltages
-                self.arm_feedback_new.digit_voltage.vbatt = float(msg.data[0]) / 100.0
-                self.arm_feedback_new.digit_voltage.v12 = float(msg.data[1]) / 100.0
-                self.arm_feedback_new.digit_voltage.v5 = float(msg.data[2]) / 100.0
-            case 55:  # Arm joint positions
-                self.saved_joint_state.position[4] = math.radians(
-                    msg.data[0]
-                )  # Wrist roll
-                self.saved_joint_state.position[5] = math.radians(
-                    msg.data[1]
-                )  # Wrist yaw
-
-    @deprecated("Uses an old message type. Will be removed at some point.")
     def publish_feedback(self):
         self.socket_pub.publish(self.arm_feedback)
         self.digit_pub.publish(self.digit_feedback)
 
-    @deprecated("Uses an old message type. Will be removed at some point.")
     def updateAngleFeedback(self, msg: str):
                 # Angle feedbacks,
         #split the msg.data by commas
@@ -405,15 +215,32 @@ class ArmNode(Node):
             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
+            #
+            #
+            #THIS NEEDS TO BE REMOVED LATER
+            #PLACEHOLDER FOR WRIST VALUE 
+            #
+            #
+            angles.append(0.0)#placeholder for wrist_continuous
+            angles.append(0.0)#placeholder for wrist
+            #
+            #
+            # # Update the arm's current 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]
             self.arm_feedback.axis3_angle = angles[3]
+            # self.get_logger().info(f"Angles: {angles}")
+            # #debug publish angles
+            # tempMsg = String()
+            # tempMsg.data = "Angles: " + str(angles)
+            # #self.debug_pub.publish(tempMsg)
         else:
             self.get_logger().info("Invalid angle feedback input format")
 
-    @deprecated("Uses an old message type. Will be removed at some point.")
+
     def updateBusVoltage(self, msg: str):
         # Bus Voltage feedbacks
         parts = msg.split(",")
@@ -428,52 +255,50 @@ class ArmNode(Node):
         else:
             self.get_logger().info("Invalid voltage feedback input format")
 
-    @deprecated("Uses an old message type. Will be removed at some point.")
-    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")
 
 
-def exit_handler(signum, frame):
-    print("Caught SIGTERM. Exiting...")
-    rclpy.try_shutdown()
-    sys.exit(0)
+    @staticmethod
+    def list_serial_ports():
+        return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
+        #return glob.glob("/dev/tty[A-Za-z]*")
 
+    def cleanup(self):
+        print("Cleaning up...")
+        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
 
-    # Catch termination signals and exit cleanly
-    signal.signal(signal.SIGTERM, exit_handler)
+    global serial_pub
+    serial_pub = SerialRelay()
+    serial_pub.run()
 
-    arm_node = ArmNode()
-
-    try:
-        rclpy.spin(arm_node)
-    except (KeyboardInterrupt, ExternalShutdownException):
-        pass
-    finally:
-        rclpy.try_shutdown()
-
-
-if __name__ == "__main__":
+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/arm_pkg/package.xml

@@ -5,12 +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>astra_msgs</depend>
+  <depend>ros2_interfaces_pkg</depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/arm_pkg/setup.cfg

@@ -2,5 +2,3 @@
 script_dir=$base/lib/arm_pkg
 [install]
 install_scripts=$base/lib/arm_pkg
-[build_scripts]
-executable= /usr/bin/env python3

src/arm_pkg/setup.py

@@ -1,22 +1,26 @@
-from setuptools import setup
+from setuptools import find_packages, setup
 
-package_name = "arm_pkg"
+package_name = 'arm_pkg'
 
 setup(
     name=package_name,
-    version="1.0.0",
-    packages=[package_name],
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
     data_files=[
-        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
-        ("share/" + package_name, ["package.xml"]),
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
     ],
-    install_requires=["setuptools"],
+    install_requires=['setuptools'],
     zip_safe=True,
-    maintainer="David",
-    maintainer_email="ds0196@uah.edu",
-    description="Relays topics related to the arm between VicCAN (through Anchor) and basestation.",
-    license="AGPL-3.0-only",
+    maintainer='tristan',
+    maintainer_email='tristanmcginnis26@gmail.com',
+    description='TODO: Package description',
+    license='All Rights Reserved',
     entry_points={
-        "console_scripts": ["arm = arm_pkg.arm_node:main"],
+        'console_scripts': [
+            'arm = arm_pkg.arm_node:main',
+            'headless = arm_pkg.arm_headless:main'
+        ],
     },
 )

src/bio_pkg/bio_pkg/bio_node.py

@@ -3,60 +3,54 @@ from rclpy.node import Node
 import serial
 import sys
 import threading
-import os
 import glob
 import time
 import atexit
 import signal
 from std_msgs.msg import String
-from astra_msgs.msg import BioControl
-from astra_msgs.msg import BioFeedback
+from ros2_interfaces_pkg.msg import BioControl
+from ros2_interfaces_pkg.msg import BioFeedback
 
 serial_pub = None
 thread = None
 
-
 class SerialRelay(Node):
     def __init__(self):
         # Initialize node
         super().__init__("bio_node")
 
         # Get launch mode parameter
-        self.declare_parameter("launch_mode", "bio")
-        self.launch_mode = self.get_parameter("launch_mode").value
+        self.declare_parameter('launch_mode', 'bio')
+        self.launch_mode = self.get_parameter('launch_mode').value
         self.get_logger().info(f"bio launch_mode is: {self.launch_mode}")
 
         # Create publishers
-        self.debug_pub = self.create_publisher(String, "/bio/feedback/debug", 10)
-        self.feedback_pub = self.create_publisher(BioFeedback, "/bio/feedback", 10)
+        self.debug_pub = self.create_publisher(String, '/bio/feedback/debug', 10)
+        self.feedback_pub = self.create_publisher(BioFeedback, '/bio/feedback', 10)
+
 
         # Create subscribers
-        self.control_sub = self.create_subscription(
-            BioControl, "/bio/control", self.send_control, 10
-        )
+        self.control_sub = self.create_subscription(BioControl, '/bio/control', self.send_control, 10)
         
         # Create a publisher for telemetry
         self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback)
 
         # Topics used in anchor mode
-        if self.launch_mode == "anchor":
-            self.anchor_sub = self.create_subscription(
-                String, "/anchor/bio/feedback", self.anchor_feedback, 10
-            )
-            self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)
+        if self.launch_mode == 'anchor':
+            self.anchor_sub = self.create_subscription(String, '/anchor/bio/feedback', self.anchor_feedback, 10)
+            self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10)
 
         self.bio_feedback = BioFeedback()
 
+
         # Search for ports IF in 'arm' (standalone) and not 'anchor' mode
-        if self.launch_mode == "bio":
+        if self.launch_mode == 'bio':
             # Loop through all serial devices on the computer to check for the MCU
             self.port = None
             for i in range(2):
                 try:
                     # connect and send a ping command
-                    set_port = (
-                        "/dev/ttyACM0"  # MCU is controlled through GPIO pins on the PI
-                    )
+                    set_port = '/dev/ttyACM0' #MCU is controlled through GPIO pins on the PI
                     ser = serial.Serial(set_port, 115200, timeout=1)
                     #print(f"Checking port {port}...")
                     ser.write(b"ping\n")
@@ -71,9 +65,7 @@ class SerialRelay(Node):
                     pass
             
             if self.port is None:
-                self.get_logger().info(
-                    "Unable to find MCU... please make sure it is connected."
-                )
+                self.get_logger().info("Unable to find MCU... please make sure it is connected.")
                 time.sleep(1)
                 sys.exit(1)
             
@@ -89,7 +81,7 @@ class SerialRelay(Node):
 
         try:
             while rclpy.ok():
-                if self.launch_mode == "bio":
+                if self.launch_mode == 'bio':
                     if self.ser.in_waiting:
                         self.read_mcu()
                     else:
@@ -99,6 +91,7 @@ class SerialRelay(Node):
         finally:
             self.cleanup()
 
+
     #Currently will just spit out all values over the /arm/feedback/debug topic as strings
     def read_mcu(self):
         try:
@@ -129,41 +122,26 @@ class SerialRelay(Node):
     def send_ik(self, msg):
         pass
 
+
     def send_control(self, msg: BioControl):
         # CITADEL Control Commands
         ################
 
+
         # Chem Pumps, only send if not zero
         if msg.pump_id != 0:
-            command = (
-                "can_relay_tovic,citadel,27,"
-                + str(msg.pump_id)
-                + ","
-                + str(msg.pump_amount)
-                + "\n"
-            )
+            command = "can_relay_tovic,citadel,27," + str(msg.pump_id) + "," + str(msg.pump_amount) + "\n"
             self.send_cmd(command)
         # Fans, only send if not zero
         if msg.fan_id != 0:
-            command = (
-                "can_relay_tovic,citadel,40,"
-                + str(msg.fan_id)
-                + ","
-                + str(msg.fan_duration)
-                + "\n"
-            )
+            command = "can_relay_tovic,citadel,40," + str(msg.fan_id) + "," + str(msg.fan_duration) + "\n"
             self.send_cmd(command)
         # Servos, only send if not zero
         if msg.servo_id != 0:
-            command = (
-                "can_relay_tovic,citadel,25,"
-                + str(msg.servo_id)
-                + ","
-                + str(int(msg.servo_state))
-                + "\n"
-            )
+            command = "can_relay_tovic,citadel,25," + str(msg.servo_id) + "," + str(int(msg.servo_state)) + "\n"
             self.send_cmd(command)
         
+
         # LSS (SCYTHE)
         command = "can_relay_tovic,citadel,24," + str(msg.bio_arm) + "\n"
         #self.send_cmd(command)
@@ -172,6 +150,7 @@ class SerialRelay(Node):
         command += "can_relay_tovic,citadel,26," + str(msg.vibration_motor) + "\n"
         #self.send_cmd(command)
         
+
         # FAERIE Control Commands 
         ################
         
@@ -189,14 +168,14 @@ class SerialRelay(Node):
         command += "can_relay_tovic,digit,42," + str(msg.drill_arm) + "\n"
         self.send_cmd(command)
 
+
+
     def send_cmd(self, msg: str):
-        if (
-            self.launch_mode == "anchor"
-        ):  # if in anchor mode, send to anchor node to relay
+        if self.launch_mode == 'anchor': #if in anchor mode, send to anchor node to relay
             output = String()
             output.data = msg
             self.anchor_pub.publish(output)
-        elif self.launch_mode == "bio":  # if in standalone mode, send to MCU directly
+        elif self.launch_mode == 'bio': #if in standalone mode, send to MCU directly
             self.get_logger().info(f"[Bio to MCU] {msg}")
             self.ser.write(bytes(msg, "utf8"))
 
@@ -205,9 +184,7 @@ class SerialRelay(Node):
         parts = str(output.strip()).split(",")
         #self.get_logger().info(f"[Bio Anchor] {msg.data}")
 
-        if output.startswith(
-            "can_relay_fromvic,citadel,54"
-        ):  # bat, 12, 5, Voltage readings * 100
+        if output.startswith("can_relay_fromvic,citadel,54"):  # bat, 12, 5, Voltage readings * 100
             self.bio_feedback.bat_voltage = float(parts[3]) / 100.0
             self.bio_feedback.voltage_12 = float(parts[4]) / 100.0
             self.bio_feedback.voltage_5 = float(parts[5]) / 100.0
@@ -225,19 +202,14 @@ class SerialRelay(Node):
 
     def cleanup(self):
         print("Cleaning up...")
-        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 main(args=None):
     rclpy.init(args=args)
     sys.excepthook = myexcepthook
@@ -246,10 +218,7 @@ def main(args=None):
     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
+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/bio_pkg/package.xml

@@ -5,11 +5,10 @@
   <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>astra_msgs</depend>
+  <depend>ros2_interfaces_pkg</depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/bio_pkg/setup.py

@@ -1,22 +1,25 @@
 from setuptools import find_packages, setup
 
-package_name = "bio_pkg"
+package_name = 'bio_pkg'
 
 setup(
     name=package_name,
-    version="0.0.0",
-    packages=find_packages(exclude=["test"]),
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
     data_files=[
-        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
-        ("share/" + package_name, ["package.xml"]),
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
     ],
-    install_requires=["setuptools"],
+    install_requires=['setuptools'],
     zip_safe=True,
-    maintainer="tristan",
-    maintainer_email="tristanmcginnis26@gmail.com",
-    description="TODO: Package description",
-    license="TODO: License declaration",
+    maintainer='tristan',
+    maintainer_email='tristanmcginnis26@gmail.com',
+    description='TODO: Package description',
+    license='TODO: License declaration',
     entry_points={
-        "console_scripts": ["bio = bio_pkg.bio_node:main"],
+        'console_scripts': [
+            'bio = bio_pkg.bio_node:main'
+        ],
     },
 )

src/core_pkg/core_pkg/core_headless.py

@@ -13,18 +13,15 @@ import os
 
 import importlib
 from std_msgs.msg import String
-from astra_msgs.msg import CoreControl
+from ros2_interfaces_pkg.msg import CoreControl
 
 
 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()
-)
+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)
 
-
 class Headless(Node):
     def __init__(self):
         # Initialize pygame first
@@ -46,15 +43,13 @@ class Headless(Node):
         # Initialize the gamepad
         self.gamepad = pygame.joystick.Joystick(0)
         self.gamepad.init()
-        print(f"Gamepad Found: {self.gamepad.get_name()}")
+        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
+        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
@@ -96,17 +91,15 @@ class Headless(Node):
         else:
             input.left_stick = -1 * round(self.gamepad.get_axis(1), 2)  # left y-axis
 
-        output = f"L: {input.left_stick}, R: {input.right_stick}, M: {max_speed}"
+        output = f'L: {input.left_stick}, R: {input.right_stick}, M: {max_speed}'
         self.get_logger().info(f"[Ctrl] {output}")
         self.publisher.publish(input)
 
-
 def main(args=None):
     rclpy.init(args=args)
     node = Headless()
     rclpy.spin(node)
     rclpy.shutdown()
 
-
-if __name__ == "__main__":
+if __name__ == '__main__':
     main()

src/core_pkg/core_pkg/core_node.py

@@ -1,7 +1,5 @@
 import rclpy
 from rclpy.node import Node
-from rclpy import qos
-from rclpy.duration import Duration
 from std_srvs.srv import Empty
 
 import signal
@@ -9,163 +7,47 @@ 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 astra_msgs.msg import CoreControl, CoreFeedback, RevMotorState
-from astra_msgs.msg import VicCAN, NewCoreFeedback, Barometer, CoreCtrlState
 
+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
 
-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,  # Best Effort subscribers are still compatible with Reliable publishers
-    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")
+        super().__init__("core_node")#previously 'serial_publisher'
 
-        # 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}")
+        # 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}")
 
-        ##################################################
-        # Topics
+        # 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)
         
-        # 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
-            )
+        # Create a publisher for telemetry
+        self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback)
 
-            self.anchor_sub = self.create_subscription(
-                String, "/anchor/core/feedback", self.anchor_feedback, 10
-            )
-            self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)
+        # Create a service server for pinging the rover
+        self.ping_service = self.create_service(Empty, '/astra/core/ping', self.ping_callback)
 
-        # Control
+        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)
 
-        # 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":
+        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()
@@ -176,7 +58,7 @@ class SerialRelay(Node):
                         ser = serial.Serial(port, 115200, timeout=1)
                         #(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:
@@ -197,7 +79,7 @@ class SerialRelay(Node):
         
             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
@@ -205,14 +87,15 @@ class SerialRelay(Node):
         thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
         thread.start()
         
+    
         try:
             while rclpy.ok():
-                if self.launch_mode == "core":
+                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
+    def read_MCU(self):
         try:
             output = str(self.ser.readline(), "utf8")
             
@@ -223,24 +106,51 @@ class SerialRelay(Node):
                 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()
-            sys.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()
-            sys.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()
-            sys.exit(1)
+            self.exit(1)
                 
     def scale_duty(self, value: float, max_speed: float):
         leftMin = -1
@@ -248,6 +158,7 @@ class SerialRelay(Node):
         rightMin = -max_speed/100.0
         rightMax = max_speed/100.0
 
+
         # Figure out how 'wide' each range is
         leftSpan = leftMax - leftMin
         rightSpan = rightMax - rightMin
@@ -258,297 +169,68 @@ class SerialRelay(Node):
         # 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"
-            )
+    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"
-            )
+            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"
+        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":
+    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":
+        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.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])
-        # GNSS Longitude
-        elif output.startswith("can_relay_fromvic,core,49"):
+        elif output.startswith("can_relay_fromvic,core,49"):#GNSS Longitude
             self.core_feedback.gps_long = float(parts[3])
-        # GNSS Satellite count and altitude
-        elif output.startswith("can_relay_fromvic,core,50"):
+        elif output.startswith("can_relay_fromvic,core,50"):#GNSS Satellite Count
             self.core_feedback.gps_sats = round(float(parts[3]))
+            #if parts length is at least 5 then we should have altitude, this is a temporary check until fully implemented
+            if len(parts) >= 5:
                 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"):
+        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]))
-        # Accel x, y, z, heading
-        elif output.startswith("can_relay_fromvic,core,52"):
+        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])
-        # 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.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
-        # BMP temperature, altitude, pressure
-        elif output.startswith("can_relay_fromvic,core,56"):
+        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.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)
@@ -556,18 +238,15 @@ class SerialRelay(Node):
     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)
@@ -575,11 +254,6 @@ def myexcepthook(type, value, tb):
         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)
@@ -590,10 +264,8 @@ def main(args=None):
    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
+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

@@ -9,7 +9,7 @@ import threading
 import time
 
 from std_msgs.msg import String
-from astra_msgs.msg import PtzControl, PtzFeedback
+from ros2_interfaces_pkg.msg import PtzControl, PtzFeedback
 
 # Import the SIYI SDK
 from core_pkg.siyi_sdk import (
@@ -18,43 +18,37 @@ from core_pkg.siyi_sdk import (
     DataStreamType,
     DataStreamFrequency,
     SingleAxis,
-    AttitudeData,
+    AttitudeData
 )
 
-
 class PtzNode(Node):
     def __init__(self):
         # Initialize node with name
         super().__init__("core_ptz")
 
         # Declare parameters
-        self.declare_parameter("camera_ip", "192.168.1.9")
-        self.declare_parameter("camera_port", 37260)
+        self.declare_parameter('camera_ip', '192.168.1.9')
+        self.declare_parameter('camera_port', 37260)
         
         # Get parameters
-        self.camera_ip = self.get_parameter("camera_ip").value
-        self.camera_port = self.get_parameter("camera_port").value
+        self.camera_ip = self.get_parameter('camera_ip').value
+        self.camera_port = self.get_parameter('camera_port').value
         
-        self.get_logger().info(
-            f"PTZ camera IP: {self.camera_ip} Port: {self.camera_port}"
-        )
+        self.get_logger().info(f"PTZ camera IP: {self.camera_ip} Port: {self.camera_port}")
         
         # Create a camera instance
         self.camera = None
-        self.camera_connected = (
-            False  # This flag is still managed but not used to gate commands
-        )
+        self.camera_connected = False # This flag is still managed but not used to gate commands
         self.loop = None
         self.thread_pool = None
         
         # Create publishers
-        self.feedback_pub = self.create_publisher(PtzFeedback, "/ptz/feedback", 10)
-        self.debug_pub = self.create_publisher(String, "/ptz/debug", 10)
+        self.feedback_pub = self.create_publisher(PtzFeedback, '/ptz/feedback', 10)
+        self.debug_pub = self.create_publisher(String, '/ptz/debug', 10)
         
         # Create subscribers
         self.control_sub = self.create_subscription(
-            PtzControl, "/ptz/control", self.handle_control_command, 10
-        )
+            PtzControl, '/ptz/control', self.handle_control_command, 10)
             
         # Create timers
         self.connection_timer = self.create_timer(5.0, self.check_camera_connection)
@@ -63,9 +57,7 @@ class PtzNode(Node):
         
         # Create feedback message
         self.feedback_msg = PtzFeedback()
-        self.feedback_msg.connected = (
-            False  # This will reflect the actual connection state
-        )
+        self.feedback_msg.connected = False # This will reflect the actual connection state
         self.feedback_msg.error_msg = "Initializing"
         
         # Flags for async operations
@@ -94,7 +86,8 @@ class PtzNode(Node):
             
             # Request attitude data stream
             await self.camera.send_data_stream_request(
-                DataStreamType.ATTITUDE_DATA, DataStreamFrequency.HZ_10
+                DataStreamType.ATTITUDE_DATA,
+                DataStreamFrequency.HZ_10
             )
             
             # Update connection status
@@ -115,12 +108,8 @@ class PtzNode(Node):
         # Update last_data_time regardless of self.camera_connected,
         # as data might arrive during a brief reconnect window.
         self.last_data_time = time.time()
-        if (
-            self.camera_connected
-        ):  # Only process for feedback if we believe we are connected
-            if cmd_id == CommandID.ATTITUDE_DATA_RESPONSE and isinstance(
-                data, AttitudeData
-            ):
+        if self.camera_connected: # Only process for feedback if we believe we are connected
+            if cmd_id == CommandID.ATTITUDE_DATA_RESPONSE and isinstance(data, AttitudeData):
                 self.feedback_msg.yaw = data.yaw
                 self.feedback_msg.pitch = data.pitch
                 self.feedback_msg.roll = data.roll
@@ -141,6 +130,7 @@ 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:
@@ -150,9 +140,7 @@ class PtzNode(Node):
                     if self.camera.is_connected: # SDK's internal connection state
                          self.run_async_func(self.camera.disconnect())
                 except Exception as e:
-                    self.get_logger().debug(
-                        f"Error during pre-reconnect disconnect: {e}"
-                    )
+                    self.get_logger().debug(f"Error during pre-reconnect disconnect: {e}")
                 # self.camera = None # Don't nullify here, connect_to_camera will re-assign or create new
                 
             self.connect_task = self.thread_pool.submit(
@@ -164,9 +152,7 @@ class PtzNode(Node):
         if self.camera_connected: # Only check health if we think we are connected
             time_since_last_data = time.time() - self.last_data_time
             if time_since_last_data > 5.0:
-                self.publish_debug(
-                    f"No camera data for {time_since_last_data:.1f}s, marking as disconnected."
-                )
+                self.publish_debug(f"No camera data for {time_since_last_data:.1f}s, marking as disconnected.")
                 self.camera_connected = False
                 self.feedback_msg.connected = False
                 self.feedback_msg.error_msg = "Connection stale (no data)"
@@ -176,19 +162,18 @@ class PtzNode(Node):
         """Handle incoming control commands."""
         # Removed: if not self.camera_connected
         if not self.camera: # Still check if camera object exists
-            self.get_logger().warning(
-                "Camera object not initialized, ignoring control command"
-            )
+            self.get_logger().warning("Camera object not initialized, ignoring control command")
             return
             
-        self.thread_pool.submit(self.run_async_func, self.process_control_command(msg))
+        self.thread_pool.submit(
+            self.run_async_func,
+            self.process_control_command(msg)
+        )
 
     async def process_control_command(self, msg):
         """Process and send the control command to the camera."""
         if not self.camera: 
-            self.get_logger().error(
-                "Process control command called but camera object is None."
-            )
+            self.get_logger().error("Process control command called but camera object is None.")
             return
         try:
             # The SDK's send_... methods will raise RuntimeError if not connected.
@@ -201,35 +186,32 @@ class PtzNode(Node):
             if msg.control_mode == 0:
                 turn_yaw = max(-100, min(100, int(msg.turn_yaw)))
                 turn_pitch = max(-100, min(100, int(msg.turn_pitch)))
-                self.get_logger().debug(
-                    f"Attempting rotation: yaw_speed={turn_yaw}, pitch_speed={turn_pitch}"
-                )
+                self.get_logger().debug(f"Attempting rotation: yaw_speed={turn_yaw}, pitch_speed={turn_pitch}")
                 await self.camera.send_rotation_command(turn_yaw, turn_pitch)
                 
             elif msg.control_mode == 1:
                 yaw = max(-135.0, min(135.0, msg.yaw))
                 pitch = max(-90.0, min(90.0, msg.pitch))
-                self.get_logger().debug(
-                    f"Attempting absolute angles: yaw={yaw}, pitch={pitch}"
-                )
+                self.get_logger().debug(f"Attempting absolute angles: yaw={yaw}, pitch={pitch}")
                 await self.camera.send_attitude_angles_command(yaw, pitch)
                 
             elif msg.control_mode == 2:
                 axis = SingleAxis.YAW if msg.axis_id == 0 else SingleAxis.PITCH
                 angle = msg.angle
-                self.get_logger().debug(
-                    f"Attempting single axis: axis={axis.name}, angle={angle}"
-                )
+                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)
+                zoom_direction = 0
+                if msg.zoom_level > 0:
+                    zoom_direction = 1  # Zoom in
+                elif msg.zoom_level < 0:
+                    zoom_direction = -1  # Zoom out
                 
-            if hasattr(msg, "stream_type") and hasattr(msg, "stream_freq"):
+                self.get_logger().debug(f"Attempting manual zoom: direction={zoom_direction}")
+                await self.camera.send_manual_zoom_command(zoom_direction)
+                
+            if hasattr(msg, 'stream_type') and hasattr(msg, 'stream_freq'):
                 if msg.stream_type > 0 and msg.stream_freq >= 0: 
                     try:
                         stream_type = DataStreamType(msg.stream_type)
@@ -237,13 +219,9 @@ class PtzNode(Node):
                         self.get_logger().info(
                             f"Attempting to set data stream: type={stream_type.name}, freq={stream_freq.name}"
                         )
-                        await self.camera.send_data_stream_request(
-                            stream_type, stream_freq
-                        )
+                        await self.camera.send_data_stream_request(stream_type, stream_freq)
                     except ValueError:
-                        self.get_logger().error(
-                            "Invalid stream type or frequency values in control message"
-                        )
+                        self.get_logger().error("Invalid stream type or frequency values in control message")
             
         except RuntimeError as e: # Catch SDK's "not connected" errors
             self.get_logger().warning(f"SDK command failed (likely not connected): {e}")
@@ -258,33 +236,26 @@ class PtzNode(Node):
     def publish_debug(self, message_text):
         """Publish debug message."""
         msg = String()
-        msg.data = (
-            f"[{self.get_clock().now().nanoseconds / 1e9:.2f}] PTZ Node: {message_text}"
-        )
+        msg.data = f"[{self.get_clock().now().nanoseconds / 1e9:.2f}] PTZ Node: {message_text}"
         self.debug_pub.publish(msg)
-        self.get_logger().debug(message_text)
+        self.get_logger().info(message_text) 
 
     def run_async_func(self, coro):
         """Run an async function in the event loop."""
         if self.loop and self.loop.is_running():
             try:
-                return asyncio.run_coroutine_threadsafe(coro, self.loop).result(
-                    timeout=5.0
-                )  # Added timeout
+                return asyncio.run_coroutine_threadsafe(coro, self.loop).result(timeout=5.0) # Added timeout
             except asyncio.TimeoutError:
                 self.get_logger().warning(f"Async function {coro.__name__} timed out.")
                 return None
             except Exception as e:
-                self.get_logger().error(
-                    f"Exception in run_async_func for {coro.__name__}: {e}"
-                )
+                self.get_logger().error(f"Exception in run_async_func for {coro.__name__}: {e}")
                 return None
         else:
-            self.get_logger().warning(
-                "Asyncio loop not running, cannot execute coroutine."
-            )
+            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
@@ -293,7 +264,8 @@ class PtzNode(Node):
             try:
                 self.get_logger().info("Disabling data stream...")
                 await self.camera.send_data_stream_request(
-                    DataStreamType.ATTITUDE_DATA, DataStreamFrequency.DISABLE
+                    DataStreamType.ATTITUDE_DATA, 
+                    DataStreamFrequency.DISABLE
                 )
                 await asyncio.sleep(0.1) 
                 
@@ -320,14 +292,10 @@ class PtzNode(Node):
         if self.loop and self.thread_pool:
             if self.loop.is_running():
                 try:
-                    future = asyncio.run_coroutine_threadsafe(
-                        self.shutdown_node_async(), self.loop
-                    )
+                    future = asyncio.run_coroutine_threadsafe(self.shutdown_node_async(), self.loop)
                     future.result(timeout=5) 
                 except Exception as e:
-                    self.get_logger().error(
-                        f"Error during async shutdown in cleanup: {e}"
-                    )
+                    self.get_logger().error(f"Error during async shutdown in cleanup: {e}")
             
             self.get_logger().info("Shutting down thread pool executor...")
             self.thread_pool.shutdown(wait=True)
@@ -338,9 +306,7 @@ class PtzNode(Node):
             
             self.get_logger().info("PTZ node resources cleaned up.")
         else:
-            self.get_logger().warning(
-                "Loop or thread_pool not initialized, skipping parts of cleanup."
-            )
+            self.get_logger().warning("Loop or thread_pool not initialized, skipping parts of cleanup.")
 
 
 def main(args=None):
@@ -351,7 +317,6 @@ def main(args=None):
     
     asyncio_thread = None
     if ptz_node.loop: 
-
         def run_event_loop(loop):
             asyncio.set_event_loop(loop)
             try:
@@ -364,7 +329,9 @@ def main(args=None):
                     loop.close()
         
         asyncio_thread = threading.Thread(
-            target=run_event_loop, args=(ptz_node.loop,), daemon=True
+            target=run_event_loop, 
+            args=(ptz_node.loop,),
+            daemon=True 
         )
         asyncio_thread.start()
     
@@ -389,5 +356,5 @@ def main(args=None):
         ptz_node.get_logger().info("ROS shutdown complete.")
 
 
-if __name__ == "__main__":
+if __name__ == '__main__':
     main()

src/core_pkg/core_pkg/siyi_sdk/siyi.py

@@ -64,6 +64,7 @@ class CommandID(Enum):
     """
 
     ROTATION_CONTROL = 0x07
+    MANUAL_ZOOM = 0x05  # Added manual zoom command
     ABSOLUTE_ZOOM = 0x08
     ATTITUDE_ANGLES = 0x0E
     SINGLE_AXIS_CONTROL = 0x41
@@ -110,7 +111,9 @@ class SiyiGimbalCamera:
 
     MAX_A8_MINI_ZOOM = 6.0  # Maximum zoom for A8 mini
 
-    def __init__(self, ip: str, port: int = 37260, *, heartbeat_interval: int = 2):
+    def __init__(
+        self, ip: str, port: int = 37260, *, heartbeat_interval: int = 2
+    ):
         self.ip = ip
         self.port = port
         self.heartbeat_interval = heartbeat_interval
@@ -122,7 +125,9 @@ class SiyiGimbalCamera:
 
     async def connect(self) -> None:
         try:
-            self.reader, self.writer = await asyncio.open_connection(self.ip, self.port)
+            self.reader, self.writer = await asyncio.open_connection(
+                self.ip, self.port
+            )
             self.is_connected = True
             asyncio.create_task(self.heartbeat_loop())
             asyncio.create_task(self._data_stream_listener())
@@ -154,7 +159,9 @@ class SiyiGimbalCamera:
             if self.is_connected:
                 await self.disconnect()
 
-    def _build_packet_header(self, cmd_id: CommandID, data_len: int) -> bytearray:
+    def _build_packet_header(
+        self, cmd_id: CommandID, data_len: int
+    ) -> bytearray:
         """Helper to build the common packet header."""
         packet = bytearray()
         packet.extend(b"\x55\x66")  # STX
@@ -173,11 +180,15 @@ class SiyiGimbalCamera:
 
     def _build_rotation_packet(self, turn_yaw: int, turn_pitch: int) -> bytes:
         data_len = 2
-        packet = self._build_packet_header(CommandID.ROTATION_CONTROL, data_len)
+        packet = self._build_packet_header(
+            CommandID.ROTATION_CONTROL, data_len
+        )
         packet.extend(struct.pack("bb", turn_yaw, turn_pitch))
         return self._finalize_packet(packet)
 
-    async def send_rotation_command(self, turn_yaw: int, turn_pitch: int) -> None:
+    async def send_rotation_command(
+        self, turn_yaw: int, turn_pitch: int
+    ) -> None:
         if not self.is_connected or not self.writer:
             raise RuntimeError(
                 "Socket is not connected or writer is None, cannot send rotation command."
@@ -189,15 +200,21 @@ class SiyiGimbalCamera:
             f"Sent rotation command with yaw_speed {turn_yaw} and pitch_speed {turn_pitch}"
         )
 
-    def _build_attitude_angles_packet(self, yaw: float, pitch: float) -> bytes:
+    def _build_attitude_angles_packet(
+        self, yaw: float, pitch: float
+    ) -> bytes:
         data_len = 4
-        packet = self._build_packet_header(CommandID.ATTITUDE_ANGLES, data_len)
+        packet = self._build_packet_header(
+            CommandID.ATTITUDE_ANGLES, data_len
+        )
         yaw_int = int(round(yaw * 10))
         pitch_int = int(round(pitch * 10))
         packet.extend(struct.pack("<hh", yaw_int, pitch_int))
         return self._finalize_packet(packet)
 
-    async def send_attitude_angles_command(self, yaw: float, pitch: float) -> None:
+    async def send_attitude_angles_command(
+        self, yaw: float, pitch: float
+    ) -> None:
         if not self.is_connected or not self.writer:
             raise RuntimeError(
                 "Socket is not connected or writer is None, cannot send attitude angles command."
@@ -205,13 +222,17 @@ class SiyiGimbalCamera:
         packet = self._build_attitude_angles_packet(yaw, pitch)
         self.writer.write(packet)
         await self.writer.drain()
-        logger.debug(f"Sent attitude angles command with yaw {yaw}° and pitch {pitch}°")
+        logger.debug(
+            f"Sent attitude angles command with yaw {yaw}° and pitch {pitch}°"
+        )
 
     def _build_single_axis_attitude_packet(
         self, angle: float, axis: SingleAxis
     ) -> bytes:
         data_len = 3
-        packet = self._build_packet_header(CommandID.SINGLE_AXIS_CONTROL, data_len)
+        packet = self._build_packet_header(
+            CommandID.SINGLE_AXIS_CONTROL, data_len
+        )
         angle_int = int(round(angle * 10))
         packet.extend(struct.pack("<hB", angle_int, axis.value))
         return self._finalize_packet(packet)
@@ -234,7 +255,9 @@ class SiyiGimbalCamera:
         self, data_type: DataStreamType, data_freq: DataStreamFrequency
     ) -> bytes:
         data_len = 2
-        packet = self._build_packet_header(CommandID.DATA_STREAM_REQUEST, data_len)
+        packet = self._build_packet_header(
+            CommandID.DATA_STREAM_REQUEST, data_len
+        )
         packet.append(data_type.value)
         packet.append(data_freq.value)
         return self._finalize_packet(packet)
@@ -257,9 +280,7 @@ class SiyiGimbalCamera:
         data_len = 2
         packet = self._build_packet_header(CommandID.ABSOLUTE_ZOOM, data_len)
         zoom_packet_value = int(round(zoom_level * 10))
-        if not (
-            0 <= zoom_packet_value <= 65535
-        ):  # Should be caught by clamping earlier
+        if not (0 <= zoom_packet_value <= 65535): # Should be caught by clamping earlier
             raise ValueError(
                 "Zoom packet value out of uint16_t range after conversion."
             )
@@ -300,6 +321,47 @@ class SiyiGimbalCamera:
             f"Sent absolute zoom command with level {zoom_level:.1f}x (original request: {original_requested_zoom:.1f}x)"
         )
 
+    def _build_manual_zoom_packet(self, zoom_direction: int) -> bytes:
+        """
+        Helper to build a manual zoom command packet.
+        
+        :param zoom_direction: Direction to zoom: 1 for in, -1 for out, 0 for stop.
+        :return: Complete packet bytes ready to send.
+        """
+        data_len = 1
+        packet = self._build_packet_header(CommandID.MANUAL_ZOOM, data_len)
+        
+        # Convert the direction to a valid byte value:
+        # -1 (zoom out) -> 2
+        # 0 (stop zoom) -> 0
+        # 1 (zoom in) -> 1
+        if zoom_direction < 0:
+            direction_byte = 2  # Zoom out
+        elif zoom_direction > 0:
+            direction_byte = 1  # Zoom in
+        else:
+            direction_byte = 0  # Stop zoom
+        
+        logger.debug(f"Manual zoom: input direction={zoom_direction}, mapped to byte={direction_byte}")
+        packet.append(direction_byte)
+        return self._finalize_packet(packet)
+
+    async def send_manual_zoom_command(self, zoom_direction: int) -> None:
+        """
+        Send a manual zoom command (0x05) to the gimbal.
+        
+        :param zoom_direction: 1 to zoom in, -1 to zoom out, 0 to stop zooming.
+        """
+        if not self.is_connected or not self.writer:
+            raise RuntimeError(
+                "Socket is not connected or writer is None, cannot send manual zoom command."
+            )
+        packet = self._build_manual_zoom_packet(zoom_direction)
+        logger.debug(f"Manual zoom packet: {packet.hex()}")
+        self.writer.write(packet)
+        await self.writer.drain()
+        logger.debug(f"Sent manual zoom command with direction {zoom_direction}")
+
     async def _read_packet(self):
         if not self.reader:
             raise RuntimeError("Reader is not initialized.")
@@ -354,7 +416,10 @@ class SiyiGimbalCamera:
                         self._data_callback(cmd_id_int, data)
                     continue
 
-                if cmd_id_enum == CommandID.ATTITUDE_DATA_RESPONSE and len(data) == 12:
+                if (
+                    cmd_id_enum == CommandID.ATTITUDE_DATA_RESPONSE
+                    and len(data) == 12
+                ):
                     try:
                         parsed = AttitudeData.from_bytes(data)
                         if self._data_callback:
@@ -362,7 +427,9 @@ class SiyiGimbalCamera:
                         else:
                             logger.info(f"Received attitude data: {parsed}")
                     except Exception as e:
-                        logger.exception(f"Failed to parse attitude data: {e}")
+                        logger.exception(
+                            f"Failed to parse attitude data: {e}"
+                        )
                         if self._data_callback:
                             self._data_callback(cmd_id_enum, data)
                 else:
@@ -404,9 +471,7 @@ async def main_sdk_test():  # Renamed to avoid conflict if this file is imported
     gimbal = SiyiGimbalCamera(gimbal_ip)
 
     def my_data_handler(cmd_id, data):
-        if cmd_id == CommandID.ATTITUDE_DATA_RESPONSE and isinstance(
-            data, AttitudeData
-        ):
+        if cmd_id == CommandID.ATTITUDE_DATA_RESPONSE and isinstance(data, AttitudeData):
             print(
                 f"Attitude: Yaw={data.yaw:.1f}, Pitch={data.pitch:.1f}, Roll={data.roll:.1f}"
             )
@@ -447,19 +512,16 @@ async def main_sdk_test():  # Renamed to avoid conflict if this file is imported
             await asyncio.sleep(2)
 
             print("SDK Test: Requesting attitude data stream at 5Hz...")
-            await gimbal.send_data_stream_request(
-                DataStreamType.ATTITUDE_DATA, DataStreamFrequency.HZ_5
-            )
+            await gimbal.send_data_stream_request(DataStreamType.ATTITUDE_DATA, DataStreamFrequency.HZ_5)
             
             print("SDK Test: Listening for data for 10 seconds...")
             await asyncio.sleep(10)
 
             print("SDK Test: Disabling attitude data stream...")
-            await gimbal.send_data_stream_request(
-                DataStreamType.ATTITUDE_DATA, DataStreamFrequency.DISABLE
-            )
+            await gimbal.send_data_stream_request(DataStreamType.ATTITUDE_DATA, DataStreamFrequency.DISABLE)
             await asyncio.sleep(1)
 
+
     except ConnectionRefusedError:
         print(
             f"SDK Test: Connection to {gimbal_ip} was refused. Is the gimbal on and accessible?"

src/core_pkg/core_pkg/siyi_sdk/test.py

@@ -24,9 +24,7 @@ async def main():
         await asyncio.sleep(2)
 
         # Command 1: Move all the way to the right (using set angles)
-        logger.info(
-            "Command 1: Move all the way to the right (using absolute angle control)"
-        )
+        logger.info("Command 1: Move all the way to the right (using absolute angle control)")
         await camera.send_attitude_angles_command(135.0, 0.0)
         await asyncio.sleep(5)
 
@@ -37,17 +35,13 @@ async def main():
         await asyncio.sleep(5)
 
         # Command 3: Stop looking down, then look up (with the single axis)
-        logger.info(
-            "Command 3: Stop looking down and start looking up (single axis control)"
-        )
+        logger.info("Command 3: Stop looking down and start looking up (single axis control)")
         await camera.send_rotation_command(0, 0)
         await camera.send_single_axis_attitude_command(135, SingleAxis.PITCH)
         await asyncio.sleep(5)
 
         # Command 4: Reset and move all the way to the left (Absolute value).
-        logger.info(
-            "Command 4: Move back to the center, and start moving all the way left"
-        )
+        logger.info("Command 4: Move back to the center, and start moving all the way left")
         await camera.send_attitude_angles_command(-135.0, 0.0)
         await asyncio.sleep(5)
 

src/core_pkg/package.xml

@@ -5,13 +5,10 @@
   <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>astra_msgs</depend>
+  <depend>ros2_interfaces_pkg</depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/core_pkg/setup.py

@@ -1,26 +1,27 @@
 from setuptools import find_packages, setup
 
-package_name = "core_pkg"
+package_name = 'core_pkg'
 
 setup(
     name=package_name,
-    version="0.0.0",
-    packages=find_packages(exclude=["test"]),
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
     data_files=[
-        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
-        ("share/" + package_name, ["package.xml"]),
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
     ],
-    install_requires=["setuptools"],
+    install_requires=['setuptools'],
     zip_safe=True,
-    maintainer="tristan",
-    maintainer_email="tristanmcginnis26@gmail.com",
-    description="Core rover control package to handle command interpretation and embedded interfacing.",
-    license="All Rights Reserved",
+    maintainer='tristan',
+    maintainer_email='tristanmcginnis26@gmail.com',
+    description='Core rover control package to handle command interpretation and embedded interfacing.',
+    license='All Rights Reserved',
     entry_points={
-        "console_scripts": [
+        'console_scripts': [
             "core = core_pkg.core_node:main",
             "headless = core_pkg.core_headless:main",
-            "ptz = core_pkg.core_ptz:main",
+            "ptz = core_pkg.core_ptz:main"
         ],
     },
 )

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>astra_msgs</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,713 +0,0 @@
-import rclpy
-from rclpy.node import Node
-from rclpy.executors import ExternalShutdownException
-from rclpy import qos
-from rclpy.duration import Duration
-
-import signal
-import time
-
-import os
-import sys
-import pwd
-import grp
-from math import copysign
-
-from std_srvs.srv import Trigger
-from std_msgs.msg import Header
-from geometry_msgs.msg import Twist, TwistStamped
-from control_msgs.msg import JointJog
-from astra_msgs.msg import CoreControl, ArmManual, BioControl
-from astra_msgs.msg import CoreCtrlState
-
-import warnings
-
-# Literally headless
-warnings.filterwarnings(
-    "ignore",
-    message="Your system is avx2 capable but pygame was not built with support for it.",
-)
-
-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),
-)
-
-
-STICK_DEADZONE = float(os.getenv("STICK_DEADZONE", "0.05"))
-ARM_DEADZONE = float(os.getenv("ARM_DEADZONE", "0.2"))
-
-
-class Headless(Node):
-    # Every non-fixed joint defined in Arm's URDF
-    # Used for JointState and JointJog messsages
-    all_joint_names = [
-        "axis_0_joint",
-        "axis_1_joint",
-        "axis_2_joint",
-        "axis_3_joint",
-        "wrist_yaw_joint",
-        "wrist_roll_joint",
-        "ef_gripper_left_joint",
-    ]
-
-    def __init__(self):
-        # Initialize pygame first
-        pygame.init()
-        pygame.joystick.init()
-        super().__init__("headless_node")
-
-        ##################################################
-        # Preamble
-
-        # 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
-        id = 0
-        while True:
-            self.num_gamepads = pygame.joystick.get_count()
-            if id >= self.num_gamepads:
-                self.get_logger().fatal("Ran out of controllers to try")
-                sys.exit(1)
-
-            try:
-                self.gamepad = pygame.joystick.Joystick(id)
-                self.gamepad.init()
-            except Exception as e:
-                self.get_logger().error("Error when initializing gamepad")
-                self.get_logger().error(e)
-                id += 1
-                continue
-            print(f"Gamepad Found: {self.gamepad.get_name()}")
-
-            if self.gamepad.get_numhats() == 0 or self.gamepad.get_numaxes() < 5:
-                self.get_logger().error("Controller not correctly initialized.")
-                if not is_user_in_group("input"):
-                    self.get_logger().warning(
-                        "If using NixOS, you may need to add yourself to the 'input' group."
-                    )
-                if is_user_in_group("plugdev"):
-                    self.get_logger().warning(
-                        "If using NixOS, you may need to remove yourself from the 'plugdev' group."
-                    )
-            else:
-                break
-            id += 1
-
-        ##################################################
-        # Parameters
-
-        self.declare_parameter("use_old_topics", True)
-        self.use_old_topics = (
-            self.get_parameter("use_old_topics").get_parameter_value().bool_value
-        )
-
-        self.declare_parameter("use_bio", False)
-        self.use_bio = self.get_parameter("use_bio").get_parameter_value().bool_value
-
-        self.declare_parameter("use_arm_ik", False)
-        self.use_arm_ik = (
-            self.get_parameter("use_arm_ik").get_parameter_value().bool_value
-        )
-
-        # NOTE: only applicable if use_old_topics == True
-        self.declare_parameter("use_new_arm_manual_scheme", True)
-        self.use_new_arm_manual_scheme = (
-            self.get_parameter("use_new_arm_manual_scheme")
-            .get_parameter_value()
-            .bool_value
-        )
-
-        # Check parameter validity
-        if self.use_arm_ik and self.use_old_topics:
-            self.get_logger().fatal("Old topics do not support arm IK control.")
-            sys.exit(1)
-        if not self.use_new_arm_manual_scheme and not self.use_old_topics:
-            self.get_logger().warn(
-                f"New arm manual does not support old control scheme. Defaulting to new scheme."
-            )
-
-        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)
-
-        ##################################################
-        # Old Topics
-
-        if self.use_old_topics:
-            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)
-
-        ##################################################
-        # New Topics
-
-        if not self.use_old_topics:
-            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.arm_manual_pub_ = self.create_publisher(
-                JointJog, "/arm/manual_new", qos_profile=control_qos
-            )
-
-            self.arm_ik_twist_publisher = self.create_publisher(
-                TwistStamped, "/servo_node/delta_twist_cmds", qos_profile=control_qos
-            )
-            self.arm_ik_jointjog_publisher = self.create_publisher(
-                JointJog, "/servo_node/delta_joint_cmds", qos_profile=control_qos
-            )
-
-            # TODO: add new bio topics
-
-        ##################################################
-        # Timers
-
-        self.create_timer(0.1, self.send_controls)
-
-        ##################################################
-        # Services
-
-        # If using IK control, we have to "start" the servo node to enable it to accept commands
-        self.servo_start_client = None
-        if self.use_arm_ik:
-            self.get_logger().info("Starting servo node for IK control...")
-            self.servo_start_client = self.create_client(
-                Trigger, "/servo_node/start_servo"
-            )
-            timeout_counter = 0
-            while not self.servo_start_client.wait_for_service(timeout_sec=1.0):
-                self.get_logger().info("Waiting for servo_node/start_servo service...")
-                timeout_counter += 1
-                if timeout_counter >= 10:
-                    self.get_logger().error(
-                        "Servo's start service not available. IK control will not work."
-                    )
-                    break
-            if self.servo_start_client.service_is_ready():
-                self.servo_start_client.call_async(Trigger.Request())
-
-        # Rumble when node is ready (returns False if rumble not supported)
-        self.gamepad.rumble(0.7, 0.8, 150)
-
-    def stop_all(self):
-        if self.use_old_topics:
-            self.core_publisher.publish(CORE_STOP_MSG)
-            self.arm_publisher.publish(ARM_STOP_MSG)
-            self.bio_publisher.publish(BIO_STOP_MSG)
-        else:
-            self.core_twist_pub_.publish(CORE_STOP_TWIST_MSG)
-            if self.use_arm_ik:
-                self.arm_ik_twist_publisher.publish(self.arm_ik_twist_stop_msg())
-            else:
-                self.arm_manual_pub_.publish(self.arm_manual_stop_msg())
-            # TODO: add bio here after implementing new topics
-
-    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() != self.num_gamepads:
-            print("Gamepad disconnected. Exiting...")
-            # Stop the rover if controller disconnected
-            self.stop_all()
-            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.stop_all()
-            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")
-            if self.ctrl_mode == "arm" and self.use_bio:
-                self.get_logger().warning("NOTE: Using bio instead of arm.")
-
-        # Actually send the controls
-        if self.ctrl_mode == "core":
-            self.send_core()
-            if self.use_old_topics:
-                if self.use_bio:
-                    self.bio_publisher.publish(BIO_STOP_MSG)
-                else:
-                    self.arm_publisher.publish(ARM_STOP_MSG)
-            # New topics shouldn't need to constantly send zeroes imo
-        else:
-            if self.use_bio:
-                self.send_bio()
-            else:
-                self.send_arm()
-            if self.use_old_topics:
-                self.core_publisher.publish(CORE_STOP_MSG)
-            # Ditto
-
-    def send_core(self):
-        # Collect controller state
-        left_stick_x = stick_deadzone(self.gamepad.get_axis(0))
-        left_stick_y = stick_deadzone(self.gamepad.get_axis(1))
-        left_trigger = stick_deadzone(self.gamepad.get_axis(2))
-        right_stick_x = stick_deadzone(self.gamepad.get_axis(3))
-        right_stick_y = stick_deadzone(self.gamepad.get_axis(4))
-        right_trigger = stick_deadzone(self.gamepad.get_axis(5))
-        button_a = self.gamepad.get_button(0)
-        button_b = self.gamepad.get_button(1)
-        button_x = self.gamepad.get_button(2)
-        button_y = self.gamepad.get_button(3)
-        left_bumper = self.gamepad.get_button(4)
-        right_bumper = self.gamepad.get_button(5)
-        dpad_input = self.gamepad.get_hat(0)
-
-        if self.use_old_topics:
-            input = CoreControl()
-            input.max_speed = 90
-
-            # 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)
-
-        else:  # New topics
-            input = Twist()
-
-            # 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.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}"
-                )
-
-    def send_arm(self):
-        # Collect controller state
-        left_stick_x = stick_deadzone(self.gamepad.get_axis(0))
-        left_stick_y = stick_deadzone(self.gamepad.get_axis(1))
-        left_trigger = stick_deadzone(self.gamepad.get_axis(2))
-        right_stick_x = stick_deadzone(self.gamepad.get_axis(3))
-        right_stick_y = stick_deadzone(self.gamepad.get_axis(4))
-        right_trigger = stick_deadzone(self.gamepad.get_axis(5))
-        button_a = self.gamepad.get_button(0)
-        button_b = self.gamepad.get_button(1)
-        button_x = self.gamepad.get_button(2)
-        button_y = self.gamepad.get_button(3)
-        left_bumper = self.gamepad.get_button(4)
-        right_bumper = self.gamepad.get_button(5)
-        dpad_input = self.gamepad.get_hat(0)
-
-        # OLD MANUAL
-        # ==========
-
-        if not self.use_arm_ik and self.use_old_topics:
-            arm_input = ArmManual()
-
-            # OLD ARM MANUAL CONTROL SCHEME
-            if not self.use_new_arm_manual_scheme:
-                # 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) > 0.15:
-                        arm_input.axis1 = round(left_stick_x)
-
-                    # Axis 2
-                    if abs(left_stick_y) > 0.15:
-                        arm_input.axis2 = -1 * round(left_stick_y)
-
-                    # Axis 3
-                    if abs(right_stick_y) > 0.15:
-                        arm_input.axis3 = -1 * round(right_stick_y)
-
-            # NEW ARM MANUAL CONTROL SCHEME
-            if self.use_new_arm_manual_scheme:
-                # Right stick: EF yaw and axis 3
-                # Left stick: axis 1 and 2
-                # D-pad: axis 0 and _
-                # Triggers: EF grippers
-                # Bumpers: EF roll
-                # A: brake
-                # B: linear actuator in
-                # X: _
-                # Y: linear actuator out
-
-                # Right stick: EF yaw and axis 3
-                arm_input.effector_yaw = stick_to_arm_direction(right_stick_x)
-                arm_input.axis3 = -1 * stick_to_arm_direction(right_stick_y)
-
-                # Left stick: axis 1 and 2
-                arm_input.axis1 = stick_to_arm_direction(left_stick_x)
-                arm_input.axis2 = -1 * stick_to_arm_direction(left_stick_y)
-
-                # D-pad: axis 0 and _
-                arm_input.axis0 = int(dpad_input[0])
-
-                # Triggers: 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
-
-                # Bumpers: EF roll
-                if left_bumper > 0 and right_bumper > 0:
-                    arm_input.effector_roll = 0
-                elif left_bumper > 0:
-                    arm_input.effector_roll = -1
-                elif right_bumper > 0:
-                    arm_input.effector_roll = 1
-
-                # A: brake
-                if button_a:
-                    arm_input.brake = True
-
-                # Y: linear actuator
-                if button_y and not button_b:
-                    arm_input.linear_actuator = 1
-                elif button_b and not button_y:
-                    arm_input.linear_actuator = -1
-                else:
-                    arm_input.linear_actuator = 0
-
-            self.arm_publisher.publish(arm_input)
-
-        # NEW MANUAL
-        # ==========
-
-        elif not self.use_arm_ik and not self.use_old_topics:
-            arm_input = JointJog()
-            arm_input.header.frame_id = "base_link"
-            arm_input.header.stamp = self.get_clock().now().to_msg()
-            arm_input.joint_names = self.all_joint_names
-            arm_input.velocities = [0.0] * len(self.all_joint_names)
-
-            # Right stick: EF yaw and axis 3
-            # Left stick: axis 1 and 2
-            # D-pad: axis 0 and _
-            # Triggers: EF grippers
-            # Bumpers: EF roll
-            # A: brake
-            # B: linear actuator in
-            # X: _
-            # Y: linear actuator out
-
-            # Right stick: EF yaw and axis 3
-            arm_input.velocities[self.all_joint_names.index("wrist_yaw_joint")] = float(
-                stick_to_arm_direction(right_stick_x)
-            )
-            arm_input.velocities[self.all_joint_names.index("axis_3_joint")] = float(
-                stick_to_arm_direction(right_stick_y)
-            )
-
-            # Left stick: axis 1 and 2
-            arm_input.velocities[self.all_joint_names.index("axis_1_joint")] = float(
-                stick_to_arm_direction(left_stick_x)
-            )
-            arm_input.velocities[self.all_joint_names.index("axis_2_joint")] = float(
-                stick_to_arm_direction(left_stick_y)
-            )
-
-            # D-pad: axis 0 and _
-            arm_input.velocities[self.all_joint_names.index("axis_0_joint")] = float(
-                dpad_input[0]
-            )
-
-            # Triggers: EF Grippers
-            if left_trigger > 0 and right_trigger > 0:
-                arm_input.velocities[
-                    self.all_joint_names.index("ef_gripper_left_joint")
-                ] = 0.0
-            elif left_trigger > 0:
-                arm_input.velocities[
-                    self.all_joint_names.index("ef_gripper_left_joint")
-                ] = -1.0
-            elif right_trigger > 0:
-                arm_input.velocities[
-                    self.all_joint_names.index("ef_gripper_left_joint")
-                ] = 1.0
-
-            # Bumpers: EF roll
-            arm_input.velocities[self.all_joint_names.index("wrist_roll_joint")] = (
-                right_bumper - left_bumper
-            )
-
-            # A: brake
-            # TODO: Brake mode
-
-            # Y: linear actuator
-            # TODO: linear actuator
-
-            self.arm_manual_pub_.publish(arm_input)
-
-        # IK (ONLY NEW)
-        # =============
-
-        elif self.use_arm_ik:
-            arm_twist = TwistStamped()
-            arm_twist.header.frame_id = "base_link"
-            arm_twist.header.stamp = self.get_clock().now().to_msg()
-            arm_jointjog = JointJog()
-            arm_jointjog.header.frame_id = "base_link"
-            arm_jointjog.header.stamp = self.get_clock().now().to_msg()
-
-            # Right stick: linear y and linear x
-            # Left stick: angular z and linear z
-            # D-pad: angular y and _
-            # Triggers: EF grippers
-            # Bumpers: angular x
-            # A: brake
-            # B: IK mode
-            # X: manual mode
-            # Y: linear actuator
-
-            # Right stick: linear y and linear x
-            arm_twist.twist.linear.y = float(right_stick_x)
-            arm_twist.twist.linear.x = float(right_stick_y)
-
-            # Left stick: angular z and linear z
-            arm_twist.twist.angular.z = float(-1 * left_stick_x)
-            arm_twist.twist.linear.z = float(-1 * left_stick_y)
-            # D-pad: angular y and _
-            arm_twist.twist.angular.y = (
-                float(0)
-                if dpad_input[0] == 0
-                else float(-1 * copysign(0.75, dpad_input[0]))
-            )
-
-            # Triggers: EF Grippers
-            if left_trigger > 0 or right_trigger > 0:
-                arm_jointjog.joint_names.append("ef_gripper_left_joint")  # type: ignore
-                arm_jointjog.velocities.append(float(right_trigger - left_trigger))
-
-            # Bumpers: angular x
-            if left_bumper > 0 and right_bumper > 0:
-                arm_twist.twist.angular.x = float(0)
-            elif left_bumper > 0:
-                arm_twist.twist.angular.x = float(1)
-            elif right_bumper > 0:
-                arm_twist.twist.angular.x = float(-1)
-
-            self.arm_ik_twist_publisher.publish(arm_twist)
-            # self.arm_ik_jointjog_publisher.publish(arm_jointjog)  # TODO: Figure this shit out
-
-    def send_bio(self):
-        # Collect controller state
-        left_stick_x = stick_deadzone(self.gamepad.get_axis(0))
-        left_stick_y = stick_deadzone(self.gamepad.get_axis(1))
-        left_trigger = stick_deadzone(self.gamepad.get_axis(2))
-        right_stick_x = stick_deadzone(self.gamepad.get_axis(3))
-        right_stick_y = stick_deadzone(self.gamepad.get_axis(4))
-        right_trigger = stick_deadzone(self.gamepad.get_axis(5))
-        button_a = self.gamepad.get_button(0)
-        button_b = self.gamepad.get_button(1)
-        button_x = self.gamepad.get_button(2)
-        button_y = self.gamepad.get_button(3)
-        left_bumper = self.gamepad.get_button(4)
-        right_bumper = self.gamepad.get_button(5)
-        dpad_input = self.gamepad.get_hat(0)
-
-        if self.use_old_topics:
-            bio_input = BioControl(
-                bio_arm=int(left_stick_y * -100),
-                drill_arm=int(round(right_stick_y) * -100),
-            )
-
-            # Drill motor (FAERIE)
-            if left_trigger > 0 or right_trigger > 0:
-                bio_input.drill = int(
-                    30 * (right_trigger - left_trigger)
-                )  # Max duty cycle 30%
-
-            self.bio_publisher.publish(bio_input)
-
-        else:
-            pass  # TODO: implement new bio control topics
-
-    def arm_manual_stop_msg(self):
-        return JointJog(
-            header=Header(frame_id="base_link", stamp=self.get_clock().now().to_msg()),
-            joint_names=self.all_joint_names,
-            velocities=[0.0] * len(self.all_joint_names),
-        )
-
-    def arm_ik_twist_stop_msg(self):
-        return TwistStamped(
-            header=Header(frame_id="base_link", stamp=self.get_clock().now().to_msg())
-        )
-
-
-def stick_deadzone(value: float, threshold=STICK_DEADZONE) -> float:
-    """Apply a deadzone to a joystick input so the motors don't sound angry"""
-    if abs(value) < threshold:
-        return 0
-    return value
-
-
-def stick_to_arm_direction(value: float, threshold=ARM_DEADZONE) -> int:
-    """Apply a larger deadzone to a stick input and make digital/binary instead of analog"""
-    if abs(value) < threshold:
-        return 0
-    return int(copysign(1, value))
-
-
-def is_user_in_group(group_name: str) -> bool:
-    # Copied from https://zetcode.com/python/os-getgrouplist/
-    try:
-        username = os.getlogin()
-
-        # Get group ID from name
-        group_info = grp.getgrnam(group_name)
-        target_gid = group_info.gr_gid
-
-        # Get user's groups
-        user_info = pwd.getpwnam(username)
-        user_groups = os.getgrouplist(username, user_info.pw_gid)
-
-        return target_gid in user_groups
-    except KeyError:
-        return False
-
-
-def exit_handler(signum, frame):
-    print("Caught SIGTERM. Exiting...")
-    rclpy.try_shutdown()
-    sys.exit(0)
-
-
-def main(args=None):
-    try:
-        rclpy.init(args=args)
-
-        # Catch termination signals and exit cleanly
-        signal.signal(signal.SIGTERM, exit_handler)
-
-        node = Headless()
-        rclpy.spin(node)
-    except (KeyboardInterrupt, ExternalShutdownException):
-        print("Caught shutdown signal. Exiting...")
-    finally:
-        rclpy.try_shutdown()
-
-
-if __name__ == "__main__":
-    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,106 +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)
-    {
-        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));
-
-        target_mcu_ = this->declare_parameter<std::string>("target_mcu", "core");
-    }
-
-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

treefmt.nix

@@ -1,9 +0,0 @@
-{ ... }:
-{
-  projectRootFile = "flake.nix";
-  programs = {
-    nixfmt.enable = true;
-    black.enable = true;
-    shfmt.enable = true;
-  };
-}