Fix action interface dictionary resize error

KCL-Planning · Feb 7, 2024 · 846f494 · 846f494
1 parent 3cdbfb2
commit 846f494
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 2 deletions.
diff --git 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
@@ -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: