#!/usr/bin/env python3
"""
K.NAKADA, kengo.nakada@gmail.com
dpose (移動量) を受け取って、現在地の差分から
移動指定を出すメソッドを呼び出す。
その際に queue に一度積ませてから処理をするのを前提とする
gRPCなどの遅延対策
"""
from typing import Any, Dict, List, Optional, Union, Tuple, Callable, Sequence
import threading
import time
import queue
from cobotta2.cobotta_bcap import CobottaBcap
[ドキュメント]
class JoyPadMover:
"""
JoyPadCore 用 mover（queue 版）
- dpose はすべて 1 モーションとして queue に積む
- halt() は
- RC8 HALT
- queue を物理破棄
- エラー処理は MoveWorker（旧実装）と同じ思想：
* 毎回 move 前に get_error_count()
* エラーがあれば reset_error() してその周期は捨てる
* 例外時も reset_error()
"""
def __init__(
self,
bcap: CobottaBcap,
interval: float = 0.02,
scale: int = 5,
logger: Optional[Any] = None,
log_level: Optional[str] = None,
) -> None:
"""
Args:
bcap:
interval (float): server内での dmove 呼び出しループの wait
scale (int): Δd 差分に対する倍率、Input_jopypad_local で設定された値を使うことになるケースが多い
logger (Any):
log_level (str):
"""
# XLogger が存在しない時に仕方がないのでデフォルトの logging を使う
if logger is None:
import logging
log_level = log_level or "INFO"
logging.basicConfig(level=log_level.upper())
logger = logging.getLogger(__name__)
self._logger = logger
self._bcap = bcap
self._interval: float = interval
self._scale: int = scale
self._coord_mode: str = "P"
# self._queue: queue.Queue[List[float]] = queue.Queue()
self._queue = queue.Queue()
self._running: bool = False
self._thread: Optional[threading.Thread] = None
# ------------------------
# parameter
# ------------------------
[ドキュメント]
def set_interval(self, interval: float) -> None:
self._interval = float(interval)
[ドキュメント]
def set_scale(self, scale: int) -> None:
self._scale = int(scale)
# ------------------------
# input
# ------------------------
[ドキュメント]
def put_dpose(self, dpose: List[float]) -> None:
"""Motion が詰まっている Queue"""
self._queue.put(dpose)
# ------------------------
# joypad_set_coord_mode
# ------------------------
[ドキュメント]
def set_coord_mode(self, mode: str):
self._coord_mode = mode
[ドキュメント]
def get_coord_mode(self) -> str:
return self._coord_mode
# ------------------------
# control
# ------------------------
[ドキュメント]
def halt(self) -> None:
"""
即時停止 & quele clear:
- RC8 HALT
- 先行入力 queue を完全破棄
"""
self._logger.info("[JoyPadMover] halt")
try:
self._bcap.halt()
except Exception as e:
self._logger.error(f"halt failed: {e}")
with self._queue.mutex:
self._logger.info("[JoyPadMover] Queue clear")
self._queue.queue.clear()
# ------------------------
# lifecycle
# JoyPadMover は dpose を queue につまれたものから
# 連続モーションとして処理する
# パッドをニュートラルに戻したら halt して move を止める
# これは、InputJoy/gRPC 側で発火させる
# これは Queue を介すると機能しないのでクライアントからのhalt命令を飛ばす
# ------------------------
# def start(self) -> None:
# """queue start"""
# if self._running:
# return
# self._running = True
# self._thread = threading.Thread(
# target=self._loop,
# daemon=True,
# )
# self._thread.start()
[ドキュメント]
def start(self) -> None:
# ==============================
# 起動時エラーの即時解除
# ==============================
try:
err = self._bcap.get_error_count()
if err > 0:
self._logger.error(f"JoyPadMover start: error_count={err}")
self._logger.error("reset_error() at startup")
self._bcap.reset_error()
except Exception as e:
self._logger.error(f"JoyPadMover start: error check failed: {e}")
self._logger.error("reset_error() at startup")
self._bcap.reset_error()
self._running = True
self._thread = threading.Thread(
target=self._loop,
daemon=True,
)
self._thread.start()
[ドキュメント]
def stop(self) -> None:
"""queue stop"""
if not self._running:
return
self._running = False
if self._thread is not None:
self._thread.join()
self._thread = None
# ------------------------
# internal loop
# ------------------------
[ドキュメント]
def _loop(self) -> None:
while self._running:
try:
# motion が詰まっている Qeue から取り出す
dpose = self._queue.get(timeout=self._interval)
except queue.Empty:
time.sleep(self._interval)
continue
# ----------------------------
# エラー検出 → 自動復帰（旧 MoveWorker と同型）
# ----------------------------
try:
err = self._bcap.get_error_count()
if err > 0:
self._logger.error(f"JoyPadMover error_count is : {err}")
self._logger.error("reset_error()")
self._bcap.reset_error()
continue
except Exception as e:
self._logger.error(f"JoyPadMover error check failed: {e}")
self._logger.error("reset_error()")
self._bcap.reset_error()
continue
# ----------------------------
# move 実行
# ----------------------------
if self._coord_mode == "P":
pos = [
dpose[0] * self._scale,
dpose[1] * self._scale,
dpose[2] * self._scale,
dpose[3] * self._scale,
dpose[4] * self._scale,
dpose[5] * self._scale,
]
else:
pos = [
dpose[0] * self._scale,
dpose[1] * self._scale,
dpose[2] * self._scale,
dpose[3] * self._scale,
dpose[4] * self._scale,
dpose[5] * self._scale,
]
try:
# 差分移動 (raw level 直接呼び出し)
# coord はデフォルトでは "P" で x, y, z, rx, ry, rz
self._bcap.robo_dmove(
pos,
coord_mode=self._coord_mode,
)
except Exception as e:
self._logger.error(f"[JoyPadMover] **Error**: robo_dmove(): {e}")
self._logger.error(f"coord_mode = {self._coord_mode}")
self._logger.error(f"dpose = {dpose}")
self._logger.error(f"pos = {pos}")
try:
self._logger.error("** reset_error() **")
self._bcap.reset_error()
except Exception as e2:
self._logger.error(f"reset_error failed: {e2}")
continue
time.sleep(self._interval)