Enable Robot Script and Update to Cmake

dobot_v4_bringup/CMakeLists.txt

@@ -293,6 +293,13 @@ install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_
 
 install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 
+install(DIRECTORY scripts DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+install(PROGRAMS scripts/RobotInitialization.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+
 ## Mark other files for installation (e.g. launch and bag files, etc.)
 # install(FILES
 #   # myfile1

dobot_v4_bringup/scripts/RobotInitialization.py

@@ -0,0 +1,33 @@
+#!/usr/bin/env python
+
+import rosnode
+import rospy
+import sys
+import copy
+import rospkg
+from std_msgs.msg import String
+import dobot_v4_bringup.srv
+import time
+from dobot_v4_bringup.srv import ClearErrorRequest, EnableRobotRequest
+
+#Creating the ros node and service client
+rospy.init_node("dobot_arm_enabler")
+rospy.wait_for_service("/dobot_v4_bringup/srv/ClearError")
+rospy.wait_for_service("/dobot_v4_bringup/srv/EnableRobot")
+
+def EnableRobot():
+    time.sleep(2)
+    clear_error_service = rospy.ServiceProxy("/dobot_v4_bringup/srv/ClearError", dobot_v4_bringup.srv.ClearError)
+
+    clear_error_response = clear_error_service()
+    if(clear_error_response.res == 0):
+        time.sleep(2)
+        enable_robot_service = rospy.ServiceProxy("/dobot_v4_bringup/srv/EnableRobot", dobot_v4_bringup.srv.EnableRobot)
+        enable_robot_response = enable_robot_service()
+        if (enable_robot_response.res != 0):
+            EnableRobot()
+    else:
+        EnableRobot()
+
+if __name__ == '__main__':
+    EnableRobot()

extend_gripper_control/scripts/gripper_control_robotiq.py

@@ -67,7 +67,6 @@ def InitializeGlobalVariables():
 
 if __name__ == '__main__':
     InitializeGlobalVariables()
-    EnableRobot()
     #Configure the modbus
     modbus_create_service = rospy.ServiceProxy("/dobot_v4_bringup/srv/ModbusCreate", dobot_v4_bringup.srv.ModbusCreate)
     modbus_config = ModbusCreateRequest()