feat: Modified arm_chanos to move in complete up-and-down yoyos for e…

…very task

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'
 

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.')

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):