def publish_feedback(self, action_id, fbstatus):
     """
     Function to publish action feedback to action_feedback topic
     """
     feedback = ActionFeedback()
     feedback.action_id = action_id
     feedback.status = fbstatus
     self._feedback_publisher.publish(feedback)
 def update_success(self):
     # GENERATED CODE
     parametersDic = self.toDictionary(self.current_action.parameters)
     self.changeKMSFact("k_can_loc", [], KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE)
     # END
     actionFeedback = ActionFeedback()
     actionFeedback.action_id = self.current_action.action_id
     actionFeedback.status = "action achieved"
     self.action_feedback_pub.publish(actionFeedback)
 def update_success(self):
     # Update the effects in the KMS
     parametersDic = self.toDictionary(self.current_action.parameters)
     self.changeKMSFact("k_gateway_location", [["gw", parametersDic["gateway"]]], KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE)
     # Update the Planning System on failure
     actionFeedback = ActionFeedback()
     actionFeedback.action_id = self.current_action.action_id
     actionFeedback.status = "action achieved"
     self.action_feedback_pub.publish(actionFeedback)
 def update_success(self):
     # GENERATED CODE
     parametersDic = self.toDictionary(self.current_action.parameters)
     self.changeKMSFact("coffee_at", [["loc", parametersDic["curr_loc"]]], KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE)
     self.changeKMSFact("grabbed_cup", [], KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE)
     self.changeKMSFact("arm_free", [], KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE)
     # END
     actionFeedback = ActionFeedback()
     actionFeedback.action_id = self.current_action.action_id
     actionFeedback.status = "action achieved"
     self.action_feedback_pub.publish(actionFeedback)
Exemple #5
0
 def update_success(self):
     # Update the effects in the KMS
     parametersDic = self.toDictionary(self.current_action.parameters)
     self.changeKMSFact("k_gateway_location",
                        [["gw", parametersDic["gateway"]]],
                        KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE)
     # Update the Planning System on failure
     actionFeedback = ActionFeedback()
     actionFeedback.action_id = self.current_action.action_id
     actionFeedback.status = "action achieved"
     self.action_feedback_pub.publish(actionFeedback)
Exemple #6
0
    def _action_feedback_from_state(self, action, state):
        success = (state == GoalStatus.SUCCEEDED)

        # Update world state
        update_request = ActionFinishedRequest(action=action.msg,
                                               success=success)
        self.action_finished_update(update_request)

        # Feedback to rosplan
        feedback_msg = ActionFeedback()
        feedback_msg.action_id = action.action_id
        feedback_msg.status = "action achieved" if success else "action failed"
        self.feedback_pub.publish(feedback_msg)
 def update_success(self):
     # Update the effects in the KMS
     parametersDic = self.toDictionary(self.current_action.parameters)
     self.changeKMSFact("at", [["par", parametersDic["areab"]]], KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE)
     instance_query_client = rospy.ServiceProxy("/kcl_rosplan/get_current_instances", GetInstanceService)
     forAllInstances = instance_query_client.call("object").instances
     for forAllInstance in forAllInstances:
         self.changeKMSFact("k_gateway_location", [["gw", parametersDic["gw"] if "gw" in parametersDic else forAllInstance]], KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE)
     # Update the Planning System on failure
     actionFeedback = ActionFeedback()
     actionFeedback.action_id = self.current_action.action_id
     actionFeedback.status = "action achieved"
     self.action_feedback_pub.publish(actionFeedback)
 def update_success(self):
     # GENERATED CODE
     parametersDic = self.toDictionary(self.current_action.parameters)
     self.changeKMSFact("robot_at_floor", [["floor", parametersDic["curr_floor"]]], KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE)
     self.changeKMSFact("robot_at_floor", [["floor", parametersDic["goal_floor"]]], KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE)
     self.changeKMSFact("robot_at", [["loc", parametersDic["floor_exit"]]], KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE)
     self.changeKMSFact("robot_in_elevator", [["elevator", parametersDic["elevator"]]], KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE)
     self.changeKMSFact("ordered", [["elevator", parametersDic["elevator"]]], KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE)
     # END
     actionFeedback = ActionFeedback()
     actionFeedback.action_id = self.current_action.action_id
     actionFeedback.status = "action achieved"
     self.action_feedback_pub.publish(actionFeedback)
 def update_success(self):
     # GENERATED CODE
     parametersDic = self.toDictionary(self.current_action.parameters)
     self.changeKMSFact("robot_at", [["loc", parametersDic["curr_loc"]]], KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE)
     self.changeKMSFact("robot_at", [["loc", parametersDic["loc"]]], KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE)
     instance_query_client = rospy.ServiceProxy("/kcl_rosplan/get_current_instances", GetInstanceService)
     forAllInstances = instance_query_client.call("obj").instances
     for forAllInstance in forAllInstances:
     	self.changeKMSFact("k_button_loc", [["button", parametersDic["e"] if "e" in parametersDic else forAllInstance]], KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE)
     self.changeKMSFact("k_can_loc", [], KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE)
     # END
     actionFeedback = ActionFeedback()
     actionFeedback.action_id = self.current_action.action_id
     actionFeedback.status = "action achieved"
     self.action_feedback_pub.publish(actionFeedback)
Exemple #10
0
def action_reciever(msg):
    actions = {}
    for act in Action.__subclasses__():
        actions[act.name or act.__name__] = act
    if msg.name in actions:
        try:
            action = actions[msg.name](msg.action_id,
                                       msg.dispatch_time,
                                       feedback,
                                       keyval_to_dict(msg.parameters))
            ids[msg.action_id] = action
            action.start(**keyval_to_dict(msg.parameters))
            action.report_success()
        except Exception as e:
            rospy.logwarn("Action '%s' failed." % msg.name, exc_info=1)
            feedback.publish(ActionFeedback(msg.action_id,
                                            "action failed",
                                            dict_to_keyval(None)))
    elif msg.name == 'cancel_action':
        if msg.action_id in ids:
            ids[msg.action_id].cancel()
    elif msg.name == 'pause_action':
        if msg.action_id in ids:
            ids[msg.action_id].pause()
    elif msg.name == 'resume_action':
        if msg.action_id in ids:
            ids[msg.action_id].resume()
Exemple #11
0
 def action_follow(self, action):
     rospy.loginfo('/coordinator/action_follow for %s', action.agent)
     action_id = action.action_id
     feedback_msg = ActionFeedback(action_id=action_id,
                                   status="action enabled")
     ac = self.clients[action.agent].actions['follow']
     ac.send_goal(BebopFollowGoal())
     return ac
Exemple #12
0
 def action_takeoff(self, action):
     rospy.loginfo('/coordinator/action_takeoff for %s', action.agent)
     action_id = action.action_id
     feedback_msg = ActionFeedback(action_id=action_id,
                                   status="action enabled")
     ac = self.clients[action.agent].actions['takeoff']
     ac.send_goal(BebopTakeOffGoal())
     return ac
Exemple #13
0
 def action_load(self, action):
     rospy.loginfo('/coordinator/action_load for %s. Action ID: %f',
                   action.agent, action.action_id)
     action_id = action.action_id
     feedback_msg = ActionFeedback(action_id=action_id,
                                   status="action enabled")
     ac = self.clients[action.agent].actions['load']
     ac.send_goal(BebopLoadGoal())
     return ac
 def update_success(self):
     # Update the effects in the KMS
     parametersDic = self.toDictionary(self.current_action.parameters)
     self.changeKMSFact("at", [["par", parametersDic["areab"]]],
                        KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE)
     instance_query_client = rospy.ServiceProxy(
         "/kcl_rosplan/get_current_instances", GetInstanceService)
     forAllInstances = instance_query_client.call("object").instances
     for forAllInstance in forAllInstances:
         self.changeKMSFact("k_gateway_location", [[
             "gw", parametersDic["gw"]
             if "gw" in parametersDic else forAllInstance
         ]], KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE)
     # Update the Planning System on failure
     actionFeedback = ActionFeedback()
     actionFeedback.action_id = self.current_action.action_id
     actionFeedback.status = "action achieved"
     self.action_feedback_pub.publish(actionFeedback)
Exemple #15
0
    def update_success(self):
        parametersDic = self.toDictionary(self.current_action.parameters)
        self.changeKMSFact("robot_at", [["loc", parametersDic["curr_loc"]]],
                           KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE)
        self.changeKMSFact("robot_at", [["loc", parametersDic["loc"]]],
                           KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE)
        instance_query_client = rospy.ServiceProxy(
            "/kcl_rosplan/get_current_instances", GetInstanceService)
        forAllInstances = instance_query_client.call("obj").instances
        for forAllInstance in forAllInstances:
            self.changeKMSFact("k_button_loc", [[
                "button",
                parametersDic["e"] if "e" in parametersDic else forAllInstance
            ]], KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE)
        self.changeKMSFact("k_can_loc", [],
                           KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE)

        actionFeedback = ActionFeedback()
        actionFeedback.action_id = self.current_action.action_id
        actionFeedback.status = "action achieved"
        self.action_feedback_pub.publish(actionFeedback)
Exemple #16
0
def _action_receiver(msg):

    global action_ids
    actions = _list_actions()

    rospy.loginfo("[RPpt][AIF] a message is received: '%d', '%s'" %
                  (msg.action_id, msg.name))
    rospy.loginfo(actions)

    if msg.name in actions:

        action_name = msg.name
        rospy.loginfo("[RPpt][AIF] start '%s' action" % action_name)

        try:
            action = actions[action_name](msg.action_id, msg.dispatch_time,
                                          feedback,
                                          keyval_to_dict(msg.parameters))
            action_ids[msg.action_id] = action

            if issubclass(action.__class__, ActionSink):
                action.execute(action_name, **keyval_to_dict(msg.parameters))
            else:
                action.execute(**keyval_to_dict(msg.parameters))

        except Exception as e:
            rospy.logwarn("[RPpt][AIF] action '%s' failed." % msg.name,
                          exc_info=1)
            feedback.publish(
                ActionFeedback(msg.action_id, "action failed",
                               dict_to_keyval(None)))
    elif msg.action_id in action_ids:

        operation = msg.name
        rospy.loginfo("[RPpt][AIF] update action '%d', doing '%s'" %
                      (msg.action_id, operation))

        if operation == "cancel_action":
            action_ids[msg.action_id].cancel()
        elif operation == "pause_action":
            action_ids[msg.action_id].pause()
        elif operation == "resume_action":
            action_ids[msg.action_id].resume()

    else:
        rospy.loginfo("[RPpt][AR] no operation")
Exemple #17
0
    def action_goto(self, action):
        rospy.loginfo('/coordinator/action_goto for %s (action ID: %f)',
                      action.agent, action.action_id)
        parameters = action.parameters
        action_id = action.action_id
        agent = action.agent
        wp = action.wp

        ac = None
        if agent in rospy.get_param('/available_drones'):
            goal = BebopMoveBaseGoal()
            ac = self.clients[action.agent].actions['goto']
        elif agent in rospy.get_param('/available_turtlebots'):
            goal = MoveBaseGoal()
            ac = self.clients[agent].actions['goto']
        if ac == None:
            raise CoordinatorError("No action client exist for agent %s" %
                                   agent)

        if not ac.wait_for_server(timeout=rospy.Duration(10)):
            rospy.loginfo("server timeout")

    #Get waypoint position from world state node
        p_rsp = self.get_waypoint_position(wp=wp)
        if not p_rsp.valid:
            raise CoordinatorError("No valid position for waypoint %s" % wp)

        goal.target_pose.header.frame_id = self.coordinate_frame
        goal.target_pose.header.stamp = rospy.Time.now()
        goal.target_pose.pose.position.x = p_rsp.x
        goal.target_pose.pose.position.y = p_rsp.y
        goal.target_pose.pose.orientation.w = 1

        # Notify action dispatcher of status
        feedback_msg = ActionFeedback(action_id=action_id,
                                      status="action enabled")
        self.feedback_pub.publish(feedback_msg)

        ac.send_goal(goal)
        return ac
    def handle_put_down(self, put_down_msg):
        fname = "{}::{}".format(self.__class__.__name__, self.handle_put_down.__name__)

        if (not self.ready):
            rospy.loginfo("{}: Pick&Place Component not ready to handle commands".format(fname))
            return

        rospy.loginfo("{}: Handling put down action".format(fname))

        (block_name, on_block_name) = self.get_parameter_values(put_down_msg.parameters, "block", "on_block")

        if (block_name is None or on_block_name is None):
            rospy.logerr("{}: Failed to find block parameters in put down message".format(fname))
            return

        query_result = self.message_store.query_named(on_block_name, "komodo_blockworld/BlockPos", False)
        if (query_result is None):
            rospy.logfatal("{}: Failed to query DB. Aborting...".format(fname))
            return

        else:
            if (len(query_result) != 1):
                rospy.logerr("{}: Found {} blocks named {}. Aborting...".format(fname, len(query_result), block_name))
                return

            on_block_pos = query_result[0][0]

            actionFeedback = ActionFeedback()
            actionFeedback.action_id = put_down_msg.action_id
            actionFeedback.status = "action enabled"
            self.action_feedback_pub.publish(actionFeedback)

            self.position_arm(on_block_pos.tablePos, on_block_pos.level + 1.5)

            self.release_grip()

            # Re-grip the block at a slightly lower position to stabilize the stack
            self.position_arm(on_block_pos.tablePos, on_block_pos.level + 1.1)
            self.grip_block(on_block_pos.level + 1)
            #rospy.sleep(2)
            self.release_grip()
            #rospy.sleep(0.2)

            self.rise_arm()

            self.apply_effects_to_KMS(block_name, on_block_name, put_down_msg.name)

            query_response = self.update_block_pos(block_name, on_block_pos.level + 1, on_block_pos.tablePos)
            if (query_response.success is False):
                rospy.logerr("{}: Failed to updat block {} position to {} {}".
                             format(fname, block_name, on_block_pos.level + 1, on_block_pos.tablePos))
                actionFeedback = ActionFeedback()
                actionFeedback.action_id = put_down_msg.action_id
                actionFeedback.status = "action failed"
                self.action_feedback_pub.publish(actionFeedback)
            else:
                rospy.logdebug("{}: Updated block {} position to {} {}".
                               format(fname, block_name, on_block_pos.level + 1, on_block_pos.tablePos))
                actionFeedback = ActionFeedback()
                actionFeedback.action_id = put_down_msg.action_id
                actionFeedback.status = "action achieved"
                self.action_feedback_pub.publish(actionFeedback)
Exemple #19
0
    def run_thread(self, dispatch_msg):

        rospy.loginfo(
            'KCL: ({}) Plan {} Action {}: State machine {} running on thread {}'
            .format(rospy.get_name(), dispatch_msg.plan_id,
                    dispatch_msg.action_id, self.get_action_name(),
                    threading.currentThread()))

        # Extend the dictionaries if needed
        if dispatch_msg.plan_id not in self._running_state:
            self._running_state[dispatch_msg.plan_id] = {}
            self._transition_value[dispatch_msg.plan_id] = {}
            self._to_start_state[dispatch_msg.plan_id] = {}
            self._transitions_counters[dispatch_msg.plan_id] = {}
        if dispatch_msg.action_id not in self._running_state[
                dispatch_msg.plan_id]:

            self._running_state[dispatch_msg.plan_id][
                dispatch_msg.action_id] = None
            self._transition_value[dispatch_msg.plan_id][
                dispatch_msg.action_id] = None
            self._to_start_state[dispatch_msg.plan_id][
                dispatch_msg.action_id] = False
            self._transitions_counters[dispatch_msg.plan_id][
                dispatch_msg.action_id] = {}
            self._transitions_counters[dispatch_msg.plan_id][
                dispatch_msg.action_id]["succeeded"] = 0
            self._transitions_counters[dispatch_msg.plan_id][
                dispatch_msg.action_id]["failed"] = 0

        # Start running the first basic action
        self._running_state[dispatch_msg.plan_id][
            dispatch_msg.action_id] = self._states["ba1"]
        action_interface = self._running_state[dispatch_msg.plan_id][
            dispatch_msg.action_id].get_action_interface()
        action_interface.run(dispatch_msg)

        # Execute the fsm
        fsm_execution_completed = False
        while not fsm_execution_completed:

            transition_value = self._transition_value[dispatch_msg.plan_id][
                dispatch_msg.action_id]

            if not transition_value == None:
                # Reset the transition value
                self._transition_value[dispatch_msg.plan_id][
                    dispatch_msg.action_id] = None

                # Based on the passed transition value get the transition and modify the counters accordingly
                transition = None
                if transition_value == "succeeded":
                    transition_counter = self._transitions_counters[
                        dispatch_msg.plan_id][
                            dispatch_msg.action_id]["succeeded"]
                    transition = self._running_state[dispatch_msg.plan_id][
                        dispatch_msg.action_id].get_transition(
                            "succeeded", transition_counter)
                    self._transitions_counters[dispatch_msg.plan_id][
                        dispatch_msg.action_id]["succeeded"] += 1
                    self._transitions_counters[dispatch_msg.plan_id][
                        dispatch_msg.action_id]["failed"] = 0

                if transition_value == "failed":
                    transition_counter = self._transitions_counters[
                        dispatch_msg.plan_id][dispatch_msg.action_id]["failed"]
                    transition = self._running_state[dispatch_msg.plan_id][
                        dispatch_msg.action_id].get_transition(
                            "failed", transition_counter)
                    self._transitions_counters[dispatch_msg.plan_id][
                        dispatch_msg.action_id]["failed"] += 1
                    self._transitions_counters[dispatch_msg.plan_id][
                        dispatch_msg.action_id]["succeeded"] = 0

                if transition.get_to_state().find("ba") != -1:

                    # If this fsm is on the highest level and it is transited for the first time to a reverse state
                    if transition.get_to_state().find("reverse") != -1 and \
                         not self._to_start_state[dispatch_msg.plan_id][dispatch_msg.action_id] and \
                         self._parent_ai is None:

                        # Mark the execution to start state
                        self._to_start_state[dispatch_msg.plan_id][
                            dispatch_msg.action_id] = True

                        # Publish feedback
                        fb = ActionFeedback()
                        fb.action_id = dispatch_msg.action_id
                        fb.plan_id = dispatch_msg.plan_id
                        fb.status = ActionFeedback.ACTION_DISPATCHED_TO_START_STATE
                        self._action_feedback_pub.publish(fb)

                    rospy.loginfo(
                        'KCL: ({}) Plan {} Action {}: Transition to state {}'.
                        format(rospy.get_name(),
                               dispatch_msg.plan_id, dispatch_msg.action_id,
                               transition.get_to_state()))

                    # Update the running state and get the names of the old and new states
                    old_running_state_name = self._running_state[
                        dispatch_msg.plan_id][
                            dispatch_msg.action_id].get_state_name()
                    self._running_state[dispatch_msg.plan_id][
                        dispatch_msg.action_id] = self._states[
                            transition.get_to_state()]
                    new_running_state_name = self._running_state[
                        dispatch_msg.plan_id][
                            dispatch_msg.action_id].get_state_name()

                    # Reset the transition counters for the next state only if transited to another state
                    if old_running_state_name != new_running_state_name:
                        self._transitions_counters[dispatch_msg.plan_id][
                            dispatch_msg.action_id]["succeeded"] = 0
                        self._transitions_counters[dispatch_msg.plan_id][
                            dispatch_msg.action_id]["failed"] = 0

                    # Run the action interface of the new state
                    action_interface = self._running_state[
                        dispatch_msg.plan_id][
                            dispatch_msg.action_id].get_action_interface()
                    action_interface.run(dispatch_msg)

                # If this function was called from the goal state of the fsm
                elif transition.get_to_state() == "goal_state":

                    rospy.loginfo(
                        'KCL: ({}) Plan {} Action {}: ActionLib {} finished to goal state'
                        .format(rospy.get_name(),
                                dispatch_msg.plan_id, dispatch_msg.action_id,
                                self.get_action_name()))
                    self._action_status[
                        plan_action_id] = ActionFeedback.ACTION_SUCCEEDED_TO_GOAL_STATE
                    fsm_execution_completed = True

                # If this function was called from the start state of the fsm
                elif transition.get_to_state() == "start_state":

                    rospy.loginfo(
                        'KCL: ({}) Plan {} Action {}: ActionLib {} finished to start state'
                        .format(rospy.get_name(),
                                dispatch_msg.plan_id, dispatch_msg.action_id,
                                self.get_action_name()))
                    self._action_status[
                        plan_action_id] = ActionFeedback.ACTION_SUCCEEDED_TO_START_STATE
                    fsm_execution_completed = True

                    # undo the start effects
                    self._kb_link.kb_undo_action_effects(
                        dispatch_msg, RPKnowledgeBaseLink.AT_START)

                # If this function was called from an error state of the fsm
                elif transition.get_to_state() == "error_state":
                    rospy.logwarn(
                        'KCL: ({}) Plan {} Action {}: State machine {} error. Human intervention needed'
                        .format(rospy.get_name(),
                                dispatch_msg.plan_id, dispatch_msg.action_id,
                                self.get_action_name()))
                else:
                    rospy.logwarn(
                        'KCL: ({}) Plan {} Action {}: State machine {} error: Transition to unknown state'
                        .format(rospy.get_name(),
                                dispatch_msg.plan_id, dispatch_msg.action_id,
                                self.get_action_name()))
Exemple #20
0
 def __new_feedback_msg(self, id, status):
     return ActionFeedback(action_id=id, status=status)
    def PubFeedback(self, action_id, status):

        feedback_msgs = ActionFeedback()
        feedback_msgs.action_id = action_id
        feedback_msgs.status = status
        self.feedback_pub.publish(feedback_msgs)
Exemple #22
0
 def feedback(self, status, info=None):
     self.feedback_pub.publish(ActionFeedback(self.action_id,
                                              status,
                                              dict_to_keyval(info)))
    def handle_pick_up(self, pick_up_msg):
        fname = "{}::{}".format(self.__class__.__name__, self.handle_pick_up.__name__)

        if (not self.ready):
            rospy.loginfo("{}: Pick&Place Component not ready to handle commands".format(fname))
            return

        rospy.loginfo("{}: Handling pick up action".format(fname))

        (block_name, from_block_name) = self.get_parameter_values(pick_up_msg.parameters, "block", "from_block")

        if (block_name is None or from_block_name is None):
            rospy.logerr("{}: Failed to find block parameters in pick up message".format(fname))
            return

        query_result = self.message_store.query_named(block_name, "komodo_blockworld/BlockPos", False)
        if (query_result is None):
            rospy.logfatal("{}: Failed to query DB. Aborting...".format(fname))
            return

        else:

            if (len(query_result) != 1):
                rospy.logerr("{}: Found {} blocks named {}. Aborting...".format(fname, len(query_result), block_name))
                return

            block_pos = query_result[0][0]
            if (block_pos.tablePos == -1 or block_pos.level == -1):
                rospy.logfatal("{}: Block {} table position is {} and it's level is {}!".
                               format(fname, block_name, block_pos.tablePos, block_pos.level))

            actionFeedback = ActionFeedback()
            actionFeedback.action_id = pick_up_msg.action_id
            actionFeedback.status = "action enabled"
            self.action_feedback_pub.publish(actionFeedback)

            self.release_grip()
            #rospy.sleep(1.5)

            self.position_arm(block_pos.tablePos, block_pos.level * 1.1)
            #rospy.sleep(1.5)

            self.grip_block(block_pos.level)
            #rospy.sleep(2)

            self.rise_arm()

            self.apply_effects_to_KMS(block_name, from_block_name, pick_up_msg.name)

            # Block is in hand remove it's position from the DB
            query_response = self.update_block_pos(block_name, -1, -1)
            if (query_response.success is False):
                rospy.logerr("{}: Failed to updat block {} position to {} {}".
                             format(fname, block_name, -1, -1))
                actionFeedback = ActionFeedback()
                actionFeedback.action_id = pick_up_msg.action_id
                actionFeedback.status = "action failed"
                self.action_feedback_pub.publish(actionFeedback)
            else:
                rospy.logdebug("{}: Updated block {} position to {} {}".
                               format(fname, block_name, -1, -1))
                actionFeedback = ActionFeedback()
                actionFeedback.action_id = pick_up_msg.action_id
                actionFeedback.status = "action achieved"
                self.action_feedback_pub.publish(actionFeedback)
 def update_fail(self):
     actionFeedback = ActionFeedback()
     actionFeedback.action_id = self.current_action.action_id
     actionFeedback.status = "action failed"
     self.action_feedback_pub.publish(actionFeedback)
 def update_fail(self):
     actionFeedback = ActionFeedback()
     actionFeedback.action_id = self.current_action.action_id
     actionFeedback.status = "action failed"
     self.action_feedback_pub.publish(actionFeedback)
Exemple #26
0
 def publish_feedback(self, plan_id, action_id, status):
     fb = ActionFeedback()
     fb.action_id = action_id
     fb.plan_id = plan_id
     fb.status = status
     self._action_feedback_pub.publish(fb)