fix

mycobot_320/mycobot_320_communication/scripts/mycobot_services.py

@@ -2,12 +2,50 @@
 # -*- coding: utf-8 -*-
 import time
 import rospy
+import os
+import fcntl
 from mycobot_communication.srv import *
 
 from pymycobot.mycobot import MyCobot
 
 mc = None
 
+# Avoid serial port conflicts and need to be locked
+def acquire(lock_file):
+    open_mode = os.O_RDWR | os.O_CREAT | os.O_TRUNC
+    fd = os.open(lock_file, open_mode)
+
+    pid = os.getpid()
+    lock_file_fd = None
+    
+    timeout = 50.0
+    start_time = current_time = time.time()
+    while current_time < start_time + timeout:
+        try:
+            # The LOCK_EX means that only one process can hold the lock
+            # The LOCK_NB means that the fcntl.flock() is not blocking
+            # and we are able to implement termination of while loop,
+            # when timeout is reached.
+            fcntl.flock(fd, fcntl.LOCK_EX | fcntl.LOCK_NB)
+        except (IOError, OSError):
+            pass
+        else:
+            lock_file_fd = fd
+            break
+        print('pid waiting for lock:%d'% pid)
+        time.sleep(1.0)
+        current_time = time.time()
+    if lock_file_fd is None:
+        os.close(fd)
+    return lock_file_fd
+
+
+def release(lock_file_fd):
+    # Do not remove the lockfile:
+    fcntl.flock(lock_file_fd, fcntl.LOCK_UN)
+    os.close(lock_file_fd)
+    return None
+
 
 def create_handle():
     global mc
@@ -43,7 +81,9 @@ def set_angles(req):
     sp = req.speed
 
     if mc:
+        lock = acquire("/tmp/mycobot_lock")
         mc.send_angles(angles, sp)
+        release(lock)
 
     return SetAnglesResponse(True)
 
@@ -51,7 +91,9 @@ def set_angles(req):
 def get_angles(req):
     """get angles,获取角度"""
     if mc:
+        lock = acquire("/tmp/mycobot_lock")
         angles = mc.get_angles()
+        release(lock)
         return GetAnglesResponse(*angles)
 
 
@@ -68,14 +110,18 @@ def set_coords(req):
     mod = req.model
 
     if mc:
+        lock = acquire("/tmp/mycobot_lock")
         mc.send_coords(coords, sp, mod)
+        release(lock)
 
     return SetCoordsResponse(True)
 
 
 def get_coords(req):
     if mc:
+        lock = acquire("/tmp/mycobot_lock")
         coords = mc.get_coords()
+        release(lock)
         return GetCoordsResponse(*coords)
 
 
@@ -83,23 +129,26 @@ def switch_status(req):
     """Gripper switch status"""
     """夹爪开关状态"""
     if mc:
+        lock = acquire("/tmp/mycobot_lock")
         if req.Status:
             mc.set_gripper_state(0, 80)
         else:
             mc.set_gripper_state(1, 80)
+        release(lock)
 
     return GripperStatusResponse(True)
 
 
 def toggle_pump(req):
     if mc:
+        lock = acquire("/tmp/mycobot_lock")
         if req.Status:
             mc.set_basic_output(req.Pin1, 0)
             mc.set_basic_output(req.Pin2, 0)
         else:
             mc.set_basic_output(req.Pin1, 1)
             mc.set_basic_output(req.Pin2, 1)
-
+        release(lock)
     return PumpStatusResponse(True)
 
 
@@ -131,11 +180,15 @@ def output_robot_message():
     atom_version = "unknown"
 
     if mc:
+        lock = acquire("/tmp/mycobot_lock")
         cn = mc.is_controller_connected()
+        release(lock)
         if cn == 1:
             connect_status = True
         time.sleep(0.1)
+        lock = acquire("/tmp/mycobot_lock")
         si = mc.is_all_servo_enable()
+        release(lock)
         if si == 1:
             servo_infomation = "all connected"