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 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)
