+ arm.to_angle(5, self.speed, 0.35-goal.command.position)
+ while True:
+ error = goal.command.position - self.lAngles[-1]
+ if abs(error) < 0.02:
+ break
+
+ self._gripper_feedback.position = self.lAngles[-1]
+ self._gripper_feedback.stalled = False
+ self._gripper_feedback.reached_goal = False
+
+ if self._as_gripper.is_preempt_requested():
+ self._as_gripper.set_preempted()
+ break
+ sleep(0.001)
+ self._gripper_result.status = GoalStatus.SUCCEEDED
+ self._gripper_result.result.position = goal.command.position
+ self._gripper_result.result.stalled = False
+ self._gripper_result.result.reached_goal = True
+ self._as_gripper.set_succeeded(self._gripper_result.result)