diff --git a/include/iri_action_server/iri_action_server.h b/include/iri_action_server/iri_action_server.h
index 65dd292a4fd9d1f2385f6e3b0d14647f3fb692dc..0278082f7c75d921ff68c99717f0d80fcbc86fcd 100755
--- a/include/iri_action_server/iri_action_server.h
+++ b/include/iri_action_server/iri_action_server.h
@@ -431,7 +431,8 @@ void IriActionServer<ActionSpec>::executeCallback(const GoalConstPtr& goal)
         //is_active = false;
 
         // stop action
-        //stop_action_callback_();
+        if(!as_.isNewGoalAvailable())
+          stop_action_callback_();
         as_.setPreempted();
       }
       // check if action has finished