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

auto_start/auto_start_anchor.sh

@@ -21,4 +21,4 @@ source /home/clucky/rover-ros2/install/setup.bash
 cd /home/clucky/rover-ros2/
 
 # Launch the ROS 2 node with the desired mode
-ros2 launch rover_launch.py mode:=anchor
+ros2 launch anchor_pkg rover.launch.py mode:=anchor

flake.nix

@@ -31,6 +31,7 @@
                 pygame
                 scipy
                 crccheck
+                black
               ]
             ))
             (

src/anchor_pkg/anchor_pkg/anchor_node.py

@@ -43,27 +43,6 @@ class SerialRelay(Node):
         # Initalize node with name
         super().__init__("anchor_node")#previously 'serial_publisher'
 
-        # New pub/sub with VicCAN
-        self.fromvic_debug_pub_ = self.create_publisher(String, '/anchor/from_vic/debug', 20)
-        self.fromvic_core_pub_ = self.create_publisher(VicCAN, '/anchor/from_vic/core', 20)
-        self.fromvic_arm_pub_ = self.create_publisher(VicCAN, '/anchor/from_vic/arm', 20)
-        self.fromvic_bio_pub_ = self.create_publisher(VicCAN, '/anchor/from_vic/bio', 20)
-
-        self.mock_mcu_sub_ = self.create_subscription(String, '/anchor/from_vic/mock_mcu', self.on_mock_fromvic, 20)
-        self.tovic_sub_ = self.create_subscription(VicCAN, '/anchor/to_vic/relay', self.on_relay_tovic_viccan, 20)
-        self.tovic_debug_sub_ = self.create_subscription(String, '/anchor/to_vic/relay_string', self.on_relay_tovic_string, 20)
-
-
-        # Create publishers 
-        self.arm_pub = self.create_publisher(String, '/anchor/arm/feedback', 10)
-        self.core_pub = self.create_publisher(String, '/anchor/core/feedback', 10)
-        self.bio_pub = self.create_publisher(String, '/anchor/bio/feedback', 10)
-
-        self.debug_pub = self.create_publisher(String, '/anchor/debug', 10)
-
-        # Create a subscriber 
-        self.relay_sub = self.create_subscription(String, '/anchor/relay', self.on_relay_tovic_string, 10)
-
         # Loop through all serial devices on the computer to check for the MCU
         self.port = None
         if port_override := os.getenv("PORT_OVERRIDE"):
@@ -98,6 +77,27 @@ class SerialRelay(Node):
         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)
+        self.core_pub = self.create_publisher(String, '/anchor/core/feedback', 10)
+        self.bio_pub = self.create_publisher(String, '/anchor/bio/feedback', 10)
+
+        self.debug_pub = self.create_publisher(String, '/anchor/debug', 10)
+
+        # Create a subscriber 
+        self.relay_sub = self.create_subscription(String, '/anchor/relay', self.on_relay_tovic_string, 10)
+
 
     def run(self):
         # This thread makes all the update processes run in the background

src/anchor_pkg/package.xml

@@ -11,6 +11,8 @@
   <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>
   <test_depend>ament_pep257</test_depend>

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,

src/core_pkg/core_pkg/core_node.py

@@ -426,6 +426,9 @@ class SerialRelay(Node):
                         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
@@ -471,6 +474,9 @@ class SerialRelay(Node):
                     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)

src/headless_pkg/src/headless_node.py

@@ -48,6 +48,14 @@ class Headless(Node):
         # 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...")
@@ -65,8 +73,6 @@ class Headless(Node):
         self.gamepad.init()
         print(f'Gamepad Found: {self.gamepad.get_name()}')
 
-        # Now initialize the ROS2 node
-        super().__init__("headless")
         self.create_timer(0.15, self.send_controls)
 
         self.core_publisher = self.create_publisher(CoreControl, '/core/control', 2)