joypad で cobotta の制御の方法

必要なプログラム

x_logger / doc
cobotta2 / doc

自分のpadのボタン番号とプログラムの対応を確認する

現在プログラムでデフォルトで想定している番号

L1: 4
L2: 6
R1: 5
R2: 7
start: 12

# elecom joypad
STICK_LEFT_X = 0
STICK_LEFT_Y = 1
STICK_RIGHT_X = 2
STICK_RIGHT_Y = 3
BUTTON_L1 = 4
BUTTON_L2 = 6
BUTTON_R1 = 5
BUTTON_R2 = 7
BUTTON_START = 12

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import pygame
import time


def main() -> None:
    pygame.init()
    pygame.joystick.init()

    if pygame.joystick.get_count() == 0:
        print("No joystick found")
        return

    js = pygame.joystick.Joystick(0)
    js.init()

    print("Joystick name:", js.get_name())
    print("num buttons:", js.get_numbuttons())
    print("Press any buttons. Ctrl+C to quit.")
    print("----")

    try:
        while True:
            pygame.event.pump()

            pressed = []
            i = 0
            while i < js.get_numbuttons():
                # ボタン
                if js.get_button(i):
                    pressed.append(i)
                i += 1

            if len(pressed) > 0:
                print("pressed buttons =", pressed)

            time.sleep(0.05)

    except KeyboardInterrupt:
        pass
    finally:
        js.quit()
        pygame.joystick.quit()
        pygame.quit()


if __name__ == "__main__":
    main()

パッドの操作: P/J 切り替え

START

パッドの操作: スピード制御

十字キーの上下

パッドの操作: Pモード (xyz)

L1 離してる(デフォルト): 位置（xyz）= 左スティックX→x, 左スティックY→y, 右スティックY→z
L1 押してる: 回転（r*）= 左スティックX→rx, 左スティックY→ry, 右スティックY→rz

パッドの操作: Jモード (J1〜J6)

L1 離してる（J1–J4): 左X→J1, 左Y→J2, 右Y→J3, 右X→J4
L1 押してる（J3–J6): 左X→J3, 左Y→J4, 右Y→J5, 右X→J6

ハンド

R1 押下（立ち上がり1回）: hand_open()
R2 押下（立ち上がり1回）→ hand_close()

Joypad プログラムを立ち上げる GUI を使わない方法

現在の cobotta モジュール内にあるプログラムだけで動かします。

server script

cobotta と通信可能な権限があるマシン上で起動する

#!/usr/bin/env python3
from cobotta2 import Config
from cobotta2.server_grpc import joypad_server
from x_logger import XLogger


if __name__ == "__main__":
    
    Config.load_yaml("config_cobotta3_server2.yaml")

    # ---- cobotta
    # BCAP_HOST = "10.170.10.37"
    BCAP_HOST = Config.COBOTTA_IP
    # BCAP_PORT = 5007
    BCAP_PORT = Config.BCAP_PORT
    # ---- joypad
    # GRPC_HOST = "10.170.10.38"
    GRPC_HOST = Config.SERVER_IP
    # GRPC_HOST = "0.0.0.0"
    # # GRPC_HOST = "localhost"
    # GRPC_PORT = 50051
    GRPC_PORT = Config.JOYPAD_PORT
    # JOYPAD_INTERVAL = 0.01
    JOYPAD_INTERVAL = Config.JOYPAD_INTERVAL

    LOG_LEVEL = "debug"

    logger = XLogger(
        log_level=LOG_LEVEL,
        logger_name=Config.JOYPAD_LOGGER_NAME,
        # log_mode="rotate",
        # log_mode="file",
        # log_name=f"c:/opt/log/{Config.JOYPAD_LOGGER_NAME}.log",
    )

    # JoyPad Server
    joypad_server(
        bcap_host=BCAP_HOST,
        bcap_port=BCAP_PORT,
        grpc_host=GRPC_HOST,
        grpc_port=GRPC_PORT,
        interval=JOYPAD_INTERVAL,
        logger=logger,
        log_level=LOG_LEVEL,
    )

設定ファイルを yaml にまとめたものを使う場合は下記の yaml ファイルを用意する

# --- cobotta-server
SERVER_IP: "10.170.10.38"
SERVER_PORT: 8000
CLIENT_LOGGER_NAME: "cobotta1_client"
SERVER_LOGGER_NAME: "cobotta1_server"
TIMEOUT: 5
LOGGER_LEVEL: "debug"
# --- joypad-server
JOYPAD_PORT: 50051
JOYPAD_LOGGER_NAME: "cobotta1_joypad"
# --- cobotta
COBOTTA_IP: "10.170.10.37"
COBOTTA_CAMERA_IP: None

joypad をつなげたPC 上で起動する

joypad-server と同じマシンでも違うマシンでも良い。ただし、ネットワークの状況があまりよくないと遅延が発生するかもしれないので注意

#!/usr/bin/env python3
"""
step 型の 呼び出し
GUI 上で独立ループから呼び出す(mainスレッドとJoyPadスレッドを一緒にするため)

mac などでjoygame などのオブジェクトを main スレッド以外で使うとハマる問題(SDL問題)
の回避で、start/stop がたから、while: step 形に変更

Kengo NAKADA
kengo.nakada@mat.shimane-u.ac.jp
kengo.nakada@gmail.com
"""
from typing import Any, Dict, List, Optional, Union, Tuple, Callable, Sequence
import time

# import grpc

from cobotta2.server_grpc import JoyPadClient
from cobotta2.server_grpc import InputGrpcClient


def step(
    logger: Optional[Any] = None,
    log_level: str = "INFO",
) -> None:
    """
    step 型 client 起動（SDL 制約回避）
    """
    from cobotta2.pad import InputGrpcClient

    if logger is None:
        import logging

        logging.basicConfig(level=log_level.upper())
        logger = logging.getLogger(__name__)

    logger.info("[CLIENT] JoyPad gRPC client main start")

    client = JoyPadClient(
        server_ip="10.170.10.38",
        server_port=50051,
        logger=logger,
        log_level=log_level,
    )

    if not client.is_connected():
        logger.error("[CLIENT] gRPC connection failed")
        return

    logger.info("[CLIENT] connected")

    client.reset_error()
    client.motor_on()
    client.take_arm()

    client.joypad_set_scale(9.0)
    client.joypad_start()

    input_pad = InputGrpcClient(
        grpc_client=client,
        interval=0.02,
        logger=logger,
    )

    try:
        while True:
            input_pad.step()
            time.sleep(0.02)
    except KeyboardInterrupt:
        logger.info("[CLIENT] KeyboardInterrupt")
    finally:
        client.joypad_stop()
        client.give_arm()
        client.motor_off()
        logger.info("[CLIENT] JoyPad gRPC client main end")


if __name__ == "__main__":
    from x_logger import XLogger

    log_level_ = "INFO"
    logger_ = XLogger(log_level=log_level_, logger_name="JoyPadClient")
    step(log_level=log_level_, logger=logger_)

GUI を使う

doc
source

作者

Kengo NAKADA