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)
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 _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)
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()
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
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
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)
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)
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")
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)
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()))
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)
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)
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)
