From d0ce79bc80dce4c1e05ac5bc71ddd2293fabc90e Mon Sep 17 00:00:00 2001 From: weijian Date: Wed, 7 Dec 2022 17:04:33 +0800 Subject: [PATCH] fix 280pi and mecharm_pi ubuntu20.04 ros1 from python3 --- mecharm/mecharm/scripts/simple_gui.py | 2 +- mecharm/mecharm/scripts/teleop_keyboard.py | 2 +- .../scripts/mecharm_services.py | 67 +++++++++++++++++-- mecharm/mecharm_pi/scripts/simple_gui.py | 7 +- mecharm/mecharm_pi/scripts/teleop_keyboard.py | 4 +- .../mycobot_280pi/scripts/simple_gui.py | 7 +- .../mycobot_280pi/scripts/teleop_keyboard.py | 2 +- .../scripts/mycobot_services.py | 61 +++++++++++++++-- 8 files changed, 129 insertions(+), 23 deletions(-) diff --git a/mecharm/mecharm/scripts/simple_gui.py b/mecharm/mecharm/scripts/simple_gui.py index 67784ec..387718d 100755 --- a/mecharm/mecharm/scripts/simple_gui.py +++ b/mecharm/mecharm/scripts/simple_gui.py @@ -33,7 +33,7 @@ class Window: # 计算 Tk 根窗口的 x 和 y 坐标 x = (self.ws / 2) - 190 y = (self.hs / 2) - 250 - self.win.geometry("430x400+{}+{}".format(x, y)) + self.win.geometry("430x400+{}+{}".format(int(x), int(y))) # layout,布局 self.set_layout() # input section,输入部分 diff --git a/mecharm/mecharm/scripts/teleop_keyboard.py b/mecharm/mecharm/scripts/teleop_keyboard.py index 3d2b971..b7d1033 100755 --- a/mecharm/mecharm/scripts/teleop_keyboard.py +++ b/mecharm/mecharm/scripts/teleop_keyboard.py @@ -84,7 +84,7 @@ def teleop_keyboard(): init_pose = [0, 0, 0, 0, 0, 0, speed] - home_pose = [0, 8, -127, 40, 0, 0, speed] + home_pose = [0, 30, 30, 0, 30, 0, speed] # rsp = set_angles(*init_pose) diff --git a/mecharm/mecharm_communication/scripts/mecharm_services.py b/mecharm/mecharm_communication/scripts/mecharm_services.py index 322127c..aa1e026 100755 --- a/mecharm/mecharm_communication/scripts/mecharm_services.py +++ b/mecharm/mecharm_communication/scripts/mecharm_services.py @@ -1,17 +1,55 @@ -#!/usr/bin/env python2 - # -*- coding: UTF-8 -*- +#!/usr/bin/env python3 +# -*- 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 - rospy.init_node("mecharm_services") + rospy.init_node("mycobot_services") rospy.loginfo("start ...") port = rospy.get_param("~port") baud = rospy.get_param("~baud") @@ -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) @@ -107,12 +156,12 @@ robot_msg = """ MyCobot Status -------------------------------- Joint Limit: - joint 1: -160 ~ +160 - joint 2: -90 ~ +90 + joint 1: -160 ~ +170 + joint 2: -85 ~ +90 joint 3: -180 ~ +45 joint 4: -160 ~ +160 joint 5: -100 ~ +100 - joint 6: -180 ~ +180 + joint 6: -∞ ~ +∞ Connect Status: %s @@ -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" diff --git a/mecharm/mecharm_pi/scripts/simple_gui.py b/mecharm/mecharm_pi/scripts/simple_gui.py index 67784ec..c7e76cf 100644 --- a/mecharm/mecharm_pi/scripts/simple_gui.py +++ b/mecharm/mecharm_pi/scripts/simple_gui.py @@ -1,6 +1,7 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # -*- coding: utf-8 -*- -import Tkinter as tk +# import Tkinter as tk # python2 +import tkinter as tk from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus import rospy import time @@ -33,7 +34,7 @@ class Window: # 计算 Tk 根窗口的 x 和 y 坐标 x = (self.ws / 2) - 190 y = (self.hs / 2) - 250 - self.win.geometry("430x400+{}+{}".format(x, y)) + self.win.geometry("430x400+{}+{}".format(int(x), int(y))) # layout,布局 self.set_layout() # input section,输入部分 diff --git a/mecharm/mecharm_pi/scripts/teleop_keyboard.py b/mecharm/mecharm_pi/scripts/teleop_keyboard.py index 3d2b971..6cfa0f3 100644 --- a/mecharm/mecharm_pi/scripts/teleop_keyboard.py +++ b/mecharm/mecharm_pi/scripts/teleop_keyboard.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # -*- coding:utf-8 -*- from __future__ import print_function from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus @@ -84,7 +84,7 @@ def teleop_keyboard(): init_pose = [0, 0, 0, 0, 0, 0, speed] - home_pose = [0, 8, -127, 40, 0, 0, speed] + home_pose = [0, 30, 30, 0, 30, 0, speed] # rsp = set_angles(*init_pose) diff --git a/mycobot_280/mycobot_280pi/scripts/simple_gui.py b/mycobot_280/mycobot_280pi/scripts/simple_gui.py index d2757cb..41f9258 100755 --- a/mycobot_280/mycobot_280pi/scripts/simple_gui.py +++ b/mycobot_280/mycobot_280pi/scripts/simple_gui.py @@ -1,6 +1,7 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # -*- coding: utf-8 -*- -import Tkinter as tk +# import Tkinter as tk +import tkinter as tk from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus import rospy import time @@ -33,7 +34,7 @@ class Window: # 计算 Tk 根窗口的 x 和 y 坐标 x = (self.ws / 2) - 190 y = (self.hs / 2) - 250 - self.win.geometry("430x400+{}+{}".format(x, y)) + self.win.geometry("430x400+{}+{}".format(int(x), int(y))) # layout,布局 self.set_layout() # input section,输入部分 diff --git a/mycobot_280/mycobot_280pi/scripts/teleop_keyboard.py b/mycobot_280/mycobot_280pi/scripts/teleop_keyboard.py index e5ee716..110f8a4 100755 --- a/mycobot_280/mycobot_280pi/scripts/teleop_keyboard.py +++ b/mycobot_280/mycobot_280pi/scripts/teleop_keyboard.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding=utf-8 from __future__ import print_function from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus diff --git a/mycobot_communication/scripts/mycobot_services.py b/mycobot_communication/scripts/mycobot_services.py index 0f105a8..cbf1b75 100755 --- a/mycobot_communication/scripts/mycobot_services.py +++ b/mycobot_communication/scripts/mycobot_services.py @@ -1,13 +1,51 @@ #!/usr/bin/env python2 -# -*- coding: utf-8 -* +# -*- 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 @@ -41,9 +79,11 @@ def set_angles(req): req.joint_6, ] sp = req.speed - print('angles1:',angles) + if mc: + lock = acquire("/tmp/mycobot_lock") mc.send_angles(angles, sp) + release(lock) return SetAnglesResponse(True) @@ -51,8 +91,9 @@ def set_angles(req): def get_angles(req): """get angles,获取角度""" if mc: + lock = acquire("/tmp/mycobot_lock") angles = mc.get_angles() - print('angles2:',angles) + release(lock) return GetAnglesResponse(*angles) @@ -69,15 +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() - print('coords:',coords) + release(lock) return GetCoordsResponse(*coords) @@ -85,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) @@ -133,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"