feat: Implements JVL command for zeroing position encoder, calls on w…

…inch startup

src/jvl_motor/CMakeLists.txt

@@ -21,6 +21,7 @@ add_service_files(
   SetPositionEnvelopeCmd.srv
   SetVelocityCmd.srv
   StopCmd.srv
+  ZeroCmd.srv
 )
 
 generate_messages(

src/jvl_motor/src/jvl_motor_node.py

@@ -61,6 +61,14 @@ def write_registers(client, reg_values):
     if not success:
         raise RuntimeError('Failed to write registers')
 
+def cmd_zero_position(client, request):
+    try:
+        write_registers(client, {
+            mac400.P_IST:  0
+        })
+    except RuntimeError:
+        rospy.logerr('Failed to clear position encoder')
+    return srv.ZeroCmdResponse()
 
 def cmd_set_position(client, request):
     try:
@@ -151,6 +159,11 @@ def main():
 
     # Create services
     services = [
+        rospy.Service(
+            '~zero_position',
+            srv.ZeroCmd,
+            functools.partial(cmd_zero_position, client)
+        ),
         rospy.Service(
             '~set_position',
             srv.SetPositionCmd,

src/jvl_motor/srv/ZeroCmd.srv

@@ -0,0 +1,4 @@
+# ZeroCmd
+# Resets the position encoder by zeroing the encoder register.
+
+---

src/phyto_arm/src/winch_node.py

@@ -11,7 +11,7 @@
 
 from ds_sensor_msgs.msg import DepthPressure
 from jvl_motor.msg import Motion
-from jvl_motor.srv import SetPositionEnvelopeCmd, SetVelocityCmd, StopCmd
+from jvl_motor.srv import SetPositionEnvelopeCmd, SetVelocityCmd, StopCmd, ZeroCmd
 
 from phyto_arm.msg import MoveToDepthAction, MoveToDepthFeedback, \
     MoveToDepthResult
@@ -84,6 +84,7 @@ def __getattr__(self, name):
 
 
 # Service proxies for interacting with the motor
+zero_position = rospy.ServiceProxy('motor/zero_position', ZeroCmd)
 set_position_envelope = rospy.ServiceProxy('motor/set_position_envelope',
     SetPositionEnvelopeCmd)
 set_velocity = rospy.ServiceProxy('motor/set_velocity', SetVelocityCmd)
@@ -332,9 +333,12 @@ async def main():
     rospy.core.add_preshutdown_hook(lambda reason: loop.stop())
 
     # Wait for services to become available
-    for svc in [ set_velocity, set_position_envelope, stop ]:
+    for svc in [ zero_position, set_velocity, set_position_envelope, stop ]:
         svc.wait_for_service()
 
+    # Reset the position encoder to zero
+    zero_position()
+
     # Subscribe to incoming messages
     rospy.Subscriber(rospy.get_param('ctd_topic'), DepthPressure, depth.update_soon)
     rospy.Subscriber('motor/motion', Motion, motor.update_soon)