下面代码被调用启动是时，data_processing_thread()线程由于是在多线程里面使用，不受终端的键盘控制，如何修改这个问题class KeyboardUrLeader(Teleoperator):
"""
SO-101 Leader Arm designed by TheRobotStudio and Hugging Face.
"""
config_class = KeyboardUrLeaderConfig
name = "keyboard_ur_leader"
def __init__(self, config: KeyboardUrLeaderConfig):
super().__init__(config)
self.config = config
self.motors = {
"link1": Motor(1, "sts3215", MotorNormMode.DEGREES),
"link2": Motor(2, "sts3215", MotorNormMode.DEGREES),
"link3": Motor(3, "sts3215", MotorNormMode.DEGREES),
"link4": Motor(4, "sts3215", MotorNormMode.DEGREES),
"link5": Motor(5, "sts3215", MotorNormMode.DEGREES),
"link6": Motor(6, "sts3215", MotorNormMode.DEGREES),
"hand1": Motor(7, "sts3215", MotorNormMode.DEGREES),
"hand2": Motor(8, "sts3215", MotorNormMode.DEGREES),
"hand3": Motor(9, "sts3215", MotorNormMode.DEGREES),
}
self.actions = []
self.idx = 0
self.num_frames = len(self.actions)
self.rtde_r = rtde_receive.RTDEReceiveInterface(self.config.port)
self.dir = True # 机器人运动方向
self.dpos = 0.002 # 键盘控制移动量
self.last_input_time = time.time()
self.current_direction = None
self.input_timeout = 0.1 # 无输入超时时间(秒)
self.jog_timeout = 0.02 # 点动连续时间(秒)
self.input_queue = queue.Queue() # 输入队列
self.input_thread()
self.data_processing_thread()
@property
def action_features(self) -> dict[str, type]:
return {f"{motor}.pos": float for motor in self.motors}
@property
def feedback_features(self) -> dict[str, type]:
return {}
@property
def is_connected(self) -> bool:
return True
def connect(self, calibrate: bool = True) -> None:
self.idx = 0
logger.info(f"{self} connected.")
@property
def is_calibrated(self) -> bool:
return True
def calibrate(self) -> None:
logger.info(f"\nRunning calibration of {self}")
def configure(self) -> None:
logger.info(f"\nRunning configure of {self}")
# 键盘增量转ur机器人pose
def keyboard_to_ur_pose(self, axis):
pose_curr_ur = self.rtde_r.getActualTCPPose()
# 末端姿态保持不变
pose_ur_back = pose_curr_ur.copy()
if self.dir:
pose_ur_back[int(axis)-1] = pose_ur_back[int(axis)-1] + self.dpos
print(f"axis_{axis} move {self.dpos*1000} mm")
else:
pose_ur_back[int(axis)-1] = pose_ur_back[int(axis)-1]- self.dpos
print(f"axis_{axis} move {-self.dpos*1000} mm")
# 设置机器人工作空间
pose_home = [-0.10938138088351179, 0.30655065287476435, 0.39158909232888983, -3.141123435198306, 0.00046462590781230415, 0.001239960331220597]
if (abs(pose_ur_back[0]-pose_home[0])>=0.2 or
abs(pose_ur_back[1]-pose_home[1])>=0.17 or
abs(pose_ur_back[2]-pose_home[2])>=0.195):
print("pose_ur_back too big!!")
# 保持当前位置
pose_curr_ur = self.rtde_r.getActualTCPPose()
return pose_curr_ur
return pose_ur_back
# 修改位移增量
def adjust_dpos(self, command):
if command == 'i':
if self.dpos >= 0.02:
self.dpos = 0.02
print(f"max dpos = {self.dpos * 1000} mm")
elif self.dpos >= 0.01 and self.dpos < 0.02:
self.dpos += 0.002
print(f"dpos = {self.dpos * 1000} mm")
elif self.dpos >= 0.001 and self.dpos < 0.01:
self.dpos += 0.001
print(f"dpos = {self.dpos * 1000} mm")
else:
self.dpos += 0.0001
print(f"dpos = {self.dpos * 1000} mm")
elif command == 'm':
if self.dpos <= 0.0001:
self.dpos = 0.0001
print(f"min dpos = {self.dpos * 1000} mm")
elif self.dpos <= 0.001 and self.dpos >= 0.00019:
self.dpos -= 0.0001
print(f"dpos = {self.dpos * 1000} mm")
elif self.dpos <= 0.01 and self.dpos >= 0.0019:
self.dpos -= 0.001
print(f"dpos = {self.dpos * 1000} mm")
else:
self.dpos -= 0.002
print(f"dpos = {self.dpos * 1000} mm")
def process_command(self, command):
"""处理键盘命令"""
# 获取数据
pose = self.keyboard_to_ur_pose(command)
self.current_direction = command
pose.extend([0.0,0.0,0.0])
return pose
def check_timeout(self):
"""检查是否超时无输入"""
current_time = time.time()
if current_time - self.last_input_time > self.input_timeout:
if self.current_direction is not None:
# print("可以跑其他中断指令")
pass
return True
return False
def update_last_input_time(self):
"""更新最后输入时间"""
self.last_input_time = time.time()
# 创建键盘输入线程
def input_thread(self):
# 创建并启动输入线程
input_thread = threading.Thread(target=input_thread_func, args=(self.input_queue,))
input_thread.daemon = True # 设置为守护线程
input_thread.start()
# 键盘数据处理
def data_processing(self):
print("机器人控制已启动")
print("使用 1/2/3/4/5/6 控制对应轴，+/- 控制方向，q键退出，i/m上下改位移增量")
print("无输入时自动停止移动")
pose_curr = self.rtde_r.getActualTCPPose()
pose_curr.extend([0.0,0.0,0.0])
self.actions = pose_curr.copy()
# 非Windows系统：设置终端为非规范模式
if not sys.platform.startswith('win'):
import termios
import tty
# 保存原始终端设置
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
try:
# 设置非规范模式（立即响应按键）
tty.setcbreak(fd)
except Exception as e:
print(f"终端设置错误: {e}")
try:
while True:
try:
# 实时处理键盘输入（无阻塞）
command = self.input_queue.get_nowait()
# q/ESC键退出程序
if command in ['q', '\x1b']:
print("\n退出程序")
break
if command in ['i', 'm']:
self.adjust_dpos(command)
# + - 键控制方向
if command in ['+', '-']:
self.dir = True if command == '+' else False
print(f"\n方向已切换为: [{'正方向' if self.dir == True else '负方向'}]")
# 处理机器人控制命令
if command in ['1', '2', '3', '4', '5', '6']:
self.action = self.process_command(command)
self.update_last_input_time()
# time.sleep(0.1)
except queue.Empty:
# 队列为空时检查超时
if self.check_timeout():
# print("\r等待输入...请输入1-6或+-或i/m ", end='', flush=True)
pass
time.sleep(0.02) # 降低CPU占用
except KeyboardInterrupt:
print("\n程序被用户中断")
finally:
# 恢复原始终端设置（非Windows系统）
if not sys.platform.startswith('win'):
try:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
except NameError:
pass # 避免未定义变量错误
print("机器人控制已停止")
# 创建键盘数据处理线程
def data_processing_thread(self):
# 创建并启动输入线程
data_processing_thread = threading.Thread(target=self.data_processing, args=())
data_processing_thread.daemon = True # 设置为守护线程
data_processing_thread.start()
def get_action(self) -> dict[str, float]:
start = time.perf_counter()
# 格式处理
# {'link1.pos': -0.0618470806532573, 'link2.pos': 0.3380488037938975, 'link3.pos': 0.372561731168205, 'link4.pos': -3.141193817478292, 'link5.pos': 0.0004445734284887, 'link6.pos': 0.001289001924306, 'hand1.pos': 0.0, 'hand2.pos': 0.0, 'hand3.pos': 0.0}
action_array = self.actions
action = {}
for i, name in enumerate(list(self.motors.keys())):
action[name] = action_array[i]
action = {f"{motor}.pos": val for motor, val in action.items()}
action = tensor_dict_to_float(action)
dt_ms = (time.perf_counter() - start) * 1e3
logger.debug(f"read action: {dt_ms:.1f}ms")
return action
解决STS2.6版在Win7环境下查看类方法不便的问题
在使用STS2.6版本的开发环境中，遇到Win7系统下仅能查看当前编辑类部分方法的情况，通过调整窗口->首选项->Java->编辑器中的设置选项，即取消Only show selected Java element，问题得以解决，显著提升了开发效率。
扫一扫
880
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
