xarm-python-sdk 1.15.2__py3-none-any.whl

xarm/__init__.py

@@ -0,0 +1,2 @@
+from .wrapper import XArmAPI
+from .version import __version__

xarm/build_backend.py

@@ -0,0 +1,17 @@
+import os
+import sys
+
+if sys.version_info.major > 3 or (sys.version_info.major == 3 and sys.version_info.minor >= 8):
+    # os.environ['_PYPROJECT_HOOKS_BUILD_BACKEND'] = 'hatchling.build'
+    # requires = [
+    #     "hatchling",
+    # ]
+    # build-backend = "hatchling.build"
+    from hatchling.build import *
+else:
+    # os.environ['_PYPROJECT_HOOKS_BUILD_BACKEND'] = 'setuptools.build_meta'
+    # requires = [
+    #     "setuptools >= 40.8.0",
+    # ]
+    # build-backend = "setuptools.build_meta"
+    from setuptools.build_meta import *

xarm/core/__init__.py

@@ -0,0 +1,2 @@
+from .config.x_code import ControllerWarn, ControllerError, ServoError
+from .config.x_config import XCONF

xarm/core/comm/__init__.py

@@ -0,0 +1,5 @@
+try:
+    from .serial_port import SerialPort
+except:
+    SerialPort = None
+from .socket_port import SocketPort

xarm/core/comm/base.py

@@ -0,0 +1,303 @@
+#!/usr/bin/env python3
+# Software License Agreement (MIT License)
+#
+# Copyright (c) 2018, UFACTORY, Inc.
+# All rights reserved.
+#
+# Author: Vinman <vinman.wen@ufactory.cc>
+
+import time
+import queue
+import socket
+import select
+import threading
+from ..utils.log import logger
+from ..utils import convert
+
+
+class RxParse(object):
+    def __init__(self, rx_que, fb_que=None):
+        self.rx_que = rx_que
+        self.fb_que = fb_que
+
+    def flush(self, fromid=-1, toid=-1):
+        pass
+
+    def put(self, data, is_report=False):
+        if not is_report and data[6] == 0xFF:
+            if not self.fb_que:
+                return
+            self.fb_que.put(data)
+        else:
+            self.rx_que.put(data)
+
+
+class Port(threading.Thread):
+    def __init__(self, rxque_max, fb_que=None):
+        super(Port, self).__init__()
+        self.daemon = True
+        self.rx_que = queue.Queue(rxque_max)
+        self.fb_que = fb_que
+        self.write_lock = threading.Lock()
+        self._connected = False
+        self.com = None
+        self.rx_parse = RxParse(self.rx_que, self.fb_que)
+        self.com_read = None
+        self.com_write = None
+        self.port_type = ''
+        self.buffer_size = 1
+        self.heartbeat_thread = None
+        self.alive = True
+
+    @property
+    def connected(self):
+        return self._connected
+
+    def run(self):
+        if self.port_type == 'report-socket':
+            self.recv_report_proc()
+        else:
+            self.recv_proc()
+            # self.recv_loop()
+
+    def close(self):
+        self.alive = False
+        if 'socket' in self.port_type:
+            try:
+                self.com.shutdown(socket.SHUT_RDWR)
+            except:
+                pass
+        try:
+            self.com.close()
+        except:
+            pass
+
+    def flush(self, fromid=-1, toid=-1):
+        if not self.connected:
+            return -1
+        while not(self.rx_que.empty()):
+            self.rx_que.queue.clear()
+        self.rx_parse.flush(fromid, toid)
+        return 0
+
+    def write(self, data):
+        if not self.connected:
+            return -1
+        try:
+            with self.write_lock:
+                logger.verbose('[{}] send: {}'.format(self.port_type, data))
+                self.com_write(data)
+            return 0
+        except Exception as e:
+            self._connected = False
+            logger.error("[{}] send error: {}".format(self.port_type, e))
+            return -1
+
+    def read(self, timeout=None):
+        if not self.connected:
+            return -1
+        try:
+            buf = self.rx_que.get(timeout=timeout)
+            logger.verbose('[{}] recv: {}'.format(self.port_type, buf))
+            return buf
+        except:
+            return -1
+        # if not self.connected:
+        #     return -1
+        # if not self.rx_que.empty():
+        #     buf = self.rx_que.get(timeout=timeout)
+        #     logger.verbose('[{}] recv: {}'.format(self.port_type, buf))
+        #     return buf
+        # else:
+        #     return -1
+
+    # def recv_loop(self):
+    #     self.alive = True
+    #     logger.debug('[{}] recv thread start'.format(self.port_type))
+    #     try:
+    #         while self.connected and self.alive:
+    #             if 'socket' in self.port_type:
+    #                 ready_input, ready_output, ready_exception = select.select([self.com], [], [])
+    #                 for indata in ready_input:
+    #                     if indata == self.com:
+    #                         rx_data = self.com_read(self.buffer_size)
+    #                         break
+    #                 else:
+    #                     continue
+    #             else:
+    #                 rx_data = self.com_read(self.com.in_waiting or self.buffer_size)
+    #             self.rx_parse.put(rx_data)
+    #     except Exception as e:
+    #         if self.alive:
+    #             logger.error('[{}] recv error: {}'.format(self.port_type, e))
+    #     finally:
+    #         self.close()
+    #     logger.debug('[{}] recv thread had stopped'.format(self.port_type))
+    #     self._connected = False
+
+    def recv_report_proc(self):
+        self.alive = True
+        logger.debug('[{}] recv thread start'.format(self.port_type))
+        failed_read_count = 0
+        timeout_count = 0
+        size = 0
+        data_num = 0
+        buffer = b''
+        size_is_not_confirm = False
+
+        data_prev_us = 0
+        data_curr_us = 0
+        data_max_interval_us = 0
+        data_over_us = 205 * 1000  # over 205ms, cnts++
+        data_over_cnts = 0
+
+        recv_prev_us = 0
+        recv_curr_us = 0
+        recv_max_interval_us = 0
+        recv_over_us = 300 * 1000  # over 300ms, cnts++
+        recv_over_cnts = 0
+
+        try:
+            while self.connected and self.alive:
+                try:
+                    data = self.com_read(4 - data_num if size == 0 else (size - data_num))
+                except socket.timeout:
+                    timeout_count += 1
+                    if timeout_count > 3:
+                        self._connected = False
+                        logger.error('[{}] socket read timeout'.format(self.port_type))
+                        break
+                    continue
+                else:
+                    if len(data) == 0:
+                        failed_read_count += 1
+                        if failed_read_count > 5:
+                            self._connected = False
+                            logger.error('[{}] socket read failed, len=0'.format(self.port_type))
+                            break
+                        time.sleep(0.1)
+                        continue
+                    data_num += len(data)
+                    buffer += data
+                    if size == 0:
+                        if data_num != 4:
+                            continue
+                        size = convert.bytes_to_u32(buffer[0:4])
+                        if size == 233:
+                            size_is_not_confirm = True
+                            size = 245
+                        logger.info('report_data_size: {}, size_is_not_confirm={}'.format(size, size_is_not_confirm))
+                    else:
+                        if data_num < size:
+                            continue
+                        if size_is_not_confirm:
+                            size_is_not_confirm = True
+                            if convert.bytes_to_u32(buffer[233:237]) == 233:
+                                size = 233
+                                buffer = buffer[233:]
+                                continue
+
+                        if convert.bytes_to_u32(buffer[0:4]) != size and not (size_is_not_confirm and size == 245 and convert.bytes_to_u32(buffer[0:4]) == 233):
+                            logger.error('report data error, close, length={}, size={}'.format(convert.bytes_to_u32(buffer[0:4]), size))
+                            break
+
+                        # # buffer[494:502]
+                        # data_curr_us = convert.bytes_to_u64(buffer[-8:])
+                        # recv_curr_us = time.monotonic() * 1000000
+                        #
+                        # if data_prev_us != 0 and recv_prev_us != 0:
+                        #     data_interval_us = data_curr_us - data_prev_us
+                        #     data_over_cnts += 1 if data_interval_us > data_over_us else 0
+                        #
+                        #     recv_interval_us = recv_curr_us - recv_prev_us
+                        #     recv_over_cnts += 1 if recv_interval_us > recv_over_us else 0
+                        #
+                        #     print_flag = False
+                        #
+                        #     if data_interval_us > data_max_interval_us:
+                        #         data_max_interval_us = data_interval_us
+                        #         print_flag = True
+                        #     elif data_interval_us > data_over_us:
+                        #         print_flag = True
+                        #
+                        #     if recv_interval_us > recv_max_interval_us:
+                        #         recv_max_interval_us = recv_interval_us
+                        #         print_flag = True
+                        #     elif recv_interval_us > recv_over_us:
+                        #         print_flag = True
+                        #
+                        #     if print_flag:
+                        #         print('[RECV] Di={}, Dmax={}, Dcnts={}, Ri={}, Rmax={}, Rcnts={}'.format(
+                        #             data_interval_us / 1000, data_max_interval_us / 1000, data_over_cnts,
+                        #             recv_interval_us / 1000, recv_max_interval_us / 1000, recv_over_cnts
+                        #         ))
+                        # data_prev_us = data_curr_us
+                        # recv_prev_us = recv_curr_us
+
+                        if self.rx_que.qsize() > 1:
+                            self.rx_que.get()
+                        self.rx_parse.put(buffer, True)
+                        buffer = b''
+                        data_num = 0
+
+                    timeout_count = 0
+                    failed_read_count = 0
+        except Exception as e:
+            if self.alive:
+                logger.error('[{}] recv error: {}'.format(self.port_type, e))
+        finally:
+            self.close()
+        logger.debug('[{}] recv thread had stopped'.format(self.port_type))
+        self._connected = False
+
+    def recv_proc(self):
+        self.alive = True
+        logger.debug('[{}] recv thread start'.format(self.port_type))
+        is_main_tcp = self.port_type == 'main-socket'
+        is_main_serial = self.port_type == 'main-serial'
+        try:
+            failed_read_count = 0
+            buffer = b''
+            while self.connected and self.alive:
+                if is_main_tcp:
+                    try:
+                        rx_data = self.com_read(self.buffer_size)
+                    except socket.timeout:
+                        continue
+                    if len(rx_data) == 0:
+                        failed_read_count += 1
+                        if failed_read_count > 5:
+                            self._connected = False
+                            logger.error('[{}] socket read failed, len=0'.format(self.port_type))
+                            break
+                        time.sleep(0.1)
+                        continue
+                    buffer += rx_data
+                    while True:
+                        if len(buffer) < 6:
+                            break
+                        length = convert.bytes_to_u16(buffer[4:6]) + 6
+                        if len(buffer) < length:
+                            break
+                        rx_data = buffer[:length]
+                        buffer = buffer[length:]
+                        self.rx_parse.put(rx_data)
+                elif is_main_serial:
+                    rx_data = self.com_read(self.com.in_waiting or self.buffer_size)
+                    self.rx_parse.put(rx_data)
+                else:
+                    break
+                failed_read_count = 0
+        except Exception as e:
+            if self.alive:
+                logger.error('[{}] recv error: {}'.format(self.port_type, e))
+        finally:
+            self.close()
+        logger.debug('[{}] recv thread had stopped'.format(self.port_type))
+        self._connected = False
+        # if self.heartbeat_thread:
+        #     try:
+        #         self.heartbeat_thread.join()
+        #     except:
+        #         pass
+

xarm/core/comm/serial_port.py

@@ -0,0 +1,44 @@
+#!/usr/bin/env python3
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2018, UFACTORY, Inc.
+# All rights reserved.
+#
+# Author: Jimy Zhang <jimy.zhang@ufactory.cc> <jimy92@163.com>
+# Author: Vinman <vinman.wen@ufactory.cc> <vinman.cub@gmail.com>
+
+
+import serial
+from ..utils.log import logger
+from .base import Port
+from .uxbus_cmd_protocol import Ux2HexProtocol
+from ..config.x_config import XCONF
+
+
+class SerialPort(Port):
+    def __init__(self, port, baud=XCONF.SerialConf.SERIAL_BAUD,
+                 rxque_max=XCONF.SerialConf.UXBUS_RXQUE_MAX, protocol=XCONF.SerialConf.UX2_HEX_PROTOCOL):
+        super(SerialPort, self).__init__(rxque_max)
+        self.port_type = 'main-serial'
+        try:
+            self.com = serial.Serial(port=port, baudrate=baud)
+            if not self.com.isOpen():
+                self._connected = False
+                raise Exception('serial is not open')
+            logger.info('{} connect {}:{} success'.format(self.port_type, port, baud))
+
+            self._connected = True
+
+            self.buffer_size = 1
+
+            if protocol == XCONF.SerialConf.UX2_HEX_PROTOCOL:
+                self.rx_parse = Ux2HexProtocol(self.rx_que,
+                                               XCONF.SerialConf.UXBUS_DEF_FROMID,
+                                               XCONF.SerialConf.UXBUS_DEF_TOID)
+            self.com_read = self.com.read
+            self.com_write = self.com.write
+            self.start()
+        except Exception as e:
+            logger.info('{} connect {}:{} failed, {}'.format(self.port_type, port, baud, e))
+            self._connected = False
+

xarm/core/comm/socket_port.py

@@ -0,0 +1,150 @@
+#!/usr/bin/env python3
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2018, UFACTORY, Inc.
+# All rights reserved.
+#
+# Author: Jimy Zhang <jimy.zhang@ufactory.cc> <jimy92@163.com>
+# Author: Vinman <vinman.wen@ufactory.cc> <vinman.cub@gmail.com>
+
+
+import queue
+import os
+import socket
+import struct
+import platform
+import threading
+import time
+from ..utils.log import logger
+from .base import Port
+from ..config.x_config import XCONF
+
+# try:
+#     if platform.system() == 'Linux':
+#         import fcntl
+#     else:
+#         fcntl = None
+# except:
+#     fcntl = None
+#
+#
+# def is_xarm_local_ip(ip):
+#     try:
+#         if platform.system() == 'Linux' and fcntl:
+#             def _get_ip(s, ifname):
+#                 try:
+#                     return socket.inet_ntoa(fcntl.ioctl(s.fileno(), 0x8915, struct.pack('256s', ifname[:15]))[20:24])
+#                 except:
+#                     pass
+#                 return ''
+#             sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+#             # gentoo system netcard name
+#             if ip == _get_ip(sock, b'enp1s0'):
+#                 return True
+#             # rasp system netcard name
+#             if ip == _get_ip(sock, b'eth0'):
+#                 return True
+#     except:
+#         pass
+#     return False
+
+
+def get_all_ips():
+    addrs = ['localhost', '127.0.0.1']
+    addrs = set(addrs)
+    try:
+        for ip in socket.gethostbyname_ex(socket.gethostname())[2]:
+            try:
+                if not ip.startswith('127.'):
+                    addrs.add(ip)
+            except:
+                pass
+    except:
+        pass
+    try:
+        sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        sock.settimeout(3)
+        sock.connect(('8.8.8.8', 53))
+        addrs.add(sock.getsockname()[0])
+    except:
+        pass
+    return addrs
+
+
+class HeartBeatThread(threading.Thread):
+    def __init__(self, sock_class):
+        threading.Thread.__init__(self)
+        self.sock_class = sock_class
+        self.daemon = True
+
+    def run(self):
+        logger.debug('{} heartbeat thread start'.format(self.sock_class.port_type))
+        heat_data = bytes([0, 0, 0, 1, 0, 2, 0, 0])
+
+        while self.sock_class.connected:
+            if self.sock_class.write(heat_data) == -1:
+                break
+            time.sleep(1)
+        logger.debug('{} heartbeat thread had stopped'.format(self.sock_class.port_type))
+
+
+class SocketPort(Port):
+    def __init__(self, server_ip, server_port, rxque_max=XCONF.SocketConf.TCP_RX_QUE_MAX, heartbeat=False,
+                 buffer_size=XCONF.SocketConf.TCP_CONTROL_BUF_SIZE, forbid_uds=False, fb_que=None):
+        is_main_tcp = server_port == XCONF.SocketConf.TCP_CONTROL_PORT or server_port == XCONF.SocketConf.TCP_CONTROL_PORT + 1
+        super(SocketPort, self).__init__(rxque_max, fb_que)
+        if is_main_tcp:
+            self.port_type = 'main-socket'
+            # self.com.setsockopt(socket.SOL_SOCKET, socket.SO_RCVTIMEO, 5)
+        else:
+            self.port_type = 'report-socket'
+        try:
+            socket.setdefaulttimeout(1)
+            use_uds = False
+            # if not forbid_uds and platform.system() == 'Linux' and is_xarm_local_ip(server_ip):
+            # if not forbid_uds and platform.system() == 'Linux' and server_ip in get_all_ips():
+            if not forbid_uds and platform.system() == 'Linux':
+                uds_path = os.path.join('/tmp/xarmcontroller_uds_{}'.format(server_port))
+                if os.path.exists(uds_path):
+                    try:
+                        self.com = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
+                        self.com.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+                        self.com.setblocking(True)
+                        self.com.settimeout(1)
+                        self.com.connect(uds_path)
+                        logger.info('{} connect {} success, uds_{}'.format(self.port_type, server_ip, server_port))
+                        use_uds = True
+                    except Exception as e:
+                        pass
+                        # logger.error('use uds error, {}'.format(e))
+            else:
+                pass
+            if not use_uds:
+                self.com = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+                self.com.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+                # self.com.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1)
+                # self.com.setsockopt(socket.SOL_TCP, socket.TCP_KEEPIDLE, 30)
+                # self.com.setsockopt(socket.SOL_TCP, socket.TCP_KEEPINTVL, 10)
+                # self.com.setsockopt(socket.SOL_TCP, socket.TCP_KEEPCNT, 3)
+                self.com.setblocking(True)
+                self.com.settimeout(1)
+                self.com.connect((server_ip, server_port))
+                logger.info('{} connect {} success'.format(self.port_type, server_ip))
+                # logger.info('{} connect {}:{} success'.format(self.port_type, server_ip, server_port))
+
+            self._connected = True
+            self.buffer_size = buffer_size
+            # time.sleep(1)
+
+            self.com_read = self.com.recv
+            self.com_write = self.com.send
+            self.write_lock = threading.Lock()
+            self.start()
+            if heartbeat:
+                self.heartbeat_thread = HeartBeatThread(self)
+                self.heartbeat_thread.start()
+        except Exception as e:
+            logger.info('{} connect {} failed, {}'.format(self.port_type, server_ip, e))
+            # logger.error('{} connect {}:{} failed, {}'.format(self.port_type, server_ip, server_port, e))
+            self._connected = False
+

xarm/core/comm/uxbus_cmd_protocol.py

@@ -0,0 +1,100 @@
+#!/usr/bin/env python3
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2018, UFACTORY, Inc.
+# All rights reserved.
+#
+# Author: Jimy Zhang <jimy.zhang@ufactory.cc> <jimy92@163.com>
+# Author: Vinman <vinman.wen@ufactory.cc> <vinman.cub@gmail.com>
+
+
+from ..utils import crc16
+from ..utils.log import logger
+
+# ux2_hex_protocol define
+UX2HEX_RXSTART_FROMID = 0
+UX2HEX_RXSTART_TOID = 1
+UX2HEX_RXSTATE_LEN = 2
+UX2HEX_RXSTATE_DATA = 3
+UX2HEX_RXSTATE_CRC1 = 4
+UX2HEX_RXSTATE_CRC2 = 5
+UX2HEX_RXLEN_MAX = 50
+
+
+class Ux2HexProtocol(object):
+    """
+    fromid and toid: broadcast address is 0xFF
+    """
+    def __init__(self, rx_que, fromid, toid):
+        self.rx_que = rx_que
+        self.rxstate = UX2HEX_RXSTART_FROMID
+        self.data_idx = 0
+        self.len = 0
+        self.fromid = fromid
+        self.toid = toid
+        self.rxbuf = None
+
+    # wipe cache , set from_id and to_id
+    def flush(self, fromid=-1, toid=-1):
+        self.rxstate = UX2HEX_RXSTART_FROMID
+        self.data_idx = 0
+        self.len = 0
+        if fromid != -1:
+            self.fromid = fromid
+        if toid != -1:
+            self.toid = toid
+
+    def put(self, rxstr, length=0):
+        if length == 0:
+            length = len(rxstr)
+        if len(rxstr) < length:
+            logger.error('len(rxstr) < length')
+
+        for i in range(length):
+            rxch = bytes([rxstr[i]])
+            # print_hex(self.DB_FLG, rxch, 1)
+            # print('state:%d' % (self.rxstate))
+            if UX2HEX_RXSTART_FROMID == self.rxstate:
+                if self.toid == rxch[0] or 255 == self.toid:
+                    self.rxbuf = rxch
+                    self.rxstate = UX2HEX_RXSTART_TOID
+
+            elif UX2HEX_RXSTART_TOID == self.rxstate:
+                if self.fromid == rxch[0] or self.fromid == 0xFF:
+                    self.rxbuf += rxch
+                    self.rxstate = UX2HEX_RXSTATE_LEN
+                else:
+                    self.rxstate = UX2HEX_RXSTART_FROMID
+
+            elif UX2HEX_RXSTATE_LEN == self.rxstate:
+                if rxch[0] < UX2HEX_RXLEN_MAX:
+                    self.rxbuf += rxch
+                    self.len = rxch[0]
+                    self.data_idx = 0
+                    self.rxstate = UX2HEX_RXSTATE_DATA
+                else:
+                    self.rxstate = UX2HEX_RXSTART_FROMID
+
+            elif UX2HEX_RXSTATE_DATA == self.rxstate:
+                if self.data_idx < self.len:
+                    self.rxbuf += rxch
+                    self.data_idx += 1
+                    if self.data_idx == self.len:
+                        self.rxstate = UX2HEX_RXSTATE_CRC1
+                else:
+                    self.rxstate = UX2HEX_RXSTART_FROMID
+
+            elif UX2HEX_RXSTATE_CRC1 == self.rxstate:
+                self.rxbuf += rxch
+                self.rxstate = UX2HEX_RXSTATE_CRC2
+
+            elif UX2HEX_RXSTATE_CRC2 == self.rxstate:
+                self.rxbuf += rxch
+                self.rxstate = UX2HEX_RXSTART_FROMID
+                crc = crc16.crc_modbus(self.rxbuf[:self.len + 3])
+                if crc[0] == self.rxbuf[self.len + 3] and crc[1] == self.rxbuf[self.len + 4]:
+                    if self.rx_que.full():
+                        self.rx_que.get()
+                    self.rx_que.put(self.rxbuf)
+                    # print(self.rxbuf)
+