diff --git a/rosplan_action_interface/src/ActionInterfaceManager.py b/rosplan_action_interface/src/ActionInterfaceManager.py index 91795420..18ff6360 100755 --- a/rosplan_action_interface/src/ActionInterfaceManager.py +++ b/rosplan_action_interface/src/ActionInterfaceManager.py @@ -87,7 +87,8 @@ def run(self): # iterate through interfaces and send feedback for interface in self._action_interfaces.values(): - for act in interface._action_status.keys(): + keys = list(interface._action_status.keys()) # copy keys to avoid dict size change during iteration + for act in keys: # action successful if interface._action_status[act] == ActionFeedback.ACTION_SUCCEEDED_TO_GOAL_STATE: @@ -96,7 +97,7 @@ def run(self): # action completed (achieved or failed) if interface._action_status[act] == ActionFeedback.ACTION_SUCCEEDED_TO_GOAL_STATE or interface._action_status[act] == ActionFeedback.ACTION_FAILED: - rospy.loginfo('KCL: ({}) Reporting action complete: {} {}'.format(rospy.get_name(), act, interface._action_name)) + rospy.loginfo('KCL: ({}) Reporting action complete: {} {}. Action {}.'.format(rospy.get_name(), act, interface._action_name, "succeeded" if interface._action_status[act] == ActionFeedback.ACTION_SUCCEEDED_TO_GOAL_STATE else "failed")) # publish feedback msg self.publish_feedback(act[0], act[1], interface._action_status[act]) # remove completed action data from interface diff --git a/rosplan_action_interface/src/ServiceActionInterface.py b/rosplan_action_interface/src/ServiceActionInterface.py index 6540f62a..fb6fc9dd 100644 --- a/rosplan_action_interface/src/ServiceActionInterface.py +++ b/rosplan_action_interface/src/ServiceActionInterface.py @@ -105,6 +105,7 @@ def run_thread(self, dispatch_msg): value = override_result[param] results_correct = self.check_result_msg(result, param, value, dispatch_msg) if not results_correct: + rospy.logwarn('KCL: ({}) Plan {} Action {}: Service {} failed to match result'.format(rospy.get_name(), dispatch_msg.plan_id, dispatch_msg.action_id, self.get_action_name())) break if results_correct: