Updated E-Stop to use keepalive client rather than the endpoint. Fixe…

…s a bug where the E-Stop would release immeaditly after being triggered (heuristicus#38)

Co-authored-by: marble-spot <[email protected]>

spot_driver/src/spot_driver/spot_ros.py

@@ -281,6 +281,11 @@ def handle_estop_soft(self, req):
         resp = self.spot_wrapper.assertEStop(False)
         return TriggerResponse(resp[0], resp[1])
 
+    def handle_estop_disengage(self, req):
+        """ROS service handler to disengage the eStop on the robot."""
+        resp = self.spot_wrapper.disengageEStop()
+        return TriggerResponse(resp[0], resp[1])
+
     def handle_clear_bahavior_fault(self, req):
         """ROS service handler for clearing behavior faults"""
         resp = self.spot_wrapper.clear_behavior_fault(req.id)
@@ -545,6 +550,8 @@ def main(self):
 
             rospy.Service("estop/hard", Trigger, self.handle_estop_hard)
             rospy.Service("estop/gentle", Trigger, self.handle_estop_soft)
+            rospy.Service("estop/release", Trigger, self.handle_estop_disengage)
+
 
             rospy.Service("stair_mode", SetBool, self.handle_stair_mode)
             rospy.Service("locomotion_mode", SetLocomotion, self.handle_locomotion_mode)

spot_driver/src/spot_driver/spot_wrapper.py

@@ -414,14 +414,23 @@ def assertEStop(self, severe=True):
         """
         try:
             if severe:
-                self._estop_endpoint.stop()
+                self._estop_keepalive.stop()
             else:
-                self._estop_endpoint.settle_then_cut()
+                self._estop_keepalive.settle_then_cut()
 
             return True, "Success"
         except:
             return False, "Error"
 
+    def disengageEStop(self):
+        """Disengages the E-Stop"""
+        try:
+            self._estop_keepalive.allow()
+            return True, "Success"
+        except:
+            return False, "Error"
+
+
     def releaseEStop(self):
         """Stop eStop keepalive"""
         if self._estop_keepalive: