From 01de41fe1fb7524edde0333e843a0dd1d94e526f Mon Sep 17 00:00:00 2001 From: Nathan Figueroa Date: Fri, 29 Mar 2024 04:19:21 +0000 Subject: [PATCH 1/6] feat: Modified arm_chanos to move in complete up-and-down yoyos for every task --- configs/example.yaml | 37 ++++---- src/phyto_arm/src/arm_chanos.py | 151 +++++++++++++++++++------------- src/phyto_arm/src/tests.py | 37 ++++++-- 3 files changed, 144 insertions(+), 81 deletions(-) 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): From 955bc72a68718b2aee025070dbd0f9f483aca78f Mon Sep 17 00:00:00 2001 From: Nathan Figueroa Date: Fri, 29 Mar 2024 05:04:28 +0000 Subject: [PATCH 2/6] test: Mock node for profiler --- src/phyto_arm/launch/mock_arm_ifcb.launch | 2 +- src/phyto_arm/src/mocks/mock_profiler_node.py | 66 +++++++++++++++++++ 2 files changed, 67 insertions(+), 1 deletion(-) create mode 100755 src/phyto_arm/src/mocks/mock_profiler_node.py diff --git a/src/phyto_arm/launch/mock_arm_ifcb.launch b/src/phyto_arm/launch/mock_arm_ifcb.launch index 83c0301..54bfaae 100644 --- a/src/phyto_arm/launch/mock_arm_ifcb.launch +++ b/src/phyto_arm/launch/mock_arm_ifcb.launch @@ -9,7 +9,7 @@ - + diff --git a/src/phyto_arm/src/mocks/mock_profiler_node.py b/src/phyto_arm/src/mocks/mock_profiler_node.py new file mode 100755 index 0000000..f2983cb --- /dev/null +++ b/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() From 2c8d079d79b60904e3b2ca1c887c18ecfad2d15c Mon Sep 17 00:00:00 2001 From: Nathan Figueroa Date: Fri, 29 Mar 2024 05:10:08 +0000 Subject: [PATCH 3/6] fix: Added missing dwell time value from config --- configs/example.yaml | 5 +++-- src/phyto_arm/src/arm_chanos.py | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/configs/example.yaml b/configs/example.yaml index 4a3530b..4f4560f 100644 --- a/configs/example.yaml +++ b/configs/example.yaml @@ -133,6 +133,7 @@ arm_ifcb: arm_chanos: tasks: + dwell_time: 60 # seconds profiler_peak: # If true, will use the profiler to find the peak value # Falls back on default_movement values otherwise @@ -147,7 +148,7 @@ arm_chanos: - -0.25 - 0.0 - 0.25 - - 0.5 + - 0.5 default_movement: # Steps will be sorted in direction of movement. absolute_steps: @@ -161,7 +162,7 @@ arm_chanos: continuous_speed: 0.005 # must be "stepped" or "continuous" downcast_type: "stepped" - upcast_type: "continuous" + 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 10c467e..dc11bb2 100755 --- a/src/phyto_arm/src/arm_chanos.py +++ b/src/phyto_arm/src/arm_chanos.py @@ -30,7 +30,7 @@ def get_next_task(self, last_task): # 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')) + 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: From 60b3bd081af0f0fd78b0fcd4bbcfda418c73ea92 Mon Sep 17 00:00:00 2001 From: Nathan Figueroa Date: Fri, 29 Mar 2024 05:31:59 +0000 Subject: [PATCH 4/6] fix: non-main launches no longer crash trying to shut down roscore --- phyto-arm | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/phyto-arm b/phyto-arm index 69ce660..7ac27b7 100755 --- a/phyto-arm +++ b/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 From a5b529b580ca87df4ae2f2918917c2dd9ea8420b Mon Sep 17 00:00:00 2001 From: Nathan Figueroa Date: Mon, 1 Apr 2024 18:02:35 +0000 Subject: [PATCH 5/6] fix: Changed RBR back to parsing 16 fields --- src/rbr_maestro3_ctd/src/rbr_maestro3_node.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/rbr_maestro3_ctd/src/rbr_maestro3_node.py b/src/rbr_maestro3_ctd/src/rbr_maestro3_node.py index 636fa7f..64886e0 100755 --- a/src/rbr_maestro3_ctd/src/rbr_maestro3_node.py +++ b/src/rbr_maestro3_ctd/src/rbr_maestro3_node.py @@ -66,8 +66,8 @@ async def main(): # Crummy parser fields = [ x.strip() for x in msg.data.split(b',') ] - if len(fields) != 15: - rospy.logwarn(f'Discarding malformed RBR message - expected 15 fields but received {len(fields)}') + if len(fields) != 16: + rospy.logwarn(f'Discarding malformed RBR message - expected 16 fields but received {len(fields)}') continue try: From 8c780b6cfafe9b171d4299029f42e6a596d39fd9 Mon Sep 17 00:00:00 2001 From: Nathan Figueroa Date: Mon, 1 Apr 2024 20:01:25 +0000 Subject: [PATCH 6/6] feat: cast type now applied regardless of profile peak. Fix log message in arm_base. --- configs/example.yaml | 32 ++++++++++++++++---------------- src/phyto_arm/src/arm_base.py | 2 +- src/phyto_arm/src/arm_chanos.py | 33 ++++++++++++++++----------------- 3 files changed, 33 insertions(+), 34 deletions(-) diff --git a/configs/example.yaml b/configs/example.yaml index 4f4560f..b8a8509 100644 --- a/configs/example.yaml +++ b/configs/example.yaml @@ -134,9 +134,14 @@ arm_ifcb: arm_chanos: tasks: 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 default_movement values 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 @@ -144,25 +149,20 @@ arm_chanos: peak_expiration: 60 # Stops to be made relative to profiler_peak offset_steps: - - -0.5 + - -0.5 # meters - -0.25 - 0.0 - 0.25 - 0.5 - default_movement: - # Steps will be sorted in direction of movement. - absolute_steps: - - 0.5 - - 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" + # 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' diff --git a/src/phyto_arm/src/arm_base.py b/src/phyto_arm/src/arm_base.py index 5879907..7c015cc 100755 --- a/src/phyto_arm/src/arm_base.py +++ b/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")}') diff --git a/src/phyto_arm/src/arm_chanos.py b/src/phyto_arm/src/arm_chanos.py index dc11bb2..e468995 100755 --- a/src/phyto_arm/src/arm_chanos.py +++ b/src/phyto_arm/src/arm_chanos.py @@ -53,30 +53,29 @@ def get_next_task(self, last_task): else: raise ValueError(f'Unexpected last task name: {last_task.name}') - # If profiler peak is enabled and ready, execute profiler peak + # 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: - 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) + 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) + self.steps = compute_absolute_steps(self.is_downcasting) + next_depth = self.steps.pop(0) + return Task('upcast_step', await_sensor, next_depth) # Global reference to arm state @@ -119,7 +118,7 @@ def clamp_steps(steps): def compute_absolute_steps(is_downcast): - steps = rospy.get_param('tasks/default_movement/absolute_steps') + steps = rospy.get_param('tasks/default_steps') steps = clamp_steps(steps) # Sort according to direction.