Skip to content
Draft
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
32 changes: 30 additions & 2 deletions unitree_sdk2py/utils/thread.py
Original file line number Diff line number Diff line change
Expand Up @@ -3,10 +3,19 @@
import errno
import ctypes
import struct
import time
import platform
import threading

from .future import Future
from .timerfd import *

_HAS_TIMERFD = False
if platform.system() == "Linux":
try:
from .timerfd import *
_HAS_TIMERFD = True
except (AttributeError, OSError):
pass

class Thread(Future):
def __init__(self, target = None, name = None, args = (), kwargs = None):
Expand Down Expand Up @@ -52,6 +61,12 @@ def Wait(self, timeout: float = None):
super().Wait(timeout)

def __LoopFunc(self):
if _HAS_TIMERFD:
self.__LoopFunc_timerfd()
else:
self.__LoopFunc_sleep()

def __LoopFunc_timerfd(self):
# clock type CLOCK_MONOTONIC = 1
tfd = timerfd_create(1, 0)
spec = itimerspec.from_seconds(self.__inter, self.__inter)
Expand All @@ -66,12 +81,25 @@ def __LoopFunc(self):

try:
buf = os.read(tfd, 8)
# print(struct.unpack("Q", buf)[0])
except OSError as e:
if e.errno != errno.EAGAIN:
raise e

os.close(tfd)

def __LoopFunc_sleep(self):
while not self.__quit:
step_start = time.perf_counter()
try:
self.__loopTarget(*self.__loopArgs, **self.__loopKwargs)
except:
info = sys.exc_info()
print(f"[RecurrentThread] target func raise exception: name={info[0].__name__}, args={str(info[1].args)}")

elapsed = time.perf_counter() - step_start
remaining = self.__inter - elapsed
if remaining > 0:
time.sleep(remaining)

def __LoopFunc_0(self):
while not self.__quit:
Expand Down