add missing packages for moveit

Anchor Serial Refactor

.envrc

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

.gitignore

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

.gitmodules

@@ -1,3 +1,6 @@
-[submodule "src/ros2_interfaces_pkg"]
-	path = src/ros2_interfaces_pkg
-	url = git@github.com:SHC-ASTRA/ros2_interfaces_pkg.git
+[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

LICENSE

@@ -0,0 +1,661 @@
+                    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,87 +1,148 @@
-# rover-ros2
+# ASTRA Rover ROS2 Packages
 
-Submodule which includes all ros2 packages for the rover. These are centrally located for modular rover operation.
+[![License: AGPL v3](https://img.shields.io/badge/License-AGPL_v3-blue.svg)](https://www.gnu.org/licenses/agpl-3.0)
 
-You will use this package to launch any module-side ROS2 nodes.
-<br>
+Includes all main ROS2 packages for the rover. These are centrally located for modular rover operation.
 
-## Software Pre-reqs
+You will use these packages to launch all rover-side ROS2 nodes.
 
-An acting base station computer will need several things:
+## Table of Contents
 
-* 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](#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)
+- [Maintainers](#maintainers)
 
-## Launching with ANCHOR
+## Software Prerequisites
 
-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`
+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.
 
-## Launching as Standalone
+### Nix
 
-For use when running independent modules through their respective computers (pi/NUC) without ANCHOR.
+With Nix, all you have to do is enter the development shell:
 
-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`
+```bash
+$ cd path/to/rover-ros2
+$ nix develop
+```
 
-## Running Headless
+### ROS2 Humble + rosdep
 
-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`
+With ROS2 Humble, start by using rosdep to install dependencies:
 
-## Connecting the GuliKit Controller
+```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 (Recommended)
+## Running
 
-* Connect controller to pc with USB-C
+```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.
+```
+
+### 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 `PORT_OVERRIDE` environment variable to point it to the fake serial port, like so:
+
+```bash
+$ PORT_OVERRIDE=/tmp/ttyACM9 ros2 launch anchor_pkg rover.launch.py
+```
+
+### 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
 * 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
+  * 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.
+
+## Maintainers
+
+| Name | Email | Discord |
+| ---- | ----- | ------- |
+| David Sharpe | <ds0196@uah.edu> | `@ddavdd` |
+| Riley McLain | <rjm0037@uah.edu> | `@ryleu` |

auto_start/auto_start_anchor.sh

@@ -0,0 +1,28 @@
+#!/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

@@ -0,0 +1,29 @@
+#!/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

@@ -0,0 +1,30 @@
+#!/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

@@ -0,0 +1,84 @@
+{
+  "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": 1770108954,
+        "narHash": "sha256-VBj6bd4LPPSfsZJPa/UPPA92dOs6tmQo0XZKqfz/3W4=",
+        "owner": "lopsided98",
+        "repo": "nix-ros-overlay",
+        "rev": "3d05d46451b376e128a1553e78b8870c75d7753a",
+        "type": "github"
+      },
+      "original": {
+        "owner": "lopsided98",
+        "ref": "develop",
+        "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"
+        ]
+      }
+    },
+    "systems": {
+      "locked": {
+        "lastModified": 1681028828,
+        "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=",
+        "owner": "nix-systems",
+        "repo": "default",
+        "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e",
+        "type": "github"
+      },
+      "original": {
+        "owner": "nix-systems",
+        "repo": "default",
+        "type": "github"
+      }
+    }
+  },
+  "root": "root",
+  "version": 7
+}

flake.nix

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

rover_launch.py

@@ -1,132 +0,0 @@
-#!/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)
-    ])

src/anchor_pkg/anchor_pkg/anchor_node.py

@@ -1,5 +1,6 @@
 import rclpy
 from rclpy.node import Node
+from rclpy.executors import ExternalShutdownException
 from std_srvs.srv import Empty
 
 import signal
@@ -7,92 +8,222 @@ import time
 import atexit
 
 import serial
+import serial.tools.list_ports
+import os
 import sys
 import threading
 import glob
 
-from std_msgs.msg import String
-from ros2_interfaces_pkg.msg import CoreFeedback
-from ros2_interfaces_pkg.msg import CoreControl
+from std_msgs.msg import String, Header
+from astra_msgs.msg import VicCAN
 
-serial_pub = None
-thread = None
+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
+]
+
+
+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
+
+    Subscribers:
+    * /anchor/from_vic/mock_mcu
+        - For testing without an actual MCU, publish strings here as if they came from an MCU
+    * /anchor/to_vic/relay
+        - Core, Arm, and Bio publish VicCAN messages to this topic to send to the MCU
+    * /anchor/to_vic/relay_string
+        - Publish raw strings to this topic to send directly to the MCU for debugging
+    """
 
-class SerialRelay(Node):
     def __init__(self):
         # Initalize node with name
-        super().__init__("anchor_node")#previously 'serial_publisher'
+        super().__init__("anchor_node")  # previously 'serial_publisher'
 
+        self.serial_port: str | None = None  # e.g., "/dev/ttyUSB0"
 
-        # 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)
+        # Serial port override
+        if port_override := os.getenv("PORT_OVERRIDE"):
+            self.serial_port = port_override
 
-        self.debug_pub = self.create_publisher(String, '/anchor/debug', 10)
-        
-        # Create a subscriber 
-        self.relay_sub = self.create_subscription(String, '/anchor/relay', self.send_cmd, 10)
+        ##################################################
+        # Serial MCU Discovery
 
+        # If there was not a port override, look for a MCU over USB for Serial.
+        if self.serial_port is None:
+            comports = serial.tools.list_ports.comports()
+            real_ports = list(
+                filter(
+                    lambda p: p.vid is not None
+                    and p.pid is not None
+                    and p.device is not None,
+                    comports,
+                )
+            )
+            recog_ports = list(filter(lambda p: (p.vid, p.pid) in KNOWN_USBS, comports))
+
+            if len(recog_ports) == 1:  # Found singular recognized MCU
+                found_port = recog_ports[0]
+                self.get_logger().info(
+                    f"Selecting MCU '{found_port.description}' at {found_port.device}."
+                )
+                self.serial_port = found_port.device  # String, location of device file; e.g., '/dev/ttyACM0'
+            elif len(recog_ports) > 1:  # Found multiple recognized MCUs
+                # Kinda jank log message
+                self.get_logger().error(
+                    f"Found multiple recognized MCUs: {[p.device for p in recog_ports].__str__()}"
+                )
+                # Don't set self.serial_port; later if-statement will exit()
+            elif (
+                len(recog_ports) == 0 and len(real_ports) > 0
+            ):  # Found real ports but none recognized; i.e. maybe found an IMU or camera but not a MCU
+                self.get_logger().error(
+                    f"No recognized MCUs found; instead found {[p.device for p in real_ports].__str__()}."
+                )
+                # Don't set self.serial_port; later if-statement will exit()
+            else:  # Found jack shit
+                self.get_logger().error("No valid Serial ports specified or found.")
+                # Don't set self.serial_port; later if-statement will exit()
+
+        # We still don't have a serial port; fall back to legacy discovery (Areeb's code)
         # Loop through all serial devices on the computer to check for the MCU
-        self.port = None
-        ports = SerialRelay.list_serial_ports()
-        for i in range(4):
-            for port in ports:
-                try:
-                    # connect and send a ping command
-                    ser = serial.Serial(port, 115200, timeout=1)
-                    #(f"Checking port {port}...")
-                    ser.write(b"ping\n")
-                    response = ser.read_until("\n")
+        if self.serial_port is None:
+            self.get_logger().warning("Falling back to legacy MCU discovery...")
+            ports = Anchor.list_serial_ports()
+            for _ in range(4):
+                if self.serial_port is not None:
+                    break
+                for port in ports:
+                    try:
+                        # connect and send a ping command
+                        ser = serial.Serial(port, 115200, timeout=1)
+                        # (f"Checking port {port}...")
+                        ser.write(b"ping\n")
+                        response = ser.read_until(bytes("\n", "utf8"))
 
-                    # if pong is in response, then we are talking with the MCU
-                    if b"pong" in response:
-                        self.port = port
-                        self.get_logger().info(f"Found MCU at {self.port}!")
-                        self.get_logger().info(f"Enabling Relay Mode")
-                        ser.write(b"can_relay_mode,on\n")
-                        break
-                except:
-                    pass
-            if self.port is not None:
-                break
-        
-        if self.port is None:
-            self.get_logger().info("Unable to find MCU...")
+                        # if pong is in response, then we are talking with the MCU
+                        if b"pong" in response:
+                            self.serial_port = port
+                            self.get_logger().info(f"Found MCU at {self.serial_port}!")
+                            break
+                    except:
+                        pass
+
+        # If port is still None then we ain't finding no mcu
+        if self.serial_port is None:
+            self.get_logger().error("Unable to find MCU. Exiting...")
             time.sleep(1)
             sys.exit(1)
-        
-        self.ser = serial.Serial(self.port, 115200)
+        # Found a Serial port, try to open it; above code has not officially opened a Serial port
+        else:
+            self.get_logger().debug(
+                f"Attempting to open Serial port '{self.serial_port}'..."
+            )
+            try:
+                self.serial_interface = serial.Serial(
+                    self.serial_port, 115200, timeout=1
+                )
+
+                # Attempt to get name of connected MCU
+                self.serial_interface.write(
+                    b"can_relay_mode,on\n"
+                )  # can_relay_ready,[mcu]
+                mcu_name: str = ""
+                for _ in range(4):
+                    response = self.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:
+                                mcu_name = args[1]
+                            break
+                    except UnicodeDecodeError:
+                        pass  # ignore malformed responses
+                self.get_logger().info(
+                    f"MCU '{mcu_name}' is ready at '{self.serial_port}'."
+                )
+
+            except serial.SerialException as e:
+                self.get_logger().error(
+                    f"Could not open Serial port '{self.serial_port}' for reason:"
+                )
+                self.get_logger().error(e.strerror)
+                time.sleep(1)
+                sys.exit(1)
+
+        # Close serial port on exit
         atexit.register(self.cleanup)
 
+        ##################################################
+        # ROS2 Topic Setup
 
-    def run(self):
-        # This thread makes all the update processes run in the background
-        global thread
-        thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
-        thread.start()
-        
-        try:
-            while rclpy.ok():
-                self.read_MCU() # Check the MCU for updates
-        except KeyboardInterrupt:
-            sys.exit(0)
+        # New pub/sub with VicCAN
+        self.fromvic_debug_pub_ = self.create_publisher(
+            String, "/anchor/from_vic/debug", 20
+        )
+        self.fromvic_core_pub_ = self.create_publisher(
+            VicCAN, "/anchor/from_vic/core", 20
+        )
+        self.fromvic_arm_pub_ = self.create_publisher(
+            VicCAN, "/anchor/from_vic/arm", 20
+        )
+        self.fromvic_bio_pub_ = self.create_publisher(
+            VicCAN, "/anchor/from_vic/bio", 20
+        )
+
+        self.mock_mcu_sub_ = self.create_subscription(
+            String, "/anchor/from_vic/mock_mcu", self.on_mock_fromvic, 20
+        )
+        self.tovic_sub_ = self.create_subscription(
+            VicCAN, "/anchor/to_vic/relay", self.on_relay_tovic_viccan, 20
+        )
+        self.tovic_debug_sub_ = self.create_subscription(
+            String, "/anchor/to_vic/relay_string", self.on_relay_tovic_string, 20
+        )
+
+        # Create publishers
+        self.arm_pub = self.create_publisher(String, "/anchor/arm/feedback", 10)
+        self.core_pub = self.create_publisher(String, "/anchor/core/feedback", 10)
+        self.bio_pub = self.create_publisher(String, "/anchor/bio/feedback", 10)
+
+        self.debug_pub = self.create_publisher(String, "/anchor/debug", 10)
+
+        # Create a subscriber
+        self.relay_sub = self.create_subscription(
+            String, "/anchor/relay", self.on_relay_tovic_string, 10
+        )
 
     def read_MCU(self):
+        """Check the USB serial port for new data from the MCU, and publish string to appropriate topics"""
         try:
-            output = str(self.ser.readline(), "utf8")
-            
+            output = str(self.serial_interface.readline(), "utf8")
+
             if output:
+                self.relay_fromvic(output)
                 # All output over debug temporarily
-                #self.get_logger().info(f"[MCU] {output}")
+                # self.get_logger().info(f"[MCU] {output}")
                 msg = String()
                 msg.data = output
+                self.debug_pub.publish(msg)
                 if output.startswith("can_relay_fromvic,core"):
                     self.core_pub.publish(msg)
-                elif output.startswith("can_relay_fromvic,arm") or output.startswith("can_relay_fromvic,digit"):  # digit for voltage readings
+                elif output.startswith("can_relay_fromvic,arm") or output.startswith(
+                    "can_relay_fromvic,digit"
+                ):  # digit for voltage readings
                     self.arm_pub.publish(msg)
-                elif output.startswith("can_relay_fromvic,citadel") or output.startswith("can_relay_fromvic,digit"):  # digit for SHT sensor
+                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
@@ -101,52 +232,134 @@ class SerialRelay(Node):
         except serial.SerialException as e:
             print(f"SerialException: {e}")
             print("Closing serial port.")
-            if self.ser.is_open:
-                self.ser.close()
-            self.exit(1)
+            try:
+                if self.serial_interface.is_open:
+                    self.serial_interface.close()
+            except:
+                pass
+            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)
+            try:
+                if self.serial_interface.is_open:
+                    self.serial_interface.close()
+            except:
+                pass
+            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)
+            # print("Closing serial port.")
+            # if self.ser.is_open:
+            #     self.ser.close()
+            # exit(1)
 
-    def send_cmd(self, msg):
+    def on_mock_fromvic(self, msg: String):
+        """For testing without an actual MCU, publish strings here as if they came from an MCU"""
+        # self.get_logger().info(f"Got command from mock MCU: {msg}")
+        self.relay_fromvic(msg.data)
+
+    def on_relay_tovic_viccan(self, msg: VicCAN):
+        """Relay a VicCAN message to the MCU"""
+        output: str = f"can_relay_tovic,{msg.mcu_name},{msg.command_id}"
+        for num in msg.data:
+            output += f",{round(num, 7)}"  # limit to 7 decimal places
+        output += "\n"
+        # self.get_logger().info(f"VicCAN relay to MCU: {output}")
+        self.serial_interface.write(bytes(output, "utf8"))
+
+    def relay_fromvic(self, msg: str):
+        """Relay a string message from the MCU to the appropriate VicCAN topic"""
+        self.fromvic_debug_pub_.publish(String(data=msg))
+        parts = msg.strip().split(",")
+        if len(parts) > 0 and parts[0] != "can_relay_fromvic":
+            self.get_logger().debug(f"Ignoring non-VicCAN message: '{msg.strip()}'")
+            return
+
+        # String validation
+        malformed: bool = False
+        malformed_reason: str = ""
+        if len(parts) < 3 or len(parts) > 7:
+            malformed = True
+            malformed_reason = (
+                f"invalid argument count (expected [3,7], got {len(parts)})"
+            )
+        elif parts[1] not in ["core", "arm", "digit", "citadel", "broadcast"]:
+            malformed = True
+            malformed_reason = f"invalid mcu_name '{parts[1]}'"
+        elif not (parts[2].isnumeric()) or int(parts[2]) < 0:
+            malformed = True
+            malformed_reason = f"command_id '{parts[2]}' is not a non-negative integer"
+        else:
+            for x in parts[3:]:
+                try:
+                    float(x)
+                except ValueError:
+                    malformed = True
+                    malformed_reason = f"data '{x}' is not a float"
+                    break
+
+        if malformed:
+            self.get_logger().warning(
+                f"Ignoring malformed from_vic message: '{msg.strip()}'; reason: {malformed_reason}"
+            )
+            return
+
+        # Have valid VicCAN message
+
+        output = VicCAN()
+        output.mcu_name = parts[1]
+        output.command_id = int(parts[2])
+        if len(parts) > 3:
+            output.data = [float(x) for x in parts[3:]]
+        output.header = Header(
+            stamp=self.get_clock().now().to_msg(), frame_id="from_vic"
+        )
+
+        # self.get_logger().info(f"Relaying from MCU: {output}")
+        if output.mcu_name == "core":
+            self.fromvic_core_pub_.publish(output)
+        elif output.mcu_name == "arm" or output.mcu_name == "digit":
+            self.fromvic_arm_pub_.publish(output)
+        elif output.mcu_name == "citadel" or output.mcu_name == "digit":
+            self.fromvic_bio_pub_.publish(output)
+
+    def on_relay_tovic_string(self, msg: String):
+        """Relay a raw string message to the MCU for debugging"""
         message = msg.data
-        #self.get_logger().info(f"Sending command to MCU: {msg}")
-        self.ser.write(bytes(message, "utf8"))
+        # self.get_logger().info(f"Sending command to MCU: {msg}")
+        self.serial_interface.write(bytes(message, "utf8"))
 
     @staticmethod
     def list_serial_ports():
         return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
-    
+
     def cleanup(self):
         print("Cleaning up before terminating...")
-        if self.ser.is_open:
-            self.ser.close()
-
-def myexcepthook(type, value, tb):
-    print("Uncaught exception:", type, value)
-    if serial_pub:
-        serial_pub.cleanup()
+        if self.serial_interface.is_open:
+            self.serial_interface.close()
 
 
 def main(args=None):
-    rclpy.init(args=args)
-    sys.excepthook = myexcepthook
+    try:
+        rclpy.init(args=args)
+        anchor_node = Anchor()
 
-    global serial_pub
+        thread = threading.Thread(target=rclpy.spin, args=(anchor_node,), daemon=True)
+        thread.start()
 
-    serial_pub = SerialRelay()
-    serial_pub.run()
+        rate = anchor_node.create_rate(100)  # 100 Hz -- arbitrary rate
+        while rclpy.ok():
+            anchor_node.read_MCU()  # Check the MCU for updates
+            rate.sleep()
+    except (KeyboardInterrupt, ExternalShutdownException):
+        print("Caught shutdown signal, shutting down...")
+    finally:
+        rclpy.try_shutdown()
 
-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.SIGTERM, lambda signum, frame: sys.exit(0)
+    )  # Catch termination signals and exit cleanly
     main()

src/anchor_pkg/launch/rover.launch.py

@@ -0,0 +1,136 @@
+#!/usr/bin/env python3
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown
+from launch.substitutions import (
+    LaunchConfiguration,
+    ThisLaunchFileDir,
+    PathJoinSubstitution,
+)
+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)])

src/anchor_pkg/package.xml

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

src/anchor_pkg/setup.py

@@ -1,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]),
-        ('share/' + package_name, ['package.xml']),
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        (path.join("share", package_name), ["package.xml"]),
+        (path.join("share", package_name, "launch"), glob("launch/*")),
     ],
-    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

@@ -12,14 +12,17 @@ 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
+from astra_msgs.msg import ControllerState
+from astra_msgs.msg import ArmManual
+from astra_msgs.msg import ArmIK
 
 
-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()
+)
 os.environ["SDL_VIDEODRIVER"] = "dummy"  # Prevents pygame from trying to open a display
 
+
 class Headless(Node):
     def __init__(self):
         # Initalize node with name
@@ -30,20 +33,18 @@ class Headless(Node):
 
         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)
+        # 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.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
 
@@ -66,38 +67,36 @@ class Headless(Node):
         # 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()}')
+        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
+                # Check the pico for updates
 
-                self.read_feedback()
-                if pygame.joystick.get_count() == 0: #if controller disconnected, wait for it to be reconnected
+                # 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_controls() #depricated, kept for reference temporarily
                         self.send_manual()
-                        self.read_feedback()
+                        # self.read_feedback()
                     self.gamepad = pygame.joystick.Joystick(0)
-                    self.gamepad.init() #re-initialized gamepad
+                    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:
@@ -105,21 +104,20 @@ class Headless(Node):
                 exit()
         input = ArmManual()
 
-
         # Triggers for gripper control
-        if self.gamepad.get_axis(2) > 0:#left trigger
+        if self.gamepad.get_axis(2) > 0:  # left trigger
             input.gripper = -1
-        elif self.gamepad.get_axis(5) > 0:#right trigger
+        elif self.gamepad.get_axis(5) > 0:  # right trigger
             input.gripper = 1
 
         # Toggle Laser
-        if self.gamepad.get_button(7):#Start
+        if self.gamepad.get_button(7):  # Start
             self.laser_status = 1
-        elif self.gamepad.get_button(6):#Back
+        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
+        if self.gamepad.get_button(5):  # right bumper, control effector
 
             # Left stick X-axis for effector yaw
             if self.gamepad.get_axis(0) > 0:
@@ -133,7 +131,7 @@ class Headless(Node):
             elif self.gamepad.get_axis(3) < 0:
                 input.effector_roll = -1
 
-        else:   # Control arm axis
+        else:  # Control arm axis
             dpad_input = self.gamepad.get_hat(0)
             input.axis0 = 0
             if dpad_input[0] == 1:
@@ -141,44 +139,44 @@ class Headless(Node):
             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(0) > 0.15 or self.gamepad.get_axis(0) < -0.15:
+                input.axis1 = round(self.gamepad.get_axis(0))
 
-            if self.gamepad.get_axis(1) > .15 or self.gamepad.get_axis(1) < -.15:
+            if self.gamepad.get_axis(1) > 0.15 or self.gamepad.get_axis(1) < -0.15:
                 input.axis2 = -1 * round(self.gamepad.get_axis(1))
 
-            if self.gamepad.get_axis(4) > .15 or self.gamepad.get_axis(4) < -.15:
+            if self.gamepad.get_axis(4) > 0.15 or self.gamepad.get_axis(4) < -0.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
+        # 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.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):
 
@@ -190,7 +188,7 @@ class Headless(Node):
 
     #     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
@@ -202,7 +200,7 @@ class Headless(Node):
     #         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
@@ -218,7 +216,7 @@ class Headless(Node):
     #     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.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
@@ -241,14 +239,12 @@ class Headless(Node):
     #     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:
@@ -257,10 +253,9 @@ class Headless(Node):
     #         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)
@@ -268,36 +263,17 @@ class Headless(Node):
     #         pass
 
 
-            
-
-
-    def read_feedback(self, msg):
-        
-        # Create a string message object
-        #msg = String()
-
-        # Set message data
-        #msg.data = output
-
-        # Publish data
-        #self.publisher.publish(msg.data)
-        
-        print(f"[MCU] {msg.data}", end="")
-        #print(f"[Pico] Publishing: {msg}")
-
-        
-
 def main(args=None):
     rclpy.init(args=args)
 
     node = Headless()
-    
+
     rclpy.spin(node)
     rclpy.shutdown()
 
-    #tb_bs = BaseStation()
-    #node.run()
+    # tb_bs = BaseStation()
+    # node.run()
 
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     main()

src/arm_pkg/arm_pkg/arm_node.py

@@ -8,55 +8,93 @@ 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
+from astra_msgs.msg import ArmManual
+from astra_msgs.msg import SocketFeedback
+from astra_msgs.msg import DigitFeedback
+from sensor_msgs.msg import JointState
+import math
+
+# control_qos = qos.QoSProfile(
+#     history=qos.QoSHistoryPolicy.KEEP_LAST,
+#     depth=1,
+#     reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
+#     durability=qos.QoSDurabilityPolicy.VOLATILE,
+#     deadline=1000,
+#     lifespan=500,
+#     liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
+#     liveliness_lease_duration=5000
+# )
 
 serial_pub = None
 thread = None
 
+
 class SerialRelay(Node):
     def __init__(self):
         # Initialize node
         super().__init__("arm_node")
 
         # Get launch mode parameter
-        self.declare_parameter('launch_mode', 'arm')
-        self.launch_mode = self.get_parameter('launch_mode').value
+        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.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)
+        self.debug_pub = self.create_publisher(String, "/arm/feedback/debug", 10)
+        self.socket_pub = self.create_publisher(
+            SocketFeedback, "/arm/feedback/socket", 10
+        )
+        self.digit_pub = self.create_publisher(DigitFeedback, "/arm/feedback/digit", 10)
+        self.feedback_timer = self.create_timer(0.25, self.publish_feedback)
 
         # Create subscribers
-        self.ik_sub = self.create_subscription(ArmIK, '/arm/control/ik', self.send_ik, 10) 
-        self.man_sub = self.create_subscription(ArmManual, '/arm/control/manual', self.send_manual, 10)
+        self.man_sub = self.create_subscription(
+            ArmManual, "/arm/control/manual", self.send_manual, 10
+        )
+
+        # New messages
+        self.joint_state_pub = self.create_publisher(JointState, "joint_states", 10)
+        self.joint_state = JointState()
+        self.joint_state.name = [
+            "Axis_0_Joint",
+            "Axis_1_Joint",
+            "Axis_2_Joint",
+            "Axis_3_Joint",
+            "Wrist_Differential_Joint",
+            "Wrist-EF_Roll_Joint",
+            "Gripper_Slider_Left",
+        ]
+        self.joint_state.position = [0.0] * len(
+            self.joint_state.name
+        )  # Initialize with zeros
+
+        self.joint_command_sub = self.create_subscription(
+            JointState, "/joint_commands", self.joint_command_callback, 10
+        )
 
         # Topics used in anchor mode
-        if self.launch_mode == 'anchor':
-            self.anchor_sub = self.create_subscription(String, '/anchor/arm/feedback', self.anchor_feedback, 10)
-            self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10)
+        if self.launch_mode == "anchor":
+            self.anchor_sub = self.create_subscription(
+                String, "/anchor/arm/feedback", self.anchor_feedback, 10
+            )
+            self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)
 
         self.arm_feedback = SocketFeedback()
         self.digit_feedback = DigitFeedback()
 
         # Search for ports IF in 'arm' (standalone) and not 'anchor' mode
-        if self.launch_mode == 'arm':
+        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 _ 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}...")
+                        # print(f"Checking port {port}...")
                         ser.write(b"ping\n")
-                        response = ser.read_until("\n")
+                        response = ser.read_until("\n")  # type: ignore
 
                         # if pong is in response, then we are talking with the MCU
                         if b"pong" in response:
@@ -67,12 +105,14 @@ class SerialRelay(Node):
                         pass
                 if self.port is not None:
                     break
-            
+
             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)
-            
+
             self.ser = serial.Serial(self.port, 115200)
             atexit.register(self.cleanup)
 
@@ -80,12 +120,12 @@ class SerialRelay(Node):
         global thread
         thread = threading.Thread(target=rclpy.spin, args=(self,), daemon=True)
         thread.start()
-        
-        #if in arm mode, will need to read from the MCU 
+
+        # if in arm mode, will need to read from the MCU
 
         try:
             while rclpy.ok():
-                if self.launch_mode == 'arm':
+                if self.launch_mode == "arm":
                     if self.ser.in_waiting:
                         self.read_mcu()
                     else:
@@ -95,13 +135,12 @@ class SerialRelay(Node):
         finally:
             self.cleanup()
 
-
-    #Currently will just spit out all values over the /arm/feedback/debug topic as strings
+    # 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}")
+                # self.get_logger().info(f"[MCU] {output}")
                 msg = String()
                 msg.data = output
                 self.debug_pub.publish(msg)
@@ -123,67 +162,64 @@ class SerialRelay(Node):
                 self.ser.close()
             pass
 
-    def send_ik(self, msg):
-        pass
+    def joint_command_callback(self, msg: JointState):
+        # Embedded takes deg*10, ROS2 uses Radians
+        positions = [math.degrees(pos) * 10 for pos in msg.position]
+        # Axis 2 & 3 URDF direction is inverted
+        positions[2] = -positions[2]
+        positions[3] = -positions[3]
 
-    def send_manual(self, msg):
+        # Set target angles for each arm axis for embedded IK PID to handle
+        command = f"can_relay_tovic,arm,32,{positions[0]},{positions[1]},{positions[2]},{positions[3]}\n"
+        # Wrist yaw and roll
+        command += f"can_relay_tovic,digit,32,{positions[4]},{positions[5]}\n"
+        # Gripper IK does not have adequate hardware yet
+        self.send_cmd(command)
+
+    def send_manual(self, msg: ArmManual):
         axis0 = msg.axis0
         axis1 = -1 * msg.axis1
         axis2 = msg.axis2
         axis3 = msg.axis3
 
-        #Send controls for arm
-        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 += "can_relay_tovic,digit,36,0," + str(msg.effector_yaw) + "\n"
-        #self.send_cmd(command)
+        # 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,digit,26," + str(msg.gripper) + "\n"
-        #self.send_cmd(command)
+        # Send controls for end effector
+
+        command += f"can_relay_tovic,digit,39,{msg.effector_yaw},{msg.effector_roll}\n"
+
+        # 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,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
-    
-    def send_cmd(self, msg):
-        if self.launch_mode == 'anchor': #if in anchor mode, send to anchor node to relay
+
+    def send_cmd(self, msg: str):
+        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
+        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"))
 
-    def anchor_feedback(self, msg):
+    def anchor_feedback(self, msg: String):
         output = msg.data
         if output.startswith("can_relay_fromvic,arm,55"):
-            #pass
+            # pass
             self.updateAngleFeedback(output)
         elif output.startswith("can_relay_fromvic,arm,54"):
-            #pass
+            # 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(",")
@@ -198,6 +234,13 @@ class SerialRelay(Node):
             parts = msg.data.split(",")
             if len(parts) >= 4:
                 self.digit_feedback.wrist_angle = float(parts[3])
+                # self.digit_feedback.wrist_roll = float(parts[4])
+                self.joint_state.position[4] = math.radians(
+                    float(parts[4])
+                )  # Wrist roll
+                self.joint_state.position[5] = math.radians(
+                    float(parts[3])
+                )  # Wrist yaw
         else:
             return
 
@@ -205,45 +248,37 @@ class SerialRelay(Node):
         self.socket_pub.publish(self.arm_feedback)
         self.digit_pub.publish(self.digit_feedback)
 
-    def updateAngleFeedback(self, msg: String):
-                # Angle feedbacks,
-        #split the msg.data by commas
-        parts = msg.data.split(",")
-        
+    def updateAngleFeedback(self, msg: str):
+        # Angle feedbacks,
+        # split the msg.data by commas
+        parts = msg.split(",")
+
         if len(parts) >= 7:
             # Extract the angles from the string
             angles_in = parts[3:7]
             # Convert the angles to floats divide by 10.0
             angles = [float(angle) / 10.0 for angle in angles_in]
-            #angles[0] = 0.0 #override axis0 to zero
-            #
-            #
-            #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)
+
+            # Joint state publisher for URDF visualization
+            self.joint_state.position[0] = math.radians(angles[0])  # Axis 0
+            self.joint_state.position[1] = math.radians(angles[1])  # Axis 1
+            self.joint_state.position[2] = math.radians(-angles[2])  # Axis 2 (inverted)
+            self.joint_state.position[3] = math.radians(-angles[3])  # Axis 3 (inverted)
+            # Wrist is handled by digit feedback
+            self.joint_state.header.stamp = self.get_clock().now().to_msg()
+            self.joint_state_pub.publish(self.joint_state)
+
         else:
             self.get_logger().info("Invalid angle feedback input format")
 
-
-    def updateBusVoltage(self, msg: String):
+    def updateBusVoltage(self, msg: str):
         # Bus Voltage feedbacks
-        parts = msg.data.split(",")
+        parts = msg.split(",")
         if len(parts) >= 7:
             # Extract the voltage from the string
             voltages_in = parts[3:7]
@@ -255,41 +290,49 @@ class SerialRelay(Node):
         else:
             self.get_logger().info("Invalid voltage feedback input format")
 
-
-    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 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
 
     @staticmethod
     def list_serial_ports():
         return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
-        #return glob.glob("/dev/tty[A-Za-z]*")
+        # return glob.glob("/dev/tty[A-Za-z]*")
 
     def cleanup(self):
         print("Cleaning up...")
-        if self.ser.is_open:
-            self.ser.close()
+        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
@@ -298,7 +341,10 @@ 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/arm_pkg/package.xml

@@ -5,10 +5,12 @@
   <version>1.0.0</version>
   <description>Core arm package which handles ROS2 commnuication.</description>
   <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
-  <license>All Rights Reserved</license>
+  <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
-  <depend>ros2_interfaces_pkg</depend>
+  <depend>common_interfaces</depend>
+  <depend>python3-numpy</depend>
+  <depend>astra_msgs</depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/arm_pkg/setup.cfg

@@ -2,3 +2,5 @@
 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,26 +1,27 @@
 from setuptools import find_packages, setup
+import os
+from glob import glob
 
-package_name = 'arm_pkg'
+package_name = "arm_pkg"
 
 setup(
     name=package_name,
-    version='0.0.0',
-    packages=find_packages(exclude=['test']),
+    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']),
+        ("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='All Rights Reserved',
+    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',
-            'headless = arm_pkg.arm_headless:main'
+        "console_scripts": [
+            "arm = arm_pkg.arm_node:main",
+            "headless = arm_pkg.arm_headless:main",
         ],
     },
 )

src/bio_pkg/bio_pkg/bio_node.py

@@ -3,56 +3,62 @@ 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 ros2_interfaces_pkg.msg import BioControl
-from ros2_interfaces_pkg.msg import BioFeedback
+from astra_msgs.msg import BioControl
+from astra_msgs.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}...")
+                    # print(f"Checking port {port}...")
                     ser.write(b"ping\n")
                     response = ser.read_until("\n")
 
@@ -63,12 +69,14 @@ class SerialRelay(Node):
                         break
                 except:
                     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)
-            
+
             self.ser = serial.Serial(self.port, 115200)
             atexit.register(self.cleanup)
 
@@ -76,12 +84,12 @@ class SerialRelay(Node):
         global thread
         thread = threading.Thread(target=rclpy.spin, args=(self,), daemon=True)
         thread.start()
-        
-        #if in arm mode, will need to read from the MCU 
+
+        # if in arm mode, will need to read from the MCU
 
         try:
             while rclpy.ok():
-                if self.launch_mode == 'bio':
+                if self.launch_mode == "bio":
                     if self.ser.in_waiting:
                         self.read_mcu()
                     else:
@@ -91,8 +99,7 @@ class SerialRelay(Node):
         finally:
             self.cleanup()
 
-
-    #Currently will just spit out all values over the /arm/feedback/debug topic as strings
+    # 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")
@@ -122,69 +129,85 @@ 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)
-        # Vibration Motor
-        command = "can_relay_tovic,citadel,26," + str(msg.vibration_motor) + "\n"
-        self.send_cmd(command)
-        
+        # self.send_cmd(command)
 
-        # FAERIE Control Commands 
+        # Vibration Motor
+        command += "can_relay_tovic,citadel,26," + str(msg.vibration_motor) + "\n"
+        # self.send_cmd(command)
+
+        # FAERIE Control Commands
         ################
-        
+
         # To be reviewed before use#
 
         # Laser
-        command = "can_relay_tovic,digit,28," + str(msg.laser) + "\n"
-        self.send_cmd(command)
+        command += "can_relay_tovic,digit,28," + str(msg.laser) + "\n"
+        # self.send_cmd(command)
 
         # Drill (SCABBARD)
-        command = f"can_relay_tovic,digit,19,{msg.drill:.2f}\n"
-        print(msg.drill)
-        self.send_cmd(command)
+        command += f"can_relay_tovic,digit,19,{msg.drill:.2f}\n"
+        # self.send_cmd(command)
 
         # Bio linear actuator
-        command = "can_relay_tovic,digit,42," + str(msg.drill_arm) + "\n"
+        command += "can_relay_tovic,digit,42," + str(msg.drill_arm) + "\n"
         self.send_cmd(command)
 
-
-
     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"))
 
     def anchor_feedback(self, msg: String):
         output = msg.data
         parts = str(output.strip()).split(",")
-        self.get_logger().info(f"[Bio Anchor] {msg.data}")
+        # self.get_logger().info(f"[Bio Anchor] {msg.data}")
 
-        if output.startswith("can_relay_fromvic,citadel,54"):  # bat, 12, 5, Voltage readings * 100
+        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
@@ -198,18 +221,23 @@ class SerialRelay(Node):
     @staticmethod
     def list_serial_ports():
         return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
-        #return glob.glob("/dev/tty[A-Za-z]*")
+        # return glob.glob("/dev/tty[A-Za-z]*")
 
     def cleanup(self):
         print("Cleaning up...")
-        if self.ser.is_open:
-            self.ser.close()
+        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
@@ -218,7 +246,10 @@ 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,10 +5,11 @@
   <version>0.0.0</version>
   <description>TODO: Package description</description>
   <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
-  <license>TODO: License declaration</license>
+  <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
-  <depend>ros2_interfaces_pkg</depend>
+  <depend>common_interfaces</depend>
+  <depend>astra_msgs</depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/bio_pkg/setup.py

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

src/core_pkg/core_pkg/core_node.py

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

src/core_pkg/core_pkg/core_ptz.py

@@ -9,7 +9,7 @@ import threading
 import time
 
 from std_msgs.msg import String
-from ros2_interfaces_pkg.msg import PtzControl, PtzFeedback
+from astra_msgs.msg import PtzControl, PtzFeedback
 
 # Import the SIYI SDK
 from core_pkg.siyi_sdk import (
@@ -18,55 +18,63 @@ 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.get_logger().info(f"PTZ camera IP: {self.camera_ip} Port: {self.camera_port}")
-        
+        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}"
+        )
+
         # 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)
         self.last_data_time = time.time()
         self.health_check_timer = self.create_timer(2.0, self.check_camera_health)
-        
+
         # 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
         self.shutdown_requested = False
-        
+
         # Set up asyncio event loop in a separate thread
         self.thread_pool = ThreadPoolExecutor(max_workers=1)
         self.loop = asyncio.new_event_loop()
-        
+
         # Connect to camera on startup
         self.connect_task = self.thread_pool.submit(
             self.run_async_func, self.connect_to_camera()
@@ -77,28 +85,27 @@ class PtzNode(Node):
         try:
             # Create a new camera instance
             self.camera = SiyiGimbalCamera(ip=self.camera_ip, port=self.camera_port)
-            
+
             # Connect to the camera
             await self.camera.connect()
-            
+
             # Set up data callback
             self.camera.set_data_callback(self.camera_data_callback)
-            
+
             # 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
             self.camera_connected = True
             self.feedback_msg.connected = True
             self.feedback_msg.error_msg = ""
-            
+
             self.publish_debug("Camera connected successfully")
-            
+
         except Exception as e:
-            self.camera_connected = False 
+            self.camera_connected = False
             self.feedback_msg.connected = False
             self.feedback_msg.error_msg = f"Connection error: {str(e)}"
             self.publish_debug(f"Camera connection failed: {str(e)}")
@@ -108,8 +115,12 @@ 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
@@ -123,36 +134,39 @@ class PtzNode(Node):
                     debug_str = f"Camera data: CMD_ID={cmd_id.name}, Data="
                 else:
                     debug_str = f"Camera data: CMD_ID={cmd_id}, Data="
-                
+
                 if isinstance(data, bytes):
                     debug_str += data.hex()
                 else:
                     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:
             self.publish_debug("Attempting to reconnect to camera...")
             if self.camera:
                 try:
-                    if self.camera.is_connected: # SDK's internal connection state
-                         self.run_async_func(self.camera.disconnect())
+                    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(
                 self.run_async_func, self.connect_to_camera()
             )
 
     def check_camera_health(self):
         """Check if we're still receiving data from the camera"""
-        if self.camera_connected: # Only check health if we think we are connected
+        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)"
@@ -161,19 +175,20 @@ class PtzNode(Node):
     def handle_control_command(self, msg):
         """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")
+        if not self.camera:  # Still check if camera object exists
+            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.")
+        if not self.camera:
+            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.
@@ -182,43 +197,55 @@ class PtzNode(Node):
                 self.get_logger().info("Attempting to reset camera to center position")
                 await self.camera.send_attitude_angles_command(0.0, 0.0)
                 return
-                
+
             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: 
+            elif msg.control_mode == 3:
                 zoom_level = msg.zoom_level
-                self.get_logger().debug(f"Attempting absolute zoom: level={zoom_level}x")
+                self.get_logger().debug(
+                    f"Attempting absolute zoom: level={zoom_level}x"
+                )
                 await self.camera.send_absolute_zoom_command(zoom_level)
-                
-            if hasattr(msg, 'stream_type') and hasattr(msg, 'stream_freq'):
-                if msg.stream_type > 0 and msg.stream_freq >= 0: 
+
+            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)
                         stream_freq = DataStreamFrequency(msg.stream_freq)
                         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")
-            
-        except RuntimeError as e: # Catch SDK's "not connected" errors
+                        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}")
             # self.camera_connected will be updated by health/connection checks
             # self.feedback_msg.error_msg = f"Command failed: {str(e)}" # Already set by health check
@@ -226,58 +253,64 @@ class PtzNode(Node):
         except Exception as e:
             self.get_logger().error(f"Error processing control command: {e}")
             self.feedback_msg.error_msg = f"Control error: {str(e)}"
-            self.feedback_pub.publish(self.feedback_msg) # Publish for other errors
+            self.feedback_pub.publish(self.feedback_msg)  # Publish for other errors
 
     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().info(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
         self.get_logger().info("Async shutdown initiated...")
-        if self.camera and self.camera.is_connected: # Check SDK's connection state
+        if self.camera and self.camera.is_connected:  # Check SDK's connection state
             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) 
-                
+                await asyncio.sleep(0.1)
+
                 self.get_logger().info("Disconnecting from camera...")
                 await self.camera.disconnect()
                 self.get_logger().info("Disconnected from camera successfully.")
-                
+
             except Exception as e:
                 self.get_logger().error(f"Error during camera shutdown: {e}")
-        self.camera_connected = False # Update node's flag
+        self.camera_connected = False  # Update node's flag
         self.feedback_msg.connected = False
         self.feedback_msg.error_msg = "Shutting down"
 
     def cleanup(self):
         """Clean up resources."""
         self.get_logger().info("PTZ node cleanup initiated.")
-        self.shutdown_requested = True 
+        self.shutdown_requested = True
 
         if self.connection_timer:
             self.connection_timer.cancel()
@@ -287,31 +320,38 @@ 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.result(timeout=5) 
+                    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)
-            
+
             if self.loop.is_running():
                 self.get_logger().info("Stopping asyncio event loop...")
                 self.loop.call_soon_threadsafe(self.loop.stop)
-            
+
             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):
     """Main function."""
     rclpy.init(args=args)
-    
+
     ptz_node = PtzNode()
-    
+
     asyncio_thread = None
-    if ptz_node.loop: 
+    if ptz_node.loop:
+
         def run_event_loop(loop):
             asyncio.set_event_loop(loop)
             try:
@@ -322,14 +362,12 @@ def main(args=None):
                 # or an unhandled exception within a task scheduled on the loop.
                 if not loop.is_closed():
                     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()
-    
+
     try:
         rclpy.spin(ptz_node)
     except KeyboardInterrupt:
@@ -338,18 +376,18 @@ def main(args=None):
         ptz_node.get_logger().info("SystemExit received, shutting down...")
     finally:
         ptz_node.get_logger().info("Initiating final cleanup...")
-        ptz_node.cleanup() # This will stop the loop and shutdown the executor
-        
+        ptz_node.cleanup()  # This will stop the loop and shutdown the executor
+
         if asyncio_thread and asyncio_thread.is_alive():
-             # The loop should have been stopped by cleanup. We just join the thread.
-             ptz_node.get_logger().info("Waiting for asyncio thread to join...")
-             asyncio_thread.join(timeout=5) 
-             if asyncio_thread.is_alive():
-                 ptz_node.get_logger().warning("Asyncio thread did not join cleanly.")
-        
+            # The loop should have been stopped by cleanup. We just join the thread.
+            ptz_node.get_logger().info("Waiting for asyncio thread to join...")
+            asyncio_thread.join(timeout=5)
+            if asyncio_thread.is_alive():
+                ptz_node.get_logger().warning("Asyncio thread did not join cleanly.")
+
         rclpy.shutdown()
         ptz_node.get_logger().info("ROS shutdown complete.")
 
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     main()

src/core_pkg/core_pkg/siyi_sdk/siyi.py

@@ -110,9 +110,7 @@ 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
@@ -124,9 +122,7 @@ 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())
@@ -158,9 +154,7 @@ 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
@@ -179,15 +173,11 @@ 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."
@@ -199,21 +189,15 @@ 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."
@@ -221,17 +205,13 @@ 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)
@@ -254,9 +234,7 @@ 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)
@@ -279,7 +257,9 @@ 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."
             )
@@ -329,24 +309,24 @@ class SiyiGimbalCamera:
         ctrl = await self.reader.readexactly(1)
         data_len_bytes = await self.reader.readexactly(2)
         data_len = struct.unpack("<H", data_len_bytes)[0]
-        seq_bytes = await self.reader.readexactly(2) # Renamed for clarity
+        seq_bytes = await self.reader.readexactly(2)  # Renamed for clarity
         # seq_val = struct.unpack("<H", seq_bytes)[0] # If you need the sequence value
         cmd_id_bytes = await self.reader.readexactly(1)
-        cmd_id_val = cmd_id_bytes[0] # Renamed for clarity
-        
+        cmd_id_val = cmd_id_bytes[0]  # Renamed for clarity
+
         # Protect against excessively large data_len
-        if data_len > 2048: # Arbitrary reasonable limit
+        if data_len > 2048:  # Arbitrary reasonable limit
             raise ValueError(f"Excessive data length received: {data_len}")
 
         data = await self.reader.readexactly(data_len)
         crc_bytes = await self.reader.readexactly(2)
         received_crc = struct.unpack("<H", crc_bytes)[0]
-        
+
         packet_without_crc = (
             stx + ctrl + data_len_bytes + seq_bytes + cmd_id_bytes + data
         )
         computed_crc = Crc16.calc(packet_without_crc)
-        
+
         if computed_crc != received_crc:
             raise ValueError(
                 f"CRC check failed. Expected {computed_crc:04X}, got {received_crc:04X}. "
@@ -374,10 +354,7 @@ 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:
@@ -385,9 +362,7 @@ 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:
@@ -410,12 +385,12 @@ class SiyiGimbalCamera:
             except ValueError as e:
                 logger.error(f"Packet error in listener: {e}")
                 # Consider adding a small delay or a mechanism to resync if this happens frequently
-                await asyncio.sleep(0.1) # Small delay before trying to read again
+                await asyncio.sleep(0.1)  # Small delay before trying to read again
                 continue
             except Exception as e:
                 logger.exception(f"Unexpected error in data stream listener: {e}")
                 # Depending on the error, you might want to break or continue
-                await asyncio.sleep(0.1) # Small delay
+                await asyncio.sleep(0.1)  # Small delay
                 continue
 
     def set_data_callback(
@@ -424,12 +399,14 @@ class SiyiGimbalCamera:
         self._data_callback = callback
 
 
-async def main_sdk_test(): # Renamed to avoid conflict if this file is imported
+async def main_sdk_test():  # Renamed to avoid conflict if this file is imported
     gimbal_ip = "192.168.144.25"
     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}"
             )
@@ -460,7 +437,7 @@ async def main_sdk_test(): # Renamed to avoid conflict if this file is imported
             print("SDK Test: Setting zoom to 6.0x (A8 mini max)")
             await gimbal.send_absolute_zoom_command(6.0)
             await asyncio.sleep(2)
-            
+
             print("SDK Test: Attempting zoom to 7.0x (should be clamped to 6.0x)")
             await gimbal.send_absolute_zoom_command(7.0)
             await asyncio.sleep(2)
@@ -470,16 +447,19 @@ 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,7 +24,9 @@ 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)
 
@@ -35,13 +37,17 @@ 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,10 +5,13 @@
   <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>All Rights Reserved</license>
+  <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
-  <depend>ros2_interfaces_pkg</depend>
+  <depend>common_interfaces</depend>
+  <depend>python3-scipy</depend>
+  <depend>python-crccheck-pip</depend>
+  <depend>astra_msgs</depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/core_pkg/setup.py

@@ -1,27 +1,26 @@
 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

@@ -0,0 +1,23 @@
+<?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

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

src/headless_pkg/setup.py

@@ -0,0 +1,24 @@
+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

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

src/latency_tester/CMakeLists.txt

@@ -0,0 +1,27 @@
+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

@@ -0,0 +1,20 @@
+<?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

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

src/servo_arm_twist_pkg/CMakeLists.txt

@@ -0,0 +1,80 @@
+cmake_minimum_required(VERSION 3.22)
+project(servo_arm_twist_pkg)
+
+# C++ Libraries #################################################
+
+# Core C++ library for calculations and collision checking.
+# Provides interface used by the component node.
+set(SERVO_LIB_NAME servo_arm_twist_lib)
+
+# Pose Tracking
+set(POSE_TRACKING pose_tracking)
+
+# Component Nodes (Shared libraries) ############################
+set(SERVO_COMPONENT_NODE servo_node)
+set(SERVO_CONTROLLER_INPUT servo_controller_input)
+
+# Executable Nodes ##############################################
+set(SERVO_NODE_MAIN_NAME servo_node_main)
+set(POSE_TRACKING_DEMO_NAME servo_pose_tracking_demo)
+set(FAKE_SERVO_CMDS_NAME fake_command_publisher)
+
+#################################################################
+
+# Common cmake code applied to all moveit packages
+find_package(moveit_common REQUIRED)
+moveit_package()
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  control_msgs
+  control_toolbox
+  geometry_msgs
+  moveit_core
+  moveit_msgs
+  moveit_ros_planning
+  pluginlib
+  rclcpp
+  rclcpp_components
+  sensor_msgs
+  std_msgs
+  std_srvs
+  tf2_eigen
+  trajectory_msgs
+)
+
+find_package(ament_cmake REQUIRED)
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3 REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+#####################
+## Component Nodes ##
+#####################
+
+# Add executable for using a controller
+add_library(${SERVO_CONTROLLER_INPUT} SHARED src/joystick_twist.cpp)
+ament_target_dependencies(${SERVO_CONTROLLER_INPUT} ${THIS_PACKAGE_INCLUDE_DEPENDS})
+rclcpp_components_register_nodes(${SERVO_CONTROLLER_INPUT} "servo_arm_twist_pkg::JoyToServoPub")
+
+#############
+## Install ##
+#############
+
+# Install Libraries
+install(
+  TARGETS
+    ${SERVO_CONTROLLER_INPUT}
+  EXPORT export_${PROJECT_NAME}
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include
+)
+
+# Install Binaries
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+ament_package()

src/servo_arm_twist_pkg/README.md

@@ -0,0 +1,3 @@
+# Moveit Servo
+
+See the [Realtime Arm Servoing Tutorial](https://moveit.picknik.ai/main/doc/realtime_servo/realtime_servo_tutorial.html) for installation instructions, quick-start guide, an overview about `moveit_servo`, and to learn how to set it up on your robot.

src/servo_arm_twist_pkg/package.xml

@@ -0,0 +1,58 @@
+<?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>servo_arm_twist_pkg</name>
+  <version>2.5.9</version>
+  <description>Provides real-time manipulator Cartesian and joint servoing.</description>
+  <maintainer email="blakeanderson@utexas.edu">Blake Anderson</maintainer>
+  <maintainer email="andyz@utexas.edu">Andy Zelenak</maintainer>
+  <maintainer email="tyler@picknik.ai">Tyler Weaver</maintainer>
+  <maintainer email="henningkayser@picknik.ai">Henning Kayser</maintainer>
+
+  <license>BSD 3-Clause</license>
+
+  <url type="website">https://ros-planning.github.io/moveit_tutorials</url>
+
+  <author>Brian O'Neil</author>
+  <author email="andyz@utexas.edu">Andy Zelenak</author>
+  <author>Blake Anderson</author>
+  <author email="alex@machinekoder.com">Alexander Rössler</author>
+  <author email="tyler@picknik.ai">Tyler Weaver</author>
+  <author email="adam.pettinger@utexas.edu">Adam Pettinger</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>moveit_common</depend>
+
+  <depend>control_msgs</depend>
+  <depend>control_toolbox</depend>
+  <depend>geometry_msgs</depend>
+  <depend>moveit_msgs</depend>
+  <depend>moveit_core</depend>
+  <depend>moveit_ros_planning_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>tf2_eigen</depend>
+  <depend>trajectory_msgs</depend>
+
+  <exec_depend>gripper_controllers</exec_depend>
+  <exec_depend>joint_state_broadcaster</exec_depend>
+  <exec_depend>joint_trajectory_controller</exec_depend>
+  <exec_depend>joy</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>moveit_configs_utils</exec_depend>
+  <exec_depend>launch_param_builder</exec_depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>controller_manager</test_depend>
+  <test_depend>ros_testing</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+</package>

src/servo_arm_twist_pkg/src/joystick_twist.cpp

@@ -0,0 +1,271 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2020, PickNik Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of PickNik Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/*      Title     : joystick_servo_example.cpp
+ *      Project   : servo_arm_twist_pkg
+ *      Created   : 08/07/2020
+ *      Author    : Adam Pettinger
+ */
+
+#include <sensor_msgs/msg/joy.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <control_msgs/msg/joint_jog.hpp>
+#include <std_srvs/srv/trigger.hpp>
+#include <moveit_msgs/msg/planning_scene.hpp>
+#include <rclcpp/client.hpp>
+#include <rclcpp/experimental/buffers/intra_process_buffer.hpp>
+#include <rclcpp/node.hpp>
+#include <rclcpp/publisher.hpp>
+#include <rclcpp/qos.hpp>
+#include <rclcpp/qos_event.hpp>
+#include <rclcpp/subscription.hpp>
+#include <rclcpp/time.hpp>
+#include <rclcpp/utilities.hpp>
+#include <thread>
+
+// We'll just set up parameters here
+const std::string JOY_TOPIC = "/joy";
+const std::string TWIST_TOPIC = "/servo_node/delta_twist_cmds";
+const std::string JOINT_TOPIC = "/servo_node/delta_joint_cmds";
+const std::string EEF_FRAME_ID = "End_Effector";
+const std::string BASE_FRAME_ID = "base_link";
+
+// Enums for button names -> axis/button array index
+// For XBOX 1 controller
+enum Axis
+{
+    LEFT_STICK_X = 0,
+    LEFT_STICK_Y = 1,
+    LEFT_TRIGGER = 2,
+    RIGHT_STICK_X = 3,
+    RIGHT_STICK_Y = 4,
+    RIGHT_TRIGGER = 5,
+    D_PAD_X = 6,
+    D_PAD_Y = 7
+};
+enum Button
+{
+    A = 0,
+    B = 1,
+    X = 2,
+    Y = 3,
+    LEFT_BUMPER = 4,
+    RIGHT_BUMPER = 5,
+    CHANGE_VIEW = 6,
+    MENU = 7,
+    HOME = 8,
+    LEFT_STICK_CLICK = 9,
+    RIGHT_STICK_CLICK = 10
+};
+
+// Some axes have offsets (e.g. the default trigger position is 1.0 not 0)
+// This will map the default values for the axes
+std::map<Axis, double> AXIS_DEFAULTS = { { LEFT_TRIGGER, 1.0 }, { RIGHT_TRIGGER, 1.0 } };
+std::map<Button, double> BUTTON_DEFAULTS;
+
+// To change controls or setup a new controller, all you should to do is change the above enums and the follow 2
+// functions
+/** \brief // This converts a joystick axes and buttons array to a TwistStamped or JointJog message
+ * @param axes The vector of continuous controller joystick axes
+ * @param buttons The vector of discrete controller button values
+ * @param twist A TwistStamped message to update in prep for publishing
+ * @param joint A JointJog message to update in prep for publishing
+ * @return return true if you want to publish a Twist, false if you want to publish a JointJog
+ */
+bool convertJoyToCmd(const std::vector<float>& axes, const std::vector<int>& buttons,
+                     std::unique_ptr<geometry_msgs::msg::TwistStamped>& twist,
+                     std::unique_ptr<control_msgs::msg::JointJog>& joint)
+{
+    // // Give joint jogging priority because it is only buttons
+    // // If any joint jog command is requested, we are only publishing joint commands
+    // if (buttons[A] || buttons[B] || buttons[X] || buttons[Y] || axes[D_PAD_X] || axes[D_PAD_Y])
+    // {
+    //   // Map the D_PAD to the proximal joints
+    //   joint->joint_names.push_back("panda_joint1");
+    //   joint->velocities.push_back(axes[D_PAD_X]);
+    //   joint->joint_names.push_back("panda_joint2");
+    //   joint->velocities.push_back(axes[D_PAD_Y]);
+
+    //   // Map the diamond to the distal joints
+    //   joint->joint_names.push_back("panda_joint7");
+    //   joint->velocities.push_back(buttons[B] - buttons[X]);
+    //   joint->joint_names.push_back("panda_joint6");
+    //   joint->velocities.push_back(buttons[Y] - buttons[A]);
+    //   return false;
+    // }
+
+    // The bread and butter: map buttons to twist commands
+    twist->twist.linear.z = axes[RIGHT_STICK_Y];
+    twist->twist.linear.y = axes[RIGHT_STICK_X];
+
+    double lin_x_right = -0.5 * (axes[RIGHT_TRIGGER] - AXIS_DEFAULTS.at(RIGHT_TRIGGER));
+    double lin_x_left = 0.5 * (axes[LEFT_TRIGGER] - AXIS_DEFAULTS.at(LEFT_TRIGGER));
+    twist->twist.linear.x = lin_x_right + lin_x_left;
+
+    twist->twist.angular.y = axes[LEFT_STICK_Y];
+    twist->twist.angular.x = axes[LEFT_STICK_X];
+
+    double roll_positive = buttons[RIGHT_BUMPER];
+    double roll_negative = -1 * (buttons[LEFT_BUMPER]);
+    twist->twist.angular.z = roll_positive + roll_negative;
+
+    return true;
+}
+
+/** \brief // This should update the frame_to_publish_ as needed for changing command frame via controller
+ * @param frame_name Set the command frame to this
+ * @param buttons The vector of discrete controller button values
+ */
+void updateCmdFrame(std::string& frame_name, const std::vector<int>& buttons)
+{
+    if (buttons[CHANGE_VIEW] && frame_name == EEF_FRAME_ID)
+        frame_name = BASE_FRAME_ID;
+    else if (buttons[MENU] && frame_name == BASE_FRAME_ID)
+        frame_name = EEF_FRAME_ID;
+}
+
+namespace servo_arm_twist_pkg
+{
+class JoyToServoPub : public rclcpp::Node
+{
+public:
+    JoyToServoPub(const rclcpp::NodeOptions& options)
+        : Node("joy_to_twist_publisher", options), frame_to_publish_(BASE_FRAME_ID)
+    {
+        // Setup pub/sub
+        joy_sub_ = this->create_subscription<sensor_msgs::msg::Joy>(
+            JOY_TOPIC, rclcpp::SystemDefaultsQoS(),
+            [this](const sensor_msgs::msg::Joy::ConstSharedPtr& msg) { return joyCB(msg); });
+
+        twist_pub_ = this->create_publisher<geometry_msgs::msg::TwistStamped>(TWIST_TOPIC, rclcpp::SystemDefaultsQoS());
+        joint_pub_ = this->create_publisher<control_msgs::msg::JointJog>(JOINT_TOPIC, rclcpp::SystemDefaultsQoS());
+        // collision_pub_ =
+        //     this->create_publisher<moveit_msgs::msg::PlanningScene>("/planning_scene", rclcpp::SystemDefaultsQoS());
+
+        // Create a service client to start the ServoNode
+        servo_start_client_ = this->create_client<std_srvs::srv::Trigger>("/servo_node/start_servo");
+        servo_start_client_->wait_for_service(std::chrono::seconds(1));
+        servo_start_client_->async_send_request(std::make_shared<std_srvs::srv::Trigger::Request>());
+
+        // // Load the collision scene asynchronously
+        // collision_pub_thread_ = std::thread([this]() {
+        //   rclcpp::sleep_for(std::chrono::seconds(3));
+        //   // Create collision object, in the way of servoing
+        //   moveit_msgs::msg::CollisionObject collision_object;
+        //   collision_object.header.frame_id = "panda_link0";
+        //   collision_object.id = "box";
+
+        //   shape_msgs::msg::SolidPrimitive table_1;
+        //   table_1.type = table_1.BOX;
+        //   table_1.dimensions = { 0.4, 0.6, 0.03 };
+
+        //   geometry_msgs::msg::Pose table_1_pose;
+        //   table_1_pose.position.x = 0.6;
+        //   table_1_pose.position.y = 0.0;
+        //   table_1_pose.position.z = 0.4;
+
+        //   shape_msgs::msg::SolidPrimitive table_2;
+        //   table_2.type = table_2.BOX;
+        //   table_2.dimensions = { 0.6, 0.4, 0.03 };
+
+        //   geometry_msgs::msg::Pose table_2_pose;
+        //   table_2_pose.position.x = 0.0;
+        //   table_2_pose.position.y = 0.5;
+        //   table_2_pose.position.z = 0.25;
+
+        //   collision_object.primitives.push_back(table_1);
+        //   collision_object.primitive_poses.push_back(table_1_pose);
+        //   collision_object.primitives.push_back(table_2);
+        //   collision_object.primitive_poses.push_back(table_2_pose);
+        //   collision_object.operation = collision_object.ADD;
+
+        //   moveit_msgs::msg::PlanningSceneWorld psw;
+        //   psw.collision_objects.push_back(collision_object);
+
+        //   auto ps = std::make_unique<moveit_msgs::msg::PlanningScene>();
+        //   ps->world = psw;
+        //   ps->is_diff = true;
+        //   collision_pub_->publish(std::move(ps));
+        // });
+    }
+
+    // ~JoyToServoPub() override
+    // {
+    //     if (collision_pub_thread_.joinable())
+    //         collision_pub_thread_.join();
+    // }
+
+    void joyCB(const sensor_msgs::msg::Joy::ConstSharedPtr& msg)
+    {
+        // Create the messages we might publish
+        auto twist_msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
+        auto joint_msg = std::make_unique<control_msgs::msg::JointJog>();
+
+        // This call updates the frame for twist commands
+        updateCmdFrame(frame_to_publish_, msg->buttons);
+
+        // Convert the joystick message to Twist or JointJog and publish
+        if (convertJoyToCmd(msg->axes, msg->buttons, twist_msg, joint_msg))
+        {
+            // publish the TwistStamped
+            twist_msg->header.frame_id = frame_to_publish_;
+            twist_msg->header.stamp = this->now();
+            twist_pub_->publish(std::move(twist_msg));
+        }
+        // else
+        // {
+        //     // publish the JointJog
+        //     joint_msg->header.stamp = this->now();
+        //     joint_msg->header.frame_id = "panda_link3";
+        //     joint_pub_->publish(std::move(joint_msg));
+        // }
+    }
+
+private:
+    rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub_;
+    rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;
+    rclcpp::Publisher<control_msgs::msg::JointJog>::SharedPtr joint_pub_;
+    rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr collision_pub_;
+    rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr servo_start_client_;
+
+    std::string frame_to_publish_;
+
+    // std::thread collision_pub_thread_;
+};  // class JoyToServoPub
+
+}  // namespace servo_arm_twist_pkg
+
+// Register the component with class_loader
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(servo_arm_twist_pkg::JoyToServoPub)

systemd/anchor.service

@@ -0,0 +1,14 @@
+[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

@@ -0,0 +1,14 @@
+[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

@@ -0,0 +1,14 @@
+[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

@@ -0,0 +1,14 @@
+[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