feat: Better speed handling

fix: Task callbacks require positional arg for result

fix: remove peak expiration, respect esp update frequency

src/phyto_arm/src/arm_base.py

@@ -129,6 +129,12 @@ def get_next_task(self, last_task):
     def start_next_task(self, move_result=None):
         self.task_lock.release()
 
+    def publish_task(self, name, depth = 0, speed = 0):
+        task_msg = ArmTask()
+        task_msg.name = name
+        task_msg.depth = depth
+        task_msg.speed = speed
+        self.task_pub.publish(task_msg)
 
     # Primary loop that should be the same for all arms, override should not be needed
     def loop(self):
@@ -140,6 +146,7 @@ def loop(self):
                 if self.winch_client is None:
                     task = self.get_next_task(task)
                     rospy.logwarn(f'No winch; running {task.name}')
+                    self.publish_task(task.name)
                     task.callback()
 
 
@@ -163,5 +170,8 @@ def loop(self):
                     # max_moving_winches limit.
 
                     task = self.get_next_task(task)
-                    rospy.logwarn(f'Goal depth {task.depth} for task {task.name}')
-                    self.send_winch_goal(task.depth, task.speed, task.callback)
+                    speed = task.speed or rospy.get_param('winch/max_speed')
+                    rospy.logwarn(f'Goal depth {task.depth}, speed {speed} for task {task.name}')
+
+                    self.publish_task(task.name, task.depth, speed)
+                    self.send_winch_goal(task.depth, speed, task.callback)

src/phyto_arm/src/arm_esp.py

@@ -10,7 +10,6 @@ class ArmESP(ArmBase):
     latest_profile = None
     profiler_peak_value = None
     profiler_peak_depth = None
-    last_update_time = None
 
     steps = []
 
@@ -28,7 +27,8 @@ def get_next_task(self, last_task):
         # 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():
             peak_depth = clamped_profiler_peak()
-            return Task("profiler_peak", wait_seconds(update_freq), peak_depth)
+            esp_update_freq = rospy.get_param('tasks/profiler_peak/update_frequency')
+            return Task("profiler_peak", wait_seconds(esp_update_freq), peak_depth)
 
         # By default hold and wait a second
         return Task('default_position_wait', wait_seconds(5), rospy.get_param('tasks/default_depth'))
@@ -46,14 +46,6 @@ def profiler_peak_ready():
         rospy.logwarn(f'Profiler peak value {arm.profiler_peak_value} is below threshold')
         return False
 
-    # Check that last peak hasn't passed expiration window
-    update_freq = rospy.Duration(rospy.get_param('tasks/profiler_peak/update_frequency'))
-
-    if arm.last_update_time is None or arm.last_update_time + update_freq < rospy.Time.now():
-        rospy.logwarn(f'Profiler peak expired at {expiration_time}')
-        arm.last_update_time = rospy.Time.now()
-        return False
-
     return True
 
 
@@ -82,7 +74,7 @@ def on_profile_msg(msg):
 
 
 def wait_seconds(duration):
-    def awaiter():
+    def awaiter(move_result):
         def timer_callback(event):
             if rospy.is_shutdown():
                 return