merged master into my branch in order to reset VRX submodule

.github/workflows/ci.yaml

@@ -24,8 +24,19 @@ concurrency:
   cancel-in-progress: true
 
 jobs:
+  avoid-duplicate-ci:
+    name: Check if CI has already be ran
+    runs-on: ubuntu-latest
+    outputs:
+      should_skip: ${{ steps.check_skip.outputs.should_skip }}
+    steps:
+      - id: check_skip
+        uses: fkirc/skip-duplicate-actions@v5
+
   super-ci:
     name: Run tests and build docs
+    needs: avoid-duplicate-ci
+    if: needs.avoid-duplicate-ci.outputs.should_skip != 'true'
     runs-on: self-hosted
     steps:
       - name: Configure catkin workspace folder structure
@@ -50,7 +61,7 @@ jobs:
       - name: Install BlueView Sonar SDK
         run: |
           cd $GITHUB_WORKSPACE/catkin_ws/src/mil
-          ./scripts/hardware_installers/install_bvtsdk --pass ${{ secrets.ZOBELISK_PASSWORD }}
+          ./scripts/hw/install_bvtsdk --pass ${{ secrets.ZOBELISK_PASSWORD }}
           ls mil_common/drivers/mil_blueview_driver
           echo $PWD
       - name: Install system dependencies
@@ -76,7 +87,7 @@ jobs:
           source devel/setup.bash
           catkin_make -DPYTHON_EXECUTABLE=/usr/bin/python3 \
                       -DPYTHON_INCLUDE_DIR=/usr/include/python3.8 \
-                      -j6 \
+                      "-j$(($(nproc)-2))" \
                       run_tests
       - name: Get test results
         run: >
@@ -101,9 +112,12 @@ jobs:
   deploy-docs:
     name: Deploy docs from master
     runs-on: self-hosted
-    needs:
-      - super-ci
-    if: github.ref == 'refs/heads/master'
+    needs: super-ci
+    # https://github.com/actions/runner/issues/491#issuecomment-850884422
+    if: |
+      always() &&
+      (needs.super-ci.result == 'skipped' || needs.super-ci.result == 'success') &&
+      github.ref == 'refs/heads/master'
     steps:
       - name: Checkout
         uses: actions/checkout@v4

.pre-commit-config.yaml

@@ -34,16 +34,17 @@ repos:
         exclude: ^docker|deprecated|NaviGator/simulation/VRX
         args: [--severity=warning, --exclude=SC1090]
   - repo: https://github.com/scop/pre-commit-shfmt
-    rev: v3.8.0-1
+    rev: v3.9.0-1
     hooks:
       - id: shfmt
         exclude: ^docker|deprecated|NaviGator/simulation/VRX
   - repo: https://github.com/astral-sh/ruff-pre-commit
     # Ruff version.
-    rev: 'v0.5.6'
+    rev: 'v0.6.3'
     hooks:
       - id: ruff
         args: [--fix, --exit-non-zero-on-fix]
+        exclude: ^docker|deprecated|NaviGator/simulation/VRX
   - repo: https://github.com/codespell-project/codespell
     rev: v2.3.0
     hooks:

NaviGator/hardware_drivers/navigator_ball_launcher/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(navigator_ball_launcher)
+
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+)
+
+catkin_python_setup()
+
+catkin_package(
+)
+if(CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  add_rostest(test/simulated.test)
+endif()

NaviGator/hardware_drivers/navigator_ball_launcher/navigator_ball_launcher/__init__.py

@@ -0,0 +1,8 @@
+"""
+The :mod:`navigator_ball_launcher` module is used to control the ball launcher
+on NaviGator. The module implements the electrical protocol to communicate with
+a board that controls the flywheel and servo to drop the balls.
+"""
+
+from .driver import BallLauncherDevice
+from .packets import ReleaseBallPacket, SetSpinPacket

NaviGator/hardware_drivers/navigator_ball_launcher/navigator_ball_launcher/driver.py

@@ -0,0 +1,91 @@
+from __future__ import annotations
+
+from typing import Union
+
+import rospy
+from electrical_protocol import AckPacket, NackPacket, ROSSerialDevice
+from navigator_msgs.srv import BallLauncherDrops, BallLauncherDropsResponse
+from std_srvs.srv import Empty, EmptyRequest, SetBool, SetBoolRequest
+
+from .packets import ReleaseBallPacket, SetSpinPacket
+
+
+class BallLauncherDevice(
+    ROSSerialDevice[
+        Union[SetSpinPacket, ReleaseBallPacket],
+        Union[AckPacket, NackPacket],
+    ],
+):
+
+    heard_ack: bool
+    heard_nack: bool
+
+    def __init__(self, port: str):
+        super().__init__(port, 115200)
+        self.drop_service = rospy.Service("~drop_ball", Empty, self.drop_ball)
+        self.spin_service = rospy.Service("~spin", SetBool, self.spin)
+        self.drops_service = rospy.Service(
+            "~number_of_drops",
+            BallLauncherDrops,
+            self._number_of_drops_srv,
+        )
+        self.heard_ack = False
+        self.heard_nack = False
+        self.drops = 0
+        self.temp_timer = None
+        print("done init")
+
+    def get_drop_count(self) -> int:
+        return self.drops
+
+    def _number_of_drops_srv(self, _):
+        return BallLauncherDropsResponse(self.get_drop_count())
+
+    def _reset_ack(self):
+        self.heard_ack = False
+        self.heard_nack = False
+
+    def _check_for_valid_response(self, event: str):
+        if self.heard_nack:
+            rospy.logerr(f"Failed to {event} (heard NACK from board)")
+        elif not self.heard_ack:
+            rospy.logerr(f"Failed to {event} (no response from board)")
+
+    def _check_for_dropped_ball(self):
+        self._check_for_valid_response("drop ball")
+
+    def _check_for_spun(self):
+        self._check_for_valid_response("set spin")
+
+    def drop_ball(self, _: EmptyRequest):
+        self._reset_ack()
+        rospy.loginfo("Dropping ball...")
+        self.drops += 1
+        self.send_packet(ReleaseBallPacket())
+        self.temp_timer = rospy.Timer(
+            rospy.Duration(1),
+            self._check_for_dropped_ball,
+            oneshot=True,
+        )
+        return {}
+
+    def spin(self, request: SetBoolRequest):
+        self._reset_ack()
+        if request.data:
+            rospy.loginfo("Spinning up the ball launcher...")
+        else:
+            rospy.loginfo("Stopping the ball launcher...")
+        self.send_packet(SetSpinPacket(request.data))
+        self.temp_timer = rospy.Timer(
+            rospy.Duration(1),
+            self._check_for_spun,
+            oneshot=True,
+        )
+        return {}
+
+    def on_packet_received(self, packet: AckPacket | NackPacket) -> None:
+        print("inc packet", packet)
+        if isinstance(packet, AckPacket):
+            self.heard_ack = True
+        elif isinstance(packet, NackPacket):
+            self.heard_nack = True

NaviGator/hardware_drivers/navigator_ball_launcher/navigator_ball_launcher/packets.py

@@ -0,0 +1,35 @@
+from dataclasses import dataclass
+
+from electrical_protocol import Packet
+
+
+@dataclass
+class ReleaseBallPacket(Packet, class_id=0x11, subclass_id=0x00, payload_format=""):
+    """
+    Packet sent by the motherboard to the board to release a ball (aka, spin the servo
+    such that one ball is released.)
+
+    A valid response to this packet being sent would be :class:`~electrical_protocol.AckPacket`
+    (if the ball was successfully released) or :class:`~electrical_protocol.NackPacket`
+    (if there was an issue).
+    """
+
+    pass
+
+
+@dataclass
+class SetSpinPacket(Packet, class_id=0x11, subclass_id=0x01, payload_format="<B"):
+    """
+    Packet sent by the motherboard to the board to spin up the flywheel.
+
+    A valid response to this packet being sent would be :class:`~electrical_protocol.AckPacket`
+    (if the flywheel was successfully spun up) or :class:`~electrical_protocol.NackPacket`
+    (if there was an issue).
+
+    Attributes:
+        spin_up (bool): A boolean used to represent whether the flywheel should
+            be spun up (aka, sped up to a fast rpm) or whether it should be spun
+            down (aka, slowed to zero rpm).
+    """
+
+    spin_up: bool

NaviGator/hardware_drivers/navigator_ball_launcher/nodes/driver.py

@@ -0,0 +1,8 @@
+#! /usr/bin/env python3
+import rospy
+from navigator_ball_launcher.driver import BallLauncherDevice
+
+if __name__ == "__main__":
+    rospy.init_node("ball_launcher")
+    device = BallLauncherDevice(str(rospy.get_param("~port")))
+    rospy.spin()

NaviGator/hardware_drivers/navigator_ball_launcher/package.xml

@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<package>
+  <name>navigator_ball_launcher</name>
+  <version>1.0.0</version>
+  <description>The navigator_ball_launcher package</description>
+  <maintainer email="[email protected]">Cameron Brown</maintainer>
+  <license>MIT</license>
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>rospy</build_depend>
+  <run_depend>rospy</run_depend>
+  <export/>
+</package>

NaviGator/hardware_drivers/navigator_ball_launcher/setup.py

@@ -0,0 +1,11 @@
+# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
+
+from catkin_pkg.python_setup import generate_distutils_setup
+from setuptools import setup
+
+# Fetch values from package.xml
+setup_args = generate_distutils_setup(
+    packages=["navigator_ball_launcher"],
+)
+
+setup(**setup_args)

NaviGator/hardware_drivers/navigator_ball_launcher/test/simulated.test

@@ -0,0 +1,5 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+    <param name="/is_simulation" value="True" />
+    <test pkg="navigator_ball_launcher" test-name="test_simulated_shooter" type="test_simulated_shooter.py" />
+</launch>

NaviGator/hardware_drivers/navigator_ball_launcher/test/test_simulated_shooter.py

@@ -0,0 +1,88 @@
+#!/usr/bin/env python3
+
+import os
+import pty
+import unittest
+
+import rospy
+import rostest
+from electrical_protocol import AckPacket
+from navigator_ball_launcher.driver import BallLauncherDevice
+from navigator_ball_launcher.packets import ReleaseBallPacket, SetSpinPacket
+from navigator_msgs.srv import (
+    BallLauncherDrops,
+    BallLauncherDropsRequest,
+)
+from std_srvs.srv import Empty, EmptyRequest, SetBool, SetBoolRequest
+
+
+class SimulatedBasicTest(unittest.TestCase):
+    def __init__(self, *args):
+        super().__init__(*args)
+
+    @classmethod
+    def setUpClass(cls):
+        cls.master, cls.slave = pty.openpty()
+        serial_name = os.ttyname(cls.slave)
+        cls.device = BallLauncherDevice(serial_name)
+        cls.drop_ball_proxy = rospy.ServiceProxy(
+            "/test_simulated_shooter/drop_ball",
+            Empty,
+        )
+        cls.spin_service_proxy = rospy.ServiceProxy(
+            "/test_simulated_shooter/spin",
+            SetBool,
+        )
+        cls.drops_service_proxy = rospy.ServiceProxy(
+            "/test_simulated_shooter/number_of_drops",
+            BallLauncherDrops,
+        )
+        cls.drop_ball_proxy.wait_for_service()
+        cls.spin_service_proxy.wait_for_service()
+        cls.drops_service_proxy.wait_for_service()
+
+    def test_drop(self):
+        before_drops_resp = self.drops_service_proxy(BallLauncherDropsRequest())
+        self.drop_ball_proxy(EmptyRequest())
+        # Ensure that we hear the packet
+        ReleaseBallPacket.from_bytes(os.read(self.master, 100))
+        # and then send back the simulated ACK
+        import time
+
+        print(time.time(), "sending")
+        os.write(self.master, bytes(AckPacket()))
+        print(time.time(), "sent!")
+        after_drops_resp = self.drops_service_proxy(BallLauncherDropsRequest())
+        self.assertEqual(before_drops_resp.count + 1, after_drops_resp.count)
+
+    def test_spin(self):
+        before_drops_resp = self.drops_service_proxy(BallLauncherDropsRequest())
+        self.spin_service_proxy(SetBoolRequest(True))
+        # Ensure that we hear the packet
+        packet = SetSpinPacket.from_bytes(os.read(self.master, 100))
+        self.assertEqual(packet.spin_up, True)
+        # and then send back the simulated ACK
+        os.write(self.master, bytes(AckPacket()))
+        self.spin_service_proxy(SetBoolRequest(False))
+        # Ensure that we hear the packet
+        packet = SetSpinPacket.from_bytes(os.read(self.master, 100))
+        self.assertEqual(packet.spin_up, False)
+        # and then send back the simulated ACK
+        os.write(self.master, bytes(AckPacket()))
+        after_drops_resp = self.drops_service_proxy(BallLauncherDropsRequest())
+        self.assertEqual(before_drops_resp.count, after_drops_resp.count)
+
+    @classmethod
+    def tearDownClass(cls):
+        os.close(cls.master)
+        os.close(cls.slave)
+
+
+if __name__ == "__main__":
+    rospy.init_node("test_simulated_shooter")
+    rostest.rosrun(
+        "navigator_ball_launcher",
+        "test_simulated_shooter",
+        SimulatedBasicTest,
+    )
+    unittest.main()

NaviGator/mission_control/navigator_launch/launch/hardware/gps.launch

@@ -2,7 +2,7 @@
     <node name="sylphase_ros_driver" pkg="navigator_tools" type="sylphase_ros_driver">
         <param name="port" value="1235" />
         <param name="child_frame_id" value="wamv/ins_link" />
-        <param name="force_z_to_zero" value="False" />
+        <param name="force_z_to_zero" value="True" />
         <remap from="/odom" to="/ins_odom" />
     </node>
 </launch>

NaviGator/mission_control/navigator_launch/launch/shore/xbox_controller.launch

@@ -1,9 +1,10 @@
 <launch>
   <arg name="wireless" default="False" />
+  <arg name="device_input" default="/dev/input/js0" unless="$(arg wireless)"/>
+  <arg name="device_input" default="/dev/input/by-id/usb-045e_0291-joystick" if="$(arg wireless)"/>
   <!-- Connect to MIL's Xbox Game controller to publish /joy messages -->
   <node pkg="joy" type="joy_node" name="joy_node">
-      <param unless="$(arg wireless)" name="dev" type="string" value="/dev/input/js0" />
-      <param if="$(arg wireless)" name="dev" type="string" value="/dev/input/by-id/usb-045e_0291-joystick" />
+      <param name="dev" type="string" value="$(arg device_input)" />
       <param name="deadzone" value="0.12" />
       <param name="autorepeat_rate" value="5"/>
   </node>

NaviGator/mission_control/navigator_launch/launch/vrx/vrx_controller.launch

@@ -2,8 +2,8 @@
 <launch>
   <node name="mrac" pkg="navigator_controller" type="mrac_controller.py" output="screen">
     <rosparam>
-      kp: [1500.0, 1500.0, 5600.0]
-      kd: [300.0, 200.0, 3000.0]
+      kp: [1000.0, 700, 5600.0]
+      kd: [200.0, 400, 3000.0]
       ki: [0.2, 0.2, 0.2]
       kg: [5.0, 5.0, 5.0, 5.0, 5.0]
       mass: 251.19