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)