fix: I hate python (#21 )

BioControl.drill expects an int, have to explicitly cast to an int or headless will crash Makes main branch functional
Merge pull request #18 from SHC-ASTRA/ros2-control
2026-02-11 17:30:36 +00:00 · 2025-10-25 11:10:07 -05:00 · 2025-10-23 03:33:16 -05:00 · 2025-10-23 02:22:31 -05:00 · 2025-10-23 01:00:13 -05:00 · 2025-10-23 00:52:19 -05:00
35 changed files with 2179 additions and 359 deletions
--- a/.envrc
+++ b/.envrc
@@ -0,0 +1 @@
 use flake
--- a/.gitignore
+++ b/.gitignore
@@ -10,4 +10,7 @@ log/
 .vscode/
 #Pycache folder
-__pycache__/
+__pycache__/
 #Direnv
 .direnv/
--- a/.gitmodules
+++ b/.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
+	url = ../ros2_interfaces_pkg
 [submodule "src/astra_description"]
 	path = src/astra_descriptions
 	url = ../astra_descriptions
--- a/661
+++ b/661
@@ -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/>.
--- a/README.md
+++ b/README.md
@@ -1,5 +1,7 @@
 # rover-ros2
 [![License: AGPL v3](https://img.shields.io/badge/License-AGPL_v3-blue.svg)](https://www.gnu.org/licenses/agpl-3.0)
 Submodule which includes all ros2 packages for the rover. These are centrally located for modular rover operation.
 You will use this package to launch any module-side ROS2 nodes.
--- a/auto_start/auto_start_anchor.sh
+++ b/auto_start/auto_start_anchor.sh
@@ -0,0 +1,24 @@
 #!/bin/bash
 # Wait for a network interface to be up (not necessarily online)
 while ! ip link show | grep -q "state UP"; do
    echo "[INFO] Waiting for active network interface..."
    sleep 1
 done
 echo "[INFO] Network interface is up!"
 # Your actual ROS node start command goes here
 echo "[INFO] Starting ROS node..."
 # Source ROS 2 Humble setup script
 source /opt/ros/humble/setup.bash
 # Source your workspace setup script
 source /home/clucky/rover-ros2/install/setup.bash
 # CD to directory
 cd /home/clucky/rover-ros2/
 # Launch the ROS 2 node with the desired mode
 ros2 launch anchor_pkg rover.launch.py mode:=anchor
--- a/auto_start/auto_start_core_headless.sh
+++ b/auto_start/auto_start_core_headless.sh
@@ -0,0 +1,24 @@
 #!/bin/bash
 # Wait for a network interface to be up (not necessarily online)
 while ! ip link show | grep -q "state UP"; do
    echo "[INFO] Waiting for active network interface..."
    sleep 1
 done
 echo "[INFO] Network interface is up!"
 # Your actual ROS node start command goes here
 echo "[INFO] Starting ROS node..."
 # Source ROS 2 Humble setup script
 source /opt/ros/humble/setup.bash
 # Source your workspace setup script
 source /home/clucky/rover-ros2/install/setup.bash
 # CD to directory
 cd /home/clucky/rover-ros2/
 # Launch the ROS 2 node
 ros2 run core_pkg headless
--- a/auto_start/auto_start_headless_full.sh
+++ b/auto_start/auto_start_headless_full.sh
@@ -0,0 +1,25 @@
 #!/bin/bash
 # Wait for a network interface to be up (not necessarily online)
 while ! ip link show | grep -q "state UP"; do
    echo "[INFO] Waiting for active network interface..."
    sleep 1
 done
 echo "[INFO] Network interface is up!"
 # Your actual ROS node start command goes here
 echo "[INFO] Starting ROS node..."
 # Source ROS 2 Humble setup script
 source /opt/ros/humble/setup.bash
 # Source your workspace setup script
 source /home/clucky/rover-ros2/install/setup.bash
 # CD to directory
 cd /home/clucky/rover-ros2/
 # Launch the ROS 2 node
 ros2 run headless_pkg headless_full
--- a/auto_start/start_rosbag.sh
+++ b/auto_start/start_rosbag.sh
@@ -0,0 +1,22 @@
 #!/bin/bash
 ANCHOR_WS="/home/clucky/rover-ros2"
 AUTONOMY_WS="/home/clucky/rover-Autonomy"
 BAG_LOCATION="/home/clucky/bags/autostart"
 # Wait for a network interface to be up (not necessarily online)
 while ! ip link show | grep -q "state UP"; do
    echo "[INFO] Waiting for active network interface..."
    sleep 1
 done
 echo "[INFO] Network interface is up!"
 source /opt/ros/humble/setup.bash
 source $ANCHOR_WS/install/setup.bash
 [ -f $AUTONOMY_WS/install/setup.bash ] && source $AUTONOMY_WS/install/setup.bash
 cd $BAG_LOCATION
 ros2 bag record -a
--- a/flake.lock
+++ b/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": 1758094726,
        "narHash": "sha256-agLnClczRtYY+kQFh5dv4wGNhtFNKK7KFOmypDhsWCs=",
        "owner": "lopsided98",
        "repo": "nix-ros-overlay",
        "rev": "9d0557032aadb65df065b1972a632572b57234b5",
        "type": "github"
      },
      "original": {
        "owner": "lopsided98",
        "ref": "master",
        "repo": "nix-ros-overlay",
        "type": "github"
      }
    },
    "nixpkgs": {
      "locked": {
        "lastModified": 1744849697,
        "narHash": "sha256-S9hqvanPSeRu6R4cw0OhvH1rJ+4/s9xIban9C4ocM/0=",
        "owner": "lopsided98",
        "repo": "nixpkgs",
        "rev": "6318f538166fef9f5118d8d78b9b43a04bb049e4",
        "type": "github"
      },
      "original": {
        "owner": "lopsided98",
        "ref": "nix-ros",
        "repo": "nixpkgs",
        "type": "github"
      }
    },
    "root": {
      "inputs": {
        "nix-ros-overlay": "nix-ros-overlay",
        "nixpkgs": [
          "nix-ros-overlay",
          "nixpkgs"
        ]
      }
    },
    "systems": {
      "locked": {
        "lastModified": 1681028828,
        "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=",
        "owner": "nix-systems",
        "repo": "default",
        "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e",
        "type": "github"
      },
      "original": {
        "owner": "nix-systems",
        "repo": "default",
        "type": "github"
      }
    }
  },
  "root": "root",
  "version": 7
 }
--- a/flake.nix
+++ b/flake.nix
@@ -0,0 +1,74 @@
 {
  description = "Development environment for ASTRA Anchor";
  inputs = {
    nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/master";
    nixpkgs.follows = "nix-ros-overlay/nixpkgs"; # IMPORTANT!!!
  };
  outputs =
    {
      self,
      nix-ros-overlay,
      nixpkgs,
    }:
    nix-ros-overlay.inputs.flake-utils.lib.eachDefaultSystem (
      system:
      let
        pkgs = import nixpkgs {
          inherit system;
          overlays = [ nix-ros-overlay.overlays.default ];
        };
      in
      {
        devShells.default = pkgs.mkShell {
          name = "ASTRA Anchor";
          packages = with pkgs; [
            colcon
            (python312.withPackages (
              p: with p; [
                pyserial
                pygame
                scipy
                crccheck
                black
              ]
            ))
            (
              with rosPackages.humble;
              buildEnv {
                paths = [
                  ros-core
                  ros2cli
                  ros2run
                  ros2bag
                  rviz2
                  xacro
                  ament-cmake-core
                  python-cmake-module
                  diff-drive-controller
                  parameter-traits
                  generate-parameter-library
                  joint-state-publisher-gui
                  robot-state-publisher
                  ros2-control
                  controller-manager
                  # ros2-controllers nixpkg does not build :(
                ];
              }
            )
          ];
          shellHook = ''
            # Display stuff
            export DISPLAY=''${DISPLAY:-:0}
            export QT_X11_NO_MITSHM=1
          '';
        };
      }
    );
  nixConfig = {
    extra-substituters = [ "https://ros.cachix.org" ];
    extra-trusted-public-keys = [ "ros.cachix.org-1:dSyZxI8geDCJrwgvCOHDoAfOm5sV1wCPjBkKL+38Rvo=" ];
  };
 }
--- a/src/anchor_pkg/anchor_pkg/anchor_node.py
+++ b/src/anchor_pkg/anchor_pkg/anchor_node.py
@@ -12,18 +12,81 @@ import sys
 import threading
 import glob
-from std_msgs.msg import String
+from std_msgs.msg import String, Header
-from ros2_interfaces_pkg.msg import CoreFeedback
+from ros2_interfaces_pkg.msg import VicCAN
 from ros2_interfaces_pkg.msg import CoreControl
 serial_pub = None
 thread = None
 """
 Publishers:
  * /anchor/from_vic/debug
    - Every string received from the MCU is published here for debugging
  * /anchor/from_vic/core
    - VicCAN messages for Core node
  * /anchor/from_vic/arm
    - VicCAN messages for Arm node
  * /anchor/from_vic/bio
    - VicCAN messages for Bio node
 Subscribers:
  * /anchor/from_vic/mock_mcu
    - For testing without an actual MCU, publish strings here as if they came from an MCU
  * /anchor/to_vic/relay
    - Core, Arm, and Bio publish VicCAN messages to this topic to send to the MCU
  * /anchor/to_vic/relay_string
    - Publish raw strings to this topic to send directly to the MCU for debugging
 """
 class SerialRelay(Node):
    def __init__(self):
        # Initalize node with name
        super().__init__("anchor_node")#previously 'serial_publisher'
        # Loop through all serial devices on the computer to check for the MCU
        self.port = None
        if port_override := os.getenv("PORT_OVERRIDE"):
            self.port = port_override
        ports = SerialRelay.list_serial_ports()
        for i in range(4):
            if self.port is not None:
                break
            for port in ports:
                try:
                    # connect and send a ping command
                    ser = serial.Serial(port, 115200, timeout=1)
                    #(f"Checking port {port}...")
                    ser.write(b"ping\n")
                    response = ser.read_until(bytes("\n", "utf8"))
                    # if pong is in response, then we are talking with the MCU
                    if b"pong" in response:
                        self.port = port
                        self.get_logger().info(f"Found MCU at {self.port}!")
                        break
                except:
                    pass
        if self.port is None:
            self.get_logger().info("Unable to find MCU...")
            time.sleep(1)
            sys.exit(1)
        self.ser = serial.Serial(self.port, 115200)
        self.get_logger().info(f"Enabling Relay Mode")
        self.ser.write(b"can_relay_mode,on\n")
        atexit.register(self.cleanup)
        # New pub/sub with VicCAN
        self.fromvic_debug_pub_ = self.create_publisher(String, '/anchor/from_vic/debug', 20)
        self.fromvic_core_pub_ = self.create_publisher(VicCAN, '/anchor/from_vic/core', 20)
        self.fromvic_arm_pub_ = self.create_publisher(VicCAN, '/anchor/from_vic/arm', 20)
        self.fromvic_bio_pub_ = self.create_publisher(VicCAN, '/anchor/from_vic/bio', 20)
        self.mock_mcu_sub_ = self.create_subscription(String, '/anchor/from_vic/mock_mcu', self.on_mock_fromvic, 20)
        self.tovic_sub_ = self.create_subscription(VicCAN, '/anchor/to_vic/relay', self.on_relay_tovic_viccan, 20)
        self.tovic_debug_sub_ = self.create_subscription(String, '/anchor/to_vic/relay_string', self.on_relay_tovic_string, 20)
        # Create publishers 
        self.arm_pub = self.create_publisher(String, '/anchor/arm/feedback', 10)
@@ -31,42 +94,9 @@ class SerialRelay(Node):
        self.bio_pub = self.create_publisher(String, '/anchor/bio/feedback', 10)
        self.debug_pub = self.create_publisher(String, '/anchor/debug', 10)
-        
+
        # Create a subscriber 
-        self.relay_sub = self.create_subscription(String, '/anchor/relay', self.send_cmd, 10)
+        self.relay_sub = self.create_subscription(String, '/anchor/relay', self.on_relay_tovic_string, 10)
        # Loop through all serial devices on the computer to check for the MCU
        self.port = None
        ports = SerialRelay.list_serial_ports()
        for i in range(4):
            for port in ports:
                try:
                    # connect and send a ping command
                    ser = serial.Serial(port, 115200, timeout=1)
                    #(f"Checking port {port}...")
                    ser.write(b"ping\n")
                    response = ser.read_until("\n")
                    ser.write(b"can_relay_mode,on\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):
@@ -82,10 +112,12 @@ class SerialRelay(Node):
            sys.exit(0)
    def read_MCU(self):
        """ Check the USB serial port for new data from the MCU, and publish string to appropriate topics """
        try:
            output = str(self.ser.readline(), "utf8")
            if output:
                self.relay_fromvic(output)
                # All output over debug temporarily
                #self.get_logger().info(f"[MCU] {output}")
                msg = String()
@@ -120,7 +152,75 @@ class SerialRelay(Node):
            #     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.ser.write(bytes(output, "utf8"))
    def relay_fromvic(self, msg: str):
        """ Relay a string message from the MCU to the appropriate VicCAN topic """
        self.fromvic_debug_pub_.publish(String(data=msg))
        parts = msg.strip().split(",")
        if len(parts) > 0 and parts[0] != "can_relay_fromvic":
            self.get_logger().debug(f"Ignoring non-VicCAN message: '{msg.strip()}'")
            return
        # String validation
        malformed: bool = False
        malformed_reason: str = ""
        if len(parts) < 3 or len(parts) > 7:
            malformed = True
            malformed_reason = f"invalid argument count (expected [3,7], got {len(parts)})"
        elif parts[1] not in ["core", "arm", "digit", "citadel", "broadcast"]:
            malformed = True
            malformed_reason = f"invalid mcu_name '{parts[1]}'"
        elif not(parts[2].isnumeric()) or int(parts[2]) < 0:
            malformed = True
            malformed_reason = f"command_id '{parts[2]}' is not a non-negative integer"
        else:
            for x in parts[3:]:
                try:
                    float(x)
                except ValueError:
                    malformed = True
                    malformed_reason = f"data '{x}' is not a float"
                    break
        if malformed:
            self.get_logger().warning(f"Ignoring malformed from_vic message: '{msg.strip()}'; reason: {malformed_reason}")
            return
        # Have valid VicCAN message
        output = VicCAN()
        output.mcu_name = parts[1]
        output.command_id = int(parts[2])
        if len(parts) > 3:
            output.data = [float(x) for x in parts[3:]]
        output.header = Header(stamp=self.get_clock().now().to_msg(), frame_id="from_vic")
        # self.get_logger().info(f"Relaying from MCU: {output}")
        if output.mcu_name == "core":
            self.fromvic_core_pub_.publish(output)
        elif output.mcu_name == "arm" or output.mcu_name == "digit":
            self.fromvic_arm_pub_.publish(output)
        elif output.mcu_name == "citadel" or output.mcu_name == "digit":
            self.fromvic_bio_pub_.publish(output)
    def on_relay_tovic_string(self, msg: String):
        """ Relay a raw string message to the MCU for debugging """
        message = msg.data
        #self.get_logger().info(f"Sending command to MCU: {msg}")
        self.ser.write(bytes(message, "utf8"))
@@ -128,7 +228,7 @@ class SerialRelay(Node):
    @staticmethod
    def list_serial_ports():
        return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
-    
+
    def cleanup(self):
        print("Cleaning up before terminating...")
        if self.ser.is_open:
--- a/src/anchor_pkg/launch/rover.launch.py
+++ b/src/anchor_pkg/launch/rover.launch.py
--- a/src/anchor_pkg/package.xml
+++ b/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>
--- a/src/anchor_pkg/setup.py
+++ b/src/anchor_pkg/setup.py
@@ -1,4 +1,6 @@
 from setuptools import find_packages, setup
 from os import path
 from glob import glob
 package_name = 'anchor_pkg'
@@ -9,7 +11,8 @@ setup(
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
+        (path.join("share", package_name), ['package.xml']),
        (path.join("share", package_name, "launch"), glob("launch/*"))
    ],
    install_requires=['setuptools'],
    zip_safe=True,
--- a/src/arm_pkg/arm_pkg/arm_node.py
+++ b/src/arm_pkg/arm_pkg/arm_node.py
@@ -237,15 +237,13 @@ class SerialRelay(Node):
        command += "can_relay_tovic,arm,39," + str(axis0) + "," + str(axis1) + "," + str(axis2) + "," + str(axis3) + "\n"
        #Send controls for end effector
        command += "can_relay_tovic,digit,35," + str(msg.effector_roll) + "\n"
        command += "can_relay_tovic,digit,36,0," + str(msg.effector_yaw) + "\n"
        # command += "can_relay_tovic,digit,35," + str(msg.effector_roll) + "\n"
        # command += "can_relay_tovic,digit,36,0," + str(msg.effector_yaw) + "\n"
        command += "can_relay_tovic,digit,39," + str(msg.effector_yaw) + "," + str(msg.effector_roll) + "\n"
        command += "can_relay_tovic,digit,26," + str(msg.gripper) + "\n"
        command += "can_relay_tovic,digit,28," + str(msg.laser) + "\n"
        command += "can_relay_tovic,digit,34," + str(msg.linear_actuator) + "\n"
@@ -260,7 +258,7 @@ class SerialRelay(Node):
            output.data = msg
            self.anchor_pub.publish(output)
        elif self.launch_mode == 'arm': #if in standalone mode, send to MCU directly
-            self.get_logger().info(f"[Arm to MCU] {msg.data}")
+            self.get_logger().info(f"[Arm to MCU] {msg}")
            self.ser.write(bytes(msg, "utf8"))
    def anchor_feedback(self, msg: String):
@@ -286,6 +284,7 @@ class SerialRelay(Node):
            parts = msg.data.split(",")
            if len(parts) >= 4:
                self.digit_feedback.wrist_angle = float(parts[3])
                # self.digit_feedback.wrist_roll = float(parts[4])
        else:
            return
--- a/src/arm_pkg/package.xml
+++ b/src/arm_pkg/package.xml
@@ -5,10 +5,14 @@
  <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>common_interfaces</depend>
  <depend>python3-numpy</depend>
  <depend>ros2_interfaces_pkg</depend>
  <!-- TODO: remove after refactored out -->
  <exec_depend>python3-ikpy-pip</exec_depend>
  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
--- a/src/astra_descriptions
+++ b/src/astra_descriptions
--- a/src/bio_pkg/package.xml
+++ b/src/bio_pkg/package.xml
@@ -5,9 +5,10 @@
  <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>ros2_interfaces_pkg</depend>
  <test_depend>ament_copyright</test_depend>
--- a/src/core_pkg/core_pkg/core_node.py
+++ b/src/core_pkg/core_pkg/core_node.py
@@ -1,305 +1,529 @@
-import rclpy
+import rclpy
-from rclpy.node import Node
+from rclpy.node import Node
-from rclpy import qos
+from rclpy import qos
-from std_srvs.srv import Empty
+from rclpy.duration import Duration
-
+from std_srvs.srv import Empty
-import signal
+
-import time
+import signal
-import atexit
+import time
-
+import atexit
-import serial
+
-import os
+import serial
-import sys
+import os
-import threading
+import sys
-import glob
+import threading
-
+import glob
-from std_msgs.msg import String
+from scipy.spatial.transform import Rotation
-from ros2_interfaces_pkg.msg import CoreFeedback
+from math import copysign, pi
-from ros2_interfaces_pkg.msg import CoreControl
+
-
+from std_msgs.msg import String, Header
-serial_pub = None
+from sensor_msgs.msg import Imu, NavSatFix, NavSatStatus, JointState
-thread = None
+from geometry_msgs.msg import TwistStamped, Twist
-
+from ros2_interfaces_pkg.msg import CoreControl, CoreFeedback, RevMotorState
-# control_qos = qos.QoSProfile(
+from ros2_interfaces_pkg.msg import VicCAN, NewCoreFeedback, Barometer, CoreCtrlState
-#     history=qos.QoSHistoryPolicy.KEEP_LAST,
+
-#     depth=1,
+
-#     reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
+serial_pub = None
-#     durability=qos.QoSDurabilityPolicy.VOLATILE,
+thread = None
-#     deadline=1000,
+
-#     lifespan=500,
+CORE_WHEELBASE = 0.836  # meters
-#     liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
+CORE_WHEEL_RADIUS = 0.171  # meters
-#     liveliness_lease_duration=5000
+CORE_GEAR_RATIO = 100.0  # Clucky: 100:1, Testbed: 64:1
-# )
+
-
+control_qos = qos.QoSProfile(
-class SerialRelay(Node):
+    history=qos.QoSHistoryPolicy.KEEP_LAST,
-    def __init__(self):
+    depth=2,
-        # Initalize node with name
+    reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
-        super().__init__("core_node")#previously 'serial_publisher'
+    durability=qos.QoSDurabilityPolicy.VOLATILE,
-
+    deadline=Duration(seconds=1),
-        # Get launch mode parameter
+    lifespan=Duration(nanoseconds=500_000_000),  # 500ms
-        self.declare_parameter('launch_mode', 'core')
+    liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
-        self.launch_mode = self.get_parameter('launch_mode').value
+    liveliness_lease_duration=Duration(seconds=5)
-        self.get_logger().info(f"core launch_mode is: {self.launch_mode}")
+)
-
+
-        # Create publishers 
+# Used to verify the length of an incoming VicCAN feedback message
-        self.debug_pub = self.create_publisher(String, '/core/debug', 10) 
+# Key is VicCAN command_id, value is expected length of data list
-        self.feedback_pub = self.create_publisher(CoreFeedback, '/core/feedback', 10)
+viccan_msg_len_dict = {
-        # Create a subscriber 
+    48: 1,
-        self.control_sub = self.create_subscription(CoreControl, '/core/control', self.send_controls, 10)
+    49: 1,
-        
+    50: 2,
-        # Create a publisher for telemetry
+    51: 4,
-        self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback)
+    52: 4,
-
+    53: 4,
-        # Create a service server for pinging the rover
+    54: 4,
-        self.ping_service = self.create_service(Empty, '/astra/core/ping', self.ping_callback)
+    56: 4,  # really 3, but viccan
-
+    58: 4   # ditto
-        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)
+
-
+class SerialRelay(Node):
-        self.core_feedback = CoreFeedback()
+    def __init__(self):
-
+        # Initalize node with name
-
+        super().__init__("core_node")
-        if self.launch_mode == 'core':
+
-            # Loop through all serial devices on the computer to check for the MCU
+        # Launch mode -- anchor vs core
-            self.port = None
+        self.declare_parameter('launch_mode', 'core')
-            ports = SerialRelay.list_serial_ports()
+        self.launch_mode = self.get_parameter('launch_mode').value
-            for i in range(2):
+        self.get_logger().info(f"Core launch_mode is: {self.launch_mode}")
-                for port in ports:
+
-                    try:
+
-                        # connect and send a ping command
+        ##################################################
-                        ser = serial.Serial(port, 115200, timeout=1)
+        # Topics
-                        #(f"Checking port {port}...")
+
-                        ser.write(b"ping\n")
+        # Anchor
-                        response = ser.read_until("\n")
+        if self.launch_mode == 'anchor':
-
+            self.anchor_fromvic_sub_ = self.create_subscription(VicCAN, '/anchor/from_vic/core', self.relay_fromvic, 20)
-                        # if pong is in response, then we are talking with the MCU
+            self.anchor_tovic_pub_ = self.create_publisher(VicCAN, '/anchor/to_vic/relay', 20)
-                        if b"pong" in response:
+
-                            self.port = port
+            self.anchor_sub = self.create_subscription(String, '/anchor/core/feedback', self.anchor_feedback, 10)
-                            self.get_logger().info(f"Found MCU at {self.port}!")
+            self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10)
-                            self.get_logger().info(f"Enabling Relay Mode")
+
-                            ser.write(b"can_relay_mode,on\n")
+        # Control
-                            break
+
-                    except:
+        # autonomy twist -- m/s and rad/s -- for autonomy, in particular Nav2
-                        pass
+        self.cmd_vel_sub_ = self.create_subscription(TwistStamped, '/cmd_vel', self.cmd_vel_callback, 1)
-                if self.port is not None:
+        # manual twist -- [-1, 1] rather than real units
-                    break
+        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
-            if self.port is None:
+        self.control_state_sub_ = self.create_subscription(CoreCtrlState, '/core/control/state', self.control_state_callback, qos_profile=control_qos)
-                self.get_logger().info("Unable to find MCU...")
+        self.twist_max_duty = 0.5  # max duty cycle for twist commands (0.0 - 1.0); walking speed is 0.5
-                time.sleep(1)
+
-                sys.exit(1)
+        # Feedback
-        
+
-            self.ser = serial.Serial(self.port, 115200)
+        # Consolidated and organized core feedback
-            atexit.register(self.cleanup)
+        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
-    def run(self):
+        self.feedback_new_state.bl_motor.id = 2
-        # This thread makes all the update processes run in the background
+        self.feedback_new_state.fr_motor.id = 3
-        global thread
+        self.feedback_new_state.br_motor.id = 4
-        thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
+        self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback)  # TODO: not sure about this
-        thread.start()
+        # 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)
-        try:
+        self.imu_pub_ = self.create_publisher(Imu, '/core/imu', qos_profile=qos.qos_profile_sensor_data)
-            while rclpy.ok():
+        self.imu_state = Imu()
-                if self.launch_mode == 'core':
+        self.imu_state.header.frame_id = "core_bno055"
-                    self.read_MCU() # Check the MCU for updates
+        # GPS (embedded u-blox M9N)
-        except KeyboardInterrupt:
+        self.gps_pub_ = self.create_publisher(NavSatFix, '/gps/fix', qos_profile=qos.qos_profile_sensor_data)
-            sys.exit(0)
+        self.gps_state = NavSatFix()
-
+        self.gps_state.header.frame_id = "core_gps_antenna"
-    def read_MCU(self):
+        self.gps_state.status.service = NavSatStatus.SERVICE_GPS
-        try:
+        self.gps_state.status.status = NavSatStatus.STATUS_NO_FIX
-            output = str(self.ser.readline(), "utf8")
+        self.gps_state.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
-            
+        # Barometer (embedded BMP-388)
-            if output:
+        self.baro_pub_ = self.create_publisher(Barometer, '/core/baro', qos_profile=qos.qos_profile_sensor_data)
-                # All output over debug temporarily
+        self.baro_state = Barometer()
-                print(f"[MCU] {output}")
+        self.baro_state.header.frame_id = "core_bmp388"
-                msg = String()
+
-                msg.data = output
+        # Old
-                self.debug_pub.publish(msg)
+        
-                return
+        # /core/control
-                # Temporary
+        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
-                # packet = output.strip().split(',')
+        self.feedback_pub = self.create_publisher(CoreFeedback, '/core/feedback', 10)
-                
+        self.core_feedback = CoreFeedback()
-                # if len(packet) >= 2 and packet[0] == "core" and packet[1] == "telemetry":
+        # Debug
-                #     feedback = CoreFeedback()
+        self.debug_pub = self.create_publisher(String, '/core/debug', 10)
-                #     feedback.gpslat = float(packet[2])
+        self.ping_service = self.create_service(Empty, '/astra/core/ping', self.ping_callback)
-                #     feedback.gpslon = float(packet[3])
+
-                #     feedback.gpssat = float(packet[4])
+
-                #     feedback.bnogyr.x = float(packet[5])
+        ##################################################
-                #     feedback.bnogyr.y = float(packet[6])
+        # Find microcontroller (Non-anchor only)
-                #     feedback.bnogyr.z = float(packet[7])
+
-                #     feedback.bnoacc.x = float(packet[8])
+        # Core (non-anchor) specific
-                #     feedback.bnoacc.y = float(packet[9])
+        if self.launch_mode == 'core':
-                #     feedback.bnoacc.z = float(packet[10])
+            # Loop through all serial devices on the computer to check for the MCU
-                #     feedback.orient = float(packet[11])
+            self.port = None
-                #     feedback.bmptemp = float(packet[12])
+            ports = SerialRelay.list_serial_ports()
-                #     feedback.bmppres = float(packet[13])
+            for i in range(2):
-                #     feedback.bmpalt = float(packet[14])
+                for port in ports:
-
+                    try:
-                #     self.telemetry_publisher.publish(feedback)
+                        # connect and send a ping command
-                # else:
+                        ser = serial.Serial(port, 115200, timeout=1)
-                #     # print(f"[MCU] {output}", end="")
+                        #(f"Checking port {port}...")
-                #     # msg = String()
+                        ser.write(b"ping\n")
-                #     # msg.data = output
+                        response = ser.read_until("\n")  # type: ignore
-                #     # self.debug_pub.publish(msg)
+
-                #     return
+                        # if pong is in response, then we are talking with the MCU
-        except serial.SerialException as e:
+                        if b"pong" in response:
-            print(f"SerialException: {e}")
+                            self.port = port
-            print("Closing serial port.")
+                            self.get_logger().info(f"Found MCU at {self.port}!")
-            if self.ser.is_open:
+                            self.get_logger().info(f"Enabling Relay Mode")
-                self.ser.close()
+                            ser.write(b"can_relay_mode,on\n")
-            self.exit(1)
+                            break
-        except TypeError as e:
+                    except:
-            print(f"TypeError: {e}")
+                        pass
-            print("Closing serial port.")
+                if self.port is not None:
-            if self.ser.is_open:
+                    break
-                self.ser.close()
+        
-            self.exit(1)
+            if self.port is None:
-        except Exception as e:
+                self.get_logger().info("Unable to find MCU...")
-            print(f"Exception: {e}")
+                time.sleep(1)
-            print("Closing serial port.")
+                sys.exit(1)
-            if self.ser.is_open:
+        
-                self.ser.close()
+            self.ser = serial.Serial(self.port, 115200)
-            self.exit(1)
+            atexit.register(self.cleanup)
-                
+        # end __init__()
-    def scale_duty(self, value: float, max_speed: float):
+
-        leftMin = -1
+
-        leftMax = 1
+    def run(self):
-        rightMin = -max_speed/100.0
+        # This thread makes all the update processes run in the background
-        rightMax = max_speed/100.0
+        global thread
-
+        thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
-
+        thread.start()
-        # Figure out how 'wide' each range is
+
-        leftSpan = leftMax - leftMin
+        try:
-        rightSpan = rightMax - rightMin
+            while rclpy.ok():
-
+                if self.launch_mode == 'core':
-        # Convert the left range into a 0-1 range (float)
+                    self.read_MCU() # Check the MCU for updates
-        valueScaled = float(value - leftMin) / float(leftSpan)
+        except KeyboardInterrupt:
-
+            sys.exit(0)
-        # Convert the 0-1 range into a value in the right range.
+
-        return str(rightMin + (valueScaled * rightSpan))
+    def read_MCU(self):  # NON-ANCHOR SPECIFIC
-
+        try:
-    def send_controls(self, msg: CoreControl):
+            output = str(self.ser.readline(), "utf8")
-        #can_relay_tovic,core,19, left_stick, right_stick 
+            
-        if(msg.turn_to_enable):
+            if output:
-            command = "can_relay_tovic,core,41," + str(msg.turn_to) + ',' + str(msg.turn_to_timeout) + '\n' 
+                # All output over debug temporarily
-        else:
+                print(f"[MCU] {output}")
-            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'
+                msg = String()
-        self.send_cmd(command)
+                msg.data = output
-
+                self.debug_pub.publish(msg)
-        # Brake mode
+                return
-        command = "can_relay_tovic,core,18," + str(int(msg.brake)) + '\n'
+        except serial.SerialException as e:
-        self.send_cmd(command)
+            print(f"SerialException: {e}")
-
+            print("Closing serial port.")
-        #print(f"[Sys] Relaying: {command}")
+            if self.ser.is_open:
-    def send_cmd(self, msg: str):
+                self.ser.close()
-        if self.launch_mode == 'anchor':
+            sys.exit(1)
-            #self.get_logger().info(f"[Core to Anchor Relay] {msg}")
+        except TypeError as e:
-            output = String()#Convert to std_msg string
+            print(f"TypeError: {e}")
-            output.data = msg
+            print("Closing serial port.")
-            self.anchor_pub.publish(output)
+            if self.ser.is_open:
-        elif self.launch_mode == 'core':
+                self.ser.close()
-            self.get_logger().info(f"[Core to MCU] {msg.data}")
+            sys.exit(1)
-            self.ser.write(bytes(msg, "utf8"))
+        except Exception as e:
-
+            print(f"Exception: {e}")
-    def anchor_feedback(self, msg: String):
+            print("Closing serial port.")
-        output = msg.data
+            if self.ser.is_open:
-        parts = str(output.strip()).split(",")
+                self.ser.close()
-        #self.get_logger().info(f"[ANCHOR FEEDBACK parts] {parts}")
+            sys.exit(1)
-        if output.startswith("can_relay_fromvic,core,48"): #GNSS Lattitude
+
-            self.core_feedback.gps_lat = float(parts[3])
+    def scale_duty(self, value: float, max_speed: float):
-        elif output.startswith("can_relay_fromvic,core,49"):#GNSS Longitude
+        leftMin = -1
-            self.core_feedback.gps_long = float(parts[3])
+        leftMax = 1
-        elif output.startswith("can_relay_fromvic,core,50"):#GNSS Satellite Count
+        rightMin = -max_speed/100.0
-            self.core_feedback.gps_sats = round(float(parts[3]))
+        rightMax = max_speed/100.0
-            #if parts length is at least 5 then we should have altitude, this is a temporary check until fully implemented
+
-            if len(parts) >= 5:
+        # Figure out how 'wide' each range is
-                self.core_feedback.gps_alt = round(float(parts[4]), 2)
+        leftSpan = leftMax - leftMin
-        elif output.startswith("can_relay_fromvic,core,51"):  #Gyro x,y,z
+        rightSpan = rightMax - rightMin
-            self.core_feedback.bno_gyro.x = float(parts[3])
+
-            self.core_feedback.bno_gyro.y = float(parts[4])
+        # Convert the left range into a 0-1 range (float)
-            self.core_feedback.bno_gyro.z = float(parts[5])
+        valueScaled = float(value - leftMin) / float(leftSpan)
-            self.core_feedback.imu_calib = round(float(parts[6]))
+
-        elif output.startswith("can_relay_fromvic,core,52"):  #Accel x,y,z, heading
+        # Convert the 0-1 range into a value in the right range.
-            self.core_feedback.bno_accel.x = float(parts[3])
+        return str(rightMin + (valueScaled * rightSpan))
-            self.core_feedback.bno_accel.y = float(parts[4])
+
-            self.core_feedback.bno_accel.z = float(parts[5])
+    def send_controls(self, msg: CoreControl):
-            self.core_feedback.orientation = float(parts[6])
+        if(msg.turn_to_enable):
-        elif output.startswith("can_relay_fromvic,core,53"):  #Rev motor feedback
+            command = "can_relay_tovic,core,41," + str(msg.turn_to) + ',' + str(msg.turn_to_timeout) + '\n' 
-            motorId = round(float(parts[3]))
+        else:
-            temp = float(parts[4]) / 10.0
+            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'
-            voltage = float(parts[5]) / 10.0
+        self.send_cmd(command)
-            current = float(parts[6]) / 10.0
+
-            if motorId == 1:
+        # Brake mode
-                self.core_feedback.fl_temp = temp
+        command = "can_relay_tovic,core,18," + str(int(msg.brake)) + '\n'
-                self.core_feedback.fl_voltage = voltage
+        self.send_cmd(command)
-                self.core_feedback.fl_current = current
+
-            elif motorId == 2:
+        #print(f"[Sys] Relaying: {command}")
-                self.core_feedback.bl_temp = temp
+
-                self.core_feedback.bl_voltage = voltage
+    def cmd_vel_callback(self, msg: TwistStamped):
-                self.core_feedback.bl_current = current
+        linear = msg.twist.linear.x
-            elif motorId == 3:
+        angular = -msg.twist.angular.z
-                self.core_feedback.fr_temp = temp
+
-                self.core_feedback.fr_voltage = voltage
+        vel_left_rads = (linear - (angular * CORE_WHEELBASE / 2)) / CORE_WHEEL_RADIUS
-                self.core_feedback.fr_current = current
+        vel_right_rads = (linear + (angular * CORE_WHEELBASE / 2)) / CORE_WHEEL_RADIUS
-            elif motorId == 4:
+
-                self.core_feedback.br_temp = temp
+        vel_left_rpm = round((vel_left_rads * 60) / (2 * 3.14159)) * CORE_GEAR_RATIO
-                self.core_feedback.br_voltage = voltage
+        vel_right_rpm = round((vel_right_rads * 60) / (2 * 3.14159)) * CORE_GEAR_RATIO
-                self.core_feedback.br_current = current
+        
-        elif output.startswith("can_relay_fromvic,core,54"):  #bat, 12, 5, 3, Voltage readings * 100
+        self.send_viccan(20, [vel_left_rpm, vel_right_rpm])
-            self.core_feedback.bat_voltage = float(parts[3]) / 100.0
+
-            self.core_feedback.voltage_12 = float(parts[4]) / 100.0
+    def twist_man_callback(self, msg: Twist):
-            self.core_feedback.voltage_5 = float(parts[5]) / 100.0
+        linear = msg.linear.x  # [-1 1] for forward/back from left stick y
-            self.core_feedback.voltage_3 = float(parts[6]) / 100.0
+        angular = msg.angular.z  # [-1 1] for left/right from right stick x
-        elif output.startswith("can_relay_fromvic,core,56"):  #BMP Temp, Altitude, Pressure
+
-            self.core_feedback.bmp_temp = float(parts[3])
+        if (linear < 0):  # reverse turning direction when going backwards (WIP)
-            self.core_feedback.bmp_alt = float(parts[4])
+            angular *= -1
-            self.core_feedback.bmp_pres = float(parts[5])
+
-        else:
+        if abs(linear) > 1 or abs(angular) > 1:
-            return
+            # if speed is greater than 1, then there is a problem
-        #self.get_logger().info(f"[Core Anchor] {msg}")
+            # 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)
-    def publish_feedback(self):
+            angular = copysign(0.25, angular)
-        #self.get_logger().info(f"[Core] {self.core_feedback}")
+
-        self.feedback_pub.publish(self.core_feedback)
+        duty_left = linear - angular
-
+        duty_right = linear + angular
-    def ping_callback(self, request, response):
+        scale = max(1, abs(duty_left), abs(duty_right))
-        return response
+        duty_left /= scale
-
+        duty_right /= scale
-
+
-    @staticmethod
+        # Apply max duty cycle
-    def list_serial_ports():
+        # Joysticks provide values [-1, 1] rather than real units
-        return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
+        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)
-    def cleanup(self):
+
-        print("Cleaning up before terminating...")
+        self.send_viccan(19, [duty_left, duty_right])
-        try:
+
-            if self.ser.is_open:
+    def control_state_callback(self, msg: CoreCtrlState):
-                self.ser.close()
+        # Brake mode
-        except Exception as e:
+        self.send_viccan(18, [msg.brake_mode])
-            exit(0)
+
-
+        # Max duty cycle
-def myexcepthook(type, value, tb):
+        self.twist_max_duty = msg.max_duty  # twist_man_callback will handle this
-    print("Uncaught exception:", type, value)
+
-    if serial_pub:
+    def send_cmd(self, msg: str):
-        serial_pub.cleanup()
+        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
-def main(args=None):
+            self.anchor_pub.publish(output)
-   rclpy.init(args=args)
+        elif self.launch_mode == 'core':
-   sys.excepthook = myexcepthook
+            self.get_logger().info(f"[Core to MCU] {msg}")
-
+            self.ser.write(bytes(msg, "utf8"))
-   global serial_pub
+
-
+
-   serial_pub = SerialRelay()
+    def send_viccan(self, cmd_id: int, data: list[float]):
-   serial_pub.run()
+        self.anchor_tovic_pub_.publish(VicCAN(
-
+            header=Header(stamp=self.get_clock().now().to_msg(), frame_id="to_vic"),
-if __name__ == '__main__':
+            mcu_name="core",
-    #signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0))  # Catch Ctrl+Z and exit cleanly
+            command_id=cmd_id,
-    signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0))  # Catch termination signals and exit cleanly
+            data=data
-    main()
+        ))
-
+
    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()
--- a/src/core_pkg/package.xml
+++ b/src/core_pkg/package.xml
@@ -5,9 +5,12 @@
  <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>common_interfaces</depend>
  <depend>python3-scipy</depend>
  <depend>python-crccheck-pip</depend>
  <depend>ros2_interfaces_pkg</depend>
  <test_depend>ament_copyright</test_depend>
--- a/src/headless_pkg/package.xml
+++ b/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>ros2_interfaces_pkg</depend>
  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
  <test_depend>ament_pep257</test_depend>
  <test_depend>python3-pytest</test_depend>
  <export>
    <build_type>ament_python</build_type>
  </export>
 </package>
--- a/src/headless_pkg/resource/headless_pkg
+++ b/src/headless_pkg/resource/headless_pkg
--- a/src/headless_pkg/setup.cfg
+++ b/src/headless_pkg/setup.cfg
@@ -0,0 +1,4 @@
 [develop]
 script_dir=$base/lib/headless_pkg
 [install]
 install_scripts=$base/lib/headless_pkg
--- a/src/headless_pkg/setup.py
+++ b/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",
        ],
    },
 )
--- a/src/headless_pkg/src/init.py
+++ b/src/headless_pkg/src/init.py
--- a/src/headless_pkg/src/headless_node.py
+++ b/src/headless_pkg/src/headless_node.py
@@ -0,0 +1,300 @@
 import rclpy
 from rclpy.node import Node
 from rclpy import qos
 from rclpy.duration import Duration
 import signal
 import time
 import atexit
 import os
 import sys
 import threading
 import glob
 from math import copysign
 from std_msgs.msg import String
 from geometry_msgs.msg import Twist
 from ros2_interfaces_pkg.msg import CoreControl, ArmManual, BioControl
 from ros2_interfaces_pkg.msg import CoreCtrlState
 import pygame
 os.environ["SDL_VIDEODRIVER"] = "dummy"  # Prevents pygame from trying to open a display
 os.environ["SDL_AUDIODRIVER"] = "dummy"  # Force pygame to use a dummy audio driver before pygame.init()
 CORE_STOP_MSG = CoreControl()  # All zeros by default
 CORE_STOP_TWIST_MSG = Twist()  # "
 ARM_STOP_MSG = ArmManual()     # "
 BIO_STOP_MSG = BioControl()    # "
 control_qos = qos.QoSProfile(
    history=qos.QoSHistoryPolicy.KEEP_LAST,
    depth=2,
    reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
    durability=qos.QoSDurabilityPolicy.VOLATILE,
    deadline=Duration(seconds=1),
    lifespan=Duration(nanoseconds=500_000_000),  # 500ms
    liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
    liveliness_lease_duration=Duration(seconds=5)
 )
 CORE_MODE = "twist"  # "twist" or "duty"
 class Headless(Node):
    def __init__(self):
        # Initialize pygame first
        pygame.init()
        pygame.joystick.init()
        super().__init__("headless")
        # Wait for anchor to start
        pub_info = self.get_publishers_info_by_topic('/anchor/from_vic/debug')
        while len(pub_info) == 0:
            self.get_logger().info("Waiting for anchor to start...")
            time.sleep(1.0)
            pub_info = self.get_publishers_info_by_topic('/anchor/from_vic/debug')
        # Wait for a gamepad to be connected
        print("Waiting for gamepad connection...")
        while pygame.joystick.get_count() == 0:
            # Process any pygame events to keep it responsive
            for event in pygame.event.get():
                if event.type == pygame.QUIT:
                    pygame.quit()
                    sys.exit(0)
            time.sleep(1.0)  # Check every second
            print("No gamepad found. Waiting...")
        # Initialize the gamepad
        self.gamepad = pygame.joystick.Joystick(0)
        self.gamepad.init()
        print(f'Gamepad Found: {self.gamepad.get_name()}')
        self.create_timer(0.15, self.send_controls)
        self.core_publisher = self.create_publisher(CoreControl, '/core/control', 2)
        self.arm_publisher = self.create_publisher(ArmManual, '/arm/control/manual', 2)
        self.bio_publisher = self.create_publisher(BioControl, '/bio/control', 2)
        self.core_twist_pub_ = self.create_publisher(Twist, '/core/twist', qos_profile=control_qos)
        self.core_state_pub_ = self.create_publisher(CoreCtrlState, '/core/control/state', qos_profile=control_qos)
        self.ctrl_mode = "core"  # Start in core mode
        self.core_brake_mode = False
        self.core_max_duty = 0.5  # Default max duty cycle (walking speed)
        # Rumble when node is ready (returns False if rumble not supported)
        self.gamepad.rumble(0.7, 0.8, 150)
    def run(self):
        # This thread makes all the update processes run in the background
        thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
        thread.start()
        try:
            while rclpy.ok():
                self.send_controls()
                time.sleep(0.1)  # Small delay to avoid CPU hogging
        except KeyboardInterrupt:
            sys.exit(0)
    def send_controls(self):
        """ Read the gamepad state and publish control messages """
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                pygame.quit()
                sys.exit(0)
        # Check if controller is still connected
        if pygame.joystick.get_count() == 0:
            print("Gamepad disconnected. Exiting...")
            # Send one last zero control message
            self.core_publisher.publish(CORE_STOP_MSG)
            self.arm_publisher.publish(ARM_STOP_MSG)
            self.bio_publisher.publish(BIO_STOP_MSG)
            self.get_logger().info("Final stop commands sent. Shutting down.")
            # Clean up
            pygame.quit()
            sys.exit(0)
        new_ctrl_mode = self.ctrl_mode  # if "" then inequality will always be true
        # Check for control mode change
        dpad_input = self.gamepad.get_hat(0)
        if dpad_input[1] == 1:
            new_ctrl_mode = "arm"
        elif dpad_input[1] == -1:
            new_ctrl_mode = "core"
        if new_ctrl_mode != self.ctrl_mode:
            self.gamepad.rumble(0.6, 0.7, 75)
            self.ctrl_mode = new_ctrl_mode
            self.get_logger().info(f"Switched to {self.ctrl_mode} control mode")
        # CORE
        if self.ctrl_mode == "core" and CORE_MODE == "duty":
            input = CoreControl()
            input.max_speed = 90
            # Collect controller state
            left_stick_y = deadzone(self.gamepad.get_axis(1))
            right_stick_y = deadzone(self.gamepad.get_axis(4))
            right_trigger = deadzone(self.gamepad.get_axis(5))
            # Right wheels
            input.right_stick = float(round(-1 * right_stick_y, 2))
            # Left wheels
            if right_trigger > 0:
                input.left_stick = input.right_stick
            else:
                input.left_stick = float(round(-1 * left_stick_y, 2))
            # Debug
            output = f'L: {input.left_stick}, R: {input.right_stick}'
            self.get_logger().info(f"[Ctrl] {output}")
            self.core_publisher.publish(input)
            self.arm_publisher.publish(ARM_STOP_MSG)
            # self.bio_publisher.publish(BIO_STOP_MSG)
        elif self.ctrl_mode == "core" and CORE_MODE == "twist":
            input = Twist()
            # Collect controller state
            left_stick_y = deadzone(self.gamepad.get_axis(1))
            right_stick_x = deadzone(self.gamepad.get_axis(3))
            button_a = self.gamepad.get_button(0)
            left_bumper = self.gamepad.get_button(4)
            right_bumper = self.gamepad.get_button(5)
            # Forward/back and Turn
            input.linear.x = -1.0 * left_stick_y
            input.angular.z = -1.0 * copysign(right_stick_x ** 2, right_stick_x)  # Exponent for finer control (curve)
            # Publish
            self.core_twist_pub_.publish(input)
            self.arm_publisher.publish(ARM_STOP_MSG)
            # self.bio_publisher.publish(BIO_STOP_MSG)
            self.get_logger().info(f"[Core Ctrl] Linear: {round(input.linear.x, 2)}, Angular: {round(input.angular.z, 2)}")
            # Brake mode
            new_brake_mode = button_a
            # Max duty cycle
            if left_bumper:
                new_max_duty = 0.25
            elif right_bumper:
                new_max_duty = 0.9
            else:
                new_max_duty = 0.5
            # Only publish if needed
            if new_brake_mode != self.core_brake_mode or new_max_duty != self.core_max_duty:
                self.core_brake_mode = new_brake_mode
                self.core_max_duty = new_max_duty
                state_msg = CoreCtrlState()
                state_msg.brake_mode = bool(self.core_brake_mode)
                state_msg.max_duty = float(self.core_max_duty)
                self.core_state_pub_.publish(state_msg)
                self.get_logger().info(f"[Core State] Brake: {self.core_brake_mode}, Max Duty: {self.core_max_duty}")
        # ARM and BIO
        if self.ctrl_mode == "arm":
            arm_input = ArmManual()
            # Collect controller state
            left_stick_x = deadzone(self.gamepad.get_axis(0))
            left_stick_y = deadzone(self.gamepad.get_axis(1))
            left_trigger = deadzone(self.gamepad.get_axis(2))
            right_stick_x = deadzone(self.gamepad.get_axis(3))
            right_stick_y = deadzone(self.gamepad.get_axis(4))
            right_trigger = deadzone(self.gamepad.get_axis(5))
            right_bumper = self.gamepad.get_button(5)
            dpad_input = self.gamepad.get_hat(0)
            # EF Grippers
            if left_trigger > 0 and right_trigger > 0:
                arm_input.gripper = 0
            elif left_trigger > 0:
                arm_input.gripper = -1
            elif right_trigger > 0:
                arm_input.gripper = 1
            # Axis 0
            if dpad_input[0] == 1:
                arm_input.axis0 = 1
            elif dpad_input[0] == -1:
                arm_input.axis0 = -1
            if right_bumper:  # Control end effector
                # Effector yaw
                if left_stick_x > 0:
                    arm_input.effector_yaw = 1
                elif left_stick_x < 0:
                    arm_input.effector_yaw = -1
                # Effector roll
                if right_stick_x > 0:
                    arm_input.effector_roll = 1
                elif right_stick_x < 0:
                    arm_input.effector_roll = -1
            else:   # Control arm axis
                # Axis 1
                if abs(left_stick_x) > .15:
                    arm_input.axis1 = round(left_stick_x)
                # Axis 2
                if abs(left_stick_y) > .15:
                    arm_input.axis2 = -1 * round(left_stick_y)
                # Axis 3
                if abs(right_stick_y) > .15:
                    arm_input.axis3 = -1 * round(right_stick_y)
            # BIO
            bio_input = BioControl(
                bio_arm=int(left_stick_y * -100),
                drill_arm=int(round(right_stick_y) * -100)
            )
            # Drill motor (FAERIE)
            if deadzone(left_trigger) > 0 or deadzone(right_trigger) > 0:
                bio_input.drill = int(30 * (right_trigger - left_trigger))  # Max duty cycle 30%
            self.core_publisher.publish(CORE_STOP_MSG)
            self.arm_publisher.publish(arm_input)
            # self.bio_publisher.publish(bio_input)
 def deadzone(value: float, threshold=0.05) -> float:
    """ Apply a deadzone to a joystick input so the motors don't sound angry """
    if abs(value) < threshold:
        return 0
    return value
 def main(args=None):
    rclpy.init(args=args)
    node = Headless()
    rclpy.spin(node)
    rclpy.shutdown()
 if __name__ == '__main__':
    signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0))  # Catch termination signals and exit cleanly
    main()
--- a/src/latency_tester/CMakeLists.txt
+++ b/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()
--- a/src/latency_tester/package.xml
+++ b/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>
--- a/src/latency_tester/src/embedded_ping.cpp
+++ b/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;
 }
--- a/src/ros2_interfaces_pkg
+++ b/src/ros2_interfaces_pkg
--- a/systemd/anchor.service
+++ b/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
--- a/systemd/astra_rosbag.service
+++ b/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
--- a/systemd/core_headless.service
+++ b/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
--- a/systemd/headless_full.service
+++ b/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
Author	SHA1	Message	Date
David Sharpe	1281236b36	fix: I hate python (#21 ) BioControl.drill expects an int, have to explicitly cast to an int or headless will crash Makes main branch functional	2025-10-25 11:10:07 -05:00
Riley M.	281e5f39d3	Merge pull request #18 from SHC-ASTRA/ros2-control Flesh out ros2_control with Gazebo	2025-10-23 03:33:16 -05:00
David	fe1ae6120f	feat: all the features * Move rover-ros2/rover_launch.py to src/anchor_pkg/launch/, renamed to rover.launch.py * Anchor now waits to initialize topics until after it has found a microcontroller. * Headless now waits for anchor to start before it starts itself * Add default cases to motor feedback for motorId * Added black to the flake.nix and package.xml	2025-10-23 02:22:31 -05:00
David	44aa4b0848	feat: ramp drill speed	2025-10-23 01:00:13 -05:00
ryleu	c4f60d6814	clean up nitpicks	2025-10-23 00:52:19 -05:00
ryleu	87e3f06562	add direnv .envrc	2025-10-22 23:47:50 -05:00
David	cc53e6efd6	chore: update astra_descriptions	2025-10-22 23:40:04 -05:00
David	d879a3bae4	feat: add drill to bio headless	2025-10-21 10:41:34 -05:00
David	ed7efb4583	fix: ptz now works in flake	2025-10-18 12:28:53 -05:00
David	2165003f35	fix: msg len requirements now account for viccan packaging There are no viccan messages with len(data) == 3, only 1, 2, or 4	2025-10-18 12:17:45 -05:00
ryleu	95ceecacaa	do not build in shellHook	2025-10-18 02:39:08 -05:00
ryleu	414254b3b7	add a PORT_OVERRIDE env var	2025-10-18 02:35:09 -05:00
ryleu	a63a3b19af	fix python deps	2025-10-18 02:24:19 -05:00
David	b12515bf11	fix: rover can turn left again	2025-10-18 02:09:50 -05:00
David	8c01efeaf7	fix: make cmd_vel QoS compatible with everything else	2025-10-18 01:48:30 -05:00
David Sharpe	aa84667aab	feat: add depencies to packages.xml files, add packages to flake.nix	2025-10-17 18:06:54 -05:00
David	7ac250fd66	fix: correct descriptions submodule name	2025-10-16 18:05:13 -05:00
David	a7ec355c4f	style: move description files to new repo (astra_descriptions)	2025-10-16 18:03:43 -05:00
David	05af7f9be4	style: rename core_rover_description to core_description	2025-10-16 16:21:13 -05:00
David	5e8b60f720	style: move core_rover_description and core_gazebo into new astra_description folder	2025-10-16 12:33:10 -05:00
David	b9a63126e1	fix: correctly spawn controllers models was commented out in CMakeLists.txt because it is currently an empty directory, but will be used at a later point	2025-10-16 12:16:49 -05:00
David	a58f9b6ada	feat: make frames work properly, rviz is now accurate	2025-10-15 02:31:47 -05:00
David	89015ee7a5	feat: add VicCAN message validity checking and core feedback data length checking	2025-10-14 15:21:23 -05:00
David	d565dbe31f	refactor: change msg.command_id from if elif to match case	2025-10-14 13:49:53 -05:00
David	2d258b3103	refactor: new feedback topics use default sensor data QoS	2025-10-14 13:45:10 -05:00
David	86d01c29e3	fix: add gear ratio to JointState	2025-10-14 11:47:23 -05:00
David	366f1e0c58	feat: add joint_state pub to Core for wheel position and velocity	2025-10-13 21:50:09 -05:00
David	6bbb5d8706	refactor: make diff_controller listen on /core/rwist Now you can control it with headless or teleop_keyboard_twist	2025-10-10 23:37:19 -05:00
David	676f86bcd0	feat: make ros2 controllers start automatically Adds load_ros2_controllers.launch.py Weird issue where if the update rate on the controller is 10, then the controller manager doesn't respond to requests, but if you set it to 100 (still less than gazebo's rate), then it works...	2025-10-10 18:58:30 -05:00
David	723aa33e3c	fix: make work slightly more consistently	2025-10-10 14:16:47 -05:00
David	86684b0bff	feat: add ros2_control to Core URDF	2025-10-10 02:22:12 -05:00
David Sharpe	e70a10a191	feat: refactor Core topics and add VicCAN (#17 , topic-refactor) Refactor Core, Anchor, and Headless with new message types Add Core URDF and extremely basic Gazebo sim files Bump ros2_interfaces_pkg to v1.1	2025-10-09 22:14:43 -05:00
David	8b8ff69c27	chore: update ros2_interfaces_pkg to v1.1 No real difference, just made the PR and release	2025-10-09 22:11:29 -05:00
David	ebd07258f0	refactor: replace speed_mode with max_duty for core control Makes it make more sense	2025-10-09 16:19:34 -05:00
David	0e256c7d22	feat: add agpl-3.0-only license	2025-10-04 23:24:23 -05:00
David	b62060026c	style: minor edits based on riley's comments	2025-10-04 23:08:04 -05:00
David	924509b733	fix: add ptz back to rover_launch.py oopsies	2025-10-04 23:06:50 -05:00
David	36da0203bc	fix: make turning input curve less aggressive ^2 instead of ^3. Not tested, hopefully makes it a lil better	2025-10-01 21:49:40 -05:00
David	87b2ab6f8c	fix!: remove bio from headless Slows down controls too much, going to need to figure something out for speeding that up	2025-10-01 11:18:31 -05:00
David	e5af28af3a	feat: add Core URDF with basic Gazebo sim	2025-09-30 17:49:01 -05:00
David	8c5287158d	fix: arm/bio headless works again I hate python	2025-09-30 11:56:05 -05:00
David	97fa17a4a8	feat: add bio to headless NOTE: UNTESTED	2025-09-29 11:34:35 -05:00
David	644459ec71	style: explain anchor topics in comment block	2025-09-29 11:09:05 -05:00
David	b1f4fe8320	refactor: change Core commands to VicCAN	2025-09-29 10:29:01 -05:00
David	a25983126f	refactor: move headless globals to class vars	2025-09-29 09:25:07 -05:00
David	647ff34fa5	feat: add rumble on headless mode switch (core/arm)	2025-09-29 09:16:43 -05:00
David	75d1a841bb	feat: add VicCAN topics to Core, sync Core with PDR	2025-09-28 19:30:40 -05:00
David	7992acf60f	feat: add VicCAN topics to Anchor	2025-09-28 19:22:56 -05:00
David	75fefa7048	fix: typo in latency tester Also bump package ver. Finishes latency_tester for now, it is functional, now we just need to use it.	2025-09-26 16:11:49 -05:00
David Sharpe	2a797693a2	Add Nix flake	2025-09-17 22:40:52 -05:00
David	49b8acc860	feat: control wrist yaw and roll at same time Also made start_rosbag.sh actually executable ._.	2025-09-15 11:43:22 -05:00
David	00be1cef52	fix: make headless rumble stronger	2025-09-11 18:33:30 -05:00
David	64405e3a0b	feat: start rosbag on boot	2025-09-11 15:56:20 -05:00
David	9370b91c7e	style: move systemd services to a folder	2025-09-11 13:32:34 -05:00
David	660ceb560d	feat: populate motor status timestamps	2025-09-10 23:57:21 -05:00
David	5f8a4a0ca7	feat: add latency_tester to test comms latency	2025-09-10 23:00:40 -05:00
David	abcb9b9a4d	feat: fully add NewCoreFeedback to core_pkg, including REV pos/vel	2025-09-10 23:00:17 -05:00
David	858e03f385	feat: add rumble on headless ready, change turn to cubic Controller rumbles for 200ms when __init__() finishes, and angular is now cubic so turning control follows a curve rather than a straight line (y=x^3 instead of y=x)	2025-09-10 22:59:27 -05:00
David	882cbc5949	fix: -float is not float??? Tested with controller but on Clucky	2025-09-10 22:46:27 -05:00
David	d5ba9ad721	feat: add speed and brake mode control Speed mode is set with bumpers, brake mode is set with 'A' button. Implemented through /core/control/state topic with custom message CoreCtrlState. Also added Core measurements from Gabe, and did some general refactoring.	2025-09-10 10:49:51 -05:00
David	93226203f1	style: clean up core and headless	2025-09-09 20:12:46 -05:00
David	4c972e6264	fix: correctly account for neg linear Rover couldn't drive forwards lmao... Now correctly inverts turning direction when driving backwards... Could use some work tho, a little confusing rn. Also got some rough measurements from the rover with a tape measurement, updated wheelbase and wheel radius.	2025-09-09 12:51:48 -05:00
David	bac9b3cbe0	fix: correct turning direction	2025-09-08 23:01:40 -05:00
David	0c1a368499	feat: add Twist support to headless and core_pkg Left stick y is forward/backward, right stick x is turn/angular. Still uses duty cycle. Published on /core/twist. On headless, needs CORE_MODE constant changed to "twist" to use new Twist command. Also added TwistStamped to core_pkg for /cmd_vel. Will use velocity control. Needs wheel measurements from Core CAD. Will be used for Autonomy.	2025-09-08 09:51:25 -05:00
David	056ffd1eb6	feat: add Twist and Imu messages to Core	2025-09-07 20:04:01 -05:00
David Sharpe	8868408ac3	feat: add arm support to headless (headless_combine #16 ) Create new headless_pkg with Core + Arm Toggle with dpad up/down; up for arm, down for core	2025-09-07 19:11:12 -05:00
David	5461fbab11	fix: round() is not float ._.	2025-09-02 12:24:38 -05:00
David	a0db7f62bc	refactor: move auto_start folder into rover-ros2 from home dir	2025-09-02 02:56:50 -05:00
David	caf20f46f6	refactor: poll controller state all at once, add deadzones UNTESTED Also adds ability to control axis 0 while controlling wrist	2025-09-02 02:43:38 -05:00
David	d3dbbf8658	style: fix author stuff for headless_pkg	2025-08-28 18:55:22 -05:00
David	4254ecc4ce	fix: python float/int for /core/control	2025-08-22 16:41:54 -05:00
David Sharpe	10757c8c71	fix: names in ROS2 files were incorrect for headless_full, add new service files for NUC	2025-08-21 16:31:59 -05:00
David Sharpe	37774c0c31	feat: add systemd files from NUC	2025-08-21 15:26:53 -05:00
David Sharpe	826f14725a	feat: create headless_pkg, combine core and arm headless Toggleable with Dpad up/down	2025-08-17 09:33:28 -05:00