diff --git a/src/main/java/com/github/rosjava_actionlib/ActionClient.java b/src/main/java/com/github/rosjava_actionlib/ActionClient.java
index cfaa6c6..b5c569d 100644
--- a/src/main/java/com/github/rosjava_actionlib/ActionClient.java
+++ b/src/main/java/com/github/rosjava_actionlib/ActionClient.java
@@ -108,7 +108,7 @@ public void detachListener(ActionClientListener target) {
      */
     public ActionFuture<T_ACTION_GOAL, T_ACTION_FEEDBACK, T_ACTION_RESULT> sendGoal(T_ACTION_GOAL agMessage, String id) {
         GoalID gid = getGoalId(agMessage);
-        if (id == "") {
+        if (id.equals("")) {
             goalIdGenerator.generateID(gid);
         } else {
             gid.setId(id);
diff --git a/src/main/java/com/github/rosjava_actionlib/ActionGoal.java b/src/main/java/com/github/rosjava_actionlib/ActionGoal.java
index 6ce1c39..538fc40 100644
--- a/src/main/java/com/github/rosjava_actionlib/ActionGoal.java
+++ b/src/main/java/com/github/rosjava_actionlib/ActionGoal.java
@@ -254,6 +254,6 @@ public void setGoalMessage(Message gm) {
     }
 
     public boolean equalId(ActionGoal ag) {
-        return (this.getGoalId() == ag.getGoalId());
+        return (this.getGoalId().equals(ag.getGoalId()));
     }
 }
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);
         }
     }