fix 280pi and mecharm_pi ubuntu20.04 ros1 from python3

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,输入部分

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)
 

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"
 

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,输入部分

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)
 

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,输入部分

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

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"