Updated the standard recovery method (service)

qcr · Apr 23, 2024 · 41b0feb · 41b0feb
1 parent b69a286
commit 41b0feb
Showing 1 changed file with 11 additions and 6 deletions.
diff --git a/armer/robots/ROSRobot.py b/armer/robots/ROSRobot.py
@@ -1541,13 +1541,18 @@ def recover_cb(self, req: EmptyRequest) -> EmptyResponse: # pylint: disable=no-s
         :return: an empty response
         :rtype: EmptyResponse
         """
+        #self.general_executor(q=self.q, collision_ignore=True, workspace_ignore=True)
+        rospy.loginfo(f"[RECOVER CB] -> Resetting from {self._controller_mode} state to JOINTS [Default]")
+        self._controller_mode = ControlMode.JOINTS
+        self.preempted = False
+
         # Recover from Error if set (basic functionality)
-        if self._controller_mode == ControlMode.ERROR:
-            rospy.loginfo(f"[RECOVER CB] -> Resetting from ERROR state to JOINTS [Default]")
-            self._controller_mode = ControlMode.JOINTS
-            self.preempted = False
-        else:
-            rospy.logwarn(f'RECOVER CB] -> Robot [{self.name}] not in ERROR state. Do Nothing.')
+        #if self._controller_mode == ControlMode.ERROR:
+        #    rospy.loginfo(f"[RECOVER CB] -> Resetting from ERROR state to JOINTS [Default]")
+        #    self._controller_mode = ControlMode.JOINTS
+        #    self.preempted = False
+        #else:
+        #    rospy.logwarn(f'RECOVER CB] -> Robot [{self.name}] not in ERROR state. Do Nothing.')
 
         return EmptyResponse()