Error when reinitializing the EGM interface fixed

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)
 

abb_egm_driver/src/abb_egm_robot_node.cpp

@@ -261,13 +261,18 @@ 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);
+          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
         if(!robot_interface)