更改磁铁state

CMakeLists.txt

@@ -57,11 +57,12 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
+add_service_files(
+  FILES
+  ClawMagnetControl.srv
+  # Service2.srv
+)
+
 
 ## Generate actions in the 'action' folder
 # add_action_files(

scripts/magnet_opt.py

@@ -3,7 +3,7 @@
 
 import sagepy
 import rospy
-
+import sage_arm_msgs.srv
 #磁铁消磁
 from sage_magnet_opt.srv import ClawMagnetControl
 from sage_magnet_opt.srv import ClawMagnetControlResponse
@@ -28,6 +28,13 @@ def magnet_set_callback(request):
     response.result = True  # or set to a default value
     return response
 
+#磁铁检测函数
+def magnet_state_check():
+    res = get_io_state()
+    if (res.D0[0]) or (res.D0[1] or (res.D0[2])):
+        rospy.logerr("磁铁异常,正在关闭磁铁")                      
+        set_magnet(False)
+
  
 
 if __name__ == '__main__':
@@ -36,5 +43,6 @@ if __name__ == '__main__':
 
     #磁铁控制功能
     stack_operate_service = rospy.Service('/claw_magnet_set',ClawMagnetControl, magnet_set_callback)
-
+    # 获取夹爪io状态
+    get_io_state = rospy.ServiceProxy('get_io_states', sage_arm_msgs.srv.GetIoStates)