def _generate_feedback_msg(self, goal):
feedback = GripperCommandFeedback()
feedback.position = self._stat.position
feedback.effort = 0
feedback.stalled = False
feedback.reached_goal = abs(self._stat.position - goal) < self._grip_threshold
return feedback
def _feedback_msg(self, position_target, position_actual, effort_actual,
stalled):
feedback = GripperCommandFeedback()
feedback.position = self.position_to_joint_conv(position_actual)
feedback.effort = self.force_to_effort_conv(effort_actual)
feedback.stalled = stalled
feedback.reached_goal = False
return feedback
def _generate_feedback_msg(self, goal):
feedback = GripperCommandFeedback()
feedback.position = self._stat.position
feedback.effort = 0
feedback.stalled = False
feedback.reached_goal = abs(self._stat.position -
goal) < GRIP_THRESHOLD
return feedback
def actionCb(self, goal):
result = GripperCommandResult()
feedback = GripperCommandFeedback()
""" Take an input command of width to open gripper. """
rospy.loginfo('Gripper Controller action goal received: position %f, max_effort %f',
goal.command.position, goal.command.max_effort)
# send command to gripper
if not self.model.setCommand(goal.command):
self.server.set_aborted()
rospy.loginfo('Gripper Controller: Aborted.')
return
# register progress so we can guess if the gripper is stalled; our buffer
# must contain up to: stalled_time / joint_states period position values
if len(self.state_cb_times) == self.state_cb_times.maxlen:
T = sum(self.state_cb_times[i] - self.state_cb_times[i-1] \
for i in xrange(1, len(self.state_cb_times) - 1)) \
/(len(self.state_cb_times) - 1)
progress = collections.deque(maxlen=round(self.stalled_time/T))
else:
self.server.set_aborted()
rospy.logerr('Gripper Controller: no messages from joint_states topic received')
return
diff_at_start = round(abs(goal.command.position - self.current_position), 3)
# keep watching for gripper position...
while True:
if self.server.is_preempt_requested():
self.server.set_preempted()
rospy.loginfo('Gripper Controller: Preempted.')
return
# synchronize with the joints state callbacks;
self.state_cb_event.wait()
self.state_cb_event.clear()
# break when we have reached the goal position...
diff = abs(goal.command.position - self.current_position)
if diff < self._EPSILON_OPENING_DIFF_:
result.reached_goal = True
break
# ...or when the gripper is exerting beyond max effort
if goal.command.max_effort and self.current_effort >= goal.command.max_effort:
# stop moving to prevent damaging the gripper
goal.command.position = self.current_position
self.model.setCommand(goal.command)
rospy.logerr('Gripper Controller: max effort reached at position %f (%f >= %f)',
self.current_position, self.current_effort, goal.command.max_effort)
result.stalled = True
break
# ...or when progress stagnates, probably signaling that the gripper is exerting max effort and not moving
progress.append(round(diff, 3)) # round to millimeter to neglect tiny motions of the stalled gripper
if len(progress) == progress.maxlen and progress.count(progress[0]) == len(progress):
if progress[0] == diff_at_start:
# we didn't move at all! is the gripper connected?
self.server.set_aborted()
rospy.logerr('Gripper Controller: gripper not moving; is the servo connected?')
return
# buffer full with all-equal positions -> gripper stalled
goal.command.position = self.current_position
self.model.setCommand(goal.command)
rospy.logerr('Gripper Controller: gripper stalled at position %f', self.current_position)
result.stalled = True
break
# publish feedback
feedback.position = self.current_position
feedback.effort = self.current_effort
self.server.publish_feedback(feedback)
# publish one last feedback and the result (identical)
feedback.position = self.current_position
feedback.effort = self.current_effort
feedback.reached_goal = result.reached_goal
feedback.stalled = result.stalled
self.server.publish_feedback(feedback)
result.position = self.current_position
result.effort = self.current_effort
self.server.set_succeeded(result)
rospy.loginfo('Gripper Controller: Succeeded (%s)', 'goal reached' if result.reached_goal else 'stalled')