diff --git a/configs/example.yaml b/configs/example.yaml index 1ce70c6..4a3530b 100644 --- a/configs/example.yaml +++ b/configs/example.yaml @@ -133,30 +133,35 @@ arm_ifcb: arm_chanos: tasks: - # Time the arm will hold at each position - dwell_time: 60 # seconds profiler_peak: # If true, will use the profiler to find the peak value - # Falls back on slowcast/staged cast behavior otherwise + # Falls back on default_movement values otherwise enabled: true # Minimum level of the profiled sensor data to trigger a sample at depth threshold: 0.0 - peak_expiration: 60 # seconds since last peak to consider it expired + # seconds since last peak to consider it expired + peak_expiration: 60 + # Stops to be made relative to profiler_peak offset_steps: + - -0.5 + - -0.25 - 0.0 + - 0.25 + - 0.5 + default_movement: + # Steps will be sorted in direction of movement. + absolute_steps: - 0.5 - - -0.5 - continuous_profile: - # Must be set to "up" or "down" - direction: "up" - speed: 0.005 # m/s - step_profile: - # steps list can be any length - # will be executed in order - steps: - - 0.7 # first step, meters - - 1.0 - - 1.5 # last step + - 2.0 + - 3.0 + - 4.0 + - 5.0 + - 6.5 + # For continuous movements (no steps), a speed can be specified. + continuous_speed: 0.005 + # must be "stepped" or "continuous" + downcast_type: "stepped" + upcast_type: "continuous" ctd_topic: '/arm_chanos/ctd/depth' diff --git a/src/phyto_arm/src/arm_chanos.py b/src/phyto_arm/src/arm_chanos.py index aa3ca0a..10c467e 100755 --- a/src/phyto_arm/src/arm_chanos.py +++ b/src/phyto_arm/src/arm_chanos.py @@ -12,11 +12,12 @@ class ArmChanos(ArmBase): profiler_peak_depth = None profiler_peak_time = None - profiler_steps = [] - absolute_steps = [] + steps = [] timer = None + is_downcasting = True + # Primary method for determining the next task to execute # Each Task object takes: # - name: a string identifying the task @@ -26,40 +27,57 @@ class ArmChanos(ArmBase): def get_next_task(self, last_task): assert rospy.get_param('winch/enabled'), 'Winch is not enabled' - # If the profiler peak task is available, keep looping through profiler steps - if rospy.get_param('tasks/profiler_peak/enabled'): - if not self.profiler_steps and profiler_peak_ready(): - self.profiler_steps = compute_profiler_steps() - if self.profiler_steps: - next_depth = self.profiler_steps.pop(0) - return Task('profiler_step', await_sensor, next_depth) - - # For continuous profile, figure out the start and goal depths - continuous_start, continuous_goal = continuous_direction() - - # If the last task was the step cast, move into position for a continuous cast - if last_task is None or last_task.name in ['profiler_step', 'last_step']: - return Task('continuous_position', self.start_next_task, continuous_start) - - # If in the continuous position, execute the continuous profile - 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']: - - # Steps are always regenerated after a continuous profile - if last_task.name == 'continuous_profile': - self.absolute_steps = rospy.get_param('tasks/step_profile/steps') - next_depth = self.absolute_steps.pop(0) - - # Detect the last step to end the loop - if not self.absolute_steps: - return Task('last_step', await_sensor, next_depth) + # Always upcast to start position first + if last_task is None: + self.is_downcasting = False + return Task('upcast_continuous', await_sensor, rospy.get_param('winch/range/min')) + + # If in the middle of stepped movement, continue to next step + if self.steps: + next_depth = self.steps.pop(0) + return Task(last_task.name, await_sensor, next_depth) + + # Finish out the upcast when upcast steps are used + if last_task.name in ['upcast_step', 'upcast_profile_step']: + return Task('upcast_continuous', self.start_next_task, rospy.get_param('winch/range/min')) + + # Finish out the downcast when downcast steps are used + elif last_task.name in ['downcast_step', 'downcast_profile_step']: + return Task('downcast_continuous', self.start_next_task, rospy.get_param('winch/range/max')) + + # Finally if a cast has completed, switch direction and start the next cast + elif last_task.name in ['upcast_continuous', 'downcast_continuous']: + self.is_downcasting = not self.is_downcasting + + # No other state should be possible + else: + raise ValueError(f'Unexpected last task name: {last_task.name}') + + # If profiler peak is enabled and ready, execute profiler peak + if rospy.get_param('tasks/profiler_peak/enabled') and profiler_peak_ready(): + self.steps = compute_profiler_steps(self.is_downcasting) + task_name = 'downcast_profile_step' if self.is_downcasting else 'upcast_profile_step' + next_depth = self.steps.pop(0) + return Task(task_name, await_sensor, next_depth) + + # If no profiler peak, execute default profile + if self.is_downcasting: + if rospy.get_param('tasks/default_movement/downcast_type') == 'continuous': + speed = rospy.get_param('tasks/default_movement/continuous_speed') + return Task('downcast_continuous', self.start_next_task, rospy.get_param('winch/range/max'), speed) + else: + self.steps = compute_absolute_steps(self.is_downcasting) + next_depth = self.steps.pop(0) + return Task('downcast_step', await_sensor, next_depth) + else: + if rospy.get_param('tasks/default_movement/upcast_type') == 'continuous': + speed = rospy.get_param('tasks/default_movement/continuous_speed') + return Task('upcast_continuous', self.start_next_task, rospy.get_param('winch/range/min'), speed) + else: + self.steps = compute_absolute_steps(self.is_downcasting) + next_depth = self.steps.pop(0) + return Task('upcast_step', await_sensor, next_depth) - return Task('absolute_step', await_sensor, next_depth) - raise ValueError(f'Unhandled next-task state where last task={last_task.name}') # Global reference to arm state arm = None @@ -84,27 +102,53 @@ def profiler_peak_ready(): return True -def compute_profiler_steps(): +def clamp_steps(steps): + DEPTH_MAX = rospy.get_param('winch/range/max') + DEPTH_MIN = rospy.get_param('winch/range/min') + clamped_steps = steps.copy() + + for i in range(len(steps)): + if steps[i] > DEPTH_MAX: + rospy.logwarn(f'Step {steps[i]} exceeds max depth, clamping to {DEPTH_MAX}') + clamped_steps[i] = DEPTH_MAX + if steps[i] < DEPTH_MIN: + rospy.logwarn(f'Step {steps[i]} exceeds min depth, clamping to {DEPTH_MIN}') + clamped_steps[i] = DEPTH_MIN + + return clamped_steps + + +def compute_absolute_steps(is_downcast): + steps = rospy.get_param('tasks/default_movement/absolute_steps') + steps = clamp_steps(steps) + + # Sort according to direction. + # If downcasting, use default ascending order. + # Otherwise, reverse to get descending order. + # Remember that in this context "ascending order" means "increasing depth". + steps.sort(reverse=not is_downcast) + return steps + + +def compute_profiler_steps(is_downcast): if arm.latest_profile is None: raise ValueError('No profiler peak available') depths = [] offsets = rospy.get_param('tasks/profiler_peak/offset_steps') - DEPTH_MAX = rospy.get_param('winch/range/max') - DEPTH_MIN = rospy.get_param('winch/range/min') - + # Add each offset to peak depth to get list of depths, + # then clamp within min/max range for offset in offsets: next_depth = arm.profiler_peak_depth + offset - - # Ensure none of the steps exceeds max or min bounds - if next_depth > DEPTH_MAX: - rospy.logwarn('Profiler peak step exceeds max depth, clamping to max') - next_depth = DEPTH_MAX - if next_depth < DEPTH_MIN: - rospy.logwarn('Profiler peak step exceeds min depth, clamping to min') - next_depth = DEPTH_MIN depths.append(next_depth) + depths = clamp_steps(depths) + + # Sort according to direction + if is_downcast: + depths.sort() + else: + depths.sort(reverse=True) return depths @@ -118,19 +162,6 @@ def on_profile_msg(msg): arm.profiler_peak_depth = arm.latest_profile.depths[argmax] -def continuous_direction(): - direction = rospy.get_param('tasks/continuous_profile/direction') - if direction == 'up': - continuous_start = rospy.get_param('winch/range/max') - continuous_goal = rospy.get_param('winch/range/min') - elif direction == 'down': - continuous_start = rospy.get_param('winch/range/min') - continuous_goal = rospy.get_param('winch/range/max') - else: - raise ValueError(f'Unexpected continuous profile direction: {direction}') - return continuous_start, continuous_goal - - def await_sensor(move_result): duration = rospy.get_param('tasks/dwell_time') rospy.loginfo(f'Waiting {duration} seconds for DC sensor to complete.') diff --git a/src/phyto_arm/src/tests.py b/src/phyto_arm/src/tests.py index 14c187b..06eb569 100755 --- a/src/phyto_arm/src/tests.py +++ b/src/phyto_arm/src/tests.py @@ -13,7 +13,7 @@ # Have not found a ROS-recommended way to import modules being tested sys.path.append(os.path.dirname(os.path.abspath(__file__))) -from arm_chanos import profiler_peak_ready, compute_profiler_steps +from arm_chanos import profiler_peak_ready, compute_profiler_steps, clamp_steps import arm_ifcb from lock_manager import NamedLockManager @@ -66,13 +66,31 @@ def test_profiler_peak_ready(self, mock_rospy, mock_arm): self.assertTrue(result) +class TestClampStemps(unittest.TestCase): + @patch('arm_chanos.arm') + @patch('arm_chanos.rospy') + def test_exceed_max(self, mock_rospy, mock_arm): + mock_arm.latest_profile = Mock() + mock_rospy.get_param.side_effect = [10.0, 0.0] + result = clamp_steps([4.0, 6.0, 11.0]) + self.assertEqual(result, [4.0, 6.0, 10.0]) + + @patch('arm_chanos.arm') + @patch('arm_chanos.rospy') + def test_exceed_min(self, mock_rospy, mock_arm): + mock_arm.latest_profile = Mock() + mock_rospy.get_param.side_effect = [10.0, 5.0] + result = clamp_steps([4.0, 6.0, 10.0]) + self.assertEqual(result, [5.0, 6.0, 10.0]) + + class TestComputeProfilerSteps(unittest.TestCase): @patch('arm_chanos.arm') @patch('arm_chanos.rospy') def test_no_profiler_peak_available(self, mock_rospy, mock_arm): mock_arm.latest_profile = None with self.assertRaises(ValueError) as exc: - compute_profiler_steps() + compute_profiler_steps(True) self.assertEqual(str(exc.exception), 'No profiler peak available') @patch('arm_chanos.arm') @@ -81,16 +99,25 @@ def test_compute_profiler_steps(self, mock_rospy, mock_arm): mock_arm.latest_profile = Mock() mock_arm.profiler_peak_depth = 5.0 mock_rospy.get_param.side_effect = [[-1.0, 1.0, 2.0], 10.0, 0.0] - result = compute_profiler_steps() + result = compute_profiler_steps(True) self.assertEqual(result, [4.0, 6.0, 7.0]) + @patch('arm_chanos.arm') + @patch('arm_chanos.rospy') + def test_compute_profiler_steps_upcast(self, mock_rospy, mock_arm): + mock_arm.latest_profile = Mock() + mock_arm.profiler_peak_depth = 5.0 + mock_rospy.get_param.side_effect = [[-1.0, 1.0, 2.0], 10.0, 0.0] + result = compute_profiler_steps(False) + self.assertEqual(result, [7.0, 6.0, 4.0]) + @patch('arm_chanos.arm') @patch('arm_chanos.rospy') def test_compute_profiler_steps_exceeds_max(self, mock_rospy, mock_arm): mock_arm.latest_profile = Mock() mock_arm.profiler_peak_depth = 9.0 mock_rospy.get_param.side_effect = [[1.0, 2.0], 10.0, 0.0] - result = compute_profiler_steps() + result = compute_profiler_steps(True) self.assertEqual(result, [10.0, 10.0]) @patch('arm_chanos.arm') @@ -99,7 +126,7 @@ def test_compute_profiler_steps_less_than_min(self, mock_rospy, mock_arm): mock_arm.latest_profile = Mock() mock_arm.profiler_peak_depth = 1.0 mock_rospy.get_param.side_effect = [[-2.0, -1.0], 10.0, 0.0] - result = compute_profiler_steps() + result = compute_profiler_steps(True) self.assertEqual(result, [0.0, 0.0]) class TestNamedLockManager(unittest.TestCase):