#!/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
from cobotta2.server_fastapi import SyncCobottaClient
from x_logger import XLogger
logging.getLogger("httpx").setLevel(logging.WARNING)
[ドキュメント]
class CobottaOrin2Ctrl:
"""
ORiN2 / FastAPI ベースの COBOTTA 制御を行うコントローラクラス。
本クラスは以下の責務を持つ。
- SyncCobottaClient を用いた同期型 COBOTTA 制御の初期化
- モータ ON / OFF 操作の仲介
- アーム Take / Give 操作の実行
- GUI（CobottaCtrl / JoyPad）のボタン状態制御
JoyPad 経由の gRPC 制御とは異なり、本クラスは
ORiN2 / FastAPI サーバを I/F とする制御系を担当する。
"""
def __init__(
self,
parent,
logger: Optional[XLogger] = None,
):
"""
CobottaOrin2Ctrl を初期化する。
Args:
parent (Any):
メイン GUI オブジェクト。
ui 属性を持ち、CobottaCtrl / JoyPad 両方の
ボタンウィジェットにアクセス可能であることを前提とする。
logger (XLogger, optional):
ログ出力用ロガー。
指定されない場合は本クラス用のデフォルトロガーを生成する。
Notes:
- 初期化時に SyncCobottaClient の生成処理を行う
- アーム取得状態 (_arm_taken) を内部フラグとして管理する
- UI ボタンの初期状態を設定する
"""
self._parent = parent
self._logger = logger or XLogger(logger_name="CobottaOrin2Ctrl")
# --- cobotta_client (Sync版)
self._client: Optional[SyncCobottaClient] = None
self.init()
# ------------------------
# UI
# ------------------------
self.ui = parent.ui
self.ui.CobottaCtrl_button_take_arm.clicked.connect(self._on_take_arm)
self.ui.CobottaCtrl_button_give_arm.clicked.connect(self._on_give_arm)
try:
self.ui.CobottaCtrl_button_take_arm.setEnabled(True)
self.ui.CobottaCtrl_button_give_arm.setEnabled(True)
except AttributeError:
pass
self._arm_taken = False
[ドキュメント]
def init(self) -> None:
"""
SyncCobottaClient を生成・初期化する。
Behavior:
- Config に定義された SERVER_IP / SERVER_PORT を用いて
SyncCobottaClient を生成する
- 生成したクライアントを内部に保持する
Raises:
RuntimeError:
Config が未初期化の場合に送出される。
Notes:
- 本メソッドは __init__ から自動的に呼び出される
- ネットワーク接続自体の成否はここでは判定しない
"""
self._logger.info("== cobotta_init()")
if self._config is None:
raise RuntimeError("Config is not initialized")
self._logger.info("== Create SyncCobottaClient()")
self._logger.info(f"SERVER_IP = {self._config.SERVER_IP}")
self._logger.info(f"SERVER_PORT = {self._config.SERVER_PORT}")
self._client = SyncCobottaClient(
server_ip=self._config.SERVER_IP,
server_port=self._config.SERVER_PORT,
logger=self._logger,
)
[ドキュメント]
def trun_off_motor(self) -> bool:
"""
COBOTTA のモータを停止（OFF）する。
Returns:
bool:
モータ停止に成功した場合は True、
クライアント未初期化または失敗時は False を返す。
Behavior:
- SyncCobottaClient.turn_off_motor() を呼び出す
- 失敗時にはエラーログを出力する
"""
self._logger.info("== turn_off_motor()")
if self._client is None:
return False
ret = self._client.turn_off_motor()
if not ret:
self._logger.error("***Failed to turn off motor***")
return False
return True
[ドキュメント]
def trun_on_motor(self) -> bool:
"""
COBOTTA のモータを起動（ON）する。
Returns:
bool:
モータ起動に成功した場合は True、
クライアント未初期化または失敗時は False を返す。
Behavior:
- SyncCobottaClient.turn_on_motor() を呼び出す
- 失敗時にはエラーログを出力する
"""
self._logger.info("== turn_on_motor()")
if self._client is None:
return False
ret = self._client.turn_on_motor()
if not ret:
self._logger.error("***Failed to turn on motor***")
return False
return True
[ドキュメント]
def give_arm(self) -> bool:
"""
COBOTTA のアーム制御権を解放する。
Returns:
bool:
正常にアーム解放できた場合は True、
失敗または例外発生時は False を返す。
Behavior:
- SyncCobottaClient.release_arm() を呼び出す
- 成功時は内部フラグ _arm_taken を False に更新する
Notes:
- クライアント未初期化時は即座に False を返す
"""
self._logger.info("== give_arm()")
if self._client is None:
self._logger.error("CobottaClient is not initialized")
return False
try:
ok = self._client.release_arm()
if not ok:
self._logger.error("Cobotta give_arm returned False")
return False
except Exception as e:
self._logger.error(f"Cobotta give_arm failed: {e}")
return False
self._arm_taken = False
return True
[ドキュメント]
def take_arm(self) -> bool:
"""
COBOTTA のアーム制御権を取得する。
Returns:
bool:
正常にアーム取得できた場合は True、
失敗または例外発生時は False を返す。
Behavior:
- 一定時間スリープ後に SyncCobottaClient.take_arm() を呼び出す
- 成功時は内部フラグ _arm_taken を True に更新する
Notes:
- ORiN2 側の状態安定を考慮し、呼び出し前に待機時間を挿入している
"""
self._logger.info("== take_arm()")
time.sleep(0.3)
if self._client is None:
self._logger.error("CobottaClient is not initialized")
return False
try:
ok = self._client.take_arm()
if not ok:
self._logger.error("Cobotta take_arm returned False")
return False
except Exception as e:
self._logger.error(f"Cobotta take_arm failed: {e}")
return False
self._arm_taken = True
return True
# -----------------------------------------------
# SLOT: cobotta
# -----------------------------------------------
[ドキュメント]
def _on_give_arm(self) -> None:
"""
UI の「Give Arm」ボタンが押されたときのスロット。
Behavior:
- CobottaCtrl 側の Take / Give ボタン状態を更新する
- JoyPad 側の Take / Give ボタン状態を更新する
Notes:
- 実際のアーム解放処理は本メソッドでは行わない
- UI 表示状態の同期のみを担当する
"""
self._logger.info("== on_give_arm()")
try:
self.ui.CobottaCtrl_button_take_arm.setEnabled(True)
self.ui.CobottaCtrl_button_give_arm.setEnabled(False)
except AttributeError:
pass
try:
self.ui.JoyPad_button_take_arm.setEnabled(True)
self.ui.JoyPad_button_give_arm.setEnabled(False)
except AttributeError:
pass
[ドキュメント]
def _on_take_arm(self) -> None:
"""
UI の「Take Arm」ボタンが押されたときのスロット。
Behavior:
- CobottaCtrl 側の Take / Give ボタン状態を更新する
- JoyPad 側の Take / Give ボタン状態を更新する
Notes:
- 実際のアーム取得処理は本メソッドでは行わない
- UI 操作と制御処理は意図的に分離されている
"""
self._logger.info("== on_take_arm()")
self.ui.CobottaCtrl_button_take_arm.setEnabled(False)
self.ui.CobottaCtrl_button_give_arm.setEnabled(True)
try:
self.ui.JoyPad_button_take_arm.setEnabled(False)
self.ui.JoyPad_button_give_arm.setEnabled(False)
except AttributeError:
pass
pass