From 2f6d55903acc19e834409b0c99afe6ee0a627552 Mon Sep 17 00:00:00 2001 From: In-Chang Baek Date: Sat, 9 Aug 2025 19:48:21 +0900 Subject: [PATCH 1/7] Add carpet tactile data collection code --- .../carpet_tactile/sensors/app/AppContext.py | 51 +++ teleop/carpet_tactile/sensors/app/AppLog.py | 68 ++++ .../carpet_tactile/sensors/app/AppOptions.py | 26 ++ .../sensors/app/FramerateMonitor.py | 32 ++ .../sensors/app/PerformanceMonitor.py | 70 ++++ .../carpet_tactile/sensors/common/__init__.py | 0 .../sensors/common/dataset_tools.py | 253 +++++++++++++++ .../sensors/common/image_tools.py | 177 ++++++++++ .../sensors/common/input_tools.py | 23 ++ .../sensors/common/math_tools.py | 37 +++ .../sensors/common/myglobals.py | 24 ++ teleop/carpet_tactile/sensors/sensors.py | 220 +++++++++++++ .../carpet_tactile/sensors/storage/Storage.py | 77 +++++ .../sensors/storage/StorageDummy.py | 33 ++ .../sensors/storage/StorageHDF5.py | 102 ++++++ .../sensors/storage/StorageImgs.py | 42 +++ .../sensors/storage/StorageMJPEG.py | 42 +++ .../sensors/storage/StorageVideo.py | 50 +++ teleop/teleop_hand_and_arm.py | 304 +++++++++++------- teleop/utils/episode_writer.py | 13 +- 20 files changed, 1529 insertions(+), 115 deletions(-) create mode 100755 teleop/carpet_tactile/sensors/app/AppContext.py create mode 100755 teleop/carpet_tactile/sensors/app/AppLog.py create mode 100755 teleop/carpet_tactile/sensors/app/AppOptions.py create mode 100755 teleop/carpet_tactile/sensors/app/FramerateMonitor.py create mode 100755 teleop/carpet_tactile/sensors/app/PerformanceMonitor.py create mode 100755 teleop/carpet_tactile/sensors/common/__init__.py create mode 100755 teleop/carpet_tactile/sensors/common/dataset_tools.py create mode 100755 teleop/carpet_tactile/sensors/common/image_tools.py create mode 100755 teleop/carpet_tactile/sensors/common/input_tools.py create mode 100755 teleop/carpet_tactile/sensors/common/math_tools.py create mode 100755 teleop/carpet_tactile/sensors/common/myglobals.py create mode 100755 teleop/carpet_tactile/sensors/sensors.py create mode 100755 teleop/carpet_tactile/sensors/storage/Storage.py create mode 100755 teleop/carpet_tactile/sensors/storage/StorageDummy.py create mode 100755 teleop/carpet_tactile/sensors/storage/StorageHDF5.py create mode 100755 teleop/carpet_tactile/sensors/storage/StorageImgs.py create mode 100755 teleop/carpet_tactile/sensors/storage/StorageMJPEG.py create mode 100755 teleop/carpet_tactile/sensors/storage/StorageVideo.py diff --git a/teleop/carpet_tactile/sensors/app/AppContext.py b/teleop/carpet_tactile/sensors/app/AppContext.py new file mode 100755 index 0000000..ec6ae87 --- /dev/null +++ b/teleop/carpet_tactile/sensors/app/AppContext.py @@ -0,0 +1,51 @@ +import numpy as np +import sys, os, re, time, shutil, math, argparse, datetime +import scipy.io as sio +import multiprocessing as mp +import multiprocessing.managers as mpm + +from ..common import myglobals, dataset_tools, input_tools +from ..app.AppLog import * +from ..app.PerformanceMonitor import * + +class AppContext(object): + '''Manages log.''' + + def __init__(self, manager): + super(AppContext, self).__init__() + + self.appLog = AppLog() + #self.performanceMonitor = manager.setupPerformanceMonitor() + self.runDate = datetime.datetime.now() + + + def release(self): + if myglobals.PERFORMANCE_LOG_ENABLED: + self.performanceMonitor.dumpToFile(os.path.join(myglobals.DATA_PATH, 'live', 'performance.mat')) + self.appLog.release() + + + # Proxies + def log(self, message, timeoutMs = myglobals.DEFAULT_CONSOLE_TIMEOUT_MS): + return self.appLog.log(message, timeoutMs) + + def print(self, message, timeoutMs = myglobals.DEFAULT_CONSOLE_TIMEOUT_MS): + return self.appLog.log(message, timeoutMs) + + def tick(self, key, desc = ''): + return None + return self.performanceMonitor.tick(key, desc) + + @staticmethod + def create(): + manager = LgManager() + #manager.start() + ctx = AppContext(manager) + return ctx + + + +class LgManager(mpm.SyncManager): pass + #'''Manages resources.''' + +#LgManager.register('setupPerformanceMonitor', setup_PerformanceMonitor, proxytype=PerformanceMonitorProxy, exposed = ('reset', 'tick', 'report', 'dumpToFile', '__str__')) diff --git a/teleop/carpet_tactile/sensors/app/AppLog.py b/teleop/carpet_tactile/sensors/app/AppLog.py new file mode 100755 index 0000000..4d925d2 --- /dev/null +++ b/teleop/carpet_tactile/sensors/app/AppLog.py @@ -0,0 +1,68 @@ +import sys; sys.path.insert(0, '.') + +import numpy as np +import sys, os, re, time, shutil, math +from joblib import Parallel, delayed +from collections import OrderedDict +import cv2 +import scipy.io as sio +try: + import torch.multiprocessing as mp +except: + import multiprocessing as mp +import ctypes + + +from ..common import myglobals, dataset_tools, image_tools + +class LogEntry(object): + + COUNTER = 0 + + def __init__(self, message, timeoutMs): + super(LogEntry, self).__init__() + self.guid = LogEntry.COUNTER + LogEntry.COUNTER += 1 + + self.created = dataset_tools.getUnixTimestamp() + self.message = message + self.timeoutMs = timeoutMs + + def isValid(self): + if self.timeoutMs <= 0: + return True + ageMs = 1000*(dataset_tools.getUnixTimestamp() - self.created) + return ageMs < self.timeoutMs + + +class AppLog(object): + '''Manages log.''' + + def __init__(self): + super(AppLog, self).__init__() + #self.logQueue = DataQueueOrdered(limit = 0) + #self.entries = [] + + + def log(self, message, timeoutMs = myglobals.DEFAULT_CONSOLE_TIMEOUT_MS): + print(message) + #entry = LogEntry(message, timeoutMs) + #self.logQueue.push(entry) + + def consume(self): + return + while True: + entry = self.logQueue.pop() + if entry is None: + break + self.entries.append(entry) + + def clear(self): + #self.entries.clear() + pass + + def release(self): + print('[AppLog] Releasing...') + #self.logQueue.release() + print('[AppLog] Released.') + diff --git a/teleop/carpet_tactile/sensors/app/AppOptions.py b/teleop/carpet_tactile/sensors/app/AppOptions.py new file mode 100755 index 0000000..c7c0eea --- /dev/null +++ b/teleop/carpet_tactile/sensors/app/AppOptions.py @@ -0,0 +1,26 @@ +import numpy as np +import sys, os, re, time, shutil, math, argparse, datetime + +class AppOptions(object): + + def __init__(self, parser): + self.parser = parser + + def getCmd(self, mods = None): + args = self.parser.parse_args() + if not mods is None: + args = self.applyMods(args, mods) + return args + + def getDefault(self, mods = None): + args = self.parser.parse_args() + for key in vars(args): + setattr(args, key, self.parser.get_default(key)) + if not mods is None: + args = self.applyMods(args, mods) + return args + + def applyMods(self, args, mods): + for k,v in mods.items(): + setattr(args, k, v) + return args diff --git a/teleop/carpet_tactile/sensors/app/FramerateMonitor.py b/teleop/carpet_tactile/sensors/app/FramerateMonitor.py new file mode 100755 index 0000000..7c543fe --- /dev/null +++ b/teleop/carpet_tactile/sensors/app/FramerateMonitor.py @@ -0,0 +1,32 @@ +import sys, os, re, time, shutil, math, random, datetime, argparse, signal, traceback +import multiprocessing as mp +import numpy as np + +class FramerateMonitor(object): + def __init__(self, maxAgeSec = 2.0, maxSamples = 120): + self.maxAgeSec = maxAgeSec + self.maxSamples = maxSamples + self.reset() + super(FramerateMonitor, self).__init__() + + def tick(self): + self.ts += [time.time()] + self._update() + + def reset(self): + self.ts = [] + + def _update(self): + self.ts = self.ts[-self.maxSamples:] + now = time.time() + for i in range(len(self.ts) - 1, -1, -1): + if now - self.ts[i] > self.maxAgeSec: + self.ts = self.ts[i+1:] + break + + + def getFps(self): + if len(self.ts) < 2: + return -1 + return (len(self.ts) - 1) / max(self.ts[-1] - self.ts[0], 1e-3) + diff --git a/teleop/carpet_tactile/sensors/app/PerformanceMonitor.py b/teleop/carpet_tactile/sensors/app/PerformanceMonitor.py new file mode 100755 index 0000000..691d53e --- /dev/null +++ b/teleop/carpet_tactile/sensors/app/PerformanceMonitor.py @@ -0,0 +1,70 @@ +import numpy as np +import sys, os, re, time, shutil, math +import scipy.io as sio +import multiprocessing as mp +import multiprocessing.managers as mpm +#import torch.multiprocessing as mp + +from ..common import myglobals, dataset_tools + +''' Loosely inspired by: https://pythonhosted.org/ruffus/html/sharing_data_across_jobs_example.html ''' + +class PerformanceMonitor(object): + + def __init__(self): + super(PerformanceMonitor, self).__init__() + self.reset() + + def tick(self, key, desc = ''): + ts = dataset_tools.getUnixTimestamp() + if not key in self.keyedTicks: + self.keyedTicks[key] = [] + self.keyedTicks[key].append(ts) + + self.timestamps.append(ts) + self.keys.append(key) + self.descs.append(desc) + #print('New keyedTicks is %d: %s' % (len(self.keyedTicks[key]), key)) + + def report(self): + res = dict() + for k,v in self.keyedTicks.items(): + if len(v) < 2: + res[k] = 0.0 + continue + ts = v[-100:] + dts = np.diff(ts) + fps = 1.0 / np.mean(dts) + res[k] = fps + return res + + def dumpToFile(self, filename): + print('[PerformanceMonitor] Dumping to %s...' % filename) + meta = dict() + meta['timestamp'] = np.array(self.timestamps, np.float) + meta['key'] = np.array(self.keys, np.object) + meta['desc'] = np.array(self.descs, np.object) + sio.savemat(filename, meta) + print('[PerformanceMonitor] Dumped to %s...' % filename) + + def reset(self): + self.keyedTicks = dict() + self.timestamps = [] + self.keys = [] + self.descs = [] + + +def setup_PerformanceMonitor(): + return PerformanceMonitor() + +class PerformanceMonitorProxy(mpm.BaseProxy): + def reset(self): + return self._callmethod('reset', []) + def tick(self, key, desc = ''): + return self._callmethod('tick', [key, desc]) + def report(self): + return self._callmethod('report', []) + def dumpToFile(self, filename): + return self._callmethod('dumpToFile', [filename]) + def __str__ (self): + return "PerformanceMonitorProxy" diff --git a/teleop/carpet_tactile/sensors/common/__init__.py b/teleop/carpet_tactile/sensors/common/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/teleop/carpet_tactile/sensors/common/dataset_tools.py b/teleop/carpet_tactile/sensors/common/dataset_tools.py new file mode 100755 index 0000000..c6fd597 --- /dev/null +++ b/teleop/carpet_tactile/sensors/common/dataset_tools.py @@ -0,0 +1,253 @@ +import sys; sys.path.insert(0, '.') + +import numpy as np +import sys, os, re, math, shutil, time, datetime +import cv2 +import scipy.io as sio +from scipy.interpolate import UnivariateSpline, PchipInterpolator + +from ..common import myglobals + +def findNearestFrame(srcTimes, targetTimes): + + orderSrc = np.argsort(srcTimes) + orderTarget = np.argsort(targetTimes) + + srcSorted = srcTimes[orderSrc] + targetSorted = targetTimes[orderTarget] + + # find nearest frame for each resampled frame + nearestFrame = np.zeros([len(srcTimes)], int) + + mi = 0 + for i in range(len(srcSorted)): + while True: + if mi == len(targetSorted) - 1: + break + if targetSorted[mi + 1] - srcSorted[i] > srcSorted[i] - targetSorted[mi]: + break + mi += 1 + nearestFrame[i] = mi + + # now we have nearest sorted target for each sorted source => invert the sorting + + # => get nearest target for each sorted source + nearestFrame = orderTarget[nearestFrame] + # => get nearest target for each source + nearestFrame = nearestFrame[np.argsort(orderSrc)] + + return nearestFrame + + +def loadMetadata(filename, silent = False): + try: + # http://stackoverflow.com/questions/6273634/access-array-contents-from-a-mat-file-loaded-using-scipy-io-loadmat-python + if not silent: + print('\tReading metadata from %s...' % filename) + #metadata = sio.loadmat(filename, squeeze_me=True, struct_as_record=False) + metadata = MatReader().loadmat(filename) + except: + print('\tFailed to read the meta file "%s"!' % filename) + return None + return metadata + +def resampleMeta(referenceTime, meta, channelFilter = [], nearestOnly = False): + + srcData = meta['data'] + srcTime = meta['header'].ts_secs + + # only use valid samples (definition may vary) + srcValid = np.ones(srcTime.shape, np.bool) + + # check if there is confidence + if 'confidence' in srcData._fieldnames: + srcValid &= np.greater(srcData.confidence, 0.9) + + srcValid = np.argwhere(srcValid)[:,0] + srcTime = srcTime[srcValid] + + # Remove oversampling (breaks interpolation with dt=0) + srcTime, srcUnique = np.unique(srcTime, return_index = True) + srcValid = srcValid[srcUnique] + + dstDt = np.mean(np.diff(referenceTime)) + srcDt = np.mean(np.diff(srcTime)) + print('\tResampling from %.2f FPS to %.2f FPS.' % (1.0 / srcDt, 1.0 / dstDt)) + #nearestOnly = False + #if dstDt > 2 * srcDt: + # print('\t\t=> target framerate is significantly lower than source => will use nearest sampling only.') + # nearestOnly = True + + # Get nearest row + nearestRow = findNearestFrame(referenceTime, srcTime) + + res = dict() + res['header'] = dict() + res['header']['ts_secs'] = referenceTime + res['header']['frame'] = meta['header'].frame[nearestRow] + + res['data'] = dict() + dstData = res['data'] + dstData['ts_secs_key'] = srcTime[nearestRow] + + # Each data channel + for channel in srcData._fieldnames: + if len(channelFilter) > 0 and not channel in channelFilter: + continue + print('\t\tProcessing channel %s...' % channel) + cData = getattr(srcData, channel) + cData = cData[srcValid] + + # Singular channels do not interpolate well + if not isinstance(cData, np.ndarray) or len(cData) <= 0: + print('\t\t\tSingular channel => just copy.') + merged[channel] = cData + continue + + origShape = cData.shape + cData = cData.reshape([cData.shape[0],-1]) + + oShape = np.array(cData.shape) + oShape[0] = len(referenceTime) + oData = np.zeros(oShape, cData.dtype) + + nDims = cData.shape[1] if len(cData.shape) >= 2 else 1 + + # interpolation - always good idea? + for dim in range(nDims): + x = cData[:, dim] + + if not nearestOnly and issubclass(cData.dtype.type, float): + # interpolate floats + #if metaName == 'world_to_front_stitched': + #import pdb;pdb.set_trace() + print('\t\t...pchip interpolation...') + spl = PchipInterpolator(srcTime, x, extrapolate = False) # pchip + #spl = UnivariateSpline(srcTime, x, k = 1, s = 0, ext = 0) # lin + oData[:,dim] = spl(referenceTime) + + # fix extrapolation and other nan + nanInds = np.squeeze(np.argwhere(np.isnan(oData[:,dim]))) + oData[nanInds,dim] = x[nearestRow[nanInds]] + else: + # just take nearest for other types + print('\t\t...nearest sampling...') + oData[:,dim] = x[nearestRow] + + if len(origShape) == 1: + #cData = cData.reshape([cData.shape[0]]) + oData = oData.reshape([oData.shape[0]]) + else: + oShape = np.array(origShape) + oShape[0] = frameCount + oData = oData.reshape(oShape) + + dstData['%s' % (channel)] = oData + + return res + + +def getIntervals(condition): + intervals = [] + a = -1; + for i in range(len(condition)): + if condition[i]: + if a < 0: + a = i + else: + if a >= 0: + intervals.append([a, i]) + a = -1 + if a >= 0: + intervals.append([a, i]) + + return np.array(intervals) + +def dir2Angles(gaze3d): + yaw = np.arctan2(gaze3d[:,0], -gaze3d[:,2]) + xz = gaze3d[:,[0,2]] + xz = np.linalg.norm(xz, ord = 2, axis = 1) + pitch = np.arctan2(gaze3d[:,1],xz) + gazeAngles = np.stack([yaw, pitch],axis=1) + return gazeAngles + + +def angles2Dir(angles): + gaze3d = np.zeros([angles.shape[0], 3], angles.dtype) + gaze3d[:,0] = np.sin(angles[:,0]) * np.cos(angles[:,1]) + gaze3d[:,2] = -np.cos(angles[:,0]) * np.cos(angles[:,1]) + gaze3d[:,1] = np.sin(angles[:,1]) + return gaze3d + + +def preparePath(path, clear = False): + if not os.path.isdir(path): + os.makedirs(path, 0o777) + if clear: + files = os.listdir(path) + for f in files: + fPath = os.path.join(path, f) + if os.path.isdir(fPath): + shutil.rmtree(fPath) + else: + os.remove(fPath) + + return path + +def getUnixTimestamp(): + return np.datetime64(datetime.datetime.now()).astype(np.int64) # unix TS in secs and microsecs + + +class MatReader(object): + + def __init__(self, flatten1D = True): + self.flatten1D = flatten1D + + def loadmat(self, filename): + meta = sio.loadmat(filename, struct_as_record=False) + + meta.pop('__header__', None) + meta.pop('__version__', None) + meta.pop('__globals__', None) + + meta = self._squeezeItem(meta) + return meta + + def _squeezeItem(self, item): + if isinstance(item, np.ndarray): + if item.dtype == np.object: + if item.size == 1: + item = item[0,0] + else: + item = item.squeeze() + elif item.dtype.type is np.str_: + item = str(item.squeeze()) + elif self.flatten1D and len(item.shape) == 2 and (item.shape[0] == 1 or item.shape[1] == 1): + #import pdb; pdb.set_trace() + item = item.flatten() + + if isinstance(item, np.ndarray) and item.dtype == np.object: + #import pdb; pdb.set_trace() + #for v in np.nditer(item, flags=['refs_ok'], op_flags=['readwrite']): + # v[...] = self._squeezeItem(v) + it = np.nditer(item, flags=['multi_index','refs_ok'], op_flags=['readwrite']) + while not it.finished: + item[it.multi_index] = self._squeezeItem(item[it.multi_index]) + it.iternext() + + + + if isinstance(item, dict): + for k,v in item.items(): + item[k] = self._squeezeItem(v) + elif isinstance(item, sio.matlab.mio5_params.mat_struct): + for k in item._fieldnames: + v = getattr(item, k) + setattr(item, k, self._squeezeItem(v)) + + return item + + +class Bunch(object): + def __init__(self, adict): + self.__dict__.update(adict) \ No newline at end of file diff --git a/teleop/carpet_tactile/sensors/common/image_tools.py b/teleop/carpet_tactile/sensors/common/image_tools.py new file mode 100755 index 0000000..cb1fc46 --- /dev/null +++ b/teleop/carpet_tactile/sensors/common/image_tools.py @@ -0,0 +1,177 @@ +import numpy as np +import cv2 +import math, os, re +from subprocess import DEVNULL, STDOUT, check_call, CalledProcessError +from ..common import myglobals + +def fitImageToBounds(img, bounds, upscale = False, interpolation = cv2.INTER_LINEAR): + inAsp = img.shape[1] / img.shape[0] + outAsp = bounds[0] / bounds[1] + + if not upscale and img.shape[1] <= bounds[0] and img.shape[0] <= bounds[1]: + return img + elif img.shape[1] == bounds[0] and img.shape[0] == bounds[1]: + return img + + if inAsp < outAsp: + # Narrow to wide + height = bounds[1] + width = math.floor(inAsp * height+ 0.5) + else: + width = bounds[0] + height = math.floor(width / inAsp + 0.5) + + res = cv2.resize(img, (int(width), int(height)), interpolation = interpolation) + if len(res.shape) < len(img.shape): + res = res[..., np.newaxis] + return res + +def letterBoxImage(img, size, return_bbox = False): + # letter box + szIn = np.array([img.shape[1], img.shape[0]]) + x0 = (size - szIn) // 2 + x1 = x0 + szIn + + res = np.zeros([size[1], size[0], img.shape[2]], img.dtype) + res[x0[1]:x1[1],x0[0]:x1[0],:] = img + + if return_bbox: + return res, np.concatenate((x0,x1-x0)) + + return res + +def resizeImageLetterBox(img, size, interpolation = cv2.INTER_LINEAR, return_bbox = False): + img = fitImageToBounds(img, size, upscale = True, interpolation = interpolation) + return letterBoxImage(img, size, return_bbox) + + +def cropImage(img, bboxRel, padding = True): + bbox = (np.array(bboxRel, float) * [img.shape[1], img.shape[0], img.shape[1], img.shape[0]]).astype(int) + + aSrc = np.maximum(bbox[:2], 0) + bSrc = np.minimum(bbox[:2] + bbox[2:], (img.shape[1], img.shape[0])) + + aDst = aSrc - bbox[:2] + bDst = aDst + (bSrc - aSrc) + if np.any(np.less_equal(bDst, aDst)): + return None + if np.any(np.less_equal(bSrc, aSrc)): + return None + + src = img[aSrc[1]:bSrc[1],aSrc[0]:bSrc[0],:] + if padding: + res = np.zeros((bbox[3], bbox[2], img.shape[2]), img.dtype) + res[aDst[1]:bDst[1],aDst[0]:bDst[0],:] = src + else: + res = src + + return res + + +def fillImage(img, color): + cv2.rectangle(img, (0, 0), (img.shape[1], img.shape[0]), tuple(map(int,color)), -1) + + +def pasteImage(dest, img, posRel, sizeRel = None, interpolation = cv2.INTER_LINEAR): + aDst = (np.array(posRel, float) * [dest.shape[1], dest.shape[0]]).astype(int) + if sizeRel is None: + bDst = aDst + [img.shape[1], img.shape[0]] + else: + sz = (np.array(sizeRel, float) * [dest.shape[1], dest.shape[0]]).astype(int) + bDst = aDst + sz + img = cv2.resize(img, (int(sz[0]), int(sz[1])), interpolation = interpolation) + + aDstSafe = np.minimum(np.maximum(aDst, 0), [dest.shape[1] - 1, dest.shape[0] - 1]) + bDstSafe = np.minimum(np.maximum(bDst, aDstSafe), [dest.shape[1] - 1, dest.shape[0] - 1]) + if np.any(bDstSafe <= aDstSafe): + return dest + + aSrc = aDstSafe - aDst + bSrc = aSrc + (bDstSafe - aDstSafe) + dest[aDstSafe[1]:bDstSafe[1],aDstSafe[0]:bDstSafe[0],:] = img[aSrc[1]:bSrc[1],aSrc[0]:bSrc[0],:] + return dest + + + + + +def gridImages(imgs, targetAspect = 16.0 / 10, layout = None): + N = len(imgs) + if layout is None: + aspect = imgs[0].shape[1] / imgs[0].shape[0] + gridAspect = targetAspect / aspect + h = math.ceil(math.exp((math.log(N) - math.log(gridAspect)) / 2)) + w = math.ceil(N / h) + else: + w = layout[0] + h = layout[1] + + rows = [] + i = 0 + for y in range(h): + row = [] + for x in range(w): + if i < N: + row += [imgs[i]] + i += 1 + else: + row += [np.zeros_like(imgs[0])] + rows += [np.concatenate(row, axis=1)] + res = np.concatenate(rows, axis=0) + return res + + +def makeVideo(framesDir, pattern = '*.jpg', framerate = 8, outputVideoFile = None, overwrite = True): + print('Making video for %s...' % (framesDir)) + + videoName = os.path.basename(framesDir) + + if outputVideoFile is None: + outputVideoFile = '%s/../%s.mp4' % (framesDir, videoName) + outputVideoFile = os.path.realpath(outputVideoFile) + if not overwrite and os.path.isfile(outputVideoFile): + print('\tAlready exists => skipping...\n') + return + + cmdArgs = ['ffmpeg', '-r', '%f' % framerate] + if myglobals.isWindows(): + # Windows does not support glob + extension = os.path.splitext(pattern)[-1] + cmdArgs += ['-i', '"%s\\%%06d%s"' % (framesDir, extension)] + else: + cmdArgs += ['-pattern_type', 'glob', + '-i', "%s/%s" % (framesDir, pattern)] + + cmdArgs += ['-r', '30', + '-vf', 'scale=trunc(iw/2)*2:trunc(ih/2)*2', + '-crf', '21', '-pix_fmt', 'yuv420p', '-y', '"%s"' % outputVideoFile] + cmdFull = ' '.join(cmdArgs) + print('\t>> %s' % cmdFull) + + #if myglobals.isWindows(): + # return False + + try: + outputStream = DEVNULL + tempLogFile = os.path.join(myglobals.TEMP_PATH, 'my-ffmpeg.log') + with open(tempLogFile, 'w') as fid: + outputStream = fid + if myglobals.isWindows(): + check_call(cmdFull, stdout=outputStream, stderr=outputStream) + else: + check_call(cmdArgs, stdout=outputStream, stderr=outputStream) + except CalledProcessError as error: + print('\tFFMPEG failed: System error! %s' % error) + if os.path.isfile(tempLogFile): + with open(tempLogFile, 'r') as fid: + print(''.join(fid.readlines())) + if os.path.isfile(outputVideoFile): + os.unlink(outputVideoFile) + return False + + if not os.path.isfile(outputVideoFile): + print('\tFFMPEG failed: No results!') + return False + + print('\tdone.') + return True \ No newline at end of file diff --git a/teleop/carpet_tactile/sensors/common/input_tools.py b/teleop/carpet_tactile/sensors/common/input_tools.py new file mode 100755 index 0000000..d65a097 --- /dev/null +++ b/teleop/carpet_tactile/sensors/common/input_tools.py @@ -0,0 +1,23 @@ +import sys +from ..common import myglobals + +def readKeyAsync(): + if myglobals.isWindows(): + import msvcrt + return ord(msvcrt.getch()) if msvcrt.kbhit() else 0 + else: + import select + res = 0 + while sys.stdin in select.select([sys.stdin], [], [], 0)[0]: + line = sys.stdin.readline() + if line and res == 0: + res = ord(line[0]) + return res + +def str2bool(v): + if v.lower() in ('yes', 'true', 't', 'y', '1'): + return True + elif v.lower() in ('no', 'false', 'f', 'n', '0'): + return False + else: + raise argparse.ArgumentTypeError('Boolean value expected.') \ No newline at end of file diff --git a/teleop/carpet_tactile/sensors/common/math_tools.py b/teleop/carpet_tactile/sensors/common/math_tools.py new file mode 100755 index 0000000..c2c99a1 --- /dev/null +++ b/teleop/carpet_tactile/sensors/common/math_tools.py @@ -0,0 +1,37 @@ +import math +import numpy as np + +def matrixInverseSafe(matrix): + if np.fabs(np.linalg.det(matrix)) < 1e-3: + return np.zeros(matrix.shape) + try: + return np.linalg.inv(matrix) + except: + return np.zeros(matrix.shape) + +def lerp(a, b, c): + return a + (b - a) * c + +def bilinearInterp(y00, y10, y01, y11, xx, xy): + return lerp(lerp(y00, y10, xx), lerp(y01, y11, xx), xy) + +def clamp(x, a, b): + return np.minimum(np.maximum(x, a), b) + +def rotX(angleRad): + s = math.sin(angleRad) + c = math.cos(angleRad) + return np.array([[1,0,0],[0,c,-s],[0,s,c]], np.float32) + +def rotY(angleRad): + s = math.sin(angleRad) + c = math.cos(angleRad) + return np.array([[c,0,s],[0,1,0],[-s,0,c]], np.float32) + +def rotZ(angleRad): + s = math.sin(angleRad) + c = math.cos(angleRad) + return np.array([[c,-s,0],[s,c,0],[0,0,1]], np.float32) + +def gauss(x, sigma = 1.0, mu = 0.0): + return 1.0 / (sigma / np.sqrt(2.0 * np.pi)) * np.exp((-0.5 *((x - mu) * (x - mu))) / (sigma * sigma)) diff --git a/teleop/carpet_tactile/sensors/common/myglobals.py b/teleop/carpet_tactile/sensors/common/myglobals.py new file mode 100755 index 0000000..fdb949a --- /dev/null +++ b/teleop/carpet_tactile/sensors/common/myglobals.py @@ -0,0 +1,24 @@ +import os, sys, inspect, imp, math +import platform, tempfile + +def isWindows(): + #return False + return platform.system() == 'Windows' + +def isServer(): + #return False + return not isWindows() + +def supportsGUI(): + #return False + return not isServer() + +APP_ROOT_PATH = os.path.realpath(os.path.abspath(os.path.join(os.path.split(inspect.getfile( inspect.currentframe() ))[0],".."))) +TEMP_PATH = tempfile.gettempdir() + +CAMERA_LATENCY_MS = 800#400 +DEFAULT_CONSOLE_TIMEOUT_MS = 5000 +PERFORMANCE_LOG_ENABLED = False + + + diff --git a/teleop/carpet_tactile/sensors/sensors.py b/teleop/carpet_tactile/sensors/sensors.py new file mode 100755 index 0000000..b7318cd --- /dev/null +++ b/teleop/carpet_tactile/sensors/sensors.py @@ -0,0 +1,220 @@ +import serial +import numpy as np +import cv2 +import multiprocessing as mp +from multiprocessing import Manager +import copy +from teleop.carpet_tactile.sensors.app.FramerateMonitor import FramerateMonitor + + +class Sensor: + def __init__(self, port, baudrate, timeout): + self.queue = Manager().Queue() + self.exit = mp.Event() + self.process = mp.Process(target=self._read, args=(self.queue, port, baudrate, timeout)) + + def start(self): + self.process.start() + #wait for init + + def close(self): + self.exit.set() + self.process.join() + + def get(self): + result = None + if self.queue.empty(): + result = self.queue.get() + else: + while not self.queue.empty(): + result = self.queue.get() + return result + + def _read(self, queue, port, baudrate, timeout): # communicate with arduino board + self.ser = serial.Serial(port, baudrate=baudrate, timeout=timeout) + _sensor_bitshift = 6 + _sensor_sample_size = (32, 32) + + i = 0 + while not self.exit.is_set(): + data = b'' + while len(data) == 0: + self.ser.reset_input_buffer() + self.ser.write('a'.encode('utf-8')) + data = self.ser.readline() + # Unpack the data. + matrix_index = data[0] - (ord('\n') + 1) + data_matrix = data[1:-1] # ignore the newline character at the end + data_matrix = np.frombuffer(data_matrix, dtype=np.uint8).astype(np.uint16) + + data_matrix = data_matrix - (ord('\n') + 1) + data_matrix = data_matrix[0::2] * (2 ** _sensor_bitshift) + data_matrix[1::2] + data_matrix = data_matrix.reshape(_sensor_sample_size) + + # append queue + queue.put(data_matrix) + i += 1 + +class MultiSensors: + def __init__(self, ports): + self.ports = ports + self.make_sensors() + + self.queue = Manager().Queue() + self.exit = mp.Event() + self.process = mp.Process(target=self._read, args=(self.queue,)) + self.fps_monitor = FramerateMonitor() + self.fps_queue = Manager().Queue() + + def make_sensors(self): + sensors = [] + for port in self.ports: + sensors.append(Sensor(port=port, baudrate=1000000, timeout=1.0)) + self.sensors = sensors + + def init_sensors(self): + for sensor in self.sensors: + sensor.start() + + init_values = [] + for sensor in self.sensors: + x = sensor.get() + init_values.append(x.astype(np.float32)) + + init_values_num = 30 + for k in range(init_values_num): + for i in range(len(self.sensors)): + x = self.sensors[i].get() + init_values[i] += x + for i in range(len(self.sensors)): + init_values[i] /= init_values_num + self.init_values = init_values + + self.process.start() + + def _read(self, queue): + while not self.exit.is_set(): + images = [] + for sensor in self.sensors: + x = sensor.get() + images.append(x) + #concat + if len(images) == 4: + ''' + ======================================================================================================== + ====================This part should be modified if visualized image is not match======================= + ======================================================================================================== + ''' + images[1] = cv2.flip(images[1], 1) + images[0] = cv2.rotate(images[0], cv2.cv2.ROTATE_90_COUNTERCLOCKWISE) + images[2] = cv2.rotate(images[2], cv2.cv2.ROTATE_90_COUNTERCLOCKWISE) + images[2] = cv2.flip(images[2], 0) + + a = np.concatenate((images[1], images[3])) + b = np.concatenate((images[0], images[2])) + ''' + ======================================================================================================== + ======================================================================================================== + ''' + + a = np.transpose(a, (1, 0)) + b = np.transpose(b, (1, 0)) + + total_image = np.concatenate((a, b)) + else: + total_image = np.concatenate(images) + + self.fps_monitor.tick() + self.fps_queue.put(round(self.fps_monitor.getFps())) + queue.put(total_image) + + def get(self): + result = None + if self.queue.empty(): + result = self.queue.get() + else: + while not self.queue.empty(): + result = self.queue.get() + return result + + def get_all(self): + if self.queue.empty(): + results = [self.queue.get()] + else: + results = [] + while not self.queue.empty(): + results.append(self.queue.get()) + return results + + def getFps(self): + result = None + if self.fps_queue.empty(): + result = self.fps_queue.get() + else: + while not self.fps_queue.empty(): + result = self.fps_queue.get() + return result + + def close(self): + self.exit.set() + self.process.join() + for sensor in self.sensors: + sensor.close() + +class SensorEnv: + def __init__(self, ports, stack_num, adaptive_calibration, normalize=True): + self.stack_num = stack_num + self.normalize = normalize + self.sensor = MultiSensors(ports) + self.buffer = [] + + denoise_sec = 1 + denoise_start = 2 + self.calibration_range = (-17 * (denoise_start+denoise_sec), -17 * denoise_start) + assert stack_num < abs(self.calibration_range[1]) + self.adaptive_calibration = adaptive_calibration + if adaptive_calibration: + self.calibration_step = 0 + self.buffer_len = abs(self.calibration_range[0]) + else: + self.calibration_step = 10 + self.buffer_len = stack_num+1 + + self.fps = 0 + self._ready() + + def _ready(self): + self.sensor.init_sensors() + base_value = [] + if not self.adaptive_calibration: + while len(base_value) < self.calibration_step: + base_value += self.sensor.get_all() + base_value = base_value[-self.calibration_step:] + base_value = np.array(base_value) + self.base_value = base_value.mean(axis=0) + + def _read(self): + self.buffer += self.sensor.get_all() + if len(self.buffer) > self.buffer_len: + self.buffer = self.buffer[-self.buffer_len:] + + def _preprocess(self, images): + if self.adaptive_calibration: + self.base_value = self.buffer[self.calibration_range[0]:self.calibration_range[1]] + self.base_value = np.array(self.base_value) + self.base_value = self.base_value.mean(axis=0) + + # images -= np.expand_dims(self.base_value, axis=0) + # images /= 200 + return images + + def get(self): + self._read() + images = self.buffer[-self.stack_num:] + if self.normalize: + images = self._preprocess(images) + self.fps = self.sensor.getFps() + return images + + def close(self): + self.sensor.close() diff --git a/teleop/carpet_tactile/sensors/storage/Storage.py b/teleop/carpet_tactile/sensors/storage/Storage.py new file mode 100755 index 0000000..c23dfb3 --- /dev/null +++ b/teleop/carpet_tactile/sensors/storage/Storage.py @@ -0,0 +1,77 @@ +import sys; sys.path.insert(0, '.') +import sys, os, re, time, shutil, math, random, datetime, argparse, signal +import numpy as np +import multiprocessing as mp +import cv2 + +from ..app.AppContext import AppContext + +class Storage(object): + + def __init__(self, outputPath, name, ctx, opts): + self.outputPath = outputPath + self.name = name + self.ctx = ctx + self.opts = opts + self.init() + super(Storage, self).__init__() + + def reset(self): + self.release() + + def init(self): + self.frameCount = 0 + self._init() + + def release(self): + self._release() + self.log('Released.') + + def addFrame(self, ts, data): + self._addFrame(ts, data) + + def log(self, msg): + self.ctx.log('[%s] %s' % (self.getName(), msg)) + + def __len__(self): + return self.frameCount + + + # Implementation details + def getName(self): + return None + + def _init(self): + pass + + def _release(self): + pass + + + def _addFrame(self, ts, data): + pass + + +def createStorage(storageType, outputPath, name, ctx, opts): + + if storageType == 'dummy': + from storage.StorageDummy import StorageDummy + StorageClass = StorageDummy + elif storageType == 'video': + from storage.StorageVideo import StorageVideo + StorageClass = StorageVideo + elif storageType == 'mjpeg': + from storage.StorageMJPEG import StorageMJPEG + StorageClass = StorageMJPEG + elif storageType == 'imgs': + from storage.StorageImgs import StorageImgs + StorageClass = StorageImgs + elif storageType == 'audio': + from storage.StorageAudio import StorageAudio + StorageClass = StorageAudio + elif storageType == 'hdf5': + from ..storage.StorageHDF5 import StorageHDF5 + StorageClass = StorageHDF5 + else: + RuntimeError('Unknown storage "%s"' % storageType) + return StorageClass(outputPath, name, ctx, opts) diff --git a/teleop/carpet_tactile/sensors/storage/StorageDummy.py b/teleop/carpet_tactile/sensors/storage/StorageDummy.py new file mode 100755 index 0000000..1e0eae3 --- /dev/null +++ b/teleop/carpet_tactile/sensors/storage/StorageDummy.py @@ -0,0 +1,33 @@ +import sys; sys.path.insert(0, '.') +import sys, os, re, time, shutil, math, random, datetime, argparse, signal +import numpy as np +import multiprocessing as mp +import cv2 + +from storage.Storage import Storage +from app.AppContext import AppContext +from common import dataset_tools + + + +class StorageDummy(Storage): + ''' + Does not store anything. + ''' + + # Implementation details + def getName(self): + return '%s (StorageDummy)' % self.name + + def _init(self): + pass + + def _release(self): + pass + + + def _addFrame(self, ts, data): + self.frameCount += 1 + + + \ No newline at end of file diff --git a/teleop/carpet_tactile/sensors/storage/StorageHDF5.py b/teleop/carpet_tactile/sensors/storage/StorageHDF5.py new file mode 100755 index 0000000..4be446b --- /dev/null +++ b/teleop/carpet_tactile/sensors/storage/StorageHDF5.py @@ -0,0 +1,102 @@ +import sys; sys.path.insert(0, '.') +import sys, os, re, time, shutil, math, random, datetime, argparse, signal +import numpy as np +import h5py + +from ..storage.Storage import Storage +from ..app.AppContext import AppContext +from ..common import dataset_tools + + +class StorageHDF5(Storage): + + # Implementation details + def getName(self): + return '%s (StorageHDF5)' % self.name + + def _init(self): + + self.blockSize = 1024 + if hasattr(self.opts, 'blockSize'): + self.blockSize = int(self.opts.blockSize) + self.log('Setting block size to %d.' % self.blockSize) + + # Setup output + self.hdf5Filename = os.path.join(dataset_tools.preparePath(self.outputPath), '%s.hdf5' % self.name) + self.log('Output file: %s' % self.hdf5Filename) + self.f = h5py.File(self.hdf5Filename, 'w') + self.inited = False + + def _release(self): + # Trim unused space + oldSize = self.f['ts'].shape[0] + newSize = self.frameCount + self.log('Trimming datasets from %d to %d frames...' % (oldSize, newSize)) + for k,v in self.f.items(): + if k == 'frame_count': + continue + self.f[k].resize(newSize, axis=0) + + # Close + self.f.close() + self.log('Closed HDF5 file %s.' % self.hdf5Filename) + + + def _addFrame(self, ts, data): + + # Add ts as a standard column + data['ts'] = int(round(ts)) + + if not self.inited: + # Initialize datasets + self.f.create_dataset('frame_count', (1,), dtype=np.uint32) + self.f['frame_count'][0] = 0 + #self.f.create_dataset('ts', (self.blockSize,), maxshape = (None,), dtype = np.float64) + + for k,v in data.items(): + if np.isscalar(v): + v = np.array([v]) + sz = [self.blockSize,] + else: + v = np.array(v) + sz = [self.blockSize, *v.shape] + maxShape = sz.copy() + maxShape[0] = None + self.f.create_dataset(k, tuple(sz), maxshape = tuple(maxShape), dtype = v.dtype) + + self.inited = True + + # Check size + oldSize = self.f['ts'].shape[0] + if oldSize == self.frameCount: + newSize = oldSize + self.blockSize + self.log('Growing datasets from %d to %d frames...' % (oldSize, newSize)) + + #self.f['ts'].resize(newSize, axis=0) + for k,v in data.items(): + self.f[k].resize(newSize, axis=0) + + # Append data + #self.f['ts'][self.frameCount] = ts + for k,v in data.items(): + self.f[k][self.frameCount,...] = v + + + # Note frame count + self.frameCount += 1 + self.f['frame_count'][0] = self.frameCount + + # Flush to prevent data loss + self.f.flush() + + + +if __name__ == "__main__": + # Test code + storage = StorageHDF5('recordings', 'test', AppContext.create(), {}) + for i in range(1500): + storage.addFrame(dataset_tools.getUnixTimestamp(), {'scalar': i, 'vec3': np.array([1,2,3], np.float32) * i, 'mat2d': np.ones((32,32), np.uint16) * i}) + storage.release() + + f = h5py.File('recordings/test.hdf5', 'r') + import pdb; pdb.set_trace() \ No newline at end of file diff --git a/teleop/carpet_tactile/sensors/storage/StorageImgs.py b/teleop/carpet_tactile/sensors/storage/StorageImgs.py new file mode 100755 index 0000000..331230b --- /dev/null +++ b/teleop/carpet_tactile/sensors/storage/StorageImgs.py @@ -0,0 +1,42 @@ +import sys; sys.path.insert(0, '.') +import sys, os, re, time, shutil, math, random, datetime, argparse, signal +import numpy as np +import multiprocessing as mp +import cv2 + +from storage.Storage import Storage +from app.AppContext import AppContext +from common import dataset_tools + + + +class StorageImgs(Storage): + + # Implementation details + def getName(self): + return '%s (StorageImgs)' % self.name + + def _init(self): + # Setup output + tsFilename = os.path.join(dataset_tools.preparePath(self.outputPath), '%s.txt' % self.name) + self.tsOut = open(tsFilename, 'w') + + def _release(self): + self.tsOut.close() + + + def _addFrame(self, ts, data): + frameSubFolder = '%03dk' % (self.frameCount // 1000) + framePath = dataset_tools.preparePath(os.path.join(self.opts.outputPath, self.name, frameSubFolder)) + filename = os.path.join(framePath, '%06d.jpg' % self.frameCount) + if 'im_jpeg' in data: + data['im_jpeg'].tofile(filename) + elif 'im' in data: + cv2.imwrite(filename, data['im'], [cv2.IMWRITE_JPEG_QUALITY, 95]) + else: + raise RuntimeError('No image data!') + self.tsOut.write('%.10f\n' % ts) + self.frameCount += 1 + + + \ No newline at end of file diff --git a/teleop/carpet_tactile/sensors/storage/StorageMJPEG.py b/teleop/carpet_tactile/sensors/storage/StorageMJPEG.py new file mode 100755 index 0000000..d58d3e7 --- /dev/null +++ b/teleop/carpet_tactile/sensors/storage/StorageMJPEG.py @@ -0,0 +1,42 @@ +import sys; sys.path.insert(0, '.') +import sys, os, re, time, shutil, math, random, datetime, argparse, signal +import numpy as np +import multiprocessing as mp +import cv2 + +from storage.Storage import Storage +from app.AppContext import AppContext +from common import dataset_tools + + + +class StorageMJPEG(Storage): + + # Implementation details + def getName(self): + return '%s (StorageMJPEG)' % self.name + + def _init(self): + # Setup output + videoFilename = os.path.join(dataset_tools.preparePath(self.outputPath), '%s.avi' % self.name) + self.log('Output file: %s' % videoFilename) + self.videoOut = open(videoFilename, 'wb') + + tsFilename = os.path.join(self.outputPath, '%s.txt' % self.name) + self.tsOut = open(tsFilename, 'w') + + def _release(self): + self.videoOut.close() + self.tsOut.close() + + + def _addFrame(self, ts, data): + assert 'im_jpeg' in data + data['im_jpeg'].tofile(self.videoOut) + + self.tsOut.write('%.10f\n' % ts) + + self.frameCount += 1 + + + \ No newline at end of file diff --git a/teleop/carpet_tactile/sensors/storage/StorageVideo.py b/teleop/carpet_tactile/sensors/storage/StorageVideo.py new file mode 100755 index 0000000..4755a15 --- /dev/null +++ b/teleop/carpet_tactile/sensors/storage/StorageVideo.py @@ -0,0 +1,50 @@ +import sys; sys.path.insert(0, '.') +import sys, os, re, time, shutil, math, random, datetime, argparse, signal +import numpy as np +import multiprocessing as mp +import cv2 + +from storage.Storage import Storage +from app.AppContext import AppContext +from common import dataset_tools + + + +class StorageVideo(Storage): + + # Implementation details + def getName(self): + return '%s (StorageVideo)' % self.name + + def _init(self): + # Setup output + fourcc = cv2.VideoWriter_fourcc(*'MJPG') + #fourcc = cv2.VideoWriter_fourcc(*'XVID') + videoFilename = os.path.join(dataset_tools.preparePath(self.outputPath), '%s.avi' % self.name) + self.log('Output file: %s' % videoFilename) + self.videoOut = cv2.VideoWriter(videoFilename, fourcc, self.opts.fps, (int(self.opts.w), int(self.opts.h))) + + tsFilename = os.path.join(self.outputPath, '%s.txt' % self.name) + self.tsOut = open(tsFilename, 'w') + + def _release(self): + self.videoOut.release() + self.videoOut = None + self.tsOut.close() + + + def _addFrame(self, ts, data): + if 'im' in data: + im = data['im'] + elif 'im_jpeg' in data: + im = cv2.imdecode(data['im_jpeg'], cv2.IMREAD_COLOR) + else: + raise RuntimeError('No image data!') + + self.videoOut.write(im) + self.tsOut.write('%.10f\n' % ts) + + self.frameCount += 1 + + + \ No newline at end of file diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 2569cb7..cf06d78 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -5,17 +5,22 @@ from multiprocessing import shared_memory, Value, Array, Lock import threading import logging_mp + +from teleop.carpet_tactile.sensors.sensors import MultiSensors + logging_mp.basic_config(level=logging_mp.INFO) logger_mp = logging_mp.get_logger(__name__) -import os +import os import sys + current_dir = os.path.dirname(os.path.abspath(__file__)) parent_dir = os.path.dirname(current_dir) sys.path.append(parent_dir) from televuer import TeleVuerWrapper -from teleop.robot_control.robot_arm import G1_29_ArmController, G1_23_ArmController, H1_2_ArmController, H1_ArmController +from teleop.robot_control.robot_arm import G1_29_ArmController, G1_23_ArmController, H1_2_ArmController, \ + H1_ArmController from teleop.robot_control.robot_arm_ik import G1_29_ArmIK, G1_23_ArmIK, H1_2_ArmIK, H1_ArmIK from teleop.robot_control.robot_hand_unitree import Dex3_1_Controller, Dex1_1_Gripper_Controller from teleop.robot_control.robot_hand_inspire import Inspire_Controller @@ -27,16 +32,21 @@ # for simulation from unitree_sdk2py.core.channel import ChannelPublisher from unitree_sdk2py.idl.std_msgs.msg.dds_ import String_ -def publish_reset_category(category: int,publisher): # Scene Reset signal + + +def publish_reset_category(category: int, publisher): # Scene Reset signal msg = String_(data=str(category)) publisher.Write(msg) logger_mp.info(f"published reset category: {category}") + # state transition start_signal = False running = True should_toggle_recording = False is_recording = False + + def on_press(key): global running, start_signal, should_toggle_recording if key == 'r': @@ -49,23 +59,38 @@ def on_press(key): should_toggle_recording = True else: logger_mp.info(f"{key} was pressed, but no action is defined for this key.") -listen_keyboard_thread = threading.Thread(target=listen_keyboard, kwargs={"on_press": on_press, "until": None, "sequential": False,}, daemon=True) + + +listen_keyboard_thread = threading.Thread(target=listen_keyboard, + kwargs={"on_press": on_press, "until": None, "sequential": False, }, + daemon=True) listen_keyboard_thread.start() if __name__ == '__main__': parser = argparse.ArgumentParser() - parser.add_argument('--task_dir', type = str, default = './utils/data', help = 'path to save data') - parser.add_argument('--frequency', type = float, default = 60.0, help = 'save data\'s frequency') + parser.add_argument('--task_dir', type=str, default='./utils/data', help='path to save data') + parser.add_argument('--frequency', type=float, default=60.0, help='save data\'s frequency') # basic control parameters - parser.add_argument('--xr-mode', type=str, choices=['hand', 'controller'], default='hand', help='Select XR device tracking source') - parser.add_argument('--arm', type=str, choices=['G1_29', 'G1_23', 'H1_2', 'H1'], default='G1_29', help='Select arm controller') - parser.add_argument('--ee', type=str, choices=['dex1', 'dex3', 'inspire1', 'brainco'], help='Select end effector controller') + parser.add_argument('--xr-mode', type=str, choices=['hand', 'controller'], default='hand', + help='Select XR device tracking source') + parser.add_argument('--arm', type=str, choices=['G1_29', 'G1_23', 'H1_2', 'H1'], default='G1_29', + help='Select arm controller') + parser.add_argument('--ee', type=str, choices=['dex1', 'dex3', 'inspire1', 'brainco'], + help='Select end effector controller') # mode flags - parser.add_argument('--record', action = 'store_true', help = 'Enable data recording') - parser.add_argument('--motion', action = 'store_true', help = 'Enable motion control mode') + parser.add_argument('--record', action='store_true', help='Enable data recording') + parser.add_argument('--motion', action='store_true', help='Enable motion control mode') parser.add_argument('--headless', action='store_true', help='Enable headless mode (no display)') - parser.add_argument('--sim', action = 'store_true', help = 'Enable isaac simulation mode') + parser.add_argument('--sim', action='store_true', help='Enable isaac simulation mode') + parser.add_argument('--carpet_tactile', action='store_true', help='Enable carpet tactile sensor data collection') + parser.add_argument('--carpet_sensitivity', type=int, default=250, + help='Set carpet tactile sensor sensitivity (default: 1.0)') + parser.add_argument('--carpet_headless', action='store_true', + help='Enable headless mode for carpet tactile sensor data collection') + parser.add_argument('--carpet_tty', type=str, default='/dev/tty.usbserial-02857AC6', + help='Set the TTY port for carpet tactile sensors (default: /dev/tty.usbserial-02857AC6)') + parser.add_argument() args = parser.parse_args() logger_mp.info(f"args: {args}") @@ -92,9 +117,31 @@ def on_press(key): 'wrist_camera_id_numbers': [2, 4], } + base_images = list() + if args.carpet_tactile: + carpet_sensor = MultiSensors([args.carpet_tty]) + logger_mp.info("initializing carpet tactile sensors...") + carpet_sensor.init_sensors() + logger_mp.info("initializing carpet tactile sensors...Done") + + for i in range(20): + total_image = carpet_sensor.get() + base_images.append(total_image) + + base_images = np.array(base_images) + base_image = np.mean(base_images, axis=0) + logger_mp.info("Carpet tactile sensors calibration done!") + - ASPECT_RATIO_THRESHOLD = 2.0 # If the aspect ratio exceeds this value, it is considered binocular - if len(img_config['head_camera_id_numbers']) > 1 or (img_config['head_camera_image_shape'][1] / img_config['head_camera_image_shape'][0] > ASPECT_RATIO_THRESHOLD): + def get_tactile_data(): + total_image = carpet_sensor.get() + total_image = total_image - base_image + return total_image + + ASPECT_RATIO_THRESHOLD = 2.0 # If the aspect ratio exceeds this value, it is considered binocular + if len(img_config['head_camera_id_numbers']) > 1 or ( + img_config['head_camera_image_shape'][1] / img_config['head_camera_image_shape'][ + 0] > ASPECT_RATIO_THRESHOLD): BINOCULAR = True else: BINOCULAR = False @@ -102,37 +149,40 @@ def on_press(key): WRIST = True else: WRIST = False - - if BINOCULAR and not (img_config['head_camera_image_shape'][1] / img_config['head_camera_image_shape'][0] > ASPECT_RATIO_THRESHOLD): + + if BINOCULAR and not (img_config['head_camera_image_shape'][1] / img_config['head_camera_image_shape'][ + 0] > ASPECT_RATIO_THRESHOLD): tv_img_shape = (img_config['head_camera_image_shape'][0], img_config['head_camera_image_shape'][1] * 2, 3) else: tv_img_shape = (img_config['head_camera_image_shape'][0], img_config['head_camera_image_shape'][1], 3) - tv_img_shm = shared_memory.SharedMemory(create = True, size = np.prod(tv_img_shape) * np.uint8().itemsize) - tv_img_array = np.ndarray(tv_img_shape, dtype = np.uint8, buffer = tv_img_shm.buf) + tv_img_shm = shared_memory.SharedMemory(create=True, size=np.prod(tv_img_shape) * np.uint8().itemsize) + tv_img_array = np.ndarray(tv_img_shape, dtype=np.uint8, buffer=tv_img_shm.buf) if WRIST and args.sim: wrist_img_shape = (img_config['wrist_camera_image_shape'][0], img_config['wrist_camera_image_shape'][1] * 2, 3) - wrist_img_shm = shared_memory.SharedMemory(create = True, size = np.prod(wrist_img_shape) * np.uint8().itemsize) - wrist_img_array = np.ndarray(wrist_img_shape, dtype = np.uint8, buffer = wrist_img_shm.buf) - img_client = ImageClient(tv_img_shape = tv_img_shape, tv_img_shm_name = tv_img_shm.name, - wrist_img_shape = wrist_img_shape, wrist_img_shm_name = wrist_img_shm.name, server_address="127.0.0.1") + wrist_img_shm = shared_memory.SharedMemory(create=True, size=np.prod(wrist_img_shape) * np.uint8().itemsize) + wrist_img_array = np.ndarray(wrist_img_shape, dtype=np.uint8, buffer=wrist_img_shm.buf) + img_client = ImageClient(tv_img_shape=tv_img_shape, tv_img_shm_name=tv_img_shm.name, + wrist_img_shape=wrist_img_shape, wrist_img_shm_name=wrist_img_shm.name, + server_address="127.0.0.1") elif WRIST and not args.sim: wrist_img_shape = (img_config['wrist_camera_image_shape'][0], img_config['wrist_camera_image_shape'][1] * 2, 3) - wrist_img_shm = shared_memory.SharedMemory(create = True, size = np.prod(wrist_img_shape) * np.uint8().itemsize) - wrist_img_array = np.ndarray(wrist_img_shape, dtype = np.uint8, buffer = wrist_img_shm.buf) - img_client = ImageClient(tv_img_shape = tv_img_shape, tv_img_shm_name = tv_img_shm.name, - wrist_img_shape = wrist_img_shape, wrist_img_shm_name = wrist_img_shm.name) + wrist_img_shm = shared_memory.SharedMemory(create=True, size=np.prod(wrist_img_shape) * np.uint8().itemsize) + wrist_img_array = np.ndarray(wrist_img_shape, dtype=np.uint8, buffer=wrist_img_shm.buf) + img_client = ImageClient(tv_img_shape=tv_img_shape, tv_img_shm_name=tv_img_shm.name, + wrist_img_shape=wrist_img_shape, wrist_img_shm_name=wrist_img_shm.name) else: - img_client = ImageClient(tv_img_shape = tv_img_shape, tv_img_shm_name = tv_img_shm.name) + img_client = ImageClient(tv_img_shape=tv_img_shape, tv_img_shm_name=tv_img_shm.name) - image_receive_thread = threading.Thread(target = img_client.receive_process, daemon = True) + image_receive_thread = threading.Thread(target=img_client.receive_process, daemon=True) image_receive_thread.daemon = True image_receive_thread.start() # television: obtain hand pose data from the XR device and transmit the robot's head camera image to the XR device. - tv_wrapper = TeleVuerWrapper(binocular=BINOCULAR, use_hand_tracking=args.xr_mode == "hand", img_shape=tv_img_shape, img_shm_name=tv_img_shm.name, - return_state_data=True, return_hand_rot_data = False) + tv_wrapper = TeleVuerWrapper(binocular=BINOCULAR, use_hand_tracking=args.xr_mode == "hand", img_shape=tv_img_shape, + img_shm_name=tv_img_shm.name, + return_state_data=True, return_hand_rot_data=False) # arm if args.arm == "G1_29": @@ -150,33 +200,38 @@ def on_press(key): # end-effector if args.ee == "dex3": - left_hand_pos_array = Array('d', 75, lock = True) # [input] - right_hand_pos_array = Array('d', 75, lock = True) # [input] + left_hand_pos_array = Array('d', 75, lock=True) # [input] + right_hand_pos_array = Array('d', 75, lock=True) # [input] dual_hand_data_lock = Lock() - dual_hand_state_array = Array('d', 14, lock = False) # [output] current left, right hand state(14) data. - dual_hand_action_array = Array('d', 14, lock = False) # [output] current left, right hand action(14) data. - hand_ctrl = Dex3_1_Controller(left_hand_pos_array, right_hand_pos_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array, simulation_mode=args.sim) + dual_hand_state_array = Array('d', 14, lock=False) # [output] current left, right hand state(14) data. + dual_hand_action_array = Array('d', 14, lock=False) # [output] current left, right hand action(14) data. + hand_ctrl = Dex3_1_Controller(left_hand_pos_array, right_hand_pos_array, dual_hand_data_lock, + dual_hand_state_array, dual_hand_action_array, simulation_mode=args.sim) elif args.ee == "dex1": - left_gripper_value = Value('d', 0.0, lock=True) # [input] - right_gripper_value = Value('d', 0.0, lock=True) # [input] + left_gripper_value = Value('d', 0.0, lock=True) # [input] + right_gripper_value = Value('d', 0.0, lock=True) # [input] dual_gripper_data_lock = Lock() - dual_gripper_state_array = Array('d', 2, lock=False) # current left, right gripper state(2) data. + dual_gripper_state_array = Array('d', 2, lock=False) # current left, right gripper state(2) data. dual_gripper_action_array = Array('d', 2, lock=False) # current left, right gripper action(2) data. - gripper_ctrl = Dex1_1_Gripper_Controller(left_gripper_value, right_gripper_value, dual_gripper_data_lock, dual_gripper_state_array, dual_gripper_action_array, simulation_mode=args.sim) + gripper_ctrl = Dex1_1_Gripper_Controller(left_gripper_value, right_gripper_value, dual_gripper_data_lock, + dual_gripper_state_array, dual_gripper_action_array, + simulation_mode=args.sim) elif args.ee == "inspire1": - left_hand_pos_array = Array('d', 75, lock = True) # [input] - right_hand_pos_array = Array('d', 75, lock = True) # [input] + left_hand_pos_array = Array('d', 75, lock=True) # [input] + right_hand_pos_array = Array('d', 75, lock=True) # [input] dual_hand_data_lock = Lock() - dual_hand_state_array = Array('d', 12, lock = False) # [output] current left, right hand state(12) data. - dual_hand_action_array = Array('d', 12, lock = False) # [output] current left, right hand action(12) data. - hand_ctrl = Inspire_Controller(left_hand_pos_array, right_hand_pos_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array, simulation_mode=args.sim) + dual_hand_state_array = Array('d', 12, lock=False) # [output] current left, right hand state(12) data. + dual_hand_action_array = Array('d', 12, lock=False) # [output] current left, right hand action(12) data. + hand_ctrl = Inspire_Controller(left_hand_pos_array, right_hand_pos_array, dual_hand_data_lock, + dual_hand_state_array, dual_hand_action_array, simulation_mode=args.sim) elif args.ee == "brainco": - left_hand_pos_array = Array('d', 75, lock = True) # [input] - right_hand_pos_array = Array('d', 75, lock = True) # [input] + left_hand_pos_array = Array('d', 75, lock=True) # [input] + right_hand_pos_array = Array('d', 75, lock=True) # [input] dual_hand_data_lock = Lock() - dual_hand_state_array = Array('d', 12, lock = False) # [output] current left, right hand state(12) data. - dual_hand_action_array = Array('d', 12, lock = False) # [output] current left, right hand action(12) data. - hand_ctrl = Brainco_Controller(left_hand_pos_array, right_hand_pos_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array, simulation_mode=args.sim) + dual_hand_state_array = Array('d', 12, lock=False) # [output] current left, right hand state(12) data. + dual_hand_action_array = Array('d', 12, lock=False) # [output] current left, right hand action(12) data. + hand_ctrl = Brainco_Controller(left_hand_pos_array, right_hand_pos_array, dual_hand_data_lock, + dual_hand_state_array, dual_hand_action_array, simulation_mode=args.sim) else: pass @@ -185,21 +240,23 @@ def on_press(key): reset_pose_publisher = ChannelPublisher("rt/reset_pose/cmd", String_) reset_pose_publisher.Init() from teleop.utils.sim_state_topic import start_sim_state_subscribe + sim_state_subscriber = start_sim_state_subscribe() # controller + motion mode if args.xr_mode == "controller" and args.motion: from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient + sport_client = LocoClient() sport_client.SetTimeout(0.0001) sport_client.Init() - + # record + headless mode if args.record and args.headless: - recorder = EpisodeWriter(task_dir = args.task_dir, frequency = args.frequency, rerun_log = False) + recorder = EpisodeWriter(task_dir=args.task_dir, frequency=args.frequency, rerun_log=False) elif args.record and not args.headless: - recorder = EpisodeWriter(task_dir = args.task_dir, frequency = args.frequency, rerun_log = True) - + recorder = EpisodeWriter(task_dir=args.task_dir, frequency=args.frequency, rerun_log=True) + try: logger_mp.info("Please enter the start signal (enter 'r' to start the subsequent program)") while not start_signal: @@ -252,9 +309,9 @@ def on_press(key): with right_gripper_value.get_lock(): right_gripper_value.value = tele_data.right_pinch_value else: - pass - - # high level control + pass + + # high level control if args.xr_mode == "controller" and args.motion: # quit teleoperate if tele_data.tele_state.right_aButton: @@ -264,17 +321,18 @@ def on_press(key): if tele_data.tele_state.left_thumbstick_state and tele_data.tele_state.right_thumbstick_state: sport_client.Damp() # control, limit velocity to within 0.3 - sport_client.Move(-tele_data.tele_state.left_thumbstick_value[1] * 0.3, - -tele_data.tele_state.left_thumbstick_value[0] * 0.3, + sport_client.Move(-tele_data.tele_state.left_thumbstick_value[1] * 0.3, + -tele_data.tele_state.left_thumbstick_value[0] * 0.3, -tele_data.tele_state.right_thumbstick_value[0] * 0.3) # get current robot state data. - current_lr_arm_q = arm_ctrl.get_current_dual_arm_q() + current_lr_arm_q = arm_ctrl.get_current_dual_arm_q() current_lr_arm_dq = arm_ctrl.get_current_dual_arm_dq() # solve ik using motor data and wrist pose, then use ik results to control arms. time_ik_start = time.time() - sol_q, sol_tauff = arm_ik.solve_ik(tele_data.left_arm_pose, tele_data.right_arm_pose, current_lr_arm_q, current_lr_arm_dq) + sol_q, sol_tauff = arm_ik.solve_ik(tele_data.left_arm_pose, tele_data.right_arm_pose, current_lr_arm_q, + current_lr_arm_dq) time_ik_end = time.time() logger_mp.debug(f"ik:\t{round(time_ik_end - time_ik_start, 6)}") arm_ctrl.ctrl_dual_arm(sol_q, sol_tauff) @@ -305,8 +363,8 @@ def on_press(key): left_hand_action = [dual_gripper_action_array[0]] right_hand_action = [dual_gripper_action_array[1]] current_body_state = arm_ctrl.get_current_motor_q().tolist() - current_body_action = [-tele_data.tele_state.left_thumbstick_value[1] * 0.3, - -tele_data.tele_state.left_thumbstick_value[0] * 0.3, + current_body_action = [-tele_data.tele_state.left_thumbstick_value[1] * 0.3, + -tele_data.tele_state.left_thumbstick_value[0] * 0.3, -tele_data.tele_state.right_thumbstick_value[0] * 0.3] elif (args.ee == "inspire1" or args.ee == "brainco") and args.xr_mode == "hand": with dual_hand_data_lock: @@ -329,7 +387,7 @@ def on_press(key): if WRIST: current_wrist_image = wrist_img_array.copy() # arm state and action - left_arm_state = current_lr_arm_q[:7] + left_arm_state = current_lr_arm_q[:7] right_arm_state = current_lr_arm_q[-7:] left_arm_action = sol_q[:7] right_arm_action = sol_q[-7:] @@ -337,71 +395,89 @@ def on_press(key): colors = {} depths = {} if BINOCULAR: - colors[f"color_{0}"] = current_tv_image[:, :tv_img_shape[1]//2] - colors[f"color_{1}"] = current_tv_image[:, tv_img_shape[1]//2:] + colors[f"color_{0}"] = current_tv_image[:, :tv_img_shape[1] // 2] + colors[f"color_{1}"] = current_tv_image[:, tv_img_shape[1] // 2:] if WRIST: - colors[f"color_{2}"] = current_wrist_image[:, :wrist_img_shape[1]//2] - colors[f"color_{3}"] = current_wrist_image[:, wrist_img_shape[1]//2:] + colors[f"color_{2}"] = current_wrist_image[:, :wrist_img_shape[1] // 2] + colors[f"color_{3}"] = current_wrist_image[:, wrist_img_shape[1] // 2:] else: colors[f"color_{0}"] = current_tv_image if WRIST: - colors[f"color_{1}"] = current_wrist_image[:, :wrist_img_shape[1]//2] - colors[f"color_{2}"] = current_wrist_image[:, wrist_img_shape[1]//2:] + colors[f"color_{1}"] = current_wrist_image[:, :wrist_img_shape[1] // 2] + colors[f"color_{2}"] = current_wrist_image[:, wrist_img_shape[1] // 2:] states = { - "left_arm": { - "qpos": left_arm_state.tolist(), # numpy.array -> list - "qvel": [], - "torque": [], - }, - "right_arm": { - "qpos": right_arm_state.tolist(), - "qvel": [], - "torque": [], - }, - "left_ee": { - "qpos": left_ee_state, - "qvel": [], - "torque": [], - }, - "right_ee": { - "qpos": right_ee_state, - "qvel": [], - "torque": [], - }, + "left_arm": { + "qpos": left_arm_state.tolist(), # numpy.array -> list + "qvel": [], + "torque": [], + }, + "right_arm": { + "qpos": right_arm_state.tolist(), + "qvel": [], + "torque": [], + }, + "left_ee": { + "qpos": left_ee_state, + "qvel": [], + "torque": [], + }, + "right_ee": { + "qpos": right_ee_state, + "qvel": [], + "torque": [], + }, "body": { "qpos": current_body_state, - }, + }, } actions = { - "left_arm": { - "qpos": left_arm_action.tolist(), - "qvel": [], - "torque": [], - }, - "right_arm": { - "qpos": right_arm_action.tolist(), - "qvel": [], - "torque": [], - }, - "left_ee": { - "qpos": left_hand_action, - "qvel": [], - "torque": [], - }, - "right_ee": { - "qpos": right_hand_action, - "qvel": [], - "torque": [], - }, + "left_arm": { + "qpos": left_arm_action.tolist(), + "qvel": [], + "torque": [], + }, + "right_arm": { + "qpos": right_arm_action.tolist(), + "qvel": [], + "torque": [], + }, + "left_ee": { + "qpos": left_hand_action, + "qvel": [], + "torque": [], + }, + "right_ee": { + "qpos": right_hand_action, + "qvel": [], + "torque": [], + }, "body": { "qpos": current_body_action, - }, + }, } + + if args.carpet_tactile: + carpet_tactiles = dict() + tactile_data = get_tactile_data() + carpet_tactiles['carpet_0'] = tactile_data + + if args.carpet_headless: + tactile_render = (tactile_data / args.carpet_sensitivity) * 255 + tactile_render = np.clip(tactile_render, 0, 255) + tactile_render = cv2.resize(tactile_render.astype(np.uint8), (500, 500)) + cv2.imshow("carpet_0", tactile_render) + cv2.waitKey(1) + + else: + carpet_tactiles = None + if args.sim: - sim_state = sim_state_subscriber.read_data() - recorder.add_item(colors=colors, depths=depths, states=states, actions=actions, sim_state=sim_state) + sim_state = sim_state_subscriber.read_data() + recorder.add_item(colors=colors, depths=depths, states=states, actions=actions, + carpet_tactiles=carpet_tactiles, sim_state=sim_state) else: - recorder.add_item(colors=colors, depths=depths, states=states, actions=actions) + recorder.add_item(colors=colors, depths=depths, states=states, actions=actions, + carpet_tactiles=carpet_tactiles) current_time = time.time() time_elapsed = current_time - start_time diff --git a/teleop/utils/episode_writer.py b/teleop/utils/episode_writer.py index 2dbb07c..3f2510a 100644 --- a/teleop/utils/episode_writer.py +++ b/teleop/utils/episode_writer.py @@ -102,11 +102,13 @@ def create_episode(self): self.color_dir = os.path.join(self.episode_dir, 'colors') self.depth_dir = os.path.join(self.episode_dir, 'depths') self.audio_dir = os.path.join(self.episode_dir, 'audios') + self.carpet_tactile_dir = os.path.join(self.episode_dir, 'carpet_tactiles') self.json_path = os.path.join(self.episode_dir, 'data.json') os.makedirs(self.episode_dir, exist_ok=True) os.makedirs(self.color_dir, exist_ok=True) os.makedirs(self.depth_dir, exist_ok=True) os.makedirs(self.audio_dir, exist_ok=True) + os.makedirs(self.carpet_tactile_dir, exist_ok=True) if self.rerun_log: self.online_logger = RerunLogger(prefix="online/", IdxRangeBoundary = 60, memory_limit="300MB") @@ -114,7 +116,7 @@ def create_episode(self): logger_mp.info(f"==> New episode created: {self.episode_dir}") return True # Return True if the episode is successfully created - def add_item(self, colors, depths=None, states=None, actions=None, tactiles=None, audios=None, sim_state=None): + def add_item(self, colors, depths=None, states=None, actions=None, tactiles=None, audios=None, sim_state=None, carpet_tactiles=None): # Increment the item ID self.item_id += 1 # Create the item data dictionary @@ -127,6 +129,7 @@ def add_item(self, colors, depths=None, states=None, actions=None, tactiles=None 'tactiles': tactiles, 'audios': audios, 'sim_state': sim_state, + 'carpet_tactiles': carpet_tactiles, } # Enqueue the item data self.item_data_queue.put(item_data) @@ -153,6 +156,7 @@ def _process_item_data(self, item_data): colors = item_data.get('colors', {}) depths = item_data.get('depths', {}) audios = item_data.get('audios', {}) + carpet_tactiles = item_data.get('carpet_tactiles', {}) # Save images if colors: @@ -177,6 +181,13 @@ def _process_item_data(self, item_data): np.save(os.path.join(self.audio_dir, audio_name), audio.astype(np.int16)) item_data['audios'][mic] = os.path.join('audios', audio_name) + if carpet_tactiles: + for carpet_key, carpet_data in carpet_tactiles.items(): + carpet_name = f'carpet_{str(idx).zfill(6)}_{carpet_key}.npy' + np.save(os.path.join(self.carpet_tactile_dir, carpet_name), carpet_data.astype(np.float32)) + item_data['carpet_tactiles'][carpet_key] = os.path.join(self.episode_dir, carpet_name) + + # Update episode data self.episode_data.append(item_data) From 331bf2cf97227781dbd7da24b8c1d4b4658db6b8 Mon Sep 17 00:00:00 2001 From: In-Chang Baek Date: Sun, 10 Aug 2025 21:17:15 +0900 Subject: [PATCH 2/7] mistype parameter for carpet headless --- teleop/teleop_hand_and_arm.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index cf06d78..97e68ea 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -461,7 +461,7 @@ def get_tactile_data(): tactile_data = get_tactile_data() carpet_tactiles['carpet_0'] = tactile_data - if args.carpet_headless: + if not args.carpet_headless: tactile_render = (tactile_data / args.carpet_sensitivity) * 255 tactile_render = np.clip(tactile_render, 0, 255) tactile_render = cv2.resize(tactile_render.astype(np.uint8), (500, 500)) From 0d5a1c6946b9c37d60d2edfc4e9e9fe30edfe969 Mon Sep 17 00:00:00 2001 From: In-Chang Baek Date: Sun, 10 Aug 2025 21:17:54 +0900 Subject: [PATCH 3/7] add data collection code with no robot --- teleop/teleop_hand_and_arm_norobot.py | 483 ++++++++++++++++++++++++++ 1 file changed, 483 insertions(+) create mode 100644 teleop/teleop_hand_and_arm_norobot.py diff --git a/teleop/teleop_hand_and_arm_norobot.py b/teleop/teleop_hand_and_arm_norobot.py new file mode 100644 index 0000000..873e471 --- /dev/null +++ b/teleop/teleop_hand_and_arm_norobot.py @@ -0,0 +1,483 @@ +import numpy as np +import time +import argparse +import cv2 +from multiprocessing import shared_memory, Value, Array, Lock +import threading +import logging_mp + +from teleop.carpet_tactile.sensors.sensors import MultiSensors + +logging_mp.basic_config(level=logging_mp.INFO) +logger_mp = logging_mp.get_logger(__name__) + +import os +import sys +current_dir = os.path.dirname(os.path.abspath(__file__)) +parent_dir = os.path.dirname(current_dir) +sys.path.append(parent_dir) + +from televuer import TeleVuerWrapper +from teleop.robot_control.robot_arm import G1_29_ArmController, G1_23_ArmController, H1_2_ArmController, H1_ArmController +# from teleop.robot_control.robot_arm_ik import G1_29_ArmIK, G1_23_ArmIK, H1_2_ArmIK, H1_ArmIK +# from teleop.robot_control.robot_hand_unitree import Dex3_1_Controller, Dex1_1_Gripper_Controller +# from teleop.robot_control.robot_hand_inspire import Inspire_Controller +# from teleop.robot_control.robot_hand_brainco import Brainco_Controller +from teleop.image_server.image_client import ImageClient +from teleop.utils.episode_writer import EpisodeWriter +from sshkeyboard import listen_keyboard, stop_listening + +# for simulation +from unitree_sdk2py.core.channel import ChannelPublisher +from unitree_sdk2py.idl.std_msgs.msg.dds_ import String_ +def publish_reset_category(category: int,publisher): # Scene Reset signal + msg = String_(data=str(category)) + publisher.Write(msg) + logger_mp.info(f"published reset category: {category}") + +# state transition +start_signal = False +running = True +should_toggle_recording = False +is_recording = False +def on_press(key): + global running, start_signal, should_toggle_recording + if key == 'r': + start_signal = True + logger_mp.info("Program start signal received.") + elif key == 'q': + stop_listening() + running = False + elif key == 's': + should_toggle_recording = True + else: + logger_mp.info(f"{key} was pressed, but no action is defined for this key.") +listen_keyboard_thread = threading.Thread(target=listen_keyboard, kwargs={"on_press": on_press, "until": None, "sequential": False,}, daemon=True) +listen_keyboard_thread.start() + +import multiprocessing as mp + +mp.set_start_method('fork', force=True) # Use spawn method for multiprocessing + +if __name__ == '__main__': + + parser = argparse.ArgumentParser() + parser.add_argument('--task_dir', type = str, default = './utils/data', help = 'path to save data') + parser.add_argument('--frequency', type = float, default = 60.0, help = 'save data\'s frequency') + + # basic control parameters + parser.add_argument('--xr-mode', type=str, choices=['hand', 'controller'], default='hand', help='Select XR device tracking source') + parser.add_argument('--arm', type=str, choices=['G1_29', 'G1_23', 'H1_2', 'H1'], default='G1_29', help='Select arm controller') + parser.add_argument('--ee', type=str, choices=['dex1', 'dex3', 'inspire1', 'brainco'], help='Select end effector controller') + # mode flags + parser.add_argument('--record', action = 'store_true', help = 'Enable data recording') + parser.add_argument('--motion', action = 'store_true', help = 'Enable motion control mode') + parser.add_argument('--headless', action='store_true', help='Enable headless mode (no display)') + parser.add_argument('--sim', action = 'store_true', help = 'Enable isaac simulation mode') + parser.add_argument('--carpet_tactile', action='store_true', help='Enable carpet tactile sensor data collection') + parser.add_argument('--carpet_sensitivity', type=int, default=250, help='Set carpet tactile sensor sensitivity (default: 1.0)') + parser.add_argument('--carpet_headless', action='store_true', help='Enable headless mode for carpet tactile sensor data collection') + parser.add_argument('--carpet_tty', type=str, default='/dev/tty.usbserial-02857AC6', help='Set the TTY port for carpet tactile sensors (default: /dev/tty.usbserial-02857AC6)') + + + args = parser.parse_args() + logger_mp.info(f"args: {args}") + + # image client: img_config should be the same as the configuration in image_server.py (of Robot's development computing unit) + if args.sim: + img_config = { + 'fps': 30, + 'head_camera_type': 'opencv', + 'head_camera_image_shape': [480, 640], # Head camera resolution + 'head_camera_id_numbers': [0], + 'wrist_camera_type': 'opencv', + 'wrist_camera_image_shape': [480, 640], # Wrist camera resolution + 'wrist_camera_id_numbers': [2, 4], + } + else: + img_config = { + 'fps': 30, + 'head_camera_type': 'opencv', + 'head_camera_image_shape': [480, 1280], # Head camera resolution + 'head_camera_id_numbers': [0], + 'wrist_camera_type': 'opencv', + 'wrist_camera_image_shape': [480, 640], # Wrist camera resolution + 'wrist_camera_id_numbers': [2, 4], + } + + base_images = list() + if args.carpet_tactile: + carpet_sensor = MultiSensors([args.carpet_tty]) + logger_mp.info("initializing carpet tactile sensors...") + carpet_sensor.init_sensors() + logger_mp.info("initializing carpet tactile sensors...Done") + + for i in range(20): + total_image = carpet_sensor.get() + base_images.append(total_image) + + base_images = np.array(base_images) + base_image = np.mean(base_images, axis=0) + logger_mp.info("Carpet tactile sensors calibration done!") + + def get_tactile_data(): + total_image = carpet_sensor.get() + total_image = total_image - base_image + return total_image + + + ASPECT_RATIO_THRESHOLD = 2.0 # If the aspect ratio exceeds this value, it is considered binocular + if len(img_config['head_camera_id_numbers']) > 1 or (img_config['head_camera_image_shape'][1] / img_config['head_camera_image_shape'][0] > ASPECT_RATIO_THRESHOLD): + BINOCULAR = True + else: + BINOCULAR = False + if 'wrist_camera_type' in img_config: + WRIST = True + else: + WRIST = False + + if BINOCULAR and not (img_config['head_camera_image_shape'][1] / img_config['head_camera_image_shape'][0] > ASPECT_RATIO_THRESHOLD): + tv_img_shape = (img_config['head_camera_image_shape'][0], img_config['head_camera_image_shape'][1] * 2, 3) + else: + tv_img_shape = (img_config['head_camera_image_shape'][0], img_config['head_camera_image_shape'][1], 3) + + tv_img_shm = shared_memory.SharedMemory(create = True, size = np.prod(tv_img_shape) * np.uint8().itemsize) + tv_img_array = np.ndarray(tv_img_shape, dtype = np.uint8, buffer = tv_img_shm.buf) + + if WRIST and args.sim: + wrist_img_shape = (img_config['wrist_camera_image_shape'][0], img_config['wrist_camera_image_shape'][1] * 2, 3) + wrist_img_shm = shared_memory.SharedMemory(create = True, size = np.prod(wrist_img_shape) * np.uint8().itemsize) + wrist_img_array = np.ndarray(wrist_img_shape, dtype = np.uint8, buffer = wrist_img_shm.buf) + img_client = ImageClient(tv_img_shape = tv_img_shape, tv_img_shm_name = tv_img_shm.name, + wrist_img_shape = wrist_img_shape, wrist_img_shm_name = wrist_img_shm.name, server_address="127.0.0.1") + elif WRIST and not args.sim: + wrist_img_shape = (img_config['wrist_camera_image_shape'][0], img_config['wrist_camera_image_shape'][1] * 2, 3) + wrist_img_shm = shared_memory.SharedMemory(create = True, size = np.prod(wrist_img_shape) * np.uint8().itemsize) + wrist_img_array = np.ndarray(wrist_img_shape, dtype = np.uint8, buffer = wrist_img_shm.buf) + img_client = ImageClient(tv_img_shape = tv_img_shape, tv_img_shm_name = tv_img_shm.name, + wrist_img_shape = wrist_img_shape, wrist_img_shm_name = wrist_img_shm.name) + else: + img_client = ImageClient(tv_img_shape = tv_img_shape, tv_img_shm_name = tv_img_shm.name) + + image_receive_thread = threading.Thread(target = img_client.receive_process, daemon = True) + image_receive_thread.daemon = True + image_receive_thread.start() + + # television: obtain hand pose data from the XR device and transmit the robot's head camera image to the XR device. + tv_wrapper = TeleVuerWrapper(binocular=BINOCULAR, use_hand_tracking=args.xr_mode == "hand", img_shape=tv_img_shape, img_shm_name=tv_img_shm.name, + return_state_data=True, return_hand_rot_data = False) + + # arm + if args.arm == "G1_29": + # arm_ctrl = G1_29_ArmController(motion_mode=args.motion, simulation_mode=args.sim) + # arm_ik = G1_29_ArmIK() + pass + elif args.arm == "G1_23": + arm_ctrl = G1_23_ArmController(simulation_mode=args.sim) + arm_ik = G1_23_ArmIK() + elif args.arm == "H1_2": + arm_ctrl = H1_2_ArmController(simulation_mode=args.sim) + arm_ik = H1_2_ArmIK() + elif args.arm == "H1": + arm_ctrl = H1_ArmController(simulation_mode=args.sim) + arm_ik = H1_ArmIK() + + # end-effector + if args.ee == "dex3": + left_hand_pos_array = Array('d', 75, lock = True) # [input] + right_hand_pos_array = Array('d', 75, lock = True) # [input] + dual_hand_data_lock = Lock() + dual_hand_state_array = Array('d', 14, lock = False) # [output] current left, right hand state(14) data. + dual_hand_action_array = Array('d', 14, lock = False) # [output] current left, right hand action(14) data. + hand_ctrl = Dex3_1_Controller(left_hand_pos_array, right_hand_pos_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array, simulation_mode=args.sim) + elif args.ee == "dex1": + left_gripper_value = Value('d', 0.0, lock=True) # [input] + right_gripper_value = Value('d', 0.0, lock=True) # [input] + dual_gripper_data_lock = Lock() + dual_gripper_state_array = Array('d', 2, lock=False) # current left, right gripper state(2) data. + dual_gripper_action_array = Array('d', 2, lock=False) # current left, right gripper action(2) data. + gripper_ctrl = Dex1_1_Gripper_Controller(left_gripper_value, right_gripper_value, dual_gripper_data_lock, dual_gripper_state_array, dual_gripper_action_array, simulation_mode=args.sim) + elif args.ee == "inspire1": + # left_hand_pos_array = Array('d', 75, lock = True) # [input] + # right_hand_pos_array = Array('d', 75, lock = True) # [input] + # dual_hand_data_lock = Lock() + # dual_hand_state_array = Array('d', 12, lock = False) # [output] current left, right hand state(12) data. + # dual_hand_action_array = Array('d', 12, lock = False) # [output] current left, right hand action(12) data. + # hand_ctrl = Inspire_Controller(left_hand_pos_array, right_hand_pos_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array, simulation_mode=args.sim) + hand_ctrl= None + elif args.ee == "brainco": + left_hand_pos_array = Array('d', 75, lock = True) # [input] + right_hand_pos_array = Array('d', 75, lock = True) # [input] + dual_hand_data_lock = Lock() + dual_hand_state_array = Array('d', 12, lock = False) # [output] current left, right hand state(12) data. + dual_hand_action_array = Array('d', 12, lock = False) # [output] current left, right hand action(12) data. + hand_ctrl = Brainco_Controller(left_hand_pos_array, right_hand_pos_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array, simulation_mode=args.sim) + else: + pass + + # simulation mode + if args.sim: + reset_pose_publisher = ChannelPublisher("rt/reset_pose/cmd", String_) + reset_pose_publisher.Init() + from teleop.utils.sim_state_topic import start_sim_state_subscribe + sim_state_subscriber = start_sim_state_subscribe() + + # controller + motion mode + if args.xr_mode == "controller" and args.motion: + from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient + sport_client = LocoClient() + sport_client.SetTimeout(0.0001) + sport_client.Init() + + # record + headless mode + if args.record and args.headless: + recorder = EpisodeWriter(task_dir = args.task_dir, frequency = args.frequency, rerun_log = False) + elif args.record and not args.headless: + recorder = EpisodeWriter(task_dir = args.task_dir, frequency = args.frequency, rerun_log = True) + + try: + logger_mp.info("Please enter the start signal (enter 'r' to start the subsequent program)") + # while not start_signal: + # time.sleep(0.01) + # arm_ctrl.speed_gradual_max() + while running: + start_time = time.time() + + if not args.headless: + tv_resized_image = cv2.resize(tv_img_array, (tv_img_shape[1] // 2, tv_img_shape[0] // 2)) + cv2.imshow("record image", tv_resized_image) + key = cv2.waitKey(1) & 0xFF + if key == ord('q'): + running = False + if args.sim: + publish_reset_category(2, reset_pose_publisher) + elif key == ord('s'): + should_toggle_recording = True + elif key == ord('a'): + if args.sim: + publish_reset_category(2, reset_pose_publisher) + + if args.record and should_toggle_recording: + should_toggle_recording = False + if not is_recording: + if recorder.create_episode(): + is_recording = True + else: + logger_mp.error("Failed to create episode. Recording not started.") + else: + is_recording = False + recorder.save_episode() + if args.sim: + publish_reset_category(1, reset_pose_publisher) + # get input data + tele_data = tv_wrapper.get_motion_state_data() + if (args.ee == "dex3" or args.ee == "inspire1" or args.ee == "brainco") and args.xr_mode == "hand": + with left_hand_pos_array.get_lock(): + left_hand_pos_array[:] = tele_data.left_hand_pos.flatten() + with right_hand_pos_array.get_lock(): + right_hand_pos_array[:] = tele_data.right_hand_pos.flatten() + elif args.ee == "dex1" and args.xr_mode == "controller": + with left_gripper_value.get_lock(): + left_gripper_value.value = tele_data.left_trigger_value + with right_gripper_value.get_lock(): + right_gripper_value.value = tele_data.right_trigger_value + elif args.ee == "dex1" and args.xr_mode == "hand": + with left_gripper_value.get_lock(): + left_gripper_value.value = tele_data.left_pinch_value + with right_gripper_value.get_lock(): + right_gripper_value.value = tele_data.right_pinch_value + else: + pass + + # high level control + if args.xr_mode == "controller" and args.motion: + # quit teleoperate + if tele_data.tele_state.right_aButton: + running = False + stop_listening() + # command robot to enter damping mode. soft emergency stop function + if tele_data.tele_state.left_thumbstick_state and tele_data.tele_state.right_thumbstick_state: + sport_client.Damp() + # control, limit velocity to within 0.3 + sport_client.Move(-tele_data.tele_state.left_thumbstick_value[1] * 0.3, + -tele_data.tele_state.left_thumbstick_value[0] * 0.3, + -tele_data.tele_state.right_thumbstick_value[0] * 0.3) + + # get current robot state data. + # current_lr_arm_q = arm_ctrl.get_current_dual_arm_q() + # current_lr_arm_dq = arm_ctrl.get_current_dual_arm_dq() + + # solve ik using motor data and wrist pose, then use ik results to control arms. + time_ik_start = time.time() + # sol_q, sol_tauff = arm_ik.solve_ik(tele_data.left_arm_pose, tele_data.right_arm_pose, current_lr_arm_q, current_lr_arm_dq) + time_ik_end = time.time() + logger_mp.debug(f"ik:\t{round(time_ik_end - time_ik_start, 6)}") + # arm_ctrl.ctrl_dual_arm(sol_q, sol_tauff) + + # record data + if args.record: + # dex hand or gripper + if args.ee == "dex3" and args.xr_mode == "hand": + with dual_hand_data_lock: + left_ee_state = dual_hand_state_array[:7] + right_ee_state = dual_hand_state_array[-7:] + left_hand_action = dual_hand_action_array[:7] + right_hand_action = dual_hand_action_array[-7:] + current_body_state = [] + current_body_action = [] + elif args.ee == "dex1" and args.xr_mode == "hand": + with dual_gripper_data_lock: + left_ee_state = [dual_gripper_state_array[0]] + right_ee_state = [dual_gripper_state_array[1]] + left_hand_action = [dual_gripper_action_array[0]] + right_hand_action = [dual_gripper_action_array[1]] + current_body_state = [] + current_body_action = [] + elif args.ee == "dex1" and args.xr_mode == "controller": + with dual_gripper_data_lock: + left_ee_state = [dual_gripper_state_array[0]] + right_ee_state = [dual_gripper_state_array[1]] + left_hand_action = [dual_gripper_action_array[0]] + right_hand_action = [dual_gripper_action_array[1]] + # current_body_state = arm_ctrl.get_current_motor_q().tolist() + current_body_action = [-tele_data.tele_state.left_thumbstick_value[1] * 0.3, + -tele_data.tele_state.left_thumbstick_value[0] * 0.3, + -tele_data.tele_state.right_thumbstick_value[0] * 0.3] + elif (args.ee == "inspire1" or args.ee == "brainco") and args.xr_mode == "hand": + with dual_hand_data_lock: + left_ee_state = dual_hand_state_array[:6] + right_ee_state = dual_hand_state_array[-6:] + left_hand_action = dual_hand_action_array[:6] + right_hand_action = dual_hand_action_array[-6:] + current_body_state = [] + current_body_action = [] + else: + left_ee_state = [] + right_ee_state = [] + left_hand_action = [] + right_hand_action = [] + current_body_state = [] + current_body_action = [] + # head image + current_tv_image = tv_img_array.copy() + # wrist image + if WRIST: + current_wrist_image = wrist_img_array.copy() + # arm state and action + # left_arm_state = current_lr_arm_q[:7] + # right_arm_state = current_lr_arm_q[-7:] + # left_arm_action = sol_q[:7] + # right_arm_action = sol_q[-7:] + if is_recording: + colors = {} + depths = {} + if BINOCULAR: + colors[f"color_{0}"] = current_tv_image[:, :tv_img_shape[1]//2] + colors[f"color_{1}"] = current_tv_image[:, tv_img_shape[1]//2:] + if WRIST: + colors[f"color_{2}"] = current_wrist_image[:, :wrist_img_shape[1]//2] + colors[f"color_{3}"] = current_wrist_image[:, wrist_img_shape[1]//2:] + else: + colors[f"color_{0}"] = current_tv_image + if WRIST: + colors[f"color_{1}"] = current_wrist_image[:, :wrist_img_shape[1]//2] + colors[f"color_{2}"] = current_wrist_image[:, wrist_img_shape[1]//2:] + states = { + "left_arm": { + # "qpos": left_arm_state.tolist(), # numpy.array -> list + "qvel": [], + "torque": [], + }, + "right_arm": { + # "qpos": right_arm_state.tolist(), + "qvel": [], + "torque": [], + }, + "left_ee": { + "qpos": left_ee_state, + "qvel": [], + "torque": [], + }, + "right_ee": { + "qpos": right_ee_state, + "qvel": [], + "torque": [], + }, + "body": { + "qpos": current_body_state, + }, + } + actions = { + "left_arm": { + # "qpos": left_arm_action.tolist(), + "qvel": [], + "torque": [], + }, + "right_arm": { + # "qpos": right_arm_action.tolist(), + "qvel": [], + "torque": [], + }, + "left_ee": { + "qpos": left_hand_action, + "qvel": [], + "torque": [], + }, + "right_ee": { + "qpos": right_hand_action, + "qvel": [], + "torque": [], + }, + "body": { + "qpos": current_body_action, + }, + } + + + if args.carpet_tactile: + print(111) + carpet_tactiles = dict() + tactile_data = get_tactile_data() + carpet_tactiles['carpet_0'] = tactile_data + + if not args.carpet_headless: + tactile_render = (tactile_data / args.carpet_sensitivity) * 255 + tactile_render = np.clip(tactile_render, 0, 255) + tactile_render = cv2.resize(tactile_render.astype(np.uint8), (500, 500)) + cv2.imshow("carpet_0", tactile_render) + cv2.waitKey(1) + + else: + carpet_tactiles = None + + if args.sim: + sim_state = sim_state_subscriber.read_data() + recorder.add_item(colors=colors, depths=depths, states=states, actions=actions, carpet_tactiles=carpet_tactiles, sim_state=sim_state) + else: + recorder.add_item(colors=colors, depths=depths, states=states, actions=actions, carpet_tactiles=carpet_tactiles) + + current_time = time.time() + time_elapsed = current_time - start_time + sleep_time = max(0, (1 / args.frequency) - time_elapsed) + time.sleep(sleep_time) + logger_mp.debug(f"main process sleep: {sleep_time}") + + except KeyboardInterrupt: + logger_mp.info("KeyboardInterrupt, exiting program...") + except Exception as e: + logger_mp.error(f"An error occurred: {e}") + logger_mp.info("Exiting program due to an error...") + finally: + # arm_ctrl.ctrl_dual_arm_go_home() + if args.sim: + sim_state_subscriber.stop_subscribe() + tv_img_shm.close() + tv_img_shm.unlink() + if WRIST: + wrist_img_shm.close() + wrist_img_shm.unlink() + if args.record: + recorder.close() + listen_keyboard_thread.join() + logger_mp.info("Finally, exiting program...") + exit(0) From 083f615a51235d283aa0453f493cc404011580c6 Mon Sep 17 00:00:00 2001 From: eunjuyummy Date: Sat, 16 Aug 2025 12:38:22 +0900 Subject: [PATCH 4/7] mod: add third images --- teleop/utils/episode_writer.py | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/teleop/utils/episode_writer.py b/teleop/utils/episode_writer.py index 3f2510a..db62bcb 100644 --- a/teleop/utils/episode_writer.py +++ b/teleop/utils/episode_writer.py @@ -103,12 +103,14 @@ def create_episode(self): self.depth_dir = os.path.join(self.episode_dir, 'depths') self.audio_dir = os.path.join(self.episode_dir, 'audios') self.carpet_tactile_dir = os.path.join(self.episode_dir, 'carpet_tactiles') + self.third_image_dir = os.path.join(self.episode_dir, 'third_images') self.json_path = os.path.join(self.episode_dir, 'data.json') os.makedirs(self.episode_dir, exist_ok=True) os.makedirs(self.color_dir, exist_ok=True) os.makedirs(self.depth_dir, exist_ok=True) os.makedirs(self.audio_dir, exist_ok=True) os.makedirs(self.carpet_tactile_dir, exist_ok=True) + os.makedirs(self.third_image_dir, exist_ok=True) if self.rerun_log: self.online_logger = RerunLogger(prefix="online/", IdxRangeBoundary = 60, memory_limit="300MB") @@ -116,7 +118,7 @@ def create_episode(self): logger_mp.info(f"==> New episode created: {self.episode_dir}") return True # Return True if the episode is successfully created - def add_item(self, colors, depths=None, states=None, actions=None, tactiles=None, audios=None, sim_state=None, carpet_tactiles=None): + def add_item(self, colors, depths=None, states=None, actions=None, tactiles=None, audios=None, sim_state=None, carpet_tactiles=None, third_images=None): # Increment the item ID self.item_id += 1 # Create the item data dictionary @@ -130,6 +132,7 @@ def add_item(self, colors, depths=None, states=None, actions=None, tactiles=None 'audios': audios, 'sim_state': sim_state, 'carpet_tactiles': carpet_tactiles, + 'third_images': third_images, } # Enqueue the item data self.item_data_queue.put(item_data) @@ -157,6 +160,7 @@ def _process_item_data(self, item_data): depths = item_data.get('depths', {}) audios = item_data.get('audios', {}) carpet_tactiles = item_data.get('carpet_tactiles', {}) + third_images = item_data.get('third_images', {}) # Save images if colors: @@ -186,6 +190,13 @@ def _process_item_data(self, item_data): carpet_name = f'carpet_{str(idx).zfill(6)}_{carpet_key}.npy' np.save(os.path.join(self.carpet_tactile_dir, carpet_name), carpet_data.astype(np.float32)) item_data['carpet_tactiles'][carpet_key] = os.path.join(self.episode_dir, carpet_name) + + if third_images: + for idx_third, (third_key, third) in enumerate(third_images.items()): + third_name = f'{str(idx).zfill(6)}_{third_key}.jpg' + if not cv2.imwrite(os.path.join(self.third_image_dir, third_name), third): + logger_mp.info(f"Failed to save third_image.") + item_data['third_images'][third_key] = os.path.join('third_images', third_name) # Update episode data From eae102898dd293796d0773fafb5fe46be8dfadf6 Mon Sep 17 00:00:00 2001 From: eunjuyummy Date: Sat, 16 Aug 2025 13:10:18 +0900 Subject: [PATCH 5/7] add third camera --- teleop/teleop_hand_and_arm.py | 65 ++- teleop/teleop_hand_and_arm_norobot.py | 14 +- teleop/teleop_hand_and_arm_third_norobot.py | 534 ++++++++++++++++++++ teleop/utils/third_camera.py | 139 +++++ 4 files changed, 733 insertions(+), 19 deletions(-) create mode 100644 teleop/teleop_hand_and_arm_third_norobot.py create mode 100644 teleop/utils/third_camera.py diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 97e68ea..47a30a3 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -18,9 +18,8 @@ parent_dir = os.path.dirname(current_dir) sys.path.append(parent_dir) -from televuer import TeleVuerWrapper -from teleop.robot_control.robot_arm import G1_29_ArmController, G1_23_ArmController, H1_2_ArmController, \ - H1_ArmController +from televuer import TeleVuerWrapper +from teleop.robot_control.robot_arm import G1_29_ArmController, G1_23_ArmController, H1_2_ArmController, H1_ArmController from teleop.robot_control.robot_arm_ik import G1_29_ArmIK, G1_23_ArmIK, H1_2_ArmIK, H1_ArmIK from teleop.robot_control.robot_hand_unitree import Dex3_1_Controller, Dex1_1_Gripper_Controller from teleop.robot_control.robot_hand_inspire import Inspire_Controller @@ -28,6 +27,7 @@ from teleop.image_server.image_client import ImageClient from teleop.utils.episode_writer import EpisodeWriter from sshkeyboard import listen_keyboard, stop_listening +from teleop.utils.third_camera import _third_camera # for simulation from unitree_sdk2py.core.channel import ChannelPublisher @@ -84,13 +84,12 @@ def on_press(key): parser.add_argument('--headless', action='store_true', help='Enable headless mode (no display)') parser.add_argument('--sim', action='store_true', help='Enable isaac simulation mode') parser.add_argument('--carpet_tactile', action='store_true', help='Enable carpet tactile sensor data collection') - parser.add_argument('--carpet_sensitivity', type=int, default=250, - help='Set carpet tactile sensor sensitivity (default: 1.0)') - parser.add_argument('--carpet_headless', action='store_true', - help='Enable headless mode for carpet tactile sensor data collection') - parser.add_argument('--carpet_tty', type=str, default='/dev/tty.usbserial-02857AC6', - help='Set the TTY port for carpet tactile sensors (default: /dev/tty.usbserial-02857AC6)') - parser.add_argument() + parser.add_argument('--carpet_sensitivity', type=int, default=250, help='Set carpet tactile sensor sensitivity (default: 1.0)') + parser.add_argument('--carpet_headless', action='store_true', help='Enable headless mode for carpet tactile sensor data collection') + parser.add_argument('--carpet_tty', type=str, default='/dev/tty.usbserial-02857AC6', help='Set the TTY port for carpet tactile sensors (default: /dev/tty.usbserial-02857AC6)') + parser.add_argument('--third_camera', action='store_true', help='Enable third camera (RealSense color via UVC)') + parser.add_argument('--third_camera_device', type=str, default='/dev/video4', help='Device path for third camera (default: /dev/video4)') + parser.add_argument('--third_camera_fps', type=int, default=30, help='Third camera fps (default: 30)') args = parser.parse_args() logger_mp.info(f"args: {args}") @@ -105,6 +104,9 @@ def on_press(key): 'wrist_camera_type': 'opencv', 'wrist_camera_image_shape': [480, 640], # Wrist camera resolution 'wrist_camera_id_numbers': [2, 4], + 'third_camera_type': 'opencv', + 'third_camera_image_shape': [480, 640], # Third camera resolution + 'third_camera_id_numbers': [5], # TODO: change the camera id } else: img_config = { @@ -115,8 +117,12 @@ def on_press(key): 'wrist_camera_type': 'opencv', 'wrist_camera_image_shape': [480, 640], # Wrist camera resolution 'wrist_camera_id_numbers': [2, 4], + 'third_camera_type': 'opencv', + 'third_camera_image_shape': [480, 640], # Third camera resolution + 'third_camera_id_numbers': [5], # TODO: change the camera id } + # carpet tactile sensors base_images = list() if args.carpet_tactile: carpet_sensor = MultiSensors([args.carpet_tty]) @@ -150,6 +156,8 @@ def get_tactile_data(): else: WRIST = False + THIRD = bool(args.third_camera) + if BINOCULAR and not (img_config['head_camera_image_shape'][1] / img_config['head_camera_image_shape'][ 0] > ASPECT_RATIO_THRESHOLD): tv_img_shape = (img_config['head_camera_image_shape'][0], img_config['head_camera_image_shape'][1] * 2, 3) @@ -175,10 +183,24 @@ def get_tactile_data(): else: img_client = ImageClient(tv_img_shape=tv_img_shape, tv_img_shm_name=tv_img_shm.name) + if THIRD: + third_img_shape = (img_config['third_camera_image_shape'][0], img_config['third_camera_image_shape'][1], 3) + third_img_shm = shared_memory.SharedMemory(create=True, size=np.prod(third_img_shape) * np.uint8().itemsize) + third_img_array = np.ndarray(third_img_shape, dtype=np.uint8, buffer=third_img_shm.buf) + third_stop_event = threading.Event() + third_stop_event.clear() + third_camera_thread = threading.Thread( + target=_third_camera, + args=(args.third_camera_device, third_img_shape, third_img_array, args.third_camera_fps, third_stop_event, logger_mp), + daemon=True + ) + third_camera_thread.start() + logger_mp.info("third camera thred is starting") + image_receive_thread = threading.Thread(target=img_client.receive_process, daemon=True) image_receive_thread.daemon = True image_receive_thread.start() - + # television: obtain hand pose data from the XR device and transmit the robot's head camera image to the XR device. tv_wrapper = TeleVuerWrapper(binocular=BINOCULAR, use_hand_tracking=args.xr_mode == "hand", img_shape=tv_img_shape, img_shm_name=tv_img_shm.name, @@ -268,6 +290,9 @@ def get_tactile_data(): if not args.headless: tv_resized_image = cv2.resize(tv_img_array, (tv_img_shape[1] // 2, tv_img_shape[0] // 2)) cv2.imshow("record image", tv_resized_image) + if THIRD: + third_resized = cv2.resize(third_img_array, (third_img_shape[1] // 2, third_img_shape[0] // 2)) + cv2.imshow("third camera", third_resized) key = cv2.waitKey(1) & 0xFF if key == ord('q'): running = False @@ -386,6 +411,8 @@ def get_tactile_data(): # wrist image if WRIST: current_wrist_image = wrist_img_array.copy() + if THIRD: + current_third_image = third_img_array.copy() # arm state and action left_arm_state = current_lr_arm_q[:7] right_arm_state = current_lr_arm_q[-7:] @@ -394,6 +421,7 @@ def get_tactile_data(): if is_recording: colors = {} depths = {} + third_images = {} if BINOCULAR: colors[f"color_{0}"] = current_tv_image[:, :tv_img_shape[1] // 2] colors[f"color_{1}"] = current_tv_image[:, tv_img_shape[1] // 2:] @@ -405,6 +433,11 @@ def get_tactile_data(): if WRIST: colors[f"color_{1}"] = current_wrist_image[:, :wrist_img_shape[1] // 2] colors[f"color_{2}"] = current_wrist_image[:, wrist_img_shape[1] // 2:] + if THIRD: + third_images[f"third_{0}"] = current_third_image + else: + third_image = None + states = { "left_arm": { "qpos": left_arm_state.tolist(), # numpy.array -> list @@ -474,10 +507,10 @@ def get_tactile_data(): if args.sim: sim_state = sim_state_subscriber.read_data() recorder.add_item(colors=colors, depths=depths, states=states, actions=actions, - carpet_tactiles=carpet_tactiles, sim_state=sim_state) + carpet_tactiles=carpet_tactiles, sim_state=sim_state, third_images=third_images) else: recorder.add_item(colors=colors, depths=depths, states=states, actions=actions, - carpet_tactiles=carpet_tactiles) + carpet_tactiles=carpet_tactiles, third_images=third_images) current_time = time.time() time_elapsed = current_time - start_time @@ -487,6 +520,9 @@ def get_tactile_data(): except KeyboardInterrupt: logger_mp.info("KeyboardInterrupt, exiting program...") + except Exception as e: + logger_mp.error(f"An error occurred: {e}") + logger_mp.info("Exiting program due to an error...") finally: arm_ctrl.ctrl_dual_arm_go_home() if args.sim: @@ -496,6 +532,9 @@ def get_tactile_data(): if WRIST: wrist_img_shm.close() wrist_img_shm.unlink() + if THIRD: + third_img_shm.close() + third_img_shm.unlink() if args.record: recorder.close() listen_keyboard_thread.join() diff --git a/teleop/teleop_hand_and_arm_norobot.py b/teleop/teleop_hand_and_arm_norobot.py index 873e471..d0a86ad 100644 --- a/teleop/teleop_hand_and_arm_norobot.py +++ b/teleop/teleop_hand_and_arm_norobot.py @@ -198,11 +198,11 @@ def get_tactile_data(): dual_gripper_action_array = Array('d', 2, lock=False) # current left, right gripper action(2) data. gripper_ctrl = Dex1_1_Gripper_Controller(left_gripper_value, right_gripper_value, dual_gripper_data_lock, dual_gripper_state_array, dual_gripper_action_array, simulation_mode=args.sim) elif args.ee == "inspire1": - # left_hand_pos_array = Array('d', 75, lock = True) # [input] - # right_hand_pos_array = Array('d', 75, lock = True) # [input] - # dual_hand_data_lock = Lock() - # dual_hand_state_array = Array('d', 12, lock = False) # [output] current left, right hand state(12) data. - # dual_hand_action_array = Array('d', 12, lock = False) # [output] current left, right hand action(12) data. + left_hand_pos_array = Array('d', 75, lock = True) # [input] + right_hand_pos_array = Array('d', 75, lock = True) # [input] + dual_hand_data_lock = Lock() + dual_hand_state_array = Array('d', 12, lock = False) # [output] current left, right hand state(12) data. + dual_hand_action_array = Array('d', 12, lock = False) # [output] current left, right hand action(12) data. # hand_ctrl = Inspire_Controller(left_hand_pos_array, right_hand_pos_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array, simulation_mode=args.sim) hand_ctrl= None elif args.ee == "brainco": @@ -316,6 +316,7 @@ def get_tactile_data(): # record data if args.record: + logger_mp.debug("record") # dex hand or gripper if args.ee == "dex3" and args.xr_mode == "hand": with dual_hand_data_lock: @@ -369,6 +370,7 @@ def get_tactile_data(): # left_arm_action = sol_q[:7] # right_arm_action = sol_q[-7:] if is_recording: + logger_mp.debug("is_recording") colors = {} depths = {} if BINOCULAR: @@ -435,7 +437,7 @@ def get_tactile_data(): if args.carpet_tactile: - print(111) + logger_mp.debug("carpet_tactile") carpet_tactiles = dict() tactile_data = get_tactile_data() carpet_tactiles['carpet_0'] = tactile_data diff --git a/teleop/teleop_hand_and_arm_third_norobot.py b/teleop/teleop_hand_and_arm_third_norobot.py new file mode 100644 index 0000000..f59c516 --- /dev/null +++ b/teleop/teleop_hand_and_arm_third_norobot.py @@ -0,0 +1,534 @@ +import numpy as np +import time +import argparse +import cv2 +from multiprocessing import shared_memory, Value, Array, Lock +import threading +import logging_mp + +from teleop.carpet_tactile.sensors.sensors import MultiSensors + +logging_mp.basic_config(level=logging_mp.INFO) +logger_mp = logging_mp.get_logger(__name__) + +import os +import sys + +current_dir = os.path.dirname(os.path.abspath(__file__)) +parent_dir = os.path.dirname(current_dir) +sys.path.append(parent_dir) + +from televuer import TeleVuerWrapper +from teleop.robot_control.robot_arm import G1_29_ArmController, G1_23_ArmController, H1_2_ArmController, H1_ArmController +#from teleop.robot_control.robot_arm_ik import G1_29_ArmIK, G1_23_ArmIK, H1_2_ArmIK, H1_ArmIK +#from teleop.robot_control.robot_hand_unitree import Dex3_1_Controller, Dex1_1_Gripper_Controller +#from teleop.robot_control.robot_hand_inspire import Inspire_Controller +#from teleop.robot_control.robot_hand_brainco import Brainco_Controller +from teleop.image_server.image_client import ImageClient +from teleop.utils.episode_writer import EpisodeWriter +from sshkeyboard import listen_keyboard, stop_listening +from teleop.utils.third_camera import _third_camera + +# for simulation +from unitree_sdk2py.core.channel import ChannelPublisher +from unitree_sdk2py.idl.std_msgs.msg.dds_ import String_ + + +def publish_reset_category(category: int, publisher): # Scene Reset signal + msg = String_(data=str(category)) + publisher.Write(msg) + logger_mp.info(f"published reset category: {category}") + + +# state transition +start_signal = False +running = True +should_toggle_recording = False +is_recording = False + + +def on_press(key): + global running, start_signal, should_toggle_recording + if key == 'r': + start_signal = True + logger_mp.info("Program start signal received.") + elif key == 'q': + stop_listening() + running = False + elif key == 's': + should_toggle_recording = True + else: + logger_mp.info(f"{key} was pressed, but no action is defined for this key.") + + +listen_keyboard_thread = threading.Thread(target=listen_keyboard, kwargs={"on_press": on_press, "until": None, "sequential": False,}, daemon=True) +listen_keyboard_thread.start() + +import multiprocessing as mp + +mp.set_start_method('fork', force=True) # Use spawn method for multiprocessing + +if __name__ == '__main__': + parser = argparse.ArgumentParser() + parser.add_argument('--task_dir', type=str, default='./utils/data', help='path to save data') + parser.add_argument('--frequency', type=float, default=60.0, help='save data\'s frequency') + + # basic control parameters + parser.add_argument('--xr-mode', type=str, choices=['hand', 'controller'], default='hand', help='Select XR device tracking source') + parser.add_argument('--arm', type=str, choices=['G1_29', 'G1_23', 'H1_2', 'H1'], default='G1_29', help='Select arm controller') + parser.add_argument('--ee', type=str, choices=['dex1', 'dex3', 'inspire1', 'brainco'], help='Select end effector controller') + # mode flags + parser.add_argument('--record', action='store_true', help='Enable data recording') + parser.add_argument('--motion', action='store_true', help='Enable motion control mode') + parser.add_argument('--headless', action='store_true', help='Enable headless mode (no display)') + parser.add_argument('--sim', action='store_true', help='Enable isaac simulation mode') + parser.add_argument('--carpet_tactile', action='store_true', help='Enable carpet tactile sensor data collection') + parser.add_argument('--carpet_sensitivity', type=int, default=250, help='Set carpet tactile sensor sensitivity (default: 1.0)') + parser.add_argument('--carpet_headless', action='store_true', help='Enable headless mode for carpet tactile sensor data collection') + parser.add_argument('--carpet_tty', type=str, default='/dev/tty.usbserial-02857AC6', help='Set the TTY port for carpet tactile sensors (default: /dev/tty.usbserial-02857AC6)') + parser.add_argument('--third_camera', action='store_true', help='Enable third camera (RealSense color via UVC)') + parser.add_argument('--third_camera_device', type=str, default='/dev/video4', help='Device path for third camera (default: /dev/video4)') + parser.add_argument('--third_camera_fps', type=int, default=30, help='Third camera fps (default: 30)') + + args = parser.parse_args() + logger_mp.info(f"args: {args}") + + # image client: img_config should be the same as the configuration in image_server.py (of Robot's development computing unit) + if args.sim: + img_config = { + 'fps': 30, + 'head_camera_type': 'opencv', + 'head_camera_image_shape': [480, 640], # Head camera resolution + 'head_camera_id_numbers': [0], + 'wrist_camera_type': 'opencv', + 'wrist_camera_image_shape': [480, 640], # Wrist camera resolution + 'wrist_camera_id_numbers': [2, 4], + } + else: + img_config = { + 'fps': 30, + 'head_camera_type': 'opencv', + 'head_camera_image_shape': [480, 1280], # Head camera resolution + 'head_camera_id_numbers': [0], + 'wrist_camera_type': 'opencv', + 'wrist_camera_image_shape': [480, 640], # Wrist camera resolution + 'wrist_camera_id_numbers': [2, 4], + 'third_camera_type': 'opencv', + 'third_camera_image_shape': [480, 640], # Third camera resolution + 'third_camera_id_numbers': [5], # TODO: change the camera id + } + + base_images = list() + if args.carpet_tactile: + carpet_sensor = MultiSensors([args.carpet_tty]) + logger_mp.info("initializing carpet tactile sensors...") + carpet_sensor.init_sensors() + logger_mp.info("initializing carpet tactile sensors...Done") + + for i in range(20): + total_image = carpet_sensor.get() + base_images.append(total_image) + + base_images = np.array(base_images) + base_image = np.mean(base_images, axis=0) + logger_mp.info("Carpet tactile sensors calibration done!") + + def get_tactile_data(): + total_image = carpet_sensor.get() + total_image = total_image - base_image + return total_image + + + ASPECT_RATIO_THRESHOLD = 2.0 # If the aspect ratio exceeds this value, it is considered binocular + if len(img_config['head_camera_id_numbers']) > 1 or (img_config['head_camera_image_shape'][1] / img_config['head_camera_image_shape'][0] > ASPECT_RATIO_THRESHOLD): + BINOCULAR = True + else: + BINOCULAR = False + if 'wrist_camera_type' in img_config: + WRIST = True + else: + WRIST = False + + THIRD = bool(args.third_camera) + + if BINOCULAR and not (img_config['head_camera_image_shape'][1] / img_config['head_camera_image_shape'][0] > ASPECT_RATIO_THRESHOLD): + tv_img_shape = (img_config['head_camera_image_shape'][0], img_config['head_camera_image_shape'][1] * 2, 3) + else: + tv_img_shape = (img_config['head_camera_image_shape'][0], img_config['head_camera_image_shape'][1], 3) + + tv_img_shm = shared_memory.SharedMemory(create=True, size=np.prod(tv_img_shape) * np.uint8().itemsize) + tv_img_array = np.ndarray(tv_img_shape, dtype=np.uint8, buffer=tv_img_shm.buf) + + if WRIST and args.sim: + wrist_img_shape = (img_config['wrist_camera_image_shape'][0], img_config['wrist_camera_image_shape'][1] * 2, 3) + wrist_img_shm = shared_memory.SharedMemory(create=True, size=np.prod(wrist_img_shape) * np.uint8().itemsize) + wrist_img_array = np.ndarray(wrist_img_shape, dtype=np.uint8, buffer=wrist_img_shm.buf) + img_client = ImageClient(tv_img_shape=tv_img_shape, tv_img_shm_name=tv_img_shm.name, + wrist_img_shape=wrist_img_shape, wrist_img_shm_name=wrist_img_shm.name, server_address="127.0.0.1") + elif WRIST and not args.sim: + wrist_img_shape = (img_config['wrist_camera_image_shape'][0], img_config['wrist_camera_image_shape'][1] * 2, 3) + wrist_img_shm = shared_memory.SharedMemory(create=True, size=np.prod(wrist_img_shape) * np.uint8().itemsize) + wrist_img_array = np.ndarray(wrist_img_shape, dtype=np.uint8, buffer=wrist_img_shm.buf) + img_client = ImageClient(tv_img_shape=tv_img_shape, tv_img_shm_name=tv_img_shm.name, + wrist_img_shape=wrist_img_shape, wrist_img_shm_name=wrist_img_shm.name) + else: + img_client = ImageClient(tv_img_shape=tv_img_shape, tv_img_shm_name=tv_img_shm.name) + + if THIRD: + third_img_shape = (img_config['third_camera_image_shape'][0], img_config['third_camera_image_shape'][1], 3) + third_img_shm = shared_memory.SharedMemory(create=True, size=np.prod(third_img_shape) * np.uint8().itemsize) + third_img_array = np.ndarray(third_img_shape, dtype=np.uint8, buffer=third_img_shm.buf) + third_stop_event = threading.Event() + third_stop_event.clear() + third_camera_thread = threading.Thread( + target=_third_camera, + args=(args.third_camera_device, third_img_shape, third_img_array, args.third_camera_fps, third_stop_event, logger_mp), + daemon=True + ) + third_camera_thread.start() + logger_mp.info("third camera thred is starting") + + image_receive_thread = threading.Thread(target=img_client.receive_process, daemon=True) + image_receive_thread.daemon = True + image_receive_thread.start() + + # television: obtain hand pose data from the XR device and transmit the robot's head camera image to the XR device. + tv_wrapper = TeleVuerWrapper(binocular=BINOCULAR, use_hand_tracking=args.xr_mode == "hand", img_shape=tv_img_shape, img_shm_name=tv_img_shm.name, return_state_data=True, return_hand_rot_data=False) + + # arm + if args.arm == "G1_29": + #arm_ctrl = G1_29_ArmController(motion_mode=args.motion, simulation_mode=args.sim) + #arm_ik = G1_29_ArmIK() + pass + elif args.arm == "G1_23": + arm_ctrl = G1_23_ArmController(simulation_mode=args.sim) + arm_ik = G1_23_ArmIK() + elif args.arm == "H1_2": + arm_ctrl = H1_2_ArmController(simulation_mode=args.sim) + arm_ik = H1_2_ArmIK() + elif args.arm == "H1": + arm_ctrl = H1_ArmController(simulation_mode=args.sim) + arm_ik = H1_ArmIK() + + # end-effector + if args.ee == "dex3": + left_hand_pos_array = Array('d', 75, lock=True) # [input] + right_hand_pos_array = Array('d', 75, lock=True) # [input] + dual_hand_data_lock = Lock() + dual_hand_state_array = Array('d', 14, lock=False) # [output] current left, right hand state(14) data. + dual_hand_action_array = Array('d', 14, lock=False) # [output] current left, right hand action(14) data. + hand_ctrl = Dex3_1_Controller(left_hand_pos_array, right_hand_pos_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array, simulation_mode=args.sim) + elif args.ee == "dex1": + left_gripper_value = Value('d', 0.0, lock=True) # [input] + right_gripper_value = Value('d', 0.0, lock=True) # [input] + dual_gripper_data_lock = Lock() + dual_gripper_state_array = Array('d', 2, lock=False) # current left, right gripper state(2) data. + dual_gripper_action_array = Array('d', 2, lock=False) # current left, right gripper action(2) data. + gripper_ctrl = Dex1_1_Gripper_Controller(left_gripper_value, right_gripper_value, dual_gripper_data_lock, dual_gripper_state_array, dual_gripper_action_array, simulation_mode=args.sim) + elif args.ee == "inspire1": + left_hand_pos_array = Array('d', 75, lock=True) # [input] + right_hand_pos_array = Array('d', 75, lock=True) # [input] + dual_hand_data_lock = Lock() + dual_hand_state_array = Array('d', 12, lock=False) # [output] current left, right hand state(12) data. + dual_hand_action_array = Array('d', 12, lock=False) # [output] current left, right hand action(12) data. + #hand_ctrl = Inspire_Controller(left_hand_pos_array, right_hand_pos_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array, simulation_mode=args.sim) + #eft_hand_pos_array = [] + hand_ctrl = None + elif args.ee == "brainco": + left_hand_pos_array = Array('d', 75, lock=True) # [input] + right_hand_pos_array = Array('d', 75, lock=True) # [input] + dual_hand_data_lock = Lock() + dual_hand_state_array = Array('d', 12, lock=False) # [output] current left, right hand state(12) data. + dual_hand_action_array = Array('d', 12, lock=False) # [output] current left, right hand action(12) data. + hand_ctrl = Brainco_Controller(left_hand_pos_array, right_hand_pos_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array, simulation_mode=args.sim) + else: + pass + + # simulation mode + if args.sim: + reset_pose_publisher = ChannelPublisher("rt/reset_pose/cmd", String_) + reset_pose_publisher.Init() + from teleop.utils.sim_state_topic import start_sim_state_subscribe + + sim_state_subscriber = start_sim_state_subscribe() + + # controller + motion mode + if args.xr_mode == "controller" and args.motion: + from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient + sport_client = LocoClient() + sport_client.SetTimeout(0.0001) + sport_client.Init() + + # record + headless mode + if args.record and args.headless: + recorder = EpisodeWriter(task_dir=args.task_dir, frequency=args.frequency, rerun_log=False) + elif args.record and not args.headless: + recorder = EpisodeWriter(task_dir=args.task_dir, frequency=args.frequency, rerun_log=True) + + flag = False + #logger_mp.info("THIRD",THIRD) + #logger_mp.info("args.headless", args.headless) + try: + logger_mp.info("Please enter the start signal (enter 'r' to start the subsequent program)") + #while not start_signal: + # time.sleep(0.01) + #arm_ctrl.speed_gradual_max() + while running: + start_time = time.time() + + if not args.headless: + tv_resized_image = cv2.resize(tv_img_array, (tv_img_shape[1] // 2, tv_img_shape[0] // 2)) + #cv2.imshow("record image", tv_resized_image) + if THIRD: + third_resized = cv2.resize(third_img_array, (third_img_shape[1] // 2, third_img_shape[0] // 2)) + cv2.imshow("third camera", third_resized) + key = cv2.waitKey(1) & 0xFF + if key == ord('q'): + running = False + if args.sim: + publish_reset_category(2, reset_pose_publisher) + elif not flag: #key == ord('s'): + should_toggle_recording = True + flag = True + elif key == ord('a'): + if args.sim: + publish_reset_category(2, reset_pose_publisher) + + if args.record and should_toggle_recording: + should_toggle_recording = False + if not is_recording: + if recorder.create_episode(): + is_recording = True + else: + logger_mp.error("Failed to create episode. Recording not started.") + else: + is_recording = False + recorder.save_episode() + if args.sim: + publish_reset_category(1, reset_pose_publisher) + # get input data + tele_data = tv_wrapper.get_motion_state_data() + if (args.ee == "dex3" or args.ee == "inspire1" or args.ee == "brainco") and args.xr_mode == "hand": + with left_hand_pos_array.get_lock(): + left_hand_pos_array[:] = tele_data.left_hand_pos.flatten() + with right_hand_pos_array.get_lock(): + right_hand_pos_array[:] = tele_data.right_hand_pos.flatten() + elif args.ee == "dex1" and args.xr_mode == "controller": + with left_gripper_value.get_lock(): + left_gripper_value.value = tele_data.left_trigger_value + with right_gripper_value.get_lock(): + right_gripper_value.value = tele_data.right_trigger_value + elif args.ee == "dex1" and args.xr_mode == "hand": + with left_gripper_value.get_lock(): + left_gripper_value.value = tele_data.left_pinch_value + with right_gripper_value.get_lock(): + right_gripper_value.value = tele_data.right_pinch_value + else: + pass + + # high level control + if args.xr_mode == "controller" and args.motion: + # quit teleoperate + if tele_data.tele_state.right_aButton: + running = False + stop_listening() + # command robot to enter damping mode. soft emergency stop function + if tele_data.tele_state.left_thumbstick_state and tele_data.tele_state.right_thumbstick_state: + sport_client.Damp() + # control, limit velocity to within 0.3 + sport_client.Move(-tele_data.tele_state.left_thumbstick_value[1] * 0.3, + -tele_data.tele_state.left_thumbstick_value[0] * 0.3, + -tele_data.tele_state.right_thumbstick_value[0] * 0.3) + + # get current robot state data. + #current_lr_arm_q = arm_ctrl.get_current_dual_arm_q() + #current_lr_arm_dq = arm_ctrl.get_current_dual_arm_dq() + + # solve ik using motor data and wrist pose, then use ik results to control arms. + time_ik_start = time.time() + #sol_q, sol_tauff = arm_ik.solve_ik(tele_data.left_arm_pose, tele_data.right_arm_pose, current_lr_arm_q, current_lr_arm_dq) + time_ik_end = time.time() + logger_mp.debug(f"ik:\t{round(time_ik_end - time_ik_start, 6)}") + #arm_ctrl.ctrl_dual_arm(sol_q, sol_tauff) + + # record data + if args.record: + # dex hand or gripper + if args.ee == "dex3" and args.xr_mode == "hand": + with dual_hand_data_lock: + left_ee_state = dual_hand_state_array[:7] + right_ee_state = dual_hand_state_array[-7:] + left_hand_action = dual_hand_action_array[:7] + right_hand_action = dual_hand_action_array[-7:] + current_body_state = [] + current_body_action = [] + elif args.ee == "dex1" and args.xr_mode == "hand": + with dual_gripper_data_lock: + left_ee_state = [dual_gripper_state_array[0]] + right_ee_state = [dual_gripper_state_array[1]] + left_hand_action = [dual_gripper_action_array[0]] + right_hand_action = [dual_gripper_action_array[1]] + current_body_state = [] + current_body_action = [] + elif args.ee == "dex1" and args.xr_mode == "controller": + with dual_gripper_data_lock: + left_ee_state = [dual_gripper_state_array[0]] + right_ee_state = [dual_gripper_state_array[1]] + left_hand_action = [dual_gripper_action_array[0]] + right_hand_action = [dual_gripper_action_array[1]] + #current_body_state = arm_ctrl.get_current_motor_q().tolist() + current_body_action = [-tele_data.tele_state.left_thumbstick_value[1] * 0.3, + -tele_data.tele_state.left_thumbstick_value[0] * 0.3, + -tele_data.tele_state.right_thumbstick_value[0] * 0.3] + elif (args.ee == "inspire1" or args.ee == "brainco") and args.xr_mode == "hand": + with dual_hand_data_lock: + left_ee_state = dual_hand_state_array[:6] + right_ee_state = dual_hand_state_array[-6:] + left_hand_action = dual_hand_action_array[:6] + right_hand_action = dual_hand_action_array[-6:] + current_body_state = [] + current_body_action = [] + else: + left_ee_state = [] + right_ee_state = [] + left_hand_action = [] + right_hand_action = [] + current_body_state = [] + current_body_action = [] + # head image + current_tv_image = tv_img_array.copy() + # wrist image + if WRIST: + current_wrist_image = wrist_img_array.copy() + if THIRD: + current_third_image = third_img_array.copy() + # arm state and action + #left_arm_state = current_lr_arm_q[:7] + #right_arm_state = current_lr_arm_q[-7:] + #left_arm_action = sol_q[:7] + #right_arm_action = sol_q[-7:] + if is_recording: + logger_mp.debug("is_recording") + colors = {} + third_images = {} + depths = {} + if BINOCULAR: + colors[f"color_{0}"] = current_tv_image[:, :tv_img_shape[1] // 2] + colors[f"color_{1}"] = current_tv_image[:, tv_img_shape[1] // 2:] + if WRIST: + colors[f"color_{2}"] = current_wrist_image[:, :wrist_img_shape[1] // 2] + colors[f"color_{3}"] = current_wrist_image[:, wrist_img_shape[1] // 2:] + else: + colors[f"color_{0}"] = current_tv_image + if WRIST: + colors[f"color_{1}"] = current_wrist_image[:, :wrist_img_shape[1] // 2] + colors[f"color_{2}"] = current_wrist_image[:, wrist_img_shape[1] // 2:] + + if THIRD: + third_images[f"third_{0}"] = current_third_image + else: + third_image = None + + states = { + "left_arm": { + #"qpos": left_arm_state.tolist(), # numpy.array -> list + "qvel": [], + "torque": [], + }, + "right_arm": { + #"qpos": right_arm_state.tolist(), + "qvel": [], + "torque": [], + }, + "left_ee": { + "qpos": left_ee_state, + "qvel": [], + "torque": [], + }, + "right_ee": { + "qpos": right_ee_state, + "qvel": [], + "torque": [], + }, + "body": { + "qpos": current_body_state, + }, + } + actions = { + "left_arm": { + #"qpos": left_arm_action.tolist(), + "qvel": [], + "torque": [], + }, + "right_arm": { + #"qpos": right_arm_action.tolist(), + "qvel": [], + "torque": [], + }, + "left_ee": { + "qpos": left_hand_action, + "qvel": [], + "torque": [], + }, + "right_ee": { + "qpos": right_hand_action, + "qvel": [], + "torque": [], + }, + "body": { + "qpos": current_body_action, + }, + } + + if args.carpet_tactile: + logger_mp.debug("carpet_tactile is ready") + carpet_tactiles = dict() + tactile_data = get_tactile_data() + carpet_tactiles['carpet_0'] = tactile_data + + if not args.carpet_headless: + tactile_render = (tactile_data / args.carpet_sensitivity) * 255 + tactile_render = np.clip(tactile_render, 0, 255) + tactile_render = cv2.resize(tactile_render.astype(np.uint8), (500, 500)) + cv2.imshow("carpet_0", tactile_render) + cv2.waitKey(1) + + else: + carpet_tactiles = None + + if args.sim: + sim_state = sim_state_subscriber.read_data() + recorder.add_item(colors=colors, depths=depths, states=states, actions=actions, + carpet_tactiles=carpet_tactiles, sim_state=sim_state, third_images=third_images) + else: + recorder.add_item(colors=colors, depths=depths, states=states, actions=actions, + carpet_tactiles=carpet_tactiles, third_images=third_images) + + current_time = time.time() + time_elapsed = current_time - start_time + sleep_time = max(0, (1 / args.frequency) - time_elapsed) + time.sleep(sleep_time) + logger_mp.debug(f"main process sleep: {sleep_time}") + + except KeyboardInterrupt: + logger_mp.info("KeyboardInterrupt, exiting program...") + except Exception as e: + logger_mp.error(f"An error occurred: {e}") + logger_mp.info("Exiting program due to an error...") + finally: + #arm_ctrl.ctrl_dual_arm_go_home() + if args.sim: + sim_state_subscriber.stop_subscribe() + tv_img_shm.close() + tv_img_shm.unlink() + if WRIST: + wrist_img_shm.close() + wrist_img_shm.unlink() + if THIRD: + third_img_shm.close() + third_img_shm.unlink() + if args.record: + recorder.close() + listen_keyboard_thread.join() + logger_mp.info("Finally, exiting program...") + exit(0) \ No newline at end of file diff --git a/teleop/utils/third_camera.py b/teleop/utils/third_camera.py new file mode 100644 index 0000000..e58b6a2 --- /dev/null +++ b/teleop/utils/third_camera.py @@ -0,0 +1,139 @@ +import os +import time +import cv2 +import numpy as np + +def _third_camera( + device, + image_shape, # (H, W, 3) + out_array, # shared np.ndarray(dtype=uint8, shape=(H,W,3)) + fps, + stop_event, + logger, + *, + warmup_frames=5, + prefer_mjpg=True, + read_fail_threshold=30, + reopen_backoff_sec=0.5, + set_buffer_size=True, + resize_if_mismatch=True, +): + H, W = image_shape[0], image_shape[1] + + assert out_array.shape[0] == H and out_array.shape[1] == W and out_array.shape[2] == 3, \ + f"out_array shape must be ({H},{W},3), got {out_array.shape}" + assert out_array.dtype == np.uint8, "out_array dtype must be uint8" + + def _open_capture(): + if os.name != 'nt': + cap = cv2.VideoCapture(device, cv2.CAP_V4L2) + if not cap.isOpened(): + cap.release() + cap = cv2.VideoCapture(device) # fallback + else: + cap = cv2.VideoCapture(device) # Windows + return cap + + cap = _open_capture() + if not cap.isOpened(): + logger.error(f"[third camera] Failed to open device: {device}") + return + + try: + if prefer_mjpg: + if not cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*'MJPG')): + pass + else: + cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*'YUYV')) + + if set_buffer_size: + try: + cap.set(cv2.CAP_PROP_BUFFERSIZE, 1) + except Exception: + pass + + cap.set(cv2.CAP_PROP_FRAME_WIDTH, W) + cap.set(cv2.CAP_PROP_FRAME_HEIGHT, H) + cap.set(cv2.CAP_PROP_FPS, fps) + + act_W = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)) + act_H = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)) + act_FPS = float(cap.get(cv2.CAP_PROP_FPS)) or fps + fourcc = int(cap.get(cv2.CAP_PROP_FOURCC)) + fourcc_s = "".join([chr((fourcc >> 8*i) & 0xFF) for i in range(4)]) if fourcc != 0 else "unknown" + + logger.info(f"[third camera] requested {W}x{H}@{fps}, negotiated {act_W}x{act_H}@{act_FPS:.2f}, FOURCC={fourcc_s}") + + for _ in range(max(0, warmup_frames)): + ret, _ = cap.read() + if not ret: + break + + interval = 1.0 / max(1.0, float(fps)) + next_t = time.perf_counter() + interval + fail_cnt = 0 + + while not stop_event.is_set(): + ret, frame = cap.read() + if not ret: + fail_cnt += 1 + if fail_cnt >= read_fail_threshold: + logger.warning(f"[third camera] read failed x{fail_cnt}, reopening ({device})") + cap.release() + time.sleep(reopen_backoff_sec) + cap = _open_capture() + if not cap.isOpened(): + logger.error(f"[third camera] reopen failed: {device}") + time.sleep(reopen_backoff_sec) + continue + tried_mjpg = cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*'MJPG')) + if not tried_mjpg: + cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*'YUYV')) + cap.set(cv2.CAP_PROP_FRAME_WIDTH, W) + cap.set(cv2.CAP_PROP_FRAME_HEIGHT, H) + cap.set(cv2.CAP_PROP_FPS, fps) + if set_buffer_size: + try: cap.set(cv2.CAP_PROP_BUFFERSIZE, 1) + except Exception: pass + + fail_cnt = 0 + else: + time.sleep(0.005) + now = time.perf_counter() + if now - next_t > 2*interval: + next_t = now + interval + continue + + fail_cnt = 0 + + if resize_if_mismatch and (frame.shape[1] != W or frame.shape[0] != H): + frame = cv2.resize(frame, (W, H), interpolation=cv2.INTER_AREA) + + if frame.ndim == 2: + frame = cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR) + elif frame.ndim == 3 and frame.shape[2] == 4: # BGRA → BGR + frame = cv2.cvtColor(frame, cv2.COLOR_BGRA2BGR) + + if frame.dtype != np.uint8: + frame = np.clip(frame, 0, 255).astype(np.uint8) + + out_array[...] = frame + + now = time.perf_counter() + if now < next_t: + time.sleep(next_t - now) + next_t += interval + else: + missed = int((now - next_t) // interval) + 1 + next_t += missed * interval + if (now - next_t) > 2*interval: + next_t = now + interval + + except Exception as e: + logger.exception(f"[third camera] unexpected error: {e}") + finally: + try: + cap.release() + except Exception: + pass + logger.info("[third camera] capture thread stopped.") \ No newline at end of file From e38b09b22b45dbc29d665cd1da085464e63a490d Mon Sep 17 00:00:00 2001 From: eunjuyummy Date: Sat, 16 Aug 2025 21:31:32 +0900 Subject: [PATCH 6/7] mod image server --- teleop/image_server/image_server.py | 54 ++++++++++++++--------------- teleop/teleop_hand_and_arm.py | 14 ++++---- 2 files changed, 33 insertions(+), 35 deletions(-) diff --git a/teleop/image_server/image_server.py b/teleop/image_server/image_server.py index 19799fb..3a06e16 100644 --- a/teleop/image_server/image_server.py +++ b/teleop/image_server/image_server.py @@ -5,8 +5,6 @@ from collections import deque import numpy as np import pyrealsense2 as rs -import logging_mp -logger_mp = logging_mp.get_logger(__name__, level=logging_mp.DEBUG) class RealSenseCamera(object): @@ -39,7 +37,7 @@ def init_realsense(self): profile = self.pipeline.start(config) self._device = profile.get_device() if self._device is None: - logger_mp.error('[Image Server] pipe_profile.get_device() is None .') + print('[Image Server] pipe_profile.get_device() is None .') if self.enable_depth: assert self._device is not None depth_sensor = self._device.first_depth_sensor() @@ -84,7 +82,7 @@ def __init__(self, device_id, img_shape, fps): # Test if the camera can read frames if not self._can_read_frame(): - logger_mp.error(f"[Image Server] Camera {self.id} Error: Failed to initialize the camera or read frames. Exiting...") + print(f"[Image Server] Camera {self.id} Error: Failed to initialize the camera or read frames. Exiting...") self.release() def _can_read_frame(self): @@ -138,7 +136,7 @@ def __init__(self, config, port = 5555, Unit_Test = False): #'wrist_camera_id_numbers': ["218622271789", "241222076627"], # serial number (realsense) } """ - logger_mp.info(config) + print(config) self.fps = config.get('fps', 30) self.head_camera_type = config.get('head_camera_type', 'opencv') self.head_image_shape = config.get('head_camera_image_shape', [480, 640]) # (height, width) @@ -163,7 +161,7 @@ def __init__(self, config, port = 5555, Unit_Test = False): camera = RealSenseCamera(img_shape=self.head_image_shape, fps=self.fps, serial_number=serial_number) self.head_cameras.append(camera) else: - logger_mp.warning(f"[Image Server] Unsupported head_camera_type: {self.head_camera_type}") + print(f"[Image Server] Unsupported head_camera_type: {self.head_camera_type}") # Initialize wrist cameras if provided self.wrist_cameras = [] @@ -177,7 +175,7 @@ def __init__(self, config, port = 5555, Unit_Test = False): camera = RealSenseCamera(img_shape=self.wrist_image_shape, fps=self.fps, serial_number=serial_number) self.wrist_cameras.append(camera) else: - logger_mp.warning(f"[Image Server] Unsupported wrist_camera_type: {self.wrist_camera_type}") + print(f"[Image Server] Unsupported wrist_camera_type: {self.wrist_camera_type}") # Set ZeroMQ context and socket self.context = zmq.Context() @@ -189,21 +187,21 @@ def __init__(self, config, port = 5555, Unit_Test = False): for cam in self.head_cameras: if isinstance(cam, OpenCVCamera): - logger_mp.info(f"[Image Server] Head camera {cam.id} resolution: {cam.cap.get(cv2.CAP_PROP_FRAME_HEIGHT)} x {cam.cap.get(cv2.CAP_PROP_FRAME_WIDTH)}") + print(f"[Image Server] Head camera {cam.id} resolution: {cam.cap.get(cv2.CAP_PROP_FRAME_HEIGHT)} x {cam.cap.get(cv2.CAP_PROP_FRAME_WIDTH)}") elif isinstance(cam, RealSenseCamera): - logger_mp.info(f"[Image Server] Head camera {cam.serial_number} resolution: {cam.img_shape[0]} x {cam.img_shape[1]}") + print(f"[Image Server] Head camera {cam.serial_number} resolution: {cam.img_shape[0]} x {cam.img_shape[1]}") else: - logger_mp.warning("[Image Server] Unknown camera type in head_cameras.") + print("[Image Server] Unknown camera type in head_cameras.") for cam in self.wrist_cameras: if isinstance(cam, OpenCVCamera): - logger_mp.info(f"[Image Server] Wrist camera {cam.id} resolution: {cam.cap.get(cv2.CAP_PROP_FRAME_HEIGHT)} x {cam.cap.get(cv2.CAP_PROP_FRAME_WIDTH)}") + print(f"[Image Server] Wrist camera {cam.id} resolution: {cam.cap.get(cv2.CAP_PROP_FRAME_HEIGHT)} x {cam.cap.get(cv2.CAP_PROP_FRAME_WIDTH)}") elif isinstance(cam, RealSenseCamera): - logger_mp.info(f"[Image Server] Wrist camera {cam.serial_number} resolution: {cam.img_shape[0]} x {cam.img_shape[1]}") + print(f"[Image Server] Wrist camera {cam.serial_number} resolution: {cam.img_shape[0]} x {cam.img_shape[1]}") else: - logger_mp.warning("[Image Server] Unknown camera type in wrist_cameras.") + print("[Image Server] Unknown camera type in wrist_cameras.") - logger_mp.info("[Image Server] Image server has started, waiting for client connections...") + print("[Image Server] Image server has started, waiting for client connections...") @@ -226,7 +224,7 @@ def _print_performance_metrics(self, current_time): if self.frame_count % 30 == 0: elapsed_time = current_time - self.start_time real_time_fps = len(self.frame_times) / self.time_window - logger_mp.info(f"[Image Server] Real-time FPS: {real_time_fps:.2f}, Total frames sent: {self.frame_count}, Elapsed time: {elapsed_time:.2f} sec") + print(f"[Image Server] Real-time FPS: {real_time_fps:.2f}, Total frames sent: {self.frame_count}, Elapsed time: {elapsed_time:.2f} sec") def _close(self): for cam in self.head_cameras: @@ -235,7 +233,7 @@ def _close(self): cam.release() self.socket.close() self.context.term() - logger_mp.info("[Image Server] The server has been closed.") + print("[Image Server] The server has been closed.") def send_process(self): try: @@ -245,12 +243,12 @@ def send_process(self): if self.head_camera_type == 'opencv': color_image = cam.get_frame() if color_image is None: - logger_mp.error("[Image Server] Head camera frame read is error.") + print("[Image Server] Head camera frame read is error.") break elif self.head_camera_type == 'realsense': color_image, depth_iamge = cam.get_frame() if color_image is None: - logger_mp.error("[Image Server] Head camera frame read is error.") + print("[Image Server] Head camera frame read is error.") break head_frames.append(color_image) if len(head_frames) != len(self.head_cameras): @@ -263,12 +261,12 @@ def send_process(self): if self.wrist_camera_type == 'opencv': color_image = cam.get_frame() if color_image is None: - logger_mp.error("[Image Server] Wrist camera frame read is error.") + print("[Image Server] Wrist camera frame read is error.") break elif self.wrist_camera_type == 'realsense': color_image, depth_iamge = cam.get_frame() if color_image is None: - logger_mp.error("[Image Server] Wrist camera frame read is error.") + print("[Image Server] Wrist camera frame read is error.") break wrist_frames.append(color_image) wrist_color = cv2.hconcat(wrist_frames) @@ -280,7 +278,7 @@ def send_process(self): ret, buffer = cv2.imencode('.jpg', full_color) if not ret: - logger_mp.error("[Image Server] Frame imencode is failed.") + print("[Image Server] Frame imencode is failed.") continue jpg_bytes = buffer.tobytes() @@ -301,7 +299,7 @@ def send_process(self): self._print_performance_metrics(current_time) except KeyboardInterrupt: - logger_mp.warning("[Image Server] Interrupted by user.") + print("[Image Server] Interrupted by user.") finally: self._close() @@ -310,12 +308,12 @@ def send_process(self): config = { 'fps': 30, 'head_camera_type': 'opencv', - 'head_camera_image_shape': [480, 1280], # Head camera resolution - 'head_camera_id_numbers': [0], - 'wrist_camera_type': 'opencv', - 'wrist_camera_image_shape': [480, 640], # Wrist camera resolution - 'wrist_camera_id_numbers': [2, 4], + 'head_camera_image_shape': [480, 848], # Head camera resolution + 'head_camera_id_numbers': ['339222071291'], + #'wrist_camera_type': 'opencv', + #'wrist_camera_image_shape': [480, 640], # Wrist camera resolution + #'wrist_camera_id_numbers': [2, 4], } server = ImageServer(config, Unit_Test=False) - server.send_process() \ No newline at end of file + server.send_process() diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 47a30a3..93d8627 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -106,20 +106,20 @@ def on_press(key): 'wrist_camera_id_numbers': [2, 4], 'third_camera_type': 'opencv', 'third_camera_image_shape': [480, 640], # Third camera resolution - 'third_camera_id_numbers': [5], # TODO: change the camera id + 'third_camera_id_numbers': [6], } else: img_config = { 'fps': 30, 'head_camera_type': 'opencv', - 'head_camera_image_shape': [480, 1280], # Head camera resolution - 'head_camera_id_numbers': [0], - 'wrist_camera_type': 'opencv', - 'wrist_camera_image_shape': [480, 640], # Wrist camera resolution - 'wrist_camera_id_numbers': [2, 4], + 'head_camera_image_shape': [480, 848], # Head camera resolution + 'head_camera_id_numbers': ['339222071291'], + #'wrist_camera_type': 'opencv', + #'wrist_camera_image_shape': [480, 640], # Wrist camera resolution + #'wrist_camera_id_numbers': [2, 4], 'third_camera_type': 'opencv', 'third_camera_image_shape': [480, 640], # Third camera resolution - 'third_camera_id_numbers': [5], # TODO: change the camera id + 'third_camera_id_numbers': [6], } # carpet tactile sensors From 0e8b5e4632df08f95d45ce50d93d0d399aa5d75e Mon Sep 17 00:00:00 2001 From: eunjuyummy Date: Sat, 16 Aug 2025 22:31:48 +0900 Subject: [PATCH 7/7] mod server and client --- teleop/image_server/image_client.py | 14 ++++++- teleop/image_server/image_server.py | 61 +++++++++++++++++++++++++++-- teleop/teleop_hand_and_arm.py | 36 ++++++----------- teleop/utils/episode_writer.py | 13 +----- 4 files changed, 81 insertions(+), 43 deletions(-) diff --git a/teleop/image_server/image_client.py b/teleop/image_server/image_client.py index 9c42581..ea45c18 100644 --- a/teleop/image_server/image_client.py +++ b/teleop/image_server/image_client.py @@ -9,8 +9,8 @@ logger_mp = logging_mp.get_logger(__name__) class ImageClient: - def __init__(self, tv_img_shape = None, tv_img_shm_name = None, wrist_img_shape = None, wrist_img_shm_name = None, - image_show = False, server_address = "192.168.123.164", port = 5555, Unit_Test = False): + def __init__(self, tv_img_shape = None, tv_img_shm_name = None, wrist_img_shape = None, wrist_img_shm_name = None, + third_img_shape = None, third_img_shm_name = None, image_show = False, server_address = "192.168.123.164", port = 5555, Unit_Test = False): """ tv_img_shape: User's expected head camera resolution shape (H, W, C). It should match the output of the image service terminal. @@ -36,6 +36,7 @@ def __init__(self, tv_img_shape = None, tv_img_shm_name = None, wrist_img_shape self.tv_img_shape = tv_img_shape self.wrist_img_shape = wrist_img_shape + self.third_img_shape = third_img_shape self.tv_enable_shm = False if self.tv_img_shape is not None and tv_img_shm_name is not None: @@ -49,6 +50,12 @@ def __init__(self, tv_img_shape = None, tv_img_shm_name = None, wrist_img_shape self.wrist_img_array = np.ndarray(wrist_img_shape, dtype = np.uint8, buffer = self.wrist_image_shm.buf) self.wrist_enable_shm = True + self.third_enable_shm = False + if self.third_img_shape is not None and third_img_shm_name is not None: + self.third_image_shm = shared_memory.SharedMemory(name=third_img_shm_name) + self.third_img_array = np.ndarray(third_img_shape, dtype = np.uint8, buffer = self.third_image_shm.buf) + self.third_enable_shm = True + # Performance evaluation parameters self._enable_performance_eval = Unit_Test if self._enable_performance_eval: @@ -164,6 +171,9 @@ def receive_process(self): if self.wrist_enable_shm: np.copyto(self.wrist_img_array, np.array(current_image[:, -self.wrist_img_shape[1]:])) + if self.third_enable_shm: + np.copyto(self.third_img_array, np.array(current_image[:, -self.third_img_shape[1]:])) + if self._image_show: height, width = current_image.shape[:2] resized_image = cv2.resize(current_image, (width // 2, height // 2)) diff --git a/teleop/image_server/image_server.py b/teleop/image_server/image_server.py index 3a06e16..973163a 100644 --- a/teleop/image_server/image_server.py +++ b/teleop/image_server/image_server.py @@ -146,6 +146,10 @@ def __init__(self, config, port = 5555, Unit_Test = False): self.wrist_image_shape = config.get('wrist_camera_image_shape', [480, 640]) # (height, width) self.wrist_camera_id_numbers = config.get('wrist_camera_id_numbers', None) + self.third_camera_type = config.get('third_camera_type', None) + self.third_image_shape = config.get('third_camera_image_shape', [480, 640]) # (height, width) + self.third_camera_id_numbers = config.get('third_camera_id_numbers', None) + self.port = port self.Unit_Test = Unit_Test @@ -177,6 +181,20 @@ def __init__(self, config, port = 5555, Unit_Test = False): else: print(f"[Image Server] Unsupported wrist_camera_type: {self.wrist_camera_type}") + # Initialize third cameras if provided + self.third_cameras = [] + if self.third_camera_type and self.third_camera_id_numbers: + if self.third_camera_type == 'opencv': + for device_id in self.third_camera_id_numbers: + camera = OpenCVCamera(device_id=device_id, img_shape=self.third_image_shape, fps=self.fps) + self.third_cameras.append(camera) + elif self.third_camera_type == 'realsense': + for serial_number in self.third_camera_id_numbers: + camera = RealSenseCamera(img_shape=self.third_image_shape, fps=self.fps, serial_number=serial_number) + self.third_cameras.append(camera) + else: + print(f"[Image Server] Unsupported third_camera_type: {self.third_camera_type}") + # Set ZeroMQ context and socket self.context = zmq.Context() self.socket = self.context.socket(zmq.PUB) @@ -201,6 +219,14 @@ def __init__(self, config, port = 5555, Unit_Test = False): else: print("[Image Server] Unknown camera type in wrist_cameras.") + for cam in self.third_cameras: + if isinstance(cam, OpenCVCamera): + print(f"[Image Server] Third camera {cam.id} resolution: {cam.cap.get(cv2.CAP_PROP_FRAME_HEIGHT)} x {cam.cap.get(cv2.CAP_PROP_FRAME_WIDTH)}") + elif isinstance(cam, RealSenseCamera): + print(f"[Image Server] Third camera {cam.serial_number} resolution: {cam.img_shape[0]} x {cam.img_shape[1]}") + else: + print("[Image Server] Unknown camera type in third_cameras.") + print("[Image Server] Image server has started, waiting for client connections...") @@ -231,6 +257,8 @@ def _close(self): cam.release() for cam in self.wrist_cameras: cam.release() + for cam in self.third_cameras: + cam.release() self.socket.close() self.context.term() print("[Image Server] The server has been closed.") @@ -246,7 +274,7 @@ def send_process(self): print("[Image Server] Head camera frame read is error.") break elif self.head_camera_type == 'realsense': - color_image, depth_iamge = cam.get_frame() + color_image, depth_image = cam.get_frame() if color_image is None: print("[Image Server] Head camera frame read is error.") break @@ -254,7 +282,7 @@ def send_process(self): if len(head_frames) != len(self.head_cameras): break head_color = cv2.hconcat(head_frames) - + if self.wrist_cameras: wrist_frames = [] for cam in self.wrist_cameras: @@ -264,7 +292,7 @@ def send_process(self): print("[Image Server] Wrist camera frame read is error.") break elif self.wrist_camera_type == 'realsense': - color_image, depth_iamge = cam.get_frame() + color_image, depth_image = cam.get_frame() if color_image is None: print("[Image Server] Wrist camera frame read is error.") break @@ -275,7 +303,28 @@ def send_process(self): full_color = cv2.hconcat([head_color, wrist_color]) else: full_color = head_color + + if self.third_cameras: + third_frames = [] + for cam in self.third_cameras: + if self.third_camera_type == 'opencv': + color_image = cam.get_frame() + if color_image is None: + print("[Image Server] Third camera frame read is error.") + break + elif self.third_camera_type == 'realsense': + color_image, depth_image = cam.get_frame() + if color_image is None: + print("[Image Server] Third camera frame read is error.") + break + third_frames.append(color_image) + third_color = cv2.hconcat(third_frames) + # Concatenate head and third frames + full_color = cv2.hconcat([head_color, third_color]) + else: + full_color = head_color + ret, buffer = cv2.imencode('.jpg', full_color) if not ret: print("[Image Server] Frame imencode is failed.") @@ -307,12 +356,16 @@ def send_process(self): if __name__ == "__main__": config = { 'fps': 30, - 'head_camera_type': 'opencv', + 'head_camera_type': 'realsense', 'head_camera_image_shape': [480, 848], # Head camera resolution 'head_camera_id_numbers': ['339222071291'], #'wrist_camera_type': 'opencv', #'wrist_camera_image_shape': [480, 640], # Wrist camera resolution #'wrist_camera_id_numbers': [2, 4], + 'third_camera_type': 'realsense', + 'third_camera_image_shape': [480, 640], # Third camera resolution + 'third_camera_id_numbers': [6], # TODO: change the id number + } server = ImageServer(config, Unit_Test=False) diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 93d8627..d4ec3ef 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -27,7 +27,6 @@ from teleop.image_server.image_client import ImageClient from teleop.utils.episode_writer import EpisodeWriter from sshkeyboard import listen_keyboard, stop_listening -from teleop.utils.third_camera import _third_camera # for simulation from unitree_sdk2py.core.channel import ChannelPublisher @@ -88,8 +87,6 @@ def on_press(key): parser.add_argument('--carpet_headless', action='store_true', help='Enable headless mode for carpet tactile sensor data collection') parser.add_argument('--carpet_tty', type=str, default='/dev/tty.usbserial-02857AC6', help='Set the TTY port for carpet tactile sensors (default: /dev/tty.usbserial-02857AC6)') parser.add_argument('--third_camera', action='store_true', help='Enable third camera (RealSense color via UVC)') - parser.add_argument('--third_camera_device', type=str, default='/dev/video4', help='Device path for third camera (default: /dev/video4)') - parser.add_argument('--third_camera_fps', type=int, default=30, help='Third camera fps (default: 30)') args = parser.parse_args() logger_mp.info(f"args: {args}") @@ -119,7 +116,7 @@ def on_press(key): #'wrist_camera_id_numbers': [2, 4], 'third_camera_type': 'opencv', 'third_camera_image_shape': [480, 640], # Third camera resolution - 'third_camera_id_numbers': [6], + 'third_camera_id_numbers': [6], # TODO: change the camera id } # carpet tactile sensors @@ -138,7 +135,6 @@ def on_press(key): base_image = np.mean(base_images, axis=0) logger_mp.info("Carpet tactile sensors calibration done!") - def get_tactile_data(): total_image = carpet_sensor.get() total_image = total_image - base_image @@ -183,19 +179,12 @@ def get_tactile_data(): else: img_client = ImageClient(tv_img_shape=tv_img_shape, tv_img_shm_name=tv_img_shm.name) - if THIRD: + if THIRD and not args.sim: third_img_shape = (img_config['third_camera_image_shape'][0], img_config['third_camera_image_shape'][1], 3) third_img_shm = shared_memory.SharedMemory(create=True, size=np.prod(third_img_shape) * np.uint8().itemsize) third_img_array = np.ndarray(third_img_shape, dtype=np.uint8, buffer=third_img_shm.buf) - third_stop_event = threading.Event() - third_stop_event.clear() - third_camera_thread = threading.Thread( - target=_third_camera, - args=(args.third_camera_device, third_img_shape, third_img_array, args.third_camera_fps, third_stop_event, logger_mp), - daemon=True - ) - third_camera_thread.start() - logger_mp.info("third camera thred is starting") + img_client = ImageClient(tv_img_shape=tv_img_shape, tv_img_shm_name=tv_img_shm.name, + third_img_shape=third_img_shape, third_img_shm_name=third_img_shm.name) image_receive_thread = threading.Thread(target=img_client.receive_process, daemon=True) image_receive_thread.daemon = True @@ -290,9 +279,9 @@ def get_tactile_data(): if not args.headless: tv_resized_image = cv2.resize(tv_img_array, (tv_img_shape[1] // 2, tv_img_shape[0] // 2)) cv2.imshow("record image", tv_resized_image) - if THIRD: - third_resized = cv2.resize(third_img_array, (third_img_shape[1] // 2, third_img_shape[0] // 2)) - cv2.imshow("third camera", third_resized) + #if THIRD: + # third_resized = cv2.resize(third_img_array, (third_img_shape[1] // 2, third_img_shape[0] // 2)) + # cv2.imshow("third camera", third_resized) key = cv2.waitKey(1) & 0xFF if key == ord('q'): running = False @@ -433,10 +422,8 @@ def get_tactile_data(): if WRIST: colors[f"color_{1}"] = current_wrist_image[:, :wrist_img_shape[1] // 2] colors[f"color_{2}"] = current_wrist_image[:, wrist_img_shape[1] // 2:] - if THIRD: - third_images[f"third_{0}"] = current_third_image - else: - third_image = None + if THIRD: + colors[f"color_{3}"] = current_third_image[:, third_img_shape[1] // 2:] states = { "left_arm": { @@ -500,17 +487,16 @@ def get_tactile_data(): tactile_render = cv2.resize(tactile_render.astype(np.uint8), (500, 500)) cv2.imshow("carpet_0", tactile_render) cv2.waitKey(1) - else: carpet_tactiles = None if args.sim: sim_state = sim_state_subscriber.read_data() recorder.add_item(colors=colors, depths=depths, states=states, actions=actions, - carpet_tactiles=carpet_tactiles, sim_state=sim_state, third_images=third_images) + carpet_tactiles=carpet_tactiles, sim_state=sim_state) else: recorder.add_item(colors=colors, depths=depths, states=states, actions=actions, - carpet_tactiles=carpet_tactiles, third_images=third_images) + carpet_tactiles=carpet_tactiles) current_time = time.time() time_elapsed = current_time - start_time diff --git a/teleop/utils/episode_writer.py b/teleop/utils/episode_writer.py index db62bcb..3f2510a 100644 --- a/teleop/utils/episode_writer.py +++ b/teleop/utils/episode_writer.py @@ -103,14 +103,12 @@ def create_episode(self): self.depth_dir = os.path.join(self.episode_dir, 'depths') self.audio_dir = os.path.join(self.episode_dir, 'audios') self.carpet_tactile_dir = os.path.join(self.episode_dir, 'carpet_tactiles') - self.third_image_dir = os.path.join(self.episode_dir, 'third_images') self.json_path = os.path.join(self.episode_dir, 'data.json') os.makedirs(self.episode_dir, exist_ok=True) os.makedirs(self.color_dir, exist_ok=True) os.makedirs(self.depth_dir, exist_ok=True) os.makedirs(self.audio_dir, exist_ok=True) os.makedirs(self.carpet_tactile_dir, exist_ok=True) - os.makedirs(self.third_image_dir, exist_ok=True) if self.rerun_log: self.online_logger = RerunLogger(prefix="online/", IdxRangeBoundary = 60, memory_limit="300MB") @@ -118,7 +116,7 @@ def create_episode(self): logger_mp.info(f"==> New episode created: {self.episode_dir}") return True # Return True if the episode is successfully created - def add_item(self, colors, depths=None, states=None, actions=None, tactiles=None, audios=None, sim_state=None, carpet_tactiles=None, third_images=None): + def add_item(self, colors, depths=None, states=None, actions=None, tactiles=None, audios=None, sim_state=None, carpet_tactiles=None): # Increment the item ID self.item_id += 1 # Create the item data dictionary @@ -132,7 +130,6 @@ def add_item(self, colors, depths=None, states=None, actions=None, tactiles=None 'audios': audios, 'sim_state': sim_state, 'carpet_tactiles': carpet_tactiles, - 'third_images': third_images, } # Enqueue the item data self.item_data_queue.put(item_data) @@ -160,7 +157,6 @@ def _process_item_data(self, item_data): depths = item_data.get('depths', {}) audios = item_data.get('audios', {}) carpet_tactiles = item_data.get('carpet_tactiles', {}) - third_images = item_data.get('third_images', {}) # Save images if colors: @@ -190,13 +186,6 @@ def _process_item_data(self, item_data): carpet_name = f'carpet_{str(idx).zfill(6)}_{carpet_key}.npy' np.save(os.path.join(self.carpet_tactile_dir, carpet_name), carpet_data.astype(np.float32)) item_data['carpet_tactiles'][carpet_key] = os.path.join(self.episode_dir, carpet_name) - - if third_images: - for idx_third, (third_key, third) in enumerate(third_images.items()): - third_name = f'{str(idx).zfill(6)}_{third_key}.jpg' - if not cv2.imwrite(os.path.join(self.third_image_dir, third_name), third): - logger_mp.info(f"Failed to save third_image.") - item_data['third_images'][third_key] = os.path.join('third_images', third_name) # Update episode data