Merge branch 'master' into simulation-raise-switch

uf-mil · Feb 22, 2024 · af21261 · af21261
2 parents 48489c4 + 6067925
commit af21261
Show file tree

Hide file tree

Showing 76 changed files with 157 additions and 145 deletions.
diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml
@@ -31,6 +31,7 @@ jobs:
       - name: Configure catkin workspace folder structure
         run: |
           mkdir -p $GITHUB_WORKSPACE/catkin_ws/src
+          sudo apt reinstall python3-pip
       - name: Check out code from GitHub
         uses: actions/[email protected]
         with:

diff --git a/.github/workflows/gh_pages.yaml b/.github/workflows/gh_pages.yaml
@@ -20,7 +20,7 @@ jobs:
 
       # Wait for the CI to finish so we can download the docs artifact
       - name: Wait for CI
-        uses: lewagon/wait-on-check-action@v1.0.0
+        uses: lewagon/wait-on-check-action@v1.3.3
         if: github.event.action != 'closed'
         with:
           ref: ${{ github.event.pull_request.head.ref }}

diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
@@ -11,15 +11,15 @@ ci:
 
 repos:
   - repo: https://github.com/adrienverge/yamllint.git
-    rev: v1.32.0
+    rev: v1.33.0
     hooks:
       - id: yamllint
   - repo: https://github.com/psf/black
-    rev: 23.9.1
+    rev: 24.1.1
     hooks:
       - id: black
   - repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v16.0.6
+    rev: v17.0.6
     hooks:
       - id: clang-format
   - repo: https://github.com/PyCQA/autoflake
@@ -34,18 +34,18 @@ repos:
         exclude: ^docker|deprecated|NaviGator/simulation/VRX
         args: [--severity=warning, --exclude=SC1090]
   - repo: https://github.com/scop/pre-commit-shfmt
-    rev: v3.7.0-1
+    rev: v3.7.0-4
     hooks:
       - id: shfmt
         exclude: ^docker|deprecated|NaviGator/simulation/VRX
   - repo: https://github.com/astral-sh/ruff-pre-commit
     # Ruff version.
-    rev: 'v0.0.292'
+    rev: 'v0.2.0'
     hooks:
       - id: ruff
         args: [--fix, --exit-non-zero-on-fix]
   - repo: https://github.com/codespell-project/codespell
-    rev: v2.2.5
+    rev: v2.2.6
     hooks:
       - id: codespell
         args:
@@ -73,11 +73,12 @@ repos:
       - id: prettier-package-xml
       - id: sort-package-xml
   - repo: https://github.com/pre-commit/pre-commit-hooks
-    rev: v4.4.0
+    rev: v4.5.0
     hooks:
       - id: check-added-large-files
       - id: check-case-conflict
       - id: check-merge-conflict
+        exclude_types: [markdown, rst]
       - id: check-executables-have-shebangs
       - id: check-symlinks
       - id: check-json

diff --git a/NaviGator/gnc/navigator_path_planner/nodes/path_planner.py b/NaviGator/gnc/navigator_path_planner/nodes/path_planner.py
@@ -1070,7 +1070,7 @@ def reevaluate_plan(self) -> None:
     def action_check(self, _: rospy.timer.TimerEvent) -> None:
         """
         Manages action preempting. Serves as the callback to a Timer operating
-          opereating every self.revisit_period seconds.
+          operating every self.revisit_period seconds.
         """
         if self.preempted or not self.move_server.is_active():
             return

diff --git a/NaviGator/hardware_drivers/navigator_kill_board/navigator_kill_board/constants.py b/NaviGator/hardware_drivers/navigator_kill_board/navigator_kill_board/constants.py
@@ -17,6 +17,7 @@
 The computer can also command a kill (for example, if ROS notices a criticaly low battery)
 by sending the COMPUTER.KILL.REQUEST and undone with COMPUTER.CLEAR.REQUEST
 """
+
 constants = {
     "TIMEOUT_SECONDS": 8.0,  # How often board must be pinged to not set HEARTBERAT_REMOTE True
     # Note: not official documented, this is just a guess

diff --git a/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/battery_voltage.py b/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/battery_voltage.py
@@ -27,9 +27,9 @@ def _check_voltage(self, msg):
             if not self._raised or self._severity != severity:
                 self.broadcaster.raise_alarm(
                     severity=severity,
-                    problem_description="battery critcaly low"
-                    if severity == 2
-                    else "battery low",
+                    problem_description=(
+                        "battery critcaly low" if severity == 2 else "battery low"
+                    ),
                     parameters={"voltage": voltage},
                 )
 

diff --git a/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/odom_kill.py b/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/odom_kill.py
@@ -51,9 +51,7 @@ def check_continuity(self, odom):
                 self._raised = True  # Avoid raising multiple times
                 rospy.logwarn("ODOM DISCONTINUITY DETECTED")
                 self.ab.raise_alarm(
-                    problem_description="ODOM DISCONTINUITY DETECTED. JUMPED {} METERS".format(
-                        jump,
-                    ),
+                    problem_description=f"ODOM DISCONTINUITY DETECTED. JUMPED {jump} METERS",
                     severity=5,
                 )
         self.last_position = position

diff --git a/NaviGator/mission_control/navigator_launch/launch/vrx/vrx_tf.launch b/NaviGator/mission_control/navigator_launch/launch/vrx/vrx_tf.launch
@@ -1,5 +1,8 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
+    <!-- Silent unnecessary TF warnings about missing joint states or missing transforms (cred: osrf/vrx#340)-->
+    <rosparam param="silent_tf_failure">true</rosparam>
+
     <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher">
       <param name="use_tf_static" value="false" />
     </node>
@@ -11,9 +14,10 @@
       </rosparam>
     </node>
 
-  <!--Publishes connection between wamv/imu_wamv_link and wamv/base_link. Same for wamv/gps_wamv_link.-->
-  <node pkg="tf2_ros" type="static_transform_publisher" name="imu_base_link_fix" args="0.3 -0.2 1.3 0 0 0 1 wamv/base_link wamv/imu_wamv_link" />
-  <node pkg="tf2_ros" type="static_transform_publisher" name="gps_base_link_fix" args="-0.85 0 1.3 0 0 1 wamv/base_link wamv/ins_link" />
-  <node pkg="tf2_ros" type="static_transform_publisher" name="base_link_enu_fix" args="0 0 0 0 0 0 1 enu wamv/base_link" />
+
+    <!--Publishes connection between wamv/imu_wamv_link and wamv/base_link. Same for wamv/gps_wamv_link.-->
+    <node pkg="tf2_ros" type="static_transform_publisher" name="imu_base_link_fix" args="0.3 -0.2 1.3 0 0 0 1 wamv/base_link wamv/imu_wamv_link" />
+    <node pkg="tf2_ros" type="static_transform_publisher" name="gps_base_link_fix" args="-0.85 0 1.3 0 0 1 wamv/base_link wamv/ins_link" />
+    <node pkg="tf2_ros" type="static_transform_publisher" name="base_link_enu_fix" args="0 0 0 0 0 0 1 enu wamv/base_link" />
 
 </launch>
diff --git a/NaviGator/mission_control/navigator_missions/launch/mission_params.yaml b/NaviGator/mission_control/navigator_missions/launch/mission_params.yaml
@@ -157,14 +157,14 @@ dock_shape_1:
   param: /mission/identify_dock/shape_1
   options: [CIRCLE, TRIANGLE, CROSS, ANY]
   description: >
-    The shape of the first bay to dock in during the dentify
+    The shape of the first bay to dock in during the identify
     Symbols and Dock mission. If set to ANY, mission should dock based on the
     color parameter and set this parameter during the mission
 dock_color_1:
   param: /mission/identify_dock/color_1
   options: [RED, GREEN, BLUE, ANY]
   description: >
-    The color of the first bay to dock in during the dentify Symbols and Dock
+    The color of the first bay to dock in during the identify Symbols and Dock
     mission. If set to ANY, mission should dock based on the shape parameter
     and set this parameter during the mission
 
@@ -174,7 +174,7 @@ dock_shape_2:
   description: >
     # yamllint disable-line rule:line-length
     The shape of the second bay to dock in during the
-    dentify Symbols and Dock mission. If set to ANY,
+    identify Symbols and Dock mission. If set to ANY,
     mission should dock based on the color parameter and set this
     parameter during the mission
 dock_color_2:
@@ -183,6 +183,6 @@ dock_color_2:
   description: >
     # yamllint disable-line rule:line-length
     The color of the second bay to dock in during the
-    dentify Symbols and Dock mission.
+    identify Symbols and Dock mission.
     If set to ANY, mission should dock based on the shape
     parameter and set this parameter during the mission
diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/constant_velocity.py b/NaviGator/mission_control/navigator_missions/navigator_missions/constant_velocity.py
@@ -66,7 +66,7 @@ def decode_parameters(cls, parameters):
         return parsed
 
     async def run(self, args):
-        # Publish a velocity of zero for a while to stabalize navigator
+        # Publish a velocity of zero for a while to stabilize navigator
         self.send_feedback("Switching trajectory to constant")
         await self.change_trajectory("constant")
         await self.nh.sleep(0.1)

diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/demonstrate_navigation.py b/NaviGator/mission_control/navigator_missions/navigator_missions/demonstrate_navigation.py
@@ -68,7 +68,7 @@ async def run(self, parameters):
             _, closest_reds = await self.get_sorted_objects("red_cylinder", 1)
             _, closest_greens = await self.get_sorted_objects("green_cylinder", 1)
 
-            # Rename the totems for their symantic name
+            # Rename the totems for their semantic name
             green_close = closest_greens[0]
             red_close = closest_reds[0]
 
@@ -83,7 +83,7 @@ async def run(self, parameters):
             _, closest_reds = await self.get_sorted_objects("red_cylinder", 2)
             _, closest_greens = await self.get_sorted_objects("green_cylinder", 2)
 
-            # Rename the totems for their symantic name
+            # Rename the totems for their semantic name
             green_far = closest_greens[1]
             red_far = closest_reds[1]
 

diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/detect_deliver_2016.py b/NaviGator/mission_control/navigator_missions/navigator_missions/detect_deliver_2016.py
@@ -153,9 +153,7 @@ async def do_circle():
                 await self.nh.sleep(0.25)
                 continue
             fprint(
-                "Shape ({}found, using normal to look at other 3 shapes if needed".format(
-                    res[0],
-                ),
+                f"Shape ({res[0]}found, using normal to look at other 3 shapes if needed",
                 title="DETECT DELIVER",
                 msg_color="green",
             )

diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/move.py b/NaviGator/mission_control/navigator_missions/navigator_missions/move.py
@@ -149,7 +149,7 @@ async def run(self, args):
                     "yl": "yaw_left",
                     "yr": "yaw_right",
                 }
-                command = command if command not in shorthand else shorthand[command]
+                command = shorthand.get(command, command)
                 movement = getattr(self.move, command)
 
                 trans_move = command[:3] != "yaw"

diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/pose_editor.py b/NaviGator/mission_control/navigator_missions/navigator_missions/pose_editor.py
@@ -341,9 +341,7 @@ def as_MoveGoal(self, move_type=MoveGoal.DRIVE, **kwargs):
         for key in kwargs:
             if not hasattr(MoveGoal, key):
                 fprint(
-                    "MoveGoal msg doesn't have a field called '{}' you tried to set via kwargs.".format(
-                        key,
-                    ),
+                    f"MoveGoal msg doesn't have a field called '{key}' you tried to set via kwargs.",
                     title="POSE_EDITOR",
                     msg_color="red",
                 )

diff --git a/NaviGator/mission_control/navigator_missions/vrx_missions/dock.py b/NaviGator/mission_control/navigator_missions/vrx_missions/dock.py
@@ -607,7 +607,7 @@ async def prepare_for_docking(self):
         # This function looks at the two squares in front of the boat
         # and it gets the middle pixel between the two squares.
         # If the middle pixel is for some reason not in the middle of our camera...
-        # adjust the boat postiion before docking
+        # adjust the boat position before docking
         print("prepare for landing!")
 
         img = await self.front_left_camera_sub.get_next_message()

diff --git a/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_wayfinding_2.py b/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_wayfinding_2.py
@@ -5,6 +5,7 @@
 from .vrx import Vrx
 
 ___author___ = "Alex Perez"
+# Optimized by Daniel Parra
 
 
 class VrxWayfinding2(Vrx):
@@ -42,12 +43,18 @@ async def run(self, parameters):
         poses = poses[:start_pose_index]
         path = path[1:]
 
-        # self.send_feedback('Sorted poses' + str(poses))
         await self.wait_for_task_such_that(lambda task: task.state in ["running"])
 
         # do movements
         for index in path:
             self.send_feedback(f"Going to {poses[index]}")
-
             # Go to goal
+            P = 0.85
+            part_way_point = [x * P for x in poses[index][0][:-1]]
+            part_way_point.append(poses[index][0][-1])
+            self.send_feedback(
+                f"\nPartway:\n{part_way_point}\nEndPoint:\n{poses[index][0]}",
+            )
+
+            await self.send_trajectory_without_path([part_way_point, poses[index][1]])
             await self.send_trajectory_without_path(poses[index])
diff --git a/NaviGator/satellite/rviz_satellite/src/aerialmap_display.cpp b/NaviGator/satellite/rviz_satellite/src/aerialmap_display.cpp
@@ -416,7 +416,7 @@ void AerialMapDisplay::assembleScene()
     // determine location of this tile, flipping y in the process
     const double x = (tile.x() - loader_->centerTileX()) * tile_w + origin_x;
     const double y = -(tile.y() - loader_->centerTileY()) * tile_h + origin_y;
-    //  don't re-use any ids
+    //  don't reuse any ids
     const std::string name_suffix = std::to_string(tile.x()) + "_" + std::to_string(tile.y()) + "_" +
                                     std::to_string(map_id_) + "_" + std::to_string(scene_id_);
 

diff --git a/NaviGator/scripts/bash_aliases.sh b/NaviGator/scripts/bash_aliases.sh
@@ -18,7 +18,7 @@ nthrust() {
 	topic="/$1_motor/cmd"
 	publishers=$(rostopic info "$topic" | grep Publishers)
 	if [ "$publishers" != "Publishers: None" ]; then
-		echo "Somone is already publishing to $topic. Perhaps you need to kill thrust mapper?"
+		echo "Someone is already publishing to $topic. Perhaps you need to kill thrust mapper?"
 		return 1
 	fi
 	rostopic pub "$topic" "roboteq_msgs/Command" "setpoint: $2" -r100

diff --git a/NaviGator/utils/navigator_msgs/srv/GetDockBays.srv b/NaviGator/utils/navigator_msgs/srv/GetDockBays.srv
@@ -2,4 +2,4 @@
 geometry_msgs/Point[3] bays   #The positions in ENU frame of the center of the three bays 0=left, 1=center, 2=right
 geometry_msgs/Vector3  normal #Vector or normal pointing away from plane of dock back
 bool success                  #False if an error occurred getting dock bays
-string error                  #Descripion of error if success=false
+string error                  #Description of error if success=false
diff --git a/NaviGator/utils/navigator_tools/nodes/navigator_status_tui b/NaviGator/utils/navigator_tools/nodes/navigator_status_tui
@@ -35,7 +35,7 @@ class nav_tui:
         self.panel.hide()
         panel.update_panels()
         self.rate = rospy.Rate(2)  # Fixed rate for while loop to update at: 2 hz
-        # The following are default field initalizations for various values that that will be overwritten
+        # The following are default field initializations for various values that that will be overwritten
         self.voltage = 0
         self.wrench = None
         self.LAT = None
@@ -214,15 +214,13 @@ class nav_tui:
             if len(self.decode_fault_status(self.FL_fault)) == 0:
                 self.window.addstr(5, self.x / 2 - 7, "No faults", curses.color_pair(3))
             else:
-                n = 0
-                for fault in self.decode_fault_status(self.FL_fault):
+                for n, fault in enumerate(self.decode_fault_status(self.FL_fault)):
                     self.window.addstr(
                         5 + n,
                         self.x / 2 - 7,
                         "%s" % fault,
                         curses.color_pair(1),
                     )
-                    n += 1
 
             self.window.addstr(14, self.x / 2 - 7, "Back Left", color)
             if len(self.decode_fault_status(self.BL_fault)) == 0:

diff --git a/NaviGator/utils/remote_control/navigator_emergency_control/nodes/navigator_emergency.py b/NaviGator/utils/remote_control/navigator_emergency_control/nodes/navigator_emergency.py
@@ -94,7 +94,7 @@ def check_for_timeout(self, joy: Joy):
 
     def joy_recieved(self, joy: Joy) -> None:
         """
-        Button elements are being assigned and simplied to readable names. The
+        Button elements are being assigned and simplified to readable names. The
         number of deployments or retractions for thrusters are being updated based
         on several conditions. Moreover, additional settings are changed based on the
         state of the controller and the activation of potential alarms or switches.

diff --git a/NaviGator/utils/voltage_gui/src/voltage_gui.py b/NaviGator/utils/voltage_gui/src/voltage_gui.py
@@ -184,7 +184,7 @@ def resizeFont(self) -> None:  # done
         threshFont = QtGui.QFont("Times", (self.fontSize) / 3, QtGui.QFont.Bold)
         self.labelThresh.setFont(threshFont)
 
-    # Sets the text of the thrshold info box
+    # Sets the text of the threshold info box
     def initThresh(self) -> None:
         """
         Sets the text of the threshold info box

diff --git a/SubjuGator/command/subjugator_launch/config/passive_sonar.yaml b/SubjuGator/command/subjugator_launch/config/passive_sonar.yaml
@@ -3,7 +3,7 @@
 dist_h: 2.286e-02
 # speed of sound in water
 v_sound: 1482
-#  target Frquency in Hz
+#  target Frequency in Hz
 triggering/target_frequency: 30000
 #  tolerance around target frequerncy in Hz
 triggering/frequency_tolerance: 100

diff --git a/SubjuGator/command/subjugator_launch/launch/subsystems/nav_box.launch b/SubjuGator/command/subjugator_launch/launch/subsystems/nav_box.launch
@@ -42,10 +42,10 @@
       <param name="frame_id" type="string" value="/imu"/>
       <rosparam>
         scale:
-        - [0.993770963897068, 0.00105871125374563, 7.659410525291767e-05]
-        - [0.00105871125374563, 0.9996814868251349, -0.0011040738267441828]
-        - [7.659410525291767e-05, -0.001104073826744163, 1.0065910531028952]
-        shift: [1.2551999807772446e-06, -1.1666595150804588e-06, 6.895773090438596e-08]
+        - [0.9991765357958566, 0.006242798579443988, -0.008472478269327878]
+        - [0.006242798579443957, 1.0016179705091928, 0.0059841151097914345]
+        - [-0.008472478269327836, 0.0059841151097913, 0.999354597532967]
+        shift: [7.889247409445414e-06, 4.879179471165382e-06, 7.46017199298374e-06]
       </rosparam>
     </node>
 

diff --git a/...drivers/magnetic_compensation/sub8_magnetic_hardsoft_compensation/scripts/generate_config b/...drivers/magnetic_compensation/sub8_magnetic_hardsoft_compensation/scripts/generate_config
@@ -132,9 +132,11 @@ if __name__ == "__main__":
     with rosbag.Bag(sys.argv[1]) as bag:
         points = numpy.array(
             [
-                mil_ros_tools.rosmsg_to_numpy(msg.magnetic_field)
-                if hasattr(msg, "magnetic_field")
-                else mil_ros_tools.rosmsg_to_numpy(msg.vector)
+                (
+                    mil_ros_tools.rosmsg_to_numpy(msg.magnetic_field)
+                    if hasattr(msg, "magnetic_field")
+                    else mil_ros_tools.rosmsg_to_numpy(msg.vector)
+                )
                 for topic, msg, t in bag.read_messages(topics=["/imu/mag_raw"])
             ],
         )

diff --git a/SubjuGator/perception/subjugator_perception/nodes/dice_detect.py b/SubjuGator/perception/subjugator_perception/nodes/dice_detect.py
@@ -93,7 +93,7 @@ def detect(self, dice_img):
         params.minConvexity = 0.8  # 1 = perfect convex hull
         # Filter by Inertia
         params.filterByInertia = True
-        params.minInertiaRatio = 0.4  # Defines the ellipsoid 1= detects only cirlces
+        params.minInertiaRatio = 0.4  # Defines the ellipsoid 1= detects only circles
         # 0 = Detects even lines
 
         # Create a detector with the parameters