Fixes to get recover service working

qcr · Apr 23, 2024 · e03074f · e03074f
1 parent 41b0feb
commit e03074f
Showing 1 changed file with 1 addition and 9 deletions.
diff --git a/armer/robots/ROSRobot.py b/armer/robots/ROSRobot.py
@@ -1541,19 +1541,11 @@ 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)
+        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.')
-
         return EmptyResponse()
 
     def recover_move_cb(self, req: EmptyRequest) -> EmptyResponse: