diff --git a/abb_egm_driver/scripts/driver_test_node.py b/abb_egm_driver/scripts/driver_test_node.py
index 501f7f1e65eef4d63255044bf7bc1eba12ae5c02..4f18dd7f6977a65f28951597ffde8971f07d36f2 100755
--- a/abb_egm_driver/scripts/driver_test_node.py
+++ b/abb_egm_driver/scripts/driver_test_node.py
@@ -131,7 +131,7 @@ def circle(move_group, robot):
     # switched to motion streaming mode when positions are streamed to the robot afterwards.
     jtp = trajectory_msgs.msg.JointTrajectoryPoint()
     jtp.positions = move_group.get_current_joint_values()
-    for _ in range(10):
+    for _ in range(20):
         joint_command_publisher.publish(jtp)
         rospy.sleep(0.1)
 
diff --git a/abb_egm_driver/src/abb_egm_robot_node.cpp b/abb_egm_driver/src/abb_egm_robot_node.cpp
index 4fd1f1b9ca01065519f85039afabf5fe87dbfb8a..2dc07ac38f7019e0ddb60737496d19cf4594a132 100644
--- a/abb_egm_driver/src/abb_egm_robot_node.cpp
+++ b/abb_egm_driver/src/abb_egm_robot_node.cpp
@@ -261,12 +261,17 @@ int main(int argc, char** argv)
         // Create the RWS interface
         if(!egm_interface)
         {
-          egm_interface = new AbbEgmInterface();
-          // Initialize the EGM interface. If it fails, restart RAPID and try again. 
+          // Create and initialize the EGM interface. If it fails, delate the interface, restart RAPID and try again. 
           // After switching on the robot controller, it often happens that the initialization fails.
           // This can be fixed by restarting RAPID.
-          if(!egm_interface->init()) restartRapid(rws_interface);
-          if(!egm_interface->init()) break;
+          egm_interface = new AbbEgmInterface();
+          if(!egm_interface->init())
+          {
+            delete egm_interface;
+            restartRapid(rws_interface);
+            egm_interface = new AbbEgmInterface();
+            if(!egm_interface->init()) break;
+          }          
         }
 
         // Create the Robot interface