Some tweaks from code review

.devcontainer/devcontainer.json

@@ -1,56 +1,56 @@
 // For format details, see https://aka.ms/devcontainer.json. For config options, see the
 // README at: https://github.com/devcontainers/templates/tree/main/src/docker-existing-dockerfile
 {
-	"name": "Existing Dockerfile",
-	"build": {
-		"context": "../",
-		// Sets the run context to one level up instead of the .devcontainer folder.
-		// Update the 'dockerFile' property if you aren't using the standard 'Dockerfile' filename.
-		"dockerfile": "../Dockerfile"
-	},
-	"workspaceFolder": "/app",
-	"mounts": [
-		// Manually mount each file/folder from root of repo since just mounting the parent folder
-		// would result in the container ROS workspace being erased. Ugly workaround for now.
-		// Alternatively could use postStart commands to symlink the workspace folder. Might
-		// introduce other headaches.
-		"source=${localWorkspaceFolder}/src/aml_ctd,target=/app/src/aml_ctd,type=bind",
+    "name": "Existing Dockerfile",
+    "build": {
+        "context": "../",
+        // Sets the run context to one level up instead of the .devcontainer folder.
+        // Update the 'dockerFile' property if you aren't using the standard 'Dockerfile' filename.
+        "dockerfile": "../Dockerfile"
+    },
+    "workspaceFolder": "/app",
+    "mounts": [
+        // Manually mount each file/folder from root of repo since just mounting the parent folder
+        // would result in the container ROS workspace being erased. Ugly workaround for now.
+        // Alternatively could use postStart commands to symlink the workspace folder. Might
+        // introduce other headaches.
+        "source=${localWorkspaceFolder}/src/aml_ctd,target=/app/src/aml_ctd,type=bind",
         "source=${localWorkspaceFolder}/src/ifcb,target=/app/src/ifcb,type=bind",
         "source=${localWorkspaceFolder}/src/jvl_motor,target=/app/src/jvl_motor,type=bind",
         "source=${localWorkspaceFolder}/src/phyto_arm,target=/app/src/phyto_arm,type=bind",
         "source=${localWorkspaceFolder}/src/rbr_maestro3_ctd,target=/app/src/rbr_maestro3_ctd,type=bind",
-		"source=${localWorkspaceFolder}/.gitignore,target=/app/.gitignore,type=bind",
-		"source=${localWorkspaceFolder}/configs,target=/app/configs,type=bind",
-		"source=${localWorkspaceFolder}/deps,target=/app/deps,type=bind",
-		"source=${localWorkspaceFolder}/.devcontainer,target=/app/.devcontainer,type=bind",
-		"source=${localWorkspaceFolder}/.git,target=/app/.git,type=bind",
-		"source=${localWorkspaceFolder}/LICENSE,target=/app/LICENSE,type=bind",
-		"source=${localWorkspaceFolder}/phyto-arm.service,target=/app/phyto-arm.service,type=bind",
-		"source=${localWorkspaceFolder}/.github,target=/app/.github,type=bind",
-		"source=${localWorkspaceFolder}/phyto-arm,target=/app/phyto-arm,type=bind",
-		"source=${localWorkspaceFolder}/scripts,target=/app/scripts,type=bind",
-		"source=/home/ifcb/IFCBacquire/Host/Routines,target=/routines,type=bind",
-		"source=/mnt/data,target=/mnt/data,type=bind"
-	],
-	"runArgs": [
-         "--device=/dev/ttyS3",
-		"--publish=9090:9090/tcp",
-    	"--publish=8098:8098/tcp",
-		"--publish=12345:12345/udp"
+        "source=${localWorkspaceFolder}/.gitignore,target=/app/.gitignore,type=bind",
+        "source=${localWorkspaceFolder}/configs,target=/app/configs,type=bind",
+        "source=${localWorkspaceFolder}/deps,target=/app/deps,type=bind",
+        "source=${localWorkspaceFolder}/.devcontainer,target=/app/.devcontainer,type=bind",
+        "source=${localWorkspaceFolder}/.git,target=/app/.git,type=bind",
+        "source=${localWorkspaceFolder}/LICENSE,target=/app/LICENSE,type=bind",
+        "source=${localWorkspaceFolder}/phyto-arm.service,target=/app/phyto-arm.service,type=bind",
+        "source=${localWorkspaceFolder}/.github,target=/app/.github,type=bind",
+        "source=${localWorkspaceFolder}/phyto-arm,target=/app/phyto-arm,type=bind",
+        "source=${localWorkspaceFolder}/scripts,target=/app/scripts,type=bind",
+        "source=/home/ifcb/IFCBacquire/Host/Routines,target=/routines,type=bind",
+        "source=/mnt/data,target=/mnt/data,type=bind"
+    ],
+    "runArgs": [
+        "--device=/dev/ttyS3",
+        "--publish=9090:9090/tcp",
+        "--publish=8098:8098/tcp",
+        "--publish=12345:12345/udp"
     ]
 
-	// Features to add to the dev container. More info: https://containers.dev/features.
-	// "features": {},
+    // Features to add to the dev container. More info: https://containers.dev/features.
+    // "features": {},
 
-	// Use 'forwardPorts' to make a list of ports inside the container available locally.
-	// "forwardPorts": [],
+    // Use 'forwardPorts' to make a list of ports inside the container available locally.
+    // "forwardPorts": [],
 
-	// Uncomment the next line to run commands after the container is created.
-	// "postCreateCommand": "cat /etc/os-release",
+    // Uncomment the next line to run commands after the container is created.
+    // "postCreateCommand": "cat /etc/os-release",
 
-	// Configure tool-specific properties.
-	// "customizations": {},
+    // Configure tool-specific properties.
+    // "customizations": {},
 
-	// Uncomment to connect as an existing user other than the container default. More info: https://aka.ms/dev-containers-non-root.
-	// "remoteUser": "devcontainer"
+    // Uncomment to connect as an existing user other than the container default. More info: https://aka.ms/dev-containers-non-root.
+    // "remoteUser": "devcontainer"
 }

.gitignore

@@ -15,4 +15,5 @@ configs
 # Build artifacts in devcontainer after mount
 apt-requirements.txt
 deps.rosinstall
-python3-requirements.txt
+python3-requirements.txt
+

Dockerfile

@@ -12,7 +12,7 @@ RUN apt update \
 
 # Install Python dependencies
 COPY deps/python3-requirements.txt ./
-RUN pip install --upgrade setuptools==69.1.0
+RUN python3 -m pip install --upgrade setuptools==69.1.0
 RUN python3 -m pip install -r python3-requirements.txt
 
 

configs/example.yaml

@@ -113,6 +113,7 @@ arm_ifcb:
         gear_ratio: 60
 
         max_speed:  0.02  # m/s
+
         # Distance from target at which we will reduce speed by half. This controls
         # the sharpness of our velocity function. A smaller number produces a more
         # abrupt stop.

deps/python3-requirements.txt

@@ -1,6 +1,7 @@
 aiohttp==3.9.2
 pyModbusTCP==0.1.8
 scipy==1.10
+
 # Workaround - eventually should move to ros-triton-classifier
 # and make PhytO-ARM triton agnostic
 tritonclient[all]==2.33.0

src/phyto_arm/action/MoveToDepth.action

@@ -5,6 +5,7 @@
 
 float32 depth  # m
 float32 velocity  # m/s
+
 ---
 
 # Result

src/phyto_arm/src/arm_base.py

@@ -1,16 +1,12 @@
 #!/usr/bin/env python3
 from dataclasses import dataclass
-import functools
-import math
-from queue import Queue
 from threading import Lock
 
 import actionlib
-import numpy as np
 import rospy
 
 from phyto_arm.msg import MoveToDepthAction, MoveToDepthGoal
-from phyto_arm.srv import LockOperation, LockOperationRequest, LockCheck, LockCheckRequest
+from phyto_arm.srv import LockCheck, LockCheckRequest, LockOperation, LockOperationRequest
 
 
 @dataclass
@@ -45,6 +41,7 @@ def __init__(self, arm_name, winch_name=None):
             acquire_move_clearance = rospy.ServiceProxy('/lock_manager/acquire', LockOperation)
             release_move_clearance = rospy.ServiceProxy('/lock_manager/release', LockOperation)
             lock_op = LockOperationRequest(arm_name)
+
             # Bind lock operations to instance
             self.request_clearance = lambda: acquire_move_clearance(lock_op).success
             self.release_clearance = lambda: release_move_clearance(lock_op).success
@@ -54,6 +51,7 @@ def __init__(self, arm_name, winch_name=None):
             rospy.loginfo('Awaiting lock manager check...')
             check_lock.wait_for_service()
             rospy.loginfo('Server acquired for lock check.')
+
             # Check whether we have a dangling acquisition to release
             if check_lock(LockCheckRequest(arm_name)).has_lock:
                 rospy.logwarn('Releasing dangling lock acquisition')
@@ -63,9 +61,9 @@ def __init__(self, arm_name, winch_name=None):
                 winch_name,
                 MoveToDepthAction
             )
-            rospy.loginfo(f"Awaiting winch server for {winch_name}")
+            rospy.loginfo(f'Awaiting winch server for {winch_name}')
             self.winch_client.wait_for_server()
-            rospy.loginfo(f"Server acquired for {winch_name}")
+            rospy.loginfo(f'Server acquired for {winch_name}')
 
 
     def winch_busy(self):
@@ -75,38 +73,43 @@ def winch_busy(self):
 
     def send_winch_goal(self, depth, speed, callback):
         global winches_moving
-        rospy.loginfo(f"Sending winch move to {depth}")
+        rospy.loginfo(f'Sending winch move to {depth}')
+
         # Ensure winch is enabled
-        if rospy.get_param('winch/enabled') != True:
+        if rospy.get_param('winch/enabled') is not True:
             raise ValueError('Move aborted: winch is disabled in config')
+
         # Safety check: Do not exceed depth bounds
         if depth < rospy.get_param('winch/range/min'):
             raise ValueError(f'Move aborted: depth {depth} is below min {rospy.get_param("winch/range/max")}')
         elif depth > rospy.get_param('winch/range/max'):
             raise ValueError(f'Move aborted: depth {depth} is above max {rospy.get_param("winch/range/max")}')
+
         # Set max speed if not specified
         if speed is None:
             speed = rospy.get_param('winch/max_speed')
+
         # Safety check: Speed cannot exceed max speed
         elif speed > rospy.get_param('winch/max_speed'):
             raise ValueError(f'Move aborted: speed {speed} is above max {rospy.get_param("winch/max_speed")}')
         if self.winch_busy():
             raise RuntimeError('Move aborted: winch in unexpected busy state')
-        rospy.loginfo(f"All checks passed; Moving winch to {depth}")
 
+        rospy.loginfo(f'All checks passed; Moving winch to {depth}')
 
         # Intermediate callback for ensuring move was a success and to release lock
         # before calling the task's callback
         def winch_done(state, move_result):
             try:
                 # Ensure winch move was successful
                 assert state == actionlib.GoalStatus.SUCCEEDED
+
                 # Free up semaphore for another winch movement
                 assert self.release_clearance()
                 callback(move_result)
             except Exception as e:
-                rospy.logerr(f"Unexpected error: {e}")
-                rospy.signal_shutdown(f"Shutting down due to unexpected error: {e}")
+                rospy.logerr(f'Unexpected error: {e}')
+                rospy.signal_shutdown(f'Shutting down due to unexpected error: {e}')
                 raise e
 
         self.winch_client.send_goal(MoveToDepthGoal(depth=depth, velocity=speed), done_cb=winch_done)
@@ -134,20 +137,24 @@ def loop(self):
                     task = self.get_next_task(task)
                     rospy.logwarn(f'No winch; running {task.name}')
                     task.callback()
+
                 # Otherwise, move winch
                 else:
-                    rospy.logwarn(f'Arm waiting for clearance')
+                    rospy.logwarn('Arm waiting for clearance')
+
                     # TODO: Consider replacing this with a queueing mechanism.  Requires setting up
                     # callbacks via ROS service calls. Unnecessarily complex for a 2 winch system,
                     # but might be the only way to achieve round-robin for many winches.
+
                     while not rospy.is_shutdown() and not self.request_clearance():
                         # Wait until central semaphore clears us to move
                         rospy.sleep(1)
+
                     # TODO: Optimize task evaluation. Currently we are blocking on the assumption that
                     # movement will be needed; this is not always the case. Not a problem for 1 or 2
                     # winches, but could get slow if the number of winches greatly exceeds the
                     # max_moving_winches limit.
+
                     task = self.get_next_task(task)
                     rospy.logwarn(f'Goal depth {task.depth} for task {task.name}')
                     self.send_winch_goal(task.depth, task.speed, task.callback)
-

src/phyto_arm/src/arm_chanos.py

@@ -16,18 +16,22 @@ class ArmChanos(ArmBase):
     #  - speed: the speed to move at (optional, will use config max if not provided)
     def get_next_task(self, last_task):
         assert rospy.get_param('winch/enabled'), 'Winch is not enabled'
+
         if last_task is None:
             return Task("upcast", self.start_next_task, rospy.get_param('winch/range/min'))
+
         if last_task.name == "upcast":
             downcast_depth = rospy.get_param('winch/range/max')
             downcast_speed = rospy.get_param('tasks/downcast/speed')
             return Task("downcast", self.start_next_task, downcast_depth, downcast_speed)
+
         if last_task.name in ["downcast", "upcast_stage"]:
             next_depth = stage_depth(self.stage_index)
             self.stage_index += 1
             if self.stage_index == len(rospy.get_param('tasks/upcast/stages')):
                 self.stage_index = 0
             return Task("upcast_stage", await_stage, next_depth)
+
         raise ValueError(f"Unhandled next-task state where last task={last_task.name}")
 
 # Global reference to arm state
@@ -38,7 +42,7 @@ def await_stage(move_result):
     duration = rospy.get_param('tasks/upcast/stage_duration')
     rospy.loginfo(f"Waiting {duration} seconds for DC sensor to complete.")
     rospy.sleep(duration)
-    rospy.loginfo(f'Done waiting for DC sensor to complete.')
+    rospy.loginfo('Done waiting for DC sensor to complete.')
     arm.start_next_task()