From 186483611d47d57c8ab594abf4e343189399c51e Mon Sep 17 00:00:00 2001
From: fmessmer <felixmessmer@gmail.com>
Date: Sat, 15 Sep 2018 17:34:00 +0200
Subject: [PATCH] fix ConcurrentModificationException in sendStatusTick

---
 .../rosjava_actionlib/ActionServer.java       | 22 ++++++++++++++-----
 1 file changed, 17 insertions(+), 5 deletions(-)

diff --git a/src/main/java/com/github/rosjava_actionlib/ActionServer.java b/src/main/java/com/github/rosjava_actionlib/ActionServer.java
index f8f3c2f..818f3cf 100644
--- a/src/main/java/com/github/rosjava_actionlib/ActionServer.java
+++ b/src/main/java/com/github/rosjava_actionlib/ActionServer.java
@@ -255,12 +255,20 @@ public void sendStatusTick() {
         GoalStatus goalStatus;
         Vector<GoalStatus> goalStatusList = new Vector<GoalStatus>();
 
-        for (ServerGoal sg : goalTracker.values()) {
-            goalStatus = node.getTopicMessageFactory().newFromType(GoalStatus._TYPE);
-            goalStatus.setGoalId(getGoalId(sg.goal));
-            goalStatus.setStatus((byte) sg.state.getState());
-            goalStatusList.add(goalStatus);
+        try {
+            for(java.util.Iterator<ServerGoal> sgIterator = goalTracker.values().iterator(); sgIterator.hasNext();) {
+                ServerGoal sg = sgIterator.next();
+                goalStatus = node.getTopicMessageFactory().newFromType(GoalStatus._TYPE);
+                goalStatus.setGoalId(getGoalId(sg.goal));
+                goalStatus.setStatus((byte) sg.state.getState());
+                goalStatusList.add(goalStatus);
+            }
+        } catch (java.util.ConcurrentModificationException exception) {
+            exception.printStackTrace(System.out);
+        } catch (Throwable throwable) {
+            throwable.printStackTrace(System.out);
         }
+
         status.setStatusList(goalStatusList);
         sendStatus(status);
     }
@@ -316,6 +324,7 @@ public void setSucceed(String goalIdString) {
         try {
             goalTracker.get(goalIdString).state.transition(ServerStateMachine.Events.SUCCEED);
         } catch (Exception e) {
+            e.printStackTrace(System.out);
         }
     }
 
@@ -327,6 +336,7 @@ public void setPreempt(String goalIdString) {
             goalTracker.get(goalIdString).state.transition(ServerStateMachine.Events.CANCEL_REQUEST);
             goalTracker.get(goalIdString).state.transition(ServerStateMachine.Events.CANCEL);
         } catch (Exception e) {
+            e.printStackTrace(System.out);
         }
     }
 
@@ -337,6 +347,7 @@ public void setAbort(String goalIdString) {
         try {
             goalTracker.get(goalIdString).state.transition(ServerStateMachine.Events.ABORT);
         } catch (Exception e) {
+            e.printStackTrace(System.out);
         }
     }
 
@@ -353,6 +364,7 @@ public void setGoalStatus(GoalStatus gstat, String gidString) {
             gstat.setGoalId(getGoalId(serverGoal.goal));
             gstat.setStatus((byte) serverGoal.state.getState());
         } catch (Exception e) {
+            e.printStackTrace(System.out);
         }
     }