# Joystick control of a real arm. Three nodes, no bench and no browser.
#
# This is the one that moves a robot. `dataflow.yml` and `dataflow-gamepad.yml`
# drive a stand-in; this one drives `dora-ulite6`, so everything the pad does
# here happens in the room. Read `The gamepad` in README.md before picking it up
# — Back is the E-STOP.
#
# Both sibling nodes are pulled by `git:`, so this runs from a fresh clone with
# no other checkouts. It needs the network on its first build, and an arm.
#
#   dora build dataflow-robot.yml --uv
#   dora run dataflow-robot.yml --uv
#
# Set ROBOT_IP and TOOL_TYPE below to match the arm in front of you. The
# defaults are `dora-ulite6`'s own: 192.168.1.192 and a vacuum tool.
#
# For the web console as well as the pad, add `dora-websocket-transceiver` and
# wire `ui_command`, `clients` and its `data_in` — README.md has that wiring.
# Without it the console ports below simply go unread, which is why `ui_state`
# has no consumer here.

strict_types: true

# Lets `dora topic echo` read what is on the wire — the velocities going to the
# arm, above all. Run under `dora start` rather than `dora run` to use it.
debug:
  enable_debug_inspection: true

nodes:
  - id: joy
    git: https://gitea.liria.cl/liria/dora-joy.git
    tag: v0.1
    build: pip install .
    path: dora-joy
    inputs:
      tick: dora/timer/millis/20
    outputs:
      - joy

  - id: teleop
    path: dora-teleop
    build: pip install .
    inputs:
      tick: dora/timer/millis/50
      # queue_size 1: joy ticks at 20 ms and this node at 50, so without it the
      # loop would work through a backlog of readings that have already been
      # overtaken. An arm should be driven by the newest one or by none.
      joy:
        source: joy/joy
        queue_size: 1
        queue_policy: drop_oldest
      response: ulite6/response
      motion_state: ulite6/motion_state
      robot_state: ulite6/robot_state
      tcp_pose: ulite6/tcp_pose
      joint_pose: ulite6/joint_pose
    outputs:
      - twist
      - command
      - jog
      - request
      - ui_state
    env:
      SPEED_MM_S: "60"
      SPEED_DEG_S: "30"
      # Keep the deadman layers ordered: JOY_TTL_S < JOG_TTL_S < the arm's own
      # VELOCITY_COMMAND_TTL_S, which is 0.5 s below. Each backs up the one
      # inside it, and a backstop that fires first backs up nothing.
      JOY_TTL_S: "0.2"
      JOG_TTL_S: "0.4"

  - id: ulite6
    git: https://gitea.liria.cl/liria/dora-ulite6.git
    tag: v0.3.0
    build: pip install .
    path: dora-ulite6
    inputs:
      tick: dora/timer/millis/100
      request: teleop/request
      jog: teleop/jog
    outputs:
      - response
      - motion_state
      - joint_pose
      - tcp_pose
      - robot_state
    # dora type-checks before it fetches a `git:` source, so it cannot read the
    # arm's own manifest from here. These three repeat what `dora-ulite6`
    # v0.3.0 annotates; `response` and `motion_state` stay untyped there and so
    # stay untyped here. Re-check them when moving to a new tag.
    output_types:
      joint_pose: std/core/v1/Float32
      tcp_pose: std/core/v1/Float32
      robot_state: std/core/v1/String
    env:
      ROBOT_IP: "192.168.1.192"
      TOOL_TYPE: "vacuum"
      # The outermost deadman, enforced by the controller whether or not any of
      # this is alive. Every timer above must stay below it.
      VELOCITY_COMMAND_TTL_S: "0.5"
      # The pad does not home. Nothing moves until somebody pushes a stick.
      INIT_MODE: "none"