Merge branch 'nathan.figueroa/multi-winch' into rzg/rbr-channels

configs/example.yaml

@@ -133,30 +133,36 @@ arm_ifcb:
 
 arm_chanos:
     tasks:
-        # Time the arm will hold at each position
         dwell_time: 60 # seconds
+        # 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"
         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_steps 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 # meters
+                - -0.25
                 - 0.0
+                - 0.25
                 - 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
+        # Absolute depths to use if profiler_peak not enabeld
+        # Steps will be sorted in direction of movement.
+        default_steps:
+            - 0.5 # meters
+            - 2.0
+            - 3.0
+            - 4.0
+            - 5.0
+            - 6.5
 
     ctd_topic: '/arm_chanos/ctd/depth'
 

phyto-arm

@@ -145,6 +145,7 @@ def _start(args):
         env['ROS_LOG_DIR'] = log_dir
 
     # The following should only launch once, with the main PhytO-ARM processes
+    roscore = None
     if args.launch_name == "main":
         # Before we continue compress any older log files
         #print('Compressing older logs')
@@ -172,7 +173,7 @@ def _start(args):
     # Since we called wait() behind subprocess's back, we need to inform it that
     # the process terminated. A little hacky.
     for proc in [roscore, rosbag, nodes]:
-        if proc.pid == pid:
+        if proc is not None and proc.pid == pid:
             proc.returncode = os.WEXITSTATUS(status) \
                               if os.WIFEXITED(status) else -1
 

src/phyto_arm/launch/mock_arm_ifcb.launch

@@ -9,7 +9,7 @@
 
         <node name="winch" pkg="phyto_arm" type="mock_winch_node.py" />
 
-        <node name="profiler" pkg="phyto_arm" type="profiler_node.py" />
+        <node name="profiler" pkg="phyto_arm" type="mock_profiler_node.py" />
 
         <node name="ifcb_runner" pkg="phyto_arm" type="mock_ifcb_runner.py" />
     </group>

src/phyto_arm/src/arm_base.py

@@ -83,7 +83,7 @@ def send_winch_goal(self, depth, speed, callback):
 
         # 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")}')
+            raise ValueError(f'Move aborted: depth {depth} is below min {rospy.get_param("winch/range/min")}')
         elif depth > rospy.get_param('winch/range/max'):
             raise ValueError(f'Move aborted: depth {depth} is above max {rospy.get_param("winch/range/max")}')
 

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,56 @@ 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', self.start_next_task, 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}')
+
+        # Handle continuous movement cases
+        speed = rospy.get_param('tasks/continuous_speed')
+        if self.is_downcasting and rospy.get_param('tasks/downcast_type') == 'continuous':
+            return Task('downcast_continuous', self.start_next_task, rospy.get_param('winch/range/max'), speed)
+        elif not self.is_downcasting and rospy.get_param('tasks/upcast_type') == 'continuous':
+            return Task('upcast_continuous', self.start_next_task, rospy.get_param('winch/range/min'), speed)
+
+        # If profiler peak is enabled and ready, execute new set of profile steps
+        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:
+            self.steps = compute_absolute_steps(self.is_downcasting)
+            next_depth = self.steps.pop(0)
+            return Task('downcast_step', await_sensor, next_depth)
+        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 +101,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_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 +161,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/mocks/mock_profiler_node.py

@@ -0,0 +1,66 @@
+#!/usr/bin/env python3
+import rospy
+import numpy as np
+from scipy.stats import norm
+
+from ds_sensor_msgs.msg import DepthPressure
+from phyto_arm.msg import MoveToDepthActionGoal, MoveToDepthActionResult, DepthProfile
+
+class MockProfiler:
+    def __init__(self):
+        self.depth_sub = rospy.Subscriber(rospy.get_param('ctd_topic'), DepthPressure, self.depth_callback)
+        self.action_goal_sub = rospy.Subscriber("winch/move_to_depth/goal", MoveToDepthActionGoal, self.action_goal_callback)
+        self.action_result_sub = rospy.Subscriber("winch/move_to_depth/result", MoveToDepthActionResult, self.action_result_callback)
+
+        self.profile_pub = {
+            'all': rospy.Publisher('~', DepthProfile, queue_size=1),
+            'down': rospy.Publisher('~downcast', DepthProfile, queue_size=1),
+            'up': rospy.Publisher('~upcast', DepthProfile, queue_size=1),
+        }
+
+        self.depths = []
+        self.is_recording = False
+
+    def depth_callback(self, data):
+        if self.is_recording:
+            self.depths.append(data.depth)
+
+    def action_goal_callback(self, data):
+        rospy.loginfo("Received action goal, starting recording")
+        self.is_recording = True
+        self.depths = []
+
+    def action_result_callback(self, data):
+        rospy.loginfo("Received action result, stopping recording")
+        self.is_recording = False
+
+        # Generate and publish mocked DepthProfile messages
+        profile_msg = DepthProfile()
+        profile_msg.header.stamp = rospy.Time.now()
+        profile_msg.goal_uuid = data.result.uuid
+        profile_msg.data_topic = rospy.get_param('ctd_topic')
+        profile_msg.data_field = "mocked_values"
+        profile_msg.depths = self.depths
+
+        # Generate mocked values using a bell curve
+        mean = 5.0  # Mean depth of the bell curve
+        std_dev = 1.5  # Standard deviation of the bell curve
+        profile_msg.values = norm.pdf(self.depths, mean, std_dev).tolist()
+
+        self.profile_pub['all'].publish(profile_msg)
+        if self.depths[0] < self.depths[-1]:
+            self.profile_pub['down'].publish(profile_msg)
+        else:
+            self.profile_pub['up'].publish(profile_msg)
+
+        rospy.loginfo("Published mocked DepthProfile")
+
+def main():
+    rospy.init_node('mock_profiler_node')
+    rospy.logwarn(f'Starting mock profiler node {rospy.get_name()}')
+
+    profiler = MockProfiler()
+    rospy.spin()
+
+if __name__ == '__main__':
+    main()