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.