diff --git a/scripts/config_validation.py b/scripts/config_validation.py index 57017e3..424aa65 100755 --- a/scripts/config_validation.py +++ b/scripts/config_validation.py @@ -1,6 +1,8 @@ #!/usr/bin/env python3 -import yaml + import sys +import yaml + def extract_annotations(file_path): annotations = {} diff --git a/src/phyto_arm/src/arm_base.py b/src/phyto_arm/src/arm_base.py index 39d0f13..1018836 100755 --- a/src/phyto_arm/src/arm_base.py +++ b/src/phyto_arm/src/arm_base.py @@ -136,7 +136,7 @@ def loop(self): 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. diff --git a/src/phyto_arm/src/arm_chanos.py b/src/phyto_arm/src/arm_chanos.py index 7dc7909..397d45a 100755 --- a/src/phyto_arm/src/arm_chanos.py +++ b/src/phyto_arm/src/arm_chanos.py @@ -46,7 +46,7 @@ def get_next_task(self, last_task): if last_task.name == "continuous_position": speed = rospy.get_param('tasks/continuous_profile/speed') return Task("continuous_profile", self.start_next_task, continuous_goal, speed) - + # After continuous, execute every step in stepped cast if last_task.name in ["continuous_profile", "absolute_step"]: @@ -70,7 +70,7 @@ def profiler_peak_ready(): if arm.latest_profile is None: rospy.logwarn('No profiler peak available') return False - + if arm.profiler_peak_value < rospy.get_param('tasks/profiler_peak/threshold'): rospy.logwarn(f'Profiler peak value {arm.profiler_peak_value} is below threshold') return False @@ -88,7 +88,7 @@ def profiler_peak_ready(): def compute_profiler_steps(): if arm.latest_profile is None: raise ValueError('No profiler peak available') - + depths = [] offsets = rospy.get_param('tasks/profiler_peak/offset_steps') @@ -134,13 +134,13 @@ def continuous_direction(): def await_sensor(move_result): duration = rospy.get_param('tasks/dwell_time') rospy.loginfo(f"Waiting {duration} seconds for DC sensor to complete.") - + def timer_callback(event): if rospy.is_shutdown(): rospy.logwarn('Node shutdown detected. Aborting DC sensor wait.') return - rospy.loginfo(f'Done waiting for DC sensor to complete.') + rospy.loginfo('Done waiting for DC sensor to complete.') arm.start_next_task() arm.timer = rospy.Timer(rospy.Duration(duration), timer_callback, oneshot=True) diff --git a/src/phyto_arm/src/arm_ifcb.py b/src/phyto_arm/src/arm_ifcb.py index dc4b416..ddaf459 100755 --- a/src/phyto_arm/src/arm_ifcb.py +++ b/src/phyto_arm/src/arm_ifcb.py @@ -7,11 +7,8 @@ import numpy as np import rospy -from ifcbclient.protocol import parse_response as parse_ifcb_msg -from ifcb.instrumentation import parse_marker as parse_ifcb_marker -from phyto_arm.msg import DepthProfile, RunIFCBGoal, RunIFCBAction - from arm_base import ArmBase, Task +from phyto_arm.msg import DepthProfile, RunIFCBGoal, RunIFCBAction class ArmIFCB(ArmBase): @@ -25,6 +22,8 @@ class ArmIFCB(ArmBase): profiler_peak_depth = None profiler_peak_value = None + last_cart_debub_time = None + last_bead_time = None # Primary method for determining the next task to execute # Each Task object takes: @@ -44,7 +43,7 @@ def get_next_task(self, last_task): # Othrwise, start off at min if last_task is None or last_task.name in ["scheduled_depth", "profiler_peak_depth", "wiz_probe"]: return Task("upcast", self.start_next_task, rospy.get_param('winch/range/min')) - + # Then perform a downcast to get a full profile if last_task.name == "upcast": return Task("downcast", handle_downcast, rospy.get_param('winch/range/max')) @@ -223,7 +222,6 @@ def handle_nowinch(): def main(): global arm - global set_state global ifcb_runner rospy.init_node('arm_ifcb', log_level=rospy.DEBUG) diff --git a/src/phyto_arm/src/ifcb_runner.py b/src/phyto_arm/src/ifcb_runner.py index 7b862c2..61af010 100755 --- a/src/phyto_arm/src/ifcb_runner.py +++ b/src/phyto_arm/src/ifcb_runner.py @@ -2,18 +2,14 @@ from dataclasses import dataclass import functools import math -from queue import Queue from threading import Event, Condition import actionlib -import numpy as np import rospy +from ds_core_msgs.msg import RawData from ifcbclient.protocol import parse_response as parse_ifcb_msg from ifcb.instrumentation import parse_marker as parse_ifcb_marker - -from ds_core_msgs.msg import RawData - from ifcb.srv import RunRoutine from phyto_arm.msg import ConductorState, ConductorStates, RunIFCBAction, RunIFCBResult diff --git a/src/phyto_arm/src/lock_manager.py b/src/phyto_arm/src/lock_manager.py index 54efd4b..a8878b5 100755 --- a/src/phyto_arm/src/lock_manager.py +++ b/src/phyto_arm/src/lock_manager.py @@ -1,7 +1,9 @@ #!/usr/bin/env python3 + +from threading import Lock, Semaphore + import rospy from phyto_arm.srv import LockOperation, LockCheck, LockOperationResponse, LockCheckResponse -from threading import Lock, Semaphore class NamedLockManager: def __init__(self, max_concurrent_locks): @@ -37,7 +39,7 @@ def check_lock(self, arm_name): if __name__ == "__main__": rospy.init_node('lock_manager') lock_manager = NamedLockManager(rospy.get_param('~max_moving_winches')) - + def handle_acquire(req): success = lock_manager.acquire_lock(req.arm_name) return LockOperationResponse(success=success) @@ -53,5 +55,5 @@ def handle_check(req): rospy.Service('~acquire', LockOperation, handle_acquire) rospy.Service('~release', LockOperation, handle_release) rospy.Service('~check', LockCheck, handle_check) - + rospy.spin() diff --git a/src/phyto_arm/src/mocks/mock_ctd.py b/src/phyto_arm/src/mocks/mock_ctd.py index 20e2d85..31b4a8b 100755 --- a/src/phyto_arm/src/mocks/mock_ctd.py +++ b/src/phyto_arm/src/mocks/mock_ctd.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 -import rospy import random +import rospy + from std_msgs.msg import Float64 from ds_sensor_msgs.msg import Ctd, DepthPressure diff --git a/src/phyto_arm/src/mocks/mock_ifcb_runner.py b/src/phyto_arm/src/mocks/mock_ifcb_runner.py index df60f65..62042c7 100755 --- a/src/phyto_arm/src/mocks/mock_ifcb_runner.py +++ b/src/phyto_arm/src/mocks/mock_ifcb_runner.py @@ -43,7 +43,7 @@ def execute_cb(self, goal): def main(): rospy.init_node('mock_ifcb_action_server') - server = MockIFCBActionServer('~sample') + MockIFCBActionServer('~sample') rospy.spin() if __name__ == '__main__': diff --git a/src/phyto_arm/src/mocks/mock_winch_node.py b/src/phyto_arm/src/mocks/mock_winch_node.py index 2c36fc5..1613d09 100755 --- a/src/phyto_arm/src/mocks/mock_winch_node.py +++ b/src/phyto_arm/src/mocks/mock_winch_node.py @@ -1,9 +1,11 @@ #!/usr/bin/env python3 import rospy import actionlib -from phyto_arm.msg import MoveToDepthAction, MoveToDepthFeedback, MoveToDepthResult + from std_msgs.msg import Float64 # Assuming depth is published as a simple float +from phyto_arm.msg import MoveToDepthAction, MoveToDepthFeedback, MoveToDepthResult + class MockActionServer: def __init__(self, name, action_spec): self._action_server = actionlib.SimpleActionServer( @@ -43,7 +45,7 @@ def execute_cb(self, goal): def main(): rospy.init_node('mock_winch') rospy.logdebug(f'Starting mock winch node {rospy.get_name()}') - server = MockActionServer('winch/move_to_depth', MoveToDepthAction) + MockActionServer('winch/move_to_depth', MoveToDepthAction) rospy.spin() if __name__ == '__main__': diff --git a/src/phyto_arm/src/profiler_node.py b/src/phyto_arm/src/profiler_node.py index 84b1569..a5dde56 100755 --- a/src/phyto_arm/src/profiler_node.py +++ b/src/phyto_arm/src/profiler_node.py @@ -7,7 +7,8 @@ import numpy as np import rospy -import scipy.interpolate, scipy.signal +import scipy.interpolate +import scipy.signal from ds_sensor_msgs.msg import DepthPressure diff --git a/src/phyto_arm/src/tests.py b/src/phyto_arm/src/tests.py index 7717554..14c187b 100755 --- a/src/phyto_arm/src/tests.py +++ b/src/phyto_arm/src/tests.py @@ -1,13 +1,12 @@ #!/usr/bin/env python3 PKG="phyto_arm" + from datetime import datetime, timedelta import os +import rospy import sys import unittest -from unittest.mock import patch, Mock, MagicMock - -import numpy as np -import rospy +from unittest.mock import patch, Mock # Hack for getting this to recognize local modules for import @@ -28,7 +27,7 @@ def test_no_profiler_peak_available(self, mock_rospy, mock_arm): result = profiler_peak_ready() self.assertFalse(result) mock_rospy.logwarn.assert_called_with('No profiler peak available') - + @patch('arm_chanos.arm') @patch('arm_chanos.rospy') def test_profiler_peak_below_threshold(self, mock_rospy, mock_arm): @@ -39,7 +38,7 @@ def test_profiler_peak_below_threshold(self, mock_rospy, mock_arm): result = profiler_peak_ready() self.assertFalse(result) mock_rospy.logwarn.assert_called_with('Profiler peak value 0.3 is below threshold') - + @patch('arm_chanos.arm') @patch('arm_chanos.rospy') def test_profiler_peak_expired(self, mock_rospy, mock_arm): @@ -53,7 +52,7 @@ def test_profiler_peak_expired(self, mock_rospy, mock_arm): result = profiler_peak_ready() self.assertFalse(result) mock_rospy.logwarn.assert_called_with('Profiler peak expired at 2023-01-01 12:00:00') - + @patch('arm_chanos.arm') @patch('arm_chanos.rospy') def test_profiler_peak_ready(self, mock_rospy, mock_arm): @@ -75,7 +74,7 @@ def test_no_profiler_peak_available(self, mock_rospy, mock_arm): with self.assertRaises(ValueError) as exc: compute_profiler_steps() self.assertEqual(str(exc.exception), 'No profiler peak available') - + @patch('arm_chanos.arm') @patch('arm_chanos.rospy') def test_compute_profiler_steps(self, mock_rospy, mock_arm): @@ -84,7 +83,7 @@ def test_compute_profiler_steps(self, mock_rospy, mock_arm): mock_rospy.get_param.side_effect = [[-1.0, 1.0, 2.0], 10.0, 0.0] result = compute_profiler_steps() self.assertEqual(result, [4.0, 6.0, 7.0]) - + @patch('arm_chanos.arm') @patch('arm_chanos.rospy') def test_compute_profiler_steps_exceeds_max(self, mock_rospy, mock_arm): @@ -93,7 +92,7 @@ def test_compute_profiler_steps_exceeds_max(self, mock_rospy, mock_arm): mock_rospy.get_param.side_effect = [[1.0, 2.0], 10.0, 0.0] result = compute_profiler_steps() self.assertEqual(result, [10.0, 10.0]) - + @patch('arm_chanos.arm') @patch('arm_chanos.rospy') def test_compute_profiler_steps_less_than_min(self, mock_rospy, mock_arm): @@ -155,15 +154,15 @@ def test_wiz_to_rostime_next_day(self, mock_datetime): # Mock datetime.now() to return a specific point in time in the evening mock_now = datetime(2024, 2, 16, 23, 0) # Feb 16, 2024, 11:00 PM mock_datetime.now.return_value = mock_now - + # Calculate the expected ROS time for the next day target_time = datetime(2024, 2, 17, 9, 0) # Target time: Feb 17, 2024, 9:00 AM expected_secs_since_epoch = target_time.timestamp() expected_ros_time = rospy.Time.from_sec(expected_secs_since_epoch) - + # Call the function under test and get the return value result_ros_time = arm_ifcb.wiz_to_rostime("09:00") - + # Verify that the result matches the expected ROS time for the next day self.assertEqual(result_ros_time.secs, expected_ros_time.secs, "The returned ROS time does not match the expected time for the next day.") @@ -173,15 +172,15 @@ def test_wiz_to_rostime_before_current_time(self, mock_datetime): # Mock datetime.now() to return a specific point in time mock_now = datetime(2024, 2, 16, 10, 30) # Feb 16, 2024, 10:30 AM mock_datetime.now.return_value = mock_now - + # Calculate the expected ROS time for the same day but later target_time = datetime(2024, 2, 16, 10, 0) # Target time: 10:00 AM same day expected_secs_since_epoch = target_time.timestamp() + 86400 # Adding one day since 10:00 AM has already passed expected_ros_time = rospy.Time.from_sec(expected_secs_since_epoch) - + # Call the function under test with a time before the current mock time to ensure it rolls over to the next day result_ros_time = arm_ifcb.wiz_to_rostime("10:00") - + # Verify that the result matches the expected ROS time for the next day due to time rollover self.assertEqual(result_ros_time.secs, expected_ros_time.secs, "The returned ROS time should roll over to the next day when the target time is before the current time.") @@ -362,5 +361,3 @@ def test_its_scheduled_depth_time_peak_below_threshold(self, mock_arm, mock_rosp # with patch('arm_ifcb.rospy.Time.now', return_value=rospy.Time.from_sec(1000)): # arm_ifcb.scheduled_depth() # self.assertEqual(mock_arm.last_scheduled_time.secs, 1000, "The last scheduled time should be updated to the current time.") - - diff --git a/src/phyto_arm/src/winch_node.py b/src/phyto_arm/src/winch_node.py index 3c7ddaa..ff1ef21 100755 --- a/src/phyto_arm/src/winch_node.py +++ b/src/phyto_arm/src/winch_node.py @@ -194,8 +194,7 @@ async def move_to_depth(server, goal): rpm_ratio = 60 * gear_ratio / spool_circumference # Create a function to convert distances to encoder counts - dist2encoder = \ - lambda d: int(round(8192 * gear_ratio * d / spool_circumference)) + dist2encoder = lambda d: int(round(8192 * gear_ratio * d / spool_circumference)) # Set some position bounds on the motor itself rospy.logdebug(f'Current motor position is {motor.value.position:.2f}') @@ -213,8 +212,7 @@ async def move_to_depth(server, goal): server.set_aborted(text='Encoder position could wrap -- reset offset') return - rospy.loginfo('Motor position envelope is ' + - f'({lower_bound:.2f}, {upper_bound:.2f})') + rospy.loginfo(f'Motor position envelope is ({lower_bound:.2f}, {upper_bound:.2f})') # Record the time that we start moving start_time = rospy.Time.now() @@ -328,8 +326,7 @@ async def main(): # Register signal handlers the same way rospy would for sig in [signal.SIGTERM, signal.SIGINT]: - loop.add_signal_handler(sig, rospy.signal_shutdown, - f'signal-{sig.value}') + loop.add_signal_handler(sig, rospy.signal_shutdown, f'signal-{sig.value}') rospy.core.add_preshutdown_hook(lambda reason: loop.stop()) # Wait for services to become available diff --git a/src/rbr_maestro3_ctd/src/udp_to_ros.py b/src/rbr_maestro3_ctd/src/udp_to_ros.py index 868b766..4b7a9e3 100755 --- a/src/rbr_maestro3_ctd/src/udp_to_ros.py +++ b/src/rbr_maestro3_ctd/src/udp_to_ros.py @@ -39,10 +39,10 @@ def udp_to_ros_node(): buffer = b"" buffer_size = 32768 # 32KB buffer size - + while not rospy.is_shutdown(): try: - data, addr = sock.recvfrom(buffer_size) + data, _ = sock.recvfrom(buffer_size) buffer += data # Process all complete messages in the buffer