def execute(self, userdata):
if len(self._screw_list) != self._number_part_screws:
Logger.logerr(
"CheckGripperIsFinished: Screw list length is different ({}) than expected ({})"
.format(self._screw_list, self._number_part_screws))
return 'error'
if len(self._screw_list) > 0:
_done = [] # Screw recognized as done
_todo = [] # Screw recognized as not yet done
_with_issue = [] # An error, gone or wrong torque
_missing = [] # screw is recognized as missing
_first_screw_id = self._selected_grippper * self._number_part_screws
for ss in enumerate(self._screw_list):
n = ss[0] + _first_screw_id
s = ss[1]
Logger.logdebug(
"CheckGripperIsFinished: id {} state is {}".format(n, s))
if s == SCREW_SCREWED:
_done.append(n)
elif s == SCREW_EMPTY:
_missing.append(n)
elif s == SCREW_INSERTED:
_todo.append(n)
elif s == SCREW_WRONG_TORQUE:
_with_issue.append(n)
elif s == SCREW_GONE:
_with_issue.append(n)
else:
Logger.logerr("CheckGripperIsFinished: Exit, screw (" +
str(n) + "), the status " + str(s) +
" is not recognized")
return 'error'
Logger.loginfo(
"CheckGripperIsFinished: Found the following\nDone {}\nTodo {}\nWith probelms={}\n missing={}"
.format(_done, _todo, _with_issue, _missing))
if len(_done) == self._number_part_screws:
Logger.loginfo(
"CheckGripperIsFinished: Exit done {}".format(_done))
return 'done'
elif len(_todo) > 0:
Logger.loginfo(
"CheckGripperIsFinished: Exit screws_available {}".format(
_todo))
return 'screws_available'
elif len(_missing):
Logger.loginfo(
"CheckGripperIsFinished: some screws are missing {}".
format(_missing))
return 'missing_screws'
else:
Logger.loginfo(
"CheckGripperIsFinished: Exit operator_check {}".format(
_with_issue))
return 'operator_check'
def _convert_input_data(self, keys, values):
result = dict()
for k, v in zip(keys, values):
try:
result[k] = self._convert_dict(cast(v))
except ValueError:
# unquoted strings will raise a ValueError, so leave it as string in this case
result[k] = str(v)
except SyntaxError as se:
Logger.logdebug('Unable to parse input value for key "%s", assuming string:\n%s\n%s' % (k, str(v), str(se)))
result[k] = str(v)
return result
def on_enter(self, userdata):
self._reset_keys()
if self.input_id_is_screw:
self.gripper_id = userdata.selected_gripper_id // self.number_part_screws
else:
self.gripper_id = userdata.selected_gripper_id
Logger.loginfo('GripperStateListener: provided gripper id {}'.format(self.gripper_id))
try:
Logger.logdebug('GripperStateListener: Subscribing topic: {}'.format(self.topic_name))
self.sub_gripper_state = rospy.Subscriber(self.topic_name, GripperArray, self._topic_rcvd)
except Exception as e:
self.error_reason = "Error subscribing topic >>>" + str(e) + "<<<"
self.start_time = rospy.get_rostime()
def execute(self, userdata):
try:
Logger.loginfo('Waiting for service /reset_screw ')
reset_screw = rospy.ServiceProxy('/reset_screw', ResetScrew)
gripper_id = int(self._screw_id / self._number_part_screws)
first_screw_id = gripper_id * self._number_part_screws
Logger.logdebug(
'ResetScrewStatesInGripper: going to reset gripper {}'.format(
gripper_id))
resp = []
for ss in enumerate(self._screw_list):
n = ss[0]
s = ss[1]
_abs_id = first_screw_id + n
Logger.loginfo(
"ResetScrewStatesInGripper: Relative id {} state is {}".
format(n + first_screw_id, s))
if s not in COMPLETED_SCREW:
Logger.logdebug(
"ResetScrewStatesInGripper: NOT SCREWED!! id {} reset it"
.format(n + first_screw_id, s))
resp.append((reset_screw(_abs_id), n + first_screw_id, s))
else:
Logger.logdebug(
"ResetScrewStatesInGripper: ALREADY SCREWED!! id {}: let it untouched"
.format(n + first_screw_id, s))
if len(resp) == 0:
userdata.result = True
userdata.reason = 'all screws are screwed'
Logger.loginfo(
'ResetScrewStatesInGripper: EXECUTE done nothing_to_do')
return 'nothing_to_do'
elif all(resp):
userdata.result = True
userdata.reason = 'success'
else:
userdata.result = False
userdata.reason = 'a problem in resetting the screw\'s gripper'
Logger.loginfo(
'ResetScrewStatesInGripper: EXECUTE done succeeded, {}'.format(
resp))
return 'succeeded'
except rospy.ServiceException, e:
userdata.reason = str(e)
Logger.logwarn(
'ResetScrewStatesInGripper: EXECUTE done aborted, reason is: '
+ str(e))
return 'aborted'
