#!/usr/bin/env python
"""
K.NAKADA, kengo.nakada@gmail.com, kengo.nakada@mat.shimane-u.ac.jp
"""
import sys
import traceback
from typing import Any, List, Optional, Sequence
import logging
from pathlib import Path
import time
# Qt
from PySide6.QtCore import QTimer
from cobotta_bcap_arm import CobottaBcapArm
# from cobotta2.pad import InputGrpcClient
from cobotta2.server_grpc import JoyPadClient
# from cobotta2.pad.input_joypad_local import InputJoyPadLocal # ★ clientは入力のみ担当
from cobotta2.pad.input_grpc_client import InputGrpcClient
from x_logger import XLogger
[ドキュメント]
class JoyPadCtrl:
"""
JoyPad 入力を用いて COBOTTA をリアルタイム制御するためのコントローラクラス。
本クラスは以下の役割を担う。
- JoyPad 入力デバイス（InputGrpcClient）からの入力取得
- gRPC 経由で JoyPadClient（ctrl）へ制御指令を送信
- Arm 制御（Take / Give）の仲介
- JoyPad の開始 / 停止、HALT 処理の管理
- 現在位置 / 関節角度の定期ポーリングと GUI 反映
本クラスは GUI（Qt）イベント駆動で動作し、
内部では QTimer による周期処理を用いる。
"""
def __init__(
self,
parent: Any,
client: JoyPadClient,
input_device: InputGrpcClient,
arm_ctrl: CobottaBcapArm,
logger: XLogger = None,
):
"""
JoyPadCtrl を初期化する。
Args:
parent (Any):
メイン GUI オブジェクト。
ui 属性を持ち、JoyPad 関連ウィジェットへアクセスできることを前提とする。
client (JoyPadClient):
gRPC 経由で COBOTTA を制御するクライアント。
JoyPad 用 ctrl API を提供する。
input_device (InputGrpcClient):
JoyPad 入力を取得するための入力デバイスクライアント。
内部で gRPC 経由の入力同期を行う。
arm_ctrl (CobottaBcapArm):
Arm の Take / Give を管理する制御クラス。
logger (XLogger, optional):
ログ出力用ロガー。
指定されない場合は JoyPadCtrl 用のデフォルトロガーを生成する。
Notes:
- JoyPad の実行状態は _running フラグで管理する
- 入力のアクティブ / 非アクティブ遷移は _was_active で追跡する
- 入力ポーリング用・現在座標表示用に QTimer を使用する
"""
self._parent = parent
self._arm_ctrl = arm_ctrl
self._client = client
self._input_device = input_device
self._logger = logger or XLogger(log_level="info", logger_name="JoyPadCtrl")
self._was_active: bool = False
self._running: bool = False
# ------------------
# UI
# ------------------
self.ui = self._parent.ui
# ------------------
# SIGNAL
# ------------------
self.ui.JoyPad_button_start.clicked.connect(self._on_joypad_start)
self.ui.JoyPad_button_stop.clicked.connect(self._on_joypad_stop)
# --- 初期ボタン状態
try:
self.ui.JoyPad_button_start.setEnabled(True)
self.ui.JoyPad_button_stop.setEnabled(True)
except AttributeError:
pass
# --- JoyPad 入力用 input_device ポーリング用タイマー
self._input_timer: QTimer = QTimer(parent)
self._input_timer.setInterval(20) # 20 ms = 50 Hz 程度
self._input_timer.timeout.connect(self._update)
self._prev_time: float = time.time()
# --- JoyPad 座標取得タイマー
self._curpos_timer = QTimer(parent)
self._curpos_timer.setInterval(200)
self._curpos_timer.timeout.connect(self._on_curpos_timer)
# --- 座標表示 combox (pos/angle/None)
self.ui.JoyPad_comboBox_mode.currentTextChanged.connect(self._on_mode_changed)
self._display_mode = "none"
[ドキュメント]
def get_client(self):
"""
内部で保持している JoyPadClient を取得する。
Returns:
JoyPadClient:
gRPC 経由の制御クライアント。
"""
return self._client
[ドキュメント]
def get_arm_ctrl(self):
"""
内部で保持している Arm 制御オブジェクトを取得する。
Returns:
CobottaBcapArm:
Arm の Take / Give 状態を管理する制御クラス。
"""
return self._arm_ctrl
# -----------------------------------------------
# JoyPad Take Arm/Give Arm
# -----------------------------------------------
[ドキュメント]
def take_arm(self) -> bool:
"""
JoyPad 経由で Arm の制御権を取得する。
Returns:
bool:
正常に Arm を取得できた場合は True、
失敗時は False を返す。
Notes:
- 実処理は CobottaBcapArm に委譲される
"""
self._logger.info("== take_arm()")
return self._arm_ctrl.take_arm()
[ドキュメント]
def give_arm(self) -> bool:
"""
JoyPad 経由で Arm の制御権を解放する。
Returns:
bool:
正常に Arm を解放できた場合は True、
失敗時は False を返す。
Notes:
- 実処理は CobottaBcapArm に委譲される
"""
self._logger.info("== give_arm()")
return self._arm_ctrl.give_arm()
[ドキュメント]
def close(self) -> None:
"""
アプリケーション終了時の JoyPad 後始末処理。
Behavior:
- JoyPad 実行中であれば停止処理を行う
- Arm を解放する（gRPC 経由）
- 入力デバイスのクローズ処理（将来拡張用コードあり）
Notes:
- 例外は捕捉され、ログに記録される
"""
self._logger.info("== close_joypad()")
try:
if self._running:
self._on_stop()
# ARM 解放（gRPC 経由）
if self._client is not None:
try:
self._logger.info("Call give_arm() in close_joypad")
ok = self._client.give_arm()
if not ok:
self._logger.error("give_arm(close) returned False")
except Exception as e:
self._logger.error(f"give_arm(close) failed: {e}")
# local input close
# if self._input_device is not None:
# try:
# self._logger.info("Call InputJoyPadLocal.close() in close_joypad")
# self._input_device.close()
# except Exception as e:
# self._logger.error(f"InputJoyPadLocal close failed: {e}")
except Exception as e:
self._logger.error(f"close_joypad error: {e}")
# -----------------------------------------------
# SLOT: JoyPad update
# -----------------------------------------------
[ドキュメント]
def _update(self) -> None:
"""
JoyPad 入力を周期的に処理するステップ関数。
本メソッドは QTimer から周期的に呼び出され、
JoyPad の入力状態を取得し、必要に応じて
gRPC 経由で COBOTTA へ制御指令を送信する。
Behavior:
- 入力デバイスの step() を実行
- dpose（7要素）の取得
- active → neutral 遷移時に HALT を送信
- active 状態では Arm が取得済みであることを保証
Notes:
- JoyPad が停止中の場合は即 return
- 接続状態・クライアント初期化状態を常に確認する
"""
if not self._running:
return
# if self._input_device is None:
# return
if self._client is None:
return
if not self._client.is_connected():
return
try:
###############################
# for local device
# self._input_device.poll()
# # expected: Optional[List[float]] of length 7
# dpose = self._input_device.get_dpose()
# is_active = dpose is not None
###############################
###############################
# joypad 制御 (for gRPC client)
self._input_device.step() # 内部的に client に send_pose() を送っている
###############################
###############################
# 値表示のため
dpose = self._input_device.get_dpose() # 内部的に取得した dpose を get
# self._logger.info(f"dpose: {dpose}")
is_active = dpose is not None
###############################
# active -> neutral: 即 HALT
if self._was_active and not is_active:
self._client.halt()
###############################
# active: dpose を送る（7要素）
# if is_active:
# if not self._arm_ctrl.arm_taken:
# return
##############################
# for local device
# dpose_list = list(dpose)
# if len(dpose_list) != 7:
# self._logger.error(
# f"invalid dpose length (expected 7): {dpose_list}"
# )
# return
# self._client.send_dpose(dpose_list)
##############################
self._was_active = is_active
except Exception as e:
self._logger.error(f"_update_joypad error: {e}")
self._on_stop()
# -----------------------------------------------
# JoyPad start/stop
# -----------------------------------------------
[ドキュメント]
def _on_joypad_start(self) -> None:
"""
JoyPad の開始ボタンが押されたときのスロット。
Behavior:
- Arm が未取得の場合は Take Arm を実行
- サーバ側の joypad_start() を呼び出す
- 入力ポーリング用 QTimer を開始
- 現在座標表示用 QTimer を開始
- UI ボタン状態を JoyPad 実行中に切り替える
Notes:
- 既に実行中の場合は何も行わない
"""
self._logger.info("== on_joypad_start()")
if self._running:
self._logger.error("JoyPad is already running")
return
if self._client is None:
raise RuntimeError("JoyPadClient is not initialized")
# if self._input_device is None:
# raise RuntimeError("InputJoyPadLocal is not initialized")
# joypad 使用時は arm を保証
if not self._arm_ctrl.arm_taken:
ok = self._arm_ctrl.take_arm()
if not ok:
self._logger.error("take_arm failed at joypad_start")
return
try:
##############################
self._client.joypad_start()
##############################
except Exception as e:
self._logger.error(f"joypad_start() failed: {e}")
return
self.ui.JoyPad_button_start.setEnabled(False)
self.ui.JoyPad_button_take_arm.setEnabled(False)
self.ui.JoyPad_button_give_arm.setEnabled(False)
self.ui.fig_label_JoyPad.setStyleSheet("QLabel { opacity: 0.4; }")
self._prev_time = time.time()
self._running = True
self._input_timer.start()
self._logger.info("JoyPad polling timer started")
self._curpos_timer.start()
self._logger.info("get_pos/jnt polling timer started")
[ドキュメント]
def _on_joypad_stop(self) -> None:
"""
JoyPad の停止ボタンが押されたときのスロット。
Behavior:
- 入力ポーリング用 QTimer を停止
- 現在座標表示用 QTimer を停止
- サーバ側の joypad_stop() を呼び出す
- UI ボタン状態を停止状態に戻す
Notes:
- JoyPad 未実行時に呼ばれた場合は何も行わない
"""
self._logger.info("== on_joypad_stop()")
if not self._running:
self._logger.info("JoyPad is not running")
return
self._running = False
self._input_timer.stop()
self._logger.info("JoyPad polling timer stopped")
self._curpos_timer.stop()
self._logger.info("get_pos/jnt polling timer stopped")
self.ui.JoyPad_button_start.setEnabled(True)
self.ui.JoyPad_button_take_arm.setEnabled(True)
self.ui.JoyPad_button_give_arm.setEnabled(False)
self.ui.fig_label_JoyPad.setStyleSheet("")
# server-side runtime stop（公開API=ctrl）
if self._client is not None:
try:
self._logger.info("Call joypad_stop()")
self._client.joypad_stop()
except Exception as e:
self._logger.error(f"joypad_stop() failed: {e}")
# -----------------------------------------------
# current pos/angle polling
# -----------------------------------------------
[ドキュメント]
def _on_curpos_timer(self) -> None:
"""
現在位置 / 関節角度を周期的に取得し GUI に反映する。
Behavior:
- 表示モードに応じて以下を取得する
- "pos" : 現在位置（XYZ + RPY）
- "angle" : 関節角度（J1〜J6）
- 取得した値を対応する LineEdit に表示する
Notes:
- JoyPad 実行中のみ動作する
- 接続状態が無効な場合は処理しない
"""
if self._display_mode == "none":
return
if not self._running:
return
if self._client is None:
return
if not self._client.is_connected():
return
if self._display_mode == "pos":
try:
pos = self._client.get_current_position()
if pos is None or len(pos) < 6:
return
x, y, z, rx, ry, rz = pos
self.ui.JoyPad_lineEdit_view_pos_x.setText(f"{x:.2f}")
self.ui.JoyPad_lineEdit_view_pos_y.setText(f"{y:.2f}")
self.ui.JoyPad_lineEdit_view_pos_z.setText(f"{z:.2f}")
self.ui.JoyPad_lineEdit_view_pos_rx.setText(f"{rx:.2f}")
self.ui.JoyPad_lineEdit_view_pos_ry.setText(f"{ry:.2f}")
self.ui.JoyPad_lineEdit_view_pos_rz.setText(f"{rz:.2f}")
except Exception as e:
self._logger.error(f"CurPos update error: {e}")
if self._display_mode == "angle":
try:
j = self._client.get_current_angle()
if j is None or len(j) < 6:
return
j1, j2, j3, j4, j5, j6 = j
self.ui.JoyPad_lineEdit_view_pos_x.setText(f"{j1:.2f}")
self.ui.JoyPad_lineEdit_view_pos_y.setText(f"{j2:.2f}")
self.ui.JoyPad_lineEdit_view_pos_z.setText(f"{j3:.2f}")
self.ui.JoyPad_lineEdit_view_pos_rx.setText(f"{j4:.2f}")
self.ui.JoyPad_lineEdit_view_pos_ry.setText(f"{j5:.2f}")
self.ui.JoyPad_lineEdit_view_pos_rz.setText(f"{j6:.2f}")
except Exception as e:
self._logger.error(f"CurJnt update error: {e}")
# -----------------------------------------------
# display mode
# -----------------------------------------------
[ドキュメント]
def _on_mode_changed(self, text: str) -> None:
"""
座標表示モード切り替え時のスロット。
Args:
text (str):
コンボボックスで選択された表示モード文字列。
"pos" / "angle" / その他。
Behavior:
- 表示モードを内部変数 _display_mode に反映する
- ログに現在の表示モードを出力する
"""
if text == "pos":
self._display_mode = "pos"
elif text == "angle":
self._display_mode = "angle"
else:
self._display_mode = "none"
self._logger.info(f"Display mode changed to {self._display_mode}")