def run_slot_action(self, goal):
# helper variables
success = True
# publish info to the console for the user
rospy.loginfo('%s: Executing, action %i on item with id %s' % (self._action_name+"_slot_action",
goal.action_type,
goal.item))
# start executing the action
self._slot_feedback.status = 1
item = Item()
if goal.action_type == SlotGoal.ITEM_INSERT:
item.kind = goal.item.ID
item.state.pose = goal.item.pose
item.state.slot_id = goal.slot_id
item.state.status.status = ItemStatus.IN_SLOT
free_target = self.get_free_target(item.kind)
if free_target:
print "free_traget is left over"
item.target = free_target
else:
print "NO free_traget is left over"
self._task_state_list_msg.items.append(item)
elif goal.action_type == SlotGoal.ITEM_REMOVED:
item = self.get_item_with_item_id(goal.item_state.info.id)
item_to_remove = None
for list_item in self._task_state_list_msg.items:
if list_item.status.status == ItemStatus.IN_SLOT and \
list_item.state.slot_id == goal.slot_id and list_item.state.kind == goal.item.ID:
item_to_remove = list_item
if item_to_remove is None:
rospy.logwarn("It was tried to remove a item from Target Tray while no item of kind {0} and in slot {1} can be found".format(goal.item.ID, goal.slot_id))
else:
self._task_state_list_msg.items.remove(item_to_remove)
else:
rospy.logwarn("no action defined")
# self._as_slot.set_rejected()
self._slot_feedback.status = -1
rospy.loginfo("new status of item with name: {0} is: {1}".format(item.target.info.name, item.state.status.status))
self._as_slot.publish_feedback(self._slot_feedback)
self.task_state_publisher.publish(self._task_state_list_msg)
if success and not self._slot_feedback.status == -1:
self._slot_result.result.state = self._task_state_list_msg
rospy.loginfo('%s: Succeeded' % self._action_name+"_slot_action")
self._as_slot.set_succeeded(self._slot_result.result)
else:
self._slot_result.result.state = self._task_state_list_msg
rospy.logwarn('%s: Aborted' % self._action_name+"_slot_action")
self._as_slot.set_aborted(self._slot_result.result)
def run_perception_action(self, goal):
# helper variables
success = True
# publish info to the console for the user
rospy.loginfo('%s: Executing, on items %s' % (self._action_name+"_perception_action", goal.perceived))
self._perception_feedback.status = 1
# remove all item which are not in the final slot
to_remove = []
for item in self._task_state_list_msg.items: # per reference
if item.state.status.status == ItemStatus.DETECTED:
to_remove.append(item)
for elem in to_remove:
self._task_state_list_msg.items.remove(elem)
# add actual perception state
for equipment in goal.perceived:
new_item = Item()
new_item.kind = equipment.ID
new_item.state.pose = equipment.pose
new_item.state.status.status = ItemStatus.DETECTED
# find free target slot
free_target = self.get_free_target(new_item.kind)
if free_target:
print "free_traget is left over"
new_item.target = free_target
else:
print "NO free_traget is left over"
# append to task_state_list
self._task_state_list_msg.items.append(new_item)
self._as_perception.publish_feedback(self._perception_feedback)
self.task_state_publisher.publish(self._task_state_list_msg)
if success:
self._perception_result.result.state = self._task_state_list_msg
rospy.loginfo('%s: Succeeded' % self._action_name+"_perception_action")
self._as_perception.set_succeeded(self._perception_result.result)
