diff --git a/mycobot_320/mycobot_320_communication/scripts/mycobot_services.py b/mycobot_320/mycobot_320_communication/scripts/mycobot_services.py index 533e7b9..7e6a870 100755 --- a/mycobot_320/mycobot_320_communication/scripts/mycobot_services.py +++ b/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"