diff --git a/.github/workflows/docker-publish.yaml b/.github/workflows/docker-publish.yaml new file mode 100644 index 0000000000..bb9aaa21ea --- /dev/null +++ b/.github/workflows/docker-publish.yaml @@ -0,0 +1,82 @@ +name: Build and Push Docker Image + +on: + push: + branches: + - ibis + tags: + - 'v*.*.*' + pull_request: + branches: + - ibis + paths: + - 'data/docker/Dockerfile.simulatorcli' + - 'data/docker/simulatorcli_entrypoint.bash' + - '.github/workflows/docker-publish.yaml' + workflow_dispatch: + +env: + REGISTRY: ghcr.io + IMAGE_NAME: ibis-ssl/framework-simulatorcli + +jobs: + build-and-push: + runs-on: ubuntu-latest + permissions: + contents: read + packages: write + attestations: write + id-token: write + + steps: + - name: Checkout repository + uses: actions/checkout@v4 + with: + submodules: recursive + + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v3 + + - name: Log in to Container Registry + if: github.event_name != 'pull_request' + uses: docker/login-action@v3 + with: + registry: ${{ env.REGISTRY }} + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + + - name: Extract metadata (tags, labels) + id: meta + uses: docker/metadata-action@v5 + with: + images: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }} + tags: | + type=raw,value=latest,enable={{is_default_branch}} + type=ref,event=branch + type=semver,pattern={{version}} + type=semver,pattern={{major}}.{{minor}} + type=sha,prefix=,format=short + type=ref,event=pr + + - name: Build and push Docker image + id: push + uses: docker/build-push-action@v6 + with: + context: . + file: ./data/docker/Dockerfile.simulatorcli + platforms: linux/amd64 + push: ${{ github.event_name != 'pull_request' }} + tags: ${{ steps.meta.outputs.tags }} + labels: ${{ steps.meta.outputs.labels }} + cache-from: type=gha + cache-to: type=gha,mode=max + provenance: true + sbom: true + + - name: Generate artifact attestation + if: github.event_name != 'pull_request' + uses: actions/attest-build-provenance@v2 + with: + subject-name: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }} + subject-digest: ${{ steps.push.outputs.digest }} + push-to-registry: true diff --git a/CMakeLists.txt b/CMakeLists.txt index 144366ffdf..695a6584d8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -222,6 +222,8 @@ find_package(V8 10.5.7) set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) add_subdirectory(src) +install(DIRECTORY config/ DESTINATION config FILES_MATCHING PATTERN "*.txt") + if(UNIX AND NOT APPLE) configure_file(data/pkg/ra.desktop.in ra.desktop) configure_file(data/pkg/ra-logplayer.desktop.in ra-logplayer.desktop) diff --git a/README.md b/README.md index de7190d8f4..55ffef85e9 100644 --- a/README.md +++ b/README.md @@ -57,6 +57,26 @@ The `simulator-cli` takes three command line arguments: A `short_file_name` is just the filename without the path or the extension. For example, to start the binary with no realism and 2018 setup, call `simulator-cli -g 2018 --realism None` +#### Robot-side position control (ibis) + +The ibis command receiver emulates the robot's STM32 (G474) main board: it +accepts `POLAR_VELOCITY_TARGET` (control mode 3) only and does **not** close a +position loop. In the robot-side position control setup, `crane` sends +`POSITION_TARGET` (mode 4) commands that are consumed by the CM4 position +controller (`cm4_sim`) running between `crane` and the simulator: + +```text +crane --mode 4--> cm4_sim --mode 3--> simulator-cli + ^ | + +--- ibis feedback ---+ +``` + +A mode 4 command arriving directly at the simulator means `cm4_sim` is missing +from the chain; the simulator stops that robot and logs why instead of +misreading the mode arguments. See +[docs/robot-side-position-control.md](docs/robot-side-position-control.md) for +the packet contract and port assignment. + ### Other utilities This repo also contains various utilities: - `amun-cli` - run an AI script from the command line. diff --git a/config/simulator-realism/Ibis.txt b/config/simulator-realism/Ibis.txt new file mode 100644 index 0000000000..b51c8cdbc6 --- /dev/null +++ b/config/simulator-realism/Ibis.txt @@ -0,0 +1,21 @@ +stddev_ball_p: 0.0014 +stddev_robot_p: 0.0013 +stddev_robot_phi: 0.01 +stddev_ball_area: 6.5 +enable_invisible_ball: true +ball_visibility_threshold: 0.4 +camera_overlap: 1 +dribbler_ball_detections: 0.05 +camera_position_error: 0.1 +robot_command_loss: 0.03 +robot_response_loss: 0.1 +missing_ball_detections: 0.05 +vision_delay: 35000000 +vision_processing_time: 10000000 +simulate_dribbling: false +object_position_offset: 0.02 +missing_robot_detections: 0.02 +command_delay: 3000000 +robot_rotation_error: 0.5 +rotated_robot_detections_start: 0.001 +rotated_robot_detections_stop: 0.3 diff --git a/data/docker/Dockerfile.simulatorcli b/data/docker/Dockerfile.simulatorcli index a7ab54fabd..e225360b87 100644 --- a/data/docker/Dockerfile.simulatorcli +++ b/data/docker/Dockerfile.simulatorcli @@ -10,8 +10,7 @@ RUN set -xe; \ apt-get install --no-install-recommends -y \ cmake make g++ libssl-dev patch \ protobuf-compiler libprotobuf-dev \ - qt6-base-dev libqt6opengl6-dev \ - qtbase5-dev libqt5opengl5-dev; \ + qt6-base-dev libqt6opengl6-dev; \ apt-get clean -y; \ rm -rf /var/lib/apt/lists/*; @@ -22,9 +21,9 @@ COPY . . RUN set -xe; \ mkdir build; \ cd build; \ - cmake -DCMAKE_BUILD_TYPE=RelWithDebInfo ..; \ + cmake -DCMAKE_BUILD_TYPE=RelWithDebInfo -DRELATIVE_DATA_DIRS=ON ..; \ make simulator-cli -j $(nproc); \ - find . -maxdepth 1 ! -name 'bin' -exec rm -r {} \; ; + cmake --install . --prefix /home/default/install; # # Run stage @@ -37,9 +36,8 @@ ARG DEBIAN_FRONTEND=noninteractive RUN set -xe; \ apt-get update; \ apt-get install --no-install-recommends -y \ - qt6-base-dev libqt6opengl6-dev \ - qtbase5-dev libqt5opengl5-dev \ - libprotobuf-dev tini; \ + libqt6core6t64 libqt6gui6t64 libqt6network6t64 libqt6opengl6t64 \ + libprotobuf32t64 tini; \ apt-get clean -y; \ rm -rf /var/lib/apt/lists/*; @@ -47,15 +45,13 @@ RUN useradd --create-home --shell /bin/bash default USER default WORKDIR /home/default -COPY --chown=default:default /data/docker/simulatorcli_entrypoint.bash . -ENTRYPOINT ["tini", "--", "./simulatorcli_entrypoint.bash"] - -COPY --chown=default:default --from=build-stage /home/default/COPYING . -COPY --chown=default:default --from=build-stage /home/default/COPYING.GPL . -COPY --chown=default:default --from=build-stage /home/default/config config -COPY --chown=default:default --from=build-stage /home/default/build build +COPY --chown=default:default --from=build-stage /home/default/install/bin/simulator-cli bin/simulator-cli +COPY --chown=default:default --from=build-stage /home/default/install/config config # 10300: Control - Accepts simulator configuration commands # 10301: Blue - Accepts robot commands by the blue team # 10302: Yellow - Accepts robot commands by the yellow team EXPOSE 10300 10301 10302 + +ENTRYPOINT ["tini", "--"] +CMD ["./bin/simulator-cli"] diff --git a/data/docker/Dockerfile.simulatorcli.dockerignore b/data/docker/Dockerfile.simulatorcli.dockerignore index f7f9b4b3ae..4996568519 100644 --- a/data/docker/Dockerfile.simulatorcli.dockerignore +++ b/data/docker/Dockerfile.simulatorcli.dockerignore @@ -16,6 +16,5 @@ /strategy /data !/data/pkg -!/data/docker/simulatorcli_entrypoint.bash # vim: filetype=gitignore diff --git a/data/docker/simulatorcli_entrypoint.bash b/data/docker/simulatorcli_entrypoint.bash deleted file mode 100755 index 0088c422d2..0000000000 --- a/data/docker/simulatorcli_entrypoint.bash +++ /dev/null @@ -1,15 +0,0 @@ -#!/usr/bin/env bash -GEOMETRY_OPTION="" -if [[ -n "$GEOMETRY" ]]; then - GEOMETRY_OPTION="--geometry=$GEOMETRY" - echo "Passing '${GEOMETRY_OPTION}'" -fi - -REALISM_OPTION="" -if [[ -n "$REALISM" ]]; then - REALISM_OPTION="--realism=$REALISM" - echo "Passing '${REALISM_OPTION}'" -fi - -echo "Starting the ER-Force simulator-cli" -exec ./build/bin/simulator-cli "$GEOMETRY_OPTION" "$REALISM_OPTION" diff --git a/data/scripts/ibis-chain-smoketest.py b/data/scripts/ibis-chain-smoketest.py new file mode 100644 index 0000000000..1400f68ca6 --- /dev/null +++ b/data/scripts/ibis-chain-smoketest.py @@ -0,0 +1,295 @@ +"""Smoke test for the ibis command path of simulator-cli. + +The simulator emulates the robot's STM32 (G474) main board: it accepts +POLAR_VELOCITY_TARGET (control mode 3) only and never closes a position loop. +POSITION_TARGET (mode 4) belongs to the robot-side CM4 controller that runs +between crane and the simulator. See docs/robot-side-position-control.md. + +Checks, all against a real simulator-cli process over UDP: + 1. a mode 3 command drives the robot + 2. a mode 4 command stops it and logs a rate-limited warning + 3. a command whose vision_global_pos disagrees with the simulator is dropped, + and says so -- this drop used to be silent, which is indistinguishable from + "commanded to hold still" while packets keep arriving + +Usage: python3 data/scripts/ibis-chain-smoketest.py [path/to/simulator-cli] +""" + +import socket +import struct +import subprocess +import sys +import tempfile +import time +from pathlib import Path + +CMD_SIZE = 64 +SLOTS = 11 +FEEDBACK_SIZE = 128 + +MODE_POLAR_VELOCITY = 3 +MODE_POSITION_TARGET = 4 + +# The bound ports are offset well away from the defaults so a running match is +# untouched. Vision output has no port option on this branch, so it goes to the +# default address; that is send-only (no bind, no conflict), but it does inject +# extra frames into anything listening there -- do not run this during a match. +IBIS_PORT = 12397 +FEEDBACK_BASE = 50700 + + +def encode_two_byte(value, value_range): + raw = int(32767.0 * (value / value_range) + 32767.0) + raw = max(0, min(65535, raw)) + return bytes([(raw >> 8) & 0xFF, raw & 0xFF]) + + +def build_command(counter, pos, yaw, mode, args=(0.0, 0.0), target=(0.0, 0.0)): + """One 64-byte RobotCommandSerializedV2, laid out as in crane's robot_packet.h.""" + d = bytearray(CMD_SIZE) + d[1] = counter & 0xFF + d[2:4] = encode_two_byte(pos[0], 32.767) # VISION_GLOBAL_X + d[4:6] = encode_two_byte(pos[1], 32.767) # VISION_GLOBAL_Y + d[6:8] = encode_two_byte(yaw, 3.14159265) # VISION_GLOBAL_THETA + d[8:10] = encode_two_byte(yaw, 3.14159265) # TARGET_GLOBAL_THETA -> omega ~ 0 + d[12:14] = encode_two_byte(4.0, 32.767) # ACCELERATION_LIMIT + d[14:16] = encode_two_byte(3.0, 32.767) # LINEAR_VELOCITY_LIMIT + d[16:18] = encode_two_byte(5.0, 32.767) # ANGULAR_VELOCITY_LIMIT + d[22] = 0x01 # FLAGS: IS_VISION_AVAILABLE + d[23] = mode # CONTROL_MODE + d[24:26] = encode_two_byte(args[0], 32.767) # CONTROL_MODE_ARGS + d[26:28] = encode_two_byte(args[1], 32.767) + d[32:34] = encode_two_byte(target[0], 32.767) # TARGET_GLOBAL_POS_X + d[34:36] = encode_two_byte(target[1], 32.767) # TARGET_GLOBAL_POS_Y + return bytes(d) + + +def build_packet(robot_id, command): + """715-byte packet: 11 slots of (robot_id, 64-byte command), others zero-filled.""" + packet = bytearray() + for slot in range(SLOTS): + if slot == robot_id: + packet += bytes([robot_id]) + command + else: + packet += bytes([0xFF]) + bytes(CMD_SIZE) + return bytes(packet) + + +def parse_feedback(data): + if len(data) != FEEDBACK_SIZE or data[0] != 0xAB or data[1] != 0xEA: + return None + return { + "counter": data[3], + "yaw": struct.unpack_from(" 0.3 and abs(after_mode4["vx"]) < 0.05 and abs(after_mode4["vy"]) < 0.05 + return ok, (f"mode3 moved {moved3:.3f} m (want > 0.3), " + f"mode4 moved {moved4:.3f} m, " + f"final vel ({after_mode4['vx']:+.3f}, {after_mode4['vy']:+.3f}) " + f"(want ~0)") + finally: + sim.close() + + +def run_position_mismatch(binary, log_path): + """A command claiming the wrong robot position is dropped, and says so. + + The simulator identifies the robot by matching the command's vision_global_pos + against the robots on the field. A sender whose own position estimate has + drifted past the threshold gets its commands dropped -- which looks exactly + like a robot commanded to hold still, so the log line is the only way to tell. + """ + sim = Simulator(binary, log_path=log_path) + try: + state = None + for _ in range(20): + if not sim.alive(): + return False, (f"simulator-cli exited (rc={sim.proc.returncode}); " + f"see {log_path}") + state = sim.recv() + if state: + break + if state is None: + return False, f"no feedback from simulator; see {log_path}" + start = dict(state) + + # Far enough past the threshold that feedback lag cannot pull it back under. + offset = 1.0 + deadline = time.time() + 2.0 + counter = 1 + while time.time() < deadline: + claimed = (state["x"] + offset, state["y"]) + cmd = build_command(counter, claimed, state["yaw"], + MODE_POLAR_VELOCITY, (1.5, 0.0)) + sim.send(build_packet(0, cmd)) + counter += 1 + time.sleep(1 / 60) + sim.rx.settimeout(0.001) + try: + while True: + got = parse_feedback(sim.rx.recv(256)) + if got: + state = got + except socket.timeout: + pass + + moved = distance(state, start) + return moved < 0.05, (f"claimed a position {offset:.1f} m off, robot moved " + f"{moved:.3f} m (want ~0)") + finally: + sim.close() + + +def main(): + repo = Path(__file__).resolve().parents[2] + binary = Path(sys.argv[1]) if len(sys.argv) > 1 else repo / "build" / "bin" / "simulator-cli" + if not binary.exists(): + print(f"simulator-cli not found at {binary}", file=sys.stderr) + return 2 + + tmp = Path(tempfile.mkdtemp(prefix="ibis-smoketest-")) + log = tmp / "simulator-cli.log" + mismatch_log = tmp / "simulator-cli-mismatch.log" + + failures = 0 + ok, detail = run_commands(binary, log) + print(f"[{'PASS' if ok else 'FAIL'}] commands: {detail}") + failures += 0 if ok else 1 + + text = log.read_text() + warnings = [line for line in text.splitlines() if "POSITION_TARGET" in line] + ok = len(warnings) > 0 + print(f"[{'PASS' if ok else 'FAIL'}] warning: {len(warnings)} POSITION_TARGET " + f"warning(s) logged (rate limited to 1/s per robot)") + failures += 0 if ok else 1 + + # The commands above always claim the position the feedback just reported, so a + # drop here would mean the matching rejects agreeing positions. + stray = [line for line in text.splitlines() if "command dropped" in line] + ok = not stray + print(f"[{'PASS' if ok else 'FAIL'}] no false drops: {len(stray)} drop warning(s) " + f"while the claimed position agreed (want 0)") + failures += 0 if ok else 1 + + ok, detail = run_position_mismatch(binary, mismatch_log) + print(f"[{'PASS' if ok else 'FAIL'}] position mismatch: {detail}") + failures += 0 if ok else 1 + + drops = [line for line in mismatch_log.read_text().splitlines() + if "command dropped" in line] + ok = len(drops) > 0 + print(f"[{'PASS' if ok else 'FAIL'}] drop warning: {len(drops)} warning(s) logged " + f"(rate limited to 1/s per robot)") + failures += 0 if ok else 1 + + print(f"\nlogs: {tmp}") + return 1 if failures else 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/data/scripts/ibis-packet-tap.py b/data/scripts/ibis-packet-tap.py new file mode 100644 index 0000000000..aefa3f0566 --- /dev/null +++ b/data/scripts/ibis-packet-tap.py @@ -0,0 +1,139 @@ +"""Inline tap for the ibis 715-byte command stream. + +Binds a UDP port, decodes every robot slot, and optionally forwards the packet +on unchanged. Put it between two hops to see what is actually on the wire +without a packet capture -- useful where loopback capture needs root but the +chain's own ports are already held by listeners: + + crane --> cm4_sim --out-port 12399 --> [tap 12399] --> simulator-cli 12346 + +Usage: + python3 data/scripts/ibis-packet-tap.py --port 12399 \ + [--forward 127.0.0.1:12346] [--robot-ids 0,1] [--every 60] [--count 0] + +--every N print one line per N packets for the robot (default 1; 0 = summary only) +--count N stop after N datagrams (default 0 = run until Ctrl-C) + +Prints, per slot: control mode, check counter, vision pose, target pose, +mode args, and the limits, so a mode 4 -> mode 3 conversion can be read +directly off both sides of a hop. +""" + +import argparse +import socket +import struct +import sys + +CMD_SIZE = 64 +SLOTS = 11 +PACKET_SIZE = SLOTS * (CMD_SIZE + 1) + +MODE_NAMES = {3: "POLAR_VELOCITY", 4: "POSITION_TARGET"} + + +def two_byte(d, i, rng): + raw = (d[i] << 8) | d[i + 1] + return (raw - 32767.0) / 32767.0 * rng + + +def decode(d): + """Offsets follow crane_sender/include/crane_sender/robot_packet.h.""" + mode = d[23] + out = { + "counter": d[1], + "mode": mode, + "vision": (two_byte(d, 2, 32.767), two_byte(d, 4, 32.767)), + "vision_theta": two_byte(d, 6, 3.14159265), + "target_theta": two_byte(d, 8, 3.14159265), + "accel_limit": two_byte(d, 12, 32.767), + "vel_limit": two_byte(d, 14, 32.767), + "flags": d[22], + "target_pos": (two_byte(d, 32, 32.767), two_byte(d, 34, 32.767)), + "terminal_velocity": two_byte(d, 36, 32.767), + } + if mode == 3: + out["args"] = ("r", two_byte(d, 24, 32.767), "theta", two_byte(d, 26, 32.767)) + elif mode == 4: + out["args"] = ("tv_x", two_byte(d, 24, 32.767), "tv_y", two_byte(d, 26, 32.767)) + else: + out["args"] = ("raw", d[24:32].hex()) + return out + + +def is_empty(d): + return not any(d) + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--port", type=int, required=True) + ap.add_argument("--bind", default="0.0.0.0") + ap.add_argument("--forward", default="", help="host:port to pass the packet on to") + ap.add_argument("--robot-ids", default="", help="only print these ids (default: all)") + ap.add_argument("--every", type=int, default=1, help="print 1 line per N packets, 0 = summary only") + ap.add_argument("--count", type=int, default=0, help="stop after N datagrams (0 = forever)") + args = ap.parse_args() + + wanted = None + if args.robot_ids: + wanted = {int(x) for x in args.robot_ids.split(",") if x.strip()} + + rx = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + rx.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + rx.bind((args.bind, args.port)) + + tx = fwd = None + if args.forward: + host, _, port = args.forward.partition(":") + fwd = (host, int(port)) + tx = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + + print(f"tap listening on {args.bind}:{args.port}" + + (f", forwarding to {fwd[0]}:{fwd[1]}" if fwd else ", not forwarding"), + flush=True) + + seen = 0 + mode_counts = {} + try: + while True: + data, _ = rx.recvfrom(2048) + if fwd: + tx.sendto(data, fwd) # 先に転送してチェーンを止めない + if len(data) != PACKET_SIZE: + print(f" !! size {len(data)} (expected {PACKET_SIZE})", flush=True) + continue + seen += 1 + for slot in range(SLOTS): + off = slot * (CMD_SIZE + 1) + rid = data[off] + d = data[off + 1: off + 1 + CMD_SIZE] + if is_empty(d) or rid >= SLOTS: + continue + if wanted is not None and rid not in wanted: + continue + info = decode(d) + mode_counts[info["mode"]] = mode_counts.get(info["mode"], 0) + 1 + if args.every and seen % args.every == 0: + name = MODE_NAMES.get(info["mode"], f"UNKNOWN({info['mode']})") + a = info["args"] + print( + f"#{seen:6d} id={rid} cnt={info['counter']:3d} mode={info['mode']}({name}) " + f"vision=({info['vision'][0]:+.3f},{info['vision'][1]:+.3f}) " + f"{a[0]}={a[1]:+.3f} {a[2]}={a[3]:+.3f} " + f"target=({info['target_pos'][0]:+.3f},{info['target_pos'][1]:+.3f}) " + f"term={info['terminal_velocity']:+.3f} " + f"vlim={info['vel_limit']:.2f} flags=0x{info['flags']:02x}", + flush=True) + if args.count and seen >= args.count: + break + except KeyboardInterrupt: + pass + finally: + print(f"\n--- {seen} datagrams ---", flush=True) + for m in sorted(mode_counts): + print(f" mode {m} ({MODE_NAMES.get(m, 'UNKNOWN')}): {mode_counts[m]} slots", flush=True) + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/data/scripts/ibis-stop-distance.py b/data/scripts/ibis-stop-distance.py new file mode 100644 index 0000000000..1a5a946829 --- /dev/null +++ b/data/scripts/ibis-stop-distance.py @@ -0,0 +1,201 @@ +"""Measure how far a robot travels after each of the three ways it can stop. + +The simulator reproduces the robot's two hardware stop paths. The wheel PID brakes +when told to (mode 3 with r=0). Drive is cut entirely when the stop condition fires +-- the real G474 answers that with omniStopAll(), which writes duty 0, and the CAN +frame has no brake bit, so hardware coasts. A third case, commands simply not +arriving, keeps executing the last command for 0.1 s and then coasts as well. +See docs/robot-side-position-control.md. + +Two things make a naive measurement of this wrong in a way that looks right: + + * Measure in +y only, and on both teams. Robots start near y=-2.8, so -y is the + wall and both x directions are blocked by other robots. A blocked run still + produces a plausible number -- 0.12 to 0.40 m, varying run to run -- and there + is nothing in the reading that says it was blocked. Both teams agreeing to a + few mm is the signal that the coast was free. + + * One simulator process per measurement. The feedback velocity field freezes when + commands stop arriving (see the fidelity gaps in the doc above), so a previous + run's frozen value is read as "already at speed" and the next run never + accelerates -- reporting a full-speed start and a stopping distance of zero. + +Usage: python3 data/scripts/ibis-stop-distance.py [kinds] [base-port] [binary] + kinds: comma separated, any of brake,estop,drop (default: all three) +""" + +import math +import socket +import struct +import subprocess +import sys +import tempfile +import time +from pathlib import Path + +# Commands are sent at this speed; the run starts measuring once the robot reaches +# TARGET_V. Stopping distance goes with speed, so the three paths are only +# comparable if they all start from the same one. A fixed-duration acceleration +# phase does not do this: the speed reached varies with the robot's start pose. +COMMAND_SPEED = 1.5 +TARGET_V = 1.45 + +MODE_POLAR_VELOCITY = 3 +HEADING_PLUS_Y = math.pi / 2 + + +def encode_two_byte(value, value_range): + raw = int(32767.0 * (value / value_range) + 32767.0) + raw = max(0, min(65535, raw)) + return bytes([(raw >> 8) & 0xFF, raw & 0xFF]) + + +def build_packet(counter, pos, speed, theta, stop_emergency=False): + """715-byte packet driving robot 0 at `speed` along global direction `theta`.""" + d = bytearray(64) + d[1] = counter & 0xFF + d[2:4] = encode_two_byte(pos[0], 32.767) # VISION_GLOBAL_X + d[4:6] = encode_two_byte(pos[1], 32.767) # VISION_GLOBAL_Y + d[6:8] = encode_two_byte(0.0, math.pi) # VISION_GLOBAL_THETA + d[8:10] = encode_two_byte(0.0, math.pi) # TARGET_GLOBAL_THETA + d[12:14] = encode_two_byte(4.0, 32.767) # ACCELERATION_LIMIT + d[14:16] = encode_two_byte(4.0, 32.767) # LINEAR_VELOCITY_LIMIT + d[16:18] = encode_two_byte(5.0, 32.767) # ANGULAR_VELOCITY_LIMIT + d[22] = 0x01 | (0x08 if stop_emergency else 0x00) # IS_VISION_AVAILABLE | STOP_EMERGENCY + d[23] = MODE_POLAR_VELOCITY + d[24:26] = encode_two_byte(speed, 32.767) # CONTROL_MODE_ARGS: r + # theta is in the same +-32.767 range as r, not +-pi (ibis_protocol.h). Packing + # it as a pi-range angle silently drives the robot in a different direction. + d[26:28] = encode_two_byte(theta, 32.767) + packet = bytearray() + for slot in range(11): + packet += (bytes([0]) + bytes(d)) if slot == 0 else (bytes([0xFF]) + bytes(64)) + return bytes(packet) + + +def parse_feedback(data): + """(x, y, vx) in metres and m/s, or None if this is not a feedback packet.""" + if len(data) != 128 or data[0] != 0xAB or data[1] != 0xEA: + return None + return (struct.unpack_from(" 1 else ["brake", "estop", "drop"] + port = int(sys.argv[2]) if len(sys.argv) > 2 else 15600 + repo = Path(__file__).resolve().parents[2] + binary = Path(sys.argv[3]) if len(sys.argv) > 3 else repo / "build" / "bin" / "simulator-cli" + if not binary.exists(): + print(f"simulator-cli not found at {binary}", file=sys.stderr) + return 2 + + results = {} + for team in ("yellow", "blue"): + row = [] + for kind in kinds: + distance, error = measure(binary, kind, team, port) + port += 2 + results[(team, kind)] = distance + row.append(f"{kind}={'FAILED' if distance is None else format(distance, '.3f')}" + + (f" ({error})" if error else "")) + print(f"{team:6s} +y: " + " ".join(row)) + + # Both teams are different robots on different parts of the field. Agreeing means + # neither hit anything; disagreeing means at least one run was blocked and the + # numbers cannot be compared against each other or against the doc. + print() + ok = True + for kind in kinds: + a, b = results[("yellow", kind)], results[("blue", kind)] + if a is None or b is None: + print(f"[FAIL] {kind}: a run did not complete") + ok = False + continue + spread = abs(a - b) + agree = spread < 0.02 + ok = ok and agree + print(f"[{'PASS' if agree else 'FAIL'}] {kind}: teams agree to {spread:.3f} m " + f"(want < 0.020; larger means something was in the way)") + return 0 if ok else 1 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/docs/robot-side-position-control.md b/docs/robot-side-position-control.md new file mode 100644 index 0000000000..a58b7b5d0f --- /dev/null +++ b/docs/robot-side-position-control.md @@ -0,0 +1,500 @@ +# ロボット側位置制御（CM4 で位置ループを閉じる構成） + +## 目的 + +従来は crane（AI）側で位置制御ループを閉じ、その出力である速度指令を無線で +ロボットへ送っていた。この構成では **不安定で遅延が乗る無線経路が位置制御 +ループの内側に入る**。 + +新構成では crane は `VisibilityGraphPlanner` が生成した **位置指令** を送り、 +ロボット側の CM4 が位置制御ループを閉じて **速度指令** をマイコン（G474）へ +渡す。無線経路はループの外側（目標値の更新経路）に移動する。 + +このドキュメントは、その構成を **シミュレータ上で実機とできるだけ同じ形** で +再現するための、3 リポジトリ間の統合仕様を定める。 + +## 構成の対応 + +### 実機 + +```text +crane ──UDP broadcast :12345 (mode 4 位置指令)──> CM4: ai_cmd_v2.out + │ 位置制御ループ + ↓ UART /dev/serial0 72B (mode 3 速度指令) + G474 (500 Hz): 加速度制御・タイヤ速度制御 + │ UART 128B feedback + ↓ + CM4: forward_robot_feedback.out + ↓ multicast 224.5.20.(100+N):50100+N + crane / host ツール +``` + +### シミュレータ + +```text +crane ──UDP :12345 (mode 4 位置指令)──> cm4_sim + │ 位置制御ループ（実機と同一コード） + ↓ UDP :12346 (mode 3 速度指令) + simulator-cli ← G474 + ロボット物理 を担当 + │ ibis feedback 128B + ↓ UDP :50100+id + cm4_sim + ↓ multicast 224.5.20.(100+id):50100+id + crane / host ツール +``` + +**対応関係** + +| 実機 | シミュレータ | 備考 | +|---|---|---| +| crane | crane | 変更なし（送信先ポートのみ切替） | +| 無線 (WiFi) | UDP + 劣化注入 | `cm4_sim` の入力側で遅延・ジッタ・ロスを注入 | +| CM4 位置制御 | `cm4_sim` | **実機と同一の position_controller ライブラリを使う** | +| UART CM4→G474 | UDP :12346 | | +| G474 の速度・加速度制御 | `simulator-cli` の `IbisCommandAdaptor` | theta P 制御 + 加速度制限 | +| ロボット物理・タイヤ | `simulator-cli` (amun/Bullet) | | +| UART G474→CM4 feedback | ibis feedback 128B (UDP) | | + +シミュレータは **G474 とロボット本体** を担当する。位置制御は行わない。 + +## パケット契約 + +### SSOT + +`RobotCommandSerializedV2`（64 バイト）の正本は +`crane/crane_sender/include/crane_sender/robot_packet.h` とする。 + +現在のオフセット一致状況（2026-09-13 実測）: + +| リポジトリ | ファイル | 状態 | +|---|---|---| +| crane | `crane_sender/include/crane_sender/robot_packet.h` | 正本 | +| G474_Orion_main | `Core/Inc/robot_packet.h` | byte 0..31 一致（32..37 は未使用のため未定義。問題なし） | +| framework | `src/simulator/ibis_protocol.h` | 一致 | +| **Orion_CM4** | `cm4/bridge/robot_packet.h` | **不一致（旧レイアウト）** | + +Orion_CM4 のコピーは `ACCELERATION_LIMIT` が無いため **byte 12 以降が 2 バイト +ずれている**（`FLAGS`=20、`CONTROL_MODE`=21）。現在これが顕在化していないのは、 +`forward_ai_cmd_v2.cpp` が受信バイト列を `memcpy` でそのまま UART へ転送する +**単なるバイト転送器** であり、デシリアライズ結果をデバッグ表示にしか使って +いないため。CM4 がパケットを解釈して制御ループを閉じた瞬間、これは正真正銘の +バグになる。**新構成の実装前に必ず統一すること。** + +### バイトオフセット（正本） + +```text + 0 HEADER + 1 CHECK_COUNTER + 2..3 VISION_GLOBAL_X (float, range 32.767) + 4..5 VISION_GLOBAL_Y (float, range 32.767) + 6..7 VISION_GLOBAL_THETA (float, range PI) + 8..9 TARGET_GLOBAL_THETA (float, range PI) +10 KICK_POWER (value * 20) +11 DRIBBLE_POWER (value * 20) +12..13 ACCELERATION_LIMIT (float, range 32.767) +14..15 LINEAR_VELOCITY_LIMIT (float, range 32.767) +16..17 ANGULAR_VELOCITY_LIMIT (float, range 32.767) +18..19 LATENCY_TIME_MS (uint16) +20..21 ELAPSED_TIME_MS_SINCE_LAST_VISION (uint16) +22 FLAGS +23 CONTROL_MODE +24..31 CONTROL_MODE_ARGS (mode により意味が変わる union) +32..33 TARGET_GLOBAL_POS_X (float, range 32.767) +34..35 TARGET_GLOBAL_POS_Y (float, range 32.767) +36..37 TERMINAL_VELOCITY (float, range 32.767) +``` + +FLAGS: bit0 `IS_VISION_AVAILABLE` / bit1 `ENABLE_CHIP` / bit3 `STOP_EMERGENCY` + +### 制御モード + +| 値 | 名前 | ARGS (24..31) | 送信元 → 受信先 | +|---|---|---|---| +| 3 | `POLAR_VELOCITY_TARGET_MODE` | `target_global_velocity_r`, `target_global_velocity_theta` | CM4 → G474 / cm4_sim → simulator-cli | +| 4 | `POSITION_TARGET_WITH_TERMINAL_VELOCITY_MODE` | `terminal_velocity_x`, `terminal_velocity_y` | crane → CM4 / crane → cm4_sim | + +#### mode 4 の終端速度フィールドの意味（重要） + +mode 4 は終端速度を 2 か所で運ぶが、**これらは冗長ではなく意味が違う**。 + +| フィールド | 位置 | 意味 | +|---|---|---| +| `terminal_velocity_x` / `terminal_velocity_y` | ARGS 24..27 | 目標位置に到達した瞬間の速度**ベクトル**（グローバル座標, m/s）。フィードフォワード項であり、向きに意味がある | +| `terminal_velocity`（= crane の `speed_limit_at_target`） | 32..37 の 36..37 | 上のベクトルの**大きさに対する上限**（スカラ）。名前に反して「終端速度そのもの」ではない | + +**受信側（CM4）が適用すべき規則**（`crane_sender/src/sim_position_controller.cpp` +`calculateSimGlobalVelocity()` が参照実装）: + +``` +feedforward = (terminal_velocity_x, terminal_velocity_y) +if terminal_velocity > 0: # スカラ上限。0 は「上限なし」であって「停止」ではない + feedforward = clampNorm(feedforward, terminal_velocity) +``` + +- **送信側に整合義務は無い。** クランプするのは受信側。 + `|v_xy| == terminal_velocity` を crane に要求しない。 +- `terminal_velocity == 0` は **上限なし**（クランプをスキップ）。 + 停止させたい場合は `terminal_velocity_x/y` 自体を 0 にする。実際 + `visibility_graph_planner.cpp` の到達時分岐はそうしている。 +- なお現行の VisibilityGraphPlanner は非最終ウェイポイントで + `speed_limit_at_target = terminal_speed` かつ + `terminal_velocity_xy = direction * terminal_speed` を入れるので、結果として + 両者の大きさは一致する。これは実装の都合であって仕様上の保証ではない。 + +#### 伝送されないフィールド + +`crane_msgs/msg/PositionTargetMode.msg` の `position_tolerance` は +**ibis パケットに載っていない**（`createRobotPacket()` が送っていない）。 +参照実装は到達判定に +`error.norm() <= position_tolerance && feedforward.norm() < 1e-4` を使うため、 +CM4 はこの判定をそのまま再現できない。 + +暫定方針: CM4 側の固定定数（例 0.02 m）を使う。 +ARGS には 28..31 の 4 バイトが空いているので、精度が問題になるなら +`position_tolerance` を 28..29 へ載せる拡張が可能。その場合は 4 リポジトリすべての +`robot_packet.h` を同時に更新すること。 + +**`CONTROL_MODE_ARGS` は union であり、`CONTROL_MODE` を見ずに復号してはならない。** +mode 4 のパケットを mode 3 として復号すると、`terminal_velocity_x/y` が +`r/theta` として読まれ、無言で暴走する。 + +### パケット全体 + +1 スロット = `robot_id` 1 バイト + コマンド 64 バイト = 65 バイト。 +11 スロット固定で 715 バイト。送信元が制御しないロボットのスロットは +ゼロ埋めする（受信側は `ibisSlotIsEmpty()` 相当で明示的にスキップすること）。 + +## ポート割当 + +| 経路 | アドレス:ポート | 備考 | +|---|---|---| +| crane → cm4_sim（位置指令） | `127.0.0.1:12345` | 実機の AI 指令ポートと同じ | +| cm4_sim → simulator-cli（速度指令） | `127.0.0.1:12346` | `simulator-cli --ibis-port 12346` | +| simulator-cli → cm4_sim（feedback） | `127.0.0.1:50100+id` | `--ibis-feedback-addr 127.0.0.1`（既定） | +| cm4_sim → crane / host（feedback 再配信） | `224.5.20.(100+id):50100+id` | 実機と同じ multicast。**crane 側に `feedback_sim_mode:=false` が必須**（下記） | +| simulator-cli → crane（vision） | `224.5.23.2:10020`（既定・変更不可） | 変更なし | + +### feedback ポートの取り合い（必読・踏むと沈黙して壊れる） + +feedback のベースポートは実機と同じ 50100 を使うが、**crane を素の `sim:=true` で +起動すると cm4_sim と衝突する。** + +`crane_robot_receiver` は `sim_mode` が真だと購読先を `127.0.0.1` に切り替える +（`robot_receiver_node.cpp:351-352`）。`crane_comm/unicast.hpp:81` は +`addr.is_multicast()` で分岐するため、`127.0.0.1` は multicast 側ではなく +`:138-139` の素の bind に落ち、**グループ参加もしない**。さらに `:78-79` で +`SO_REUSEADDR` と **`SO_REUSEPORT` の両方**を設定している。 + +つまり `sim:=true` の crane は、cm4_sim が simulator-cli の feedback を受けるために +必要な `127.0.0.1:50100+id` を、まったく同じ形で先に押さえる。 + +`SO_REUSEPORT` 付きの 2 ソケットが同一ポートを bind した場合の実測（unicast 200 発、 +送信元は単一ソケット）: + +| | 受信数 | +|---|---| +| socket A | **0** | +| socket B | **200** | + +**均等分割ではなく片方が全部取る。** `SO_REUSEPORT` の振り分けは送信元を含む +4-tuple ハッシュで決まるため、simulator-cli が単一ソケットから送る feedback は +単一フローとなり、必ずどちらか一方に全量が入る。どちらが当たるかはハッシュ次第で +実行ごとに変わりうる。 + +新構成では feedback が位置制御ループ内で**唯一の位置信号**なので、これは +「cm4_sim の位置制御が完全に死ぬ」か「正常に動く」かの二択になり、しかも +実行ごとに変わるため切り分けが極めて困難になる。 + +**対処**: CM4-in-the-loop 構成では crane に **`feedback_sim_mode:=false`** を渡す +（`crane.launch.xml` に新設済み。既定は `$(var sim)` なので既存構成は不変）。 +これで crane は実機と同じ `224.5.20.(100+id):50100+id` の multicast を bind し、 +unicast は cm4_sim が独占する。 + +この条件下では両者は競合しない。multicast 受け（`0.0.0.0:port` bind + group join）と +unicast 受け（`127.0.0.1:port` bind）の同居を実測した結果: + +| 送信 | multicast socket | unicast socket | +|---|---|---| +| unicast 200 発 | 0 | 200 | +| multicast 200 発 | 200 | 0 | + +完全に分離され、取り違えも取り合いも起きない。 + +simulator-cli の `--ibis-feedback-addr` の既定値が `127.0.0.1` なので、 +feedback 関連のオプション指定は不要である。 + +vision の出力先ポート・アドレスを変えるオプションは無い（`ibis` ブランチ時点）。 +`simulator-cli` は vision を **10020 番**でマルチキャストする（通常の 10006 ではなく、 +大会ネットワークでの衝突回避のため）。同一ホストで複数インスタンスを走らせると +vision が混線するので、並列実行が必要ならコンテナのネットワーク分離を使うこと。 + +また、`simulator-cli` は SSL tracker（`TrackedFrame`）を出力しない。 +crane はロボットを tracker 経路で追跡するため、**外部の auto-referee が必要**である +（`docker/scenario/docker-compose.yaml` の `autoref-tigers` がこれを担っている）。 + +### 指令の位置照合（必読・踏むと沈黙して壊れる） + +`simulator-cli` は指令に載っている `vision_global_pos`（byte 2..5）を +フィールド上のロボット位置と突き合わせて、**どちらのチームの何番か**を判定する +（`IbisCommandAdaptor`）。実機の無線には「青/黄」の区別が無く、ロボット自身が +自分宛のスロットだけを読むため、シミュレータ側はこの照合でチームを決めるしかない。 + +照合の閾値は `IBIS_POSITION_MATCH_THRESHOLD = 0.5` m（`ibis_protocol.h`）である。 +**送信側が思っているロボット位置が真の位置から 0.5 m 以上ずれると、その指令は +破棄される。** + +実測（`-g 2020B --realism None`、mode 3 で 3 秒指令）: + +| 指令の位置ずれ | 3 秒の移動量 | +|---|---| +| 0.00 m | 1.47 m | +| 0.40 m | 1.61 m | +| 0.49 m | 1.07 m | +| **0.60 m** | **0.00 m** | +| 3.00 m | 0.00 m | + +この破棄は **2026-09-13 以前は完全に無音**だった。パケットは届き続け、 +`check_counter` も進み、警告も出ず、ロボットだけが動かない。「その場で静止せよ」 +と指令されている状態と区別が付かない。現在は 1 秒/台 のレート制限付きで + +``` +ibis: robot 0 command dropped -- vision_global_pos (-3.700, -2.800) is 0.599 m +from the robot, over the 0.50 m match threshold. +``` + +を出す。最も近い候補までの距離を併記するので、「推定が少し古いだけ」（閾値を +わずかに超える）のか「まったく別の場所を指している」のかを切り分けられる。 + +**A/B 比較にとっては非対称な罠である。** `cm4_sim` は `--vision-echo-feedback` +で feedback 由来の実位置を `vision_global_pos` に上書きしてから下流へ流すため、 +**新構成側はこの破棄に免疫がある**。一方、基準側（crane → simulator-cli 直送）は +crane の world model 推定をそのまま送るので無防備である。crane の推定が 0.5 m +ずれると、**基準側だけが指令を失って動かず、新構成側は正常に動く** — A/B の差が +制御方式の優劣ではなく破棄の有無で決まってしまう。基準側の計測前に、この警告が +出ていないことを必ず確認すること。 + +### ログのバッファリング（Docker / systemd で踏む） + +`simulator-cli` は `main()` で `setvbuf(stdout, nullptr, _IOLBF, 0)` を呼び、 +**自分で stdout を行バッファリングする**。呼び出し側が `stdbuf -oL` を付ける必要は無い。 + +**`stderr` には設定しない。** glibc は stderr を既定で **バッファ無し**にしており、 +`_IOLBF` はそれより弱い設定になる（改行で終わらない出力を抱え込むようになる）。 +実測（改行なしで出力してから `SIGTERM`）: + +| stderr の設定 | 残ったバイト数 | +|---|---| +| 既定（glibc, バッファ無し） | 27 | +| `setvbuf(_IOLBF)` | **0** | + +これが無いと、stdout がパイプやファイルのとき（Docker・systemd・テストハーネスは +すべてそう）ブロックバッファリングされ、`log()` は flush しないので **SIGTERM で +書きかけが丸ごと消える**。実測（同一条件、setvbuf の有無だけが違う）: + +| | SIGTERM 後のログ | +|---|---| +| setvbuf 無し | **0 バイト / 0 行** | +| setvbuf あり | 1049 バイト / 6 行（破棄警告 3 行を含む） | + +**異常時こそログが消える**ため、公開イメージの動作確認で「破棄警告 0 行」という +誤った結論を一度出している（実際には破棄されていた）。 + +`cm4_sim` / `ai_cmd_v2` も同じ理由で `setvbuf` するようになった（Orion_CM4 `d61a94d`）。 + +## タイミング契約 + +### 各段のレート + +シミュレータは実時間で動く（wall clock 駆動）。 + +| 段 | 実機 | シミュレータ | +|---|---|---| +| crane 指令 | 約 60 Hz | vision フレーム毎 = 62.5 Hz | +| CM4 位置制御ループ | 1 kHz（`usleep(1000)` ポーリング） | `--rate-hz`（既定 1 kHz） | +| G474 メインループ | 500 Hz (`MAIN_LOOP_CYCLE`) | 125 Hz（sim 基本ループ 8 ms） | +| vision 送出 | 約 60 Hz | 62.5 Hz（基本ループ 2 回に 1 回） | +| feedback | 約 125 Hz | 125 Hz（基本ループ毎） | + +simulator-cli の基本ループは **8 ms = 125 Hz**（`Simulator::setScaling()` の +"scale default timing of 8 milliseconds (125Hz)"）で、vision はその 2 回に 1 回なので +62.5 Hz。Bullet の物理サブステップは 250 Hz（`SUB_TIMESTEP = 1/250.f`）。 + +CM4 制御ループ 1 kHz に対し G474 相当（シミュレータ基本ループ）が 125 Hz なので、 +**実機（1 kHz : 500 Hz）より内側ループが粗い**。制御ゲインを詰める際はこの差を +意識すること。 + +### feedback はループ駆動である + +`simulator-cli` の ibis feedback は独立したタイマーではなく +`Simulator::handleSimulatorTick()` の中（`src/amun/simulator/simulator.cpp`）で +**シミュレータ基本ループ 1 回につき 1 回** 送出される。位置は +`world::SimulatorState`（ground truth）由来なので毎回新鮮である +（`--ibis-feedback-hz` オプションは存在しない）。 + +新構成では **この feedback の位置が位置制御ループ内で唯一の位置信号** になる。 +従来は装飾的なテレメトリだったが、これからは制御品質を直接左右する。 + +### check_counter の扱い（重要） + +G474 の `checkConnect2AI()`（`Core/Src/ai_comm.c`）は +**`check_counter` が変化し続けること** を AI 接続生存の判定に使う。 +変化が `AI_CMD_TIMEOUT(0.5) * MAIN_LOOP_CYCLE(500)` = **250 ms** 途切れると +`connected_ai = false` になる。 + +従来の CM4 は crane のパケットをそのまま転送していたため、`check_counter` は +crane が採番したものだった。新構成では CM4 が自分の制御周期で新しい指令を +生成するので、**CM4 が `check_counter` を自分で採番する** 必要がある。 + +その結果、G474 の `connected_ai` は **crane の生存を意味しなくなる**。 +crane からのパケットが途絶えた場合の安全停止は、CM4 側で明示的に実装すること +（例: crane 無通信が一定時間続いたら `STOP_EMERGENCY` を立てる、または +速度指令をゼロにする）。これは実機・シミュレータ双方に共通の要件である。 + +### cm4_sim のペーシング + +`cm4_sim` は実機の `forward_ai_cmd_v2.cpp` と同じ構造にする。 + +- `--rate-hz`（既定 1 kHz）の固定周期で回し、最後に受信した feedback を使う。 +- **feedback 待ちでブロックしてはならない。** ソケットはノンブロッキングにし、 + 届いていなければ前回値で制御する（実機の `usleep(1000)` ポーリングループと同じ）。 +- 11 台分を **1 プロセス・1 データグラムに集約** して送る。台数分のプロセスを + 立てると、同一ポートへの送信が増えるだけで実機の構成に近づかない。 +- crane の指令は約 60 Hz でしか来ないので、CM4 制御ループはその間 + 同じ目標値に対して feedback だけを更新しながら回ることになる。これは実機と同じ。 + +## framework 側の実装状況（本リポジトリ・実装済み） + +- `src/simulator/ibis_protocol.h` + - `IBIS_MODE_POLAR_VELOCITY_TARGET` (3) / `IBIS_MODE_POSITION_TARGET` (4) を定義。 + - `ibisDeserialize()` が `CONTROL_MODE` を見て ARGS を復号するようになった。 + 従来は mode を無視して常に polar velocity として読んでいた。 + - `target_global_pos` / `terminal_velocity` / `vision_global_theta` / + `is_vision_available` を復号対象に追加。 + - `ibisSlotIsEmpty()` を追加。 +- `src/simulator/simulator.cpp` `IbisCommandAdaptor` + - 空スロットを明示的にスキップ。 + - **mode 3 以外を受け取ったらロボットを停止し、1 秒/台 のレート制限付きで警告** + を出す。mode 4 が届くのは「cm4_sim が経路に入っていない」設定ミスであり、 + 無言で誤解釈するより停止して理由を出すほうが安全かつデバッグしやすい。 + - **`vision_global_pos` の照合に失敗して指令を破棄したときも警告を出す** + （1 秒/台）。最も近い候補までの距離を併記する。 + 「指令の位置照合」節を参照。 + +simulator-cli 側に位置制御は **実装しない**。実装すると制御則のコピーが +4 つ目になり、`robot_packet.h` が 3 リポジトリで食い違った問題を繰り返す。 + +## 検証（A/B 比較） + +この設計の主張は「無線経路をループ外に出すと、遅延・ジッタ・ロスに強くなる」 +である。それを示すには、同じ劣化条件下で旧構成と新構成を比較する必要がある。 + +比較を成立させるには、**劣化注入点が両構成で同一**でなければならない。 +旧構成を「crane → simulator-cli 直送」にすると経路上に注入器が無く、比較にならない。 + +そこで `cm4_sim` は **受信した mode で振る舞いを切り替える**: + +| 受信 mode | cm4_sim の動作 | +|---|---| +| 3（polar velocity） | 位置制御をせずそのまま転送（passthrough）。実機の CM4 の現行動作と同じ | +| 4（position target） | 位置制御ループを回して mode 3 を生成 | + +劣化注入は mode によらず**入力側で常に適用**する。これで両構成は次のようになる。 + +| | 旧構成 | 新構成 | +|---|---|---| +| crane | 位置ループを閉じ mode 3 を送る | mode 4 を送る | +| 経路 | crane → cm4_sim(passthrough) → simulator-cli | crane → cm4_sim(位置制御) → simulator-cli | +| 劣化注入 | cm4_sim 入力側 | cm4_sim 入力側（同一コード経路） | +| 位置ループの位置 | crane（無線がループ内） | CM4（無線がループ外） | + +**独立変数は「位置ループをどこで閉じるか」だけ**になり、転送経路・注入点・注入実装が +両者で完全に一致する。旧構成が厳密な「直送」でなくなるが、localhost の UDP 1 ホップ +追加は無線劣化に比べて無視できる。mode による分岐は実機の CM4 にも必要な後方互換 +経路なので、シミュレータ専用の仕掛けを増やすことにもならない。 + +`cm4_sim` の入力側に `--rx-delay-ms` / `--rx-jitter-ms` / `--rx-loss-rate` と +再現用のシードを実装し、両構成を同一条件で走らせて追従誤差・オーバーシュート・ +到達時間を比較する。 + +### simulator-cli 側のスモークテスト + +`data/scripts/ibis-chain-smoketest.py` が、実際に `simulator-cli` を起動して +UDP 越しに以下を検証する。 + +```bash +python3 data/scripts/ibis-chain-smoketest.py [path/to/simulator-cli] +``` + +1. mode 3 でロボットが動く +2. mode 4 でロボットが停止し、レート制限付き警告が出る +3. 位置が一致している間は破棄警告が出ない（偽陽性が無い） +4. `vision_global_pos` を 1.0 m ずらした指令は破棄され、ロボットが動かない +5. その破棄がレート制限付きで警告として出る + +`cm4_sim` を実装する前に simulator-cli 側の契約を固定するためのもの。 +既定ポートとは離れたポート（12397 / 50700 / 10097 / 11097）を使うので、 +動作中の試合には影響しない。 + +## 既知の忠実度ギャップ + +- **feedback の位置が真値である。** シミュレータの feedback は ground truth を + そのまま返すため、ノイズも vision 遅延も無い。実機の G474 は + `vision_based_position` としてタイヤオドメトリと vision を融合した推定値を返す。 + シミュレータ上の位置制御は実機より良く見える。必要ならノイズ注入を追加する。 +- **停止は2経路あり、片方は惰走。実機と構造が一致している。** 実機 G474 は + `sys->stop_flag || stop_emergency || !is_vision_available || + elapsed_time_ms_since_last_vision > 500` で `omniStopAll()` に入り + （`state_func.c:314`）、4輪の電圧を 0 にするだけ（`omni_wheel.c:62`）。CAN フレームは + duty の float 4 バイトのみでブレーキビットを持たない（`actuator.c:12`）。つまり + **実機のこの分岐は制動ではなく惰走**。一方 mode 3 で `r = 0` を送った場合は + `omniMoveIndiv` に入り車輪 PID が効く。シミュレータもこの2経路を再現している。 + + | 停止のかけ方 | 経路 | 1.45 m/s からの停止距離 | + |---|---|---| + | mode 3 で `r = 0` | 車輪 PID（能動制動） | 0.13 m | + | `STOP_EMERGENCY` / vision 断 | `move_command` を出さない（惰走） | 0.63 m | + | 指令が届かない | 0.1 s は前の指令のまま → standby（惰走） | 0.76 m | + + **測定は必ず +y 方向で、blue と yellow の両方で取ること。** ロボットの開始位置は + y = -2.8 付近で、-y は壁、±x は他のロボットに塞がれている。塞がれた向きで測ると + 停止距離が 0.12〜0.40 m の範囲で不規則にばらつき、しかも「それらしい値」に見える。 + 両チームで測って一致すれば自由惰走、食い違えば何かに当たっている。測定は + `data/scripts/ibis-stop-distance.py` にしてあり、両チームの一致も検査する。 + + **指令途絶が `STOP_EMERGENCY` より 0.13 m 長いのは正しい。** `SimRobot` は最後の + 指令から 0.1 s 経つまで前の指令を実行し続けるので、その間は駆動力がかかっている。 + `STOP_EMERGENCY` は指令が届いた時点で即座に駆動を切るので、その分だけ短い。 + **ただしこの 0.1 s は実機の 250 ms（`connected_ai` タイムアウト）より短い**ので、 + 指令途絶時の惰走距離はシミュレータの方が実機より短く出る。 + + 残る不確定は1段下で、**duty 0 が空転なのか短絡制動なのかはモータボード側の + ファームウェアが決める**。これは `G474_Orion_main` にも CM4 側にも無いため、 + 惰走距離の絶対値は実機で測るしかない。上の 0.63 m は比較対象であって実機の + 予測値ではない。なお mode 4 が届いた場合の停止だけは能動制動のまま残してある。 + これは実機に対応物が無い「構成ミス」の合図なので、惰走させると発見が遅れるため。 +- **feedback の速度は指令が届いたときしか更新されない。** 速度（byte 52/56）は + `RadioResponse` 由来のキャッシュで、`RadioResponse` は指令が届いたときにしか + 生成されない。一方で位置（byte 44/48）は毎周期 vision から詰め直される。このため + 指令が破棄されている間、**同じパケットの中で位置は新鮮なのに速度は破棄直前の値で + 凍る**。実測ではロボットが約 1.0 s で停止した後も 1.46 m/s を返し続けた。 + 「速度が出ているのに位置が動かない」feedback を見たら、ロボットの挙動ではなく + 指令が届いているかをまず疑うこと。`cm4_sim` は位置だけを使ってループを閉じるので + 現状の制御には影響しないが、速度を見る診断は騙される。なお `STOP_EMERGENCY` に + よる停止では指令自体は届き続けるので、速度は正しく 0 まで減衰する。 +- **ボールセンサ・キック状態。** `kick_status` は常に 0。 +- **ローカルカメラ。** `cam_server_v3` 相当は無い。`cm4_sim` はカメラ領域を + ゼロ埋めすること（実機のカメラ未接続時と同じ扱い）。 +- **バッテリ電圧・温度等。** 固定値。 + +## 関連ファイル + +- `src/simulator/ibis_protocol.h` — プロトコル定義（framework 側） +- `data/scripts/ibis-chain-smoketest.py` — ibis コマンド経路のスモークテスト +- `data/scripts/ibis-stop-distance.py` — 停止 3 経路の停止距離測定（両チーム一致で検査） +- `src/simulator/simulator.cpp` — `IbisCommandAdaptor` / `IbisFeedbackAdaptor` +- `src/amun/simulator/simulator.cpp` — `handleSimulatorTick()`、feedback 送出 +- `Orion_CM4/cm4/bridge/robot_packet.h` — CM4 側パケット定義（要統一） +- `Orion_CM4/doc/control_packet.md` — CM4 側の制御パケット仕様 +- `G474_Orion_main/Core/Src/ai_comm.c` — `check_counter` による接続監視 +- `crane/crane_sender/src/ibis_sender_node.cpp` — mode 4 送信 +- `crane/crane_local_planner/src/visibility_graph_planner.cpp` — 位置目標の生成 diff --git a/src/amun/simulator/include/simulator/simulator.h b/src/amun/simulator/include/simulator/simulator.h index e41c8040ea..4b76d0977a 100644 --- a/src/amun/simulator/include/simulator/simulator.h +++ b/src/amun/simulator/include/simulator/simulator.h @@ -35,7 +35,7 @@ // higher values break the rolling friction of the ball const float SIMULATOR_SCALE = 10.0f; -const float SUB_TIMESTEP = 1/200.f; +const float SUB_TIMESTEP = 1/250.f; const float COLLISION_MARGIN = 0.04f; const unsigned FOCAL_LENGTH = 390; @@ -78,6 +78,7 @@ class camun::simulator::Simulator : public QObject void sendStatus(const Status &status); void sendRadioResponses(const QList &responses); void sendRealData(const QByteArray& data); // sends amun::SimulatorState + void sendGroundTruth(const QByteArray& data); // sends world::SimulatorState at 125Hz, no noise void sendSSLSimError(const QList& errors, ErrorSource source); public slots: @@ -117,7 +118,7 @@ private slots: const Timer *m_timer; QTimer *m_trigger; qint64 m_time; - qint64 m_lastSentStatusTime; + unsigned int m_simulationFrameCounter; double m_timeScaling; bool m_enabled; bool m_charge; diff --git a/src/amun/simulator/simrobot.cpp b/src/amun/simulator/simrobot.cpp index 198ebf0bc1..5cfe92a05a 100644 --- a/src/amun/simulator/simrobot.cpp +++ b/src/amun/simulator/simrobot.cpp @@ -204,6 +204,7 @@ void SimRobot::setDribbleMode(bool perfectDribbler) stopDribbling(); } m_perfectDribbler = perfectDribbler; + m_dribblerReleaseCooldown = 0.0; } bool SimRobot::handleMoveCommand() @@ -336,10 +337,25 @@ void SimRobot::begin(SimBall *ball, double time) m_inStandby = true; } + if (m_dribblerReleaseCooldown > 0.0) { + m_dribblerReleaseCooldown = std::max(0.0, m_dribblerReleaseCooldown - time); + } + + // detect excessive constraint force and release the ball (grSim: checkDribbleFeedback, threshold 0.5 N) + if (m_holdBallConstraint) { + const float impulse = m_holdBallConstraint->getAppliedImpulse(); + const float force = impulse / SUB_TIMESTEP; + if (force > 0.5f) { + stopDribbling(); + m_dribblerReleaseCooldown = 0.1; + } + } + // enable dribbler if necessary - if (!m_inStandby && m_sslCommand.has_dribbler_speed() && m_sslCommand.dribbler_speed() > 0) { + if (!m_inStandby && m_sslCommand.has_dribbler_speed() && m_sslCommand.dribbler_speed() > 0 + && m_dribblerReleaseCooldown <= 0.0) { dribble(ball, m_sslCommand.dribbler_speed()); - } else { + } else if (m_dribblerReleaseCooldown <= 0.0) { stopDribbling(); } diff --git a/src/amun/simulator/simrobot.h b/src/amun/simulator/simrobot.h index 588a2fd77c..e9004325e2 100644 --- a/src/amun/simulator/simrobot.h +++ b/src/amun/simulator/simrobot.h @@ -113,6 +113,7 @@ class camun::simulator::SimRobot: public QObject float error_sum_omega; bool m_perfectDribbler = false; + double m_dribblerReleaseCooldown = 0.0; float m_rotationError = 0.0f; qint64 m_lastSendTime = 0; diff --git a/src/amun/simulator/simulator.cpp b/src/amun/simulator/simulator.cpp index e8f5132aec..6cd47fd6aa 100644 --- a/src/amun/simulator/simulator.cpp +++ b/src/amun/simulator/simulator.cpp @@ -118,7 +118,7 @@ Simulator::Simulator(const Timer *timer, const amun::SimulatorSetup &setup, bool m_isPartial(useManualTrigger), m_timer(timer), m_time(0), - m_lastSentStatusTime(0), + m_simulationFrameCounter(0), m_timeScaling(1.), m_enabled(false), m_charge(false), @@ -126,7 +126,7 @@ Simulator::Simulator(const Timer *timer, const amun::SimulatorSetup &setup, bool m_visionProcessingTime(5 * 1000 * 1000), m_aggregator(new ErrorAggregator(this)) { - // triggers by default every 5 milliseconds if simulator is enabled + // triggers by default every 8 milliseconds if simulator is enabled // timing may change if time is scaled m_trigger = new QTimer(this); m_trigger->setTimerType(Qt::PreciseTimer); @@ -274,9 +274,24 @@ void Simulator::process() m_data->dynamicsWorld->stepSimulation(timeDelta, 10, SUB_TIMESTEP); m_time = current_time; - // only send a vision packet every third frame = 15 ms - epsilon (=half frame) - // gives a vision frequency of 66.67Hz - if (m_lastSentStatusTime + 12500000 <= m_time) { + // Emit ground truth robot positions at full simulation rate (125Hz), without noise + { + world::SimulatorState gt; + gt.set_time(m_time); + for (auto it = m_data->robotsBlue.cbegin(); it != m_data->robotsBlue.cend(); ++it) { + it.value().first->update(gt.add_blue_robots(), m_data->ball); + } + for (auto it = m_data->robotsYellow.cbegin(); it != m_data->robotsYellow.cend(); ++it) { + it.value().first->update(gt.add_yellow_robots(), m_data->ball); + } + QByteArray gtData(static_cast(gt.ByteSizeLong()), 0); + gt.SerializeToArray(gtData.data(), gtData.size()); + emit sendGroundTruth(gtData); + } + + // send a vision packet every second simulation frame + // with the 125 Hz base loop this results in an effective vision frequency of 62.5 Hz + if ((m_simulationFrameCounter++ % 2) == 0) { auto data = createVisionPacket(); @@ -297,8 +312,6 @@ void Simulator::process() timer->start(timeout); m_visionTimers.enqueue(timer); } - - m_lastSentStatusTime = m_time; } // send timing information @@ -785,6 +798,7 @@ void Simulator::handleCommand(const Command &command) if (sim.has_enable()) { m_enabled = sim.enable(); m_time = m_timer->currentTime(); + m_simulationFrameCounter = 0; // update timer when simulator status is changed setScaling(m_timeScaling); } @@ -950,8 +964,8 @@ void Simulator::setScaling(double scaling) // clear pending vision packets resetVisionPackets(); } else { - // scale default timing of 5 milliseconds - const int t = 5 / scaling; + // scale default timing of 8 milliseconds (125Hz) + const int t = 8 / scaling; m_trigger->start(qMax(1, t)); // The vision packet timings are wrong after a scaling change diff --git a/src/simulator/CMakeLists.txt b/src/simulator/CMakeLists.txt index d1b44a175d..5cd05e1810 100644 --- a/src/simulator/CMakeLists.txt +++ b/src/simulator/CMakeLists.txt @@ -20,6 +20,7 @@ add_executable(simulator-cli WIN32 MACOSX_BUNDLE simulator.cpp ssl_robocup_server.cpp + packet_sender_thread.cpp ) target_link_libraries(simulator-cli @@ -29,3 +30,5 @@ target_link_libraries(simulator-cli Qt6::Widgets amun::simulator ) + +install(TARGETS simulator-cli RUNTIME DESTINATION bin) diff --git a/src/simulator/ibis_protocol.h b/src/simulator/ibis_protocol.h new file mode 100644 index 0000000000..710a18a29d --- /dev/null +++ b/src/simulator/ibis_protocol.h @@ -0,0 +1,319 @@ +/* + * ibis-ssl binary protocol constants, data types, and packet functions. + * + * Ported from ibis-ssl/grSim fork (ibis_command_receiver.cpp / binary_feedback_sender.cpp). + * No grSim-specific dependencies -- uses only standard C++ and stdint. + */ + +#pragma once + +#include +#include +#include + +// --------------------------------------------------------------------------- +// Protocol constants +// --------------------------------------------------------------------------- + +constexpr int IBIS_ROBOT_SLOTS = 11; +constexpr int IBIS_CMD_SIZE = 64; +constexpr int IBIS_SLOT_SIZE = IBIS_CMD_SIZE + 1; +constexpr int IBIS_PACKET_SIZE = IBIS_SLOT_SIZE * IBIS_ROBOT_SLOTS; // 715 + +constexpr int IBIS_DEFAULT_PORT = 12345; +constexpr double IBIS_CHIP_ANGLE_DEG = 30.0; +constexpr double IBIS_CHIP_ANGLE_RAD = IBIS_CHIP_ANGLE_DEG * M_PI / 180.0; +constexpr double IBIS_MAX_KICK_SPEED = 8.0; // m/s +constexpr double IBIS_THETA_P_GAIN = 4.0; +constexpr double IBIS_DT = 1.0 / 30.0; +constexpr int IBIS_FEEDBACK_SIZE = 128; +constexpr int IBIS_FEEDBACK_PORT_BASE = 50100; +constexpr double IBIS_POSITION_MATCH_THRESHOLD = 0.5; // metres + +// The real STM32 main board stops the wheels when vision has been lost for +// longer than this (G474_Orion_main/Core/Src/state_func.c). +constexpr uint16_t IBIS_VISION_LOST_TIMEOUT_MS = 500; + +// Control modes. Must match crane_sender/include/crane_sender/robot_packet.h +// and Orion_CM4/cm4/bridge/robot_packet.h (ControlMode enum). +// +// The simulator plays the role of the robot's STM32 (G474) main board, which +// only ever implements POLAR_VELOCITY_TARGET. POSITION_TARGET is closed on the +// robot's CM4 (cm4_sim in simulation), never here -- see +// docs/robot-side-position-control.md. +constexpr uint8_t IBIS_MODE_POLAR_VELOCITY_TARGET = 3; +constexpr uint8_t IBIS_MODE_POSITION_TARGET = 4; + +// --------------------------------------------------------------------------- +// Byte offsets in the 64-byte RobotCommandSerializedV2 (from crane's robot_packet.h) +// --------------------------------------------------------------------------- + +enum IbisAddress { + HEADER = 0, + CHECK_COUNTER = 1, + VISION_GLOBAL_X_H = 2, + VISION_GLOBAL_X_L = 3, + VISION_GLOBAL_Y_H = 4, + VISION_GLOBAL_Y_L = 5, + VISION_GLOBAL_TH_H = 6, + VISION_GLOBAL_TH_L = 7, + TARGET_GLOBAL_TH_H = 8, + TARGET_GLOBAL_TH_L = 9, + KICK_POWER = 10, + DRIBBLE_POWER = 11, + ACCEL_LIMIT_H = 12, + ACCEL_LIMIT_L = 13, + LINEAR_VEL_LIMIT_H = 14, + LINEAR_VEL_LIMIT_L = 15, + ANGULAR_VEL_LIMIT_H = 16, + ANGULAR_VEL_LIMIT_L = 17, + LATENCY_MS_H = 18, + LATENCY_MS_L = 19, + ELAPSED_VISION_H = 20, + ELAPSED_VISION_L = 21, + FLAGS = 22, + CONTROL_MODE = 23, + CONTROL_MODE_ARGS = 24, + // CONTROL_MODE_ARGS size = 8, args end at offset 31 + TARGET_POS_X_H = 32, + TARGET_POS_X_L = 33, + TARGET_POS_Y_H = 34, + TARGET_POS_Y_L = 35, + TERMINAL_VEL_H = 36, + TERMINAL_VEL_L = 37, +}; + +enum IbisFlagBit { + IS_VISION_AVAILABLE = 0, + ENABLE_CHIP = 1, + STOP_EMERGENCY = 3, +}; + +// --------------------------------------------------------------------------- +// Deserialized ibis command +// --------------------------------------------------------------------------- + +struct IbisCommand { + uint8_t control_mode; // IBIS_MODE_* + float vision_global_pos[2]; // metres, SSL vision coordinate system + float vision_global_theta; // radians + bool is_vision_available; + float target_global_theta; // radians + float kick_power; // 0..1 normalised + float dribble_power; // 0..1 normalised + bool enable_chip; + bool stop_emergency; + float acceleration_limit; // m/s^2 (0 means "use default") + float linear_velocity_limit; // m/s (0 means "no limit") + float angular_velocity_limit; // rad/s + uint16_t latency_time_ms; + uint16_t elapsed_time_ms_since_last_vision; + float polar_velocity_r; // m/s (mode 3 args) + float polar_velocity_theta; // radians (global direction, mode 3 args) + float terminal_velocity_xy[2]; // m/s (mode 4 args) + float target_global_pos[2]; // metres (fixed field, modes >= 4) + float terminal_velocity; // m/s (fixed field, modes >= 4) + uint8_t check_counter; +}; + +// Cached SSL vision state for one robot (position in mm, orientation in rad). +struct IbisVisionState { + float x_mm = 0.0f; + float y_mm = 0.0f; + float orientation_rad = 0.0f; + bool valid = false; +}; + +// Pure deserialization (same logic as grSim ibis_command_receiver.cpp) + +inline float ibisDecodeTwoByte(uint8_t high, uint8_t low, float range) +{ + uint16_t two_byte = (static_cast(high) << 8) | low; + return static_cast(two_byte - 32767.f) / 32767.f * range; +} + +inline IbisCommand ibisDeserialize(const uint8_t* d) +{ + IbisCommand cmd; + cmd.check_counter = d[CHECK_COUNTER]; + cmd.control_mode = d[CONTROL_MODE]; + cmd.vision_global_pos[0] = ibisDecodeTwoByte(d[VISION_GLOBAL_X_H], d[VISION_GLOBAL_X_L], 32.767f); + cmd.vision_global_pos[1] = ibisDecodeTwoByte(d[VISION_GLOBAL_Y_H], d[VISION_GLOBAL_Y_L], 32.767f); + cmd.vision_global_theta = ibisDecodeTwoByte(d[VISION_GLOBAL_TH_H], d[VISION_GLOBAL_TH_L], static_cast(M_PI)); + cmd.target_global_theta = ibisDecodeTwoByte(d[TARGET_GLOBAL_TH_H], d[TARGET_GLOBAL_TH_L], static_cast(M_PI)); + cmd.kick_power = d[KICK_POWER] / 20.f; + cmd.dribble_power = d[DRIBBLE_POWER] / 20.f; + cmd.acceleration_limit = ibisDecodeTwoByte(d[ACCEL_LIMIT_H], d[ACCEL_LIMIT_L], 32.767f); + cmd.linear_velocity_limit = ibisDecodeTwoByte(d[LINEAR_VEL_LIMIT_H], d[LINEAR_VEL_LIMIT_L], 32.767f); + cmd.angular_velocity_limit = ibisDecodeTwoByte(d[ANGULAR_VEL_LIMIT_H], d[ANGULAR_VEL_LIMIT_L], 32.767f); + + // These two are plain uint16 (high, low), not the +/-range float encoding. + cmd.latency_time_ms = static_cast((d[LATENCY_MS_H] << 8) | d[LATENCY_MS_L]); + cmd.elapsed_time_ms_since_last_vision = + static_cast((d[ELAPSED_VISION_H] << 8) | d[ELAPSED_VISION_L]); + + uint8_t flags = d[FLAGS]; + cmd.is_vision_available = (flags >> IS_VISION_AVAILABLE) & 0x01; + cmd.enable_chip = (flags >> ENABLE_CHIP) & 0x01; + cmd.stop_emergency = (flags >> STOP_EMERGENCY) & 0x01; + + // CONTROL_MODE_ARGS (offset 24..31) is a union: its meaning depends on + // control_mode. Decoding it unconditionally as polar velocity would read + // mode 4's terminal_velocity_x/y as r/theta, which is silent garbage. + cmd.polar_velocity_r = 0.0f; + cmd.polar_velocity_theta = 0.0f; + cmd.terminal_velocity_xy[0] = 0.0f; + cmd.terminal_velocity_xy[1] = 0.0f; + switch (cmd.control_mode) { + case IBIS_MODE_POLAR_VELOCITY_TARGET: + cmd.polar_velocity_r = ibisDecodeTwoByte(d[CONTROL_MODE_ARGS + 0], d[CONTROL_MODE_ARGS + 1], 32.767f); + cmd.polar_velocity_theta = ibisDecodeTwoByte(d[CONTROL_MODE_ARGS + 2], d[CONTROL_MODE_ARGS + 3], 32.767f); + break; + case IBIS_MODE_POSITION_TARGET: + cmd.terminal_velocity_xy[0] = ibisDecodeTwoByte(d[CONTROL_MODE_ARGS + 0], d[CONTROL_MODE_ARGS + 1], 32.767f); + cmd.terminal_velocity_xy[1] = ibisDecodeTwoByte(d[CONTROL_MODE_ARGS + 2], d[CONTROL_MODE_ARGS + 3], 32.767f); + break; + default: + break; + } + + // Fixed fields, present regardless of mode (offsets 32..37). + cmd.target_global_pos[0] = ibisDecodeTwoByte(d[TARGET_POS_X_H], d[TARGET_POS_X_L], 32.767f); + cmd.target_global_pos[1] = ibisDecodeTwoByte(d[TARGET_POS_Y_H], d[TARGET_POS_Y_L], 32.767f); + cmd.terminal_velocity = ibisDecodeTwoByte(d[TERMINAL_VEL_H], d[TERMINAL_VEL_L], 32.767f); + + return cmd; +} + +// Mirrors the wheel-stop condition of the real STM32 main board +// (G474_Orion_main/Core/Src/state_func.c): it halts the wheels on emergency +// stop, on vision being unavailable, and on vision having gone stale. The +// simulator emulates that board, so it must stop for the same reasons -- +// otherwise the robot keeps driving in simulation under conditions that would +// park it on real hardware, which matters most under injected packet loss. +inline bool ibisShouldStop(const IbisCommand& cmd) +{ + return cmd.stop_emergency + || !cmd.is_vision_available + || cmd.elapsed_time_ms_since_last_vision > IBIS_VISION_LOST_TIMEOUT_MS; +} + +// True when a robot slot carries no command at all. Senders zero-fill the slots +// of robots they do not control; a zero-filled slot decodes to a position of +// (-32.767, -32.767) which no team match would accept, but relying on that is +// accidental -- check explicitly instead. +inline bool ibisSlotIsEmpty(const uint8_t* d) +{ + for (int i = 0; i < IBIS_CMD_SIZE; ++i) { + if (d[i] != 0) { return false; } + } + return true; +} + +// --------------------------------------------------------------------------- +// 128-byte feedback packet builder +// Ported from grSim BinaryFeedbackSender::buildPacket, using scalar args +// instead of Robot* so it has no grSim/ODE dependency. +// +// Parameters: +// buffer - 128-byte output buffer (must be pre-allocated) +// counter - AI command check counter echo (caller supplies) +// tx_cycle - rolling transmit counter (caller increments) +// yaw_rad - orientation in RADIANS; written to the wire in DEGREES +// ball_detected - is ball in contact with dribbler +// kick_status - 0=none, 1=flat, 2=chip +// odom_x_m - position x in metres (SSL vision coords) +// odom_y_m - position y in metres (SSL vision coords) +// vel_x_ms - global velocity x in m/s (SSL vision coords) +// vel_y_ms - global velocity y in m/s (SSL vision coords) +// +// The layout follows the real robot's STM32 main board byte for byte +// (G474_Orion_main/Core/Src/ai_comm.c sendRobotInfo()), so a consumer cannot +// tell the simulator from real hardware by parsing. Fields the simulator has +// no source for are left at the value real hardware sends when that sensor is +// absent (zero), rather than repurposed -- a simulator-only marker byte would +// decode as a real field on the consumer side. +// --------------------------------------------------------------------------- + +inline void ibisBuildFeedbackPacket( + uint8_t* buffer, + uint8_t counter, + uint8_t tx_cycle, + float yaw_rad, + bool ball_detected, + uint8_t kick_status, + float odom_x_m, + float odom_y_m, + float vel_x_ms, + float vel_y_ms) +{ + std::memset(buffer, 0, IBIS_FEEDBACK_SIZE); + + // Header (0-1) + buffer[0] = 0xAB; + buffer[1] = 0xEA; + + // Checksum placeholder (2). Real hardware writes the constant 10 here + // ("CRC, 10:dummy" in ai_comm.c); it never computes a real checksum, so + // host-side checksum validation fails on real packets too. Match that + // rather than inventing a value -- the robot id is implied by the port. + buffer[2] = 10; + + // AI command check counter echo (3) + buffer[3] = counter; + + // Yaw angle (4-7). The wire format is DEGREES: real hardware sends + // imu->yaw_deg here. Sending radians makes consumers that use this field + // (e.g. crane_latency_estimator) behave differently in simulation than on + // the robot, by a factor of 180/pi. + const float yaw_deg = yaw_rad * static_cast(180.0 / M_PI); + std::memcpy(&buffer[4], &yaw_deg, sizeof(float)); + + // Battery voltage: fixed 24.0 V (8-11) + float voltage = 24.0f; + std::memcpy(&buffer[8], &voltage, sizeof(float)); + + // Ball detection sensors 0-1 (12-13), then the transmit cycle counter (14). + // Byte 14 is NOT a third ball sensor: real hardware puts tx_cycle_count + // there (ai_comm.c). Consumers only key the ball sensor off byte 12. + buffer[12] = ball_detected ? 1 : 0; + buffer[13] = ball_detected ? 1 : 0; + buffer[14] = tx_cycle; + + // Kick status (15): 0=none, 1=flat, 2=chip + buffer[15] = kick_status; + + // Error info (16-23): 0 (no errors) + // Motor current (24-27): 0 + // Ball detection 3 (28): 0 + // Already zeroed by memset. + + // Temperature: fixed 25 C (29-35) + for (int i = 29; i <= 35; i++) { + buffer[i] = 25; + } + + // Angle diff (36-39): 0 + float angle_diff = 0.0f; + std::memcpy(&buffer[36], &angle_diff, sizeof(float)); + + // Capacitor voltage: fixed 200.0 V (40-43) + float cap_voltage = 200.0f; + std::memcpy(&buffer[40], &cap_voltage, sizeof(float)); + + // Odometry position (44-51) + std::memcpy(&buffer[44], &odom_x_m, sizeof(float)); + std::memcpy(&buffer[48], &odom_y_m, sizeof(float)); + + // Global velocity (52-59) + std::memcpy(&buffer[52], &vel_x_ms, sizeof(float)); + std::memcpy(&buffer[56], &vel_y_ms, sizeof(float)); + + // Local camera block (60-63): camera_pos_x_div2 / camera_pos_y / + // camera_radius_div4 / camera_fps. The simulator has no local camera, so + // these stay 0 -- exactly what real hardware sends with no camera attached. + // (This byte previously carried a 0x01 "simulator" marker, which consumers + // decoded as camera_pos_x = 2.) + + // Extended data (64-127): tx_value_array on real hardware, 0 here. +} diff --git a/src/simulator/packet_sender_thread.cpp b/src/simulator/packet_sender_thread.cpp new file mode 100644 index 0000000000..015865c382 --- /dev/null +++ b/src/simulator/packet_sender_thread.cpp @@ -0,0 +1,57 @@ +#include "packet_sender_thread.h" + +#include +#include +#include + +PacketSenderThread::PacketSenderThread(QObject* parent) + : QThread(parent) +{ + start(); +} + +PacketSenderThread::~PacketSenderThread() +{ + stop(); + wait(); +} + +void PacketSenderThread::enqueue(QByteArray data, const QHostAddress& addr, quint16 port) +{ + QMutexLocker locker(&mutex_); + queue_.enqueue({std::move(data), addr, port}); + cond_.wakeOne(); +} + +void PacketSenderThread::stop() +{ + QMutexLocker locker(&mutex_); + running_ = false; + cond_.wakeOne(); +} + +void PacketSenderThread::run() +{ + QUdpSocket socket; + socket.setSocketOption(QAbstractSocket::MulticastTtlOption, QVariant(1)); + + QList batch; + while (true) { + batch.clear(); + { + QMutexLocker locker(&mutex_); + while (queue_.isEmpty() && running_) { + cond_.wait(&mutex_); + } + if (!running_ && queue_.isEmpty()) { + break; + } + while (!queue_.isEmpty()) { + batch.append(queue_.dequeue()); + } + } + for (const Packet& pkt : batch) { + socket.writeDatagram(pkt.data, pkt.addr, pkt.port); + } + } +} diff --git a/src/simulator/packet_sender_thread.h b/src/simulator/packet_sender_thread.h new file mode 100644 index 0000000000..5786efb64f --- /dev/null +++ b/src/simulator/packet_sender_thread.h @@ -0,0 +1,36 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +// Sends UDP packets asynchronously from a dedicated worker thread. +// The producer (main/rcv thread) calls enqueue(); the worker thread +// batches and sends them so that UDP send latency does not block callers. +class PacketSenderThread : public QThread { + Q_OBJECT +public: + struct Packet { + QByteArray data; + QHostAddress addr; + quint16 port; + }; + + explicit PacketSenderThread(QObject* parent = nullptr); + ~PacketSenderThread() override; + + void enqueue(QByteArray data, const QHostAddress& addr, quint16 port); + void stop(); + +protected: + void run() override; + +private: + QQueue queue_; + QMutex mutex_; + QWaitCondition cond_; + bool running_ = true; +}; diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp index 004b9e84f0..f1fa6f3ae8 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -46,6 +47,12 @@ #include "core/sslprotocols.h" #include "ssl_robocup_server.h" +#include "ibis_protocol.h" +#include "packet_sender_thread.h" + +#include "protobuf/ssl_vision/ssl_wrapper.pb.h" +#include "protobuf/ssl_gc/state/ssl_gc_referee_message.pb.h" +#include "protobuf/world.pb.h" /** * Stand alone Erforce simulator @@ -592,6 +599,7 @@ class SimProxy: public QObject { void sendSSLSimError(const QList& errors, ErrorSource source); // out void sendRadioResponses(const QList &responses); // out void gotPacket(const QByteArray &data, qint64 time, QString sender); // out + void sendGroundTruth(const QByteArray& data); // out - world::SimulatorState at 125Hz void gotCommand(const Command &command); // internal void handleRadioCommands(const SSLSimRobotControl& control, bool isBlue, qint64 processingStart); // in public slots: @@ -637,6 +645,7 @@ void SimProxy::handleCommand(const Command &command) { connect(this, &SimProxy::handleRadioCommands, m_sim, &Simulator::handleRadioCommands); connect(m_sim, &Simulator::sendSSLSimError, this, &SimProxy::sendSSLSimError); connect(m_sim, &Simulator::sendRadioResponses, this, &SimProxy::sendRadioResponses); + connect(m_sim, &Simulator::sendGroundTruth, this, &SimProxy::sendGroundTruth); auto* simCommand = m_teamCommand->mutable_simulator(); simCommand->set_enable(true); auto* trCommand = m_teamCommand->mutable_transceiver(); @@ -646,12 +655,517 @@ void SimProxy::handleCommand(const Command &command) { emit gotCommand(command); } +class IbisCommandAdaptor : public QObject { + Q_OBJECT +public: + IbisCommandAdaptor(int port, Timer* timer, double accSpeedup, double accBrake) + : m_server(this) + , m_timer(timer) + , m_accSpeedup(accSpeedup) + , m_accBrake(accBrake) + { + m_server.bind(QHostAddress::Any, static_cast(port)); + connect(&m_server, &QUdpSocket::readyRead, this, &IbisCommandAdaptor::handleDatagrams); + } + +public slots: + void handleVisionData(const QByteArray& data, qint64, QString) { + SSL_WrapperPacket pkt; + if (!pkt.ParseFromArray(data.data(), data.size()) || !pkt.has_detection()) { + return; + } + const auto& det = pkt.detection(); + for (const auto& r : det.robots_blue()) { + if (r.has_robot_id() && r.has_orientation()) { + const uint32_t id = r.robot_id(); + if (id < kMaxRobots) { + m_vision[0][id] = {r.x(), r.y(), r.orientation(), true}; + } + } + } + for (const auto& r : det.robots_yellow()) { + if (r.has_robot_id() && r.has_orientation()) { + const uint32_t id = r.robot_id(); + if (id < kMaxRobots) { + m_vision[1][id] = {r.x(), r.y(), r.orientation(), true}; + } + } + } + } + +signals: + void sendRadioCommands(const SSLSimRobotControl& commands, bool isBlue, qint64 processingDelay); + +private slots: + void handleDatagrams() { + while (m_server.hasPendingDatagrams()) { + const qint64 start = m_timer->currentTime(); + auto datagram = m_server.receiveDatagram(); + const auto& data = datagram.data(); + + if (data.size() != IBIS_PACKET_SIZE) { + continue; + } + + const uint8_t* buf = reinterpret_cast(data.constData()); + + SSLSimRobotControl blueControl{new sslsim::RobotControl}; + SSLSimRobotControl yellowControl{new sslsim::RobotControl}; + bool hasBlue = false, hasYellow = false; + + for (int slot = 0; slot < IBIS_ROBOT_SLOTS; ++slot) { + const int offset = slot * IBIS_SLOT_SIZE; + const uint8_t robot_id = buf[offset]; + if (robot_id >= IBIS_ROBOT_SLOTS) { + continue; + } + + const uint8_t* cmd_data = buf + offset + 1; + // Senders zero-fill the slots of robots they do not control. + if (ibisSlotIsEmpty(cmd_data)) { + continue; + } + if (cmd_data[CHECK_COUNTER] == m_robotStates[robot_id].last_check_counter) { + continue; + } + + const IbisCommand cmd = ibisDeserialize(cmd_data); + m_robotStates[robot_id].last_check_counter = cmd.check_counter; + + // Team auto-detection: match vision_global_pos against cached positions. + // Also keeps the matched vision entry for orientation lookup below. + int teamIdx = -1; + const IbisVisionState* vis = nullptr; + double nearest = -1.0; + for (int t = 0; t < 2; ++t) { + const IbisVisionState& v = m_vision[t][robot_id]; + if (!v.valid) { continue; } + const float dx = v.x_mm / 1000.0f - cmd.vision_global_pos[0]; + const float dy = v.y_mm / 1000.0f - cmd.vision_global_pos[1]; + const double dist = std::hypot(dx, dy); + if (nearest < 0.0 || dist < nearest) { + nearest = dist; + } + if (dist < IBIS_POSITION_MATCH_THRESHOLD) { + teamIdx = t; + vis = &v; + break; + } + } + if (teamIdx < 0) { + warnPositionMismatch(robot_id, cmd, nearest); + continue; + } + m_robotStates[robot_id].match_warned = false; + const bool ibisIsBlue = (teamIdx == 0); + + auto* robotCmd = ibisIsBlue + ? blueControl->add_robot_commands() + : yellowControl->add_robot_commands(); + robotCmd->set_id(robot_id); + + if (ibisShouldStop(cmd)) { + // Deliberately no move_command: the real G474 answers this condition + // with omniStopAll() (state_func.c:314), which writes duty 0 to all + // four wheels, and the CAN frame carries duty only -- there is no + // brake bit (actuator.c:12). So hardware coasts here, it does not + // brake. Leaving move_command unset lands on SimRobot's + // !has_move_command() early return, which skips the wheel PID and + // lets the robot coast the same way. Emitting a zero velocity would + // instead drive the PID and brake in 0.133 m vs 0.573 m coasting, + // making every safety stop look better in simulation than on the + // field. prev_v* still has to be cleared: it is the mode 3 + // acceleration-limiter state, so a stale value would ramp the first + // command after the stop clears from the pre-stop velocity. + m_robotStates[robot_id].prev_vx = 0.0; + m_robotStates[robot_id].prev_vy = 0.0; + } else if (cmd.control_mode != IBIS_MODE_POLAR_VELOCITY_TARGET) { + // This adaptor emulates the robot's STM32 (G474) main board, which + // implements POLAR_VELOCITY_TARGET only. Any other mode means the + // chain is misconfigured -- most likely a POSITION_TARGET command + // that should have been consumed by the robot-side position loop + // (cm4_sim) before reaching the simulator. Hold the robot still and + // say why, rather than steering on reinterpreted mode args. + warnUnsupportedMode(robot_id, cmd.control_mode); + auto* lv = robotCmd->mutable_move_command()->mutable_local_velocity(); + lv->set_forward(0.0f); + lv->set_left(0.0f); + lv->set_angular(0.0f); + m_robotStates[robot_id].prev_vx = 0.0; + m_robotStates[robot_id].prev_vy = 0.0; + } else { + const double current_theta = vis->orientation_rad; + + double theta_error = cmd.target_global_theta - current_theta; + while (theta_error > M_PI) theta_error -= 2.0 * M_PI; + while (theta_error < -M_PI) theta_error += 2.0 * M_PI; + + double omega = IBIS_THETA_P_GAIN * theta_error; + omega = std::max(-static_cast(cmd.angular_velocity_limit), + std::min(omega, static_cast(cmd.angular_velocity_limit))); + + const double predicted_theta = current_theta + omega * IBIS_DT; + const double vel_angle = cmd.polar_velocity_theta - predicted_theta; + double target_vx = cmd.polar_velocity_r * std::cos(vel_angle); + double target_vy = cmd.polar_velocity_r * std::sin(vel_angle); + + auto& state = m_robotStates[robot_id]; + const double current_speed = std::hypot(state.prev_vx, state.prev_vy); + const double target_speed = std::hypot(target_vx, target_vy); + double acc_limit = (target_speed < current_speed) ? m_accBrake : m_accSpeedup; + if (cmd.acceleration_limit > 0.0f && cmd.acceleration_limit < static_cast(acc_limit)) { + acc_limit = cmd.acceleration_limit; + } + + const double delta_vx = target_vx - state.prev_vx; + const double delta_vy = target_vy - state.prev_vy; + const double delta_norm = std::hypot(delta_vx, delta_vy); + const double max_delta = acc_limit * IBIS_DT; + + double out_vx, out_vy; + if (delta_norm > max_delta && delta_norm > 1e-9) { + out_vx = state.prev_vx + (delta_vx / delta_norm) * max_delta; + out_vy = state.prev_vy + (delta_vy / delta_norm) * max_delta; + } else { + out_vx = target_vx; + out_vy = target_vy; + } + + const double out_speed = std::hypot(out_vx, out_vy); + if (cmd.linear_velocity_limit > 0.0f && out_speed > cmd.linear_velocity_limit) { + out_vx = out_vx / out_speed * cmd.linear_velocity_limit; + out_vy = out_vy / out_speed * cmd.linear_velocity_limit; + } + + state.prev_vx = out_vx; + state.prev_vy = out_vy; + + auto* lv = robotCmd->mutable_move_command()->mutable_local_velocity(); + lv->set_forward(static_cast(out_vx)); + lv->set_left(static_cast(out_vy)); + lv->set_angular(static_cast(omega)); + + if (cmd.kick_power > 0.001f) { + robotCmd->set_kick_speed(static_cast(IBIS_MAX_KICK_SPEED * cmd.kick_power)); + robotCmd->set_kick_angle(cmd.enable_chip ? static_cast(IBIS_CHIP_ANGLE_DEG) : 0.0f); + } + if (cmd.dribble_power > 0.001f) { + // Match Amun's normalized 0..1 dribbler command conversion. + constexpr float kMaxDribblerSpeedRpm = static_cast(150.0 * 60.0 * 0.5 / M_PI); + robotCmd->set_dribbler_speed(kMaxDribblerSpeedRpm * cmd.dribble_power); + } + } + + if (ibisIsBlue) { hasBlue = true; } else { hasYellow = true; } + } + + if (hasBlue) { + emit sendRadioCommands(blueControl, true, start); + } + if (hasYellow) { + emit sendRadioCommands(yellowControl, false, start); + } + warnLatency(m_timer->currentTime() - start); + } + } + +private: + // Logs at most once per second per robot, so a persistently misconfigured + // chain produces a readable hint instead of a flood at the command rate. + void warnUnsupportedMode(int robot_id, uint8_t mode) { + constexpr qint64 kWarnIntervalNs = 1000LL * 1000LL * 1000LL; + auto& state = m_robotStates[robot_id]; + const qint64 now = m_timer->currentTime(); + if (state.mode_warned && now - state.last_mode_warn_ns < kWarnIntervalNs) { + return; + } + state.mode_warned = true; + state.last_mode_warn_ns = now; + if (mode == IBIS_MODE_POSITION_TARGET) { + log(stdout, + "ibis: robot %d sent POSITION_TARGET (mode %u), robot stopped. The simulator " + "emulates the STM32 main board and does not close a position loop -- run the " + "CM4 position controller (cm4_sim) between crane and the simulator. " + "See docs/robot-side-position-control.md\n", + robot_id, static_cast(mode)); + } else { + log(stdout, + "ibis: robot %d sent unsupported control mode %u, robot stopped " + "(expected POLAR_VELOCITY_TARGET = %u)\n", + robot_id, static_cast(mode), + static_cast(IBIS_MODE_POLAR_VELOCITY_TARGET)); + } + } + + // The command carries the sender's own estimate of where the robot is + // (vision_global_pos). When it does not match any robot on the field the command + // is dropped -- silently, until this warning was added. A silent drop is very hard + // to tell apart from "the robot is commanded to hold still": the packets keep + // arriving, check_counter keeps advancing, and nothing moves. Report the nearest + // candidate so the reader can see whether the estimate is merely stale (slightly + // over the threshold) or pointing somewhere else entirely. + void warnPositionMismatch(int robot_id, const IbisCommand& cmd, double nearest) { + constexpr qint64 kWarnIntervalNs = 1000LL * 1000LL * 1000LL; + auto& state = m_robotStates[robot_id]; + const qint64 now = m_timer->currentTime(); + if (state.match_warned && now - state.last_match_warn_ns < kWarnIntervalNs) { + return; + } + state.match_warned = true; + state.last_match_warn_ns = now; + if (nearest < 0.0) { + log(stdout, + "ibis: robot %d command dropped -- no robot with this id is on the field yet " + "(command claims the robot is at %.3f, %.3f). Commands are ignored until " + "vision reports the robot.\n", + robot_id, cmd.vision_global_pos[0], cmd.vision_global_pos[1]); + } else { + log(stdout, + "ibis: robot %d command dropped -- vision_global_pos (%.3f, %.3f) is %.3f m " + "from the robot, over the %.2f m match threshold. The sender's position " + "estimate disagrees with the simulator; the robot coasts to a stop and " + "stays there until they agree. See docs/robot-side-position-control.md\n", + robot_id, cmd.vision_global_pos[0], cmd.vision_global_pos[1], + nearest, IBIS_POSITION_MATCH_THRESHOLD); + } + } + + static constexpr uint32_t kMaxRobots = 16; + + struct PerRobotState { + double prev_vx = 0.0; + double prev_vy = 0.0; + uint8_t last_check_counter = 0xFF; + qint64 last_mode_warn_ns = 0; + bool mode_warned = false; + qint64 last_match_warn_ns = 0; + bool match_warned = false; + }; + + // m_vision[0] = blue, m_vision[1] = yellow + IbisVisionState m_vision[2][kMaxRobots] = {}; + PerRobotState m_robotStates[IBIS_ROBOT_SLOTS] = {}; + + QUdpSocket m_server; + Timer* m_timer; + double m_accSpeedup; + double m_accBrake; +}; + +class RefereeTeamDetector : public QObject { + Q_OBJECT +public: + RefereeTeamDetector(const QString& teamName, bool localhost, quint16 port = SSL_GAME_CONTROLLER_PORT) + : m_socket(this) + , m_teamName(teamName.toLower().trimmed()) + { + m_socket.bind(QHostAddress::AnyIPv4, + port, + QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint); + if (!localhost) { + m_socket.joinMulticastGroup(QHostAddress(SSL_GAME_CONTROLLER_ADDRESS)); + } + connect(&m_socket, &QUdpSocket::readyRead, this, &RefereeTeamDetector::handleDatagrams); + } + +signals: + void teamDetected(bool ibisIsBlue); + +private slots: + void handleDatagrams() { + while (m_socket.hasPendingDatagrams()) { + auto datagram = m_socket.receiveDatagram(); + SSL_Referee ref; + if (!ref.ParseFromArray(datagram.data().data(), datagram.data().size())) { + continue; + } + auto tryMatch = [&](const SSL_Referee::TeamInfo& info, bool isBlue) { + if (!info.has_name()) { return false; } + if (QString::fromStdString(info.name()).toLower().trimmed() != m_teamName) { return false; } + disconnect(&m_socket, &QUdpSocket::readyRead, this, &RefereeTeamDetector::handleDatagrams); + emit teamDetected(isBlue); + return true; + }; + if (ref.has_blue() && tryMatch(ref.blue(), true)) { return; } + if (ref.has_yellow() && tryMatch(ref.yellow(), false)) { return; } + } + } + +private: + QUdpSocket m_socket; + QString m_teamName; +}; + +class IbisFeedbackAdaptor : public QObject { + Q_OBJECT +public: + // explicitColorSet: --ibis-team-color was given, so the colour is known up + // front and the referee (if also enabled) may only correct it later. + IbisFeedbackAdaptor(const QHostAddress& addr, quint16 portBase, bool useReferee, + bool explicitColorSet, bool explicitIsBlue) + : m_sender(new PacketSenderThread()) + , m_addr(addr) + , m_portBase(portBase) + , m_ibisIsBlue(explicitColorSet ? explicitIsBlue : true) + , m_refereeResolved(explicitColorSet || !useReferee) + {} + + ~IbisFeedbackAdaptor() override { + m_sender->stop(); + m_sender->wait(); + delete m_sender; + } + +public slots: + // world::SimulatorState を受け取るたびに IbisVisionState を更新し、そのループの feedback を送信する（125Hz） + // world::SimRobot の座標系: p_x/p_y はゲーム座標系のメートル単位（Bullet座標 / SIMULATOR_SCALE） + // SSL座標系への変換: x_mm = p_y * 1000, y_mm = -p_x * 1000 + // 方向角: クォータニオン (i,j,k,real) の回転行列第1列から yaw を算出 + void handleGroundTruth(const QByteArray& data) { + world::SimulatorState state; + if (!state.ParseFromArray(data.data(), data.size())) { + return; + } + auto updateTeam = [this](const auto& robots, int teamIdx) { + for (const auto& r : robots) { + const uint32_t id = r.id(); + if (id >= kMaxRobots) { continue; } + const float qx = r.rotation().i(); + const float qy = r.rotation().j(); + const float qz = r.rotation().k(); + const float qw = r.rotation().real(); + const float dir_x = 1.0f - 2.0f * (qy*qy + qz*qz); + const float dir_y = 2.0f * (qx*qy + qw*qz); + m_vision[teamIdx][id] = { + r.p_y() * 1000.0f, + -r.p_x() * 1000.0f, + std::atan2(dir_y, dir_x), + true + }; + } + }; + updateTeam(state.blue_robots(), 0); + updateTeam(state.yellow_robots(), 1); + if (!m_refereeResolved) { + // No feedback at all goes out until the colour is known. Say so: + // a controller that closes its position loop on this feedback has + // no position signal while this message is printing. + if (++m_waitLogCount % 200 == 0) { + log(stdout, + "ibis: NO FEEDBACK IS BEING SENT -- still waiting for the Game Controller " + "to identify team color. If the referee does not carry the team name, " + "pass --ibis-team-color blue|yellow instead of --ibis-use-referee.\n"); + } + return; + } + + const int teamIdx = m_ibisIsBlue ? 0 : 1; + for (uint32_t id = 0; id < kMaxRobots; ++id) { + const IbisVisionState& vis = m_vision[teamIdx][id]; + if (!vis.valid) { continue; } + + uint8_t buffer[IBIS_FEEDBACK_SIZE]; + ibisBuildFeedbackPacket( + buffer, + // Byte 3 echoes the AI command check counter on real hardware. + // This adaptor does not see the command stream, so it sends a + // free-running counter instead -- still usable for staleness + // detection, but it does not correlate with a specific command. + m_counters[id]++, + m_txCycles[id]++, + vis.orientation_rad, + m_robotCache[id].ball_detected, + 0, // kick_status not tracked in ER-Force simulator + vis.x_mm / 1000.0f, + vis.y_mm / 1000.0f, + m_robotCache[id].vel_x, + m_robotCache[id].vel_y); + + m_sender->enqueue( + QByteArray(reinterpret_cast(buffer), IBIS_FEEDBACK_SIZE), + m_addr, + m_portBase + static_cast(id)); + } + } + + void handleRobotResponse(const QList& responses) { + if (!m_refereeResolved) { + return; + } + + const int teamIdx = m_ibisIsBlue ? 0 : 1; + for (const auto& resp : responses) { + if (!resp.has_is_blue() || resp.is_blue() != m_ibisIsBlue) { continue; } + if (!resp.has_estimated_speed()) { continue; } + const uint32_t id = resp.id(); + if (id >= kMaxRobots) { continue; } + + const IbisVisionState& vis = m_vision[teamIdx][id]; + if (!vis.valid) { continue; } + + // ロボットローカル速度（v_f=前方, v_s=左）をグローバルSSL座標に変換してキャッシュ + const float theta = vis.orientation_rad; + const float v_f = resp.estimated_speed().v_f(); + const float v_s = resp.estimated_speed().v_s(); + m_robotCache[id].vel_x = v_f * std::cos(theta) - v_s * std::sin(theta); + m_robotCache[id].vel_y = v_f * std::sin(theta) + v_s * std::cos(theta); + m_robotCache[id].ball_detected = resp.has_ball_detected() && resp.ball_detected(); + } + } + + void handleRefereePacket(bool ibisIsBlue) { + m_ibisIsBlue = ibisIsBlue; + if (!m_refereeResolved) { + m_refereeResolved = true; + log(stdout, "ibis: team color resolved to %s from Game Controller\n", + ibisIsBlue ? "BLUE" : "YELLOW"); + } + } + +private: + static constexpr uint32_t kMaxRobots = 16; + + struct RobotCache { + float vel_x = 0.0f; + float vel_y = 0.0f; + bool ball_detected = false; + }; + + // m_vision[0] = blue, m_vision[1] = yellow + IbisVisionState m_vision[2][kMaxRobots] = {}; + RobotCache m_robotCache[kMaxRobots] = {}; + uint8_t m_counters[kMaxRobots] = {}; + uint8_t m_txCycles[kMaxRobots] = {}; + int m_waitLogCount = 0; + + PacketSenderThread* m_sender; + QHostAddress m_addr; + quint16 m_portBase; + bool m_ibisIsBlue; + bool m_refereeResolved; +}; + #include "simulator.moc" int main(int argc, char* argv[]) { + // Line-buffer stdout. It is block-buffered when it is a pipe or a file -- which + // is how this runs under Docker, systemd, or a test harness -- and log() never + // flushes, so a SIGTERM discards everything written since the last 4 KiB + // boundary. In practice that means the log is empty exactly when something went + // wrong and someone goes looking for it: a container stopped after a failed run + // has produced zero lines here. Do it in the process rather than leaving it to + // the caller to remember `stdbuf -oL`, which the published image's own entrypoint + // does not do. + // + // stderr is deliberately left alone: glibc leaves it unbuffered, which already + // survives an abrupt exit. Setting _IOLBF on it would be a weaker guarantee, not + // a stronger one -- it would start holding back a write that does not end in a + // newline, which is the opposite of what this is for. + std::setvbuf(stdout, nullptr, _IOLBF, 0); + QCoreApplication app(argc, argv); app.setApplicationName("Simulator"); app.setOrganizationName("ER-Force"); @@ -671,12 +1185,32 @@ int main(int argc, char* argv[]) parser.addHelpOption(); QCommandLineOption geometryConfig({"g", "geometry"}, "The geometry file to load as default", "file", "2020"); - QCommandLineOption realismConfig("realism", "Simulator realism configuration (short file name without the .txt)", "realism", "Realistic"); + QCommandLineOption realismConfig("realism", "Simulator realism configuration (short file name without the .txt)", "realism", "Ibis"); QCommandLineOption localhostConfig("localhost", "Use localhost as the output address for the simulator"); parser.addOption(geometryConfig); parser.addOption(realismConfig); parser.addOption(localhostConfig); + // ibis binary protocol options (always enabled on the ibis branch) + QCommandLineOption ibisPortOpt("ibis-port", "ibis command receiver UDP port", "port", QString::number(IBIS_DEFAULT_PORT)); + QCommandLineOption ibisFeedbackAddrOpt("ibis-feedback-addr", "ibis feedback destination address", "addr", "127.0.0.1"); + QCommandLineOption ibisFeedbackPortBaseOpt("ibis-feedback-port-base", "ibis feedback base port (robotId is added)", "port", QString::number(IBIS_FEEDBACK_PORT_BASE)); + QCommandLineOption ibisFeedbackTeamNameOpt("ibis-feedback-team-name", "Team name to look up in Game Controller for color detection", "name", "ibis"); + QCommandLineOption ibisUseRefereeOpt("ibis-use-referee", "Use Game Controller referee to auto-detect ibis team color"); + QCommandLineOption ibisTeamColorOpt("ibis-team-color", "Set the ibis team color explicitly (blue|yellow), instead of detecting it from the Game Controller", "color", ""); + QCommandLineOption ibisAccSpeedupOpt("ibis-acc-speedup", "Acceleration limit for speedup [m/s^2]", "accel", "4.0"); + QCommandLineOption ibisAccBrakeOpt("ibis-acc-brake", "Acceleration limit for braking [m/s^2]", "accel", "6.0"); + QCommandLineOption ibisRefereePortOpt("ibis-referee-port", "Game Controller multicast port for team color detection", "port", QString::number(SSL_GAME_CONTROLLER_PORT)); + parser.addOption(ibisPortOpt); + parser.addOption(ibisFeedbackAddrOpt); + parser.addOption(ibisFeedbackPortBaseOpt); + parser.addOption(ibisFeedbackTeamNameOpt); + parser.addOption(ibisUseRefereeOpt); + parser.addOption(ibisTeamColorOpt); + parser.addOption(ibisAccSpeedupOpt); + parser.addOption(ibisAccBrakeOpt); + parser.addOption(ibisRefereePortOpt); + parser.process(app); auto* desc = sslsim::RobotSpecs::descriptor(); @@ -748,6 +1282,83 @@ int main(int argc, char* argv[]) vision.moveToThread(&rcv_thread); commands.moveToThread(&rcv_thread); + // ibis binary protocol components (always enabled on the ibis branch) + { + const int cmdPort = parser.value(ibisPortOpt).toInt(); + const double accSpeedup = parser.value(ibisAccSpeedupOpt).toDouble(); + const double accBrake = parser.value(ibisAccBrakeOpt).toDouble(); + const QHostAddress fbAddr = QHostAddress(parser.value(ibisFeedbackAddrOpt)); + const quint16 fbPortBase = static_cast(parser.value(ibisFeedbackPortBaseOpt).toUInt()); + const bool useReferee = parser.isSet(ibisUseRefereeOpt); + + // Team colour selects which team's ground truth the feedback carries. + // Getting it wrong is silent and nasty: the feedback still flows, but + // it describes the opponent's robots, so anything closing a position + // loop on it steers on the wrong positions. + const QString teamColorStr = parser.value(ibisTeamColorOpt).trimmed().toLower(); + bool explicitColorSet = false; + bool explicitIsBlue = true; + if (!teamColorStr.isEmpty()) { + if (teamColorStr == "blue") { + explicitColorSet = true; explicitIsBlue = true; + } else if (teamColorStr == "yellow") { + explicitColorSet = true; explicitIsBlue = false; + } else { + log(stdout, "ibis: unknown --ibis-team-color '%s', expected blue or yellow\n", + teamColorStr.toStdString().c_str()); + return 1; + } + } + if (explicitColorSet) { + log(stdout, "ibis: team color set to %s by --ibis-team-color\n", + explicitIsBlue ? "BLUE" : "YELLOW"); + } else if (useReferee) { + log(stdout, "ibis: team color will be detected from the Game Controller " + "(team name '%s'); no feedback is sent until it resolves\n", + parser.value(ibisFeedbackTeamNameOpt).toStdString().c_str()); + } else { + log(stdout, "ibis: WARNING no team color given and referee detection is off -- " + "assuming BLUE. If ibis plays yellow, the feedback will carry the " + "opponent's positions. Pass --ibis-team-color blue|yellow.\n"); + } + const quint16 refereePort = static_cast(parser.value(ibisRefereePortOpt).toUInt()); + + auto* ibisCmd = new IbisCommandAdaptor(cmdPort, &timer, accSpeedup, accBrake); + auto* ibisFb = new IbisFeedbackAdaptor(fbAddr, fbPortBase, useReferee, + explicitColorSet, explicitIsBlue); + + // IbisCommandAdaptor receives vision data to cache robot positions/orientations + QObject::connect(&sim, &SimProxy::gotPacket, + ibisCmd, &IbisCommandAdaptor::handleVisionData); + // IbisCommandAdaptor sends converted commands to the simulator + QObject::connect(ibisCmd, &IbisCommandAdaptor::sendRadioCommands, + &sim, &SimProxy::handleRadioCommands); + + // IbisFeedbackAdaptor receives ground truth positions at 125Hz and emits one feedback packet per loop + QObject::connect(&sim, &SimProxy::sendGroundTruth, + ibisFb, &IbisFeedbackAdaptor::handleGroundTruth); + // IbisFeedbackAdaptor receives radio responses for velocity and ball detection + QObject::connect(&sim, &SimProxy::sendRadioResponses, + ibisFb, &IbisFeedbackAdaptor::handleRobotResponse); + + if (useReferee) { + auto* referee = new RefereeTeamDetector( + parser.value(ibisFeedbackTeamNameOpt), + parser.isSet(localhostConfig), + refereePort); + QObject::connect(referee, &RefereeTeamDetector::teamDetected, + ibisFb, &IbisFeedbackAdaptor::handleRefereePacket); + referee->moveToThread(&rcv_thread); + } + + ibisCmd->moveToThread(&rcv_thread); + ibisFb->moveToThread(&rcv_thread); + + log(stdout, "ibis: command receiver on UDP port %d\n", cmdPort); + log(stdout, "ibis: feedback sender to %s base port %d synchronized to simulator loop (125 Hz)\n", + parser.value(ibisFeedbackAddrOpt).toStdString().c_str(), + static_cast(fbPortBase)); + } rcv_thread.start();