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: