From a29ed6988da8c3255788c3ea5f4bb1e72e286ca6 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Mon, 10 Aug 2026 18:06:43 +0800 Subject: [PATCH 01/86] chore: Update infrastructure scripts, CI and build config --- .github/workflows/update-image.yml | 1 + .script/complete/_local-context | 22 +++ .script/complete/_remote-context | 22 +++ .script/host/rmcs | 33 +++- .script/local-context | 89 +++++++++ .script/remote-context | 10 + .script/remote-status | 2 +- .script/rmcs-cli | 220 +++++++++++++++++++++ .script/scan-remote | 278 +++++++++------------------ Dockerfile | 2 +- rmcs_ws/src/rmcs_core/CMakeLists.txt | 4 +- rmcs_ws/src/rmcs_core/package.xml | 3 + rmcs_ws/src/rmcs_core/plugins.xml | 18 +- 13 files changed, 509 insertions(+), 195 deletions(-) create mode 100644 .script/complete/_local-context create mode 100644 .script/complete/_remote-context create mode 100755 .script/local-context create mode 100755 .script/remote-context create mode 100755 .script/rmcs-cli diff --git a/.github/workflows/update-image.yml b/.github/workflows/update-image.yml index 78e039a00..75a4fbfc0 100644 --- a/.github/workflows/update-image.yml +++ b/.github/workflows/update-image.yml @@ -8,6 +8,7 @@ on: paths: - Dockerfile - .github/workflows/update-image.yml + - .script/template/ - .script/build-rmcs-cross - rmcs_ws/toolchain.cmake diff --git a/.script/complete/_local-context b/.script/complete/_local-context new file mode 100644 index 000000000..6faef6c1f --- /dev/null +++ b/.script/complete/_local-context @@ -0,0 +1,22 @@ +#compdef local-context + +_arguments \ + '1:key:(game_stage robot_health robot_bullet)' \ + '2:value:->value' + +case "$state" in + value) + case "${words[2]}" in + game_stage) + _values 'game stage' \ + not_start \ + preparation \ + referee_check \ + countdown \ + started \ + settling \ + unknown + ;; + esac + ;; +esac diff --git a/.script/complete/_remote-context b/.script/complete/_remote-context new file mode 100644 index 000000000..67e89b1b7 --- /dev/null +++ b/.script/complete/_remote-context @@ -0,0 +1,22 @@ +#compdef remote-context + +_arguments \ + '1:key:(game_stage robot_health robot_bullet)' \ + '2:value:->value' + +case "$state" in + value) + case "${words[2]}" in + game_stage) + _values 'game stage' \ + not_start \ + preparation \ + referee_check \ + countdown \ + started \ + settling \ + unknown + ;; + esac + ;; +esac diff --git a/.script/host/rmcs b/.script/host/rmcs index f94a7cf75..8d8696d9f 100755 --- a/.script/host/rmcs +++ b/.script/host/rmcs @@ -6,19 +6,24 @@ readonly DEVELOPER_NAME="ubuntu" readonly NVIM_PATH="/opt/nvim/bin/nvim" readonly NVIM_PORT=6666 readonly NVIM_HOST="localhost" +readonly RMCS_PATH="/workspaces/RMCS/" function show_help() { local project_dir="$1" local service="$2" - echo "Usage: $(basename "$0") [path] [zsh|n|nvim|neovide|vim|ide]" + echo "Usage: $(basename "$0") [path] [zsh|recreate|n|nvim|neovide|vim|ide|ai]" echo " Project dir: $project_dir" echo " Service: $service" } +function setup_container() { + docker compose up -d --no-recreate +} + function rmcs_zsh() { local service="$1" echo "Starting and entering container..." - docker compose up -d + setup_container docker compose exec "$service" zsh } @@ -29,7 +34,7 @@ function rmcs_nvim() { local port=$NVIM_PORT echo "Starting container..." - docker compose up -d + setup_container echo "Checking available port and starting nvim headless server..." while nc -z "$NVIM_HOST" "$port" 2>/dev/null; do @@ -38,7 +43,7 @@ function rmcs_nvim() { done echo "Starting nvim server on port $port..." - docker compose exec -u "$DEVELOPER_NAME" -d "$service" \ + docker compose exec -u "$DEVELOPER_NAME" -w "$RMCS_PATH" -d "$service" \ "$NVIM_PATH" --headless --listen "$NVIM_HOST:$port" for i in $(seq 1 $timeout); do @@ -60,6 +65,20 @@ function rmcs_nvim() { fi } +function rmcs_recreate() { + echo "Recreating container..." + docker compose up -d --force-recreate +} + +function rmcs_ai() { + local service="$1" + local agent="${RMCS_AGENT:-opencode}" + echo "Starting container and launching Agent ($agent)..." + setup_container + docker compose exec -u "$DEVELOPER_NAME" -w "$RMCS_PATH" "$service" \ + zsh -ic "exec ${agent}" +} + function main() { local project_dir command @@ -84,9 +103,15 @@ function main() { zsh) rmcs_zsh "$service" ;; + recreate) + rmcs_recreate + ;; n | nvim | neovide | vim | ide) rmcs_nvim "$service" ;; + ai) + rmcs_ai "$service" + ;; *) show_help "$project_dir" "$service" ;; diff --git a/.script/local-context b/.script/local-context new file mode 100755 index 000000000..7ffa214b6 --- /dev/null +++ b/.script/local-context @@ -0,0 +1,89 @@ +#!/usr/bin/env bash + +set -euo pipefail + +BASE="/tmp/rmcs-navigation/context" + +usage() { + cat <<'EOF' +Usage: local-context + +key: + game_stage | robot_health | robot_bullet + +value: + - game_stage: not_start | preparation | referee_check | countdown | started | settling | unknown + (also accepts 0..5 and 255) + - others: integer + +Examples: + local-context game_stage started + local-context game_stage 4 + local-context robot_health 350 +EOF +} + +if [[ $# -ne 2 ]]; then + usage + exit 1 +fi + +key="$1" +raw_value="$2" + +case "$key" in +game_stage | robot_health | robot_bullet) ;; +*) + echo "Invalid key: ${key}" >&2 + usage + exit 1 + ;; +esac + +value="$raw_value" +if [[ "$key" == "game_stage" ]]; then + normalized="${raw_value,,}" + case "$normalized" in + not_start) value=0 ;; + preparation) value=1 ;; + referee_check) value=2 ;; + countdown) value=3 ;; + started) value=4 ;; + settling) value=5 ;; + unknown) value=255 ;; + *) + if [[ "$normalized" =~ ^[0-9]+$ ]]; then + value="$normalized" + else + echo "Invalid game_stage value: ${raw_value}" >&2 + usage + exit 1 + fi + ;; + esac + + if ((value < 0 || (value > 5 && value != 255))); then + echo "game_stage out of range: ${value} (expected 0..5 or 255)" >&2 + exit 1 + fi +else + if ! [[ "$raw_value" =~ ^[0-9]+$ ]]; then + echo "${key} must be an integer: ${raw_value}" >&2 + exit 1 + fi +fi + +fifo="${BASE}/${key}" +if [[ ! -p "${fifo}" ]]; then + echo "Context FIFO not found: ${fifo}" >&2 + echo "Start rmcs-navigation first so /tmp/rmcs-navigation/context/ is created." >&2 + exit 1 +fi + +echo "[local-context] key: ${key}" +echo "[local-context] value: ${value}" +echo "[local-context] fifo: ${fifo}" + +printf '%s' "${value}" >"${fifo}" + +echo "Wrote ${key}=${value} to ${fifo}" diff --git a/.script/remote-context b/.script/remote-context new file mode 100755 index 000000000..4ffaf264c --- /dev/null +++ b/.script/remote-context @@ -0,0 +1,10 @@ +#!/usr/bin/env bash + +set -euo pipefail + +SCRIPT_DIR="$(dirname "$(readlink -f "$0")")" + +{ + printf 'source /root/env_setup.bash\n' + cat "${SCRIPT_DIR}/local-context" +} | ssh remote bash -s -- "$@" diff --git a/.script/remote-status b/.script/remote-status index f5805e05c..c7461321e 100755 --- a/.script/remote-status +++ b/.script/remote-status @@ -31,7 +31,7 @@ call_status_service() { return fi - printf "=== %s ===\n" "$service" + printf "Robot Status [%s]:\n\n" "$service" local raw raw="$(ros2 service call "$service" std_srvs/srv/Trigger "{}" 2>&1 || true)" diff --git a/.script/rmcs-cli b/.script/rmcs-cli new file mode 100755 index 000000000..92c281754 --- /dev/null +++ b/.script/rmcs-cli @@ -0,0 +1,220 @@ +#!/usr/bin/env python3 +"""rmcs-cli - RMCS 开发工作流管理器""" + +import subprocess +import sys +import os +import termios +import tty +import select +import time + +SESSION = "rmcs-cli" +RMCS_PATH = os.getenv("RMCS_PATH", "/workspaces/RMCS") +SCRIPT_DIR = os.path.join(RMCS_PATH, ".script") + +# 运行时由 setup_tmux 填充 +_pane_control = None +_pane_build = None +_pane_sync = None +_pane_remote = None + + +def log(msg): + print(msg, flush=True) + + +def run_cmd(cmd): + return subprocess.run(cmd, shell=True).returncode + + +def _list_pane_ids(): + result = subprocess.run( + ["tmux", "list-panes", "-t", SESSION, "-F", "#{pane_id}"], + capture_output=True, text=True + ) + return result.stdout.strip().split("\n") + + +def send_pane(pane_id, cmd): + subprocess.run(["tmux", "send-keys", "-t", pane_id, cmd, "Enter"]) + + +def setup_tmux(): + global _pane_control, _pane_build, _pane_sync, _pane_remote + + result = subprocess.run( + ["tmux", "has-session", "-t", SESSION], + capture_output=True + ) + if result.returncode == 0: + subprocess.run(["tmux", "attach", "-t", SESSION]) + sys.exit(0) + + subprocess.run([ + "tmux", "new-session", "-d", "-s", SESSION, + "-x", "160", "-y", "40" + ]) + + first_pane = _list_pane_ids()[0] + + # 左右分割 + subprocess.run(["tmux", "split-window", "-h", "-t", first_pane]) + time.sleep(0.1) + panes = _list_pane_ids() + + # 左边上下分割 + subprocess.run(["tmux", "split-window", "-v", "-t", panes[0]]) + time.sleep(0.1) + + # 右边上下分割 (原始右半 pane) + subprocess.run(["tmux", "split-window", "-v", "-t", panes[1]]) + time.sleep(0.1) + + # 布局: [0]=左上(控制) [1]=左下(同步) [2]=右上(编译) [3]=右下(远程) + final = _list_pane_ids() + _pane_control = final[0] + _pane_sync = final[1] + _pane_build = final[2] + _pane_remote = final[3] + + +def cleanup(): + subprocess.run(["tmux", "kill-session", "-t", SESSION]) + + +def workflow(): + log("▸ 编译中...") + send_pane(_pane_build, f"cd {RMCS_PATH} && .script/build-rmcs") + + log(" 等待编译完成...") + while True: + result = subprocess.run( + ["tmux", "capture-pane", "-t", _pane_build, "-p"], + capture_output=True, text=True + ) + if "Summary:" in result.stdout or "failed" in result.stdout: + break + time.sleep(1) + + if "failed" in result.stdout: + log("✗ 编译失败") + return + + log(" 完成") + + log("▸ 等待同步...") + if run_cmd(f"{SCRIPT_DIR}/wait-sync") != 0: + log("✗ 同步失败") + return + log(" 完成") + + log("▸ 部署中...") + send_pane(_pane_remote, f"{SCRIPT_DIR}/attach-remote -r") + log(" 已发送到远程窗格") + + +def read_key(): + if select.select([sys.stdin], [], [], 0.1)[0]: + return sys.stdin.read(1) + return None + + +def main(): + if run_cmd("which tmux") != 0: + sys.exit("错误: 未找到 tmux") + + if os.environ.get("RMCS_CLI") == "1": + run_control_panel() + else: + launch_session() + + +def launch_session(): + setup_tmux() + + # 在控制面板窗格中重新运行自己 + subprocess.run([ + "tmux", "send-keys", "-t", _pane_control, + f"cd {RMCS_PATH} && RMCS_CLI=1 {__file__}", "Enter" + ]) + + subprocess.run(["tmux", "select-pane", "-t", _pane_control]) + subprocess.run(["tmux", "attach", "-t", SESSION]) + + +def run_control_panel(): + global _pane_control, _pane_build, _pane_sync, _pane_remote + + time.sleep(0.5) + + # 从环境恢复 pane ID (launch_session 设置后，窗格已知) + panes = _list_pane_ids() + _pane_control = panes[0] + _pane_sync = panes[1] + _pane_build = panes[2] + _pane_remote = panes[3] + + # 自动启动 sync-remote + send_pane(_pane_sync, f"cd {RMCS_PATH} && .script/sync-remote") + log("同步已在后台启动") + log("") + log("rmcs-cli v0.1.0") + log("") + log(" 命令") + log(" r 编译 -> 同步 -> 部署") + log(" b 仅编译") + log(" s 仅同步") + log(" a 仅部署") + log(" q 退出") + log("") + + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + try: + tty.setraw(fd) + while True: + key = read_key() + if key is None: + continue + + if key == "r": + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + workflow() + tty.setraw(fd) + + elif key == "b": + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + log("▸ 编译中...") + send_pane(_pane_build, f"cd {RMCS_PATH} && .script/build-rmcs") + tty.setraw(fd) + + elif key == "s": + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + log("▸ 重新启动同步...") + subprocess.run(["tmux", "send-keys", "-t", _pane_sync, "C-c"]) + time.sleep(0.3) + send_pane(_pane_sync, f"cd {RMCS_PATH} && .script/sync-remote") + tty.setraw(fd) + + elif key == "a": + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + log("▸ 重新部署...") + subprocess.run( + ["tmux", "send-keys", "-t", _pane_remote, "C-c"]) + time.sleep(0.3) + send_pane(_pane_remote, f"{SCRIPT_DIR}/attach-remote -r") + tty.setraw(fd) + + elif key in ("q", "\x03"): + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + log("退出中...") + cleanup() + break + + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + + +if __name__ == "__main__": + main() diff --git a/.script/scan-remote b/.script/scan-remote index 65973bfb7..3e752f5d4 100755 --- a/.script/scan-remote +++ b/.script/scan-remote @@ -6,6 +6,7 @@ import json import os import re import select +import shutil import socket import subprocess import sys @@ -18,10 +19,10 @@ from colorama import Fore, Style SSH_PORT = 2022 SSH_USER = "root" -CONNECT_TIMEOUT = 0.25 +CONNECT_TIMEOUT = 0.75 BANNER_TIMEOUT = 0.35 SSH_PROBE_TIMEOUT = 2.0 -DEFAULT_192_168_SEGMENTS = range(1, 11) +DEFAULT_SCAN_SEGMENTS = range(1, 6) SKIP_PREFIXES = ("lo", "docker", "br-", "veth", "zt", "tailscale") SPINNER_FRAMES = "⠋⠙⠹⠸⠼⠴⠦⠧⠇⠏" REFRESH_INTERVAL = 0.08 @@ -48,47 +49,41 @@ class RawTerminal: def __exit__(self, *_): termios.tcsetattr(self._fd, termios.TCSANOW, self._old) + _SIMPLE_KEYS = { + b"\r": "enter", b"\n": "enter", + b"\t": "next", b"s": "next", b"S": "next", b"\x0e": "next", + b"w": "prev", b"W": "prev", b"\x10": "prev", + b"\x7f": "backspace", b"\x08": "backspace", + } + _ESCAPE_DIRS = {b"A": "prev", b"B": "next", b"Z": "prev"} + @staticmethod def read_key(): + if not RawTerminal.key_ready(0.02): + return None raw = sys.stdin.buffer.raw.read(1) - if raw == b"\x03": - raise KeyboardInterrupt - if raw in (b"\r", b"\n"): - return "enter" - if raw == b"\t": - return "next" - if raw in (b"w", b"W"): - return "prev" - if raw in (b"s", b"S"): - return "next" - if raw == b"\x0e": - return "next" - if raw == b"\x10": - return "prev" - if raw in (b"\x7f", b"\x08"): - return "backspace" - if raw == b"\x1b": - if not RawTerminal.key_ready(0.02): - return "quit" - nxt = sys.stdin.buffer.raw.read(1) - if nxt != b"[": - return "escape" - if not RawTerminal.key_ready(0.02): - return "escape" - direction = sys.stdin.buffer.raw.read(1) - if direction == b"A": - return "prev" - if direction == b"B": - return "next" - if direction == b"Z": - return "prev" - return "escape" if raw in (b"q", b"Q"): - return "quit" + os._exit(130) + if raw in RawTerminal._SIMPLE_KEYS: + return RawTerminal._SIMPLE_KEYS[raw] + if raw == b"\x1b": + return RawTerminal._read_escape() if len(raw) == 1 and 32 <= raw[0] <= 126: return raw.decode() return None + @staticmethod + def _read_escape(): + if not RawTerminal.key_ready(0.02): + os._exit(130) + nxt = sys.stdin.buffer.raw.read(1) + if nxt != b"[" or not RawTerminal.key_ready(0.02): + os._exit(130) + direction = sys.stdin.buffer.raw.read(1) + if direction in RawTerminal._ESCAPE_DIRS: + return RawTerminal._ESCAPE_DIRS[direction] + os._exit(130) + @staticmethod def key_ready(timeout): ready, _, _ = select.select([sys.stdin], [], [], timeout) @@ -171,7 +166,8 @@ def default_networks(): if ip.is_loopback: continue - interface = ipaddress.ip_interface(f"{addr.address}/{addr.netmask}") + interface = ipaddress.ip_interface( + f"{addr.address}/{addr.netmask}") if not interface.ip.is_private: continue @@ -187,13 +183,11 @@ def default_networks(): def expanded_networks(interface): ip = interface.ip - if ip.packed[0] == 192 and ip.packed[1] == 168: - return [ - ipaddress.ip_network(f"192.168.{segment}.0/24") - for segment in DEFAULT_192_168_SEGMENTS - ] - - return [ipaddress.ip_network(f"{ip}/24", strict=False)] + prefix = ".".join(str(part) for part in ip.packed[:2]) + return [ + ipaddress.ip_network(f"{prefix}.{segment}.0/24") + for segment in DEFAULT_SCAN_SEGMENTS + ] def dedupe_networks(networks): @@ -251,7 +245,8 @@ def probe_host(host): def ssh_lookup(host): - avahi_name = ssh_run(host, "grep -E '^host-name=' /etc/avahi/avahi-daemon.conf | cut -d= -f2-") + avahi_name = ssh_run( + host, "grep -E '^host-name=' /etc/avahi/avahi-daemon.conf | cut -d= -f2-") if avahi_name: return avahi_name, "avahi" @@ -397,8 +392,8 @@ def scan_all_networks_until_selected( ) selected_result = None - - with concurrent.futures.ThreadPoolExecutor(max_workers=max_workers) as pool: + pool = concurrent.futures.ThreadPoolExecutor(max_workers=max_workers) + try: future_to_network = { pool.submit(probe_host, host): network for network, host in iter_interleaved_targets(network_targets) @@ -406,7 +401,7 @@ def scan_all_networks_until_selected( pending = set(future_to_network) while pending: - if on_key is not None and RawTerminal.key_ready(0.02): + if on_key is not None: key = RawTerminal.read_key() if key is not None: selected_result = on_key(key) @@ -444,6 +439,8 @@ def scan_all_networks_until_selected( if selected_result is not None: pool.shutdown(wait=False, cancel_futures=True) return sort_results(results), selected_result + finally: + pool.shutdown(wait=False, cancel_futures=True) return sort_results(results), None @@ -466,35 +463,6 @@ def iter_interleaved_targets(network_targets): pending = next_pending -def scan_network(network, on_progress=None, on_found=None): - targets = host_candidates([network]) - max_workers = min(128, max(8, len(targets))) - scanned = 0 - found = 0 - results = [] - - if on_progress is not None: - on_progress(network, scanned, len(targets), found, False) - - with concurrent.futures.ThreadPoolExecutor(max_workers=max_workers) as pool: - futures = [pool.submit(probe_host, host) for host in targets] - for future in concurrent.futures.as_completed(futures): - scanned += 1 - result = future.result() - if result is not None: - found += 1 - results.append(result) - if on_found is not None: - on_found(network, result) - if on_progress is not None: - on_progress(network, scanned, len(targets), found, False) - - if on_progress is not None: - on_progress(network, scanned, len(targets), found, True) - - return sort_results(results) - - def sort_results(results): return sorted( results, @@ -505,7 +473,8 @@ def sort_results(results): class ProgressView: def __init__(self, networks): self.networks = [str(network) for network in networks] - self.network_width = max((len(network) for network in self.networks), default=0) + self.network_width = max((len(network) + for network in self.networks), default=0) self.state = { network: { "scanned": 0, @@ -523,6 +492,7 @@ class ProgressView: self._thread = None self._selection = None self._prompt = None + self._interactive_render = sys.stdout.isatty() and os.getenv("TERM") != "dumb" def start(self): self._thread = threading.Thread(target=self._refresh_loop, daemon=True) @@ -552,6 +522,9 @@ class ProgressView: self.state[key]["results"].append(result) def render(self): + if not self._interactive_render: + return + with self._lock: lines = [] frame = SPINNER_FRAMES[self._frame % len(SPINNER_FRAMES)] @@ -595,15 +568,20 @@ class ProgressView: line = ( f"{TOKYO_BG} {TOKYO_ACCENT}{marker} " f"{TOKYO_IP}{result['ip']:<{result_ip_width}} " - f"{TOKYO_HOST}{result['hostname_label']:<{result_hostname_width}} " - f"{TOKYO_BANNER}{result['banner']}{Style.RESET_ALL}" + f"{TOKYO_HOST}{result['hostname_label']:<{ + result_hostname_width}} " + f"{TOKYO_BANNER}{result['banner']}{ + Style.RESET_ALL}" ) else: line = ( f" {Fore.CYAN}{marker}{Style.RESET_ALL} " - f"{Fore.GREEN}{result['ip']:<{result_ip_width}}{Style.RESET_ALL} " - f"{Fore.MAGENTA}{result['hostname_label']:<{result_hostname_width}}{Style.RESET_ALL} " - f"{Fore.LIGHTBLACK_EX}{result['banner']}{Style.RESET_ALL}" + f"{Fore.GREEN}{result['ip']:<{result_ip_width}}{ + Style.RESET_ALL} " + f"{Fore.MAGENTA}{result['hostname_label']:<{ + result_hostname_width}}{Style.RESET_ALL} " + f"{Fore.LIGHTBLACK_EX}{ + result['banner']}{Style.RESET_ALL}" ) lines.append(line) @@ -611,17 +589,38 @@ class ProgressView: lines.append("") lines.append(self._prompt) + width = max(20, shutil.get_terminal_size((120, 24)).columns) + lines = [self._fit_line(line, width) for line in lines] + if self._rendered_lines: - sys.stdout.write(f"\033[{self._rendered_lines}F") + sys.stdout.write(f"\r\033[{self._rendered_lines}A") for line in lines: - sys.stdout.write("\033[2K") + sys.stdout.write("\033[2K\r") sys.stdout.write(line) sys.stdout.write("\n") for _ in range(max(0, self._rendered_lines - len(lines))): - sys.stdout.write("\033[2K\n") + sys.stdout.write("\033[2K\r\n") sys.stdout.flush() self._rendered_lines = len(lines) + @staticmethod + def _fit_line(line, width): + result = [] + visible = 0 + index = 0 + while index < len(line) and visible < width - 1: + if line[index] == "\033": + end = line.find("m", index) + if end == -1: + break + result.append(line[index:end + 1]) + index = end + 1 + continue + result.append(line[index]) + visible += 1 + index += 1 + return "".join(result) + Style.RESET_ALL + def set_selection(self, network, ip, prompt): with self._lock: self._selection = (network, ip) @@ -639,7 +638,8 @@ class ProgressView: for network in self.networks: ordered = sorted( self.state[network]["results"], - key=lambda item: tuple(int(part) for part in item["ip"].split(".")), + key=lambda item: tuple(int(part) + for part in item["ip"].split(".")), ) for result in ordered: flattened.append((network, result)) @@ -713,92 +713,6 @@ def print_results(results, json_mode, interactive_mode): print(format_table(results)) -def interactive_select_remote(view): - candidates = view.flatten_results() - if not candidates: - return None - - selected = 0 - confirm_mode = False - confirm_buf = "" - - def render_prompt(): - network, result = candidates[selected] - if confirm_mode: - prompt = ( - f"Set {Fore.GREEN}{result['ip']}{Style.RESET_ALL} as " - f"{Fore.CYAN}remote{Style.RESET_ALL}? [Y/n] {confirm_buf}" - ) - else: - prompt = ( - "Select result with Tab/Shift+Tab/↑/↓/W/S/Ctrl+N/Ctrl+P, " - "press Enter to continue, q/Esc/Ctrl+C to exit." - ) - view.set_selection(network, result["ip"], prompt) - - def confirm_choice(): - _, result = candidates[selected] - set_remote(result["ip"]) - view.clear_prompt() - print( - f"Successfully set remote host to " - f"{Fore.LIGHTGREEN_EX}{result['ip']}{Style.RESET_ALL}." - ) - return result["ip"] - - render_prompt() - with RawTerminal(): - while True: - key = RawTerminal.read_key() - if key is None: - continue - - if confirm_mode: - if key == "enter": - answer = confirm_buf.strip().lower() - if answer in ("", "y", "yes"): - return confirm_choice() - if answer in ("n", "no"): - confirm_mode = False - confirm_buf = "" - render_prompt() - continue - if key == "backspace": - confirm_buf = confirm_buf[:-1] - render_prompt() - continue - if key == "escape": - confirm_mode = False - confirm_buf = "" - render_prompt() - continue - if key in ("next", "prev"): - confirm_mode = False - confirm_buf = "" - elif isinstance(key, str) and len(key) == 1: - confirm_buf += key - render_prompt() - continue - - if key == "next": - selected = (selected + 1) % len(candidates) - confirm_mode = False - confirm_buf = "" - render_prompt() - continue - if key == "prev": - selected = (selected - 1) % len(candidates) - confirm_mode = False - confirm_buf = "" - render_prompt() - continue - if key == "enter": - confirm_mode = True - confirm_buf = "" - render_prompt() - continue - - def interactive_scan_and_select(networks): view = ProgressView(networks) selected = {"index": 0, "key": None} @@ -825,19 +739,19 @@ def interactive_scan_and_select(networks): view.set_selection( None, None, - "Scanning... press q/Esc/Ctrl+C to exit. Selection becomes available once a result appears.", + "Scanning... press q/Esc to exit. Selection becomes available once a result appears.", ) return sync_selection(candidates) network, result = candidates[selected["index"]] - selected["key"] = (network, result["ip"]) - scan_done = all(view.state[network_name]["done"] for network_name in view.networks) + scan_done = all(view.state[network_name]["done"] + for network_name in view.networks) prompt = ( "Scan complete. Select with Tab/Shift+Tab/↑/↓/W/S/Ctrl+N/Ctrl+P, " - "press Enter to set selected remote, q/Esc/Ctrl+C to exit." + "press Enter to set selected remote, q/Esc to exit." if scan_done - else "Select with Tab/Shift+Tab/↑/↓/W/S/Ctrl+N/Ctrl+P, press Enter to set immediately, q/Esc/Ctrl+C to exit." + else "Select with Tab/Shift+Tab/↑/↓/W/S/Ctrl+N/Ctrl+P, press Enter to set immediately, q/Esc to exit." ) view.set_selection( network, @@ -846,9 +760,6 @@ def interactive_scan_and_select(networks): ) def on_key(key): - if key in ("quit", "escape"): - raise KeyboardInterrupt - candidates = view.flatten_results() if not candidates: return None @@ -876,6 +787,8 @@ def interactive_scan_and_select(networks): with RawTerminal(): while True: key = RawTerminal.read_key() + if key is None: + continue chosen = on_key(key) if chosen is not None: return chosen @@ -888,7 +801,8 @@ def interactive_scan_and_select(networks): networks, on_key=on_key, on_progress=view.update_progress, - on_found=lambda network, result: (view.add_result(network, result), render_prompt()), + on_found=lambda network, result: ( + view.add_result(network, result), render_prompt()), ) if chosen is None and results: render_prompt() @@ -931,4 +845,4 @@ if __name__ == "__main__": try: sys.exit(main(sys.argv[1:])) except KeyboardInterrupt: - sys.exit(1) + os._exit(130) diff --git a/Dockerfile b/Dockerfile index cf9734363..f53de75e4 100644 --- a/Dockerfile +++ b/Dockerfile @@ -38,7 +38,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ libceres-dev \ ros-$ROS_DISTRO-rviz2 ros-$ROS_DISTRO-foxglove-bridge \ ros-$ROS_DISTRO-pcl-ros ros-$ROS_DISTRO-pcl-conversions ros-$ROS_DISTRO-pcl-msgs \ - ros-$ROS_DISTRO-navigation2 ros-$ROS_DISTRO-nav2-msgs \ + ros-$ROS_DISTRO-navigation2 ros-$ROS_DISTRO-nav2-msgs ros-$ROS_DISTRO-mavlink \ lua5.4 liblua5.4-0 liblua5.4-dev && \ apt-get clean && \ rm -rf /var/lib/apt/lists/* /tmp/* diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index 81bf0b613..0ae6350ad 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -18,8 +18,8 @@ include(FetchContent) set(BUILD_STATIC_LIBRMCS ON CACHE BOOL "Build static librmcs SDK" FORCE) FetchContent_Declare( librmcs - URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.2.0/librmcs-sdk-src-3.2.0.zip - URL_HASH SHA256=f81c3af7fbcf35727a8a7586200db8e9bf668ee0f448529de4bfd3eb7c36ed6f + URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.3.0b1/librmcs-sdk-src-3.3.0-beta.1-debug.zip + URL_HASH SHA256=446d632b23b652dd309f35075ef90928dee89f42e8ec71417fa2439445e0b4ff DOWNLOAD_EXTRACT_TIMESTAMP TRUE ) FetchContent_MakeAvailable(librmcs) diff --git a/rmcs_ws/src/rmcs_core/package.xml b/rmcs_ws/src/rmcs_core/package.xml index 4312d3341..b3d6d14af 100644 --- a/rmcs_ws/src/rmcs_core/package.xml +++ b/rmcs_ws/src/rmcs_core/package.xml @@ -20,6 +20,9 @@ rmcs_msgs rmcs_executor rmcs_description + mavlink + nav_msgs + ament_index_cpp ament_lint_auto ament_lint_common diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 3ac3bf0cc..a066759ae 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -1,12 +1,12 @@ - - - - + + + + @@ -18,9 +18,10 @@ + + - @@ -47,14 +48,21 @@ + + + + + + + From c57326225a67a40fccea4387958aa5d473a70e90 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Mon, 10 Aug 2026 18:06:45 +0800 Subject: [PATCH 02/86] feat: Update rmcs_msgs messages and rmcs_utility --- .../include/rmcs_msgs/chassis_mode.hpp | 22 +++- .../rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp | 7 +- .../rmcs_msgs/include/rmcs_msgs/robot_id.hpp | 12 +- .../include/rmcs_msgs/sentry_event.hpp | 27 +++++ .../include/rmcs_utility/csv_writer.hpp | 106 ++++++++++++++++++ .../rmcs_utility/rclcpp/node_mixin.hpp | 54 +++++++++ .../include/rmcs_utility/ring_buffer.hpp | 39 ++++++- 7 files changed, 257 insertions(+), 10 deletions(-) create mode 100644 rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/sentry_event.hpp create mode 100644 rmcs_ws/src/rmcs_utility/include/rmcs_utility/csv_writer.hpp create mode 100644 rmcs_ws/src/rmcs_utility/include/rmcs_utility/rclcpp/node_mixin.hpp diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp index 92d391b59..a279b92c7 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp @@ -5,10 +5,22 @@ namespace rmcs_msgs { enum class ChassisMode : uint8_t { - AUTO = 0, - SPIN = 1, - STEP_DOWN = 2, - LAUNCH_RAMP = 3, + AUTO, + SPIN_SLOW, + SPIN_FAST, + STEP_DOWN, + LAUNCH_RAMP, + ALIGNMENT, + ALIGNMENT_POWERED, + CLIMB, }; -} // namespace rmcs_msgs \ No newline at end of file +constexpr auto is_powered(ChassisMode mode) noexcept { + return mode == ChassisMode::ALIGNMENT_POWERED || mode == ChassisMode::LAUNCH_RAMP + || mode == ChassisMode::CLIMB; +} +constexpr auto is_spining(ChassisMode mode) noexcept { + return mode == ChassisMode::SPIN_SLOW || mode == ChassisMode::SPIN_FAST; +} + +} // namespace rmcs_msgs diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp index b2ec7c371..1cccd175a 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp @@ -20,6 +20,7 @@ #include "mouse.hpp" // IWYU pragma: export #include "robot_color.hpp" // IWYU pragma: export #include "robot_id.hpp" // IWYU pragma: export +#include "sentry_event.hpp" // IWYU pragma: export #include "serial_interface.hpp" // IWYU pragma: export #include "shoot_mode.hpp" // IWYU pragma: export #include "shoot_status.hpp" // IWYU pragma: export @@ -43,9 +44,13 @@ constexpr auto to_string(GameStage stage) noexcept -> const char* { constexpr auto to_string(ChassisMode mode) noexcept -> const char* { switch (mode) { case ChassisMode::AUTO: return "AUTO"; - case ChassisMode::SPIN: return "SPIN"; + case ChassisMode::SPIN_FAST: return "SPIN_FAST"; case ChassisMode::STEP_DOWN: return "STEP_DOWN"; case ChassisMode::LAUNCH_RAMP: return "LAUNCH_RAMP"; + case ChassisMode::SPIN_SLOW: return "SPIN_SLOW"; + case ChassisMode::ALIGNMENT: return "ALIGNMENT"; + case ChassisMode::ALIGNMENT_POWERED: return "ALIGNMENT_POWERED"; + case ChassisMode::CLIMB: return "CLIMB"; } return "INVALID"; } diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_id.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_id.hpp index 61e063e2a..2c82568ba 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_id.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_id.hpp @@ -67,11 +67,17 @@ class RobotId { constexpr bool operator==(const Value value) const { return value_ == value; } constexpr bool operator!=(const Value value) const { return value_ != value; } - constexpr RobotColor color() const { + constexpr RobotColor color() const noexcept { + if (value_ == Value::UNKNOWN) { + return RobotColor::UNKNOWN; + } return value_ & 0x40 ? RobotColor::BLUE : RobotColor::RED; } - constexpr ArmorID id() const { + constexpr ArmorID id() const noexcept { + if (value_ == Value::UNKNOWN) { + return ArmorID::Unknown; + } return value_ > 100 ? static_cast(value_ - 100) : static_cast(value_); } @@ -79,4 +85,4 @@ class RobotId { Value value_; }; -} // namespace rmcs_msgs \ No newline at end of file +} // namespace rmcs_msgs diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/sentry_event.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/sentry_event.hpp new file mode 100644 index 000000000..be171924c --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/sentry_event.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include + +namespace rmcs_msgs { + +enum class SentryEvent : std::uint8_t { + SWITCH_POSE_ATTACK, + SWITCH_POSE_DEFENSE, + SWITCH_POSE_MOVE, + SWITCH_POSE_POWERED_ATTACK, + SWITCH_POSE_POWERED_DEFENSE, + SWITCH_POSE_POWERED_MOVE, + + CONFIRM_REBIRTH, + CONFIRM_INSTANT_REBIRTH, + + EXCHANGE_AMMO_SUPPLY_POINT, + EXCHANGE_AMMO_REMOTE, + EXCHANGE_HP_REMOTE, + + ACTIVATE_ENERGY_CORE, + + COUNT, +}; + +} // namespace rmcs_msgs diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/csv_writer.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/csv_writer.hpp new file mode 100644 index 000000000..137488989 --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/csv_writer.hpp @@ -0,0 +1,106 @@ +#pragma once + +#include +#include +#include +#include +#include + +namespace rmcs_utility { + +class CsvWriter { +public: + CsvWriter() = default; + explicit CsvWriter(const std::filesystem::path& path) { open(path); } + + CsvWriter(const CsvWriter&) = delete; + CsvWriter& operator=(const CsvWriter&) = delete; + CsvWriter(CsvWriter&&) = delete; + CsvWriter& operator=(CsvWriter&&) = delete; + + ~CsvWriter() { close(); } + + void open(const std::filesystem::path& path) { + close(); + + if (!path.parent_path().empty()) + std::filesystem::create_directories(path.parent_path()); + + stream_.open(path, std::ios::out | std::ios::trunc); + if (!stream_.is_open()) + throw std::runtime_error("Failed to open csv file: " + path.string()); + + path_ = path; + } + + [[nodiscard]] bool is_open() const { return stream_.is_open(); } + + const std::filesystem::path& path() const { return path_; } + + void flush() { + if (stream_.is_open()) + stream_.flush(); + } + + void close() { + if (stream_.is_open()) { + stream_.flush(); + stream_.close(); + } + path_.clear(); + } + + template + void write_row(const Values&... values) { + ensure_open(); + + bool first = true; + (write_field(first, values), ...); + stream_ << '\n'; + } + +private: + void ensure_open() const { + if (!stream_.is_open()) + throw std::runtime_error("CsvWriter is not open"); + } + + template + void write_field(bool& first, const T& value) { + if (!first) + stream_ << ','; + first = false; + write_value(value); + } + + void write_value(std::string_view value) { + if (value.find_first_of(",\"\r\n") == std::string_view::npos) { + stream_ << value; + return; + } + + stream_ << '"'; + for (const char ch : value) { + if (ch == '"') + stream_ << "\"\""; + else + stream_ << ch; + } + stream_ << '"'; + } + + void write_value(const std::string& value) { write_value(std::string_view{value}); } + void write_value(const char* value) { + write_value(value == nullptr ? std::string_view{} : std::string_view{value}); + } + + template + void write_value(const T& value) { + stream_ << value; + } + + std::ofstream stream_; + std::filesystem::path path_; +}; + +} // namespace rmcs_utility \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/rclcpp/node_mixin.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/rclcpp/node_mixin.hpp new file mode 100644 index 000000000..24eb14d19 --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/rclcpp/node_mixin.hpp @@ -0,0 +1,54 @@ +#pragma once +#include +#include + +namespace rmcs_utility { + +struct NodeMixin { + using node = NodeMixin; + + static constexpr auto options() noexcept { + return rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true); + } + + template + auto info(this const Self& self, std::format_string fmt, Args&&... args) -> void { + auto text = std::format(fmt, std::forward(args)...); + RCLCPP_INFO(self.get_logger(), "%s", text.c_str()); + } + + template + auto warn(this const Self& self, std::format_string fmt, Args&&... args) -> void { + auto text = std::format(fmt, std::forward(args)...); + RCLCPP_WARN(self.get_logger(), "%s", text.c_str()); + } + + template + auto error(this const Self& self, std::format_string fmt, Args&&... args) -> void { + auto text = std::format(fmt, std::forward(args)...); + RCLCPP_ERROR(self.get_logger(), "%s", text.c_str()); + } + + template + auto param(this const auto& self, const std::string& name, T& dst) { + if (self.has_parameter(name)) { + dst = self.template get_parameter_or(name, T{}); + return; + } + + self.error("param [ {} ] for {} is needed", name, self.get_name()); + throw std::runtime_error{"lack of param"}; + } + template + auto param_or(this const auto& self, const std::string& name, T1& dst, const T2& fallback) + requires std::convertible_to { + dst = self.template get_parameter_or(name, fallback); + } + + template + auto param_or(this const auto& self, const std::string& name, const T& fallback) -> T { + return self.template get_parameter_or(name, fallback); + } +}; + +} // namespace rmcs_utility diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/ring_buffer.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/ring_buffer.hpp index d6c5ca5db..2b30b1f8b 100644 --- a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/ring_buffer.hpp +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/ring_buffer.hpp @@ -291,6 +291,43 @@ class RingBuffer { return std::launder(reinterpret_cast(storage_[out & mask_].data)); } + /*! + * @brief Visit elements from the head without consuming them + * @tparam F Functor with signature `void(T&)` + * @param callback_functor Invoked for each readable element in FIFO order + * @param count Maximum number of elements to inspect (defaults to all available) + * @return Number of elements actually visited + * @note Consumer-only. Does not advance `out_` or destroy elements. + * Mutations performed through the callback are applied in place to the + * buffered elements. + */ + template + requires requires(F& f, T& t) { + { f(t) } noexcept; + } size_t peek_front_n(F callback_functor, size_t count = std::numeric_limits::max()) { + const auto in = in_.load(std::memory_order::acquire); + const auto out = out_.load(std::memory_order::relaxed); + + const auto readable = in - out; + count = std::min(count, readable); + if (!count) + return 0; + + const auto offset = out & mask_; + const auto slice = std::min(count, max_size() - offset); + + auto process = [&callback_functor](std::byte* storage) { + auto& element = *std::launder(reinterpret_cast(storage)); + callback_functor(element); + }; + for (size_t i = 0; i < slice; i++) + process(storage_[offset + i].data); + for (size_t i = 0; i < count - slice; i++) + process(storage_[i].data); + + return count; + } + /*! * @brief Peek the last produced element (consumer side) * @return Pointer to the last element, or nullptr if empty @@ -493,4 +530,4 @@ class RingBuffer { std::atomic in_{0}, out_{0}; }; -} // namespace rmcs_utility +} // namespace rmcs_utility \ No newline at end of file From 6f94a4914a6b92abf8bc70ff74274d2520c0a17e Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Mon, 10 Aug 2026 18:06:47 +0800 Subject: [PATCH 03/86] feat(hardware): Add VT13 remote control and update device drivers --- .../src/hardware/device/dji_motor.hpp | 53 ++- .../rmcs_core/src/hardware/device/dr16.hpp | 110 +++--- .../src/hardware/device/lk_motor.hpp | 20 +- .../src/hardware/device/remote_control.hpp | 177 +++++++++ .../rmcs_core/src/hardware/device/vt13.hpp | 344 ++++++++++++++++++ .../src/hardware/util/status_monitor.hpp | 42 +++ 6 files changed, 679 insertions(+), 67 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/util/status_monitor.hpp diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp index d1c56cbfd..1bc1a17ea 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp @@ -24,8 +24,9 @@ class DjiMotor { enum class Type : uint8_t { kGM6020, kGM6020Voltage, kM3508, kM2006 }; struct Config { - explicit Config(Type motor_type) - : motor_type(motor_type) { + explicit Config(Type motor_type, std::uint8_t id = 0) + : motor_type(motor_type) + , id(id) { switch (motor_type) { case Type::kGM6020: case Type::kGM6020Voltage: reduction_ratio = 1.0; break; @@ -42,6 +43,7 @@ class DjiMotor { Config& enable_multi_turn_angle() { return multi_turn_angle_enabled = true, *this; } Type motor_type; + std::uint8_t id; int encoder_zero_point = 0; double reduction_ratio; bool reversed; @@ -77,6 +79,8 @@ class DjiMotor { ~DjiMotor() = default; void configure(const Config& config) { + type_ = config.motor_type; + id_ = config.id; encoder_zero_point_ = config.encoder_zero_point % kRawAngleMax; if (encoder_zero_point_ < 0) encoder_zero_point_ += kRawAngleMax; @@ -135,6 +139,37 @@ class DjiMotor { can_data_.store(CanPacket8{can_data}, std::memory_order_relaxed); } + static constexpr auto recv_id(Type type, std::uint8_t index) -> std::uint32_t { + switch (type) { + case Type::kGM6020: + case Type::kGM6020Voltage: return 0x204 + index; + case Type::kM3508: + case Type::kM2006: return 0x200 + index; + } + return 0; + } + + static constexpr auto send_id(Type type, std::uint8_t index) -> std::uint32_t { + switch (type) { + case Type::kGM6020: return index <= 4 ? 0x1FE : 0x2FE; + case Type::kGM6020Voltage: return index <= 4 ? 0x1FF : 0x2FF; + case Type::kM3508: + case Type::kM2006: return index <= 4 ? 0x200 : 0x1FF; + } + return 0; + } + + auto id() const noexcept -> std::uint8_t { return id_; } + auto recv_id() const noexcept -> std::uint32_t { return recv_id(type_, id_); } + auto send_id() const noexcept -> std::uint32_t { return send_id(type_, id_); } + + bool match_then_store_status(std::uint32_t can_id, std::span can_data) { + if (can_id != recv_id()) + return false; + store_status(can_data); + return true; + } + void update_status() { const auto feedback = std::bit_cast(can_data_.load(std::memory_order::relaxed)); @@ -174,7 +209,7 @@ class DjiMotor { } double control_torque() const { - if (control_torque_.ready()) [[likely]] + if (control_torque_.ready() && id_ != 0) [[likely]] return *control_torque_; else return 0.0; @@ -217,6 +252,8 @@ class DjiMotor { uint8_t unused; }; + Type type_ = Type::kM3508; + std::uint8_t id_ = 0; std::atomic can_data_; static constexpr int kRawAngleMax = 8192; @@ -244,3 +281,13 @@ class DjiMotor { }; } // namespace rmcs_core::hardware::device + +namespace rmcs_core::hardware::device { + +inline auto operator<<(CanPacket8& packet, const DjiMotor& motor) -> CanPacket8& { + if (motor.id() != 0) + packet.data[(motor.id() - 1) % 4] = motor.generate_command().data; + return packet; +} + +} // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp index 75152b179..51895755b 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp @@ -6,11 +6,9 @@ #include #include +#include #include -#include -#include -#include #include #include #include @@ -19,32 +17,7 @@ namespace rmcs_core::hardware::device { class Dr16 { public: - explicit Dr16(rmcs_executor::Component& component) { - component.register_output( - "/remote/joystick/right", joystick_right_output_, Eigen::Vector2d::Zero()); - component.register_output( - "/remote/joystick/left", joystick_left_output_, Eigen::Vector2d::Zero()); - - component.register_output( - "/remote/switch/right", switch_right_output_, rmcs_msgs::Switch::UNKNOWN); - component.register_output( - "/remote/switch/left", switch_left_output_, rmcs_msgs::Switch::UNKNOWN); - - component.register_output( - "/remote/mouse/velocity", mouse_velocity_output_, Eigen::Vector2d::Zero()); - component.register_output("/remote/mouse/mouse_wheel", mouse_wheel_output_); - - component.register_output("/remote/mouse", mouse_output_); - std::memset(&*mouse_output_, 0, sizeof(*mouse_output_)); - component.register_output("/remote/keyboard", keyboard_output_); - std::memset(&*keyboard_output_, 0, sizeof(*keyboard_output_)); - - component.register_output("/remote/rotary_knob", rotary_knob_output_); - - // Simulate the rotary knob as a switch, with anti-shake algorithm. - component.register_output( - "/remote/rotary_knob_switch", rotary_knob_switch_output_, rmcs_msgs::Switch::UNKNOWN); - } + Dr16() = default; void store_status(const std::byte* uart_data, size_t uart_data_length) { if (uart_data_length != 6 + 8 + 4) @@ -73,9 +46,17 @@ class Dr16 { std::memcpy(&part3, uart_data, 4); uart_data += 4; data_part3_.store(part3, std::memory_order::relaxed); + + last_remote_control_received_at_ = Clock::now(); + valid_ = true; } void update_status() { + const auto now = Clock::now(); + refresh_validity(now); + if (!valid_) + return; + auto part1 alignas(uint64_t) = std::bit_cast(data_part1_.load(std::memory_order::relaxed)); @@ -110,19 +91,6 @@ class Dr16 { keyboard_ = part3.keyboard; rotary_knob_ = channel_to_double(part3.rotary_knob); - *joystick_right_output_ = joystick_right(); - *joystick_left_output_ = joystick_left(); - - *switch_right_output_ = switch_right(); - *switch_left_output_ = switch_left(); - - *mouse_velocity_output_ = mouse_velocity(); - *mouse_wheel_output_ = mouse_wheel(); - - *mouse_output_ = mouse(); - *keyboard_output_ = keyboard(); - - *rotary_knob_output_ = rotary_knob(); update_rotary_knob_switch(); } @@ -182,6 +150,12 @@ class Dr16 { rmcs_msgs::Mouse mouse() const { return std::bit_cast(mouse_); } rmcs_msgs::Keyboard keyboard() const { return std::bit_cast(keyboard_); } + rmcs_msgs::Switch rotary_knob_switch() const { return rotary_knob_switch_; } + + bool valid() const noexcept { return valid_; } + + void set_timeout_enabled(bool enabled) { timeout_enabled_ = enabled; } + double rotary_knob() const { return rotary_knob_; } double mouse_wheel() const { return mouse_wheel_; } @@ -193,7 +167,7 @@ class Dr16 { constexpr double divider = 0.7, anti_shake_shift = 0.05; double upper_divider = divider, lower_divider = -divider; - auto& switch_value = *rotary_knob_switch_output_; + auto switch_value = rotary_knob_switch_; if (switch_value == rmcs_msgs::Switch::UP) upper_divider -= anti_shake_shift, lower_divider -= anti_shake_shift; else if (switch_value == rmcs_msgs::Switch::MIDDLE) @@ -201,7 +175,7 @@ class Dr16 { else if (switch_value == rmcs_msgs::Switch::DOWN) upper_divider += anti_shake_shift, lower_divider += anti_shake_shift; - const auto knob_value = -*rotary_knob_output_; + const auto knob_value = -rotary_knob_; if (knob_value > upper_divider) { switch_value = rmcs_msgs::Switch::UP; } else if (knob_value < lower_divider) { @@ -209,6 +183,33 @@ class Dr16 { } else { switch_value = rmcs_msgs::Switch::MIDDLE; } + rotary_knob_switch_ = switch_value; + } + + using Clock = std::chrono::steady_clock; + using TimePoint = Clock::time_point; + + static constexpr auto kFreshTimeout = std::chrono::milliseconds(500); + + void refresh_validity(const TimePoint now) { + if (!timeout_enabled_ || !valid_ || now - last_remote_control_received_at_ <= kFreshTimeout) + return; + + reset_remote_control_state(); + valid_ = false; + } + + void reset_remote_control_state() { + joystick_right_ = Vector::zero(); + joystick_left_ = Vector::zero(); + switch_right_ = Switch::kUnknown; + switch_left_ = Switch::kUnknown; + mouse_velocity_ = Vector::zero(); + mouse_wheel_ = 0.0; + mouse_ = Mouse::zero(); + keyboard_ = Keyboard::zero(); + rotary_knob_ = 0.0; + rotary_knob_switch_ = rmcs_msgs::Switch::UNKNOWN; } struct [[gnu::packed]] Dr16DataPart1 { @@ -270,27 +271,16 @@ class Dr16 { Switch switch_left_ = Switch::kUnknown; Vector mouse_velocity_ = Vector::zero(); + double mouse_wheel_ = 0.0; Mouse mouse_ = Mouse::zero(); Keyboard keyboard_ = Keyboard::zero(); double rotary_knob_ = 0.0; - double mouse_wheel_ = 0.0; - - rmcs_executor::Component::OutputInterface joystick_right_output_; - rmcs_executor::Component::OutputInterface joystick_left_output_; - - rmcs_executor::Component::OutputInterface switch_right_output_; - rmcs_executor::Component::OutputInterface switch_left_output_; - - rmcs_executor::Component::OutputInterface mouse_velocity_output_; - rmcs_executor::Component::OutputInterface mouse_wheel_output_; - - rmcs_executor::Component::OutputInterface mouse_output_; - rmcs_executor::Component::OutputInterface keyboard_output_; - - rmcs_executor::Component::OutputInterface rotary_knob_output_; - rmcs_executor::Component::OutputInterface rotary_knob_switch_output_; + rmcs_msgs::Switch rotary_knob_switch_ = rmcs_msgs::Switch::UNKNOWN; + TimePoint last_remote_control_received_at_ = TimePoint::min(); + bool valid_ = false; + bool timeout_enabled_ = true; }; } // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp index 60348bb69..1e4b7c01a 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp @@ -487,10 +487,22 @@ class LkMotor { int32_t to_absolute_command_angle(double angle) const { angle = angle_to_command_angle_coefficient_ * angle; const auto one_turn = std::abs(angle_to_command_angle_coefficient_) * 2 * std::numbers::pi; - // TODO: The offset should be N turns (calculated from the motor's reported multi-turn angle - // vs encoder position at startup), not hardcoded to 1 turn. - angle -= one_turn; - angle += one_turn * static_cast(encoder_zero_point_) / raw_angle_modulus_; + const auto encoder_offset = + one_turn * static_cast(encoder_zero_point_) / raw_angle_modulus_; + + angle += encoder_offset; + if (multi_turn_angle_enabled_) { + // Map to the equivalent target turn nearest to current motor position. + const auto current_command_angle = + angle_to_command_angle_coefficient_ * angle_ + encoder_offset; + if (one_turn > 0.0) + angle += one_turn * std::round((current_command_angle - angle) / one_turn); + } else { + // Preserve historical single-turn command mapping. + angle -= one_turn; + } + // hero encoder 专用，不能改 + angle = std::round( std::clamp( angle, std::numeric_limits::min(), std::numeric_limits::max())); diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp new file mode 100644 index 000000000..689436bc6 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp @@ -0,0 +1,177 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include "hardware/device/dr16.hpp" +#include "hardware/device/vt13.hpp" + +namespace rmcs_core::hardware::device { + +/* +遥控输入仲裁： +- vt13 valid S挡：vt13主控 | 比赛用 +- vt13 valid C挡：等同于dr16双下 | 疯车救车 +- 其他情况：dr16主控；dr16无效则进入空安全态 +- 旋钮始终来自 dr16，dr16 无效则清零 +*/ + +class RemoteControl { +public: + explicit RemoteControl(rmcs_executor::Component& component) { + component.register_output( + "/remote/joystick/right", joystick_right_output_, Eigen::Vector2d::Zero()); + component.register_output( + "/remote/joystick/left", joystick_left_output_, Eigen::Vector2d::Zero()); + + component.register_output( + "/remote/switch/right", switch_right_output_, rmcs_msgs::Switch::UNKNOWN); + component.register_output( + "/remote/switch/left", switch_left_output_, rmcs_msgs::Switch::UNKNOWN); + + component.register_output("/remote/rotary_knob", rotary_knob_output_, 0.0); + component.register_output( + "/remote/rotary_knob_switch", rotary_knob_switch_output_, rmcs_msgs::Switch::UNKNOWN); + + component.register_output( + "/remote/mouse/velocity", mouse_velocity_output_, Eigen::Vector2d::Zero()); + component.register_output("/remote/mouse/mouse_wheel", mouse_wheel_output_, 0.0); + + component.register_output("/remote/mouse", mouse_output_, rmcs_msgs::Mouse::zero()); + component.register_output( + "/remote/keyboard", keyboard_output_, rmcs_msgs::Keyboard::zero()); + } + + void register_dr16(Dr16* dr16) { dr16_ = dr16; } + void register_vt13(Vt13* vt13) { vt13_ = vt13; } + + void update() { + update_timeout_interlock(); + + const auto control_source = select_control_source(); + const auto snapshot = build_snapshot(control_source); + + *joystick_right_output_ = snapshot.joystick_right; + *joystick_left_output_ = snapshot.joystick_left; + + *switch_right_output_ = snapshot.switch_right; + *switch_left_output_ = snapshot.switch_left; + + *mouse_velocity_output_ = snapshot.mouse_velocity; + *mouse_wheel_output_ = snapshot.mouse_wheel; + + *mouse_output_ = snapshot.mouse; + *keyboard_output_ = snapshot.keyboard; + + if (dr16_ && dr16_->valid()) { + *rotary_knob_output_ = dr16_->rotary_knob(); + *rotary_knob_switch_output_ = dr16_->rotary_knob_switch(); + } else { + *rotary_knob_output_ = 0.0; + *rotary_knob_switch_output_ = rmcs_msgs::Switch::UNKNOWN; + } + } + +private: + enum class ControlSource { + kDr16, + kVt13Sport, + kCineSafe, + kInvalidSafe, + }; + + struct Snapshot { + Eigen::Vector2d joystick_right = Eigen::Vector2d::Zero(); + Eigen::Vector2d joystick_left = Eigen::Vector2d::Zero(); + + rmcs_msgs::Switch switch_right = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Switch switch_left = rmcs_msgs::Switch::UNKNOWN; + + Eigen::Vector2d mouse_velocity = Eigen::Vector2d::Zero(); + double mouse_wheel = 0.0; + + rmcs_msgs::Mouse mouse = rmcs_msgs::Mouse::zero(); + rmcs_msgs::Keyboard keyboard = rmcs_msgs::Keyboard::zero(); + }; + + // 超时互锁：仅当对方 valid 时本设备才允许超时失效，保证至少一路不失效 + auto update_timeout_interlock() const -> void { + const auto dr16_ok = dr16_ && dr16_->valid(); + const auto vt13_ok = vt13_ && vt13_->valid(); + if (dr16_) + dr16_->set_timeout_enabled(vt13_ok); + if (vt13_) + vt13_->set_timeout_enabled(dr16_ok); + } + + ControlSource select_control_source() const { + if (vt13_ && vt13_->valid()) { + switch (vt13_->mode_switch()) { + case Vt13::ModeSwitch::kSport: return ControlSource::kVt13Sport; + case Vt13::ModeSwitch::kCine: return ControlSource::kCineSafe; + case Vt13::ModeSwitch::kNormal: + case Vt13::ModeSwitch::kUnknown: break; + } + } + + return (dr16_ && dr16_->valid()) ? ControlSource::kDr16 : ControlSource::kInvalidSafe; + } + + Snapshot build_snapshot(ControlSource source) const { + Snapshot snapshot{}; + switch (source) { + case ControlSource::kDr16: + snapshot.joystick_right = dr16_->joystick_right(); + snapshot.joystick_left = dr16_->joystick_left(); + snapshot.switch_right = dr16_->switch_right(); + snapshot.switch_left = dr16_->switch_left(); + snapshot.mouse_velocity = dr16_->mouse_velocity(); + snapshot.mouse_wheel = dr16_->mouse_wheel(); + snapshot.mouse = dr16_->mouse(); + snapshot.keyboard = dr16_->keyboard(); + break; + case ControlSource::kVt13Sport: + snapshot.joystick_right = vt13_->joystick_right(); + snapshot.joystick_left = vt13_->joystick_left(); + snapshot.switch_right = rmcs_msgs::Switch::MIDDLE; + snapshot.switch_left = rmcs_msgs::Switch::MIDDLE; + snapshot.mouse_velocity = vt13_->mouse_velocity(); + snapshot.mouse_wheel = vt13_->mouse_wheel(); + snapshot.mouse = vt13_->mouse(); + snapshot.keyboard = vt13_->keyboard(); + break; + case ControlSource::kCineSafe: + snapshot.switch_right = rmcs_msgs::Switch::DOWN; + snapshot.switch_left = rmcs_msgs::Switch::DOWN; + break; + case ControlSource::kInvalidSafe: break; + } + + return snapshot; + } + + Dr16* dr16_{nullptr}; + Vt13* vt13_{nullptr}; + + rmcs_executor::Component::OutputInterface joystick_right_output_; + rmcs_executor::Component::OutputInterface joystick_left_output_; + + rmcs_executor::Component::OutputInterface switch_right_output_; + rmcs_executor::Component::OutputInterface switch_left_output_; + + rmcs_executor::Component::OutputInterface rotary_knob_output_; + rmcs_executor::Component::OutputInterface rotary_knob_switch_output_; + + rmcs_executor::Component::OutputInterface mouse_velocity_output_; + rmcs_executor::Component::OutputInterface mouse_wheel_output_; + + rmcs_executor::Component::OutputInterface mouse_output_; + rmcs_executor::Component::OutputInterface keyboard_output_; +}; + +} // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp new file mode 100644 index 000000000..327999820 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp @@ -0,0 +1,344 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware::device { + +class Vt13 { +public: + enum class ModeSwitch : uint8_t { + kUnknown = 0, + kCine = 1, + kNormal = 2, + kSport = 3, + }; + + Vt13() = default; + + void store_status(std::span uart_data) { + store_calls_.fetch_add(1, std::memory_order_relaxed); + received_bytes_.fetch_add(uart_data.size(), std::memory_order_relaxed); + + const auto written = data_buffer_.emplace_back_n( + [iter = uart_data.cbegin()](std::byte* storage) mutable noexcept { + *storage = *iter++; + }, + uart_data.size()); + if (written != uart_data.size()) { + const auto dropped = uart_data.size() - written; + overflow_count_.fetch_add(1, std::memory_order_relaxed); + overflow_dropped_bytes_.fetch_add(dropped, std::memory_order_relaxed); + if (should_log_overflow()) { + RCLCPP_WARN( + logger_, "VT13 input buffer overflow: dropped %zu of %zu bytes", dropped, + uart_data.size()); + } + } + } + + void update_status() { + const auto now = Clock::now(); + auto readable = data_buffer_.readable(); + peak_readable_ = std::max(peak_readable_, readable); + + while (readable) { + ReadResult result = VerificationFailed{}; + + const std::byte front = *data_buffer_.peek_front(); + if (front == std::byte{0xa9}) + result = read_remote_control_data(readable, now); + else if (front == std::byte(0xa5)) + result = read_referee_style_data(readable, now); + else { + unknown_prefix_count_++; + if (should_log_verification_failure(now)) { + RCLCPP_WARN( + logger_, "VT13 unknown prefix: front=0x%02x readable=%zu", + std::to_integer(front), readable); + } + } + + if (std::holds_alternative(result)) { + break; + } + if (std::holds_alternative(result)) { + verification_failures_++; + data_buffer_.pop_front([](std::byte&&) noexcept {}); + readable--; + continue; + } + if (std::holds_alternative(result)) { + readable -= std::get(result).read; + continue; + } + } + + refresh_validity(now); + } + + ModeSwitch mode_switch() const noexcept { return mode_switch_; } + bool valid() const noexcept { return valid_; } + + void set_timeout_enabled(bool enabled) { timeout_enabled_ = enabled; } + + const Eigen::Vector2d& joystick_left() const noexcept { return joystick_left_; } + const Eigen::Vector2d& joystick_right() const noexcept { return joystick_right_; } + + const Eigen::Vector2d& mouse_velocity() const noexcept { return mouse_velocity_; } + double mouse_wheel() const noexcept { return mouse_wheel_; } + + rmcs_msgs::Mouse mouse() const noexcept { return mouse_; } + rmcs_msgs::Keyboard keyboard() const noexcept { return keyboard_; } + +private: + using Clock = std::chrono::steady_clock; + using TimePoint = Clock::time_point; + + static constexpr auto kFreshTimeout = std::chrono::milliseconds(500); + static constexpr auto kVerificationLogInterval = std::chrono::seconds(1); + static constexpr auto kOverflowLogInterval = std::chrono::seconds(1); + static constexpr auto kStatisticsLogInterval = std::chrono::seconds(5); + static constexpr std::size_t kRefereeFrameMaxSize = 256; + + struct Incomplete {}; + struct VerificationFailed {}; + struct Success { + std::size_t read; + }; + using ReadResult = std::variant; + + struct [[gnu::packed]] RemoteControlData { + static constexpr uint16_t kHeaderMagic = 0x53a9; + + uint16_t header; + + uint16_t joystick_channel0 : 11; + uint16_t joystick_channel1 : 11; + uint16_t joystick_channel2 : 11; + uint16_t joystick_channel3 : 11; + + uint8_t mode_switch : 2; + uint8_t pause_button : 1; + uint8_t left_custom_button : 1; + uint8_t right_custom_button : 1; + uint16_t dial : 11; + uint8_t trigger : 1; + uint8_t padding1 : 3; + + int16_t mouse_velocity_x; + int16_t mouse_velocity_y; + int16_t mouse_velocity_z; + uint8_t mouse_left : 2; + uint8_t mouse_right : 2; + uint8_t mouse_middle : 2; + uint8_t padding2 : 2; + + uint16_t keyboard; + + uint16_t crc16; + }; + + struct [[gnu::packed]] RefereeFrameHeader { + uint8_t sof; + uint16_t data_length; + uint8_t seq; + uint8_t crc8; + }; + + ReadResult read_remote_control_data(const std::size_t readable, const TimePoint now) { + if (readable < sizeof(RemoteControlData)) + return Incomplete{}; + + RemoteControlData data; + data_buffer_.peek_front_n( + [dst = reinterpret_cast(&data)](std::byte src) mutable noexcept { + *dst++ = src; + }, + sizeof(RemoteControlData)); + + if (data.header != RemoteControlData::kHeaderMagic) { + remote_bad_header_count_++; + if (should_log_verification_failure(now)) { + RCLCPP_WARN( + logger_, "VT13 remote control header invalid: header=0x%04x readable=%zu", + data.header, readable); + } + return VerificationFailed{}; + } + if (!rmcs_utility::dji_crc::verify_crc16(data)) { + remote_bad_crc_count_++; + if (should_log_verification_failure(now)) + RCLCPP_WARN(logger_, "VT13 remote control crc16 invalid: readable=%zu", readable); + return VerificationFailed{}; + } + + data_buffer_.pop_front_n([](std::byte&&) noexcept {}, sizeof(RemoteControlData)); + + update_remote_control_data(data); + valid_ = true; + last_remote_control_received_at_ = now; + remote_success_count_++; + return Success{sizeof(RemoteControlData)}; + } + + void update_remote_control_data(const RemoteControlData& data) { + mode_switch_ = static_cast(data.mode_switch + 1); + + joystick_right_ = { + channel_to_double(static_cast(data.joystick_channel1)), + -channel_to_double(static_cast(data.joystick_channel0)), + }; + joystick_left_ = { + channel_to_double(static_cast(data.joystick_channel2)), + -channel_to_double(static_cast(data.joystick_channel3)), + }; + + mouse_velocity_ = { + -data.mouse_velocity_y / 32768.0, + -data.mouse_velocity_x / 32768.0, + }; + mouse_wheel_ = -static_cast(data.mouse_velocity_z) / 32768.0; + + mouse_ = { + .left = static_cast(data.mouse_left), + .right = static_cast(data.mouse_right), + }; + keyboard_ = std::bit_cast(data.keyboard); + } + + ReadResult read_referee_style_data(const std::size_t readable, const TimePoint now) { + if (readable < sizeof(RefereeFrameHeader)) + return Incomplete{}; + + RefereeFrameHeader header; + data_buffer_.peek_front_n( + [dst = reinterpret_cast(&header)](std::byte src) mutable noexcept { + *dst++ = src; + }, + sizeof(RefereeFrameHeader)); + + if (!rmcs_utility::dji_crc::verify_crc8(header)) { + referee_bad_crc8_count_++; + if (should_log_verification_failure(now)) + RCLCPP_WARN(logger_, "VT13 referee header crc8 invalid: readable=%zu", readable); + return VerificationFailed{}; + } + + const std::size_t total_frame_size = + sizeof(RefereeFrameHeader) + 2 + header.data_length + 2; + if (total_frame_size > kRefereeFrameMaxSize) { + referee_oversize_count_++; + if (should_log_verification_failure(now)) { + RCLCPP_WARN( + logger_, "VT13 referee frame oversized: data_length=%u total=%zu readable=%zu", + header.data_length, total_frame_size, readable); + } + return VerificationFailed{}; + } + if (readable < total_frame_size) + return Incomplete{}; + + data_buffer_.pop_front_n([](std::byte&&) noexcept {}, total_frame_size); + referee_discarded_count_++; + return Success{total_frame_size}; + } + + bool should_log_verification_failure(const TimePoint now) { + if (last_verification_log_time_ != TimePoint::min() + && now - last_verification_log_time_ < kVerificationLogInterval) + return false; + last_verification_log_time_ = now; + return true; + } + + bool should_log_overflow() { + const auto now = Clock::now(); + if (last_overflow_log_time_ != TimePoint::min() + && now - last_overflow_log_time_ < kOverflowLogInterval) + return false; + + last_overflow_log_time_ = now; + return true; + } + + void refresh_validity(const TimePoint now) { + if (!timeout_enabled_ || !valid_ || now - last_remote_control_received_at_ <= kFreshTimeout) + return; + + reset_remote_control_state(); + valid_ = false; + } + + void reset_remote_control_state() { + mode_switch_ = ModeSwitch::kUnknown; + joystick_left_ = Eigen::Vector2d::Zero(); + joystick_right_ = Eigen::Vector2d::Zero(); + mouse_velocity_ = Eigen::Vector2d::Zero(); + mouse_wheel_ = 0; + mouse_ = rmcs_msgs::Mouse::zero(); + keyboard_ = rmcs_msgs::Keyboard::zero(); + } + + static double channel_to_double(int32_t value) { + value -= 1024; + if (-660 <= value && value <= 660) + return value / 660.0; + return 0.0; + } + + rclcpp::Logger logger_ = rclcpp::get_logger("vt13"); + rmcs_utility::RingBuffer data_buffer_{1024}; + + std::atomic store_calls_{0}; + std::atomic received_bytes_{0}; + std::atomic overflow_count_{0}; + std::atomic overflow_dropped_bytes_{0}; + + TimePoint last_remote_control_received_at_ = TimePoint::min(); + TimePoint last_verification_log_time_ = TimePoint::min(); + TimePoint last_overflow_log_time_ = TimePoint::min(); + TimePoint last_statistics_log_time_ = TimePoint::min(); + + bool valid_ = false; + bool timeout_enabled_ = true; + std::size_t peak_readable_ = 0; + std::size_t remote_success_count_ = 0; + std::size_t verification_failures_ = 0; + std::size_t remote_bad_header_count_ = 0; + std::size_t remote_bad_crc_count_ = 0; + std::size_t referee_discarded_count_ = 0; + std::size_t referee_bad_crc8_count_ = 0; + std::size_t referee_oversize_count_ = 0; + std::size_t unknown_prefix_count_ = 0; + + ModeSwitch mode_switch_ = ModeSwitch::kUnknown; + + Eigen::Vector2d joystick_left_ = Eigen::Vector2d::Zero(); + Eigen::Vector2d joystick_right_ = Eigen::Vector2d::Zero(); + + Eigen::Vector2d mouse_velocity_ = Eigen::Vector2d::Zero(); + double mouse_wheel_ = 0; + + rmcs_msgs::Mouse mouse_ = rmcs_msgs::Mouse::zero(); + rmcs_msgs::Keyboard keyboard_ = rmcs_msgs::Keyboard::zero(); +}; + +} // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/util/status_monitor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/util/status_monitor.hpp new file mode 100644 index 000000000..ff2b4c0cd --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/util/status_monitor.hpp @@ -0,0 +1,42 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware { + +class StatusMonitor { +public: + void tick(const std::string& channel_name, const std::string& item) { + if (enabled_) + status_[channel_name].insert(item); + } + + void tick(const std::string& channel_name, std::uint32_t id) { + if (enabled_) + status_[channel_name].insert(std::format("{:#x}", id)); + } + + [[nodiscard]] auto text() const -> std::vector { + auto strings = std::vector{}; + for (const auto& [channel, items] : status_) { + auto items_content = std::string{"| "}; + for (const auto& item : items) + items_content += item + " | "; + strings.emplace_back(std::format("{}: {}", channel, items_content)); + } + return strings; + } + + void set_enable(bool enabled) noexcept { enabled_ = enabled; } + +private: + std::unordered_map> status_; + bool enabled_ = true; +}; + +} // namespace rmcs_core::hardware From 688af6475014150697d89fa391be09448b551bc1 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Mon, 10 Aug 2026 18:06:52 +0800 Subject: [PATCH 04/86] feat(controller): Improve shared controllers and add value collector --- .../chassis/chassis_climber_controller.cpp | 13 +- .../controller/chassis/chassis_controller.cpp | 283 +++++++++++++----- .../chassis/chassis_power_controller.cpp | 19 +- .../chassis/steering_wheel_controller.cpp | 42 ++- .../controller/gimbal/dual_yaw_controller.cpp | 277 ++++++++++++++--- .../controller/gimbal/eccentric_dual_yaw.cpp | 278 ++++++++++------- .../gimbal/eccentric_dual_yaw_solver.hpp | 160 ++++++++++ .../src/controller/gimbal/player_viewer.cpp | 16 +- .../gimbal/two_axis_gimbal_solver.hpp | 34 +-- .../bullet_feeder_controller_17mm.cpp | 32 +- .../shooting/friction_wheel_controller.cpp | 15 +- .../controller/shooting/putter_controller.cpp | 141 +++++---- .../controller/shooting/shooting_recorder.cpp | 14 +- .../rmcs_core/src/debug/value_collector.cpp | 120 ++++++++ .../src/referee/app/ui/shape/shape.hpp | 2 - .../referee/app/ui/widget/animated_toggle.hpp | 75 +++++ .../app/ui/widget/crosshair_circle.hpp | 5 + .../src/referee/app/ui/widget/status_ring.hpp | 25 +- 18 files changed, 1189 insertions(+), 362 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw_solver.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/debug/value_collector.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/animated_toggle.hpp diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp index cfd1a6695..840c23c2f 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp @@ -308,8 +308,11 @@ class ChassisClimberController } if (!manual_support_retracting_) { - if (back_climber_zero_velocity_hold_) + if (back_climber_zero_velocity_hold_) { control.back_climber_velocity = 0.0; + } else { + start_back_climber_retract("Auto retract"); + } return control; } @@ -600,7 +603,7 @@ class ChassisClimberController void dual_motor_sync_control( double setpoint, double left_velocity, double right_velocity, pid::MatrixPidCalculator<2>& pid_calculator, double& left_torque_out, - double& right_torque_out) { + double& right_torque_out) const { if (std::isnan(setpoint)) { left_torque_out = nan_; @@ -619,9 +622,9 @@ class ChassisClimberController right_torque_out = control_torques[1]; } - void limit_back_climber_retract_torque( + static void limit_back_climber_retract_torque( double back_climber_velocity_setpoint, double& left_torque, double& right_torque, - double max_torque) const { + double max_torque) { if (!std::isfinite(back_climber_velocity_setpoint) || back_climber_velocity_setpoint >= 0.0) return; @@ -638,6 +641,7 @@ class ChassisClimberController } rclcpp::Logger logger_; + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); static constexpr double kAutoClimbAlignThreshold = 0.10; static constexpr double kAutoClimbAlignVelocityThreshold = 0.2; @@ -717,6 +721,5 @@ class ChassisClimberController } // namespace rmcs_core::controller::chassis #include - PLUGINLIB_EXPORT_CLASS( rmcs_core::controller::chassis::ChassisClimberController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp index 5b7d9bcad..81f6bbaf3 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp @@ -1,65 +1,91 @@ -#include +#include "controller/pid/pid_calculator.hpp" + +#include +#include + +#include #include #include #include #include #include -#include #include - -#include "controller/pid/pid_calculator.hpp" +#include namespace rmcs_core::controller::chassis { class ChassisController : public rmcs_executor::Component - , public rclcpp::Node { + , public rclcpp::Node + , public rmcs_utility::NodeMixin { public: ChassisController() - : Node( - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , following_velocity_controller_(7.0, 0.0, 0.0) { - following_velocity_controller_.output_max = angular_velocity_max; + : Node{get_component_name(), node::options()} { + + following_velocity_controller_.output_max = +angular_velocity_max; following_velocity_controller_.output_min = -angular_velocity_max; register_input("/remote/joystick/right", joystick_right_); - register_input("/remote/joystick/left", joystick_left_); register_input("/remote/switch/right", switch_right_); register_input("/remote/switch/left", switch_left_); - register_input("/remote/mouse/velocity", mouse_velocity_); - register_input("/remote/mouse", mouse_); register_input("/remote/keyboard", keyboard_); - register_input("/remote/rotary_knob", rotary_knob_); register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); - register_output("/chassis/angle", chassis_angle_, nan); - register_output("/chassis/control_angle", chassis_control_angle_, nan); + register_input("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, false); + + register_input("/chassis/climber/direction", chassis_climb_direction_, false); + register_input("/chassis/climber/speed", chassis_climb_speed_, false); + register_input("/chassis/climber/measure_yaw", chassis_measure_yaw_, false); - register_output("/chassis/control_mode", mode_); + register_input("/rmcs_navigation/enable_control", navigation_enable_control_, false); + register_input("/rmcs_navigation/chassis_velocity", navigation_command_velocity_, false); + register_input("/rmcs_navigation/chassis_behavior", navigation_chassis_behavior_, false); + + register_output("/chassis/angle", chassis_angle_, kNaN); + register_output("/chassis/control_angle", chassis_control_angle_, kNaN); + register_output("/chassis/control_mode", mode_, rmcs_msgs::ChassisMode::ALIGNMENT); register_output("/chassis/control_velocity", chassis_control_velocity_); } void before_updating() override { if (!gimbal_yaw_angle_.ready()) { gimbal_yaw_angle_.make_and_bind_directly(0.0); - RCLCPP_WARN(get_logger(), "Failed to fetch \"/gimbal/yaw/angle\". Set to 0.0."); + node::warn("Failed to fetch \"/gimbal/yaw/angle\". Set to 0.0."); } if (!gimbal_yaw_angle_error_.ready()) { gimbal_yaw_angle_error_.make_and_bind_directly(0.0); - RCLCPP_WARN( - get_logger(), "Failed to fetch \"/gimbal/yaw/control_angle_error\". Set to 0.0."); + node::warn("Failed to fetch \"/gimbal/yaw/control_angle_error\". Set to 0.0."); + } + + if (!chassis_climb_direction_.ready()) { + chassis_climb_direction_.make_and_bind_directly(kNaN); + } + if (!chassis_climb_speed_.ready()) { + chassis_climb_speed_.make_and_bind_directly(kNaN); + } + if (!chassis_measure_yaw_.ready()) { + chassis_measure_yaw_.make_and_bind_directly(kNaN); + } + + if (!navigation_enable_control_.ready()) { + navigation_enable_control_.make_and_bind_directly(false); + } + if (!navigation_command_velocity_.ready()) { + navigation_command_velocity_.make_and_bind_directly(Eigen::Vector2d::Zero()); + } + if (!navigation_chassis_behavior_.ready()) { + navigation_chassis_behavior_.make_and_bind_directly(rmcs_msgs::ChassisMode::AUTO); } } void update() override { using namespace rmcs_msgs; - auto switch_right = *switch_right_; - auto switch_left = *switch_left_; - auto keyboard = *keyboard_; + const auto switch_right = *switch_right_; + const auto switch_left = *switch_left_; + const auto keyboard = *keyboard_; do { if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) @@ -71,28 +97,41 @@ class ChassisController auto mode = *mode_; if (switch_left != Switch::DOWN) { if (last_switch_right_ == Switch::MIDDLE && switch_right == Switch::DOWN) { - if (mode == rmcs_msgs::ChassisMode::SPIN) { - mode = rmcs_msgs::ChassisMode::STEP_DOWN; - } else { - mode = rmcs_msgs::ChassisMode::SPIN; + if (mode != rmcs_msgs::ChassisMode::SPIN_FAST) { + mode = rmcs_msgs::ChassisMode::SPIN_FAST; spinning_forward_ = !spinning_forward_; + } else { + mode = rmcs_msgs::ChassisMode::STEP_DOWN; } } else if (!last_keyboard_.c && keyboard.c) { - if (mode == rmcs_msgs::ChassisMode::SPIN) { - mode = rmcs_msgs::ChassisMode::AUTO; - } else { - mode = rmcs_msgs::ChassisMode::SPIN; + if (mode != rmcs_msgs::ChassisMode::SPIN_FAST) { + mode = rmcs_msgs::ChassisMode::SPIN_FAST; spinning_forward_ = !spinning_forward_; + } else { + mode = rmcs_msgs::ChassisMode::AUTO; } } else if (!last_keyboard_.x && keyboard.x) { - mode = mode == rmcs_msgs::ChassisMode::LAUNCH_RAMP - ? rmcs_msgs::ChassisMode::AUTO - : rmcs_msgs::ChassisMode::LAUNCH_RAMP; + mode = mode != rmcs_msgs::ChassisMode::LAUNCH_RAMP + ? rmcs_msgs::ChassisMode::LAUNCH_RAMP + : rmcs_msgs::ChassisMode::AUTO; } else if (!last_keyboard_.z && keyboard.z) { - mode = mode == rmcs_msgs::ChassisMode::STEP_DOWN - ? rmcs_msgs::ChassisMode::AUTO - : rmcs_msgs::ChassisMode::STEP_DOWN; + mode = mode != rmcs_msgs::ChassisMode::STEP_DOWN + ? rmcs_msgs::ChassisMode::STEP_DOWN + : rmcs_msgs::ChassisMode::AUTO; + } + + if (*navigation_enable_control_ + && navigation_command_velocity_->array().isFinite().all()) { + mode = *navigation_chassis_behavior_; } + + if (climb_active()) { + mode = ChassisMode::CLIMB; + } else if (mode == ChassisMode::CLIMB) { + mode = ChassisMode::AUTO; + } + + update_spin_stuck_watchdog(mode); *mode_ = mode; } @@ -105,11 +144,47 @@ class ChassisController } void reset_all_controls() { - *mode_ = rmcs_msgs::ChassisMode::AUTO; + *mode_ = rmcs_msgs::ChassisMode::ALIGNMENT; + *chassis_control_velocity_ = {kNaN, kNaN, kNaN}; - *chassis_control_velocity_ = {nan, nan, nan}; + spin_stuck_count_ = 0; + spin_reverse_cooldown_ = 0; + following_velocity_controller_.reset(); } + auto update_spin_stuck_watchdog(rmcs_msgs::ChassisMode mode) -> void { + constexpr auto kSpinStuckConfirmTicks = std::size_t{300}; + constexpr auto kSpinReverseCooldownTicks = std::size_t{1000}; + constexpr auto kSpinStuckAngularVelocityRatio = double{0.2}; + + using rmcs_msgs::ChassisMode; + + if (spin_reverse_cooldown_ > 0) { + --spin_reverse_cooldown_; + spin_stuck_count_ = 0; + return; + } + + if (!rmcs_msgs::is_spining(mode) || !chassis_yaw_velocity_imu_.ready()) { + spin_stuck_count_ = 0; + return; + } + + const auto expected = (mode == ChassisMode::SPIN_FAST ? 0.6 : 0.3) * angular_velocity_max; + if (std::abs(*chassis_yaw_velocity_imu_) >= kSpinStuckAngularVelocityRatio * expected) { + spin_stuck_count_ = 0; + return; + } + + if (++spin_stuck_count_ < kSpinStuckConfirmTicks) + return; + + spinning_forward_ = !spinning_forward_; + spin_reverse_cooldown_ = kSpinReverseCooldownTicks; + spin_stuck_count_ = 0; + + node::warn("Spin stuck detected, reverse spinning direction."); + } void update_velocity_control() { auto translational_velocity = update_translational_velocity_control(); auto angular_velocity = update_angular_velocity_control(); @@ -117,7 +192,40 @@ class ChassisController chassis_control_velocity_->vector << translational_velocity, angular_velocity; } + auto climb_active() const -> bool { + return std::isfinite(*chassis_climb_direction_) && std::isfinite(*chassis_climb_speed_) + && std::isfinite(*chassis_measure_yaw_); + } + + static auto normalize_signed_angle(double angle) noexcept { + constexpr auto kTwoPi = 2.0 * std::numbers::pi; + while (angle >= std::numbers::pi) + angle -= kTwoPi; + while (angle < -std::numbers::pi) + angle += kTwoPi; + return angle; + } + Eigen::Vector2d update_translational_velocity_control() { + using namespace rmcs_msgs; + + if (*mode_ == ChassisMode::CLIMB) { + // speed 以底盘正向 direction 为正向：上坡为正前进，下坡为负倒车 + return {*chassis_climb_speed_, 0.0}; + } + + if (*navigation_enable_control_) { + const auto command = *navigation_command_velocity_; + if (command.array().isFinite().all()) { + Eigen::Vector2d superimposed = + command + *joystick_right_ * translational_velocity_max; + if (superimposed.norm() > translational_velocity_max) + superimposed *= translational_velocity_max / superimposed.norm(); + + return Eigen::Rotation2Dd{*gimbal_yaw_angle_} * superimposed; + } + } + auto keyboard = *keyboard_; Eigen::Vector2d keyboard_move{keyboard.w - keyboard.s, keyboard.a - keyboard.d}; @@ -134,20 +242,42 @@ class ChassisController double update_angular_velocity_control() { double angular_velocity = 0.0; - double chassis_control_angle = nan; + double chassis_control_angle = kNaN; + using namespace rmcs_msgs; switch (*mode_) { - case rmcs_msgs::ChassisMode::AUTO: break; - case rmcs_msgs::ChassisMode::SPIN: { + case ChassisMode::AUTO: break; + + case ChassisMode::SPIN_FAST: angular_velocity = 0.6 * (spinning_forward_ ? angular_velocity_max : -angular_velocity_max); + break; + case ChassisMode::SPIN_SLOW: + angular_velocity = + 0.3 * (spinning_forward_ ? angular_velocity_max : -angular_velocity_max); + break; + + // @NOTE: Align With 4 Sides + case ChassisMode::ALIGNMENT_POWERED: [[fallthrough]]; + case ChassisMode::ALIGNMENT: { + const auto speed = chassis_control_velocity_->vector.head<2>(); + const auto line1 = Eigen::Vector2d{speed.x(), 0}; + const auto line2 = Eigen::Vector2d{0, speed.y()}; + + const auto signed_angle = [](const Eigen::Vector2d& from, const Eigen::Vector2d& to) { + return std::atan2(from.x() * to.y() - from.y() * to.x(), from.dot(to)); + }; + const double angle1 = signed_angle(speed, line1); + const double angle2 = signed_angle(speed, line2); + const double min = (std::abs(angle1) < std::abs(angle2)) ? angle1 : angle2; + + angular_velocity = following_velocity_controller_.update(-min); } break; - case rmcs_msgs::ChassisMode::STEP_DOWN: { + + // @NOTE: Align With 2 Sides + case ChassisMode::STEP_DOWN: { double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); - // err: [0, 2pi) -> [0, alignment) -> signed. - // In step-down mode, two sides of the chassis can be used for alignment. - // TODO: Dynamically determine the split angle based on chassis velocity. constexpr double alignment = std::numbers::pi; while (err > alignment / 2) { chassis_control_angle -= alignment; @@ -158,19 +288,30 @@ class ChassisController angular_velocity = following_velocity_controller_.update(err); } break; - case rmcs_msgs::ChassisMode::LAUNCH_RAMP: { + + // @NOTE: Align With 1 Sides + case ChassisMode::LAUNCH_RAMP: { double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); - // err: [0, 2pi) -> signed - // In launch ramp mode, only one direction can be used for alignment. - // TODO: Dynamically determine the split angle based on chassis velocity. constexpr double alignment = 2 * std::numbers::pi; if (err > alignment / 2) err -= alignment; angular_velocity = following_velocity_controller_.update(err); } break; + + case ChassisMode::CLIMB: { + chassis_control_angle = *chassis_climb_direction_; + + const auto err = normalize_signed_angle(chassis_control_angle - *chassis_measure_yaw_); + angular_velocity = following_velocity_controller_.update(err); + + *chassis_angle_ = *chassis_measure_yaw_; + *chassis_control_angle_ = chassis_control_angle; + return angular_velocity; + } } + *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; *chassis_control_angle_ = chassis_control_angle; @@ -181,37 +322,25 @@ class ChassisController chassis_control_angle = *gimbal_yaw_angle_error_; if (chassis_control_angle < 0) chassis_control_angle += 2 * std::numbers::pi; - // chassis_control_angle: [0, 2pi). - // err = setpoint - measurement - // ^ ^ - // |gimbal_yaw_angle_error |chassis_angle - // ^ - // |(2pi - gimbal_yaw_angle) double err = chassis_control_angle + *gimbal_yaw_angle_; if (err >= 2 * std::numbers::pi) err -= 2 * std::numbers::pi; - // err: [0, 2pi). return err; } private: - static constexpr double inf = std::numeric_limits::infinity(); - static constexpr double nan = std::numeric_limits::quiet_NaN(); + static constexpr double kInf = std::numeric_limits::infinity(); + static constexpr double kNaN = std::numeric_limits::quiet_NaN(); - // Maximum control velocities - static constexpr double translational_velocity_max = 10.0; - static constexpr double angular_velocity_max = 16.0; + const double translational_velocity_max{node::param_or("translational_velocity_max", 10.0)}; + const double angular_velocity_max{node::param_or("angular_velocity_max", 16.0)}; InputInterface joystick_right_; - InputInterface joystick_left_; InputInterface switch_right_; InputInterface switch_left_; - InputInterface mouse_velocity_; - InputInterface mouse_; InputInterface keyboard_; - InputInterface rotary_knob_; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; @@ -220,9 +349,26 @@ class ChassisController InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; OutputInterface chassis_angle_, chassis_control_angle_; + InputInterface chassis_yaw_velocity_imu_; + InputInterface chassis_climb_direction_; + InputInterface chassis_climb_speed_; + InputInterface chassis_measure_yaw_; + + InputInterface navigation_enable_control_; + InputInterface navigation_command_velocity_; + InputInterface navigation_chassis_behavior_; + OutputInterface mode_; bool spinning_forward_ = true; - pid::PidCalculator following_velocity_controller_; + + std::size_t spin_stuck_count_ = 0; + std::size_t spin_reverse_cooldown_ = 0; + + pid::PidCalculator following_velocity_controller_{ + node::param_or("following_velocity_kp", 8.0), + node::param_or("following_velocity_ki", 0.0), + node::param_or("following_velocity_kd", 0.0), + }; OutputInterface chassis_control_velocity_; }; @@ -230,5 +376,4 @@ class ChassisController } // namespace rmcs_core::controller::chassis #include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::ChassisController, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::ChassisController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp index 3186ef629..f9e8e83a9 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp @@ -30,6 +30,7 @@ class ChassisPowerController register_input("/chassis/power", chassis_power_); register_input("/chassis/supercap/voltage", supercap_voltage_); register_input("/chassis/supercap/enabled", supercap_enabled_); + register_input("/rmcs_navigation/enable_supercap", navigation_supercap_, false); register_input("/referee/chassis/power_limit", chassis_power_limit_referee_); register_input("/referee/chassis/buffer_energy", chassis_buffer_energy_referee_); @@ -41,6 +42,8 @@ class ChassisPowerController "/chassis/supercap/voltage/control_line", supercap_voltage_control_line_, 12.5); register_output("/chassis/supercap/voltage/base_line", supercap_voltage_base_line_, 12.0); register_output("/chassis/supercap/voltage/dead_line", supercap_voltage_dead_line_, 11.0); + + register_output("/chassis/climber/front/control_power_limit", control_power_limit_, 0.0); } void update() override { @@ -94,6 +97,7 @@ class ChassisPowerController virtual_buffer_energy_ = virtual_buffer_energy_limit_; boost_mode_ = false; *chassis_control_power_limit_ = 0.0; + *control_power_limit_ = 0.0; } void update_virtual_buffer_energy() { @@ -107,10 +111,12 @@ class ChassisPowerController void update_control_power_limit() { double power_limit; - if (boost_mode_ && *supercap_enabled_) - power_limit = *mode_ == rmcs_msgs::ChassisMode::LAUNCH_RAMP - ? inf_ - : *chassis_power_limit_referee_ + 80.0; + const auto navigation_supercap_boost = + navigation_supercap_.ready() && *navigation_supercap_; + + if ((boost_mode_ || navigation_supercap_boost) && *supercap_enabled_) + power_limit = + rmcs_msgs::is_powered(*mode_) ? inf_ : *chassis_power_limit_referee_ + 80.0; else power_limit = *chassis_power_limit_referee_; chassis_power_limit_expected_ = power_limit; @@ -132,6 +138,7 @@ class ChassisPowerController power_limit *= virtual_buffer_energy_ / virtual_buffer_energy_limit_; *chassis_control_power_limit_ = power_limit; + *control_power_limit_ = power_limit; } void update_ui() { @@ -156,6 +163,7 @@ class ChassisPowerController InputInterface supercap_voltage_; InputInterface supercap_enabled_; + InputInterface navigation_supercap_; InputInterface chassis_power_limit_referee_; InputInterface chassis_buffer_energy_referee_; @@ -168,6 +176,7 @@ class ChassisPowerController OutputInterface supercap_voltage_control_line_; OutputInterface supercap_voltage_base_line_; OutputInterface supercap_voltage_dead_line_; + OutputInterface control_power_limit_; ui::Integer chassis_power_ui_{ui::Shape::Color::WHITE, 15, 2, ui::x_center, 100, 0}; ui::Integer chassis_control_power_limit_ui_{ @@ -179,4 +188,4 @@ class ChassisPowerController #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::chassis::ChassisPowerController, rmcs_executor::Component) \ No newline at end of file + rmcs_core::controller::chassis::ChassisPowerController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_controller.cpp index 83af9894c..2f0abb7bd 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_controller.cpp @@ -35,13 +35,28 @@ class SteeringWheelController , no_load_power_(get_parameter("no_load_power").as_double()) , control_acceleration_filter_(5.0, 1000.0) , chassis_velocity_expected_(Eigen::Vector3d::Zero()) - , chassis_translational_velocity_pid_(5.0, 0.0, 1.0) - , chassis_angular_velocity_pid_(5.0, 0.0, 1.0) + , chassis_translational_velocity_pid_( + get_parameter("chassis_translation_kp").as_double(), + get_parameter("chassis_translation_ki").as_double(), + get_parameter("chassis_translation_kd").as_double()) + , chassis_angular_velocity_pid_( + get_parameter("chassis_angular_velocity_kp").as_double(), + get_parameter("chassis_angular_velocity_ki").as_double(), + get_parameter("chassis_angular_velocity_kd").as_double()) , cos_varphi_(1, 0, -1, 0) // 0, pi/2, pi, 3pi/2 , sin_varphi_(0, 1, 0, -1) - , steering_velocity_pid_(0.15, 0.0, 0.0) - , steering_angle_pid_(30.0, 0.0, 0.0) - , wheel_velocity_pid_(0.6, 0.0, 0.0) { + , steering_velocity_pid_( + get_parameter("steering_velocity_kp").as_double(), + get_parameter("steering_velocity_ki").as_double(), + get_parameter("steering_velocity_kd").as_double()) + , steering_angle_pid_( + get_parameter("steering_angle_kp").as_double(), + get_parameter("steering_angle_ki").as_double(), + get_parameter("steering_angle_kd").as_double()) + , wheel_velocity_pid_( + get_parameter("wheel_velocity_kp").as_double(), + get_parameter("wheel_velocity_ki").as_double(), + get_parameter("wheel_velocity_kd").as_double()) { register_input("/remote/joystick/right", joystick_right_); register_input("/remote/joystick/left", joystick_left_); @@ -81,6 +96,18 @@ class SteeringWheelController "/chassis/right_back_wheel/control_torque", right_back_wheel_control_torque_); register_output( "/chassis/right_front_wheel/control_torque", right_front_wheel_control_torque_); + + double translation_integral_limit; + if (get_parameter("chassis_translation_integral_limit", translation_integral_limit)) { + chassis_translational_velocity_pid_.integral_min.setConstant(-translation_integral_limit); + chassis_translational_velocity_pid_.integral_max.setConstant(+translation_integral_limit); + } + + double angular_velocity_integral_limit; + if (get_parameter("chassis_angular_velocity_integral_limit", angular_velocity_integral_limit)) { + chassis_angular_velocity_pid_.integral_min = -angular_velocity_integral_limit; + chassis_angular_velocity_pid_.integral_max = +angular_velocity_integral_limit; + } } void update() override { @@ -244,6 +271,9 @@ class SteeringWheelController const double& angular_control_velocity = chassis_control_velocity[2]; const double& angular_velocity = chassis_velocity_expected[2]; + if (std::abs(angular_control_velocity) < 1e-6) + chassis_angular_velocity_pid_.reset(); + double angular_control_acceleration = chassis_angular_velocity_pid_.update(angular_control_velocity - angular_velocity); @@ -501,4 +531,4 @@ class SteeringWheelController #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::chassis::SteeringWheelController, rmcs_executor::Component) \ No newline at end of file + rmcs_core::controller::chassis::SteeringWheelController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp index 0d49184e8..84f8c0862 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp @@ -1,5 +1,6 @@ #include +#include #include #include @@ -39,21 +40,22 @@ class DualYawController register_input("/gimbal/top_yaw/velocity", top_yaw_velocity_); register_input("/gimbal/bottom_yaw/angle", bottom_yaw_angle_); register_input("/gimbal/bottom_yaw/velocity", bottom_yaw_velocity_); + register_input("/gimbal/bottom_yaw/raw_angle", bottom_yaw_raw_angle_); register_input("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); register_input("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_); + register_input("/gimbal/yaw_brake/velocity", yaw_brake_velocity_); register_input("/gimbal/mode", gimbal_mode_); register_input("/gimbal/yaw/control_angle_error", control_angle_error_); register_input("/gimbal/yaw/control_angle_shift", control_angle_shift_, false); + register_input("/gimbal/bottom_yaw/torque", bottom_yaw_torque_); register_output("/gimbal/top_yaw/control_torque", top_yaw_control_torque_, 0.0); register_output("/gimbal/bottom_yaw/control_torque", bottom_yaw_control_torque_, 0.0); - register_output("/gimbal/top_yaw/control_angle", top_yaw_control_angle_, nan_); - register_output( - "/gimbal/bottom_yaw/control_angle_shift", bottom_yaw_control_angle_shift_, nan_); - register_output("/gimbal/top_yaw/control_angle_shift", top_yaw_control_angle_shift_, nan_); register_output("/gimbal/bottom_yaw/control_angle", bottom_yaw_control_angle_, nan_); + register_output("/gimbal/top_yaw/control_angle_shift", top_yaw_control_angle_shift_, nan_); + register_output("/gimbal/yaw_brake/control_torque", yaw_brake_control_torque_, nan_); status_component_ = create_partner_component(get_component_name() + "_status"); @@ -68,60 +70,249 @@ class DualYawController } void update() override { - const auto mode = *gimbal_mode_; - if (mode == rmcs_msgs::GimbalMode::ENCODER) { - const bool entering_encoder = last_gimbal_mode_ != rmcs_msgs::GimbalMode::ENCODER; - if (entering_encoder) { - if (std::isfinite(*top_yaw_angle_)) { - top_yaw_encoder_locked_ = true; - } else { - top_yaw_encoder_locked_ = false; - } - } - *bottom_yaw_control_angle_shift_ = *control_angle_shift_; - if (top_yaw_encoder_locked_) - *top_yaw_control_angle_ = top_yaw_encoder_angle_; - else - *top_yaw_control_angle_ = nan_; - } else { - *top_yaw_control_angle_ = nan_; - *bottom_yaw_control_angle_shift_ = nan_; - top_yaw_encoder_angle_ = *top_yaw_angle_; - top_yaw_encoder_locked_ = false; + + const bool entering_encoder = mode == rmcs_msgs::GimbalMode::ENCODER + && last_gimbal_mode_ != rmcs_msgs::GimbalMode::ENCODER; + + const bool leaving_encoder = mode != rmcs_msgs::GimbalMode::ENCODER + && last_gimbal_mode_ == rmcs_msgs::GimbalMode::ENCODER; + + if (entering_encoder) { + top_yaw_angle_pid_.reset(); + top_yaw_velocity_pid_.reset(); + bottom_yaw_angle_pid_.reset(); + bottom_yaw_velocity_pid_.reset(); + + yaw_brake_engage_count_ = 0; + yaw_brake_release_elapsed_count_ = 0; + yaw_brake_release_stop_count_ = 0; + yaw_brake_release_seen_motion_ = false; + encoder_state_ = EncoderState::AlignTargetRawAngle; } - last_gimbal_mode_ = mode; + if (leaving_encoder) { + top_yaw_angle_pid_.reset(); + top_yaw_velocity_pid_.reset(); + bottom_yaw_angle_pid_.reset(); + bottom_yaw_velocity_pid_.reset(); + + if (encoder_state_ == EncoderState::EngageYawBrake + || encoder_state_ == EncoderState::BrakeLocked) { + yaw_brake_release_elapsed_count_ = 0; + yaw_brake_release_stop_count_ = 0; + yaw_brake_release_seen_motion_ = false; + encoder_state_ = EncoderState::ReleaseYawBrake; + } else { + yaw_brake_engage_count_ = 0; + yaw_brake_release_elapsed_count_ = 0; + yaw_brake_release_stop_count_ = 0; + yaw_brake_release_seen_motion_ = false; + *yaw_brake_control_torque_ = nan_; + encoder_state_ = EncoderState::Idle; + } + } + if (mode == rmcs_msgs::GimbalMode::ENCODER && encoder_state_ == EncoderState::Idle) { + top_yaw_angle_pid_.reset(); + top_yaw_velocity_pid_.reset(); + bottom_yaw_angle_pid_.reset(); + bottom_yaw_velocity_pid_.reset(); + + yaw_brake_engage_count_ = 0; + yaw_brake_release_elapsed_count_ = 0; + yaw_brake_release_stop_count_ = 0; + yaw_brake_release_seen_motion_ = false; + encoder_state_ = EncoderState::AlignTargetRawAngle; + } - if (std::isnan(*control_angle_error_)) { + // RCLCPP_INFO(get_logger(), "bottom_yaw_raw_angle: %ld", *bottom_yaw_raw_angle_); + // RCLCPP_INFO(get_logger(), "bottom_yaw_control_torque: %f", *bottom_yaw_control_torque_); + // RCLCPP_INFO(get_logger(), "bottom_yaw_torque: %f", *bottom_yaw_torque_); + // RCLCPP_INFO( + // get_logger(), "encoder_state: %s", + // encoder_state_ == EncoderState::Idle ? "Idle" + // : encoder_state_ == EncoderState::AlignTargetRawAngle ? "AlignTargetRawAngle" + // : encoder_state_ == EncoderState::EngageYawBrake ? "EngageYawBrake" + // : encoder_state_ == EncoderState::BrakeLocked ? "BrakeLocked" + // : "ReleaseYawBrake"); + const bool hold_encoder_for_brake_release = encoder_state_ == EncoderState::ReleaseYawBrake; + + if (mode == rmcs_msgs::GimbalMode::ENCODER || hold_encoder_for_brake_release) { *top_yaw_control_torque_ = nan_; - *bottom_yaw_control_torque_ = nan_; + *bottom_yaw_control_angle_ = nan_; + + auto wrap_raw_delta = [](int64_t diff) -> int64_t { + diff %= kBottomYawRawAngleModulus; + if (diff <= -(kBottomYawRawAngleModulus / 2)) + diff += kBottomYawRawAngleModulus; + else if (diff > (kBottomYawRawAngleModulus / 2)) + diff -= kBottomYawRawAngleModulus; + return diff; + }; + + if (encoder_state_ == EncoderState::AlignTargetRawAngle) { + *top_yaw_control_angle_shift_ = 0.0; + *yaw_brake_control_torque_ = nan_; + + if (!bottom_yaw_raw_angle_.ready()) { + *bottom_yaw_control_torque_ = nan_; + } else { + const int64_t raw_error_count = + wrap_raw_delta(kEncoderBottomYawTargetRawAngle - *bottom_yaw_raw_angle_); + const int64_t raw_error_abs = + raw_error_count >= 0 ? raw_error_count : -raw_error_count; + + const bool bottom_yaw_aligned = + raw_error_abs <= kEncoderBottomYawTargetToleranceRawAngle + && std::abs(*bottom_yaw_velocity_) + <= kEncoderBottomYawLockVelocityThreshold; + + if (bottom_yaw_aligned) { + *bottom_yaw_control_torque_ = nan_; + bottom_yaw_angle_pid_.reset(); + bottom_yaw_velocity_pid_.reset(); + yaw_brake_engage_count_ = 0; + encoder_state_ = EncoderState::EngageYawBrake; + } else { + const double raw_error_angle = + kEncoderBottomYawRawAngleErrorSign + * static_cast(raw_error_count) + / static_cast(kBottomYawRawAngleModulus) * 2.0 + * std::numbers::pi; + + const double target_velocity = + bottom_yaw_angle_pid_.update(raw_error_angle); + const double velocity_error = target_velocity - *bottom_yaw_velocity_; + double control_torque = bottom_yaw_velocity_pid_.update(velocity_error); + + constexpr double kBreakawayTorque = 2.5; + constexpr double kBreakawayVelocityThreshold = 0.10; + constexpr int64_t kBreakawayErrorThreshold = 80; + + if (std::abs(*bottom_yaw_velocity_) < kBreakawayVelocityThreshold + && std::abs(raw_error_count) > kBreakawayErrorThreshold + && std::abs(control_torque) < kBreakawayTorque) { + control_torque = + velocity_error >= 0.0 ? kBreakawayTorque : -kBreakawayTorque; + } + + *bottom_yaw_control_torque_ = control_torque; + } + } + + } else if (encoder_state_ == EncoderState::EngageYawBrake) { + *top_yaw_control_angle_shift_ = 0.0; + *bottom_yaw_control_torque_ = nan_; + *yaw_brake_control_torque_ = kYawBrakeEngageTorque; + + if (std::abs(*yaw_brake_velocity_) < kYawBrakeEngageVelocityThreshold) { + ++yaw_brake_engage_count_; + } else { + yaw_brake_engage_count_ = 0; + } + + if (yaw_brake_engage_count_ >= kYawBrakeEngageConfirmCount) { + yaw_brake_engage_count_ = 0; + *yaw_brake_control_torque_ = nan_; + encoder_state_ = EncoderState::BrakeLocked; + } + + } else if (encoder_state_ == EncoderState::BrakeLocked) { + constexpr double kBrakeLockedHoldTorque = 0.6; + *bottom_yaw_control_torque_ = kBrakeLockedHoldTorque; + *yaw_brake_control_torque_ = nan_; + *top_yaw_control_angle_shift_ = *control_angle_shift_; + + } else if (encoder_state_ == EncoderState::ReleaseYawBrake) { + *top_yaw_control_angle_shift_ = 0.0; + *bottom_yaw_control_torque_ = nan_; + *yaw_brake_control_torque_ = kYawBrakeReleaseTorque; + + ++yaw_brake_release_elapsed_count_; + + if (std::abs(*yaw_brake_velocity_) > kYawBrakeReleaseMotionVelocityThreshold) { + yaw_brake_release_seen_motion_ = true; + } + + if (yaw_brake_release_seen_motion_ + && std::abs(*yaw_brake_velocity_) < kYawBrakeReleaseStopVelocityThreshold) { + ++yaw_brake_release_stop_count_; + } else { + yaw_brake_release_stop_count_ = 0; + } + + if (yaw_brake_release_stop_count_ >= kYawBrakeReleaseStopConfirmCount) { + yaw_brake_engage_count_ = 0; + yaw_brake_release_elapsed_count_ = 0; + yaw_brake_release_stop_count_ = 0; + yaw_brake_release_seen_motion_ = false; + *yaw_brake_control_torque_ = nan_; + encoder_state_ = EncoderState::Idle; + } else if (yaw_brake_release_elapsed_count_ >= kYawBrakeReleaseTimeoutCount) { + RCLCPP_WARN( + get_logger(), "Yaw brake release timed out, stop applying reverse torque " + "for protection."); + yaw_brake_engage_count_ = 0; + yaw_brake_release_elapsed_count_ = 0; + yaw_brake_release_stop_count_ = 0; + yaw_brake_release_seen_motion_ = false; + *yaw_brake_control_torque_ = nan_; + encoder_state_ = EncoderState::Idle; + } + + } else { + *top_yaw_control_angle_shift_ = 0.0; + *bottom_yaw_control_torque_ = nan_; + *yaw_brake_control_torque_ = nan_; + } + } else { + *yaw_brake_control_torque_ = nan_; + *top_yaw_control_torque_ = top_yaw_velocity_pid_.update( top_yaw_angle_pid_.update(*control_angle_error_) - *gimbal_yaw_velocity_imu_); *bottom_yaw_control_torque_ = bottom_yaw_velocity_pid_.update( bottom_yaw_angle_pid_.update(bottom_yaw_control_error()) - bottom_yaw_velocity_imu()); + + *bottom_yaw_control_angle_ = nan_; + *top_yaw_control_angle_shift_ = nan_; } + + last_gimbal_mode_ = mode; } private: + enum class EncoderState { + Idle, + AlignTargetRawAngle, + EngageYawBrake, + BrakeLocked, + ReleaseYawBrake + }; static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - double wrap_angle(double angle) { - while (angle > 0) - angle += 2 * std::numbers::pi; - while (angle >= 2 * std::numbers::pi) - angle -= 2 * std::numbers::pi; - return angle; - } + static constexpr int64_t kBottomYawRawAngleModulus = 1 << 16; + static constexpr int64_t kEncoderBottomYawTargetRawAngle = 2434; + static constexpr int64_t kEncoderBottomYawTargetToleranceRawAngle = 80; + static constexpr double kEncoderBottomYawLockVelocityThreshold = 0.15; + static constexpr double kEncoderBottomYawRawAngleErrorSign = -1.0; + + static constexpr double kYawBrakeEngageTorque = -0.3; + static constexpr double kYawBrakeEngageVelocityThreshold = 0.1; + static constexpr int kYawBrakeEngageConfirmCount = 50; + + static constexpr double kYawBrakeReleaseTorque = 0.3; + static constexpr double kYawBrakeReleaseMotionVelocityThreshold = 0.2; + static constexpr double kYawBrakeReleaseStopVelocityThreshold = 0.08; + static constexpr int kYawBrakeReleaseStopConfirmCount = 20; + static constexpr int kYawBrakeReleaseTimeoutCount = 400; double bottom_yaw_control_error() { if (!std::isfinite(*top_yaw_angle_) || !std::isfinite(*control_angle_error_)) return nan_; - // Avoid relying on top_yaw_angle in [0, 2pi) and control_angle_error in [-pi, pi]. constexpr double alignment = 2 * std::numbers::pi; double err = std::fmod(*top_yaw_angle_ + *control_angle_error_ + std::numbers::pi, alignment); @@ -135,27 +326,31 @@ class DualYawController InputInterface top_yaw_angle_, top_yaw_velocity_; InputInterface bottom_yaw_angle_, bottom_yaw_velocity_; + InputInterface bottom_yaw_raw_angle_; InputInterface gimbal_yaw_velocity_imu_, chassis_yaw_velocity_imu_; + InputInterface yaw_brake_velocity_; InputInterface gimbal_mode_; InputInterface control_angle_error_, control_angle_shift_; + InputInterface bottom_yaw_torque_; pid::PidCalculator top_yaw_angle_pid_, top_yaw_velocity_pid_; pid::PidCalculator bottom_yaw_angle_pid_, bottom_yaw_velocity_pid_; OutputInterface top_yaw_control_torque_; OutputInterface bottom_yaw_control_torque_; - - OutputInterface top_yaw_control_angle_; - OutputInterface bottom_yaw_control_angle_shift_; - OutputInterface bottom_yaw_control_angle_; OutputInterface top_yaw_control_angle_shift_; + OutputInterface yaw_brake_control_torque_; rmcs_msgs::GimbalMode last_gimbal_mode_ = rmcs_msgs::GimbalMode::IMU; - bool top_yaw_encoder_locked_ = false; - double top_yaw_encoder_angle_ = nan_; + EncoderState encoder_state_ = EncoderState::Idle; + + int yaw_brake_engage_count_ = 0; + int yaw_brake_release_elapsed_count_ = 0; + int yaw_brake_release_stop_count_ = 0; + bool yaw_brake_release_seen_motion_ = false; class DualYawStatus : public rmcs_executor::Component { public: diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp index 4e6c4e72d..526853c05 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp @@ -1,3 +1,6 @@ +#include "controller/gimbal/eccentric_dual_yaw_solver.hpp" +#include "controller/pid/pid_calculator.hpp" + #include #include #include @@ -12,11 +15,52 @@ #include #include -#include "controller/pid/pid_calculator.hpp" - namespace rmcs_core::controller::gimbal { using namespace rmcs_description; +// 对 top 关节自瞄参考角做差分+低通滤波的目标角速度前馈， +// 补偿斜坡跟踪滞后；切板跳变时清零。 +struct YawRateFeedforward { + double gain = 1.0; + double cutoff_hz = 15.0; + double max_rate = 6.0; + double jump_threshold = 0.05; + + auto update(double azimuth, std::chrono::steady_clock::time_point now) -> double { + if (std::isfinite(prev_azimuth_)) { + const auto dt = std::chrono::duration(now - prev_timestamp_).count(); + const auto delta = limit_rad(azimuth - prev_azimuth_); + if (std::abs(delta) > jump_threshold) { + filtered_rate_ = 0.0; + } else if (dt > kMinDt) { + const auto raw = delta / dt; + const auto alpha = dt / (dt + 1.0 / (2.0 * std::numbers::pi * cutoff_hz)); + filtered_rate_ += alpha * (raw - filtered_rate_); + } + } + prev_azimuth_ = azimuth; + prev_timestamp_ = now; + return gain * std::clamp(filtered_rate_, -max_rate, max_rate); + } + +private: + static constexpr double kNaN = std::numeric_limits::quiet_NaN(); + static constexpr double kMinDt = 1e-6; + + static auto limit_rad(double angle) -> double { + constexpr double kPi = std::numbers::pi_v; + while (angle > kPi) + angle -= 2.0 * kPi; + while (angle <= -kPi) + angle += 2.0 * kPi; + return angle; + } + + double prev_azimuth_ = kNaN; + double filtered_rate_ = 0.0; + std::chrono::steady_clock::time_point prev_timestamp_{}; +}; + class EccentricDualYaw : public rmcs_executor::Component , public rclcpp::Node { @@ -24,9 +68,20 @@ class EccentricDualYaw EccentricDualYaw() : Node{ get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} {} + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} { + get_parameter_or("top_yaw_velocity_ff_gain", top_yaw_ff_.gain, 1.0); + get_parameter_or("top_yaw_ff_cutoff_hz", top_yaw_ff_.cutoff_hz, 15.0); + get_parameter_or("top_yaw_ff_max", top_yaw_ff_.max_rate, 6.0); + get_parameter_or("top_yaw_ff_jump_threshold", top_yaw_ff_.jump_threshold, 0.05); + } auto before_updating() -> void override { + if (!input_.navigation_enable_control.ready()) { + input_.navigation_enable_control.make_and_bind_directly(false); + input_.navigation_toward.make_and_bind_directly(kVecNaN); + RCLCPP_INFO(get_logger(), "Manual mode without navigation gimbal control"); + } + enter_disabled_state(); previous_actual_yaw_ = current_barrel_yaw_pitch().first; previous_yaw_timestamp_ = *input_.timestamp; @@ -42,56 +97,73 @@ class EccentricDualYaw return; } - const double yaw_shift = kJoystickSensitivity * input_.joystick_left->y() - + kMouseSensitivity * input_.mouse_velocity->y(); - const double pitch_shift = -kJoystickSensitivity * input_.joystick_left->x() - - kMouseSensitivity * input_.mouse_velocity->x(); - - manual_bottom_yaw_target_ = limit_rad(manual_bottom_yaw_target_ + yaw_shift); - manual_pitch_target_ = - std::clamp(manual_pitch_target_ + pitch_shift, upper_limit_, lower_limit_); - - const auto manual_target = ControlTarget{ - .bottom_yaw = {.target = manual_bottom_yaw_target_}, - .top_yaw = {.target = 0.0}, - .pitch = {.target = manual_pitch_target_}, - }; - apply_control(manual_target); + // 自动瞄准控制。 + if (input_.enable_autoaim()) { + const auto error = solver_.update( + EccentricDualYawSolver::AutoAim{ + *input_.tf, + *input_.top_yaw_angle, + *input_.auto_aim_control_direction, + *input_.auto_aim_robot_center, + upper_limit_, + lower_limit_, + }); + const auto top_yaw_ff = + top_yaw_ff_.update(solver_.top_target_azimuth(), *input_.timestamp); + apply_control(error.bottom_yaw, error.top_yaw, error.pitch, top_yaw_ff); + + const auto [_, cur_pitch] = current_barrel_yaw_pitch(); + stored_bottom_yaw_target_ = limit_rad(current_bottom_world_yaw() + error.bottom_yaw); + stored_pitch_target_ = + std::clamp(limit_rad(cur_pitch + error.pitch), upper_limit_, lower_limit_); + return; + } + + const auto yaw_shift = +kJoystickSensitivity * input_.joystick_left->y() + + kMouseSensitivity * input_.mouse_velocity->y(); + const auto pitch_shift = -kJoystickSensitivity * input_.joystick_left->x() + - kMouseSensitivity * input_.mouse_velocity->x(); + + auto nav_yshift = double{0.}; + auto nav_pshift = double{0.}; + if (input_.enable_navigation()) { + constexpr auto kGimbalFree = std::numeric_limits::min(); + const auto& toward = *input_.navigation_toward; + if (toward.x() == kGimbalFree && toward.y() == kGimbalFree) { + enter_disabled_state(); + return; + } + if (std::isfinite(toward.x())) + nav_yshift = limit_rad(toward.x() - stored_bottom_yaw_target_); + if (std::isfinite(toward.y())) + nav_pshift = limit_rad( + std::clamp(toward.y(), upper_limit_, lower_limit_) - stored_pitch_target_); + } + + stored_bottom_yaw_target_ = limit_rad(stored_bottom_yaw_target_ + nav_yshift + yaw_shift); + stored_pitch_target_ = + std::clamp(stored_pitch_target_ + nav_pshift + pitch_shift, upper_limit_, lower_limit_); + + apply_control( + limit_rad(+stored_bottom_yaw_target_ - current_bottom_world_yaw()), + limit_rad(-*input_.top_yaw_angle), + limit_rad(+stored_pitch_target_ - actual_yaw_pitch.second)); } private: static constexpr double kNaN = std::numeric_limits::quiet_NaN(); + static inline auto kVecNaN = Eigen::Vector2d{kNaN, kNaN}; + static constexpr double kEpsilon = 1e-9; + static constexpr double kMinDt = 1e-6; static constexpr double kJoystickSensitivity = 0.006; static constexpr double kMouseSensitivity = 0.5; const double upper_limit_{get_parameter("upper_limit").as_double()}; const double lower_limit_{get_parameter("lower_limit").as_double()}; - const double bottom_yaw_viscous_ff_gain_{get_parameter_or("bottom_yaw_viscous_ff_gain", 0.0)}; - const double bottom_yaw_coulomb_ff_gain_{get_parameter_or("bottom_yaw_coulomb_ff_gain", 0.0)}; - const double bottom_yaw_coulomb_ff_tanh_gain_{ - get_parameter_or("bottom_yaw_coulomb_ff_tanh_gain", 100.0)}; - const double k_top_to_bottom_{get_parameter_or("k_top_to_bottom", 0.0)}; - const double top_yaw_viscous_ff_gain_{get_parameter_or("top_yaw_viscous_ff_gain", 0.0)}; - const double top_yaw_coulomb_ff_gain_{get_parameter_or("top_yaw_coulomb_ff_gain", 0.0)}; - const double top_yaw_coulomb_ff_tanh_gain_{ - get_parameter_or("top_yaw_coulomb_ff_tanh_gain", 100.0)}; - const double pitch_viscous_ff_gain_{get_parameter_or("pitch_viscous_ff_gain", 0.0)}; - const double pitch_coulomb_ff_gain_{get_parameter_or("pitch_coulomb_ff_gain", 0.0)}; - const double pitch_coulomb_ff_tanh_gain_{get_parameter_or("pitch_coulomb_ff_tanh_gain", 100.0)}; - const double pitch_gravity_ff_gain_{get_parameter_or("pitch_gravity_ff_gain", 0.0)}; - const double pitch_gravity_ff_phase_{get_parameter_or("pitch_gravity_ff_phase", 0.0)}; - - struct AxisCommand { - double target = 0.0; - double velocity_ff = 0.0; - double acceleration_ff = 0.0; - }; - struct ControlTarget { - AxisCommand bottom_yaw; - AxisCommand top_yaw; - AxisCommand pitch; - }; + EccentricDualYawSolver solver_; + + YawRateFeedforward top_yaw_ff_; struct Input { explicit Input(rmcs_executor::Component& component) { @@ -99,6 +171,7 @@ class EccentricDualYaw component.register_input("/remote/switch/right", switch_right); component.register_input("/remote/switch/left", switch_left); component.register_input("/remote/mouse/velocity", mouse_velocity); + component.register_input("/remote/mouse", mouse); component.register_input("/predefined/timestamp", timestamp); component.register_input("/tf", tf); @@ -110,6 +183,13 @@ class EccentricDualYaw component.register_input("/gimbal/pitch/angle", pitch_angle); component.register_input("/gimbal/pitch/velocity", pitch_velocity); component.register_input("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu); + + component.register_input( + "/auto_aim/control_direction", auto_aim_control_direction, false); + component.register_input("/auto_aim/robot_center", auto_aim_robot_center, false); + component.register_input( + "/rmcs_navigation/enable_control", navigation_enable_control, false); + component.register_input("/rmcs_navigation/gimbal_toward", navigation_toward, false); } auto enable_control() const noexcept -> bool { @@ -121,10 +201,33 @@ class EccentricDualYaw return true; } + auto enable_autoaim() const noexcept -> bool { + using namespace rmcs_msgs; + if (*switch_right != Switch::UP && !mouse->right) + return false; + if (!auto_aim_control_direction.ready()) + return false; + const auto& dir = *auto_aim_control_direction; + if (!auto_aim_robot_center.ready()) + return false; + const auto& center = *auto_aim_robot_center; + return !dir.isZero() && std::isfinite(dir.x()) && std::isfinite(dir.y()) + && std::isfinite(dir.z()) && !center.isZero() && std::isfinite(center.x()) + && std::isfinite(center.y()) && std::isfinite(center.z()); + } + + auto enable_navigation() const noexcept -> bool { + if (!*navigation_enable_control || !navigation_toward.ready()) + return false; + + return true; + } + InputInterface joystick_left; InputInterface switch_right; InputInterface switch_left; InputInterface mouse_velocity; + InputInterface mouse; InputInterface timestamp; InputInterface tf; @@ -136,6 +239,11 @@ class EccentricDualYaw InputInterface pitch_angle; InputInterface pitch_velocity; InputInterface chassis_yaw_velocity_imu; + + InputInterface auto_aim_control_direction; + InputInterface auto_aim_robot_center; + InputInterface navigation_enable_control; + InputInterface navigation_toward; } input_{*this}; struct Output { @@ -169,8 +277,8 @@ class EccentricDualYaw pid::PidCalculator pitch_angle_pid_{pid::make_pid_calculator(*this, "pitch_angle_")}; pid::PidCalculator pitch_velocity_pid_{pid::make_pid_calculator(*this, "pitch_velocity_")}; - double manual_bottom_yaw_target_ = 0.0; - double manual_pitch_target_ = 0.0; + double stored_bottom_yaw_target_ = 0.0; + double stored_pitch_target_ = 0.0; double previous_actual_yaw_ = 0.0; std::chrono::steady_clock::time_point previous_yaw_timestamp_{}; @@ -199,8 +307,9 @@ class EccentricDualYaw auto enter_disabled_state() -> void { reset_all_controls(); - manual_bottom_yaw_target_ = current_bottom_world_yaw(); - manual_pitch_target_ = + solver_.update(EccentricDualYawSolver::SetDisabled{}); + stored_bottom_yaw_target_ = current_bottom_world_yaw(); + stored_pitch_target_ = std::clamp(limit_rad(*input_.pitch_angle), upper_limit_, lower_limit_); *output_.yaw_control_angle_error = kNaN; @@ -208,9 +317,9 @@ class EccentricDualYaw auto compute_actual_yaw_velocity(double actual_yaw) -> double { const auto now = *input_.timestamp; - const double dt = std::chrono::duration(now - previous_yaw_timestamp_).count(); + const auto dt = std::chrono::duration(now - previous_yaw_timestamp_).count(); double velocity = 0.0; - if (dt > 1e-6) + if (dt > kMinDt) velocity = limit_rad(actual_yaw - previous_actual_yaw_) / dt; previous_actual_yaw_ = actual_yaw; previous_yaw_timestamp_ = now; @@ -221,11 +330,11 @@ class EccentricDualYaw auto direction = fast_tf::cast( PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *input_.tf); Eigen::Vector3d vector = *direction; - if (vector.norm() > 1e-9) + if (vector.norm() > kEpsilon) vector.normalize(); else vector = Eigen::Vector3d::UnitX(); - const double xy_norm = std::hypot(vector.x(), vector.y()); + const auto xy_norm = std::hypot(vector.x(), vector.y()); return {std::atan2(vector.y(), vector.x()), std::atan2(-vector.z(), xy_norm)}; } @@ -234,68 +343,30 @@ class EccentricDualYaw BottomYawLink::DirectionVector{Eigen::Vector3d::UnitX()}, *input_.tf); Eigen::Vector3d vector = *direction; vector.z() = 0.0; - if (vector.norm() > 1e-9) + if (vector.norm() > kEpsilon) vector.normalize(); else vector = Eigen::Vector3d::UnitX(); return std::atan2(vector.y(), vector.x()); } - auto apply_control(const ControlTarget& target) -> void { - - constexpr auto friction_feedforward = [](double viscous_gain, double coulomb_gain, - double tanh_gain, double velocity) -> double { - return (viscous_gain * velocity) + (coulomb_gain * std::tanh(tanh_gain * velocity)); - }; - - const double current_bottom_angle = current_bottom_world_yaw(); - const double current_bottom_velocity = + auto apply_control( + double bottom_yaw_error, double top_yaw_error, double pitch_error, + double top_yaw_feedforward = 0.0) -> void { + const auto current_bottom_velocity = *input_.bottom_yaw_velocity + *input_.chassis_yaw_velocity_imu; - const double current_top_angle = limit_rad(*input_.top_yaw_angle); - const double current_pitch_angle = limit_rad(*input_.pitch_angle); - - const double bottom_yaw_error = limit_rad(target.bottom_yaw.target - current_bottom_angle); - const double top_yaw_error = limit_rad(target.top_yaw.target - current_top_angle); - const double pitch_error = limit_rad(target.pitch.target - current_pitch_angle); - - const double bottom_velocity_ref = - bottom_yaw_angle_pid_.update(bottom_yaw_error) + target.bottom_yaw.velocity_ff; - const double top_velocity_ref = - top_yaw_angle_pid_.update(top_yaw_error) + target.top_yaw.velocity_ff; - const double pitch_velocity_ref = - pitch_angle_pid_.update(pitch_error) + target.pitch.velocity_ff; - - const double bottom_world_velocity_ff = - target.bottom_yaw.velocity_ff + *input_.chassis_yaw_velocity_imu; - const double top_yaw_continuous_torque_ff = - target.top_yaw.acceleration_ff + top_yaw_viscous_ff_gain_ * target.top_yaw.velocity_ff; - const double bottom_yaw_torque_ff = - target.bottom_yaw.acceleration_ff - + friction_feedforward( - bottom_yaw_viscous_ff_gain_, bottom_yaw_coulomb_ff_gain_, - bottom_yaw_coulomb_ff_tanh_gain_, bottom_world_velocity_ff) - - k_top_to_bottom_ * top_yaw_continuous_torque_ff; - - const double top_yaw_torque_ff = target.top_yaw.acceleration_ff - + friction_feedforward( - top_yaw_viscous_ff_gain_, top_yaw_coulomb_ff_gain_, - top_yaw_coulomb_ff_tanh_gain_, top_velocity_ref); - const double pitch_torque_ff = - target.pitch.acceleration_ff - + friction_feedforward( - pitch_viscous_ff_gain_, pitch_coulomb_ff_gain_, pitch_coulomb_ff_tanh_gain_, - pitch_velocity_ref) - + pitch_gravity_ff_gain_ * std::sin(current_pitch_angle - pitch_gravity_ff_phase_); - *output_.bottom_yaw_control_torque = - bottom_yaw_velocity_pid_.update(bottom_velocity_ref - current_bottom_velocity) - + bottom_yaw_torque_ff; + const auto bottom_velocity_ref = bottom_yaw_angle_pid_.update(bottom_yaw_error); + const auto top_velocity_ref = + top_yaw_angle_pid_.update(top_yaw_error) + top_yaw_feedforward; + const auto pitch_velocity_ref = pitch_angle_pid_.update(pitch_error); + *output_.top_yaw_control_torque = - top_yaw_velocity_pid_.update(top_velocity_ref - *input_.top_yaw_velocity) - + top_yaw_torque_ff; + top_yaw_velocity_pid_.update(top_velocity_ref - *input_.top_yaw_velocity); + *output_.bottom_yaw_control_torque = + bottom_yaw_velocity_pid_.update(bottom_velocity_ref - current_bottom_velocity); *output_.pitch_control_torque = - pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity) - + pitch_torque_ff; + pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity); *output_.yaw_control_angle_error = bottom_yaw_error; } @@ -304,5 +375,4 @@ class EccentricDualYaw } // namespace rmcs_core::controller::gimbal #include - PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::gimbal::EccentricDualYaw, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw_solver.hpp new file mode 100644 index 000000000..41409120f --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw_solver.hpp @@ -0,0 +1,160 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace rmcs_core::controller::gimbal { + +class EccentricDualYawSolver { +public: + struct Error { + double bottom_yaw = kNaN_; + double top_yaw = kNaN_; + double pitch = kNaN_; + bool valid = false; + }; + + class Operation { + friend class EccentricDualYawSolver; + virtual auto update(EccentricDualYawSolver& solver) const -> Error = 0; + }; + + auto update(const Operation& op) -> Error { return op.update(*this); } + auto enabled() const -> bool { return enabled_; } + + // top 关节的自瞄参考角（desired_top），供角速度前馈差分使用 + auto top_target_azimuth() const -> double { return top_target_azimuth_; } + + class SetDisabled : public Operation { + private: + auto update(EccentricDualYawSolver& s) const -> Error override { + s.enabled_ = false; + return {kNaN_, kNaN_, kNaN_, false}; + } + }; + + class AutoAim : public Operation { + public: + AutoAim( + const rmcs_description::Tf& tf, double top_yaw_angle, + const Eigen::Vector3d& control_direction, Eigen::Vector3d robot_center, + double upper_pitch, double lower_pitch) + : tf_(tf) + , top_yaw_angle_(top_yaw_angle) + , dir_(control_direction.normalized()) + , center_(std::move(robot_center)) + , upper_(upper_pitch) + , lower_(lower_pitch) {} + + private: + auto update(EccentricDualYawSolver& s) const -> Error override { + using namespace rmcs_description; + + const auto dir_gcl = + fast_tf::cast(OdomGimbalImu::DirectionVector{dir_}, tf_); + const auto ctr_gcl = + fast_tf::cast(OdomGimbalImu::Position{center_}, tf_); + const auto cam_gcl = fast_tf::lookup_transform(tf_); + const auto brl_imu = fast_tf::cast( + PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, tf_); + const auto btm_gcl = fast_tf::cast( + BottomYawLink::DirectionVector{Eigen::Vector3d::UnitX()}, tf_); + + const Eigen::Vector3d dir = normalize(*dir_gcl); + const Eigen::Vector3d brl = normalize(*brl_imu); + const Eigen::Vector3d btm = normalize(*btm_gcl); + const Eigen::Vector3d diff = *ctr_gcl - cam_gcl.translation(); + + const double center_azimuth = + (diff.head<2>().norm() > kEpsilon) ? std::atan2(diff.y(), diff.x()) : 0.0; + const double barrel_azimuth = std::atan2(dir.y(), dir.x()); + const double barrel_pitch = std::atan2(-dir_.z(), std::hypot(dir_.x(), dir_.y())); + const double current_btm = std::atan2(btm.y(), btm.x()); + const double current_brl = std::atan2(-brl.z(), std::hypot(brl.x(), brl.y())); + const double current_top = limit_rad(top_yaw_angle_); + + const double bottom_error = limit_rad(center_azimuth - current_btm); + const double desired_top = limit_rad(barrel_azimuth - center_azimuth); + const double top_error = limit_rad(desired_top - current_top); + s.top_target_azimuth_ = desired_top; + const double desired_pitch = std::clamp(barrel_pitch, upper_, lower_); + const double pitch_error = limit_rad(desired_pitch - current_brl); + + s.enabled_ = true; + return {bottom_error, top_error, pitch_error, true}; + } + + const rmcs_description::Tf& tf_; + double top_yaw_angle_; + Eigen::Vector3d dir_, center_; + double upper_, lower_; + }; + + class Navigation : public Operation { + public: + Navigation( + double top_yaw_angle, Eigen::Vector2d toward, double current_bottom_yaw, + double current_pitch_yaw, double fallback_bottom, double fallback_pitch, double upper, + double lower) + : top_yaw_angle_(top_yaw_angle) + , toward_(std::move(toward)) + , current_bottom_(current_bottom_yaw) + , current_pitch_(current_pitch_yaw) + , fallback_bottom_(fallback_bottom) + , fallback_pitch_(fallback_pitch) + , upper_(upper) + , lower_(lower) {} + + private: + auto update(EccentricDualYawSolver& s) const -> Error override { + double bx = std::isfinite(toward_.x()) ? toward_.x() : fallback_bottom_; + double by = std::isfinite(toward_.y()) ? toward_.y() : fallback_pitch_; + by = std::clamp(by, upper_, lower_); + + const double current_top = limit_rad(top_yaw_angle_); + + s.enabled_ = true; + return { + limit_rad(bx - current_bottom_), + limit_rad(0.0 - current_top), + limit_rad(by - current_pitch_), + true, + }; + } + double top_yaw_angle_; + Eigen::Vector2d toward_; + double current_bottom_, current_pitch_, fallback_bottom_, fallback_pitch_, upper_, lower_; + }; + +private: + static constexpr double kNaN_ = std::numeric_limits::quiet_NaN(); + static constexpr double kEpsilon = 1e-9; + + static auto limit_rad(double a) -> double { + constexpr double p = std::numbers::pi_v; + while (a > p) + a -= 2.0 * p; + while (a <= -p) + a += 2.0 * p; + return a; + } + + static auto normalize(const Eigen::Vector3d& v) -> Eigen::Vector3d { + double n = v.norm(); + if (n > kEpsilon) + return v / n; + return Eigen::Vector3d::UnitX(); + } + + bool enabled_ = false; + double top_target_azimuth_ = kNaN_; +}; + +} // namespace rmcs_core::controller::gimbal diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/player_viewer.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/player_viewer.cpp index d4d98715f..72822ef4a 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/player_viewer.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/player_viewer.cpp @@ -27,7 +27,6 @@ class PlayerViewer register_input("/remote/mouse/mouse_wheel", mouse_wheel_); register_input("/remote/keyboard", keyboard_); - register_input("/gimbal/player_viewer/angle", gimbal_pitch_angle_); register_input("/gimbal/player_viewer/raw_angle", gimbal_pitch_raw_angle_); register_input("/gimbal/player_viewer/angle", gimbal_player_viewer_angle_); @@ -54,16 +53,16 @@ class PlayerViewer || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { reset_all_controls(); } else { - if (!last_keyboard_.q && keyboard.q) { - scope_active_ = !scope_active_; - *is_scope_active_ = scope_active_; - scope_viewer_reset_ = scope_active_; - } if (!last_keyboard_.e && keyboard.e) { viewer_init_angle_ = keyboard.ctrl ? kCtrlInitViewerAngle : kEInitViewerAngle; viewer_reset_ = true; scope_viewer_reset_ = false; } + if (!last_keyboard_.q && keyboard.q) { + scope_active_ = !scope_active_; + *is_scope_active_ = scope_active_; + scope_viewer_reset_ = scope_active_; + } update_viewer_control(); }; @@ -106,7 +105,7 @@ class PlayerViewer *viewer_control_angle_ += *viewer_delta_angle_by_mouse_wheel_; } } - *viewer_control_angle_ = std::clamp(*viewer_control_angle_, upper_limit_, lower_limit_); + *viewer_control_angle_ = std::clamp(*viewer_control_angle_, lower_limit_, upper_limit_); auto norm_angle = [](double angle) { return (angle > pi_) ? angle - 2 * pi_ : angle; }; @@ -130,7 +129,7 @@ class PlayerViewer static constexpr double pi_ = std::numbers::pi; // The steering-hero viewer angle limit range is [0.68, 1.17]. - static constexpr double kEInitViewerAngle = 0.38905; // Move here when E is pressed. + static constexpr double kEInitViewerAngle = -0.065769; // Move here when E is pressed. static constexpr double kCtrlInitViewerAngle = 0.38905; // Move here when Ctrl is pressed. bool scope_viewer_reset_{false}; @@ -143,7 +142,6 @@ class PlayerViewer InputInterface keyboard_; InputInterface mouse_wheel_; - InputInterface gimbal_pitch_angle_; InputInterface gimbal_pitch_raw_angle_; InputInterface gimbal_player_viewer_angle_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp index 038319695..5c048d0ef 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp @@ -23,14 +23,10 @@ class TwoAxisGimbalSolver { }; public: - TwoAxisGimbalSolver( - rmcs_executor::Component& component, double upper_limit, double lower_limit, - bool use_encoder_pitch = false) + TwoAxisGimbalSolver(rmcs_executor::Component& component, double upper_limit, double lower_limit) : upper_limit_(std::cos(upper_limit), -std::sin(upper_limit)) - , lower_limit_(std::cos(lower_limit), -std::sin(lower_limit)) - , use_encoder_pitch_(use_encoder_pitch) { + , lower_limit_(std::cos(lower_limit), -std::sin(lower_limit)) { - component.register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); component.register_input("/tf", tf_); } @@ -114,9 +110,7 @@ class TwoAxisGimbalSolver { if (!control_enabled_) return {nan_, nan_}; - auto [control_direction_yaw_link, pitch] = - use_encoder_pitch_ ? pitch_link_to_yaw_link_from_encoder(control_direction) - : pitch_link_to_yaw_link(control_direction); + auto [control_direction_yaw_link, pitch] = pitch_link_to_yaw_link(control_direction); clamp_control_direction(control_direction_yaw_link); if (!control_enabled_) @@ -155,25 +149,6 @@ class TwoAxisGimbalSolver { return result; } - auto pitch_link_to_yaw_link_from_encoder(const PitchLink::DirectionVector& dir) const - -> std::pair { - - std::pair result; - auto& [dir_yaw_link, pitch_cs] = result; - - const double encoder_pitch = *gimbal_pitch_angle_; - pitch_cs = {std::cos(encoder_pitch), -std::sin(encoder_pitch)}; - - const auto& [x, y, z] = *dir; - dir_yaw_link = { - x * pitch_cs.x() - z * pitch_cs.y(), - y, - x * pitch_cs.y() + z * pitch_cs.x(), - }; - - return result; - } - static PitchLink::DirectionVector yaw_link_to_pitch_link(const YawLink::DirectionVector& dir, const Eigen::Vector2d& pitch) { @@ -238,9 +213,6 @@ class TwoAxisGimbalSolver { static constexpr double nan_ = std::numeric_limits::quiet_NaN(); const Eigen::Vector2d upper_limit_, lower_limit_; - bool use_encoder_pitch_ = false; - - rmcs_executor::Component::InputInterface gimbal_pitch_angle_; rmcs_executor::Component::InputInterface tf_; double yaw_cw_min_ = 0.; diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp index 4b845caae..a9420de94 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp @@ -14,8 +14,7 @@ class BulletFeederController17mm BulletFeederController17mm() : Node( get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , logger_(get_logger()) { + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { double bullets_per_feeder_turn = get_parameter("bullets_per_feeder_turn").as_double(); double bullet_feeder_angle_per_bullet = 2 * std::numbers::pi / bullets_per_feeder_turn; @@ -50,6 +49,7 @@ class BulletFeederController17mm register_input("/remote/keyboard", keyboard_); register_input("/auto_aim/should_shoot", should_shoot_, false); + register_input("/auto_aim/single_shoot", single_shoot_, false); register_input("/gimbal/bullet_feeder/velocity", bullet_feeder_velocity_); register_output( @@ -61,6 +61,8 @@ class BulletFeederController17mm void before_updating() override { if (!should_shoot_.ready()) should_shoot_.bind_directly(false); + if (!single_shoot_.ready()) + single_shoot_.bind_directly(false); } void update() override { @@ -72,6 +74,8 @@ class BulletFeederController17mm const auto keyboard = *keyboard_; using namespace rmcs_msgs; + const bool current_should_shoot = *should_shoot_; + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { reset_all_controls(); @@ -79,19 +83,27 @@ class BulletFeederController17mm std::int64_t bullet_allowance = 0; if (switch_right != Switch::DOWN) { - shoot_mode = keyboard.f ? ShootMode::SINGLE : ShootMode::AUTOMATIC; + // @NOTE: Single shoot is only for rune mode, so remove manual switch + // shoot_mode = keyboard.f ? ShootMode::SINGLE : ShootMode::AUTOMATIC; + shoot_mode = ShootMode::AUTOMATIC; single_shot_stop_counter_ = std::max(0, single_shot_stop_counter_ - 1); temporary_single_shot_counter_ = std::max(0, temporary_single_shot_counter_ - 1); - if (!last_mouse_.left && mouse.left) + const auto current_single_shoot = *single_shoot_; + + /* */ if (!last_mouse_.left && mouse.left) { // NOLINT single_shot_stop_counter_ = single_shot_max_stop_delay_; - else if (last_switch_left_ != Switch::DOWN && switch_left == Switch::DOWN) { + } else if (last_switch_left_ != Switch::DOWN && switch_left == Switch::DOWN) { single_shot_stop_counter_ = single_shot_max_stop_delay_; temporary_single_shot_counter_ = 500; + } else if (current_single_shoot && current_should_shoot && !last_should_shoot_) { + single_shot_stop_counter_ = single_shot_max_stop_delay_; } shoot_mode = temporary_single_shot_counter_ > 0 ? ShootMode::SINGLE : shoot_mode; + if (current_single_shoot) + shoot_mode = ShootMode::SINGLE; if (*bullet_fired_) single_shot_stop_counter_ = 0; @@ -118,6 +130,7 @@ class BulletFeederController17mm last_switch_left_ = switch_left; last_mouse_ = mouse; last_keyboard_ = keyboard; + last_should_shoot_ = current_should_shoot; } private: @@ -163,14 +176,15 @@ class BulletFeederController17mm } else { if (bullet_feeder_working_status_ == 500) { enter_jam_protection(); - RCLCPP_INFO(logger_, "Instant jammed! Count = %d", bullet_feeder_jammed_count_); + RCLCPP_INFO( + get_logger(), "Instant jammed! Count = %d", bullet_feeder_jammed_count_); } else if (bullet_feeder_working_status_ > 0) { bullet_feeder_working_status_ = 0; } else if (bullet_feeder_working_status_ > -500) { bullet_feeder_working_status_--; } else { enter_jam_protection(); - RCLCPP_INFO(logger_, "Jammed! Count = %d", bullet_feeder_jammed_count_); + RCLCPP_INFO(get_logger(), "Jammed! Count = %d", bullet_feeder_jammed_count_); } } } @@ -189,8 +203,6 @@ class BulletFeederController17mm static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - rclcpp::Logger logger_; - double bullet_feeder_working_velocity, bullet_feeder_safe_shot_velocity; double bullet_feeder_eject_velocity_, bullet_feeder_deep_eject_velocity_; int bullet_feeder_eject_time_, bullet_feeder_deep_eject_time_; @@ -207,6 +219,8 @@ class BulletFeederController17mm InputInterface keyboard_; InputInterface should_shoot_; + InputInterface single_shoot_; + bool last_should_shoot_ = false; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp index a83dc5f4d..76beb6868 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp @@ -45,10 +45,15 @@ class FrictionWheelController friction_count_ = friction_wheels.size(); friction_working_velocities_ = std::make_unique(friction_count_); friction_velocities_ = std::make_unique[]>(friction_count_); + friction_working_velocity_outputs_ = + std::make_unique[]>(friction_count_); friction_control_velocities_ = std::make_unique[]>(friction_count_); for (size_t i = 0; i < friction_count_; i++) { friction_working_velocities_[i] = friction_working_velocities[i]; register_input(friction_wheels[i] + "/velocity", friction_velocities_[i]); + register_output( + friction_wheels[i] + "/working_velocity", friction_working_velocity_outputs_[i], + friction_working_velocities_[i]); register_output( friction_wheels[i] + "/control_velocity", friction_control_velocities_[i], nan_); } @@ -74,6 +79,8 @@ class FrictionWheelController } if (switch_right != Switch::DOWN) { + update_friction_working_velocity_outputs(); + if ((!last_keyboard_.v && keyboard.v) || (last_switch_left_ == Switch::MIDDLE && switch_left == Switch::UP)) { friction_enabled_ = !friction_enabled_; @@ -107,6 +114,11 @@ class FrictionWheelController *friction_ready_ = *friction_jammed_ = *bullet_fired_ = false; } + void update_friction_working_velocity_outputs() { + for (size_t i = 0; i < friction_count_; i++) + *friction_working_velocity_outputs_[i] = friction_working_velocities_[i]; + } + void update_friction_velocities() { if (std::isnan(friction_soft_start_stop_percentage_)) { friction_soft_start_stop_percentage_ = 0.0; @@ -197,6 +209,7 @@ class FrictionWheelController std::unique_ptr friction_working_velocities_; std::unique_ptr[]> friction_velocities_; + std::unique_ptr[]> friction_working_velocity_outputs_; bool friction_enabled_ = false; @@ -219,4 +232,4 @@ class FrictionWheelController #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::shooting::FrictionWheelController, rmcs_executor::Component) \ No newline at end of file + rmcs_core::controller::shooting::FrictionWheelController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp index 0b0c63755..eadb30281 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp @@ -68,16 +68,6 @@ class PutterController set_pid_parameter(bullet_feeder_velocity_pid_, "bullet_feeder_velocity"); set_pid_parameter(putter_return_velocity_pid_, "putter_return_velocity"); - putter_velocity_pid_.kp = 0.004; - putter_velocity_pid_.ki = 0.0001; - putter_velocity_pid_.kd = 0.001; - putter_velocity_pid_.integral_max = 0.03; - putter_velocity_pid_.integral_min = 0.; - - putter_return_angle_pid.kp = 0.0001; - // putter_return_angle_pid.ki = 0.000001; - putter_return_angle_pid.kd = 0.; - register_output( "/gimbal/bullet_feeder/control_torque", bullet_feeder_control_torque_, nan_); register_output("/gimbal/putter/control_torque", putter_control_torque_, nan_); @@ -85,13 +75,17 @@ class PutterController register_output("/gimbal/shoot/delay_ms", shoot_delay_ms_, nan_); // auto_aim - register_input("/gimbal/auto_aim/fire_control", fire_control_, false); + register_input("/auto_aim/should_shoot", should_shoot_, false); register_output("/gimbal/shooter/mode", shoot_mode_, rmcs_msgs::ShootMode::SINGLE); register_output("/gimbal/shooter/condiction", shoot_condiction_); + register_output("/gimbal/shooter/preloaded_ready", preloaded_ready_, false); } - ~PutterController() {} + void before_updating() override { + if (!should_shoot_.ready()) + should_shoot_.bind_directly(false); + } void update() override { const auto switch_right = *switch_right_; @@ -108,13 +102,13 @@ class PutterController } // Normal control flow after the putter has been initialized. - if (putter_is_initialized_) { + if (putter_initialized) { // Handling during bullet-feeder jam-protection cooldown. if (bullet_feeder_reverse_end_ > 0) { bullet_feeder_reverse_end_--; // Early cooldown stage: reverse the feeder to clear the jam. - if (bullet_feeder_reverse_end_ > 500) + if (bullet_feeder_reverse_end_ > 300) *bullet_feeder_control_torque_ = bullet_feeder_velocity_pid_.update( -low_latency_velocity_ / 2 - *bullet_feeder_velocity_); else { @@ -123,6 +117,14 @@ class PutterController *bullet_feeder_control_torque_ = 0.0; } + if (!bullet_feeder_reverse_end_ && shoot_stage_ == ShootStage::PRELOADED) + // RCLCPP_INFO(get_logger(), "Reverse finished"); + { + *preloaded_ready_ = true; + } else { + *preloaded_ready_ = false; + } + } else { // Normal operating mode: only fire when the friction wheels are ready. if (*friction_ready_) { @@ -146,8 +148,7 @@ class PutterController && switch_left == rmcs_msgs::Switch::DOWN); const bool auto_fire_now = - (switch_right == Switch::UP || (mouse.right && mouse.left)) - && (*fire_control_); + (switch_right == Switch::UP || mouse.right) && *should_shoot_; const bool auto_trigger_emergence = mouse.right && (click_count_ >= 2); @@ -157,10 +158,10 @@ class PutterController if (manual_trigger || auto_trigger || auto_trigger_emergence) { if (*control_bullet_allowance_limited_by_heat_ > 0 - && (shoot_stage_ == ShootStage::PRELOADED || first_shot_)) { + && (shoot_stage_ == ShootStage::PRELOADED || shoot_first)) { set_shooting(); last_fire_time_ = now; - first_shot_ = false; + shoot_first = false; } } if (auto_trigger_emergence) { @@ -174,36 +175,32 @@ class PutterController low_latency_velocity_ - *bullet_feeder_velocity_); // Velocity loop. update_locked_detection(); - // This includes the photoelectric-sensor logic: if triggered, switch to // preloaded; otherwise reverse briefly and continue rotating. } if (shoot_stage_ == ShootStage::SHOOTING) { // Firing state: detect whether the bullet has been fired. - if (*bullet_fired_ - || *putter_angle_ - putter_start_point_ >= putter_stroke_) { - shot_fired_ = true; - } + // if (*bullet_fired_ && !shooted) { + // RCLCPP_INFO(get_logger(), "DETECT: Bullet fired!"); + // shooted = true; + // } - update_putter_jam_detection(); + // if (*putter_angle_ - putter_startpoint >= putter_stroke_ && !shooted) { + // RCLCPP_INFO(get_logger(), "DETECT: Putter stroke completed!"); + // shooted = true; + // } - if (shot_fired_) { + if (shooted) { // Bullet fired: return the putter. - const auto angle_err = putter_start_point_ - *putter_angle_; - if (angle_err > -0.1) { - *putter_control_torque_ = 0.; - set_preloading(); - shot_fired_ = false; - } else { - *putter_control_torque_ = - putter_return_velocity_pid_.update(-80. - *putter_velocity_); - putter_timeout_detection(); - } + *putter_control_torque_ = + putter_return_velocity_pid_.update(-50. - *putter_velocity_); + putter_timeout_detection(); } else { // Bullet not fired yet: continue advancing. *putter_control_torque_ = - putter_return_velocity_pid_.update(60. - *putter_velocity_); + putter_return_velocity_pid_.update(120. - *putter_velocity_); + update_putter_jam_detection(); } } } else { @@ -240,11 +237,9 @@ class PutterController shoot_stage_ = ShootStage::PRELOADED; - putter_is_initialized_ = false; - putter_start_point_ = nan_; + putter_initialized = false; + putter_startpoint = nan_; putter_return_velocity_pid_.reset(); - putter_velocity_pid_.reset(); - putter_return_angle_pid.reset(); *putter_control_torque_ = nan_; bullet_feeder_faulty_count_ = 0; @@ -252,54 +247,60 @@ class PutterController *shoot_delay_ms_ = nan_; } - void set_preloading() { shoot_stage_ = ShootStage::PRELOADING; } + void set_preloading() { + RCLCPP_INFO(get_logger(), "PRELOADING"); + shoot_stage_ = ShootStage::PRELOADING; + } - void set_preloaded() { shoot_stage_ = ShootStage::PRELOADED; } + void set_preloaded() { + RCLCPP_INFO(get_logger(), "PRELOADED"); + shoot_stage_ = ShootStage::PRELOADED; + } - void set_shooting() { shoot_stage_ = ShootStage::SHOOTING; } + void set_shooting() { + RCLCPP_INFO(get_logger(), "SHOOTING"); + shoot_stage_ = ShootStage::SHOOTING; + } void update_locked_detection() { // If feeder speed is near zero and the photoelectric sensor is triggered, // treat it as locked and start reversing. - if (*bullet_feeder_velocity_ < 0.5 && *bullet_feeder_control_torque_ > 0.1) { locked_detect_count_++; } else { locked_detect_count_ = 0; } - if (locked_detect_count_ > 300) { + if (locked_detect_count_ > 150) { if (*photoelectric_sensor_status_) { set_preloaded(); } // If the photoelectric sensor was not triggered, treat it as a simple jam, // reverse briefly, then continue until stall. locked_detect_count_ = 0; - enter_jam_protection(); + enter_reverse_protection(); } } void update_putter_jam_detection() { - if ((*putter_control_torque_ > -0.03 && shoot_stage_ == ShootStage::PRELOADING) - || (*putter_control_torque_ < 0.05 && shoot_stage_ == ShootStage::SHOOTING) - || std::isnan(*putter_control_torque_)) { + if (std::abs(*putter_velocity_) > 0.1 || std::isnan(*putter_control_torque_)) { putter_faulty_count_ = 0; - return; + } else { + putter_faulty_count_++; } // Accumulate a fault count when the torque is abnormal. - if (putter_faulty_count_ < 500) - ++putter_faulty_count_; - else { + if (putter_faulty_count_ >= 50) { putter_faulty_count_ = 0; if (shoot_stage_ != ShootStage::SHOOTING) { // Stall detected outside the firing state: the putter is in position, // so mark it initialized. - putter_is_initialized_ = true; - putter_start_point_ = *putter_angle_; + putter_initialized = true; + putter_startpoint = *putter_angle_; } else { // Stall detected during firing: treat the bullet as fired. - shot_fired_ = true; + RCLCPP_INFO(get_logger(), "DETECT: Putter freezed"); + shooted = true; } } } @@ -308,23 +309,23 @@ class PutterController // If the putter stays in the firing state too long without extending, // treat it as finished and move to the next state. if (shoot_stage_ == ShootStage::SHOOTING) { - if (shot_fired_) { - if (putter_timeout_count_ < 1600) + if (shooted) { + if (putter_timeout_count_ < 400) ++putter_timeout_count_; else { putter_timeout_count_ = 0; + RCLCPP_INFO(get_logger(), "PUTTER TIMEOUT"); set_preloading(); - shot_fired_ = false; + shooted = false; } } } } - void enter_jam_protection() { - // Set the target angle to 60 degrees behind the current angle. + void enter_reverse_protection() { locked_detect_count_ = 0; bullet_feeder_faulty_count_ = 0; - bullet_feeder_reverse_end_ = 800; + bullet_feeder_reverse_end_ = 400; bullet_feeder_velocity_pid_.reset(); } @@ -332,7 +333,7 @@ class PutterController std::numeric_limits::quiet_NaN(); ///< Not-a-number constant. static constexpr double inf_ = std::numeric_limits::infinity(); ///< Infinity constant. - static constexpr double putter_stroke_ = 11.5; ///< Putter stroke length. + static constexpr double putter_stroke_ = 12.0; ///< Putter stroke length. static constexpr double max_bullet_feeder_control_torque_ = 0.1; static constexpr double bullet_feeder_angle_per_bullet_ = 2 * std::numbers::pi / 6; @@ -341,8 +342,8 @@ class PutterController InputInterface photoelectric_sensor_status_; InputInterface grayscale_sensor_status_; InputInterface bullet_fired_; - bool shot_fired_{false}; - bool first_shot_{true}; + bool shooted{false}; + bool shoot_first{true}; InputInterface friction_ready_; @@ -361,15 +362,13 @@ class PutterController InputInterface control_bullet_allowance_limited_by_heat_; - bool putter_is_initialized_ = false; + bool putter_initialized = false; int putter_faulty_count_ = 0; int putter_timeout_count_ = 0; - double putter_start_point_ = nan_; + double putter_startpoint = nan_; pid::PidCalculator putter_return_velocity_pid_; InputInterface putter_velocity_; - pid::PidCalculator putter_velocity_pid_; - enum class ShootStage { PRELOADING, PRELOADED, SHOOTING }; ShootStage shoot_stage_ = ShootStage::PRELOADING; @@ -378,14 +377,13 @@ class PutterController OutputInterface bullet_feeder_control_torque_; InputInterface putter_angle_; - pid::PidCalculator putter_return_angle_pid; OutputInterface putter_control_torque_; int bullet_feeder_faulty_count_ = 0; OutputInterface shoot_delay_ms_; - InputInterface fire_control_; + InputInterface should_shoot_; std::chrono::steady_clock::time_point last_fire_time_{}; std::chrono::steady_clock::time_point last_click_time_{}; int click_count_ = 0; @@ -398,6 +396,7 @@ class PutterController OutputInterface shoot_mode_; OutputInterface shoot_condiction_; + OutputInterface preloaded_ready_; }; } // namespace rmcs_core::controller::shooting diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp index 6cefe2753..a3ee4b704 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp @@ -62,12 +62,12 @@ class ShootingRecorder velocities.erase(velocities.begin()); } - analysis3(); + analysis1(); auto log_text = std::string{}; auto timestamp = timestamp_to_string(*shoot_timestamp_); - if (friction_wheel_count_ == 4) { + if (friction_wheel_count_ == 6) { log_text = fmt::format( "{},{},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f}", *initial_speed_, (int)velocities.size(), // @@ -97,7 +97,7 @@ class ShootingRecorder InputInterface initial_speed_; InputInterface shoot_timestamp_; - std::size_t friction_wheel_count_ = 2; + std::size_t friction_wheel_count_ = 6; std::array, 2> friction_wheels_velocity_; /// @brief For log @@ -165,10 +165,10 @@ class ShootingRecorder int excellence_count = 0; int pass_count = 0; for (int i = 0; i < int(velocities.size()); i++) { - if (velocities[i] >= velocity_ - 0.1 && velocities[i] <= velocity_ + 0.1) { + if (velocities[i] >= velocity_ - 0.05 && velocities[i] <= velocity_ + 0.05) { pass_count += 1; } - if (velocities[i] >= velocity_ - 0.05 && velocities[i] <= velocity_ + 0.05) { + if (velocities[i] >= velocity_ - 0.025 && velocities[i] <= velocity_ + 0.025) { excellence_count += 1; } } @@ -224,10 +224,10 @@ class ShootingRecorder int pass_count = 0; for (const auto& v : velocities) { - if (v >= aim_velocity - 0.05 && v <= aim_velocity + 0.05) { + if (v >= aim_velocity - 0.025 && v <= aim_velocity + 0.025) { excellence_count += 1; } - if (v >= aim_velocity - 0.1 && v <= aim_velocity + 0.1) { + if (v >= aim_velocity - 0.05 && v <= aim_velocity + 0.05) { pass_count += 1; } } diff --git a/rmcs_ws/src/rmcs_core/src/debug/value_collector.cpp b/rmcs_ws/src/rmcs_core/src/debug/value_collector.cpp new file mode 100644 index 000000000..d5099e8fc --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/debug/value_collector.cpp @@ -0,0 +1,120 @@ +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::debug { + +class ValueCollector + : public rmcs_executor::Component + , public rclcpp::Node + , public rmcs_utility::NodeMixin { +public: + ValueCollector() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) {} + + auto before_pairing(const OutputInfoMap& output_map) -> void override { + node::param("csv_path", csv_path_); + + { + constexpr auto kPlaceholder = std::string_view{""}; + const auto pos = csv_path_.find(kPlaceholder); + if (pos != std::string::npos) { + const auto now = std::chrono::system_clock::now(); + const auto time = std::chrono::system_clock::to_time_t(now); + + auto ss = std::ostringstream{}; + ss << std::put_time(std::localtime(&time), "%Y-%m-%d_%H-%M-%S"); + csv_path_.replace(pos, kPlaceholder.size(), ss.str()); + } + } + + node::param("signals", signal_names_); + node::param("write_interval", write_interval_); + node::param("flush_interval", flush_interval_); + + if (csv_path_.empty() || signal_names_.empty()) + return; + + for (const auto& name : signal_names_) { + const auto it = output_map.find(name); + if (it == output_map.end()) { + node::error("signal '{}' not found", name); + continue; + } + if (it->second.type.get() != typeid(double)) { + node::error("signal '{}' type is not double", name); + continue; + } + + auto unit = std::make_unique(); + unit->name = name; + register_input(name, unit->value); + units_.push_back(std::move(unit)); + } + + if (units_.empty()) + return; + + csv_file_.open(csv_path_, std::ios::out | std::ios::trunc); + if (!csv_file_.is_open()) { + node::error("failed to open {}", csv_path_); + return; + } + + csv_file_ << "index"; + for (const auto& unit : units_) + csv_file_ << "," << unit->name; + csv_file_ << "\n"; + csv_file_.flush(); + + node::info("collecting {} signals to {}", units_.size(), csv_path_); + } + + auto update() -> void override { + if (units_.empty() || !csv_file_.is_open()) + return; + + if (tick_++ % write_interval_ != 0) + return; + + csv_file_ << sample_count_++; + for (const auto& unit : units_) + csv_file_ << "," << *unit->value; + csv_file_ << "\n"; + + if (sample_count_ % flush_interval_ == 0) + csv_file_.flush(); + } + +private: + struct SignalUnit { + std::string name; + InputInterface value; + }; + + std::string csv_path_; + std::vector signal_names_; + int write_interval_ = 1; + int flush_interval_ = 100; + + std::vector> units_; + std::ofstream csv_file_; + + int tick_ = 0; + int sample_count_ = 0; +}; + +} // namespace rmcs_core::debug + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::debug::ValueCollector, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp index 37be26aeb..a0bb9b3b1 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp @@ -750,8 +750,6 @@ class Text : public Shape { const char* value() const { return value_; } void set_value(const char* value) { - if (value_ == value) - return; value_ = value; set_modified(); } diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/animated_toggle.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/animated_toggle.hpp new file mode 100644 index 000000000..e61edbeed --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/animated_toggle.hpp @@ -0,0 +1,75 @@ +#pragma once + +#include +#include +#include + +namespace rmcs_core::referee::app::ui { + +class AnimatedToggle { +public: + using Clock = std::chrono::steady_clock; + + explicit AnimatedToggle( + std::chrono::duration duration = std::chrono::duration{0.5}) + : duration_(duration) {} + + void set_duration(std::chrono::duration duration) { + duration_ = std::max(duration, std::chrono::duration{0.0}); + } + + void reset(bool active) { + initialized_ = false; + value_ = active ? 1.0 : 0.0; + target_ = active; + } + + double update(Clock::time_point now, bool active) { + if (!initialized_) { + initialized_ = true; + start_time_ = now; + start_value_ = active ? 1.0 : 0.0; + end_value_ = start_value_; + value_ = start_value_; + target_ = active; + return value_; + } + + if (active != target_) { + target_ = active; + start_value_ = value_; + end_value_ = active ? 1.0 : 0.0; + start_time_ = now; + } + + if (duration_.count() <= 0.0) { + value_ = end_value_; + return value_; + } + + const double elapsed = std::chrono::duration{now - start_time_}.count(); + const double t = std::clamp(elapsed / duration_.count(), 0.0, 1.0); + value_ = std::lerp(start_value_, end_value_, ease_in_out_cubic_(t)); + return value_; + } + + double value() const { return value_; } + +private: + static double ease_in_out_cubic_(double t) { + t = std::clamp(t, 0.0, 1.0); + if (t < 0.5) + return 4.0 * t * t * t; + return 1.0 - std::pow(-2.0 * t + 2.0, 3.0) / 2.0; + } + + std::chrono::duration duration_; + Clock::time_point start_time_{}; + double start_value_ = 0.0; + double end_value_ = 0.0; + double value_ = 0.0; + bool initialized_ = false; + bool target_ = false; +}; + +} // namespace rmcs_core::referee::app::ui diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair_circle.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair_circle.hpp index 6b5bee863..a45a58c46 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair_circle.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair_circle.hpp @@ -14,8 +14,13 @@ class CrossHairCircle { : circle_(color, width, x, y, r, r, visible) {} void set_visible(bool value) { circle_.set_visible(value); } + void set_color(Shape::Color color) { circle_.set_color(color); } void set_r(uint16_t r) { circle_.set_r(r); } void set_width(uint16_t width) { circle_.set_width(width); } + void set_x(uint16_t x) { circle_.set_x(x); } + void set_y(uint16_t y) { circle_.set_y(y); } + uint16_t x() const { return circle_.x(); } + uint16_t y() const { return circle_.y(); } private: Circle circle_; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp index c45eaf932..8a9d573d1 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp @@ -249,10 +249,23 @@ class StatusRing { } void update_supercap(double value, bool enable) { - auto angle = 275 + calculate_angle(value, 10.5, supercap_limit_) + 1; + auto angle = 275 + calculate_angle(value, 8.0, supercap_limit_) + 1; supercap_status_.set_angle_end(static_cast(angle)); - if (value > 22.6) { + if (value > 20.0) { + supercap_status_.set_color(enable ? Shape::Color::CYAN : Shape::Color::GREEN); + } else if (value > 13.5) { + supercap_status_.set_color(enable ? Shape::Color::YELLOW : Shape::Color::ORANGE); + } else { + supercap_status_.set_color(enable ? Shape::Color::PURPLE : Shape::Color::PINK); + } + } + + void update_supercap_energy(double value, bool enable, double cutoff_voltage) { + auto angle = 275 + calculate_energy_angle(value, cutoff_voltage, supercap_limit_) + 1; + supercap_status_.set_angle_end(static_cast(angle)); + + if (value > 20.0) { supercap_status_.set_color(enable ? Shape::Color::CYAN : Shape::Color::GREEN); } else if (value > 13.5) { supercap_status_.set_color(enable ? Shape::Color::YELLOW : Shape::Color::ORANGE); @@ -308,6 +321,14 @@ class StatusRing { return visible_angle * std::clamp(value - min, 0.0, max - min) / (max - min); } + static constexpr double + calculate_energy_angle(double value, double cutoff_voltage, double full_voltage) { + const double clamped_value = std::clamp(value, cutoff_voltage, full_voltage); + const double numerator = clamped_value * clamped_value - cutoff_voltage * cutoff_voltage; + const double denominator = full_voltage * full_voltage - cutoff_voltage * cutoff_voltage; + return visible_angle * numerator / denominator; + } + void set_limits( double supercap_limit, double battery_limit, double friction_limit, int16_t bullet_limit) { supercap_limit_ = supercap_limit; From ce19b6db8a23bcbd7ab9220901f7b9032c52a40b Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Mon, 10 Aug 2026 18:06:53 +0800 Subject: [PATCH 05/86] feat(tools): Add system identification tools --- .../fit_friction_velocity_pid.py | 414 ++++++++ .script/identification/fit_gravity_torque.py | 495 +++++++++ .script/identification/fit_sweep_graybox.py | 969 ++++++++++++++++++ .../static_torque_test_controller.cpp | 469 +++++++++ .../swept_frequency_controller.cpp | 382 +++++++ 5 files changed, 2729 insertions(+) create mode 100644 .script/identification/fit_friction_velocity_pid.py create mode 100644 .script/identification/fit_gravity_torque.py create mode 100644 .script/identification/fit_sweep_graybox.py create mode 100644 rmcs_ws/src/rmcs_core/src/identification/static_torque_test_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/identification/swept_frequency_controller.cpp diff --git a/.script/identification/fit_friction_velocity_pid.py b/.script/identification/fit_friction_velocity_pid.py new file mode 100644 index 000000000..a60ebe418 --- /dev/null +++ b/.script/identification/fit_friction_velocity_pid.py @@ -0,0 +1,414 @@ +#!/usr/bin/env python3 + +from __future__ import annotations + +import argparse +import csv +import math +import statistics +import sys +from dataclasses import dataclass +from pathlib import Path + +import numpy as np + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description=( + "Fit a first-order friction-wheel speed plant from SweptFrequencyController CSV logs " + "and emit gains compatible with rmcs_core::controller::pid::PidController." + ) + ) + parser.add_argument("csv_paths", nargs="+", type=Path, help="One or more sweep CSV files") + parser.add_argument( + "--input-signal", + choices=("torque", "control_torque"), + default="torque", + help="Plant input column. Prefer measured torque when available.", + ) + parser.add_argument( + "--window-length", + type=int, + default=31, + help="Odd Savitzky-Golay window length used on velocity before differentiation.", + ) + parser.add_argument( + "--poly-order", + type=int, + default=3, + help="Savitzky-Golay polynomial order.", + ) + parser.add_argument( + "--trim-start", + type=float, + default=1.5, + help="Discard the first N seconds to remove enable transients.", + ) + parser.add_argument( + "--trim-end", + type=float, + default=0.5, + help="Discard the last N seconds to avoid sweep shutoff transients.", + ) + parser.add_argument( + "--fc-hz", + type=float, + default=None, + help="Target closed-loop crossover for all wheels. Default: front=22Hz, back=18Hz.", + ) + parser.add_argument( + "--td-ratio", + type=float, + default=0.0, + help="Derivative time as a fraction of the identified mechanical time constant. Keep 0 unless velocity is very clean.", + ) + parser.add_argument( + "--split-ratio", + type=float, + default=0.08, + help="Integral action only inside +/- split_ratio * steady_speed.", + ) + parser.add_argument( + "--i-output-ratio", + type=float, + default=0.35, + help="Clamp integral contribution to this fraction of the identified AC torque span.", + ) + parser.add_argument( + "--min-r2", + type=float, + default=0.80, + help="Warn when the acceleration fit R^2 drops below this value.", + ) + parser.add_argument( + "--group-average", + action="store_true", + help="Also emit averaged gain-only blocks for front wheels and back wheels.", + ) + return parser.parse_args() + + +@dataclass +class FitResult: + csv_path: Path + target: str + controller_name: str + dt: float + samples: int + steady_speed: float + steady_torque: float + inertia: float + damping: float + tau_mech: float + fit_r2: float + fc_hz: float + kp: float + ki: float + kd: float + integral_limit: float + split_limit: float + output_limit: float + torque_span: float + + +def savitzky_golay( + y: np.ndarray, dt: float, window_length: int, poly_order: int, deriv: int +) -> np.ndarray: + half_window = window_length // 2 + offsets = np.arange(-half_window, half_window + 1, dtype=float) * dt + vandermonde = np.vstack([offsets**order for order in range(poly_order + 1)]).T + coefficients = math.factorial(deriv) * np.linalg.pinv(vandermonde)[deriv] + windows = np.lib.stride_tricks.sliding_window_view(y, window_length) + return windows @ coefficients + + +def infer_target(fieldnames: list[str]) -> str: + candidates = { + name[: -len("/velocity")] + for name in fieldnames + if name.endswith("/velocity") and name != "velocity" + } + if len(candidates) != 1: + raise ValueError("failed to infer a unique target from CSV headers") + return next(iter(candidates)) + + +def require_columns(fieldnames: list[str], target: str) -> dict[str, str]: + columns = { + "elapsed_s": "elapsed_s", + "control_torque": f"{target}/control_torque", + "torque": f"{target}/torque", + "velocity": f"{target}/velocity", + } + missing = [value for value in columns.values() if value not in fieldnames] + if missing: + raise ValueError(f"missing required columns: {', '.join(missing)}") + return columns + + +def default_fc_hz(target: str) -> float: + if "front" in target: + return 22.0 + if "back" in target: + return 18.0 + return 20.0 + + +def controller_name_for(target: str) -> str: + return target.rsplit("/", 1)[-1] + "_velocity_pid_controller" + + +def read_csv(path: Path, input_signal: str) -> tuple[str, np.ndarray, np.ndarray, np.ndarray]: + with path.open(newline="") as csv_file: + reader = csv.DictReader(csv_file) + rows = list(reader) + if reader.fieldnames is None: + raise ValueError(f"CSV has no header: {path}") + if not rows: + raise ValueError(f"CSV has no data rows: {path}") + target = infer_target(reader.fieldnames) + columns = require_columns(reader.fieldnames, target) + + elapsed = np.array([float(row[columns["elapsed_s"]]) for row in rows], dtype=float) + velocity = np.array([float(row[columns["velocity"]]) for row in rows], dtype=float) + torque = np.array([float(row[columns[input_signal]]) for row in rows], dtype=float) + return target, elapsed, velocity, torque + + +def fit_first_order_model( + elapsed: np.ndarray, velocity: np.ndarray, torque: np.ndarray, args: argparse.Namespace +) -> tuple[np.ndarray, np.ndarray, np.ndarray, float]: + if args.window_length < 5 or args.window_length % 2 == 0: + raise ValueError("window-length must be an odd integer >= 5") + if args.poly_order < 2 or args.poly_order >= args.window_length: + raise ValueError("poly-order must be >= 2 and smaller than window-length") + if elapsed.size <= args.window_length: + raise ValueError("too few samples for the requested Savitzky-Golay window") + + dt = float(np.median(np.diff(elapsed))) + if not math.isfinite(dt) or dt <= 0.0: + raise ValueError("failed to determine a valid dt from elapsed_s") + + velocity_smooth = savitzky_golay(velocity, dt, args.window_length, args.poly_order, deriv=0) + acceleration_smooth = savitzky_golay( + velocity, dt, args.window_length, args.poly_order, deriv=1 + ) + + half_window = args.window_length // 2 + center = slice(half_window, elapsed.size - half_window) + elapsed_fit = elapsed[center] + torque_fit = torque[center] + + mask = np.isfinite(elapsed_fit) + mask &= np.isfinite(velocity_smooth) + mask &= np.isfinite(acceleration_smooth) + mask &= np.isfinite(torque_fit) + if args.trim_start > 0.0: + mask &= elapsed_fit >= args.trim_start + if args.trim_end > 0.0: + mask &= elapsed_fit <= elapsed_fit[-1] - args.trim_end + if int(np.count_nonzero(mask)) < 20: + raise ValueError("too few usable samples remain after trimming") + + return velocity_smooth[mask], acceleration_smooth[mask], torque_fit[mask], dt + + +def normalize_sign( + velocity: np.ndarray, acceleration: np.ndarray, torque: np.ndarray +) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + if float(np.median(velocity)) >= 0.0: + return velocity, acceleration, torque + return -velocity, -acceleration, -torque + + +def fit_one(path: Path, args: argparse.Namespace) -> FitResult: + target, elapsed, velocity, torque = read_csv(path, args.input_signal) + velocity_fit, acceleration_fit, torque_fit, dt = fit_first_order_model( + elapsed, velocity, torque, args + ) + velocity_fit, acceleration_fit, torque_fit = normalize_sign( + velocity_fit, acceleration_fit, torque_fit + ) + + steady_speed = float(np.median(velocity_fit)) + steady_torque = float(np.median(torque_fit)) + regressors = np.column_stack([velocity_fit - steady_speed, torque_fit - steady_torque]) + coefficients, _, _, _ = np.linalg.lstsq(regressors, acceleration_fit, rcond=None) + a_vel, b_tau = coefficients.tolist() + if not math.isfinite(a_vel) or not math.isfinite(b_tau): + raise ValueError("least-squares result is not finite") + if b_tau <= 0.0: + raise ValueError(f"identified torque gain <= 0 (b={b_tau:.6e})") + if a_vel >= 0.0: + raise ValueError(f"identified damping slope >= 0 (a={a_vel:.6e})") + + prediction = regressors @ coefficients + residual = acceleration_fit - prediction + centered = acceleration_fit - float(np.mean(acceleration_fit)) + ss_res = float(residual @ residual) + ss_tot = float(centered @ centered) + fit_r2 = float("nan") if ss_tot <= 0.0 else 1.0 - ss_res / ss_tot + + inertia = 1.0 / b_tau + damping = -a_vel / b_tau + tau_mech = inertia / damping + fc_hz = args.fc_hz if args.fc_hz is not None else default_fc_hz(target) + wc = 2.0 * math.pi * fc_hz + + kp = wc * inertia + ki = wc * damping * dt + td = max(0.0, args.td_ratio) * tau_mech + kd = 0.0 if td == 0.0 else (kp * td) / dt + + torque_span = float(np.percentile(np.abs(torque_fit - steady_torque), 95)) + max_i_output = max(0.05, args.i_output_ratio * torque_span) + integral_limit = 0.0 if ki <= 0.0 else max_i_output / ki + split_limit = max(5.0, args.split_ratio * abs(steady_speed)) + output_limit = max( + 0.1, + abs(steady_torque) + 1.2 * torque_span, + 1.3 * float(np.percentile(np.abs(torque_fit), 99)), + ) + + return FitResult( + csv_path=path, + target=target, + controller_name=controller_name_for(target), + dt=dt, + samples=int(velocity_fit.size), + steady_speed=steady_speed, + steady_torque=steady_torque, + inertia=inertia, + damping=damping, + tau_mech=tau_mech, + fit_r2=fit_r2, + fc_hz=fc_hz, + kp=kp, + ki=ki, + kd=kd, + integral_limit=integral_limit, + split_limit=split_limit, + output_limit=output_limit, + torque_span=torque_span, + ) + + +def format_yaml_block(result: FitResult, controller_name: str | None = None) -> str: + controller_name = result.controller_name if controller_name is None else controller_name + return "\n".join( + [ + f"{controller_name}:", + " ros__parameters:", + f" measurement: {result.target}/velocity", + f" setpoint: {result.target}/control_velocity", + f" control: {result.target}/control_torque", + f" kp: {result.kp:.9f}", + f" ki: {result.ki:.9f}", + f" kd: {result.kd:.9f}", + f" integral_min: {-result.integral_limit:.6f}", + f" integral_max: {result.integral_limit:.6f}", + f" integral_split_min: {-result.split_limit:.6f}", + f" integral_split_max: {result.split_limit:.6f}", + f" output_min: {-result.output_limit:.6f}", + f" output_max: {result.output_limit:.6f}", + ] + ) + + +def format_gain_only_block(name: str, result: FitResult) -> str: + return "\n".join( + [ + f"{name}:", + " ros__parameters:", + f" kp: {result.kp:.9f}", + f" ki: {result.ki:.9f}", + f" kd: {result.kd:.9f}", + f" integral_min: {-result.integral_limit:.6f}", + f" integral_max: {result.integral_limit:.6f}", + f" integral_split_min: {-result.split_limit:.6f}", + f" integral_split_max: {result.split_limit:.6f}", + f" output_min: {-result.output_limit:.6f}", + f" output_max: {result.output_limit:.6f}", + ] + ) + + +def print_result(result: FitResult, min_r2: float) -> None: + print(f"CSV: {result.csv_path}") + print(f" target: {result.target}") + print(f" samples: {result.samples}") + print(f" dt: {result.dt:.6f} s") + print(f" steady_speed: {result.steady_speed:.6f} rad/s") + print(f" steady_torque: {result.steady_torque:.6f} N*m") + print(f" J: {result.inertia:.9f}") + print(f" B: {result.damping:.9f}") + print(f" tau_mech: {result.tau_mech:.6f} s") + print(f" fit R^2: {result.fit_r2:.6f}") + print(f" chosen fc: {result.fc_hz:.2f} Hz") + if math.isfinite(result.fit_r2) and result.fit_r2 < min_r2: + print( + " warning: fit quality is weak; reduce sweep amplitude or clean the data before using this PID" + ) + print(format_yaml_block(result)) + + +def make_average_result(group_name: str, members: list[FitResult]) -> FitResult: + template = members[0] + return FitResult( + csv_path=template.csv_path, + target=template.target, + controller_name=group_name, + dt=statistics.mean(item.dt for item in members), + samples=sum(item.samples for item in members), + steady_speed=statistics.mean(item.steady_speed for item in members), + steady_torque=statistics.mean(item.steady_torque for item in members), + inertia=statistics.mean(item.inertia for item in members), + damping=statistics.mean(item.damping for item in members), + tau_mech=statistics.mean(item.tau_mech for item in members), + fit_r2=statistics.mean(item.fit_r2 for item in members), + fc_hz=statistics.mean(item.fc_hz for item in members), + kp=statistics.mean(item.kp for item in members), + ki=statistics.mean(item.ki for item in members), + kd=statistics.mean(item.kd for item in members), + integral_limit=statistics.mean(item.integral_limit for item in members), + split_limit=statistics.mean(item.split_limit for item in members), + output_limit=statistics.mean(item.output_limit for item in members), + torque_span=statistics.mean(item.torque_span for item in members), + ) + + +def main() -> int: + args = parse_args() + results: list[FitResult] = [] + for path in args.csv_paths: + results.append(fit_one(path, args)) + + for index, result in enumerate(results): + if index: + print() + print_result(result, args.min_r2) + + if args.group_average: + front = [item for item in results if "front" in item.target] + back = [item for item in results if "back" in item.target] + if front: + print() + print("Front average:") + average = make_average_result("front_friction_velocity_pid_average", front) + print(format_gain_only_block("front_friction_velocity_pid_average", average)) + if back: + print() + print("Back average:") + average = make_average_result("back_friction_velocity_pid_average", back) + print(format_gain_only_block("back_friction_velocity_pid_average", average)) + + return 0 + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except ValueError as error: + print(f"error: {error}", file=sys.stderr) + raise SystemExit(2) diff --git a/.script/identification/fit_gravity_torque.py b/.script/identification/fit_gravity_torque.py new file mode 100644 index 000000000..264d24de4 --- /dev/null +++ b/.script/identification/fit_gravity_torque.py @@ -0,0 +1,495 @@ +#!/usr/bin/env python3 + +from __future__ import annotations + +import argparse +import csv +import json +import math +import sys +from dataclasses import asdict, dataclass +from pathlib import Path +from typing import Iterable + + +def wrap_to_pi(angle: float) -> float: + wrapped = math.remainder(angle, 2.0 * math.pi) + if wrapped <= -math.pi: + wrapped += 2.0 * math.pi + return wrapped + + +def circular_mean(angles: list[float]) -> float: + sin_sum = sum(math.sin(angle) for angle in angles) + cos_sum = sum(math.cos(angle) for angle in angles) + if abs(sin_sum) < 1e-12 and abs(cos_sum) < 1e-12: + return wrap_to_pi(angles[0]) + return wrap_to_pi(math.atan2(sin_sum, cos_sum)) + + +@dataclass +class FitResult: + signal_name: str + sample_count: int + angle_min: float + angle_max: float + angle_span: float + gain: float + phase: float + phase_deg: float + zero_crossing_angle: float + zero_crossing_angle_deg: float + peak_angle: float + peak_angle_deg: float + rmse: float + mae: float + r2: float + design_condition: float + + +@dataclass +class PairingSummary: + available_point_count: int + paired_point_count: int + dropped_point_count: int + mean_control_friction: float + mean_torque_friction: float + + +@dataclass +class AveragedSample: + point_index: int + angle: float + setpoint: float + control_torque: float + torque: float + control_friction: float + torque_friction: float + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description=( + "Fit the gray-box gravity torque model u_ff = G * sin(theta + phi) " + "from static torque test CSV data." + ) + ) + parser.add_argument("csv_path", type=Path, help="Path to static_torque_test_controller CSV") + parser.add_argument( + "--target", + type=str, + default=None, + help="Target prefix such as /gimbal/pitch. If omitted, infer from CSV headers.", + ) + parser.add_argument( + "--signal", + choices=("control_torque", "torque"), + default="torque", + help="Primary signal to fit. Defaults to torque.", + ) + parser.add_argument( + "--velocity-threshold", + type=float, + default=0.1, + help=( + "Discard rows with abs(velocity) larger than this threshold. " + "Use a negative value to disable filtering." + ), + ) + parser.add_argument( + "--json-output", + type=Path, + default=None, + help="Optional path to write fit summary as JSON.", + ) + return parser.parse_args() + + +def infer_target(fieldnames: Iterable[str]) -> str: + angle_targets = {name[: -len("/angle")] for name in fieldnames if name.endswith("/angle")} + if len(angle_targets) != 1: + raise ValueError( + "Failed to infer target from CSV headers. Please pass --target explicitly." + ) + return next(iter(angle_targets)) + + +def read_rows(path: Path) -> tuple[list[dict[str, str]], list[str]]: + with path.open(newline="") as csv_file: + reader = csv.DictReader(csv_file) + rows = list(reader) + if reader.fieldnames is None: + raise ValueError(f"CSV file has no header: {path}") + return rows, reader.fieldnames + + +def require_columns(fieldnames: list[str], target: str) -> dict[str, str]: + columns = { + "update_count": "update_count", + "elapsed_s": "elapsed_s", + "control_torque": f"{target}/control_torque", + "torque": f"{target}/torque", + "velocity": f"{target}/velocity", + "angle": f"{target}/angle", + "point_index": "point_index", + "approach_direction": "approach_direction", + "setpoint": "setpoint", + } + + missing = [ + column + for key, column in columns.items() + if key in {"update_count", "elapsed_s", "control_torque", "torque", "velocity", "angle"} + and column not in fieldnames + ] + if missing: + joined = ", ".join(missing) + raise ValueError(f"Missing required CSV columns: {joined}") + return columns + + +def select_static_rows( + rows: list[dict[str, str]], columns: dict[str, str], velocity_threshold: float +) -> list[dict[str, str]]: + if velocity_threshold < 0.0: + return rows + + filtered_rows = [] + for row in rows: + velocity = float(row[columns["velocity"]]) + if abs(velocity) <= velocity_threshold: + filtered_rows.append(row) + return filtered_rows + + +def has_bidirectional_columns(fieldnames: list[str], columns: dict[str, str]) -> bool: + return all( + columns[key] in fieldnames for key in ("point_index", "approach_direction", "setpoint") + ) + + +def fit_sine_model(samples: list[tuple[float, float]], signal_name: str) -> FitResult: + if len(samples) < 3: + raise ValueError("Need at least 3 samples to fit the sine model.") + + s_s = 0.0 + s_c = 0.0 + c_c = 0.0 + s_y = 0.0 + c_y = 0.0 + angles: list[float] = [] + values: list[float] = [] + + for angle, value in samples: + wrapped_angle = wrap_to_pi(angle) + sin_theta = math.sin(wrapped_angle) + cos_theta = math.cos(wrapped_angle) + + s_s += sin_theta * sin_theta + s_c += sin_theta * cos_theta + c_c += cos_theta * cos_theta + s_y += sin_theta * value + c_y += cos_theta * value + + angles.append(wrapped_angle) + values.append(value) + + determinant = s_s * c_c - s_c * s_c + if abs(determinant) < 1e-12: + raise ValueError( + "The regression design matrix is singular. The angle coverage is likely too narrow." + ) + + sin_gain = (s_y * c_c - c_y * s_c) / determinant + cos_gain = (s_s * c_y - s_c * s_y) / determinant + + gain = math.hypot(sin_gain, cos_gain) + phase = math.atan2(cos_gain, sin_gain) + + predictions = [gain * math.sin(angle + phase) for angle in angles] + residuals = [value - prediction for value, prediction in zip(values, predictions)] + + mean_value = sum(values) / len(values) + ss_res = sum(residual * residual for residual in residuals) + ss_tot = sum((value - mean_value) ** 2 for value in values) + rmse = math.sqrt(ss_res / len(values)) + mae = sum(abs(residual) for residual in residuals) / len(values) + r2 = float("nan") if ss_tot <= 0.0 else 1.0 - ss_res / ss_tot + + trace = s_s + c_c + disc = math.sqrt(max(0.0, (s_s - c_c) ** 2 + 4.0 * s_c * s_c)) + lambda_max = 0.5 * (trace + disc) + lambda_min = 0.5 * (trace - disc) + condition = float("inf") if lambda_min <= 0.0 else lambda_max / lambda_min + + angle_min = min(angles) + angle_max = max(angles) + + zero_crossing_angle = wrap_to_pi(-phase) + peak_angle = wrap_to_pi(math.pi * 0.5 - phase) + + return FitResult( + signal_name=signal_name, + sample_count=len(values), + angle_min=angle_min, + angle_max=angle_max, + angle_span=angle_max - angle_min, + gain=gain, + phase=phase, + phase_deg=math.degrees(phase), + zero_crossing_angle=zero_crossing_angle, + zero_crossing_angle_deg=math.degrees(zero_crossing_angle), + peak_angle=peak_angle, + peak_angle_deg=math.degrees(peak_angle), + rmse=rmse, + mae=mae, + r2=r2, + design_condition=condition, + ) + + +def pair_bidirectional_rows( + rows: list[dict[str, str]], columns: dict[str, str] +) -> tuple[list[AveragedSample], PairingSummary]: + grouped_rows: dict[int, dict[int, dict[str, str]]] = {} + + for row in rows: + approach_direction = int(row[columns["approach_direction"]]) + if approach_direction not in (-1, +1): + continue + + point_index = int(row[columns["point_index"]]) + grouped_rows.setdefault(point_index, {})[approach_direction] = row + + averaged_samples: list[AveragedSample] = [] + control_friction_values: list[float] = [] + torque_friction_values: list[float] = [] + + for point_index in sorted(grouped_rows): + paired_rows = grouped_rows[point_index] + if -1 not in paired_rows or +1 not in paired_rows: + continue + + negative_row = paired_rows[-1] + positive_row = paired_rows[+1] + + angle = circular_mean( + [ + float(negative_row[columns["angle"]]), + float(positive_row[columns["angle"]]), + ] + ) + setpoint = circular_mean( + [ + float(negative_row[columns["setpoint"]]), + float(positive_row[columns["setpoint"]]), + ] + ) + + control_negative = float(negative_row[columns["control_torque"]]) + control_positive = float(positive_row[columns["control_torque"]]) + torque_negative = float(negative_row[columns["torque"]]) + torque_positive = float(positive_row[columns["torque"]]) + + control_friction = 0.5 * (control_positive - control_negative) + torque_friction = 0.5 * (torque_positive - torque_negative) + + averaged_samples.append( + AveragedSample( + point_index=point_index, + angle=angle, + setpoint=setpoint, + control_torque=0.5 * (control_positive + control_negative), + torque=0.5 * (torque_positive + torque_negative), + control_friction=control_friction, + torque_friction=torque_friction, + ) + ) + control_friction_values.append(abs(control_friction)) + torque_friction_values.append(abs(torque_friction)) + + available_point_count = len(grouped_rows) + paired_point_count = len(averaged_samples) + dropped_point_count = available_point_count - paired_point_count + + summary = PairingSummary( + available_point_count=available_point_count, + paired_point_count=paired_point_count, + dropped_point_count=dropped_point_count, + mean_control_friction=( + sum(control_friction_values) / len(control_friction_values) + if control_friction_values + else float("nan") + ), + mean_torque_friction=( + sum(torque_friction_values) / len(torque_friction_values) + if torque_friction_values + else float("nan") + ), + ) + return averaged_samples, summary + + +def print_fit(result: FitResult, label: str) -> None: + print(f"{label}:") + print(f" signal: {result.signal_name}") + print(f" samples: {result.sample_count}") + print( + " angle span: " + f"[{result.angle_min:.6f}, {result.angle_max:.6f}] rad " + f"({math.degrees(result.angle_span):.2f} deg)" + ) + print(f" G: {result.gain:.6f}") + print(f" phi: {result.phase:.6f} rad ({result.phase_deg:.2f} deg)") + print( + " model: " + f"u_ff = {result.gain:.6f} * sin(theta {'+' if result.phase >= 0.0 else '-'} " + f"{abs(result.phase):.6f})" + ) + print( + " zero crossing angle (-phi): " + f"{result.zero_crossing_angle:.6f} rad ({result.zero_crossing_angle_deg:.2f} deg)" + ) + print( + " peak angle (pi/2 - phi): " + f"{result.peak_angle:.6f} rad ({result.peak_angle_deg:.2f} deg)" + ) + print(f" RMSE: {result.rmse:.6f}") + print(f" MAE: {result.mae:.6f}") + print(f" R^2: {result.r2:.6f}") + print(f" design condition: {result.design_condition:.3f}") + + +def print_warnings(result: FitResult) -> None: + warnings: list[str] = [] + if math.degrees(result.angle_span) < 90.0: + warnings.append( + "angle coverage is below 90 deg, so phase identification may be weak or biased" + ) + if result.design_condition > 10.0: + warnings.append( + "sin/cos regressors are ill-conditioned for this dataset, so phase is sensitive to noise" + ) + if math.isfinite(result.r2) and result.r2 < 0.8: + warnings.append( + "R^2 is low; gravity may be mixed with friction, backlash, or insufficient settling" + ) + + if not warnings: + return + + print("Warnings:") + for item in warnings: + print(f" - {item}") + + +def raw_samples_from_rows( + rows: list[dict[str, str]], columns: dict[str, str], signal_key: str +) -> list[tuple[float, float]]: + return [ + (float(row[columns["angle"]]), float(row[columns[signal_key]])) + for row in rows + ] + + +def averaged_samples_to_tuples( + rows: list[AveragedSample], signal_key: str +) -> list[tuple[float, float]]: + return [(row.angle, getattr(row, signal_key)) for row in rows] + + +def main() -> int: + args = parse_args() + + rows, fieldnames = read_rows(args.csv_path) + target = args.target if args.target is not None else infer_target(fieldnames) + columns = require_columns(fieldnames, target) + + static_rows = select_static_rows(rows, columns, args.velocity_threshold) + if not static_rows: + raise ValueError("No rows remain after velocity filtering.") + + payload: dict[str, object] = { + "csv_path": str(args.csv_path), + "target": target, + "velocity_threshold": args.velocity_threshold, + "rows_total": len(rows), + "rows_used": len(static_rows), + } + + print(f"CSV: {args.csv_path}") + print(f"Target: {target}") + if args.velocity_threshold >= 0.0: + print(f"Velocity filter: abs({columns['velocity']}) <= {args.velocity_threshold}") + else: + print("Velocity filter: disabled") + print(f"Rows used: {len(static_rows)} / {len(rows)}") + + primary_signal_key = args.signal + reference_signal_key = "torque" if primary_signal_key == "control_torque" else "control_torque" + + if has_bidirectional_columns(fieldnames, columns): + averaged_samples, pairing_summary = pair_bidirectional_rows(static_rows, columns) + if not averaged_samples: + raise ValueError( + "No bidirectional pairs were found. The CSV may be incomplete or the test did not reach both directions." + ) + + primary_fit = fit_sine_model( + averaged_samples_to_tuples(averaged_samples, primary_signal_key), + f"paired_avg:{columns[primary_signal_key]}", + ) + reference_fit = fit_sine_model( + averaged_samples_to_tuples(averaged_samples, reference_signal_key), + f"paired_avg:{columns[reference_signal_key]}", + ) + + print("Mode: bidirectional pairing with friction cancellation") + print( + "Pairing: " + f"{pairing_summary.paired_point_count} paired / {pairing_summary.available_point_count} available points " + f"({pairing_summary.dropped_point_count} dropped)" + ) + print( + "Mean half-difference friction: " + f"control={pairing_summary.mean_control_friction:.6f}, " + f"torque={pairing_summary.mean_torque_friction:.6f}" + ) + + payload["pairing"] = asdict(pairing_summary) + payload["mode"] = "bidirectional" + else: + primary_fit = fit_sine_model( + raw_samples_from_rows(static_rows, columns, primary_signal_key), + columns[primary_signal_key], + ) + reference_fit = fit_sine_model( + raw_samples_from_rows(static_rows, columns, reference_signal_key), + columns[reference_signal_key], + ) + print("Mode: legacy single-direction fit without friction cancellation") + payload["mode"] = "legacy_single_direction" + + print_fit(primary_fit, "Primary fit") + print_fit(reference_fit, "Reference fit") + print_warnings(primary_fit) + print("Suggested params:") + print(f" gravity_gain: {primary_fit.gain:.6f}") + print(f" gravity_phase: {primary_fit.phase:.6f}") + + payload["primary_fit"] = asdict(primary_fit) + payload["reference_fit"] = asdict(reference_fit) + + if args.json_output is not None: + args.json_output.parent.mkdir(parents=True, exist_ok=True) + args.json_output.write_text(json.dumps(payload, indent=2) + "\n") + + return 0 + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except ValueError as error: + print(f"error: {error}", file=sys.stderr) + raise SystemExit(2) diff --git a/.script/identification/fit_sweep_graybox.py b/.script/identification/fit_sweep_graybox.py new file mode 100644 index 000000000..5ae876270 --- /dev/null +++ b/.script/identification/fit_sweep_graybox.py @@ -0,0 +1,969 @@ +#!/usr/bin/env python3 + +from __future__ import annotations + +import argparse +import csv +import json +import math +import sys +from dataclasses import asdict, dataclass +from pathlib import Path +from typing import Iterable + +import numpy as np + +try: + from scipy.optimize import least_squares +except ModuleNotFoundError: + least_squares = None + + +@dataclass +class OdeSegment: + dt: np.ndarray + angle: np.ndarray + velocity: np.ndarray + tau: np.ndarray + + +@dataclass +class GrayboxFitResult: + fit_mode: str + signal_name: str + residual_name: str + gravity_mode: str + sample_count: int + dt: float + window_length: int + poly_order: int + tanh_gain: float + angle_min: float + angle_max: float + angle_span: float + inertia: float + viscous_damping: float + coulomb_friction: float + gravity_gain: float + gravity_phase: float + gravity_phase_deg: float + gravity_zero_crossing: float + gravity_zero_crossing_deg: float + torque_bias: float + rmse: float + mae: float + r2: float + design_condition: float + shooting_window: float + segment_count: int + optimizer_nfev: int + optimizer_status: int + optimizer_success: bool + optimizer_message: str + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description=( + "Fit the gray-box sweep model " + "J * theta_dd + B * theta_d + Fc * tanh(k * theta_d) + G * sin(theta + phi) = tau_ff" + ) + ) + parser.add_argument("csv_path", type=Path, help="Path to swept_frequency_controller CSV") + parser.add_argument( + "--target", + type=str, + default=None, + help="Target prefix such as /gimbal/pitch. If omitted, infer from CSV headers.", + ) + parser.add_argument( + "--input-signal", + choices=("control_torque", "torque"), + default="torque", + help="Signal used as tau_ff. Defaults to torque.", + ) + parser.add_argument( + "--fit-mode", + choices=("linear", "ode-angle"), + default="linear", + help="linear uses differentiated-velocity regression; ode-angle integrates the ODE and fits angle only.", + ) + parser.add_argument( + "--window-length", + type=int, + default=31, + help="Odd Savitzky-Golay window length for smoothing velocity. Defaults to 31.", + ) + parser.add_argument( + "--poly-order", + type=int, + default=3, + help="Savitzky-Golay polynomial order for velocity smoothing and linear-mode differentiation.", + ) + parser.add_argument( + "--tanh-gain", + type=float, + default=100.0, + help="Gain k used in Fc * tanh(k * theta_d). Defaults to 100.", + ) + parser.add_argument( + "--trim-start", + type=float, + default=0.0, + help="Discard samples before this elapsed time in seconds.", + ) + parser.add_argument( + "--trim-end", + type=float, + default=0.0, + help="Discard samples within this many seconds from the end of the file.", + ) + parser.add_argument( + "--shooting-window", + type=float, + default=0.5, + help="Segment duration in seconds for ode-angle multiple shooting. Defaults to 0.5.", + ) + parser.add_argument( + "--ode-max-nfev", + type=int, + default=200, + help="Maximum least-squares function evaluations for ode-angle mode. Defaults to 200.", + ) + parser.add_argument( + "--fixed-gravity-gain", + type=float, + default=None, + help="If set together with --fixed-gravity-phase, hold gravity gain G fixed.", + ) + parser.add_argument( + "--fixed-gravity-phase", + type=float, + default=None, + help="If set together with --fixed-gravity-gain, hold gravity phase phi fixed in radians.", + ) + parser.add_argument( + "--json-output", + type=Path, + default=None, + help="Optional path to write fit summary as JSON.", + ) + return parser.parse_args() + + +def infer_target(fieldnames: Iterable[str]) -> str: + angle_targets = {name[: -len("/angle")] for name in fieldnames if name.endswith("/angle")} + if len(angle_targets) != 1: + raise ValueError( + "Failed to infer target from CSV headers. Please pass --target explicitly." + ) + return next(iter(angle_targets)) + + +def read_rows(path: Path) -> tuple[list[dict[str, str]], list[str]]: + with path.open(newline="") as csv_file: + reader = csv.DictReader(csv_file) + rows = list(reader) + if reader.fieldnames is None: + raise ValueError(f"CSV file has no header: {path}") + return rows, reader.fieldnames + + +def require_columns(fieldnames: list[str], target: str) -> dict[str, str]: + columns = { + "elapsed_s": "elapsed_s", + "control_torque": f"{target}/control_torque", + "torque": f"{target}/torque", + "velocity": f"{target}/velocity", + "angle": f"{target}/angle", + } + + missing = [column for column in columns.values() if column not in fieldnames] + if missing: + joined = ", ".join(missing) + raise ValueError(f"Missing required CSV columns: {joined}") + return columns + + +def validate_args(args: argparse.Namespace) -> None: + if args.fit_mode == "ode-angle" and least_squares is None: + raise ValueError( + "ode-angle mode requires scipy. Install it in .venv or run with a Python environment that has scipy." + ) + if args.window_length < 5 or args.window_length % 2 == 0: + raise ValueError("window-length must be an odd integer >= 5") + if args.poly_order < 2: + raise ValueError("poly-order must be >= 2") + if args.poly_order >= args.window_length: + raise ValueError("poly-order must be smaller than window-length") + if args.tanh_gain <= 0.0 or not math.isfinite(args.tanh_gain): + raise ValueError("tanh-gain must be finite and positive") + if args.trim_start < 0.0 or args.trim_end < 0.0: + raise ValueError("trim-start and trim-end must be non-negative") + if args.shooting_window <= 0.0 or not math.isfinite(args.shooting_window): + raise ValueError("shooting-window must be finite and positive") + if args.ode_max_nfev <= 0: + raise ValueError("ode-max-nfev must be positive") + fixed_gain_set = args.fixed_gravity_gain is not None + fixed_phase_set = args.fixed_gravity_phase is not None + if fixed_gain_set != fixed_phase_set: + raise ValueError( + "fixed-gravity-gain and fixed-gravity-phase must be provided together" + ) + if fixed_gain_set and ( + not math.isfinite(args.fixed_gravity_gain) + or not math.isfinite(args.fixed_gravity_phase) + or args.fixed_gravity_gain < 0.0 + ): + raise ValueError( + "fixed gravity parameters must be finite and fixed-gravity-gain must be non-negative" + ) + + +def savitzky_golay(y: np.ndarray, dt: float, window_length: int, poly_order: int, deriv: int) -> np.ndarray: + half_window = window_length // 2 + offsets = np.arange(-half_window, half_window + 1, dtype=float) * dt + vandermonde = np.vstack([offsets**order for order in range(poly_order + 1)]).T + coefficients = math.factorial(deriv) * np.linalg.pinv(vandermonde)[deriv] + windows = np.lib.stride_tricks.sliding_window_view(y, window_length) + return windows @ coefficients + + +def compute_metrics(measured: np.ndarray, prediction: np.ndarray) -> tuple[float, float, float]: + residual = measured - prediction + centered = measured - float(np.mean(measured)) + ss_res = float(residual @ residual) + ss_tot = float(centered @ centered) + rmse = math.sqrt(ss_res / measured.size) + mae = float(np.mean(np.abs(residual))) + r2 = float("nan") if ss_tot <= 0.0 else 1.0 - ss_res / ss_tot + return rmse, mae, r2 + + +def fit_linear_graybox( + angle: np.ndarray, + velocity: np.ndarray, + acceleration: np.ndarray, + tau_ff: np.ndarray, + signal_name: str, + dt: float, + window_length: int, + poly_order: int, + tanh_gain: float, +) -> GrayboxFitResult: + regressors = np.column_stack( + [ + acceleration, + velocity, + np.tanh(tanh_gain * velocity), + np.sin(angle), + np.cos(angle), + ] + ) + + coefficients, _, _, _ = np.linalg.lstsq(regressors, tau_ff, rcond=None) + prediction = regressors @ coefficients + + inertia, viscous_damping, coulomb_friction, gravity_sin, gravity_cos = coefficients.tolist() + gravity_gain = math.hypot(gravity_sin, gravity_cos) + gravity_phase = math.atan2(gravity_cos, gravity_sin) + + rmse, mae, r2 = compute_metrics(tau_ff, prediction) + + return GrayboxFitResult( + fit_mode="linear", + signal_name=signal_name, + residual_name=signal_name, + gravity_mode="free", + sample_count=int(tau_ff.size), + dt=dt, + window_length=window_length, + poly_order=poly_order, + tanh_gain=tanh_gain, + angle_min=float(np.min(angle)), + angle_max=float(np.max(angle)), + angle_span=float(np.max(angle) - np.min(angle)), + inertia=float(inertia), + viscous_damping=float(viscous_damping), + coulomb_friction=float(coulomb_friction), + gravity_gain=float(gravity_gain), + gravity_phase=float(gravity_phase), + gravity_phase_deg=float(math.degrees(gravity_phase)), + gravity_zero_crossing=float(-gravity_phase), + gravity_zero_crossing_deg=float(math.degrees(-gravity_phase)), + torque_bias=0.0, + rmse=rmse, + mae=mae, + r2=r2, + design_condition=float(np.linalg.cond(regressors)), + shooting_window=float("nan"), + segment_count=1, + optimizer_nfev=0, + optimizer_status=0, + optimizer_success=True, + optimizer_message="linear least squares", + ) + + +def fit_fixed_gravity_graybox( + angle: np.ndarray, + velocity: np.ndarray, + acceleration: np.ndarray, + tau_ff: np.ndarray, + signal_name: str, + dt: float, + window_length: int, + poly_order: int, + tanh_gain: float, + gravity_gain: float, + gravity_phase: float, +) -> GrayboxFitResult: + gravity_term = gravity_gain * np.sin(angle + gravity_phase) + regressors = np.column_stack( + [ + acceleration, + velocity, + np.tanh(tanh_gain * velocity), + ] + ) + + coefficients, _, _, _ = np.linalg.lstsq(regressors, tau_ff - gravity_term, rcond=None) + prediction = regressors @ coefficients + gravity_term + + inertia, viscous_damping, coulomb_friction = coefficients.tolist() + + rmse, mae, r2 = compute_metrics(tau_ff, prediction) + + return GrayboxFitResult( + fit_mode="linear", + signal_name=signal_name, + residual_name=signal_name, + gravity_mode="fixed", + sample_count=int(tau_ff.size), + dt=dt, + window_length=window_length, + poly_order=poly_order, + tanh_gain=tanh_gain, + angle_min=float(np.min(angle)), + angle_max=float(np.max(angle)), + angle_span=float(np.max(angle) - np.min(angle)), + inertia=float(inertia), + viscous_damping=float(viscous_damping), + coulomb_friction=float(coulomb_friction), + gravity_gain=float(gravity_gain), + gravity_phase=float(gravity_phase), + gravity_phase_deg=float(math.degrees(gravity_phase)), + gravity_zero_crossing=float(-gravity_phase), + gravity_zero_crossing_deg=float(math.degrees(-gravity_phase)), + torque_bias=0.0, + rmse=rmse, + mae=mae, + r2=r2, + design_condition=float(np.linalg.cond(regressors)), + shooting_window=float("nan"), + segment_count=1, + optimizer_nfev=0, + optimizer_status=0, + optimizer_success=True, + optimizer_message="linear least squares", + ) + + +def build_ode_segments( + elapsed: np.ndarray, + angle: np.ndarray, + velocity: np.ndarray, + tau_ff: np.ndarray, + shooting_window: float, + dt: float, +) -> list[OdeSegment]: + segment_samples = max(2, int(round(shooting_window / dt)) + 1) + segments: list[OdeSegment] = [] + start = 0 + while start < angle.size - 1: + stop = min(start + segment_samples, angle.size) + if stop - start < 2: + break + elapsed_segment = elapsed[start:stop] + segments.append( + OdeSegment( + dt=np.diff(elapsed_segment), + angle=angle[start:stop], + velocity=velocity[start:stop], + tau=tau_ff[start:stop], + ) + ) + if stop == angle.size: + break + start = stop - 1 + return segments + + +def build_ode_initial_guess( + angle: np.ndarray, + velocity: np.ndarray, + tau_ff: np.ndarray, + tanh_gain: float, + fixed_gravity_gain: float | None, + fixed_gravity_phase: float | None, +) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + tanh_velocity = np.tanh(tanh_gain * velocity) + tau_scale = max(1.0, float(np.percentile(np.abs(tau_ff), 95))) + velocity_scale = max(1e-3, float(np.percentile(np.abs(velocity), 95))) + damping_upper = max(10.0, 50.0 * tau_scale / velocity_scale) + friction_upper = max(10.0, 10.0 * tau_scale) + gravity_upper = max(10.0, 10.0 * tau_scale) + bias_limit = max(10.0, 5.0 * tau_scale) + + if fixed_gravity_gain is None: + regressors = np.column_stack( + [ + velocity, + tanh_velocity, + np.sin(angle), + np.cos(angle), + np.ones_like(angle), + ] + ) + coefficients, _, _, _ = np.linalg.lstsq(regressors, tau_ff, rcond=None) + damping0, friction0, gravity_sin0, gravity_cos0, bias0 = coefficients.tolist() + gravity_gain0 = math.hypot(gravity_sin0, gravity_cos0) + gravity_phase0 = math.atan2(gravity_cos0, gravity_sin0) + x0 = np.array( + [ + 0.01, + abs(damping0), + abs(friction0), + max(1e-6, gravity_gain0), + gravity_phase0, + bias0, + ], + dtype=float, + ) + lower = np.array([1e-6, 0.0, 0.0, 0.0, -4.0 * math.pi, -bias_limit], dtype=float) + upper = np.array( + [10.0, damping_upper, friction_upper, gravity_upper, 4.0 * math.pi, bias_limit], + dtype=float, + ) + return np.clip(x0, lower, upper), lower, upper + + gravity_term = fixed_gravity_gain * np.sin(angle + fixed_gravity_phase) + regressors = np.column_stack([velocity, tanh_velocity, np.ones_like(angle)]) + coefficients, _, _, _ = np.linalg.lstsq(regressors, tau_ff - gravity_term, rcond=None) + damping0, friction0, bias0 = coefficients.tolist() + x0 = np.array([0.01, abs(damping0), abs(friction0), bias0], dtype=float) + lower = np.array([1e-6, 0.0, 0.0, -bias_limit], dtype=float) + upper = np.array([10.0, damping_upper, friction_upper, bias_limit], dtype=float) + return np.clip(x0, lower, upper), lower, upper + + +def unpack_ode_parameters( + parameters: np.ndarray, + fixed_gravity_gain: float | None, + fixed_gravity_phase: float | None, +) -> tuple[float, float, float, float, float, float]: + if fixed_gravity_gain is None: + inertia, viscous_damping, coulomb_friction, gravity_gain, gravity_phase, torque_bias = ( + parameters.tolist() + ) + return ( + float(inertia), + float(viscous_damping), + float(coulomb_friction), + float(gravity_gain), + float(gravity_phase), + float(torque_bias), + ) + + inertia, viscous_damping, coulomb_friction, torque_bias = parameters.tolist() + return ( + float(inertia), + float(viscous_damping), + float(coulomb_friction), + float(fixed_gravity_gain), + float(fixed_gravity_phase), + float(torque_bias), + ) + + +def simulate_segment_angle( + segment: OdeSegment, + inertia: float, + viscous_damping: float, + coulomb_friction: float, + gravity_gain: float, + gravity_phase: float, + tanh_gain: float, + torque_bias: float, +) -> np.ndarray: + def angular_acceleration(theta: float, omega: float, tau: float) -> float: + return ( + tau + + torque_bias + - viscous_damping * omega + - coulomb_friction * math.tanh(tanh_gain * omega) + - gravity_gain * math.sin(theta + gravity_phase) + ) / inertia + + predicted = np.empty_like(segment.angle) + theta = float(segment.angle[0]) + omega = float(segment.velocity[0]) + predicted[0] = theta + + for index, delta_t in enumerate(segment.dt): + tau0 = float(segment.tau[index]) + tau1 = float(segment.tau[index + 1]) + tau_mid = 0.5 * (tau0 + tau1) + + k1_theta = omega + k1_omega = angular_acceleration(theta, omega, tau0) + + theta_k2 = theta + 0.5 * delta_t * k1_theta + omega_k2 = omega + 0.5 * delta_t * k1_omega + k2_theta = omega_k2 + k2_omega = angular_acceleration(theta_k2, omega_k2, tau_mid) + + theta_k3 = theta + 0.5 * delta_t * k2_theta + omega_k3 = omega + 0.5 * delta_t * k2_omega + k3_theta = omega_k3 + k3_omega = angular_acceleration(theta_k3, omega_k3, tau_mid) + + theta_k4 = theta + delta_t * k3_theta + omega_k4 = omega + delta_t * k3_omega + k4_theta = omega_k4 + k4_omega = angular_acceleration(theta_k4, omega_k4, tau1) + + theta += (delta_t / 6.0) * (k1_theta + 2.0 * k2_theta + 2.0 * k3_theta + k4_theta) + omega += (delta_t / 6.0) * (k1_omega + 2.0 * k2_omega + 2.0 * k3_omega + k4_omega) + predicted[index + 1] = theta + + return predicted + + +def simulate_shooting_prediction( + segments: list[OdeSegment], + inertia: float, + viscous_damping: float, + coulomb_friction: float, + gravity_gain: float, + gravity_phase: float, + tanh_gain: float, + torque_bias: float, +) -> tuple[np.ndarray, np.ndarray]: + measured_parts: list[np.ndarray] = [] + predicted_parts: list[np.ndarray] = [] + for segment in segments: + predicted = simulate_segment_angle( + segment, + inertia, + viscous_damping, + coulomb_friction, + gravity_gain, + gravity_phase, + tanh_gain, + torque_bias, + ) + measured_parts.append(segment.angle[1:]) + predicted_parts.append(predicted[1:]) + return np.concatenate(measured_parts), np.concatenate(predicted_parts) + + +def fit_ode_angle_graybox( + elapsed: np.ndarray, + angle: np.ndarray, + velocity: np.ndarray, + tau_ff: np.ndarray, + signal_name: str, + dt: float, + window_length: int, + poly_order: int, + tanh_gain: float, + shooting_window: float, + ode_max_nfev: int, + fixed_gravity_gain: float | None, + fixed_gravity_phase: float | None, +) -> GrayboxFitResult: + segments = build_ode_segments(elapsed, angle, velocity, tau_ff, shooting_window, dt) + if not segments: + raise ValueError("Too few samples remain to build ODE shooting segments.") + + x0, lower, upper = build_ode_initial_guess( + angle, velocity, tau_ff, tanh_gain, fixed_gravity_gain, fixed_gravity_phase + ) + + def residual_function(parameters: np.ndarray) -> np.ndarray: + ( + inertia, + viscous_damping, + coulomb_friction, + gravity_gain, + gravity_phase, + torque_bias, + ) = unpack_ode_parameters(parameters, fixed_gravity_gain, fixed_gravity_phase) + measured, predicted = simulate_shooting_prediction( + segments, + inertia, + viscous_damping, + coulomb_friction, + gravity_gain, + gravity_phase, + tanh_gain, + torque_bias, + ) + return predicted - measured + + optimization = least_squares( + residual_function, + x0, + bounds=(lower, upper), + method="trf", + x_scale=np.maximum(np.abs(x0), 1e-3), + max_nfev=ode_max_nfev, + ) + + ( + inertia, + viscous_damping, + coulomb_friction, + gravity_gain, + gravity_phase, + torque_bias, + ) = unpack_ode_parameters( + optimization.x, fixed_gravity_gain, fixed_gravity_phase + ) + measured_angle, predicted_angle = simulate_shooting_prediction( + segments, + inertia, + viscous_damping, + coulomb_friction, + gravity_gain, + gravity_phase, + tanh_gain, + torque_bias, + ) + rmse, mae, r2 = compute_metrics(measured_angle, predicted_angle) + + return GrayboxFitResult( + fit_mode="ode-angle", + signal_name=signal_name, + residual_name="angle", + gravity_mode="fixed" if fixed_gravity_gain is not None else "free", + sample_count=int(angle.size), + dt=dt, + window_length=window_length, + poly_order=poly_order, + tanh_gain=tanh_gain, + angle_min=float(np.min(angle)), + angle_max=float(np.max(angle)), + angle_span=float(np.max(angle) - np.min(angle)), + inertia=float(inertia), + viscous_damping=float(viscous_damping), + coulomb_friction=float(coulomb_friction), + gravity_gain=float(gravity_gain), + gravity_phase=float(gravity_phase), + gravity_phase_deg=float(math.degrees(gravity_phase)), + gravity_zero_crossing=float(-gravity_phase), + gravity_zero_crossing_deg=float(math.degrees(-gravity_phase)), + torque_bias=float(torque_bias), + rmse=rmse, + mae=mae, + r2=r2, + design_condition=float("nan"), + shooting_window=shooting_window, + segment_count=len(segments), + optimizer_nfev=int(optimization.nfev), + optimizer_status=int(optimization.status), + optimizer_success=bool(optimization.success), + optimizer_message=str(optimization.message), + ) + + +def print_fit(result: GrayboxFitResult, label: str) -> None: + print(f"{label}:") + print(f" fit mode: {result.fit_mode}") + print(f" signal: {result.signal_name}") + print(f" residual: {result.residual_name}") + print(f" gravity mode: {result.gravity_mode}") + print(f" samples: {result.sample_count}") + print( + " angle span: " + f"[{result.angle_min:.6f}, {result.angle_max:.6f}] rad " + f"({math.degrees(result.angle_span):.2f} deg)" + ) + print( + " model: " + f"{result.inertia:.6f} * theta_dd + {result.viscous_damping:.6f} * theta_d " + f"+ {result.coulomb_friction:.6f} * tanh({result.tanh_gain:.1f} * theta_d) " + f"+ {result.gravity_gain:.6f} * sin(theta {'+' if result.gravity_phase >= 0.0 else '-'} " + f"{abs(result.gravity_phase):.6f}) = tau_ff" + ) + print(f" J: {result.inertia:.6f}") + print(f" B: {result.viscous_damping:.6f}") + print(f" Fc: {result.coulomb_friction:.6f}") + print(f" G: {result.gravity_gain:.6f}") + print( + f" phi: {result.gravity_phase:.6f} rad ({result.gravity_phase_deg:.2f} deg)" + ) + print( + " gravity zero crossing (-phi): " + f"{result.gravity_zero_crossing:.6f} rad " + f"({result.gravity_zero_crossing_deg:.2f} deg)" + ) + if result.fit_mode == "ode-angle": + print( + f" shooting window: {result.shooting_window:.3f} s " + f"({result.segment_count} segments)" + ) + print(f" torque bias: {result.torque_bias:.6f}") + print( + " optimizer: " + f"success={result.optimizer_success}, " + f"status={result.optimizer_status}, " + f"nfev={result.optimizer_nfev}" + ) + print(f" Angle RMSE: {result.rmse:.6f}") + print(f" Angle MAE: {result.mae:.6f}") + else: + print(f" RMSE: {result.rmse:.6f}") + print(f" MAE: {result.mae:.6f}") + print(f" R^2: {result.r2:.6f}") + if math.isfinite(result.design_condition): + print(f" design condition: {result.design_condition:.3f}") + + +def print_warnings(result: GrayboxFitResult) -> None: + warnings: list[str] = [] + if result.inertia <= 0.0: + warnings.append("identified inertia J is non-positive") + if result.viscous_damping < 0.0: + warnings.append("identified viscous damping B is negative") + if result.coulomb_friction < 0.0: + warnings.append("identified Coulomb friction Fc is negative") + if math.degrees(result.angle_span) < 90.0: + warnings.append("angle coverage is below 90 deg, so gravity phase may be weakly observable") + if result.fit_mode == "linear": + if result.design_condition > 100.0: + warnings.append("regression matrix is ill-conditioned; parameters may be sensitive to noise") + if math.isfinite(result.r2) and result.r2 < 0.7: + warnings.append("R^2 is modest; derivative noise or model mismatch may still dominate") + else: + if not result.optimizer_success: + warnings.append(f"optimizer did not report success: {result.optimizer_message}") + if abs(result.torque_bias) > max(0.1, 0.2 * result.gravity_gain): + warnings.append("identified torque bias is large; input signal may have a non-zero offset") + if math.isfinite(result.r2) and result.r2 < 0.7: + warnings.append("angle-only fit is modest; excitation or gravity observability may still be weak") + + if not warnings: + return + + print("Warnings:") + for item in warnings: + print(f" - {item}") + + +def main() -> int: + args = parse_args() + validate_args(args) + + rows, fieldnames = read_rows(args.csv_path) + if not rows: + raise ValueError("CSV file is empty.") + + target = args.target if args.target is not None else infer_target(fieldnames) + columns = require_columns(fieldnames, target) + + elapsed = np.array([float(row[columns["elapsed_s"]]) for row in rows], dtype=float) + angle = np.array([float(row[columns["angle"]]) for row in rows], dtype=float) + velocity = np.array([float(row[columns["velocity"]]) for row in rows], dtype=float) + control_torque = np.array([float(row[columns["control_torque"]]) for row in rows], dtype=float) + measured_torque = np.array([float(row[columns["torque"]]) for row in rows], dtype=float) + + if elapsed.size <= args.window_length: + raise ValueError("Too few samples for the requested Savitzky-Golay window length.") + + dt = float(np.median(np.diff(elapsed))) + if not math.isfinite(dt) or dt <= 0.0: + raise ValueError("Failed to determine a valid time step from elapsed_s.") + + half_window = args.window_length // 2 + velocity_smooth = savitzky_golay( + velocity, dt, args.window_length, args.poly_order, deriv=0 + ) + + center_slice = slice(half_window, elapsed.size - half_window) + elapsed_valid = elapsed[center_slice] + angle_valid = np.unwrap(angle[center_slice]) + control_torque_valid = control_torque[center_slice] + measured_torque_valid = measured_torque[center_slice] + + mask = np.ones_like(elapsed_valid, dtype=bool) + if args.trim_start > 0.0: + mask &= elapsed_valid >= args.trim_start + if args.trim_end > 0.0: + mask &= elapsed_valid <= elapsed_valid[-1] - args.trim_end + + if int(np.count_nonzero(mask)) < 10: + raise ValueError("Too few samples remain after trimming.") + + elapsed_fit = elapsed_valid[mask] + angle_fit = angle_valid[mask] + velocity_fit = velocity_smooth[mask] + control_torque_fit = control_torque_valid[mask] + measured_torque_fit = measured_torque_valid[mask] + + primary_signal = ( + control_torque_fit if args.input_signal == "control_torque" else measured_torque_fit + ) + primary_signal_name = columns[args.input_signal] + reference_signal = ( + measured_torque_fit if args.input_signal == "control_torque" else control_torque_fit + ) + reference_signal_name = ( + columns["torque"] if args.input_signal == "control_torque" else columns["control_torque"] + ) + + if args.fit_mode == "linear": + acceleration = savitzky_golay( + velocity, dt, args.window_length, args.poly_order, deriv=1 + ) + acceleration_fit = acceleration[mask] + fit_function = fit_linear_graybox + fit_kwargs: dict[str, float] = {} + if args.fixed_gravity_gain is not None: + fit_function = fit_fixed_gravity_graybox + fit_kwargs = { + "gravity_gain": args.fixed_gravity_gain, + "gravity_phase": args.fixed_gravity_phase, + } + + primary_fit = fit_function( + angle_fit, + velocity_fit, + acceleration_fit, + primary_signal, + primary_signal_name, + dt, + args.window_length, + args.poly_order, + args.tanh_gain, + **fit_kwargs, + ) + reference_fit = fit_function( + angle_fit, + velocity_fit, + acceleration_fit, + reference_signal, + reference_signal_name, + dt, + args.window_length, + args.poly_order, + args.tanh_gain, + **fit_kwargs, + ) + else: + primary_fit = fit_ode_angle_graybox( + elapsed_fit, + angle_fit, + velocity_fit, + primary_signal, + primary_signal_name, + dt, + args.window_length, + args.poly_order, + args.tanh_gain, + args.shooting_window, + args.ode_max_nfev, + args.fixed_gravity_gain, + args.fixed_gravity_phase, + ) + reference_fit = fit_ode_angle_graybox( + elapsed_fit, + angle_fit, + velocity_fit, + reference_signal, + reference_signal_name, + dt, + args.window_length, + args.poly_order, + args.tanh_gain, + args.shooting_window, + args.ode_max_nfev, + args.fixed_gravity_gain, + args.fixed_gravity_phase, + ) + + print(f"CSV: {args.csv_path}") + print(f"Target: {target}") + print(f"Rows loaded: {len(rows)}") + print(f"Rows used: {primary_fit.sample_count}") + print(f"Median dt: {dt:.6f} s") + print(f"Fit mode: {args.fit_mode}") + if args.fit_mode == "linear": + print( + f"Savitzky-Golay: window_length={args.window_length}, poly_order={args.poly_order}, " + f"source={columns['velocity']}" + ) + else: + print( + "Velocity smoothing: " + f"window_length={args.window_length}, poly_order={args.poly_order}, " + f"source={columns['velocity']}" + ) + print( + "ODE multiple shooting: " + f"window={args.shooting_window:.3f} s, max_nfev={args.ode_max_nfev}" + ) + if args.fixed_gravity_gain is not None: + print( + "Fixed gravity: " + f"G={args.fixed_gravity_gain:.6f}, phi={args.fixed_gravity_phase:.6f} rad" + ) + if args.trim_start > 0.0 or args.trim_end > 0.0: + print(f"Trim: start={args.trim_start:.3f} s, end={args.trim_end:.3f} s") + print_fit(primary_fit, "Primary fit") + print_fit(reference_fit, "Reference fit") + print_warnings(primary_fit) + print("Suggested params:") + print(f" inertia: {primary_fit.inertia:.6f}") + print(f" viscous_damping: {primary_fit.viscous_damping:.6f}") + print(f" coulomb_friction: {primary_fit.coulomb_friction:.6f}") + print(f" gravity_gain: {primary_fit.gravity_gain:.6f}") + print(f" gravity_phase: {primary_fit.gravity_phase:.6f}") + if args.fit_mode == "ode-angle": + print("Nuisance params:") + print(f" torque_bias: {primary_fit.torque_bias:.6f}") + + if args.json_output is not None: + args.json_output.parent.mkdir(parents=True, exist_ok=True) + payload = { + "csv_path": str(args.csv_path), + "target": target, + "input_signal": args.input_signal, + "fit_mode": args.fit_mode, + "rows_total": len(rows), + "rows_used": primary_fit.sample_count, + "dt": dt, + "window_length": args.window_length, + "poly_order": args.poly_order, + "tanh_gain": args.tanh_gain, + "trim_start": args.trim_start, + "trim_end": args.trim_end, + "shooting_window": args.shooting_window, + "ode_max_nfev": args.ode_max_nfev, + "fixed_gravity_gain": args.fixed_gravity_gain, + "fixed_gravity_phase": args.fixed_gravity_phase, + "primary_fit": asdict(primary_fit), + "reference_fit": asdict(reference_fit), + } + args.json_output.write_text(json.dumps(payload, indent=2) + "\n") + + return 0 + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except ValueError as error: + print(f"error: {error}", file=sys.stderr) + raise SystemExit(2) diff --git a/rmcs_ws/src/rmcs_core/src/identification/static_torque_test_controller.cpp b/rmcs_ws/src/rmcs_core/src/identification/static_torque_test_controller.cpp new file mode 100644 index 000000000..e537da8e8 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/identification/static_torque_test_controller.cpp @@ -0,0 +1,469 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "controller/pid/pid_calculator.hpp" + +namespace rmcs_core::controller::identification { + +namespace { + +using Clock = std::chrono::steady_clock; + +constexpr auto kCenterInfoInterval = std::chrono::duration(0.5); +constexpr double kRangeTolerance = 1e-9; + +template +T require_parameter(rclcpp::Node& node, const std::string& name) { + if (!node.has_parameter(name)) + throw std::runtime_error("Missing required parameter: " + name); + return node.get_parameter(name).get_value(); +} + +void load_optional_parameter(rclcpp::Node& node, const std::string& name, double& value) { + node.get_parameter(name, value); +} + +void configure_pid_limits( + rclcpp::Node& node, const std::string& prefix, pid::PidCalculator& calculator) { + load_optional_parameter(node, prefix + "_integral_min", calculator.integral_min); + load_optional_parameter(node, prefix + "_integral_max", calculator.integral_max); + load_optional_parameter(node, prefix + "_integral_split_min", calculator.integral_split_min); + load_optional_parameter(node, prefix + "_integral_split_max", calculator.integral_split_max); + load_optional_parameter(node, prefix + "_output_min", calculator.output_min); + load_optional_parameter(node, prefix + "_output_max", calculator.output_max); +} + +std::string normalize_target(std::string target) { + while (!target.empty() && target.back() == '/') + target.pop_back(); + + if (target.empty()) + throw std::runtime_error("Parameter 'target' cannot be empty"); + + if (target.front() != '/') + target.insert(target.begin(), '/'); + + return target; +} + +std::string interface_name(const std::string& target, std::string_view suffix) { + return target + "/" + std::string{suffix}; +} + +std::string sanitize_file_component(std::string_view text) { + std::string sanitized; + sanitized.reserve(text.size()); + + for (const char ch : text) { + if (std::isalnum(static_cast(ch))) + sanitized.push_back(ch); + else + sanitized.push_back('_'); + } + + if (sanitized.empty()) + sanitized = "target"; + + return sanitized; +} + +double wrap_to_pi(double angle) { + constexpr double kPi = std::numbers::pi_v; + angle = std::remainder(angle, 2.0 * kPi); + if (angle <= -kPi) + angle += 2.0 * kPi; + return angle; +} + +enum class RemoteMode { + kMeasureRange, + kCenterHold, + kTestCommand, + kIdle, +}; + +RemoteMode decode_remote_mode(rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right) { + using rmcs_msgs::Switch; + if (switch_left == Switch::MIDDLE && switch_right == Switch::DOWN) + return RemoteMode::kMeasureRange; + if (switch_left == Switch::MIDDLE && switch_right == Switch::MIDDLE) + return RemoteMode::kCenterHold; + if (switch_left == Switch::MIDDLE && switch_right == Switch::UP) + return RemoteMode::kTestCommand; + return RemoteMode::kIdle; +} + +} // namespace + +class StaticTorqueTestController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + StaticTorqueTestController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , target_(normalize_target(require_parameter(*this, "target"))) + , interval_angle_(require_parameter(*this, "interval_angle")) + , wait_time_s_(require_parameter(*this, "wait_time")) + , wait_duration_(std::chrono::duration(wait_time_s_)) + , border_clip_(require_parameter(*this, "border_clip")) + , control_torque_name_(interface_name(target_, "control_torque")) + , measured_torque_name_(interface_name(target_, "torque")) + , measured_velocity_name_(interface_name(target_, "velocity")) + , measured_angle_name_(interface_name(target_, "angle")) { + if (!std::isfinite(interval_angle_) || interval_angle_ <= 0.0) + throw std::runtime_error("interval_angle must be finite and positive"); + if (!std::isfinite(wait_time_s_) || wait_time_s_ <= 0.0) + throw std::runtime_error("wait_time must be finite and positive"); + if (!std::isfinite(border_clip_) || border_clip_ < 0.0) + throw std::runtime_error("border_clip must be finite and non-negative"); + + position_pid_.kp = require_parameter(*this, "position_kp"); + position_pid_.ki = require_parameter(*this, "position_ki"); + position_pid_.kd = require_parameter(*this, "position_kd"); + velocity_pid_.kp = require_parameter(*this, "velocity_kp"); + velocity_pid_.ki = require_parameter(*this, "velocity_ki"); + velocity_pid_.kd = require_parameter(*this, "velocity_kd"); + + configure_pid_limits(*this, "position", position_pid_); + configure_pid_limits(*this, "velocity", velocity_pid_); + + register_input("/predefined/update_count", update_count_); + register_input("/predefined/timestamp", timestamp_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/switch/right", switch_right_); + register_input(measured_torque_name_, measured_torque_); + register_input(measured_velocity_name_, measured_velocity_); + register_input(measured_angle_name_, measured_angle_); + + register_output(control_torque_name_, control_torque_, nan_); + } + + ~StaticTorqueTestController() override { stop_test(true); } + + void before_updating() override { + position_pid_.reset(); + velocity_pid_.reset(); + stop_test(true); + + *control_torque_ = nan_; + angle_tracking_initialized_ = false; + range_initialized_ = false; + remote_mode_initialized_ = false; + next_center_info_time_ = Clock::time_point{}; + } + + void update() override { + update_angle_tracking(*measured_angle_); + + const auto remote_mode = decode_remote_mode(*switch_left_, *switch_right_); + const bool mode_changed = !remote_mode_initialized_ || remote_mode != last_remote_mode_; + + switch (remote_mode) { + case RemoteMode::kMeasureRange: handle_measure_range(); break; + case RemoteMode::kCenterHold: handle_center_hold(mode_changed); break; + case RemoteMode::kTestCommand: handle_test_command(mode_changed); break; + case RemoteMode::kIdle: handle_idle(); break; + } + + last_remote_mode_ = remote_mode; + remote_mode_initialized_ = true; + } + +private: + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + + void update_angle_tracking(double raw_angle) { + current_wrapped_angle_ = wrap_to_pi(raw_angle); + + if (!angle_tracking_initialized_) { + current_continuous_angle_ = current_wrapped_angle_; + last_wrapped_angle_ = current_wrapped_angle_; + angle_tracking_initialized_ = true; + return; + } + + current_continuous_angle_ += wrap_to_pi(current_wrapped_angle_ - last_wrapped_angle_); + last_wrapped_angle_ = current_wrapped_angle_; + } + + void handle_measure_range() { + stop_test(true); + position_pid_.reset(); + velocity_pid_.reset(); + update_measured_range(); + *control_torque_ = nan_; + } + + void update_measured_range() { + if (!range_initialized_) { + range_min_ = current_continuous_angle_; + range_max_ = current_continuous_angle_; + range_initialized_ = true; + return; + } + + range_min_ = std::min(range_min_, current_continuous_angle_); + range_max_ = std::max(range_max_, current_continuous_angle_); + } + + void handle_center_hold(bool mode_changed) { + stop_test(true); + + if (!range_initialized_) { + *control_torque_ = nan_; + return; + } + + if (mode_changed) { + position_pid_.reset(); + velocity_pid_.reset(); + next_center_info_time_ = *timestamp_; + } + + active_setpoint_ = 0.5 * (range_min_ + range_max_); + *control_torque_ = calculate_pid_output(active_setpoint_); + + maybe_log_center_hold_info(); + } + + void maybe_log_center_hold_info() { + if (*timestamp_ < next_center_info_time_) + return; + + const double control_error = active_setpoint_ - current_continuous_angle_; + RCLCPP_INFO( + get_logger(), + "Center hold error=%.6f rad, setpoint=%.6f rad, angle=%.6f rad, range=[%.6f, %.6f] rad", + control_error, wrap_to_pi(active_setpoint_), current_wrapped_angle_, + wrap_to_pi(range_min_), wrap_to_pi(range_max_)); + + do { + next_center_info_time_ += + std::chrono::duration_cast(kCenterInfoInterval); + } while (*timestamp_ >= next_center_info_time_); + } + + void handle_test_command(bool mode_changed) { + if (mode_changed) { + if (last_remote_mode_ == RemoteMode::kCenterHold) { + if (!start_test()) { + *control_torque_ = nan_; + return; + } + } else if (!test_setpoint_valid_) { + *control_torque_ = nan_; + return; + } else { + position_pid_.reset(); + velocity_pid_.reset(); + } + } + + if (!test_setpoint_valid_) { + *control_torque_ = nan_; + return; + } + + *control_torque_ = calculate_pid_output(active_setpoint_); + + if (!test_active_) + return; + + if (*timestamp_ - dwell_start_time_ < wait_duration_) + return; + + record_test_point(); + + const double next_setpoint = active_setpoint_ + interval_angle_; + if (next_setpoint > clipped_range_max_ + kRangeTolerance) { + finish_test_sequence(); + return; + } + + active_setpoint_ = next_setpoint; + dwell_start_time_ = *timestamp_; + position_pid_.reset(); + velocity_pid_.reset(); + } + + bool start_test() { + stop_test(true); + + if (!range_initialized_) { + RCLCPP_WARN(get_logger(), "Cannot start static torque test before range is measured."); + return false; + } + + clipped_range_min_ = range_min_ + border_clip_; + clipped_range_max_ = range_max_ - border_clip_; + if (clipped_range_min_ > clipped_range_max_ + kRangeTolerance) { + RCLCPP_WARN( + get_logger(), + "border_clip=%.6f rad leaves no feasible range, measured span=%.6f rad", + border_clip_, range_max_ - range_min_); + return false; + } + + RCLCPP_INFO( + get_logger(), + "Measured mechanical limits continuous=[%.6f, %.6f] rad, wrapped=[%.6f, %.6f] rad, " + "clipped=[%.6f, %.6f] rad", + range_min_, range_max_, wrap_to_pi(range_min_), wrap_to_pi(range_max_), + wrap_to_pi(clipped_range_min_), wrap_to_pi(clipped_range_max_)); + + const auto path = build_csv_path(); + try { + csv_writer_.open(path); + csv_writer_.write_row( + "update_count", "elapsed_s", control_torque_name_, measured_torque_name_, + measured_velocity_name_, measured_angle_name_); + csv_writer_.flush(); + } catch (const std::exception& exception) { + const auto path_string = path.string(); + RCLCPP_ERROR( + get_logger(), "Failed to start static torque log '%s': %s", path_string.c_str(), + exception.what()); + return false; + } + + current_csv_path_ = path; + test_start_time_ = *timestamp_; + dwell_start_time_ = *timestamp_; + active_setpoint_ = clipped_range_min_; + test_setpoint_valid_ = true; + test_active_ = true; + position_pid_.reset(); + velocity_pid_.reset(); + + const auto path_string = current_csv_path_.string(); + RCLCPP_INFO( + get_logger(), "Started static torque test at %.6f rad, log=%s", + wrap_to_pi(active_setpoint_), path_string.c_str()); + return true; + } + + void record_test_point() { + const double elapsed_s = + std::max(0.0, std::chrono::duration(*timestamp_ - test_start_time_).count()); + + csv_writer_.write_row( + *update_count_, elapsed_s, *control_torque_, *measured_torque_, *measured_velocity_, + current_wrapped_angle_); + csv_writer_.flush(); + } + + void finish_test_sequence() { + test_active_ = false; + close_csv(); + + RCLCPP_INFO( + get_logger(), "Static torque test finished, final setpoint=%.6f rad", + wrap_to_pi(active_setpoint_)); + } + + void handle_idle() { + stop_test(true); + position_pid_.reset(); + velocity_pid_.reset(); + *control_torque_ = nan_; + } + + double calculate_pid_output(double setpoint) { + const double position_error = setpoint - current_continuous_angle_; + const double velocity_setpoint = position_pid_.update(position_error); + return velocity_pid_.update(velocity_setpoint - *measured_velocity_); + } + + void stop_test(bool clear_setpoint_hold) { + close_csv(); + test_active_ = false; + if (clear_setpoint_hold) + test_setpoint_valid_ = false; + } + + void close_csv() { + if (!csv_writer_.is_open()) + return; + + csv_writer_.flush(); + csv_writer_.close(); + current_csv_path_.clear(); + } + + std::filesystem::path build_csv_path() const { + const auto file_name = std::string{"static_torque_test_controller_"} + + sanitize_file_component(target_) + "_test_" + + std::to_string(*update_count_) + ".csv"; + return std::filesystem::path{"/tmp"} / file_name; + } + + const std::string target_; + const double interval_angle_; + const double wait_time_s_; + const std::chrono::duration wait_duration_; + const double border_clip_; + + const std::string control_torque_name_; + const std::string measured_torque_name_; + const std::string measured_velocity_name_; + const std::string measured_angle_name_; + + InputInterface update_count_; + InputInterface timestamp_; + InputInterface switch_left_; + InputInterface switch_right_; + InputInterface measured_torque_; + InputInterface measured_velocity_; + InputInterface measured_angle_; + + OutputInterface control_torque_; + + pid::PidCalculator position_pid_; + pid::PidCalculator velocity_pid_; + + bool angle_tracking_initialized_ = false; + double last_wrapped_angle_ = 0.0; + double current_wrapped_angle_ = 0.0; + double current_continuous_angle_ = 0.0; + + bool range_initialized_ = false; + double range_min_ = 0.0; + double range_max_ = 0.0; + + bool remote_mode_initialized_ = false; + RemoteMode last_remote_mode_ = RemoteMode::kIdle; + Clock::time_point next_center_info_time_{}; + + double active_setpoint_ = 0.0; + bool test_setpoint_valid_ = false; + bool test_active_ = false; + double clipped_range_min_ = 0.0; + double clipped_range_max_ = 0.0; + Clock::time_point test_start_time_{}; + Clock::time_point dwell_start_time_{}; + rmcs_utility::CsvWriter csv_writer_; + std::filesystem::path current_csv_path_; +}; + +} // namespace rmcs_core::controller::identification + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::identification::StaticTorqueTestController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/identification/swept_frequency_controller.cpp b/rmcs_ws/src/rmcs_core/src/identification/swept_frequency_controller.cpp new file mode 100644 index 000000000..eed799cb0 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/identification/swept_frequency_controller.cpp @@ -0,0 +1,382 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "controller/pid/pid_calculator.hpp" + +namespace rmcs_core::controller::identification { + +namespace { + +using Clock = std::chrono::steady_clock; + +constexpr auto kFlushInterval = std::chrono::duration(0.1); + +template +T require_parameter(rclcpp::Node& node, const std::string& name) { + if (!node.has_parameter(name)) + throw std::runtime_error("Missing required parameter: " + name); + return node.get_parameter(name).get_value(); +} + +template +T parameter_or_declare(rclcpp::Node& node, const std::string& name, const T& default_value) { + if (!node.has_parameter(name)) + node.declare_parameter(name, default_value); + return node.get_parameter(name).get_value(); +} + +void load_optional_parameter(rclcpp::Node& node, const std::string& name, double& value) { + node.get_parameter(name, value); +} + +void configure_pid_limits( + rclcpp::Node& node, const std::string& prefix, pid::PidCalculator& calculator) { + load_optional_parameter(node, prefix + "_integral_min", calculator.integral_min); + load_optional_parameter(node, prefix + "_integral_max", calculator.integral_max); + load_optional_parameter(node, prefix + "_integral_split_min", calculator.integral_split_min); + load_optional_parameter(node, prefix + "_integral_split_max", calculator.integral_split_max); + load_optional_parameter(node, prefix + "_output_min", calculator.output_min); + load_optional_parameter(node, prefix + "_output_max", calculator.output_max); +} + +std::string normalize_target(std::string target) { + while (!target.empty() && target.back() == '/') + target.pop_back(); + + if (target.empty()) + throw std::runtime_error("Parameter 'target' cannot be empty"); + + if (target.front() != '/') + target.insert(target.begin(), '/'); + + return target; +} + +std::string interface_name(const std::string& target, std::string_view suffix) { + return target + "/" + std::string{suffix}; +} + +std::string sanitize_file_component(std::string_view text) { + std::string sanitized; + sanitized.reserve(text.size()); + + for (const char ch : text) { + if (std::isalnum(static_cast(ch))) + sanitized.push_back(ch); + else + sanitized.push_back('_'); + } + + if (sanitized.empty()) + sanitized = "target"; + + return sanitized; +} + +double wrap_to_pi(double angle) { + constexpr double kPi = std::numbers::pi_v; + angle = std::remainder(angle, 2.0 * kPi); + if (angle <= -kPi) + angle += 2.0 * kPi; + return angle; +} + +bool controller_enabled(rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right) { + using rmcs_msgs::Switch; + if (switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + return false; + return !(switch_left == Switch::DOWN && switch_right == Switch::DOWN); +} + +double chirp_output( + double elapsed_s, double start_freq, double end_freq, double duration_s, double amplitude, + bool logarithmic) { + if (logarithmic) { + if (std::abs(end_freq - start_freq) <= std::numeric_limits::epsilon()) { + return amplitude * std::sin(2.0 * std::numbers::pi_v * start_freq * elapsed_s); + } + + const double ratio = end_freq / start_freq; + const double phase = 2.0 * std::numbers::pi_v * start_freq * duration_s + / std::log(ratio) * (std::pow(ratio, elapsed_s / duration_s) - 1.0); + return amplitude * std::sin(phase); + } + + const double sweep_rate = (end_freq - start_freq) / duration_s; + const double phase = 2.0 * std::numbers::pi_v + * (start_freq * elapsed_s + 0.5 * sweep_rate * elapsed_s * elapsed_s); + return amplitude * std::sin(phase); +} + +} // namespace + +class SweptFrequencyController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + SweptFrequencyController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , target_(normalize_target(require_parameter(*this, "target"))) + , sweep_enabled_(parameter_or_declare(*this, "sweep", false)) + , pid_enabled_(parameter_or_declare(*this, "pid", false)) + , logarithmic_(parameter_or_declare(*this, "logarithmic", false)) + , dc_offset_(parameter_or_declare(*this, "dc_offset", 0.0)) + , control_torque_name_(interface_name(target_, "control_torque")) + , measured_torque_name_(interface_name(target_, "torque")) + , measured_velocity_name_(interface_name(target_, "velocity")) + , measured_angle_name_(interface_name(target_, "angle")) { + if (sweep_enabled_) { + start_freq_ = require_parameter(*this, "start_freq"); + end_freq_ = require_parameter(*this, "end_freq"); + sweep_duration_s_ = require_parameter(*this, "duration"); + amplitude_ = require_parameter(*this, "amplitude"); + + if (!std::isfinite(start_freq_) || start_freq_ < 0.0) + throw std::runtime_error("start_freq must be finite and non-negative"); + if (!std::isfinite(end_freq_) || end_freq_ < 0.0) + throw std::runtime_error("end_freq must be finite and non-negative"); + if (!std::isfinite(sweep_duration_s_) || sweep_duration_s_ <= 0.0) + throw std::runtime_error("duration must be finite and positive"); + if (!std::isfinite(amplitude_)) + throw std::runtime_error("amplitude must be finite"); + if (logarithmic_ && (start_freq_ <= 0.0 || end_freq_ <= 0.0)) + throw std::runtime_error( + "logarithmic sweep requires positive start_freq and end_freq"); + } + + if (pid_enabled_) { + setpoint_ = require_parameter(*this, "setpoint"); + position_pid_.kp = require_parameter(*this, "position_kp"); + position_pid_.ki = require_parameter(*this, "position_ki"); + position_pid_.kd = require_parameter(*this, "position_kd"); + velocity_pid_.kp = require_parameter(*this, "velocity_kp"); + velocity_pid_.ki = require_parameter(*this, "velocity_ki"); + velocity_pid_.kd = require_parameter(*this, "velocity_kd"); + + if (!std::isfinite(setpoint_)) + throw std::runtime_error("setpoint must be finite"); + + configure_pid_limits(*this, "position", position_pid_); + configure_pid_limits(*this, "velocity", velocity_pid_); + } + + register_input("/predefined/update_count", update_count_); + register_input("/predefined/timestamp", timestamp_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/switch/right", switch_right_); + register_input(measured_torque_name_, measured_torque_); + register_input(measured_velocity_name_, measured_velocity_); + register_input(measured_angle_name_, measured_angle_); + + register_output(control_torque_name_, control_torque_, nan_); + } + + ~SweptFrequencyController() override { finish_sweep(); } + + void before_updating() override { + reset_pid_state(); + finish_sweep(); + + *control_torque_ = nan_; + last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; + } + + void update() override { + const auto current_switch_left = *switch_left_; + const auto current_switch_right = *switch_right_; + const bool is_enabled = controller_enabled(current_switch_left, current_switch_right); + + if (!is_enabled) { + reset_pid_state(); + finish_sweep(); + *control_torque_ = nan_; + store_switch_state(current_switch_left, current_switch_right); + return; + } + + if (should_start_sweep(current_switch_left, current_switch_right)) + start_sweep(); + + const double pid_output = pid_enabled_ ? calculate_pid_output() : 0.0; + const double dc_offset_output = dc_offset_; + + bool sweep_finished = false; + const double sweep_output = sweep_active_ ? calculate_sweep_output(sweep_finished) : 0.0; + + const double control_output = pid_output + dc_offset_output + sweep_output; + *control_torque_ = control_output; + + if (sweep_active_) { + log_sample(pid_output, sweep_output, control_output); + if (sweep_finished) + finish_sweep(); + } + + store_switch_state(current_switch_left, current_switch_right); + } + +private: + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + + bool should_start_sweep( + rmcs_msgs::Switch current_switch_left, rmcs_msgs::Switch current_switch_right) const { + using rmcs_msgs::Switch; + return sweep_enabled_ && !sweep_active_ && last_switch_left_ == Switch::MIDDLE + && last_switch_right_ == Switch::MIDDLE && current_switch_left == Switch::MIDDLE + && current_switch_right == Switch::UP; + } + + void store_switch_state( + rmcs_msgs::Switch current_switch_left, rmcs_msgs::Switch current_switch_right) { + last_switch_left_ = current_switch_left; + last_switch_right_ = current_switch_right; + } + + void reset_pid_state() { + position_pid_.reset(); + velocity_pid_.reset(); + } + + double calculate_pid_output() { + const double position_error = wrap_to_pi(setpoint_ - *measured_angle_); + const double velocity_setpoint = position_pid_.update(position_error); + return velocity_pid_.update(velocity_setpoint - *measured_velocity_); + } + + void start_sweep() { + finish_sweep(); + + const auto path = build_csv_path(); + try { + csv_writer_.open(path); + csv_writer_.write_row( + "update_count", "elapsed_s", "pid_output", "sweep_output", control_torque_name_, + measured_torque_name_, measured_velocity_name_, measured_angle_name_); + } catch (const std::exception& exception) { + const auto path_string = path.string(); + RCLCPP_ERROR( + get_logger(), "Failed to start sweep log '%s': %s", path_string.c_str(), + exception.what()); + return; + } + + sweep_active_ = true; + sweep_start_time_ = *timestamp_; + next_flush_time_ = + sweep_start_time_ + std::chrono::duration_cast(kFlushInterval); + current_csv_path_ = path; + + const auto path_string = current_csv_path_.string(); + RCLCPP_INFO(get_logger(), "Started sweep logging to %s", path_string.c_str()); + } + + void finish_sweep() { + if (!sweep_active_ && !csv_writer_.is_open()) + return; + + csv_writer_.flush(); + csv_writer_.close(); + + if (!current_csv_path_.empty()) { + const auto path_string = current_csv_path_.string(); + RCLCPP_INFO(get_logger(), "Finished sweep logging to %s", path_string.c_str()); + } + + current_csv_path_.clear(); + sweep_active_ = false; + } + + double calculate_sweep_output(bool& sweep_finished) const { + const double elapsed_s = + std::max(0.0, std::chrono::duration(*timestamp_ - sweep_start_time_).count()); + const double clamped_elapsed_s = std::clamp(elapsed_s, 0.0, sweep_duration_s_); + sweep_finished = elapsed_s >= sweep_duration_s_; + return chirp_output( + clamped_elapsed_s, start_freq_, end_freq_, sweep_duration_s_, amplitude_, logarithmic_); + } + + void log_sample(double pid_output, double sweep_output, double control_output) { + const double elapsed_s = + std::max(0.0, std::chrono::duration(*timestamp_ - sweep_start_time_).count()); + + csv_writer_.write_row( + *update_count_, elapsed_s, pid_output, sweep_output, control_output, *measured_torque_, + *measured_velocity_, *measured_angle_); + + if (*timestamp_ >= next_flush_time_) { + csv_writer_.flush(); + while (*timestamp_ >= next_flush_time_) { + next_flush_time_ += std::chrono::duration_cast(kFlushInterval); + } + } + } + + std::filesystem::path build_csv_path() const { + const auto file_name = std::string{"swept_frequency_controller_"} + + sanitize_file_component(target_) + "_sweep_" + + std::to_string(*update_count_) + ".csv"; + return std::filesystem::path{"/tmp"} / file_name; + } + + const std::string target_; + const bool sweep_enabled_; + const bool pid_enabled_; + const bool logarithmic_; + const double dc_offset_; + + const std::string control_torque_name_; + const std::string measured_torque_name_; + const std::string measured_velocity_name_; + const std::string measured_angle_name_; + + double start_freq_ = 0.0; + double end_freq_ = 0.0; + double sweep_duration_s_ = 0.0; + double amplitude_ = 0.0; + + InputInterface update_count_; + InputInterface timestamp_; + InputInterface switch_left_; + InputInterface switch_right_; + InputInterface measured_torque_; + InputInterface measured_velocity_; + InputInterface measured_angle_; + + OutputInterface control_torque_; + + pid::PidCalculator position_pid_; + pid::PidCalculator velocity_pid_; + double setpoint_ = 0.0; + + bool sweep_active_ = false; + Clock::time_point sweep_start_time_{}; + Clock::time_point next_flush_time_{}; + rmcs_utility::CsvWriter csv_writer_; + std::filesystem::path current_csv_path_; + + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; +}; + +} // namespace rmcs_core::controller::identification + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::identification::SweptFrequencyController, rmcs_executor::Component) From 8d9a286f018b8efcb8f4ae32cc92bb0f27a806bb Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Mon, 10 Aug 2026 18:06:59 +0800 Subject: [PATCH 06/86] feat(auto_aim): Integrate auto_aim v2 as submodule --- .gitmodules | 3 + .script/autoaim-debug | 71 +++++++++++++++++++ rmcs_ws/src/rmcs_auto_aim_v2 | 1 + .../rmcs_bringup/config/auto_aim_test.yaml | 52 ++++++++++++++ .../rmcs_core/src/referee/app/ui/auto_aim.cpp | 41 +++++++++++ 5 files changed, 168 insertions(+) create mode 100755 .script/autoaim-debug create mode 160000 rmcs_ws/src/rmcs_auto_aim_v2 create mode 100644 rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml diff --git a/.gitmodules b/.gitmodules index 61e5f08fb..de2c6a1cb 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,6 @@ [submodule "rmcs_ws/src/fast_tf"] path = rmcs_ws/src/fast_tf url = https://github.com/qzhhhi/FastTF.git +[submodule "rmcs_ws/src/rmcs_auto_aim_v2"] + path = rmcs_ws/src/rmcs_auto_aim_v2 + url = https://github.com/Alliance-Algorithm/rmcs_auto_aim_v2.git diff --git a/.script/autoaim-debug b/.script/autoaim-debug new file mode 100755 index 000000000..11a9ea264 --- /dev/null +++ b/.script/autoaim-debug @@ -0,0 +1,71 @@ +#!/usr/bin/env bash + +set -euo pipefail + +SESSION="autoaim-debug" + +usage() { + cat <<'EOF' +Usage: autoaim-debug + + local Start autoaim debug tmux session locally. + remote Start autoaim debug tmux session on ssh remote. +EOF +} + +write_start_script() { + local path="$1" + cat >"${path}" <<'SCRIPT' +set -euo pipefail + +SESSION="autoaim-debug" + +if [[ -f "${HOME}/env_setup.bash" ]]; then + ENV_SETUP="${HOME}/env_setup.bash" +elif [[ -f "/root/env_setup.bash" ]]; then + ENV_SETUP="/root/env_setup.bash" +else + echo "env_setup.bash not found in \$HOME or /root" >&2 + exit 1 +fi + +tmux kill-session -t "${SESSION}" 2>/dev/null || true + +tmux new-session -d -s "${SESSION}" -n "foxglove" \ + "bash -lc 'source \"${ENV_SETUP}\" && ros2 launch foxglove_bridge foxglove_bridge_launch.xml'" + +tmux new-window -t "${SESSION}" -n "streamer" \ + "bash -lc '/opt/autoaim/bin/start-streamer'" +SCRIPT + chmod +x "${path}" +} + +if [[ $# -ne 1 ]]; then + usage + exit 1 +fi + +case "$1" in +local) + tmp=$(mktemp) + trap 'rm -f "${tmp}"' EXIT + write_start_script "${tmp}" + bash "${tmp}" + tmux has-session -t "${SESSION}" + echo "autoaim-debug local started in tmux session: ${SESSION}" + echo "Attach with: tmux attach -t ${SESSION}" + ;; +remote) + tmp=$(mktemp) + trap 'rm -f "${tmp}"' EXIT + write_start_script "${tmp}" + scp -q "${tmp}" remote:/tmp/autoaim-debug-start.sh + ssh-remote "bash /tmp/autoaim-debug-start.sh && rm -f /tmp/autoaim-debug-start.sh && tmux has-session -t ${SESSION}" + echo "autoaim-debug remote started in tmux session: ${SESSION}" + echo "Attach with: ssh-remote \"tmux attach -t ${SESSION}\"" + ;; +*) + usage + exit 1 + ;; +esac diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 new file mode 160000 index 000000000..a261f18c4 --- /dev/null +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -0,0 +1 @@ +Subproject commit a261f18c453d26bea7f08335c0daebb4cdda4b36 diff --git a/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml b/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml new file mode 100644 index 000000000..7f812fa52 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml @@ -0,0 +1,52 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + # - rmcs::AutoAimPlayerComponent -> auto_aim_player + - rmcs::AutoAimVideoPlayerComponent -> auto_aim_video_player + # - rmcs::AutoAimRecorderComponent -> auto_aim_recorder + - rmcs::AutoAimComponent -> auto_aim_component + +auto_aim_player: + ros__parameters: + input_path: "/workspaces/data/autoaim/robot/blue_fast_track/" + loop_play: true + +auto_aim_video_player: + ros__parameters: + input_path: "/workspaces/data/autoaim/静止看前哨站.avi" + framerate: 80.0 + loop_play: true + +auto_aim_recorder: + ros__parameters: + output_path: "/tmp/autoaim/records" + queue_depth: 16 + flush_every_n_frames: 64 + max_duration_seconds: 0 + max_videos_size_gb: 0.0 + auto_record: false + +auto_aim_component: + ros__parameters: + dangerous_fallback: "red" + manual_shoot: false + enable_rune: true + track_ids: [HERO, ENGINEER, INFANTRY_3, INFANTRY_4, SENTRY, OUTPOST] + camera_translation: [0., 0., 0.] + + fire_control: + bullet_speed: 22.5 + shoot_delay: 0.0 + offset_yaw: 0.0 + offset_pitch: 0.0 + attack_window: 60.0 + degraded_angle_speed: 12.0 + window_redundancy: 0.8 + window_hysteresis: 0.2 + attack_preaim: false + require_stable_command: true + yaw_tolerance: 0.07 + pitch_tolerance: 0.04 + rune_idle_duration: 0.4 + rune_shoot_duration: 0.2 diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp index b874a1655..8e01acd86 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp @@ -1,6 +1,10 @@ #include #include +#include +#include #include +#include +#include #include #include @@ -30,10 +34,14 @@ class AutoAimUi register_input("/tf", tf_, true); register_input("/auto_aim/robot_center", robot_center_, true); register_input("/auto_aim/should_shoot", should_shoot_, true); + register_input("/auto_aim/single_shoot", single_shoot_, true); } void update() override { + const auto type = *single_shoot_ ? "RUNE" : "ARMOR"; + if (!robot_center_->allFinite() || robot_center_->isZero()) { + set_distance_text(type, std::nullopt); hide_all(); return; } @@ -43,6 +51,7 @@ class AutoAimUi || point.x() >= kScreenW || point.x() < 0 // || point.y() >= kScreenH || point.y() < 0 // ) { + set_distance_text(type, std::nullopt); hide_all(); return; } @@ -53,6 +62,17 @@ class AutoAimUi const auto color = *should_shoot_ ? Shape::Color::ORANGE : Shape::Color::GREEN; const auto radius = *should_shoot_ ? 10 : 15; + { + const auto distance = robot_center_->norm(); + if (!std::isfinite(distance)) { + set_distance_text(type, std::nullopt); + hide_all(); + return; + } + + set_distance_text(type, distance); + } + center_ring_.set_color(color); center_ring_.set_x(x); center_ring_.set_y(y); @@ -108,6 +128,7 @@ class AutoAimUi private: static constexpr std::uint16_t kScreenW = 1920; static constexpr std::uint16_t kScreenH = 1080; + static constexpr size_t kMaxTextLength = 30; static constexpr double kFx = 730.7267062695; static constexpr double kFy = 730.5886055073; @@ -125,6 +146,7 @@ class AutoAimUi InputInterface tf_; InputInterface robot_center_; InputInterface should_shoot_; + InputInterface single_shoot_; Circle center_ring_{Shape::Color::GREEN, 2, 0, 0, 5, 5, false}; Line cross_top_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false}; @@ -132,6 +154,11 @@ class AutoAimUi Line cross_left_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false}; Line cross_right_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false}; + std::string target_distance_text_{"unknown"}; + Text target_distance_indicator_{ + Shape::Color::GREEN, 15, 2, kScreenW / 2 + 34, kScreenH / 2 + 24, "unknown", false, + }; + void hide_all() { center_ring_.set_visible(false); cross_top_.set_visible(false); @@ -140,6 +167,20 @@ class AutoAimUi cross_right_.set_visible(false); } + void set_distance_text(const char* type, std::optional distance) { + auto& text = target_distance_text_; + text.resize(kMaxTextLength); + std::ranges::fill(text, ' '); + + if (distance) + std::format_to(std::ranges::begin(text), "{} | {:.1f}m\0", type, *distance); + else + std::format_to(std::ranges::begin(text), "{} | NONE\0", type); + + target_distance_indicator_.set_value(text.data()); + target_distance_indicator_.set_visible(true); + } + Eigen::Vector2d reproject(const Eigen::Vector3d& center) const { const auto camera_pose = fast_tf::lookup_transform(*tf_); From 21ebb97fcc7c6272f18fc0e1430e054d1f6d7602 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Mon, 10 Aug 2026 18:07:03 +0800 Subject: [PATCH 07/86] feat(sentry): Add sentry robot with climber, navigation and decision --- .../rmcs_bringup/config/navigation_test.yaml | 11 + rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 246 +++++-- .../chassis/climber/co_schduler.hpp | 231 ++++++ .../chassis/climber/stick_group.hpp | 196 +++++ .../chassis/climber/track_group.hpp | 146 ++++ .../src/controller/chassis/sentry_climber.cpp | 654 +++++++++++++++++ rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp | 690 +++++++++++------- .../command/interaction/sentry_decision.cpp | 243 ++++++ rmcs_ws/src/rmcs_core/src/referee/status.cpp | 50 +- .../rmcs_core/src/referee/status/field.hpp | 97 ++- 10 files changed, 2212 insertions(+), 352 deletions(-) create mode 100644 rmcs_ws/src/rmcs_bringup/config/navigation_test.yaml create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/climber/co_schduler.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/climber/stick_group.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/climber/track_group.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/navigation_test.yaml b/rmcs_ws/src/rmcs_bringup/config/navigation_test.yaml new file mode 100644 index 000000000..52e1e8c42 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/navigation_test.yaml @@ -0,0 +1,11 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs::navigation::Navigation -> rmcs_navigation + +rmcs_navigation: + ros__parameters: + command_vel_name: "/cmd_vel" + endpoint: "rmuc" + enable_goal_topic_forward: true diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index cec1cd164..c4c0bd3aa 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -6,107 +6,208 @@ rmcs_executor: - rmcs_core::referee::Status -> referee_status - rmcs_core::referee::Command -> referee_command - - rmcs_core::referee::command::Interaction -> referee_interaction + - rmcs_core::referee::command::interaction::SentryDecision -> referee_sentry_decision - rmcs_core::referee::command::interaction::Ui -> referee_ui - rmcs_core::controller::gimbal::EccentricDualYaw -> gimbal_controller - - # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster - - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller - rmcs_core::controller::shooting::HeatController -> heat_controller - rmcs_core::controller::shooting::BulletFeederController17mm -> bullet_feeder_controller - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller - + - rmcs_core::controller::chassis::SentryClimber -> sentry_climber - rmcs_core::controller::chassis::ChassisController -> chassis_controller - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller # - rmcs::navigation::Navigation -> rmcs_navigation - - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + - rmcs::AutoAimRecorderComponent -> auto_aim_recorder + - rmcs::AutoAimCapturerComponent -> auto_aim_capturer + - rmcs::AutoAimComponent -> auto_aim_component + + # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster + +rmcs_navigation: + ros__parameters: + command_vel_name: "/cmd_vel" + endpoint: "rmuc" + enable_goal_topic_forward: true + +auto_aim_capturer: + ros__parameters: + camera_name: "" + exposure_us: 1500.0 + gain: 8.0 + framerate: 120.0 + invert_image: true + rls_tau_sec: 10.0 + use_hardware_sync: true + delay_ms: 6.5 + +auto_aim_recorder: + ros__parameters: + output_path: "/tmp/autoaim/records" + queue_depth: 16 + flush_every_n_frames: 64 + max_duration_seconds: 300 + max_videos_size_gb: 100.0 + auto_record: false + +auto_aim_component: + ros__parameters: + manual_shoot: true + enable_rune: true + # track_ids: [OUTPOST, BASE] + track_ids: [HERO, ENGINEER, INFANTRY_3, INFANTRY_4, SENTRY, OUTPOST, BASE] + camera_translation: [0.07128, 0.0, 0.0481] + fire_control: + bullet_speed: 22.5 + shoot_delay: 0.05 + offset_yaw: -0.3 + offset_pitch: -0.1 + attack_window: 80.0 + degraded_angle_speed: 12.0 + window_redundancy: 0.8 + window_hysteresis: 0.2 + attack_preaim: false + require_stable_command: false + yaw_tolerance: 0.07 + pitch_tolerance: 0.04 + rune_idle_duration: 0.4 + rune_shoot_duration: 0.2 value_broadcaster: ros__parameters: forward_list: - - /gimbal/bottom_yaw/angle - - /gimbal/bottom_yaw/velocity - - /gimbal/pitch/angle - - /gimbal/pitch/control_torque - - /gimbal/top_yaw/angle - - /gimbal/top_yaw/velocity - -# The positive direction is the one that battery exists + - /gimbal/yaw/velocity_imu + - /gimbal/pitch/velocity_imu + sentry_hardware: ros__parameters: - board_serial_top_board: "af-da30" - board_serial_bottom_board: "d4-2184" - board_serial_gimbal_board: "d4-1d2b" - bottom_yaw_motor_zero_point: 23932 - top_yaw_motor_zero_point: 32916 - pitch_motor_zero_point: 1275 - left_front_zero_point: 7131 - left_back_zero_point: 3740 - right_back_zero_point: 1327 - right_front_zero_point: 5147 + board_serial_bottom_board: "af-b4e5" + board_serial_gimbal_board: "af-8b8b" -rmcs_navigation: - ros__parameters: - # 策略名称： - # - fast-push-output "速推前哨站" - # - kill-robots "杀伤优先" - decision: "fast-push-output" - command_vel_name: "/cmd_vel" - mock_context: false - endpoint: "test" - enable_goal_topic_forward: true + pitch_motor_zero_point: 12453 + + bottom_yaw_motor_zero_point: 54253 + top_yaw_motor_zero_point: 32736 + + left_front_zero_point: 5776 + left_back_zero_point: 3784 + right_back_zero_point: 3048 + right_front_zero_point: 5135 gimbal_controller: ros__parameters: - upper_limit: -0.39518 + upper_limit: -0.65 lower_limit: 0.36 - top_yaw_angle_kp: 45.0 - top_yaw_angle_ki: 0.0 - top_yaw_angle_kd: 0.0 + top_yaw_angle_kp: 30.0 + top_yaw_angle_ki: 0.008 + top_yaw_angle_kd: 0.005 top_yaw_velocity_kp: 2.160 top_yaw_velocity_ki: 0.0 top_yaw_velocity_kd: 0.0 bottom_yaw_angle_kp: 15.0 - bottom_yaw_angle_ki: 0.0 + bottom_yaw_angle_ki: 0.01 bottom_yaw_angle_kd: 0.0 - bottom_yaw_velocity_kp: 2.8125 - bottom_yaw_velocity_ki: 0.0 + bottom_yaw_velocity_kp: 2.75 + bottom_yaw_velocity_ki: 0.00125 bottom_yaw_velocity_kd: 0.0 - pitch_angle_kp: 32.0 - pitch_angle_ki: 0.0 + pitch_angle_kp: 35.0 + pitch_angle_ki: 0.01 pitch_angle_kd: 0.0 pitch_velocity_kp: 2.5 - pitch_velocity_ki: 0.0 + pitch_velocity_ki: 0.01 pitch_velocity_kd: 0.0 - k_top_to_bottom: -1.0 + top_yaw_angle_integral_min: -187.0 + top_yaw_angle_integral_max: 187.0 + top_yaw_velocity_integral_min: -2400.0 + top_yaw_velocity_integral_max: 2400.0 + + bottom_yaw_angle_integral_min: -150.0 + bottom_yaw_angle_integral_max: 150.0 + bottom_yaw_velocity_integral_min: -2400.0 + bottom_yaw_velocity_integral_max: 2400.0 - bottom_yaw_viscous_ff_gain: 0.000311875 - # bottom_yaw_coulomb_ff_gain: 0.457343 - # bottom_yaw_coulomb_ff_tanh_gain: 100.0 - top_yaw_viscous_ff_gain: 0.231 - # top_yaw_coulomb_ff_gain: 1.12 - # top_yaw_coulomb_ff_tanh_gain: 100.0 - pitch_viscous_ff_gain: 0.33 - # pitch_coulomb_ff_gain: 0.95 - # pitch_coulomb_ff_tanh_gain: 100.0 - pitch_gravity_ff_gain: 2.128 - pitch_gravity_ff_phase: 1.438 + pitch_angle_integral_min: -150.0 + pitch_angle_integral_max: 150.0 + pitch_velocity_integral_min: -2400.0 + pitch_velocity_integral_max: 2400.0 + + top_yaw_velocity_ff_gain: 1.0 + top_yaw_ff_cutoff_hz: 10.0 + top_yaw_ff_max: 2.0 + top_yaw_ff_jump_threshold: 0.01 chassis_controller: ros__parameters: - navigation_velocity_scale: 1.0 + angular_velocity_max: 10.0 + translational_velocity_max: 10.0 + following_velocity_kp: 7.0 + following_velocity_ki: 0.0 + following_velocity_kd: 0.0 + +sentry_climber: + ros__parameters: + track_group: + speed_rush: 20.0 + kp: 1.0 + ki: 0.0 + kd: 0.5 + sync_coefficient: 0.2 + power_estimate_bias: 0.0 + power_estimate_k_tau2: 1.0 + power_estimate_k_mech: 1.0 + stick_group: + speed_drop: 30.0 + speed_rise: 60.0 + rise_torque_limit: 1.5 + land_speed_begin: 100.0 + land_speed_final: 10.0 + land_duration: 0.5 + land_torque_limit: 8.0 + blocked_torque_threshold: 0.1 + blocked_speed_threshold: 0.1 + kp: 1.0 + ki: 0.0 + kd: 0.0 + sync_coefficient: 0.2 + hold_torque: 0.01 + block_hold: 0.05 + align: + err: 0.18 + w: 0.2 + hold: 0.05 + timeout: 15.0 + climb: + approach_pitch: 0.585 + leveled_pitch: 0.05 + approach_vx: 1.2 + deploy_vx: 0.3 + dash_vx: 3.0 + retract_vx: 0.0 + dash_min: 0.1 + dash_duration: 0.8 + stick_timeout: 8.0 + approach_timeout: 6.0 + land: + dash_vx: 1.0 + soft_vx: 0.3 + land_pitch: 0.15 + land_delay: 0.2 + stick_timeout: 8.0 + soft_timeout: 3.0 + settle_timeout: 8.0 + leave_vx: 0.3 + leave_duration: 1.0 friction_wheel_controller: ros__parameters: @@ -114,14 +215,14 @@ friction_wheel_controller: - /gimbal/left_friction - /gimbal/right_friction friction_velocities: - - 630.0 - - 630.0 + - 575.0 + - 575.0 friction_soft_start_stop_time: 1.0 heat_controller: ros__parameters: heat_per_shot: 10000 - reserved_heat: 10000 + reserved_heat: 30000 bullet_feeder_controller: ros__parameters: @@ -171,6 +272,25 @@ steering_wheel_controller: k1: 2.958580e+00 k2: 3.082190e-03 no_load_power: 11.37 - chassis_translation_kp: 8.0 - chassis_translation_ki: 0.0 - chassis_translation_kd: 0.0 + + chassis_translation_kp: 20.0 + chassis_translation_ki: 0.00 + chassis_translation_kd: 0.00 + chassis_translation_integral_limit: 200.0 + + chassis_angular_velocity_kp: 8.0 + chassis_angular_velocity_ki: 0.005 + chassis_angular_velocity_kd: 1.0 + chassis_angular_velocity_integral_limit: 200.0 + + steering_velocity_kp: 0.15 + steering_velocity_ki: 0.0 + steering_velocity_kd: 0.0 + + steering_angle_kp: 30.0 + steering_angle_ki: 0.0 + steering_angle_kd: 0.0 + + wheel_velocity_kp: 0.5 + wheel_velocity_ki: 0.0 + wheel_velocity_kd: 0.0 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/co_schduler.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/co_schduler.hpp new file mode 100644 index 000000000..654a23dbb --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/co_schduler.hpp @@ -0,0 +1,231 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core { + +// 协程调度器：对齐 rmcs-navigation Lua 调度器语义 +// - 任务携带 resume_request 谓词，每拍先问谓词，true 才 resume +// - 等待原语挂起时替换谓词，且至少让出一拍（控制循环时序确定性） +// - append 返回弱持有 Handle，cancel 为共享标志，任务消亡后 no-op +struct CoSchduler { + struct Task { + struct promise_type { + std::function resume_request{[] { return true; }}; + std::exception_ptr error{}; + + static constexpr auto initial_suspend() noexcept { return std::suspend_always{}; } + static constexpr auto final_suspend() noexcept { return std::suspend_always{}; } + + auto get_return_object(); + + static constexpr auto return_void() noexcept {} + + auto unhandled_exception() noexcept { error = std::current_exception(); } + + // co_yield {} 等价于 Tick：下拍唤醒并重置谓词 + auto yield_value(std::monostate) noexcept { + resume_request = [] { return true; }; + return std::suspend_always{}; + } + }; + + explicit Task(std::coroutine_handle handle) noexcept + : handle_{handle} {} + + Task(const Task&) = delete; + Task& operator=(const Task&) = delete; + + Task(Task&& other) noexcept + : handle_{std::exchange(other.handle_, {})} {} + + auto operator=(Task&& other) noexcept -> Task& { + if (this != &other) { + if (handle_) + handle_.destroy(); + handle_ = std::exchange(other.handle_, {}); + } + return *this; + } + + ~Task() { + if (handle_) + handle_.destroy(); + } + + auto release() noexcept { return std::exchange(handle_, {}); } + + private: + std::coroutine_handle handle_{}; + }; + + // co_await Tick{}：下一拍唤醒 + struct Tick { + static constexpr auto await_ready() noexcept { return false; } + + template + static auto await_suspend(std::coroutine_handle handle) { + handle.promise().resume_request = [] { return true; }; + } + + static constexpr auto await_resume() noexcept {} + }; + + // co_await Sleep{ duration }：到点唤醒（steady_clock，至少一拍） + struct Sleep { + std::chrono::steady_clock::duration duration; + + static constexpr auto await_ready() noexcept { return false; } + + template + auto await_suspend(std::coroutine_handle handle) { + const auto deadline = std::chrono::steady_clock::now() + duration; + handle.promise().resume_request = [deadline] { + return std::chrono::steady_clock::now() >= deadline; + }; + } + + static constexpr auto await_resume() noexcept {} + }; + + // co_await WaitUntil{ .monitor = ..., .timeout = ... }：条件满足或超时唤醒 + // await_resume 返回 is_timeout + struct WaitUntil { + std::function monitor; + std::chrono::steady_clock::duration timeout = std::chrono::steady_clock::duration::max(); + + struct State { + std::function monitor; + std::chrono::steady_clock::time_point deadline; + bool timed_out = false; + }; + + std::shared_ptr state{}; + + static constexpr auto await_ready() noexcept { return false; } + + template + auto await_suspend(std::coroutine_handle handle) { + state = std::make_shared(State{ + .monitor = std::move(monitor), + .deadline = std::chrono::steady_clock::now() + timeout, + }); + + handle.promise().resume_request = [state = state] { + if (state->monitor()) + return true; + + state->timed_out = std::chrono::steady_clock::now() >= state->deadline; + return state->timed_out; + }; + } + + auto await_resume() const noexcept { return state->timed_out; } + }; + + struct Slot { + explicit Slot(std::coroutine_handle handle) noexcept + : handle{handle} {} + + Slot(const Slot&) = delete; + Slot& operator=(const Slot&) = delete; + + ~Slot() { + if (handle) + handle.destroy(); + } + + std::coroutine_handle handle; + std::atomic_bool cancelled{false}; + }; + + // 取消句柄：弱持有任务，任务消亡后操作均为 no-op + struct Handle { + Handle() = default; + + auto cancel() const { + if (const auto locked = slot.lock()) + locked->cancelled.store(true, std::memory_order::relaxed); + } + + auto done() const { + const auto locked = slot.lock(); + return !locked || locked->cancelled.load(std::memory_order::relaxed) + || locked->handle.done(); + } + + private: + friend struct CoSchduler; + + explicit Handle(std::weak_ptr slot) noexcept + : slot{std::move(slot)} {} + + std::weak_ptr slot; + }; + + // 接管任务所有权，返回取消句柄；fire-and-forget 合法 + auto append(Task task) { + auto slot = std::make_shared(task.release()); + pending_.push_back(slot); + return Handle{slot}; + } + + auto spin_once() { + slots_.insert(slots_.end(), pending_.begin(), pending_.end()); + pending_.clear(); + + auto error = std::exception_ptr{}; + + std::erase_if(slots_, [&](const std::shared_ptr& slot) { + if (slot->cancelled.load(std::memory_order::relaxed)) + return true; + + auto& promise = slot->handle.promise(); + + if (slot->handle.done()) { + if (promise.error && !error) + error = promise.error; + return true; + } + + if (promise.resume_request()) { + slot->handle.resume(); + + if (slot->handle.done()) { + if (promise.error && !error) + error = promise.error; + return true; + } + } + + return false; + }); + + if (error) + std::rethrow_exception(error); + } + + // 急停清场：销毁全部任务帧，协程局部变量正常析构 + auto stop_all() { + slots_.clear(); + pending_.clear(); + } + +private: + std::vector> slots_{}; + std::vector> pending_{}; +}; + +inline auto CoSchduler::Task::promise_type::get_return_object() { + return Task{std::coroutine_handle::from_promise(*this)}; +} + +} // namespace rmcs_core diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/stick_group.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/stick_group.hpp new file mode 100644 index 000000000..294cf362f --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/stick_group.hpp @@ -0,0 +1,196 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include +#include + +#include "controller/pid/matrix_pid_calculator.hpp" + +namespace rmcs_core::controller::chassis::climber { + +struct StickGroup { + static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); + + template + using InputInterface = rmcs_executor::Component::InputInterface; + + template + using OutputInterface = rmcs_executor::Component::OutputInterface; + + enum class State { + kFree, + kHold, + kDrop, + kRise, + kLand, + kKeep, + } state = State::kFree; + + struct Config { + double speed_drop; + double speed_rise; + + double rise_torque_limit; + + // kLand：begin→final 速度变化时间(s)；越小越快贴到 final；t≥T 后恒 final 缓收 + double land_speed_begin; + double land_speed_final; + double land_duration; + double land_torque_limit; + + double blocked_torque_threshold; + double blocked_speed_threshold; + + double kp; + double ki; + double kd; + double sync_coefficient; + double hold_torque; + + auto get_speed(State state, double land_elapsed = 0.0) const noexcept { + switch (state) { + case State::kFree: return kNaN; + case State::kHold: return 0.0; + case State::kDrop: return +speed_drop; + case State::kRise: [[fallthrough]]; + case State::kKeep: return -speed_rise; + case State::kLand: { + // 指数进度 α：t=0 → begin，t≥T → final，前快后慢减震 + constexpr auto kShape = 3.0; + const auto T = std::max(land_duration, 1e-3); + if (land_elapsed >= T) + return -land_speed_final; + + const auto u = land_elapsed / T; + const auto alpha = (1.0 - std::exp(-kShape * u)) / (1.0 - std::exp(-kShape)); + return -(land_speed_begin + (land_speed_final - land_speed_begin) * alpha); + } + } + std::unreachable(); + } + + auto get_torque_limit(State state) const noexcept { + switch (state) { + case State::kRise: [[fallthrough]]; + case State::kKeep: return rise_torque_limit; + case State::kLand: return land_torque_limit; + default: return std::numeric_limits::infinity(); + } + } + } config; + + rmcs_executor::Component& command; + + // Interfaces + InputInterface l_velocity; + InputInterface r_velocity; + InputInterface l_torque; + InputInterface r_torque; + + OutputInterface l_control_torque; + OutputInterface r_control_torque; + + // PID + pid::MatrixPidCalculator<2> velocity_pid; + + std::chrono::steady_clock::time_point land_start_timestamp; + + explicit StickGroup(rmcs_executor::Component& command, const Config& config) + : config{config} + , command{command} + , velocity_pid{config.kp, config.ki, config.kd} { + + command.register_input("/chassis/climber/left_back_motor/velocity", l_velocity); + command.register_input("/chassis/climber/right_back_motor/velocity", r_velocity); + command.register_input("/chassis/climber/left_back_motor/torque", l_torque); + command.register_input("/chassis/climber/right_back_motor/torque", r_torque); + + command.register_output( + "/chassis/climber/left_back_motor/control_torque", l_control_torque, kNaN); + command.register_output( + "/chassis/climber/right_back_motor/control_torque", r_control_torque, kNaN); + } + + auto spin_once() { + if (state == State::kKeep && get_block()) { + *l_control_torque = -config.hold_torque; + *r_control_torque = -config.hold_torque; + return; + } + + const auto land_elapsed = + std::chrono::duration(std::chrono::steady_clock::now() - land_start_timestamp) + .count(); + const auto target_speed = config.get_speed(state, land_elapsed); + + if (std::isnan(target_speed)) { + *l_control_torque = kNaN; + *r_control_torque = kNaN; + return; + } + + auto torque = Eigen::Vector2d{}; + { + const auto setpoint_error = Eigen::Vector2d{ + target_speed - *l_velocity, + target_speed - *r_velocity, + }; + const auto relative_velocity = Eigen::Vector2d{ + *l_velocity - *r_velocity, + *r_velocity - *l_velocity, + }; + + torque = + velocity_pid.update(setpoint_error - config.sync_coefficient * relative_velocity); + } + + { + const auto torque_limit = config.get_torque_limit(state); + + if (target_speed < 0.0 && std::isfinite(torque_limit) && torque_limit > 0.0) { + const auto peak = std::max(std::abs(torque[0]), std::abs(torque[1])); + if (peak > torque_limit) { + torque *= torque_limit / peak; + } + } + + *l_control_torque = torque[0]; + *r_control_torque = torque[1]; + } + } + + auto set_state(State target) { + if (state == target) + return; + + state = target; + velocity_pid.reset(); + + if (target == State::kLand) { + land_start_timestamp = std::chrono::steady_clock::now(); + } + } + + auto get_state() const noexcept { return state; } + + auto get_block() const noexcept -> bool { + const auto is_blocked = [](double torque, double velocity, double torque_threshold, + double velocity_threshold) { + return std::abs(torque) > torque_threshold && std::abs(velocity) < velocity_threshold; + }; + + return is_blocked( + *l_torque, *l_velocity, config.blocked_torque_threshold, + config.blocked_speed_threshold) + || is_blocked( + *r_torque, *r_velocity, config.blocked_torque_threshold, + config.blocked_speed_threshold); + } +}; + +} // namespace rmcs_core::controller::chassis::climber diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/track_group.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/track_group.hpp new file mode 100644 index 000000000..fe8215176 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/track_group.hpp @@ -0,0 +1,146 @@ +#pragma once + +#include +#include +#include +#include + +#include +#include + +#include "controller/pid/matrix_pid_calculator.hpp" + +namespace rmcs_core::controller::chassis::climber { + +struct TrackGroup { + static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); + + template + using InputInterface = rmcs_executor::Component::InputInterface; + + template + using OutputInterface = rmcs_executor::Component::OutputInterface; + + enum class State { + kFree, + kHold, + kRush, + } state = State::kFree; + + struct Config { + double speed_rush; + + double kp; + double ki; + double kd; + double sync_coefficient; + + double power_estimate_bias; + double power_estimate_k_tau2; + double power_estimate_k_mech; + + auto get_speed(State state) const noexcept { + switch (state) { + case State::kFree: return kNaN; + case State::kHold: return 0.0; + case State::kRush: return speed_rush; + } + std::unreachable(); + } + } config; + + rmcs_executor::Component& command; + + // Interfaces + InputInterface l_velocity; + InputInterface r_velocity; + InputInterface l_max_torque; + InputInterface r_max_torque; + InputInterface control_power_limit; + + OutputInterface l_control_torque; + OutputInterface r_control_torque; + + // PID + pid::MatrixPidCalculator<2> velocity_pid; + + explicit TrackGroup(rmcs_executor::Component& command, const Config& config) + : config{config} + , command{command} + , velocity_pid{config.kp, config.ki, config.kd} { + + command.register_input("/chassis/climber/left_front_motor/velocity", l_velocity); + command.register_input("/chassis/climber/right_front_motor/velocity", r_velocity); + command.register_input("/chassis/climber/left_front_motor/max_torque", l_max_torque); + command.register_input("/chassis/climber/right_front_motor/max_torque", r_max_torque); + command.register_input("/chassis/climber/front/control_power_limit", control_power_limit); + + command.register_output( + "/chassis/climber/left_front_motor/control_torque", l_control_torque, kNaN); + command.register_output( + "/chassis/climber/right_front_motor/control_torque", r_control_torque, kNaN); + } + + auto spin_once() { + const auto target_speed = config.get_speed(state); + + if (std::isnan(target_speed)) { + *l_control_torque = kNaN; + *r_control_torque = kNaN; + return; + } + + auto torque = Eigen::Vector2d{}; + { + const auto setpoint_error = Eigen::Vector2d{ + target_speed - *l_velocity, + target_speed - *r_velocity, + }; + const auto relative_speed = Eigen::Vector2d{ + *l_velocity - *r_velocity, + *r_velocity - *l_velocity, + }; + + torque = velocity_pid.update(setpoint_error - config.sync_coefficient * relative_speed); + } + + { + const auto power_limit = *control_power_limit; + if (power_limit <= 0.0) { + *l_control_torque = 0.0; + *r_control_torque = 0.0; + return; + } + + auto l_torque = std::clamp(torque[0], -*l_max_torque, *l_max_torque); + auto r_torque = std::clamp(torque[1], -*r_max_torque, *r_max_torque); + + const auto estimated_power = + config.power_estimate_bias + + config.power_estimate_k_tau2 * (std::pow(l_torque, 2) + std::pow(r_torque, 2)) + + config.power_estimate_k_mech + * (std::abs(l_torque * *l_velocity) + std::abs(r_torque * *r_velocity)); + + if (estimated_power > power_limit && estimated_power > 0.0) { + const auto scale = std::clamp(power_limit / estimated_power, 0.0, 1.0); + l_torque *= scale; + r_torque *= scale; + } + + *l_control_torque = l_torque; + *r_control_torque = r_torque; + } + } + + auto set_state(State target) { + if (state == target) + return; + + state = target; + velocity_pid.reset(); + } + + auto get_state() const noexcept { return state; } +}; + +} // namespace rmcs_core::controller::chassis::climber diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp new file mode 100644 index 000000000..f57cc995e --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp @@ -0,0 +1,654 @@ +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "controller/chassis/climber/co_schduler.hpp" +#include "controller/chassis/climber/stick_group.hpp" +#include "controller/chassis/climber/track_group.hpp" + +namespace rmcs_core::controller::chassis { + +class SentryClimber + : public rmcs_executor::Component + , public rclcpp::Node + , public rmcs_utility::NodeMixin { + + static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); + + using TrackState = climber::TrackGroup::State; + using StickState = climber::StickGroup::State; + + struct Config { + climber::TrackGroup::Config track; + climber::StickGroup::Config stick; + + struct Align { + double err; + double w; + double hold; + double timeout; + } align; + + struct Climb { + double approach_pitch; + double leveled_pitch; + double approach_vx; + double deploy_vx; + double dash_vx; + double retract_vx; + double dash_min; + double dash_duration; + double stick_timeout; + double approach_timeout; + } climb; + + struct Land { + double dash_vx; + double soft_vx; + double land_pitch; + double land_delay; + double stick_timeout; + double soft_timeout; + double settle_timeout; + double leave_vx; + double leave_duration; + } land; + + double block_hold; + + template + static auto load(ParamOr&& param_or) -> Config { + return Config{ + .track = + { + .speed_rush = param_or("track_group.speed_rush", 20.0), + .kp = param_or("track_group.kp", 1.0), + .ki = param_or("track_group.ki", 0.0), + .kd = param_or("track_group.kd", 0.5), + .sync_coefficient = param_or("track_group.sync_coefficient", 0.2), + .power_estimate_bias = param_or("track_group.power_estimate_bias", 0.0), + .power_estimate_k_tau2 = param_or("track_group.power_estimate_k_tau2", 1.0), + .power_estimate_k_mech = param_or("track_group.power_estimate_k_mech", 1.0), + }, + .stick = + { + .speed_drop = param_or("stick_group.speed_drop", 30.0), + .speed_rise = param_or("stick_group.speed_rise", 60.0), + .rise_torque_limit = param_or("stick_group.rise_torque_limit", 0.5), + .land_speed_begin = param_or("stick_group.land_speed_begin", 30.0), + .land_speed_final = param_or("stick_group.land_speed_final", 2.0), + .land_duration = param_or("stick_group.land_duration", 0.5), + .land_torque_limit = param_or("stick_group.land_torque_limit", 8.0), + .blocked_torque_threshold = + param_or("stick_group.blocked_torque_threshold", 0.1), + .blocked_speed_threshold = + param_or("stick_group.blocked_speed_threshold", 0.1), + .kp = param_or("stick_group.kp", 0.5), + .ki = param_or("stick_group.ki", 0.0), + .kd = param_or("stick_group.kd", 0.0), + .sync_coefficient = param_or("stick_group.sync_coefficient", 0.2), + .hold_torque = param_or("stick_group.hold_torque", 0.01), + }, + .align = + { + .err = param_or("align.err", 0.10), + .w = param_or("align.w", 0.2), + .hold = param_or("align.hold", 0.05), + .timeout = param_or("align.timeout", 15.0), + }, + .climb = + { + .approach_pitch = param_or("climb.approach_pitch", 0.585), + .leveled_pitch = param_or("climb.leveled_pitch", 0.05), + .approach_vx = param_or("climb.approach_vx", 1.2), + .deploy_vx = param_or("climb.deploy_vx", 0.3), + .dash_vx = param_or("climb.dash_vx", 3.0), + .retract_vx = param_or("climb.retract_vx", 0.3), + .dash_min = param_or("climb.dash_min", 0.1), + .dash_duration = param_or("climb.dash_duration", 3.0), + .stick_timeout = param_or("climb.stick_timeout", 8.0), + .approach_timeout = param_or("climb.approach_timeout", 8.0), + }, + .land = + { + .dash_vx = param_or("land.dash_vx", 0.8), + .soft_vx = param_or("land.soft_vx", 0.4), + .land_pitch = param_or("land.land_pitch", 0.15), + .land_delay = param_or("land.land_delay", 0.2), + .stick_timeout = param_or("land.stick_timeout", 8.0), + .soft_timeout = param_or("land.soft_timeout", 3.0), + .settle_timeout = param_or("land.settle_timeout", 8.0), + .leave_vx = param_or("land.leave_vx", 0.5), + .leave_duration = param_or("land.leave_duration", 1.0), + }, + .block_hold = param_or("block_hold", 0.05), + }; + } + }; + + // 仅输入与派生；不负责 output + struct Context { + InputInterface l_switch; + InputInterface r_switch; + InputInterface keyboard; + InputInterface rotary_knob; + + InputInterface nav_cross_direction; + InputInterface nav_is_climb; + + InputInterface chassis_pitch; + InputInterface chassis_yaw_rate; + InputInterface measure_yaw; + + static constexpr auto normalize_angle(double angle) noexcept { + while (angle >= std::numbers::pi) + angle -= 2.0 * std::numbers::pi; + while (angle < -std::numbers::pi) + angle += 2.0 * std::numbers::pi; + return angle; + } + + auto bind(Component& component) noexcept { + // 当 nav_cross_direction 发生 isnan -> !isnan 的变化时，视作一次跨越地形事件请求 + // 反之，会立刻取消请求，终止当前事件 + component.register_input( + "/rmcs_navigation/request/cross_direction", nav_cross_direction, false); + component.register_input("/rmcs_navigation/request/is_climb", nav_is_climb, false); + + component.register_input("/remote/switch/left", l_switch, false); + component.register_input("/remote/switch/right", r_switch, false); + component.register_input("/remote/keyboard", keyboard, false); + component.register_input("/remote/rotary_knob_switch", rotary_knob, false); + + component.register_input("/chassis/pitch_imu", chassis_pitch, false); + component.register_input("/chassis/yaw/velocity_imu", chassis_yaw_rate, false); + component.register_input("/chassis/climber/measure_yaw", measure_yaw, false); + } + + auto load_fallback(std::invocable auto&& handler) { + using namespace rmcs_msgs; + + const auto ensure_bind = + [&](InputInterface& input, T default_value, std::string_view name) { + if (input.ready() == false) { + input.make_and_bind_directly(default_value); + std::invoke(handler, name); + } + }; + + ensure_bind(nav_cross_direction, kNaN, "nav_cross_direction"); + ensure_bind(nav_is_climb, false, "nav_is_climb"); + + ensure_bind(l_switch, Switch::UNKNOWN, "l_switch"); + ensure_bind(r_switch, Switch::UNKNOWN, "r_switch"); + ensure_bind(keyboard, Keyboard::zero(), "keyboard"); + ensure_bind(rotary_knob, Switch::UNKNOWN, "rotary_knob"); + + ensure_bind(chassis_pitch, 0.0, "chassis_pitch"); + ensure_bind(chassis_yaw_rate, 0.0, "chassis_yaw_rate"); + ensure_bind(measure_yaw, kNaN, "measure_yaw"); + } + + auto is_estop() const { + using namespace rmcs_msgs; + const auto l = *l_switch; + const auto r = *r_switch; + return l == Switch::UNKNOWN || r == Switch::UNKNOWN + || (l == Switch::DOWN && r == Switch::DOWN); + } + + auto align_error(double goal) const noexcept { + return normalize_angle(*measure_yaw - goal); + } + + auto wait_align( + double goal, double err_limit, double w_limit, std::chrono::steady_clock::duration hold, + std::chrono::steady_clock::duration timeout) const { + constexpr auto kSinceInit = std::optional{}; + return CoSchduler::WaitUntil{ + .monitor = + [=, this, hold_since = kSinceInit]() mutable { + if (!std::isfinite(*measure_yaw)) + return false; + + const auto stable = std::abs(align_error(goal)) < err_limit + && std::abs(*chassis_yaw_rate) < w_limit; + + const auto now = std::chrono::steady_clock::now(); + if (stable) { + if (!hold_since.has_value()) + hold_since = now; + else if (now - *hold_since >= hold) + return true; + } else { + hold_since.reset(); + } + return false; + }, + .timeout = timeout, + }; + } + + } context; + + OutputInterface chassis_track_direction; // 以履带方向为正向 + OutputInterface chassis_climb_speed; // 正向为基准的速度值 + OutputInterface + chassis_climb_status; // 事件进度: 0=空闲, 1=成功, -1=失败, (0,1)阶段小数 + + // /chassis/climber/status 阶段编码：(0, 0.55) 上台阶，[0.55, 1) 下台阶 + static constexpr double kStatusClimbAlign = 0.1; + static constexpr double kStatusClimbApproach = 0.2; + static constexpr double kStatusClimbDeploy = 0.3; + static constexpr double kStatusClimbDash = 0.4; + static constexpr double kStatusClimbRetract = 0.5; + static constexpr double kStatusLandAlign = 0.6; + static constexpr double kStatusLandDash = 0.7; + static constexpr double kStatusLandSettle = 0.75; + static constexpr double kStatusLandSoft = 0.8; + static constexpr double kStatusLandFinal = 0.9; + static constexpr double kStatusLandLeave = 0.95; + + struct SimpleComponent : public rmcs_executor::Component { + std::function fn; + + template + explicit SimpleComponent(Fn&& fn) + : fn{std::forward(fn)} {} + + auto update() -> void override { fn(); } + }; + + std::shared_ptr output_component{ + create_partner_component( + get_component_name() + "_output", [this] { std::ignore = this; }), + }; + + std::unique_ptr track_group; + std::unique_ptr stick_group; + CoSchduler schduler; + + Config config; + CoSchduler::Handle task_handler; + + static constexpr auto seconds_to_duration(double seconds) noexcept { + return std::chrono::duration_cast( + std::chrono::duration{seconds}); + } + + auto release_climber() noexcept { + *chassis_track_direction = kNaN; + *chassis_climb_speed = kNaN; + track_group->set_state(TrackState::kFree); + stick_group->set_state(// + context.is_estop() ? StickState::kFree : StickState::kKeep); + } + + auto wait_block(std::chrono::steady_clock::duration timeout) { + constexpr auto kSinceInit = std::optional{}; + const auto hold = seconds_to_duration(config.block_hold); + return CoSchduler::WaitUntil{ + .monitor = + [this, hold, hold_since = kSinceInit]() mutable { + const auto now = std::chrono::steady_clock::now(); + if (stick_group->get_block()) { + if (!hold_since.has_value()) + hold_since = now; + else if (now - *hold_since >= hold) + return true; + } else { + hold_since.reset(); + } + return false; + }, + .timeout = timeout, + }; + } + + auto spin_context() -> CoSchduler::Task { + using namespace rmcs_msgs; + + auto last_keyboard = Keyboard::zero(); + auto last_rotary = Switch::UNKNOWN; + + auto last_nav_cross_dir = kNaN; + + const auto cancel_task = [this] { + if (!task_handler.done()) { + task_handler.cancel(); + task_handler = {}; + } + *chassis_climb_status = 0.0; + release_climber(); + }; + + while (true) { + const auto keyboard = *context.keyboard; + const auto rotary = *context.rotary_knob; + + const auto nav_cross_dir = *context.nav_cross_direction; + const auto nav_is_climb = *context.nav_is_climb; + + const auto nav_request = + !std::isfinite(last_nav_cross_dir) && std::isfinite(nav_cross_dir); + const auto nav_canceled = + std::isfinite(last_nav_cross_dir) && !std::isfinite(nav_cross_dir); + + const auto step_direction = std::isfinite(*context.nav_cross_direction) + ? *context.nav_cross_direction + : *context.measure_yaw; + + const auto stop_intent = + (last_rotary != Switch::MIDDLE && rotary == Switch::MIDDLE) || nav_canceled; + const auto land_intent = (last_rotary != Switch::DOWN && rotary == Switch::DOWN) + || (nav_request && !nav_is_climb); + const auto rise_intent = (last_rotary != Switch::UP && rotary == Switch::UP) + || (last_keyboard.g == false && keyboard.g == true) + || (nav_request && nav_is_climb); + + do { + if (context.is_estop() || stop_intent) { + cancel_task(); + break; + } + if (rise_intent) { + if (!task_handler.done()) { + cancel_task(); + } else if (!std::isfinite(*context.measure_yaw)) { + *chassis_climb_status = -1; + node::error("climb start rejected: measure_yaw invalid"); + } else { + task_handler = schduler.append(climb(step_direction)); + } + break; + } + if (land_intent) { + if (!task_handler.done()) { + cancel_task(); + } else if (!std::isfinite(*context.measure_yaw)) { + *chassis_climb_status = -1; + node::error("land start rejected: measure_yaw invalid"); + } else { + task_handler = schduler.append(land(step_direction)); + } + break; + } + } while (false); + + if (!context.is_estop() && task_handler.done()) + release_climber(); + + last_keyboard = keyboard; + last_rotary = rotary; + + last_nav_cross_dir = nav_cross_dir; + + co_await CoSchduler::Tick{}; + } + } + + auto spin_groups() -> CoSchduler::Task { + while (true) { + track_group->spin_once(); + stick_group->spin_once(); + co_await CoSchduler::Tick{}; + } + } + + auto climb(double direction) -> CoSchduler::Task { + using namespace std::chrono_literals; + + *chassis_climb_status = kStatusClimbAlign; + + node::info("Climb start, direction={:.3f}", direction); + *chassis_track_direction = direction; + + // [] 将底盘与台阶方向对齐 + track_group->set_state(TrackState::kHold); + stick_group->set_state(StickState::kHold); + *chassis_climb_speed = 0.0; + { + const auto t0 = std::chrono::steady_clock::now(); + const auto timed_out = co_await context.wait_align( + direction, config.align.err, config.align.w, seconds_to_duration(config.align.hold), + seconds_to_duration(config.align.timeout)); + if (timed_out || !std::isfinite(*context.measure_yaw)) { + node::warn("climb ALIGN failed"); + release_climber(); + *chassis_climb_status = -1; + co_return; + } + const auto elapsed = + std::chrono::duration(std::chrono::steady_clock::now() - t0); + node::info( + "climb ALIGN done: err={:.3f}, took={:.3f}s", context.align_error(direction), + elapsed.count()); + } + + // [] 冲向台阶，开启履带，让底盘沿着台阶边缘上升，直到倾斜到一定角度 + *chassis_climb_status = kStatusClimbApproach; + track_group->set_state(TrackState::kRush); + stick_group->set_state(StickState::kHold); + *chassis_climb_speed = config.climb.approach_vx; + { + auto count = std::size_t{0}; + auto timeout = bool{false}; + do { + if (timeout) { + *chassis_climb_speed = -config.climb.approach_vx; + co_await CoSchduler::Sleep{500ms}; + + *chassis_climb_speed = +config.climb.approach_vx; + } + + timeout = co_await CoSchduler::WaitUntil{ + .monitor = + [this] { return *context.chassis_pitch > config.climb.approach_pitch; }, + .timeout = seconds_to_duration(config.climb.approach_timeout), + }; + if (timeout) + node::warn("climb APPROACH timeout, retry"); + + if (count++ > 2) { + node::error("上台阶彻底失败"); + release_climber(); + *chassis_climb_status = -1; + co_return; + } + } while (timeout); + } + + // [] 伸出撑杆，同时慢速向台阶方向前进 + *chassis_climb_status = kStatusClimbDeploy; + track_group->set_state(TrackState::kHold); + stick_group->set_state(StickState::kDrop); + *chassis_climb_speed = config.climb.deploy_vx; + { + const auto timed_out = + co_await wait_block(seconds_to_duration(config.climb.stick_timeout)); + if (timed_out) + node::warn("climb DEPLOY stick timeout, continue"); + } + + // [] 撑杆已完全伸出，全力冲上台阶，保持一定时间间隔 + *chassis_climb_status = kStatusClimbDash; + track_group->set_state(TrackState::kHold); + stick_group->set_state(StickState::kHold); + *chassis_climb_speed = config.climb.dash_vx; + co_await CoSchduler::Sleep{seconds_to_duration(config.climb.dash_duration)}; + + // [] 上台阶完毕，收回撑杆 + *chassis_climb_status = kStatusClimbRetract; + track_group->set_state(TrackState::kHold); + stick_group->set_state(StickState::kRise); + *chassis_climb_speed = config.climb.retract_vx; + { + const auto timed_out = + co_await wait_block(seconds_to_duration(config.climb.stick_timeout)); + if (timed_out) + node::warn("climb RETRACT stick timeout, continue"); + } + + *chassis_climb_speed = config.climb.dash_vx; + co_await CoSchduler::Sleep{500ms}; + + *chassis_climb_status = 1.0; + release_climber(); + } + + auto land(double direction) -> CoSchduler::Task { + *chassis_climb_status = kStatusLandAlign; + + node::info("Land start, direction={:.3f}", direction); + *chassis_track_direction = direction + std::numbers::pi; + + // [] 底盘对齐方向，准备下台阶 + track_group->set_state(TrackState::kHold); + stick_group->set_state(StickState::kHold); + *chassis_climb_speed = 0.0; + { + const auto timed_out = co_await context.wait_align( + direction + std::numbers::pi, config.align.err, config.align.w, + seconds_to_duration(config.align.hold), seconds_to_duration(config.align.timeout)); + if (timed_out) { + node::warn("land ALIGN failed"); + release_climber(); + *chassis_climb_status = -1; + co_return; + } + } + + // [] 伸出撑杆，以较快速度冲下台阶 + *chassis_climb_status = kStatusLandDash; + track_group->set_state(TrackState::kHold); + stick_group->set_state(StickState::kDrop); + *chassis_climb_speed = -config.land.dash_vx; + { + const auto timed_out = + co_await wait_block(seconds_to_duration(config.land.stick_timeout)); + if (timed_out) + node::warn("land DEPLOY stick timeout, continue"); + } + + // [] 保持撑杆伸出，直到撑杆从台阶落下，底盘倾角低于某个阈值，趋近水平 + *chassis_climb_status = kStatusLandSettle; + track_group->set_state(TrackState::kHold); + stick_group->set_state(StickState::kDrop); + *chassis_climb_speed = -config.land.dash_vx; + { + const auto timed_out = co_await CoSchduler::WaitUntil{ + .monitor = + [this] { return std::abs(*context.chassis_pitch) < config.land.land_pitch; }, + .timeout = seconds_to_duration(config.land.settle_timeout), + }; + if (timed_out) + node::warn("land SETTLE timeout, continue"); + } + using namespace std::chrono_literals; + co_await CoSchduler::Sleep{seconds_to_duration(config.land.land_delay)}; + + // [] 撑杆按照速度曲线收回，减少落地震动，并缓慢前进，让履带顺着台阶落下 + *chassis_climb_status = kStatusLandSoft; + track_group->set_state(TrackState::kHold); + stick_group->set_state(StickState::kLand); + *chassis_climb_speed = -config.land.soft_vx; + co_await CoSchduler::Sleep{seconds_to_duration(config.stick.land_duration)}; + { + const auto timed_out = + co_await wait_block(seconds_to_duration(config.land.soft_timeout)); + if (timed_out) + node::warn("land SOFT stick timeout, continue"); + } + + // [] 等待完全落地，底盘倾角趋近水平 + { + const auto timed_out = co_await CoSchduler::WaitUntil{ + .monitor = + [this] { return std::abs(*context.chassis_pitch) < config.land.land_pitch; }, + .timeout = seconds_to_duration(config.land.settle_timeout), + }; + if (timed_out) + node::warn("land SETTLE timeout, continue"); + } + + // [] 完全收回撑杆，结束下台阶 + *chassis_climb_status = kStatusLandFinal; + track_group->set_state(TrackState::kFree); + stick_group->set_state(StickState::kRise); + *chassis_climb_speed = kNaN; + { + const auto timed_out = + co_await wait_block(seconds_to_duration(config.land.stick_timeout)); + if (timed_out) + node::warn("land FINAL stick timeout, continue"); + } + + // [] 撑杆收回后，以一定速度向前（驶离台阶方向）运动一段时间 + *chassis_climb_status = kStatusLandLeave; + track_group->set_state(TrackState::kHold); + stick_group->set_state(StickState::kHold); + *chassis_climb_speed = -config.land.leave_vx; + co_await CoSchduler::Sleep{seconds_to_duration(config.land.leave_duration)}; + + *chassis_climb_status = 1.0; + release_climber(); + } + +public: + SentryClimber() + : Node{get_component_name(), node::options()} { + const auto read_parameter = [this](std::string_view name, double fallback) { + return node::param_or(std::string{name}, fallback); + }; + + config = Config::load(read_parameter); + + track_group = std::make_unique(*this, config.track); + stick_group = std::make_unique(*this, config.stick); + + context.bind(*this); + + // 底盘契约输出挂在 partner 上，保证更新序在主逻辑之后对下游可见 + output_component->register_output( + "/chassis/climber/direction", chassis_track_direction, kNaN); + output_component->register_output("/chassis/climber/speed", chassis_climb_speed, kNaN); + output_component->register_output("/chassis/climber/status", chassis_climb_status, 0.0); + + schduler.append(spin_context()); + schduler.append(spin_groups()); + } + + auto before_updating() -> void override { + context.load_fallback([this](std::string_view name) { + node::warn("Failed to fetch input '{}'. Bind to fallback.", name); + }); + } + + auto update() -> void override { + try { + schduler.spin_once(); + } catch (const std::exception& e) { + node::error("climber routine exception: {}", e.what()); + task_handler.cancel(); + task_handler = {}; + track_group->set_state(climber::TrackGroup::State::kFree); + stick_group->set_state(climber::StickGroup::State::kFree); + release_climber(); + } + } +}; + +} // namespace rmcs_core::controller::chassis + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::SentryClimber, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp index 9d917a2b1..fecbe501f 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp @@ -1,38 +1,54 @@ -#include "hardware/device/bmi088.hpp" -#include "hardware/device/can_packet.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/lk_motor.hpp" -#include "hardware/device/supercap.hpp" - +#include #include #include -#include -#include +#include #include #include #include #include +#include +#include #include #include #include +#include "hardware/device/bmi088_ekf.hpp" +#include "hardware/device/board_clock_lifter.hpp" +#include "hardware/device/can_packet.hpp" +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/lk_motor.hpp" +#include "hardware/device/remote_control.hpp" +#include "hardware/device/supercap.hpp" +#include "hardware/util/status_monitor.hpp" + namespace rmcs_core::hardware { class Sentry : public rmcs_executor::Component , public rclcpp::Node { + + static constexpr auto kPosition = + std::array{"left_front", "left_back", "right_back", "right_front"}; + public: Sentry() : Node( get_component_name(), rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) { + constexpr auto kNaN = std::numeric_limits::quiet_NaN(); + register_input("/predefined/timestamp", timestamp_); + register_output("/tf", tf_); + register_output("/chassis/climber/measure_yaw", chassis_measure_yaw_, kNaN); + register_output("/auto_aim/camera_transform", camera_transform_); + register_output("/auto_aim/barrel_direction", barrel_direction_); + register_output("/auto_aim/yaw_velocity", yaw_velocity_, kNaN); - // For command: remote-status + // 提供 remote-status 命令服务。 using Srv = std_srvs::srv::Trigger; status_service_ = create_service( "/rmcs/service/robot_status", @@ -40,14 +56,13 @@ class Sentry status_service_callback(response); }); - top_board_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_top_board").as_string()); + remote_control_ = std::make_unique(*this); - bottom_board_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_bottom_board").as_string()); + gimbal_board_ = std::make_unique( + *this, *command_component_, get_parameter("board_serial_gimbal_board").as_string()); - gimbal_board_ = - std::make_unique(get_parameter("board_serial_gimbal_board").as_string()); + chassis_board_ = std::make_unique( + *this, *command_component_, get_parameter("board_serial_bottom_board").as_string()); tf_->set_transform( Eigen::Translation3d{0.08, 0.0, 0.0}); @@ -55,185 +70,193 @@ class Sentry Eigen::Translation3d{0.07128, 0.0, 0.0481}); } - auto update() -> void override { - top_board_->update(); - bottom_board_->update(); + void update() override { gimbal_board_->update(); - tf_->set_transform( - gimbal_board_->imu_pose().conjugate()); + chassis_board_->update(); + remote_control_->update(); + + using namespace rmcs_description; + *camera_transform_ = fast_tf::lookup_transform(*tf_); + *barrel_direction_ = *fast_tf::cast( + PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + *yaw_velocity_ = gimbal_board_->yaw_velocity(); + + const auto chassis_direction = + fast_tf::cast(BaseLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + *chassis_measure_yaw_ = std::atan2(chassis_direction->y(), chassis_direction->x()); } private: - class GimbalBoard final : private librmcs::agent::CBoard { + class GimbalBoard final : public librmcs::board::RmcsBoardLite::Callback { public: - explicit GimbalBoard(std::string_view board_serial = {}) - : librmcs::agent::CBoard(board_serial) { - bmi088_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(y, -x, z); }); - } - - GimbalBoard(const GimbalBoard&) = delete; - GimbalBoard& operator=(const GimbalBoard&) = delete; - GimbalBoard(GimbalBoard&&) = delete; - GimbalBoard& operator=(GimbalBoard&&) = delete; - - ~GimbalBoard() override = default; - - auto update() -> void { - bmi088_.update_status(); - imu_pose_ = Eigen::Quaterniond{bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - } - - auto imu_pose() const -> Eigen::Quaterniond { return imu_pose_; } - - private: - auto accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) - -> void override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); - } - - auto gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) - -> void override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); - } - - device::Bmi088 bmi088_{1000, 0.2, 0.0}; - Eigen::Quaterniond imu_pose_ = Eigen::Quaterniond::Identity(); - }; - - class TopBoard final : private librmcs::agent::RmcsBoardLite { - friend class Sentry; - - public: - explicit TopBoard( + explicit GimbalBoard( Sentry& sentry, rmcs_executor::Component& sentry_command, - std::string_view board_serial = {}, librmcs::agent::AdvancedOptions options = {}) - : librmcs::agent::RmcsBoardLite(board_serial, options) - , tf_(sentry.tf_) - , bmi088_(1000, 0.2, 0.0) + std::string_view board_serial = {}) + : tf_(sentry.tf_) , gimbal_pitch_motor_(sentry, sentry_command, "/gimbal/pitch") , gimbal_top_yaw_motor_(sentry, sentry_command, "/gimbal/top_yaw") , gimbal_bullet_feeder_(sentry, sentry_command, "/gimbal/bullet_feeder") , gimbal_left_friction_(sentry, sentry_command, "/gimbal/left_friction") , gimbal_right_friction_(sentry, sentry_command, "/gimbal/right_friction") { + + using namespace device; + + auto zero_point = int{0}; + sentry.get_parameter("pitch_motor_zero_point", zero_point); gimbal_pitch_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( - static_cast(sentry.get_parameter("pitch_motor_zero_point").as_int()))); + LkMotor::Config{LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point(zero_point)); + sentry.get_parameter("top_yaw_motor_zero_point", zero_point); gimbal_top_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( - static_cast(sentry.get_parameter("top_yaw_motor_zero_point").as_int()))); + LkMotor::Config{LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point(zero_point)); gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + DjiMotor::Config{DjiMotor::Type::kM3508, 4} .enable_multi_turn_angle() .set_reversed() .set_reduction_ratio(19 * 2)); gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + DjiMotor::Config{DjiMotor::Type::kM3508, 2}.set_reduction_ratio(1.)); gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(1.) - .set_reversed()); - - sentry.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - sentry.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); - - bmi088_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(-x, -y, z); }); + DjiMotor::Config{DjiMotor::Type::kM3508, 1}.set_reduction_ratio(1.).set_reversed()); + + sentry.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_, 0.0); + sentry.register_output( + "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_, 0.0); + + sentry.register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); + sentry.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); + + board_ = std::make_unique(*this, board_serial); + board_->start_transmit().gpio_digital_read( + Spec::kGpios.kUart1Rx, { + .period_ms = 0, + .asap = false, + .rising_edge = false, + .falling_edge = true, + .capture_timestamp = true, + .pull = librmcs::data::GpioPull::kUp, + }); } - auto update() -> void { - gimbal_top_yaw_motor_.update_status(); - gimbal_pitch_motor_.update_status(); - - const auto pitch_angle = - std::remainder(gimbal_pitch_motor_.angle(), 2.0 * std::numbers::pi_v); - - bmi088_.update_status(); - const Eigen::Quaterniond gimbal_bmi088_pose{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + auto yaw_velocity() const -> double { return *gimbal_yaw_velocity_bmi088_; } - tf_->set_transform( - gimbal_bmi088_pose.conjugate()); - - *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); - *gimbal_pitch_velocity_bmi088_ = bmi088_.gy(); + auto status() const -> std::vector { return monitor_.text(); } + void update() { gimbal_bullet_feeder_.update_status(); gimbal_left_friction_.update_status(); gimbal_right_friction_.update_status(); + gimbal_top_yaw_motor_.update_status(); tf_->set_state( gimbal_top_yaw_motor_.angle()); + + gimbal_pitch_motor_.update_status(); + const auto pitch_angle = + std::remainder(gimbal_pitch_motor_.angle(), 2.0 * std::numbers::pi); tf_->set_state(pitch_angle); - } - auto command_update() -> void { - auto builder = start_transmit(); - - builder.can0_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_right_friction_.generate_command(), - gimbal_left_friction_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - } - .as_bytes(), - }); + if (const auto snapshot = bmi088_.snapshot()) { + tf_->set_transform( + snapshot->orientation.conjugate()); - builder.can3_transmit({ - .can_id = 0x141, - .can_data = gimbal_top_yaw_motor_.generate_torque_command().as_bytes(), - }); + *gimbal_yaw_velocity_bmi088_ = snapshot->gyro_body.z(); + *gimbal_pitch_velocity_bmi088_ = snapshot->gyro_body.y(); + } + } - builder.can2_transmit({ - .can_id = 0x141, - .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), - }); + void command_update() const { + board_->start_transmit() + .can_transmit( + Spec::kCans.kCan0, + { + .can_id = gimbal_right_friction_.send_id(), + .can_data = + device::CanPacket8{ + gimbal_right_friction_.generate_command(), + gimbal_left_friction_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + gimbal_bullet_feeder_.generate_command(), + } + .as_bytes(), + }) + .can_transmit( + Spec::kCans.kCan3, + { + .can_id = 0x141, + .can_data = gimbal_top_yaw_motor_.generate_torque_command().as_bytes(), + }) + .can_transmit( + Spec::kCans.kCan3, + { + .can_id = 0x142, + .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), + }); } - private: - auto can0_receive_callback(const librmcs::data::CanDataView& data) -> void override { + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; - auto can_id = data.can_id; - if (can_id == 0x202) { - gimbal_left_friction_.store_status(data.can_data); - } else if (can_id == 0x201) { - gimbal_right_friction_.store_status(data.can_data); - } else if (can_id == 0x204) { - gimbal_bullet_feeder_.store_status(data.can_data); + + const auto& can_id = data.can_id; + const auto& can_data = data.can_data; + + if (can == Spec::kCans.kCan0) { + /*^^*/ gimbal_left_friction_.match_then_store_status(can_id, can_data) + || gimbal_right_friction_.match_then_store_status(can_id, can_data) + || gimbal_bullet_feeder_.match_then_store_status(can_id, can_data); + + monitor_.tick("Gimbal::Can0", can_id); + + } else if (can == Spec::kCans.kCan3) { + /*^^*/ if (can_id == 0x141) { + gimbal_top_yaw_motor_.store_status(can_data); + } else if (can_id == 0x142) { + gimbal_pitch_motor_.store_status(can_data); + } + + monitor_.tick("Gimbal::Can3", can_id); } } - auto can2_receive_callback(const librmcs::data::CanDataView& data) -> void override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + void gpio_digital_read_result_callback( + const Spec::Gpio& gpio, const View::GpioDigital& data) override { + if (!data.timestamp_quarter_us) return; - auto can_id = data.can_id; - if (can_id == 0x141) - gimbal_pitch_motor_.store_status(data.can_data); - } - auto can3_receive_callback(const librmcs::data::CanDataView& data) -> void override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - if (can_id == 0x141) - gimbal_top_yaw_motor_.store_status(data.can_data); + if (gpio == Spec::kGpios.kUart1Rx) { + if (data.high) + return; + + const auto timestamp = + board_clock_lifter_.lift_timestamp(*data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + + camera_signal_output_.emit(*timestamp); + } } - auto accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) - -> void override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { + const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); + bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); + monitor_.tick("Gimbal::Imu", "Acc"); } - auto gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) - -> void override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + monitor_.tick("Gimbal::Imu", "Gyr"); + const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + + auto snapshot = + bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); + if (!snapshot) + return; + + imu_snapshot_output_.emit(*snapshot); } OutputInterface& tf_; @@ -241,26 +264,33 @@ class Sentry OutputInterface gimbal_yaw_velocity_bmi088_; OutputInterface gimbal_pitch_velocity_bmi088_; - device::Bmi088 bmi088_; + EventOutputInterface camera_signal_output_; + EventOutputInterface imu_snapshot_output_; + + device::Bmi088Ekf bmi088_{device::Bmi088Ekf::Config{ + .body_to_sensor = + Eigen::AngleAxisd{std::numbers::pi, Eigen::Vector3d::UnitZ()}.toRotationMatrix(), + }}; + device::BoardClockLifter board_clock_lifter_; + device::LkMotor gimbal_pitch_motor_; device::LkMotor gimbal_top_yaw_motor_; device::DjiMotor gimbal_bullet_feeder_; device::DjiMotor gimbal_left_friction_; device::DjiMotor gimbal_right_friction_; - }; - class BottomBoard final : private librmcs::agent::CBoard { - friend class Sentry; + StatusMonitor monitor_{}; + std::unique_ptr board_; + }; + class ChassisBoard final : public librmcs::board::RmcsBoardLite::Callback { public: - explicit BottomBoard( + explicit ChassisBoard( Sentry& sentry, rmcs_executor::Component& sentry_command, std::string_view board_serial = {}) - : librmcs::agent::CBoard(board_serial) - , imu_(1000, 0.2, 0.0) - , tf_(sentry.tf_) - , dr16_(sentry) + : tf_(sentry.tf_) + , dr16_{} , gimbal_bottom_yaw_motor_(sentry, sentry_command, "/gimbal/bottom_yaw") , chassis_wheel_motors_( {sentry, sentry_command, "/chassis/left_front_wheel"}, @@ -272,54 +302,81 @@ class Sentry {sentry, sentry_command, "/chassis/left_back_steering"}, {sentry, sentry_command, "/chassis/right_back_steering"}, {sentry, sentry_command, "/chassis/right_front_steering"}) + , chassis_front_climber_motor_( + {sentry, sentry_command, "/chassis/climber/left_front_motor"}, + {sentry, sentry_command, "/chassis/climber/right_front_motor"}) + , chassis_back_climber_motor_( + {sentry, sentry_command, "/chassis/climber/left_back_motor"}, + {sentry, sentry_command, "/chassis/climber/right_back_motor"}) , supercap_(sentry, sentry_command) { + + using namespace device; + sentry.register_output("/referee/serial", referee_serial_); + sentry.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0.0); + sentry.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); referee_serial_->read = [this](std::byte* buffer, size_t size) { return referee_ring_buffer_receive_.pop_front_n( [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); }; referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart1_transmit( - {.uart_data = std::span{buffer, size}}); + board_->start_transmit().uart_transmit( + Spec::kUarts.kUart0, {.uart_data = std::span{buffer, size}}); return size; }; const auto zero_point = sentry.get_parameter("bottom_yaw_motor_zero_point").as_int(); gimbal_bottom_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} - .set_reversed() - .set_encoder_zero_point(static_cast(zero_point))); + LkMotor::Config{LkMotor::Type::kMG6012Ei8}.set_reversed().set_encoder_zero_point( + static_cast(zero_point))); - for (auto& motor : chassis_wheel_motors_) { + constexpr auto kWheelIds = std::array{2, 1, 2, 4}; + for (auto&& [motor, id] : std::views::zip(chassis_wheel_motors_, kWheelIds)) { motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + DjiMotor::Config{DjiMotor::Type::kM3508, id} .set_reduction_ratio(11.) .enable_multi_turn_angle() .set_reversed()); } - constexpr auto kSteerNames = std::array{ - "right_back_zero_point", - "right_front_zero_point", - "left_front_zero_point", - "left_back_zero_point", - }; - for (auto&& [motor, name] : std::views::zip(chassis_steer_motors_, kSteerNames)) { - const auto zero_point = sentry.get_parameter(name).as_int(); + constexpr auto kSteerIds = std::array{2, 1, 1, 2}; + for (auto&& [motor, name, id] : + std::views::zip(chassis_steer_motors_, kPosition, kSteerIds)) { + const auto zero_point = + sentry.get_parameter(std::string{name} + "_zero_point").as_int(); motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + DjiMotor::Config{DjiMotor::Type::kGM6020, id} .set_reversed() .set_encoder_zero_point(static_cast(zero_point)) .enable_multi_turn_angle()); } - sentry.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); + chassis_front_climber_motor_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1}.set_reduction_ratio( + 19.)); + chassis_front_climber_motor_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2} + .set_reversed() + .set_reduction_ratio(19.)); + chassis_back_climber_motor_[0].configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} + .set_reversed() + .enable_multi_turn_angle()); + chassis_back_climber_motor_[1].configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} + .enable_multi_turn_angle()); + + board_ = std::make_unique(*this, board_serial); + + sentry.remote_control_->register_dr16(&dr16_); } - auto update() -> void { - imu_.update_status(); - *chassis_yaw_velocity_imu_ = imu_.gz(); + auto status() const -> std::vector { return monitor_.text(); } + + void update() { + gimbal_bottom_yaw_motor_.update_status(); + dr16_.update_status(); supercap_.update_status(); for (auto& motor : chassis_wheel_motors_) @@ -327,114 +384,194 @@ class Sentry for (auto& motor : chassis_steer_motors_) motor.update_status(); - dr16_.update_status(); - gimbal_bottom_yaw_motor_.update_status(); + chassis_front_climber_motor_[0].update_status(); + chassis_front_climber_motor_[1].update_status(); + chassis_back_climber_motor_[0].update_status(); + chassis_back_climber_motor_[1].update_status(); + tf_->set_state( gimbal_bottom_yaw_motor_.angle()); + + if (const auto snapshot = bmi088_.snapshot()) { + const auto& q = snapshot->orientation; + *chassis_pitch_imu_ = -std::asin(2.0 * (q.w() * q.y() - q.z() * q.x())); + *chassis_yaw_velocity_imu_ = snapshot->gyro_body.z(); + } } - auto command_update() -> void { + void command_update() { using namespace device; - auto builder = start_transmit(); - builder.can1_transmit({ - .can_id = 0x141, - .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), - }); - auto cache = CanPacket8{}; - auto generate = [&](std::uint32_t id, std::ranges::range auto& motors, auto... args) { - auto command = [&](T arg) { - if constexpr (std::same_as) { - return arg; - } else { - const auto valid = arg >= 0 && arg < 4; - return valid ? motors[arg].generate_command() - : CanPacket8::PaddingQuarter{}; - } - }; - cache = CanPacket8{command(args)...}; - return librmcs::data::CanDataView{.can_id = id, .can_data = cache.as_bytes()}; + auto generate = [&](const auto& motors, // + CanPacket8::Quarter extra = {}, std::size_t slot = 4) { + auto slots = std::array{}; + slots.fill(CanPacket8::PaddingQuarter{}); + std::uint32_t can_id = 0; + for (auto& motor : motors) { + slots[(motor.id() - 1) % 4] = motor.generate_command(); + if (can_id == 0) + can_id = motor.send_id(); + } + if (slot < 4) + slots[slot] = extra; + cache = CanPacket8{slots[0], slots[1], slots[2], slots[3]}; + return librmcs::data::CanDataView{.can_id = can_id, .can_data = cache.as_bytes()}; }; - if (can_transmission_mode_) { - builder.can1_transmit(generate(0x200, chassis_wheel_motors_, 1, 0, -1, -1)) - .can2_transmit(generate(0x200, chassis_wheel_motors_, -1, 2, -1, 3)); - } else { - builder.can1_transmit(generate(0x1FE, chassis_steer_motors_, 1, 0, -1, -1)) - .can2_transmit(generate( - 0x1FE, chassis_steer_motors_, 2, 3, -1, supercap_.generate_command())); - } - can_transmission_mode_ = !can_transmission_mode_; - } - - private: - auto dbus_receive_callback(const librmcs::data::UartDataView& data) -> void override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + auto supercap_package = CanPacket8{ + CanPacket8::PaddingQuarter{}, + CanPacket8::PaddingQuarter{}, + CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + }; + auto bottom_yaw_package = gimbal_bottom_yaw_motor_.generate_command(); + + board_->start_transmit() + .can_transmit( + Spec::kCans.kCan0, + { + .can_id = 0x142, + .can_data = chassis_back_climber_motor_[0].generate_command().as_bytes(), + }) + .can_transmit( + Spec::kCans.kCan0, + { + .can_id = 0x143, + .can_data = chassis_back_climber_motor_[1].generate_command().as_bytes(), + }) + + .can_transmit( + Spec::kCans.kCan1, generate(std::views::counted(chassis_wheel_motors_, 2))) + .can_transmit( + Spec::kCans.kCan2, generate(std::views::counted(chassis_wheel_motors_ + 2, 2))) + + .can_transmit( + Spec::kCans.kCan1, generate(std::views::counted(chassis_steer_motors_, 2))) + .can_transmit( + Spec::kCans.kCan2, generate(std::views::counted(chassis_steer_motors_ + 2, 2))) + + .can_transmit( + Spec::kCans.kCan3, {.can_id = 0x1fe, .can_data = supercap_package.as_bytes()}) + .can_transmit( + Spec::kCans.kCan3, + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_front_climber_motor_[0].generate_command(), + chassis_front_climber_motor_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }) + .can_transmit( + Spec::kCans.kCan3, + {.can_id = 0x141, .can_data = bottom_yaw_package.as_bytes()}); } - auto can1_receive_callback(const librmcs::data::CanDataView& data) -> void override { + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; - auto can_id = data.can_id; - if (can_id == 0x201) - chassis_wheel_motors_[1].store_status(data.can_data); - else if (can_id == 0x202) - chassis_wheel_motors_[0].store_status(data.can_data); - else if (can_id == 0x205) - chassis_steer_motors_[1].store_status(data.can_data); - else if (can_id == 0x206) - chassis_steer_motors_[0].store_status(data.can_data); - else if (can_id == 0x141) - gimbal_bottom_yaw_motor_.store_status(data.can_data); - } - auto can2_receive_callback(const librmcs::data::CanDataView& data) -> void override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - if (can_id == 0x202) - chassis_wheel_motors_[2].store_status(data.can_data); - else if (can_id == 0x204) - chassis_wheel_motors_[3].store_status(data.can_data); - else if (can_id == 0x205) - chassis_steer_motors_[2].store_status(data.can_data); - else if (can_id == 0x206) - chassis_steer_motors_[3].store_status(data.can_data); - else if (can_id == 0x300) - supercap_.store_status(data.can_data); + const auto& can_id = data.can_id; + const auto& can_data = data.can_data; + + if (can == Spec::kCans.kCan0) { + if (can_id == 0x142) { + chassis_back_climber_motor_[0].store_status(data.can_data); + } else if (can_id == 0x143) { + chassis_back_climber_motor_[1].store_status(data.can_data); + } + + monitor_.tick("Chassis::Can0", can_id); + + } else if (can == Spec::kCans.kCan1) { + /*^^*/ chassis_wheel_motors_[0].match_then_store_status(can_id, can_data) + || chassis_wheel_motors_[1].match_then_store_status(can_id, can_data) + + || chassis_steer_motors_[0].match_then_store_status(can_id, can_data) + || chassis_steer_motors_[1].match_then_store_status(can_id, can_data); + + monitor_.tick("Chassis::Can1", can_id); + + } else if (can == Spec::kCans.kCan2) { + /*^^*/ chassis_wheel_motors_[2].match_then_store_status(can_id, can_data) + || chassis_wheel_motors_[3].match_then_store_status(can_id, can_data) + + || chassis_steer_motors_[2].match_then_store_status(can_id, can_data) + || chassis_steer_motors_[3].match_then_store_status(can_id, can_data); + + monitor_.tick("Chassis::Can2", can_id); + + } else if (can == Spec::kCans.kCan3) { + if (can_id == 0x300) { + supercap_.store_status(can_data); + } else if (can_id == 0x141) { + gimbal_bottom_yaw_motor_.store_status(data.can_data); + } else { + /*^^*/ chassis_front_climber_motor_[0].match_then_store_status(can_id, can_data) + || chassis_front_climber_motor_[1].match_then_store_status( + can_id, can_data); + } + + monitor_.tick("Chassis::Can3", can_id); + } } - auto uart1_receive_callback(const librmcs::data::UartDataView& data) -> void override { - const auto* uart_data = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&uart_data](std::byte* storage) noexcept { *storage = *uart_data++; }, - data.uart_data.size()); + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + if (uart == Spec::kUarts.kDbus) { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + monitor_.tick("Chassis::Dbus", "Active"); + } else if (uart == Spec::kUarts.kUart0) { + const auto* uart_data = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&uart_data](std::byte* storage) noexcept { *storage = *uart_data++; }, + data.uart_data.size()); + monitor_.tick("Chassis::Uart0", "Active"); + } } - auto accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) - -> void override { - imu_.store_accelerometer_status(data.x, data.y, data.z); + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { + const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); + bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); + monitor_.tick("Chassis::Imu", "Acc"); } - auto gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) - -> void override { - imu_.store_gyroscope_status(data.x, data.y, data.z); + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + monitor_.tick("Chassis::Imu", "Gyr"); + const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + + auto snapshot = + bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); + if (!snapshot) + return; } - bool can_transmission_mode_ = true; - device::Bmi088 imu_; + device::Bmi088Ekf bmi088_; + device::BoardClockLifter board_clock_lifter_; OutputInterface& tf_; device::Dr16 dr16_; device::LkMotor gimbal_bottom_yaw_motor_; device::DjiMotor chassis_wheel_motors_[4]; device::DjiMotor chassis_steer_motors_[4]; + + device::DjiMotor chassis_front_climber_motor_[2]; + device::LkMotor chassis_back_climber_motor_[2]; + device::Supercap supercap_; rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; OutputInterface referee_serial_; OutputInterface chassis_yaw_velocity_imu_; + OutputInterface chassis_pitch_imu_; + + StatusMonitor monitor_{}; + std::unique_ptr board_; }; struct CommandTransmitter : public rmcs_executor::Component { @@ -444,11 +581,11 @@ class Sentry explicit CommandTransmitter(Fn&& fn) : fn{std::forward(fn)} {} - auto update() -> void override { fn(); } + void update() override { fn(); } }; - auto status_service_callback(const std::shared_ptr& response) - -> void { + void + status_service_callback(const std::shared_ptr& response) { response->success = true; auto feedback_message = std::ostringstream{}; @@ -456,28 +593,36 @@ class Sentry std::println(feedback_message, format, std::forward(args)...); }; - text("Gimbal Status"); - text("- Bottom Yaw: {}", bottom_board_->gimbal_bottom_yaw_motor_.last_raw_angle()); - text("- Top Yaw: {}", top_board_->gimbal_top_yaw_motor_.last_raw_angle()); - text("- Pitch Angle: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); - - text("Chassis Status"); - constexpr auto kPosition = - std::array{"right back", "right front", "left front", "left back"}; - constexpr auto kMaxLength = - std::ranges::max_element(kPosition, {}, &std::string_view::size)->size(); + text( + " bottom_yaw_motor_zero_point: {}", + chassis_board_->gimbal_bottom_yaw_motor_.last_raw_angle()); + text(" pitch_motor_zero_point: {}", gimbal_board_->gimbal_pitch_motor_.last_raw_angle()); + text( + " top_yaw_motor_zero_point: {}", + gimbal_board_->gimbal_top_yaw_motor_.last_raw_angle()); + text(""); for (auto&& [index, motor] : - std::views::zip(kPosition, bottom_board_->chassis_steer_motors_)) { - text("- {:{}}: {}", index, kMaxLength, motor.last_raw_angle()); + std::views::zip(kPosition, chassis_board_->chassis_steer_motors_)) { + text(" {}_zero_point: {}", index, motor.last_raw_angle()); + } + + text("\nGimbalBoard Status:"); + for (const auto& line : gimbal_board_->status()) { + text("> {}", line); + } + + text("\nChassisBoard Status:"); + for (const auto& line : chassis_board_->status()) { + text("> {}", line); } response->message = feedback_message.str(); } - auto command_update() -> void { - top_board_->command_update(); - bottom_board_->command_update(); + void command_update() { + gimbal_board_->command_update(); + chassis_board_->command_update(); } std::shared_ptr command_component_{ create_partner_component( @@ -486,9 +631,14 @@ class Sentry InputInterface timestamp_; OutputInterface tf_; + OutputInterface camera_transform_; + OutputInterface barrel_direction_; + OutputInterface yaw_velocity_; + OutputInterface chassis_measure_yaw_; + std::unique_ptr gimbal_board_; - std::unique_ptr top_board_; - std::unique_ptr bottom_board_; + std::unique_ptr chassis_board_; + std::unique_ptr remote_control_; std::shared_ptr> status_service_; }; diff --git a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/sentry_decision.cpp b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/sentry_decision.cpp index e69de29bb..ec4403fa4 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/sentry_decision.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/sentry_decision.cpp @@ -0,0 +1,243 @@ +#include "referee/command/field.hpp" +#include "referee/command/interaction/header.hpp" +#include "referee/status/field.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::referee::command::interaction { + +class SentryDecision + : public rmcs_executor::Component + , public rclcpp::Node { +public: + using Command = status::SentryCommand; + using Posture = Command::Posture; + using SentryEvent = rmcs_msgs::SentryEvent; + using EventCounts = std::unordered_map; + using Clock = std::chrono::steady_clock; + + InputInterface sentry_events_; + InputInterface robot_id_; + InputInterface sentry_posture_fb_; + InputInterface robot_hp_fb_; + InputInterface energy_core_status_; + InputInterface can_rebirth_free_; + + OutputInterface sentry_decision_field_; + + Header header_{}; + Command command_{}; + + EventCounts cached_events_; + std::unordered_set requests_; + std::unordered_map pose_targets_; + std::unordered_set logged_events_; + std::uint8_t last_fb_posture_ = 3; + bool last_can_rebirth_free_ = false; + + static inline const auto kPoseEvents = std::unordered_set{ + SentryEvent::SWITCH_POSE_ATTACK, + SentryEvent::SWITCH_POSE_DEFENSE, + SentryEvent::SWITCH_POSE_MOVE, + SentryEvent::SWITCH_POSE_POWERED_ATTACK, + SentryEvent::SWITCH_POSE_POWERED_DEFENSE, + SentryEvent::SWITCH_POSE_POWERED_MOVE, + }; + + static auto to_posture(SentryEvent event) -> Posture { + switch (event) { + case SentryEvent::SWITCH_POSE_ATTACK: return Posture::ATTACK; + case SentryEvent::SWITCH_POSE_DEFENSE: return Posture::DEFENSE; + case SentryEvent::SWITCH_POSE_MOVE: return Posture::MOVE; + case SentryEvent::SWITCH_POSE_POWERED_ATTACK: return Posture::POWERED_ATTACK; + case SentryEvent::SWITCH_POSE_POWERED_DEFENSE: return Posture::POWERED_DEFENSE; + case SentryEvent::SWITCH_POSE_POWERED_MOVE: return Posture::POWERED_MOVE; + default: return Posture::UNKNOWN; + } + } + + static constexpr auto kEventPriority = std::array{ + SentryEvent::CONFIRM_REBIRTH, + SentryEvent::CONFIRM_INSTANT_REBIRTH, + SentryEvent::SWITCH_POSE_ATTACK, + SentryEvent::SWITCH_POSE_DEFENSE, + SentryEvent::SWITCH_POSE_MOVE, + SentryEvent::SWITCH_POSE_POWERED_ATTACK, + SentryEvent::SWITCH_POSE_POWERED_DEFENSE, + SentryEvent::SWITCH_POSE_POWERED_MOVE, + SentryEvent::EXCHANGE_AMMO_SUPPLY_POINT, + SentryEvent::EXCHANGE_AMMO_REMOTE, + SentryEvent::EXCHANGE_HP_REMOTE, + SentryEvent::ACTIVATE_ENERGY_CORE, + }; + + SentryDecision() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} { + + register_input("/referee/id", robot_id_); + register_input("/rmcs_navigation/sentry_events", sentry_events_, false); + register_input("/referee/sentry/posture", sentry_posture_fb_, false); + register_input("/referee/current_hp", robot_hp_fb_, false); + register_input( + "/referee/event/ally_big_energy_activation_status", energy_core_status_, false); + register_input("/referee/sentry/can_rebirth_free", can_rebirth_free_, false); + + register_output("/referee/command/interaction/sentry_decision", sentry_decision_field_); + } + + auto before_updating() -> void override { + if (!sentry_events_.ready()) + sentry_events_.make_and_bind_directly(); + if (!sentry_posture_fb_.ready()) + sentry_posture_fb_.make_and_bind_directly(uint8_t{3}); + if (!robot_hp_fb_.ready()) + robot_hp_fb_.make_and_bind_directly(uint16_t{0}); + if (!energy_core_status_.ready()) + energy_core_status_.make_and_bind_directly(uint8_t{0}); + if (!can_rebirth_free_.ready()) + can_rebirth_free_.make_and_bind_directly(false); + } + + auto update() -> void override { + if (*robot_id_ == rmcs_msgs::RobotId::UNKNOWN) { + *sentry_decision_field_ = Field{}; + return; + } + + detect_new_events(); + + const auto can_rebirth_free = *can_rebirth_free_; + if (can_rebirth_free && !last_can_rebirth_free_) { + requests_.insert(SentryEvent::CONFIRM_REBIRTH); + } + last_can_rebirth_free_ = can_rebirth_free; + + consume_one_event(); + verify_feedback(); + } + +private: + auto detect_new_events() -> void { + const auto& input = *sentry_events_; + + for (const auto event : kEventPriority) { + auto input_it = input.find(event); + auto input_count = (input_it != input.end()) ? input_it->second : uint16_t{0}; + auto cache_count = cached_events_[event]; + + if (cache_count != input_count) { + if (kPoseEvents.contains(event)) { + for (const auto rm : kPoseEvents) + requests_.erase(rm); + } + requests_.insert(event); + cached_events_[event] = input_count; + } + } + } + + auto consume_one_event() -> void { + if (requests_.empty()) { + *sentry_decision_field_ = Field{}; + return; + } + + const auto id = rmcs_msgs::FullRobotId{*robot_id_}; + header_.command_id = 0x0120; + header_.sender_id = id; + header_.receiver_id = rmcs_msgs::FullRobotId::REFEREE_SERVER; + + for (const auto event : kEventPriority) { + if (!requests_.contains(event)) + continue; + + command_ = Command{}; + + if (kPoseEvents.contains(event)) { + command_.posture = to_posture(event); + pose_targets_[event] = to_posture(event); + } else if (event == SentryEvent::CONFIRM_REBIRTH) { + command_.rebirth_confirm = 1; + } else if (event == SentryEvent::CONFIRM_INSTANT_REBIRTH) { + command_.instant_rebirth_confirm = 1; + } else if (event == SentryEvent::EXCHANGE_AMMO_SUPPLY_POINT) { + command_.ammo_exchange = 1; + } else if (event == SentryEvent::EXCHANGE_AMMO_REMOTE) { + command_.remote_ammo_request = 1; + } else if (event == SentryEvent::EXCHANGE_HP_REMOTE) { + command_.remote_hp_request = 1; + } else if (event == SentryEvent::ACTIVATE_ENERGY_CORE) { + command_.energy_core_confirm = 1; + } + + *sentry_decision_field_ = MAKE_FIELD(header_, command_); + + if (kPoseEvents.contains(event)) { + if (!logged_events_.contains(event)) { + RCLCPP_INFO( + get_logger(), "Sentry pose command: %d", + std::to_underlying(command_.posture)); + logged_events_.insert(event); + } + } + break; + } + } + + auto verify_feedback() -> void { + const auto fb_posture_id = *sentry_posture_fb_; + const auto fb_hp = *robot_hp_fb_; + const auto energy_core_status = *energy_core_status_; + + if (fb_posture_id != last_fb_posture_) { + RCLCPP_INFO( + get_logger(), "Sentry posture feedback: %d → %d", last_fb_posture_, fb_posture_id); + last_fb_posture_ = fb_posture_id; + } + + auto to_erase = std::vector{}; + for (const auto event : requests_) { + if (kPoseEvents.contains(event)) { + auto it = pose_targets_.find(event); + if (it != pose_targets_.end() + && static_cast(it->second) == fb_posture_id) { + to_erase.push_back(event); + pose_targets_.erase(it); + logged_events_.erase(event); + } + } else if ( + event == SentryEvent::CONFIRM_REBIRTH + || event == SentryEvent::CONFIRM_INSTANT_REBIRTH) { + if (fb_hp > 0) { + to_erase.push_back(event); + } + } else if (event == SentryEvent::ACTIVATE_ENERGY_CORE) { + // FIXME: 加 5s 超时销毁 + if (energy_core_status != 0) { + to_erase.push_back(event); + } + } else { + to_erase.push_back(event); + } + } + + for (const auto event : to_erase) + requests_.erase(event); + } +}; + +} // namespace rmcs_core::referee::command::interaction + +#include +PLUGINLIB_EXPORT_CLASS( + rmcs_core::referee::command::interaction::SentryDecision, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/status.cpp b/rmcs_ws/src/rmcs_core/src/referee/status.cpp index c29bc3c2b..c0a7db62e 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status.cpp @@ -49,6 +49,14 @@ class Status register_output("/referee/chassis/buffer_energy", robot_buffer_energy_, 60.0); register_output("/referee/chassis/output_status", chassis_output_status_, false); + register_output("/referee/sentry/posture", sentry_posture_, uint8_t{3}); + register_output("/referee/sentry/is_powered", sentry_is_powered_, false); + register_output("/referee/sentry/is_disengaged", sentry_is_disengaged_, false); + register_output("/referee/sentry/can_rebirth_free", sentry_can_rebirth_free_, false); + register_output("/referee/sentry/can_rebirth_gold", sentry_can_rebirth_gold_, false); + register_output( + "/referee/sentry/rebirth_gold_cost", sentry_rebirth_gold_cost_, uint16_t{0}); + register_output("/referee/robots/hp", robots_hp_); register_output("/referee/ally/hero_hp", ally_hero_hp_, 0); register_output("/referee/ally/engineer_hp", ally_engineer_hp_, 0); @@ -56,6 +64,9 @@ class Status register_output("/referee/ally/infantry_2_hp", ally_infantry_2_hp_, 0); register_output("/referee/ally/outpost/hp", ally_outpost_hp_, 0); register_output("/referee/ally/base/hp", ally_base_hp_, 0); + register_output("/referee/enemy/outpost/hp", enemy_outpost_hp_, 0); + register_output("/referee/enemy/base/hp", enemy_base_hp_, 0); + register_output("/referee/damage_difference", damage_difference_, int16_t{0}); register_output("/referee/current_hp", robot_current_hp_); register_output("/referee/position/x", robot_position_x_, 0.0); register_output("/referee/position/y", robot_position_y_, 0.0); @@ -149,7 +160,7 @@ class Status auto command_id = frame_.body.command_id; if (command_id == 0x0001) update_game_status(); - if (command_id == 0x0003) + else if (command_id == 0x0003) update_game_robot_hp(); else if (command_id == 0x0101) update_event_data(); @@ -167,6 +178,8 @@ class Status update_shoot_data(); else if (command_id == 0x0208) update_bullet_allowance(); + else if (command_id == 0x020D) + update_sentry_info(); else if (command_id == 0x0303) update_map_command(); } @@ -187,16 +200,15 @@ class Status void update_event_data() { auto& data = reinterpret_cast(frame_.body.data); - const uint32_t event_data = data.event_data; - *ally_small_energy_activation_status_ = (event_data >> 3) & 0x03; - *ally_big_energy_activation_status_ = (event_data >> 5) & 0x03; - *ally_fortress_occupation_status_ = (event_data >> 25) & 0x03; + *ally_small_energy_activation_status_ = data.ally_small_energy_activation_status; + *ally_big_energy_activation_status_ = data.ally_big_energy_activation_status; + *ally_fortress_occupation_status_ = data.ally_fortress_occupation_status; } void update_dart_info() { auto& data = reinterpret_cast(frame_.body.data); - *dart_latest_hit_target_total_count_ = (data.dart_info >> 3) & 0x07; + *dart_latest_hit_target_total_count_ = data.latest_hit_target_total_count; } void update_game_robot_hp() { @@ -208,6 +220,9 @@ class Status *ally_infantry_2_hp_ = data.ally_4_robot_hp; *ally_outpost_hp_ = data.ally_outpost_hp; *ally_base_hp_ = data.ally_base_hp; + *enemy_outpost_hp_ = data.enemy_outpost_hp; + *enemy_base_hp_ = data.enemy_base_hp; + *damage_difference_ = data.damage_difference; } void update_robot_status() { @@ -228,7 +243,7 @@ class Status else *robot_chassis_power_limit_ = static_cast(data.chassis_power_limit); - *chassis_output_status_ = data.power_management_status & (1u << 1); + *chassis_output_status_ = data.power_management_chassis_output; } void update_power_heat_data() { @@ -263,6 +278,17 @@ class Status *robot_fortress_17mm_bullet_allowance_ = data.projectile_allowance_fortress; } + void update_sentry_info() { + auto& data = reinterpret_cast(frame_.body.data); + + *sentry_posture_ = static_cast(data.posture + (data.is_powered ? 3 : 0)); + *sentry_is_powered_ = data.is_powered; + *sentry_is_disengaged_ = data.is_disengaged; + *sentry_can_rebirth_free_ = data.can_rebirth_free; + *sentry_can_rebirth_gold_ = data.can_rebirth_gold; + *sentry_rebirth_gold_cost_ = data.rebirth_gold_cost; + } + void update_map_command() { if (frame_.header.data_length < sizeof(MapCommand)) { RCLCPP_WARN( @@ -329,6 +355,13 @@ class Status OutputInterface robot_chassis_power_limit_; OutputInterface chassis_output_status_; + OutputInterface sentry_posture_; + OutputInterface sentry_is_powered_; + OutputInterface sentry_is_disengaged_; + OutputInterface sentry_can_rebirth_free_; + OutputInterface sentry_can_rebirth_gold_; + OutputInterface sentry_rebirth_gold_cost_; + rmcs_utility::TickTimer power_heat_data_watchdog_; OutputInterface robot_chassis_power_; OutputInterface robot_buffer_energy_; @@ -340,6 +373,9 @@ class Status OutputInterface ally_infantry_2_hp_; OutputInterface ally_outpost_hp_; OutputInterface ally_base_hp_; + OutputInterface enemy_outpost_hp_; + OutputInterface enemy_base_hp_; + OutputInterface damage_difference_; OutputInterface robot_current_hp_; OutputInterface robot_position_x_; OutputInterface robot_position_y_; diff --git a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp index ad321125e..ad5e21619 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp @@ -16,31 +16,56 @@ struct __attribute__((packed)) GameRobotHp { uint16_t ally_2_robot_hp; uint16_t ally_3_robot_hp; uint16_t ally_4_robot_hp; - uint16_t reserved; + int16_t damage_difference; uint16_t ally_7_robot_hp; uint16_t ally_outpost_hp; uint16_t ally_base_hp; + uint16_t enemy_outpost_hp; + uint16_t enemy_base_hp; }; struct __attribute__((packed)) EventData { - uint32_t event_data; + std::uint32_t ally_supply_zone_occupied : 1 = 0; + std::uint32_t reserved_1 : 1 = 0; + std::uint32_t ally_supply_zone_occupied_rmul : 1 = 0; + std::uint32_t ally_small_energy_activation_status : 2 = 0; + std::uint32_t ally_big_energy_activation_status : 2 = 0; + std::uint32_t ally_central_highland_occupied : 2 = 0; + std::uint32_t ally_trapezoidal_highland_occupied : 2 = 0; + std::uint32_t enemy_dart_latest_hit_time : 9 = 0; + std::uint32_t enemy_dart_latest_hit_target : 3 = 0; + std::uint32_t center_gain_point_occupied : 2 = 0; + std::uint32_t ally_fortress_occupation_status : 2 = 0; + std::uint32_t ally_outpost_gain_point_occupied : 2 = 0; + std::uint32_t ally_base_gain_point_occupied : 1 = 0; + std::uint32_t reserved_30_31 : 2 = 0; }; +static_assert(sizeof(EventData) == 4); struct __attribute__((packed)) DartInfo { - uint8_t dart_remaining_time; - uint16_t dart_info; + std::uint8_t dart_remaining_time; + std::uint16_t latest_hit_target : 3 = 0; + std::uint16_t latest_hit_target_total_count : 3 = 0; + std::uint16_t selected_target : 3 = 0; + std::uint16_t reserved : 7 = 0; }; +static_assert(sizeof(DartInfo) == 3); struct __attribute__((packed)) RobotStatus { - uint8_t robot_id; - uint8_t robot_level; - uint16_t current_hp; - uint16_t maximum_hp; - uint16_t shooter_barrel_cooling_value; - uint16_t shooter_barrel_heat_limit; - uint16_t chassis_power_limit; - uint8_t power_management_status; + std::uint8_t robot_id; + std::uint8_t robot_level; + std::uint16_t current_hp; + std::uint16_t maximum_hp; + std::uint16_t shooter_barrel_cooling_value; + std::uint16_t shooter_barrel_heat_limit; + std::uint16_t chassis_power_limit; + float bullet_speed_limit; + std::uint8_t power_management_gimbal_output : 1 = 0; + std::uint8_t power_management_chassis_output : 1 = 0; + std::uint8_t power_management_shooter_output : 1 = 0; + std::uint8_t reserved : 5 = 0; }; +static_assert(sizeof(RobotStatus) == 17); struct __attribute__((packed)) PowerHeatData { uint16_t reserved_1; @@ -85,4 +110,52 @@ struct __attribute__((packed)) MapCommand { }; static_assert(sizeof(MapCommand) == 12); +struct __attribute__((packed)) SentryCommand { + enum class Posture : std::uint8_t { + UNKNOWN = 0, + ATTACK = 1, + DEFENSE = 2, + MOVE = 3, + POWERED_ATTACK = 4, + POWERED_DEFENSE = 5, + POWERED_MOVE = 6, + }; + + std::uint32_t rebirth_confirm : 1 = 0; + std::uint32_t instant_rebirth_confirm : 1 = 0; + std::uint32_t ammo_exchange : 11 = 0; + std::uint32_t remote_ammo_request : 4 = 0; + std::uint32_t remote_hp_request : 4 = 0; + Posture posture : 3 = Posture::UNKNOWN; + std::uint32_t energy_core_confirm : 1 = 0; + std::uint32_t reserved : 7 = 0; +}; +static_assert(sizeof(SentryCommand) == 4); + +struct __attribute__((packed)) SentryInfo { + std::uint32_t ammo_exchange_count : 11 = 0; + std::uint32_t remote_ammo_exchange_count : 4 = 0; + std::uint32_t remote_hp_exchange_count : 4 = 0; + std::uint32_t can_rebirth_free : 1 = 0; + std::uint32_t can_rebirth_gold : 1 = 0; + std::uint32_t rebirth_gold_cost : 10 = 0; + std::uint32_t reserved_31 : 1 = 0; + + std::uint16_t is_disengaged : 1 = 0; + std::uint16_t remaining_17mm_ammo_exchangeable : 11 = 0; + std::uint16_t posture : 2 = 0; + std::uint16_t energy_core_activatable : 1 = 0; + std::uint16_t is_powered : 1 = 0; + + std::uint64_t attack_posture_remaining_time : 8 = 0; + std::uint64_t defense_posture_remaining_time : 8 = 0; + std::uint64_t move_posture_remaining_time : 8 = 0; + std::uint64_t reserved_24_31 : 8 = 0; + std::uint64_t powered_attack_remaining_time : 8 = 0; + std::uint64_t powered_defense_remaining_time : 8 = 0; + std::uint64_t powered_move_remaining_time : 8 = 0; + std::uint64_t reserved_56_63 : 8 = 0; +}; +static_assert(sizeof(SentryInfo) == 14); + } // namespace rmcs_core::referee::status From 08e3c7d63af59c72a7ebeaf958ac9da21aa4f7be Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Mon, 10 Aug 2026 18:07:07 +0800 Subject: [PATCH 08/86] feat(hero): Rework hero with six-friction steering chassis --- .../steering-hero-little-six-friction.yaml | 529 ++++++++++ .../config/steering-hero-little.yaml | 417 -------- .../rmcs_bringup/config/steering-hero.yaml | 399 -------- .../chassis/hero_chassis_controller.cpp | 24 +- .../gimbal/hero_gimbal_controller.cpp | 98 +- .../hero_friction_wheel_controller.cpp | 11 +- .../shooting/hero_heat_controller.cpp | 5 +- ... => steering-hero-little-six-friction.cpp} | 747 +++++++------- .../rmcs_core/src/hardware/steering-hero.cpp | 908 ------------------ .../src/rmcs_core/src/referee/app/ui/hero.cpp | 116 +-- 10 files changed, 1059 insertions(+), 2195 deletions(-) create mode 100644 rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml delete mode 100644 rmcs_ws/src/rmcs_bringup/config/steering-hero-little.yaml delete mode 100644 rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml rename rmcs_ws/src/rmcs_core/src/hardware/{steering-hero-little.cpp => steering-hero-little-six-friction.cpp} (54%) delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml new file mode 100644 index 000000000..fb58305a2 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml @@ -0,0 +1,529 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::SteeringHeroLittle -> hero_hardware + + - rmcs_core::referee::Status -> referee_status + - rmcs_core::referee::command::Interaction -> referee_interaction + - rmcs_core::referee::command::interaction::Ui -> referee_ui + - rmcs_core::referee::app::ui::Hero -> referee_ui_hero + - rmcs_core::referee::Command -> referee_command + + - rmcs_core::controller::gimbal::HeroGimbalController -> gimbal_controller + - rmcs_core::controller::gimbal::DualYawController -> dual_yaw_controller + - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller + - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller + + - rmcs_core::controller::gimbal::PlayerViewer -> gimbal_player_viewer_controller + - rmcs_core::controller::pid::ErrorPidController -> viewer_angle_pid_controller + + - rmcs_core::controller::shooting::HeroFrictionWheelController -> friction_wheel_controller + - rmcs_core::controller::shooting::HeroHeatController -> heat_controller + - rmcs_core::controller::shooting::PutterController -> bullet_feeder_controller + - rmcs_core::controller::pid::PidController -> first_back_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> first_front_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> second_back_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> second_front_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> third_back_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> third_front_friction_velocity_pid_controller + - rmcs_core::controller::shooting::ShootingRecorder -> shooting_recorder + + - rmcs_core::controller::chassis::SteeringWheelStatus -> steering_wheel_status + - rmcs_core::controller::chassis::HeroChassisController -> chassis_controller + - rmcs_core::controller::chassis::HeroChassisPowerController -> chassis_power_controller + - rmcs_core::controller::chassis::HeroSteeringWheelController -> steering_wheel_controller + - rmcs_core::controller::chassis::ChassisClimberController -> climber_controller + + - rmcs::AutoAimCapturerComponent -> auto_aim_capturer + - rmcs::AutoAimComponent -> auto_aim_component + - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui + + # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster + + # - rmcs_core::controller::identification::SweptFrequencyController -> pitch_swept_frequency_controller + # - rmcs_core::controller::identification::StaticTorqueTestController -> pitch_static_torque_test_controller + # - rmcs_core::controller::identification::SweptFrequencyController -> top_yaw_swept_frequency_controller + # - rmcs_core::controller::identification::SweptFrequencyController -> bottom_yaw_swept_frequency_controller + # - rmcs_core::controller::identification::SweptFrequencyController -> first_front_friction_swept_frequency_controller + # - rmcs_core::controller::identification::SweptFrequencyController -> second_front_friction_swept_frequency_controller + # - rmcs_core::controller::identification::SweptFrequencyController -> third_front_friction_swept_frequency_controller + # - rmcs_core::controller::identification::SweptFrequencyController -> first_back_friction_swept_frequency_controller + # - rmcs_core::controller::identification::SweptFrequencyController -> second_back_friction_swept_frequency_controller + # - rmcs_core::controller::identification::SweptFrequencyController -> third_back_friction_swept_frequency_controller + +hero_hardware: + ros__parameters: + board_serial_top_board: "AF-8AE3-4EC1-03C3-C494-88FE-2DC4-3018-0298" + serial_bottom_rmcs_board: "AF-60BB-7484-FA24-F3FC-399B-454D-22FA-1B1D" + bottom_yaw_motor_zero_point: 35110 + pitch_motor_zero_point: 23653 + top_yaw_motor_zero_point: 48525 + viewer_motor_zero_point: 31940 + external_imu_port: /dev/ttyUSB0 + bullet_feeder_motor_zero_point: 60480 #39045 + left_front_zero_point: 5790 + right_front_zero_point: 5114 + left_back_zero_point: 7868 + right_back_zero_point: 1640 + +auto_aim_capturer: + ros__parameters: + camera_name: "" + exposure_us: 2000.0 + gain: 8.0 + framerate: 80.0 + invert_image: false + rls_tau_sec: 10.0 + use_hardware_sync: false + delay_ms: 6.5 + +auto_aim_component: + ros__parameters: + # WARN: 危险！可选 red | blue，生效后裁判系统 ID 缺席时 + # 将强行绑定为对应阵营哨兵，仅供调试，严禁比赛启用。 + # 留空或填 unknow 表示禁用。 + dangerous_fallback: "" + manual_shoot: false + enable_rune: false + camera_translation: [0.25, 0.0, -0.05] + fire_control: + bullet_speed: 11.5 + shoot_delay: 0.1 + offset_yaw: 0.0 + offset_pitch: 0.0 + attack_window: 120.0 + degraded_angle_speed: 12.0 + window_redundancy: 0.8 + window_hysteresis: 0.2 + attack_preaim: false + require_stable_command: false + yaw_tolerance: 0.07 + pitch_tolerance: 0.04 + rune_idle_duration: 0.4 + rune_shoot_duration: 0.2 + +auto_aim_ui: + ros__parameters: + offset_x: 0.0 + offset_y: 0.0 + offset_z: 0.0 + +value_broadcaster: + ros__parameters: + forward_list: + # - /gimbal/top_yaw/control_angle + # - /chassis/climber/left_front_motor/torque + # - /chassis/climber/right_front_motor/torque + # - /chassis/left_front_steering/torque + # - /chassis/left_back_steering/torque + # - /chassis/right_front_steering/torque + # - /chassis/right_back_steering/torque + - /chassis/left_front_steering/velocity + - /chassis/left_back_steering/velocity + - /chassis/right_front_steering/velocity + - /chassis/right_back_steering/velocity + # - /shoot/heat12707 + # - /chassis/power + # - /referee/chassis/power + # - /referee/chassis/power_limit + # - /chassis/control_power_limit + # - /chassis/climber/front/control_power_limit + # - /chassis/climber/front/power_demand_estimate + # - /chassis/climber/front/actual_power_estimate + # - /chassis/steering_wheel/actual_power_estimate + # - /gimbal/putter/velocity + - /gimbal/first_front_friction/velocity + - /gimbal/first_back_friction/velocity + - /gimbal/second_front_friction/velocity + - /gimbal/second_back_friction/velocity + - /gimbal/third_front_friction/velocity + - /gimbal/third_back_friction/velocity + - /gimbal/first_front_friction/control_torque + - /gimbal/first_back_friction/control_torque + - /gimbal/second_front_friction/control_torque + - /gimbal/second_back_friction/control_torque + - /gimbal/third_front_friction/control_torque + - /gimbal/third_back_friction/control_torque + # - /gimbal/bottom_yaw/torque + # - /gimbal/bottom_yaw/angle + # - /gimbal/top_yaw/angle + # - /gimbal/pitch/angle + # - /chassis/power + # - /chassis/supercap/voltage + # - /chassis/control_power_limit + # - /referee/chassis/power_limit + # - /referee/chassis/buffer_energy + # - /chassis/climber/front/control_power_limit + # - /chassis/climber/front/power_demand_estimate + # - /chassis/climber/front/actual_power_estimate + # - /chassis/steering_wheel/actual_power_estimate + # - /gimbal/bullet_feeder/torque + # - /gimbal/bullet_feeder/control_torque + # - /gimbal/putter/angle + # - /gimbal/putter/velocity + # - /gimbal/putter/torque + # - /gimbal/putter/control_torque + # - /gimbal/bullet_feeder/velocity + # - /gimbal/bullet_feeder/angle + +climber_controller: + ros__parameters: + front_climber_velocity: 22.0 + back_climber_velocity: 30.0 + auto_climb_support_retract_velocity_fast: 70.0 + auto_climb_support_retract_velocity_slow: 20.0 + auto_climb_approach_chassis_velocity: 2.0 + auto_climb_support_deploy_chassis_velocity: 0.4 + auto_climb_support_retract_chassis_velocity: 0.15 + auto_climb_dash_chassis_velocity: 3.0 + first_stair_dash_leveled_pitch_threshold: 0.05 + second_stair_dash_leveled_pitch_threshold: -0.09 + sync_coefficient: 0.2 + first_stair_approach_pitch: 0.517 + second_stair_approach_pitch: 0.37 #0.365 + front_kp: 1.0 + front_ki: 0.0 + front_kd: 0.5 + front_power_estimate_bias: 0.0 + front_power_estimate_k_tau2: 1.0 + front_power_estimate_k_mech: 1.0 + back_kp: 0.5 + back_ki: 0.0 + back_kd: 0.0 + +chassis_power_controller: + ros__parameters: + front_climber_power_limit_max: 50.0 + drive_power_limit_floor: 50.0 + auto_climb_min_control_power_limit: 130.0 + +gimbal_controller: + ros__parameters: + upper_limit: -0.718 + lower_limit: 0.255 + +dual_yaw_controller: + ros__parameters: + top_yaw_angle_kp: 14.0 # 30.2 + top_yaw_angle_ki: 0.0 + top_yaw_angle_kd: 0.0 + top_yaw_velocity_kp: 9.37 # 11.0 + top_yaw_velocity_ki: 0.00033 # 0.00029 + top_yaw_velocity_kd: 0.0 + top_yaw_velocity_integral_min: -2500.0 + top_yaw_velocity_integral_max: 2500.0 + bottom_yaw_angle_kp: 10.0 # 18.4 + bottom_yaw_angle_ki: 0.0 + bottom_yaw_angle_kd: 0.0 + bottom_yaw_velocity_kp: 2.00 # 2.81 + bottom_yaw_velocity_ki: 0.000071 # 0.00028 + bottom_yaw_velocity_kd: 0.0 + bottom_yaw_velocity_integral_min: -2500.0 + bottom_yaw_velocity_integral_max: 2500.0 + +pitch_angle_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/control_angle_error + control: /gimbal/pitch/control_velocity + kp: 18.6 + ki: 0.0 + kd: 0.0 + +pitch_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/velocity_imu + setpoint: /gimbal/pitch/control_velocity + control: /gimbal/pitch/control_torque + kp: 16.05 + ki: 0.00014 + kd: 0.0 + integral_min: -2500.0 + integral_max: 2500.0 + +gimbal_player_viewer_controller: + ros__parameters: + upper_limit: 0.415996 + lower_limit: -0.066441 + +viewer_angle_pid_controller: + ros__parameters: + measurement: /gimbal/player_viewer/control_angle_error + control: /gimbal/player_viewer/control_velocity + kp: 4.00 + ki: 0.00 + kd: 0.50 + +bullet_feeder_controller: + ros__parameters: + bullet_feeder_velocity_kp: 5.0 + bullet_feeder_velocity_ki: 0.1 + bullet_feeder_velocity_kd: 0.0 + bullet_feeder_velocity_integral_min: 0.0 + bullet_feeder_velocity_integral_max: 60.0 + bullet_feeder_angle_kp: 6.0 + bullet_feeder_angle_ki: 0.0 + bullet_feeder_angle_kd: 1.6 + putter_return_velocity_kp: 0.003 + putter_return_velocity_ki: 0.00005 + putter_return_velocity_kd: 0.0 + putter_return_velocity_integral_min: -0.03 + putter_return_velocity_integral_max: 0.0 + photoelectric_allow: true + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/first_front_friction + - /gimbal/second_front_friction + - /gimbal/third_front_friction + - /gimbal/first_back_friction + - /gimbal/second_back_friction + - /gimbal/third_back_friction + friction_velocities_profile_0: + - 390.0 + - 390.0 + - 390.0 + - 480.0 + - 480.0 + - 480.0 + # - 527.0 + # - 527.0 + # - 527.0 + # - 435.0 + # - 435.0 + # - 435.0 + friction_velocities_profile_1: + - 527.0 + - 527.0 + - 527.0 + - 435.0 + - 435.0 + - 435.0 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 100000 + reserved_heat: 0 + +shooting_recorder: + ros__parameters: + friction_wheel_count: 6 + aim_velocity: 16.15 + log_mode: 1 # 1: trigger, 2: timing + +first_front_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/first_front_friction/velocity + setpoint: /gimbal/first_front_friction/control_velocity + control: /gimbal/first_front_friction/control_torque + kp: 0.006233371 + ki: 0.00 #0.00003 + kd: 0.000001 + +second_front_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/second_front_friction/velocity + setpoint: /gimbal/second_front_friction/control_velocity + control: /gimbal/second_front_friction/control_torque + kp: 0.006035661 + ki: 0.00 #0.00003 + kd: 0.000001 + +third_front_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/third_front_friction/velocity + setpoint: /gimbal/third_front_friction/control_velocity + control: /gimbal/third_front_friction/control_torque + kp: 0.006192421 + ki: 0.00 #0.00003 + kd: 0.000001 + +first_back_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/first_back_friction/velocity + setpoint: /gimbal/first_back_friction/control_velocity + control: /gimbal/first_back_friction/control_torque + kp: 0.007749503 + ki: 0.00 + kd: 0.00003 + +second_back_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/second_back_friction/velocity + setpoint: /gimbal/second_back_friction/control_velocity + control: /gimbal/second_back_friction/control_torque + kp: 0.007795934 + ki: 0.00 + kd: 0.00003 + +third_back_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/third_back_friction/velocity + setpoint: /gimbal/third_back_friction/control_velocity + control: /gimbal/third_back_friction/control_torque + kp: 0.007760993 + ki: 0.00 + kd: 0.00003 + +steering_wheel_status: + ros__parameters: + vehicle_radius: 0.286378 + wheel_radius: 0.055 + +steering_wheel_controller: + ros__parameters: + mess: 22.0 + moment_of_inertia: 1.08 + vehicle_radius: 0.286378 + wheel_radius: 0.055 + friction_coefficient: 0.6 + k1: 2.958580e+00 + k2: 3.082190e-03 + no_load_power: 11.37 + +pitch_swept_frequency_controller: + ros__parameters: + target: /gimbal/pitch + + sweep: true + logarithmic: true + start_freq: 0.1 + end_freq: 10.0 + duration: 60.0 + amplitude: 6.8 + + pid: true + setpoint: 0.0 + position_kp: 20.0 + position_ki: 0.0 + position_kd: 0.0 + velocity_kp: 1.65 + velocity_ki: 0.0 + velocity_kd: 0.0 + dc_offset: 0.0 + +pitch_static_torque_test_controller: + ros__parameters: + target: /gimbal/pitch + + interval_angle: 0.02 + wait_time: 1.5 + border_clip: 0.02 + + position_kp: 12.0 + position_ki: 0.0 + position_kd: 0.0 + + velocity_kp: 3.2 + velocity_ki: 0.0005 + velocity_kd: 0.0 + + velocity_integral_min: -2500.0 + velocity_integral_max: 2500.0 + +top_yaw_swept_frequency_controller: + ros__parameters: + target: /gimbal/top_yaw + + sweep: true + logarithmic: true + start_freq: 0.1 + end_freq: 10.0 + duration: 60.0 + amplitude: 15.0 + + pid: true + setpoint: 0.0 + position_kp: 10.0 + position_ki: 0.0 + position_kd: 0.0 + velocity_kp: 3.0 + velocity_ki: 0.0 + velocity_kd: 0.0 + dc_offset: 0.0 + +bottom_yaw_swept_frequency_controller: + ros__parameters: + target: /gimbal/bottom_yaw + + sweep: true + logarithmic: true + start_freq: 0.1 + end_freq: 4.0 + duration: 80.0 + amplitude: 1.0 + +first_front_friction_swept_frequency_controller: + ros__parameters: + target: /gimbal/first_front_friction + sweep: true + pid: false + logarithmic: true + start_freq: 1.0 + end_freq: 45.0 + duration: 60.0 + amplitude: 0.08 + dc_offset: 0.01 + +second_front_friction_swept_frequency_controller: + ros__parameters: + target: /gimbal/second_front_friction + sweep: true + pid: false + logarithmic: true + start_freq: 1.0 + end_freq: 45.0 + duration: 60.0 + amplitude: 0.08 + dc_offset: 0.04 + +third_front_friction_swept_frequency_controller: + ros__parameters: + target: /gimbal/third_front_friction + sweep: true + pid: false + logarithmic: true + start_freq: 1.0 + end_freq: 45.0 + duration: 60.0 + amplitude: 0.08 + dc_offset: 0.02 + +first_back_friction_swept_frequency_controller: + ros__parameters: + target: /gimbal/first_back_friction + sweep: true + pid: false + logarithmic: true + start_freq: 1.0 + end_freq: 45.0 + duration: 60.0 + amplitude: 0.08 + dc_offset: 0.021 + +second_back_friction_swept_frequency_controller: + ros__parameters: + target: /gimbal/second_back_friction + sweep: true + pid: false + logarithmic: true + start_freq: 1.0 + end_freq: 45.0 + duration: 60.0 + amplitude: 0.08 + dc_offset: 0.029 + +third_back_friction_swept_frequency_controller: + ros__parameters: + target: /gimbal/third_back_friction + sweep: true + pid: false + logarithmic: true + start_freq: 1.0 + end_freq: 45.0 + duration: 40.0 + amplitude: 0.08 + dc_offset: 0.00 diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little.yaml deleted file mode 100644 index 539707670..000000000 --- a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little.yaml +++ /dev/null @@ -1,417 +0,0 @@ -rmcs_executor: - ros__parameters: - update_rate: 1000.0 - components: - - rmcs_core::hardware::SteeringHeroLittle -> hero_hardware - - - rmcs_core::referee::Status -> referee_status - - rmcs_core::referee::command::Interaction -> referee_interaction - - rmcs_core::referee::command::interaction::Ui -> referee_ui - - rmcs_core::referee::app::ui::Hero -> referee_ui_hero - - rmcs_core::referee::Command -> referee_command - - - rmcs_core::controller::gimbal::HeroGimbalController -> gimbal_controller - - rmcs_core::controller::gimbal::DualYawController -> dual_yaw_controller - - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller - - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller - - - rmcs_core::controller::gimbal::PlayerViewer -> gimbal_player_viewer_controller - - rmcs_core::controller::pid::ErrorPidController -> viewer_angle_pid_controller - - - rmcs_core::controller::shooting::HeroFrictionWheelController -> friction_wheel_controller - - rmcs_core::controller::shooting::HeroHeatController -> heat_controller - - rmcs_core::controller::shooting::PutterController -> bullet_feeder_controller - - rmcs_core::controller::pid::PidController -> first_left_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> first_right_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> second_left_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> second_right_friction_velocity_pid_controller - # - rmcs_core::controller::shooting::ShootingRecorder -> shooting_recorder - - - rmcs_core::controller::chassis::SteeringWheelStatus -> steering_wheel_status - - rmcs_core::controller::chassis::HeroChassisController -> chassis_controller - - rmcs_core::controller::chassis::HeroChassisPowerController -> chassis_power_controller - - rmcs_core::controller::chassis::HeroSteeringWheelController -> steering_wheel_controller - - rmcs_core::controller::chassis::ChassisClimberController -> climber_controller - - # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer - # - rmcs_auto_aim::AutoAimController -> auto_aim_controller - - # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster - # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster - - # - sp_vision_25::bridge::HeroAutoAimBridge -> hero_auto_aim_bridge - - # - rmcs_core::controller::identification::SweptFrequencyController -> pitch_swept_frequency_controller - # - rmcs_core::controller::identification::StaticTorqueTestController -> pitch_static_torque_test_controller - # - rmcs_core::controller::identification::SweptFrequencyController -> top_yaw_swept_frequency_controller - # - rmcs_core::controller::identification::SweptFrequencyController -> bottom_yaw_swept_frequency_controller - -hero_hardware: - ros__parameters: - board_serial_top_board: "AF-BFF7-0B6F-46A5-2B4B-AA20-89C2-E180-64B9" - serial_bottom_rmcs_board: "AF-60BB-7484-FA24-F3FC-399B-454D-22FA-1B1D" - bottom_yaw_motor_zero_point: 35078 - pitch_motor_zero_point: 57241 - top_yaw_motor_zero_point: 48537 - viewer_motor_zero_point: 31940 - external_imu_port: /dev/ttyUSB0 - bullet_feeder_motor_zero_point: 60480 #39045 - left_front_zero_point: 5799 - right_front_zero_point: 3700 - left_back_zero_point: 7862 - right_back_zero_point: 1608 - -value_broadcaster: - ros__parameters: - forward_list: - - /gimbal/top_yaw/control_angle - - /chassis/climber/left_front_motor/torque - - /chassis/climber/right_front_motor/torque - - /chassis/left_front_steering/torque - - /chassis/left_back_steering/torque - - /chassis/right_front_steering/torque - - /chassis/right_back_steering/torque - # - /shoot/heat12707 - # - /chassis/power - # - /referee/chassis/power - # - /referee/chassis/power_limit - # - /chassis/control_power_limit - # - /chassis/climber/front/control_power_limit - # - /chassis/climber/front/power_demand_estimate - # - /chassis/climber/front/actual_power_estimate - # - /chassis/steering_wheel/actual_power_estimate - # - /gimbal/putter/velocity - - /gimbal/first_left_friction/velocity - - /gimbal/first_right_friction/velocity - - /gimbal/second_left_friction/velocity - - /gimbal/second_right_friction/velocity - # - /gimbal/first_left_friction/control_torque - # - /gimbal/first_second_friction/control_torque - # - /gimbal/second_left_friction/control_torque - # - /gimbal/second_right_friction/control_torque - # - /gimbal/bottom_yaw/torque - # - /gimbal/bottom_yaw/angle - # - /gimbal/top_yaw/angle - # - /gimbal/pitch/angle - # - /chassis/power - - /chassis/supercap/voltage - # - /chassis/control_power_limit - # - /referee/chassis/power_limit - # - /referee/chassis/buffer_energy - # - /chassis/climber/front/control_power_limit - # - /chassis/climber/front/power_demand_estimate - # - /chassis/climber/front/actual_power_estimate - # - /chassis/steering_wheel/actual_power_estimate - - /gimbal/bullet_feeder/torque - - /gimbal/bullet_feeder/control_torque - - /gimbal/putter/angle - - /gimbal/putter/velocity - - /gimbal/putter/torque - - /gimbal/putter/control_torque - - /gimbal/bullet_feeder/velocity - - /gimbal/bullet_feeder/angle - - -climber_controller: - ros__parameters: - front_climber_velocity: 20.0 - back_climber_velocity: 30.0 - auto_climb_support_retract_velocity_fast: 60.0 - auto_climb_support_retract_velocity_slow: 20.0 - auto_climb_approach_chassis_velocity: 2.0 - auto_climb_support_deploy_chassis_velocity: 0.3 - auto_climb_support_retract_chassis_velocity: 0.3 - auto_climb_dash_chassis_velocity: 3.0 - first_stair_dash_leveled_pitch_threshold: 0.05 - second_stair_dash_leveled_pitch_threshold: -0.09 - sync_coefficient: 0.2 - first_stair_approach_pitch: 0.517 - second_stair_approach_pitch: 0.365 - front_kp: 1.0 - front_ki: 0.0 - front_kd: 0.5 - front_power_estimate_bias: 0.0 - front_power_estimate_k_tau2: 1.0 - front_power_estimate_k_mech: 1.0 - back_kp: 0.5 - back_ki: 0.0 - back_kd: 0.0 - -chassis_power_controller: - ros__parameters: - front_climber_power_limit_max: 50.0 - drive_power_limit_floor: 50.0 - auto_climb_min_control_power_limit: 130.0 - -gimbal_controller: - ros__parameters: - upper_limit: -0.668 - lower_limit: 0.320 - -dual_yaw_controller: - ros__parameters: - top_yaw_angle_kp: 14.0 #30.2 - top_yaw_angle_ki: 0.0 - top_yaw_angle_kd: 0.0 - top_yaw_velocity_kp: 9.37 #11.0 - top_yaw_velocity_ki: 0.00033 #0.00029 - top_yaw_velocity_kd: 0.0 - top_yaw_velocity_integral_min: -2500.0 - top_yaw_velocity_integral_max: 2500.0 - bottom_yaw_angle_kp: 10.0 #18.4 - bottom_yaw_angle_ki: 0.0 - bottom_yaw_angle_kd: 0.0 - bottom_yaw_velocity_kp: 2.00 #2.81 - bottom_yaw_velocity_ki: 0.000071 #0.00028 - bottom_yaw_velocity_kd: 0.0 - bottom_yaw_velocity_integral_min: -2500.0 - bottom_yaw_velocity_integral_max: 2500.0 - -pitch_angle_pid_controller: - ros__parameters: - measurement: /gimbal/pitch/control_angle_error - control: /gimbal/pitch/control_velocity - kp: 18.6 - ki: 0.0 - kd: 0.0 - -pitch_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/pitch/velocity_imu - setpoint: /gimbal/pitch/control_velocity - control: /gimbal/pitch/control_torque - kp: 16.05 - ki: 0.00014 - kd: 0.0 - integral_min: -2500.0 - integral_max: 2500.0 - -gimbal_player_viewer_controller: - ros__parameters: - upper_limit: 0.415996 - lower_limit: -0.066441 - -viewer_angle_pid_controller: - ros__parameters: - measurement: /gimbal/player_viewer/control_angle_error - control: /gimbal/player_viewer/control_velocity - kp: 4.00 - ki: 0.00 - kd: 0.50 - -bullet_feeder_controller: - ros__parameters: - bullet_feeder_velocity_kp: 5.0 - bullet_feeder_velocity_ki: 0.1 #1.1 - bullet_feeder_velocity_kd: 0.0 - bullet_feeder_velocity_integral_min: 0.0 - bullet_feeder_velocity_integral_max: 60.0 - bullet_feeder_angle_kp: 6.0 - bullet_feeder_angle_ki: 0.0 - bullet_feeder_angle_kd: 1.6 - putter_return_velocity_kp: 0.0025 - putter_return_velocity_ki: 0.00005 - putter_return_velocity_kd: 0.0 - putter_return_velocity_integral_min: -0.03 - putter_return_velocity_integral_max: 0.0 - photoelectric_allow: true - -friction_wheel_controller: - ros__parameters: - friction_wheels: - - /gimbal/second_left_friction - - /gimbal/second_right_friction - - /gimbal/first_left_friction - - /gimbal/first_right_friction - friction_velocities_profile_0: - - 380.0 - - 380.0 - - 545.0 - - 545.0 - friction_velocities_profile_1: - - 535.0 - - 535.0 - - 595.0 - - 595.0 - friction_soft_start_stop_time: 1.0 - -heat_controller: - ros__parameters: - heat_per_shot: 100000 - reserved_heat: 0 - -shooting_recorder: - ros__parameters: - friction_wheel_count: 4 - aim_velocity: 11.8 - log_mode: 1 # 1: trigger, 2: timing - -first_left_friction_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/first_left_friction/velocity - setpoint: /gimbal/first_left_friction/control_velocity - control: /gimbal/first_left_friction/control_torque - kp: 0.006 - ki: 0.00 - kd: 0.00016 - -first_right_friction_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/first_right_friction/velocity - setpoint: /gimbal/first_right_friction/control_velocity - control: /gimbal/first_right_friction/control_torque - kp: 0.006 - ki: 0.00 - kd: 0.00016 - -second_left_friction_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/second_left_friction/velocity - setpoint: /gimbal/second_left_friction/control_velocity - control: /gimbal/second_left_friction/control_torque - kp: 0.006 - ki: 0.00 - kd: 0.00008 - -second_right_friction_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/second_right_friction/velocity - setpoint: /gimbal/second_right_friction/control_velocity - control: /gimbal/second_right_friction/control_torque - kp: 0.006 - ki: 0.00 - kd: 0.00008 - -steering_wheel_status: - ros__parameters: - vehicle_radius: 0.286378 - wheel_radius: 0.055 - -steering_wheel_controller: - ros__parameters: - mess: 22.0 - moment_of_inertia: 1.08 - vehicle_radius: 0.318198 - wheel_radius: 0.055 - friction_coefficient: 0.6 - k1: 2.958580e+00 - k2: 3.082190e-03 - no_load_power: 11.37 - -auto_aim_controller: - ros__parameters: - # capture - use_video: false # If true, use video stream instead of camera. - video_path: "/workspaces/RMCS/rmcs_ws/resources/1.avi" - exposure_time: 1 - invert_image: false - # identifier - armor_model_path: "/models/mlp.onnx" - # pnp - fx: 1.722231837421459e+03 - fy: 1.724876404292754e+03 - cx: 7.013056440882832e+02 - cy: 5.645821718351237e+02 - k1: -0.064232403853946 - k2: -0.087667493884102 - k3: 0.792381808294582 - # tracker - armor_predict_duration: 500 - # controller - gimbal_predict_duration: 100 - yaw_error: 0. - pitch_error: 0. - shoot_velocity: 28.0 - predict_sec: 0.095 - # etc - buff_predict_duration: 200 - buff_model_path: "/models/buff_nocolor_v6.onnx" - omni_exposure: 1000.0 - record_fps: 120 - debug: false # Setup in actual using.Debug mode is used when referee is not ready - debug_color: 0 # 0 For blue while 1 for red. mine - debug_robot_id: 4 - debug_buff_mode: false - record: false - raw_img_pub: false # Set false in actual use - image_viewer_type: 2 - -hero_auto_aim_bridge: - ros__parameters: - config_file: "configs/standard3.yaml" - bullet_speed_fallback: 11.4 - result_timeout: 0.1 # 0.08 - debug: false - - -pitch_swept_frequency_controller: - ros__parameters: - target: /gimbal/pitch - - sweep: true - logarithmic: true - start_freq: 0.1 - end_freq: 10.0 - duration: 60.0 - amplitude: 10.0 - - pid: true - setpoint: 0.0 - position_kp: 20.0 - position_ki: 0.0 - position_kd: 0.0 - velocity_kp: 1.65 - velocity_ki: 0.0 - velocity_kd: 0.0 - dc_offset: 0.0 - -pitch_static_torque_test_controller: - ros__parameters: - target: /gimbal/pitch - - interval_angle: 0.05 - wait_time: 1.5 - border_clip: 0.05 - - position_kp: 12.0 - position_ki: 0.0 - position_kd: 0.0 - - velocity_kp: 3.2 - velocity_ki: 0.0005 - velocity_kd: 0.0 - - velocity_integral_min: -2500.0 - velocity_integral_max: 2500.0 - -top_yaw_swept_frequency_controller: - ros__parameters: - target: /gimbal/top_yaw - - sweep: true - logarithmic: true - start_freq: 0.1 - end_freq: 10.0 - duration: 60.0 - amplitude: 15.0 - - pid: true - setpoint: 0.0 - position_kp: 10.0 - position_ki: 0.0 - position_kd: 0.0 - velocity_kp: 3.0 - velocity_ki: 0.0 - velocity_kd: 0.0 - dc_offset: 0.0 - -bottom_yaw_swept_frequency_controller: - ros__parameters: - target: /gimbal/bottom_yaw - - sweep: true - logarithmic: true - start_freq: 0.1 - end_freq: 4.0 - duration: 80.0 - amplitude: 1.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml deleted file mode 100644 index a6e01d35c..000000000 --- a/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml +++ /dev/null @@ -1,399 +0,0 @@ -rmcs_executor: - ros__parameters: - update_rate: 1000.0 - components: - - rmcs_core::hardware::SteeringHero -> hero_hardware - - - rmcs_core::referee::Status -> referee_status - - rmcs_core::referee::command::Interaction -> referee_interaction - - rmcs_core::referee::command::interaction::Ui -> referee_ui - - rmcs_core::referee::app::ui::Hero -> referee_ui_hero - - rmcs_core::referee::Command -> referee_command - - - rmcs_core::controller::gimbal::HeroGimbalController -> gimbal_controller - - rmcs_core::controller::gimbal::DualYawController -> dual_yaw_controller - - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller - - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller - - - rmcs_core::controller::gimbal::PlayerViewer -> gimbal_player_viewer_controller - - rmcs_core::controller::pid::ErrorPidController -> viewer_angle_pid_controller - - - rmcs_core::controller::shooting::HeroFrictionWheelController -> friction_wheel_controller - - rmcs_core::controller::shooting::HeroHeatController -> heat_controller - - rmcs_core::controller::shooting::PutterController -> bullet_feeder_controller - - rmcs_core::controller::pid::PidController -> first_left_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> first_right_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> second_left_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> second_right_friction_velocity_pid_controller - # - rmcs_core::controller::shooting::ShootingRecorder -> shooting_recorder - - - rmcs_core::controller::chassis::SteeringWheelStatus -> steering_wheel_status - - rmcs_core::controller::chassis::HeroChassisController -> chassis_controller - - rmcs_core::controller::chassis::HeroChassisPowerController -> chassis_power_controller - - rmcs_core::controller::chassis::HeroSteeringWheelController -> steering_wheel_controller - - rmcs_core::controller::chassis::ChassisClimberController -> climber_controller - - # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer - # - rmcs_auto_aim::AutoAimController -> auto_aim_controller - - # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster - # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster - - # - sp_vision_25::bridge::HeroAutoAimBridge -> hero_auto_aim_bridge - - # - rmcs_core::controller::identification::SweptFrequencyController -> pitch_swept_frequency_controller - # - rmcs_core::controller::identification::StaticTorqueTestController -> pitch_static_torque_test_controller - # - rmcs_core::controller::identification::SweptFrequencyController -> top_yaw_swept_frequency_controller - # - rmcs_core::controller::identification::SweptFrequencyController -> bottom_yaw_swept_frequency_controller - - - - -hero_hardware: - ros__parameters: - board_serial_top_board: "D4-2BCA-2E47-76CD-23BC-0B78-684B" - board_serial_bottom_board_one: "D4-7973-19A9-EA40-4A3E-306F-10F9" - board_serial_bottom_board_two: "D4-3674-7174-8768-879E-E44A-3931" - bottom_yaw_motor_zero_point: 37424 - pitch_motor_zero_point: 14916 - top_yaw_motor_zero_point: 33076 - viewer_motor_zero_point: 3030 - external_imu_port: /dev/ttyUSB0 - bullet_feeder_motor_zero_point: 11865 - left_front_zero_point: 4398 - right_front_zero_point: 1081 - left_back_zero_point: 3794 - right_back_zero_point: 5839 - - - - -value_broadcaster: - ros__parameters: - forward_list: - # - /shoot/heat - - /chassis/power - # - /referee/chassis/power - # - /referee/chassis/power_limit - - /chassis/control_power_limit - - /chassis/climber/front/control_power_limit - - /chassis/climber/front/power_demand_estimate - - /chassis/climber/front/actual_power_estimate - - /chassis/steering_wheel/actual_power_estimate - # - /chassis/supercap/voltage - # - /gimbal/putter/velocity - - /gimbal/first_left_friction/velocity - - /gimbal/first_right_friction/velocity - - /gimbal/second_left_friction/velocity - - /gimbal/second_right_friction/velocity - # - /gimbal/first_left_friction/control_torque - # - /gimbal/first_second_friction/control_torque - # - /gimbal/second_left_friction/control_torque - # - /gimbal/second_right_friction/control_torque - # - /gimbal/bottom_yaw/torque - - /gimbal/bottom_yaw/control_angle_shift - - /gimbal/bottom_yaw/angle - - /gimbal/top_yaw/angle - - /gimbal/pitch/angle - - # - /gimbal/pitch/velocity_imu - # - /gimbal/pitch/control_velocity - # - /gimbal/pitch/control_torque - - - /gimbal/auto_aim/plan_yaw - - /gimbal/auto_aim/plan_pitch - -climber_controller: - ros__parameters: - front_climber_velocity: 20.0 - back_climber_velocity: 30.0 - auto_climb_support_retract_velocity_fast: 60.0 - auto_climb_support_retract_velocity_slow: 20.0 - auto_climb_approach_chassis_velocity: 2.0 - auto_climb_support_deploy_chassis_velocity: 0.3 - auto_climb_support_retract_chassis_velocity: 0.3 - auto_climb_dash_chassis_velocity: 3.0 - first_stair_dash_leveled_pitch_threshold: 0.05 - second_stair_dash_leveled_pitch_threshold: -0.09 - sync_coefficient: 0.2 - first_stair_approach_pitch: 0.517 - second_stair_approach_pitch: 0.365 - front_kp: 1.0 - front_ki: 0.0 - front_kd: 0.5 - front_power_estimate_bias: 0.0 - front_power_estimate_k_tau2: 1.0 - front_power_estimate_k_mech: 1.0 - back_kp: 0.5 - back_ki: 0.0 - back_kd: 0.0 - -chassis_power_controller: - ros__parameters: - front_climber_power_limit_max: 60.0 - drive_power_limit_floor: 50.0 - auto_climb_min_control_power_limit: 150.0 - -gimbal_controller: - ros__parameters: - upper_limit: -0.688 - lower_limit: 0.357 - -dual_yaw_controller: - ros__parameters: - top_yaw_angle_kp: 5.0 - top_yaw_angle_ki: 0.0 - top_yaw_angle_kd: 0.0 - top_yaw_velocity_kp: 10.0 - top_yaw_velocity_ki: 0.0 - top_yaw_velocity_kd: 0.0 - top_yaw_velocity_integral_min: -2500.0 - top_yaw_velocity_integral_max: 2500.0 - bottom_yaw_angle_kp: 13.9 - bottom_yaw_angle_ki: 0.0 - bottom_yaw_angle_kd: 0.0 - bottom_yaw_velocity_kp: 2.0 - bottom_yaw_velocity_ki: 0.0 - bottom_yaw_velocity_kd: 0.0 - -# dual_yaw_controller: -# ros__parameters: -# top_yaw_angle_kp: 24.5 -# top_yaw_angle_ki: 0.0 -# top_yaw_angle_kd: 0.0 -# top_yaw_velocity_kp: 77.4 -# top_yaw_velocity_ki: 0.004 -# top_yaw_velocity_kd: 1.0 -# bottom_yaw_angle_kp: 8.6 -# bottom_yaw_angle_ki: 0.0 -# bottom_yaw_angle_kd: 0.0 -# bottom_yaw_velocity_kp: 25.85 -# bottom_yaw_velocity_ki: 0.0 -# bottom_yaw_velocity_kd: 50.0 - -pitch_angle_pid_controller: - ros__parameters: - measurement: /gimbal/pitch/control_angle_error - control: /gimbal/pitch/control_velocity - kp: 10.0 - ki: 0.0 - kd: 0.0 - -pitch_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/pitch/velocity_imu - setpoint: /gimbal/pitch/control_velocity - control: /gimbal/pitch/control_torque - kp: 12.00 #45.00 - ki: 0.00 - kd: 0.0 #1.00 - -gimbal_player_viewer_controller: - ros__parameters: - upper_limit: 0.68 - lower_limit: 1.17 - -viewer_angle_pid_controller: - ros__parameters: - measurement: /gimbal/player_viewer/control_angle_error - control: /gimbal/player_viewer/control_velocity - kp: 17.00 - ki: 0.00 - kd: 2.00 - -friction_wheel_controller: - ros__parameters: - friction_wheels: - - /gimbal/first_left_friction - - /gimbal/first_right_friction - - /gimbal/second_left_friction - - /gimbal/second_right_friction - friction_velocities_profile_0: - - 368.00 - - 368.00 - - 532.00 - - 532.00 - friction_velocities_profile_1: - - 525.0 - - 525.0 - - 585.0 - - 585.0 - friction_soft_start_stop_time: 1.0 - -heat_controller: - ros__parameters: - heat_per_shot: 100000 - reserved_heat: 0 - -shooting_recorder: - ros__parameters: - friction_wheel_count: 4 - aim_velocity: 11.8 - log_mode: 1 # 1: trigger, 2: timing - -bullet_feeder_controller: - ros__parameters: - bullet_feeder_velocity_kp: 5.5 - bullet_feeder_velocity_ki: 1.1 - bullet_feeder_velocity_kd: 0.0 - bullet_feeder_velocity_integral_min: 0.0 - bullet_feeder_velocity_integral_max: 60.0 - bullet_feeder_angle_kp: 5.0 - bullet_feeder_angle_ki: 0.0 - bullet_feeder_angle_kd: 1.0 - putter_return_velocity_kp: 0.0015 - putter_return_velocity_ki: 0.00005 - putter_return_velocity_kd: 0.0 - putter_return_velocity_integral_min: -0.03 - putter_return_velocity_integral_max: 0.0 - photoelectric_allow: false - -first_left_friction_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/first_left_friction/velocity - setpoint: /gimbal/first_left_friction/control_velocity - control: /gimbal/first_left_friction/control_torque - kp: 0.0005 - ki: 0.00 - kd: 0.00004 - -first_right_friction_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/first_right_friction/velocity - setpoint: /gimbal/first_right_friction/control_velocity - control: /gimbal/first_right_friction/control_torque - kp: 0.0005 - ki: 0.00 - kd: 0.00004 - -second_left_friction_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/second_left_friction/velocity - setpoint: /gimbal/second_left_friction/control_velocity - control: /gimbal/second_left_friction/control_torque - kp: 0.0009 - ki: 0.00 - kd: 0.00008 - -second_right_friction_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/second_right_friction/velocity - setpoint: /gimbal/second_right_friction/control_velocity - control: /gimbal/second_right_friction/control_torque - kp: 0.0009 - ki: 0.00 - kd: 0.00008 - -steering_wheel_status: - ros__parameters: - vehicle_radius: 0.318198 - wheel_radius: 0.055 - -steering_wheel_controller: - ros__parameters: - mess: 22.0 - moment_of_inertia: 1.08 - vehicle_radius: 0.318198 - wheel_radius: 0.055 - friction_coefficient: 0.6 - k1: 2.958580e+00 - k2: 3.082190e-03 - no_load_power: 11.37 - -auto_aim_controller: - ros__parameters: - # capture - use_video: false # If true, use video stream instead of camera. - video_path: "/workspaces/RMCS/rmcs_ws/resources/1.avi" - exposure_time: 1 - invert_image: false - # identifier - armor_model_path: "/models/mlp.onnx" - # pnp - fx: 1.722231837421459e+03 - fy: 1.724876404292754e+03 - cx: 7.013056440882832e+02 - cy: 5.645821718351237e+02 - k1: -0.064232403853946 - k2: -0.087667493884102 - k3: 0.792381808294582 - # tracker - armor_predict_duration: 500 - # controller - gimbal_predict_duration: 100 - yaw_error: 0. - pitch_error: 0. - shoot_velocity: 28.0 - predict_sec: 0.095 - # etc - buff_predict_duration: 200 - buff_model_path: "/models/buff_nocolor_v6.onnx" - omni_exposure: 1000.0 - record_fps: 120 - debug: false # Setup in actual using.Debug mode is used when referee is not ready - debug_color: 0 # 0 For blue while 1 for red. mine - debug_robot_id: 4 - debug_buff_mode: false - record: false - raw_img_pub: false # Set false in actual use - image_viewer_type: 2 - -# hero_auto_aim_bridge: -# ros__parameters: -# config_file: "configs/standard3.yaml" -# bullet_speed_fallback: 11.7 -# result_timeout: 0.1 # 0.08 -# debug: false - -pitch_swept_frequency_controller: - ros__parameters: - target: /gimbal/pitch - - sweep: true - logarithmic: true - start_freq: 0.1 - end_freq: 10.0 - duration: 60.0 - amplitude: 10.0 - - pid: true - setpoint: 0.0 - position_kp: 20.0 - position_ki: 0.0 - position_kd: 0.0 - velocity_kp: 1.65 - velocity_ki: 0.0 - velocity_kd: 0.0 - dc_offset: 0.0 - -top_yaw_swept_frequency_controller: - ros__parameters: - target: /gimbal/top_yaw - - sweep: true - logarithmic: true - start_freq: 0.1 - end_freq: 10.0 - duration: 60.0 - amplitude: 20.0 - - pid: true - setpoint: 0.0 - position_kp: 10.0 - position_ki: 0.0 - position_kd: 0.0 - velocity_kp: 3.0 - velocity_ki: 0.0 - velocity_kd: 0.0 - dc_offset: 0.0 - -bottom_yaw_swept_frequency_controller: - ros__parameters: - target: /gimbal/bottom_yaw - - sweep: true - logarithmic: true - start_freq: 0.1 - end_freq: 4.0 - duration: 80.0 - amplitude: 2.0 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp index dd12e09ee..9291886ed 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp @@ -81,18 +81,18 @@ class HeroChassisController if (switch_left != Switch::DOWN) { if (last_switch_right_ == Switch::MIDDLE && switch_right == Switch::DOWN) { - if (mode == rmcs_msgs::ChassisMode::SPIN) { - mode = rmcs_msgs::ChassisMode::STEP_DOWN; - } else { - mode = rmcs_msgs::ChassisMode::SPIN; + if (mode != rmcs_msgs::ChassisMode::SPIN_FAST) { + mode = rmcs_msgs::ChassisMode::SPIN_FAST; spinning_forward_ = !spinning_forward_; + } else { + mode = rmcs_msgs::ChassisMode::STEP_DOWN; } } else if (!last_keyboard_.c && keyboard.c) { - if (mode == rmcs_msgs::ChassisMode::SPIN) { - mode = rmcs_msgs::ChassisMode::AUTO; - } else { - mode = rmcs_msgs::ChassisMode::SPIN; + if (mode != rmcs_msgs::ChassisMode::SPIN_FAST) { + mode = rmcs_msgs::ChassisMode::SPIN_FAST; spinning_forward_ = !spinning_forward_; + } else { + mode = rmcs_msgs::ChassisMode::AUTO; } } else if (!last_keyboard_.x && keyboard.x) { if (mode != rmcs_msgs::ChassisMode::STEP_DOWN @@ -189,8 +189,11 @@ class HeroChassisController angular_velocity *= std::clamp(measured_translational_speed / translational_velocity_max, 0.0, 0.3); + angular_velocity = 0.0; + } break; - case rmcs_msgs::ChassisMode::SPIN: { + case rmcs_msgs::ChassisMode::SPIN_SLOW: [[fallthrough]]; + case rmcs_msgs::ChassisMode::SPIN_FAST: { angular_velocity = 0.6 * (spinning_forward_ ? angular_velocity_max : -angular_velocity_max); } break; @@ -198,6 +201,9 @@ class HeroChassisController angular_velocity = update_following_angular_velocity(step_down_facing_, chassis_control_angle); } break; + case rmcs_msgs::ChassisMode::ALIGNMENT: [[fallthrough]]; + case rmcs_msgs::ChassisMode::ALIGNMENT_POWERED: [[fallthrough]]; + case rmcs_msgs::ChassisMode::CLIMB: [[fallthrough]]; case rmcs_msgs::ChassisMode::LAUNCH_RAMP: { double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp index 939f17f6d..f3f8d1310 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp @@ -24,11 +24,7 @@ class HeroGimbalController HeroGimbalController() : Node( get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , upper_limit_(get_parameter("upper_limit").as_double()) - , lower_limit_(get_parameter("lower_limit").as_double()) - , imu_gimbal_solver(*this, upper_limit_, lower_limit_) - , encoder_gimbal_solver(*this, upper_limit_, lower_limit_) { + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { register_input("/remote/joystick/left", joystick_left_); register_input("/remote/switch/left", switch_left_); @@ -37,9 +33,9 @@ class HeroGimbalController register_input("/remote/mouse", mouse_); register_input("/remote/keyboard", keyboard_); - register_input("/gimbal/auto_aim/control_direction", auto_aim_control_direction_, false); - register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); - register_input("/gimbal/pitch/raw_angle", gimbal_pitch_raw_angle_); + register_input("/auto_aim/should_control", auto_aim_should_control_, false); + register_input("/auto_aim/control_direction", auto_aim_control_direction_, false); + register_input("/tf", tf_); register_output("/gimbal/mode", gimbal_mode_, rmcs_msgs::GimbalMode::IMU); @@ -53,6 +49,7 @@ class HeroGimbalController const auto& switch_left = *switch_left_; const auto& switch_right = *switch_right_; + // RCLCPP_INFO(get_logger(), "pitch %f", *gimbal_pitch_angle_); do { using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) @@ -70,20 +67,28 @@ class HeroGimbalController } } + bool switch_encoder_to_imu_by_c = false; + + if (!last_keyboard_.c && keyboard_->c && gimbal_mode_keyboard_ == GimbalMode::ENCODER) { + gimbal_mode_keyboard_ = GimbalMode::IMU; + switch_encoder_to_imu_by_c = true; + } + *gimbal_mode_ = gimbal_mode_keyboard_; //*gimbal_mode_ = switch_right == Switch::UP ? GimbalMode::ENCODER : GimbalMode::IMU; if (*gimbal_mode_ == GimbalMode::IMU) { - auto angle_error = update_imu_control(); + auto angle_error = switch_encoder_to_imu_by_c ? enter_imu_hold_current_pose() + : update_imu_control(); *yaw_angle_error_ = angle_error.yaw_angle_error; *pitch_angle_error_ = angle_error.pitch_angle_error; - encoder_gimbal_solver.update(PreciseTwoAxisGimbalSolver::SetDisabled{}); + encoder_gimbal_solver_.update(PreciseTwoAxisGimbalSolver::SetDisabled{}); *yaw_control_angle_shift_ = nan_; *pitch_control_angle_ = nan_; } else { - imu_gimbal_solver.update(TwoAxisGimbalSolver::SetDisabled{}); + imu_gimbal_solver_.update(TwoAxisGimbalSolver::SetDisabled{}); *yaw_angle_error_ = nan_; *pitch_angle_error_ = nan_; @@ -97,8 +102,8 @@ class HeroGimbalController } void reset_all_control() { - imu_gimbal_solver.update(TwoAxisGimbalSolver::SetDisabled{}); - encoder_gimbal_solver.update(PreciseTwoAxisGimbalSolver::SetDisabled{}); + imu_gimbal_solver_.update(TwoAxisGimbalSolver::SetDisabled{}); + encoder_gimbal_solver_.update(PreciseTwoAxisGimbalSolver::SetDisabled{}); gimbal_mode_keyboard_ = rmcs_msgs::GimbalMode::IMU; *gimbal_mode_ = rmcs_msgs::GimbalMode::IMU; @@ -110,16 +115,20 @@ class HeroGimbalController } TwoAxisGimbalSolver::AngleError update_imu_control() { - if (auto_aim_control_direction_.ready() - && (mouse_->right || *switch_right_ == rmcs_msgs::Switch::UP) - && !auto_aim_control_direction_->isZero()) { - return imu_gimbal_solver.update( + const auto auto_aim_requested = mouse_->right || *switch_right_ == rmcs_msgs::Switch::UP; + const auto should_control = auto_aim_should_control_.ready() && *auto_aim_should_control_; + const auto valid_control = auto_aim_control_direction_.ready() + && auto_aim_control_direction_->allFinite() + && !auto_aim_control_direction_->isZero(); + + if (auto_aim_requested && should_control && valid_control) { + return imu_gimbal_solver_.update( TwoAxisGimbalSolver::SetControlDirection{ OdomImu::DirectionVector{*auto_aim_control_direction_}}); } - if (!imu_gimbal_solver.enabled()) - return imu_gimbal_solver.update(TwoAxisGimbalSolver::SetToLevel{}); + if (!imu_gimbal_solver_.enabled()) + return imu_gimbal_solver_.update(TwoAxisGimbalSolver::SetToLevel{}); constexpr double joystick_sensitivity = 0.006; constexpr double mouse_sensitivity = 0.5; @@ -129,34 +138,46 @@ class HeroGimbalController double pitch_shift = -joystick_sensitivity * joystick_left_->x() + mouse_sensitivity * mouse_velocity_->x(); - return imu_gimbal_solver.update( + return imu_gimbal_solver_.update( TwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); } + TwoAxisGimbalSolver::AngleError enter_imu_hold_current_pose() { + if (!tf_.ready()) + return update_imu_control(); + + auto current_direction = + fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + + return imu_gimbal_solver_.update( + TwoAxisGimbalSolver::SetControlDirection{OdomImu::DirectionVector{*current_direction}}); + } + PreciseTwoAxisGimbalSolver::ControlAngle update_encoder_control() { - if (!encoder_gimbal_solver.enabled()) { - return encoder_gimbal_solver.update( + if (!encoder_gimbal_solver_.enabled()) { + return encoder_gimbal_solver_.update( PreciseTwoAxisGimbalSolver::SetControlPitch{encoder_init_pitch_}); } constexpr double mouse_yaw_sensitivity = 0.5 * 0.114; constexpr double mouse_pitch_sensitivity = 0.5 * 0.095; - constexpr double joystick_sensitivity = 0.006 * 0.05; + constexpr double joystick_sensitivity = 0.006 * 0.02; double yaw_shift = joystick_sensitivity * joystick_left_->y() + mouse_yaw_sensitivity * mouse_velocity_->y(); double pitch_shift = -joystick_sensitivity * joystick_left_->x() + mouse_pitch_sensitivity * mouse_velocity_->x(); - return encoder_gimbal_solver.update( + return encoder_gimbal_solver_.update( PreciseTwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); } private: static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - static constexpr double kEInitPitch = -0.476972; // Initial angle for standalone E. - static constexpr double kCtrlEInitPitch = -0.541591; // Initial angle for Ctrl+E. + static constexpr double kEInitPitch = -0.346584; // Initial angle for standalone E. + static constexpr double kCtrlEInitPitch = -0.471795; // Initial angle for Ctrl+E. + double encoder_init_pitch_ = kEInitPitch; InputInterface joystick_left_; InputInterface switch_right_; @@ -167,24 +188,35 @@ class HeroGimbalController rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + InputInterface auto_aim_should_control_; InputInterface auto_aim_control_direction_; - InputInterface gimbal_pitch_angle_; - InputInterface gimbal_pitch_raw_angle_; + InputInterface tf_; rmcs_msgs::GimbalMode gimbal_mode_keyboard_ = rmcs_msgs::GimbalMode::IMU; OutputInterface gimbal_mode_; - const double upper_limit_, lower_limit_; - TwoAxisGimbalSolver imu_gimbal_solver; - PreciseTwoAxisGimbalSolver encoder_gimbal_solver; - OutputInterface yaw_angle_error_, pitch_angle_error_; OutputInterface yaw_control_angle_shift_, pitch_control_angle_; + + struct SimpleComponent : Component { + auto update() -> void override {} + }; + std::shared_ptr imu_gimbal_solver_component_ = + create_partner_component("imu_gimbal_solver"); + std::shared_ptr encoder_gimbal_solver_component_ = + create_partner_component("encoder_gimbal_solver"); + + const double upper_limit_{get_parameter("upper_limit").as_double()}; + const double lower_limit_{get_parameter("lower_limit").as_double()}; + + TwoAxisGimbalSolver imu_gimbal_solver_ = { + *imu_gimbal_solver_component_, upper_limit_, lower_limit_}; + PreciseTwoAxisGimbalSolver encoder_gimbal_solver_ = { + *encoder_gimbal_solver_component_, upper_limit_, lower_limit_}; }; } // namespace rmcs_core::controller::gimbal #include - PLUGINLIB_EXPORT_CLASS( rmcs_core::controller::gimbal::HeroGimbalController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_friction_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_friction_wheel_controller.cpp index 9f4e62f01..1656d2276 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_friction_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_friction_wheel_controller.cpp @@ -183,7 +183,7 @@ class HeroFrictionWheelController bool detect_friction_faulty() { for (size_t i = 0; i < friction_count_; i++) { if (abs(*friction_velocities_[i]) < abs(*friction_control_velocities_[i] * 0.5)) - return true; + return false; } return false; } @@ -192,13 +192,8 @@ class HeroFrictionWheelController bool detect_bullet_fire() { bool fired = false; - - // TODO(steering-hero): This intentionally keeps the legacy merge behavior by monitoring - // friction_velocities_[2]. Historically, hero config ordering mapped [2,3] to the first - // stage used for fire detection. Replace this hard-coded index with an explicit first- - // stage mapping once the wheel ordering semantics are unified. if (!std::isnan(last_primary_friction_velocity_)) { - double differential = *friction_velocities_[2] - last_primary_friction_velocity_; + double differential = *friction_velocities_[0] - last_primary_friction_velocity_; if (differential < 0.1) primary_friction_velocity_decrease_integral_ += differential; else { @@ -209,7 +204,7 @@ class HeroFrictionWheelController primary_friction_velocity_decrease_integral_ = 0; } } - last_primary_friction_velocity_ = *friction_velocities_[2]; + last_primary_friction_velocity_ = *friction_velocities_[0]; return fired; } diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_heat_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_heat_controller.cpp index dfd2df620..86a136c63 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_heat_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_heat_controller.cpp @@ -31,8 +31,10 @@ class HeroHeatController void update() override { shooter_heat_ = std::max(0, shooter_heat_ - *shooter_cooling_); - if (*bullet_fired_) + const bool bullet_fired = *bullet_fired_; + if (bullet_fired && !last_bullet_fired_) shooter_heat_ += heat_per_shot; + last_bullet_fired_ = bullet_fired; *control_bullet_allowance_ = std::max( 0, (*shooter_heat_limit_ - shooter_heat_ - reserved_heat) / heat_per_shot); @@ -49,6 +51,7 @@ class HeroHeatController const int64_t heat_per_shot; const int64_t reserved_heat; + bool last_bullet_fired_ = false; int64_t shooter_heat_ = 0; OutputInterface shooting_heat_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp similarity index 54% rename from rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp rename to rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp index 40150e605..b216fa6eb 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp @@ -7,13 +7,13 @@ #include #include #include +#include #include #include #include #include -#include -#include +#include #include #include #include @@ -23,16 +23,22 @@ #include #include #include +#include +#include #include #include #include #include "hardware/device/bmi088.hpp" +#include "hardware/device/bmi088_ekf.hpp" +#include "hardware/device/board_clock_lifter.hpp" #include "hardware/device/can_packet.hpp" #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" +#include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" +#include "hardware/device/vt13.hpp" namespace rmcs_core::hardware { @@ -120,11 +126,18 @@ class SteeringHeroLittle register_output("/tf", tf_); + register_output( + "/auto_aim/camera_transform", camera_transform_, Eigen::Isometry3d::Identity()); + register_output("/auto_aim/barrel_direction", barrel_direction_, Eigen::Vector3d::UnitX()); + register_output("/auto_aim/yaw_velocity", auto_aim_yaw_velocity_, 0.0); + gimbal_calibrate_subscription_ = create_subscription( "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { gimbal_calibrate_subscription_callback(std::move(msg)); }); + remote_control_ = std::make_unique(*this); + top_board_ = std::make_unique( *this, *command_component_, get_parameter("board_serial_top_board").as_string()); @@ -132,7 +145,7 @@ class SteeringHeroLittle *this, *command_component_, get_parameter("serial_bottom_rmcs_board").as_string()); tf_->set_transform( - Eigen::Translation3d{0.06603, 0.0, 0.082}); + Eigen::Translation3d{0.22, 0.0, -0.05}); } SteeringHeroLittle(const SteeringHeroLittle&) = delete; @@ -145,12 +158,19 @@ class SteeringHeroLittle void update() override { top_board_->update(); bottom_board_->update(); + remote_control_->update(); tf_->set_state( bottom_board_->gimbal_bottom_yaw_motor_.angle() + top_board_->gimbal_top_yaw_motor_.angle()); tf_->set_state( top_board_->gimbal_pitch_motor_.angle()); + + using namespace rmcs_description; + *camera_transform_ = fast_tf::lookup_transform(*tf_); + *barrel_direction_ = + *fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + *auto_aim_yaw_velocity_ = top_board_->gimbal_yaw_velocity(); } void command_update() { @@ -200,27 +220,25 @@ class SteeringHeroLittle }; std::shared_ptr command_component_; - class TopBoard final : private librmcs::agent::RmcsBoardLite { - public: - friend class SteeringHeroLittle; + struct TopBoard final : librmcs::board::RmcsBoardLite::Callback { explicit TopBoard( SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, std::string_view board_serial = {}) - : librmcs::agent::RmcsBoardLite(board_serial) - , logger_(steering_hero.get_logger()) - // , can0_receive_rate_counter_(logger_, "bottom/can0") - // , can1_receive_rate_counter_(logger_, "bottom/can1") - // , can2_receive_rate_counter_(logger_, "bottom/can2") - // , can3_receive_rate_counter_(logger_, "bottom/can3") + : logger_(steering_hero.get_logger()) , tf_(steering_hero.tf_) - , imu_(1000, 0.2, 0.0) + , bmi088_{device::Bmi088Ekf::Config{ + .body_to_sensor = + Eigen::AngleAxisd{std::numbers::pi / 2.0, Eigen::Vector3d::UnitZ()} + .toRotationMatrix()}} , gimbal_top_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/top_yaw") , gimbal_pitch_motor_(steering_hero, steering_hero_command, "/gimbal/pitch") , gimbal_friction_wheels_( - {steering_hero, steering_hero_command, "/gimbal/first_right_friction"}, - {steering_hero, steering_hero_command, "/gimbal/first_left_friction"}, - {steering_hero, steering_hero_command, "/gimbal/second_right_friction"}, - {steering_hero, steering_hero_command, "/gimbal/second_left_friction"}) + {steering_hero, steering_hero_command, "/gimbal/first_front_friction"}, + {steering_hero, steering_hero_command, "/gimbal/first_back_friction"}, + {steering_hero, steering_hero_command, "/gimbal/second_front_friction"}, + {steering_hero, steering_hero_command, "/gimbal/second_back_friction"}, + {steering_hero, steering_hero_command, "/gimbal/third_front_friction"}, + {steering_hero, steering_hero_command, "/gimbal/third_back_friction"}) , gimbal_bullet_feeder_(steering_hero, steering_hero_command, "/gimbal/bullet_feeder") , putter_motor_(steering_hero, steering_hero_command, "/gimbal/putter") , gimbal_scope_motor_(steering_hero, steering_hero_command, "/gimbal/scope") @@ -240,15 +258,25 @@ class SteeringHeroLittle steering_hero.get_parameter("pitch_motor_zero_point").as_int())) .enable_multi_turn_angle()); gimbal_friction_wheels_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} + .set_reversed() + .set_reduction_ratio(1.)); gimbal_friction_wheels_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2} .set_reversed() .set_reduction_ratio(1.)); gimbal_friction_wheels_[2].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 3}.set_reduction_ratio( + 1.)); gimbal_friction_wheels_[3].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 4}.set_reduction_ratio( + 1.)); + gimbal_friction_wheels_[4].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} + .set_reversed() + .set_reduction_ratio(1.)); + gimbal_friction_wheels_[5].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2} .set_reversed() .set_reduction_ratio(1.)); gimbal_bullet_feeder_.configure( @@ -259,10 +287,11 @@ class SteeringHeroLittle .set_reversed() .enable_multi_turn_angle()); putter_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 3} .set_reduction_ratio(1.) .enable_multi_turn_angle()); - gimbal_scope_motor_.configure(device::DjiMotor::Config{device::DjiMotor::Type::kM2006}); + gimbal_scope_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM2006, 4}); gimbal_player_viewer_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} .set_encoder_zero_point( @@ -274,30 +303,21 @@ class SteeringHeroLittle steering_hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); steering_hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + steering_hero.register_output( + "/gimbal/auto_aim/exposure_signal", camera_signal_output_); + steering_hero.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); + steering_hero.register_output( "/gimbal/photoelectric_sensor", photoelectric_sensor_status_, false); steering_hero.register_output( "/gimbal/grayscale_sensor", grayscale_sensor_status_, false); - steering_hero.register_output( - "/auto_aim/image_capturer/timestamp", camera_capturer_trigger_timestamp_, 0); - steering_hero.register_output( - "/auto_aim/image_capturer/trigger", camera_capturer_trigger_, 0); - - imu_.set_coordinate_mapping([](double x, double y, double z) { - // Get the mapping with the following code. - // The rotation angle must be an exact multiple of 90 degrees, otherwise - // use a matrix. - return std::make_tuple(-y, x, z); - }); - } + board_ = std::make_unique(*this, board_serial); - TopBoard(const TopBoard&) = delete; - TopBoard& operator=(const TopBoard&) = delete; - TopBoard(TopBoard&&) = delete; - TopBoard& operator=(TopBoard&&) = delete; + board_->start_transmit().uart_config(Spec::kUarts.kUart0, {.baudrate = 921600}); - ~TopBoard() final = default; + steering_hero.remote_control_->register_vt13(&vt13_); + } void update() { // can0_receive_rate_counter_.report_if_due(); @@ -305,14 +325,15 @@ class SteeringHeroLittle // can2_receive_rate_counter_.report_if_due(); // can3_receive_rate_counter_.report_if_due(); - imu_.update_status(); - Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; + vt13_.update_status(); - tf_->set_transform( - gimbal_imu_pose.conjugate()); + if (auto snapshot = bmi088_.snapshot()) { + tf_->set_transform( + snapshot->orientation.conjugate()); - *gimbal_yaw_velocity_imu_ = imu_.gz(); - *gimbal_pitch_velocity_imu_ = imu_.gy(); + *gimbal_yaw_velocity_imu_ = snapshot->gyro_body.z(); + *gimbal_pitch_velocity_imu_ = snapshot->gyro_body.y(); + } gimbal_top_yaw_motor_.update_status(); gimbal_pitch_motor_.update_status(); @@ -331,159 +352,190 @@ class SteeringHeroLittle gimbal_scope_motor_.update_status(); - if (last_camera_capturer_trigger_timestamp_ != *camera_capturer_trigger_timestamp_) - *camera_capturer_trigger_ = true; - last_camera_capturer_trigger_timestamp_ = *camera_capturer_trigger_timestamp_; - *photoelectric_sensor_status_ = photoelectric_sensor_status_atomic.load(); *grayscale_sensor_status_ = grayscale_sensor_status_atomic.load(); + + if (++count_ == 250) { + for (int i = 0; i < 6; ++i) { + if (friciton_detect[i] == 0) { + RCLCPP_WARN(logger_, "friction can id 0x%03X missing", i + 0x201); + } + } + std::fill_n(friciton_detect, 6, 0); + for (int i = 0; i < 3; ++i) { + if (can0_detect[i] == 0) { + RCLCPP_WARN(logger_, "top board can id 0x%03X missing", i + 0x141); + } + } + std::fill_n(can0_detect, 3, 0); + count_ = 0; + } } void command_update() { - auto builder = start_transmit(); + auto builder = board_->start_transmit(); if (std::isfinite(gimbal_pitch_motor_.control_angle())) - builder.can0_transmit({ - .can_id = 0x142, - .can_data = gimbal_pitch_motor_ - .generate_angle_command(gimbal_pitch_motor_.control_angle()) - .as_bytes(), - }); + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x143, + .can_data = gimbal_pitch_motor_ + .generate_angle_command(gimbal_pitch_motor_.control_angle()) + .as_bytes(), + }); else - builder.can0_transmit({ - .can_id = 0x142, - .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), - }); // Used to distinguish pitch encoder control from IMU control. - - builder.can0_transmit({ - .can_id = 0x141, - .can_data = gimbal_top_yaw_motor_.generate_command().as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_friction_wheels_[0].generate_command(), - gimbal_friction_wheels_[1].generate_command(), - gimbal_friction_wheels_[2].generate_command(), - gimbal_friction_wheels_[3].generate_command(), - } - .as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x1FF, - .can_data = - device::CanPacket8{ - putter_motor_.generate_command(), - gimbal_scope_motor_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x143, - .can_data = - gimbal_player_viewer_motor_ - .generate_velocity_command(gimbal_player_viewer_motor_.control_velocity()) - .as_bytes(), - }); - - builder.can3_transmit({ - .can_id = 0x142, - .can_data = gimbal_bullet_feeder_.generate_torque_command().as_bytes(), - }); + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x143, + .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), + }); // Used to distinguish pitch encoder control from IMU control. + + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x141, + .can_data = gimbal_top_yaw_motor_.generate_command().as_bytes(), + }); - builder.gpio_digital_read( - librmcs::spec::rmcs_board_lite::kGpioDescriptors[2], + builder.can_transmit( + Spec::kCans.kCan0, // { - .period_ms = 20, - .pull = librmcs::data::GpioPull::kUp, + .can_id = 0x142, + .can_data = gimbal_bullet_feeder_.generate_torque_command().as_bytes(), }); - builder.gpio_digital_read( - librmcs::spec::rmcs_board_lite::kGpioDescriptors[3], + builder.can_transmit( + Spec::kCans.kCan1, // { - .period_ms = 20, - .pull = librmcs::data::GpioPull::kUp, + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_friction_wheels_[0].generate_command(), + gimbal_friction_wheels_[1].generate_command(), + gimbal_friction_wheels_[2].generate_command(), + gimbal_friction_wheels_[3].generate_command(), + } + .as_bytes(), }); - } - private: - void can0_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can0_receive_rate_counter_.record(can_id); - if (can_id == 0x141) { - gimbal_top_yaw_motor_.store_status(data.can_data); - } else if (can_id == 0x142) { - gimbal_pitch_motor_.store_status(data.can_data); - } - } + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_friction_wheels_[4].generate_command(), + gimbal_friction_wheels_[5].generate_command(), + putter_motor_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can1_receive_rate_counter_.record(can_id); - if (can_id == 0x201) { - gimbal_friction_wheels_[0].store_status(data.can_data); - } else if (can_id == 0x202) { - gimbal_friction_wheels_[1].store_status(data.can_data); - } else if (can_id == 0x203) { - gimbal_friction_wheels_[2].store_status(data.can_data); - } else if (can_id == 0x204) { - gimbal_friction_wheels_[3].store_status(data.can_data); - } else if (can_id == 0x205) { - putter_motor_.store_status(data.can_data); - } else if (can_id == 0x206) { - gimbal_scope_motor_.store_status(data.can_data); - } + builder + .gpio_digital_read( + Spec::kGpios.kUart1Rx, + { + .period_ms = 20, + .pull = librmcs::data::GpioPull::kUp, + }) + .gpio_digital_read( + Spec::kGpios.kUart1Tx, { + .period_ms = 0, + .asap = false, + .rising_edge = false, + .falling_edge = true, + .capture_timestamp = true, + .pull = librmcs::data::GpioPull::kUp, + }); } - void can2_receive_callback(const librmcs::data::CanDataView& data) override { + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; auto can_id = data.can_id; - // can2_receive_rate_counter_.record(can_id); - if (can_id == 0x143) { - gimbal_player_viewer_motor_.store_status(data.can_data); + if (can == Spec::kCans.kCan0) { + // can0_receive_rate_counter_.record(can_id); + can0_detect[can_id - 0x141] = 1; + if (can_id == 0x141) { + gimbal_top_yaw_motor_.store_status(data.can_data); + } else if (can_id == 0x143) { + gimbal_pitch_motor_.store_status(data.can_data); + } else if (can_id == 0x142) { + gimbal_bullet_feeder_.store_status(data.can_data); + } + } else if (can == Spec::kCans.kCan1) { + // can1_receive_rate_counter_.record(can_id); + friciton_detect[can_id - 0x201] = 1; + if (can_id == 0x201) { + gimbal_friction_wheels_[0].store_status(data.can_data); + } else if (can_id == 0x202) { + gimbal_friction_wheels_[1].store_status(data.can_data); + } else if (can_id == 0x203) { + gimbal_friction_wheels_[2].store_status(data.can_data); + } else if (can_id == 0x204) { + gimbal_friction_wheels_[3].store_status(data.can_data); + } + } else if (can == Spec::kCans.kCan2) { + // can2_receive_rate_counter_.record(can_id); + if (can_id == 0x203) { + putter_motor_.store_status(data.can_data); + } else if (can_id == 0x201) { + gimbal_friction_wheels_[4].store_status(data.can_data); + friciton_detect[4] = 1; + } else if (can_id == 0x202) { + gimbal_friction_wheels_[5].store_status(data.can_data); + friciton_detect[5] = 1; + } } } - void can3_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can3_receive_rate_counter_.record(can_id); - if (can_id == 0x142) { - gimbal_bullet_feeder_.store_status(data.can_data); + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + if (uart == Spec::kUarts.kUart0) { + vt13_.store_status(data.uart_data); } } void gpio_digital_read_result_callback( - const librmcs::spec::rmcs_board_lite::GpioDescriptor& gpio, - const librmcs::data::GpioDigitalDataView& data) override { - if (gpio.channel_index == 2) { + const Spec::Gpio& gpio, const View::GpioDigital& data) override { + + /* */ if (gpio == Spec::kGpios.kUart1Rx) { photoelectric_sensor_status_atomic.store(data.high); - } else if (gpio.channel_index == 3) { - grayscale_sensor_status_atomic.store(!data.high); + } else if (gpio == Spec::kGpios.kUart1Tx) { + if (!data.timestamp_quarter_us) + return; + const auto timestamp = + board_clock_lifter_.lift_timestamp(*data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + camera_signal_output_.emit(*timestamp); } } - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { + const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); + bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); } - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + + if (auto snapshot = + bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp)) + imu_snapshot_output_.emit(*snapshot); } + [[nodiscard]] auto gimbal_yaw_velocity() const -> double { + return *gimbal_yaw_velocity_imu_; + } + + [[nodiscard]] device::Vt13& vt13() noexcept { return vt13_; } + [[nodiscard]] const device::Vt13& vt13() const noexcept { return vt13_; } + rclcpp::Logger logger_; // CanReceiveRateCounter can0_receive_rate_counter_; // CanReceiveRateCounter can1_receive_rate_counter_; @@ -491,12 +543,16 @@ class SteeringHeroLittle // CanReceiveRateCounter can3_receive_rate_counter_; OutputInterface& tf_; - std::time_t last_camera_capturer_trigger_timestamp_{0}; + int count_ = 0; + int friciton_detect[6]; + int can0_detect[3]; - device::Bmi088 imu_; + device::Bmi088Ekf bmi088_; + device::BoardClockLifter board_clock_lifter_; + device::Vt13 vt13_; device::LkMotor gimbal_top_yaw_motor_; device::LkMotor gimbal_pitch_motor_; - device::DjiMotor gimbal_friction_wheels_[4]; + device::DjiMotor gimbal_friction_wheels_[6]; device::LkMotor gimbal_bullet_feeder_; device::DjiMotor putter_motor_; device::DjiMotor gimbal_scope_motor_; @@ -506,27 +562,25 @@ class SteeringHeroLittle OutputInterface gimbal_pitch_velocity_imu_; OutputInterface photoelectric_sensor_status_; OutputInterface grayscale_sensor_status_; - OutputInterface camera_capturer_trigger_; - OutputInterface camera_capturer_trigger_timestamp_; + EventOutputInterface camera_signal_output_; + EventOutputInterface imu_snapshot_output_; std::atomic photoelectric_sensor_status_atomic{false}; std::atomic grayscale_sensor_status_atomic{false}; + + std::unique_ptr board_; }; - class BottomBoard final : private librmcs::agent::RmcsBoardLite { - public: - friend class SteeringHeroLittle; + struct BottomBoard final : librmcs::board::RmcsBoardLite::Callback { explicit BottomBoard( SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, std::string_view board_serial = {}) - : librmcs::agent::RmcsBoardLite( - board_serial, {.dangerously_skip_version_checks = false}) - , logger_(steering_hero.get_logger()) + : logger_(steering_hero.get_logger()) // , can0_receive_rate_counter_(logger_, "bottom/can0") // , can1_receive_rate_counter_(logger_, "bottom/can1") // , can2_receive_rate_counter_(logger_, "bottom/can2") // , can3_receive_rate_counter_(logger_, "bottom/can3") , imu_(1000, 0.2, 0.0) - , dr16_(steering_hero) + , dr16_{} , supercap_(steering_hero, steering_hero_command) , chassis_steering_motors_( {steering_hero, steering_hero_command, "/chassis/left_front_steering"}, @@ -544,65 +598,70 @@ class SteeringHeroLittle , chassis_back_climber_motor_( {steering_hero, steering_hero_command, "/chassis/climber/left_back_motor"}, {steering_hero, steering_hero_command, "/chassis/climber/right_back_motor"}) + , yaw_brake_motor_(steering_hero, steering_hero_command, "/gimbal/yaw_brake") , gimbal_bottom_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/bottom_yaw") { // chassis_steering_motors_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020, 4} .set_encoder_zero_point( static_cast( steering_hero.get_parameter("left_front_zero_point").as_int())) .set_reversed()); chassis_steering_motors_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020, 1} .set_encoder_zero_point( static_cast( steering_hero.get_parameter("right_front_zero_point").as_int())) .set_reversed()); chassis_steering_motors_[2].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020, 3} .set_encoder_zero_point( static_cast( steering_hero.get_parameter("left_back_zero_point").as_int())) .set_reversed()); chassis_steering_motors_[3].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020, 2} .set_encoder_zero_point( static_cast( steering_hero.get_parameter("right_back_zero_point").as_int())) .set_reversed()); chassis_wheel_motors_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 4} .set_reversed() .set_reduction_ratio(2232. / 169.)); chassis_wheel_motors_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} .set_reversed() .set_reduction_ratio(2232. / 169.)); chassis_wheel_motors_[2].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 3} .set_reversed() .set_reduction_ratio(2232. / 169.)); chassis_wheel_motors_[3].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 4} .set_reversed() .set_reduction_ratio(2232. / 169.)); chassis_front_climber_motor_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} .set_reversed() .set_reduction_ratio(19.)); chassis_front_climber_motor_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2}.set_reduction_ratio( + 19.)); chassis_back_climber_motor_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2} .enable_multi_turn_angle() .set_reduction_ratio(19.)); chassis_back_climber_motor_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 4} .set_reversed() .enable_multi_turn_angle() .set_reduction_ratio(19.)); + yaw_brake_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM2006, 3}.set_reduction_ratio( + 1.)); gimbal_bottom_yaw_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} .set_reversed() @@ -617,8 +676,8 @@ class SteeringHeroLittle [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); }; referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart0_transmit( - {.uart_data = std::span{buffer, size}}); + board_->start_transmit().uart_transmit( + Spec::kUarts.kUart0, {.uart_data = std::span{buffer, size}}); return size; }; steering_hero.register_output( @@ -629,14 +688,11 @@ class SteeringHeroLittle steering_hero.register_output( "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); steering_hero.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); - } - BottomBoard(const BottomBoard&) = delete; - BottomBoard& operator=(const BottomBoard&) = delete; - BottomBoard(BottomBoard&&) = delete; - BottomBoard& operator=(BottomBoard&&) = delete; + board_ = std::make_unique(*this, board_serial); - ~BottomBoard() final = default; + steering_hero.remote_control_->register_dr16(&dr16_); + } void update() { // can0_receive_rate_counter_.report_if_due(); @@ -661,167 +717,186 @@ class SteeringHeroLittle for (auto& motor : chassis_steering_motors_) motor.update_status(); + yaw_brake_motor_.update_status(); gimbal_bottom_yaw_motor_.update_status(); + + if (++count_ == 250) { + for (int i = 0; i < 8; ++i) { + if (check[i] == 0) { + RCLCPP_WARN(logger_, "bottom board can id 0x%03X missing", i + 0x201); + } + } + std::fill_n(check, 8, 0); + count_ = 0; + } } void command_update() { - auto builder = start_transmit(); - - builder.can0_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), - chassis_wheel_motors_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); + auto builder = board_->start_transmit(); - builder.can0_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steering_motors_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - chassis_steering_motors_[0].generate_command(), - } - .as_bytes(), - }); + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[0].generate_command(), + chassis_wheel_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - chassis_wheel_motors_[2].generate_command(), - chassis_wheel_motors_[3].generate_command(), - } - .as_bytes(), - }); + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + chassis_steering_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + chassis_steering_motors_[0].generate_command(), + } + .as_bytes(), + }); - builder.can1_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - chassis_steering_motors_[3].generate_command(), - chassis_steering_motors_[2].generate_command(), - supercap_.generate_command(), - } - .as_bytes(), - }); + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + chassis_wheel_motors_[2].generate_command(), + chassis_wheel_motors_[3].generate_command(), + } + .as_bytes(), + }); - builder.can3_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - chassis_back_climber_motor_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - chassis_back_climber_motor_[0].generate_command(), - } - .as_bytes(), - }); + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + chassis_steering_motors_[3].generate_command(), + chassis_steering_motors_[2].generate_command(), + supercap_.generate_command(), + } + .as_bytes(), + }); - builder.can3_transmit({ - .can_id = 0x141, - .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), - }); + builder.can_transmit( + Spec::kCans.kCan3, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + chassis_back_climber_motor_[1].generate_command(), + yaw_brake_motor_.generate_command(), + chassis_back_climber_motor_[0].generate_command(), + } + .as_bytes(), + }); - builder.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_front_climber_motor_[0].generate_command(), - device::CanPacket8::PaddingQuarter{}, - chassis_front_climber_motor_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - } + builder.can_transmit( + Spec::kCans.kCan3, // + { + .can_id = 0x141, + .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), + }); - private: - void can0_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can0_receive_rate_counter_.record(can_id); - if (can_id == 0x201) { - chassis_wheel_motors_[0].store_status(data.can_data); - } else if (can_id == 0x202) { - chassis_wheel_motors_[1].store_status(data.can_data); - } else if (can_id == 0x205) { - chassis_steering_motors_[1].store_status(data.can_data); - } else if (can_id == 0x208) { - chassis_steering_motors_[0].store_status(data.can_data); - } + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_front_climber_motor_[0].generate_command(), + device::CanPacket8::PaddingQuarter{}, + chassis_front_climber_motor_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); } - void can1_receive_callback(const librmcs::data::CanDataView& data) override { + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; auto can_id = data.can_id; - // can1_receive_rate_counter_.record(can_id); - if (can_id == 0x203) { - chassis_wheel_motors_[2].store_status(data.can_data); - } else if (can_id == 0x204) { - chassis_wheel_motors_[3].store_status(data.can_data); - } else if (can_id == 0x207) { - chassis_steering_motors_[2].store_status(data.can_data); - } else if (can_id == 0x206) { - chassis_steering_motors_[3].store_status(data.can_data); - } else if (can_id == 0x300) { - supercap_.store_status(data.can_data); + if (can == Spec::kCans.kCan0) { + // can0_receive_rate_counter_.record(can_id); + check[can_id - 0x201] = 1; + if (can_id == 0x201) { + chassis_wheel_motors_[0].store_status(data.can_data); + } else if (can_id == 0x202) { + chassis_wheel_motors_[1].store_status(data.can_data); + } else if (can_id == 0x205) { + chassis_steering_motors_[1].store_status(data.can_data); + } else if (can_id == 0x208) { + chassis_steering_motors_[0].store_status(data.can_data); + } + } else if (can == Spec::kCans.kCan1) { + // can1_receive_rate_counter_.record(can_id); + if (can_id != 0x300) + check[can_id - 0x201] = 1; + if (can_id == 0x203) { + chassis_wheel_motors_[2].store_status(data.can_data); + } else if (can_id == 0x204) { + chassis_wheel_motors_[3].store_status(data.can_data); + } else if (can_id == 0x207) { + chassis_steering_motors_[2].store_status(data.can_data); + } else if (can_id == 0x206) { + chassis_steering_motors_[3].store_status(data.can_data); + } else if (can_id == 0x300) { + supercap_.store_status(data.can_data); + } + } else if (can == Spec::kCans.kCan2) { + // can2_receive_rate_counter_.record(can_id); + if (can_id == 0x201) { + chassis_front_climber_motor_[0].store_status(data.can_data); + } else if (can_id == 0x203) { + chassis_front_climber_motor_[1].store_status(data.can_data); + } + } else if (can == Spec::kCans.kCan3) { + // can3_receive_rate_counter_.record(can_id); + if (can_id == 0x202) { + chassis_back_climber_motor_[1].store_status(data.can_data); + } else if (can_id == 0x204) { + chassis_back_climber_motor_[0].store_status(data.can_data); + } else if (can_id == 0x203) { + yaw_brake_motor_.store_status(data.can_data); + } else if (can_id == 0x141) { + gimbal_bottom_yaw_motor_.store_status(data.can_data); + } } } - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - auto can_id = data.can_id; - // can2_receive_rate_counter_.record(can_id); - if (can_id == 0x201) { - chassis_front_climber_motor_[0].store_status(data.can_data); - } else if (can_id == 0x203) { - chassis_front_climber_motor_[1].store_status(data.can_data); + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + if (uart == Spec::kUarts.kUart0) { + const std::byte* ptr = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, + data.uart_data.size()); + } else if (uart == Spec::kUarts.kDbus) { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); } } - void can3_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can3_receive_rate_counter_.record(can_id); - if (can_id == 0x202) { - chassis_back_climber_motor_[1].store_status(data.can_data); - } else if (can_id == 0x204) { - chassis_back_climber_motor_[0].store_status(data.can_data); - } else if (can_id == 0x141) { - gimbal_bottom_yaw_motor_.store_status(data.can_data); - } - } + [[nodiscard]] device::Dr16& dr16() noexcept { return dr16_; } + [[nodiscard]] const device::Dr16& dr16() const noexcept { return dr16_; } - void uart0_receive_callback(const librmcs::data::UartDataView& data) override { - const std::byte* ptr = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, data.uart_data.size()); - } - - void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { imu_.store_accelerometer_status(data.x, data.y, data.z); } - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { imu_.store_gyroscope_status(data.x, data.y, data.z); } @@ -831,6 +906,9 @@ class SteeringHeroLittle // CanReceiveRateCounter can2_receive_rate_counter_; // CanReceiveRateCounter can3_receive_rate_counter_; + int count_ = 0; + int check[10] = {0}; + device::Bmi088 imu_; device::Dr16 dr16_; device::Supercap supercap_; @@ -839,6 +917,7 @@ class SteeringHeroLittle device::DjiMotor chassis_wheel_motors_[4]; device::DjiMotor chassis_front_climber_motor_[2]; device::DjiMotor chassis_back_climber_motor_[2]; + device::DjiMotor yaw_brake_motor_; device::LkMotor gimbal_bottom_yaw_motor_; rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; @@ -848,18 +927,24 @@ class SteeringHeroLittle OutputInterface powermeter_charge_power_limit_; OutputInterface chassis_yaw_velocity_imu_; OutputInterface chassis_pitch_imu_; + + std::unique_ptr board_; }; OutputInterface tf_; + OutputInterface camera_transform_; + OutputInterface barrel_direction_; + OutputInterface auto_aim_yaw_velocity_; + rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; std::shared_ptr top_board_; std::shared_ptr bottom_board_; + std::unique_ptr remote_control_; }; } // namespace rmcs_core::hardware #include - PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringHeroLittle, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp deleted file mode 100644 index b0b811f0e..000000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp +++ /dev/null @@ -1,908 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "hardware/device/bmi088.hpp" -#include "hardware/device/can_packet.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/lk_motor.hpp" -#include "hardware/device/supercap.hpp" - -namespace rmcs_core::hardware { - -class CanReceiveRateCounter { -public: - explicit CanReceiveRateCounter(rclcpp::Logger logger, std::string_view channel_name) - : logger_(std::move(logger)) - , channel_name_(channel_name) {} - - void record(std::uint32_t can_id) { - const auto now = Clock::now(); - - std::lock_guard lock{mutex_}; - auto& status = statuses_[can_id]; - ++status.receive_count; - status.last_receive_time = now; - - report_if_due(now); - } - - void report_if_due() { - const auto now = Clock::now(); - - std::lock_guard lock{mutex_}; - report_if_due(now); - } - -private: - using Clock = std::chrono::steady_clock; - - struct Status { - std::size_t receive_count{0}; - Clock::time_point last_receive_time{}; - }; - - void report_if_due(Clock::time_point now) { - if (statuses_.empty()) - return; - - if (last_report_time_ == Clock::time_point{}) { - last_report_time_ = now; - return; - } - - const auto elapsed = now - last_report_time_; - if (elapsed < kReportInterval) - return; - - const auto elapsed_seconds = std::chrono::duration(elapsed).count(); - for (auto& [can_id, status] : statuses_) { - const bool attached = now - status.last_receive_time <= kMissTimeout; - RCLCPP_INFO( - logger_, "[can rx] %.*s id=0x%03X rate=%.1fHz status=%s", - static_cast(channel_name_.size()), channel_name_.data(), - static_cast(can_id), - static_cast(status.receive_count) / elapsed_seconds, - attached ? "attach" : "miss"); - status.receive_count = 0; - } - - last_report_time_ = now; - } - - static constexpr std::chrono::milliseconds kReportInterval{1000}; - static constexpr std::chrono::milliseconds kMissTimeout{1000}; - - rclcpp::Logger logger_; - std::string_view channel_name_; - std::mutex mutex_; - Clock::time_point last_report_time_{}; - std::map statuses_; -}; - -class SteeringHero - : public rmcs_executor::Component - , public rclcpp::Node { -public: - SteeringHero() - : Node( - get_component_name(), - rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) - , command_component_( - create_partner_component( - get_component_name() + "_command", *this)) { - - register_output("/tf", tf_); - - gimbal_calibrate_subscription_ = create_subscription( - "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - gimbal_calibrate_subscription_callback(std::move(msg)); - }); - - top_board_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_top_board").as_string()); - - bottom_board_one_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_bottom_board_one").as_string()); - - bottom_board_two_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_bottom_board_two").as_string()); - - tf_->set_transform( - Eigen::Translation3d{0.06603, 0.0, 0.082}); - } - - SteeringHero(const SteeringHero&) = delete; - SteeringHero& operator=(const SteeringHero&) = delete; - SteeringHero(SteeringHero&&) = delete; - SteeringHero& operator=(SteeringHero&&) = delete; - - ~SteeringHero() override = default; - - void update() override { - top_board_->update(); - bottom_board_one_->update(); - bottom_board_two_->update(); - - tf_->set_state( - bottom_board_two_->gimbal_bottom_yaw_motor_.angle() - + top_board_->gimbal_top_yaw_motor_.angle()); - tf_->set_state( - top_board_->gimbal_pitch_motor_.angle()); - } - - void command_update() { - top_board_->command_update(); - bottom_board_one_->command_update(); - bottom_board_two_->command_update(); - } - -private: - void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New bottom yaw offset: %ld", - bottom_board_two_->gimbal_bottom_yaw_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New pitch offset: %ld", - top_board_->gimbal_pitch_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New top yaw offset: %ld", - top_board_->gimbal_top_yaw_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New bullet feeder offset: %ld", - top_board_->gimbal_bullet_feeder_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] left front steering offset: %d", - bottom_board_one_->chassis_steering_motors_[0].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] right front steering offset: %d", - bottom_board_one_->chassis_steering_motors_[1].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] left back steering offset: %d", - bottom_board_two_->chassis_steering_motors2_[0].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] right back steering offset: %d", - bottom_board_two_->chassis_steering_motors2_[1].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] left front wheel offset: %d", - bottom_board_one_->chassis_wheel_motors_[0].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] right front wheel offset: %d", - bottom_board_one_->chassis_wheel_motors_[1].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] left back wheel offset: %d", - bottom_board_two_->chassis_wheel_motors2_[0].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] right back wheel offset: %d", - bottom_board_two_->chassis_wheel_motors2_[1].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[booster calibration] left front friction wheel offset: %d", - top_board_->gimbal_friction_wheels_[0].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[booster calibration] right front friction wheel offset: %d", - top_board_->gimbal_friction_wheels_[1].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[booster calibration] left back friction wheel offset: %d", - top_board_->gimbal_friction_wheels_[2].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[booster calibration] right back friction wheel offset: %d", - top_board_->gimbal_friction_wheels_[3].calibrate_zero_point()); - } - - class SteeringHeroCommand : public rmcs_executor::Component { - public: - explicit SteeringHeroCommand(SteeringHero& hero) - : hero_(hero) {} - - void update() override { hero_.command_update(); } - - SteeringHero& hero_; - }; - std::shared_ptr command_component_; - - class TopBoard final : private librmcs::agent::CBoard { - public: - friend class SteeringHero; - explicit TopBoard( - SteeringHero& steering_hero, SteeringHeroCommand& steering_hero_command, - std::string_view board_serial = {}) - : librmcs::agent::CBoard(board_serial) - , logger_(steering_hero.get_logger()) - , tf_(steering_hero.tf_) - // , can1_receive_rate_counter_(logger_, "bottom/can1") - // , can2_receive_rate_counter_(logger_, "bottom/can2") - , imu_(1000, 0.2, 0.0) - , gimbal_top_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/top_yaw") - , gimbal_pitch_motor_(steering_hero, steering_hero_command, "/gimbal/pitch") - , gimbal_friction_wheels_( - {steering_hero, steering_hero_command, "/gimbal/first_left_friction"}, - {steering_hero, steering_hero_command, "/gimbal/first_right_friction"}, - {steering_hero, steering_hero_command, "/gimbal/second_left_friction"}, - {steering_hero, steering_hero_command, "/gimbal/second_right_friction"}) - , gimbal_bullet_feeder_(steering_hero, steering_hero_command, "/gimbal/bullet_feeder") - , putter_motor_(steering_hero, steering_hero_command, "/gimbal/putter") - , gimbal_scope_motor_(steering_hero, steering_hero_command, "/gimbal/scope") - , gimbal_player_viewer_motor_( - steering_hero, steering_hero_command, "/gimbal/player_viewer") { - - gimbal_top_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10}.set_encoder_zero_point( - static_cast( - steering_hero.get_parameter("top_yaw_motor_zero_point").as_int()))); - gimbal_pitch_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} - .set_encoder_zero_point( - static_cast( - steering_hero.get_parameter("pitch_motor_zero_point").as_int())) - .enable_multi_turn_angle()); - gimbal_friction_wheels_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); - gimbal_friction_wheels_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(1.)); - gimbal_friction_wheels_[2].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); - gimbal_friction_wheels_[3].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(1.)); - gimbal_bullet_feeder_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} - .set_encoder_zero_point( - static_cast( - steering_hero.get_parameter("bullet_feeder_motor_zero_point").as_int())) - .set_reversed() - .enable_multi_turn_angle()); - putter_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(1.) - .enable_multi_turn_angle()); - gimbal_scope_motor_.configure(device::DjiMotor::Config{device::DjiMotor::Type::kM2006}); - gimbal_player_viewer_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} - .set_encoder_zero_point( - static_cast( - steering_hero.get_parameter("viewer_motor_zero_point").as_int())) - .set_reversed() - .enable_multi_turn_angle()); - - steering_hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); - steering_hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); - - steering_hero.register_output( - "/gimbal/photoelectric_sensor", photoelectric_sensor_status_, false); - steering_hero.register_output( - "/gimbal/grayscale_sensor", grayscale_sensor_status_, false); - steering_hero.register_output( - "/auto_aim/image_capturer/timestamp", camera_capturer_trigger_timestamp_, 0); - steering_hero.register_output( - "/auto_aim/image_capturer/trigger", camera_capturer_trigger_, 0); - - imu_.set_coordinate_mapping([](double x, double y, double z) { - // Get the mapping with the following code. - // The rotation angle must be an exact multiple of 90 degrees, otherwise - // use a matrix. - - return std::make_tuple(x, y, z); - }); - } - - TopBoard(const TopBoard&) = delete; - TopBoard& operator=(const TopBoard&) = delete; - TopBoard(TopBoard&&) = delete; - TopBoard& operator=(TopBoard&&) = delete; - - ~TopBoard() final = default; - - void update() { - imu_.update_status(); - Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; - - tf_->set_transform( - gimbal_imu_pose.conjugate()); - - *gimbal_yaw_velocity_imu_ = imu_.gz(); - *gimbal_pitch_velocity_imu_ = imu_.gy(); - - gimbal_top_yaw_motor_.update_status(); - gimbal_pitch_motor_.update_status(); - tf_->set_state( - gimbal_pitch_motor_.angle()); - - for (auto& motor : gimbal_friction_wheels_) - motor.update_status(); - - gimbal_bullet_feeder_.update_status(); - putter_motor_.update_status(); - - gimbal_player_viewer_motor_.update_status(); - tf_->set_state( - gimbal_player_viewer_motor_.angle()); - - gimbal_scope_motor_.update_status(); - - if (last_camera_capturer_trigger_timestamp_ != *camera_capturer_trigger_timestamp_) - *camera_capturer_trigger_ = true; - last_camera_capturer_trigger_timestamp_ = *camera_capturer_trigger_timestamp_; - - *photoelectric_sensor_status_ = photoelectric_sensor_status_atomic.load(); - *grayscale_sensor_status_ = grayscale_sensor_status_atomic.load(); - } - - void command_update() { - auto builder = start_transmit(); - - if (control_flag == 0) { - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_friction_wheels_[3].generate_command(), - gimbal_friction_wheels_[1].generate_command(), - gimbal_friction_wheels_[2].generate_command(), - gimbal_friction_wheels_[0].generate_command(), - } - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x143, - .can_data = gimbal_player_viewer_motor_ - .generate_velocity_command( - gimbal_player_viewer_motor_.control_velocity()) - .as_bytes(), - }); - - if (std::isfinite(gimbal_pitch_motor_.control_angle())) - builder.can2_transmit({ - .can_id = 0x142, - .can_data = gimbal_pitch_motor_ - .generate_angle_command(gimbal_pitch_motor_.control_angle()) - .as_bytes(), - }); - else - builder.can2_transmit({ - .can_id = 0x142, - .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), - }); // Used to distinguish pitch encoder control from IMU control. - - } else { - builder.can1_transmit({ - .can_id = 0x1FF, - .can_data = - device::CanPacket8{ - putter_motor_.generate_command(), - gimbal_scope_motor_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x141, - .can_data = gimbal_bullet_feeder_.generate_torque_command().as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x141, - .can_data = gimbal_top_yaw_motor_.generate_command().as_bytes(), - }); - } - - builder.gpio_digital_read( - librmcs::spec::c_board::kGpioDescriptors[6], - { - .period_ms = 20, - .pull = librmcs::data::GpioPull::kUp, - }); - - builder.gpio_digital_read( - librmcs::spec::c_board::kGpioDescriptors[4], - { - .period_ms = 20, - .pull = librmcs::data::GpioPull::kUp, - }); - - if (control_flag == 0) { - control_flag = 1; - } else { - control_flag = 0; - } - } - - private: - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can1_receive_rate_counter_.record(can_id); - if (can_id == 0x204) { - - gimbal_friction_wheels_[0].store_status(data.can_data); - } else if (can_id == 0x202) { - gimbal_friction_wheels_[1].store_status(data.can_data); - } else if (can_id == 0x203) { - gimbal_friction_wheels_[2].store_status(data.can_data); - } else if (can_id == 0x201) { - gimbal_friction_wheels_[3].store_status(data.can_data); - } else if (can_id == 0x205) { - putter_motor_.store_status(data.can_data); - } else if (can_id == 0x141) { - gimbal_bullet_feeder_.store_status(data.can_data); - } else if (can_id == 0x206) { - gimbal_scope_motor_.store_status(data.can_data); - } - } - - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can2_receive_rate_counter_.record(can_id); - if (can_id == 0x141) { - gimbal_top_yaw_motor_.store_status(data.can_data); - } else if (can_id == 0x142) { - gimbal_pitch_motor_.store_status(data.can_data); - } else if (can_id == 0x143) { - gimbal_player_viewer_motor_.store_status(data.can_data); - } - } - - void gpio_digital_read_result_callback( - const librmcs::spec::c_board::GpioDescriptor& gpio, - const librmcs::data::GpioDigitalDataView& data) override { - if (gpio.channel_index == 6) { - photoelectric_sensor_status_atomic.store(!data.high); - } else if (gpio.channel_index == 4) { - grayscale_sensor_status_atomic.store(!data.high); - } - } - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - - int control_flag = 0; - rclcpp::Logger logger_; - OutputInterface& tf_; - // CanReceiveRateCounter can1_receive_rate_counter_; - // CanReceiveRateCounter can2_receive_rate_counter_; - - std::time_t last_camera_capturer_trigger_timestamp_{0}; - - device::Bmi088 imu_; - device::LkMotor gimbal_top_yaw_motor_; - device::LkMotor gimbal_pitch_motor_; - device::DjiMotor gimbal_friction_wheels_[4]; - device::LkMotor gimbal_bullet_feeder_; - device::DjiMotor putter_motor_; - device::DjiMotor gimbal_scope_motor_; - device::LkMotor gimbal_player_viewer_motor_; - - OutputInterface gimbal_yaw_velocity_imu_; - OutputInterface gimbal_pitch_velocity_imu_; - OutputInterface photoelectric_sensor_status_; - OutputInterface grayscale_sensor_status_; - OutputInterface camera_capturer_trigger_; - OutputInterface camera_capturer_trigger_timestamp_; - std::atomic photoelectric_sensor_status_atomic{false}; - std::atomic grayscale_sensor_status_atomic{false}; - }; - - class BottomBoard_one final : private librmcs::agent::CBoard { - public: - friend class SteeringHero; - explicit BottomBoard_one( - SteeringHero& steering_hero, SteeringHeroCommand& steering_hero_command, - std::string_view board_serial = {}) - : librmcs::agent::CBoard(board_serial) - , logger_(steering_hero.get_logger()) - // , can1_receive_rate_counter_(logger_, "bottom/can1") - // , can2_receive_rate_counter_(logger_, "bottom/can2") - , imu_(1000, 0.2, 0.0) - , chassis_front_climber_motor_( - {steering_hero, steering_hero_command, "/chassis/climber/left_front_motor"}, - {steering_hero, steering_hero_command, "/chassis/climber/right_front_motor"}) - , chassis_back_climber_motor_( - {steering_hero, steering_hero_command, "/chassis/climber/left_back_motor"}, - {steering_hero, steering_hero_command, "/chassis/climber/right_back_motor"}) - , chassis_steering_motors_( - {steering_hero, steering_hero_command, "/chassis/left_front_steering"}, - {steering_hero, steering_hero_command, "/chassis/right_front_steering"}) - , chassis_wheel_motors_( - {steering_hero, steering_hero_command, "/chassis/left_front_wheel"}, - {steering_hero, steering_hero_command, "/chassis/right_front_wheel"}) { - // - - chassis_steering_motors_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast( - steering_hero.get_parameter("left_front_zero_point").as_int())) - .set_reversed()); - chassis_steering_motors_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast( - steering_hero.get_parameter("right_front_zero_point").as_int())) - .set_reversed()); - chassis_wheel_motors_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)); - chassis_wheel_motors_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)); - - chassis_front_climber_motor_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(19.)); - chassis_front_climber_motor_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - chassis_back_climber_motor_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .enable_multi_turn_angle() - .set_reduction_ratio(19.)); - chassis_back_climber_motor_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .enable_multi_turn_angle() - .set_reduction_ratio(19.)); - - steering_hero.register_output( - "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - steering_hero.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); - } - BottomBoard_one(const BottomBoard_one&) = delete; - BottomBoard_one& operator=(const BottomBoard_one&) = delete; - BottomBoard_one(BottomBoard_one&&) = delete; - BottomBoard_one& operator=(BottomBoard_one&&) = delete; - - ~BottomBoard_one() final = default; - - void update() { - imu_.update_status(); - // can1_receive_rate_counter_.report_if_due(); - // can2_receive_rate_counter_.report_if_due(); - - *chassis_yaw_velocity_imu_ = imu_.gz(); - *chassis_pitch_imu_ = -std::asin(2.0 * (imu_.q0() * imu_.q2() - imu_.q3() * imu_.q1())); - - chassis_front_climber_motor_[0].update_status(); - chassis_front_climber_motor_[1].update_status(); - chassis_back_climber_motor_[0].update_status(); - chassis_back_climber_motor_[1].update_status(); - - for (auto& motor : chassis_wheel_motors_) - motor.update_status(); - for (auto& motor : chassis_steering_motors_) - motor.update_status(); - } - - void command_update() { - auto builder = start_transmit(); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[1].generate_command(), - chassis_wheel_motors_[0].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - chassis_steering_motors_[0].generate_command(), - device::CanPacket8::PaddingQuarter{}, - chassis_steering_motors_[1].generate_command(), - } - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_back_climber_motor_[0].generate_command(), - chassis_back_climber_motor_[1].generate_command(), - chassis_front_climber_motor_[0].generate_command(), - chassis_front_climber_motor_[1].generate_command(), - } - .as_bytes(), - }); - } - - private: - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can1_receive_rate_counter_.record(can_id); - if (can_id == 0x201) { - chassis_wheel_motors_[1].store_status(data.can_data); - } else if (can_id == 0x202) { - chassis_wheel_motors_[0].store_status(data.can_data); - } else if (can_id == 0x206) { - chassis_steering_motors_[0].store_status(data.can_data); - } else if (can_id == 0x208) { - chassis_steering_motors_[1].store_status(data.can_data); - } - } - - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can2_receive_rate_counter_.record(can_id); - if (can_id == 0x203) { - chassis_front_climber_motor_[0].store_status(data.can_data); - } else if (can_id == 0x204) { - chassis_front_climber_motor_[1].store_status(data.can_data); - } else if (can_id == 0x201) { - chassis_back_climber_motor_[0].store_status(data.can_data); - } else if (can_id == 0x202) { - chassis_back_climber_motor_[1].store_status(data.can_data); - } - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - - rclcpp::Logger logger_; - // CanReceiveRateCounter can1_receive_rate_counter_; - // CanReceiveRateCounter can2_receive_rate_counter_; - - device::Bmi088 imu_; - device::DjiMotor chassis_front_climber_motor_[2]; - device::DjiMotor chassis_back_climber_motor_[2]; - device::DjiMotor chassis_steering_motors_[2]; - device::DjiMotor chassis_wheel_motors_[2]; - - OutputInterface chassis_yaw_velocity_imu_; - OutputInterface chassis_pitch_imu_; - OutputInterface gimbal_yaw_velocity_imu_; - OutputInterface gimbal_pitch_velocity_imu_; - }; - - class BottomBoard_two final : private librmcs::agent::CBoard { - public: - friend class SteeringHero; - explicit BottomBoard_two( - SteeringHero& steering_hero, SteeringHeroCommand& steering_hero_command, - std::string_view board_serial = {}) - : librmcs::agent::CBoard(board_serial) - , logger_(steering_hero.get_logger()) - // , can1_receive_rate_counter_(logger_, "bottom/can1") - // , can2_receive_rate_counter_(logger_, "bottom/can2") - , imu_(1000, 0.2, 0.0) - , dr16_(steering_hero) - , supercap_(steering_hero, steering_hero_command) - , chassis_steering_motors2_( - {steering_hero, steering_hero_command, "/chassis/left_back_steering"}, - {steering_hero, steering_hero_command, "/chassis/right_back_steering"}) - , chassis_wheel_motors2_( - {steering_hero, steering_hero_command, "/chassis/left_back_wheel"}, - {steering_hero, steering_hero_command, "/chassis/right_back_wheel"}) - , gimbal_bottom_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/bottom_yaw") { - chassis_steering_motors2_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast( - steering_hero.get_parameter("left_back_zero_point").as_int())) - .set_reversed()); - chassis_steering_motors2_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast( - steering_hero.get_parameter("right_back_zero_point").as_int())) - .set_reversed()); - chassis_wheel_motors2_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)); - chassis_wheel_motors2_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)); - gimbal_bottom_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} - .set_reversed() - .set_encoder_zero_point( - static_cast( - steering_hero.get_parameter("bottom_yaw_motor_zero_point").as_int()))); - steering_hero.register_output("/referee/serial", referee_serial_); - referee_serial_->read = [this](std::byte* buffer, size_t size) { - return referee_ring_buffer_receive_.pop_front_n( - [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); - }; - referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart1_transmit( - {.uart_data = std::span{buffer, size}}); - return size; - }; - steering_hero.register_output( - "/chassis/powermeter/control_enable", powermeter_control_enabled_, false); - steering_hero.register_output( - "/chassis/powermeter/charge_power_limit", powermeter_charge_power_limit_, 0.); - } - - BottomBoard_two(const BottomBoard_two&) = delete; - BottomBoard_two& operator=(const BottomBoard_two&) = delete; - BottomBoard_two(BottomBoard_two&&) = delete; - BottomBoard_two& operator=(BottomBoard_two&&) = delete; - - ~BottomBoard_two() final = default; - - void update() { - imu_.update_status(); - dr16_.update_status(); - supercap_.update_status(); - // can1_receive_rate_counter_.report_if_due(); - // can2_receive_rate_counter_.report_if_due(); - - for (auto& motor : chassis_wheel_motors2_) - motor.update_status(); - for (auto& motor : chassis_steering_motors2_) - motor.update_status(); - - gimbal_bottom_yaw_motor_.update_status(); - } - - void command_update() { - auto builder = start_transmit(); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - chassis_wheel_motors2_[1].generate_command(), - chassis_wheel_motors2_[0].generate_command(), - } - .as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steering_motors2_[0].generate_command(), - device::CanPacket8::PaddingQuarter{}, - chassis_steering_motors2_[1].generate_command(), - supercap_.generate_command(), - } - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x141, - .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), - }); - } - - private: - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can1_receive_rate_counter_.record(can_id); - if (can_id == 0x203) { - chassis_wheel_motors2_[1].store_status(data.can_data); - - } else if (can_id == 0x204) { - chassis_wheel_motors2_[0].store_status(data.can_data); - } else if (can_id == 0x205) { - chassis_steering_motors2_[0].store_status(data.can_data); - } else if (can_id == 0x207) { - chassis_steering_motors2_[1].store_status(data.can_data); - } else if (can_id == 0x300) { - supercap_.store_status(data.can_data); - } - } - - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can2_receive_rate_counter_.record(can_id); - if (can_id == 0x141) { - gimbal_bottom_yaw_motor_.store_status(data.can_data); - } - } - - rclcpp::Logger logger_; - - void uart1_receive_callback(const librmcs::data::UartDataView& data) override { - const auto* uart_data = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&uart_data](std::byte* storage) noexcept { *storage = *uart_data++; }, - data.uart_data.size()); - } - - void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - - // CanReceiveRateCounter can1_receive_rate_counter_; - // CanReceiveRateCounter can2_receive_rate_counter_; - device::Bmi088 imu_; - - OutputInterface powermeter_control_enabled_; - OutputInterface powermeter_charge_power_limit_; - - device::Dr16 dr16_; - device::Supercap supercap_; - - device::DjiMotor chassis_steering_motors2_[2]; - device::DjiMotor chassis_wheel_motors2_[2]; - device::LkMotor gimbal_bottom_yaw_motor_; - - rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; - OutputInterface referee_serial_; - }; - - OutputInterface tf_; - - rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; - - std::shared_ptr top_board_; - std::shared_ptr bottom_board_one_; - std::shared_ptr bottom_board_two_; -}; - -} // namespace rmcs_core::hardware - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringHero, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp index 0bac8649f..6fe24a0c1 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp @@ -42,23 +42,20 @@ class Hero , bottom_yaw_angle_number_( Shape::Color::YELLOW, 20, 5, x_center + 270, y_center - 65, 0.0, false) , time_reminder_(Shape::Color::PINK, 50, 5, x_center + 150, y_center + 65, 0, false) - // , bullet_allowance_label_( - // Shape::Color::YELLOW, 18, 3, x_center - 300, y_center + 270, "bullet", false) - // , bullet_allowance_number_( - // Shape::Color::YELLOW, 20, 5, x_center - 170, y_center + 270, 0, false) , bullet_allowance_number_( Shape::Color::YELLOW, 20, 5, x_center - 220, y_center + 270, 0, false) , friction_profile_number_( Shape::Color::GREEN, friction_profile_number_font_size, 5, 0, 0, 12, false) , friction_profile_indicator_{Line(Shape::Color::WHITE, friction_profile_box_line_width, 0, 0, 0, 0, false), Line(Shape::Color::WHITE, friction_profile_box_line_width, 0, 0, 0, 0, false), Line(Shape::Color::WHITE, friction_profile_box_line_width, 0, 0, 0, 0, false), Line(Shape::Color::WHITE, friction_profile_box_line_width, 0, 0, 0, 0, true)} - , center_green_line_( - Shape::Color::GREEN, green_line_width, x_center - green_line_half_length, - y_center - green_line_offset_y, x_center + green_line_half_length, - y_center - green_line_offset_y, true) - , tracking_pink_line_( - Shape::Color::PINK, pink_line_width, x_center - pink_line_half_length, - y_center + pink_line_offset_y, x_center + pink_line_half_length, - y_center + pink_line_offset_y, false) { + // , center_green_line_( + // Shape::Color::GREEN, green_line_width, x_center - green_line_half_length, + // y_center - green_line_offset_y, x_center + green_line_half_length, + // y_center - green_line_offset_y, true) + // , tracking_pink_line_( + // Shape::Color::PINK, pink_line_width, x_center - pink_line_half_length, + // y_center + pink_line_offset_y, x_center + pink_line_half_length, + // y_center + pink_line_offset_y, false) + { chassis_control_direction_indicator_.set_x(x_center); chassis_control_direction_indicator_.set_y(y_center); @@ -83,9 +80,9 @@ class Hero register_input("/gimbal/control_bullet_allowance/limited_by_heat", robot_bullet_allowance_); register_input( - "/gimbal/first_left_friction/control_velocity", left_friction_control_velocity_); - register_input("/gimbal/first_left_friction/velocity", left_friction_velocity_); - register_input("/gimbal/first_right_friction/velocity", right_friction_velocity_); + "/gimbal/first_back_friction/control_velocity", back_friction_control_velocity_); + register_input("/gimbal/first_back_friction/velocity", back_friction_velocity_); + register_input("/gimbal/first_front_friction/velocity", front_friction_velocity_); register_input("/gimbal/friction_profile_1_active", friction_profile_1_active_, false); // register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); @@ -96,25 +93,18 @@ class Hero register_input("/gimbal/bottom_yaw/raw_angle", bottom_yaw_raw_angle_); // register_input("/gimbal/auto_aim/laser_distance", laser_distance_); - register_input("/gimbal/shooter/condiction", shoot_condiction_); - - register_input("/gimbal/shooter/mode", shoot_mode_); + register_input("/gimbal/shooter/preloaded_ready", shooter_preloaded_ready_, false); // register_input("/gimbal/scope/active", is_scope_active_); register_input("/remote/mouse", mouse_); register_input("/referee/game/stage", game_stage_); - - // register_input("/gimbal/auto_aim/fire_control", auto_aim_fire_control_, false); - // register_input("/gimbal/auto_aim/target_confidence", auto_aim_target_confidence_, false); } void update() override { update_normal_ui(); - // update_bullet_allowance(); // update_sniper_ui(); - // update_state_word(); // if (*is_scope_active_) { // set_normal_ui_visible(false); @@ -144,11 +134,10 @@ class Hero yaw_angle_number_.set_visible(value); pitch_angle_number_.set_visible(value); bottom_yaw_angle_number_.set_visible(value); - // bullet_allowance_label_.set_visible(value); bullet_allowance_number_.set_visible(value); friction_profile_number_.set_visible(value); - center_green_line_.set_visible(value); - tracking_pink_line_.set_visible(value); + // center_green_line_.set_visible(value); + // tracking_pink_line_.set_visible(value); const bool show_friction_profile_box = value && friction_profile_1_active_.ready() && *friction_profile_1_active_; @@ -164,10 +153,14 @@ class Hero pitch_angle_number_.set_value(static_cast(*gimbal_pitch_raw_angle_)); update_pitch_raw_angle_color(); bottom_yaw_angle_number_.set_value(static_cast(*bottom_yaw_raw_angle_)); - update_bottom_yaw_tracking_lines(); + // update_bottom_yaw_tracking_lines(); const int32_t bullet_allowance = static_cast(std::max(0, *robot_bullet_allowance_)); bullet_allowance_number_.set_value(bullet_allowance); + const bool shooter_preloaded_ready = + shooter_preloaded_ready_.ready() && *shooter_preloaded_ready_; + bullet_allowance_number_.set_color( + shooter_preloaded_ready ? Shape::Color::GREEN : Shape::Color::PINK); const uint16_t yaw_right = yaw_raw_angle_x @@ -217,16 +210,10 @@ class Hero friction_profile_indicator_[3].set_x2(box_left); friction_profile_indicator_[3].set_y2(box_bottom); status_ring_.update_friction_wheel_speed( - std::min(*left_friction_velocity_, *right_friction_velocity_), - *left_friction_control_velocity_ > 0); + std::min(*back_friction_velocity_, *front_friction_velocity_), + *back_friction_control_velocity_ > 0); status_ring_.update_supercap(*supercap_voltage_, true); status_ring_.update_battery_power(*chassis_voltage_); - // const bool auto_aim_locked = auto_aim_fire_control_.ready() && *auto_aim_fire_control_; - // const double target_confidence_value = - // auto_aim_target_confidence_.ready() ? *auto_aim_target_confidence_ : 0.0; - - // status_ring_.update_auto_aim_feedback(auto_aim_locked, target_confidence_value); - // update_static_status_ring(); last_keyboard_ = *keyboard_; } @@ -317,50 +304,6 @@ class Hero return; } - void update_static_status_ring() { - auto auto_aim_enable = mouse_->right == 1; - auto precise_enable = *shoot_mode_ == rmcs_msgs::ShootMode::PRECISE; - - status_ring_.update_static_parts({auto_aim_enable, precise_enable}); - } - - // void update_bullet_allowance() { - - // std::string text = "BULLET : " + std::to_string(max(0,*robot_bullet_allowance_)); - // char* allow = text.data(); - // auto color = Shape::Color::YELLOW; - - // bullet_allowance_number_.set_value(allow); - // bullet_allowance_number_.set_font_size(14); - // bullet_allowance_number_.set_color(color); - // bullet_allowance_number_.set_visible(true); - // bullet_allowance_number_.set_xy(x_center - 240, y_center + 288); - // } - - void update_state_word() { - - const char* text = "OK"; - auto color = Shape::Color::GREEN; - - if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::FRICTION_WAITING) { - text = " WAITING "; - } else if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::SHOOT) { - text = " SHOOT "; - } else if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::FIRED) { - text = " FIRED "; - } else if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::JAM) { - text = " JAM "; - } else if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::PRELOADING) { - text = "PRELOADING"; - } - - state_word_.set_value(text); - state_word_.set_font_size(30); - state_word_.set_color(color); - state_word_.set_visible(true); - state_word_.set_xy(x_center - 800, y_center + 200); - } - void update_chassis_direction_indicator() { auto chassis_mode = *chassis_mode_; @@ -375,7 +318,7 @@ class Hero return static_cast(degrees); }; // chassis_direction_indicator_.set_color( - // chassis_mode == rmcs_msgs::ChassisMode::SPIN ? Shape::Color::GREEN + // chassis_mode == rmcs_msgs::ChassisMode::SPIN_FAST ? Shape::Color::GREEN // : Shape::Color::PINK); // chassis_direction_indicator_.set_angle(to_referee_angle(*chassis_angle_), 30); const bool left_track_active = @@ -475,9 +418,9 @@ class Hero InputInterface robot_bullet_allowance_; - InputInterface left_friction_control_velocity_; - InputInterface left_friction_velocity_; - InputInterface right_friction_velocity_; + InputInterface back_friction_control_velocity_; + InputInterface back_friction_velocity_; + InputInterface front_friction_velocity_; InputInterface friction_profile_1_active_; InputInterface mouse_; @@ -493,8 +436,7 @@ class Hero InputInterface bottom_yaw_angle_; // InputInterface laser_distance_; - InputInterface shoot_mode_; - InputInterface shoot_condiction_; + InputInterface shooter_preloaded_ready_; // InputInterface is_scope_active_; StatusRing status_ring_; @@ -511,7 +453,6 @@ class Hero Text state_word_; Integer time_reminder_; - // Text bullet_allowance_label_; Integer bullet_allowance_number_; Integer friction_profile_number_; Line friction_profile_indicator_[4]; @@ -520,9 +461,6 @@ class Hero bool bottom_yaw_tracking_enabled_ = false; double bottom_yaw_anchor_angle_rad_ = 0.0; - - // InputInterface auto_aim_fire_control_; - // InputInterface auto_aim_target_confidence_; }; } // namespace rmcs_core::referee::app::ui From 80cb37c647c8d0a1362d696594d67c7312b9c0f1 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Mon, 10 Aug 2026 18:07:09 +0800 Subject: [PATCH 09/86] feat(infantry): Update omni infantry and drop steering infantry --- .../config/steering-infantry.yaml | 138 ----- .../rmcs_core/src/hardware/omni_infantry.cpp | 237 ++++---- .../src/hardware/steering-infantry.cpp | 539 ------------------ .../rmcs_core/src/referee/app/ui/infantry.cpp | 6 +- 4 files changed, 124 insertions(+), 796 deletions(-) delete mode 100644 rmcs_ws/src/rmcs_bringup/config/steering-infantry.yaml delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/steering-infantry.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-infantry.yaml deleted file mode 100644 index 2e344d140..000000000 --- a/rmcs_ws/src/rmcs_bringup/config/steering-infantry.yaml +++ /dev/null @@ -1,138 +0,0 @@ -rmcs_executor: - ros__parameters: - update_rate: 1000.0 - components: - - rmcs_core::hardware::SteeringInfantry -> infantry_hardware - - - rmcs_core::referee::Status -> referee_status - - rmcs_core::referee::command::Interaction -> referee_interaction - - rmcs_core::referee::command::interaction::Ui -> referee_ui - - rmcs_core::referee::app::ui::Infantry -> referee_ui_infantry - - rmcs_core::referee::Command -> referee_command - - - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller - - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller - - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller - - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller - - - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller - - rmcs_core::controller::shooting::HeatController -> heat_controller - - rmcs_core::controller::shooting::BulletFeederController17mm -> bullet_feeder_controller - - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller - - - rmcs_core::controller::chassis::ChassisController -> chassis_controller - - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller - -infantry_hardware: - ros__parameters: - board_serial_top_board: "AF-B4E5-CE0E-4342-FF2C-F9E2-DE47-2D85-9B75" - board_serial_bottom_board: "AF-EEF5-24BA-6675-F1B1-C797-50AF-869C-870E" - yaw_motor_zero_point: 20993 - pitch_motor_zero_point: 18874 - left_front_zero_point: 1695 - right_front_zero_point: 5068 - left_back_zero_point: 7870 - right_back_zero_point: 3141 - - -gimbal_controller: - ros__parameters: - upper_limit: -0.54 - lower_limit: 0.274 - -pitch_angle_pid_controller: - ros__parameters: - measurement: /gimbal/pitch/control_angle_error - control: /gimbal/pitch/control_velocity - kp: 15.0 - ki: 0.0 - kd: 1.5 - -yaw_angle_pid_controller: - ros__parameters: - measurement: /gimbal/yaw/control_angle_error - control: /gimbal/yaw/control_velocity - kp: 20.0 - ki: 0.0 - kd: 0.9 - -yaw_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/yaw/velocity_imu - setpoint: /gimbal/yaw/control_velocity - control: /gimbal/yaw/control_torque - kp: 2.5 - ki: 0.00 - kd: 0.6 - -friction_wheel_controller: - ros__parameters: - friction_wheels: - - /gimbal/left_friction - - /gimbal/right_friction - friction_velocities: - - 590.0 - - 590.0 - friction_soft_start_stop_time: 0.3 - -heat_controller: - ros__parameters: - heat_per_shot: 10000 - reserved_heat: 0 - -bullet_feeder_controller: - ros__parameters: - bullets_per_feeder_turn: 9.0 - shot_frequency: 27.0 - safe_shot_frequency: 10.0 - eject_frequency: 15.0 - eject_time: 0.15 - deep_eject_frequency: 15.0 - deep_eject_time: 0.20 - single_shot_max_stop_delay: 2.0 - -shooting_recorder: - ros__parameters: - friction_wheel_count: 2 - log_mode: 1 - -left_friction_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/left_friction/velocity - setpoint: /gimbal/left_friction/control_velocity - control: /gimbal/left_friction/control_torque - kp: 0.003436926 - ki: 0.00 - kd: 0.009373434 - -right_friction_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/right_friction/velocity - setpoint: /gimbal/right_friction/control_velocity - control: /gimbal/right_friction/control_torque - kp: 0.003436926 - ki: 0.00 - kd: 0.009373434 - -bullet_feeder_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/bullet_feeder/velocity - setpoint: /gimbal/bullet_feeder/control_velocity - control: /gimbal/bullet_feeder/control_torque - kp: 0.7 - ki: 0.0 - kd: 0.0 - -steering_wheel_controller: - ros__parameters: - mess: 22.0 - moment_of_inertia: 1.08 - vehicle_radius: 0.28284271247462 - wheel_radius: 0.055 - friction_coefficient: 0.6 - k1: 2.958580e+00 - k2: 3.082190e-03 - no_load_power: 11.37 diff --git a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp index 84b720776..b8c5c4af9 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp @@ -5,9 +5,8 @@ #include #include -#include +#include #include -#include #include #include #include @@ -27,6 +26,7 @@ #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" +#include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" namespace rmcs_core::hardware { @@ -34,13 +34,12 @@ namespace rmcs_core::hardware { class OmniInfantry : public rmcs_executor::Component , public rclcpp::Node - , private librmcs::agent::RmcsBoardLite { + , public librmcs::board::RmcsBoardLite::Callback { public: OmniInfantry() : Node{ get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , librmcs::agent::RmcsBoardLite{get_parameter("board_serial").as_string()} , logger_(get_logger()) , infantry_command_( create_partner_component(get_component_name() + "_command", *this)) @@ -55,11 +54,11 @@ class OmniInfantry , gimbal_left_friction_(*this, *infantry_command_, "/gimbal/left_friction") , gimbal_right_friction_(*this, *infantry_command_, "/gimbal/right_friction") , gimbal_bullet_feeder_(*this, *infantry_command_, "/gimbal/bullet_feeder") - , dr16_{*this} { + , dr16_{} { for (auto& motor : chassis_wheel_motors_) motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} .set_reversed() .set_reduction_ratio(13.) .enable_multi_turn_angle()); @@ -76,13 +75,13 @@ class OmniInfantry static_cast(get_parameter("pitch_motor_zero_point").as_int()))); gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1}.set_reduction_ratio(1.)); gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2} .set_reversed() .set_reduction_ratio(1.)); gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); + device::DjiMotor::Config{device::DjiMotor::Type::kM2006, 2}.enable_multi_turn_angle()); register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); @@ -90,16 +89,18 @@ class OmniInfantry register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); register_output("/tf", tf_); - start_transmit().gpio_digital_read( - librmcs::spec::rmcs_board_lite::kGpioDescriptors.kUart0Tx, - { - .period_ms = 0, - .asap = false, - .rising_edge = false, - .falling_edge = true, - .capture_timestamp = true, - .pull = librmcs::data::GpioPull::kUp, - }); + board_ = std::make_unique( + *this, get_parameter("board_serial").as_string()); + + board_->start_transmit().gpio_digital_read( + Spec::kGpios.kUart0Tx, { + .period_ms = 0, + .asap = false, + .rising_edge = false, + .falling_edge = true, + .capture_timestamp = true, + .pull = librmcs::data::GpioPull::kUp, + }); using namespace rmcs_description; // NOLINT(google-build-using-namespace) tf_->set_transform(Eigen::Translation3d{0.06603, 0.0, 0.082}); @@ -128,10 +129,13 @@ class OmniInfantry [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); }; referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart1_transmit( - {.uart_data = std::span{buffer, size}}); + board_->start_transmit().uart_transmit( + Spec::kUarts.kUart1, {.uart_data = std::span{buffer, size}}); return size; }; + + remote_control_ = std::make_unique(*this); + remote_control_->register_dr16(&dr16_); } OmniInfantry(const OmniInfantry&) = delete; @@ -145,57 +149,63 @@ class OmniInfantry update_motors(); update_imu(); dr16_.update_status(); + remote_control_->update(); supercap_.update_status(); } void command_update() { - auto builder = start_transmit(); - - builder.can1_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - } - .as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x145, - .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), - chassis_wheel_motors_[1].generate_command(), - chassis_wheel_motors_[2].generate_command(), - chassis_wheel_motors_[3].generate_command(), - } - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x142, - .can_data = gimbal_pitch_motor_.generate_velocity_command().as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - gimbal_left_friction_.generate_command(), - gimbal_right_friction_.generate_command(), - } - .as_bytes(), - }); + auto builder = board_->start_transmit(); + + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + } + .as_bytes(), + }); + + builder.can_transmit( + Spec::kCans.kCan1, // + {.can_id = 0x145, .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes()}); + + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[0].generate_command(), + chassis_wheel_motors_[1].generate_command(), + chassis_wheel_motors_[2].generate_command(), + chassis_wheel_motors_[3].generate_command(), + } + .as_bytes(), + }); + + builder.can_transmit( + Spec::kCans.kCan2, // + {.can_id = 0x142, + .can_data = gimbal_pitch_motor_.generate_velocity_command().as_bytes()}); + + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + gimbal_bullet_feeder_.generate_command(), + gimbal_left_friction_.generate_command(), + gimbal_right_friction_.generate_command(), + } + .as_bytes(), + }); } private: @@ -239,50 +249,42 @@ class OmniInfantry gimbal_pitch_motor_.calibrate_zero_point()); } - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - - auto can_id = data.can_id; - if (can_id == 0x201) { - auto& motor = chassis_wheel_motors_[0]; - motor.store_status(data.can_data); - } else if (can_id == 0x202) { - auto& motor = chassis_wheel_motors_[1]; - motor.store_status(data.can_data); - } else if (can_id == 0x203) { - auto& motor = chassis_wheel_motors_[2]; - motor.store_status(data.can_data); - } else if (can_id == 0x204) { - auto& motor = chassis_wheel_motors_[3]; - motor.store_status(data.can_data); - } else if (can_id == 0x145) { - gimbal_yaw_motor_.store_status(data.can_data); - } else if (can_id == 0x300) { - supercap_.store_status(data.can_data); - } - } - - void can2_receive_callback(const librmcs::data::CanDataView& data) override { + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; - auto can_id = data.can_id; - if (can_id == 0x142) { - gimbal_pitch_motor_.store_status(data.can_data); - } else if (can_id == 0x202) { - gimbal_bullet_feeder_.store_status(data.can_data); - } else if (can_id == 0x203) { - gimbal_left_friction_.store_status(data.can_data); - } else if (can_id == 0x204) { - gimbal_right_friction_.store_status(data.can_data); + if (can == Spec::kCans.kCan1) { + auto can_id = data.can_id; + if (can_id == 0x201) { + chassis_wheel_motors_[0].store_status(data.can_data); + } else if (can_id == 0x202) { + chassis_wheel_motors_[1].store_status(data.can_data); + } else if (can_id == 0x203) { + chassis_wheel_motors_[2].store_status(data.can_data); + } else if (can_id == 0x204) { + chassis_wheel_motors_[3].store_status(data.can_data); + } else if (can_id == 0x145) { + gimbal_yaw_motor_.store_status(data.can_data); + } else if (can_id == 0x300) { + supercap_.store_status(data.can_data); + } + } else if (can == Spec::kCans.kCan2) { + auto can_id = data.can_id; + if (can_id == 0x142) { + gimbal_pitch_motor_.store_status(data.can_data); + } else if (can_id == 0x202) { + gimbal_bullet_feeder_.store_status(data.can_data); + } else if (can_id == 0x203) { + gimbal_left_friction_.store_status(data.can_data); + } else if (can_id == 0x204) { + gimbal_right_friction_.store_status(data.can_data); + } } } void gpio_digital_read_result_callback( - const librmcs::spec::rmcs_board_lite::GpioDescriptor& gpio, - const librmcs::data::GpioDigitalDataView& data) override { - if (gpio != librmcs::spec::rmcs_board_lite::kGpioDescriptors.kUart0Tx) + const Spec::Gpio& gpio, const View::GpioDigital& data) override { + if (gpio != Spec::kGpios.kUart0Tx) return; if (!data.timestamp_quarter_us) return; @@ -294,23 +296,23 @@ class OmniInfantry camera_signal_output_.emit(*timestamp); } - void uart1_receive_callback(const librmcs::data::UartDataView& data) override { - const auto* uart_data = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&uart_data](std::byte* storage) noexcept { *storage = *uart_data++; }, - data.uart_data.size()); - } - - void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + if (uart == Spec::kUarts.kUart1) { + const auto* uart_data = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&uart_data](std::byte* storage) noexcept { *storage = *uart_data++; }, + data.uart_data.size()); + } else if (uart == Spec::kUarts.kDbus) { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + } } - void accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) override { + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); } - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); if (!timestamp.has_value()) return; @@ -326,6 +328,8 @@ class OmniInfantry private: rclcpp::Logger logger_; + std::unique_ptr board_; + class InfantryCommand : public rmcs_executor::Component { public: explicit InfantryCommand(OmniInfantry& infantry) @@ -351,6 +355,7 @@ class OmniInfantry device::DjiMotor gimbal_bullet_feeder_; device::Dr16 dr16_; + std::unique_ptr remote_control_; device::Bmi088Ekf bmi088_; device::BoardClockLifter board_clock_lifter_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-infantry.cpp deleted file mode 100644 index 140e2db12..000000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-infantry.cpp +++ /dev/null @@ -1,539 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "hardware/device/bmi088.hpp" -#include "hardware/device/can_packet.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/lk_motor.hpp" -#include "hardware/device/supercap.hpp" - -namespace rmcs_core::hardware { - -class SteeringInfantry - : public rmcs_executor::Component - , public rclcpp::Node { -public: - SteeringInfantry() - : Node( - get_component_name(), - rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) - , command_component_( - create_partner_component( - get_component_name() + "_command", *this)) { - register_output("/tf", tf_); - gimbal_calibrate_subscription_ = create_subscription( - "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - gimbal_calibrate_subscription_callback(std::move(msg)); - }); - steers_calibrate_subscription_ = create_subscription( - "/steers/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - steers_calibrate_subscription_callback(std::move(msg)); - }); - - top_board_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_top_board").as_string()); - bottom_board_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_bottom_board").as_string()); - - tf_->set_transform( - Eigen::Translation3d{0.06603, 0.0, 0.082}); - } - - SteeringInfantry(const SteeringInfantry&) = delete; - SteeringInfantry& operator=(const SteeringInfantry&) = delete; - SteeringInfantry(SteeringInfantry&&) = delete; - SteeringInfantry& operator=(SteeringInfantry&&) = delete; - - ~SteeringInfantry() override = default; - - void update() override { - top_board_->update(); - bottom_board_->update(); - } - - void command_update() { - top_board_->command_update(); - bottom_board_->command_update(); - } - -private: - void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New yaw offset: %ld", - bottom_board_->gimbal_yaw_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New pitch offset: %ld", - top_board_->gimbal_pitch_motor_.calibrate_zero_point()); - } - - void steers_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - RCLCPP_INFO( - get_logger(), "[steer calibration] New left front offset: %d", - bottom_board_->chassis_front_steering_motors_[0].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[steer calibration] New left back offset: %d", - bottom_board_->chassis_back_steering_motors_[0].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[steer calibration] New right back offset: %d", - bottom_board_->chassis_back_steering_motors_[1].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[steer calibration] New right front offset: %d", - bottom_board_->chassis_front_steering_motors_[1].calibrate_zero_point()); - } - - class SteeringInfantryCommand : public rmcs_executor::Component { - public: - explicit SteeringInfantryCommand(SteeringInfantry& infantry_) - : infantry_(infantry_) {} - - void update() override { infantry_.command_update(); } - - SteeringInfantry& infantry_; - }; - std::shared_ptr command_component_; - - class TopBoard final : private librmcs::agent::RmcsBoardLite { - public: - friend class SteeringInfantry; - explicit TopBoard( - SteeringInfantry& steering_infantry, SteeringInfantryCommand& steering_infantry_command, - std::string_view board_serial = {}) - : librmcs::agent::RmcsBoardLite(board_serial, {true}) - , tf_(steering_infantry.tf_) - , imu_(1000, 0.2, 0.0) - , gimbal_pitch_motor_(steering_infantry, steering_infantry_command, "/gimbal/pitch") - , gimbal_left_friction_( - steering_infantry, steering_infantry_command, "/gimbal/left_friction") - , gimbal_right_friction_( - steering_infantry, steering_infantry_command, "/gimbal/right_friction") { - - gimbal_pitch_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} - .set_encoder_zero_point( - static_cast( - steering_infantry.get_parameter("pitch_motor_zero_point").as_int())) - .enable_multi_turn_angle()); - gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); - gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(1.) - .set_reversed()); - steering_infantry.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); - steering_infantry.register_output( - "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); - imu_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(x, y, z); }); - } - - TopBoard(const TopBoard&) = delete; - TopBoard& operator=(const TopBoard&) = delete; - TopBoard(TopBoard&&) = delete; - TopBoard& operator=(TopBoard&&) = delete; - - ~TopBoard() final = default; - - void update() { - imu_.update_status(); - const Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; - - tf_->set_transform( - gimbal_imu_pose.conjugate()); - - *gimbal_yaw_velocity_imu_ = imu_.gz(); - *gimbal_pitch_velocity_imu_ = imu_.gy(); - - gimbal_pitch_motor_.update_status(); - gimbal_left_friction_.update_status(); - gimbal_right_friction_.update_status(); - - tf_->set_state( - gimbal_pitch_motor_.angle()); - } - - void command_update() { - auto builder = start_transmit(); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_right_friction_.generate_command(), - gimbal_left_friction_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x143, - .can_data = gimbal_pitch_motor_.generate_velocity_command().as_bytes(), - }); - } - - private: - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - if (can_id == 0x201) { - gimbal_right_friction_.store_status(data.can_data); - } else if (can_id == 0x202) { - gimbal_left_friction_.store_status(data.can_data); - } - } - - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - if (can_id == 0x143) { - gimbal_pitch_motor_.store_status(data.can_data); - } - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - - OutputInterface& tf_; - - device::Bmi088 imu_; - - device::LkMotor gimbal_pitch_motor_; - device::DjiMotor gimbal_left_friction_; - device::DjiMotor gimbal_right_friction_; - - OutputInterface gimbal_yaw_velocity_imu_; - OutputInterface gimbal_pitch_velocity_imu_; - }; - - class BottomBoard final : private librmcs::agent::RmcsBoardLite { - public: - friend class SteeringInfantry; - explicit BottomBoard( - SteeringInfantry& steering_infantry, SteeringInfantryCommand& steering_infantry_command, - std::string_view board_serial = {}) - : librmcs::agent::RmcsBoardLite(board_serial) - , tf_(steering_infantry.tf_) - , imu_(1000, 0.2, 0.0) - , dr16_(steering_infantry) - , gimbal_yaw_motor_(steering_infantry, steering_infantry_command, "/gimbal/yaw") - , supercap_(steering_infantry, steering_infantry_command) - , gimbal_bullet_feeder_( - steering_infantry, steering_infantry_command, "/gimbal/bullet_feeder") - , chassis_front_steering_motors_( - {steering_infantry, steering_infantry_command, "/chassis/left_front_steering"}, - {steering_infantry, steering_infantry_command, "/chassis/right_front_steering"}) - , chassis_front_wheel_motors_( - {steering_infantry, steering_infantry_command, "/chassis/left_front_wheel"}, - {steering_infantry, steering_infantry_command, "/chassis/right_front_wheel"}) - , chassis_back_steering_motors_( - {steering_infantry, steering_infantry_command, "/chassis/left_back_steering"}, - {steering_infantry, steering_infantry_command, "/chassis/right_back_steering"}) - , chassis_back_wheel_motors_( - {steering_infantry, steering_infantry_command, "/chassis/left_back_wheel"}, - {steering_infantry, steering_infantry_command, "/chassis/right_back_wheel"}) { - gimbal_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( - static_cast( - steering_infantry.get_parameter("yaw_motor_zero_point").as_int()))); - gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .enable_multi_turn_angle()); - - chassis_front_steering_motors_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast( - steering_infantry.get_parameter("left_front_zero_point").as_int())) - .set_reversed()); - chassis_front_steering_motors_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast( - steering_infantry.get_parameter("right_front_zero_point").as_int())) - .set_reversed()); - chassis_back_steering_motors_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast( - steering_infantry.get_parameter("left_back_zero_point").as_int())) - .set_reversed()); - chassis_back_steering_motors_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast( - steering_infantry.get_parameter("right_back_zero_point").as_int())) - .set_reversed()); - - chassis_front_wheel_motors_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)); - chassis_front_wheel_motors_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)); - chassis_back_wheel_motors_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)); - chassis_back_wheel_motors_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)); - - steering_infantry.register_output("/referee/serial", referee_serial_); - referee_serial_->read = [this](std::byte* buffer, size_t size) { - return referee_ring_buffer_receive_.pop_front_n( - [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); - }; - referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart0_transmit( - {.uart_data = std::span{buffer, size}}); - return size; - }; - - steering_infantry.register_output( - "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - } - - BottomBoard(const BottomBoard&) = delete; - BottomBoard& operator=(const BottomBoard&) = delete; - BottomBoard(BottomBoard&&) = delete; - BottomBoard& operator=(BottomBoard&&) = delete; - - ~BottomBoard() final = default; - - void update() { - imu_.update_status(); - dr16_.update_status(); - - *chassis_yaw_velocity_imu_ = imu_.gz(); - - gimbal_yaw_motor_.update_status(); - supercap_.update_status(); - gimbal_bullet_feeder_.update_status(); - - tf_->set_state( - gimbal_yaw_motor_.angle()); - - for (auto& motor : chassis_front_wheel_motors_) - motor.update_status(); - for (auto& motor : chassis_front_steering_motors_) - motor.update_status(); - for (auto& motor : chassis_back_wheel_motors_) - motor.update_status(); - for (auto& motor : chassis_back_steering_motors_) - motor.update_status(); - } - - void command_update() { - auto builder = start_transmit(); - builder.can0_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_back_wheel_motors_[0].generate_command(), - chassis_back_wheel_motors_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - - builder.can0_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_back_steering_motors_[0].generate_command(), - chassis_back_steering_motors_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - } - .as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_front_wheel_motors_[0].generate_command(), - chassis_front_wheel_motors_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_front_steering_motors_[0].generate_command(), - chassis_front_steering_motors_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - - } - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x142, - .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes(), - }); - - builder.can3_transmit({ - .can_id = 0x1FF, - .can_data = - device::CanPacket8{ - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - } - - private: - void can0_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - - if (can_id == 0x201) { - chassis_back_wheel_motors_[0].store_status(data.can_data); - } else if (can_id == 0x202) { - chassis_back_wheel_motors_[1].store_status(data.can_data); - } else if (can_id == 0x205) { - chassis_back_steering_motors_[0].store_status(data.can_data); - } else if (can_id == 0x206) { - chassis_back_steering_motors_[1].store_status(data.can_data); - } else if (can_id == 0x300) { - supercap_.store_status(data.can_data); - } - } - - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - - if (can_id == 0x201) { - chassis_front_wheel_motors_[0].store_status(data.can_data); - } else if (can_id == 0x202) { - chassis_front_wheel_motors_[1].store_status(data.can_data); - } else if (can_id == 0x205) { - chassis_front_steering_motors_[0].store_status(data.can_data); - } else if (can_id == 0x206) { - chassis_front_steering_motors_[1].store_status(data.can_data); - } - } - - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - if (can_id == 0x142) { - gimbal_yaw_motor_.store_status(data.can_data); - } - } - - void can3_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - - if (can_id == 0x205) { - gimbal_bullet_feeder_.store_status(data.can_data); - } - } - - void uart0_receive_callback(const librmcs::data::UartDataView& data) override { - const auto* uart_data = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&uart_data](std::byte* storage) noexcept { *storage = *uart_data++; }, - data.uart_data.size()); - } - - void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - - OutputInterface& tf_; - - device::Bmi088 imu_; - device::Dr16 dr16_; - - device::LkMotor gimbal_yaw_motor_; - - device::Supercap supercap_; - device::DjiMotor gimbal_bullet_feeder_; - - device::DjiMotor chassis_front_steering_motors_[2]; - device::DjiMotor chassis_front_wheel_motors_[2]; - device::DjiMotor chassis_back_steering_motors_[2]; - device::DjiMotor chassis_back_wheel_motors_[2]; - - rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; - OutputInterface referee_serial_; - - OutputInterface chassis_yaw_velocity_imu_; - }; - - OutputInterface tf_; - - rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; - rclcpp::Subscription::SharedPtr steers_calibrate_subscription_; - - std::shared_ptr top_board_; - std::shared_ptr bottom_board_; -}; - -} // namespace rmcs_core::hardware - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringInfantry, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp index a28dc60e2..d78f8add4 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp @@ -106,8 +106,8 @@ class Infantry std::round((2 * std::numbers::pi - angle) / std::numbers::pi * 180)); }; chassis_direction_indicator_.set_color( - chassis_mode == rmcs_msgs::ChassisMode::SPIN ? Shape::Color::GREEN - : Shape::Color::PINK); + chassis_mode == rmcs_msgs::ChassisMode::SPIN_FAST ? Shape::Color::GREEN + : Shape::Color::PINK); chassis_direction_indicator_.set_angle(to_referee_angle(*chassis_angle_), 30); bool chassis_control_direction_indicator_visible = false; @@ -181,4 +181,4 @@ class Infantry #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::Infantry, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::Infantry, rmcs_executor::Component) From 13ca2ccaaf88d026ed7f0d2570cbd6e1fb79852b Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Mon, 10 Aug 2026 18:07:14 +0800 Subject: [PATCH 10/86] feat(deformable-infantry): Rework deformable chassis and add omni-b --- ...g.yaml => deformable-infantry-omni-b.yaml} | 266 ++-- .../config/deformable-infantry-omni.yaml | 278 ++--- .../controller/chassis/deformable_chassis.cpp | 1091 +++-------------- .../chassis/deformable_joint_controller.cpp | 316 ++--- .../chassis/deformable_joint_layer.hpp | 176 --- .../controller/chassis/deformable_mode.hpp | 331 +++++ .../deformable_omni_wheel_controller.cpp | 71 +- .../chassis/deformable_suspension.cpp | 627 ++++++++++ .../chassis/deformable_wheel_controller.cpp | 873 ------------- .../deformable_infantry_gimbal_controller.cpp | 187 ++- .../hardware/deformable-infantry-omni-b.cpp | 876 +++++++++++++ .../src/hardware/deformable-infantry-omni.cpp | 847 +++++++------ .../hardware/deformable-infantry-steering.cpp | 876 ------------- .../referee/app/ui/deformable_infantry_ui.cpp | 144 ++- .../ui/widget/deformable_chassis_top_view.hpp | 23 +- 15 files changed, 3129 insertions(+), 3853 deletions(-) rename rmcs_ws/src/rmcs_bringup/config/{deformable-infantry-steering.yaml => deformable-infantry-omni-b.yaml} (56%) delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_layer.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_wheel_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml similarity index 56% rename from rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml rename to rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index b1ec953d6..aa952607c 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -2,7 +2,7 @@ rmcs_executor: ros__parameters: update_rate: 1000.0 components: - - rmcs_core::hardware::DeformableInfantryV2 -> deformable_infantry + - rmcs_core::hardware::DeformableInfantryOmniB -> deformable_infantry - rmcs_core::referee::Status -> referee_status - rmcs_core::referee::Command -> referee_command @@ -21,46 +21,91 @@ rmcs_executor: - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller - rmcs_core::controller::chassis::DeformableChassis -> chassis_controller + - rmcs_core::controller::chassis::DeformableSuspension -> deformable_suspension - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - - rmcs_core::controller::chassis::DeformableChassisController -> deformable_chassis_controller + - rmcs_core::controller::chassis::DeformableOmniWheelController -> deformable_chassis_controller - rmcs_core::controller::chassis::DeformableJointController -> lf_joint_controller - rmcs_core::controller::chassis::DeformableJointController -> lb_joint_controller - rmcs_core::controller::chassis::DeformableJointController -> rb_joint_controller - rmcs_core::controller::chassis::DeformableJointController -> rf_joint_controller + - rmcs::AutoAimComponent -> auto_aim_component + - rmcs::AutoAimCapturerComponent -> auto_aim_capturer + - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui + # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + # - rmcs_core::debug::ValueCollector -> value_collector + +value_collector: + ros__parameters: + csv_path: "/tmp/pitch_.csv" + signals: + - /gimbal/pitch/angle + - /gimbal/pitch/velocity + - /gimbal/pitch/velocity_imu + - /gimbal/pitch/angle_error + - /gimbal/pitch/control_torque + - /gimbal/pitch/control_velocity + write_interval: 5 + flush_interval: 1000 value_broadcaster: ros__parameters: forward_list: - - /chassis/left_front_joint/torque - - /chassis/left_back_joint/torque - - /chassis/right_front_joint/torque - - /chassis/right_back_joint/torque - - /chassis/left_front_joint/suspension_mode - - /chassis/left_back_joint/suspension_mode - - /chassis/right_front_joint/suspension_mode - - /chassis/right_back_joint/suspension_mode - - /chassis/left_front_joint/suspension_torque - - /chassis/left_back_joint/suspension_torque - - /chassis/right_front_joint/suspension_torque - - /chassis/right_back_joint/suspension_torque - - /chassis/left_front_joint/control_torque - - /chassis/left_back_joint/control_torque - - /chassis/right_front_joint/control_torque - - /chassis/right_back_joint/control_torque + - /gimbal/pitch/angle + - /gimbal/pitch/velocity + +auto_aim_capturer: + ros__parameters: + camera_name: "" + exposure_us: 4000.0 + gain: 8.0 + framerate: 120.0 + invert_image: false + rls_tau_sec: 10.0 + use_hardware_sync: false + delay_ms: 6.5 + +auto_aim_component: + ros__parameters: + # WARN: 危险！可选 red | blue，生效后裁判系统 ID 缺席时 + # 将强行绑定为对应阵营哨兵，仅供调试，严禁比赛启用。 + # 留空或填 unknow 表示禁用。 + dangerous_fallback: "" + manual_shoot: true + enable_rune: true + camera_translation: [0.058, -0.08, 0.0] + fire_control: + bullet_speed: 22.5 + shoot_delay: 0.04 + offset_yaw: +2.5 + offset_pitch: +0.5 + attack_window: 80.0 + degraded_angle_speed: 12.0 + window_redundancy: 0.8 + window_hysteresis: 0.2 + attack_preaim: false + require_stable_command: false + yaw_tolerance: 0.07 + pitch_tolerance: 0.04 + rune_idle_duration: 0.4 + rune_shoot_duration: 0.2 + +auto_aim_ui: + ros__parameters: + offset_x: 0.0 + offset_y: +0.08 + offset_z: 0.0 deformable_infantry: ros__parameters: - serial_filter_rmcs_board: "AF-23FB-EE32-B892-1302-AE70-D640-7B4E-0CBF" - serial_filter_top_board: "AF-ABAC-786D-1B53-99F6-00A2-42A6-AA95-9D69" - left_front_zero_point: 7173 - left_back_zero_point: 5167 - right_back_zero_point: 3098 - right_front_zero_point: 6485 - yaw_motor_zero_point: 39442 - pitch_motor_zero_point: 56556 + serial_filter_bottom_board: "AF-73B2-E8A1-A544-79ED-5BDA-D088-7F21-A6A6" + serial_filter_top_board: "AF-C26A-0C9C-CF41-3E3C-1596-524B-7527-5744" + chassis_radius: 0.2341741 + rod_length: 0.140 + yaw_motor_zero_point: 57900 + pitch_motor_zero_point: 56354 debug_log_supercap: false debug_log_wheel_motor: false debug_log_deformable_joint_motor: false @@ -68,27 +113,52 @@ deformable_infantry: chassis_controller: ros__parameters: # Deploy geometry / chassis-owned joint intent - min_angle: 20.0 - max_angle: 50.0 + min_angle: 5.0 + max_angle: 59.0 active_suspension_enable: true spin_ratio: 1.0 +deformable_suspension: + ros__parameters: # IMU attitude correction at min-angle stance. - active_suspension_pitch_kp: 8.0 - active_suspension_pitch_ki: 0.35 - active_suspension_pitch_kd: 0.28 - - active_suspension_roll_kp: 8.0 - active_suspension_roll_ki: 0.35 - active_suspension_roll_kd: 0.28 - - active_suspension_pitch_angle_diff_limit_deg: 45.0 - active_suspension_roll_angle_diff_limit_deg: 45.0 - active_suspension_pid_integral_limit_deg: 20.0 + active_suspension_pitch_outer_kp: 12.0 + active_suspension_pitch_outer_ki: 0.02 + active_suspension_pitch_outer_kd: 0.0 + active_suspension_pitch_outer_integral_min: -2.0 + active_suspension_pitch_outer_integral_max: 2.0 + active_suspension_pitch_outer_output_min: -3.0 + active_suspension_pitch_outer_output_max: 3.0 + + active_suspension_pitch_inner_kp: 0.45 + active_suspension_pitch_inner_ki: 0.0 + active_suspension_pitch_inner_kd: 0.0 + active_suspension_pitch_inner_integral_min: -1.0 + active_suspension_pitch_inner_integral_max: 1.0 + active_suspension_pitch_inner_output_min: -0.785 + active_suspension_pitch_inner_output_max: 0.785 + + active_suspension_roll_outer_kp: 12.0 + active_suspension_roll_outer_ki: 0.02 + active_suspension_roll_outer_kd: 0.0 + active_suspension_roll_outer_integral_min: -2.0 + active_suspension_roll_outer_integral_max: 2.0 + active_suspension_roll_outer_output_min: -3.0 + active_suspension_roll_outer_output_max: 3.0 + + active_suspension_roll_inner_kp: 0.45 + active_suspension_roll_inner_ki: 0.0 + active_suspension_roll_inner_kd: 0.0 + active_suspension_roll_inner_integral_min: -1.0 + active_suspension_roll_inner_integral_max: 1.0 + active_suspension_roll_inner_output_min: -0.785 + active_suspension_roll_inner_output_max: 0.785 # Chassis-owned joint intent trajectory limits while attitude correction is active. active_suspension_target_velocity_limit_deg: 80.0 active_suspension_target_acceleration_limit_deg: 360.0 + active_suspension_correction_velocity_limit_deg: 720.0 + active_suspension_correction_acceleration_limit_deg: 3600.0 + active_suspension_rate_lpf_cutoff_hz: 10.0 # Automatic IMU mounting-error calibration. # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. @@ -97,29 +167,29 @@ chassis_controller: gimbal_controller: ros__parameters: - inertia: 1.0 # kg·m² - friction: 1.65 # Nm/(rad/s) - - upper_limit: -0.61 # -35 deg - lower_limit: 0.05 # 6 deg - use_encoder_pitch: true + upper_limit: -0.47123 # -27 deg + lower_limit: 0.10 # 8 deg + ctrl_hold_pitch_target_angle: 0.0 - yaw_angle_kp: 10.0 + yaw_angle_kp: 15.0 yaw_angle_ki: 0.0 yaw_angle_kd: 0.0 - yaw_velocity_kp: 8.0 + yaw_velocity_kp: 15.0 yaw_velocity_ki: 0.0 yaw_velocity_kd: 0.0 - pitch_angle_kp: 40.0 - pitch_angle_ki: 0.0 - pitch_angle_kd: 0.0 + pitch_angle_kp: 35.0 + pitch_angle_ki: 0.02 + pitch_angle_kd: 0.3 - pitch_velocity_kp: 3.0 + pitch_velocity_kp: 2.0 pitch_velocity_ki: 0.0 pitch_velocity_kd: 0.0 + pitch_gravity_ff_gain: 4.302 + pitch_gravity_ff_phase: 0.589 + pitch_torque_control: true friction_wheel_controller: @@ -128,14 +198,14 @@ friction_wheel_controller: - /gimbal/left_friction - /gimbal/right_friction friction_velocities: - - 600.0 - - 600.0 + - 580.0 + - 580.0 friction_soft_start_stop_time: 1.0 heat_controller: ros__parameters: heat_per_shot: 10000 - reserved_heat: 0 + reserved_heat: 15000 bullet_feeder_controller: ros__parameters: @@ -171,34 +241,26 @@ bullet_feeder_velocity_pid_controller: measurement: /gimbal/bullet_feeder/velocity setpoint: /gimbal/bullet_feeder/control_velocity control: /gimbal/bullet_feeder/control_torque - kp: 1.4 + kp: 1.5 ki: 0.0 kd: 0.0 deformable_chassis_controller: ros__parameters: - mass: 23.0 + mass: 25.5 moment_of_inertia: 1.0 - chassis_radius: 0.2341741 - rod_length: 0.150 - wheel_radius: 0.055 - friction_coefficient: 0.6 + wheel_radius: 0.075 + friction_coefficient: 6.6 k1: 2.958580e+00 k2: 3.082190e-03 no_load_power: 11.37 lf_joint_controller: ros__parameters: - # Joint-local servo inputs produced by chassis intent generation measurement_angle: /chassis/left_front_joint/physical_angle - measurement_velocity: /chassis/left_front_joint/physical_velocity setpoint_angle: /chassis/left_front_joint/target_physical_angle setpoint_velocity: /chassis/left_front_joint/target_physical_velocity - mode_input: /chassis/left_front_joint/suspension_mode - suspension_torque: /chassis/left_front_joint/suspension_torque control: /chassis/left_front_joint/control_torque - - # Normal ADRC servo mode dt: 0.001 b0: -1.0 kt: 1.0 @@ -216,33 +278,11 @@ lf_joint_controller: output_min: -200.0 output_max: 200.0 - # Suspension ADRC servo mode - suspension_td_h: 0.001 - suspension_td_r: 12.0 - suspension_eso_w0: 80.0 - suspension_k1: 6.0 - suspension_k2: 3.0 - suspension_alpha1: 0.75 - suspension_alpha2: 0.7 - suspension_delta: 0.02 - suspension_u_min: -35.0 - suspension_u_max: 35.0 - suspension_output_min: -35.0 - suspension_output_max: 35.0 - - # Joint-local feedforward / limit shaping - torque_feedforward_gain: 0.0 - suspension_torque_feedforward_gain: -1.0 - lb_joint_controller: ros__parameters: - # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/left_back_joint/physical_angle - measurement_velocity: /chassis/left_back_joint/physical_velocity setpoint_angle: /chassis/left_back_joint/target_physical_angle setpoint_velocity: /chassis/left_back_joint/target_physical_velocity - mode_input: /chassis/left_back_joint/suspension_mode - suspension_torque: /chassis/left_back_joint/suspension_torque control: /chassis/left_back_joint/control_torque dt: 0.001 b0: -1.0 @@ -260,30 +300,12 @@ lb_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 - suspension_td_h: 0.001 - suspension_td_r: 12.0 - suspension_eso_w0: 80.0 - suspension_k1: 6.0 - suspension_k2: 3.0 - suspension_alpha1: 0.75 - suspension_alpha2: 0.7 - suspension_delta: 0.02 - suspension_u_min: -35.0 - suspension_u_max: 35.0 - suspension_output_min: -35.0 - suspension_output_max: 35.0 - torque_feedforward_gain: 0.0 - suspension_torque_feedforward_gain: -1.0 rb_joint_controller: ros__parameters: - # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_back_joint/physical_angle - measurement_velocity: /chassis/right_back_joint/physical_velocity setpoint_angle: /chassis/right_back_joint/target_physical_angle setpoint_velocity: /chassis/right_back_joint/target_physical_velocity - mode_input: /chassis/right_back_joint/suspension_mode - suspension_torque: /chassis/right_back_joint/suspension_torque control: /chassis/right_back_joint/control_torque dt: 0.001 b0: -1.0 @@ -301,30 +323,12 @@ rb_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 - suspension_td_h: 0.001 - suspension_td_r: 12.0 - suspension_eso_w0: 80.0 - suspension_k1: 6.0 - suspension_k2: 3.0 - suspension_alpha1: 0.75 - suspension_alpha2: 0.7 - suspension_delta: 0.02 - suspension_u_min: -35.0 - suspension_u_max: 35.0 - suspension_output_min: -35.0 - suspension_output_max: 35.0 - torque_feedforward_gain: 0.0 - suspension_torque_feedforward_gain: -1.0 rf_joint_controller: ros__parameters: - # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_front_joint/physical_angle - measurement_velocity: /chassis/right_front_joint/physical_velocity setpoint_angle: /chassis/right_front_joint/target_physical_angle setpoint_velocity: /chassis/right_front_joint/target_physical_velocity - mode_input: /chassis/right_front_joint/suspension_mode - suspension_torque: /chassis/right_front_joint/suspension_torque control: /chassis/right_front_joint/control_torque dt: 0.001 b0: -1.0 @@ -342,17 +346,3 @@ rf_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 - suspension_td_h: 0.001 - suspension_td_r: 12.0 - suspension_eso_w0: 80.0 - suspension_k1: 6.0 - suspension_k2: 3.0 - suspension_alpha1: 0.75 - suspension_alpha2: 0.7 - suspension_delta: 0.02 - suspension_u_min: -35.0 - suspension_u_max: 35.0 - suspension_output_min: -35.0 - suspension_output_max: 35.0 - torque_feedforward_gain: 0.0 - suspension_torque_feedforward_gain: -1.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index ac97a8e38..272d4f091 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -9,7 +9,7 @@ rmcs_executor: - rmcs_core::referee::command::Interaction -> referee_interaction - rmcs_core::referee::command::interaction::Ui -> referee_ui - - rmcs_core::referee::app::ui::Infantry -> referee_ui_infantry + - rmcs_core::referee::app::ui::DeformableInfantry -> referee_ui_infantry - rmcs_core::controller::gimbal::DeformableInfantryGimbalController -> gimbal_controller @@ -21,6 +21,7 @@ rmcs_executor: - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller - rmcs_core::controller::chassis::DeformableChassis -> chassis_controller + - rmcs_core::controller::chassis::DeformableSuspension -> deformable_suspension - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::DeformableOmniWheelController -> deformable_chassis_controller @@ -29,26 +30,82 @@ rmcs_executor: - rmcs_core::controller::chassis::DeformableJointController -> rb_joint_controller - rmcs_core::controller::chassis::DeformableJointController -> rf_joint_controller - - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster - # - rmcs::AutoAimComponent -> auto_aim_component + - rmcs::AutoAimComponent -> auto_aim_component + - rmcs::AutoAimCapturerComponent -> auto_aim_capturer + - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui + + # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + # - rmcs_core::debug::ValueCollector -> value_collector + +value_collector: + ros__parameters: + csv_path: "/tmp/pitch_.csv" + signals: + - /gimbal/pitch/angle + - /gimbal/pitch/velocity + - /gimbal/pitch/velocity_imu + - /gimbal/pitch/angle_error + - /gimbal/pitch/control_torque + - /gimbal/pitch/control_velocity + write_interval: 5 + flush_interval: 1000 value_broadcaster: ros__parameters: forward_list: - - /gimbal/yaw/angle - - /gimbal/yaw/velocity + - /gimbal/pitch/angle + - /gimbal/pitch/velocity + +auto_aim_capturer: + ros__parameters: + camera_name: "" + exposure_us: 4000.0 + gain: 8.0 + framerate: 120.0 + invert_image: false + rls_tau_sec: 10.0 + use_hardware_sync: true + delay_ms: 6.5 + +auto_aim_component: + ros__parameters: + # WARN: 危险！可选 red | blue，生效后裁判系统 ID 缺席时 + # 将强行绑定为对应阵营哨兵，仅供调试，严禁比赛启用。 + # 留空或填 unknow 表示禁用。 + dangerous_fallback: "" + manual_shoot: true + enable_rune: true + camera_translation: [0.058, -0.08, 0.0] + fire_control: + bullet_speed: 22.5 + shoot_delay: 0.04 + offset_yaw: +0.1 + offset_pitch: -0.4 + attack_window: 80.0 + degraded_angle_speed: 12.0 + window_redundancy: 0.8 + window_hysteresis: 0.2 + attack_preaim: false + require_stable_command: false + yaw_tolerance: 0.07 + pitch_tolerance: 0.04 + rune_idle_duration: 0.4 + rune_shoot_duration: 0.2 + +auto_aim_ui: + ros__parameters: + offset_x: 0.0 + offset_y: -0.08 + offset_z: 0.0 deformable_infantry: ros__parameters: - serial_filter_rmcs_board: "AF-73B2-E8A1-A544-79ED-5BDA-D088-7F21-A6A6" - serial_filter_top_board: "D4-0262-9E84-E715-CADB-9894-7241" - serial_filter_imu: "AF-8217-B05F-811B-6F3E-04EA-448E-9D03-CA2C" - left_front_zero_point: 374 - left_back_zero_point: 5801 - right_back_zero_point: 7817 - right_front_zero_point: 7136 - yaw_motor_zero_point: 50642 - pitch_motor_zero_point: 6245 + serial_filter_bottom_board: "AF-23FB-EE32-B892-1302-AE70-D640-7B4E-0CBF" + serial_filter_top_board: "AF-7A42-07AA-D181-0356-7715-6D7C-4C65-5762" + chassis_radius: 0.2341741 + rod_length: 0.140 + yaw_motor_zero_point: 43365 + pitch_motor_zero_point: 6432 debug_log_supercap: false debug_log_wheel_motor: false debug_log_deformable_joint_motor: false @@ -56,41 +113,52 @@ deformable_infantry: chassis_controller: ros__parameters: # Deploy geometry / chassis-owned joint intent - min_angle: 8.0 - max_angle: 58.0 + min_angle: 5.0 + max_angle: 59.0 active_suspension_enable: true spin_ratio: 1.0 - # Suspension geometry / support model. - active_suspension_mass: 22.5 - active_suspension_rod_length: 0.150 - active_suspension_Kz: 150.0 - active_suspension_D_leg: 10.0 - active_suspension_torque_limit: 80.0 - active_suspension_gravity_comp_gain: 1.0 - active_suspension_control_acceleration_limit: 6.0 - active_suspension_preload_angle_deg: 8.0 - active_suspension_entry_offset_deg: 1.5 - active_suspension_ride_height_offset_deg: 3.0 - active_suspension_hold_travel_deg: 5.0 - active_suspension_activation_velocity_threshold_deg: 15.0 - - # IMU attitude correction as suspension force bias. - active_suspension_pitch_kp: 8.0 - active_suspension_pitch_ki: 0.35 - active_suspension_pitch_kd: 0.28 - - active_suspension_roll_kp: 8.0 - active_suspension_roll_ki: 0.35 - active_suspension_roll_kd: 0.28 - - active_suspension_pitch_angle_diff_limit_deg: 45.0 - active_suspension_roll_angle_diff_limit_deg: 45.0 - active_suspension_pid_integral_limit_deg: 20.0 +deformable_suspension: + ros__parameters: + # IMU attitude correction at min-angle stance. + active_suspension_pitch_outer_kp: 12.0 + active_suspension_pitch_outer_ki: 0.02 + active_suspension_pitch_outer_kd: 0.0 + active_suspension_pitch_outer_integral_min: -2.0 + active_suspension_pitch_outer_integral_max: 2.0 + active_suspension_pitch_outer_output_min: -3.0 + active_suspension_pitch_outer_output_max: 3.0 + + active_suspension_pitch_inner_kp: 0.45 + active_suspension_pitch_inner_ki: 0.0 + active_suspension_pitch_inner_kd: 0.0 + active_suspension_pitch_inner_integral_min: -1.0 + active_suspension_pitch_inner_integral_max: 1.0 + active_suspension_pitch_inner_output_min: -0.785 + active_suspension_pitch_inner_output_max: 0.785 + + active_suspension_roll_outer_kp: 12.0 + active_suspension_roll_outer_ki: 0.02 + active_suspension_roll_outer_kd: 0.0 + active_suspension_roll_outer_integral_min: -2.0 + active_suspension_roll_outer_integral_max: 2.0 + active_suspension_roll_outer_output_min: -3.0 + active_suspension_roll_outer_output_max: 3.0 + + active_suspension_roll_inner_kp: 0.45 + active_suspension_roll_inner_ki: 0.0 + active_suspension_roll_inner_kd: 0.0 + active_suspension_roll_inner_integral_min: -1.0 + active_suspension_roll_inner_integral_max: 1.0 + active_suspension_roll_inner_output_min: -0.785 + active_suspension_roll_inner_output_max: 0.785 # Chassis-owned joint intent trajectory limits while attitude correction is active. active_suspension_target_velocity_limit_deg: 80.0 active_suspension_target_acceleration_limit_deg: 360.0 + active_suspension_correction_velocity_limit_deg: 720.0 + active_suspension_correction_acceleration_limit_deg: 3600.0 + active_suspension_rate_lpf_cutoff_hz: 10.0 # Automatic IMU mounting-error calibration. # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. @@ -99,49 +167,45 @@ chassis_controller: gimbal_controller: ros__parameters: - upper_limit: -0.61 # -35 deg - lower_limit: 0.05 # 6 deg + upper_limit: -0.47123 # -27 deg + lower_limit: 0.10 # 8 deg + ctrl_hold_pitch_target_angle: 0.0 - yaw_angle_kp: 30.0 + yaw_angle_kp: 10.0 yaw_angle_ki: 0.0 yaw_angle_kd: 0.0 - yaw_velocity_kp: 15.0 + yaw_velocity_kp: 10.0 yaw_velocity_ki: 0.0 yaw_velocity_kd: 0.0 - yaw_vel_ff_gain: 0.47 - yaw_acc_ff_gain: 0.00 + pitch_angle_kp: 35.0 + pitch_angle_ki: 0.02 + pitch_angle_kd: 0.3 - pitch_angle_kp: 25.0 - pitch_angle_ki: 0.0 - pitch_angle_kd: 0.0 - - pitch_velocity_kp: 2.2 + pitch_velocity_kp: 2.0 pitch_velocity_ki: 0.0 pitch_velocity_kd: 0.0 - pitch_acc_ff_gain: 0.10 + pitch_gravity_ff_gain: 4.302 + pitch_gravity_ff_phase: 0.589 pitch_torque_control: true - pitch_fusion_enabled: true - pitch_fusion_alpha: 0.98 - friction_wheel_controller: ros__parameters: friction_wheels: - /gimbal/left_friction - /gimbal/right_friction friction_velocities: - - 600.0 - - 600.0 + - 580.0 + - 580.0 friction_soft_start_stop_time: 1.0 heat_controller: ros__parameters: heat_per_shot: 10000 - reserved_heat: 0 + reserved_heat: 15000 bullet_feeder_controller: ros__parameters: @@ -183,30 +247,22 @@ bullet_feeder_velocity_pid_controller: deformable_chassis_controller: ros__parameters: - mass: 22.5 + mass: 25.5 moment_of_inertia: 1.0 - chassis_radius: 0.2341741 - rod_length: 0.150 - wheel_radius: 0.055 - friction_coefficient: 66.6 + wheel_radius: 0.075 + friction_coefficient: 6.6 k1: 2.958580e+00 k2: 3.082190e-03 no_load_power: 11.37 lf_joint_controller: ros__parameters: - # Joint-local servo inputs produced by chassis intent generation measurement_angle: /chassis/left_front_joint/physical_angle - measurement_velocity: /chassis/left_front_joint/physical_velocity setpoint_angle: /chassis/left_front_joint/target_physical_angle setpoint_velocity: /chassis/left_front_joint/target_physical_velocity - mode_input: /chassis/left_front_joint/suspension_mode - suspension_torque: /chassis/left_front_joint/suspension_torque control: /chassis/left_front_joint/control_torque - - # Normal ADRC servo mode dt: 0.001 - b0: -0.60 + b0: -1.0 kt: 1.0 td_h: 0.001 td_r: 50.0 @@ -222,36 +278,14 @@ lf_joint_controller: output_min: -200.0 output_max: 200.0 - # Suspension ADRC servo mode - suspension_td_h: 0.001 - suspension_td_r: 12.0 - suspension_eso_w0: 80.0 - suspension_k1: 6.0 - suspension_k2: 3.0 - suspension_alpha1: 0.75 - suspension_alpha2: 0.7 - suspension_delta: 0.02 - suspension_u_min: -35.0 - suspension_u_max: 35.0 - suspension_output_min: -35.0 - suspension_output_max: 35.0 - - # Joint-local feedforward / limit shaping - torque_feedforward_gain: 0.0 - suspension_torque_feedforward_gain: -1.0 - lb_joint_controller: ros__parameters: - # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/left_back_joint/physical_angle - measurement_velocity: /chassis/left_back_joint/physical_velocity setpoint_angle: /chassis/left_back_joint/target_physical_angle setpoint_velocity: /chassis/left_back_joint/target_physical_velocity - mode_input: /chassis/left_back_joint/suspension_mode - suspension_torque: /chassis/left_back_joint/suspension_torque control: /chassis/left_back_joint/control_torque dt: 0.001 - b0: -0.60 + b0: -1.0 kt: 1.0 td_h: 0.001 td_r: 50.0 @@ -266,33 +300,15 @@ lb_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 - suspension_td_h: 0.001 - suspension_td_r: 12.0 - suspension_eso_w0: 80.0 - suspension_k1: 6.0 - suspension_k2: 3.0 - suspension_alpha1: 0.75 - suspension_alpha2: 0.7 - suspension_delta: 0.02 - suspension_u_min: -35.0 - suspension_u_max: 35.0 - suspension_output_min: -35.0 - suspension_output_max: 35.0 - torque_feedforward_gain: 0.0 - suspension_torque_feedforward_gain: -1.0 rb_joint_controller: ros__parameters: - # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_back_joint/physical_angle - measurement_velocity: /chassis/right_back_joint/physical_velocity setpoint_angle: /chassis/right_back_joint/target_physical_angle setpoint_velocity: /chassis/right_back_joint/target_physical_velocity - mode_input: /chassis/right_back_joint/suspension_mode - suspension_torque: /chassis/right_back_joint/suspension_torque control: /chassis/right_back_joint/control_torque dt: 0.001 - b0: -0.60 + b0: -1.0 kt: 1.0 td_h: 0.001 td_r: 50.0 @@ -307,33 +323,15 @@ rb_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 - suspension_td_h: 0.001 - suspension_td_r: 12.0 - suspension_eso_w0: 80.0 - suspension_k1: 6.0 - suspension_k2: 3.0 - suspension_alpha1: 0.75 - suspension_alpha2: 0.7 - suspension_delta: 0.02 - suspension_u_min: -35.0 - suspension_u_max: 35.0 - suspension_output_min: -35.0 - suspension_output_max: 35.0 - torque_feedforward_gain: 0.0 - suspension_torque_feedforward_gain: -1.0 rf_joint_controller: ros__parameters: - # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_front_joint/physical_angle - measurement_velocity: /chassis/right_front_joint/physical_velocity setpoint_angle: /chassis/right_front_joint/target_physical_angle setpoint_velocity: /chassis/right_front_joint/target_physical_velocity - mode_input: /chassis/right_front_joint/suspension_mode - suspension_torque: /chassis/right_front_joint/suspension_torque control: /chassis/right_front_joint/control_torque dt: 0.001 - b0: -0.60 + b0: -1.0 kt: 1.0 td_h: 0.001 td_r: 50.0 @@ -348,17 +346,3 @@ rf_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 - suspension_td_h: 0.001 - suspension_td_r: 12.0 - suspension_eso_w0: 80.0 - suspension_k1: 6.0 - suspension_k2: 3.0 - suspension_alpha1: 0.75 - suspension_alpha2: 0.7 - suspension_delta: 0.02 - suspension_u_min: -35.0 - suspension_u_max: 35.0 - suspension_output_min: -35.0 - suspension_output_max: 35.0 - torque_feedforward_gain: 0.0 - suspension_torque_feedforward_gain: -1.0 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index c9c61ace5..397400cfe 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -1,13 +1,12 @@ #include #include #include -#include -#include #include #include -#include +#include #include +#include #include #include @@ -15,1004 +14,270 @@ #include #include #include -#include #include +#include "controller/chassis/deformable_mode.hpp" #include "controller/pid/pid_calculator.hpp" -#include "deformable_joint_layer.hpp" namespace rmcs_core::controller::chassis { -enum class SuspensionPhase : uint8_t { kInactive, kArming, kActive, kReleasing }; - -struct AttitudeBias { - double pitch_force = 0.0; - double roll_force = 0.0; -}; - -struct LegControlState { - SuspensionPhase phase = SuspensionPhase::kInactive; - double support_force = 0.0; - double contact_confidence = 1.0; - double filtered_contact_confidence = 1.0; - double phase_elapsed = 0.0; - bool requested_deploy = false; - bool output_active = false; - bool contact_latched = false; -}; - -struct LegCommand { - double requested_target_angle = std::numeric_limits::quiet_NaN(); - double final_target_angle = std::numeric_limits::quiet_NaN(); - double target_velocity = 0.0; - double target_acceleration = 0.0; - bool suspension_mode = false; - double suspension_torque = std::numeric_limits::quiet_NaN(); -}; - -struct AttitudePidAxis { - double kp = 20.0; - double ki = 0.0; - double kd = 0.0; - double integral = 0.0; - double integral_limit = std::numeric_limits::infinity(); - double output_limit = std::numeric_limits::infinity(); - - void reset() { integral = 0.0; } - - double update(double error, double rate, double dt) { - if (!std::isfinite(error) || !std::isfinite(rate) || !std::isfinite(dt) || dt <= 0.0) { - reset(); - return std::numeric_limits::quiet_NaN(); - } - integral = std::clamp(integral + error * dt, -integral_limit, integral_limit); - return std::clamp(kp * error + ki * integral - kd * rate, -output_limit, output_limit); - } -}; - -struct SuspensionParams { - double mass, rod_length, Kz, pitch_kp, pitch_ki, pitch_kd, roll_kp, roll_ki, roll_kd, D_leg; - double com_height, wheel_base_half_x, wheel_base_half_y; - double gravity_comp_gain, control_acceleration_limit; - double preload_angle, entry_offset, ride_height_offset, hold_travel; - double activation_velocity_threshold; - double target_physical_velocity_limit, target_physical_acceleration_limit; - double torque_limit; - double pitch_angle_diff_limit, roll_angle_diff_limit, pid_integral_limit; -}; - -// --- ChassisVelocityControl --- -struct ChassisVelocityControl { - static constexpr double kTranslationalVelocityMax = 10.0; - static constexpr double kAngularVelocityMax = 10.0; - - void configure(double spin_ratio) { - spin_ratio_ = std::clamp(spin_ratio, 0.0, 1.0); - following_velocity_controller_.output_max = kAngularVelocityMax; - following_velocity_controller_.output_min = -kAngularVelocityMax; - } - - void set_spin_forward(bool forward) { spinning_forward_ = forward; } - - Eigen::Vector2d compute_translational( - const Eigen::Vector2d& joystick_right, const rmcs_msgs::Keyboard& keyboard, - double gimbal_yaw_angle) { - Eigen::Vector2d tv = - Eigen::Rotation2Dd{gimbal_yaw_angle} - * (joystick_right + Eigen::Vector2d{keyboard.w - keyboard.s, keyboard.a - keyboard.d}); - if (tv.norm() > 1.0) - tv.normalize(); - return tv * kTranslationalVelocityMax; - } - - struct AngularResult { - double angular_velocity = 0.0; - double chassis_angle = std::numeric_limits::quiet_NaN(); - double chassis_control_angle = std::numeric_limits::quiet_NaN(); - }; - - AngularResult compute_angular( - rmcs_msgs::ChassisMode mode, double gimbal_yaw_angle, double gimbal_yaw_angle_error, - bool apply_toggle_forward) { - AngularResult result; - switch (mode) { - case rmcs_msgs::ChassisMode::AUTO: break; - case rmcs_msgs::ChassisMode::SPIN: - if (apply_toggle_forward) - spinning_forward_ = !spinning_forward_; - result.angular_velocity = std::clamp( - spin_ratio_ * (spinning_forward_ ? kAngularVelocityMax : -kAngularVelocityMax), - -kAngularVelocityMax, kAngularVelocityMax); - break; - case rmcs_msgs::ChassisMode::STEP_DOWN: - result.angular_velocity = following_velocity_controller_.update(calc_angle_err_( - result.chassis_control_angle, gimbal_yaw_angle_error, gimbal_yaw_angle, - std::numbers::pi)); - break; - case rmcs_msgs::ChassisMode::LAUNCH_RAMP: { - double e = calc_angle_err_( - result.chassis_control_angle, gimbal_yaw_angle_error, gimbal_yaw_angle, - 2 * std::numbers::pi); - if (e > std::numbers::pi) - e -= 2 * std::numbers::pi; - result.angular_velocity = following_velocity_controller_.update(e); - break; - } - default: break; - } - result.chassis_angle = 2 * std::numbers::pi - gimbal_yaw_angle; - return result; - } - - void update_acceleration_estimate( - const Eigen::Vector2d& translational_velocity, double dt, double limit) { - if (!translational_velocity.array().isFinite().all()) { - control_acceleration_estimate_.setZero(); - last_translational_velocity_.setZero(); - last_valid_ = false; - return; - } - if (!last_valid_) { - last_translational_velocity_ = translational_velocity; - control_acceleration_estimate_.setZero(); - last_valid_ = true; - return; - } - const Eigen::Vector2d cap = Eigen::Vector2d::Constant(limit); - control_acceleration_estimate_ = - ((translational_velocity - last_translational_velocity_) / dt) - .cwiseMax(-cap) - .cwiseMin(cap); - last_translational_velocity_ = translational_velocity; - } - - void reset_acceleration_estimate() { - control_acceleration_estimate_.setZero(); - last_translational_velocity_.setZero(); - last_valid_ = false; - } - - Eigen::Vector2d control_acceleration_estimate() const { return control_acceleration_estimate_; } - bool spinning_forward() const { return spinning_forward_; } - -private: - double spin_ratio_ = 1.0; - bool spinning_forward_ = true; - pid::PidCalculator following_velocity_controller_{10.0, 0.0, 0.0}; - Eigen::Vector2d control_acceleration_estimate_ = Eigen::Vector2d::Zero(); - Eigen::Vector2d last_translational_velocity_ = Eigen::Vector2d::Zero(); - bool last_valid_ = false; - - double - calc_angle_err_(double& cca, double yaw_error, double yaw_angle, double alignment) const { - cca = yaw_error; - if (cca < 0) - cca += 2 * std::numbers::pi; - double e = cca + yaw_angle; - if (e >= 2 * std::numbers::pi) - e -= 2 * std::numbers::pi; - while (e > alignment / 2) { - cca -= alignment; - if (cca < 0) - cca += 2 * std::numbers::pi; - e -= alignment; - } - return e; - } -}; - -// --- ActiveSuspension --- -struct ActiveSuspension { - static constexpr double kNaN = std::numeric_limits::quiet_NaN(); - static constexpr double kGravity = 9.81; - static constexpr double kMaxAttitudeRad = 30.0 * std::numbers::pi / 180.0; - static constexpr double kMinForceArmSin = 0.1; - static constexpr double kContactConfidenceEnterThreshold = 0.55; - static constexpr double kContactConfidenceExitThreshold = 0.35; - static constexpr double kContactConfidenceFilterAlpha = 0.25; - static constexpr double kMinimumArmingTime = 0.02; - static constexpr std::array kPitchSigns = {-1.0, 1.0, 1.0, -1.0}; - static constexpr std::array kRollSigns = {1.0, 1.0, -1.0, -1.0}; - - void load_params(rclcpp::Node& node, double min_angle, double max_angle) { - params_ = SuspensionParams{ - .mass = node.get_parameter_or("active_suspension_mass", 22.5), - .rod_length = node.get_parameter_or("active_suspension_rod_length", 0.150), - .Kz = node.get_parameter_or("active_suspension_Kz", 150.0), - - .pitch_kp = node.get_parameter_or("active_suspension_pitch_kp", 200.0), - .pitch_ki = node.get_parameter_or("active_suspension_pitch_ki", 0.0), - .pitch_kd = node.get_parameter_or("active_suspension_pitch_kd", 20.0), - - .roll_kp = node.get_parameter_or("active_suspension_roll_kp", 200.0), - .roll_ki = node.get_parameter_or("active_suspension_roll_ki", 0.0), - .roll_kd = node.get_parameter_or("active_suspension_roll_kd", 20.0), - - .D_leg = node.get_parameter_or("active_suspension_D_leg", 10.0), - .com_height = node.get_parameter_or("active_suspension_com_height", 0.15), - .wheel_base_half_x = node.get_parameter_or( - "active_suspension_wheel_base_half_x", 0.2341741 / std::numbers::sqrt2), - .wheel_base_half_y = node.get_parameter_or( - "active_suspension_wheel_base_half_y", 0.2341741 / std::numbers::sqrt2), - .gravity_comp_gain = node.get_parameter_or("active_suspension_gravity_comp_gain", 1.0), - .control_acceleration_limit = std::abs( - node.get_parameter_or("active_suspension_control_acceleration_limit", 6.0)), - .preload_angle = - std::abs(node.get_parameter_or("active_suspension_preload_angle_deg", 8.0)) - * std::numbers::pi / 180.0, - .entry_offset = - std::abs(node.get_parameter_or( - "active_suspension_entry_offset_deg", - node.get_parameter_or("active_suspension_enter_deploy_tolerance_deg", 1.5))) - * std::numbers::pi / 180.0, - .ride_height_offset = - std::abs(node.get_parameter_or("active_suspension_ride_height_offset_deg", 0.0)) - * std::numbers::pi / 180.0, - .hold_travel = - std::abs(node.get_parameter_or( - "active_suspension_hold_travel_deg", - node.get_parameter_or("active_suspension_exit_deploy_tolerance_deg", 3.0))) - * std::numbers::pi / 180.0, - .activation_velocity_threshold = - node.get_parameter_or("active_suspension_activation_velocity_threshold_deg", 15.0) - * std::numbers::pi / 180.0, - .target_physical_velocity_limit = - std::max( - node.get_parameter_or( - "active_suspension_target_velocity_limit_deg", - node.get_parameter_or("target_physical_velocity_limit", 180.0)), - 1e-6) - * std::numbers::pi / 180.0, - .target_physical_acceleration_limit = - std::max( - node.get_parameter_or( - "active_suspension_target_acceleration_limit_deg", - node.get_parameter_or("target_physical_acceleration_limit", 720.0)), - 1e-6) - * std::numbers::pi / 180.0, - .torque_limit = std::abs(node.get_parameter_or("active_suspension_torque_limit", 80.0)), - .pitch_angle_diff_limit = - std::abs(node.get_parameter_or( - "active_suspension_pitch_angle_diff_limit_deg", max_angle - min_angle)) - * std::numbers::pi / 180.0, - .roll_angle_diff_limit = - std::abs(node.get_parameter_or( - "active_suspension_roll_angle_diff_limit_deg", max_angle - min_angle)) - * std::numbers::pi / 180.0, - .pid_integral_limit = - std::abs(node.get_parameter_or( - "active_suspension_pid_integral_limit_deg", max_angle - min_angle)) - * std::numbers::pi / 180.0, - }; - - pitch_pid_.kp = params_.pitch_kp; - pitch_pid_.ki = params_.pitch_ki; - pitch_pid_.kd = params_.pitch_kd; - - roll_pid_.kp = params_.roll_kp; - roll_pid_.ki = params_.roll_ki; - roll_pid_.kd = params_.roll_kd; - - pitch_pid_.integral_limit = params_.pid_integral_limit; - pitch_pid_.output_limit = params_.pitch_angle_diff_limit; - roll_pid_.integral_limit = params_.pid_integral_limit; - roll_pid_.output_limit = params_.roll_angle_diff_limit; - - enabled_ = node.get_parameter_or("active_suspension_enable", false); - calib_wait_ = std::max(node.get_parameter_or("chassis_imu_calibration_wait_s", 2.0), 0.0); - calib_sample_ = - std::max(node.get_parameter_or("chassis_imu_calibration_sample_s", 3.0), 1e-6); - } - - bool enabled() const { return enabled_; } - void set_enabled(bool v) { enabled_ = v; } - - void update( - const JointFeedbackFrame& fb, double imu_pitch, double imu_roll, double imu_pitch_rate, - double imu_roll_rate, double dt, bool requested, double min_angle_deg, double max_angle_deg, - const Eigen::Vector2d& control_accel, - std::array& current_target_physical_angles, - std::array& out_suspension_mode, - std::array& out_suspension_torque) { - static constexpr auto deg_to_rad = [](double d) { return d * std::numbers::pi / 180.0; }; - - clear_outputs_(out_suspension_mode, out_suspension_torque); - prepare_commands_(); - - if (!requested) { - reset_state_(); - current_target_physical_angles = requested_target_angles_; - publish_outputs_(out_suspension_mode, out_suspension_torque); - return; - } - - const double deploy = deg_to_rad(min_angle_deg); - const double entry = deploy + params_.entry_offset; - const double ride_height = - std::clamp(deploy + params_.ride_height_offset, deploy, deg_to_rad(max_angle_deg)); - const double support_zero_angle = deploy - params_.preload_angle; - const double release = ride_height + params_.hold_travel; - - AttitudeBias bias = - compute_attitude_bias_(imu_pitch, imu_roll, imu_pitch_rate, imu_roll_rate, dt); - if (!std::isfinite(bias.pitch_force) || !std::isfinite(bias.roll_force)) { - reset_state_(); - current_target_physical_angles.fill(deploy); - publish_outputs_(out_suspension_mode, out_suspension_torque); - return; - } - - update_leg_contact_estimates_(fb); - update_leg_states_(fb, entry, release, dt); - compute_leg_support_intents_(fb, bias, support_zero_angle, ride_height, control_accel); - current_target_physical_angles = target_angles_; - publish_outputs_(out_suspension_mode, out_suspension_torque); - } - - void update_imu_calibration( - bool symmetric_targets, double imu_pitch, double imu_roll, double dt) { - if (!symmetric_targets) { - calib_hold_ = 0.0; - calib_count_ = 0; - calib_pitch_sum_ = 0.0; - calib_roll_sum_ = 0.0; - calib_done_ = false; - return; - } - if (!std::isfinite(imu_pitch) || !std::isfinite(imu_roll)) - return; - calib_hold_ += dt; - if (calib_hold_ < calib_wait_) - return; - double end = calib_wait_ + calib_sample_; - if (calib_hold_ < end) { - calib_pitch_sum_ += imu_pitch; - calib_roll_sum_ += imu_roll; - ++calib_count_; - return; - } - if (calib_done_) - return; - calib_done_ = true; - if (calib_count_ == 0) - return; - imu_pitch_offset_ = calib_pitch_sum_ / static_cast(calib_count_); - imu_roll_offset_ = calib_roll_sum_ / static_cast(calib_count_); - } - - void reset_calibration() { - calib_hold_ = 0.0; - calib_count_ = 0; - calib_pitch_sum_ = 0.0; - calib_roll_sum_ = 0.0; - calib_done_ = false; - } - - void reset_all() { - reset_state_(); - reset_calibration(); - } - - double target_vel_limit() const { return params_.target_physical_velocity_limit; } - double target_accel_limit() const { return params_.target_physical_acceleration_limit; } - double control_accel_limit() const { return params_.control_acceleration_limit; } - -private: - SuspensionParams params_{}; - bool enabled_ = false; - AttitudePidAxis pitch_pid_, roll_pid_; - double imu_pitch_offset_ = 0.0, imu_roll_offset_ = 0.0; - double calib_wait_ = 0.0, calib_sample_ = 0.0; - double calib_hold_ = 0.0; - size_t calib_count_ = 0; - double calib_pitch_sum_ = 0.0, calib_roll_sum_ = 0.0; - bool calib_done_ = false; - std::array suspension_active_{}; - std::array leg_states_{}; - std::array leg_commands_{}; - std::array requested_target_angles_{}; - std::array target_angles_{}; - - static double deg_to_rad_(double d) { return d * std::numbers::pi / 180.0; } - - void clear_outputs_( - std::array& modes, std::array& torques) { - modes.fill(false); - torques.fill(kNaN); - } - void publish_outputs_( - std::array& modes, std::array& torques) { - for (size_t i = 0; i < kJointCount; ++i) { - modes[i] = leg_commands_[i].suspension_mode; - torques[i] = leg_commands_[i].suspension_torque; - } - } - - void reset_state_() { - suspension_active_.fill(false); - for (size_t i = 0; i < kJointCount; ++i) { - leg_states_[i] = LegControlState{}; - leg_commands_[i] = LegCommand{}; - } - } - - void prepare_commands_() { - for (size_t i = 0; i < kJointCount; ++i) { - leg_commands_[i] = LegCommand{ - .requested_target_angle = requested_target_angles_[i], - .final_target_angle = target_angles_[i], - }; - leg_states_[i].output_active = false; - leg_states_[i].support_force = 0.0; - } - } - - static LegFeedback leg_feedback_at_(const JointFeedbackFrame& fb, size_t i) { - return { - .motor_angle = fb.motor_angles[i], - .physical_angle = fb.physical_angles[i], - .physical_velocity = fb.physical_velocities[i], - .joint_torque = fb.joint_torques[i], - .eso_z2 = fb.eso_z2[i], - .eso_z3 = fb.eso_z3[i]}; - } - - double estimate_contact_(const LegFeedback& lf) const { - double c = 1.0; - if (std::isfinite(lf.eso_z3)) - c -= std::clamp(std::abs(lf.eso_z3) / 80.0, 0.0, 0.5); - if (std::isfinite(lf.joint_torque)) - c += std::clamp(std::abs(lf.joint_torque) / 20.0, 0.0, 0.3); - if (std::isfinite(lf.physical_velocity)) - c -= std::clamp(std::abs(lf.physical_velocity) / 10.0, 0.0, 0.2); - return std::clamp(c, 0.0, 1.0); - } - - bool contact_ready_(const LegControlState& s) const { - return s.contact_latched - || s.filtered_contact_confidence >= kContactConfidenceEnterThreshold; - } - - void update_leg_contact_estimates_(const JointFeedbackFrame& fb) { - for (size_t i = 0; i < kJointCount; ++i) { - auto& s = leg_states_[i]; - s.contact_confidence = estimate_contact_(leg_feedback_at_(fb, i)); - s.filtered_contact_confidence = std::clamp( - (1.0 - kContactConfidenceFilterAlpha) * s.filtered_contact_confidence - + kContactConfidenceFilterAlpha * s.contact_confidence, - 0.0, 1.0); - if (s.filtered_contact_confidence >= kContactConfidenceEnterThreshold) - s.contact_latched = true; - else if (s.filtered_contact_confidence <= kContactConfidenceExitThreshold) - s.contact_latched = false; - } - } - - void update_leg_states_(const JointFeedbackFrame& fb, double entry, double release, double dt) { - for (size_t i = 0; i < kJointCount; ++i) { - auto lf = leg_feedback_at_(fb, i); - bool rd = - std::isfinite(requested_target_angles_[i]) && requested_target_angles_[i] <= entry; - update_suspension_state_(i, lf, rd, entry, release, dt); - } - } - - void compute_leg_support_intents_( - const JointFeedbackFrame& fb, const AttitudeBias& bias, double support_zero_angle, - double ride_height, const Eigen::Vector2d& control_accel) { - for (size_t i = 0; i < kJointCount; ++i) { - auto lf = leg_feedback_at_(fb, i); - auto& s = leg_states_[i]; - if (s.phase != SuspensionPhase::kActive) - continue; - s.support_force = - compute_leg_support_force_(i, lf, bias, support_zero_angle, control_accel); - leg_commands_[i].final_target_angle = ride_height; - leg_commands_[i].suspension_mode = true; - leg_commands_[i].suspension_torque = - leg_force_to_torque_(s.support_force, lf.physical_angle); - } - } - - AttitudeBias compute_attitude_bias_( - double pitch, double roll, double pitch_rate, double roll_rate, double dt) { - double corrected_pitch = - std::clamp(pitch - imu_pitch_offset_, -kMaxAttitudeRad, kMaxAttitudeRad); - double corrected_roll = - std::clamp(roll - imu_roll_offset_, -kMaxAttitudeRad, kMaxAttitudeRad); - double pitch_force = pitch_pid_.update(-corrected_pitch, pitch_rate, dt); - double roll_force = roll_pid_.update(corrected_roll, -roll_rate, dt); - if (!std::isfinite(pitch_force) || !std::isfinite(roll_force)) - return {kNaN, kNaN}; - return {pitch_force, roll_force}; - } - - double compute_leg_support_force_( - size_t i, const LegFeedback& lf, const AttitudeBias& bias, double support_zero_angle, - const Eigen::Vector2d& control_accel) const { - double f = params_.gravity_comp_gain * params_.mass * kGravity / 4.0 - + params_.Kz * (lf.physical_angle - support_zero_angle) - + params_.D_leg * lf.physical_velocity; - f += kPitchSigns[i] * bias.pitch_force + kRollSigns[i] * bias.roll_force; - if (params_.com_height > 0.0 && params_.wheel_base_half_x > 1e-6 - && params_.wheel_base_half_y > 1e-6) { - f += kPitchSigns[i] * params_.mass * control_accel.x() * params_.com_height - / (4.0 * params_.wheel_base_half_x); - f += kRollSigns[i] * params_.mass * control_accel.y() * params_.com_height - / (4.0 * params_.wheel_base_half_y); - } - return std::max(f, 0.0); - } - - double leg_force_to_torque_(double force, double angle) const { - return std::clamp( - force * params_.rod_length * std::max(std::sin(angle), kMinForceArmSin), - -params_.torque_limit, params_.torque_limit); - } - - void update_suspension_state_( - size_t i, const LegFeedback& lf, bool rd, double entry, double release, double dt) { - auto& s = leg_states_[i]; - s.phase_elapsed += (s.requested_deploy != rd) ? 0.0 : dt; - s.requested_deploy = rd; - if (!rd || !std::isfinite(lf.physical_angle) || !std::isfinite(lf.physical_velocity)) { - s.phase = SuspensionPhase::kInactive; - suspension_active_[i] = false; - s.output_active = false; - s.phase_elapsed = 0.0; - s.contact_latched = false; - return; - } - bool eok = lf.physical_angle <= entry; - bool vok = std::abs(lf.physical_velocity) <= params_.activation_velocity_threshold; - bool cok = contact_ready_(s); - switch (s.phase) { - case SuspensionPhase::kInactive: - suspension_active_[i] = false; - s.output_active = false; - if (eok) { - s.phase = SuspensionPhase::kArming; - s.phase_elapsed = 0.0; - } - break; - case SuspensionPhase::kArming: - suspension_active_[i] = false; - s.output_active = false; - if (!eok) { - s.phase = SuspensionPhase::kInactive; - s.phase_elapsed = 0.0; - break; - } - if (vok && std::isfinite(lf.motor_angle) - && (cok || s.phase_elapsed >= kMinimumArmingTime)) { - suspension_active_[i] = true; - s.phase = SuspensionPhase::kActive; - s.output_active = true; - s.phase_elapsed = 0.0; - } - break; - case SuspensionPhase::kActive: - if (lf.physical_angle > release || (!cok && s.phase_elapsed >= kMinimumArmingTime)) { - suspension_active_[i] = false; - s.phase = SuspensionPhase::kReleasing; - s.output_active = false; - s.phase_elapsed = 0.0; - break; - } - suspension_active_[i] = true; - s.output_active = true; - break; - case SuspensionPhase::kReleasing: - suspension_active_[i] = false; - s.output_active = false; - if (!eok || s.phase_elapsed >= kMinimumArmingTime) { - s.phase = SuspensionPhase::kInactive; - s.phase_elapsed = 0.0; - } - break; - } - } -}; - -// ============================================================ -// DeformableChassis — thin orchestrator -// ============================================================ class DeformableChassis : public rmcs_executor::Component , public rclcpp::Node { public: - DeformableChassis() + explicit DeformableChassis() : Node( get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { - - const double spin_ratio = std::clamp(get_parameter_or("spin_ratio", 0.6), 0.0, 1.0); - const double min_angle = get_parameter_or("min_angle", 15.0); - const double max_angle = get_parameter_or("max_angle", 55.0); - const double vel_limit = std::max( - std::abs(get_parameter_or("target_physical_velocity_limit", 180.0)) * std::numbers::pi - / 180.0, - 1e-6); - const double accel_limit = std::max( - std::abs(get_parameter_or("target_physical_acceleration_limit", 720.0)) - * std::numbers::pi / 180.0, - 1e-6); - - velocity_control_.configure(spin_ratio); - suspension_.load_params(*this, min_angle, max_angle); - trajectory_.init(min_angle, max_angle, vel_limit, accel_limit); + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , following_velocity_controller_(10.0, 0.0, 0.0) + , spin_ratio_(std::clamp(get_parameter_or("spin_ratio", 0.6), 0.0, 1.0)) + , joint_mode_mgr_(*this) { - for (size_t i = 0; i < kJointCount; ++i) - joint_offsets_[i] = - get_parameter_or(std::string(kJointNames[i]) + "_joint_offset", 0.0); + following_velocity_controller_.output_max = angular_velocity_max_; + following_velocity_controller_.output_min = -angular_velocity_max_; register_input("/remote/joystick/right", joystick_right_); - register_input("/remote/joystick/left", joystick_left_); register_input("/remote/switch/right", switch_right_); register_input("/remote/switch/left", switch_left_); - register_input("/remote/mouse/velocity", mouse_velocity_); - register_input("/remote/mouse", mouse_); register_input("/remote/keyboard", keyboard_); register_input("/remote/rotary_knob", rotary_knob_); register_input("/predefined/update_rate", update_rate_); + register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); - auto reg_joint_input = [this](size_t i) { - const auto& name = kJointNames[i]; - auto& j = joints_[i]; - register_input(joint_path_(name, "angle"), j.angle, false); - register_input(joint_path_(name, "physical_angle"), j.physical_angle, false); - register_input(joint_path_(name, "physical_velocity"), j.physical_velocity, false); - register_input(joint_path_(name, "torque"), j.torque, false); - register_input(joint_path_(name, "encoder_angle"), j.encoder_angle, false); - register_input(joint_path_(name, "eso_z2"), j.eso_z2, false); - register_input(joint_path_(name, "eso_z3"), j.eso_z3, false); - }; - for (size_t i = 0; i < kJointCount; ++i) - reg_joint_input(i); - - register_input("/chassis/imu/pitch", chassis_imu_pitch_, false); - register_input("/chassis/imu/roll", chassis_imu_roll_, false); - register_input("/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, false); - register_input("/chassis/imu/roll_rate", chassis_imu_roll_rate_, false); - - register_output("/gimbal/scope/control_torque", scope_motor_control_torque_, kNaN); - register_output("/chassis/angle", chassis_angle_, kNaN); - register_output("/chassis/control_angle", chassis_control_angle_, kNaN); + register_output("/chassis/angle", chassis_angle_, nan_); + register_output("/chassis/control_angle", chassis_control_angle_, nan_); register_output("/chassis/control_mode", mode_); register_output("/chassis/control_velocity", chassis_control_velocity_); - - auto reg_joint_output = [this](size_t i) { - const auto& name = kJointNames[i]; - auto& j = joints_[i]; - register_output(joint_path_(name, "control_angle_error"), angle_errors_[i], kNaN); - register_output(joint_path_(name, "target_angle"), j.target_angle, kNaN); - register_output( - joint_path_(name, "target_physical_angle"), j.target_physical_angle, kNaN); - register_output( - joint_path_(name, "target_physical_velocity"), j.target_physical_velocity, kNaN); - register_output( - joint_path_(name, "target_physical_acceleration"), j.target_physical_acceleration, - kNaN); - register_output(joint_path_(name, "suspension_torque"), j.suspension_torque, kNaN); - }; + register_output("/chassis/pitch_lock_active", pitch_lock_active_, false); + register_output("/chassis/active_suspension/active", active_suspension_active_, false); + register_output("/chassis/deformable/low_prone_active", low_prone_active_, false); + register_output( + "/chassis/deformable/symmetric_posture_target", symmetric_posture_target_, true); + register_output("/chassis/deformable/correction_inverted", correction_inverted_, false); + register_output( + "/chassis/deformable/min_angle_deg", min_angle_deg_, joint_mode_mgr_.min_angle()); + register_output( + "/chassis/deformable/max_angle_deg", max_angle_deg_, joint_mode_mgr_.max_angle()); + register_output( + "/chassis/deformable/suspension_reference_angle_deg", suspension_reference_angle_deg_, + joint_mode_mgr_.suspension_reference_angle_deg()); + register_output( + "/chassis/deformable/reset_count", deformable_reset_count_, static_cast(0)); for (size_t i = 0; i < kJointCount; ++i) { - reg_joint_output(i); register_output( - joint_path_(kJointNames[i], "suspension_mode"), suspension_modes_[i], false); + fmt::format("/chassis/deformable/{}_joint/posture_target_angle", kJointName[i]), + joint_posture_target_angle_rad_[i], deg_to_rad(joint_mode_mgr_.max_angle())); } - register_output("/chassis/processed_encoder/angle", processed_encoder_angle_, kNaN); *mode_ = rmcs_msgs::ChassisMode::AUTO; - chassis_control_velocity_->vector << kNaN, kNaN, kNaN; - - int offset_count = 0; - for (size_t i = 0; i < kJointCount; ++i) - if (has_parameter(std::string(kJointNames[i]) + "_joint_offset")) - ++offset_count; - if (offset_count > 0 && offset_count != static_cast(kJointCount)) - throw std::runtime_error( - "joint offsets must be configured for all four joints or removed entirely"); - joint_feedback_source_ = (offset_count == static_cast(kJointCount)) - ? JointFeedbackSource::kLegacyEncoderAngle - : JointFeedbackSource::kMotorAngle; + *pitch_lock_active_ = false; + *active_suspension_active_ = false; + *low_prone_active_ = false; + *symmetric_posture_target_ = true; + *correction_inverted_ = false; + chassis_control_velocity_->vector << nan_, nan_, nan_; } void before_updating() override { - auto ensure = [this](auto& field, double value, const char* name) { - if (!field.ready()) { - field.make_and_bind_directly(value); - RCLCPP_WARN(get_logger(), "Failed to fetch \"%s\". Set to %.1f.", name, value); - } - }; - ensure(gimbal_yaw_angle_, 0.0, "/gimbal/yaw/angle"); - ensure(gimbal_yaw_angle_error_, 0.0, "/gimbal/yaw/control_angle_error"); - for (auto& j : joints_) - ensure(j.torque, 0.0, "joint torque"); - ensure(chassis_imu_pitch_, 0.0, "chassis imu pitch"); - ensure(chassis_imu_roll_, 0.0, "chassis imu roll"); - ensure(chassis_imu_pitch_rate_, 0.0, "chassis imu pitch_rate"); - ensure(chassis_imu_roll_rate_, 0.0, "chassis imu roll_rate"); - validate_joint_feedback_inputs_(); + if (!gimbal_yaw_angle_.ready()) { + gimbal_yaw_angle_.make_and_bind_directly(0.0); + RCLCPP_WARN(get_logger(), "Failed to fetch \"/gimbal/yaw/angle\". Set to 0.0."); + } + if (!gimbal_yaw_angle_error_.ready()) { + gimbal_yaw_angle_error_.make_and_bind_directly(0.0); + RCLCPP_WARN( + get_logger(), "Failed to fetch \"/gimbal/yaw/control_angle_error\". " + "Set to 0.0."); + } } void update() override { using rmcs_msgs::Switch; + const auto switch_right = *switch_right_; const auto switch_left = *switch_left_; const auto keyboard = *keyboard_; + do { if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { - reset_all_controls_(); + reset_all_controls(); break; } - update_mode_from_inputs_(switch_left, switch_right, keyboard); - update_velocity_control_(keyboard); - update_lift_target_toggle_(keyboard); - run_joint_intent_pipeline_(); - } while (false); - last_switch_right_ = switch_right; - last_switch_left_ = switch_left; - last_keyboard_ = keyboard; - } -private: - static constexpr double kNaN = std::numeric_limits::quiet_NaN(); - static constexpr double kRadToDeg = 180.0 / std::numbers::pi; + double rotary_knob = rotary_knob_.ready() ? *rotary_knob_ : 0.0; - static std::string joint_path_(const char* name, const char* suffix) { - char b[128]; - std::snprintf(b, sizeof(b), "/chassis/%s_joint/%s", name, suffix); - return {b}; - } + joint_mode_mgr_.update(switch_left, switch_right, keyboard, rotary_knob, update_dt()); - void validate_joint_feedback_inputs_() const { - bool ok = true; - for (const auto& j : joints_) { - if (joint_feedback_source_ == JointFeedbackSource::kMotorAngle) { - if (!j.angle.ready()) - ok = false; - } else { - if (!j.encoder_angle.ready()) - ok = false; - } - } - if (ok) - return; - throw std::runtime_error( - joint_feedback_source_ == JointFeedbackSource::kMotorAngle - ? "missing V2 joint feedback inputs: /chassis/*_joint/angle" - : "missing legacy joint feedback inputs: /chassis/*_joint/encoder_angle"); - } + *mode_ = joint_mode_mgr_.mode(); + *pitch_lock_active_ = joint_mode_mgr_.pitch_lock_active(); + *active_suspension_active_ = joint_mode_mgr_.suspension_active(); + *low_prone_active_ = joint_mode_mgr_.low_prone_active(); + *symmetric_posture_target_ = joint_mode_mgr_.symmetric_posture_target(); + *correction_inverted_ = joint_mode_mgr_.correction_inverted(); + *min_angle_deg_ = joint_mode_mgr_.min_angle(); + *max_angle_deg_ = joint_mode_mgr_.max_angle(); + *suspension_reference_angle_deg_ = joint_mode_mgr_.suspension_reference_angle_deg(); + publish_joint_posture_targets_(); - // --- helpers --- - bool suspension_requested_() const { - return suspension_.enabled() - && (keyboard_->ctrl - || (switch_left_.ready() && switch_right_.ready() - && *switch_left_ == rmcs_msgs::Switch::DOWN - && *switch_right_ == rmcs_msgs::Switch::UP)); + update_velocity_control(); + } while (false); } - // --- mode --- - void update_mode_from_inputs_( - rmcs_msgs::Switch sl, rmcs_msgs::Switch sr, const rmcs_msgs::Keyboard& kb) { - auto m = *mode_; - if (sl == rmcs_msgs::Switch::DOWN) - return; - if (last_switch_right_ == rmcs_msgs::Switch::MIDDLE && sr == rmcs_msgs::Switch::DOWN) { - m = (m == rmcs_msgs::ChassisMode::SPIN) ? rmcs_msgs::ChassisMode::STEP_DOWN - : rmcs_msgs::ChassisMode::SPIN; - } else if (!last_keyboard_.c && kb.c) { - m = (m == rmcs_msgs::ChassisMode::SPIN) ? rmcs_msgs::ChassisMode::AUTO - : rmcs_msgs::ChassisMode::SPIN; - } else if (!last_keyboard_.x && kb.x) { - m = (m == rmcs_msgs::ChassisMode::LAUNCH_RAMP) ? rmcs_msgs::ChassisMode::AUTO - : rmcs_msgs::ChassisMode::LAUNCH_RAMP; - } else if (!last_keyboard_.z && kb.z) { - m = (m == rmcs_msgs::ChassisMode::STEP_DOWN) ? rmcs_msgs::ChassisMode::AUTO - : rmcs_msgs::ChassisMode::STEP_DOWN; - } - *mode_ = m; - } +private: + static constexpr size_t kJointCount = 4; + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double translational_velocity_max_ = 10.0; + static constexpr double angular_velocity_max_ = 30.0; + static constexpr double default_dt_ = 1e-3; - // --- velocity --- - void update_velocity_control_(const rmcs_msgs::Keyboard& kb) { - auto tv = velocity_control_.compute_translational(*joystick_right_, kb, *gimbal_yaw_angle_); - bool toggle = (last_keyboard_.c != kb.c && *mode_ != rmcs_msgs::ChassisMode::SPIN); - auto ar = velocity_control_.compute_angular( - *mode_, *gimbal_yaw_angle_, *gimbal_yaw_angle_error_, toggle); - double dt = update_dt_(); - velocity_control_.update_acceleration_estimate(tv, dt, suspension_.control_accel_limit()); - *chassis_angle_ = ar.chassis_angle; - *chassis_control_angle_ = ar.chassis_control_angle; - chassis_control_velocity_->vector << tv, ar.angular_velocity; - } + void reset_all_controls() { + joint_mode_mgr_.reset(); + *deformable_reset_count_ += 1; - double update_dt_() const { + *mode_ = rmcs_msgs::ChassisMode::AUTO; + *pitch_lock_active_ = false; + *active_suspension_active_ = false; + *low_prone_active_ = false; + *symmetric_posture_target_ = true; + *correction_inverted_ = false; + *min_angle_deg_ = joint_mode_mgr_.min_angle(); + *max_angle_deg_ = joint_mode_mgr_.max_angle(); + *suspension_reference_angle_deg_ = joint_mode_mgr_.suspension_reference_angle_deg(); + publish_joint_posture_targets_(); + + chassis_control_velocity_->vector << nan_, nan_, nan_; + *chassis_angle_ = nan_; + *chassis_control_angle_ = nan_; + } + + double update_dt() const { if (update_rate_.ready() && std::isfinite(*update_rate_) && *update_rate_ > 1e-6) return 1.0 / *update_rate_; - return 1e-3; + return default_dt_; } - // --- lift toggle --- - void update_lift_target_toggle_(rmcs_msgs::Keyboard keyboard) { - constexpr double kRotaryKnobEdgeThreshold = 0.7; + void update_velocity_control() { + Eigen::Vector2d translational_velocity = update_translational_velocity_control(); + double angular_velocity = update_angular_velocity_control(); + chassis_control_velocity_->vector << translational_velocity, angular_velocity; + } - const bool keyboard_toggle = !last_keyboard_.q && keyboard.q; - const bool rotary_knob_toggle = last_rotary_knob_ < kRotaryKnobEdgeThreshold - && *rotary_knob_ >= kRotaryKnobEdgeThreshold; + Eigen::Vector2d update_translational_velocity_control() { + const auto keyboard = *keyboard_; + Eigen::Vector2d keyboard_move{keyboard.w - keyboard.s, keyboard.a - keyboard.d}; - if (apply_symmetric_target_) - trajectory_.fill_symmetric_targets(); + Eigen::Vector2d translational_velocity = + Eigen::Rotation2Dd{*gimbal_yaw_angle_} * (*joystick_right_ + keyboard_move); - if (rotary_knob_toggle || keyboard_toggle) { - trajectory_.set_target_angle( - std::abs(trajectory_.target_angle() - trajectory_.max_angle()) < 1e-6 - ? trajectory_.min_angle() - : trajectory_.max_angle()); - apply_symmetric_target_ = true; - } + if (translational_velocity.norm() > 1.0) + translational_velocity.normalize(); - last_rotary_knob_ = *rotary_knob_; + translational_velocity *= translational_velocity_max_; + return translational_velocity; } - // --- reset --- - void reset_all_controls_() { - *mode_ = rmcs_msgs::ChassisMode::AUTO; - velocity_control_.reset_acceleration_estimate(); - suspension_.reset_all(); - chassis_control_velocity_->vector << kNaN, kNaN, kNaN; - *chassis_angle_ = kNaN; - *chassis_control_angle_ = kNaN; - trajectory_.reset(trajectory_.max_angle()); - *scope_motor_control_torque_ = kNaN; - for (auto& e : angle_errors_) - *e = kNaN; - for (auto& j : joints_) { - *j.target_angle = kNaN; - *j.target_physical_angle = kNaN; - *j.target_physical_velocity = kNaN; - *j.target_physical_acceleration = kNaN; - *j.suspension_torque = kNaN; - } - for (auto& m : suspension_modes_) - *m = false; - *processed_encoder_angle_ = kNaN; - } + double update_angular_velocity_control() { + double angular_velocity = 0.0; + double chassis_control_angle = nan_; - // --- feedback --- - JointFeedbackFrame read_joint_feedback_() const { - JointFeedbackFrame f; - f.motor_angles.fill(kNaN); - f.physical_angles.fill(kNaN); - f.physical_velocities.fill(kNaN); - f.joint_torques.fill(kNaN); - f.eso_z2.fill(kNaN); - f.eso_z3.fill(kNaN); - for (size_t i = 0; i < kJointCount; ++i) { - const auto& j = joints_[i]; - if (j.angle.ready() && std::isfinite(*j.angle)) { - f.motor_angles[i] = *j.angle; - f.physical_angles[i] = 1.090830782496456 - f.motor_angles[i]; - } - if (j.physical_angle.ready() && std::isfinite(*j.physical_angle)) - f.physical_angles[i] = *j.physical_angle; - if (j.physical_velocity.ready() && std::isfinite(*j.physical_velocity)) - f.physical_velocities[i] = *j.physical_velocity; - if (j.torque.ready() && std::isfinite(*j.torque)) - f.joint_torques[i] = *j.torque; - if (j.eso_z2.ready() && std::isfinite(*j.eso_z2)) - f.eso_z2[i] = *j.eso_z2; - if (j.eso_z3.ready() && std::isfinite(*j.eso_z3)) - f.eso_z3[i] = *j.eso_z3; - } - return f; - } + switch (*mode_) { + case rmcs_msgs::ChassisMode::AUTO: break; - // --- main pipeline --- - void run_joint_intent_pipeline_() { - auto feedback = read_joint_feedback_(); + case rmcs_msgs::ChassisMode::SPIN_FAST: { + bool forward = joint_mode_mgr_.spinning_forward(); + angular_velocity = + spin_ratio_ * (forward ? angular_velocity_max_ : -angular_velocity_max_); + angular_velocity = + std::clamp(angular_velocity, -angular_velocity_max_, angular_velocity_max_); + } break; + + case rmcs_msgs::ChassisMode::STEP_DOWN: { + double chassis_angle_error = + calculate_unsigned_chassis_angle_error(chassis_control_angle); + + constexpr double alignment = std::numbers::pi; + while (chassis_angle_error > alignment / 2) { + chassis_control_angle -= alignment; + if (chassis_control_angle < 0) + chassis_control_angle += 2 * std::numbers::pi; + chassis_angle_error -= alignment; + } - suspension_.update_imu_calibration( - trajectory_.symmetric_requested(), *chassis_imu_pitch_, *chassis_imu_roll_, - update_dt_()); + angular_velocity = following_velocity_controller_.update(chassis_angle_error); + } break; - if (!trajectory_.active() - && !trajectory_.initialize_from_feedback( - feedback.motor_angles, feedback.physical_angles)) { - reset_all_controls_(); - return; + default: break; } - if (apply_symmetric_target_) - trajectory_.fill_symmetric_targets(); - trajectory_.refresh_deploy_targets( - suspension_requested_(), keyboard_->ctrl, trajectory_.min_angle()); + *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; + *chassis_control_angle_ = chassis_control_angle; - scope_motor_control_(keyboard_->ctrl); + return angular_velocity; + } - auto target_physical = trajectory_.current_physical(); - std::array sus_modes, sus_torques; - sus_modes.fill(false); - sus_torques.fill(kNaN); + double calculate_unsigned_chassis_angle_error(double& chassis_control_angle) { + chassis_control_angle = *gimbal_yaw_angle_error_; + if (chassis_control_angle < 0) + chassis_control_angle += 2 * std::numbers::pi; - suspension_.update( - feedback, *chassis_imu_pitch_, *chassis_imu_roll_, *chassis_imu_pitch_rate_, - *chassis_imu_roll_rate_, update_dt_(), suspension_requested_(), trajectory_.min_angle(), - trajectory_.max_angle(), velocity_control_.control_acceleration_estimate(), - target_physical, sus_modes, sus_torques); + double unsigned_angle_error = chassis_control_angle + *gimbal_yaw_angle_; + if (unsigned_angle_error >= 2 * std::numbers::pi) + unsigned_angle_error -= 2 * std::numbers::pi; - trajectory_.update_trajectory( - update_dt_(), suspension_requested_(), suspension_.target_vel_limit(), - suspension_.target_accel_limit()); + return unsigned_angle_error; + } - for (size_t i = 0; i < kJointCount; ++i) { - *joints_[i].target_angle = trajectory_.target_angles()[i]; - *joints_[i].target_physical_angle = trajectory_.target_physical_angles()[i]; - *joints_[i].target_physical_velocity = trajectory_.target_velocities()[i]; - *joints_[i].target_physical_acceleration = trajectory_.target_accelerations()[i]; - *joints_[i].suspension_torque = sus_torques[i]; - *suspension_modes_[i] = static_cast(sus_modes[i]); - } + static double deg_to_rad(double deg) { return deg * std::numbers::pi / 180.0; } - if (apply_symmetric_target_ && trajectory_.symmetric_requested()) - trajectory_.fill_symmetric_targets(); + void publish_joint_posture_targets_() { + std::array targets_deg{}; + joint_mode_mgr_.copy_joint_posture_target_deg(targets_deg); - double sum = 0.0; - int cnt = 0; - for (const auto& j : joints_) { - if (j.physical_angle.ready() && std::isfinite(*j.physical_angle)) { - sum += *j.physical_angle; - ++cnt; - } - } - *processed_encoder_angle_ = (cnt > 0) ? kRadToDeg * sum / cnt : kNaN; + for (size_t i = 0; i < kJointCount; ++i) + *joint_posture_target_angle_rad_[i] = deg_to_rad(targets_deg[i]); } - void scope_motor_control_(bool prone_override) { - if (prone_override && *mode_ != rmcs_msgs::ChassisMode::SPIN) - *scope_motor_control_torque_ = -0.3; - else - *scope_motor_control_torque_ = 0.3; - } + static constexpr const char* kJointName[] = { + "left_front", + "left_back", + "right_back", + "right_front", + }; - // --- member variables --- - InputInterface joystick_right_, joystick_left_; - InputInterface switch_right_, switch_left_; - InputInterface mouse_velocity_; - InputInterface mouse_; + InputInterface joystick_right_; + InputInterface switch_right_; + InputInterface switch_left_; InputInterface keyboard_; - InputInterface rotary_knob_, update_rate_; - double last_rotary_knob_ = 0.0; - rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + InputInterface rotary_knob_; + InputInterface update_rate_; InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; OutputInterface chassis_angle_, chassis_control_angle_; + OutputInterface mode_; OutputInterface chassis_control_velocity_; - - ChassisVelocityControl velocity_control_; - ActiveSuspension suspension_; - JointTrajectoryPlanner trajectory_; - bool apply_symmetric_target_ = true; - - std::array joints_{}; - std::array angle_errors_; - std::array, kJointCount> suspension_modes_; - InputInterface chassis_imu_pitch_, chassis_imu_roll_, chassis_imu_pitch_rate_, - chassis_imu_roll_rate_; - OutputInterface scope_motor_control_torque_, processed_encoder_angle_; - - std::array joint_offsets_{}; // FIXME: Unused Var - JointFeedbackSource joint_feedback_source_ = JointFeedbackSource::kLegacyEncoderAngle; + OutputInterface pitch_lock_active_; + OutputInterface active_suspension_active_; + OutputInterface low_prone_active_; + OutputInterface symmetric_posture_target_; + OutputInterface correction_inverted_; + OutputInterface min_angle_deg_; + OutputInterface max_angle_deg_; + OutputInterface suspension_reference_angle_deg_; + OutputInterface deformable_reset_count_; + std::array, kJointCount> joint_posture_target_angle_rad_; + + pid::PidCalculator following_velocity_controller_; + const double spin_ratio_; + + DeformableChassisModeManager joint_mode_mgr_; }; } // namespace rmcs_core::controller::chassis #include + PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::DeformableChassis, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp index a629449c3..88bfb2041 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp @@ -2,7 +2,6 @@ #include #include #include -#include #include #include @@ -33,31 +32,21 @@ class DeformableJointController : public rmcs_executor::Component , public rclcpp::Node { public: - // Joint controller owns only joint-local servo execution. Higher-level deploy/suspension - // intent is generated upstream by DeformableChassis via setpoint/mode/feedforward inputs. - struct ModeConfig { - rmcs_core::controller::adrc::TD::Config td; - rmcs_core::controller::adrc::ESO::Config eso; - rmcs_core::controller::adrc::NLESF::Config nlesf; - double output_min = -std::numeric_limits::infinity(); - double output_max = std::numeric_limits::infinity(); - double torque_feedforward_gain = 0.0; - }; - - struct InputSnapshot { - double measurement_angle = std::numeric_limits::quiet_NaN(); - double setpoint_angle = std::numeric_limits::quiet_NaN(); - double joint_torque_feedforward = 0.0; - bool suspension_mode = false; - }; - - DeformableJointController() + explicit DeformableJointController() : Node( get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { - register_interfaces_(); - load_mode_configs_(); - apply_mode_config_(normal_mode_config_); + register_input(get_parameter("measurement_angle").as_string(), measurement_angle_); + register_input(get_parameter("setpoint_angle").as_string(), setpoint_angle_); + if (has_parameter("setpoint_velocity")) { + register_input( + get_parameter("setpoint_velocity").as_string(), setpoint_velocity_, false); + use_setpoint_velocity_ = true; + } + register_output(get_parameter("control").as_string(), control_torque_, nan_); + + load_config_(); + apply_config_(); } void update() override { @@ -67,208 +56,88 @@ class DeformableJointController return; } - update_mode_selection_(inputs.suspension_mode); - const auto& mode_config = active_mode_config_(inputs.suspension_mode); - const auto [output_min, output_max] = effective_output_limits_(mode_config); - if (std::isnan(output_min) || std::isnan(output_max) || output_min > output_max) { - disable_output_(); - return; - } - - if (!ensure_feedforward_ready_(mode_config, inputs)) { - disable_output_(); - return; - } - initialize_if_needed_(inputs); - rmcs_core::controller::adrc::ESO::Output eso_out; double control_torque = nan_; - if (!run_joint_servo_( - inputs, mode_config, output_min, output_max, eso_out, control_torque)) { + if (!run_joint_servo_(inputs, control_torque)) { disable_output_(); return; } - publish_control_output_(control_torque, eso_out); + publish_control_output_(control_torque); } private: - void register_interfaces_() { - register_input(get_parameter("measurement_angle").as_string(), measurement_angle_); - register_input(get_parameter("measurement_velocity").as_string(), measurement_velocity_); - register_input(get_parameter("setpoint_angle").as_string(), setpoint_angle_); - - if (has_parameter("setpoint_velocity")) { - register_input( - get_parameter("setpoint_velocity").as_string(), setpoint_velocity_input_); - } else { - setpoint_velocity_input_.make_and_bind_directly(0.0); - } - - if (has_parameter("mode_input")) { - register_input(get_parameter("mode_input").as_string(), suspension_mode_input_); - } else { - suspension_mode_input_.make_and_bind_directly(false); - } - - if (has_parameter("suspension_torque")) { - register_input( - get_parameter("suspension_torque").as_string(), joint_torque_feedforward_input_); - } else { - joint_torque_feedforward_input_.make_and_bind_directly(0.0); - } - if (has_parameter("limit")) { - register_input(get_parameter("limit").as_string(), output_limit_); - use_dynamic_limit_ = true; - } + // Joint controller owns only the local angle-servo execution. Chassis publishes the + // higher-level target angle trajectory; this controller turns that target into motor torque. + struct ControllerConfig { + rmcs_core::controller::adrc::TD::Config td; + rmcs_core::controller::adrc::ESO::Config eso; + rmcs_core::controller::adrc::NLESF::Config nlesf; + double output_min = -std::numeric_limits::infinity(); + double output_max = std::numeric_limits::infinity(); + }; - register_output(get_parameter("control").as_string(), control_torque_, nan_); - if (has_parameter("eso_z2_output")) { - register_output(get_parameter("eso_z2_output").as_string(), eso_z2_output_, nan_); - } - if (has_parameter("eso_z3_output")) { - register_output(get_parameter("eso_z3_output").as_string(), eso_z3_output_, nan_); - } - } + struct InputSnapshot { + double measurement_angle = std::numeric_limits::quiet_NaN(); + double setpoint_angle = std::numeric_limits::quiet_NaN(); + double setpoint_velocity = std::numeric_limits::quiet_NaN(); + }; - void load_mode_configs_() { + void load_config_() { dt_ = load_parameter_or(*this, "dt", 0.001); b0_ = load_parameter_or(*this, "b0", 1.0); kt_ = load_parameter_or(*this, "kt", 1.0); - normal_mode_config_.td.h = load_parameter_or(*this, "td_h", dt_); - normal_mode_config_.td.r = load_parameter_or(*this, "td_r", 300.0); - normal_mode_config_.td.max_vel = + config_.td.h = load_parameter_or(*this, "td_h", dt_); + config_.td.r = load_parameter_or(*this, "td_r", 300.0); + config_.td.max_vel = load_parameter_or(*this, "td_max_vel", std::numeric_limits::infinity()); - normal_mode_config_.td.max_acc = + config_.td.max_acc = load_parameter_or(*this, "td_max_acc", std::numeric_limits::infinity()); - normal_mode_config_.eso.h = dt_; - normal_mode_config_.eso.b0 = b0_; - normal_mode_config_.eso.w0 = load_parameter_or(*this, "eso_w0", 80.0); - normal_mode_config_.eso.auto_beta = load_parameter_or(*this, "eso_auto_beta", true); - normal_mode_config_.eso.beta1 = - load_parameter_or(*this, "eso_beta1", 3.0 * normal_mode_config_.eso.w0); - normal_mode_config_.eso.beta2 = load_parameter_or( - *this, "eso_beta2", 3.0 * normal_mode_config_.eso.w0 * normal_mode_config_.eso.w0); - normal_mode_config_.eso.beta3 = load_parameter_or( - *this, "eso_beta3", - normal_mode_config_.eso.w0 * normal_mode_config_.eso.w0 * normal_mode_config_.eso.w0); - normal_mode_config_.eso.z3_limit = load_parameter_or(*this, "eso_z3_limit", 1e9); - - normal_mode_config_.nlesf.k1 = load_parameter_or(*this, "k1", 50.0); - normal_mode_config_.nlesf.k2 = load_parameter_or(*this, "k2", 5.0); - normal_mode_config_.nlesf.alpha1 = load_parameter_or(*this, "alpha1", 0.75); - normal_mode_config_.nlesf.alpha2 = load_parameter_or(*this, "alpha2", 1.25); - normal_mode_config_.nlesf.delta = load_parameter_or(*this, "delta", 0.01); - normal_mode_config_.nlesf.u_min = + config_.eso.h = dt_; + config_.eso.b0 = b0_; + config_.eso.w0 = load_parameter_or(*this, "eso_w0", 80.0); + config_.eso.auto_beta = load_parameter_or(*this, "eso_auto_beta", true); + config_.eso.beta1 = load_parameter_or(*this, "eso_beta1", 3.0 * config_.eso.w0); + config_.eso.beta2 = + load_parameter_or(*this, "eso_beta2", 3.0 * config_.eso.w0 * config_.eso.w0); + config_.eso.beta3 = + load_parameter_or(*this, "eso_beta3", config_.eso.w0 * config_.eso.w0 * config_.eso.w0); + config_.eso.z3_limit = load_parameter_or(*this, "eso_z3_limit", 1e9); + + config_.nlesf.k1 = load_parameter_or(*this, "k1", 50.0); + config_.nlesf.k2 = load_parameter_or(*this, "k2", 5.0); + config_.nlesf.alpha1 = load_parameter_or(*this, "alpha1", 0.75); + config_.nlesf.alpha2 = load_parameter_or(*this, "alpha2", 1.25); + config_.nlesf.delta = load_parameter_or(*this, "delta", 0.01); + config_.nlesf.u_min = load_parameter_or(*this, "u_min", -std::numeric_limits::infinity()); - normal_mode_config_.nlesf.u_max = + config_.nlesf.u_max = load_parameter_or(*this, "u_max", std::numeric_limits::infinity()); - normal_mode_config_.output_min = + config_.output_min = load_parameter_or(*this, "output_min", -std::numeric_limits::infinity()); - normal_mode_config_.output_max = + config_.output_max = load_parameter_or(*this, "output_max", std::numeric_limits::infinity()); - if (normal_mode_config_.output_min > normal_mode_config_.output_max) { - std::swap(normal_mode_config_.output_min, normal_mode_config_.output_max); - } - normal_mode_config_.torque_feedforward_gain = - load_parameter_or(*this, "torque_feedforward_gain", 0.0); - - suspension_mode_config_ = normal_mode_config_; - suspension_mode_config_.td.h = - load_parameter_or(*this, "suspension_td_h", suspension_mode_config_.td.h); - suspension_mode_config_.td.r = - load_parameter_or(*this, "suspension_td_r", suspension_mode_config_.td.r); - suspension_mode_config_.td.max_vel = - load_parameter_or(*this, "suspension_td_max_vel", suspension_mode_config_.td.max_vel); - suspension_mode_config_.td.max_acc = - load_parameter_or(*this, "suspension_td_max_acc", suspension_mode_config_.td.max_acc); - suspension_mode_config_.eso.w0 = - load_parameter_or(*this, "suspension_eso_w0", suspension_mode_config_.eso.w0); - suspension_mode_config_.eso.auto_beta = load_parameter_or( - *this, "suspension_eso_auto_beta", suspension_mode_config_.eso.auto_beta); - suspension_mode_config_.eso.beta1 = - load_parameter_or(*this, "suspension_eso_beta1", suspension_mode_config_.eso.beta1); - suspension_mode_config_.eso.beta2 = - load_parameter_or(*this, "suspension_eso_beta2", suspension_mode_config_.eso.beta2); - suspension_mode_config_.eso.beta3 = - load_parameter_or(*this, "suspension_eso_beta3", suspension_mode_config_.eso.beta3); - suspension_mode_config_.eso.z3_limit = load_parameter_or( - *this, "suspension_eso_z3_limit", suspension_mode_config_.eso.z3_limit); - suspension_mode_config_.nlesf.k1 = - load_parameter_or(*this, "suspension_k1", suspension_mode_config_.nlesf.k1); - suspension_mode_config_.nlesf.k2 = - load_parameter_or(*this, "suspension_k2", suspension_mode_config_.nlesf.k2); - suspension_mode_config_.nlesf.alpha1 = - load_parameter_or(*this, "suspension_alpha1", suspension_mode_config_.nlesf.alpha1); - suspension_mode_config_.nlesf.alpha2 = - load_parameter_or(*this, "suspension_alpha2", suspension_mode_config_.nlesf.alpha2); - suspension_mode_config_.nlesf.delta = - load_parameter_or(*this, "suspension_delta", suspension_mode_config_.nlesf.delta); - suspension_mode_config_.nlesf.u_min = - load_parameter_or(*this, "suspension_u_min", suspension_mode_config_.nlesf.u_min); - suspension_mode_config_.nlesf.u_max = - load_parameter_or(*this, "suspension_u_max", suspension_mode_config_.nlesf.u_max); - suspension_mode_config_.output_min = - load_parameter_or(*this, "suspension_output_min", normal_mode_config_.output_min); - suspension_mode_config_.output_max = - load_parameter_or(*this, "suspension_output_max", normal_mode_config_.output_max); - if (suspension_mode_config_.output_min > suspension_mode_config_.output_max) { - std::swap(suspension_mode_config_.output_min, suspension_mode_config_.output_max); + if (config_.output_min > config_.output_max) { + std::swap(config_.output_min, config_.output_max); } - suspension_mode_config_.torque_feedforward_gain = - load_parameter_or(*this, "suspension_torque_feedforward_gain", 1.0); } bool read_inputs_(InputSnapshot& inputs) const { inputs.measurement_angle = *measurement_angle_; inputs.setpoint_angle = *setpoint_angle_; - inputs.joint_torque_feedforward = *joint_torque_feedforward_input_; - inputs.suspension_mode = *suspension_mode_input_; - return std::isfinite(inputs.measurement_angle) && std::isfinite(inputs.setpoint_angle); - } - - void update_mode_selection_(bool suspension_mode) { - if (suspension_mode != last_suspension_mode_) { - apply_mode_config_(active_mode_config_(suspension_mode)); - last_suspension_mode_ = suspension_mode; + if (use_setpoint_velocity_ && setpoint_velocity_.ready() + && std::isfinite(*setpoint_velocity_)) { + inputs.setpoint_velocity = *setpoint_velocity_; } + return std::isfinite(inputs.measurement_angle) && std::isfinite(inputs.setpoint_angle); } - const ModeConfig& active_mode_config_(bool suspension_mode) const { - return suspension_mode ? suspension_mode_config_ : normal_mode_config_; - } - - void apply_mode_config_(const ModeConfig& mode_config) { - td_.set_config(mode_config.td); - eso_.set_config(mode_config.eso); - nlesf_.set_config(mode_config.nlesf); - } - - std::pair effective_output_limits_(const ModeConfig& mode_config) const { - double output_min = mode_config.output_min; - double output_max = mode_config.output_max; - if (use_dynamic_limit_) { - if (!output_limit_.ready()) { - return {nan_, nan_}; - } - const double limit = *output_limit_; - if (!std::isfinite(limit)) { - return {nan_, nan_}; - } - - const double effective_limit = std::max(0.0, limit); - output_min = std::max(output_min, -effective_limit); - output_max = std::min(output_max, effective_limit); - } - return {output_min, output_max}; - } - - bool ensure_feedforward_ready_( - const ModeConfig& mode_config, const InputSnapshot& inputs) const { - return mode_config.torque_feedforward_gain == 0.0 - || std::isfinite(inputs.joint_torque_feedforward); + void apply_config_() { + td_.set_config(config_.td); + eso_.set_config(config_.eso); + nlesf_.set_config(config_.nlesf); } void initialize_if_needed_(const InputSnapshot& inputs) { @@ -279,21 +148,22 @@ class DeformableJointController initialized_ = true; } - bool run_joint_servo_( - const InputSnapshot& inputs, const ModeConfig& mode_config, double output_min, - double output_max, rmcs_core::controller::adrc::ESO::Output& eso_out, - double& control_torque) { - const auto td_out = td_.update(inputs.setpoint_angle); - eso_out = eso_.update(inputs.measurement_angle, last_u_); + bool run_joint_servo_(const InputSnapshot& inputs, double& control_torque) { + const auto eso_out = eso_.update(inputs.measurement_angle, last_u_); - const double e1 = td_out.x1 - eso_out.z1; - const double e2 = td_out.x2 - eso_out.z2; + double reference_angle = inputs.setpoint_angle; + double reference_velocity = inputs.setpoint_velocity; + if (!std::isfinite(reference_velocity)) { + const auto td_out = td_.update(inputs.setpoint_angle); + reference_angle = td_out.x1; + reference_velocity = td_out.x2; + } + + const double e1 = reference_angle - eso_out.z1; + const double e2 = reference_velocity - eso_out.z2; control_torque = kt_ * nlesf_.compute(e1, e2, eso_out.z3, b0_).u; - if (mode_config.torque_feedforward_gain != 0.0) { - control_torque += mode_config.torque_feedforward_gain * inputs.joint_torque_feedforward; - } - control_torque = std::clamp(control_torque, output_min, output_max); + control_torque = std::clamp(control_torque, config_.output_min, config_.output_max); return std::isfinite(control_torque); } @@ -303,63 +173,37 @@ class DeformableJointController last_u_ = 0.0; } - void publish_control_output_( - double control_torque, const rmcs_core::controller::adrc::ESO::Output& eso_out) { + void publish_control_output_(double control_torque) { *control_torque_ = control_torque; last_u_ = control_torque; - publish_eso_state_(eso_out); - } - - void publish_eso_state_(const rmcs_core::controller::adrc::ESO::Output& eso_out) { - if (eso_z2_output_.active()) { - *eso_z2_output_ = eso_out.z2; - } - if (eso_z3_output_.active()) { - *eso_z3_output_ = eso_out.z3; - } } void disable_output_() { initialized_ = false; last_u_ = 0.0; *control_torque_ = nan_; - if (eso_z2_output_.active()) { - *eso_z2_output_ = nan_; - } - if (eso_z3_output_.active()) { - *eso_z3_output_ = nan_; - } } static constexpr double nan_ = std::numeric_limits::quiet_NaN(); InputInterface measurement_angle_; - InputInterface measurement_velocity_; InputInterface setpoint_angle_; - // Kept for graph compatibility; TD derives the actual servo-rate target locally. - InputInterface setpoint_velocity_input_; - InputInterface suspension_mode_input_; - InputInterface joint_torque_feedforward_input_; - InputInterface output_limit_; + InputInterface setpoint_velocity_; OutputInterface control_torque_; - OutputInterface eso_z2_output_; - OutputInterface eso_z3_output_; rmcs_core::controller::adrc::TD td_; rmcs_core::controller::adrc::ESO eso_; rmcs_core::controller::adrc::NLESF nlesf_; - ModeConfig normal_mode_config_; - ModeConfig suspension_mode_config_; + ControllerConfig config_; double dt_ = 0.001; double b0_ = 1.0; double kt_ = 1.0; double last_u_ = 0.0; - bool use_dynamic_limit_ = false; + bool use_setpoint_velocity_ = false; bool initialized_ = false; - bool last_suspension_mode_ = false; }; } // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_layer.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_layer.hpp deleted file mode 100644 index b4ca1e2d5..000000000 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_layer.hpp +++ /dev/null @@ -1,176 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -#include - -namespace rmcs_core::controller::chassis { - -enum class JointFeedbackSource : uint8_t { kLegacyEncoderAngle, kMotorAngle }; - -enum JointIndex : size_t { - kLeftFront = 0, - kLeftBack = 1, - kRightBack = 2, - kRightFront = 3, - kJointCount = 4 -}; - -inline constexpr std::array kJointNames{ - "left_front", "left_back", "right_back", "right_front"}; - -struct JointFeedbackFrame { - std::array motor_angles{}; - std::array physical_angles{}; - std::array physical_velocities{}; - std::array joint_torques{}; - std::array eso_z2{}; - std::array eso_z3{}; -}; - -struct LegFeedback { - double motor_angle = std::numeric_limits::quiet_NaN(); - double physical_angle = std::numeric_limits::quiet_NaN(); - double physical_velocity = std::numeric_limits::quiet_NaN(); - double joint_torque = std::numeric_limits::quiet_NaN(); - double eso_z2 = std::numeric_limits::quiet_NaN(); - double eso_z3 = std::numeric_limits::quiet_NaN(); -}; - -struct JointIO { - using In = rmcs_executor::Component::InputInterface; - using Out = rmcs_executor::Component::OutputInterface; - In angle, physical_angle, physical_velocity, torque, encoder_angle, eso_z2, eso_z3; - Out target_angle, target_physical_angle, target_physical_velocity, target_physical_acceleration; - Out suspension_torque; -}; - -struct JointTrajectoryPlanner { - static constexpr double kJointZeroPhysicalAngleRad = 1.090830782496456; - - void - init(double min_angle, double max_angle, double velocity_limit, double acceleration_limit) { - min_angle_ = min_angle; - max_angle_ = max_angle; - velocity_limit_ = velocity_limit; - acceleration_limit_ = acceleration_limit; - } - - void set_target_angle(double angle) { current_target_angle_ = angle; } - double target_angle() const { return current_target_angle_; } - - bool initialize_from_feedback( - const std::array& motor_angles, - const std::array& physical_angles) { - for (size_t i = 0; i < kJointCount; ++i) - if (!std::isfinite(motor_angles[i]) || !std::isfinite(physical_angles[i])) - return false; - target_motor_state_ = motor_angles; - target_physical_state_ = physical_angles; - target_velocity_state_.fill(0.0); - target_acceleration_state_.fill(0.0); - requested_physical_ = physical_angles; - current_physical_ = physical_angles; - active_ = true; - return true; - } - - void sync_from_feedback(size_t index, double motor_angle, double physical_angle) { - target_motor_state_[index] = motor_angle; - target_physical_state_[index] = physical_angle; - target_velocity_state_[index] = 0.0; - target_acceleration_state_[index] = 0.0; - } - - bool active() const { return active_; } - void set_active(bool value) { active_ = value; } - - void fill_symmetric_targets() { per_joint_targets_.fill(current_target_angle_); } - - bool symmetric_requested() const { - for (size_t i = 1; i < kJointCount; ++i) - if (std::abs(per_joint_targets_[0] - per_joint_targets_[i]) > 1e-6) - return false; - return true; - } - - void refresh_deploy_targets( - bool deploy_requested, bool /*prone_override*/, double deploy_angle) { - for (size_t i = 0; i < kJointCount; ++i) - requested_physical_[i] = per_joint_targets_[i] * std::numbers::pi / 180.0; - if (deploy_requested) - requested_physical_.fill(deploy_angle * std::numbers::pi / 180.0); - current_physical_ = requested_physical_; - } - - void update_trajectory( - double delta_time, bool use_suspension_limits, double suspension_velocity_limit, - double suspension_acceleration_limit) { - double velocity_limit = use_suspension_limits ? suspension_velocity_limit : velocity_limit_; - double acceleration_limit = - use_suspension_limits ? suspension_acceleration_limit : acceleration_limit_; - for (size_t i = 0; i < kJointCount; ++i) { - double target = current_physical_[i]; - double current_position = target_physical_state_[i]; - double current_velocity = target_velocity_state_[i]; - double error = target - current_position; - double max_velocity = std::sqrt(2.0 * acceleration_limit * std::abs(error)); - double command_velocity = std::copysign(std::min(max_velocity, velocity_limit), error); - double delta_velocity = command_velocity - current_velocity; - double command_acceleration = - std::clamp(delta_velocity / delta_time, -acceleration_limit, acceleration_limit); - target_acceleration_state_[i] = command_acceleration; - target_velocity_state_[i] = std::clamp( - current_velocity + command_acceleration * delta_time, -velocity_limit, - velocity_limit); - target_physical_state_[i] += target_velocity_state_[i] * delta_time; - target_motor_state_[i] = kJointZeroPhysicalAngleRad - target_physical_state_[i]; - } - } - - const std::array& target_angles() const { return target_motor_state_; } - const std::array& target_physical_angles() const { - return target_physical_state_; - } - const std::array& target_velocities() const { - return target_velocity_state_; - } - const std::array& target_accelerations() const { - return target_acceleration_state_; - } - const std::array& current_physical() const { return current_physical_; } - - void reset(double angle) { - current_target_angle_ = angle; - per_joint_targets_.fill(angle); - active_ = false; - target_motor_state_.fill(0.0); - target_physical_state_.fill(0.0); - target_velocity_state_.fill(0.0); - target_acceleration_state_.fill(0.0); - } - - double min_angle() const { return min_angle_; } - double max_angle() const { return max_angle_; } - -private: - double min_angle_ = 15.0; - double max_angle_ = 55.0; - double velocity_limit_ = 1.0; - double acceleration_limit_ = 1.0; - double current_target_angle_ = 55.0; - std::array per_joint_targets_{55.0, 55.0, 55.0, 55.0}; - bool active_ = false; - std::array target_motor_state_{}; - std::array target_physical_state_{}; - std::array target_velocity_state_{}; - std::array target_acceleration_state_{}; - std::array requested_physical_{}; - std::array current_physical_{}; -}; - -} // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp new file mode 100644 index 000000000..dac11d423 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp @@ -0,0 +1,331 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace rmcs_core::controller::chassis { + +class DeformableChassisModeManager { +public: + enum class SuspensionMode : uint8_t { + OFF = 0, + ACTIVE = 1, + }; + + struct JointPostureState { + rmcs_msgs::ChassisMode mode = rmcs_msgs::ChassisMode::AUTO; + bool ctrl_low_prone_active = false; + bool low_prone_active = false; + bool pitch_lock_active = false; + bool suspension_active = false; + SuspensionMode suspension_mode = SuspensionMode::OFF; + bool symmetric_posture_target = true; + bool spinning_forward = true; + std::array joint_posture_target_deg = {58.0, 58.0, 58.0, 58.0}; + double suspension_reference_angle_deg = 58.0; + }; + + explicit DeformableChassisModeManager(rclcpp::Node& node) + : min_angle_(node.get_parameter_or("min_angle", 5.0)) + , max_angle_(node.get_parameter_or("max_angle", 59.0)) + , active_suspension_base_angle_( + std::clamp( + node.get_parameter_or("active_suspension_base_angle", max_angle_), + min_angle_ - 5.0, max_angle_)) + , suspension_enable_(node.get_parameter_or("active_suspension_enable", false)) { + current_target_angle_ = max_angle_; + joint_current_target_angle_.fill(max_angle_); + update_joint_posture_state_(false); + } + + void reset() { + joint_posture_state_.mode = rmcs_msgs::ChassisMode::AUTO; + joint_posture_state_.ctrl_low_prone_active = false; + joint_posture_state_.low_prone_active = false; + joint_posture_state_.pitch_lock_active = false; + joint_posture_state_.suspension_active = false; + joint_posture_state_.suspension_mode = SuspensionMode::OFF; + joint_posture_state_.symmetric_posture_target = true; + joint_posture_state_.spinning_forward = true; + joint_posture_state_.joint_posture_target_deg.fill(max_angle_); + joint_posture_state_.suspension_reference_angle_deg = max_angle_; + + current_target_angle_ = max_angle_; + active_suspension_base_angle_ = max_angle_; + joint_current_target_angle_.fill(max_angle_); + apply_symmetric_target_ = true; + suspension_enabled_by_toggle_ = false; + low_prone_enabled_by_toggle_ = false; + + last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; + last_keyboard_ = rmcs_msgs::Keyboard::zero(); + last_rotary_knob_ = 0.0; + + update_joint_posture_state_(false); + } + + void update( + rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, + const rmcs_msgs::Keyboard& keyboard, double rotary_knob, double dt) { + + update_mode_from_inputs_(switch_left, switch_right, keyboard); + update_low_prone_toggle_from_inputs_(switch_left, switch_right); + + joint_posture_state_.ctrl_low_prone_active = keyboard.ctrl; + joint_posture_state_.low_prone_active = + joint_posture_state_.ctrl_low_prone_active || low_prone_enabled_by_toggle_; + joint_posture_state_.pitch_lock_active = joint_posture_state_.ctrl_low_prone_active; + + update_suspension_mode_from_inputs_(switch_left, switch_right, keyboard, rotary_knob); + update_posture_target_from_inputs_(switch_left, switch_right, keyboard, rotary_knob, dt); + update_joint_posture_state_(joint_posture_state_.low_prone_active); + + last_switch_right_ = switch_right; + last_keyboard_ = keyboard; + } + + rmcs_msgs::ChassisMode mode() const { return joint_posture_state_.mode; } + bool pitch_lock_active() const { return joint_posture_state_.pitch_lock_active; } + bool suspension_active() const { return joint_posture_state_.suspension_active; } + bool low_prone_active() const { return joint_posture_state_.low_prone_active; } + bool symmetric_posture_target() const { return joint_posture_state_.symmetric_posture_target; } + bool spinning_forward() const { return joint_posture_state_.spinning_forward; } + double suspension_reference_angle_deg() const { + return joint_posture_state_.suspension_reference_angle_deg; + } + void copy_joint_posture_target_deg(std::array& out) const { + out = joint_posture_state_.joint_posture_target_deg; + } + + const JointPostureState& joint_posture_state() const { return joint_posture_state_; } + + double min_angle() const { return min_angle_; } + double max_angle() const { return max_angle_; } + double max_angle_rad() const { return deg_to_rad_(max_angle_); } + + double active_suspension_min_angle_rad() const { return deg_to_rad_(min_angle_ - 5.0); } + + bool correction_inverted() const { + double midpoint = (min_angle_ - 5.0 + max_angle_) / 2.0; + return joint_posture_state_.suspension_reference_angle_deg > midpoint; + } + +private: + static constexpr size_t kLeftFront = 0; + static constexpr size_t kLeftBack = 1; + static constexpr size_t kRightBack = 2; + static constexpr size_t kRightFront = 3; + static constexpr size_t kJointCount = 4; + + static double deg_to_rad_(double deg) { return deg * std::numbers::pi / 180.0; } + + static bool + symmetric_joint_target_requested_(const std::array& joint_target_deg) { + constexpr double epsilon = 1e-6; + return std::all_of(joint_target_deg.begin() + 1, joint_target_deg.end(), [&](double v) { + return std::abs(v - joint_target_deg.front()) <= epsilon; + }); + } + + void update_mode_from_inputs_( + rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, + const rmcs_msgs::Keyboard& keyboard) { + + auto next_mode = joint_posture_state_.mode; + if (switch_left == rmcs_msgs::Switch::DOWN) { + joint_posture_state_.mode = next_mode; + return; + } + + if (last_switch_right_ == rmcs_msgs::Switch::MIDDLE + && switch_right == rmcs_msgs::Switch::DOWN) { + if (next_mode == rmcs_msgs::ChassisMode::SPIN_FAST) { + next_mode = rmcs_msgs::ChassisMode::STEP_DOWN; + } else { + next_mode = rmcs_msgs::ChassisMode::SPIN_FAST; + joint_posture_state_.spinning_forward = !joint_posture_state_.spinning_forward; + } + } else if (!last_keyboard_.c && keyboard.c) { + if (next_mode == rmcs_msgs::ChassisMode::SPIN_FAST) { + next_mode = rmcs_msgs::ChassisMode::AUTO; + } else { + next_mode = rmcs_msgs::ChassisMode::SPIN_FAST; + joint_posture_state_.spinning_forward = !joint_posture_state_.spinning_forward; + } + } else if (!last_keyboard_.z && keyboard.z) { + next_mode = next_mode == rmcs_msgs::ChassisMode::STEP_DOWN + ? rmcs_msgs::ChassisMode::AUTO + : rmcs_msgs::ChassisMode::STEP_DOWN; + } + + joint_posture_state_.mode = next_mode; + } + + void apply_front_high_rear_low_target_() { + joint_current_target_angle_[kLeftFront] = max_angle_; + joint_current_target_angle_[kRightFront] = max_angle_; + joint_current_target_angle_[kLeftBack] = min_angle_; + joint_current_target_angle_[kRightBack] = min_angle_; + apply_symmetric_target_ = false; + } + + void apply_front_low_rear_high_target_() { + joint_current_target_angle_[kLeftFront] = min_angle_; + joint_current_target_angle_[kRightFront] = min_angle_; + joint_current_target_angle_[kLeftBack] = max_angle_; + joint_current_target_angle_[kRightBack] = max_angle_; + apply_symmetric_target_ = false; + } + + void toggle_front_back_posture_target_() { + if (joint_current_target_angle_[kLeftFront] > joint_current_target_angle_[kLeftBack]) + apply_front_low_rear_high_target_(); + else + apply_front_high_rear_low_target_(); + } + + void update_suspension_mode_from_inputs_( + rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, + const rmcs_msgs::Keyboard& keyboard, double rotary_knob) { + const bool remote_suspension_rotary_mode = + switch_left == rmcs_msgs::Switch::DOWN && switch_right == rmcs_msgs::Switch::MIDDLE; + const bool remote_active_toggle_requested = + remote_suspension_rotary_mode && rotary_knob_down_edge_(rotary_knob); + + const bool keyboard_active_suspension_toggle_requested = !last_keyboard_.e && keyboard.e; + if (keyboard_active_suspension_toggle_requested || remote_active_toggle_requested) + suspension_enabled_by_toggle_ = !suspension_enabled_by_toggle_; + + const bool active_requested = + suspension_enable_ + && (joint_posture_state_.low_prone_active || suspension_enabled_by_toggle_); + + joint_posture_state_.suspension_mode = SuspensionMode::OFF; + if (active_requested) + joint_posture_state_.suspension_mode = SuspensionMode::ACTIVE; + + joint_posture_state_.suspension_active = + joint_posture_state_.suspension_mode == SuspensionMode::ACTIVE; + } + + void update_low_prone_toggle_from_inputs_( + rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right) { + if (switch_left == rmcs_msgs::Switch::DOWN && switch_right == rmcs_msgs::Switch::UP + && last_switch_right_ == rmcs_msgs::Switch::MIDDLE) { + low_prone_enabled_by_toggle_ = !low_prone_enabled_by_toggle_; + } + } + + void update_posture_target_from_inputs_( + rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, + const rmcs_msgs::Keyboard& keyboard, double rotary_knob, double /*dt*/) { + const bool remote_joint_posture_rotary_mode = + switch_left == rmcs_msgs::Switch::MIDDLE && switch_right == rmcs_msgs::Switch::MIDDLE; + + const bool keyboard_posture_toggle_condition = !last_keyboard_.q && keyboard.q; + const bool remote_posture_toggle_condition = + remote_joint_posture_rotary_mode && rotary_knob_down_edge_(rotary_knob); + const bool remote_front_back_posture_toggle_condition = + remote_joint_posture_rotary_mode && rotary_knob_up_edge_(rotary_knob); + const bool front_high_rear_low = !last_keyboard_.b && keyboard.b; + const bool front_low_rear_high = !last_keyboard_.g && keyboard.g; + + if (apply_symmetric_target_) + joint_current_target_angle_.fill(current_target_angle_); + + const bool posture_toggle_requested = + remote_posture_toggle_condition || keyboard_posture_toggle_condition; + + if (posture_toggle_requested) { + if (joint_posture_state_.suspension_active) { + active_suspension_base_angle_ = + (std::abs(active_suspension_base_angle_ - max_angle_) < 1e-6) ? min_angle_ + : max_angle_; + current_target_angle_ = active_suspension_base_angle_; + apply_symmetric_target_ = true; + joint_current_target_angle_.fill(current_target_angle_); + } else { + current_target_angle_ = + (std::abs(current_target_angle_ - max_angle_) < 1e-6) ? min_angle_ : max_angle_; + apply_symmetric_target_ = true; + joint_current_target_angle_.fill(current_target_angle_); + } + } else if (remote_front_back_posture_toggle_condition) { + toggle_front_back_posture_target_(); + } else if (front_high_rear_low) { + apply_front_high_rear_low_target_(); + } else if (front_low_rear_high) { + apply_front_low_rear_high_target_(); + } + + last_rotary_knob_ = rotary_knob; + } + + bool rotary_knob_down_edge_(double rotary_knob) const { + constexpr double rotary_knob_edge_threshold = 0.7; + return last_rotary_knob_ < rotary_knob_edge_threshold + && rotary_knob >= rotary_knob_edge_threshold; + } + + bool rotary_knob_up_edge_(double rotary_knob) const { + constexpr double rotary_knob_edge_threshold = 0.7; + return last_rotary_knob_ > -rotary_knob_edge_threshold + && rotary_knob <= -rotary_knob_edge_threshold; + } + + void update_joint_posture_state_(bool low_prone_active) { + std::array effective_joint_posture_target_deg = + joint_current_target_angle_; + if (low_prone_active) + effective_joint_posture_target_deg.fill(min_angle_ - 5.0); + + joint_posture_state_.joint_posture_target_deg = effective_joint_posture_target_deg; + joint_posture_state_.symmetric_posture_target = + symmetric_joint_target_requested_(effective_joint_posture_target_deg); + + if (joint_posture_state_.suspension_active) { + joint_posture_state_.suspension_reference_angle_deg = + low_prone_active ? min_angle_ : active_suspension_base_angle_; + return; + } + + if (joint_posture_state_.symmetric_posture_target) { + joint_posture_state_.suspension_reference_angle_deg = + effective_joint_posture_target_deg.front(); + return; + } + + double posture_angle_sum = 0.0; + for (double angle_deg : effective_joint_posture_target_deg) + posture_angle_sum += angle_deg; + joint_posture_state_.suspension_reference_angle_deg = + posture_angle_sum / static_cast(kJointCount); + } + + JointPostureState joint_posture_state_; + + double min_angle_; + double max_angle_; + double active_suspension_base_angle_; + bool suspension_enable_; + + double current_target_angle_; + std::array joint_current_target_angle_; + bool apply_symmetric_target_ = true; + bool suspension_enabled_by_toggle_ = false; + bool low_prone_enabled_by_toggle_ = false; + + rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + double last_rotary_knob_ = 0.0; +}; + +} // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp index ec5922b8c..81b7b230d 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp @@ -1,11 +1,13 @@ #include #include +#include #include #include #include #include +#include #include #include #include @@ -41,22 +43,17 @@ class DeformableOmniWheelController register_input("/chassis/left_front_wheel/max_torque", wheel_motor_max_control_torque_); - register_input("/chassis/left_front_wheel/velocity", left_front_velocity_); - register_input("/chassis/left_back_wheel/velocity", left_back_velocity_); - register_input("/chassis/right_back_wheel/velocity", right_back_velocity_); - register_input("/chassis/right_front_wheel/velocity", right_front_velocity_); + for (size_t i = 0; i < kWheelCount; ++i) { + register_input( + fmt::format("/chassis/{}_wheel/velocity", kWheelName[i]), wheel_velocity_[i]); + register_output( + fmt::format("/chassis/{}_wheel/control_torque", kWheelName[i]), + wheel_control_torque_[i], nan_); + } register_input("/chassis/control_velocity", chassis_control_velocity_); register_input("/chassis/control_power_limit", power_limit_); register_input("/chassis/radius", chassis_radius_); - - register_output( - "/chassis/left_front_wheel/control_torque", left_front_control_torque_, nan_); - register_output("/chassis/left_back_wheel/control_torque", left_back_control_torque_, nan_); - register_output( - "/chassis/right_back_wheel/control_torque", right_back_control_torque_, nan_); - register_output( - "/chassis/right_front_wheel/control_torque", right_front_control_torque_, nan_); } void before_updating() override { @@ -76,9 +73,9 @@ class DeformableOmniWheelController return; } - Eigen::Vector4d wheel_velocities = { - *left_front_velocity_, *left_back_velocity_, *right_back_velocity_, - *right_front_velocity_}; + Eigen::Vector4d wheel_velocities; + for (size_t i = 0; i < kWheelCount; ++i) + wheel_velocities[i] = *wheel_velocity_[i]; const auto chassis_velocity = calculate_chassis_velocity(wheel_velocities); auto chassis_control_torque = calculate_chassis_control_torque(chassis_velocity); @@ -89,23 +86,29 @@ class DeformableOmniWheelController const auto wheel_control_torques = calculate_wheel_control_torques(chassis_control_torque, wheel_pid_torques); - *left_front_control_torque_ = wheel_control_torques[0]; - *left_back_control_torque_ = wheel_control_torques[1]; - *right_back_control_torque_ = wheel_control_torques[2]; - *right_front_control_torque_ = wheel_control_torques[3]; + for (size_t i = 0; i < kWheelCount; ++i) + *wheel_control_torque_[i] = wheel_control_torques[i]; } private: + static constexpr size_t kWheelCount = 4; + static constexpr const char* kWheelName[] = { + "left_front", + "left_back", + "right_back", + "right_front", + }; + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double g_ = 9.81; + struct ChassisControlTorque { Eigen::Vector2d torque; Eigen::Vector2d lambda; }; void reset_all_controls() { - *left_front_control_torque_ = 0.0; - *left_back_control_torque_ = 0.0; - *right_back_control_torque_ = 0.0; - *right_front_control_torque_ = 0.0; + for (size_t i = 0; i < kWheelCount; ++i) + *wheel_control_torque_[i] = 0.0; } Eigen::Vector3d calculate_chassis_velocity(const Eigen::Vector4d& wheel_velocities) const { @@ -122,16 +125,17 @@ class DeformableOmniWheelController ChassisControlTorque calculate_chassis_control_torque(const Eigen::Vector3d& chassis_velocity) { ChassisControlTorque result; - Eigen::Vector3d err = chassis_control_velocity_->vector - chassis_velocity; + Eigen::Vector3d chassis_velocity_error = + chassis_control_velocity_->vector - chassis_velocity; Eigen::Vector2d translational_torque = (-std::numbers::sqrt2 / 4 * wheel_radius_) * mass_ - * translational_velocity_pid_calculator_.update(err.head<2>()); + * translational_velocity_pid_calculator_.update(chassis_velocity_error.head<2>()); result.torque.x() = translational_torque.norm(); const double a_plus_b = std::numbers::sqrt2 * std::max(*chassis_radius_, 1e-6); result.torque.y() = (-std::numbers::sqrt2 / 4 * wheel_radius_) * (moment_of_inertia_ / a_plus_b) - * angular_velocity_pid_calculator_.update(err[2]); + * angular_velocity_pid_calculator_.update(chassis_velocity_error[2]); Eigen::Vector2d translational_torque_direction; if (result.torque.x() > 0) @@ -230,10 +234,6 @@ class DeformableOmniWheelController return wheel_torques; } - static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - - static constexpr double g_ = 9.81; - const double mass_; const double moment_of_inertia_; const double wheel_radius_; @@ -245,10 +245,8 @@ class DeformableOmniWheelController InputInterface wheel_motor_max_control_torque_; - InputInterface left_front_velocity_; - InputInterface left_back_velocity_; - InputInterface right_back_velocity_; - InputInterface right_front_velocity_; + std::array, kWheelCount> wheel_velocity_; + std::array, kWheelCount> wheel_control_torque_; InputInterface chassis_control_velocity_; InputInterface power_limit_; @@ -260,11 +258,6 @@ class DeformableOmniWheelController pid::MatrixPidCalculator<4> wheel_velocity_pid_; QcpSolver qcp_solver_; - - OutputInterface left_front_control_torque_; - OutputInterface left_back_control_torque_; - OutputInterface right_back_control_torque_; - OutputInterface right_front_control_torque_; }; } // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp new file mode 100644 index 000000000..6da008098 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp @@ -0,0 +1,627 @@ +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "controller/pid/pid_calculator.hpp" +#include "filter/low_pass_filter.hpp" + +namespace rmcs_core::controller::chassis { + +class DeformableSuspension + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DeformableSuspension() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + load_config_(); + + register_input("/predefined/update_rate", update_rate_, false); + + register_input("/chassis/active_suspension/active", active_suspension_active_); + register_input("/chassis/deformable/reset_count", reset_count_, false); + register_input("/chassis/deformable/low_prone_active", low_prone_active_); + register_input("/chassis/deformable/symmetric_posture_target", symmetric_posture_target_); + register_input("/chassis/deformable/correction_inverted", correction_inverted_); + register_input("/chassis/deformable/min_angle_deg", min_angle_deg_); + register_input("/chassis/deformable/max_angle_deg", max_angle_deg_); + register_input( + "/chassis/deformable/suspension_reference_angle_deg", suspension_reference_angle_deg_); + + register_input("/chassis/imu/pitch", chassis_imu_pitch_, false); + register_input("/chassis/imu/roll", chassis_imu_roll_, false); + register_input("/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, false); + register_input("/chassis/imu/roll_rate", chassis_imu_roll_rate_, false); + + for (size_t i = 0; i < kJointCount; ++i) { + register_input( + std::string{"/chassis/deformable/"} + kJointName[i] + "_joint/posture_target_angle", + joint_posture_target_angle_rad_[i]); + register_input( + std::string{"/chassis/"} + kJointName[i] + "_joint/physical_angle", + joint_physical_angle_[i], false); + register_output( + std::string{"/chassis/"} + kJointName[i] + "_joint/target_physical_angle", + joint_target_angle_[i], nan_); + register_output( + std::string{"/chassis/"} + kJointName[i] + "_joint/target_physical_velocity", + joint_target_velocity_[i], nan_); + register_output( + std::string{"/chassis/"} + kJointName[i] + "_joint/target_physical_acceleration", + joint_target_acceleration_[i], nan_); + register_output( + std::string{"/chassis/"} + kJointName[i] + "_joint/control_angle_error", + joint_angle_error_[i], nan_); + } + } + + void before_updating() override { + if (!update_rate_.ready()) + update_rate_.make_and_bind_directly(1000.0); + if (!reset_count_.ready()) + reset_count_.make_and_bind_directly(static_cast(0)); + if (!chassis_imu_pitch_.ready()) + chassis_imu_pitch_.make_and_bind_directly(0.0); + if (!chassis_imu_roll_.ready()) + chassis_imu_roll_.make_and_bind_directly(0.0); + if (!chassis_imu_pitch_rate_.ready()) + chassis_imu_pitch_rate_.make_and_bind_directly(0.0); + if (!chassis_imu_roll_rate_.ready()) + chassis_imu_roll_rate_.make_and_bind_directly(0.0); + + configure_active_rate_filters_(1.0 / update_dt_()); + validate_joint_feedback_inputs_(); + reset_all_controls_(); + last_reset_count_ = *reset_count_; + } + + void update() override { + if (*reset_count_ != last_reset_count_) { + reset_all_controls_(); + last_reset_count_ = *reset_count_; + return; + } + + const auto current_physical_angles = read_feedback_(); + + if (!init_joint_targets_from_feedback_(current_physical_angles)) { + publish_nan_joint_targets_(); + return; + } + + const auto posture_target_angles_rad = read_posture_target_angles_rad_(); + const auto dt = update_dt_(); + + double filtered_pitch_rate = *chassis_imu_pitch_rate_; + double filtered_roll_rate = *chassis_imu_roll_rate_; + filter_attitude_rates_(filtered_pitch_rate, filtered_roll_rate); + + if (*active_suspension_active_) + calibrate_(*chassis_imu_pitch_, *chassis_imu_roll_, *symmetric_posture_target_, dt); + + std::array joint_angle_states{}; + copy_joint_angle_states_(joint_angle_states); + update_suspension_state_( + *chassis_imu_pitch_ - pitch_offset_value_, *chassis_imu_roll_ - roll_offset_value_, + filtered_pitch_rate, filtered_roll_rate, *active_suspension_active_, *low_prone_active_, + *min_angle_deg_, *max_angle_deg_, *suspension_reference_angle_deg_, + *correction_inverted_, joint_angle_states, dt); + + const auto target_angles_rad = compute_joint_trajectory_targets_( + posture_target_angles_rad, *active_suspension_active_, *low_prone_active_, + *min_angle_deg_, *suspension_reference_angle_deg_); + + run_joint_trajectory_(target_angles_rad, *active_suspension_active_, dt); + publish_joint_targets_(current_physical_angles); + } + +private: + static constexpr size_t kJointCount = 4; + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double offset_limit_rad_ = 1.0 * std::numbers::pi / 180.0; + static constexpr size_t kLeftFront = 0; + static constexpr size_t kLeftBack = 1; + static constexpr size_t kRightBack = 2; + static constexpr size_t kRightFront = 3; + static constexpr const char* kJointName[] = { + "left_front", + "left_back", + "right_back", + "right_front", + }; + + static double deg_to_rad_(double deg) { return deg * std::numbers::pi / 180.0; } + + void validate_joint_feedback_inputs_() const { + for (size_t i = 0; i < kJointCount; ++i) + if (!joint_physical_angle_[i].ready()) + throw std::runtime_error( + "missing deformable chassis feedback interfaces: expected " + "/chassis/*_joint/physical_angle"); + } + + double update_dt_() const { + if (update_rate_.ready() && std::isfinite(*update_rate_) && *update_rate_ > 1e-6) + return 1.0 / *update_rate_; + return 1e-3; + } + + void load_pid_( + const std::string& prefix, pid::PidCalculator& pid, double kp_default, double ki_default, + double kd_default, double integral_min_default, double integral_max_default, + double output_min_default, double output_max_default) { + pid.kp = get_parameter_or(prefix + "kp", kp_default); + pid.ki = get_parameter_or(prefix + "ki", ki_default); + pid.kd = get_parameter_or(prefix + "kd", kd_default); + pid.integral_min = get_parameter_or(prefix + "integral_min", integral_min_default); + pid.integral_max = get_parameter_or(prefix + "integral_max", integral_max_default); + pid.output_min = get_parameter_or(prefix + "output_min", output_min_default); + pid.output_max = get_parameter_or(prefix + "output_max", output_max_default); + } + + void load_config_() { + joint_target_vel_limit_ = std::max( + deg_to_rad_(std::abs(get_parameter_or("target_physical_velocity_limit", 180.0))), 1e-6); + joint_target_acc_limit_ = std::max( + deg_to_rad_(std::abs(get_parameter_or("target_physical_acceleration_limit", 720.0))), + 1e-6); + suspension_target_vel_limit_ = std::max( + deg_to_rad_( + std::abs(get_parameter_or( + "active_suspension_target_velocity_limit_deg", + get_parameter_or("target_physical_velocity_limit", 180.0)))), + 1e-6); + suspension_target_acc_limit_ = std::max( + deg_to_rad_( + std::abs(get_parameter_or( + "active_suspension_target_acceleration_limit_deg", + get_parameter_or("target_physical_acceleration_limit", 720.0)))), + 1e-6); + + load_pid_( + "active_suspension_pitch_outer_", pitch_outer_pid_, 8.0, 0.35, 0.28, -2.0, 2.0, -3.0, + 3.0); + load_pid_( + "active_suspension_pitch_inner_", pitch_inner_pid_, 2.0, 0.0, 0.0, -1.0, 1.0, -0.785, + 0.785); + load_pid_( + "active_suspension_roll_outer_", roll_outer_pid_, 8.0, 0.35, 0.28, -2.0, 2.0, -3.0, + 3.0); + load_pid_( + "active_suspension_roll_inner_", roll_inner_pid_, 2.0, 0.0, 0.0, -1.0, 1.0, -0.785, + 0.785); + + active_correction_vel_limit_ = std::max( + deg_to_rad_( + std::abs( + get_parameter_or("active_suspension_correction_velocity_limit_deg", 720.0))), + 1e-6); + active_correction_acc_limit_ = std::max( + deg_to_rad_( + std::abs(get_parameter_or( + "active_suspension_correction_acceleration_limit_deg", 3600.0))), + 1e-6); + active_rate_lpf_cutoff_hz_ = + std::max(get_parameter_or("active_suspension_rate_lpf_cutoff_hz", 10.0), 1e-6); + + calibration_wait_time_ = + std::max(get_parameter_or("chassis_imu_calibration_wait_s", 2.0), 0.0); + calibration_sample_time_ = + std::max(get_parameter_or("chassis_imu_calibration_sample_s", 3.0), 1e-6); + } + + std::array read_feedback_() const { + std::array angles; + angles.fill(nan_); + + for (size_t i = 0; i < kJointCount; ++i) + if (joint_physical_angle_[i].ready() && std::isfinite(*joint_physical_angle_[i])) + angles[i] = *joint_physical_angle_[i]; + + return angles; + } + + std::array read_posture_target_angles_rad_() const { + std::array targets{}; + for (size_t i = 0; i < kJointCount; ++i) + targets[i] = *joint_posture_target_angle_rad_[i]; + return targets; + } + + std::array compute_joint_trajectory_targets_( + const std::array& posture_target_angles_rad, bool suspension_active, + bool low_prone_active, double min_angle_deg, double suspension_reference_angle_deg) const { + if (!suspension_active) + return posture_target_angles_rad; + + std::array target_angles_rad{}; + double target_angle_rad = low_prone_active ? deg_to_rad_(min_angle_deg - 5.0) + : deg_to_rad_(suspension_reference_angle_deg); + target_angles_rad.fill(target_angle_rad); + return target_angles_rad; + } + + void reset_attitude_() { + pitch_outer_pid_.reset(); + pitch_inner_pid_.reset(); + roll_outer_pid_.reset(); + roll_inner_pid_.reset(); + correction_target_rad_.fill(0.0); + } + + void reset_calibration_window_() { + calibration_hold_elapsed_ = 0.0; + sample_count_ = 0; + pitch_sum_ = 0.0; + roll_sum_ = 0.0; + calibration_completed_for_window_ = false; + } + + void reset_all_controls_() { + reset_attitude_(); + pitch_rate_filter_.reset(); + roll_rate_filter_.reset(); + correction_state_rad_.fill(0.0); + correction_velocity_state_rad_.fill(0.0); + correction_acceleration_state_rad_.fill(0.0); + joint_target_active_.fill(false); + joint_target_angle_state_rad_.fill(nan_); + joint_target_velocity_state_rad_.fill(0.0); + joint_target_acceleration_state_rad_.fill(0.0); + reset_calibration_window_(); + calibrated_once_ = false; + pitch_offset_value_ = 0.0; + roll_offset_value_ = 0.0; + + for (size_t i = 0; i < kJointCount; ++i) { + *joint_target_angle_[i] = nan_; + *joint_target_velocity_[i] = nan_; + *joint_target_acceleration_[i] = nan_; + *joint_angle_error_[i] = nan_; + } + } + + void calibrate_(double pitch, double roll, bool symmetric_target, double dt) { + if (calibrated_once_) + return; + + if (!symmetric_target) { + reset_calibration_window_(); + return; + } + + if (!std::isfinite(pitch) || !std::isfinite(roll)) + return; + + calibration_hold_elapsed_ += dt; + if (calibration_hold_elapsed_ < calibration_wait_time_) + return; + + const double calibration_end = calibration_wait_time_ + calibration_sample_time_; + if (calibration_hold_elapsed_ < calibration_end) { + pitch_sum_ += pitch; + roll_sum_ += roll; + ++sample_count_; + return; + } + + if (calibration_completed_for_window_) + return; + + calibration_completed_for_window_ = true; + if (sample_count_ == 0) + return; + + pitch_offset_value_ = std::clamp( + pitch_sum_ / static_cast(sample_count_), -offset_limit_rad_, offset_limit_rad_); + roll_offset_value_ = std::clamp( + roll_sum_ / static_cast(sample_count_), -offset_limit_rad_, offset_limit_rad_); + calibrated_once_ = true; + } + + bool init_joint_targets_from_feedback_(const std::array& physical_angles) { + bool any_active_value = false; + for (size_t i = 0; i < kJointCount; ++i) { + if (std::isfinite(physical_angles[i]) && !joint_target_active_[i]) { + joint_target_angle_state_rad_[i] = physical_angles[i]; + joint_target_velocity_state_rad_[i] = 0.0; + joint_target_acceleration_state_rad_[i] = 0.0; + joint_target_active_[i] = true; + } + any_active_value = any_active_value || joint_target_active_[i]; + } + return any_active_value; + } + + void configure_active_rate_filters_(double sampling_frequency) { + const double clamped_sampling_frequency = std::max(sampling_frequency, 1e-6); + if (std::abs(active_rate_filter_sampling_hz_ - clamped_sampling_frequency) < 1e-6) + return; + + pitch_rate_filter_.set_cutoff(active_rate_lpf_cutoff_hz_, clamped_sampling_frequency); + roll_rate_filter_.set_cutoff(active_rate_lpf_cutoff_hz_, clamped_sampling_frequency); + active_rate_filter_sampling_hz_ = clamped_sampling_frequency; + } + + void filter_attitude_rates_(double& pitch_rate, double& roll_rate) { + if (std::isfinite(pitch_rate)) + pitch_rate = pitch_rate_filter_.update(pitch_rate); + if (std::isfinite(roll_rate)) + roll_rate = roll_rate_filter_.update(roll_rate); + } + + void compute_correction_targets_(double pitch_diff, double roll_diff, bool inverted) { + if (inverted) { + const double front_pitch_contribution = std::max(pitch_diff, 0.0); + const double back_pitch_contribution = std::max(-pitch_diff, 0.0); + const double left_roll_contribution = std::max(-roll_diff, 0.0); + const double right_roll_contribution = std::max(roll_diff, 0.0); + correction_target_rad_[kLeftFront] = + -(front_pitch_contribution + left_roll_contribution); + correction_target_rad_[kLeftBack] = -(back_pitch_contribution + left_roll_contribution); + correction_target_rad_[kRightBack] = + -(back_pitch_contribution + right_roll_contribution); + correction_target_rad_[kRightFront] = + -(front_pitch_contribution + right_roll_contribution); + } else { + const double front_pitch_contribution = std::max(-pitch_diff, 0.0); + const double back_pitch_contribution = std::max(pitch_diff, 0.0); + const double left_roll_contribution = std::max(roll_diff, 0.0); + const double right_roll_contribution = std::max(-roll_diff, 0.0); + correction_target_rad_[kLeftFront] = front_pitch_contribution + left_roll_contribution; + correction_target_rad_[kLeftBack] = back_pitch_contribution + left_roll_contribution; + correction_target_rad_[kRightBack] = back_pitch_contribution + right_roll_contribution; + correction_target_rad_[kRightFront] = + front_pitch_contribution + right_roll_contribution; + } + } + + void run_correction_trajectory_( + bool low_prone_override_active, double min_angle_deg, double max_angle_deg, + double base_angle_deg, const std::array& base_joint_angles, + double correction_vel_limit, double correction_acc_limit, double dt) { + const double max_target_rad = deg_to_rad_(max_angle_deg); + const double min_susp_rad = deg_to_rad_(min_angle_deg - 5.0); + + for (size_t i = 0; i < kJointCount; ++i) { + const double base_angle = + std::isfinite(base_joint_angles[i]) + ? base_joint_angles[i] + : (low_prone_override_active ? min_susp_rad : deg_to_rad_(base_angle_deg)); + + const double correction_min = min_susp_rad - base_angle; + const double correction_max = max_target_rad - base_angle; + const double target = + std::clamp(correction_target_rad_[i], correction_min, correction_max); + + double& angle_state = correction_state_rad_[i]; + double& velocity_state = correction_velocity_state_rad_[i]; + double& acceleration_state = correction_acceleration_state_rad_[i]; + + const double position_error = target - angle_state; + const double stopping_distance = + velocity_state * velocity_state / (2.0 * correction_acc_limit); + + double desired_velocity = 0.0; + if (std::abs(position_error) > 1e-6 && std::abs(position_error) > stopping_distance) + desired_velocity = std::copysign(correction_vel_limit, position_error); + + const double velocity_error = desired_velocity - velocity_state; + acceleration_state = + std::clamp(velocity_error / dt, -correction_acc_limit, correction_acc_limit); + + velocity_state += acceleration_state * dt; + velocity_state = + std::clamp(velocity_state, -correction_vel_limit, correction_vel_limit); + angle_state += velocity_state * dt; + + const double next_error = target - angle_state; + if ((position_error > 0.0 && next_error < 0.0) + || (position_error < 0.0 && next_error > 0.0) + || (std::abs(next_error) < 1e-5 && std::abs(velocity_state) < 1e-3)) { + angle_state = target; + velocity_state = 0.0; + acceleration_state = 0.0; + } + } + } + + void update_suspension_state_( + double pitch, double roll, double pitch_rate, double roll_rate, bool suspension_active, + bool low_prone_override_active, double min_angle_deg, double max_angle_deg, + double base_angle_deg, bool correction_inverted, + const std::array& base_joint_angles, double dt) { + if (!suspension_active) { + reset_attitude_(); + run_correction_trajectory_( + low_prone_override_active, min_angle_deg, max_angle_deg, base_angle_deg, + base_joint_angles, active_correction_vel_limit_, active_correction_acc_limit_, dt); + return; + } + + constexpr double max_attitude = 30.0 * std::numbers::pi / 180.0; + const double clamped_pitch = std::clamp(pitch, -max_attitude, max_attitude); + const double clamped_roll = std::clamp(roll, -max_attitude, max_attitude); + + const double pitch_outer = pitch_outer_pid_.update(-clamped_pitch); + const double roll_outer = roll_outer_pid_.update(clamped_roll); + const double pitch_diff = pitch_inner_pid_.update(pitch_outer - pitch_rate); + const double roll_diff = roll_inner_pid_.update(roll_outer + roll_rate); + + if (!std::isfinite(pitch_diff) || !std::isfinite(roll_diff)) { + reset_attitude_(); + return; + } + + compute_correction_targets_(pitch_diff, roll_diff, correction_inverted); + run_correction_trajectory_( + low_prone_override_active, min_angle_deg, max_angle_deg, base_angle_deg, + base_joint_angles, active_correction_vel_limit_, active_correction_acc_limit_, dt); + } + + void run_joint_trajectory_( + const std::array& target_angles_rad, bool suspension_active, + double dt) { + for (size_t i = 0; i < kJointCount; ++i) { + if (!joint_target_active_[i]) + continue; + + double& angle_state = joint_target_angle_state_rad_[i]; + double& velocity_state = joint_target_velocity_state_rad_[i]; + double& acceleration_state = joint_target_acceleration_state_rad_[i]; + const double target = target_angles_rad[i]; + + const double vel_limit = + suspension_active ? suspension_target_vel_limit_ : joint_target_vel_limit_; + const double acc_limit = + suspension_active ? suspension_target_acc_limit_ : joint_target_acc_limit_; + + if (!std::isfinite(target) || !std::isfinite(angle_state)) + continue; + + const double position_error = target - angle_state; + const double stopping_distance = velocity_state * velocity_state / (2.0 * acc_limit); + + double desired_velocity = 0.0; + if (std::abs(position_error) > 1e-6 && std::abs(position_error) > stopping_distance) + desired_velocity = std::copysign(vel_limit, position_error); + + const double velocity_error = desired_velocity - velocity_state; + acceleration_state = std::clamp(velocity_error / dt, -acc_limit, acc_limit); + + velocity_state += acceleration_state * dt; + velocity_state = std::clamp(velocity_state, -vel_limit, vel_limit); + angle_state += velocity_state * dt; + + const double next_error = target - angle_state; + if ((position_error > 0.0 && next_error < 0.0) + || (position_error < 0.0 && next_error > 0.0) + || (std::abs(next_error) < 1e-5 && std::abs(velocity_state) < 1e-3)) { + angle_state = target; + velocity_state = 0.0; + acceleration_state = 0.0; + } + } + } + + bool any_joint_target_active_() const { + for (size_t i = 0; i < kJointCount; ++i) + if (joint_target_active_[i]) + return true; + return false; + } + + void copy_joint_angle_states_(std::array& out) const { + out = joint_target_angle_state_rad_; + } + + void publish_joint_targets_(const std::array& feedback_angles) { + const double min_angle_rad = deg_to_rad_(*min_angle_deg_ - 5.0); + const double max_angle_rad = deg_to_rad_(*max_angle_deg_); + + if (!any_joint_target_active_()) { + publish_nan_joint_targets_(); + return; + } + + for (size_t i = 0; i < kJointCount; ++i) { + if (!joint_target_active_[i]) { + *joint_target_angle_[i] = nan_; + *joint_target_velocity_[i] = nan_; + *joint_target_acceleration_[i] = nan_; + *joint_angle_error_[i] = nan_; + continue; + } + + const double target = joint_target_angle_state_rad_[i] + correction_state_rad_[i]; + *joint_target_angle_[i] = std::clamp(target, min_angle_rad, max_angle_rad); + *joint_target_velocity_[i] = + joint_target_velocity_state_rad_[i] + correction_velocity_state_rad_[i]; + *joint_target_acceleration_[i] = + joint_target_acceleration_state_rad_[i] + correction_acceleration_state_rad_[i]; + *joint_angle_error_[i] = std::isfinite(feedback_angles[i]) + ? feedback_angles[i] - *joint_target_angle_[i] + : nan_; + } + } + + void publish_nan_joint_targets_() { reset_all_controls_(); } + + InputInterface update_rate_; + + InputInterface active_suspension_active_; + InputInterface reset_count_; + InputInterface low_prone_active_; + InputInterface symmetric_posture_target_; + InputInterface correction_inverted_; + InputInterface min_angle_deg_; + InputInterface max_angle_deg_; + InputInterface suspension_reference_angle_deg_; + + InputInterface chassis_imu_pitch_; + InputInterface chassis_imu_roll_; + InputInterface chassis_imu_pitch_rate_; + InputInterface chassis_imu_roll_rate_; + + std::array, kJointCount> joint_posture_target_angle_rad_; + std::array, kJointCount> joint_physical_angle_; + + std::array, kJointCount> joint_target_angle_; + std::array, kJointCount> joint_target_velocity_; + std::array, kJointCount> joint_target_acceleration_; + std::array, kJointCount> joint_angle_error_; + + pid::PidCalculator pitch_outer_pid_{}; + pid::PidCalculator pitch_inner_pid_{}; + pid::PidCalculator roll_outer_pid_{}; + pid::PidCalculator roll_inner_pid_{}; + filter::LowPassFilter<1> pitch_rate_filter_{1.0}; + filter::LowPassFilter<1> roll_rate_filter_{1.0}; + + double active_correction_vel_limit_ = 40.0; + double active_correction_acc_limit_ = 200.0; + double active_rate_lpf_cutoff_hz_ = 10.0; + double active_rate_filter_sampling_hz_ = 0.0; + + double calibration_wait_time_ = 2.0; + double calibration_sample_time_ = 3.0; + double calibration_hold_elapsed_ = 0.0; + size_t sample_count_ = 0; + double pitch_sum_ = 0.0; + double roll_sum_ = 0.0; + bool calibration_completed_for_window_ = false; + bool calibrated_once_ = false; + double pitch_offset_value_ = 0.0; + double roll_offset_value_ = 0.0; + + std::array correction_target_rad_ = {0.0, 0.0, 0.0, 0.0}; + std::array correction_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + std::array correction_velocity_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + std::array correction_acceleration_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + + std::array joint_target_active_ = {false, false, false, false}; + std::array joint_target_angle_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + std::array joint_target_velocity_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + std::array joint_target_acceleration_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + + double joint_target_vel_limit_ = 0.0; + double joint_target_acc_limit_ = 0.0; + double suspension_target_vel_limit_ = 0.0; + double suspension_target_acc_limit_ = 0.0; + size_t last_reset_count_ = 0; +}; + +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::DeformableSuspension, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_wheel_controller.cpp deleted file mode 100644 index e25d9cdb8..000000000 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_wheel_controller.cpp +++ /dev/null @@ -1,873 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -#include "controller/chassis/qcp_solver.hpp" -#include "controller/pid/matrix_pid_calculator.hpp" -#include "controller/pid/pid_calculator.hpp" -#include "filter/low_pass_filter.hpp" - -namespace rmcs_core::controller::chassis { - -class DeformableChassisController - : public rmcs_executor::Component - , public rclcpp::Node { - - enum class WheelIndex : size_t { - LeftFront = 0, - LeftBack = 1, - RightBack = 2, - RightFront = 3, - Count = 4 - }; - - static constexpr size_t kWheelCount = static_cast(WheelIndex::Count); - - struct EllipseParameters { - double a, b, c, d, e, f; - }; - -public: - explicit DeformableChassisController() - : Node( - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , mass_(get_parameter("mass").as_double()) - , moment_of_inertia_(get_parameter("moment_of_inertia").as_double()) - , chassis_radius_(get_parameter("chassis_radius").as_double()) - , rod_length_(get_parameter("rod_length").as_double()) - , wheel_radius_(get_parameter("wheel_radius").as_double()) - , friction_coefficient_(get_parameter("friction_coefficient").as_double()) - , k1_(get_parameter("k1").as_double()) - , k2_(get_parameter("k2").as_double()) - , no_load_power_(get_parameter("no_load_power").as_double()) - , ellipse_coeff_quadratic_translational_( - k1_ * mass_ * mass_ * wheel_radius_ * wheel_radius_ / 16.0) - , ellipse_coeff_cross_term_( - k1_ * mass_ * moment_of_inertia_ * wheel_radius_ * wheel_radius_ / 8.0) - , ellipse_coeff_quadratic_angular_( - k1_ * moment_of_inertia_ * moment_of_inertia_ * wheel_radius_ * wheel_radius_ / 16.0) - , ellipse_coeff_linear_translational_(mass_ * wheel_radius_ / 4.0) - , ellipse_coeff_linear_angular_(moment_of_inertia_ * wheel_radius_ / 4.0) - , vehicle_radius_(Eigen::Vector4d::Constant(chassis_radius_ + rod_length_)) - , control_acceleration_filter_(5.0, 1000.0) - , chassis_velocity_expected_(Eigen::Vector3d::Zero()) - , chassis_translational_velocity_pid_(5.0, 0.0, 1.0) - , chassis_angular_velocity_pid_(5.0, 0.0, 1.0) - , steering_velocity_pid_(0.15, 0.0, 0.0) - , steering_angle_pid_(30.0, 0.0, 0.0) - , wheel_velocity_pid_(0.6, 0.0, 0.0) { - - register_input("/remote/joystick/right", joystick_right_); - register_input("/remote/joystick/left", joystick_left_); - - register_input("/chassis/left_front_steering/angle", left_front_steering_angle_); - register_input("/chassis/left_back_steering/angle", left_back_steering_angle_); - register_input("/chassis/right_back_steering/angle", right_back_steering_angle_); - register_input("/chassis/right_front_steering/angle", right_front_steering_angle_); - - register_input("/chassis/left_front_steering/velocity", left_front_steering_velocity_); - register_input("/chassis/left_back_steering/velocity", left_back_steering_velocity_); - register_input("/chassis/right_back_steering/velocity", right_back_steering_velocity_); - register_input("/chassis/right_front_steering/velocity", right_front_steering_velocity_); - - register_input("/chassis/left_front_wheel/velocity", left_front_wheel_velocity_); - register_input("/chassis/left_back_wheel/velocity", left_back_wheel_velocity_); - register_input("/chassis/right_back_wheel/velocity", right_back_wheel_velocity_); - register_input("/chassis/right_front_wheel/velocity", right_front_wheel_velocity_); - - register_input("/chassis/left_front_joint/physical_angle", left_front_joint_angle_); - register_input("/chassis/left_back_joint/physical_angle", left_back_joint_angle_); - register_input("/chassis/right_back_joint/physical_angle", right_back_joint_angle_); - register_input("/chassis/right_front_joint/physical_angle", right_front_joint_angle_); - - register_input("/chassis/left_front_joint/physical_velocity", left_front_joint_velocity_); - register_input("/chassis/left_back_joint/physical_velocity", left_back_joint_velocity_); - register_input("/chassis/right_back_joint/physical_velocity", right_back_joint_velocity_); - register_input("/chassis/right_front_joint/physical_velocity", right_front_joint_velocity_); - - register_input( - "/chassis/left_front_joint/target_physical_angle", - left_front_joint_target_physical_angle_, false); - register_input( - "/chassis/left_back_joint/target_physical_angle", - left_back_joint_target_physical_angle_, false); - register_input( - "/chassis/right_back_joint/target_physical_angle", - right_back_joint_target_physical_angle_, false); - register_input( - "/chassis/right_front_joint/target_physical_angle", - right_front_joint_target_physical_angle_, false); - register_input( - "/chassis/left_front_joint/target_physical_velocity", - left_front_joint_target_physical_velocity_, false); - register_input( - "/chassis/left_back_joint/target_physical_velocity", - left_back_joint_target_physical_velocity_, false); - register_input( - "/chassis/right_back_joint/target_physical_velocity", - right_back_joint_target_physical_velocity_, false); - register_input( - "/chassis/right_front_joint/target_physical_velocity", - right_front_joint_target_physical_velocity_, false); - register_input( - "/chassis/left_front_joint/target_physical_acceleration", - left_front_joint_target_physical_acceleration_, false); - register_input( - "/chassis/left_back_joint/target_physical_acceleration", - left_back_joint_target_physical_acceleration_, false); - register_input( - "/chassis/right_back_joint/target_physical_acceleration", - right_back_joint_target_physical_acceleration_, false); - register_input( - "/chassis/right_front_joint/target_physical_acceleration", - right_front_joint_target_physical_acceleration_, false); - - register_input("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_); - register_input("/chassis/control_velocity", chassis_control_velocity_); - register_input("/chassis/control_power_limit", power_limit_); - - register_output( - "/chassis/left_front_steering/control_torque", left_front_steering_control_torque_); - register_output( - "/chassis/left_back_steering/control_torque", left_back_steering_control_torque_); - register_output( - "/chassis/right_back_steering/control_torque", right_back_steering_control_torque_); - register_output( - "/chassis/right_front_steering/control_torque", right_front_steering_control_torque_); - - register_output( - "/chassis/left_front_wheel/control_torque", left_front_wheel_control_torque_); - register_output("/chassis/left_back_wheel/control_torque", left_back_wheel_control_torque_); - register_output( - "/chassis/right_back_wheel/control_torque", right_back_wheel_control_torque_); - register_output( - "/chassis/right_front_wheel/control_torque", right_front_wheel_control_torque_); - } - - void update() override { - if (std::isnan(chassis_control_velocity_->vector[0])) { - reset_all_controls(); - return; - } - - const JointFeedbackStates joint_feedback = update_joint_feedback_states_(); - const JointTargetStates joint_target = update_joint_target_states_(); - if (joint_feedback.valid) { - vehicle_radius_ = joint_feedback.radius; - RCLCPP_INFO_THROTTLE( - get_logger(), *get_clock(), 1000, - "physical joint angle[deg] lf=%.2f lb=%.2f rb=%.2f rf=%.2f, radius[m] lf=%.3f " - "lb=%.3f rb=%.3f rf=%.3f", - joint_feedback.alpha_rad[0] * 180.0 / std::numbers::pi, - joint_feedback.alpha_rad[1] * 180.0 / std::numbers::pi, - joint_feedback.alpha_rad[2] * 180.0 / std::numbers::pi, - joint_feedback.alpha_rad[3] * 180.0 / std::numbers::pi, vehicle_radius_[0], - vehicle_radius_[1], vehicle_radius_[2], vehicle_radius_[3]); - } - - integral_yaw_angle_imu(); - - const auto steering_status = calculate_steering_status(); - const auto wheel_velocities = calculate_wheel_velocities(); - const auto chassis_velocity = - calculate_chassis_velocity(steering_status, wheel_velocities, joint_feedback); - auto chassis_status_expected = - calculate_chassis_status_expected(chassis_velocity, joint_target, joint_feedback); - const auto chassis_control_velocity = calculate_chassis_control_velocity(); - const auto chassis_acceleration = calculate_chassis_control_acceleration( - chassis_status_expected.velocity, chassis_control_velocity); - const double power_limit = - *power_limit_ - no_load_power_ - k2_ * wheel_velocities.array().pow(2).sum(); - const auto wheel_pid_torques = - calculate_wheel_pid_torques(steering_status, wheel_velocities, chassis_status_expected); - const auto constrained_chassis_acceleration = constrain_chassis_control_acceleration( - steering_status, wheel_velocities, joint_target, chassis_acceleration, - wheel_pid_torques, power_limit); - const auto filtered_chassis_acceleration = - odom_to_base_link_vector(control_acceleration_filter_.update( - base_link_to_odom_vector(constrained_chassis_acceleration))); - const auto steering_torques = calculate_steering_control_torques( - steering_status, chassis_status_expected, joint_target, joint_feedback, - filtered_chassis_acceleration); - const auto wheel_torques = calculate_wheel_control_torques( - steering_status, joint_target, joint_feedback, filtered_chassis_acceleration, - wheel_pid_torques); - - update_control_torques(steering_torques, wheel_torques); - update_chassis_velocity_expected(filtered_chassis_acceleration); - } - -private: - struct SteeringStatus { - Eigen::Vector4d angle = Eigen::Vector4d::Zero(); - Eigen::Vector4d cos_angle = Eigen::Vector4d::Zero(); - Eigen::Vector4d sin_angle = Eigen::Vector4d::Zero(); - Eigen::Vector4d velocity = Eigen::Vector4d::Zero(); - Eigen::Vector4d sin_angle_minus_phi = Eigen::Vector4d::Zero(); - Eigen::Vector4d cos_angle_minus_phi = Eigen::Vector4d::Zero(); - }; - - struct ChassisStatus { - Eigen::Vector3d velocity = Eigen::Vector3d::Zero(); - Eigen::Vector4d wheel_velocity_x = Eigen::Vector4d::Zero(); - Eigen::Vector4d wheel_velocity_y = Eigen::Vector4d::Zero(); - }; - - struct JointStateData { - Eigen::Vector4d alpha_rad = Eigen::Vector4d::Zero(); - Eigen::Vector4d alpha_dot_rad = Eigen::Vector4d::Zero(); - Eigen::Vector4d alpha_ddot_rad = Eigen::Vector4d::Zero(); - Eigen::Vector4d radius = Eigen::Vector4d::Zero(); - Eigen::Vector4d radius_dot = Eigen::Vector4d::Zero(); - Eigen::Vector4d radius_ddot = Eigen::Vector4d::Zero(); - bool valid = false; - }; - - struct JointFeedbackStates : JointStateData {}; - - struct JointTargetStates : JointStateData { - bool has_velocity = false; - bool has_acceleration = false; - }; - - enum class JointStateSource : uint8_t { Target, Feedback }; - - struct JointStateView { - const Eigen::Vector4d& alpha_rad; - const Eigen::Vector4d& alpha_dot_rad; - const Eigen::Vector4d& alpha_ddot_rad; - const Eigen::Vector4d& radius; - const Eigen::Vector4d& radius_dot; - const Eigen::Vector4d& radius_ddot; - JointStateSource source; - bool valid; - }; - - static JointStateView - select_joint_state(const JointTargetStates& target, const JointFeedbackStates& feedback) { - if (target.valid) { - return { - target.alpha_rad, target.alpha_dot_rad, target.alpha_ddot_rad, target.radius, - target.radius_dot, target.radius_ddot, JointStateSource::Target, target.valid}; - } - - return {feedback.alpha_rad, feedback.alpha_dot_rad, - feedback.alpha_ddot_rad, feedback.radius, - feedback.radius_dot, feedback.radius_ddot, - JointStateSource::Feedback, feedback.valid}; - } - - [[nodiscard]] static Eigen::Vector4d read_required_inputs_( - const InputInterface& left_front, const InputInterface& left_back, - const InputInterface& right_back, const InputInterface& right_front) { - return {*left_front, *left_back, *right_back, *right_front}; - } - - [[nodiscard]] static Eigen::Vector4d read_optional_inputs_( - const InputInterface& left_front, const InputInterface& left_back, - const InputInterface& right_back, const InputInterface& right_front) { - return { - left_front.ready() ? *left_front : nan_, - left_back.ready() ? *left_back : nan_, - right_back.ready() ? *right_back : nan_, - right_front.ready() ? *right_front : nan_, - }; - } - - static void populate_joint_geometry_( - const Eigen::Vector4d& alpha_rad, const Eigen::Vector4d& alpha_dot_rad, - const Eigen::Vector4d& alpha_ddot_rad, double chassis_radius, double rod_length, - Eigen::Vector4d& radius, Eigen::Vector4d& radius_dot, Eigen::Vector4d& radius_ddot) { - radius = chassis_radius + rod_length * alpha_rad.array().cos(); - radius_dot = -rod_length * alpha_rad.array().sin() * alpha_dot_rad.array(); - radius_ddot = -rod_length * alpha_rad.array().cos() * alpha_dot_rad.array().square() - - rod_length * alpha_rad.array().sin() * alpha_ddot_rad.array(); - } - - [[nodiscard]] JointFeedbackStates update_joint_feedback_states_() { - JointFeedbackStates joint; - joint.alpha_rad = read_required_inputs_( - left_front_joint_angle_, left_back_joint_angle_, right_back_joint_angle_, - right_front_joint_angle_); - joint.alpha_dot_rad = read_required_inputs_( - left_front_joint_velocity_, left_back_joint_velocity_, right_back_joint_velocity_, - right_front_joint_velocity_); - - if (!joint.alpha_rad.array().isFinite().all() - || !joint.alpha_dot_rad.array().isFinite().all()) - return joint; - - if (last_joint_velocity_valid_) { - joint.alpha_ddot_rad = (joint.alpha_dot_rad - last_joint_velocity_) / dt_; - } - - last_joint_velocity_ = joint.alpha_dot_rad; - last_joint_velocity_valid_ = true; - - populate_joint_geometry_( - joint.alpha_rad, joint.alpha_dot_rad, joint.alpha_ddot_rad, chassis_radius_, - rod_length_, joint.radius, joint.radius_dot, joint.radius_ddot); - - joint.valid = joint.radius.array().isFinite().all() - && joint.radius_dot.array().isFinite().all() - && joint.radius_ddot.array().isFinite().all(); - - return joint; - } - - [[nodiscard]] JointTargetStates update_joint_target_states_() { - JointTargetStates joint; - joint.alpha_rad = read_optional_inputs_( - left_front_joint_target_physical_angle_, left_back_joint_target_physical_angle_, - right_back_joint_target_physical_angle_, right_front_joint_target_physical_angle_); - - if (!joint.alpha_rad.array().isFinite().all()) - return joint; - - const Eigen::Vector4d target_velocity = read_optional_inputs_( - left_front_joint_target_physical_velocity_, left_back_joint_target_physical_velocity_, - right_back_joint_target_physical_velocity_, - right_front_joint_target_physical_velocity_); - if (target_velocity.array().isFinite().all()) { - joint.alpha_dot_rad = target_velocity; - joint.has_velocity = true; - } else if (last_joint_target_angle_valid_) { - joint.alpha_dot_rad = (joint.alpha_rad - last_joint_target_angle_) / dt_; - joint.has_velocity = true; - } - - const Eigen::Vector4d target_acceleration = read_optional_inputs_( - left_front_joint_target_physical_acceleration_, - left_back_joint_target_physical_acceleration_, - right_back_joint_target_physical_acceleration_, - right_front_joint_target_physical_acceleration_); - if (target_acceleration.array().isFinite().all()) { - joint.alpha_ddot_rad = target_acceleration; - joint.has_acceleration = true; - } - - last_joint_target_angle_ = joint.alpha_rad; - last_joint_target_angle_valid_ = true; - - populate_joint_geometry_( - joint.alpha_rad, joint.alpha_dot_rad, joint.alpha_ddot_rad, chassis_radius_, - rod_length_, joint.radius, joint.radius_dot, joint.radius_ddot); - - joint.valid = joint.radius.array().isFinite().all() - && joint.radius_dot.array().isFinite().all() - && joint.radius_ddot.array().isFinite().all(); - return joint; - } - - void reset_all_controls() { - control_acceleration_filter_.reset(); - - chassis_yaw_angle_imu_ = 0.0; - chassis_velocity_expected_ = Eigen::Vector3d::Zero(); - vehicle_radius_ = Eigen::Vector4d::Constant(chassis_radius_ + rod_length_); - last_joint_velocity_ = Eigen::Vector4d::Zero(); - last_joint_velocity_valid_ = false; - last_joint_target_angle_ = Eigen::Vector4d::Zero(); - last_joint_target_angle_valid_ = false; - - *left_front_steering_control_torque_ = 0.0; - *left_back_steering_control_torque_ = 0.0; - *right_back_steering_control_torque_ = 0.0; - *right_front_steering_control_torque_ = 0.0; - - *left_front_wheel_control_torque_ = 0.0; - *left_back_wheel_control_torque_ = 0.0; - *right_back_wheel_control_torque_ = 0.0; - *right_front_wheel_control_torque_ = 0.0; - } - - void integral_yaw_angle_imu() { - chassis_yaw_angle_imu_ += *chassis_yaw_velocity_imu_ * dt_; - chassis_yaw_angle_imu_ = std::fmod(chassis_yaw_angle_imu_, 2 * std::numbers::pi); - } - - [[nodiscard]] SteeringStatus calculate_steering_status() const { - SteeringStatus steering_status; - steering_status.angle = read_required_inputs_( - left_front_steering_angle_, left_back_steering_angle_, right_back_steering_angle_, - right_front_steering_angle_); - steering_status.angle.array() -= std::numbers::pi / 4; - steering_status.cos_angle = steering_status.angle.array().cos(); - steering_status.sin_angle = steering_status.angle.array().sin(); - - for (size_t i = 0; i < kWheelCount; ++i) { - const double angle_minus_phi = steering_status.angle[i] - phi_[i]; - steering_status.sin_angle_minus_phi[i] = std::sin(angle_minus_phi); - steering_status.cos_angle_minus_phi[i] = std::cos(angle_minus_phi); - } - - steering_status.velocity = read_required_inputs_( - left_front_steering_velocity_, left_back_steering_velocity_, - right_back_steering_velocity_, right_front_steering_velocity_); - return steering_status; - } - - [[nodiscard]] Eigen::Vector4d calculate_wheel_velocities() const { - return read_required_inputs_( - left_front_wheel_velocity_, left_back_wheel_velocity_, right_back_wheel_velocity_, - right_front_wheel_velocity_); - } - - /** - * @brief Observe chassis velocity from wheel velocities using least squares - * - * Solves: A·x = b for x = [vx, vy, ωz] - * where A_i = [cos(ζᵢ), sin(ζᵢ), R_i·sin(ζᵢ - φᵢ)] - * b_i = r·ωᵢ - Ṙᵢ·cos(ζᵢ - φᵢ) - */ - [[nodiscard]] Eigen::Vector3d calculate_chassis_velocity( - const SteeringStatus& steering_status, Eigen::Ref wheel_velocities, - const JointFeedbackStates& joint) const { - Eigen::Vector4d wheel_velocities_eff = wheel_velocities; - if (joint.valid) { - const Eigen::Vector4d clamped_radius_dot = - joint.radius_dot.cwiseMax(-0.1).cwiseMin(0.1); - const Eigen::Vector4d wheel_omega_mech = - (clamped_radius_dot.array() * phi_cos_vec_.array() - * steering_status.cos_angle.array() - + clamped_radius_dot.array() * phi_sin_vec_.array() - * steering_status.sin_angle.array()) - / wheel_radius_; - wheel_velocities_eff -= wheel_omega_mech; - } - - const double one_quarter_r = wheel_radius_ / 4.0; - Eigen::Vector3d velocity; - velocity.x() = one_quarter_r * wheel_velocities_eff.dot(steering_status.cos_angle); - velocity.y() = one_quarter_r * wheel_velocities_eff.dot(steering_status.sin_angle); - velocity.z() = - -one_quarter_r - * (-wheel_velocities_eff[0] * steering_status.sin_angle[0] / vehicle_radius_[0] - + wheel_velocities_eff[1] * steering_status.cos_angle[1] / vehicle_radius_[1] - + wheel_velocities_eff[2] * steering_status.sin_angle[2] / vehicle_radius_[2] - - wheel_velocities_eff[3] * steering_status.cos_angle[3] / vehicle_radius_[3]); - return velocity; - } - - /** - * @brief Calculate expected chassis status with energy scaling - * - * Wheel center velocity: v_i = v + ω·R_i·e_t,i + Ṙᵢ·e_r,i - */ - [[nodiscard]] ChassisStatus calculate_chassis_status_expected( - Eigen::Ref chassis_velocity, const JointTargetStates& joint_target, - const JointFeedbackStates& joint_feedback) { - const double chassis_energy = calculate_chassis_energy(chassis_velocity); - const double chassis_energy_expected = calculate_chassis_energy(chassis_velocity_expected_); - - if (std::isfinite(chassis_energy) && std::isfinite(chassis_energy_expected) - && chassis_energy_expected > chassis_energy && chassis_energy_expected > 1e-12) { - const double k = std::sqrt(chassis_energy / chassis_energy_expected); - if (std::isfinite(k) && k >= 0.0) - chassis_velocity_expected_ *= k; - } - - ChassisStatus chassis_status_expected; - chassis_status_expected.velocity = odom_to_base_link_vector(chassis_velocity_expected_); - - const auto joint = select_joint_state(joint_target, joint_feedback); - - const double vx = chassis_status_expected.velocity.x(); - const double vy = chassis_status_expected.velocity.y(); - const double vz = chassis_status_expected.velocity.z(); - for (size_t i = 0; i < kWheelCount; ++i) { - const double radius = joint.valid ? joint.radius[i] : vehicle_radius_[i]; - const double clamped_radius_dot = - joint.valid ? std::clamp(joint.radius_dot[i], -0.1, 0.1) : 0.0; - const Eigen::Vector2d wheel_velocity = Eigen::Vector2d(vx, vy) - + vz * radius * tangential_unit_fast_(i) - + clamped_radius_dot * radial_unit_fast_(i); - chassis_status_expected.wheel_velocity_x[i] = wheel_velocity.x(); - chassis_status_expected.wheel_velocity_y[i] = wheel_velocity.y(); - } - - return chassis_status_expected; - } - - [[nodiscard]] Eigen::Vector3d calculate_chassis_control_velocity() const { - Eigen::Vector3d chassis_control_velocity = chassis_control_velocity_->vector; - chassis_control_velocity.head<2>() = - Eigen::Rotation2Dd(-std::numbers::pi / 4) * chassis_control_velocity.head<2>(); - return chassis_control_velocity; - } - - [[nodiscard]] Eigen::Vector3d calculate_chassis_control_acceleration( - Eigen::Ref chassis_velocity_expected, - Eigen::Ref chassis_control_velocity) { - Eigen::Vector2d translational_control_acceleration = - chassis_translational_velocity_pid_.update( - chassis_control_velocity.head<2>() - chassis_velocity_expected.head<2>()); - - const double angular_control_acceleration = chassis_angular_velocity_pid_.update( - chassis_control_velocity[2] - chassis_velocity_expected[2]); - - Eigen::Vector3d chassis_control_acceleration; - chassis_control_acceleration << translational_control_acceleration, - angular_control_acceleration; - if (chassis_control_acceleration.lpNorm<1>() < 1e-1) - chassis_control_acceleration.setZero(); - return chassis_control_acceleration; - } - - [[nodiscard]] Eigen::Vector4d calculate_wheel_pid_torques( - const SteeringStatus& steering_status, Eigen::Ref wheel_velocities, - const ChassisStatus& chassis_status_expected) { - const Eigen::Vector4d wheel_control_velocity = - chassis_status_expected.wheel_velocity_x.array() * steering_status.cos_angle.array() - + chassis_status_expected.wheel_velocity_y.array() * steering_status.sin_angle.array(); - return wheel_velocity_pid_.update( - wheel_control_velocity / wheel_radius_ - wheel_velocities); - } - - [[nodiscard]] Eigen::Vector3d constrain_chassis_control_acceleration( - const SteeringStatus& steering_status, Eigen::Ref wheel_velocities, - const JointTargetStates& joint_target, - Eigen::Ref chassis_acceleration, - Eigen::Ref wheel_pid_torques, const double& power_limit) { - Eigen::Vector2d translational_acceleration_direction = chassis_acceleration.head<2>(); - double translational_acceleration_max = translational_acceleration_direction.norm(); - if (translational_acceleration_max > 0.0) - translational_acceleration_direction /= translational_acceleration_max; - - double angular_acceleration_max = chassis_acceleration.z(); - double angular_acceleration_direction = angular_acceleration_max > 0 ? 1.0 : -1.0; - angular_acceleration_max *= angular_acceleration_direction; - - const double rhombus_right = friction_coefficient_ * g_; - const double constraint_radius = - joint_target.valid ? joint_target.radius.mean() : vehicle_radius_.mean(); - const double rhombus_top = rhombus_right * mass_ * constraint_radius / moment_of_inertia_; - - const auto params = calculate_ellipse_parameters( - steering_status, wheel_velocities, joint_target, translational_acceleration_direction, - angular_acceleration_direction, wheel_pid_torques); - - const QcpSolver::QuadraticConstraint quadratic_constraint{ - params.a, params.b, params.c, params.d, params.e, params.f - power_limit}; - - Eigen::Vector2d best_point = qcp_solver_.solve( - {1.0, 0.2}, {translational_acceleration_max, angular_acceleration_max}, - {rhombus_right, rhombus_top}, quadratic_constraint); - - const double min_translational = 0.3 * rhombus_right; - if (best_point.x() < min_translational - && translational_acceleration_max > min_translational) - best_point.x() = min_translational; - - Eigen::Vector3d best_acceleration; - best_acceleration << best_point.x() * translational_acceleration_direction, - best_point.y() * angular_acceleration_direction; - return best_acceleration; - } - - [[nodiscard]] EllipseParameters calculate_ellipse_parameters( - const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities, - const JointTargetStates& joint_target, - const Eigen::Vector2d& translational_acceleration_direction, - const double& angular_acceleration_direction, - const Eigen::Vector4d& wheel_torque_base) const { - EllipseParameters params{0, 0, 0, 0, 0, 0}; - - for (size_t i = 0; i < kWheelCount; ++i) { - const double constraint_radius = - joint_target.valid ? joint_target.radius[i] : vehicle_radius_[i]; - const double cos_alpha_minus_gamma = - steering_status.cos_angle[i] * translational_acceleration_direction.x() - + steering_status.sin_angle[i] * translational_acceleration_direction.y(); - const double sin_alpha_minus_varphi = steering_status.sin_angle_minus_phi[i]; - const double double_k1_torque_base_plus_wheel_velocity = - 2 * k1_ * wheel_torque_base[i] + wheel_velocities[i]; - - params.a += ellipse_coeff_quadratic_translational_ * cos_alpha_minus_gamma - * cos_alpha_minus_gamma; - params.b += ellipse_coeff_cross_term_ * angular_acceleration_direction - * cos_alpha_minus_gamma * sin_alpha_minus_varphi / constraint_radius; - params.c += ellipse_coeff_quadratic_angular_ * sin_alpha_minus_varphi - * sin_alpha_minus_varphi / (constraint_radius * constraint_radius); - params.d += ellipse_coeff_linear_translational_ - * double_k1_torque_base_plus_wheel_velocity * cos_alpha_minus_gamma; - params.e += ellipse_coeff_linear_angular_ * angular_acceleration_direction - * double_k1_torque_base_plus_wheel_velocity * sin_alpha_minus_varphi - / constraint_radius; - params.f += wheel_torque_base[i] * (k1_ * wheel_torque_base[i] + wheel_velocities[i]); - } - - return params; - } - - [[nodiscard]] Eigen::Vector4d calculate_steering_control_torques( - const SteeringStatus& steering_status, const ChassisStatus& chassis_status_expected, - const JointTargetStates& joint_target, const JointFeedbackStates& joint_feedback, - const Eigen::Vector3d& chassis_acceleration) { - const double vx = chassis_status_expected.velocity.x(); - const double vy = chassis_status_expected.velocity.y(); - const double vz = chassis_status_expected.velocity.z(); - const double ax = chassis_acceleration.x(); - const double ay = chassis_acceleration.y(); - const double az = chassis_acceleration.z(); - - const auto joint = select_joint_state(joint_target, joint_feedback); - if (!joint.valid) [[unlikely]] - return Eigen::Vector4d::Zero(); - - Eigen::Vector4d dot_r_squared = chassis_status_expected.wheel_velocity_x.array().square() - + chassis_status_expected.wheel_velocity_y.array().square(); - - Eigen::Vector4d steering_control_velocity = - vx * ay - vy * ax - vz * (vx * vx + vy * vy) - + joint.radius.array() * (az * vx - vz * (ax + vz * vy)) * phi_cos_vec_.array() - + joint.radius.array() * (az * vy - vz * (ay - vz * vx)) * phi_sin_vec_.array(); - Eigen::Vector4d steering_control_angle; - - for (size_t i = 0; i < kWheelCount; ++i) { - if (dot_r_squared[i] > 1e-2) { - steering_control_velocity[i] /= dot_r_squared[i]; - steering_control_angle[i] = std::atan2( - chassis_status_expected.wheel_velocity_y[i], - chassis_status_expected.wheel_velocity_x[i]); - } else { - const double x = - ax - joint.radius[i] * (az * phi_sin_vec_[i] + vz * vz * phi_cos_vec_[i]); - const double y = - ay + joint.radius[i] * (az * phi_cos_vec_[i] - vz * vz * phi_sin_vec_[i]); - if (x * x + y * y > 1e-6) { - steering_control_velocity[i] = 0.0; - steering_control_angle[i] = std::atan2(y, x); - } else { - steering_control_velocity[i] = nan_; - steering_control_angle[i] = nan_; - } - } - } - - Eigen::Vector4d steering_torque = steering_velocity_pid_.update( - steering_control_velocity - + steering_angle_pid_.update( - (steering_control_angle - steering_status.angle).unaryExpr([](double diff) { - diff = std::fmod(diff, std::numbers::pi); - if (diff < -std::numbers::pi / 2) - diff += std::numbers::pi; - else if (diff > std::numbers::pi / 2) - diff -= std::numbers::pi; - return diff; - })) - - steering_status.velocity); - - return steering_torque.unaryExpr([](double v) { return std::isnan(v) ? 0.0 : v; }); - } - - [[nodiscard]] Eigen::Vector4d calculate_wheel_control_torques( - const SteeringStatus& steering_status, const JointTargetStates& joint_target, - const JointFeedbackStates& joint_feedback, const Eigen::Vector3d& chassis_acceleration, - const Eigen::Vector4d& wheel_pid_torques) const { - const auto joint = select_joint_state(joint_target, joint_feedback); - - const double ax = chassis_acceleration.x(); - const double ay = chassis_acceleration.y(); - const double az = chassis_acceleration.z(); - - Eigen::Vector4d wheel_torque = - wheel_radius_ - * (ax * mass_ * steering_status.cos_angle.array() - + ay * mass_ * steering_status.sin_angle.array() - + az * moment_of_inertia_ * steering_status.sin_angle_minus_phi.array() - / joint.radius.array()) - / 4.0; - - wheel_torque += wheel_pid_torques; - return wheel_torque; - } - - void update_control_torques( - const Eigen::Vector4d& steering_torque, const Eigen::Vector4d& wheel_torque) { - *left_front_steering_control_torque_ = steering_torque[0]; - *left_back_steering_control_torque_ = steering_torque[1]; - *right_back_steering_control_torque_ = steering_torque[2]; - *right_front_steering_control_torque_ = steering_torque[3]; - - *left_front_wheel_control_torque_ = wheel_torque[0]; - *left_back_wheel_control_torque_ = wheel_torque[1]; - *right_back_wheel_control_torque_ = wheel_torque[2]; - *right_front_wheel_control_torque_ = wheel_torque[3]; - } - - void update_chassis_velocity_expected(const Eigen::Vector3d& chassis_acceleration) { - chassis_velocity_expected_ += dt_ * base_link_to_odom_vector(chassis_acceleration); - } - - Eigen::Vector3d base_link_to_odom_vector(Eigen::Vector3d vector) const { - vector.head<2>() = Eigen::Rotation2Dd(chassis_yaw_angle_imu_) * vector.head<2>(); - return vector; - } - - Eigen::Vector3d odom_to_base_link_vector(Eigen::Vector3d vector) const { - vector.head<2>() = Eigen::Rotation2Dd(-chassis_yaw_angle_imu_) * vector.head<2>(); - return vector; - } - - [[nodiscard]] double calculate_chassis_energy(const Eigen::Vector3d& velocity) const { - return mass_ * velocity.head<2>().squaredNorm() - + moment_of_inertia_ * velocity.z() * velocity.z(); - } - - static Eigen::Vector2d radial_unit_(double phi) { return {std::cos(phi), std::sin(phi)}; } - - static Eigen::Vector2d tangential_unit_(double phi) { return {-std::sin(phi), std::cos(phi)}; } - - [[nodiscard]] Eigen::Vector2d radial_unit_fast_(size_t wheel_index) const { - return {phi_cos_[wheel_index], phi_sin_[wheel_index]}; - } - - [[nodiscard]] Eigen::Vector2d tangential_unit_fast_(size_t wheel_index) const { - return {-phi_sin_[wheel_index], phi_cos_[wheel_index]}; - } - - static double wrap_to_half_pi_(double diff) { - diff = std::fmod(diff, std::numbers::pi); - if (diff < -std::numbers::pi / 2) - diff += std::numbers::pi; - else if (diff > std::numbers::pi / 2) - diff -= std::numbers::pi; - return diff; - } - - static constexpr std::array phi_ = { - 0.0, - std::numbers::pi / 2, - std::numbers::pi, - -std::numbers::pi / 2, - }; - - static constexpr std::array phi_cos_ = { - 1.0, - 0.0, - -1.0, - 0.0, - }; - - static constexpr std::array phi_sin_ = { - 0.0, - 1.0, - 0.0, - -1.0, - }; - - static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - static constexpr double dt_ = 1e-3; - static constexpr double g_ = 9.81; - - const double mass_; - const double moment_of_inertia_; - const double chassis_radius_; - const double rod_length_; - const double wheel_radius_; - const double friction_coefficient_; - const double k1_; - const double k2_; - const double no_load_power_; - - // Precomputed constants for calculate_ellipse_parameters - const double ellipse_coeff_quadratic_translational_; // k1 * mass^2 * wheel_radius^2 / 16 - const double ellipse_coeff_cross_term_; // k1 * mass * moment_of_inertia * wheel_radius^2 / 8 - const double ellipse_coeff_quadratic_angular_; // k1 * moment_of_inertia^2 * wheel_radius^2 / 16 - const double ellipse_coeff_linear_translational_; // mass * wheel_radius / 4 - const double ellipse_coeff_linear_angular_; // moment_of_inertia * wheel_radius / 4 - - Eigen::Vector4d vehicle_radius_; - const Eigen::Vector4d phi_cos_vec_{1.0, 0.0, -1.0, 0.0}; - const Eigen::Vector4d phi_sin_vec_{0.0, 1.0, 0.0, -1.0}; - Eigen::Vector4d last_joint_velocity_ = Eigen::Vector4d::Zero(); - bool last_joint_velocity_valid_ = false; - Eigen::Vector4d last_joint_target_angle_ = Eigen::Vector4d::Zero(); - bool last_joint_target_angle_valid_ = false; - - InputInterface joystick_right_; - InputInterface joystick_left_; - - InputInterface left_front_steering_angle_; - InputInterface left_back_steering_angle_; - InputInterface right_back_steering_angle_; - InputInterface right_front_steering_angle_; - - InputInterface left_front_steering_velocity_; - InputInterface left_back_steering_velocity_; - InputInterface right_back_steering_velocity_; - InputInterface right_front_steering_velocity_; - - InputInterface left_front_wheel_velocity_; - InputInterface left_back_wheel_velocity_; - InputInterface right_back_wheel_velocity_; - InputInterface right_front_wheel_velocity_; - - InputInterface left_front_joint_angle_; - InputInterface left_back_joint_angle_; - InputInterface right_back_joint_angle_; - InputInterface right_front_joint_angle_; - - InputInterface left_front_joint_velocity_; - InputInterface left_back_joint_velocity_; - InputInterface right_back_joint_velocity_; - InputInterface right_front_joint_velocity_; - - InputInterface left_front_joint_target_physical_angle_; - InputInterface left_back_joint_target_physical_angle_; - InputInterface right_back_joint_target_physical_angle_; - InputInterface right_front_joint_target_physical_angle_; - InputInterface left_front_joint_target_physical_velocity_; - InputInterface left_back_joint_target_physical_velocity_; - InputInterface right_back_joint_target_physical_velocity_; - InputInterface right_front_joint_target_physical_velocity_; - InputInterface left_front_joint_target_physical_acceleration_; - InputInterface left_back_joint_target_physical_acceleration_; - InputInterface right_back_joint_target_physical_acceleration_; - InputInterface right_front_joint_target_physical_acceleration_; - - InputInterface chassis_yaw_velocity_imu_; - InputInterface chassis_control_velocity_; - InputInterface power_limit_; - - OutputInterface left_front_steering_control_torque_; - OutputInterface left_back_steering_control_torque_; - OutputInterface right_back_steering_control_torque_; - OutputInterface right_front_steering_control_torque_; - - OutputInterface left_front_wheel_control_torque_; - OutputInterface left_back_wheel_control_torque_; - OutputInterface right_back_wheel_control_torque_; - OutputInterface right_front_wheel_control_torque_; - - QcpSolver qcp_solver_; - filter::LowPassFilter<3> control_acceleration_filter_; - - double chassis_yaw_angle_imu_ = 0.0; - Eigen::Vector3d chassis_velocity_expected_ = Eigen::Vector3d::Zero(); - - pid::MatrixPidCalculator<2> chassis_translational_velocity_pid_; - pid::PidCalculator chassis_angular_velocity_pid_; - pid::MatrixPidCalculator<4> steering_velocity_pid_; - pid::MatrixPidCalculator<4> steering_angle_pid_; - pid::MatrixPidCalculator<4> wheel_velocity_pid_; -}; - -} // namespace rmcs_core::controller::chassis - -#include - -PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::chassis::DeformableChassisController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp index 6d3fbb720..9396df1d1 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp @@ -5,17 +5,16 @@ #include #include -#include +#include #include #include #include +#include #include #include namespace rmcs_core::controller::gimbal { -using namespace rmcs_description; - class DeformableInfantryGimbalController : public rmcs_executor::Component , public rclcpp::Node { @@ -24,18 +23,25 @@ class DeformableInfantryGimbalController : Node( get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + configure_pid("yaw_angle", yaw_angle_pid_); configure_pid("yaw_velocity", yaw_velocity_pid_); configure_pid("pitch_angle", pitch_angle_pid_); configure_pid("pitch_velocity", pitch_velocity_pid_); + get_parameter("pitch_torque_control", pitch_torque_control_enabled_); get_parameter("manual_joystick_sensitivity", joystick_sensitivity_); get_parameter("manual_mouse_sensitivity", mouse_sensitivity_); + + get_parameter_or("pitch_gravity_ff_gain", pitch_gravity_ff_gain_, 0.0); + get_parameter_or("pitch_gravity_ff_phase", pitch_gravity_ff_phase_, 0.0); + get_parameter_or("ctrl_hold_pitch_target_angle", ctrl_hold_pitch_target_angle_, 0.0); } auto update() -> void override { const auto switch_right = *input_.switch_right; const auto switch_left = *input_.switch_left; + const auto keyboard = *input_.keyboard; using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) @@ -44,6 +50,14 @@ class DeformableInfantryGimbalController return; } + update_pitch_lock_state(switch_left, switch_right, keyboard); + + if (ctrl_hold_requested()) { + update_ctrl_hold_control(); + } else { + deactivate_ctrl_hold(); + } + const auto auto_aim_active = auto_aim_requested() && input_.auto_aim_should_control.ready() && *input_.auto_aim_should_control && input_.auto_aim_control_direction.ready() @@ -53,32 +67,49 @@ class DeformableInfantryGimbalController auto_aim_active ? update_auto_aim_control() : update_manual_control(); *output_.yaw_angle_error = angle_error.yaw_angle_error; - *output_.pitch_angle_error = angle_error.pitch_angle_error; + if (!ctrl_hold_active_) + *output_.pitch_angle_error = angle_error.pitch_angle_error; - if (!std::isfinite(angle_error.yaw_angle_error) - || !std::isfinite(angle_error.pitch_angle_error)) { - reset_control_outputs(); - return; + if (!std::isfinite(angle_error.yaw_angle_error)) { + yaw_angle_pid_.reset(); + yaw_velocity_pid_.reset(); + *output_.yaw_control_torque = kNaN; } - const auto yaw_velocity_ref = yaw_angle_pid_.update(angle_error.yaw_angle_error); - const auto pitch_velocity_ref = pitch_angle_pid_.update(angle_error.pitch_angle_error); + if (std::isfinite(angle_error.yaw_angle_error)) { + const auto yaw_velocity_ref = yaw_angle_pid_.update(angle_error.yaw_angle_error); + *output_.yaw_control_torque = + yaw_velocity_pid_.update(yaw_velocity_ref - *input_.yaw_velocity_imu); + } - *output_.yaw_control_torque = - yaw_velocity_pid_.update(yaw_velocity_ref - *input_.yaw_velocity_imu); - if (pitch_torque_control_enabled_) { - *output_.pitch_control_velocity = kNaN; - *output_.pitch_control_torque = - pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity_imu); - } else { - pitch_velocity_pid_.reset(); - *output_.pitch_control_velocity = pitch_velocity_ref; - *output_.pitch_control_torque = kNaN; + if (!ctrl_hold_active_) { + if (!std::isfinite(angle_error.pitch_angle_error)) { + pitch_angle_pid_.reset(); + pitch_velocity_pid_.reset(); + *output_.pitch_control_velocity = kNaN; + *output_.pitch_control_torque = kNaN; + } else { + const auto pitch_gravity_ff = pitch_gravity_feedforward(); + const auto pitch_velocity_ref = + pitch_angle_pid_.update(angle_error.pitch_angle_error); + + if (pitch_torque_control_enabled_) { + *output_.pitch_control_velocity = kNaN; + *output_.pitch_control_torque = + pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity_imu) + + pitch_gravity_ff; + } else { + pitch_velocity_pid_.reset(); + *output_.pitch_control_velocity = pitch_velocity_ref; + *output_.pitch_control_torque = kNaN; + } + } } } private: static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); + static constexpr auto kDefaultDt = 1e-3; auto configure_pid(const std::string& prefix, pid::PidCalculator& calculator) -> void { get_parameter(prefix + "_integral_min", calculator.integral_min); @@ -92,13 +123,16 @@ class DeformableInfantryGimbalController struct Input { explicit Input(rmcs_executor::Component& component) { component.register_input("/remote/joystick/left", joystick_left); + component.register_input("/remote/keyboard", keyboard); component.register_input("/remote/switch/right", switch_right); component.register_input("/remote/switch/left", switch_left); component.register_input("/remote/mouse/velocity", mouse_velocity); component.register_input("/remote/mouse", mouse); + component.register_input("/predefined/update_rate", update_rate, false); - component.register_input("/tf", tf); + component.register_input("/gimbal/yaw/angle", yaw_angle); component.register_input("/gimbal/yaw/velocity", yaw_velocity); + component.register_input("/gimbal/pitch/angle", pitch_angle); component.register_input("/gimbal/pitch/velocity", pitch_velocity); component.register_input("/gimbal/yaw/velocity_imu", yaw_velocity_imu); component.register_input("/gimbal/pitch/velocity_imu", pitch_velocity_imu); @@ -109,13 +143,16 @@ class DeformableInfantryGimbalController } InputInterface joystick_left; + InputInterface keyboard; InputInterface switch_right; InputInterface switch_left; InputInterface mouse_velocity; InputInterface mouse; + InputInterface update_rate; - InputInterface tf; + InputInterface yaw_angle; InputInterface yaw_velocity; + InputInterface pitch_angle; InputInterface pitch_velocity; InputInterface yaw_velocity_imu; InputInterface pitch_velocity_imu; @@ -127,20 +164,38 @@ class DeformableInfantryGimbalController struct Output { explicit Output(rmcs_executor::Component& component) { component.register_output("/gimbal/yaw/control_torque", yaw_control_torque, kNaN); + component.register_output("/gimbal/yaw/control_angle", yaw_control_angle, kNaN); component.register_output( "/gimbal/pitch/control_velocity", pitch_control_velocity, kNaN); component.register_output("/gimbal/pitch/control_torque", pitch_control_torque, kNaN); + component.register_output("/gimbal/pitch/control_angle", pitch_control_angle, kNaN); component.register_output("/gimbal/yaw/control_angle_error", yaw_angle_error, kNaN); component.register_output("/gimbal/pitch/control_angle_error", pitch_angle_error, kNaN); } OutputInterface yaw_control_torque; + OutputInterface yaw_control_angle; OutputInterface pitch_control_velocity; OutputInterface pitch_control_torque; + OutputInterface pitch_control_angle; OutputInterface yaw_angle_error; OutputInterface pitch_angle_error; } output_{*this}; + auto ctrl_hold_requested() const -> bool { return pitch_lock_active_; } + + auto update_dt() const -> double { + if (input_.update_rate.ready() && std::isfinite(*input_.update_rate) + && *input_.update_rate > 1e-6) + return 1.0 / *input_.update_rate; + return kDefaultDt; + } + + auto manual_yaw_shift() const -> double { + return joystick_sensitivity_ * input_.joystick_left->y() + + mouse_sensitivity_ * input_.mouse_velocity->y(); + } + auto auto_aim_requested() const -> bool { return input_.mouse->right || *input_.switch_right == rmcs_msgs::Switch::UP; } @@ -155,24 +210,97 @@ class DeformableInfantryGimbalController if (!gimbal_solver_.enabled()) return gimbal_solver_.update(TwoAxisGimbalSolver::SetToLevel{}); - const auto yaw_shift = joystick_sensitivity_ * input_.joystick_left->y() - + mouse_sensitivity_ * input_.mouse_velocity->y(); + const auto yaw_shift = manual_yaw_shift(); + const auto pitch_shift = -joystick_sensitivity_ * input_.joystick_left->x() + mouse_sensitivity_ * input_.mouse_velocity->x(); return gimbal_solver_.update(TwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); } + auto pitch_gravity_feedforward() const -> double { + if (ctrl_hold_active_) + return 0.0; + if (!input_.pitch_angle.ready() || !std::isfinite(*input_.pitch_angle)) + return 0.0; + return pitch_gravity_ff_gain_ * std::sin(*input_.pitch_angle - pitch_gravity_ff_phase_); + } + + auto update_pitch_lock_state( + rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, + const rmcs_msgs::Keyboard& keyboard) -> void { + if (switch_left == rmcs_msgs::Switch::DOWN && switch_right == rmcs_msgs::Switch::UP + && last_switch_right_ == rmcs_msgs::Switch::MIDDLE) { + suspension_on_by_switch_ = !suspension_on_by_switch_; + } + + pitch_lock_active_ = keyboard.ctrl || suspension_on_by_switch_; + last_switch_right_ = switch_right; + } + + auto activate_ctrl_hold() -> void { + ctrl_hold_active_ = true; + pitch_angle_pid_.reset(); + pitch_velocity_pid_.reset(); + } + + auto deactivate_ctrl_hold() -> void { + if (!ctrl_hold_active_) + return; + + ctrl_hold_active_ = false; + pitch_angle_pid_.reset(); + pitch_velocity_pid_.reset(); + *output_.pitch_control_angle = kNaN; + } + + auto update_ctrl_hold_control() -> void { + if (!ctrl_hold_active_) + activate_ctrl_hold(); + + *output_.yaw_control_angle = kNaN; + *output_.pitch_control_velocity = kNaN; + *output_.pitch_control_torque = kNaN; + *output_.pitch_control_angle = kNaN; + + if (input_.pitch_angle.ready() && std::isfinite(*input_.pitch_angle)) { + auto pitch_target_error = ctrl_hold_pitch_target_angle_ - *input_.pitch_angle; + if (pitch_target_error > std::numbers::pi) + pitch_target_error -= 2 * std::numbers::pi; + else if (pitch_target_error < -std::numbers::pi) + pitch_target_error += 2 * std::numbers::pi; + + *output_.pitch_angle_error = pitch_target_error; + const auto pitch_velocity_ref = pitch_angle_pid_.update(pitch_target_error); + if (pitch_torque_control_enabled_) { + *output_.pitch_control_velocity = kNaN; + *output_.pitch_control_torque = + pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity_imu) + + pitch_gravity_feedforward(); + } else { + pitch_velocity_pid_.reset(); + *output_.pitch_control_velocity = pitch_velocity_ref; + *output_.pitch_control_torque = kNaN; + } + } + } + auto reset_control_outputs() -> void { yaw_angle_pid_.reset(); yaw_velocity_pid_.reset(); pitch_angle_pid_.reset(); pitch_velocity_pid_.reset(); *output_.yaw_control_torque = kNaN; + *output_.yaw_control_angle = kNaN; *output_.pitch_control_velocity = kNaN; *output_.pitch_control_torque = kNaN; + *output_.pitch_control_angle = kNaN; } auto reset_all_controls() -> void { + deactivate_ctrl_hold(); + pitch_lock_active_ = false; + suspension_on_by_switch_ = false; + last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; gimbal_solver_.update(TwoAxisGimbalSolver::SetDisabled{}); *output_.yaw_angle_error = kNaN; *output_.pitch_angle_error = kNaN; @@ -180,8 +308,7 @@ class DeformableInfantryGimbalController } TwoAxisGimbalSolver gimbal_solver_{ - *this, get_parameter("upper_limit").as_double(), get_parameter("lower_limit").as_double(), - true}; + *this, get_parameter("upper_limit").as_double(), get_parameter("lower_limit").as_double()}; pid::PidCalculator yaw_angle_pid_{ get_parameter("yaw_angle_kp").as_double(), @@ -207,11 +334,17 @@ class DeformableInfantryGimbalController double joystick_sensitivity_ = 0.003; double mouse_sensitivity_ = 0.5; bool pitch_torque_control_enabled_ = false; + double ctrl_hold_pitch_target_angle_ = 0.0; + double pitch_gravity_ff_gain_ = 0.0; + double pitch_gravity_ff_phase_ = 0.0; + bool pitch_lock_active_ = false; + bool suspension_on_by_switch_ = false; + rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; + bool ctrl_hold_active_ = false; }; } // namespace rmcs_core::controller::gimbal #include - PLUGINLIB_EXPORT_CLASS( rmcs_core::controller::gimbal::DeformableInfantryGimbalController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp new file mode 100644 index 000000000..8efa589ba --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -0,0 +1,876 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "hardware/device/bmi088.hpp" +#include "hardware/device/bmi088_ekf.hpp" +#include "hardware/device/board_clock_lifter.hpp" +#include "hardware/device/can_packet.hpp" +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/lk_motor.hpp" +#include "hardware/device/remote_control.hpp" +#include "hardware/device/supercap.hpp" +#include "hardware/device/vt13.hpp" +#include "hardware/util/status_monitor.hpp" + +namespace rmcs_core::hardware { + +using Clock = std::chrono::steady_clock; + +class DeformableInfantryOmniB + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DeformableInfantryOmniB() + : Node( + get_component_name(), + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) + , command_(create_partner_component(get_component_name() + "_command", *this)) { + using namespace rmcs_description; + + register_input("/predefined/timestamp", timestamp_); + register_output("/tf", tf_); + register_output( + "/auto_aim/camera_transform", camera_transform_, Eigen::Isometry3d::Identity()); + register_output("/auto_aim/barrel_direction", barrel_direction_, Eigen::Vector3d::UnitX()); + register_output("/auto_aim/yaw_velocity", auto_aim_yaw_velocity_, 0.0); + + tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); + + remote_control_ = std::make_unique(*this); + + bottom_board_ = std::make_unique( + *this, *command_, get_parameter("serial_filter_bottom_board").as_string()); + top_board_ = std::make_unique( + *this, *command_, get_parameter("serial_filter_top_board").as_string()); + + // For command: remote-status + using Srv = std_srvs::srv::Trigger; + status_service_ = create_service( + "/rmcs/service/robot_status", + [this](const Srv::Request::SharedPtr&, const Srv::Response::SharedPtr& response) { + status_service_callback(response); + }); + } + + ~DeformableInfantryOmniB() override = default; + + void before_updating() override { top_board_->request_hard_sync_read(); } + + void update() override { + bottom_board_->update(); + top_board_->update(); + remote_control_->update(); + + using namespace rmcs_description; + *camera_transform_ = fast_tf::lookup_transform(*tf_); + *barrel_direction_ = + *fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + *auto_aim_yaw_velocity_ = top_board_->gimbal_yaw_velocity(); + } + + void command_update() { + const bool even = ((cmd_tick_++ & 1u) == 0u); + bottom_board_->command_update(even); + top_board_->command_update(); + } + +private: + static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); + static constexpr auto kLeftFront = 0; + static constexpr auto kLeftBack = 1; + static constexpr auto kRightBack = 2; + static constexpr auto kRightFront = 3; + static constexpr auto kJointName = std::array{ + "left_front", + "left_back", + "right_back", + "right_front", + }; + + class Command : public Component { + public: + explicit Command(DeformableInfantryOmniB& deformableInfantry) + : deformableInfantry(deformableInfantry) {} + + void update() override { deformableInfantry.command_update(); } + + DeformableInfantryOmniB& deformableInfantry; + }; + + struct TopBoard final : public librmcs::board::RmcsBoardLite::Callback { + public: + explicit TopBoard( + DeformableInfantryOmniB& status, Component& command, + const std::string& serial_filter = {}) + : status_{status} + , tf_{status.tf_} + , bmi088_{device::Bmi088Ekf::Config{ + .body_to_sensor = + Eigen::AngleAxisd{std::numbers::pi / 2.0, Eigen::Vector3d::UnitX()} + .toRotationMatrix()}} + , gimbal_pitch_motor_(status, command, "/gimbal/pitch") + , gimbal_left_friction_(status, command, "/gimbal/left_friction") + , gimbal_right_friction_(status, command, "/gimbal/right_friction") { + + gimbal_pitch_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} + .set_reversed() + .set_encoder_zero_point( + static_cast(status.get_parameter("pitch_motor_zero_point").as_int()))); + + gimbal_left_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} + .set_reduction_ratio(1.) + .set_reversed()); + gimbal_right_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2}.set_reduction_ratio( + 1.)); + + status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); + status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); + status.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); + status.register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); + + auto options = librmcs::board::AdvancedOptions{}; + options.dangerously_skip_version_checks = true; + board_ = std::make_unique(*this, serial_filter, options); + + board_->start_transmit().gpio_digital_read( + Spec::kGpios.kUart1Rx, // + { + .period_ms = 0, + .asap = false, + .rising_edge = false, + .falling_edge = true, + .capture_timestamp = true, + .pull = librmcs::data::GpioPull::kUp, + }); + + board_->start_transmit().uart_config(Spec::kUarts.kUart0, {.baudrate = 921600}); + + status_.remote_control_->register_vt13(&vt13_); + } + ~TopBoard() override = default; + + [[nodiscard]] auto gimbal_yaw_velocity() const -> double { + return *gimbal_yaw_velocity_bmi088_; + } + + void request_hard_sync_read() { + // RMCS-lite top board variant currently has no GPIO hard-sync request + // path. + } + + void update() { + vt13_.update_status(); + gimbal_pitch_motor_.update_status(); + gimbal_left_friction_.update_status(); + gimbal_right_friction_.update_status(); + + const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); + + if (auto snapshot = bmi088_.snapshot()) { + *gimbal_pitch_velocity_bmi088_ = snapshot->gyro_body.y(); + *gimbal_yaw_velocity_bmi088_ = snapshot->gyro_body.z(); + tf_->set_transform( + snapshot->orientation.conjugate()); + } + + tf_->set_state( + pitch_encoder_angle); + } + + void command_update() const { + auto builder = board_->start_transmit(); + { + auto packet = gimbal_pitch_motor_.generate_torque_command(); + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x141, + .can_data = packet.as_bytes(), + }); + } + { + auto packet = device::CanPacket8{uint64_t{0}}; + packet << gimbal_right_friction_; + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = gimbal_right_friction_.send_id(), + .can_data = packet.as_bytes(), + }); + } + { + auto packet = device::CanPacket8{uint64_t{0}}; + packet << gimbal_left_friction_; + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = gimbal_left_friction_.send_id(), + .can_data = packet.as_bytes(), + }); + } + } + + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + if (can == Spec::kCans.kCan0) { + if (data.can_id == 0x141) + gimbal_pitch_motor_.store_status(data.can_data); + monitor_.tick("Top::Can0", data.can_id); + } else if (can == Spec::kCans.kCan1) { + gimbal_right_friction_.match_then_store_status(data.can_id, data.can_data); + monitor_.tick("Top::Can1", data.can_id); + } else if (can == Spec::kCans.kCan2) { + gimbal_left_friction_.match_then_store_status(data.can_id, data.can_data); + monitor_.tick("Top::Can2", data.can_id); + } + } + + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + if (uart == Spec::kUarts.kUart0) + vt13_.store_status(data.uart_data); + } + + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { + const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); + bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); + monitor_.tick("Top::Imu", "Acc"); + } + + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); + monitor_.tick("Top::Imu", "Gyr"); + if (!timestamp.has_value()) + return; + auto snapshot = + bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); + if (snapshot) + imu_snapshot_output_.emit(*snapshot); + } + + void gpio_digital_read_result_callback( + const Spec::Gpio& gpio, const View::GpioDigital& data) override { + if (gpio != Spec::kGpios.kUart1Rx) + return; + if (!data.timestamp_quarter_us) + return; + + const auto timestamp = board_clock_lifter_.lift_timestamp(*data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + + camera_signal_output_.emit(*timestamp); + monitor_.tick("Top::CameraSync", "Active"); + } + + auto status() const -> std::vector { return monitor_.text(); } + + DeformableInfantryOmniB& status_; + OutputInterface& tf_; + OutputInterface gimbal_yaw_velocity_bmi088_; + OutputInterface gimbal_pitch_velocity_bmi088_; + + EventOutputInterface imu_snapshot_output_; + EventOutputInterface camera_signal_output_; + + device::Bmi088Ekf bmi088_; + device::BoardClockLifter board_clock_lifter_; + device::Vt13 vt13_; + device::LkMotor gimbal_pitch_motor_; + device::DjiMotor gimbal_left_friction_; + device::DjiMotor gimbal_right_friction_; + + StatusMonitor monitor_{}; + std::unique_ptr board_; + }; + + struct BottomBoard final : public librmcs::board::RmcsBoardLite::Callback { + public: + explicit BottomBoard( + DeformableInfantryOmniB& status, Component& command, + const std::string& serial_filter = {}) + : status_{status} + , command_{command} + , kChassisRadiusBase(status.get_parameter("chassis_radius").as_double()) + , kRodLength(status.get_parameter("rod_length").as_double()) + , kDefaultRadius(kChassisRadiusBase + kRodLength) { + + status.register_output("/referee/serial", referee_serial_); + referee_serial_->read = [this](std::byte* buffer, size_t size) { + return referee_ring_buffer_receive_.pop_front_n( + [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) { + board_->start_transmit().uart_transmit( + Spec::kUarts.kUart0, {.uart_data = std::span{buffer, size}}); + return size; + }; + + gimbal_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( + static_cast(status.get_parameter("yaw_motor_zero_point").as_int()))); + + for (auto& motor : chassis_wheel_motors_) + motor.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} + .set_reversed() + .set_reduction_ratio(13.0) + .enable_multi_turn_angle()); + + for (auto& motor : chassis_joint_motors_) + motor.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei36} + .set_reversed() + .enable_multi_turn_angle()); + + imu_.set_coordinate_mapping([](double x, double y, double z) { + // Keep the existing chassis yaw axis mapping explicit until the bottom-board IMU + // installation is re-validated on hardware. + return std::make_tuple(-y, x, z); + }); + + gimbal_bullet_feeder_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM2006, 3} + .enable_multi_turn_angle()); + + status.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); + status.register_output("/chassis/imu/pitch", chassis_imu_pitch_, 0.0); + status.register_output("/chassis/imu/roll", chassis_imu_roll_, 0.0); + status.register_output("/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, 0.0); + status.register_output("/chassis/imu/roll_rate", chassis_imu_roll_rate_, 0.0); + for (size_t i = 0; i < 4; ++i) { + status.register_output( + std::format( + "/chassis/{}_joint/physical_angle", DeformableInfantryOmniB::kJointName[i]), + joint_physical_angle_[i], kNaN); + status.register_output( + std::format( + "/chassis/{}_joint/physical_velocity", + DeformableInfantryOmniB::kJointName[i]), + joint_physical_velocity_[i], kNaN); + } + status.register_output("/chassis/encoder/alpha", encoder_alpha_, kNaN); + status.register_output("/chassis/encoder/alpha_dot", encoder_alpha_dot_, kNaN); + status.register_output("/chassis/radius", radius_, kDefaultRadius); + + status.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); + status.get_parameter_or("debug_log_wheel_motor", debug_log_wheel_motor_, false); + status.get_parameter_or( + "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); + + auto options = librmcs::board::AdvancedOptions{}; + options.dangerously_skip_version_checks = true; + board_ = std::make_unique(*this, serial_filter, options); + + status_.remote_control_->register_dr16(&dr16_); + } + void update() { + imu_.update_status(); + *chassis_yaw_velocity_imu_ = imu_.gz(); + { + const double q0 = imu_.q0(); + const double q1 = imu_.q1(); + const double q2 = imu_.q2(); + const double q3 = imu_.q3(); + + double sin_pitch = 2.0 * (q0 * q2 - q3 * q1); + sin_pitch = std::clamp(sin_pitch, -1.0, 1.0); + + const double standard_pitch = std::asin(sin_pitch); + const double standard_roll = + std::atan2(2.0 * (q0 * q1 + q2 * q3), 1.0 - 2.0 * (q1 * q1 + q2 * q2)); + + // Export chassis attitude using the requested convention: + // pitch < 0 when the front is higher, roll > 0 when the left side is higher. + *chassis_imu_pitch_ = -standard_pitch; + *chassis_imu_roll_ = standard_roll; + *chassis_imu_pitch_rate_ = -imu_.gy(); + *chassis_imu_roll_rate_ = imu_.gx(); + } + + for (auto& motor : chassis_wheel_motors_) + motor.update_status(); + for (auto& motor : chassis_joint_motors_) + motor.update_status(); + + for (size_t i = 0; i < 4; ++i) + update_joint_physical_feedback_( + i, joint_physical_angle_[i], joint_physical_velocity_[i]); + + update_geometry_feedback_(); + if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) + log_chassis_feedback_once_per_second_(); + + dr16_.update_status(); + gimbal_yaw_motor_.update_status(); + if (supercap_status_received_.load(std::memory_order_relaxed)) + supercap_.update_status(); + if (debug_log_supercap_) + log_supercap_feedback_once_per_second_(); + gimbal_bullet_feeder_.update_status(); + + tf_->set_state( + gimbal_yaw_motor_.angle()); + } + + void command_update(bool even) { + auto builder = board_->start_transmit(); + if (even) { + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kLeftFront].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kLeftBack].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kRightBack].generate_command(), + device::CanPacket8::PaddingQuarter{}, + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan3, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kRightFront].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = 0x142, + .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + } + .as_bytes(), + }); + } else { + for (size_t i = 0; i < 4; ++i) { + switch (i) { + case kLeftFront: + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + case kLeftBack: + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + case kRightBack: + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + case kRightFront: + builder.can_transmit( + Spec::kCans.kCan3, // + { + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + default: break; + } + } + } + } + + static constexpr double kJointZeroPhysicalAngleRad = 62.5 * std::numbers::pi / 180.0; + + DeformableInfantryOmniB& status_; + Component& command_; + + std::unique_ptr board_; + + // Interfaces + + OutputInterface& tf_{status_.tf_}; + + OutputInterface chassis_yaw_velocity_imu_; + OutputInterface chassis_imu_pitch_; + OutputInterface chassis_imu_roll_; + OutputInterface chassis_imu_pitch_rate_; + OutputInterface chassis_imu_roll_rate_; + + std::array, 4> joint_physical_angle_; + std::array, 4> joint_physical_velocity_; + + OutputInterface encoder_alpha_; + OutputInterface encoder_alpha_dot_; + OutputInterface radius_; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; + OutputInterface referee_serial_; + + // State + + std::atomic wheel_status_received_[4] = {false, false, false, false}; + std::atomic joint_status_received_[4] = {false, false, false, false}; + + bool debug_log_supercap_ = false; + bool debug_log_wheel_motor_ = false; + bool debug_log_deformable_joint_motor_ = false; + + const double kChassisRadiusBase; + const double kRodLength; + const double kDefaultRadius; + + Clock::time_point next_chassis_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; + Clock::time_point next_supercap_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; + + // Device + + device::Bmi088 imu_{1000, 0.2, 0.0}; + device::LkMotor gimbal_yaw_motor_{status_, command_, "/gimbal/yaw"}; + device::Dr16 dr16_; + + device::DjiMotor chassis_wheel_motors_[4]{ + device::DjiMotor{status_, command_, "/chassis/left_front_wheel"}, + device::DjiMotor{status_, command_, "/chassis/left_back_wheel"}, + device::DjiMotor{status_, command_, "/chassis/right_back_wheel"}, + device::DjiMotor{status_, command_, "/chassis/right_front_wheel"}, + }; + device::LkMotor chassis_joint_motors_[4]{ + device::LkMotor{status_, command_, "/chassis/left_front_joint"}, + device::LkMotor{status_, command_, "/chassis/left_back_joint"}, + device::LkMotor{status_, command_, "/chassis/right_back_joint"}, + device::LkMotor{status_, command_, "/chassis/right_front_joint"}, + }; + + std::atomic latest_supercap_status_{device::CanPacket8{uint64_t{0}}}; + std::atomic supercap_status_received_{false}; + device::Supercap supercap_{status_, command_}; + + device::DjiMotor gimbal_bullet_feeder_{status_, command_, "/gimbal/bullet_feeder"}; + + void process_chassis_can_receive_(size_t index, const View::Can& data) { + if (data.is_extended_can_id || data.is_remote_transmission) + return; + if (data.can_id == 0x201) { + chassis_wheel_motors_[index].store_status(data.can_data); + wheel_status_received_[index].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x141) { + chassis_joint_motors_[index].store_status(data.can_data); + joint_status_received_[index].store(true, std::memory_order_relaxed); + } + } + + void update_joint_physical_feedback_( + size_t index, OutputInterface& angle_output, + OutputInterface& velocity_output) { + + if (!joint_status_received_[index].load(std::memory_order_relaxed)) { + *angle_output = kNaN; + *velocity_output = kNaN; + return; + } + + const auto to_physical_angle = [](double motor_angle) { + return kJointZeroPhysicalAngleRad - motor_angle; + }; + const auto to_physical_velocity = [](double motor_velocity) { return -motor_velocity; }; + + *angle_output = to_physical_angle(chassis_joint_motors_[index].angle()); + *velocity_output = to_physical_velocity(chassis_joint_motors_[index].velocity()); + } + + void update_geometry_feedback_() { + const Eigen::Vector4d alpha_rad{ + *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], + *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront]}; + const Eigen::Vector4d alpha_dot_rad{ + *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], + *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront]}; + + if (!alpha_rad.array().isFinite().all() || !alpha_dot_rad.array().isFinite().all()) { + *encoder_alpha_ = kNaN; + *encoder_alpha_dot_ = kNaN; + *radius_ = kDefaultRadius; + RCLCPP_WARN_THROTTLE( + status_.get_logger(), *status_.get_clock(), 1000, + "deformable joint feedback invalid, fallback chassis radius to default %.3f m", + kDefaultRadius); + return; + } + + *encoder_alpha_ = alpha_rad.mean(); + *encoder_alpha_dot_ = alpha_dot_rad.mean(); + *radius_ = (kChassisRadiusBase + kRodLength * alpha_rad.array().cos()).mean(); + } + + void log_chassis_feedback_once_per_second_() { + const auto now = Clock::now(); + if (now < next_chassis_feedback_log_time_) + return; + + const auto wheel_rx = [this](size_t index) { + return wheel_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; + }; + const auto joint_rx = [this](size_t index) { + return joint_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; + }; + + if (debug_log_wheel_motor_) { + std::string wheel_rx_str; + for (size_t i = 0; i < 4; ++i) { + if (i > 0) + wheel_rx_str.push_back(' '); + wheel_rx_str.push_back(wheel_rx(i)); + } + RCLCPP_INFO( + status_.get_logger(), + "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " + "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " + "rx=[%s]", + chassis_wheel_motors_[kLeftFront].angle(), + chassis_wheel_motors_[kLeftBack].angle(), + chassis_wheel_motors_[kRightBack].angle(), + chassis_wheel_motors_[kRightFront].angle(), + chassis_wheel_motors_[kLeftFront].angle(), + chassis_wheel_motors_[kLeftBack].angle(), + chassis_wheel_motors_[kRightBack].angle(), + chassis_wheel_motors_[kRightFront].angle(), wheel_rx_str.c_str()); + } + + if (debug_log_deformable_joint_motor_) { + std::string joint_rx_str; + for (size_t i = 0; i < 4; ++i) { + if (i > 0) + joint_rx_str.push_back(' '); + joint_rx_str.push_back(joint_rx(i)); + } + RCLCPP_INFO( + status_.get_logger(), + "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " + "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " + "rx=[%s]", + *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], + *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront], + *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], + *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront], + joint_rx_str.c_str()); + } + + next_chassis_feedback_log_time_ = now + std::chrono::seconds(1); + } + + void log_supercap_feedback_once_per_second_() { + const auto now = Clock::now(); + if (now < next_supercap_feedback_log_time_) + return; + + const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); + auto supercap_raw_packet = latest_supercap_status_.load(std::memory_order_relaxed); + const auto supercap_raw_bytes = supercap_raw_packet.as_bytes(); + + RCLCPP_INFO( + status_.get_logger(), + "[supercap] can1 rx=%c id=0x300 enabled=%d supercap_v=% .3f chassis_v=% .3f " + "power=% .3f raw=[%02X %02X %02X %02X %02X %02X %02X %02X]", + supercap_rx ? 'Y' : 'N', supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, + supercap_rx ? supercap_.supercap_voltage() : kNaN, + supercap_rx ? supercap_.chassis_voltage() : kNaN, + supercap_rx ? supercap_.chassis_power() : kNaN, + std::to_integer(supercap_raw_bytes[0]), + std::to_integer(supercap_raw_bytes[1]), + std::to_integer(supercap_raw_bytes[2]), + std::to_integer(supercap_raw_bytes[3]), + std::to_integer(supercap_raw_bytes[4]), + std::to_integer(supercap_raw_bytes[5]), + std::to_integer(supercap_raw_bytes[6]), + std::to_integer(supercap_raw_bytes[7])); + + next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); + } + + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) + return; + if (can == Spec::kCans.kCan0) { + process_chassis_can_receive_(0, data); + monitor_.tick("Bottom::Can0", data.can_id); + } else if (can == Spec::kCans.kCan1) { + process_chassis_can_receive_(1, data); + if (!data.is_extended_can_id && !data.is_remote_transmission + && data.can_id == 0x300) { + if (data.can_data.size() == 8) + latest_supercap_status_.store( + device::CanPacket8{data.can_data}, std::memory_order_relaxed); + supercap_.store_status(data.can_data); + supercap_status_received_.store(true, std::memory_order_relaxed); + } + monitor_.tick("Bottom::Can1", data.can_id); + } else if (can == Spec::kCans.kCan2) { + process_chassis_can_receive_(2, data); + if (data.is_extended_can_id || data.is_remote_transmission) + return; + if (data.can_id == 0x142) + gimbal_yaw_motor_.store_status(data.can_data); + else if (data.can_id == 0x203) + gimbal_bullet_feeder_.store_status(data.can_data); + monitor_.tick("Bottom::Can2", data.can_id); + } else if (can == Spec::kCans.kCan3) { + process_chassis_can_receive_(3, data); + monitor_.tick("Bottom::Can3", data.can_id); + } + } + + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + if (uart == Spec::kUarts.kDbus) { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + monitor_.tick("Bottom::Dbus", "Active"); + } else if (uart == Spec::kUarts.kUart0) { + const std::byte* ptr = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, + data.uart_data.size()); + monitor_.tick("Bottom::Uart0", "Active"); + } + } + + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { + imu_.store_accelerometer_status(data.x, data.y, data.z); + monitor_.tick("Bottom::Imu", "Acc"); + } + + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); + monitor_.tick("Bottom::Imu", "Gyr"); + } + + auto status() const -> std::vector { return monitor_.text(); } + + StatusMonitor monitor_{}; + }; + + auto status_service_callback(const std::shared_ptr& response) + -> void { + response->success = true; + + auto feedback_message = std::ostringstream{}; + auto text = [&](std::format_string format, Args&&... args) { + std::println(feedback_message, format, std::forward(args)...); + }; + + text(" yaw_motor_zero_point: {}", bottom_board_->gimbal_yaw_motor_.last_raw_angle()); + text(" pitch_motor_zero_point: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); + constexpr auto kPosition = + std::array{"left_front", "left_back", "right_back", "right_front"}; + + text(""); + for (auto&& [index, motor] : + std::views::zip(kPosition, bottom_board_->chassis_joint_motors_)) { + text(" {}_zero_point: {}", index, motor.last_raw_angle()); + } + + text("\nBottomBoard Status:"); + for (const auto& line : bottom_board_->status()) + text("> {}", line); + + text("\nTopBoard Status:"); + for (const auto& line : top_board_->status()) + text("> {}", line); + + response->message = feedback_message.str(); + } + + OutputInterface tf_; + OutputInterface camera_transform_; + OutputInterface barrel_direction_; + OutputInterface auto_aim_yaw_velocity_; + InputInterface timestamp_; + + std::unique_ptr bottom_board_; + std::unique_ptr top_board_; + std::unique_ptr remote_control_; + + std::shared_ptr command_; + uint32_t cmd_tick_ = 0; + + std::shared_ptr> status_service_; +}; + +} // namespace rmcs_core::hardware + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DeformableInfantryOmniB, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index 3b656dd6c..83c40c97b 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -1,11 +1,5 @@ -#include "hardware/device/bmi088.hpp" -#include "hardware/device/can_packet.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/lk_motor.hpp" -#include "hardware/device/supercap.hpp" - #include +#include #include #include #include @@ -18,17 +12,30 @@ #include #include -#include +#include +#include #include #include -#include -#include +#include #include #include +#include +#include #include #include +#include "hardware/device/bmi088.hpp" +#include "hardware/device/bmi088_ekf.hpp" +#include "hardware/device/board_clock_lifter.hpp" +#include "hardware/device/can_packet.hpp" +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/lk_motor.hpp" +#include "hardware/device/remote_control.hpp" +#include "hardware/device/supercap.hpp" +#include "hardware/util/status_monitor.hpp" + namespace rmcs_core::hardware { using Clock = std::chrono::steady_clock; @@ -46,15 +53,19 @@ class DeformableInfantryOmni register_input("/predefined/timestamp", timestamp_); register_output("/tf", tf_); + register_output( + "/auto_aim/camera_transform", camera_transform_, Eigen::Isometry3d::Identity()); + register_output("/auto_aim/barrel_direction", barrel_direction_, Eigen::Vector3d::UnitX()); + register_output("/auto_aim/yaw_velocity", auto_aim_yaw_velocity_, 0.0); + + tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); - tf_->set_transform(Eigen::Translation3d{0.16, 0.0, 0.15}); + remote_control_ = std::make_unique(*this); bottom_board_ = std::make_unique( - *this, *command_, get_parameter("serial_filter_rmcs_board").as_string()); + *this, *command_, get_parameter("serial_filter_bottom_board").as_string()); top_board_ = std::make_unique( *this, *command_, get_parameter("serial_filter_top_board").as_string()); - imu_board_ = - std::make_unique(*this, get_parameter("serial_filter_imu").as_string()); // For command: remote-status using Srv = std_srvs::srv::Trigger; @@ -67,10 +78,18 @@ class DeformableInfantryOmni ~DeformableInfantryOmni() override = default; + void before_updating() override { top_board_->request_hard_sync_read(); } + void update() override { bottom_board_->update(); top_board_->update(); - imu_board_->update(); + remote_control_->update(); + + using namespace rmcs_description; + *camera_transform_ = fast_tf::lookup_transform(*tf_); + *barrel_direction_ = + *fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + *auto_aim_yaw_velocity_ = top_board_->gimbal_yaw_velocity(); } void command_update() { @@ -80,7 +99,17 @@ class DeformableInfantryOmni } private: - static constexpr double kNaN = std::numeric_limits::quiet_NaN(); + static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); + static constexpr auto kLeftFront = 0; + static constexpr auto kLeftBack = 1; + static constexpr auto kRightBack = 2; + static constexpr auto kRightFront = 3; + static constexpr auto kJointName = std::array{ + "left_front", + "left_back", + "right_back", + "right_front", + }; class Command : public rmcs_executor::Component { public: @@ -92,18 +121,16 @@ class DeformableInfantryOmni DeformableInfantryOmni& deformableInfantry; }; - class BottomBoard final : private librmcs::agent::RmcsBoardLite { - friend class DeformableInfantryOmni; - + struct BottomBoard final : public librmcs::board::RmcsBoardLite::Callback { public: explicit BottomBoard( DeformableInfantryOmni& status, rmcs_executor::Component& command, const std::string& serial_filter = {}) - : RmcsBoardLite{ - serial_filter, - librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} - , status_{status} - , command_{command} { + : status_{status} + , command_{command} + , kChassisRadiusBase(status.get_parameter("chassis_radius").as_double()) + , kRodLength(status.get_parameter("rod_length").as_double()) + , kDefaultRadius(kChassisRadiusBase + kRodLength) { status.register_output("/referee/serial", referee_serial_); referee_serial_->read = [this](std::byte* buffer, size_t size) { @@ -111,8 +138,8 @@ class DeformableInfantryOmni [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); }; referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart0_transmit( - {.uart_data = std::span{buffer, size}}); + board_->start_transmit().uart_transmit( + Spec::kUarts.kUart0, {.uart_data = std::span{buffer, size}}); return size; }; @@ -122,10 +149,9 @@ class DeformableInfantryOmni for (auto& motor : chassis_wheel_motors_) motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(11.0) - .enable_multi_turn_angle() - .set_reversed()); + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} + .set_reduction_ratio(19.0) + .enable_multi_turn_angle()); for (auto& motor : chassis_joint_motors_) motor.configure( @@ -140,45 +166,40 @@ class DeformableInfantryOmni }); gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); + device::DjiMotor::Config{device::DjiMotor::Type::kM2006, 3} + .enable_multi_turn_angle()); status.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); status.register_output("/chassis/imu/pitch", chassis_imu_pitch_, 0.0); status.register_output("/chassis/imu/roll", chassis_imu_roll_, 0.0); status.register_output("/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, 0.0); status.register_output("/chassis/imu/roll_rate", chassis_imu_roll_rate_, 0.0); - status.register_output( - "/chassis/left_front_joint/physical_angle", left_front_joint_physical_angle_, kNaN); - status.register_output( - "/chassis/left_back_joint/physical_angle", left_back_joint_physical_angle_, kNaN); - status.register_output( - "/chassis/right_back_joint/physical_angle", right_back_joint_physical_angle_, kNaN); - status.register_output( - "/chassis/right_front_joint/physical_angle", right_front_joint_physical_angle_, - kNaN); - status.register_output( - "/chassis/left_front_joint/physical_velocity", left_front_joint_physical_velocity_, - kNaN); - status.register_output( - "/chassis/left_back_joint/physical_velocity", left_back_joint_physical_velocity_, - kNaN); - status.register_output( - "/chassis/right_back_joint/physical_velocity", right_back_joint_physical_velocity_, - kNaN); - status.register_output( - "/chassis/right_front_joint/physical_velocity", - right_front_joint_physical_velocity_, kNaN); + for (size_t i = 0; i < 4; ++i) { + status.register_output( + std::format( + "/chassis/{}_joint/physical_angle", DeformableInfantryOmni::kJointName[i]), + joint_physical_angle_[i], kNaN); + status.register_output( + std::format( + "/chassis/{}_joint/physical_velocity", + DeformableInfantryOmni::kJointName[i]), + joint_physical_velocity_[i], kNaN); + } status.register_output("/chassis/encoder/alpha", encoder_alpha_, kNaN); status.register_output("/chassis/encoder/alpha_dot", encoder_alpha_dot_, kNaN); - status.register_output("/chassis/radius", radius_, kNaN); + status.register_output("/chassis/radius", radius_, kDefaultRadius); status.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); status.get_parameter_or("debug_log_wheel_motor", debug_log_wheel_motor_, false); status.get_parameter_or( "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); - } - ~BottomBoard() override = default; + auto options = librmcs::board::AdvancedOptions{}; + options.dangerously_skip_version_checks = true; + board_ = std::make_unique(*this, serial_filter, options); + + status_.remote_control_->register_dr16(&dr16_); + } void update() { imu_.update_status(); @@ -209,14 +230,9 @@ class DeformableInfantryOmni for (auto& motor : chassis_joint_motors_) motor.update_status(); - update_joint_physical_feedback_( - 0, left_front_joint_physical_angle_, left_front_joint_physical_velocity_); - update_joint_physical_feedback_( - 1, left_back_joint_physical_angle_, left_back_joint_physical_velocity_); - update_joint_physical_feedback_( - 2, right_back_joint_physical_angle_, right_back_joint_physical_velocity_); - update_joint_physical_feedback_( - 3, right_front_joint_physical_angle_, right_front_joint_physical_velocity_); + for (size_t i = 0; i < 4; ++i) + update_joint_physical_feedback_( + i, joint_physical_angle_[i], joint_physical_velocity_[i]); update_geometry_feedback_(); if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) @@ -235,98 +251,168 @@ class DeformableInfantryOmni } void command_update(bool even) { - auto builder = start_transmit(); + auto builder = board_->start_transmit(); if (even) { - builder.can0_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[2].generate_command(), - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can3_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[3].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x142, - .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes(), - }); - builder.can1_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - } - .as_bytes(), - }); + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kLeftFront].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kLeftBack].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kRightBack].generate_command(), + device::CanPacket8::PaddingQuarter{}, + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan3, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kRightFront].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = 0x142, + .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + } + .as_bytes(), + }); } else { - builder.can0_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[0].generate_command().as_bytes(), - }); - builder.can1_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[1].generate_command().as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[2].generate_command().as_bytes(), - }); - builder.can3_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[3].generate_command().as_bytes(), - }); + for (size_t i = 0; i < 4; ++i) { + switch (i) { + case kLeftFront: + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + case kLeftBack: + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + case kRightBack: + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + case kRightFront: + builder.can_transmit( + Spec::kCans.kCan3, // + { + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + default: break; + } + } } } - private: - static constexpr double joint_zero_physical_angle_rad_ = 62.5 * std::numbers::pi / 180.0; - static constexpr double chassis_radius_base_ = 0.2341741; - static constexpr double rod_length_ = 0.150; + static constexpr double kJointZeroPhysicalAngleRad = 62.5 * std::numbers::pi / 180.0; DeformableInfantryOmni& status_; Component& command_; + std::unique_ptr board_; + + // Interfaces + + OutputInterface& tf_{status_.tf_}; + + OutputInterface chassis_yaw_velocity_imu_; + OutputInterface chassis_imu_pitch_; + OutputInterface chassis_imu_roll_; + OutputInterface chassis_imu_pitch_rate_; + OutputInterface chassis_imu_roll_rate_; + + std::array, 4> joint_physical_angle_; + std::array, 4> joint_physical_velocity_; + + OutputInterface encoder_alpha_; + OutputInterface encoder_alpha_dot_; + OutputInterface radius_; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; + OutputInterface referee_serial_; + + // State + + std::atomic wheel_status_received_[4] = {false, false, false, false}; + std::atomic joint_status_received_[4] = {false, false, false, false}; + + bool debug_log_supercap_ = false; + bool debug_log_wheel_motor_ = false; + bool debug_log_deformable_joint_motor_ = false; + + const double kChassisRadiusBase; + const double kRodLength; + const double kDefaultRadius; + + Clock::time_point next_chassis_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; + Clock::time_point next_supercap_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; + + // Device + device::Bmi088 imu_{1000, 0.2, 0.0}; device::LkMotor gimbal_yaw_motor_{status_, command_, "/gimbal/yaw"}; - device::Dr16 dr16_{status_}; + device::Dr16 dr16_{}; device::DjiMotor chassis_wheel_motors_[4]{ device::DjiMotor{status_, command_, "/chassis/left_front_wheel"}, @@ -341,39 +427,23 @@ class DeformableInfantryOmni device::LkMotor{status_, command_, "/chassis/right_front_joint"}, }; - std::atomic wheel_status_received_[4] = {false, false, false, false}; - std::atomic joint_status_received_[4] = {false, false, false, false}; - bool debug_log_supercap_ = false; - bool debug_log_wheel_motor_ = false; - bool debug_log_deformable_joint_motor_ = false; - Clock::time_point next_chassis_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - Clock::time_point next_supercap_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - device::Supercap supercap_{status_, command_}; std::atomic latest_supercap_status_{device::CanPacket8{uint64_t{0}}}; std::atomic supercap_status_received_{false}; - device::DjiMotor gimbal_bullet_feeder_{status_, command_, "/gimbal/bullet_feeder"}; - - OutputInterface& tf_{status_.tf_}; + device::Supercap supercap_{status_, command_}; - OutputInterface chassis_yaw_velocity_imu_; - OutputInterface chassis_imu_pitch_; - OutputInterface chassis_imu_roll_; - OutputInterface chassis_imu_pitch_rate_; - OutputInterface chassis_imu_roll_rate_; - OutputInterface left_front_joint_physical_angle_; - OutputInterface left_back_joint_physical_angle_; - OutputInterface right_back_joint_physical_angle_; - OutputInterface right_front_joint_physical_angle_; - OutputInterface left_front_joint_physical_velocity_; - OutputInterface left_back_joint_physical_velocity_; - OutputInterface right_back_joint_physical_velocity_; - OutputInterface right_front_joint_physical_velocity_; - OutputInterface encoder_alpha_; - OutputInterface encoder_alpha_dot_; - OutputInterface radius_; + device::DjiMotor gimbal_bullet_feeder_{status_, command_, "/gimbal/bullet_feeder"}; - rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; - OutputInterface referee_serial_; + void process_chassis_can_receive_(size_t index, const View::Can& data) { + if (data.is_extended_can_id || data.is_remote_transmission) + return; + if (data.can_id == 0x201) { + chassis_wheel_motors_[index].store_status(data.can_data); + wheel_status_received_[index].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x141) { + chassis_joint_motors_[index].store_status(data.can_data); + joint_status_received_[index].store(true, std::memory_order_relaxed); + } + } void update_joint_physical_feedback_( size_t index, OutputInterface& angle_output, @@ -386,7 +456,7 @@ class DeformableInfantryOmni } const auto to_physical_angle = [](double motor_angle) { - return joint_zero_physical_angle_rad_ - motor_angle; + return kJointZeroPhysicalAngleRad - motor_angle; }; const auto to_physical_velocity = [](double motor_velocity) { return -motor_velocity; }; @@ -396,22 +466,26 @@ class DeformableInfantryOmni void update_geometry_feedback_() { const Eigen::Vector4d alpha_rad{ - *left_front_joint_physical_angle_, *left_back_joint_physical_angle_, - *right_back_joint_physical_angle_, *right_front_joint_physical_angle_}; + *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], + *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront]}; const Eigen::Vector4d alpha_dot_rad{ - *left_front_joint_physical_velocity_, *left_back_joint_physical_velocity_, - *right_back_joint_physical_velocity_, *right_front_joint_physical_velocity_}; + *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], + *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront]}; if (!alpha_rad.array().isFinite().all() || !alpha_dot_rad.array().isFinite().all()) { *encoder_alpha_ = kNaN; *encoder_alpha_dot_ = kNaN; - *radius_ = kNaN; + *radius_ = kDefaultRadius; + RCLCPP_WARN_THROTTLE( + status_.get_logger(), *status_.get_clock(), 1000, + "deformable joint feedback invalid, fallback chassis radius to default %.3f m", + kDefaultRadius); return; } *encoder_alpha_ = alpha_rad.mean(); *encoder_alpha_dot_ = alpha_dot_rad.mean(); - *radius_ = (chassis_radius_base_ + rod_length_ * alpha_rad.array().cos()).mean(); + *radius_ = (kChassisRadiusBase + kRodLength * alpha_rad.array().cos()).mean(); } void log_chassis_feedback_once_per_second_() { @@ -427,29 +501,44 @@ class DeformableInfantryOmni }; if (debug_log_wheel_motor_) { + std::string wheel_rx_str; + for (size_t i = 0; i < 4; ++i) { + if (i > 0) + wheel_rx_str.push_back(' '); + wheel_rx_str.push_back(wheel_rx(i)); + } RCLCPP_INFO( status_.get_logger(), "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " - "rx=[%c %c %c %c]", - chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), - chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), - chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), - chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), wheel_rx(0), - wheel_rx(1), wheel_rx(2), wheel_rx(3)); + "rx=[%s]", + chassis_wheel_motors_[kLeftFront].angle(), + chassis_wheel_motors_[kLeftBack].angle(), + chassis_wheel_motors_[kRightBack].angle(), + chassis_wheel_motors_[kRightFront].angle(), + chassis_wheel_motors_[kLeftFront].angle(), + chassis_wheel_motors_[kLeftBack].angle(), + chassis_wheel_motors_[kRightBack].angle(), + chassis_wheel_motors_[kRightFront].angle(), wheel_rx_str.c_str()); } if (debug_log_deformable_joint_motor_) { + std::string joint_rx_str; + for (size_t i = 0; i < 4; ++i) { + if (i > 0) + joint_rx_str.push_back(' '); + joint_rx_str.push_back(joint_rx(i)); + } RCLCPP_INFO( status_.get_logger(), "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "rx=[%c %c %c %c]", - *left_front_joint_physical_angle_, *left_back_joint_physical_angle_, - *right_back_joint_physical_angle_, *right_front_joint_physical_angle_, - *left_front_joint_physical_velocity_, *left_back_joint_physical_velocity_, - *right_back_joint_physical_velocity_, *right_front_joint_physical_velocity_, - joint_rx(0), joint_rx(1), joint_rx(2), joint_rx(3)); + "rx=[%s]", + *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], + *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront], + *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], + *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront], + joint_rx_str.c_str()); } next_chassis_feedback_log_time_ = now + std::chrono::seconds(1); @@ -484,145 +573,79 @@ class DeformableInfantryOmni next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); } - void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); - } - - void can0_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[0].store_status(data.can_data); - wheel_status_received_[0].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[0].store_status(data.can_data); - joint_status_received_[0].store(true, std::memory_order_relaxed); - } - } - - void can1_receive_callback(const librmcs::data::CanDataView& data) override { + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { if (data.is_extended_can_id || data.is_remote_transmission) return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[1].store_status(data.can_data); - wheel_status_received_[1].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[1].store_status(data.can_data); - joint_status_received_[1].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x300) { - if (data.can_data.size() == 8) - latest_supercap_status_.store( - device::CanPacket8{data.can_data}, std::memory_order_relaxed); - supercap_.store_status(data.can_data); - supercap_status_received_.store(true, std::memory_order_relaxed); + if (can == Spec::kCans.kCan0) { + process_chassis_can_receive_(0, data); + monitor_.tick("Bottom::Can0", data.can_id); + } else if (can == Spec::kCans.kCan1) { + process_chassis_can_receive_(1, data); + if (!data.is_extended_can_id && !data.is_remote_transmission + && data.can_id == 0x300) { + if (data.can_data.size() == 8) + latest_supercap_status_.store( + device::CanPacket8{data.can_data}, std::memory_order_relaxed); + supercap_.store_status(data.can_data); + supercap_status_received_.store(true, std::memory_order_relaxed); + } + monitor_.tick("Bottom::Can1", data.can_id); + } else if (can == Spec::kCans.kCan2) { + process_chassis_can_receive_(2, data); + if (data.is_extended_can_id || data.is_remote_transmission) + return; + if (data.can_id == 0x142) + gimbal_yaw_motor_.store_status(data.can_data); + else if (data.can_id == 0x203) + gimbal_bullet_feeder_.store_status(data.can_data); + monitor_.tick("Bottom::Can2", data.can_id); + } else if (can == Spec::kCans.kCan3) { + process_chassis_can_receive_(3, data); + monitor_.tick("Bottom::Can3", data.can_id); } } - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[2].store_status(data.can_data); - wheel_status_received_[2].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[2].store_status(data.can_data); - joint_status_received_[2].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x142) { - gimbal_yaw_motor_.store_status(data.can_data); - } else if (data.can_id == 0x203) { - gimbal_bullet_feeder_.store_status(data.can_data); - } - } - - void can3_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[3].store_status(data.can_data); - wheel_status_received_[3].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[3].store_status(data.can_data); - joint_status_received_[3].store(true, std::memory_order_relaxed); + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + if (uart == Spec::kUarts.kDbus) { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + monitor_.tick("Bottom::Dbus", "Active"); + } else if (uart == Spec::kUarts.kUart0) { + const std::byte* ptr = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, + data.uart_data.size()); + monitor_.tick("Bottom::Uart0", "Active"); } } - void uart0_receive_callback(const librmcs::data::UartDataView& data) override { - const std::byte* ptr = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, data.uart_data.size()); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { imu_.store_accelerometer_status(data.x, data.y, data.z); + monitor_.tick("Bottom::Imu", "Acc"); } - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { imu_.store_gyroscope_status(data.x, data.y, data.z); - } - }; - - class ImuBoard final : private librmcs::agent::RmcsBoardLite { - friend class DeformableInfantryOmni; - - public: - explicit ImuBoard(DeformableInfantryOmni& status, const std::string& serial_filter = {}) - : RmcsBoardLite{ - serial_filter, - librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} - , tf_{status.tf_} - , bmi088_{1000, 0.2, 0.0} { - - status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); - - bmi088_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(x, z, -y); }); + monitor_.tick("Bottom::Imu", "Gyr"); } - ~ImuBoard() override = default; - - void update() { - bmi088_.update_status(); - Eigen::Quaterniond const gimbal_imu_pose{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - - tf_->set_transform( - gimbal_imu_pose.conjugate()); - - *gimbal_pitch_velocity_imu_ = bmi088_.gy(); - } + auto status() const -> std::vector { return monitor_.text(); } - private: - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); - } - - OutputInterface& tf_; - OutputInterface gimbal_pitch_velocity_imu_; - - device::Bmi088 bmi088_; + StatusMonitor monitor_{}; }; - class TopBoard final : private librmcs::agent::CBoard { - friend class DeformableInfantryOmni; - + struct TopBoard final : public librmcs::board::RmcsBoardLite::Callback { public: explicit TopBoard( - DeformableInfantryOmni& status, Command& command, const std::string& serial_filter = {}) - : librmcs::agent::CBoard( - serial_filter, - librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) - , tf_(status.tf_) - , bmi088_(1000, 0.2, 0.0) + DeformableInfantryOmni& status, rmcs_executor::Component& command, + const std::string& serial_filter = {}) + : tf_{status.tf_} + , bmi088_{device::Bmi088Ekf::Config{ + .body_to_sensor = + Eigen::AngleAxisd{std::numbers::pi / 2.0, Eigen::Vector3d::UnitX()} + .toRotationMatrix()}} , gimbal_pitch_motor_(status, command, "/gimbal/pitch") , gimbal_left_friction_(status, command, "/gimbal/left_friction") - , gimbal_right_friction_(status, command, "/gimbal/right_friction") - , scope_motor_(status, command, "/gimbal/scope") { + , gimbal_right_friction_(status, command, "/gimbal/right_friction") { gimbal_pitch_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} @@ -631,96 +654,165 @@ class DeformableInfantryOmni static_cast(status.get_parameter("pitch_motor_zero_point").as_int()))); gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} .set_reduction_ratio(1.) .set_reversed()); gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); - - scope_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2}.set_reduction_ratio( + 1.)); + + status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); + status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); + status.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); + status.register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); + + auto options = librmcs::board::AdvancedOptions{}; + options.dangerously_skip_version_checks = true; + board_ = std::make_unique(*this, serial_filter, options); + + board_->start_transmit().gpio_digital_read( + Spec::kGpios.kUart1Rx, { + .period_ms = 0, + .asap = false, + .rising_edge = false, + .falling_edge = true, + .capture_timestamp = true, + .pull = librmcs::data::GpioPull::kUp, + }); + } - status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); + ~TopBoard() override = default; - bmi088_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(y, -x, z); }); + [[nodiscard]] auto gimbal_yaw_velocity() const -> double { + return *gimbal_yaw_velocity_bmi088_; } - ~TopBoard() override = default; + void request_hard_sync_read() { + // RMCS-lite top board variant currently has no GPIO hard-sync request + // path. + } void update() { - bmi088_.update_status(); gimbal_pitch_motor_.update_status(); gimbal_left_friction_.update_status(); gimbal_right_friction_.update_status(); - scope_motor_.update_status(); const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); - *gimbal_yaw_velocity_imu_ = bmi088_.gz(); + if (auto snapshot = bmi088_.snapshot()) { + *gimbal_pitch_velocity_bmi088_ = snapshot->gyro_body.y(); + *gimbal_yaw_velocity_bmi088_ = snapshot->gyro_body.z(); + tf_->set_transform( + snapshot->orientation.conjugate()); + } tf_->set_state( pitch_encoder_angle); } - void command_update() { - auto builder = start_transmit(); - builder.can1_transmit({ - .can_id = 0x141, - .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_left_friction_.generate_command(), - gimbal_right_friction_.generate_command(), - scope_motor_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); + void command_update() const { + auto builder = board_->start_transmit(); + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x141, + .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_left_friction_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + gimbal_right_friction_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); } - private: - void uart1_receive_callback(const librmcs::data::UartDataView&) override {} - - void can1_receive_callback(const librmcs::data::CanDataView& data) override { + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; - if (data.can_id == 0x141) - gimbal_pitch_motor_.store_status(data.can_data); + if (can == Spec::kCans.kCan0) { + if (data.can_id == 0x141) + gimbal_pitch_motor_.store_status(data.can_data); + monitor_.tick("Top::Can0", data.can_id); + } else if (can == Spec::kCans.kCan1) { + if (data.can_id == 0x201) + gimbal_left_friction_.store_status(data.can_data); + monitor_.tick("Top::Can1", data.can_id); + } else if (can == Spec::kCans.kCan2) { + if (data.can_id == 0x202) + gimbal_right_friction_.store_status(data.can_data); + monitor_.tick("Top::Can2", data.can_id); + } } - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - if (data.can_id == 0x201) - gimbal_left_friction_.store_status(data.can_data); - else if (data.can_id == 0x202) - gimbal_right_friction_.store_status(data.can_data); - else if (data.can_id == 0x203) - scope_motor_.store_status(data.can_data); + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { + const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); + bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); + monitor_.tick("Top::Imu", "Acc"); } - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); + monitor_.tick("Top::Imu", "Gyr"); + if (!timestamp.has_value()) + return; + auto snapshot = + bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); + if (snapshot) + imu_snapshot_output_.emit(*snapshot); } - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); + void gpio_digital_read_result_callback( + const Spec::Gpio& gpio, const View::GpioDigital& data) override { + if (gpio != Spec::kGpios.kUart1Rx) + return; + if (!data.timestamp_quarter_us) + return; + + const auto timestamp = board_clock_lifter_.lift_timestamp(*data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + + camera_signal_output_.emit(*timestamp); + monitor_.tick("Top::CameraSync", "Active"); } + auto status() const -> std::vector { return monitor_.text(); } + OutputInterface& tf_; - OutputInterface gimbal_yaw_velocity_imu_; + OutputInterface gimbal_yaw_velocity_bmi088_; + OutputInterface gimbal_pitch_velocity_bmi088_; - device::Bmi088 bmi088_; + EventOutputInterface imu_snapshot_output_; + EventOutputInterface camera_signal_output_; + + device::Bmi088Ekf bmi088_; + device::BoardClockLifter board_clock_lifter_; device::LkMotor gimbal_pitch_motor_; device::DjiMotor gimbal_left_friction_; device::DjiMotor gimbal_right_friction_; - device::DjiMotor scope_motor_; + + StatusMonitor monitor_{}; + std::unique_ptr board_; }; auto status_service_callback(const std::shared_ptr& response) @@ -732,30 +824,37 @@ class DeformableInfantryOmni std::println(feedback_message, format, std::forward(args)...); }; - text("Gimbal Status"); - text("- Yaw: {}", bottom_board_->gimbal_yaw_motor_.last_raw_angle()); - text("- Pitch: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); - - text("Chassis Status"); + text(" yaw_motor_zero_point: {}", bottom_board_->gimbal_yaw_motor_.last_raw_angle()); + text(" pitch_motor_zero_point: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); constexpr auto kPosition = - std::array{"left front", "left back", "right back", "right front"}; - constexpr auto kMaxLength = - std::ranges::max_element(kPosition, {}, &std::string_view::size)->size(); + std::array{"left_front", "left_back", "right_back", "right_front"}; + text(""); for (auto&& [index, motor] : std::views::zip(kPosition, bottom_board_->chassis_joint_motors_)) { - text("- {:{}}: {}", index, kMaxLength, motor.last_raw_angle()); + text(" {}_zero_point: {}", index, motor.last_raw_angle()); } + text("\nBottomBoard Status:"); + for (const auto& line : bottom_board_->status()) + text("> {}", line); + + text("\nTopBoard Status:"); + for (const auto& line : top_board_->status()) + text("> {}", line); + response->message = feedback_message.str(); } OutputInterface tf_; + OutputInterface camera_transform_; + OutputInterface barrel_direction_; + OutputInterface auto_aim_yaw_velocity_; InputInterface timestamp_; - std::unique_ptr imu_board_; - std::unique_ptr top_board_; std::unique_ptr bottom_board_; + std::unique_ptr top_board_; + std::unique_ptr remote_control_; std::shared_ptr command_; uint32_t cmd_tick_ = 0; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp deleted file mode 100644 index 6514c58b9..000000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp +++ /dev/null @@ -1,876 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include - -#include "hardware/device/bmi088.hpp" -#include "hardware/device/can_packet.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/lk_motor.hpp" -#include "hardware/device/supercap.hpp" - -namespace rmcs_core::hardware { - -using Clock = std::chrono::steady_clock; - -class DeformableInfantryV2 - : public rmcs_executor::Component - , public rclcpp::Node { -public: - DeformableInfantryV2() - : Node( - get_component_name(), - rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) - , deformable_infantry_command_( - create_partner_component( - get_component_name() + "_command", *this)) { - using namespace rmcs_description; - - register_input("/predefined/timestamp", timestamp_); - register_output("/tf", tf_); - - tf_->set_transform(Eigen::Translation3d{0.16, 0.0, 0.15}); - - steers_calibrate_subscription_ = create_subscription( - "/steers/calibrate", rclcpp::QoS(1), [this](std_msgs::msg::Int32::UniquePtr msg) { - steers_calibrate_subscription_callback(std::move(msg)); - }); - - joints_calibrate_subscription_ = create_subscription( - "/joints/calibrate", rclcpp::QoS(1), [this](std_msgs::msg::Int32::UniquePtr msg) { - joints_calibrate_subscription_callback(std::move(msg)); - }); - - gimbal_calibrate_subscription_ = create_subscription( - "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - gimbal_calibrate_subscription_callback(std::move(msg)); - }); - - rmcs_board_lite = std::make_unique( - *this, *deformable_infantry_command_, - get_parameter("serial_filter_rmcs_board").as_string()); - top_board_ = std::make_unique( - *this, *deformable_infantry_command_, - get_parameter("serial_filter_top_board").as_string()); - } - - ~DeformableInfantryV2() override = default; - - void before_updating() override { - top_board_->request_hard_sync_read(); - next_hard_sync_log_time_ = Clock::now() + std::chrono::seconds(1); - } - - void update() override { - rmcs_board_lite->update(); - top_board_->update(); - } - - void command_update() { - const bool even = ((cmd_tick_++ & 1u) == 0u); - rmcs_board_lite->command_update(even); - top_board_->command_update(); - } - -private: - class DeformableInfantryV2Command; - class BottomBoard; - class TopBoard; - - void steers_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - if (!rmcs_board_lite) - return; - - RCLCPP_INFO( - get_logger(), "New left front offset: %d", - rmcs_board_lite->chassis_steer_motors_[0].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "New left back offset: %d", - rmcs_board_lite->chassis_steer_motors_[1].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "New right back offset: %d", - rmcs_board_lite->chassis_steer_motors_[2].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "New right front offset: %d", - rmcs_board_lite->chassis_steer_motors_[3].calibrate_zero_point()); - } - - void joints_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - if (!rmcs_board_lite) - return; - - RCLCPP_INFO( - get_logger(), "New left front offset: %ld", - rmcs_board_lite->chassis_joint_motors_[0].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "New left back offset: %ld", - rmcs_board_lite->chassis_joint_motors_[1].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "New right back offset: %ld", - rmcs_board_lite->chassis_joint_motors_[2].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "New right front offset: %ld", - rmcs_board_lite->chassis_joint_motors_[3].calibrate_zero_point()); - } - - void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - if (!rmcs_board_lite || !top_board_) - return; - - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New yaw offset: %ld", - rmcs_board_lite->gimbal_yaw_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New pitch offset: %ld", - top_board_->gimbal_pitch_motor_.calibrate_zero_point()); - } - - class DeformableInfantryV2Command : public rmcs_executor::Component { - public: - explicit DeformableInfantryV2Command(DeformableInfantryV2& deformableInfantry) - : deformableInfantry(deformableInfantry) {} - - void update() override { deformableInfantry.command_update(); } - - DeformableInfantryV2& deformableInfantry; - }; - - class BottomBoard final : private librmcs::agent::RmcsBoardLite { - public: - friend class DeformableInfantryV2; - - static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - - explicit BottomBoard( - DeformableInfantryV2& deformableInfantry, - DeformableInfantryV2Command& deformableInfantry_command, std::string serial_filter = {}) - : librmcs::agent::RmcsBoardLite( - serial_filter, - librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) - , deformable_infantry_(deformableInfantry) - , tf_(deformableInfantry.tf_) - , imu_(1000, 0.2, 0.0) - , gimbal_yaw_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/yaw") - , dr16_(deformableInfantry) - , chassis_wheel_motors_{device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/left_front_wheel"}, device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/left_back_wheel"}, device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/right_back_wheel"}, device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/right_front_wheel"},} - , chassis_steer_motors_{device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/left_front_steering"}, device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/left_back_steering"}, device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/right_back_steering"}, device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/right_front_steering"}} - , chassis_joint_motors_{device::LkMotor{deformableInfantry, deformableInfantry_command, "/chassis/left_front_joint"}, device::LkMotor{deformableInfantry, deformableInfantry_command, "/chassis/left_back_joint"}, device::LkMotor{deformableInfantry, deformableInfantry_command, "/chassis/right_back_joint"}, device::LkMotor{deformableInfantry, deformableInfantry_command, "/chassis/right_front_joint"}} - , next_chassis_feedback_log_time_(Clock::now() + std::chrono::seconds(1)) - , next_supercap_feedback_log_time_(Clock::now() + std::chrono::seconds(1)) - , supercap_(deformableInfantry, deformableInfantry_command) - , gimbal_bullet_feeder_( - deformableInfantry, deformableInfantry_command, "/gimbal/bullet_feeder") { - - deformableInfantry.register_output("/referee/serial", referee_serial_); - referee_serial_->read = [this](std::byte* buffer, size_t size) { - return referee_ring_buffer_receive_.pop_front_n( - [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); - }; - referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart0_transmit( - {.uart_data = std::span{buffer, size}}); - return size; - }; - - gimbal_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( - static_cast( - deformableInfantry.get_parameter("yaw_motor_zero_point").as_int()))); - - for (auto& motor : chassis_wheel_motors_) - motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(10.0) - .enable_multi_turn_angle() - .set_reversed()); - - // V2: LK MG5010 i36 direct-drive joint motors, built-in encoder zero point - for (auto& motor : chassis_joint_motors_) - motor.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei36} - .set_reversed() - .enable_multi_turn_angle()); - - imu_.set_coordinate_mapping([](double x, double y, double z) { - // Keep the existing chassis yaw axis mapping explicit until the bottom-board IMU - // installation is re-validated on hardware. - return std::make_tuple(-y, x, z); - }); - - gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); - - chassis_steer_motors_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - deformableInfantry.get_parameter("left_front_zero_point").as_int())) - .enable_multi_turn_angle()); - chassis_steer_motors_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - deformableInfantry.get_parameter("left_back_zero_point").as_int())) - .enable_multi_turn_angle()); - chassis_steer_motors_[2].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - deformableInfantry.get_parameter("right_back_zero_point").as_int())) - .enable_multi_turn_angle()); - chassis_steer_motors_[3].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - deformableInfantry.get_parameter("right_front_zero_point").as_int())) - .enable_multi_turn_angle()); - - deformableInfantry.register_output( - "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - deformableInfantry.register_output("/chassis/imu/pitch", chassis_imu_pitch_, 0.0); - deformableInfantry.register_output("/chassis/imu/roll", chassis_imu_roll_, 0.0); - deformableInfantry.register_output( - "/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, 0.0); - deformableInfantry.register_output( - "/chassis/imu/roll_rate", chassis_imu_roll_rate_, 0.0); - deformableInfantry.register_output( - "/chassis/left_front_joint/physical_angle", left_front_joint_physical_angle_, nan_); - deformableInfantry.register_output( - "/chassis/left_back_joint/physical_angle", left_back_joint_physical_angle_, nan_); - deformableInfantry.register_output( - "/chassis/right_back_joint/physical_angle", right_back_joint_physical_angle_, nan_); - deformableInfantry.register_output( - "/chassis/right_front_joint/physical_angle", right_front_joint_physical_angle_, - nan_); - deformableInfantry.register_output( - "/chassis/left_front_joint/physical_velocity", left_front_joint_physical_velocity_, - nan_); - deformableInfantry.register_output( - "/chassis/left_back_joint/physical_velocity", left_back_joint_physical_velocity_, - nan_); - deformableInfantry.register_output( - "/chassis/right_back_joint/physical_velocity", right_back_joint_physical_velocity_, - nan_); - deformableInfantry.register_output( - "/chassis/right_front_joint/physical_velocity", - right_front_joint_physical_velocity_, nan_); - deformableInfantry.register_output("/chassis/encoder/alpha", encoder_alpha_, nan_); - deformableInfantry.register_output( - "/chassis/encoder/alpha_dot", encoder_alpha_dot_, nan_); - deformableInfantry.register_output("/chassis/radius", radius_, nan_); - - deformableInfantry.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); - deformableInfantry.get_parameter_or( - "debug_log_wheel_motor", debug_log_wheel_motor_, false); - deformableInfantry.get_parameter_or( - "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); - } - - ~BottomBoard() override = default; - - void update() { - imu_.update_status(); - *chassis_yaw_velocity_imu_ = imu_.gz(); - { - const double q0 = imu_.q0(); - const double q1 = imu_.q1(); - const double q2 = imu_.q2(); - const double q3 = imu_.q3(); - - double sin_pitch = 2.0 * (q0 * q2 - q3 * q1); - sin_pitch = std::clamp(sin_pitch, -1.0, 1.0); - - const double standard_pitch = std::asin(sin_pitch); - const double standard_roll = - std::atan2(2.0 * (q0 * q1 + q2 * q3), 1.0 - 2.0 * (q1 * q1 + q2 * q2)); - - // Export chassis attitude using the requested convention: - // pitch < 0 when the front is higher, roll > 0 when the left side is higher. - *chassis_imu_pitch_ = -standard_pitch; - *chassis_imu_roll_ = standard_roll; - *chassis_imu_pitch_rate_ = -imu_.gy(); - *chassis_imu_roll_rate_ = imu_.gx(); - } - - for (auto& motor : chassis_wheel_motors_) - motor.update_status(); - for (auto& motor : chassis_steer_motors_) - motor.update_status(); - for (auto& motor : chassis_joint_motors_) - motor.update_status(); - - update_joint_physical_feedback_( - 0, left_front_joint_physical_angle_, left_front_joint_physical_velocity_); - update_joint_physical_feedback_( - 1, left_back_joint_physical_angle_, left_back_joint_physical_velocity_); - update_joint_physical_feedback_( - 2, right_back_joint_physical_angle_, right_back_joint_physical_velocity_); - update_joint_physical_feedback_( - 3, right_front_joint_physical_angle_, right_front_joint_physical_velocity_); - - update_geometry_feedback_(); - if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) - log_chassis_feedback_once_per_second_(); - - dr16_.update_status(); - gimbal_yaw_motor_.update_status(); - if (supercap_status_received_.load(std::memory_order_relaxed)) - supercap_.update_status(); - if (debug_log_supercap_) - log_supercap_feedback_once_per_second_(); - gimbal_bullet_feeder_.update_status(); - - tf_->set_state( - gimbal_yaw_motor_.angle()); - } - - void command_update(bool even) { - auto builder = start_transmit(); - if (even) { - // Steer motors: same as V1 - builder.can0_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steer_motors_[0].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can1_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steer_motors_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - } - .as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steer_motors_[2].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can3_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steer_motors_[3].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - } else { - // V2: Wheel DJI frames (wheel only, no joint packed in) - builder.can0_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[2].generate_command(), - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can3_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[3].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - - // V2: Joint LK motors - individual CAN frames - builder.can0_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[0].generate_command().as_bytes(), - }); - builder.can1_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[1].generate_command().as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[2].generate_command().as_bytes(), - }); - builder.can3_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[3].generate_command().as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x142, - .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes(), - }); - } - } - - private: - DeformableInfantryV2& deformable_infantry_; - - static constexpr double joint_zero_physical_angle_rad_ = 62.5 * std::numbers::pi / 180.0; - static constexpr double chassis_radius_base_ = 0.2341741; - static constexpr double rod_length_ = 0.150; - - static double to_physical_angle_(double motor_angle) { - return joint_zero_physical_angle_rad_ - motor_angle; - } - - static double to_physical_velocity_(double motor_velocity) { return -motor_velocity; } - - void update_joint_physical_feedback_( - size_t index, OutputInterface& angle_output, - OutputInterface& velocity_output) { - if (!joint_status_received_[index].load(std::memory_order_relaxed)) { - *angle_output = nan_; - *velocity_output = nan_; - return; - } - - *angle_output = to_physical_angle_(chassis_joint_motors_[index].angle()); - *velocity_output = to_physical_velocity_(chassis_joint_motors_[index].velocity()); - } - - void update_geometry_feedback_() { - const Eigen::Vector4d alpha_rad{ - *left_front_joint_physical_angle_, *left_back_joint_physical_angle_, - *right_back_joint_physical_angle_, *right_front_joint_physical_angle_}; - const Eigen::Vector4d alpha_dot_rad{ - *left_front_joint_physical_velocity_, *left_back_joint_physical_velocity_, - *right_back_joint_physical_velocity_, *right_front_joint_physical_velocity_}; - - if (!alpha_rad.array().isFinite().all() || !alpha_dot_rad.array().isFinite().all()) { - *encoder_alpha_ = nan_; - *encoder_alpha_dot_ = nan_; - *radius_ = nan_; - return; - } - - *encoder_alpha_ = alpha_rad.mean(); - *encoder_alpha_dot_ = alpha_dot_rad.mean(); - *radius_ = (chassis_radius_base_ + rod_length_ * alpha_rad.array().cos()).mean(); - } - - void log_chassis_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_chassis_feedback_log_time_) - return; - - const auto wheel_rx = [this](size_t index) { - return wheel_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - const auto joint_rx = [this](size_t index) { - return joint_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - - if (debug_log_wheel_motor_) { - RCLCPP_INFO( - deformable_infantry_.get_logger(), - "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " - "rx=[%c %c %c %c]", - chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), - chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), - chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), - chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), wheel_rx(0), - wheel_rx(1), wheel_rx(2), wheel_rx(3)); - } - - if (debug_log_deformable_joint_motor_) { - RCLCPP_INFO( - deformable_infantry_.get_logger(), - "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "rx=[%c %c %c %c]", - *left_front_joint_physical_angle_, *left_back_joint_physical_angle_, - *right_back_joint_physical_angle_, *right_front_joint_physical_angle_, - *left_front_joint_physical_velocity_, *left_back_joint_physical_velocity_, - *right_back_joint_physical_velocity_, *right_front_joint_physical_velocity_, - joint_rx(0), joint_rx(1), joint_rx(2), joint_rx(3)); - } - - next_chassis_feedback_log_time_ = now + std::chrono::seconds(1); - } - - void log_supercap_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_supercap_feedback_log_time_) - return; - - const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); - auto supercap_raw_packet = latest_supercap_status_.load(std::memory_order_relaxed); - const auto supercap_raw_bytes = supercap_raw_packet.as_bytes(); - - RCLCPP_INFO( - deformable_infantry_.get_logger(), - "[supercap] can1 rx=%c id=0x300 enabled=%d supercap_v=% .3f chassis_v=% .3f " - "power=% .3f raw=[%02X %02X %02X %02X %02X %02X %02X %02X]", - supercap_rx ? 'Y' : 'N', supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, - supercap_rx ? supercap_.supercap_voltage() : nan_, - supercap_rx ? supercap_.chassis_voltage() : nan_, - supercap_rx ? supercap_.chassis_power() : nan_, - std::to_integer(supercap_raw_bytes[0]), - std::to_integer(supercap_raw_bytes[1]), - std::to_integer(supercap_raw_bytes[2]), - std::to_integer(supercap_raw_bytes[3]), - std::to_integer(supercap_raw_bytes[4]), - std::to_integer(supercap_raw_bytes[5]), - std::to_integer(supercap_raw_bytes[6]), - std::to_integer(supercap_raw_bytes[7])); - - next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); - } - - void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); - } - - void can0_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[0].store_status(data.can_data); - wheel_status_received_[0].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[0].store_status(data.can_data); - joint_status_received_[0].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x205) - chassis_steer_motors_[0].store_status(data.can_data); - } - - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[1].store_status(data.can_data); - wheel_status_received_[1].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[1].store_status(data.can_data); - joint_status_received_[1].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x205) - chassis_steer_motors_[1].store_status(data.can_data); - else if (data.can_id == 0x300) { - if (data.can_data.size() == 8) - latest_supercap_status_.store( - device::CanPacket8{data.can_data}, std::memory_order_relaxed); - supercap_.store_status(data.can_data); - supercap_status_received_.store(true, std::memory_order_relaxed); - } - } - - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[2].store_status(data.can_data); - wheel_status_received_[2].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[2].store_status(data.can_data); - joint_status_received_[2].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x142) { - gimbal_yaw_motor_.store_status(data.can_data); - } else if (data.can_id == 0x203) { - gimbal_bullet_feeder_.store_status(data.can_data); - } else if (data.can_id == 0x205) - chassis_steer_motors_[2].store_status(data.can_data); - } - - void can3_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[3].store_status(data.can_data); - wheel_status_received_[3].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[3].store_status(data.can_data); - joint_status_received_[3].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x205) - chassis_steer_motors_[3].store_status(data.can_data); - } - - void uart0_receive_callback(const librmcs::data::UartDataView& data) override { - const std::byte* ptr = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, data.uart_data.size()); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - - OutputInterface& tf_; - - device::Bmi088 imu_; - device::LkMotor gimbal_yaw_motor_; - device::Dr16 dr16_; - device::DjiMotor chassis_wheel_motors_[4]; - device::DjiMotor chassis_steer_motors_[4]; - device::LkMotor chassis_joint_motors_[4]; - std::atomic wheel_status_received_[4] = {false, false, false, false}; - std::atomic joint_status_received_[4] = {false, false, false, false}; - bool debug_log_supercap_ = false; - bool debug_log_wheel_motor_ = false; - bool debug_log_deformable_joint_motor_ = false; - Clock::time_point next_chassis_feedback_log_time_; - Clock::time_point next_supercap_feedback_log_time_; - device::Supercap supercap_; - std::atomic latest_supercap_status_{device::CanPacket8{uint64_t{0}}}; - std::atomic supercap_status_received_{false}; - device::DjiMotor gimbal_bullet_feeder_; - - rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; - OutputInterface referee_serial_; - - OutputInterface chassis_yaw_velocity_imu_; - OutputInterface chassis_imu_pitch_; - OutputInterface chassis_imu_roll_; - OutputInterface chassis_imu_pitch_rate_; - OutputInterface chassis_imu_roll_rate_; - OutputInterface left_front_joint_physical_angle_; - OutputInterface left_back_joint_physical_angle_; - OutputInterface right_back_joint_physical_angle_; - OutputInterface right_front_joint_physical_angle_; - OutputInterface left_front_joint_physical_velocity_; - OutputInterface left_back_joint_physical_velocity_; - OutputInterface right_back_joint_physical_velocity_; - OutputInterface right_front_joint_physical_velocity_; - OutputInterface encoder_alpha_; - OutputInterface encoder_alpha_dot_; - OutputInterface radius_; - }; - - class TopBoard final : private librmcs::agent::RmcsBoardLite { - public: - friend class DeformableInfantryV2; - - explicit TopBoard( - DeformableInfantryV2& deformableInfantry, - DeformableInfantryV2Command& deformableInfantry_command, std::string serial_filter = {}) - : librmcs::agent::RmcsBoardLite( - serial_filter, - librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) - , hard_sync_pending_(deformableInfantry.hard_sync_pending_) - , tf_(deformableInfantry.tf_) - , bmi088_(1000, 0.2, 0.0) - , gimbal_pitch_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/pitch") - , gimbal_left_friction_( - deformableInfantry, deformableInfantry_command, "/gimbal/left_friction") - , gimbal_right_friction_( - deformableInfantry, deformableInfantry_command, "/gimbal/right_friction") - , scope_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/scope") { - - gimbal_pitch_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} - .set_reversed() - .set_encoder_zero_point( - static_cast( - deformableInfantry.get_parameter("pitch_motor_zero_point").as_int()))); - - gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(1.) - .set_reversed()); - gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); - - scope_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); - - deformableInfantry.register_output( - "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - deformableInfantry.register_output( - "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_encoder_); - - bmi088_.set_coordinate_mapping([](double x, double y, double z) { - // Top board BMI088 maps to gimbal frame as (-x, -y, z). - return std::make_tuple(y, -x, z); - }); - } - - ~TopBoard() override = default; - - void request_hard_sync_read() { - // RMCS-lite top board variant currently has no GPIO hard-sync request path. - } - - void update() { - bmi088_.update_status(); - - gimbal_pitch_motor_.update_status(); - gimbal_left_friction_.update_status(); - gimbal_right_friction_.update_status(); - scope_motor_.update_status(); - - const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); - Eigen::Quaterniond const odom_imu_to_yaw_link{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - Eigen::Quaterniond const yaw_link_to_odom_imu = odom_imu_to_yaw_link.conjugate(); - Eigen::Quaterniond pitch_link_to_odom_imu = - Eigen::Quaterniond{ - Eigen::AngleAxisd{-pitch_encoder_angle, Eigen::Vector3d::UnitY()}} - * yaw_link_to_odom_imu; - pitch_link_to_odom_imu.normalize(); - - *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); - *gimbal_pitch_velocity_encoder_ = gimbal_pitch_motor_.velocity(); - // The BMI088 is mounted on the yaw link. fast_tf stores PitchLink -> OdomImu, so use - // the encoder pitch from the TF tree to move the yaw-link pose back into PitchLink. - tf_->set_transform( - pitch_link_to_odom_imu); - - tf_->set_state( - pitch_encoder_angle); - } - - void command_update() { - auto builder = start_transmit(); - builder.can0_transmit({ - .can_id = 0x141, - .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_left_friction_.generate_command(), - gimbal_right_friction_.generate_command(), - scope_motor_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - } - - private: - void uart1_receive_callback(const librmcs::data::UartDataView&) override {} - - void can0_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - if (data.can_id == 0x141) - gimbal_pitch_motor_.store_status(data.can_data); - } - - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - if (data.can_id == 0x201) - gimbal_left_friction_.store_status(data.can_data); - else if (data.can_id == 0x202) - gimbal_right_friction_.store_status(data.can_data); - else if (data.can_id == 0x203) - scope_motor_.store_status(data.can_data); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); - } - - std::atomic& hard_sync_pending_; - OutputInterface& tf_; - - OutputInterface gimbal_yaw_velocity_bmi088_; - OutputInterface gimbal_pitch_velocity_encoder_; - - device::Bmi088 bmi088_; - device::LkMotor gimbal_pitch_motor_; - device::DjiMotor gimbal_left_friction_; - device::DjiMotor gimbal_right_friction_; - device::DjiMotor scope_motor_; - }; - - OutputInterface tf_; - InputInterface timestamp_; - std::atomic hard_sync_pending_{false}; - size_t hard_sync_snapshot_count_ = 0; - Clock::time_point next_hard_sync_log_time_{}; - - std::shared_ptr deformable_infantry_command_; - std::unique_ptr rmcs_board_lite; - std::unique_ptr top_board_; - - rclcpp::Subscription::SharedPtr steers_calibrate_subscription_; - rclcpp::Subscription::SharedPtr joints_calibrate_subscription_; - rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; - - uint32_t cmd_tick_ = 0; -}; - -} // namespace rmcs_core::hardware - -#include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DeformableInfantryV2, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp index 2be69d868..b5e7640e1 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp @@ -1,15 +1,20 @@ #include #include +#include #include #include +#include +#include #include #include #include #include +#include #include #include "referee/app/ui/shape/shape.hpp" +#include "referee/app/ui/widget/animated_toggle.hpp" #include "referee/app/ui/widget/crosshair_circle.hpp" #include "referee/app/ui/widget/deformable_chassis_top_view.hpp" #include "referee/app/ui/widget/status_ring.hpp" @@ -26,13 +31,16 @@ class DeformableInfantry get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , crosshair_circle_(Shape::Color::WHITE, x_center - 2, y_center - 30, 8, 2) - , status_ring_(26.5, 26.5, 600, 300) + , status_ring_(24.0, 26.5, 600, 300) , horizontal_center_guidelines_( {Shape::Color::WHITE, 2, x_center - 360, y_center, x_center - 110, y_center}, {Shape::Color::WHITE, 2, x_center + 110, y_center, x_center + 360, y_center}) , vertical_center_guidelines_( {Shape::Color::WHITE, 2, x_center, 800, x_center, y_center + 110}, {Shape::Color::WHITE, 2, x_center, y_center - 110, x_center, 200}) + , friction_wheel_speed_indicator_( + Shape::Color::PINK, friction_wheel_speed_indicator_font_size_, 2, 0, + friction_wheel_speed_indicator_y(), 0) , chassis_direction_indicator_(Shape::Color::PINK, 8, x_center, y_center, 0, 0, 84, 84) , time_reminder_(Shape::Color::PINK, 50, 5, x_center + 150, y_center + 65, 0, false) { @@ -47,55 +55,79 @@ class DeformableInfantry deformable_chassis_leg_arcs_.set_angle_range( deformable_leg_min_angle_deg, deformable_leg_max_angle_deg); + register_input("/predefined/timestamp", timestamp_); register_input("/chassis/control_mode", chassis_mode_); + register_input("/chassis/active_suspension/active", active_suspension_active_); register_input("/chassis/angle", chassis_angle_); - register_input("/chassis/supercap/voltage", supercap_voltage_); register_input("/chassis/supercap/enabled", supercap_enabled_); register_input("/chassis/voltage", chassis_voltage_); - register_input("/chassis/left_front_wheel/velocity", left_front_velocity_); - register_input("/chassis/left_back_wheel/velocity", left_back_velocity_); - register_input("/chassis/right_back_wheel/velocity", right_back_velocity_); - register_input("/chassis/right_front_wheel/velocity", right_front_velocity_); - - register_input( - "/chassis/left_front_joint/physical_angle", left_front_joint_physical_angle_, false); - register_input( - "/chassis/left_back_joint/physical_angle", left_back_joint_physical_angle_, false); - register_input( - "/chassis/right_back_joint/physical_angle", right_back_joint_physical_angle_, false); - register_input( - "/chassis/right_front_joint/physical_angle", right_front_joint_physical_angle_, false); + for (size_t i = 0; i < kJointCount; ++i) { + register_input( + fmt::format("/chassis/{}_joint/physical_angle", kJointName[i]), + joint_physical_angle_[i], false); + } register_input("/referee/shooter/bullet_allowance", robot_bullet_allowance_); + register_input("/gimbal/left_friction/working_velocity", left_friction_working_velocity_); register_input("/gimbal/left_friction/control_velocity", left_friction_control_velocity_); register_input("/gimbal/left_friction/velocity", left_friction_velocity_); register_input("/gimbal/right_friction/velocity", right_friction_velocity_); register_input("/remote/mouse", mouse_); + register_input("/remote/keyboard", keyboard_); register_input("/referee/game/stage", game_stage_); + + friction_wheel_speed_indicator_.set_center_x(friction_wheel_speed_indicator_center_x()); + + crosshair_base_x_ = crosshair_circle_.x(); + crosshair_base_y_ = crosshair_circle_.y(); + ctrl_transition_.reset(false); } void update() override { update_chassis_direction_indicator(); update_deformable_chassis_leg_arcs(); + update_ctrl_ui(); status_ring_.update_bullet_allowance(*robot_bullet_allowance_); - status_ring_.update_friction_wheel_speed( - std::min(*left_friction_velocity_, *right_friction_velocity_), - *left_friction_control_velocity_ > 0); - status_ring_.update_supercap(*supercap_voltage_, *supercap_enabled_); + const double friction_wheel_speed = + std::min(*left_friction_velocity_, *right_friction_velocity_); + const bool friction_wheel_enabled = *left_friction_control_velocity_ > 0; + const auto friction_wheel_speed_value = + static_cast(std::lround(*left_friction_working_velocity_)); + status_ring_.update_friction_wheel_speed(friction_wheel_speed, friction_wheel_enabled); + if (friction_wheel_speed_indicator_.value() != friction_wheel_speed_value) { + friction_wheel_speed_indicator_.set_value(friction_wheel_speed_value); + friction_wheel_speed_indicator_.set_center_x(friction_wheel_speed_indicator_center_x()); + } + friction_wheel_speed_indicator_.set_color( + friction_wheel_enabled ? Shape::Color::GREEN : Shape::Color::PINK); + status_ring_.update_supercap_energy( + *supercap_voltage_, *supercap_enabled_, supercap_cutoff_voltage); status_ring_.update_battery_power(*chassis_voltage_); status_ring_.update_auto_aim_enable(mouse_->right == 1); } private: + void update_ctrl_ui() { + const bool ctrl_active = keyboard_.ready() && keyboard_->ctrl; + const double reveal = ctrl_transition_.update(*timestamp_, ctrl_active); + + crosshair_circle_.set_x( + static_cast( + std::lround(static_cast(crosshair_base_x_) + 45.0 * reveal))); + crosshair_circle_.set_y( + static_cast( + std::lround(static_cast(crosshair_base_y_) + 20.0 * reveal))); + } + void update_time_reminder() { if (!game_stage_.ready()) return; @@ -104,44 +136,62 @@ class DeformableInfantry void update_chassis_direction_indicator() { auto chassis_mode = *chassis_mode_; - auto to_referee_angle = [](double angle) { - return static_cast( - std::round((2 * std::numbers::pi - angle) / std::numbers::pi * 180)); - }; chassis_direction_indicator_.set_color(chassis_direction_indicator_color(chassis_mode)); - chassis_direction_indicator_.set_angle(to_referee_angle(*chassis_angle_), 30); + chassis_direction_indicator_.set_angle(0, 30); } static Shape::Color chassis_direction_indicator_color(rmcs_msgs::ChassisMode mode) { switch (mode) { - case rmcs_msgs::ChassisMode::SPIN: return Shape::Color::GREEN; + case rmcs_msgs::ChassisMode::SPIN_FAST: return Shape::Color::GREEN; case rmcs_msgs::ChassisMode::AUTO: return Shape::Color::CYAN; - case rmcs_msgs::ChassisMode::STEP_DOWN: return Shape::Color::WHITE; - default: return Shape::Color::PINK; + case rmcs_msgs::ChassisMode::STEP_DOWN: return Shape::Color::PINK; + default: return Shape::Color::WHITE; } } void update_deformable_chassis_leg_arcs() { - if (!left_front_joint_physical_angle_.ready() || !left_back_joint_physical_angle_.ready() - || !right_back_joint_physical_angle_.ready() - || !right_front_joint_physical_angle_.ready()) { + if (!std::all_of( + joint_physical_angle_.begin(), joint_physical_angle_.end(), + [](const auto& j) { return j.ready(); })) { deformable_chassis_leg_arcs_.set_visible(false); return; } - const std::array leg_angles = { - *left_front_joint_physical_angle_, - *left_back_joint_physical_angle_, - *right_back_joint_physical_angle_, - *right_front_joint_physical_angle_, - }; - deformable_chassis_leg_arcs_.update(*chassis_angle_, leg_angles); + if (!chassis_angle_.ready()) { + deformable_chassis_leg_arcs_.set_visible(false); + return; + } + + std::array leg_angles; + for (size_t i = 0; i < kJointCount; ++i) + leg_angles[i] = *joint_physical_angle_[i]; + deformable_chassis_leg_arcs_.update( + *chassis_angle_, leg_angles, + active_suspension_active_.ready() && *active_suspension_active_); } static constexpr uint16_t screen_width = 1920, screen_height = 1080; static constexpr uint16_t x_center = screen_width / 2, y_center = screen_height / 2; + static constexpr double friction_wheel_speed_indicator_radius_ = 430.0; + static constexpr uint16_t friction_wheel_speed_indicator_font_size_ = 20; + static constexpr double supercap_cutoff_voltage = 8.0; + + static uint16_t friction_wheel_speed_indicator_center_x() { + return static_cast(std::lround( + static_cast(x_center) + + friction_wheel_speed_indicator_radius_ / std::numbers::sqrt2)); + } + static uint16_t friction_wheel_speed_indicator_y() { + return static_cast(std::lround( + static_cast(y_center) + + friction_wheel_speed_indicator_radius_ / std::numbers::sqrt2 + - static_cast(friction_wheel_speed_indicator_font_size_) / 2.0)); + } + + InputInterface timestamp_; InputInterface chassis_mode_; + InputInterface active_suspension_active_; InputInterface chassis_angle_; InputInterface supercap_voltage_; @@ -149,18 +199,25 @@ class DeformableInfantry InputInterface chassis_voltage_; - InputInterface left_front_velocity_, left_back_velocity_, right_back_velocity_, - right_front_velocity_; - InputInterface left_front_joint_physical_angle_, left_back_joint_physical_angle_, - right_back_joint_physical_angle_, right_front_joint_physical_angle_; + static constexpr size_t kJointCount = 4; + static constexpr const char* kJointName[] = { + "left_front", + "left_back", + "right_back", + "right_front", + }; + + std::array, kJointCount> joint_physical_angle_; InputInterface robot_bullet_allowance_; + InputInterface left_friction_working_velocity_; InputInterface left_friction_control_velocity_; InputInterface left_friction_velocity_; InputInterface right_friction_velocity_; InputInterface mouse_; + InputInterface keyboard_; InputInterface game_stage_; @@ -169,10 +226,15 @@ class DeformableInfantry Line horizontal_center_guidelines_[2]; Line vertical_center_guidelines_[2]; + Integer friction_wheel_speed_indicator_; Arc chassis_direction_indicator_; DeformableChassisLegArcs deformable_chassis_leg_arcs_; + AnimatedToggle ctrl_transition_{}; + uint16_t crosshair_base_x_ = 0; + uint16_t crosshair_base_y_ = 0; + Integer time_reminder_; }; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp index 8caef5fff..dc21216b5 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp @@ -21,7 +21,8 @@ class DeformableChassisLegArcs { std::swap(min_angle_rad_, max_angle_rad_); } - void update(double chassis_angle, const std::array& leg_angles) { + void update( + double chassis_angle, const std::array& leg_angles, bool active_suspension) { if (!valid_angle_range_()) { set_visible(false); return; @@ -36,7 +37,7 @@ class DeformableChassisLegArcs { last_leg_angles_[i] = leg_angles[i]; update_leg_( legs_[i], last_chassis_angle_ + leg_base_mid_angles_[i], leg_radii_near_[i], - leg_radii_far_[i], last_leg_angles_[i]); + leg_radii_far_[i], last_leg_angles_[i], active_suspension); } } @@ -50,9 +51,9 @@ class DeformableChassisLegArcs { static constexpr uint16_t center_y_ = 1080 / 2; static constexpr uint16_t front_leg_radius_near_ = 102; - static constexpr uint16_t rear_leg_radius_near_ = 112; + static constexpr uint16_t rear_leg_radius_near_ = 102; static constexpr uint16_t front_leg_radius_far_ = 132; - static constexpr uint16_t rear_leg_radius_far_ = 142; + static constexpr uint16_t rear_leg_radius_far_ = 132; static constexpr uint16_t prone_leg_width_ = 6; static constexpr uint16_t upright_leg_width_ = 16; @@ -106,17 +107,13 @@ class DeformableChassisLegArcs { (leg_angle - min_angle_rad_) / (max_angle_rad_ - min_angle_rad_), 0.0, 1.0); } - static Shape::Color leg_color_(double normalized_extension) { - if (normalized_extension < 1.0 / 3.0) - return Shape::Color::ORANGE; - if (normalized_extension < 2.0 / 3.0) - return Shape::Color::YELLOW; - return Shape::Color::WHITE; + static Shape::Color leg_color_(bool active_suspension) { + return active_suspension ? Shape::Color::YELLOW : Shape::Color::WHITE; } void update_leg_( - Arc& leg, double body_angle, uint16_t near_radius, uint16_t far_radius, - double leg_angle) const { + Arc& leg, double body_angle, uint16_t near_radius, uint16_t far_radius, double leg_angle, + bool active_suspension) const { const double normalized_extension = normalized_leg_extension_(leg_angle); // Min angle looks like a thin leg stretching radially outward from the center ring. const uint16_t radius = static_cast( @@ -131,7 +128,7 @@ class DeformableChassisLegArcs { leg.set_y(center_y_); leg.set_r(radius); leg.set_width(width); - leg.set_color(leg_color_(normalized_extension)); + leg.set_color(leg_color_(active_suspension)); leg.set_angle(to_referee_angle_(body_angle), half_angle); } From a27e3d14c36ef8d780287e154b32ed4a70bbbdc9 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Mon, 10 Aug 2026 18:07:15 +0800 Subject: [PATCH 11/86] feat(flight): Update flight hardware and add PX4 vision bridge --- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 75 ++++- .../controller/flight/px4_vision_bridge.cpp | 266 ++++++++++++++++++ rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 216 +++++++------- 3 files changed, 448 insertions(+), 109 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/controller/flight/px4_vision_bridge.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index d0082799b..04bb643e9 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -3,6 +3,7 @@ rmcs_executor: update_rate: 1000.0 components: - rmcs_core::hardware::Flight -> flight_hardware + - rmcs_core::hardware::Px4VisionBridge -> px4_vision_bridge - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller @@ -26,15 +27,69 @@ rmcs_executor: # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster - - rmcs::AutoAimComponent - -odin_ros_driver: - ros__parameters: - enabled: true - config_file: "/rmcs_install/share/odin_ros_driver/config/control_command.yaml" - node_name: host_sdk_sample - respawn: true - respawn_delay: 1.0 + - rmcs::AutoAimCapturerComponent -> auto_aim_capturer + # - rmcs::AutoAimPlayerComponent -> auto_aim_player + - rmcs::AutoAimRecorderComponent -> auto_aim_recorder + - rmcs::AutoAimComponent -> auto_aim_component + +auto_aim_capturer: + ros__parameters: + camera_name: "" + exposure_us: 3000.0 + gain: 8.0 + framerate: 120.0 + invert_image: false + rls_tau_sec: 10.0 + use_hardware_sync: false + delay_ms: 6.5 + +auto_aim_recorder: + ros__parameters: + output_path: "/tmp/autoaim/records" + queue_depth: 16 + flush_every_n_frames: 64 + max_duration_seconds: 0 + max_videos_size_gb: 0.0 + +auto_aim_component: + ros__parameters: + # WARN: 危险！可选 red | blue，生效后裁判系统 ID 缺席时 + # 将强行绑定为对应阵营哨兵，仅供调试，严禁比赛启用。 + # 留空或填 unknow 表示禁用。 + dangerous_fallback: "" + manual_shoot: true + enable_rune: false + camera_translation: [0.10238, 0.0, 0.05286] + fire_control: + bullet_speed: 22.5 + shoot_delay: 0.05 + offset_yaw: +1.3 #越大越左 + offset_pitch: +1.7 #越大越下 + attack_window: 80.0 + degraded_angle_speed: 12.0 + window_redundancy: 0.8 + window_hysteresis: 0.2 + is_lazy_gimbal: false + attack_preaim: false + require_stable_command: false + yaw_tolerance: 0.07 + pitch_tolerance: 0.04 + rune_idle_duration: 0.4 + rune_shoot_duration: 0.2 + +px4_vision_bridge: + ros__parameters: + source_topic: /odin1/odometry_highfreq + system_id: 1 # 与 PX4 MAV_SYS_ID 一致 + component_id: 197 # MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY + max_send_rate_hz: 50.0 + # 挂载角(ZYX, rad): 传感器系->机体系, 初值 roll=π pitch=π/2 yaw=0 + mount_rpy: [3.14159265358979, 1.5707963267949, 0.0] + # Odin1 自启看门狗: 里程计断流超过 timeout 秒且距上次拉起超过 cooldown 秒 + # 才执行 tmux-launch.sh; 数据正常时不会重启 Odin1 (保住 SLAM 预热) + odin_autostart: true + odin_watchdog_timeout: 10.0 + odin_restart_cooldown: 60.0 # 需覆盖 SLAM 预热 30~60s value_broadcaster: ros__parameters: @@ -88,7 +143,7 @@ yaw_velocity_pid_controller: measurement: /gimbal/yaw/velocity_imu setpoint: /gimbal/yaw/control_velocity control: /gimbal/yaw/control_torque - kp: 8.0 + kp: 6.0 ki: 0.0 kd: 0.0 diff --git a/rmcs_ws/src/rmcs_core/src/controller/flight/px4_vision_bridge.cpp b/rmcs_ws/src/rmcs_core/src/controller/flight/px4_vision_bridge.cpp new file mode 100644 index 000000000..40c64049c --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/flight/px4_vision_bridge.cpp @@ -0,0 +1,266 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware { + +class Px4VisionBridge + : public rmcs_executor::Component + , public rclcpp::Node { +public: + Px4VisionBridge() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , logger_(get_logger()) { + + register_input("/px4/serial", px4_serial_); + + source_topic_ = get_parameter("source_topic").as_string(); + system_id_ = get_parameter("system_id").as_int(); + component_id_ = get_parameter("component_id").as_int(); + max_send_rate_hz_ = get_parameter("max_send_rate_hz").as_double(); + + const auto m = get_parameter("mount_rpy").as_double_array(); + + q_mount_ = quaternion_from_rpy_zyx(m.at(0), m.at(1), m.at(2)); + + min_send_interval_ = rclcpp::Duration::from_seconds(1.0 / max_send_rate_hz_); + last_send_time_ = now(); + last_heartbeat_send_ = now(); + + odom_subscription_ = create_subscription( + source_topic_, rclcpp::SensorDataQoS{}, + [this](nav_msgs::msg::Odometry::UniquePtr msg) { odometry_callback(std::move(msg)); }); + + odin_autostart_ = get_parameter("odin_autostart").as_bool(); + odin_watchdog_timeout_ = get_parameter("odin_watchdog_timeout").as_double(); + odin_restart_cooldown_ = get_parameter("odin_restart_cooldown").as_double(); + + last_odom_arrival_ns_.store(steady_now_ns(), std::memory_order_relaxed); + if (odin_autostart_) + odin_manager_thread_ = + std::jthread{[this](const std::stop_token& st) { odin_manager_loop(st); }}; + } + + Px4VisionBridge(const Px4VisionBridge&) = delete; + Px4VisionBridge& operator=(const Px4VisionBridge&) = delete; + + void update() override { + send_heartbeat_if_due(); // 1hz HEARTBEAT + send_vision_if_pending(); // 消费回调缓存的最新位姿,变换并发送 + } + +private: + struct OdomSample { + rclcpp::Time stamp{0, 0, RCL_ROS_TIME}; + Eigen::Vector3d position{Eigen::Vector3d::Zero()}; + Eigen::Quaterniond orientation{Eigen::Quaterniond::Identity()}; + }; + + void odometry_callback(const nav_msgs::msg::Odometry::UniquePtr msg) { + last_odom_arrival_ns_.store(steady_now_ns(), std::memory_order_relaxed); + + const auto& p = msg->pose.pose.position; + const auto& o = msg->pose.pose.orientation; + + const std::scoped_lock lock{odom_mutex_}; + pending_odom_.stamp = rclcpp::Time{msg->header.stamp}; + pending_odom_.position = Eigen::Vector3d{p.x, p.y, p.z}; + pending_odom_.orientation = Eigen::Quaterniond{o.w, o.x, o.y, o.z}; + odom_pending_ = true; + } + + void send_vision_if_pending() { + if ((now() - last_send_time_) < min_send_interval_) + return; + + OdomSample sample; + { + const std::scoped_lock lock{odom_mutex_}; + if (!odom_pending_) + return; + odom_pending_ = false; + sample = pending_odom_; + } + last_send_time_ = now(); + + const auto x = static_cast(sample.position.y()); + const auto y = static_cast(sample.position.x()); + const auto z = static_cast(-sample.position.z()); + + const Eigen::Quaterniond q_enu_flu = sample.orientation * q_mount_; + + Eigen::Quaterniond q_ned_frd = kNedEnuQ * (q_enu_flu * kAircraftBaselinkQ); + + q_ned_frd.normalize(); + double roll, pitch, yaw; + quaternion_to_rpy_zyx(q_ned_frd, roll, pitch, yaw); + + const auto usec = static_cast(sample.stamp.nanoseconds()) / 1000ull; + + float covariance[21]; + std::fill(std::begin(covariance), std::end(covariance), NAN); + + mavlink_message_t msg_mavlink; + mavlink_msg_vision_position_estimate_pack( + system_id_, component_id_, &msg_mavlink, usec, x, y, z, static_cast(roll), + static_cast(pitch), static_cast(yaw), covariance, + /*reset_counter=*/0); + + send_message(msg_mavlink); + } + + void send_heartbeat_if_due() { + if ((now() - last_heartbeat_send_).seconds() < 1.0) + return; + last_heartbeat_send_ = now(); + mavlink_message_t msg_mavlink; + mavlink_msg_heartbeat_pack( + system_id_, component_id_, &msg_mavlink, MAV_TYPE_ONBOARD_CONTROLLER, + MAV_AUTOPILOT_INVALID, + /*base_mode=*/0, + /*custom_mode=*/0, MAV_STATE_ACTIVE); + send_message(msg_mavlink); + } + + void send_message(const mavlink_message_t& msg) { + if (!px4_serial_.active()) [[unlikely]] + return; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + const uint16_t len = mavlink_msg_to_send_buffer(buffer, &msg); + px4_serial_->write(reinterpret_cast(buffer), len); + } + + static int64_t steady_now_ns() { + return std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); + } + + // 独立线程看门狗：断流超时且过了冷却期就拉起 tmux-launch.sh； + void odin_manager_loop(const std::stop_token& st) { + const std::string cmd = "\"" + ament_index_cpp::get_package_prefix("odin_ros_driver") + + "/lib/odin_ros_driver/tmux-launch.sh\""; + + const auto timeout_ns = static_cast(odin_watchdog_timeout_ * 1e9); + const auto cooldown_ns = static_cast(odin_restart_cooldown_ * 1e9); + int64_t last_launch_ns = steady_now_ns() - cooldown_ns; // 首次断流即可拉起 + bool online = true; // 启动宽限：先假定在线，超时才告警 + + // 锁仅为 wait_for 语义存在 request_stop 会直接唤醒等待 + std::mutex sleep_mutex; + std::condition_variable_any sleep_cv; + std::unique_lock lock{sleep_mutex}; + while (!sleep_cv.wait_for( + lock, st, std::chrono::seconds{1}, [&] { return st.stop_requested(); })) { + + const auto now_ns = steady_now_ns(); + const bool fresh = + (now_ns - last_odom_arrival_ns_.load(std::memory_order_relaxed)) < timeout_ns; + + if (fresh != online) { + online = fresh; + if (online) + RCLCPP_INFO(logger_, "Odin1 online, odometry flowing"); + else + RCLCPP_WARN( + logger_, + "Odin1 offline (no odometry for %.0fs), relaunching every %.0fs until " + "online", + odin_watchdog_timeout_, odin_restart_cooldown_); + } + + if (!online && (now_ns - last_launch_ns) > cooldown_ns) { + last_launch_ns = now_ns; + // （脚本内部的清杀/启动轮询）存在阻塞，单独在本线程做 + if (std::system(cmd.c_str()) != 0) + RCLCPP_ERROR(logger_, "Odin1 launch script failed"); + } + } + } + + static Eigen::Quaterniond quaternion_from_rpy_zyx(double roll, double pitch, double yaw) { + return Eigen::Quaterniond{ + Eigen::AngleAxisd{yaw, Eigen::Vector3d::UnitZ()} + * Eigen::AngleAxisd{pitch, Eigen::Vector3d::UnitY()} + * Eigen::AngleAxisd{roll, Eigen::Vector3d::UnitX()}}; + } + + static void quaternion_to_rpy_zyx( + const Eigen::Quaterniond& q, double& roll, double& pitch, double& yaw) { + const double w = q.w(), x = q.x(), y = q.y(), z = q.z(); + const double m20 = 2.0 * (x * z - w * y); // -sin(pitch) + const double m21 = 2.0 * (y * z + w * x); + const double m22 = 1.0 - 2.0 * (x * x + y * y); + const double sin_pitch = std::clamp(-m20, -1.0, 1.0); + const double cos_pitch = std::sqrt(m21 * m21 + m22 * m22); + + if (cos_pitch > 1e-9) { + pitch = std::atan2(sin_pitch, cos_pitch); + roll = std::atan2(m21, m22); + yaw = std::atan2(2.0 * (x * y + w * z), 1.0 - 2.0 * (y * y + z * z)); + return; + } + // 万向节锁 (pitch=±π/2)：roll/yaw 只剩组合自由度可观测， + // 约定 roll=0、组合角全部归 yaw，保证三元组重建仍是原旋转 + const double m11 = 1.0 - 2.0 * (x * x + z * z); + const double m12 = 2.0 * (y * z - w * x); + roll = 0.0; + pitch = (sin_pitch > 0.0) ? M_PI_2 : -M_PI_2; + yaw = (sin_pitch > 0.0) ? std::atan2(m12, m11) : std::atan2(-m12, m11); + } + + // ENU->NED + static inline const Eigen::Quaterniond kNedEnuQ{0.0, 0.70710678118655, 0.70710678118655, 0.0}; + // FLU->FRD + static inline const Eigen::Quaterniond kAircraftBaselinkQ{0.0, 1.0, 0.0, 0.0}; + + rclcpp::Logger logger_; + InputInterface px4_serial_; + rclcpp::Subscription::SharedPtr odom_subscription_; + + // 回调线程(rclcpp spin)与 update() 线程(executor)间的最新位姿交接 + std::mutex odom_mutex_; + OdomSample pending_odom_; + bool odom_pending_ = false; + + std::string source_topic_; + uint8_t system_id_; + uint8_t component_id_; + double max_send_rate_hz_; + Eigen::Quaterniond q_mount_; + + rclcpp::Duration min_send_interval_ = rclcpp::Duration::from_seconds(0.02); + rclcpp::Time last_send_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + rclcpp::Time last_heartbeat_send_{0, 0, RCL_ROS_TIME}; + + // Odin1 拉起与看门狗 + bool odin_autostart_; + double odin_watchdog_timeout_; + double odin_restart_cooldown_; + std::atomic last_odom_arrival_ns_{0}; + // 必须最后声明：其隐式析构 request_stop+join，须早于线程所用成员的销毁 + std::jthread odin_manager_thread_; +}; + +} // namespace rmcs_core::hardware + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Px4VisionBridge, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index c35bb8dfe..754807075 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -2,7 +2,6 @@ #include #include #include -#include #include #include @@ -10,29 +9,32 @@ #include #include #include +#include +#include #include #include #include -#include "hardware/device/bmi088.hpp" +#include "hardware/device/bmi088_ekf.hpp" +#include "hardware/device/board_clock_lifter.hpp" #include "hardware/device/can_packet.hpp" #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" -#include "librmcs/agent/rmcs_board_lite.hpp" +#include "hardware/device/remote_control.hpp" +#include "librmcs/board/rmcs_board_lite.hpp" namespace rmcs_core::hardware { class Flight : public rmcs_executor::Component , public rclcpp::Node - , private librmcs::agent::RmcsBoardLite { + , public librmcs::board::RmcsBoardLite::Callback { public: Flight() : Node{ get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , RmcsBoardLite{get_parameter("board_serial").as_string()} { + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} { gimbal_yaw_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::kMHF7015} @@ -43,16 +45,13 @@ class Flight device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( static_cast(get_parameter("pitch_motor_zero_point").as_int()))); gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 3} .set_reversed() .set_reduction_ratio(1.0)); gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.0)); + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 4}.set_reduction_ratio(1.0)); gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); - - bmi088_.set_coordinate_mapping( - [](double x, double y, double z) { return std::tuple{y, z, x}; }); + device::DjiMotor::Config{device::DjiMotor::Type::kM2006, 1}.enable_multi_turn_angle()); using namespace rmcs_description; @@ -61,12 +60,11 @@ class Flight tf_->set_transform( Eigen::Translation3d{kCameraPostionX, 0.0, kCameraPostionZ}); - register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); - register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_, 0.0); + register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_, 0.0); register_output("/tf", tf_); - register_output("/auto_aim/camera_transform", camera_transform_); - register_output("/auto_aim/barrel_direction", barrel_direction_); + register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); register_output("/referee/serial", referee_serial_); referee_serial_->read = [this](std::byte* buffer, size_t size) { @@ -74,11 +72,22 @@ class Flight [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); }; referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart1_transmit( - {.uart_data = std::span{buffer, size}}); + board_->start_transmit().uart_transmit( + Spec::kUarts.kUart1, {.uart_data = std::span{buffer, size}}); + return size; + }; + + register_output("/px4/serial", px4_serial_); + px4_serial_->read = [](std::byte*, size_t) { return size_t{0}; }; + px4_serial_->write = [this](const std::byte* buffer, size_t size) { + board_->start_transmit().uart_transmit( + Spec::kUarts.kUart0, {.uart_data = std::span{buffer, size}}); return size; }; + remote_control_ = std::make_unique(*this); + remote_control_->register_dr16(&dr16_); + status_service_ = create_service( "/rmcs/service/robot_status", [this]( @@ -86,46 +95,53 @@ class Flight const std_srvs::srv::Trigger::Response::SharedPtr& response) { status_service_callback(response); }); + + board_ = std::make_unique( + *this, get_parameter("board_serial").as_string()); } void update() override { update_motors(); update_imu(); dr16_.update_status(); - - using namespace rmcs_description; - *camera_transform_ = fast_tf::lookup_transform(*tf_); - *barrel_direction_ = - *fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + remote_control_->update(); } void command_update() { - auto builder = start_transmit(); + auto builder = board_->start_transmit(); builder - .can0_transmit( - {.can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - gimbal_left_friction_.generate_command(), - gimbal_right_friction_.generate_command(), - } - .as_bytes()}) - .can1_transmit( - {.can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes()}) - .can2_transmit( + .can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + gimbal_left_friction_.generate_command(), + gimbal_right_friction_.generate_command(), + } + .as_bytes(), + }) + .can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }) + .can_transmit( + Spec::kCans.kCan2, // {.can_id = 0x141, .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes()}) - .can3_transmit( + .can_transmit( + Spec::kCans.kCan3, // {.can_id = 0x142, .can_data = gimbal_pitch_motor_.generate_command().as_bytes()}); } @@ -147,13 +163,12 @@ class Flight void update_imu() { using namespace rmcs_description; - bmi088_.update_status(); - const auto gimbal_imu_pose = - Eigen::Quaterniond{bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - tf_->set_transform(gimbal_imu_pose.conjugate()); + if (const auto snapshot = bmi088_.snapshot()) { + tf_->set_transform(snapshot->orientation.conjugate()); - *gimbal_yaw_velocity_imu_ = bmi088_.gz(); - *gimbal_pitch_velocity_imu_ = bmi088_.gy(); + *gimbal_yaw_velocity_imu_ = snapshot->gyro_body.z(); + *gimbal_pitch_velocity_imu_ = snapshot->gyro_body.y(); + } } void @@ -172,63 +187,59 @@ class Flight } protected: - void can0_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission || data.can_data.size() < 8) - [[unlikely]] - return; - - if (data.can_id == 0x203) { - gimbal_left_friction_.store_status(data.can_data); - } else if (data.can_id == 0x204) { - gimbal_right_friction_.store_status(data.can_data); - } - } - void can1_receive_callback(const librmcs::data::CanDataView& data) override { + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { if (data.is_extended_can_id || data.is_remote_transmission || data.can_data.size() < 8) [[unlikely]] return; - if (data.can_id == 0x201) { - gimbal_bullet_feeder_.store_status(data.can_data); + if (can == Spec::kCans.kCan0) { + if (data.can_id == 0x203) { + gimbal_left_friction_.store_status(data.can_data); + } else if (data.can_id == 0x204) { + gimbal_right_friction_.store_status(data.can_data); + } + } else if (can == Spec::kCans.kCan1) { + if (data.can_id == 0x201) { + gimbal_bullet_feeder_.store_status(data.can_data); + } + } else if (can == Spec::kCans.kCan2) { + if (data.can_id == 0x141) { + gimbal_yaw_motor_.store_status(data.can_data); + } + } else if (can == Spec::kCans.kCan3) { + if (data.can_id == 0x142) { + gimbal_pitch_motor_.store_status(data.can_data); + } } } - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission || data.can_data.size() < 8) - [[unlikely]] - return; - - if (data.can_id == 0x141) { - gimbal_yaw_motor_.store_status(data.can_data); + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + if (uart == Spec::kUarts.kUart1) { + const std::byte* ptr = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&ptr](std::byte* storage) noexcept { new (storage) std::byte{*ptr++}; }, + data.uart_data.size()); + } else if (uart == Spec::kUarts.kDbus) { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); } } - void can3_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission || data.can_data.size() < 8) - [[unlikely]] - return; - if (data.can_id == 0x142) { - gimbal_pitch_motor_.store_status(data.can_data); - } + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { + const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); + bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); } - void uart1_receive_callback(const librmcs::data::UartDataView& data) override { - const std::byte* ptr = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&ptr](std::byte* storage) noexcept { new (storage) std::byte{*ptr++}; }, - data.uart_data.size()); - } - - void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); - } + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; - void accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); - } + const auto snapshot = + bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); + if (!snapshot) + return; - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); + imu_snapshot_output_.emit(*snapshot); } private: @@ -247,22 +258,29 @@ class Flight }; std::shared_ptr> status_service_; + std::unique_ptr board_; + device::LkMotor gimbal_yaw_motor_{*this, *command_component_, "/gimbal/yaw"}; device::LkMotor gimbal_pitch_motor_{*this, *command_component_, "/gimbal/pitch"}; device::DjiMotor gimbal_left_friction_{*this, *command_component_, "/gimbal/left_friction"}; device::DjiMotor gimbal_right_friction_{*this, *command_component_, "/gimbal/right_friction"}; device::DjiMotor gimbal_bullet_feeder_{*this, *command_component_, "/gimbal/bullet_feeder"}; - device::Dr16 dr16_{*this}; - device::Bmi088 bmi088_{1000.0, 0.2, 0.00}; + device::Dr16 dr16_; + std::unique_ptr remote_control_; + // 等价于旧 Bmi088 的坐标映射 (x, y, z) -> (y, z, x)：body = body_to_sensor^T * sensor + device::Bmi088Ekf bmi088_{device::Bmi088Ekf::Config{ + .body_to_sensor = (Eigen::Matrix3d{} << 0, 0, 1, 1, 0, 0, 0, 1, 0).finished(), + }}; + device::BoardClockLifter board_clock_lifter_; OutputInterface gimbal_yaw_velocity_imu_; OutputInterface gimbal_pitch_velocity_imu_; OutputInterface tf_; OutputInterface referee_serial_; + OutputInterface px4_serial_; - OutputInterface camera_transform_; - OutputInterface barrel_direction_; + EventOutputInterface imu_snapshot_output_; rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; }; From 05da2e4f177de22b09583f61d4fc76347e2980d0 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Mon, 13 Jul 2026 07:33:24 +0800 Subject: [PATCH 12/86] feat: Merge deformable-infantry with three-board omni support - Sync both omni and omni-b to BottomBoard/TopBoard/ImuBoard architecture - Update board serial filters and DeformableSuspension config in YAMLs - Remove Vt13/RemoteControl; use Dr16 direct /remote/* outputs - Clean up unrelated changes from feat/deformable-infantry --- .../config/deformable-infantry-omni-b.yaml | 129 +- .../config/deformable-infantry-omni.yaml | 112 +- rmcs_ws/src/rmcs_core/plugins.xml | 14 +- .../controller/chassis/deformable_chassis.cpp | 18 +- .../chassis/deformable_joint_controller.cpp | 4 +- .../controller/chassis/deformable_mode.hpp | 16 +- .../deformable_omni_wheel_controller.cpp | 30 +- .../chassis/deformable_suspension.cpp | 97 +- .../deformable_infantry_gimbal_controller.cpp | 88 +- .../shooting/friction_wheel_controller.cpp | 1 + .../hardware/deformable-infantry-omni-b.cpp | 1138 ++++++++--------- .../src/hardware/deformable-infantry-omni.cpp | 698 +++++----- .../referee/app/ui/deformable_infantry_ui.cpp | 30 +- 13 files changed, 1149 insertions(+), 1226 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index aa952607c..7e91dcc4b 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -30,80 +30,24 @@ rmcs_executor: - rmcs_core::controller::chassis::DeformableJointController -> rb_joint_controller - rmcs_core::controller::chassis::DeformableJointController -> rf_joint_controller - - rmcs::AutoAimComponent -> auto_aim_component - - rmcs::AutoAimCapturerComponent -> auto_aim_capturer - - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui - # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster - # - rmcs_core::debug::ValueCollector -> value_collector - -value_collector: - ros__parameters: - csv_path: "/tmp/pitch_.csv" - signals: - - /gimbal/pitch/angle - - /gimbal/pitch/velocity - - /gimbal/pitch/velocity_imu - - /gimbal/pitch/angle_error - - /gimbal/pitch/control_torque - - /gimbal/pitch/control_velocity - write_interval: 5 - flush_interval: 1000 + # - rmcs::AutoAimComponent value_broadcaster: ros__parameters: forward_list: - - /gimbal/pitch/angle - - /gimbal/pitch/velocity - -auto_aim_capturer: - ros__parameters: - camera_name: "" - exposure_us: 4000.0 - gain: 8.0 - framerate: 120.0 - invert_image: false - rls_tau_sec: 10.0 - use_hardware_sync: false - delay_ms: 6.5 - -auto_aim_component: - ros__parameters: - # WARN: 危险！可选 red | blue，生效后裁判系统 ID 缺席时 - # 将强行绑定为对应阵营哨兵，仅供调试，严禁比赛启用。 - # 留空或填 unknow 表示禁用。 - dangerous_fallback: "" - manual_shoot: true - enable_rune: true - camera_translation: [0.058, -0.08, 0.0] - fire_control: - bullet_speed: 22.5 - shoot_delay: 0.04 - offset_yaw: +2.5 - offset_pitch: +0.5 - attack_window: 80.0 - degraded_angle_speed: 12.0 - window_redundancy: 0.8 - window_hysteresis: 0.2 - attack_preaim: false - require_stable_command: false - yaw_tolerance: 0.07 - pitch_tolerance: 0.04 - rune_idle_duration: 0.4 - rune_shoot_duration: 0.2 - -auto_aim_ui: - ros__parameters: - offset_x: 0.0 - offset_y: +0.08 - offset_z: 0.0 + - /gimbal/yaw/angle + - /gimbal/yaw/velocity deformable_infantry: ros__parameters: - serial_filter_bottom_board: "AF-73B2-E8A1-A544-79ED-5BDA-D088-7F21-A6A6" - serial_filter_top_board: "AF-C26A-0C9C-CF41-3E3C-1596-524B-7527-5744" - chassis_radius: 0.2341741 - rod_length: 0.140 + serial_filter_rmcs_board: "AF-73B2-E8A1-A544-79ED-5BDA-D088-7F21-A6A6" + serial_filter_top_board: "AF-ABAC-786D-1B53-99F6-00A2-42A6-AA95-9D69" + serial_filter_imu: "AF-C26A-0C9C-CF41-3E3C-1596-524B-7527-5744" + left_front_zero_point: 7173 + left_back_zero_point: 5167 + right_back_zero_point: 3098 + right_front_zero_point: 6485 yaw_motor_zero_point: 57900 pitch_motor_zero_point: 56354 debug_log_supercap: false @@ -113,17 +57,17 @@ deformable_infantry: chassis_controller: ros__parameters: # Deploy geometry / chassis-owned joint intent - min_angle: 5.0 - max_angle: 59.0 + min_angle: 8.0 + max_angle: 58.0 active_suspension_enable: true spin_ratio: 1.0 deformable_suspension: ros__parameters: # IMU attitude correction at min-angle stance. - active_suspension_pitch_outer_kp: 12.0 - active_suspension_pitch_outer_ki: 0.02 - active_suspension_pitch_outer_kd: 0.0 + active_suspension_pitch_outer_kp: 8.0 + active_suspension_pitch_outer_ki: 0.35 + active_suspension_pitch_outer_kd: 0.28 active_suspension_pitch_outer_integral_min: -2.0 active_suspension_pitch_outer_integral_max: 2.0 active_suspension_pitch_outer_output_min: -3.0 @@ -137,9 +81,9 @@ deformable_suspension: active_suspension_pitch_inner_output_min: -0.785 active_suspension_pitch_inner_output_max: 0.785 - active_suspension_roll_outer_kp: 12.0 - active_suspension_roll_outer_ki: 0.02 - active_suspension_roll_outer_kd: 0.0 + active_suspension_roll_outer_kp: 8.0 + active_suspension_roll_outer_ki: 0.35 + active_suspension_roll_outer_kd: 0.28 active_suspension_roll_outer_integral_min: -2.0 active_suspension_roll_outer_integral_max: 2.0 active_suspension_roll_outer_output_min: -3.0 @@ -167,11 +111,10 @@ deformable_suspension: gimbal_controller: ros__parameters: - upper_limit: -0.47123 # -27 deg - lower_limit: 0.10 # 8 deg - ctrl_hold_pitch_target_angle: 0.0 + upper_limit: -0.65 # -35 deg + lower_limit: 0.05 # 6 deg - yaw_angle_kp: 15.0 + yaw_angle_kp: 30.0 yaw_angle_ki: 0.0 yaw_angle_kd: 0.0 @@ -179,16 +122,18 @@ gimbal_controller: yaw_velocity_ki: 0.0 yaw_velocity_kd: 0.0 - pitch_angle_kp: 35.0 - pitch_angle_ki: 0.02 - pitch_angle_kd: 0.3 + yaw_vel_ff_gain: 0.47 + yaw_acc_ff_gain: 0.00 + + pitch_angle_kp: 25.0 + pitch_angle_ki: 0.0 + pitch_angle_kd: 0.0 - pitch_velocity_kp: 2.0 + pitch_velocity_kp: 2.2 pitch_velocity_ki: 0.0 pitch_velocity_kd: 0.0 - pitch_gravity_ff_gain: 4.302 - pitch_gravity_ff_phase: 0.589 + pitch_acc_ff_gain: 0.10 pitch_torque_control: true @@ -205,7 +150,7 @@ friction_wheel_controller: heat_controller: ros__parameters: heat_per_shot: 10000 - reserved_heat: 15000 + reserved_heat: 5000 bullet_feeder_controller: ros__parameters: @@ -247,9 +192,11 @@ bullet_feeder_velocity_pid_controller: deformable_chassis_controller: ros__parameters: - mass: 25.5 + mass: 23.0 moment_of_inertia: 1.0 - wheel_radius: 0.075 + chassis_radius: 0.2341741 + rod_length: 0.150 + wheel_radius: 0.07 friction_coefficient: 6.6 k1: 2.958580e+00 k2: 3.082190e-03 @@ -257,10 +204,12 @@ deformable_chassis_controller: lf_joint_controller: ros__parameters: + # Joint-local servo inputs produced by chassis intent generation measurement_angle: /chassis/left_front_joint/physical_angle setpoint_angle: /chassis/left_front_joint/target_physical_angle setpoint_velocity: /chassis/left_front_joint/target_physical_velocity control: /chassis/left_front_joint/control_torque + dt: 0.001 b0: -1.0 kt: 1.0 @@ -277,9 +226,9 @@ lf_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 - lb_joint_controller: ros__parameters: + # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/left_back_joint/physical_angle setpoint_angle: /chassis/left_back_joint/target_physical_angle setpoint_velocity: /chassis/left_back_joint/target_physical_velocity @@ -300,9 +249,9 @@ lb_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 - rb_joint_controller: ros__parameters: + # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_back_joint/physical_angle setpoint_angle: /chassis/right_back_joint/target_physical_angle setpoint_velocity: /chassis/right_back_joint/target_physical_velocity @@ -323,9 +272,9 @@ rb_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 - rf_joint_controller: ros__parameters: + # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_front_joint/physical_angle setpoint_angle: /chassis/right_front_joint/target_physical_angle setpoint_velocity: /chassis/right_front_joint/target_physical_velocity diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 272d4f091..023fe4e39 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -30,80 +30,32 @@ rmcs_executor: - rmcs_core::controller::chassis::DeformableJointController -> rb_joint_controller - rmcs_core::controller::chassis::DeformableJointController -> rf_joint_controller - - rmcs::AutoAimComponent -> auto_aim_component - - rmcs::AutoAimCapturerComponent -> auto_aim_capturer - - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui - # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster - # - rmcs_core::debug::ValueCollector -> value_collector - -value_collector: - ros__parameters: - csv_path: "/tmp/pitch_.csv" - signals: - - /gimbal/pitch/angle - - /gimbal/pitch/velocity - - /gimbal/pitch/velocity_imu - - /gimbal/pitch/angle_error - - /gimbal/pitch/control_torque - - /gimbal/pitch/control_velocity - write_interval: 5 - flush_interval: 1000 + # - rmcs::AutoAimComponent value_broadcaster: ros__parameters: forward_list: - - /gimbal/pitch/angle - - /gimbal/pitch/velocity - -auto_aim_capturer: - ros__parameters: - camera_name: "" - exposure_us: 4000.0 - gain: 8.0 - framerate: 120.0 - invert_image: false - rls_tau_sec: 10.0 - use_hardware_sync: true - delay_ms: 6.5 - -auto_aim_component: - ros__parameters: - # WARN: 危险！可选 red | blue，生效后裁判系统 ID 缺席时 - # 将强行绑定为对应阵营哨兵，仅供调试，严禁比赛启用。 - # 留空或填 unknow 表示禁用。 - dangerous_fallback: "" - manual_shoot: true - enable_rune: true - camera_translation: [0.058, -0.08, 0.0] - fire_control: - bullet_speed: 22.5 - shoot_delay: 0.04 - offset_yaw: +0.1 - offset_pitch: -0.4 - attack_window: 80.0 - degraded_angle_speed: 12.0 - window_redundancy: 0.8 - window_hysteresis: 0.2 - attack_preaim: false - require_stable_command: false - yaw_tolerance: 0.07 - pitch_tolerance: 0.04 - rune_idle_duration: 0.4 - rune_shoot_duration: 0.2 - -auto_aim_ui: - ros__parameters: - offset_x: 0.0 - offset_y: -0.08 - offset_z: 0.0 + - /gimbal/yaw/angle + - /gimbal/yaw/velocity + - /chassis/left_front_joint/physical_angle + - /chassis/left_front_joint/physical_velocity + - /chassis/left_back_joint/physical_angle + - /chassis/left_back_joint/physical_velocity + - /chassis/right_back_joint/physical_angle + - /chassis/right_back_joint/physical_velocity + - /chassis/right_front_joint/physical_angle + - /chassis/right_front_joint/physical_velocity deformable_infantry: ros__parameters: - serial_filter_bottom_board: "AF-23FB-EE32-B892-1302-AE70-D640-7B4E-0CBF" - serial_filter_top_board: "AF-7A42-07AA-D181-0356-7715-6D7C-4C65-5762" - chassis_radius: 0.2341741 - rod_length: 0.140 + serial_filter_rmcs_board: "AF-23FB-EE32-B892-1302-AE70-D640-7B4E-0CBF" + serial_filter_top_board: "AF-8AE3-4EC1-03C3-C494-88FE-2DC4-3018-0298" + serial_filter_imu: "AF-7A42-07AA-D181-0356-7715-6D7C-4C65-5762" + left_front_zero_point: 374 + left_back_zero_point: 5801 + right_back_zero_point: 7817 + right_front_zero_point: 7136 yaw_motor_zero_point: 43365 pitch_motor_zero_point: 6432 debug_log_supercap: false @@ -168,27 +120,31 @@ deformable_suspension: gimbal_controller: ros__parameters: upper_limit: -0.47123 # -27 deg - lower_limit: 0.10 # 8 deg + lower_limit: 0.15707 # 9 deg ctrl_hold_pitch_target_angle: 0.0 - yaw_angle_kp: 10.0 + yaw_angle_kp: 30.0 yaw_angle_ki: 0.0 yaw_angle_kd: 0.0 - yaw_velocity_kp: 10.0 + yaw_velocity_kp: 15.0 yaw_velocity_ki: 0.0 yaw_velocity_kd: 0.0 - pitch_angle_kp: 35.0 - pitch_angle_ki: 0.02 - pitch_angle_kd: 0.3 + yaw_vel_ff_gain: 0.47 + yaw_acc_ff_gain: 0.00 - pitch_velocity_kp: 2.0 + pitch_angle_kp: 7.2 + pitch_angle_ki: 0.0 + pitch_angle_kd: 0.0 + + pitch_velocity_kp: 3.0 pitch_velocity_ki: 0.0 pitch_velocity_kd: 0.0 - pitch_gravity_ff_gain: 4.302 - pitch_gravity_ff_phase: 0.589 + pitch_acc_ff_gain: 0.10 + pitch_gravity_ff_gain: 5.95 + pitch_gravity_ff_phase: 0.66 pitch_torque_control: true @@ -204,8 +160,8 @@ friction_wheel_controller: heat_controller: ros__parameters: - heat_per_shot: 10000 - reserved_heat: 15000 + heat_per_shot: 10 + reserved_heat: 15 bullet_feeder_controller: ros__parameters: @@ -249,6 +205,8 @@ deformable_chassis_controller: ros__parameters: mass: 25.5 moment_of_inertia: 1.0 + chassis_radius: 0.2341741 + rod_length: 0.140 wheel_radius: 0.075 friction_coefficient: 6.6 k1: 2.958580e+00 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index a066759ae..44fda9043 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -1,12 +1,12 @@ + + + - - - @@ -18,7 +18,6 @@ - @@ -48,21 +47,14 @@ - - - - - - - diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index 397400cfe..9cea6ef5b 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -53,18 +53,14 @@ class DeformableChassis register_output("/chassis/pitch_lock_active", pitch_lock_active_, false); register_output("/chassis/active_suspension/active", active_suspension_active_, false); register_output("/chassis/deformable/low_prone_active", low_prone_active_, false); - register_output( - "/chassis/deformable/symmetric_posture_target", symmetric_posture_target_, true); + register_output("/chassis/deformable/symmetric_posture_target", symmetric_posture_target_, true); register_output("/chassis/deformable/correction_inverted", correction_inverted_, false); + register_output("/chassis/deformable/min_angle_deg", min_angle_deg_, joint_mode_mgr_.min_angle()); + register_output("/chassis/deformable/max_angle_deg", max_angle_deg_, joint_mode_mgr_.max_angle()); register_output( - "/chassis/deformable/min_angle_deg", min_angle_deg_, joint_mode_mgr_.min_angle()); - register_output( - "/chassis/deformable/max_angle_deg", max_angle_deg_, joint_mode_mgr_.max_angle()); - register_output( - "/chassis/deformable/suspension_reference_angle_deg", suspension_reference_angle_deg_, - joint_mode_mgr_.suspension_reference_angle_deg()); - register_output( - "/chassis/deformable/reset_count", deformable_reset_count_, static_cast(0)); + "/chassis/deformable/suspension_reference_angle_deg", + suspension_reference_angle_deg_, joint_mode_mgr_.suspension_reference_angle_deg()); + register_output("/chassis/deformable/reset_count", deformable_reset_count_, static_cast(0)); for (size_t i = 0; i < kJointCount; ++i) { register_output( fmt::format("/chassis/deformable/{}_joint/posture_target_angle", kJointName[i]), @@ -186,7 +182,7 @@ class DeformableChassis switch (*mode_) { case rmcs_msgs::ChassisMode::AUTO: break; - case rmcs_msgs::ChassisMode::SPIN_FAST: { + case rmcs_msgs::ChassisMode::SPIN: { bool forward = joint_mode_mgr_.spinning_forward(); angular_velocity = spin_ratio_ * (forward ? angular_velocity_max_ : -angular_velocity_max_); diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp index 88bfb2041..38bb7ec1c 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp @@ -102,8 +102,8 @@ class DeformableJointController config_.eso.beta1 = load_parameter_or(*this, "eso_beta1", 3.0 * config_.eso.w0); config_.eso.beta2 = load_parameter_or(*this, "eso_beta2", 3.0 * config_.eso.w0 * config_.eso.w0); - config_.eso.beta3 = - load_parameter_or(*this, "eso_beta3", config_.eso.w0 * config_.eso.w0 * config_.eso.w0); + config_.eso.beta3 = load_parameter_or( + *this, "eso_beta3", config_.eso.w0 * config_.eso.w0 * config_.eso.w0); config_.eso.z3_limit = load_parameter_or(*this, "eso_z3_limit", 1e9); config_.nlesf.k1 = load_parameter_or(*this, "k1", 50.0); diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp index dac11d423..923a9ff2c 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp @@ -82,7 +82,8 @@ class DeformableChassisModeManager { joint_posture_state_.ctrl_low_prone_active = keyboard.ctrl; joint_posture_state_.low_prone_active = joint_posture_state_.ctrl_low_prone_active || low_prone_enabled_by_toggle_; - joint_posture_state_.pitch_lock_active = joint_posture_state_.ctrl_low_prone_active; + joint_posture_state_.pitch_lock_active = + joint_posture_state_.ctrl_low_prone_active; update_suspension_mode_from_inputs_(switch_left, switch_right, keyboard, rotary_knob); update_posture_target_from_inputs_(switch_left, switch_right, keyboard, rotary_knob, dt); @@ -147,17 +148,17 @@ class DeformableChassisModeManager { if (last_switch_right_ == rmcs_msgs::Switch::MIDDLE && switch_right == rmcs_msgs::Switch::DOWN) { - if (next_mode == rmcs_msgs::ChassisMode::SPIN_FAST) { + if (next_mode == rmcs_msgs::ChassisMode::SPIN) { next_mode = rmcs_msgs::ChassisMode::STEP_DOWN; } else { - next_mode = rmcs_msgs::ChassisMode::SPIN_FAST; + next_mode = rmcs_msgs::ChassisMode::SPIN; joint_posture_state_.spinning_forward = !joint_posture_state_.spinning_forward; } } else if (!last_keyboard_.c && keyboard.c) { - if (next_mode == rmcs_msgs::ChassisMode::SPIN_FAST) { + if (next_mode == rmcs_msgs::ChassisMode::SPIN) { next_mode = rmcs_msgs::ChassisMode::AUTO; } else { - next_mode = rmcs_msgs::ChassisMode::SPIN_FAST; + next_mode = rmcs_msgs::ChassisMode::SPIN; joint_posture_state_.spinning_forward = !joint_posture_state_.spinning_forward; } } else if (!last_keyboard_.z && keyboard.z) { @@ -247,8 +248,9 @@ class DeformableChassisModeManager { if (posture_toggle_requested) { if (joint_posture_state_.suspension_active) { active_suspension_base_angle_ = - (std::abs(active_suspension_base_angle_ - max_angle_) < 1e-6) ? min_angle_ - : max_angle_; + (std::abs(active_suspension_base_angle_ - max_angle_) < 1e-6) + ? min_angle_ + : max_angle_; current_target_angle_ = active_suspension_base_angle_; apply_symmetric_target_ = true; joint_current_target_angle_.fill(current_target_angle_); diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp index 81b7b230d..74bc3d6b7 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp @@ -91,7 +91,7 @@ class DeformableOmniWheelController } private: - static constexpr size_t kWheelCount = 4; + static constexpr size_t kWheelCount = 4; static constexpr const char* kWheelName[] = { "left_front", "left_back", @@ -99,7 +99,7 @@ class DeformableOmniWheelController "right_front", }; static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - static constexpr double g_ = 9.81; + static constexpr double g_ = 9.81; struct ChassisControlTorque { Eigen::Vector2d torque; @@ -113,7 +113,7 @@ class DeformableOmniWheelController Eigen::Vector3d calculate_chassis_velocity(const Eigen::Vector4d& wheel_velocities) const { const auto& [w1, w2, w3, w4] = wheel_velocities; - const double a_plus_b = std::numbers::sqrt2 * std::max(*chassis_radius_, 1e-6); + const double a_plus_b = std::numbers::sqrt2 * std::max(*chassis_radius_, 1e-6); Eigen::Vector3d velocity; velocity.x() = -w1 - w2 + w3 + w4; velocity.y() = w1 - w2 - w3 + w4; @@ -133,16 +133,16 @@ class DeformableOmniWheelController result.torque.x() = translational_torque.norm(); const double a_plus_b = std::numbers::sqrt2 * std::max(*chassis_radius_, 1e-6); - result.torque.y() = (-std::numbers::sqrt2 / 4 * wheel_radius_) - * (moment_of_inertia_ / a_plus_b) - * angular_velocity_pid_calculator_.update(chassis_velocity_error[2]); + result.torque.y() = (-std::numbers::sqrt2 / 4 * wheel_radius_) + * (moment_of_inertia_ / a_plus_b) + * angular_velocity_pid_calculator_.update(chassis_velocity_error[2]); Eigen::Vector2d translational_torque_direction; if (result.torque.x() > 0) translational_torque_direction = translational_torque / result.torque.x(); else translational_torque_direction = Eigen::Vector2d::UnitX(); - auto& [x, y] = translational_torque_direction; + auto& [x, y] = translational_torque_direction; result.lambda = {-x + y, -x - y}; return result; @@ -167,13 +167,13 @@ class DeformableOmniWheelController const Eigen::Vector4d& wheel_pid_torques) const { const auto& [w1, w2, w3, w4] = wheel_velocities; - const auto& [x_max, y_max] = chassis_control_torque.torque; - const double y_sign = y_max > 0 ? 1.0 : -1.0; + const auto& [x_max, y_max] = chassis_control_torque.torque; + const double y_sign = y_max > 0 ? 1.0 : -1.0; const auto& [lambda_1, lambda_2] = chassis_control_torque.lambda; const auto& [t1, t2, t3, t4] = wheel_pid_torques; - const double rhombus_top = (friction_coefficient_ * mass_ * g_ * wheel_radius_) / 4; + const double rhombus_top = (friction_coefficient_ * mass_ * g_ * wheel_radius_) / 4; const double rhombus_right = rhombus_top / std::max(std::abs(lambda_1), std::abs(lambda_2)); const double a = 4 * k1_; @@ -189,14 +189,14 @@ class DeformableOmniWheelController Eigen::Vector2d result = Eigen::Vector2d::Constant(nan_); if (com_height_ > 1e-6) { - const double dir_x = -(lambda_1 + lambda_2) / 2.0; - const double dir_y = (lambda_1 - lambda_2) / 2.0; - const double coeff = -com_height_ / (std::numbers::sqrt2 * wheel_radius_); + const double dir_x = -(lambda_1 + lambda_2) / 2.0; + const double dir_y = (lambda_1 - lambda_2) / 2.0; + const double coeff = -com_height_ / (std::numbers::sqrt2 * wheel_radius_); const double gamma_1 = coeff * (+dir_x / chassis_radius_x_ + dir_y / chassis_radius_y_); const double gamma_2 = coeff * (-dir_x / chassis_radius_x_ + dir_y / chassis_radius_y_); const double force_to_torque = friction_coefficient_ * wheel_radius_; - const double rhs = force_to_torque * mass_ * g_ / 4.0; + const double rhs = force_to_torque * mass_ * g_ / 4.0; const std::vector half_planes = { {lambda_1 - force_to_torque * gamma_1, y_sign, rhs}, {-lambda_1 - force_to_torque * gamma_1, -y_sign, rhs}, @@ -223,7 +223,7 @@ class DeformableOmniWheelController static Eigen::Vector4d calculate_wheel_control_torques( ChassisControlTorque chassis_control_torque, Eigen::Vector4d wheel_pid_torques) { const auto& [lambda_1, lambda_2] = chassis_control_torque.lambda; - Eigen::Vector4d wheel_torques = { + Eigen::Vector4d wheel_torques = { +lambda_1 * chassis_control_torque.torque.x(), +lambda_2 * chassis_control_torque.torque.x(), -lambda_1 * chassis_control_torque.torque.x(), diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp index 6da008098..3f562ba01 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp @@ -3,8 +3,8 @@ #include #include #include -#include #include +#include #include #include @@ -29,12 +29,14 @@ class DeformableSuspension register_input("/chassis/active_suspension/active", active_suspension_active_); register_input("/chassis/deformable/reset_count", reset_count_, false); register_input("/chassis/deformable/low_prone_active", low_prone_active_); - register_input("/chassis/deformable/symmetric_posture_target", symmetric_posture_target_); + register_input( + "/chassis/deformable/symmetric_posture_target", symmetric_posture_target_); register_input("/chassis/deformable/correction_inverted", correction_inverted_); register_input("/chassis/deformable/min_angle_deg", min_angle_deg_); register_input("/chassis/deformable/max_angle_deg", max_angle_deg_); register_input( - "/chassis/deformable/suspension_reference_angle_deg", suspension_reference_angle_deg_); + "/chassis/deformable/suspension_reference_angle_deg", + suspension_reference_angle_deg_); register_input("/chassis/imu/pitch", chassis_imu_pitch_, false); register_input("/chassis/imu/roll", chassis_imu_roll_, false); @@ -52,10 +54,12 @@ class DeformableSuspension std::string{"/chassis/"} + kJointName[i] + "_joint/target_physical_angle", joint_target_angle_[i], nan_); register_output( - std::string{"/chassis/"} + kJointName[i] + "_joint/target_physical_velocity", + std::string{"/chassis/"} + kJointName[i] + + "_joint/target_physical_velocity", joint_target_velocity_[i], nan_); register_output( - std::string{"/chassis/"} + kJointName[i] + "_joint/target_physical_acceleration", + std::string{"/chassis/"} + kJointName[i] + + "_joint/target_physical_acceleration", joint_target_acceleration_[i], nan_); register_output( std::string{"/chassis/"} + kJointName[i] + "_joint/control_angle_error", @@ -111,9 +115,9 @@ class DeformableSuspension copy_joint_angle_states_(joint_angle_states); update_suspension_state_( *chassis_imu_pitch_ - pitch_offset_value_, *chassis_imu_roll_ - roll_offset_value_, - filtered_pitch_rate, filtered_roll_rate, *active_suspension_active_, *low_prone_active_, - *min_angle_deg_, *max_angle_deg_, *suspension_reference_angle_deg_, - *correction_inverted_, joint_angle_states, dt); + filtered_pitch_rate, filtered_roll_rate, *active_suspension_active_, + *low_prone_active_, *min_angle_deg_, *max_angle_deg_, + *suspension_reference_angle_deg_, *correction_inverted_, joint_angle_states, dt); const auto target_angles_rad = compute_joint_trajectory_targets_( posture_target_angles_rad, *active_suspension_active_, *low_prone_active_, @@ -155,9 +159,9 @@ class DeformableSuspension } void load_pid_( - const std::string& prefix, pid::PidCalculator& pid, double kp_default, double ki_default, - double kd_default, double integral_min_default, double integral_max_default, - double output_min_default, double output_max_default) { + const std::string& prefix, pid::PidCalculator& pid, double kp_default, + double ki_default, double kd_default, double integral_min_default, + double integral_max_default, double output_min_default, double output_max_default) { pid.kp = get_parameter_or(prefix + "kp", kp_default); pid.ki = get_parameter_or(prefix + "ki", ki_default); pid.kd = get_parameter_or(prefix + "kd", kd_default); @@ -169,51 +173,47 @@ class DeformableSuspension void load_config_() { joint_target_vel_limit_ = std::max( - deg_to_rad_(std::abs(get_parameter_or("target_physical_velocity_limit", 180.0))), 1e-6); + deg_to_rad_(std::abs(get_parameter_or("target_physical_velocity_limit", 180.0))), + 1e-6); joint_target_acc_limit_ = std::max( deg_to_rad_(std::abs(get_parameter_or("target_physical_acceleration_limit", 720.0))), 1e-6); suspension_target_vel_limit_ = std::max( - deg_to_rad_( - std::abs(get_parameter_or( - "active_suspension_target_velocity_limit_deg", - get_parameter_or("target_physical_velocity_limit", 180.0)))), + deg_to_rad_(std::abs(get_parameter_or( + "active_suspension_target_velocity_limit_deg", + get_parameter_or("target_physical_velocity_limit", 180.0)))), 1e-6); suspension_target_acc_limit_ = std::max( - deg_to_rad_( - std::abs(get_parameter_or( - "active_suspension_target_acceleration_limit_deg", - get_parameter_or("target_physical_acceleration_limit", 720.0)))), + deg_to_rad_(std::abs(get_parameter_or( + "active_suspension_target_acceleration_limit_deg", + get_parameter_or("target_physical_acceleration_limit", 720.0)))), 1e-6); load_pid_( - "active_suspension_pitch_outer_", pitch_outer_pid_, 8.0, 0.35, 0.28, -2.0, 2.0, -3.0, - 3.0); + "active_suspension_pitch_outer_", pitch_outer_pid_, 8.0, 0.35, 0.28, -2.0, 2.0, + -3.0, 3.0); load_pid_( - "active_suspension_pitch_inner_", pitch_inner_pid_, 2.0, 0.0, 0.0, -1.0, 1.0, -0.785, - 0.785); + "active_suspension_pitch_inner_", pitch_inner_pid_, 2.0, 0.0, 0.0, -1.0, 1.0, + -0.785, 0.785); load_pid_( - "active_suspension_roll_outer_", roll_outer_pid_, 8.0, 0.35, 0.28, -2.0, 2.0, -3.0, - 3.0); + "active_suspension_roll_outer_", roll_outer_pid_, 8.0, 0.35, 0.28, -2.0, 2.0, + -3.0, 3.0); load_pid_( - "active_suspension_roll_inner_", roll_inner_pid_, 2.0, 0.0, 0.0, -1.0, 1.0, -0.785, - 0.785); + "active_suspension_roll_inner_", roll_inner_pid_, 2.0, 0.0, 0.0, -1.0, 1.0, + -0.785, 0.785); active_correction_vel_limit_ = std::max( - deg_to_rad_( - std::abs( - get_parameter_or("active_suspension_correction_velocity_limit_deg", 720.0))), + deg_to_rad_(std::abs( + get_parameter_or("active_suspension_correction_velocity_limit_deg", 720.0))), 1e-6); active_correction_acc_limit_ = std::max( - deg_to_rad_( - std::abs(get_parameter_or( - "active_suspension_correction_acceleration_limit_deg", 3600.0))), + deg_to_rad_(std::abs( + get_parameter_or("active_suspension_correction_acceleration_limit_deg", 3600.0))), 1e-6); - active_rate_lpf_cutoff_hz_ = - std::max(get_parameter_or("active_suspension_rate_lpf_cutoff_hz", 10.0), 1e-6); + active_rate_lpf_cutoff_hz_ = std::max( + get_parameter_or("active_suspension_rate_lpf_cutoff_hz", 10.0), 1e-6); - calibration_wait_time_ = - std::max(get_parameter_or("chassis_imu_calibration_wait_s", 2.0), 0.0); + calibration_wait_time_ = std::max(get_parameter_or("chassis_imu_calibration_wait_s", 2.0), 0.0); calibration_sample_time_ = std::max(get_parameter_or("chassis_imu_calibration_sample_s", 3.0), 1e-6); } @@ -366,7 +366,8 @@ class DeformableSuspension const double right_roll_contribution = std::max(roll_diff, 0.0); correction_target_rad_[kLeftFront] = -(front_pitch_contribution + left_roll_contribution); - correction_target_rad_[kLeftBack] = -(back_pitch_contribution + left_roll_contribution); + correction_target_rad_[kLeftBack] = + -(back_pitch_contribution + left_roll_contribution); correction_target_rad_[kRightBack] = -(back_pitch_contribution + right_roll_contribution); correction_target_rad_[kRightFront] = @@ -379,8 +380,7 @@ class DeformableSuspension correction_target_rad_[kLeftFront] = front_pitch_contribution + left_roll_contribution; correction_target_rad_[kLeftBack] = back_pitch_contribution + left_roll_contribution; correction_target_rad_[kRightBack] = back_pitch_contribution + right_roll_contribution; - correction_target_rad_[kRightFront] = - front_pitch_contribution + right_roll_contribution; + correction_target_rad_[kRightFront] = front_pitch_contribution + right_roll_contribution; } } @@ -392,10 +392,10 @@ class DeformableSuspension const double min_susp_rad = deg_to_rad_(min_angle_deg - 5.0); for (size_t i = 0; i < kJointCount; ++i) { - const double base_angle = - std::isfinite(base_joint_angles[i]) - ? base_joint_angles[i] - : (low_prone_override_active ? min_susp_rad : deg_to_rad_(base_angle_deg)); + const double base_angle = std::isfinite(base_joint_angles[i]) + ? base_joint_angles[i] + : (low_prone_override_active ? min_susp_rad + : deg_to_rad_(base_angle_deg)); const double correction_min = min_susp_rad - base_angle; const double correction_max = max_target_rad - base_angle; @@ -419,8 +419,7 @@ class DeformableSuspension std::clamp(velocity_error / dt, -correction_acc_limit, correction_acc_limit); velocity_state += acceleration_state * dt; - velocity_state = - std::clamp(velocity_state, -correction_vel_limit, correction_vel_limit); + velocity_state = std::clamp(velocity_state, -correction_vel_limit, correction_vel_limit); angle_state += velocity_state * dt; const double next_error = target - angle_state; @@ -553,7 +552,9 @@ class DeformableSuspension } } - void publish_nan_joint_targets_() { reset_all_controls_(); } + void publish_nan_joint_targets_() { + reset_all_controls_(); + } InputInterface update_rate_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp index 9396df1d1..bf4588b1e 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp @@ -5,7 +5,7 @@ #include #include -#include +#include #include #include #include @@ -15,6 +15,8 @@ namespace rmcs_core::controller::gimbal { +using namespace rmcs_description; + class DeformableInfantryGimbalController : public rmcs_executor::Component , public rclcpp::Node { @@ -23,16 +25,16 @@ class DeformableInfantryGimbalController : Node( get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { - configure_pid("yaw_angle", yaw_angle_pid_); configure_pid("yaw_velocity", yaw_velocity_pid_); configure_pid("pitch_angle", pitch_angle_pid_); configure_pid("pitch_velocity", pitch_velocity_pid_); - get_parameter("pitch_torque_control", pitch_torque_control_enabled_); get_parameter("manual_joystick_sensitivity", joystick_sensitivity_); get_parameter("manual_mouse_sensitivity", mouse_sensitivity_); - + get_parameter("yaw_vel_ff_gain", yaw_vel_ff_gain_); + get_parameter("yaw_acc_ff_gain", yaw_acc_ff_gain_); + get_parameter("pitch_acc_ff_gain", pitch_acc_ff_gain_); get_parameter_or("pitch_gravity_ff_gain", pitch_gravity_ff_gain_, 0.0); get_parameter_or("pitch_gravity_ff_phase", pitch_gravity_ff_phase_, 0.0); get_parameter_or("ctrl_hold_pitch_target_angle", ctrl_hold_pitch_target_angle_, 0.0); @@ -40,8 +42,8 @@ class DeformableInfantryGimbalController auto update() -> void override { const auto switch_right = *input_.switch_right; - const auto switch_left = *input_.switch_left; - const auto keyboard = *input_.keyboard; + const auto switch_left = *input_.switch_left; + const auto keyboard = *input_.keyboard; using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) @@ -76,10 +78,24 @@ class DeformableInfantryGimbalController *output_.yaw_control_torque = kNaN; } + const auto feedforward_enabled = auto_aim_active + && input_.auto_aim_feedforward_valid.ready() + && *input_.auto_aim_feedforward_valid; + if (std::isfinite(angle_error.yaw_angle_error)) { - const auto yaw_velocity_ref = yaw_angle_pid_.update(angle_error.yaw_angle_error); + const auto yaw_velocity_ff = feedforward_enabled && input_.auto_aim_yaw_rate.ready() + && std::isfinite(*input_.auto_aim_yaw_rate) + ? yaw_vel_ff_gain_ * *input_.auto_aim_yaw_rate + : 0.0; + const auto yaw_acc_ff = feedforward_enabled && input_.auto_aim_yaw_acc.ready() + && std::isfinite(*input_.auto_aim_yaw_acc) + ? yaw_acc_ff_gain_ * *input_.auto_aim_yaw_acc + : 0.0; + + const auto yaw_velocity_ref = + yaw_angle_pid_.update(angle_error.yaw_angle_error) + yaw_velocity_ff; *output_.yaw_control_torque = - yaw_velocity_pid_.update(yaw_velocity_ref - *input_.yaw_velocity_imu); + yaw_velocity_pid_.update(yaw_velocity_ref - *input_.yaw_velocity_imu) + yaw_acc_ff; } if (!ctrl_hold_active_) { @@ -87,8 +103,12 @@ class DeformableInfantryGimbalController pitch_angle_pid_.reset(); pitch_velocity_pid_.reset(); *output_.pitch_control_velocity = kNaN; - *output_.pitch_control_torque = kNaN; + *output_.pitch_control_torque = kNaN; } else { + const auto pitch_acc_ff = feedforward_enabled && input_.auto_aim_pitch_acc.ready() + && std::isfinite(*input_.auto_aim_pitch_acc) + ? pitch_acc_ff_gain_ * *input_.auto_aim_pitch_acc + : 0.0; const auto pitch_gravity_ff = pitch_gravity_feedforward(); const auto pitch_velocity_ref = pitch_angle_pid_.update(angle_error.pitch_angle_error); @@ -97,18 +117,19 @@ class DeformableInfantryGimbalController *output_.pitch_control_velocity = kNaN; *output_.pitch_control_torque = pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity_imu) + + pitch_acc_ff + pitch_gravity_ff; } else { pitch_velocity_pid_.reset(); *output_.pitch_control_velocity = pitch_velocity_ref; - *output_.pitch_control_torque = kNaN; + *output_.pitch_control_torque = kNaN; } } } } private: - static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); + static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); static constexpr auto kDefaultDt = 1e-3; auto configure_pid(const std::string& prefix, pid::PidCalculator& calculator) -> void { @@ -140,6 +161,11 @@ class DeformableInfantryGimbalController component.register_input("/auto_aim/should_control", auto_aim_should_control, false); component.register_input( "/auto_aim/control_direction", auto_aim_control_direction, false); + component.register_input( + "/auto_aim/feedforward_valid", auto_aim_feedforward_valid, false); + component.register_input("/auto_aim/yaw_rate", auto_aim_yaw_rate, false); + component.register_input("/auto_aim/yaw_acc", auto_aim_yaw_acc, false); + component.register_input("/auto_aim/pitch_acc", auto_aim_pitch_acc, false); } InputInterface joystick_left; @@ -159,6 +185,10 @@ class DeformableInfantryGimbalController InputInterface auto_aim_should_control; InputInterface auto_aim_control_direction; + InputInterface auto_aim_feedforward_valid; + InputInterface auto_aim_yaw_rate; + InputInterface auto_aim_yaw_acc; + InputInterface auto_aim_pitch_acc; } input_{*this}; struct Output { @@ -182,7 +212,9 @@ class DeformableInfantryGimbalController OutputInterface pitch_angle_error; } output_{*this}; - auto ctrl_hold_requested() const -> bool { return pitch_lock_active_; } + auto ctrl_hold_requested() const -> bool { + return pitch_lock_active_; + } auto update_dt() const -> double { if (input_.update_rate.ready() && std::isfinite(*input_.update_rate) @@ -257,10 +289,10 @@ class DeformableInfantryGimbalController if (!ctrl_hold_active_) activate_ctrl_hold(); - *output_.yaw_control_angle = kNaN; + *output_.yaw_control_angle = kNaN; *output_.pitch_control_velocity = kNaN; - *output_.pitch_control_torque = kNaN; - *output_.pitch_control_angle = kNaN; + *output_.pitch_control_torque = kNaN; + *output_.pitch_control_angle = kNaN; if (input_.pitch_angle.ready() && std::isfinite(*input_.pitch_angle)) { auto pitch_target_error = ctrl_hold_pitch_target_angle_ - *input_.pitch_angle; @@ -269,17 +301,17 @@ class DeformableInfantryGimbalController else if (pitch_target_error < -std::numbers::pi) pitch_target_error += 2 * std::numbers::pi; - *output_.pitch_angle_error = pitch_target_error; + *output_.pitch_angle_error = pitch_target_error; const auto pitch_velocity_ref = pitch_angle_pid_.update(pitch_target_error); if (pitch_torque_control_enabled_) { *output_.pitch_control_velocity = kNaN; *output_.pitch_control_torque = pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity_imu) - + pitch_gravity_feedforward(); + + pitch_gravity_feedforward(); } else { pitch_velocity_pid_.reset(); *output_.pitch_control_velocity = pitch_velocity_ref; - *output_.pitch_control_torque = kNaN; + *output_.pitch_control_torque = kNaN; } } } @@ -289,11 +321,11 @@ class DeformableInfantryGimbalController yaw_velocity_pid_.reset(); pitch_angle_pid_.reset(); pitch_velocity_pid_.reset(); - *output_.yaw_control_torque = kNaN; - *output_.yaw_control_angle = kNaN; + *output_.yaw_control_torque = kNaN; + *output_.yaw_control_angle = kNaN; *output_.pitch_control_velocity = kNaN; - *output_.pitch_control_torque = kNaN; - *output_.pitch_control_angle = kNaN; + *output_.pitch_control_torque = kNaN; + *output_.pitch_control_angle = kNaN; } auto reset_all_controls() -> void { @@ -302,7 +334,7 @@ class DeformableInfantryGimbalController suspension_on_by_switch_ = false; last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; gimbal_solver_.update(TwoAxisGimbalSolver::SetDisabled{}); - *output_.yaw_angle_error = kNaN; + *output_.yaw_angle_error = kNaN; *output_.pitch_angle_error = kNaN; reset_control_outputs(); } @@ -331,9 +363,12 @@ class DeformableInfantryGimbalController get_parameter("pitch_velocity_kd").as_double(), }; - double joystick_sensitivity_ = 0.003; - double mouse_sensitivity_ = 0.5; - bool pitch_torque_control_enabled_ = false; + double joystick_sensitivity_ = 0.003; + double mouse_sensitivity_ = 0.5; + bool pitch_torque_control_enabled_ = false; + double yaw_vel_ff_gain_ = 0.0; + double yaw_acc_ff_gain_ = 0.0; + double pitch_acc_ff_gain_ = 0.0; double ctrl_hold_pitch_target_angle_ = 0.0; double pitch_gravity_ff_gain_ = 0.0; double pitch_gravity_ff_phase_ = 0.0; @@ -346,5 +381,6 @@ class DeformableInfantryGimbalController } // namespace rmcs_core::controller::gimbal #include + PLUGINLIB_EXPORT_CLASS( rmcs_core::controller::gimbal::DeformableInfantryGimbalController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp index 76beb6868..f0b277f1c 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp @@ -97,6 +97,7 @@ class FrictionWheelController last_switch_left_ = switch_left; last_keyboard_ = keyboard; } + } private: diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index 8efa589ba..9c58749ef 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -1,5 +1,4 @@ #include -#include #include #include #include @@ -7,34 +6,30 @@ #include #include #include -#include #include #include #include #include +#include +#include #include -#include -#include #include #include -#include -#include #include #include +#include +#include + +#include #include "hardware/device/bmi088.hpp" -#include "hardware/device/bmi088_ekf.hpp" -#include "hardware/device/board_clock_lifter.hpp" #include "hardware/device/can_packet.hpp" #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" -#include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" -#include "hardware/device/vt13.hpp" -#include "hardware/util/status_monitor.hpp" namespace rmcs_core::hardware { @@ -48,7 +43,9 @@ class DeformableInfantryOmniB : Node( get_component_name(), rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) - , command_(create_partner_component(get_component_name() + "_command", *this)) { + , deformable_infantry_command_( + create_partner_component( + get_component_name() + "_command", *this)) { using namespace rmcs_description; register_input("/predefined/timestamp", timestamp_); @@ -60,13 +57,6 @@ class DeformableInfantryOmniB tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); - remote_control_ = std::make_unique(*this); - - bottom_board_ = std::make_unique( - *this, *command_, get_parameter("serial_filter_bottom_board").as_string()); - top_board_ = std::make_unique( - *this, *command_, get_parameter("serial_filter_top_board").as_string()); - // For command: remote-status using Srv = std_srvs::srv::Trigger; status_service_ = create_service( @@ -74,16 +64,32 @@ class DeformableInfantryOmniB [this](const Srv::Request::SharedPtr&, const Srv::Response::SharedPtr& response) { status_service_callback(response); }); + + std::string serial_filter_imu; + get_parameter_or("serial_filter_imu", serial_filter_imu, std::string{}); + + rmcs_board_lite = std::make_unique( + *this, *deformable_infantry_command_, + get_parameter("serial_filter_rmcs_board").as_string()); + top_board_ = std::make_unique( + *this, *deformable_infantry_command_, + get_parameter("serial_filter_top_board").as_string(), !serial_filter_imu.empty()); + if (!serial_filter_imu.empty()) + imu_board_ = std::make_unique(*this, serial_filter_imu); } ~DeformableInfantryOmniB() override = default; - void before_updating() override { top_board_->request_hard_sync_read(); } + void before_updating() override { + top_board_->request_hard_sync_read(); + next_hard_sync_log_time_ = Clock::now() + std::chrono::seconds(1); + } void update() override { - bottom_board_->update(); + rmcs_board_lite->update(); top_board_->update(); - remote_control_->update(); + if (imu_board_) + imu_board_->update(); using namespace rmcs_description; *camera_transform_ = fast_tf::lookup_transform(*tf_); @@ -94,26 +100,19 @@ class DeformableInfantryOmniB void command_update() { const bool even = ((cmd_tick_++ & 1u) == 0u); - bottom_board_->command_update(even); + rmcs_board_lite->command_update(even); top_board_->command_update(); } private: - static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); - static constexpr auto kLeftFront = 0; - static constexpr auto kLeftBack = 1; - static constexpr auto kRightBack = 2; - static constexpr auto kRightFront = 3; - static constexpr auto kJointName = std::array{ - "left_front", - "left_back", - "right_back", - "right_front", - }; + class DeformableInfantryOmniBCommand; + class BottomBoard; + class ImuBoard; + class TopBoard; - class Command : public Component { + class DeformableInfantryOmniBCommand : public rmcs_executor::Component { public: - explicit Command(DeformableInfantryOmniB& deformableInfantry) + explicit DeformableInfantryOmniBCommand(DeformableInfantryOmniB& deformableInfantry) : deformableInfantry(deformableInfantry) {} void update() override { deformableInfantry.command_update(); } @@ -121,229 +120,47 @@ class DeformableInfantryOmniB DeformableInfantryOmniB& deformableInfantry; }; - struct TopBoard final : public librmcs::board::RmcsBoardLite::Callback { + class BottomBoard final : private librmcs::agent::RmcsBoardLite { public: - explicit TopBoard( - DeformableInfantryOmniB& status, Component& command, - const std::string& serial_filter = {}) - : status_{status} - , tf_{status.tf_} - , bmi088_{device::Bmi088Ekf::Config{ - .body_to_sensor = - Eigen::AngleAxisd{std::numbers::pi / 2.0, Eigen::Vector3d::UnitX()} - .toRotationMatrix()}} - , gimbal_pitch_motor_(status, command, "/gimbal/pitch") - , gimbal_left_friction_(status, command, "/gimbal/left_friction") - , gimbal_right_friction_(status, command, "/gimbal/right_friction") { - - gimbal_pitch_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} - .set_reversed() - .set_encoder_zero_point( - static_cast(status.get_parameter("pitch_motor_zero_point").as_int()))); - - gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} - .set_reduction_ratio(1.) - .set_reversed()); - gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2}.set_reduction_ratio( - 1.)); - - status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); - status.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); - status.register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); - - auto options = librmcs::board::AdvancedOptions{}; - options.dangerously_skip_version_checks = true; - board_ = std::make_unique(*this, serial_filter, options); - - board_->start_transmit().gpio_digital_read( - Spec::kGpios.kUart1Rx, // - { - .period_ms = 0, - .asap = false, - .rising_edge = false, - .falling_edge = true, - .capture_timestamp = true, - .pull = librmcs::data::GpioPull::kUp, - }); - - board_->start_transmit().uart_config(Spec::kUarts.kUart0, {.baudrate = 921600}); - - status_.remote_control_->register_vt13(&vt13_); - } - ~TopBoard() override = default; - - [[nodiscard]] auto gimbal_yaw_velocity() const -> double { - return *gimbal_yaw_velocity_bmi088_; - } - - void request_hard_sync_read() { - // RMCS-lite top board variant currently has no GPIO hard-sync request - // path. - } - - void update() { - vt13_.update_status(); - gimbal_pitch_motor_.update_status(); - gimbal_left_friction_.update_status(); - gimbal_right_friction_.update_status(); - - const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); - - if (auto snapshot = bmi088_.snapshot()) { - *gimbal_pitch_velocity_bmi088_ = snapshot->gyro_body.y(); - *gimbal_yaw_velocity_bmi088_ = snapshot->gyro_body.z(); - tf_->set_transform( - snapshot->orientation.conjugate()); - } - - tf_->set_state( - pitch_encoder_angle); - } - - void command_update() const { - auto builder = board_->start_transmit(); - { - auto packet = gimbal_pitch_motor_.generate_torque_command(); - builder.can_transmit( - Spec::kCans.kCan0, // - { - .can_id = 0x141, - .can_data = packet.as_bytes(), - }); - } - { - auto packet = device::CanPacket8{uint64_t{0}}; - packet << gimbal_right_friction_; - builder.can_transmit( - Spec::kCans.kCan1, // - { - .can_id = gimbal_right_friction_.send_id(), - .can_data = packet.as_bytes(), - }); - } - { - auto packet = device::CanPacket8{uint64_t{0}}; - packet << gimbal_left_friction_; - builder.can_transmit( - Spec::kCans.kCan2, // - { - .can_id = gimbal_left_friction_.send_id(), - .can_data = packet.as_bytes(), - }); - } - } - - void can_receive_callback(const Spec::Can& can, const View::Can& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - if (can == Spec::kCans.kCan0) { - if (data.can_id == 0x141) - gimbal_pitch_motor_.store_status(data.can_data); - monitor_.tick("Top::Can0", data.can_id); - } else if (can == Spec::kCans.kCan1) { - gimbal_right_friction_.match_then_store_status(data.can_id, data.can_data); - monitor_.tick("Top::Can1", data.can_id); - } else if (can == Spec::kCans.kCan2) { - gimbal_left_friction_.match_then_store_status(data.can_id, data.can_data); - monitor_.tick("Top::Can2", data.can_id); - } - } - - void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { - if (uart == Spec::kUarts.kUart0) - vt13_.store_status(data.uart_data); - } - - void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { - const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); - bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); - monitor_.tick("Top::Imu", "Acc"); - } - - void gyroscope_receive_callback(const View::ImuGyroscope& data) override { - const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); - monitor_.tick("Top::Imu", "Gyr"); - if (!timestamp.has_value()) - return; - auto snapshot = - bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); - if (snapshot) - imu_snapshot_output_.emit(*snapshot); - } - - void gpio_digital_read_result_callback( - const Spec::Gpio& gpio, const View::GpioDigital& data) override { - if (gpio != Spec::kGpios.kUart1Rx) - return; - if (!data.timestamp_quarter_us) - return; - - const auto timestamp = board_clock_lifter_.lift_timestamp(*data.timestamp_quarter_us); - if (!timestamp.has_value()) - return; - - camera_signal_output_.emit(*timestamp); - monitor_.tick("Top::CameraSync", "Active"); - } + friend class DeformableInfantryOmniB; - auto status() const -> std::vector { return monitor_.text(); } + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - DeformableInfantryOmniB& status_; - OutputInterface& tf_; - OutputInterface gimbal_yaw_velocity_bmi088_; - OutputInterface gimbal_pitch_velocity_bmi088_; - - EventOutputInterface imu_snapshot_output_; - EventOutputInterface camera_signal_output_; - - device::Bmi088Ekf bmi088_; - device::BoardClockLifter board_clock_lifter_; - device::Vt13 vt13_; - device::LkMotor gimbal_pitch_motor_; - device::DjiMotor gimbal_left_friction_; - device::DjiMotor gimbal_right_friction_; - - StatusMonitor monitor_{}; - std::unique_ptr board_; - }; - - struct BottomBoard final : public librmcs::board::RmcsBoardLite::Callback { - public: explicit BottomBoard( - DeformableInfantryOmniB& status, Component& command, + DeformableInfantryOmniB& deformableInfantry, + DeformableInfantryOmniBCommand& deformableInfantry_command, const std::string& serial_filter = {}) - : status_{status} - , command_{command} - , kChassisRadiusBase(status.get_parameter("chassis_radius").as_double()) - , kRodLength(status.get_parameter("rod_length").as_double()) - , kDefaultRadius(kChassisRadiusBase + kRodLength) { - - status.register_output("/referee/serial", referee_serial_); + : RmcsBoardLite{ + serial_filter, + librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} + , deformable_infantry_{deformableInfantry} + , command_{deformableInfantry_command} + , tf_{deformableInfantry.tf_} { + + deformableInfantry.register_output("/referee/serial", referee_serial_); referee_serial_->read = [this](std::byte* buffer, size_t size) { return referee_ring_buffer_receive_.pop_front_n( [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); }; referee_serial_->write = [this](const std::byte* buffer, size_t size) { - board_->start_transmit().uart_transmit( - Spec::kUarts.kUart0, {.uart_data = std::span{buffer, size}}); + start_transmit().uart0_transmit( + {.uart_data = std::span{buffer, size}}); return size; }; gimbal_yaw_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( - static_cast(status.get_parameter("yaw_motor_zero_point").as_int()))); + static_cast( + deformableInfantry.get_parameter("yaw_motor_zero_point").as_int()))); for (auto& motor : chassis_wheel_motors_) motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} - .set_reversed() + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reduction_ratio(13.0) - .enable_multi_turn_angle()); + .enable_multi_turn_angle() + .set_reversed()); + // V2: LK MG5010 i36 direct-drive joint motors, built-in encoder zero point for (auto& motor : chassis_joint_motors_) motor.configure( device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei36} @@ -357,40 +174,51 @@ class DeformableInfantryOmniB }); gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006, 3} - .enable_multi_turn_angle()); - - status.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - status.register_output("/chassis/imu/pitch", chassis_imu_pitch_, 0.0); - status.register_output("/chassis/imu/roll", chassis_imu_roll_, 0.0); - status.register_output("/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, 0.0); - status.register_output("/chassis/imu/roll_rate", chassis_imu_roll_rate_, 0.0); - for (size_t i = 0; i < 4; ++i) { - status.register_output( - std::format( - "/chassis/{}_joint/physical_angle", DeformableInfantryOmniB::kJointName[i]), - joint_physical_angle_[i], kNaN); - status.register_output( - std::format( - "/chassis/{}_joint/physical_velocity", - DeformableInfantryOmniB::kJointName[i]), - joint_physical_velocity_[i], kNaN); - } - status.register_output("/chassis/encoder/alpha", encoder_alpha_, kNaN); - status.register_output("/chassis/encoder/alpha_dot", encoder_alpha_dot_, kNaN); - status.register_output("/chassis/radius", radius_, kDefaultRadius); - - status.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); - status.get_parameter_or("debug_log_wheel_motor", debug_log_wheel_motor_, false); - status.get_parameter_or( + device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); + + deformableInfantry.register_output( + "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); + deformableInfantry.register_output("/chassis/imu/pitch", chassis_imu_pitch_, 0.0); + deformableInfantry.register_output("/chassis/imu/roll", chassis_imu_roll_, 0.0); + deformableInfantry.register_output( + "/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, 0.0); + deformableInfantry.register_output( + "/chassis/imu/roll_rate", chassis_imu_roll_rate_, 0.0); + deformableInfantry.register_output( + "/chassis/left_front_joint/physical_angle", left_front_joint_physical_angle_, nan_); + deformableInfantry.register_output( + "/chassis/left_back_joint/physical_angle", left_back_joint_physical_angle_, nan_); + deformableInfantry.register_output( + "/chassis/right_back_joint/physical_angle", right_back_joint_physical_angle_, nan_); + deformableInfantry.register_output( + "/chassis/right_front_joint/physical_angle", right_front_joint_physical_angle_, + nan_); + deformableInfantry.register_output( + "/chassis/left_front_joint/physical_velocity", left_front_joint_physical_velocity_, + nan_); + deformableInfantry.register_output( + "/chassis/left_back_joint/physical_velocity", left_back_joint_physical_velocity_, + nan_); + deformableInfantry.register_output( + "/chassis/right_back_joint/physical_velocity", right_back_joint_physical_velocity_, + nan_); + deformableInfantry.register_output( + "/chassis/right_front_joint/physical_velocity", + right_front_joint_physical_velocity_, nan_); + deformableInfantry.register_output("/chassis/encoder/alpha", encoder_alpha_, nan_); + deformableInfantry.register_output( + "/chassis/encoder/alpha_dot", encoder_alpha_dot_, nan_); + deformableInfantry.register_output("/chassis/radius", radius_, default_radius_); + + deformableInfantry.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); + deformableInfantry.get_parameter_or( + "debug_log_wheel_motor", debug_log_wheel_motor_, false); + deformableInfantry.get_parameter_or( "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); + } - auto options = librmcs::board::AdvancedOptions{}; - options.dangerously_skip_version_checks = true; - board_ = std::make_unique(*this, serial_filter, options); + ~BottomBoard() override = default; - status_.remote_control_->register_dr16(&dr16_); - } void update() { imu_.update_status(); *chassis_yaw_velocity_imu_ = imu_.gz(); @@ -420,9 +248,14 @@ class DeformableInfantryOmniB for (auto& motor : chassis_joint_motors_) motor.update_status(); - for (size_t i = 0; i < 4; ++i) - update_joint_physical_feedback_( - i, joint_physical_angle_[i], joint_physical_velocity_[i]); + update_joint_physical_feedback_( + 0, left_front_joint_physical_angle_, left_front_joint_physical_velocity_); + update_joint_physical_feedback_( + 1, left_back_joint_physical_angle_, left_back_joint_physical_velocity_); + update_joint_physical_feedback_( + 2, right_back_joint_physical_angle_, right_back_joint_physical_velocity_); + update_joint_physical_feedback_( + 3, right_front_joint_physical_angle_, right_front_joint_physical_velocity_); update_geometry_feedback_(); if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) @@ -441,241 +274,137 @@ class DeformableInfantryOmniB } void command_update(bool even) { - auto builder = board_->start_transmit(); + auto builder = start_transmit(); if (even) { - builder.can_transmit( - Spec::kCans.kCan0, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[kLeftFront].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan1, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[kLeftBack].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan2, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[kRightBack].generate_command(), - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan3, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[kRightFront].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan2, // - { - .can_id = 0x142, - .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan1, // - { - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - } - .as_bytes(), - }); + builder.can0_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[0].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can2_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[2].generate_command(), + device::CanPacket8::PaddingQuarter{}, + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can3_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[3].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can2_transmit({ + .can_id = 0x142, + .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes(), + }); + builder.can1_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + } + .as_bytes(), + }); } else { - for (size_t i = 0; i < 4; ++i) { - switch (i) { - case kLeftFront: - builder.can_transmit( - Spec::kCans.kCan0, // - { - .can_id = 0x141, - .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), - }); - break; - case kLeftBack: - builder.can_transmit( - Spec::kCans.kCan1, // - { - .can_id = 0x141, - .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), - }); - break; - case kRightBack: - builder.can_transmit( - Spec::kCans.kCan2, // - { - .can_id = 0x141, - .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), - }); - break; - case kRightFront: - builder.can_transmit( - Spec::kCans.kCan3, // - { - .can_id = 0x141, - .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), - }); - break; - default: break; - } - } + builder.can0_transmit({ + .can_id = 0x141, + .can_data = chassis_joint_motors_[0].generate_command().as_bytes(), + }); + builder.can1_transmit({ + .can_id = 0x141, + .can_data = chassis_joint_motors_[1].generate_command().as_bytes(), + }); + builder.can2_transmit({ + .can_id = 0x141, + .can_data = chassis_joint_motors_[2].generate_command().as_bytes(), + }); + builder.can3_transmit({ + .can_id = 0x141, + .can_data = chassis_joint_motors_[3].generate_command().as_bytes(), + }); } } - static constexpr double kJointZeroPhysicalAngleRad = 62.5 * std::numbers::pi / 180.0; - - DeformableInfantryOmniB& status_; - Component& command_; - - std::unique_ptr board_; - - // Interfaces - - OutputInterface& tf_{status_.tf_}; - - OutputInterface chassis_yaw_velocity_imu_; - OutputInterface chassis_imu_pitch_; - OutputInterface chassis_imu_roll_; - OutputInterface chassis_imu_pitch_rate_; - OutputInterface chassis_imu_roll_rate_; + private: + DeformableInfantryOmniB& deformable_infantry_; + rmcs_executor::Component& command_; - std::array, 4> joint_physical_angle_; - std::array, 4> joint_physical_velocity_; + static constexpr double joint_zero_physical_angle_rad_ = 62.5 * std::numbers::pi / 180.0; + static constexpr double chassis_radius_base_ = 0.2341741; + static constexpr double rod_length_ = 0.150; + static constexpr double default_radius_ = 0.5 * rod_length_ + chassis_radius_base_; - OutputInterface encoder_alpha_; - OutputInterface encoder_alpha_dot_; - OutputInterface radius_; - - rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; - OutputInterface referee_serial_; - - // State - - std::atomic wheel_status_received_[4] = {false, false, false, false}; - std::atomic joint_status_received_[4] = {false, false, false, false}; - - bool debug_log_supercap_ = false; - bool debug_log_wheel_motor_ = false; - bool debug_log_deformable_joint_motor_ = false; - - const double kChassisRadiusBase; - const double kRodLength; - const double kDefaultRadius; - - Clock::time_point next_chassis_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - Clock::time_point next_supercap_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - - // Device - - device::Bmi088 imu_{1000, 0.2, 0.0}; - device::LkMotor gimbal_yaw_motor_{status_, command_, "/gimbal/yaw"}; - device::Dr16 dr16_; - - device::DjiMotor chassis_wheel_motors_[4]{ - device::DjiMotor{status_, command_, "/chassis/left_front_wheel"}, - device::DjiMotor{status_, command_, "/chassis/left_back_wheel"}, - device::DjiMotor{status_, command_, "/chassis/right_back_wheel"}, - device::DjiMotor{status_, command_, "/chassis/right_front_wheel"}, - }; - device::LkMotor chassis_joint_motors_[4]{ - device::LkMotor{status_, command_, "/chassis/left_front_joint"}, - device::LkMotor{status_, command_, "/chassis/left_back_joint"}, - device::LkMotor{status_, command_, "/chassis/right_back_joint"}, - device::LkMotor{status_, command_, "/chassis/right_front_joint"}, - }; - - std::atomic latest_supercap_status_{device::CanPacket8{uint64_t{0}}}; - std::atomic supercap_status_received_{false}; - device::Supercap supercap_{status_, command_}; - - device::DjiMotor gimbal_bullet_feeder_{status_, command_, "/gimbal/bullet_feeder"}; - - void process_chassis_can_receive_(size_t index, const View::Can& data) { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[index].store_status(data.can_data); - wheel_status_received_[index].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[index].store_status(data.can_data); - joint_status_received_[index].store(true, std::memory_order_relaxed); - } + static double to_physical_angle_(double motor_angle) { + return joint_zero_physical_angle_rad_ - motor_angle; } + static double to_physical_velocity_(double motor_velocity) { return -motor_velocity; } + void update_joint_physical_feedback_( size_t index, OutputInterface& angle_output, OutputInterface& velocity_output) { - if (!joint_status_received_[index].load(std::memory_order_relaxed)) { - *angle_output = kNaN; - *velocity_output = kNaN; + *angle_output = nan_; + *velocity_output = nan_; return; } - const auto to_physical_angle = [](double motor_angle) { - return kJointZeroPhysicalAngleRad - motor_angle; - }; - const auto to_physical_velocity = [](double motor_velocity) { return -motor_velocity; }; - - *angle_output = to_physical_angle(chassis_joint_motors_[index].angle()); - *velocity_output = to_physical_velocity(chassis_joint_motors_[index].velocity()); + *angle_output = to_physical_angle_(chassis_joint_motors_[index].angle()); + *velocity_output = to_physical_velocity_(chassis_joint_motors_[index].velocity()); } void update_geometry_feedback_() { const Eigen::Vector4d alpha_rad{ - *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], - *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront]}; + *left_front_joint_physical_angle_, *left_back_joint_physical_angle_, + *right_back_joint_physical_angle_, *right_front_joint_physical_angle_}; const Eigen::Vector4d alpha_dot_rad{ - *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], - *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront]}; + *left_front_joint_physical_velocity_, *left_back_joint_physical_velocity_, + *right_back_joint_physical_velocity_, *right_front_joint_physical_velocity_}; if (!alpha_rad.array().isFinite().all() || !alpha_dot_rad.array().isFinite().all()) { - *encoder_alpha_ = kNaN; - *encoder_alpha_dot_ = kNaN; - *radius_ = kDefaultRadius; + *encoder_alpha_ = nan_; + *encoder_alpha_dot_ = nan_; + *radius_ = default_radius_; RCLCPP_WARN_THROTTLE( - status_.get_logger(), *status_.get_clock(), 1000, + deformable_infantry_.get_logger(), *deformable_infantry_.get_clock(), 1000, "deformable joint feedback invalid, fallback chassis radius to default %.3f m", - kDefaultRadius); + default_radius_); return; } *encoder_alpha_ = alpha_rad.mean(); *encoder_alpha_dot_ = alpha_dot_rad.mean(); - *radius_ = (kChassisRadiusBase + kRodLength * alpha_rad.array().cos()).mean(); + *radius_ = (chassis_radius_base_ + rod_length_ * alpha_rad.array().cos()).mean(); } void log_chassis_feedback_once_per_second_() { @@ -691,44 +420,29 @@ class DeformableInfantryOmniB }; if (debug_log_wheel_motor_) { - std::string wheel_rx_str; - for (size_t i = 0; i < 4; ++i) { - if (i > 0) - wheel_rx_str.push_back(' '); - wheel_rx_str.push_back(wheel_rx(i)); - } RCLCPP_INFO( - status_.get_logger(), + deformable_infantry_.get_logger(), "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " - "rx=[%s]", - chassis_wheel_motors_[kLeftFront].angle(), - chassis_wheel_motors_[kLeftBack].angle(), - chassis_wheel_motors_[kRightBack].angle(), - chassis_wheel_motors_[kRightFront].angle(), - chassis_wheel_motors_[kLeftFront].angle(), - chassis_wheel_motors_[kLeftBack].angle(), - chassis_wheel_motors_[kRightBack].angle(), - chassis_wheel_motors_[kRightFront].angle(), wheel_rx_str.c_str()); + "rx=[%c %c %c %c]", + chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), + chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), + chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), + chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), wheel_rx(0), + wheel_rx(1), wheel_rx(2), wheel_rx(3)); } if (debug_log_deformable_joint_motor_) { - std::string joint_rx_str; - for (size_t i = 0; i < 4; ++i) { - if (i > 0) - joint_rx_str.push_back(' '); - joint_rx_str.push_back(joint_rx(i)); - } RCLCPP_INFO( - status_.get_logger(), + deformable_infantry_.get_logger(), "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "rx=[%s]", - *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], - *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront], - *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], - *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront], - joint_rx_str.c_str()); + "rx=[%c %c %c %c]", + *left_front_joint_physical_angle_, *left_back_joint_physical_angle_, + *right_back_joint_physical_angle_, *right_front_joint_physical_angle_, + *left_front_joint_physical_velocity_, *left_back_joint_physical_velocity_, + *right_back_joint_physical_velocity_, *right_front_joint_physical_velocity_, + joint_rx(0), joint_rx(1), joint_rx(2), joint_rx(3)); } next_chassis_feedback_log_time_ = now + std::chrono::seconds(1); @@ -744,13 +458,13 @@ class DeformableInfantryOmniB const auto supercap_raw_bytes = supercap_raw_packet.as_bytes(); RCLCPP_INFO( - status_.get_logger(), + deformable_infantry_.get_logger(), "[supercap] can1 rx=%c id=0x300 enabled=%d supercap_v=% .3f chassis_v=% .3f " "power=% .3f raw=[%02X %02X %02X %02X %02X %02X %02X %02X]", supercap_rx ? 'Y' : 'N', supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, - supercap_rx ? supercap_.supercap_voltage() : kNaN, - supercap_rx ? supercap_.chassis_voltage() : kNaN, - supercap_rx ? supercap_.chassis_power() : kNaN, + supercap_rx ? supercap_.supercap_voltage() : nan_, + supercap_rx ? supercap_.chassis_voltage() : nan_, + supercap_rx ? supercap_.chassis_power() : nan_, std::to_integer(supercap_raw_bytes[0]), std::to_integer(supercap_raw_bytes[1]), std::to_integer(supercap_raw_bytes[2]), @@ -763,64 +477,344 @@ class DeformableInfantryOmniB next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); } - void can_receive_callback(const Spec::Can& can, const View::Can& data) override { + void dbus_receive_callback(const librmcs::data::UartDataView& data) override { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + } + + void can0_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) + return; + if (data.can_id == 0x201) { + chassis_wheel_motors_[0].store_status(data.can_data); + wheel_status_received_[0].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x141) { + chassis_joint_motors_[0].store_status(data.can_data); + joint_status_received_[0].store(true, std::memory_order_relaxed); + } + } + + void can1_receive_callback(const librmcs::data::CanDataView& data) override { if (data.is_extended_can_id || data.is_remote_transmission) return; - if (can == Spec::kCans.kCan0) { - process_chassis_can_receive_(0, data); - monitor_.tick("Bottom::Can0", data.can_id); - } else if (can == Spec::kCans.kCan1) { - process_chassis_can_receive_(1, data); - if (!data.is_extended_can_id && !data.is_remote_transmission - && data.can_id == 0x300) { - if (data.can_data.size() == 8) - latest_supercap_status_.store( - device::CanPacket8{data.can_data}, std::memory_order_relaxed); - supercap_.store_status(data.can_data); - supercap_status_received_.store(true, std::memory_order_relaxed); - } - monitor_.tick("Bottom::Can1", data.can_id); - } else if (can == Spec::kCans.kCan2) { - process_chassis_can_receive_(2, data); - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x142) - gimbal_yaw_motor_.store_status(data.can_data); - else if (data.can_id == 0x203) - gimbal_bullet_feeder_.store_status(data.can_data); - monitor_.tick("Bottom::Can2", data.can_id); - } else if (can == Spec::kCans.kCan3) { - process_chassis_can_receive_(3, data); - monitor_.tick("Bottom::Can3", data.can_id); + if (data.can_id == 0x201) { + chassis_wheel_motors_[1].store_status(data.can_data); + wheel_status_received_[1].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x141) { + chassis_joint_motors_[1].store_status(data.can_data); + joint_status_received_[1].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x300) { + if (data.can_data.size() == 8) + latest_supercap_status_.store( + device::CanPacket8{data.can_data}, std::memory_order_relaxed); + supercap_.store_status(data.can_data); + supercap_status_received_.store(true, std::memory_order_relaxed); } } - void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { - if (uart == Spec::kUarts.kDbus) { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); - monitor_.tick("Bottom::Dbus", "Active"); - } else if (uart == Spec::kUarts.kUart0) { - const std::byte* ptr = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, - data.uart_data.size()); - monitor_.tick("Bottom::Uart0", "Active"); + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) + return; + if (data.can_id == 0x201) { + chassis_wheel_motors_[2].store_status(data.can_data); + wheel_status_received_[2].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x141) { + chassis_joint_motors_[2].store_status(data.can_data); + joint_status_received_[2].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x142) { + gimbal_yaw_motor_.store_status(data.can_data); + } else if (data.can_id == 0x203) { + gimbal_bullet_feeder_.store_status(data.can_data); } } - void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { + void can3_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) + return; + if (data.can_id == 0x201) { + chassis_wheel_motors_[3].store_status(data.can_data); + wheel_status_received_[3].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x141) { + chassis_joint_motors_[3].store_status(data.can_data); + joint_status_received_[3].store(true, std::memory_order_relaxed); + } + } + + void uart0_receive_callback(const librmcs::data::UartDataView& data) override { + const std::byte* ptr = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, data.uart_data.size()); + } + + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { imu_.store_accelerometer_status(data.x, data.y, data.z); - monitor_.tick("Bottom::Imu", "Acc"); } - void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { imu_.store_gyroscope_status(data.x, data.y, data.z); - monitor_.tick("Bottom::Imu", "Gyr"); } - auto status() const -> std::vector { return monitor_.text(); } + OutputInterface& tf_; + + device::Bmi088 imu_{1000, 0.2, 0.0}; + device::LkMotor gimbal_yaw_motor_{deformable_infantry_, command_, "/gimbal/yaw"}; + device::Dr16 dr16_{deformable_infantry_}; + + device::DjiMotor chassis_wheel_motors_[4]{ + device::DjiMotor{deformable_infantry_, command_, "/chassis/left_front_wheel"}, + device::DjiMotor{deformable_infantry_, command_, "/chassis/left_back_wheel"}, + device::DjiMotor{deformable_infantry_, command_, "/chassis/right_back_wheel"}, + device::DjiMotor{deformable_infantry_, command_, "/chassis/right_front_wheel"}, + }; + device::LkMotor chassis_joint_motors_[4]{ + device::LkMotor{deformable_infantry_, command_, "/chassis/left_front_joint"}, + device::LkMotor{deformable_infantry_, command_, "/chassis/left_back_joint"}, + device::LkMotor{deformable_infantry_, command_, "/chassis/right_back_joint"}, + device::LkMotor{deformable_infantry_, command_, "/chassis/right_front_joint"}, + }; + + std::atomic wheel_status_received_[4] = {false, false, false, false}; + std::atomic joint_status_received_[4] = {false, false, false, false}; + bool debug_log_supercap_ = false; + bool debug_log_wheel_motor_ = false; + bool debug_log_deformable_joint_motor_ = false; + Clock::time_point next_chassis_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; + Clock::time_point next_supercap_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; + device::Supercap supercap_{deformable_infantry_, command_}; + std::atomic latest_supercap_status_{device::CanPacket8{uint64_t{0}}}; + std::atomic supercap_status_received_{false}; + device::DjiMotor gimbal_bullet_feeder_{ + deformable_infantry_, command_, "/gimbal/bullet_feeder"}; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; + OutputInterface referee_serial_; + + OutputInterface chassis_yaw_velocity_imu_; + OutputInterface chassis_imu_pitch_; + OutputInterface chassis_imu_roll_; + OutputInterface chassis_imu_pitch_rate_; + OutputInterface chassis_imu_roll_rate_; + OutputInterface left_front_joint_physical_angle_; + OutputInterface left_back_joint_physical_angle_; + OutputInterface right_back_joint_physical_angle_; + OutputInterface right_front_joint_physical_angle_; + OutputInterface left_front_joint_physical_velocity_; + OutputInterface left_back_joint_physical_velocity_; + OutputInterface right_back_joint_physical_velocity_; + OutputInterface right_front_joint_physical_velocity_; + OutputInterface encoder_alpha_; + OutputInterface encoder_alpha_dot_; + OutputInterface radius_; + }; + + class ImuBoard final : private librmcs::agent::RmcsBoardLite { + friend class DeformableInfantryOmniB; + + public: + explicit ImuBoard( + DeformableInfantryOmniB& deformableInfantry, const std::string& serial_filter = {}) + : RmcsBoardLite{ + serial_filter, + librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} + , tf_{deformableInfantry.tf_} + , bmi088_{1000, 0.2, 0.0} { + + deformableInfantry.register_output( + "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + + bmi088_.set_coordinate_mapping( + [](double x, double y, double z) { return std::make_tuple(x, z, -y); }); + } + + ~ImuBoard() override = default; + + void update() { + bmi088_.update_status(); + Eigen::Quaterniond const gimbal_imu_pose{ + bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + + tf_->set_transform( + gimbal_imu_pose.conjugate()); + + *gimbal_pitch_velocity_imu_ = bmi088_.gy(); + } + + private: + void uart0_receive_callback(const librmcs::data::UartDataView& data) override { + (void)data; + // VT13 is not used in this configuration; Dr16 publishes /remote/... directly. + } + + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + bmi088_.store_accelerometer_status(data.x, data.y, data.z); + } + + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + bmi088_.store_gyroscope_status(data.x, data.y, data.z); + } + + OutputInterface& tf_; + OutputInterface gimbal_pitch_velocity_imu_; + + device::Bmi088 bmi088_; + }; + + class TopBoard final : private librmcs::agent::RmcsBoardLite { + public: + friend class DeformableInfantryOmniB; + + explicit TopBoard( + DeformableInfantryOmniB& deformableInfantry, + DeformableInfantryOmniBCommand& deformableInfantry_command, + const std::string& serial_filter = {}, bool has_external_imu_board = false) + : librmcs::agent::RmcsBoardLite( + serial_filter, + librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) + , has_external_imu_board_(has_external_imu_board) + , tf_(deformableInfantry.tf_) + , bmi088_(1000, 0.2, 0.0) + , gimbal_pitch_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/pitch") + , gimbal_left_friction_( + deformableInfantry, deformableInfantry_command, "/gimbal/left_friction") + , gimbal_right_friction_( + deformableInfantry, deformableInfantry_command, "/gimbal/right_friction") + , scope_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/scope") { + + gimbal_pitch_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} + .set_reversed() + .set_encoder_zero_point( + static_cast( + deformableInfantry.get_parameter("pitch_motor_zero_point").as_int()))); + + gimbal_left_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reduction_ratio(1.) + .set_reversed()); + gimbal_right_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + + scope_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); + + deformableInfantry.register_output( + "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); + if (!has_external_imu_board_) + deformableInfantry.register_output( + "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_encoder_); + + bmi088_.set_coordinate_mapping([](double x, double y, double z) { + // Top board BMI088 maps to gimbal frame as (-x, -y, z). + return std::make_tuple(-x, -y, z); + }); + } + + ~TopBoard() override = default; + + [[nodiscard]] auto gimbal_yaw_velocity() const -> double { + return *gimbal_yaw_velocity_bmi088_; + } + + void request_hard_sync_read() { + // RMCS-lite top board variant currently has no GPIO hard-sync request + // path. + } - StatusMonitor monitor_{}; + void update() { + bmi088_.update_status(); + + gimbal_pitch_motor_.update_status(); + gimbal_left_friction_.update_status(); + gimbal_right_friction_.update_status(); + scope_motor_.update_status(); + + const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); + + *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); + if (!has_external_imu_board_) { + Eigen::Quaterniond const odom_imu_to_yaw_link{ + bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + Eigen::Quaterniond const yaw_link_to_odom_imu = odom_imu_to_yaw_link.conjugate(); + Eigen::Quaterniond pitch_link_to_odom_imu = + Eigen::Quaterniond{ + Eigen::AngleAxisd{-pitch_encoder_angle, Eigen::Vector3d::UnitY()}} + * yaw_link_to_odom_imu; + pitch_link_to_odom_imu.normalize(); + + *gimbal_pitch_velocity_encoder_ = gimbal_pitch_motor_.velocity(); + // The BMI088 is mounted on the yaw link. fast_tf stores PitchLink -> + // OdomImu, so use the encoder pitch from the TF tree to move the + // yaw-link pose back into PitchLink. + tf_->set_transform( + pitch_link_to_odom_imu); + } + + tf_->set_state( + pitch_encoder_angle); + } + + void command_update() { + auto builder = start_transmit(); + builder.can0_transmit({ + .can_id = 0x141, + .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_left_friction_.generate_command(), + gimbal_right_friction_.generate_command(), + scope_motor_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + } + + private: + void uart1_receive_callback(const librmcs::data::UartDataView&) override {} + + void can0_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + if (data.can_id == 0x141) + gimbal_pitch_motor_.store_status(data.can_data); + } + + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + if (data.can_id == 0x201) + gimbal_left_friction_.store_status(data.can_data); + else if (data.can_id == 0x202) + gimbal_right_friction_.store_status(data.can_data); + else if (data.can_id == 0x203) + scope_motor_.store_status(data.can_data); + } + + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + bmi088_.store_accelerometer_status(data.x, data.y, data.z); + } + + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + bmi088_.store_gyroscope_status(data.x, data.y, data.z); + } + + bool has_external_imu_board_ = false; + OutputInterface& tf_; + + OutputInterface gimbal_yaw_velocity_bmi088_; + OutputInterface gimbal_pitch_velocity_encoder_; + + device::Bmi088 bmi088_; + device::LkMotor gimbal_pitch_motor_; + device::DjiMotor gimbal_left_friction_; + device::DjiMotor gimbal_right_friction_; + device::DjiMotor scope_motor_; }; auto status_service_callback(const std::shared_ptr& response) @@ -832,24 +826,15 @@ class DeformableInfantryOmniB std::println(feedback_message, format, std::forward(args)...); }; - text(" yaw_motor_zero_point: {}", bottom_board_->gimbal_yaw_motor_.last_raw_angle()); - text(" pitch_motor_zero_point: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); - constexpr auto kPosition = - std::array{"left_front", "left_back", "right_back", "right_front"}; - - text(""); - for (auto&& [index, motor] : - std::views::zip(kPosition, bottom_board_->chassis_joint_motors_)) { - text(" {}_zero_point: {}", index, motor.last_raw_angle()); - } + text("Gimbal Status"); + text("- Yaw: {}", rmcs_board_lite->gimbal_yaw_motor_.last_raw_angle()); + text("- Pitch: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); - text("\nBottomBoard Status:"); - for (const auto& line : bottom_board_->status()) - text("> {}", line); - - text("\nTopBoard Status:"); - for (const auto& line : top_board_->status()) - text("> {}", line); + text("Chassis Status"); + text("- left front: {}", rmcs_board_lite->chassis_joint_motors_[0].last_raw_angle()); + text("- left back: {}", rmcs_board_lite->chassis_joint_motors_[1].last_raw_angle()); + text("- right back: {}", rmcs_board_lite->chassis_joint_motors_[2].last_raw_angle()); + text("- right front: {}", rmcs_board_lite->chassis_joint_motors_[3].last_raw_angle()); response->message = feedback_message.str(); } @@ -859,15 +844,16 @@ class DeformableInfantryOmniB OutputInterface barrel_direction_; OutputInterface auto_aim_yaw_velocity_; InputInterface timestamp_; + std::atomic hard_sync_pending_{false}; + Clock::time_point next_hard_sync_log_time_{}; - std::unique_ptr bottom_board_; + std::shared_ptr deformable_infantry_command_; + std::unique_ptr rmcs_board_lite; + std::unique_ptr imu_board_; std::unique_ptr top_board_; - std::unique_ptr remote_control_; - - std::shared_ptr command_; - uint32_t cmd_tick_ = 0; std::shared_ptr> status_service_; + uint32_t cmd_tick_ = 0; }; } // namespace rmcs_core::hardware diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index 83c40c97b..d3b5bb464 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -17,24 +17,18 @@ #include #include -#include +#include #include #include -#include -#include #include #include #include "hardware/device/bmi088.hpp" -#include "hardware/device/bmi088_ekf.hpp" -#include "hardware/device/board_clock_lifter.hpp" #include "hardware/device/can_packet.hpp" #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" -#include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" -#include "hardware/util/status_monitor.hpp" namespace rmcs_core::hardware { @@ -60,12 +54,15 @@ class DeformableInfantryOmni tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); - remote_control_ = std::make_unique(*this); - bottom_board_ = std::make_unique( - *this, *command_, get_parameter("serial_filter_bottom_board").as_string()); + *this, *command_, get_parameter("serial_filter_rmcs_board").as_string()); + std::string serial_filter_imu; + get_parameter_or("serial_filter_imu", serial_filter_imu, std::string{}); top_board_ = std::make_unique( - *this, *command_, get_parameter("serial_filter_top_board").as_string()); + *this, *command_, get_parameter("serial_filter_top_board").as_string(), + !serial_filter_imu.empty()); + if (!serial_filter_imu.empty()) + imu_board_ = std::make_unique(*this, serial_filter_imu); // For command: remote-status using Srv = std_srvs::srv::Trigger; @@ -83,7 +80,8 @@ class DeformableInfantryOmni void update() override { bottom_board_->update(); top_board_->update(); - remote_control_->update(); + if (imu_board_) + imu_board_->update(); using namespace rmcs_description; *camera_transform_ = fast_tf::lookup_transform(*tf_); @@ -99,12 +97,12 @@ class DeformableInfantryOmni } private: - static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); - static constexpr auto kLeftFront = 0; - static constexpr auto kLeftBack = 1; - static constexpr auto kRightBack = 2; - static constexpr auto kRightFront = 3; - static constexpr auto kJointName = std::array{ + static constexpr double kNaN = std::numeric_limits::quiet_NaN(); + static constexpr size_t kLeftFront = 0; + static constexpr size_t kLeftBack = 1; + static constexpr size_t kRightBack = 2; + static constexpr size_t kRightFront = 3; + static constexpr const char* kJointName[] = { "left_front", "left_back", "right_back", @@ -121,16 +119,18 @@ class DeformableInfantryOmni DeformableInfantryOmni& deformableInfantry; }; - struct BottomBoard final : public librmcs::board::RmcsBoardLite::Callback { + class BottomBoard final : private librmcs::agent::RmcsBoardLite { + friend class DeformableInfantryOmni; + public: explicit BottomBoard( DeformableInfantryOmni& status, rmcs_executor::Component& command, const std::string& serial_filter = {}) - : status_{status} - , command_{command} - , kChassisRadiusBase(status.get_parameter("chassis_radius").as_double()) - , kRodLength(status.get_parameter("rod_length").as_double()) - , kDefaultRadius(kChassisRadiusBase + kRodLength) { + : RmcsBoardLite{ + serial_filter, + librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} + , status_{status} + , command_{command} { status.register_output("/referee/serial", referee_serial_); referee_serial_->read = [this](std::byte* buffer, size_t size) { @@ -138,8 +138,8 @@ class DeformableInfantryOmni [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); }; referee_serial_->write = [this](const std::byte* buffer, size_t size) { - board_->start_transmit().uart_transmit( - Spec::kUarts.kUart0, {.uart_data = std::span{buffer, size}}); + start_transmit().uart0_transmit( + {.uart_data = std::span{buffer, size}}); return size; }; @@ -149,7 +149,7 @@ class DeformableInfantryOmni for (auto& motor : chassis_wheel_motors_) motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reduction_ratio(19.0) .enable_multi_turn_angle()); @@ -166,8 +166,7 @@ class DeformableInfantryOmni }); gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006, 3} - .enable_multi_turn_angle()); + device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); status.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); status.register_output("/chassis/imu/pitch", chassis_imu_pitch_, 0.0); @@ -187,20 +186,16 @@ class DeformableInfantryOmni } status.register_output("/chassis/encoder/alpha", encoder_alpha_, kNaN); status.register_output("/chassis/encoder/alpha_dot", encoder_alpha_dot_, kNaN); - status.register_output("/chassis/radius", radius_, kDefaultRadius); + status.register_output("/chassis/radius", radius_, default_radius_); status.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); status.get_parameter_or("debug_log_wheel_motor", debug_log_wheel_motor_, false); status.get_parameter_or( "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); - - auto options = librmcs::board::AdvancedOptions{}; - options.dangerously_skip_version_checks = true; - board_ = std::make_unique(*this, serial_filter, options); - - status_.remote_control_->register_dr16(&dr16_); } + ~BottomBoard() override = default; + void update() { imu_.update_status(); *chassis_yaw_velocity_imu_ = imu_.gz(); @@ -251,113 +246,93 @@ class DeformableInfantryOmni } void command_update(bool even) { - auto builder = board_->start_transmit(); + auto builder = start_transmit(); if (even) { - builder.can_transmit( - Spec::kCans.kCan0, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[kLeftFront].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan1, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[kLeftBack].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan2, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[kRightBack].generate_command(), - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan3, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[kRightFront].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan2, // - { - .can_id = 0x142, - .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan1, // - { - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - } - .as_bytes(), - }); + builder.can0_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kLeftFront].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kLeftBack].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can2_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kRightBack].generate_command(), + device::CanPacket8::PaddingQuarter{}, + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can3_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kRightFront].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can2_transmit({ + .can_id = 0x142, + .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), + }); + builder.can1_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + } + .as_bytes(), + }); } else { for (size_t i = 0; i < 4; ++i) { switch (i) { case kLeftFront: - builder.can_transmit( - Spec::kCans.kCan0, // - { - .can_id = 0x141, - .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), - }); + builder.can0_transmit({ + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); break; case kLeftBack: - builder.can_transmit( - Spec::kCans.kCan1, // - { - .can_id = 0x141, - .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), - }); + builder.can1_transmit({ + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); break; case kRightBack: - builder.can_transmit( - Spec::kCans.kCan2, // - { - .can_id = 0x141, - .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), - }); + builder.can2_transmit({ + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); break; case kRightFront: - builder.can_transmit( - Spec::kCans.kCan3, // - { - .can_id = 0x141, - .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), - }); + builder.can3_transmit({ + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); break; default: break; } @@ -365,54 +340,18 @@ class DeformableInfantryOmni } } - static constexpr double kJointZeroPhysicalAngleRad = 62.5 * std::numbers::pi / 180.0; + private: + static constexpr double joint_zero_physical_angle_rad_ = 62.5 * std::numbers::pi / 180.0; + static constexpr double chassis_radius_base_ = 0.2341741; + static constexpr double rod_length_ = 0.150; + static constexpr double default_radius_ = chassis_radius_base_ + rod_length_; DeformableInfantryOmni& status_; Component& command_; - std::unique_ptr board_; - - // Interfaces - - OutputInterface& tf_{status_.tf_}; - - OutputInterface chassis_yaw_velocity_imu_; - OutputInterface chassis_imu_pitch_; - OutputInterface chassis_imu_roll_; - OutputInterface chassis_imu_pitch_rate_; - OutputInterface chassis_imu_roll_rate_; - - std::array, 4> joint_physical_angle_; - std::array, 4> joint_physical_velocity_; - - OutputInterface encoder_alpha_; - OutputInterface encoder_alpha_dot_; - OutputInterface radius_; - - rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; - OutputInterface referee_serial_; - - // State - - std::atomic wheel_status_received_[4] = {false, false, false, false}; - std::atomic joint_status_received_[4] = {false, false, false, false}; - - bool debug_log_supercap_ = false; - bool debug_log_wheel_motor_ = false; - bool debug_log_deformable_joint_motor_ = false; - - const double kChassisRadiusBase; - const double kRodLength; - const double kDefaultRadius; - - Clock::time_point next_chassis_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - Clock::time_point next_supercap_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - - // Device - device::Bmi088 imu_{1000, 0.2, 0.0}; device::LkMotor gimbal_yaw_motor_{status_, command_, "/gimbal/yaw"}; - device::Dr16 dr16_{}; + device::Dr16 dr16_{status_}; device::DjiMotor chassis_wheel_motors_[4]{ device::DjiMotor{status_, command_, "/chassis/left_front_wheel"}, @@ -427,23 +366,33 @@ class DeformableInfantryOmni device::LkMotor{status_, command_, "/chassis/right_front_joint"}, }; + std::atomic wheel_status_received_[4] = {false, false, false, false}; + std::atomic joint_status_received_[4] = {false, false, false, false}; + bool debug_log_supercap_ = false; + bool debug_log_wheel_motor_ = false; + bool debug_log_deformable_joint_motor_ = false; + Clock::time_point next_chassis_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; + Clock::time_point next_supercap_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; + device::Supercap supercap_{status_, command_}; std::atomic latest_supercap_status_{device::CanPacket8{uint64_t{0}}}; std::atomic supercap_status_received_{false}; - device::Supercap supercap_{status_, command_}; - device::DjiMotor gimbal_bullet_feeder_{status_, command_, "/gimbal/bullet_feeder"}; - void process_chassis_can_receive_(size_t index, const View::Can& data) { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[index].store_status(data.can_data); - wheel_status_received_[index].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[index].store_status(data.can_data); - joint_status_received_[index].store(true, std::memory_order_relaxed); - } - } + OutputInterface& tf_{status_.tf_}; + + OutputInterface chassis_yaw_velocity_imu_; + OutputInterface chassis_imu_pitch_; + OutputInterface chassis_imu_roll_; + OutputInterface chassis_imu_pitch_rate_; + OutputInterface chassis_imu_roll_rate_; + std::array, 4> joint_physical_angle_; + std::array, 4> joint_physical_velocity_; + OutputInterface encoder_alpha_; + OutputInterface encoder_alpha_dot_; + OutputInterface radius_; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; + OutputInterface referee_serial_; void update_joint_physical_feedback_( size_t index, OutputInterface& angle_output, @@ -456,7 +405,7 @@ class DeformableInfantryOmni } const auto to_physical_angle = [](double motor_angle) { - return kJointZeroPhysicalAngleRad - motor_angle; + return joint_zero_physical_angle_rad_ - motor_angle; }; const auto to_physical_velocity = [](double motor_velocity) { return -motor_velocity; }; @@ -475,17 +424,17 @@ class DeformableInfantryOmni if (!alpha_rad.array().isFinite().all() || !alpha_dot_rad.array().isFinite().all()) { *encoder_alpha_ = kNaN; *encoder_alpha_dot_ = kNaN; - *radius_ = kDefaultRadius; + *radius_ = default_radius_; RCLCPP_WARN_THROTTLE( status_.get_logger(), *status_.get_clock(), 1000, "deformable joint feedback invalid, fallback chassis radius to default %.3f m", - kDefaultRadius); + default_radius_); return; } *encoder_alpha_ = alpha_rad.mean(); *encoder_alpha_dot_ = alpha_dot_rad.mean(); - *radius_ = (kChassisRadiusBase + kRodLength * alpha_rad.array().cos()).mean(); + *radius_ = (chassis_radius_base_ + rod_length_ * alpha_rad.array().cos()).mean(); } void log_chassis_feedback_once_per_second_() { @@ -573,76 +522,132 @@ class DeformableInfantryOmni next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); } - void can_receive_callback(const Spec::Can& can, const View::Can& data) override { + void dbus_receive_callback(const librmcs::data::UartDataView& data) override { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + } + + void process_chassis_can_receive_(size_t index, const librmcs::data::CanDataView& data) { if (data.is_extended_can_id || data.is_remote_transmission) return; - if (can == Spec::kCans.kCan0) { - process_chassis_can_receive_(0, data); - monitor_.tick("Bottom::Can0", data.can_id); - } else if (can == Spec::kCans.kCan1) { - process_chassis_can_receive_(1, data); - if (!data.is_extended_can_id && !data.is_remote_transmission - && data.can_id == 0x300) { - if (data.can_data.size() == 8) - latest_supercap_status_.store( - device::CanPacket8{data.can_data}, std::memory_order_relaxed); - supercap_.store_status(data.can_data); - supercap_status_received_.store(true, std::memory_order_relaxed); - } - monitor_.tick("Bottom::Can1", data.can_id); - } else if (can == Spec::kCans.kCan2) { - process_chassis_can_receive_(2, data); - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x142) - gimbal_yaw_motor_.store_status(data.can_data); - else if (data.can_id == 0x203) - gimbal_bullet_feeder_.store_status(data.can_data); - monitor_.tick("Bottom::Can2", data.can_id); - } else if (can == Spec::kCans.kCan3) { - process_chassis_can_receive_(3, data); - monitor_.tick("Bottom::Can3", data.can_id); + if (data.can_id == 0x201) { + chassis_wheel_motors_[index].store_status(data.can_data); + wheel_status_received_[index].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x141) { + chassis_joint_motors_[index].store_status(data.can_data); + joint_status_received_[index].store(true, std::memory_order_relaxed); } } - void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { - if (uart == Spec::kUarts.kDbus) { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); - monitor_.tick("Bottom::Dbus", "Active"); - } else if (uart == Spec::kUarts.kUart0) { - const std::byte* ptr = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, - data.uart_data.size()); - monitor_.tick("Bottom::Uart0", "Active"); + void can0_receive_callback(const librmcs::data::CanDataView& data) override { + process_chassis_can_receive_(0, data); + } + + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + process_chassis_can_receive_(1, data); + if (!data.is_extended_can_id && !data.is_remote_transmission && data.can_id == 0x300) { + if (data.can_data.size() == 8) + latest_supercap_status_.store( + device::CanPacket8{data.can_data}, std::memory_order_relaxed); + supercap_.store_status(data.can_data); + supercap_status_received_.store(true, std::memory_order_relaxed); } } - void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + process_chassis_can_receive_(2, data); + if (data.is_extended_can_id || data.is_remote_transmission) + return; + if (data.can_id == 0x142) + gimbal_yaw_motor_.store_status(data.can_data); + else if (data.can_id == 0x203) + gimbal_bullet_feeder_.store_status(data.can_data); + } + + void can3_receive_callback(const librmcs::data::CanDataView& data) override { + process_chassis_can_receive_(3, data); + } + + void uart0_receive_callback(const librmcs::data::UartDataView& data) override { + const std::byte* ptr = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, data.uart_data.size()); + } + + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { imu_.store_accelerometer_status(data.x, data.y, data.z); - monitor_.tick("Bottom::Imu", "Acc"); } - void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { imu_.store_gyroscope_status(data.x, data.y, data.z); - monitor_.tick("Bottom::Imu", "Gyr"); + } + }; + + class ImuBoard final : private librmcs::agent::RmcsBoardLite { + friend class DeformableInfantryOmni; + + public: + explicit ImuBoard( + DeformableInfantryOmni& status, const std::string& serial_filter = {}) + : RmcsBoardLite{ + serial_filter, + librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} + , tf_{status.tf_} + , bmi088_{1000, 0.2, 0.0} { + + status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + + bmi088_.set_coordinate_mapping( + [](double x, double y, double z) { return std::make_tuple(x, z, -y); }); + } + + ~ImuBoard() override = default; + + void update() { + bmi088_.update_status(); + Eigen::Quaterniond const gimbal_imu_pose{ + bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + + tf_->set_transform( + gimbal_imu_pose.conjugate()); + + *gimbal_pitch_velocity_imu_ = bmi088_.gy(); + } + + private: + void uart0_receive_callback(const librmcs::data::UartDataView& data) override { + (void)data; + // VT13 is not used in this configuration; Dr16 publishes /remote/... directly. + } + + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + bmi088_.store_accelerometer_status(data.x, data.y, data.z); } - auto status() const -> std::vector { return monitor_.text(); } + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + bmi088_.store_gyroscope_status(data.x, data.y, data.z); + } + + OutputInterface& tf_; + OutputInterface gimbal_pitch_velocity_imu_; - StatusMonitor monitor_{}; + device::Bmi088 bmi088_; }; - struct TopBoard final : public librmcs::board::RmcsBoardLite::Callback { + class TopBoard final : private librmcs::agent::RmcsBoardLite { + friend class DeformableInfantryOmni; + public: explicit TopBoard( - DeformableInfantryOmni& status, rmcs_executor::Component& command, - const std::string& serial_filter = {}) - : tf_{status.tf_} - , bmi088_{device::Bmi088Ekf::Config{ - .body_to_sensor = - Eigen::AngleAxisd{std::numbers::pi / 2.0, Eigen::Vector3d::UnitX()} - .toRotationMatrix()}} + DeformableInfantryOmni& status, Command& command, + const std::string& serial_filter = {}, bool has_external_imu_board = false) + : librmcs::agent::RmcsBoardLite( + serial_filter, + librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) + , has_external_imu_board_(has_external_imu_board) + , tf_(status.tf_) + , bmi088_(1000, 0.2, 0.0) , gimbal_pitch_motor_(status, command, "/gimbal/pitch") , gimbal_left_friction_(status, command, "/gimbal/left_friction") , gimbal_right_friction_(status, command, "/gimbal/right_friction") { @@ -654,31 +659,20 @@ class DeformableInfantryOmni static_cast(status.get_parameter("pitch_motor_zero_point").as_int()))); gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reduction_ratio(1.) .set_reversed()); gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2}.set_reduction_ratio( - 1.)); + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); - status.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); - status.register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); - - auto options = librmcs::board::AdvancedOptions{}; - options.dangerously_skip_version_checks = true; - board_ = std::make_unique(*this, serial_filter, options); + if (!has_external_imu_board_) + status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_encoder_); - board_->start_transmit().gpio_digital_read( - Spec::kGpios.kUart1Rx, { - .period_ms = 0, - .asap = false, - .rising_edge = false, - .falling_edge = true, - .capture_timestamp = true, - .pull = librmcs::data::GpioPull::kUp, - }); + bmi088_.set_coordinate_mapping([](double x, double y, double z) { + // Top board BMI088 maps to gimbal frame as (-x, -y, z). + return std::make_tuple(-x, -y, z); + }); } ~TopBoard() override = default; @@ -693,126 +687,110 @@ class DeformableInfantryOmni } void update() { + bmi088_.update_status(); + gimbal_pitch_motor_.update_status(); gimbal_left_friction_.update_status(); gimbal_right_friction_.update_status(); const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); - if (auto snapshot = bmi088_.snapshot()) { - *gimbal_pitch_velocity_bmi088_ = snapshot->gyro_body.y(); - *gimbal_yaw_velocity_bmi088_ = snapshot->gyro_body.z(); + *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); + if (!has_external_imu_board_) { + Eigen::Quaterniond const odom_imu_to_yaw_link{ + bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + Eigen::Quaterniond const yaw_link_to_odom_imu = odom_imu_to_yaw_link.conjugate(); + Eigen::Quaterniond pitch_link_to_odom_imu = + Eigen::Quaterniond{ + Eigen::AngleAxisd{-pitch_encoder_angle, Eigen::Vector3d::UnitY()}} + * yaw_link_to_odom_imu; + pitch_link_to_odom_imu.normalize(); + + *gimbal_pitch_velocity_encoder_ = gimbal_pitch_motor_.velocity(); + // The BMI088 is mounted on the yaw link. fast_tf stores PitchLink -> + // OdomImu, so use the encoder pitch from the TF tree to move the + // yaw-link pose back into PitchLink. tf_->set_transform( - snapshot->orientation.conjugate()); + pitch_link_to_odom_imu); } tf_->set_state( pitch_encoder_angle); } - void command_update() const { - auto builder = board_->start_transmit(); - builder.can_transmit( - Spec::kCans.kCan0, // - { - .can_id = 0x141, - .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan1, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_left_friction_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan2, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - gimbal_right_friction_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); + void command_update() { + auto builder = start_transmit(); + builder.can0_transmit({ + .can_id = 0x141, + .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), + }); + builder.can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_left_friction_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can2_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + gimbal_right_friction_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); } - void can_receive_callback(const Spec::Can& can, const View::Can& data) override { + private: + void uart1_receive_callback(const librmcs::data::UartDataView&) override {} + + void can0_receive_callback(const librmcs::data::CanDataView& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; - if (can == Spec::kCans.kCan0) { - if (data.can_id == 0x141) - gimbal_pitch_motor_.store_status(data.can_data); - monitor_.tick("Top::Can0", data.can_id); - } else if (can == Spec::kCans.kCan1) { - if (data.can_id == 0x201) - gimbal_left_friction_.store_status(data.can_data); - monitor_.tick("Top::Can1", data.can_id); - } else if (can == Spec::kCans.kCan2) { - if (data.can_id == 0x202) - gimbal_right_friction_.store_status(data.can_data); - monitor_.tick("Top::Can2", data.can_id); - } + if (data.can_id == 0x141) + gimbal_pitch_motor_.store_status(data.can_data); } - void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { - const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); - bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); - monitor_.tick("Top::Imu", "Acc"); - } - - void gyroscope_receive_callback(const View::ImuGyroscope& data) override { - const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); - monitor_.tick("Top::Imu", "Gyr"); - if (!timestamp.has_value()) + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; - auto snapshot = - bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); - if (snapshot) - imu_snapshot_output_.emit(*snapshot); + if (data.can_id == 0x201) + gimbal_left_friction_.store_status(data.can_data); } - void gpio_digital_read_result_callback( - const Spec::Gpio& gpio, const View::GpioDigital& data) override { - if (gpio != Spec::kGpios.kUart1Rx) - return; - if (!data.timestamp_quarter_us) - return; - - const auto timestamp = board_clock_lifter_.lift_timestamp(*data.timestamp_quarter_us); - if (!timestamp.has_value()) + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; + if (data.can_id == 0x202) + gimbal_right_friction_.store_status(data.can_data); + } - camera_signal_output_.emit(*timestamp); - monitor_.tick("Top::CameraSync", "Active"); + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + bmi088_.store_accelerometer_status(data.x, data.y, data.z); } - auto status() const -> std::vector { return monitor_.text(); } + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + bmi088_.store_gyroscope_status(data.x, data.y, data.z); + } + bool has_external_imu_board_ = false; OutputInterface& tf_; - OutputInterface gimbal_yaw_velocity_bmi088_; - OutputInterface gimbal_pitch_velocity_bmi088_; - EventOutputInterface imu_snapshot_output_; - EventOutputInterface camera_signal_output_; + OutputInterface gimbal_yaw_velocity_bmi088_; + OutputInterface gimbal_pitch_velocity_encoder_; - device::Bmi088Ekf bmi088_; - device::BoardClockLifter board_clock_lifter_; + device::Bmi088 bmi088_; device::LkMotor gimbal_pitch_motor_; device::DjiMotor gimbal_left_friction_; device::DjiMotor gimbal_right_friction_; - - StatusMonitor monitor_{}; - std::unique_ptr board_; }; auto status_service_callback(const std::shared_ptr& response) @@ -824,25 +802,21 @@ class DeformableInfantryOmni std::println(feedback_message, format, std::forward(args)...); }; - text(" yaw_motor_zero_point: {}", bottom_board_->gimbal_yaw_motor_.last_raw_angle()); - text(" pitch_motor_zero_point: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); + text("Gimbal Status"); + text("- Yaw: {}", bottom_board_->gimbal_yaw_motor_.last_raw_angle()); + text("- Pitch: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); + + text("Chassis Status"); constexpr auto kPosition = - std::array{"left_front", "left_back", "right_back", "right_front"}; + std::array{"left front", "left back", "right back", "right front"}; + constexpr auto kMaxLength = + std::ranges::max_element(kPosition, {}, &std::string_view::size)->size(); - text(""); for (auto&& [index, motor] : std::views::zip(kPosition, bottom_board_->chassis_joint_motors_)) { - text(" {}_zero_point: {}", index, motor.last_raw_angle()); + text("- {:{}}: {}", index, kMaxLength, motor.last_raw_angle()); } - text("\nBottomBoard Status:"); - for (const auto& line : bottom_board_->status()) - text("> {}", line); - - text("\nTopBoard Status:"); - for (const auto& line : top_board_->status()) - text("> {}", line); - response->message = feedback_message.str(); } @@ -854,7 +828,7 @@ class DeformableInfantryOmni std::unique_ptr bottom_board_; std::unique_ptr top_board_; - std::unique_ptr remote_control_; + std::unique_ptr imu_board_; std::shared_ptr command_; uint32_t cmd_tick_ = 0; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp index b5e7640e1..b5d640d34 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp @@ -3,8 +3,10 @@ #include #include #include +#include #include +#include #include #include #include @@ -80,6 +82,7 @@ class DeformableInfantry register_input("/remote/mouse", mouse_); register_input("/remote/keyboard", keyboard_); + register_input("/auto_aim/robot_center", auto_aim_robot_center_, false); register_input("/referee/game/stage", game_stage_); @@ -94,6 +97,7 @@ class DeformableInfantry update_chassis_direction_indicator(); update_deformable_chassis_leg_arcs(); update_ctrl_ui(); + update_auto_aim_feedback(); status_ring_.update_bullet_allowance(*robot_bullet_allowance_); const double friction_wheel_speed = @@ -133,6 +137,25 @@ class DeformableInfantry return; } + void update_auto_aim_feedback() { + if (!auto_aim_robot_center_.ready() || !auto_aim_robot_center_->allFinite()) { + target_distance_indicator_.set_visible(false); + return; + } + + const double distance = auto_aim_robot_center_->norm(); + if (!std::isfinite(distance)) { + target_distance_indicator_.set_visible(false); + return; + } + + target_distance_text_index_ ^= 1u; + auto& text = target_distance_text_[target_distance_text_index_]; + std::snprintf(text.data(), text.size(), "%.1fm", distance); + target_distance_indicator_.set_value(text.data()); + target_distance_indicator_.set_visible(true); + } + void update_chassis_direction_indicator() { auto chassis_mode = *chassis_mode_; @@ -142,7 +165,7 @@ class DeformableInfantry static Shape::Color chassis_direction_indicator_color(rmcs_msgs::ChassisMode mode) { switch (mode) { - case rmcs_msgs::ChassisMode::SPIN_FAST: return Shape::Color::GREEN; + case rmcs_msgs::ChassisMode::SPIN: return Shape::Color::GREEN; case rmcs_msgs::ChassisMode::AUTO: return Shape::Color::CYAN; case rmcs_msgs::ChassisMode::STEP_DOWN: return Shape::Color::PINK; default: return Shape::Color::WHITE; @@ -218,6 +241,7 @@ class DeformableInfantry InputInterface mouse_; InputInterface keyboard_; + InputInterface auto_aim_robot_center_; InputInterface game_stage_; @@ -230,6 +254,10 @@ class DeformableInfantry Arc chassis_direction_indicator_; DeformableChassisLegArcs deformable_chassis_leg_arcs_; + Text target_distance_indicator_{Shape::Color::GREEN, 20, 2, x_center + 34, + y_center + 24, "", false}; + std::array, 2> target_distance_text_{}; + size_t target_distance_text_index_ = 0; AnimatedToggle ctrl_transition_{}; uint16_t crosshair_base_x_ = 0; From 44b91d01bf3382544ca6ed3f94f7b3001bc802b5 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Mon, 13 Jul 2026 17:33:51 +0800 Subject: [PATCH 13/86] fix(deformable-infantry): delete imu_board in deformable_infantry --- .../config/deformable-infantry-omni.yaml | 5 +- .../src/hardware/deformable-infantry-omni.cpp | 106 +++--------------- 2 files changed, 18 insertions(+), 93 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 023fe4e39..b912b5093 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -49,9 +49,8 @@ value_broadcaster: deformable_infantry: ros__parameters: - serial_filter_rmcs_board: "AF-23FB-EE32-B892-1302-AE70-D640-7B4E-0CBF" - serial_filter_top_board: "AF-8AE3-4EC1-03C3-C494-88FE-2DC4-3018-0298" - serial_filter_imu: "AF-7A42-07AA-D181-0356-7715-6D7C-4C65-5762" + serial_filter_bottom_board: "AF-23FB-EE32-B892-1302-AE70-D640-7B4E-0CBF" + serial_filter_top_board: "AF-7A42-07AA-D181-0356-7715-6D7C-4C65-5762" left_front_zero_point: 374 left_back_zero_point: 5801 right_back_zero_point: 7817 diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index d3b5bb464..14695e815 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -55,14 +55,9 @@ class DeformableInfantryOmni tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); bottom_board_ = std::make_unique( - *this, *command_, get_parameter("serial_filter_rmcs_board").as_string()); - std::string serial_filter_imu; - get_parameter_or("serial_filter_imu", serial_filter_imu, std::string{}); + *this, *command_, get_parameter("serial_filter_bottom_board").as_string()); top_board_ = std::make_unique( - *this, *command_, get_parameter("serial_filter_top_board").as_string(), - !serial_filter_imu.empty()); - if (!serial_filter_imu.empty()) - imu_board_ = std::make_unique(*this, serial_filter_imu); + *this, *command_, get_parameter("serial_filter_top_board").as_string()); // For command: remote-status using Srv = std_srvs::srv::Trigger; @@ -80,8 +75,6 @@ class DeformableInfantryOmni void update() override { bottom_board_->update(); top_board_->update(); - if (imu_board_) - imu_board_->update(); using namespace rmcs_description; *camera_transform_ = fast_tf::lookup_transform(*tf_); @@ -583,71 +576,18 @@ class DeformableInfantryOmni } }; - class ImuBoard final : private librmcs::agent::RmcsBoardLite { + class TopBoard final : private librmcs::agent::RmcsBoardLite { friend class DeformableInfantryOmni; public: - explicit ImuBoard( - DeformableInfantryOmni& status, const std::string& serial_filter = {}) + explicit TopBoard( + DeformableInfantryOmni& status, rmcs_executor::Component& command, + const std::string& serial_filter = {}) : RmcsBoardLite{ serial_filter, librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} , tf_{status.tf_} - , bmi088_{1000, 0.2, 0.0} { - - status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); - - bmi088_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(x, z, -y); }); - } - - ~ImuBoard() override = default; - - void update() { - bmi088_.update_status(); - Eigen::Quaterniond const gimbal_imu_pose{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - - tf_->set_transform( - gimbal_imu_pose.conjugate()); - - *gimbal_pitch_velocity_imu_ = bmi088_.gy(); - } - - private: - void uart0_receive_callback(const librmcs::data::UartDataView& data) override { - (void)data; - // VT13 is not used in this configuration; Dr16 publishes /remote/... directly. - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); - } - - OutputInterface& tf_; - OutputInterface gimbal_pitch_velocity_imu_; - - device::Bmi088 bmi088_; - }; - - class TopBoard final : private librmcs::agent::RmcsBoardLite { - friend class DeformableInfantryOmni; - - public: - explicit TopBoard( - DeformableInfantryOmni& status, Command& command, - const std::string& serial_filter = {}, bool has_external_imu_board = false) - : librmcs::agent::RmcsBoardLite( - serial_filter, - librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) - , has_external_imu_board_(has_external_imu_board) - , tf_(status.tf_) - , bmi088_(1000, 0.2, 0.0) + , bmi088_{1000, 0.2, 0.0} , gimbal_pitch_motor_(status, command, "/gimbal/pitch") , gimbal_left_friction_(status, command, "/gimbal/left_friction") , gimbal_right_friction_(status, command, "/gimbal/right_friction") { @@ -666,12 +606,11 @@ class DeformableInfantryOmni device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - if (!has_external_imu_board_) - status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_encoder_); + status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); bmi088_.set_coordinate_mapping([](double x, double y, double z) { // Top board BMI088 maps to gimbal frame as (-x, -y, z). - return std::make_tuple(-x, -y, z); + return std::make_tuple(x, z, -y); }); } @@ -695,27 +634,16 @@ class DeformableInfantryOmni const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); + Eigen::Quaterniond const gimbal_imu_pose{ + bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + + *gimbal_pitch_velocity_bmi088_ = bmi088_.gy(); *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); - if (!has_external_imu_board_) { - Eigen::Quaterniond const odom_imu_to_yaw_link{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - Eigen::Quaterniond const yaw_link_to_odom_imu = odom_imu_to_yaw_link.conjugate(); - Eigen::Quaterniond pitch_link_to_odom_imu = - Eigen::Quaterniond{ - Eigen::AngleAxisd{-pitch_encoder_angle, Eigen::Vector3d::UnitY()}} - * yaw_link_to_odom_imu; - pitch_link_to_odom_imu.normalize(); - - *gimbal_pitch_velocity_encoder_ = gimbal_pitch_motor_.velocity(); - // The BMI088 is mounted on the yaw link. fast_tf stores PitchLink -> - // OdomImu, so use the encoder pitch from the TF tree to move the - // yaw-link pose back into PitchLink. - tf_->set_transform( - pitch_link_to_odom_imu); - } tf_->set_state( pitch_encoder_angle); + tf_->set_transform( + gimbal_imu_pose.conjugate()); } void command_update() { @@ -781,11 +709,10 @@ class DeformableInfantryOmni bmi088_.store_gyroscope_status(data.x, data.y, data.z); } - bool has_external_imu_board_ = false; OutputInterface& tf_; OutputInterface gimbal_yaw_velocity_bmi088_; - OutputInterface gimbal_pitch_velocity_encoder_; + OutputInterface gimbal_pitch_velocity_bmi088_; device::Bmi088 bmi088_; device::LkMotor gimbal_pitch_motor_; @@ -828,7 +755,6 @@ class DeformableInfantryOmni std::unique_ptr bottom_board_; std::unique_ptr top_board_; - std::unique_ptr imu_board_; std::shared_ptr command_; uint32_t cmd_tick_ = 0; From 52adbf558f074afc3f8f70e1ee55ed4ecb4dff20 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Tue, 14 Jul 2026 12:18:04 +0800 Subject: [PATCH 14/86] wip: Adapt imu snapshot sync with hikcamera --- .../config/deformable-infantry-omni-b.yaml | 18 ++-- .../config/deformable-infantry-omni.yaml | 13 ++- .../deformable_infantry_gimbal_controller.cpp | 88 ++++++------------ .../src/hardware/deformable-infantry-omni.cpp | 92 +++++++++++++------ 4 files changed, 114 insertions(+), 97 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 7e91dcc4b..5f1c382f6 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -30,8 +30,19 @@ rmcs_executor: - rmcs_core::controller::chassis::DeformableJointController -> rb_joint_controller - rmcs_core::controller::chassis::DeformableJointController -> rf_joint_controller - # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster # - rmcs::AutoAimComponent + # - rmcs::AutoAimCapturerComponent -> auto_aim_capturer + + # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + +auto_aim_capturer: + ros__parameters: + camera_name: "" + exposure_us: 2000.0 + gain: 16.9807 + framerate: 249.1 + invert_image: false + rls_tau_sec: 10.0 value_broadcaster: ros__parameters: @@ -122,9 +133,6 @@ gimbal_controller: yaw_velocity_ki: 0.0 yaw_velocity_kd: 0.0 - yaw_vel_ff_gain: 0.47 - yaw_acc_ff_gain: 0.00 - pitch_angle_kp: 25.0 pitch_angle_ki: 0.0 pitch_angle_kd: 0.0 @@ -133,8 +141,6 @@ gimbal_controller: pitch_velocity_ki: 0.0 pitch_velocity_kd: 0.0 - pitch_acc_ff_gain: 0.10 - pitch_torque_control: true friction_wheel_controller: diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index b912b5093..888275fa0 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -30,8 +30,19 @@ rmcs_executor: - rmcs_core::controller::chassis::DeformableJointController -> rb_joint_controller - rmcs_core::controller::chassis::DeformableJointController -> rf_joint_controller + - rmcs::AutoAimComponent + - rmcs::AutoAimCapturerComponent -> auto_aim_capturer + # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster - # - rmcs::AutoAimComponent + +auto_aim_capturer: + ros__parameters: + camera_name: "" + exposure_us: 2000.0 + gain: 8.0 + framerate: 120.0 + invert_image: false + rls_tau_sec: 10.0 value_broadcaster: ros__parameters: diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp index bf4588b1e..9396df1d1 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp @@ -5,7 +5,7 @@ #include #include -#include +#include #include #include #include @@ -15,8 +15,6 @@ namespace rmcs_core::controller::gimbal { -using namespace rmcs_description; - class DeformableInfantryGimbalController : public rmcs_executor::Component , public rclcpp::Node { @@ -25,16 +23,16 @@ class DeformableInfantryGimbalController : Node( get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + configure_pid("yaw_angle", yaw_angle_pid_); configure_pid("yaw_velocity", yaw_velocity_pid_); configure_pid("pitch_angle", pitch_angle_pid_); configure_pid("pitch_velocity", pitch_velocity_pid_); + get_parameter("pitch_torque_control", pitch_torque_control_enabled_); get_parameter("manual_joystick_sensitivity", joystick_sensitivity_); get_parameter("manual_mouse_sensitivity", mouse_sensitivity_); - get_parameter("yaw_vel_ff_gain", yaw_vel_ff_gain_); - get_parameter("yaw_acc_ff_gain", yaw_acc_ff_gain_); - get_parameter("pitch_acc_ff_gain", pitch_acc_ff_gain_); + get_parameter_or("pitch_gravity_ff_gain", pitch_gravity_ff_gain_, 0.0); get_parameter_or("pitch_gravity_ff_phase", pitch_gravity_ff_phase_, 0.0); get_parameter_or("ctrl_hold_pitch_target_angle", ctrl_hold_pitch_target_angle_, 0.0); @@ -42,8 +40,8 @@ class DeformableInfantryGimbalController auto update() -> void override { const auto switch_right = *input_.switch_right; - const auto switch_left = *input_.switch_left; - const auto keyboard = *input_.keyboard; + const auto switch_left = *input_.switch_left; + const auto keyboard = *input_.keyboard; using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) @@ -78,24 +76,10 @@ class DeformableInfantryGimbalController *output_.yaw_control_torque = kNaN; } - const auto feedforward_enabled = auto_aim_active - && input_.auto_aim_feedforward_valid.ready() - && *input_.auto_aim_feedforward_valid; - if (std::isfinite(angle_error.yaw_angle_error)) { - const auto yaw_velocity_ff = feedforward_enabled && input_.auto_aim_yaw_rate.ready() - && std::isfinite(*input_.auto_aim_yaw_rate) - ? yaw_vel_ff_gain_ * *input_.auto_aim_yaw_rate - : 0.0; - const auto yaw_acc_ff = feedforward_enabled && input_.auto_aim_yaw_acc.ready() - && std::isfinite(*input_.auto_aim_yaw_acc) - ? yaw_acc_ff_gain_ * *input_.auto_aim_yaw_acc - : 0.0; - - const auto yaw_velocity_ref = - yaw_angle_pid_.update(angle_error.yaw_angle_error) + yaw_velocity_ff; + const auto yaw_velocity_ref = yaw_angle_pid_.update(angle_error.yaw_angle_error); *output_.yaw_control_torque = - yaw_velocity_pid_.update(yaw_velocity_ref - *input_.yaw_velocity_imu) + yaw_acc_ff; + yaw_velocity_pid_.update(yaw_velocity_ref - *input_.yaw_velocity_imu); } if (!ctrl_hold_active_) { @@ -103,12 +87,8 @@ class DeformableInfantryGimbalController pitch_angle_pid_.reset(); pitch_velocity_pid_.reset(); *output_.pitch_control_velocity = kNaN; - *output_.pitch_control_torque = kNaN; + *output_.pitch_control_torque = kNaN; } else { - const auto pitch_acc_ff = feedforward_enabled && input_.auto_aim_pitch_acc.ready() - && std::isfinite(*input_.auto_aim_pitch_acc) - ? pitch_acc_ff_gain_ * *input_.auto_aim_pitch_acc - : 0.0; const auto pitch_gravity_ff = pitch_gravity_feedforward(); const auto pitch_velocity_ref = pitch_angle_pid_.update(angle_error.pitch_angle_error); @@ -117,19 +97,18 @@ class DeformableInfantryGimbalController *output_.pitch_control_velocity = kNaN; *output_.pitch_control_torque = pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity_imu) - + pitch_acc_ff + pitch_gravity_ff; } else { pitch_velocity_pid_.reset(); *output_.pitch_control_velocity = pitch_velocity_ref; - *output_.pitch_control_torque = kNaN; + *output_.pitch_control_torque = kNaN; } } } } private: - static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); + static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); static constexpr auto kDefaultDt = 1e-3; auto configure_pid(const std::string& prefix, pid::PidCalculator& calculator) -> void { @@ -161,11 +140,6 @@ class DeformableInfantryGimbalController component.register_input("/auto_aim/should_control", auto_aim_should_control, false); component.register_input( "/auto_aim/control_direction", auto_aim_control_direction, false); - component.register_input( - "/auto_aim/feedforward_valid", auto_aim_feedforward_valid, false); - component.register_input("/auto_aim/yaw_rate", auto_aim_yaw_rate, false); - component.register_input("/auto_aim/yaw_acc", auto_aim_yaw_acc, false); - component.register_input("/auto_aim/pitch_acc", auto_aim_pitch_acc, false); } InputInterface joystick_left; @@ -185,10 +159,6 @@ class DeformableInfantryGimbalController InputInterface auto_aim_should_control; InputInterface auto_aim_control_direction; - InputInterface auto_aim_feedforward_valid; - InputInterface auto_aim_yaw_rate; - InputInterface auto_aim_yaw_acc; - InputInterface auto_aim_pitch_acc; } input_{*this}; struct Output { @@ -212,9 +182,7 @@ class DeformableInfantryGimbalController OutputInterface pitch_angle_error; } output_{*this}; - auto ctrl_hold_requested() const -> bool { - return pitch_lock_active_; - } + auto ctrl_hold_requested() const -> bool { return pitch_lock_active_; } auto update_dt() const -> double { if (input_.update_rate.ready() && std::isfinite(*input_.update_rate) @@ -289,10 +257,10 @@ class DeformableInfantryGimbalController if (!ctrl_hold_active_) activate_ctrl_hold(); - *output_.yaw_control_angle = kNaN; + *output_.yaw_control_angle = kNaN; *output_.pitch_control_velocity = kNaN; - *output_.pitch_control_torque = kNaN; - *output_.pitch_control_angle = kNaN; + *output_.pitch_control_torque = kNaN; + *output_.pitch_control_angle = kNaN; if (input_.pitch_angle.ready() && std::isfinite(*input_.pitch_angle)) { auto pitch_target_error = ctrl_hold_pitch_target_angle_ - *input_.pitch_angle; @@ -301,17 +269,17 @@ class DeformableInfantryGimbalController else if (pitch_target_error < -std::numbers::pi) pitch_target_error += 2 * std::numbers::pi; - *output_.pitch_angle_error = pitch_target_error; + *output_.pitch_angle_error = pitch_target_error; const auto pitch_velocity_ref = pitch_angle_pid_.update(pitch_target_error); if (pitch_torque_control_enabled_) { *output_.pitch_control_velocity = kNaN; *output_.pitch_control_torque = pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity_imu) - + pitch_gravity_feedforward(); + + pitch_gravity_feedforward(); } else { pitch_velocity_pid_.reset(); *output_.pitch_control_velocity = pitch_velocity_ref; - *output_.pitch_control_torque = kNaN; + *output_.pitch_control_torque = kNaN; } } } @@ -321,11 +289,11 @@ class DeformableInfantryGimbalController yaw_velocity_pid_.reset(); pitch_angle_pid_.reset(); pitch_velocity_pid_.reset(); - *output_.yaw_control_torque = kNaN; - *output_.yaw_control_angle = kNaN; + *output_.yaw_control_torque = kNaN; + *output_.yaw_control_angle = kNaN; *output_.pitch_control_velocity = kNaN; - *output_.pitch_control_torque = kNaN; - *output_.pitch_control_angle = kNaN; + *output_.pitch_control_torque = kNaN; + *output_.pitch_control_angle = kNaN; } auto reset_all_controls() -> void { @@ -334,7 +302,7 @@ class DeformableInfantryGimbalController suspension_on_by_switch_ = false; last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; gimbal_solver_.update(TwoAxisGimbalSolver::SetDisabled{}); - *output_.yaw_angle_error = kNaN; + *output_.yaw_angle_error = kNaN; *output_.pitch_angle_error = kNaN; reset_control_outputs(); } @@ -363,12 +331,9 @@ class DeformableInfantryGimbalController get_parameter("pitch_velocity_kd").as_double(), }; - double joystick_sensitivity_ = 0.003; - double mouse_sensitivity_ = 0.5; - bool pitch_torque_control_enabled_ = false; - double yaw_vel_ff_gain_ = 0.0; - double yaw_acc_ff_gain_ = 0.0; - double pitch_acc_ff_gain_ = 0.0; + double joystick_sensitivity_ = 0.003; + double mouse_sensitivity_ = 0.5; + bool pitch_torque_control_enabled_ = false; double ctrl_hold_pitch_target_angle_ = 0.0; double pitch_gravity_ff_gain_ = 0.0; double pitch_gravity_ff_phase_ = 0.0; @@ -381,6 +346,5 @@ class DeformableInfantryGimbalController } // namespace rmcs_core::controller::gimbal #include - PLUGINLIB_EXPORT_CLASS( rmcs_core::controller::gimbal::DeformableInfantryGimbalController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index 14695e815..27e5c9648 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -20,10 +20,14 @@ #include #include #include +#include +#include #include #include #include "hardware/device/bmi088.hpp" +#include "hardware/device/bmi088_ekf.hpp" +#include "hardware/device/board_clock_lifter.hpp" #include "hardware/device/can_packet.hpp" #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" @@ -179,7 +183,7 @@ class DeformableInfantryOmni } status.register_output("/chassis/encoder/alpha", encoder_alpha_, kNaN); status.register_output("/chassis/encoder/alpha_dot", encoder_alpha_dot_, kNaN); - status.register_output("/chassis/radius", radius_, default_radius_); + status.register_output("/chassis/radius", radius_, kDefaultRadius); status.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); status.get_parameter_or("debug_log_wheel_motor", debug_log_wheel_motor_, false); @@ -334,10 +338,10 @@ class DeformableInfantryOmni } private: - static constexpr double joint_zero_physical_angle_rad_ = 62.5 * std::numbers::pi / 180.0; - static constexpr double chassis_radius_base_ = 0.2341741; - static constexpr double rod_length_ = 0.150; - static constexpr double default_radius_ = chassis_radius_base_ + rod_length_; + static constexpr double kJointZeroPhysicalAngleRad = 62.5 * std::numbers::pi / 180.0; + static constexpr double kChassisRadiusBase = 0.2341741; + static constexpr double kRodLength = 0.150; + static constexpr double kDefaultRadius = kChassisRadiusBase + kRodLength; DeformableInfantryOmni& status_; Component& command_; @@ -398,7 +402,7 @@ class DeformableInfantryOmni } const auto to_physical_angle = [](double motor_angle) { - return joint_zero_physical_angle_rad_ - motor_angle; + return kJointZeroPhysicalAngleRad - motor_angle; }; const auto to_physical_velocity = [](double motor_velocity) { return -motor_velocity; }; @@ -417,17 +421,17 @@ class DeformableInfantryOmni if (!alpha_rad.array().isFinite().all() || !alpha_dot_rad.array().isFinite().all()) { *encoder_alpha_ = kNaN; *encoder_alpha_dot_ = kNaN; - *radius_ = default_radius_; + *radius_ = kDefaultRadius; RCLCPP_WARN_THROTTLE( status_.get_logger(), *status_.get_clock(), 1000, "deformable joint feedback invalid, fallback chassis radius to default %.3f m", - default_radius_); + kDefaultRadius); return; } *encoder_alpha_ = alpha_rad.mean(); *encoder_alpha_dot_ = alpha_dot_rad.mean(); - *radius_ = (chassis_radius_base_ + rod_length_ * alpha_rad.array().cos()).mean(); + *radius_ = (kChassisRadiusBase + kRodLength * alpha_rad.array().cos()).mean(); } void log_chassis_feedback_once_per_second_() { @@ -587,7 +591,8 @@ class DeformableInfantryOmni serial_filter, librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} , tf_{status.tf_} - , bmi088_{1000, 0.2, 0.0} + , bmi088_{device::Bmi088Ekf::Config{ + .body_to_sensor = (Eigen::Matrix3d() << 1, 0, 0, 0, 0, -1, 0, 1, 0).finished()}} , gimbal_pitch_motor_(status, command, "/gimbal/pitch") , gimbal_left_friction_(status, command, "/gimbal/left_friction") , gimbal_right_friction_(status, command, "/gimbal/right_friction") { @@ -607,11 +612,19 @@ class DeformableInfantryOmni status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); - - bmi088_.set_coordinate_mapping([](double x, double y, double z) { - // Top board BMI088 maps to gimbal frame as (-x, -y, z). - return std::make_tuple(x, z, -y); - }); + status.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); + status.register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); + + start_transmit().gpio_digital_read( + librmcs::spec::rmcs_board_lite::kGpioDescriptors.kUart1Rx, + { + .period_ms = 0, + .asap = false, + .rising_edge = false, + .falling_edge = true, + .capture_timestamp = true, + .pull = librmcs::data::GpioPull::kUp, + }); } ~TopBoard() override = default; @@ -626,24 +639,21 @@ class DeformableInfantryOmni } void update() { - bmi088_.update_status(); - gimbal_pitch_motor_.update_status(); gimbal_left_friction_.update_status(); gimbal_right_friction_.update_status(); const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); - Eigen::Quaterniond const gimbal_imu_pose{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - - *gimbal_pitch_velocity_bmi088_ = bmi088_.gy(); - *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); + if (auto snapshot = bmi088_.snapshot()) { + *gimbal_pitch_velocity_bmi088_ = snapshot->gyro_body.y(); + *gimbal_yaw_velocity_bmi088_ = snapshot->gyro_body.z(); + tf_->set_transform( + snapshot->orientation.conjugate()); + } tf_->set_state( pitch_encoder_angle); - tf_->set_transform( - gimbal_imu_pose.conjugate()); } void command_update() { @@ -702,19 +712,45 @@ class DeformableInfantryOmni void accelerometer_receive_callback( const librmcs::data::AccelerometerDataView& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); + const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); + bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); } void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); + const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + auto snapshot = + bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); + if (snapshot) + imu_snapshot_output_.emit(*snapshot); } - OutputInterface& tf_; + auto gpio_digital_read_result_callback( + const librmcs::spec::rmcs_board_lite::GpioDescriptor& gpio, + const librmcs::data::GpioDigitalDataView& data) -> void override { + if (gpio != librmcs::spec::rmcs_board_lite::kGpioDescriptors.kUart1Rx) + return; + if (!data.timestamp_quarter_us) + return; + + const auto timestamp = board_clock_lifter_.lift_timestamp(*data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + + camera_signal_output_.emit(*timestamp); + } + + OutputInterface& tf_; OutputInterface gimbal_yaw_velocity_bmi088_; OutputInterface gimbal_pitch_velocity_bmi088_; - device::Bmi088 bmi088_; + EventOutputInterface imu_snapshot_output_; + EventOutputInterface camera_signal_output_; + + device::Bmi088Ekf bmi088_; + device::BoardClockLifter board_clock_lifter_; device::LkMotor gimbal_pitch_motor_; device::DjiMotor gimbal_left_friction_; device::DjiMotor gimbal_right_friction_; From a7eb0f64dab157a69cbcd2d86b2dd431dac8c54f Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Tue, 14 Jul 2026 14:32:45 +0800 Subject: [PATCH 15/86] wip: Update librmcs version and adjust deformable infantry pid config --- .../config/deformable-infantry-omni.yaml | 37 +- .../hardware/deformable-infantry-omni-b.cpp | 252 ++++--- .../src/hardware/deformable-infantry-omni.cpp | 499 +++++++------- rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 129 ++-- .../rmcs_core/src/hardware/omni_infantry.cpp | 111 ++- rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp | 644 +++++++----------- .../steering-hero-little-six-friction.cpp | 567 +++++++-------- 7 files changed, 974 insertions(+), 1265 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 888275fa0..85a017264 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -33,7 +33,7 @@ rmcs_executor: - rmcs::AutoAimComponent - rmcs::AutoAimCapturerComponent -> auto_aim_capturer - # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster auto_aim_capturer: ros__parameters: @@ -47,16 +47,16 @@ auto_aim_capturer: value_broadcaster: ros__parameters: forward_list: - - /gimbal/yaw/angle - - /gimbal/yaw/velocity - - /chassis/left_front_joint/physical_angle - - /chassis/left_front_joint/physical_velocity - - /chassis/left_back_joint/physical_angle - - /chassis/left_back_joint/physical_velocity - - /chassis/right_back_joint/physical_angle - - /chassis/right_back_joint/physical_velocity - - /chassis/right_front_joint/physical_angle - - /chassis/right_front_joint/physical_velocity + - /gimbal/pitch/angle + - /gimbal/pitch/velocity + # - /chassis/left_front_joint/physical_angle + # - /chassis/left_front_joint/physical_velocity + # - /chassis/left_back_joint/physical_angle + # - /chassis/left_back_joint/physical_velocity + # - /chassis/right_back_joint/physical_angle + # - /chassis/right_back_joint/physical_velocity + # - /chassis/right_front_joint/physical_angle + # - /chassis/right_front_joint/physical_velocity deformable_infantry: ros__parameters: @@ -141,19 +141,16 @@ gimbal_controller: yaw_velocity_ki: 0.0 yaw_velocity_kd: 0.0 - yaw_vel_ff_gain: 0.47 - yaw_acc_ff_gain: 0.00 + pitch_angle_kp: 35.0 + pitch_angle_ki: 0.02 + pitch_angle_kd: 0.3 - pitch_angle_kp: 7.2 - pitch_angle_ki: 0.0 - pitch_angle_kd: 0.0 - - pitch_velocity_kp: 3.0 + pitch_velocity_kp: 2.0 pitch_velocity_ki: 0.0 pitch_velocity_kd: 0.0 - pitch_acc_ff_gain: 0.10 - pitch_gravity_ff_gain: 5.95 + # pitch_gravity_ff_gain: 5.95 + pitch_gravity_ff_gain: 0.0 pitch_gravity_ff_phase: 0.66 pitch_torque_control: true diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index 9c58749ef..0c1694081 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include "hardware/device/bmi088.hpp" #include "hardware/device/can_packet.hpp" @@ -120,20 +120,15 @@ class DeformableInfantryOmniB DeformableInfantryOmniB& deformableInfantry; }; - class BottomBoard final : private librmcs::agent::RmcsBoardLite { + struct BottomBoard final : public librmcs::board::RmcsBoardLite::Callback { public: - friend class DeformableInfantryOmniB; - static constexpr double nan_ = std::numeric_limits::quiet_NaN(); explicit BottomBoard( DeformableInfantryOmniB& deformableInfantry, DeformableInfantryOmniBCommand& deformableInfantry_command, const std::string& serial_filter = {}) - : RmcsBoardLite{ - serial_filter, - librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} - , deformable_infantry_{deformableInfantry} + : deformable_infantry_{deformableInfantry} , command_{deformableInfantry_command} , tf_{deformableInfantry.tf_} { @@ -143,7 +138,8 @@ class DeformableInfantryOmniB [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); }; referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart0_transmit( + board_->start_transmit().uart_transmit( + Spec::kUarts.kUart0, {.uart_data = std::span{buffer, size}}); return size; }; @@ -215,9 +211,13 @@ class DeformableInfantryOmniB "debug_log_wheel_motor", debug_log_wheel_motor_, false); deformableInfantry.get_parameter_or( "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); - } - ~BottomBoard() override = default; + auto options = librmcs::board::AdvancedOptions{}; + options.dangerously_skip_version_checks = true; + board_ = std::make_unique( + *this, serial_filter, + options); + } void update() { imu_.update_status(); @@ -274,9 +274,9 @@ class DeformableInfantryOmniB } void command_update(bool even) { - auto builder = start_transmit(); + auto builder = board_->start_transmit(); if (even) { - builder.can0_transmit({ + builder.can_transmit(Spec::kCans.kCan0, { .can_id = 0x200, .can_data = device::CanPacket8{ @@ -287,7 +287,7 @@ class DeformableInfantryOmniB } .as_bytes(), }); - builder.can1_transmit({ + builder.can_transmit(Spec::kCans.kCan1, { .can_id = 0x200, .can_data = device::CanPacket8{ @@ -298,7 +298,7 @@ class DeformableInfantryOmniB } .as_bytes(), }); - builder.can2_transmit({ + builder.can_transmit(Spec::kCans.kCan2, { .can_id = 0x200, .can_data = device::CanPacket8{ @@ -309,7 +309,7 @@ class DeformableInfantryOmniB } .as_bytes(), }); - builder.can3_transmit({ + builder.can_transmit(Spec::kCans.kCan3, { .can_id = 0x200, .can_data = device::CanPacket8{ @@ -320,11 +320,11 @@ class DeformableInfantryOmniB } .as_bytes(), }); - builder.can2_transmit({ + builder.can_transmit(Spec::kCans.kCan2, { .can_id = 0x142, .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes(), }); - builder.can1_transmit({ + builder.can_transmit(Spec::kCans.kCan1, { .can_id = 0x1FE, .can_data = device::CanPacket8{ @@ -336,26 +336,25 @@ class DeformableInfantryOmniB .as_bytes(), }); } else { - builder.can0_transmit({ + builder.can_transmit(Spec::kCans.kCan0, { .can_id = 0x141, .can_data = chassis_joint_motors_[0].generate_command().as_bytes(), }); - builder.can1_transmit({ + builder.can_transmit(Spec::kCans.kCan1, { .can_id = 0x141, .can_data = chassis_joint_motors_[1].generate_command().as_bytes(), }); - builder.can2_transmit({ + builder.can_transmit(Spec::kCans.kCan2, { .can_id = 0x141, .can_data = chassis_joint_motors_[2].generate_command().as_bytes(), }); - builder.can3_transmit({ + builder.can_transmit(Spec::kCans.kCan3, { .can_id = 0x141, .can_data = chassis_joint_motors_[3].generate_command().as_bytes(), }); } } - private: DeformableInfantryOmniB& deformable_infantry_; rmcs_executor::Component& command_; @@ -477,80 +476,70 @@ class DeformableInfantryOmniB next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); } - void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); - } - - void can0_receive_callback(const librmcs::data::CanDataView& data) override { + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { if (data.is_extended_can_id || data.is_remote_transmission) return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[0].store_status(data.can_data); - wheel_status_received_[0].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[0].store_status(data.can_data); - joint_status_received_[0].store(true, std::memory_order_relaxed); + if (can == Spec::kCans.kCan0) { + if (data.can_id == 0x201) { + chassis_wheel_motors_[0].store_status(data.can_data); + wheel_status_received_[0].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x141) { + chassis_joint_motors_[0].store_status(data.can_data); + joint_status_received_[0].store(true, std::memory_order_relaxed); + } + } else if (can == Spec::kCans.kCan1) { + if (data.can_id == 0x201) { + chassis_wheel_motors_[1].store_status(data.can_data); + wheel_status_received_[1].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x141) { + chassis_joint_motors_[1].store_status(data.can_data); + joint_status_received_[1].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x300) { + if (data.can_data.size() == 8) + latest_supercap_status_.store( + device::CanPacket8{data.can_data}, std::memory_order_relaxed); + supercap_.store_status(data.can_data); + supercap_status_received_.store(true, std::memory_order_relaxed); + } + } else if (can == Spec::kCans.kCan2) { + if (data.can_id == 0x201) { + chassis_wheel_motors_[2].store_status(data.can_data); + wheel_status_received_[2].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x141) { + chassis_joint_motors_[2].store_status(data.can_data); + joint_status_received_[2].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x142) { + gimbal_yaw_motor_.store_status(data.can_data); + } else if (data.can_id == 0x203) { + gimbal_bullet_feeder_.store_status(data.can_data); + } + } else if (can == Spec::kCans.kCan3) { + if (data.can_id == 0x201) { + chassis_wheel_motors_[3].store_status(data.can_data); + wheel_status_received_[3].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x141) { + chassis_joint_motors_[3].store_status(data.can_data); + joint_status_received_[3].store(true, std::memory_order_relaxed); + } } } - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[1].store_status(data.can_data); - wheel_status_received_[1].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[1].store_status(data.can_data); - joint_status_received_[1].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x300) { - if (data.can_data.size() == 8) - latest_supercap_status_.store( - device::CanPacket8{data.can_data}, std::memory_order_relaxed); - supercap_.store_status(data.can_data); - supercap_status_received_.store(true, std::memory_order_relaxed); - } - } - - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[2].store_status(data.can_data); - wheel_status_received_[2].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[2].store_status(data.can_data); - joint_status_received_[2].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x142) { - gimbal_yaw_motor_.store_status(data.can_data); - } else if (data.can_id == 0x203) { - gimbal_bullet_feeder_.store_status(data.can_data); - } - } - - void can3_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[3].store_status(data.can_data); - wheel_status_received_[3].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[3].store_status(data.can_data); - joint_status_received_[3].store(true, std::memory_order_relaxed); + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + if (uart == Spec::kUarts.kDbus) { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + } else if (uart == Spec::kUarts.kUart0) { + const std::byte* ptr = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, + data.uart_data.size()); } } - void uart0_receive_callback(const librmcs::data::UartDataView& data) override { - const std::byte* ptr = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, data.uart_data.size()); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { imu_.store_accelerometer_status(data.x, data.y, data.z); } - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { imu_.store_gyroscope_status(data.x, data.y, data.z); } @@ -605,18 +594,15 @@ class DeformableInfantryOmniB OutputInterface encoder_alpha_; OutputInterface encoder_alpha_dot_; OutputInterface radius_; - }; - class ImuBoard final : private librmcs::agent::RmcsBoardLite { - friend class DeformableInfantryOmniB; + std::unique_ptr board_; + }; + struct ImuBoard final : public librmcs::board::RmcsBoardLite::Callback { public: explicit ImuBoard( DeformableInfantryOmniB& deformableInfantry, const std::string& serial_filter = {}) - : RmcsBoardLite{ - serial_filter, - librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} - , tf_{deformableInfantry.tf_} + : tf_{deformableInfantry.tf_} , bmi088_{1000, 0.2, 0.0} { deformableInfantry.register_output( @@ -624,9 +610,13 @@ class DeformableInfantryOmniB bmi088_.set_coordinate_mapping( [](double x, double y, double z) { return std::make_tuple(x, z, -y); }); - } - ~ImuBoard() override = default; + auto options = librmcs::board::AdvancedOptions{}; + options.dangerously_skip_version_checks = true; + board_ = std::make_unique( + *this, serial_filter, + options); + } void update() { bmi088_.update_status(); @@ -639,18 +629,16 @@ class DeformableInfantryOmniB *gimbal_pitch_velocity_imu_ = bmi088_.gy(); } - private: - void uart0_receive_callback(const librmcs::data::UartDataView& data) override { + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + (void)uart; (void)data; - // VT13 is not used in this configuration; Dr16 publishes /remote/... directly. } - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { bmi088_.store_accelerometer_status(data.x, data.y, data.z); } - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { bmi088_.store_gyroscope_status(data.x, data.y, data.z); } @@ -658,20 +646,17 @@ class DeformableInfantryOmniB OutputInterface gimbal_pitch_velocity_imu_; device::Bmi088 bmi088_; + + std::unique_ptr board_; }; - class TopBoard final : private librmcs::agent::RmcsBoardLite { + struct TopBoard final : public librmcs::board::RmcsBoardLite::Callback { public: - friend class DeformableInfantryOmniB; - explicit TopBoard( DeformableInfantryOmniB& deformableInfantry, DeformableInfantryOmniBCommand& deformableInfantry_command, const std::string& serial_filter = {}, bool has_external_imu_board = false) - : librmcs::agent::RmcsBoardLite( - serial_filter, - librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) - , has_external_imu_board_(has_external_imu_board) + : has_external_imu_board_(has_external_imu_board) , tf_(deformableInfantry.tf_) , bmi088_(1000, 0.2, 0.0) , gimbal_pitch_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/pitch") @@ -708,9 +693,13 @@ class DeformableInfantryOmniB // Top board BMI088 maps to gimbal frame as (-x, -y, z). return std::make_tuple(-x, -y, z); }); - } - ~TopBoard() override = default; + auto options = librmcs::board::AdvancedOptions{}; + options.dangerously_skip_version_checks = true; + board_ = std::make_unique( + *this, serial_filter, + options); + } [[nodiscard]] auto gimbal_yaw_velocity() const -> double { return *gimbal_yaw_velocity_bmi088_; @@ -755,13 +744,13 @@ class DeformableInfantryOmniB } void command_update() { - auto builder = start_transmit(); - builder.can0_transmit({ + auto builder = board_->start_transmit(); + builder.can_transmit(Spec::kCans.kCan0, { .can_id = 0x141, .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), }); - builder.can1_transmit({ + builder.can_transmit(Spec::kCans.kCan1, { .can_id = 0x200, .can_data = device::CanPacket8{ @@ -774,33 +763,32 @@ class DeformableInfantryOmniB }); } - private: - void uart1_receive_callback(const librmcs::data::UartDataView&) override {} - - void can0_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - if (data.can_id == 0x141) - gimbal_pitch_motor_.store_status(data.can_data); + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + (void)uart; + (void)data; } - void can1_receive_callback(const librmcs::data::CanDataView& data) override { + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; - if (data.can_id == 0x201) - gimbal_left_friction_.store_status(data.can_data); - else if (data.can_id == 0x202) - gimbal_right_friction_.store_status(data.can_data); - else if (data.can_id == 0x203) - scope_motor_.store_status(data.can_data); + if (can == Spec::kCans.kCan0) { + if (data.can_id == 0x141) + gimbal_pitch_motor_.store_status(data.can_data); + } else if (can == Spec::kCans.kCan1) { + if (data.can_id == 0x201) + gimbal_left_friction_.store_status(data.can_data); + else if (data.can_id == 0x202) + gimbal_right_friction_.store_status(data.can_data); + else if (data.can_id == 0x203) + scope_motor_.store_status(data.can_data); + } } - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { bmi088_.store_accelerometer_status(data.x, data.y, data.z); } - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { bmi088_.store_gyroscope_status(data.x, data.y, data.z); } @@ -815,6 +803,8 @@ class DeformableInfantryOmniB device::DjiMotor gimbal_left_friction_; device::DjiMotor gimbal_right_friction_; device::DjiMotor scope_motor_; + + std::unique_ptr board_; }; auto status_service_callback(const std::shared_ptr& response) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index 27e5c9648..5c3d5956d 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include #include @@ -94,12 +94,12 @@ class DeformableInfantryOmni } private: - static constexpr double kNaN = std::numeric_limits::quiet_NaN(); - static constexpr size_t kLeftFront = 0; - static constexpr size_t kLeftBack = 1; - static constexpr size_t kRightBack = 2; - static constexpr size_t kRightFront = 3; - static constexpr const char* kJointName[] = { + static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); + static constexpr auto kLeftFront = 0; + static constexpr auto kLeftBack = 1; + static constexpr auto kRightBack = 2; + static constexpr auto kRightFront = 3; + static constexpr auto kJointName = std::array{ "left_front", "left_back", "right_back", @@ -116,17 +116,12 @@ class DeformableInfantryOmni DeformableInfantryOmni& deformableInfantry; }; - class BottomBoard final : private librmcs::agent::RmcsBoardLite { - friend class DeformableInfantryOmni; - + struct BottomBoard final : public librmcs::board::RmcsBoardLite::Callback { public: explicit BottomBoard( DeformableInfantryOmni& status, rmcs_executor::Component& command, const std::string& serial_filter = {}) - : RmcsBoardLite{ - serial_filter, - librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} - , status_{status} + : status_{status} , command_{command} { status.register_output("/referee/serial", referee_serial_); @@ -135,8 +130,8 @@ class DeformableInfantryOmni [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); }; referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart0_transmit( - {.uart_data = std::span{buffer, size}}); + board_->start_transmit().uart_transmit( + Spec::kUarts.kUart0, {.uart_data = std::span{buffer, size}}); return size; }; @@ -189,9 +184,11 @@ class DeformableInfantryOmni status.get_parameter_or("debug_log_wheel_motor", debug_log_wheel_motor_, false); status.get_parameter_or( "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); - } - ~BottomBoard() override = default; + auto options = librmcs::board::AdvancedOptions{}; + options.dangerously_skip_version_checks = true; + board_ = std::make_unique(*this, serial_filter, options); + } void update() { imu_.update_status(); @@ -243,93 +240,113 @@ class DeformableInfantryOmni } void command_update(bool even) { - auto builder = start_transmit(); + auto builder = board_->start_transmit(); if (even) { - builder.can0_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[kLeftFront].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[kLeftBack].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[kRightBack].generate_command(), - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can3_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[kRightFront].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x142, - .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), - }); - builder.can1_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - } - .as_bytes(), - }); + builder.can_transmit( + Spec::kCans.kCan0, + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kLeftFront].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan1, + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kLeftBack].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan2, + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kRightBack].generate_command(), + device::CanPacket8::PaddingQuarter{}, + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan3, + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kRightFront].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan2, + { + .can_id = 0x142, + .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + } + .as_bytes(), + }); } else { for (size_t i = 0; i < 4; ++i) { switch (i) { case kLeftFront: - builder.can0_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), - }); + builder.can_transmit( + Spec::kCans.kCan0, + { + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); break; case kLeftBack: - builder.can1_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), - }); + builder.can_transmit( + Spec::kCans.kCan1, + { + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); break; case kRightBack: - builder.can2_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), - }); + builder.can_transmit( + Spec::kCans.kCan2, + { + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); break; case kRightFront: - builder.can3_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), - }); + builder.can_transmit( + Spec::kCans.kCan3, + { + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); break; default: break; } @@ -337,7 +354,6 @@ class DeformableInfantryOmni } } - private: static constexpr double kJointZeroPhysicalAngleRad = 62.5 * std::numbers::pi / 180.0; static constexpr double kChassisRadiusBase = 0.2341741; static constexpr double kRodLength = 0.150; @@ -346,6 +362,42 @@ class DeformableInfantryOmni DeformableInfantryOmni& status_; Component& command_; + std::unique_ptr board_; + + // Interfaces + + OutputInterface& tf_{status_.tf_}; + + OutputInterface chassis_yaw_velocity_imu_; + OutputInterface chassis_imu_pitch_; + OutputInterface chassis_imu_roll_; + OutputInterface chassis_imu_pitch_rate_; + OutputInterface chassis_imu_roll_rate_; + + std::array, 4> joint_physical_angle_; + std::array, 4> joint_physical_velocity_; + + OutputInterface encoder_alpha_; + OutputInterface encoder_alpha_dot_; + OutputInterface radius_; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; + OutputInterface referee_serial_; + + // State + + std::atomic wheel_status_received_[4] = {false, false, false, false}; + std::atomic joint_status_received_[4] = {false, false, false, false}; + + bool debug_log_supercap_ = false; + bool debug_log_wheel_motor_ = false; + bool debug_log_deformable_joint_motor_ = false; + + Clock::time_point next_chassis_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; + Clock::time_point next_supercap_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; + + // Device + device::Bmi088 imu_{1000, 0.2, 0.0}; device::LkMotor gimbal_yaw_motor_{status_, command_, "/gimbal/yaw"}; device::Dr16 dr16_{status_}; @@ -363,33 +415,23 @@ class DeformableInfantryOmni device::LkMotor{status_, command_, "/chassis/right_front_joint"}, }; - std::atomic wheel_status_received_[4] = {false, false, false, false}; - std::atomic joint_status_received_[4] = {false, false, false, false}; - bool debug_log_supercap_ = false; - bool debug_log_wheel_motor_ = false; - bool debug_log_deformable_joint_motor_ = false; - Clock::time_point next_chassis_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - Clock::time_point next_supercap_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - device::Supercap supercap_{status_, command_}; std::atomic latest_supercap_status_{device::CanPacket8{uint64_t{0}}}; std::atomic supercap_status_received_{false}; - device::DjiMotor gimbal_bullet_feeder_{status_, command_, "/gimbal/bullet_feeder"}; - - OutputInterface& tf_{status_.tf_}; + device::Supercap supercap_{status_, command_}; - OutputInterface chassis_yaw_velocity_imu_; - OutputInterface chassis_imu_pitch_; - OutputInterface chassis_imu_roll_; - OutputInterface chassis_imu_pitch_rate_; - OutputInterface chassis_imu_roll_rate_; - std::array, 4> joint_physical_angle_; - std::array, 4> joint_physical_velocity_; - OutputInterface encoder_alpha_; - OutputInterface encoder_alpha_dot_; - OutputInterface radius_; + device::DjiMotor gimbal_bullet_feeder_{status_, command_, "/gimbal/bullet_feeder"}; - rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; - OutputInterface referee_serial_; + void process_chassis_can_receive_(size_t index, const View::Can& data) { + if (data.is_extended_can_id || data.is_remote_transmission) + return; + if (data.can_id == 0x201) { + chassis_wheel_motors_[index].store_status(data.can_data); + wheel_status_received_[index].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x141) { + chassis_joint_motors_[index].store_status(data.can_data); + joint_status_received_[index].store(true, std::memory_order_relaxed); + } + } void update_joint_physical_feedback_( size_t index, OutputInterface& angle_output, @@ -519,78 +561,58 @@ class DeformableInfantryOmni next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); } - void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); - } - - void process_chassis_can_receive_(size_t index, const librmcs::data::CanDataView& data) { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[index].store_status(data.can_data); - wheel_status_received_[index].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[index].store_status(data.can_data); - joint_status_received_[index].store(true, std::memory_order_relaxed); + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { + if (can == Spec::kCans.kCan0) { + process_chassis_can_receive_(0, data); + } else if (can == Spec::kCans.kCan1) { + process_chassis_can_receive_(1, data); + if (!data.is_extended_can_id && !data.is_remote_transmission + && data.can_id == 0x300) { + if (data.can_data.size() == 8) + latest_supercap_status_.store( + device::CanPacket8{data.can_data}, std::memory_order_relaxed); + supercap_.store_status(data.can_data); + supercap_status_received_.store(true, std::memory_order_relaxed); + } + } else if (can == Spec::kCans.kCan2) { + process_chassis_can_receive_(2, data); + if (data.is_extended_can_id || data.is_remote_transmission) + return; + if (data.can_id == 0x142) + gimbal_yaw_motor_.store_status(data.can_data); + else if (data.can_id == 0x203) + gimbal_bullet_feeder_.store_status(data.can_data); + } else if (can == Spec::kCans.kCan3) { + process_chassis_can_receive_(3, data); } } - void can0_receive_callback(const librmcs::data::CanDataView& data) override { - process_chassis_can_receive_(0, data); - } - - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - process_chassis_can_receive_(1, data); - if (!data.is_extended_can_id && !data.is_remote_transmission && data.can_id == 0x300) { - if (data.can_data.size() == 8) - latest_supercap_status_.store( - device::CanPacket8{data.can_data}, std::memory_order_relaxed); - supercap_.store_status(data.can_data); - supercap_status_received_.store(true, std::memory_order_relaxed); + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + if (uart == Spec::kUarts.kDbus) { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + } else if (uart == Spec::kUarts.kUart0) { + const std::byte* ptr = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, + data.uart_data.size()); } } - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - process_chassis_can_receive_(2, data); - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x142) - gimbal_yaw_motor_.store_status(data.can_data); - else if (data.can_id == 0x203) - gimbal_bullet_feeder_.store_status(data.can_data); - } - - void can3_receive_callback(const librmcs::data::CanDataView& data) override { - process_chassis_can_receive_(3, data); - } - - void uart0_receive_callback(const librmcs::data::UartDataView& data) override { - const std::byte* ptr = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, data.uart_data.size()); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { imu_.store_accelerometer_status(data.x, data.y, data.z); } - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { imu_.store_gyroscope_status(data.x, data.y, data.z); } }; - class TopBoard final : private librmcs::agent::RmcsBoardLite { - friend class DeformableInfantryOmni; - + struct TopBoard final : public librmcs::board::RmcsBoardLite::Callback { public: explicit TopBoard( DeformableInfantryOmni& status, rmcs_executor::Component& command, const std::string& serial_filter = {}) - : RmcsBoardLite{ - serial_filter, - librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} - , tf_{status.tf_} + : tf_{status.tf_} , bmi088_{device::Bmi088Ekf::Config{ .body_to_sensor = (Eigen::Matrix3d() << 1, 0, 0, 0, 0, -1, 0, 1, 0).finished()}} , gimbal_pitch_motor_(status, command, "/gimbal/pitch") @@ -615,16 +637,19 @@ class DeformableInfantryOmni status.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); status.register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); - start_transmit().gpio_digital_read( - librmcs::spec::rmcs_board_lite::kGpioDescriptors.kUart1Rx, - { - .period_ms = 0, - .asap = false, - .rising_edge = false, - .falling_edge = true, - .capture_timestamp = true, - .pull = librmcs::data::GpioPull::kUp, - }); + auto options = librmcs::board::AdvancedOptions{}; + options.dangerously_skip_version_checks = true; + board_ = std::make_unique(*this, serial_filter, options); + + board_->start_transmit().gpio_digital_read( + Spec::kGpios.kUart1Rx, { + .period_ms = 0, + .asap = false, + .rising_edge = false, + .falling_edge = true, + .capture_timestamp = true, + .pull = librmcs::data::GpioPull::kUp, + }); } ~TopBoard() override = default; @@ -656,67 +681,63 @@ class DeformableInfantryOmni pitch_encoder_angle); } - void command_update() { - auto builder = start_transmit(); - builder.can0_transmit({ - .can_id = 0x141, - .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), - }); - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_left_friction_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - gimbal_right_friction_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - } - - private: - void uart1_receive_callback(const librmcs::data::UartDataView&) override {} - - void can0_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - if (data.can_id == 0x141) - gimbal_pitch_motor_.store_status(data.can_data); - } - - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - if (data.can_id == 0x201) - gimbal_left_friction_.store_status(data.can_data); + void command_update() const { + auto builder = board_->start_transmit(); + builder.can_transmit( + Spec::kCans.kCan0, + { + .can_id = 0x141, + .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_left_friction_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + gimbal_right_friction_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); } - void can2_receive_callback(const librmcs::data::CanDataView& data) override { + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; - if (data.can_id == 0x202) - gimbal_right_friction_.store_status(data.can_data); + if (can == Spec::kCans.kCan0) { + if (data.can_id == 0x141) + gimbal_pitch_motor_.store_status(data.can_data); + } else if (can == Spec::kCans.kCan1) { + if (data.can_id == 0x201) + gimbal_left_friction_.store_status(data.can_data); + } else if (can == Spec::kCans.kCan2) { + if (data.can_id == 0x202) + gimbal_right_friction_.store_status(data.can_data); + } } - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); } - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); if (!timestamp.has_value()) return; @@ -726,11 +747,9 @@ class DeformableInfantryOmni imu_snapshot_output_.emit(*snapshot); } - auto gpio_digital_read_result_callback( - const librmcs::spec::rmcs_board_lite::GpioDescriptor& gpio, - const librmcs::data::GpioDigitalDataView& data) -> void override { - - if (gpio != librmcs::spec::rmcs_board_lite::kGpioDescriptors.kUart1Rx) + void gpio_digital_read_result_callback( + const Spec::Gpio& gpio, const View::GpioDigital& data) override { + if (gpio != Spec::kGpios.kUart1Rx) return; if (!data.timestamp_quarter_us) return; @@ -754,6 +773,8 @@ class DeformableInfantryOmni device::LkMotor gimbal_pitch_motor_; device::DjiMotor gimbal_left_friction_; device::DjiMotor gimbal_right_friction_; + + std::unique_ptr board_; }; auto status_service_callback(const std::shared_ptr& response) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index 754807075..df2c270e8 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include @@ -9,19 +10,15 @@ #include #include #include -#include -#include #include #include #include -#include "hardware/device/bmi088_ekf.hpp" -#include "hardware/device/board_clock_lifter.hpp" +#include "hardware/device/bmi088.hpp" #include "hardware/device/can_packet.hpp" #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" -#include "hardware/device/remote_control.hpp" #include "librmcs/board/rmcs_board_lite.hpp" namespace rmcs_core::hardware { @@ -45,13 +42,16 @@ class Flight device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( static_cast(get_parameter("pitch_motor_zero_point").as_int()))); gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 3} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reversed() .set_reduction_ratio(1.0)); gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 4}.set_reduction_ratio(1.0)); + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.0)); gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006, 1}.enable_multi_turn_angle()); + device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); + + bmi088_.set_coordinate_mapping( + [](double x, double y, double z) { return std::tuple{y, z, x}; }); using namespace rmcs_description; @@ -60,11 +60,12 @@ class Flight tf_->set_transform( Eigen::Translation3d{kCameraPostionX, 0.0, kCameraPostionZ}); - register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_, 0.0); - register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_, 0.0); + register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); + register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); register_output("/tf", tf_); - register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); + register_output("/auto_aim/camera_transform", camera_transform_); + register_output("/auto_aim/barrel_direction", barrel_direction_); register_output("/referee/serial", referee_serial_); referee_serial_->read = [this](std::byte* buffer, size_t size) { @@ -73,21 +74,11 @@ class Flight }; referee_serial_->write = [this](const std::byte* buffer, size_t size) { board_->start_transmit().uart_transmit( - Spec::kUarts.kUart1, {.uart_data = std::span{buffer, size}}); + Spec::kUarts.kUart1, + {.uart_data = std::span{buffer, size}}); return size; }; - register_output("/px4/serial", px4_serial_); - px4_serial_->read = [](std::byte*, size_t) { return size_t{0}; }; - px4_serial_->write = [this](const std::byte* buffer, size_t size) { - board_->start_transmit().uart_transmit( - Spec::kUarts.kUart0, {.uart_data = std::span{buffer, size}}); - return size; - }; - - remote_control_ = std::make_unique(*this); - remote_control_->register_dr16(&dr16_); - status_service_ = create_service( "/rmcs/service/robot_status", [this]( @@ -104,44 +95,44 @@ class Flight update_motors(); update_imu(); dr16_.update_status(); - remote_control_->update(); + + using namespace rmcs_description; + *camera_transform_ = fast_tf::lookup_transform(*tf_); + *barrel_direction_ = + *fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); } void command_update() { auto builder = board_->start_transmit(); builder .can_transmit( - Spec::kCans.kCan0, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - gimbal_left_friction_.generate_command(), - gimbal_right_friction_.generate_command(), - } - .as_bytes(), - }) + Spec::kCans.kCan0, + {.can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + gimbal_left_friction_.generate_command(), + gimbal_right_friction_.generate_command(), + } + .as_bytes()}) .can_transmit( - Spec::kCans.kCan1, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }) + Spec::kCans.kCan1, + {.can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes()}) .can_transmit( - Spec::kCans.kCan2, // + Spec::kCans.kCan2, {.can_id = 0x141, .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes()}) .can_transmit( - Spec::kCans.kCan3, // + Spec::kCans.kCan3, {.can_id = 0x142, .can_data = gimbal_pitch_motor_.generate_command().as_bytes()}); } @@ -163,12 +154,13 @@ class Flight void update_imu() { using namespace rmcs_description; - if (const auto snapshot = bmi088_.snapshot()) { - tf_->set_transform(snapshot->orientation.conjugate()); + bmi088_.update_status(); + const auto gimbal_imu_pose = + Eigen::Quaterniond{bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + tf_->set_transform(gimbal_imu_pose.conjugate()); - *gimbal_yaw_velocity_imu_ = snapshot->gyro_body.z(); - *gimbal_pitch_velocity_imu_ = snapshot->gyro_body.y(); - } + *gimbal_yaw_velocity_imu_ = bmi088_.gz(); + *gimbal_pitch_velocity_imu_ = bmi088_.gy(); } void @@ -225,21 +217,11 @@ class Flight } void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { - const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); - bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); + bmi088_.store_accelerometer_status(data.x, data.y, data.z); } void gyroscope_receive_callback(const View::ImuGyroscope& data) override { - const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); - if (!timestamp.has_value()) - return; - - const auto snapshot = - bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); - if (!snapshot) - return; - - imu_snapshot_output_.emit(*snapshot); + bmi088_.store_gyroscope_status(data.x, data.y, data.z); } private: @@ -266,21 +248,16 @@ class Flight device::DjiMotor gimbal_right_friction_{*this, *command_component_, "/gimbal/right_friction"}; device::DjiMotor gimbal_bullet_feeder_{*this, *command_component_, "/gimbal/bullet_feeder"}; - device::Dr16 dr16_; - std::unique_ptr remote_control_; - // 等价于旧 Bmi088 的坐标映射 (x, y, z) -> (y, z, x)：body = body_to_sensor^T * sensor - device::Bmi088Ekf bmi088_{device::Bmi088Ekf::Config{ - .body_to_sensor = (Eigen::Matrix3d{} << 0, 0, 1, 1, 0, 0, 0, 1, 0).finished(), - }}; - device::BoardClockLifter board_clock_lifter_; + device::Dr16 dr16_{*this}; + device::Bmi088 bmi088_{1000.0, 0.2, 0.00}; OutputInterface gimbal_yaw_velocity_imu_; OutputInterface gimbal_pitch_velocity_imu_; OutputInterface tf_; OutputInterface referee_serial_; - OutputInterface px4_serial_; - EventOutputInterface imu_snapshot_output_; + OutputInterface camera_transform_; + OutputInterface barrel_direction_; rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; }; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp index b8c5c4af9..a678b9ae2 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp @@ -26,7 +26,6 @@ #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" -#include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" namespace rmcs_core::hardware { @@ -54,11 +53,11 @@ class OmniInfantry , gimbal_left_friction_(*this, *infantry_command_, "/gimbal/left_friction") , gimbal_right_friction_(*this, *infantry_command_, "/gimbal/right_friction") , gimbal_bullet_feeder_(*this, *infantry_command_, "/gimbal/bullet_feeder") - , dr16_{} { + , dr16_{*this} { for (auto& motor : chassis_wheel_motors_) motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reversed() .set_reduction_ratio(13.) .enable_multi_turn_angle()); @@ -75,13 +74,13 @@ class OmniInfantry static_cast(get_parameter("pitch_motor_zero_point").as_int()))); gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1}.set_reduction_ratio(1.)); + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reversed() .set_reduction_ratio(1.)); gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006, 2}.enable_multi_turn_angle()); + device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); @@ -93,14 +92,15 @@ class OmniInfantry *this, get_parameter("board_serial").as_string()); board_->start_transmit().gpio_digital_read( - Spec::kGpios.kUart0Tx, { - .period_ms = 0, - .asap = false, - .rising_edge = false, - .falling_edge = true, - .capture_timestamp = true, - .pull = librmcs::data::GpioPull::kUp, - }); + Spec::kGpios.kUart0Tx, + { + .period_ms = 0, + .asap = false, + .rising_edge = false, + .falling_edge = true, + .capture_timestamp = true, + .pull = librmcs::data::GpioPull::kUp, + }); using namespace rmcs_description; // NOLINT(google-build-using-namespace) tf_->set_transform(Eigen::Translation3d{0.06603, 0.0, 0.082}); @@ -130,12 +130,10 @@ class OmniInfantry }; referee_serial_->write = [this](const std::byte* buffer, size_t size) { board_->start_transmit().uart_transmit( - Spec::kUarts.kUart1, {.uart_data = std::span{buffer, size}}); + Spec::kUarts.kUart1, + {.uart_data = std::span{buffer, size}}); return size; }; - - remote_control_ = std::make_unique(*this); - remote_control_->register_dr16(&dr16_); } OmniInfantry(const OmniInfantry&) = delete; @@ -149,7 +147,6 @@ class OmniInfantry update_motors(); update_imu(); dr16_.update_status(); - remote_control_->update(); supercap_.update_status(); } @@ -157,55 +154,50 @@ class OmniInfantry auto builder = board_->start_transmit(); builder.can_transmit( - Spec::kCans.kCan1, // - { - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - } - .as_bytes(), - }); + Spec::kCans.kCan1, + {.can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + } + .as_bytes()}); builder.can_transmit( - Spec::kCans.kCan1, // - {.can_id = 0x145, .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes()}); + Spec::kCans.kCan1, + {.can_id = 0x145, + .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes()}); builder.can_transmit( - Spec::kCans.kCan1, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), - chassis_wheel_motors_[1].generate_command(), - chassis_wheel_motors_[2].generate_command(), - chassis_wheel_motors_[3].generate_command(), - } - .as_bytes(), - }); + Spec::kCans.kCan1, + {.can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[0].generate_command(), + chassis_wheel_motors_[1].generate_command(), + chassis_wheel_motors_[2].generate_command(), + chassis_wheel_motors_[3].generate_command(), + } + .as_bytes()}); builder.can_transmit( - Spec::kCans.kCan2, // - {.can_id = 0x142, + Spec::kCans.kCan2, + {.can_id = 0x142, .can_data = gimbal_pitch_motor_.generate_velocity_command().as_bytes()}); builder.can_transmit( - Spec::kCans.kCan2, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - gimbal_left_friction_.generate_command(), - gimbal_right_friction_.generate_command(), - } - .as_bytes(), - }); + Spec::kCans.kCan2, + {.can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + gimbal_bullet_feeder_.generate_command(), + gimbal_left_friction_.generate_command(), + gimbal_right_friction_.generate_command(), + } + .as_bytes()}); } private: @@ -355,7 +347,6 @@ class OmniInfantry device::DjiMotor gimbal_bullet_feeder_; device::Dr16 dr16_; - std::unique_ptr remote_control_; device::Bmi088Ekf bmi088_; device::BoardClockLifter board_clock_lifter_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp index fecbe501f..9b6aa67fd 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp @@ -1,54 +1,38 @@ -#include +#include "hardware/device/bmi088.hpp" +#include "hardware/device/can_packet.hpp" +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/lk_motor.hpp" +#include "hardware/device/supercap.hpp" + #include #include +#include #include #include #include #include #include -#include -#include #include #include #include -#include "hardware/device/bmi088_ekf.hpp" -#include "hardware/device/board_clock_lifter.hpp" -#include "hardware/device/can_packet.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/lk_motor.hpp" -#include "hardware/device/remote_control.hpp" -#include "hardware/device/supercap.hpp" -#include "hardware/util/status_monitor.hpp" - namespace rmcs_core::hardware { class Sentry : public rmcs_executor::Component , public rclcpp::Node { - - static constexpr auto kPosition = - std::array{"left_front", "left_back", "right_back", "right_front"}; - public: Sentry() : Node( get_component_name(), rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) { - constexpr auto kNaN = std::numeric_limits::quiet_NaN(); - register_input("/predefined/timestamp", timestamp_); - register_output("/tf", tf_); - register_output("/chassis/climber/measure_yaw", chassis_measure_yaw_, kNaN); - register_output("/auto_aim/camera_transform", camera_transform_); - register_output("/auto_aim/barrel_direction", barrel_direction_); - register_output("/auto_aim/yaw_velocity", yaw_velocity_, kNaN); - // 提供 remote-status 命令服务。 + // For command: remote-status using Srv = std_srvs::srv::Trigger; status_service_ = create_service( "/rmcs/service/robot_status", @@ -56,207 +40,191 @@ class Sentry status_service_callback(response); }); - remote_control_ = std::make_unique(*this); - - gimbal_board_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_gimbal_board").as_string()); + top_board_ = std::make_unique( + *this, *command_component_, get_parameter("board_serial_top_board").as_string()); - chassis_board_ = std::make_unique( + bottom_board_ = std::make_unique( *this, *command_component_, get_parameter("board_serial_bottom_board").as_string()); + gimbal_board_ = + std::make_unique(get_parameter("board_serial_gimbal_board").as_string()); + tf_->set_transform( Eigen::Translation3d{0.08, 0.0, 0.0}); tf_->set_transform( Eigen::Translation3d{0.07128, 0.0, 0.0481}); } - void update() override { + auto update() -> void override { + top_board_->update(); + bottom_board_->update(); gimbal_board_->update(); - chassis_board_->update(); - remote_control_->update(); - - using namespace rmcs_description; - *camera_transform_ = fast_tf::lookup_transform(*tf_); - *barrel_direction_ = *fast_tf::cast( - PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); - *yaw_velocity_ = gimbal_board_->yaw_velocity(); - - const auto chassis_direction = - fast_tf::cast(BaseLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); - *chassis_measure_yaw_ = std::atan2(chassis_direction->y(), chassis_direction->x()); + tf_->set_transform( + gimbal_board_->imu_pose().conjugate()); } private: - class GimbalBoard final : public librmcs::board::RmcsBoardLite::Callback { + class GimbalBoard final : public librmcs::board::CBoard::Callback { public: - explicit GimbalBoard( + explicit GimbalBoard(std::string_view board_serial = {}) { + bmi088_.set_coordinate_mapping( + [](double x, double y, double z) { return std::make_tuple(y, -x, z); }); + board_ = std::make_unique(*this, board_serial); + } + + GimbalBoard(const GimbalBoard&) = delete; + GimbalBoard& operator=(const GimbalBoard&) = delete; + GimbalBoard(GimbalBoard&&) = delete; + GimbalBoard& operator=(GimbalBoard&&) = delete; + + ~GimbalBoard() override = default; + + auto update() -> void { + bmi088_.update_status(); + imu_pose_ = Eigen::Quaterniond{bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + } + + auto imu_pose() const -> Eigen::Quaterniond { return imu_pose_; } + + private: + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { + bmi088_.store_accelerometer_status(data.x, data.y, data.z); + } + + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + bmi088_.store_gyroscope_status(data.x, data.y, data.z); + } + + device::Bmi088 bmi088_{1000, 0.2, 0.0}; + Eigen::Quaterniond imu_pose_ = Eigen::Quaterniond::Identity(); + std::unique_ptr board_; + }; + + class TopBoard final : public librmcs::board::RmcsBoardLite::Callback { + friend class Sentry; + + public: + explicit TopBoard( Sentry& sentry, rmcs_executor::Component& sentry_command, - std::string_view board_serial = {}) + std::string_view board_serial = {}, + librmcs::board::AdvancedOptions options = {}) : tf_(sentry.tf_) + , bmi088_(1000, 0.2, 0.0) , gimbal_pitch_motor_(sentry, sentry_command, "/gimbal/pitch") , gimbal_top_yaw_motor_(sentry, sentry_command, "/gimbal/top_yaw") , gimbal_bullet_feeder_(sentry, sentry_command, "/gimbal/bullet_feeder") , gimbal_left_friction_(sentry, sentry_command, "/gimbal/left_friction") , gimbal_right_friction_(sentry, sentry_command, "/gimbal/right_friction") { - - using namespace device; - - auto zero_point = int{0}; - sentry.get_parameter("pitch_motor_zero_point", zero_point); gimbal_pitch_motor_.configure( - LkMotor::Config{LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point(zero_point)); + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( + static_cast(sentry.get_parameter("pitch_motor_zero_point").as_int()))); - sentry.get_parameter("top_yaw_motor_zero_point", zero_point); gimbal_top_yaw_motor_.configure( - LkMotor::Config{LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point(zero_point)); + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( + static_cast(sentry.get_parameter("top_yaw_motor_zero_point").as_int()))); gimbal_bullet_feeder_.configure( - DjiMotor::Config{DjiMotor::Type::kM3508, 4} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .enable_multi_turn_angle() .set_reversed() .set_reduction_ratio(19 * 2)); gimbal_left_friction_.configure( - DjiMotor::Config{DjiMotor::Type::kM3508, 2}.set_reduction_ratio(1.)); + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); gimbal_right_friction_.configure( - DjiMotor::Config{DjiMotor::Type::kM3508, 1}.set_reduction_ratio(1.).set_reversed()); - - sentry.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_, 0.0); - sentry.register_output( - "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_, 0.0); - - sentry.register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); - sentry.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); - - board_ = std::make_unique(*this, board_serial); - board_->start_transmit().gpio_digital_read( - Spec::kGpios.kUart1Rx, { - .period_ms = 0, - .asap = false, - .rising_edge = false, - .falling_edge = true, - .capture_timestamp = true, - .pull = librmcs::data::GpioPull::kUp, - }); + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reduction_ratio(1.) + .set_reversed()); + + sentry.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); + sentry.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); + + bmi088_.set_coordinate_mapping( + [](double x, double y, double z) { return std::make_tuple(-x, -y, z); }); + + board_ = std::make_unique(*this, board_serial, options); } - auto yaw_velocity() const -> double { return *gimbal_yaw_velocity_bmi088_; } + auto update() -> void { + gimbal_top_yaw_motor_.update_status(); + gimbal_pitch_motor_.update_status(); + + const auto pitch_angle = + std::remainder(gimbal_pitch_motor_.angle(), 2.0 * std::numbers::pi_v); + + bmi088_.update_status(); + const Eigen::Quaterniond gimbal_bmi088_pose{ + bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - auto status() const -> std::vector { return monitor_.text(); } + tf_->set_transform( + gimbal_bmi088_pose.conjugate()); + + *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); + *gimbal_pitch_velocity_bmi088_ = bmi088_.gy(); - void update() { gimbal_bullet_feeder_.update_status(); gimbal_left_friction_.update_status(); gimbal_right_friction_.update_status(); - gimbal_top_yaw_motor_.update_status(); tf_->set_state( gimbal_top_yaw_motor_.angle()); - - gimbal_pitch_motor_.update_status(); - const auto pitch_angle = - std::remainder(gimbal_pitch_motor_.angle(), 2.0 * std::numbers::pi); tf_->set_state(pitch_angle); + } - if (const auto snapshot = bmi088_.snapshot()) { - tf_->set_transform( - snapshot->orientation.conjugate()); + auto command_update() -> void { + auto builder = board_->start_transmit(); + + builder.can_transmit(Spec::kCans.kCan0, { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_right_friction_.generate_command(), + gimbal_left_friction_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + gimbal_bullet_feeder_.generate_command(), + } + .as_bytes(), + }); - *gimbal_yaw_velocity_bmi088_ = snapshot->gyro_body.z(); - *gimbal_pitch_velocity_bmi088_ = snapshot->gyro_body.y(); - } - } + builder.can_transmit(Spec::kCans.kCan3, { + .can_id = 0x141, + .can_data = gimbal_top_yaw_motor_.generate_torque_command().as_bytes(), + }); - void command_update() const { - board_->start_transmit() - .can_transmit( - Spec::kCans.kCan0, - { - .can_id = gimbal_right_friction_.send_id(), - .can_data = - device::CanPacket8{ - gimbal_right_friction_.generate_command(), - gimbal_left_friction_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - } - .as_bytes(), - }) - .can_transmit( - Spec::kCans.kCan3, - { - .can_id = 0x141, - .can_data = gimbal_top_yaw_motor_.generate_torque_command().as_bytes(), - }) - .can_transmit( - Spec::kCans.kCan3, - { - .can_id = 0x142, - .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), - }); + builder.can_transmit(Spec::kCans.kCan2, { + .can_id = 0x141, + .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), + }); } + private: void can_receive_callback(const Spec::Can& can, const View::Can& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; - - const auto& can_id = data.can_id; - const auto& can_data = data.can_data; - + auto can_id = data.can_id; if (can == Spec::kCans.kCan0) { - /*^^*/ gimbal_left_friction_.match_then_store_status(can_id, can_data) - || gimbal_right_friction_.match_then_store_status(can_id, can_data) - || gimbal_bullet_feeder_.match_then_store_status(can_id, can_data); - - monitor_.tick("Gimbal::Can0", can_id); - - } else if (can == Spec::kCans.kCan3) { - /*^^*/ if (can_id == 0x141) { - gimbal_top_yaw_motor_.store_status(can_data); - } else if (can_id == 0x142) { - gimbal_pitch_motor_.store_status(can_data); + if (can_id == 0x202) { + gimbal_left_friction_.store_status(data.can_data); + } else if (can_id == 0x201) { + gimbal_right_friction_.store_status(data.can_data); + } else if (can_id == 0x204) { + gimbal_bullet_feeder_.store_status(data.can_data); } - - monitor_.tick("Gimbal::Can3", can_id); - } - } - - void gpio_digital_read_result_callback( - const Spec::Gpio& gpio, const View::GpioDigital& data) override { - if (!data.timestamp_quarter_us) - return; - - if (gpio == Spec::kGpios.kUart1Rx) { - if (data.high) - return; - - const auto timestamp = - board_clock_lifter_.lift_timestamp(*data.timestamp_quarter_us); - if (!timestamp.has_value()) - return; - - camera_signal_output_.emit(*timestamp); + } else if (can == Spec::kCans.kCan2) { + if (can_id == 0x141) + gimbal_pitch_motor_.store_status(data.can_data); + } else if (can == Spec::kCans.kCan3) { + if (can_id == 0x141) + gimbal_top_yaw_motor_.store_status(data.can_data); } } void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { - const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); - bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); - monitor_.tick("Gimbal::Imu", "Acc"); + bmi088_.store_accelerometer_status(data.x, data.y, data.z); } void gyroscope_receive_callback(const View::ImuGyroscope& data) override { - monitor_.tick("Gimbal::Imu", "Gyr"); - const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); - if (!timestamp.has_value()) - return; - - auto snapshot = - bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); - if (!snapshot) - return; - - imu_snapshot_output_.emit(*snapshot); + bmi088_.store_gyroscope_status(data.x, data.y, data.z); } OutputInterface& tf_; @@ -264,33 +232,26 @@ class Sentry OutputInterface gimbal_yaw_velocity_bmi088_; OutputInterface gimbal_pitch_velocity_bmi088_; - EventOutputInterface camera_signal_output_; - EventOutputInterface imu_snapshot_output_; - - device::Bmi088Ekf bmi088_{device::Bmi088Ekf::Config{ - .body_to_sensor = - Eigen::AngleAxisd{std::numbers::pi, Eigen::Vector3d::UnitZ()}.toRotationMatrix(), - }}; - device::BoardClockLifter board_clock_lifter_; - + device::Bmi088 bmi088_; device::LkMotor gimbal_pitch_motor_; device::LkMotor gimbal_top_yaw_motor_; device::DjiMotor gimbal_bullet_feeder_; device::DjiMotor gimbal_left_friction_; device::DjiMotor gimbal_right_friction_; - - StatusMonitor monitor_{}; std::unique_ptr board_; }; - class ChassisBoard final : public librmcs::board::RmcsBoardLite::Callback { + class BottomBoard final : public librmcs::board::CBoard::Callback { + friend class Sentry; + public: - explicit ChassisBoard( + explicit BottomBoard( Sentry& sentry, rmcs_executor::Component& sentry_command, std::string_view board_serial = {}) - : tf_(sentry.tf_) - , dr16_{} + : imu_(1000, 0.2, 0.0) + , tf_(sentry.tf_) + , dr16_(sentry) , gimbal_bottom_yaw_motor_(sentry, sentry_command, "/gimbal/bottom_yaw") , chassis_wheel_motors_( {sentry, sentry_command, "/chassis/left_front_wheel"}, @@ -302,19 +263,8 @@ class Sentry {sentry, sentry_command, "/chassis/left_back_steering"}, {sentry, sentry_command, "/chassis/right_back_steering"}, {sentry, sentry_command, "/chassis/right_front_steering"}) - , chassis_front_climber_motor_( - {sentry, sentry_command, "/chassis/climber/left_front_motor"}, - {sentry, sentry_command, "/chassis/climber/right_front_motor"}) - , chassis_back_climber_motor_( - {sentry, sentry_command, "/chassis/climber/left_back_motor"}, - {sentry, sentry_command, "/chassis/climber/right_back_motor"}) , supercap_(sentry, sentry_command) { - - using namespace device; - sentry.register_output("/referee/serial", referee_serial_); - sentry.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0.0); - sentry.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); referee_serial_->read = [this](std::byte* buffer, size_t size) { return referee_ring_buffer_receive_.pop_front_n( @@ -322,61 +272,48 @@ class Sentry }; referee_serial_->write = [this](const std::byte* buffer, size_t size) { board_->start_transmit().uart_transmit( - Spec::kUarts.kUart0, {.uart_data = std::span{buffer, size}}); + Spec::kUarts.kUart1, + {.uart_data = std::span{buffer, size}}); return size; }; const auto zero_point = sentry.get_parameter("bottom_yaw_motor_zero_point").as_int(); gimbal_bottom_yaw_motor_.configure( - LkMotor::Config{LkMotor::Type::kMG6012Ei8}.set_reversed().set_encoder_zero_point( - static_cast(zero_point))); + device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} + .set_reversed() + .set_encoder_zero_point(static_cast(zero_point))); - constexpr auto kWheelIds = std::array{2, 1, 2, 4}; - for (auto&& [motor, id] : std::views::zip(chassis_wheel_motors_, kWheelIds)) { + for (auto& motor : chassis_wheel_motors_) { motor.configure( - DjiMotor::Config{DjiMotor::Type::kM3508, id} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reduction_ratio(11.) .enable_multi_turn_angle() .set_reversed()); } - constexpr auto kSteerIds = std::array{2, 1, 1, 2}; - for (auto&& [motor, name, id] : - std::views::zip(chassis_steer_motors_, kPosition, kSteerIds)) { - const auto zero_point = - sentry.get_parameter(std::string{name} + "_zero_point").as_int(); + constexpr auto kSteerNames = std::array{ + "right_back_zero_point", + "right_front_zero_point", + "left_front_zero_point", + "left_back_zero_point", + }; + for (auto&& [motor, name] : std::views::zip(chassis_steer_motors_, kSteerNames)) { + const auto zero_point = sentry.get_parameter(name).as_int(); motor.configure( - DjiMotor::Config{DjiMotor::Type::kGM6020, id} + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} .set_reversed() .set_encoder_zero_point(static_cast(zero_point)) .enable_multi_turn_angle()); } - chassis_front_climber_motor_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1}.set_reduction_ratio( - 19.)); - chassis_front_climber_motor_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2} - .set_reversed() - .set_reduction_ratio(19.)); - chassis_back_climber_motor_[0].configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} - .set_reversed() - .enable_multi_turn_angle()); - chassis_back_climber_motor_[1].configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} - .enable_multi_turn_angle()); - - board_ = std::make_unique(*this, board_serial); + sentry.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - sentry.remote_control_->register_dr16(&dr16_); + board_ = std::make_unique(*this, board_serial); } - auto status() const -> std::vector { return monitor_.text(); } - - void update() { - gimbal_bottom_yaw_motor_.update_status(); - dr16_.update_status(); + auto update() -> void { + imu_.update_status(); + *chassis_yaw_velocity_imu_ = imu_.gz(); supercap_.update_status(); for (auto& motor : chassis_wheel_motors_) @@ -384,194 +321,110 @@ class Sentry for (auto& motor : chassis_steer_motors_) motor.update_status(); - chassis_front_climber_motor_[0].update_status(); - chassis_front_climber_motor_[1].update_status(); - chassis_back_climber_motor_[0].update_status(); - chassis_back_climber_motor_[1].update_status(); - + dr16_.update_status(); + gimbal_bottom_yaw_motor_.update_status(); tf_->set_state( gimbal_bottom_yaw_motor_.angle()); - - if (const auto snapshot = bmi088_.snapshot()) { - const auto& q = snapshot->orientation; - *chassis_pitch_imu_ = -std::asin(2.0 * (q.w() * q.y() - q.z() * q.x())); - *chassis_yaw_velocity_imu_ = snapshot->gyro_body.z(); - } } - void command_update() { + auto command_update() -> void { using namespace device; + auto builder = board_->start_transmit(); + builder.can_transmit(Spec::kCans.kCan1, { + .can_id = 0x141, + .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), + }); + auto cache = CanPacket8{}; - auto generate = [&](const auto& motors, // - CanPacket8::Quarter extra = {}, std::size_t slot = 4) { - auto slots = std::array{}; - slots.fill(CanPacket8::PaddingQuarter{}); - std::uint32_t can_id = 0; - for (auto& motor : motors) { - slots[(motor.id() - 1) % 4] = motor.generate_command(); - if (can_id == 0) - can_id = motor.send_id(); - } - if (slot < 4) - slots[slot] = extra; - cache = CanPacket8{slots[0], slots[1], slots[2], slots[3]}; - return librmcs::data::CanDataView{.can_id = can_id, .can_data = cache.as_bytes()}; + auto generate = [&](std::uint32_t id, std::ranges::range auto& motors, auto... args) { + auto command = [&](T arg) { + if constexpr (std::same_as) { + return arg; + } else { + const auto valid = arg >= 0 && arg < 4; + return valid ? motors[arg].generate_command() + : CanPacket8::PaddingQuarter{}; + } + }; + cache = CanPacket8{command(args)...}; + return librmcs::data::CanDataView{.can_id = id, .can_data = cache.as_bytes()}; }; - auto supercap_package = CanPacket8{ - CanPacket8::PaddingQuarter{}, - CanPacket8::PaddingQuarter{}, - CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - }; - auto bottom_yaw_package = gimbal_bottom_yaw_motor_.generate_command(); - - board_->start_transmit() - .can_transmit( - Spec::kCans.kCan0, - { - .can_id = 0x142, - .can_data = chassis_back_climber_motor_[0].generate_command().as_bytes(), - }) - .can_transmit( - Spec::kCans.kCan0, - { - .can_id = 0x143, - .can_data = chassis_back_climber_motor_[1].generate_command().as_bytes(), - }) - - .can_transmit( - Spec::kCans.kCan1, generate(std::views::counted(chassis_wheel_motors_, 2))) - .can_transmit( - Spec::kCans.kCan2, generate(std::views::counted(chassis_wheel_motors_ + 2, 2))) - - .can_transmit( - Spec::kCans.kCan1, generate(std::views::counted(chassis_steer_motors_, 2))) - .can_transmit( - Spec::kCans.kCan2, generate(std::views::counted(chassis_steer_motors_ + 2, 2))) - - .can_transmit( - Spec::kCans.kCan3, {.can_id = 0x1fe, .can_data = supercap_package.as_bytes()}) - .can_transmit( - Spec::kCans.kCan3, - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_front_climber_motor_[0].generate_command(), - chassis_front_climber_motor_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }) - .can_transmit( - Spec::kCans.kCan3, - {.can_id = 0x141, .can_data = bottom_yaw_package.as_bytes()}); + if (can_transmission_mode_) { + builder.can_transmit(Spec::kCans.kCan1, generate(0x200, chassis_wheel_motors_, 1, 0, -1, -1)) + .can_transmit(Spec::kCans.kCan2, generate(0x200, chassis_wheel_motors_, -1, 2, -1, 3)); + } else { + builder.can_transmit(Spec::kCans.kCan1, generate(0x1FE, chassis_steer_motors_, 1, 0, -1, -1)) + .can_transmit(Spec::kCans.kCan2, generate( + 0x1FE, chassis_steer_motors_, 2, 3, -1, supercap_.generate_command())); + } + can_transmission_mode_ = !can_transmission_mode_; } + private: void can_receive_callback(const Spec::Can& can, const View::Can& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; - - const auto& can_id = data.can_id; - const auto& can_data = data.can_data; - - if (can == Spec::kCans.kCan0) { - if (can_id == 0x142) { - chassis_back_climber_motor_[0].store_status(data.can_data); - } else if (can_id == 0x143) { - chassis_back_climber_motor_[1].store_status(data.can_data); - } - - monitor_.tick("Chassis::Can0", can_id); - - } else if (can == Spec::kCans.kCan1) { - /*^^*/ chassis_wheel_motors_[0].match_then_store_status(can_id, can_data) - || chassis_wheel_motors_[1].match_then_store_status(can_id, can_data) - - || chassis_steer_motors_[0].match_then_store_status(can_id, can_data) - || chassis_steer_motors_[1].match_then_store_status(can_id, can_data); - - monitor_.tick("Chassis::Can1", can_id); - - } else if (can == Spec::kCans.kCan2) { - /*^^*/ chassis_wheel_motors_[2].match_then_store_status(can_id, can_data) - || chassis_wheel_motors_[3].match_then_store_status(can_id, can_data) - - || chassis_steer_motors_[2].match_then_store_status(can_id, can_data) - || chassis_steer_motors_[3].match_then_store_status(can_id, can_data); - - monitor_.tick("Chassis::Can2", can_id); - - } else if (can == Spec::kCans.kCan3) { - if (can_id == 0x300) { - supercap_.store_status(can_data); - } else if (can_id == 0x141) { + auto can_id = data.can_id; + if (can == Spec::kCans.kCan1) { + if (can_id == 0x201) + chassis_wheel_motors_[1].store_status(data.can_data); + else if (can_id == 0x202) + chassis_wheel_motors_[0].store_status(data.can_data); + else if (can_id == 0x205) + chassis_steer_motors_[1].store_status(data.can_data); + else if (can_id == 0x206) + chassis_steer_motors_[0].store_status(data.can_data); + else if (can_id == 0x141) gimbal_bottom_yaw_motor_.store_status(data.can_data); - } else { - /*^^*/ chassis_front_climber_motor_[0].match_then_store_status(can_id, can_data) - || chassis_front_climber_motor_[1].match_then_store_status( - can_id, can_data); - } - - monitor_.tick("Chassis::Can3", can_id); + } else if (can == Spec::kCans.kCan2) { + if (can_id == 0x202) + chassis_wheel_motors_[2].store_status(data.can_data); + else if (can_id == 0x204) + chassis_wheel_motors_[3].store_status(data.can_data); + else if (can_id == 0x205) + chassis_steer_motors_[2].store_status(data.can_data); + else if (can_id == 0x206) + chassis_steer_motors_[3].store_status(data.can_data); + else if (can_id == 0x300) + supercap_.store_status(data.can_data); } } void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { if (uart == Spec::kUarts.kDbus) { dr16_.store_status(data.uart_data.data(), data.uart_data.size()); - monitor_.tick("Chassis::Dbus", "Active"); - } else if (uart == Spec::kUarts.kUart0) { + } else if (uart == Spec::kUarts.kUart1) { const auto* uart_data = data.uart_data.data(); referee_ring_buffer_receive_.emplace_back_n( [&uart_data](std::byte* storage) noexcept { *storage = *uart_data++; }, data.uart_data.size()); - monitor_.tick("Chassis::Uart0", "Active"); } } void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { - const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); - bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); - monitor_.tick("Chassis::Imu", "Acc"); + imu_.store_accelerometer_status(data.x, data.y, data.z); } void gyroscope_receive_callback(const View::ImuGyroscope& data) override { - monitor_.tick("Chassis::Imu", "Gyr"); - const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); - if (!timestamp.has_value()) - return; - - auto snapshot = - bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); - if (!snapshot) - return; + imu_.store_gyroscope_status(data.x, data.y, data.z); } - device::Bmi088Ekf bmi088_; - device::BoardClockLifter board_clock_lifter_; + bool can_transmission_mode_ = true; + device::Bmi088 imu_; OutputInterface& tf_; device::Dr16 dr16_; device::LkMotor gimbal_bottom_yaw_motor_; device::DjiMotor chassis_wheel_motors_[4]; device::DjiMotor chassis_steer_motors_[4]; - - device::DjiMotor chassis_front_climber_motor_[2]; - device::LkMotor chassis_back_climber_motor_[2]; - device::Supercap supercap_; rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; OutputInterface referee_serial_; OutputInterface chassis_yaw_velocity_imu_; - OutputInterface chassis_pitch_imu_; - - StatusMonitor monitor_{}; - std::unique_ptr board_; + std::unique_ptr board_; }; struct CommandTransmitter : public rmcs_executor::Component { @@ -581,11 +434,11 @@ class Sentry explicit CommandTransmitter(Fn&& fn) : fn{std::forward(fn)} {} - void update() override { fn(); } + auto update() -> void override { fn(); } }; - void - status_service_callback(const std::shared_ptr& response) { + auto status_service_callback(const std::shared_ptr& response) + -> void { response->success = true; auto feedback_message = std::ostringstream{}; @@ -593,36 +446,28 @@ class Sentry std::println(feedback_message, format, std::forward(args)...); }; - text( - " bottom_yaw_motor_zero_point: {}", - chassis_board_->gimbal_bottom_yaw_motor_.last_raw_angle()); - text(" pitch_motor_zero_point: {}", gimbal_board_->gimbal_pitch_motor_.last_raw_angle()); - text( - " top_yaw_motor_zero_point: {}", - gimbal_board_->gimbal_top_yaw_motor_.last_raw_angle()); - - text(""); - for (auto&& [index, motor] : - std::views::zip(kPosition, chassis_board_->chassis_steer_motors_)) { - text(" {}_zero_point: {}", index, motor.last_raw_angle()); - } + text("Gimbal Status"); + text("- Bottom Yaw: {}", bottom_board_->gimbal_bottom_yaw_motor_.last_raw_angle()); + text("- Top Yaw: {}", top_board_->gimbal_top_yaw_motor_.last_raw_angle()); + text("- Pitch Angle: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); - text("\nGimbalBoard Status:"); - for (const auto& line : gimbal_board_->status()) { - text("> {}", line); - } + text("Chassis Status"); + constexpr auto kPosition = + std::array{"right back", "right front", "left front", "left back"}; + constexpr auto kMaxLength = + std::ranges::max_element(kPosition, {}, &std::string_view::size)->size(); - text("\nChassisBoard Status:"); - for (const auto& line : chassis_board_->status()) { - text("> {}", line); + for (auto&& [index, motor] : + std::views::zip(kPosition, bottom_board_->chassis_steer_motors_)) { + text("- {:{}}: {}", index, kMaxLength, motor.last_raw_angle()); } response->message = feedback_message.str(); } - void command_update() { - gimbal_board_->command_update(); - chassis_board_->command_update(); + auto command_update() -> void { + top_board_->command_update(); + bottom_board_->command_update(); } std::shared_ptr command_component_{ create_partner_component( @@ -631,14 +476,9 @@ class Sentry InputInterface timestamp_; OutputInterface tf_; - OutputInterface camera_transform_; - OutputInterface barrel_direction_; - OutputInterface yaw_velocity_; - OutputInterface chassis_measure_yaw_; - std::unique_ptr gimbal_board_; - std::unique_ptr chassis_board_; - std::unique_ptr remote_control_; + std::unique_ptr top_board_; + std::unique_ptr bottom_board_; std::shared_ptr> status_service_; }; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp index b216fa6eb..98851e89d 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp @@ -7,12 +7,12 @@ #include #include #include -#include #include #include #include #include +#include #include #include #include @@ -23,22 +23,16 @@ #include #include #include -#include -#include #include #include #include #include "hardware/device/bmi088.hpp" -#include "hardware/device/bmi088_ekf.hpp" -#include "hardware/device/board_clock_lifter.hpp" #include "hardware/device/can_packet.hpp" #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" -#include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" -#include "hardware/device/vt13.hpp" namespace rmcs_core::hardware { @@ -126,18 +120,11 @@ class SteeringHeroLittle register_output("/tf", tf_); - register_output( - "/auto_aim/camera_transform", camera_transform_, Eigen::Isometry3d::Identity()); - register_output("/auto_aim/barrel_direction", barrel_direction_, Eigen::Vector3d::UnitX()); - register_output("/auto_aim/yaw_velocity", auto_aim_yaw_velocity_, 0.0); - gimbal_calibrate_subscription_ = create_subscription( "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { gimbal_calibrate_subscription_callback(std::move(msg)); }); - remote_control_ = std::make_unique(*this); - top_board_ = std::make_unique( *this, *command_component_, get_parameter("board_serial_top_board").as_string()); @@ -145,7 +132,7 @@ class SteeringHeroLittle *this, *command_component_, get_parameter("serial_bottom_rmcs_board").as_string()); tf_->set_transform( - Eigen::Translation3d{0.22, 0.0, -0.05}); + Eigen::Translation3d{0.06603, 0.0, 0.082}); } SteeringHeroLittle(const SteeringHeroLittle&) = delete; @@ -158,19 +145,12 @@ class SteeringHeroLittle void update() override { top_board_->update(); bottom_board_->update(); - remote_control_->update(); tf_->set_state( bottom_board_->gimbal_bottom_yaw_motor_.angle() + top_board_->gimbal_top_yaw_motor_.angle()); tf_->set_state( top_board_->gimbal_pitch_motor_.angle()); - - using namespace rmcs_description; - *camera_transform_ = fast_tf::lookup_transform(*tf_); - *barrel_direction_ = - *fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); - *auto_aim_yaw_velocity_ = top_board_->gimbal_yaw_velocity(); } void command_update() { @@ -220,25 +200,26 @@ class SteeringHeroLittle }; std::shared_ptr command_component_; - struct TopBoard final : librmcs::board::RmcsBoardLite::Callback { + class TopBoard final : public librmcs::board::RmcsBoardLite::Callback { + public: + friend class SteeringHeroLittle; explicit TopBoard( SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, std::string_view board_serial = {}) : logger_(steering_hero.get_logger()) + // , can0_receive_rate_counter_(logger_, "bottom/can0") + // , can1_receive_rate_counter_(logger_, "bottom/can1") + // , can2_receive_rate_counter_(logger_, "bottom/can2") + // , can3_receive_rate_counter_(logger_, "bottom/can3") , tf_(steering_hero.tf_) - , bmi088_{device::Bmi088Ekf::Config{ - .body_to_sensor = - Eigen::AngleAxisd{std::numbers::pi / 2.0, Eigen::Vector3d::UnitZ()} - .toRotationMatrix()}} + , imu_(1000, 0.2, 0.0) , gimbal_top_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/top_yaw") , gimbal_pitch_motor_(steering_hero, steering_hero_command, "/gimbal/pitch") , gimbal_friction_wheels_( - {steering_hero, steering_hero_command, "/gimbal/first_front_friction"}, - {steering_hero, steering_hero_command, "/gimbal/first_back_friction"}, - {steering_hero, steering_hero_command, "/gimbal/second_front_friction"}, - {steering_hero, steering_hero_command, "/gimbal/second_back_friction"}, - {steering_hero, steering_hero_command, "/gimbal/third_front_friction"}, - {steering_hero, steering_hero_command, "/gimbal/third_back_friction"}) + {steering_hero, steering_hero_command, "/gimbal/first_right_friction"}, + {steering_hero, steering_hero_command, "/gimbal/first_left_friction"}, + {steering_hero, steering_hero_command, "/gimbal/second_right_friction"}, + {steering_hero, steering_hero_command, "/gimbal/second_left_friction"}) , gimbal_bullet_feeder_(steering_hero, steering_hero_command, "/gimbal/bullet_feeder") , putter_motor_(steering_hero, steering_hero_command, "/gimbal/putter") , gimbal_scope_motor_(steering_hero, steering_hero_command, "/gimbal/scope") @@ -258,25 +239,15 @@ class SteeringHeroLittle steering_hero.get_parameter("pitch_motor_zero_point").as_int())) .enable_multi_turn_angle()); gimbal_friction_wheels_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} - .set_reversed() - .set_reduction_ratio(1.)); + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); gimbal_friction_wheels_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reversed() .set_reduction_ratio(1.)); gimbal_friction_wheels_[2].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 3}.set_reduction_ratio( - 1.)); + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); gimbal_friction_wheels_[3].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 4}.set_reduction_ratio( - 1.)); - gimbal_friction_wheels_[4].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} - .set_reversed() - .set_reduction_ratio(1.)); - gimbal_friction_wheels_[5].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reversed() .set_reduction_ratio(1.)); gimbal_bullet_feeder_.configure( @@ -287,11 +258,10 @@ class SteeringHeroLittle .set_reversed() .enable_multi_turn_angle()); putter_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 3} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reduction_ratio(1.) .enable_multi_turn_angle()); - gimbal_scope_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006, 4}); + gimbal_scope_motor_.configure(device::DjiMotor::Config{device::DjiMotor::Type::kM2006}); gimbal_player_viewer_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} .set_encoder_zero_point( @@ -303,37 +273,47 @@ class SteeringHeroLittle steering_hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); steering_hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); - steering_hero.register_output( - "/gimbal/auto_aim/exposure_signal", camera_signal_output_); - steering_hero.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); - steering_hero.register_output( "/gimbal/photoelectric_sensor", photoelectric_sensor_status_, false); steering_hero.register_output( "/gimbal/grayscale_sensor", grayscale_sensor_status_, false); + steering_hero.register_output( + "/auto_aim/image_capturer/timestamp", camera_capturer_trigger_timestamp_, 0); + steering_hero.register_output( + "/auto_aim/image_capturer/trigger", camera_capturer_trigger_, 0); - board_ = std::make_unique(*this, board_serial); + imu_.set_coordinate_mapping([](double x, double y, double z) { + // Get the mapping with the following code. + // The rotation angle must be an exact multiple of 90 degrees, otherwise + // use a matrix. - board_->start_transmit().uart_config(Spec::kUarts.kUart0, {.baudrate = 921600}); + return std::make_tuple(-y, x, z); + }); - steering_hero.remote_control_->register_vt13(&vt13_); + board_ = std::make_unique(*this, board_serial); } + TopBoard(const TopBoard&) = delete; + TopBoard& operator=(const TopBoard&) = delete; + TopBoard(TopBoard&&) = delete; + TopBoard& operator=(TopBoard&&) = delete; + + ~TopBoard() final = default; + void update() { // can0_receive_rate_counter_.report_if_due(); // can1_receive_rate_counter_.report_if_due(); // can2_receive_rate_counter_.report_if_due(); // can3_receive_rate_counter_.report_if_due(); - vt13_.update_status(); + imu_.update_status(); + Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; - if (auto snapshot = bmi088_.snapshot()) { - tf_->set_transform( - snapshot->orientation.conjugate()); + tf_->set_transform( + gimbal_imu_pose.conjugate()); - *gimbal_yaw_velocity_imu_ = snapshot->gyro_body.z(); - *gimbal_pitch_velocity_imu_ = snapshot->gyro_body.y(); - } + *gimbal_yaw_velocity_imu_ = imu_.gz(); + *gimbal_pitch_velocity_imu_ = imu_.gy(); gimbal_top_yaw_motor_.update_status(); gimbal_pitch_motor_.update_status(); @@ -352,123 +332,101 @@ class SteeringHeroLittle gimbal_scope_motor_.update_status(); + if (last_camera_capturer_trigger_timestamp_ != *camera_capturer_trigger_timestamp_) + *camera_capturer_trigger_ = true; + last_camera_capturer_trigger_timestamp_ = *camera_capturer_trigger_timestamp_; + *photoelectric_sensor_status_ = photoelectric_sensor_status_atomic.load(); *grayscale_sensor_status_ = grayscale_sensor_status_atomic.load(); - - if (++count_ == 250) { - for (int i = 0; i < 6; ++i) { - if (friciton_detect[i] == 0) { - RCLCPP_WARN(logger_, "friction can id 0x%03X missing", i + 0x201); - } - } - std::fill_n(friciton_detect, 6, 0); - for (int i = 0; i < 3; ++i) { - if (can0_detect[i] == 0) { - RCLCPP_WARN(logger_, "top board can id 0x%03X missing", i + 0x141); - } - } - std::fill_n(can0_detect, 3, 0); - count_ = 0; - } } void command_update() { auto builder = board_->start_transmit(); if (std::isfinite(gimbal_pitch_motor_.control_angle())) - builder.can_transmit( - Spec::kCans.kCan0, // - { - .can_id = 0x143, - .can_data = gimbal_pitch_motor_ - .generate_angle_command(gimbal_pitch_motor_.control_angle()) - .as_bytes(), - }); - else - builder.can_transmit( - Spec::kCans.kCan0, // - { - .can_id = 0x143, - .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), - }); // Used to distinguish pitch encoder control from IMU control. - - builder.can_transmit( - Spec::kCans.kCan0, // - { - .can_id = 0x141, - .can_data = gimbal_top_yaw_motor_.generate_command().as_bytes(), - }); - - builder.can_transmit( - Spec::kCans.kCan0, // - { + builder.can_transmit(Spec::kCans.kCan0, { .can_id = 0x142, - .can_data = gimbal_bullet_feeder_.generate_torque_command().as_bytes(), + .can_data = gimbal_pitch_motor_ + .generate_angle_command(gimbal_pitch_motor_.control_angle()) + .as_bytes(), }); + else + builder.can_transmit(Spec::kCans.kCan0, { + .can_id = 0x142, + .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), + }); // Used to distinguish pitch encoder control from IMU control. + + builder.can_transmit(Spec::kCans.kCan0, { + .can_id = 0x141, + .can_data = gimbal_top_yaw_motor_.generate_command().as_bytes(), + }); + + builder.can_transmit(Spec::kCans.kCan1, { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_friction_wheels_[0].generate_command(), + gimbal_friction_wheels_[1].generate_command(), + gimbal_friction_wheels_[2].generate_command(), + gimbal_friction_wheels_[3].generate_command(), + } + .as_bytes(), + }); + + builder.can_transmit(Spec::kCans.kCan1, { + .can_id = 0x1FF, + .can_data = + device::CanPacket8{ + putter_motor_.generate_command(), + gimbal_scope_motor_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); - builder.can_transmit( - Spec::kCans.kCan1, // + builder.can_transmit(Spec::kCans.kCan2, { + .can_id = 0x143, + .can_data = + gimbal_player_viewer_motor_ + .generate_velocity_command(gimbal_player_viewer_motor_.control_velocity()) + .as_bytes(), + }); + + builder.can_transmit(Spec::kCans.kCan3, { + .can_id = 0x142, + .can_data = gimbal_bullet_feeder_.generate_torque_command().as_bytes(), + }); + + builder.gpio_digital_read( + Spec::kGpios[2], { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_friction_wheels_[0].generate_command(), - gimbal_friction_wheels_[1].generate_command(), - gimbal_friction_wheels_[2].generate_command(), - gimbal_friction_wheels_[3].generate_command(), - } - .as_bytes(), + .period_ms = 20, + .pull = librmcs::data::GpioPull::kUp, }); - builder.can_transmit( - Spec::kCans.kCan2, // + builder.gpio_digital_read( + Spec::kGpios[3], { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_friction_wheels_[4].generate_command(), - gimbal_friction_wheels_[5].generate_command(), - putter_motor_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), + .period_ms = 20, + .pull = librmcs::data::GpioPull::kUp, }); - - builder - .gpio_digital_read( - Spec::kGpios.kUart1Rx, - { - .period_ms = 20, - .pull = librmcs::data::GpioPull::kUp, - }) - .gpio_digital_read( - Spec::kGpios.kUart1Tx, { - .period_ms = 0, - .asap = false, - .rising_edge = false, - .falling_edge = true, - .capture_timestamp = true, - .pull = librmcs::data::GpioPull::kUp, - }); } + private: void can_receive_callback(const Spec::Can& can, const View::Can& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; auto can_id = data.can_id; if (can == Spec::kCans.kCan0) { // can0_receive_rate_counter_.record(can_id); - can0_detect[can_id - 0x141] = 1; if (can_id == 0x141) { gimbal_top_yaw_motor_.store_status(data.can_data); - } else if (can_id == 0x143) { - gimbal_pitch_motor_.store_status(data.can_data); } else if (can_id == 0x142) { - gimbal_bullet_feeder_.store_status(data.can_data); + gimbal_pitch_motor_.store_status(data.can_data); } } else if (can == Spec::kCans.kCan1) { // can1_receive_rate_counter_.record(can_id); - friciton_detect[can_id - 0x201] = 1; if (can_id == 0x201) { gimbal_friction_wheels_[0].store_status(data.can_data); } else if (can_id == 0x202) { @@ -477,65 +435,41 @@ class SteeringHeroLittle gimbal_friction_wheels_[2].store_status(data.can_data); } else if (can_id == 0x204) { gimbal_friction_wheels_[3].store_status(data.can_data); + } else if (can_id == 0x205) { + putter_motor_.store_status(data.can_data); + } else if (can_id == 0x206) { + gimbal_scope_motor_.store_status(data.can_data); } } else if (can == Spec::kCans.kCan2) { // can2_receive_rate_counter_.record(can_id); - if (can_id == 0x203) { - putter_motor_.store_status(data.can_data); - } else if (can_id == 0x201) { - gimbal_friction_wheels_[4].store_status(data.can_data); - friciton_detect[4] = 1; - } else if (can_id == 0x202) { - gimbal_friction_wheels_[5].store_status(data.can_data); - friciton_detect[5] = 1; + if (can_id == 0x143) { + gimbal_player_viewer_motor_.store_status(data.can_data); + } + } else if (can == Spec::kCans.kCan3) { + // can3_receive_rate_counter_.record(can_id); + if (can_id == 0x142) { + gimbal_bullet_feeder_.store_status(data.can_data); } - } - } - - void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { - if (uart == Spec::kUarts.kUart0) { - vt13_.store_status(data.uart_data); } } void gpio_digital_read_result_callback( const Spec::Gpio& gpio, const View::GpioDigital& data) override { - - /* */ if (gpio == Spec::kGpios.kUart1Rx) { + if (gpio.channel_index == 2) { photoelectric_sensor_status_atomic.store(data.high); - } else if (gpio == Spec::kGpios.kUart1Tx) { - if (!data.timestamp_quarter_us) - return; - const auto timestamp = - board_clock_lifter_.lift_timestamp(*data.timestamp_quarter_us); - if (!timestamp.has_value()) - return; - camera_signal_output_.emit(*timestamp); + } else if (gpio.channel_index == 3) { + grayscale_sensor_status_atomic.store(!data.high); } } void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { - const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); - bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); + imu_.store_accelerometer_status(data.x, data.y, data.z); } void gyroscope_receive_callback(const View::ImuGyroscope& data) override { - const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); - if (!timestamp.has_value()) - return; - - if (auto snapshot = - bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp)) - imu_snapshot_output_.emit(*snapshot); - } - - [[nodiscard]] auto gimbal_yaw_velocity() const -> double { - return *gimbal_yaw_velocity_imu_; + imu_.store_gyroscope_status(data.x, data.y, data.z); } - [[nodiscard]] device::Vt13& vt13() noexcept { return vt13_; } - [[nodiscard]] const device::Vt13& vt13() const noexcept { return vt13_; } - rclcpp::Logger logger_; // CanReceiveRateCounter can0_receive_rate_counter_; // CanReceiveRateCounter can1_receive_rate_counter_; @@ -543,16 +477,12 @@ class SteeringHeroLittle // CanReceiveRateCounter can3_receive_rate_counter_; OutputInterface& tf_; - int count_ = 0; - int friciton_detect[6]; - int can0_detect[3]; + std::time_t last_camera_capturer_trigger_timestamp_{0}; - device::Bmi088Ekf bmi088_; - device::BoardClockLifter board_clock_lifter_; - device::Vt13 vt13_; + device::Bmi088 imu_; device::LkMotor gimbal_top_yaw_motor_; device::LkMotor gimbal_pitch_motor_; - device::DjiMotor gimbal_friction_wheels_[6]; + device::DjiMotor gimbal_friction_wheels_[4]; device::LkMotor gimbal_bullet_feeder_; device::DjiMotor putter_motor_; device::DjiMotor gimbal_scope_motor_; @@ -562,15 +492,16 @@ class SteeringHeroLittle OutputInterface gimbal_pitch_velocity_imu_; OutputInterface photoelectric_sensor_status_; OutputInterface grayscale_sensor_status_; - EventOutputInterface camera_signal_output_; - EventOutputInterface imu_snapshot_output_; + OutputInterface camera_capturer_trigger_; + OutputInterface camera_capturer_trigger_timestamp_; std::atomic photoelectric_sensor_status_atomic{false}; std::atomic grayscale_sensor_status_atomic{false}; - std::unique_ptr board_; }; - struct BottomBoard final : librmcs::board::RmcsBoardLite::Callback { + class BottomBoard final : public librmcs::board::RmcsBoardLite::Callback { + public: + friend class SteeringHeroLittle; explicit BottomBoard( SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, std::string_view board_serial = {}) @@ -580,7 +511,7 @@ class SteeringHeroLittle // , can2_receive_rate_counter_(logger_, "bottom/can2") // , can3_receive_rate_counter_(logger_, "bottom/can3") , imu_(1000, 0.2, 0.0) - , dr16_{} + , dr16_(steering_hero) , supercap_(steering_hero, steering_hero_command) , chassis_steering_motors_( {steering_hero, steering_hero_command, "/chassis/left_front_steering"}, @@ -598,70 +529,65 @@ class SteeringHeroLittle , chassis_back_climber_motor_( {steering_hero, steering_hero_command, "/chassis/climber/left_back_motor"}, {steering_hero, steering_hero_command, "/chassis/climber/right_back_motor"}) - , yaw_brake_motor_(steering_hero, steering_hero_command, "/gimbal/yaw_brake") , gimbal_bottom_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/bottom_yaw") { // chassis_steering_motors_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020, 4} + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} .set_encoder_zero_point( static_cast( steering_hero.get_parameter("left_front_zero_point").as_int())) .set_reversed()); chassis_steering_motors_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020, 1} + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} .set_encoder_zero_point( static_cast( steering_hero.get_parameter("right_front_zero_point").as_int())) .set_reversed()); chassis_steering_motors_[2].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020, 3} + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} .set_encoder_zero_point( static_cast( steering_hero.get_parameter("left_back_zero_point").as_int())) .set_reversed()); chassis_steering_motors_[3].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020, 2} + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} .set_encoder_zero_point( static_cast( steering_hero.get_parameter("right_back_zero_point").as_int())) .set_reversed()); chassis_wheel_motors_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 4} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reversed() .set_reduction_ratio(2232. / 169.)); chassis_wheel_motors_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reversed() .set_reduction_ratio(2232. / 169.)); chassis_wheel_motors_[2].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 3} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reversed() .set_reduction_ratio(2232. / 169.)); chassis_wheel_motors_[3].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 4} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reversed() .set_reduction_ratio(2232. / 169.)); chassis_front_climber_motor_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reversed() .set_reduction_ratio(19.)); chassis_front_climber_motor_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2}.set_reduction_ratio( - 19.)); + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); chassis_back_climber_motor_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .enable_multi_turn_angle() .set_reduction_ratio(19.)); chassis_back_climber_motor_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 4} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reversed() .enable_multi_turn_angle() .set_reduction_ratio(19.)); - yaw_brake_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006, 3}.set_reduction_ratio( - 1.)); gimbal_bottom_yaw_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} .set_reversed() @@ -677,7 +603,8 @@ class SteeringHeroLittle }; referee_serial_->write = [this](const std::byte* buffer, size_t size) { board_->start_transmit().uart_transmit( - Spec::kUarts.kUart0, {.uart_data = std::span{buffer, size}}); + Spec::kUarts.kUart0, + {.uart_data = std::span{buffer, size}}); return size; }; steering_hero.register_output( @@ -689,11 +616,18 @@ class SteeringHeroLittle "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); steering_hero.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); - board_ = std::make_unique(*this, board_serial); - - steering_hero.remote_control_->register_dr16(&dr16_); + board_ = std::make_unique( + *this, board_serial, + librmcs::board::AdvancedOptions{}); } + BottomBoard(const BottomBoard&) = delete; + BottomBoard& operator=(const BottomBoard&) = delete; + BottomBoard(BottomBoard&&) = delete; + BottomBoard& operator=(BottomBoard&&) = delete; + + ~BottomBoard() final = default; + void update() { // can0_receive_rate_counter_.report_if_due(); // can1_receive_rate_counter_.report_if_due(); @@ -717,122 +651,97 @@ class SteeringHeroLittle for (auto& motor : chassis_steering_motors_) motor.update_status(); - yaw_brake_motor_.update_status(); gimbal_bottom_yaw_motor_.update_status(); - - if (++count_ == 250) { - for (int i = 0; i < 8; ++i) { - if (check[i] == 0) { - RCLCPP_WARN(logger_, "bottom board can id 0x%03X missing", i + 0x201); - } - } - std::fill_n(check, 8, 0); - count_ = 0; - } } void command_update() { auto builder = board_->start_transmit(); - builder.can_transmit( - Spec::kCans.kCan0, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), - chassis_wheel_motors_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); + builder.can_transmit(Spec::kCans.kCan0, { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[0].generate_command(), + chassis_wheel_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); - builder.can_transmit( - Spec::kCans.kCan0, // - { - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steering_motors_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - chassis_steering_motors_[0].generate_command(), - } - .as_bytes(), - }); + builder.can_transmit(Spec::kCans.kCan0, { + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + chassis_steering_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + chassis_steering_motors_[0].generate_command(), + } + .as_bytes(), + }); - builder.can_transmit( - Spec::kCans.kCan1, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - chassis_wheel_motors_[2].generate_command(), - chassis_wheel_motors_[3].generate_command(), - } - .as_bytes(), - }); + builder.can_transmit(Spec::kCans.kCan1, { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + chassis_wheel_motors_[2].generate_command(), + chassis_wheel_motors_[3].generate_command(), + } + .as_bytes(), + }); - builder.can_transmit( - Spec::kCans.kCan1, // - { - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - chassis_steering_motors_[3].generate_command(), - chassis_steering_motors_[2].generate_command(), - supercap_.generate_command(), - } - .as_bytes(), - }); + builder.can_transmit(Spec::kCans.kCan1, { + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + chassis_steering_motors_[3].generate_command(), + chassis_steering_motors_[2].generate_command(), + supercap_.generate_command(), + } + .as_bytes(), + }); - builder.can_transmit( - Spec::kCans.kCan3, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - chassis_back_climber_motor_[1].generate_command(), - yaw_brake_motor_.generate_command(), - chassis_back_climber_motor_[0].generate_command(), - } - .as_bytes(), - }); + builder.can_transmit(Spec::kCans.kCan3, { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + chassis_back_climber_motor_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + chassis_back_climber_motor_[0].generate_command(), + } + .as_bytes(), + }); - builder.can_transmit( - Spec::kCans.kCan3, // - { - .can_id = 0x141, - .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), - }); + builder.can_transmit(Spec::kCans.kCan3, { + .can_id = 0x141, + .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), + }); - builder.can_transmit( - Spec::kCans.kCan2, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_front_climber_motor_[0].generate_command(), - device::CanPacket8::PaddingQuarter{}, - chassis_front_climber_motor_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); + builder.can_transmit(Spec::kCans.kCan2, { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_front_climber_motor_[0].generate_command(), + device::CanPacket8::PaddingQuarter{}, + chassis_front_climber_motor_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); } + private: void can_receive_callback(const Spec::Can& can, const View::Can& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; auto can_id = data.can_id; if (can == Spec::kCans.kCan0) { // can0_receive_rate_counter_.record(can_id); - check[can_id - 0x201] = 1; if (can_id == 0x201) { chassis_wheel_motors_[0].store_status(data.can_data); } else if (can_id == 0x202) { @@ -844,8 +753,6 @@ class SteeringHeroLittle } } else if (can == Spec::kCans.kCan1) { // can1_receive_rate_counter_.record(can_id); - if (can_id != 0x300) - check[can_id - 0x201] = 1; if (can_id == 0x203) { chassis_wheel_motors_[2].store_status(data.can_data); } else if (can_id == 0x204) { @@ -870,8 +777,6 @@ class SteeringHeroLittle chassis_back_climber_motor_[1].store_status(data.can_data); } else if (can_id == 0x204) { chassis_back_climber_motor_[0].store_status(data.can_data); - } else if (can_id == 0x203) { - yaw_brake_motor_.store_status(data.can_data); } else if (can_id == 0x141) { gimbal_bottom_yaw_motor_.store_status(data.can_data); } @@ -889,9 +794,6 @@ class SteeringHeroLittle } } - [[nodiscard]] device::Dr16& dr16() noexcept { return dr16_; } - [[nodiscard]] const device::Dr16& dr16() const noexcept { return dr16_; } - void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { imu_.store_accelerometer_status(data.x, data.y, data.z); } @@ -906,9 +808,6 @@ class SteeringHeroLittle // CanReceiveRateCounter can2_receive_rate_counter_; // CanReceiveRateCounter can3_receive_rate_counter_; - int count_ = 0; - int check[10] = {0}; - device::Bmi088 imu_; device::Dr16 dr16_; device::Supercap supercap_; @@ -917,7 +816,6 @@ class SteeringHeroLittle device::DjiMotor chassis_wheel_motors_[4]; device::DjiMotor chassis_front_climber_motor_[2]; device::DjiMotor chassis_back_climber_motor_[2]; - device::DjiMotor yaw_brake_motor_; device::LkMotor gimbal_bottom_yaw_motor_; rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; @@ -927,24 +825,19 @@ class SteeringHeroLittle OutputInterface powermeter_charge_power_limit_; OutputInterface chassis_yaw_velocity_imu_; OutputInterface chassis_pitch_imu_; - std::unique_ptr board_; }; OutputInterface tf_; - OutputInterface camera_transform_; - OutputInterface barrel_direction_; - OutputInterface auto_aim_yaw_velocity_; - rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; std::shared_ptr top_board_; std::shared_ptr bottom_board_; - std::unique_ptr remote_control_; }; } // namespace rmcs_core::hardware #include + PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringHeroLittle, rmcs_executor::Component) From d246805aa2962d354f68056b583d982261392acb Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Wed, 15 Jul 2026 02:00:04 +0800 Subject: [PATCH 16/86] feat: Add value collector and update auto aim ui --- .../config/deformable-infantry-omni.yaml | 48 ++++++++++++------- rmcs_ws/src/rmcs_core/plugins.xml | 2 + .../rmcs_core/src/referee/app/ui/auto_aim.cpp | 42 ++++++---------- .../referee/app/ui/deformable_infantry_ui.cpp | 28 ----------- 4 files changed, 47 insertions(+), 73 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 85a017264..687c6e985 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -10,6 +10,7 @@ rmcs_executor: - rmcs_core::referee::command::Interaction -> referee_interaction - rmcs_core::referee::command::interaction::Ui -> referee_ui - rmcs_core::referee::app::ui::DeformableInfantry -> referee_ui_infantry + - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui - rmcs_core::controller::gimbal::DeformableInfantryGimbalController -> gimbal_controller @@ -33,7 +34,27 @@ rmcs_executor: - rmcs::AutoAimComponent - rmcs::AutoAimCapturerComponent -> auto_aim_capturer - - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + # - rmcs_core::debug::ValueCollector -> value_collector + +value_collector: + ros__parameters: + csv_path: "/tmp/pitch_.csv" + signals: + - /gimbal/pitch/angle + - /gimbal/pitch/velocity + - /gimbal/pitch/velocity_imu + - /gimbal/pitch/angle_error + - /gimbal/pitch/control_torque + - /gimbal/pitch/control_velocity + write_interval: 5 + flush_interval: 1000 + +value_broadcaster: + ros__parameters: + forward_list: + - /gimbal/pitch/angle + - /gimbal/pitch/velocity auto_aim_capturer: ros__parameters: @@ -44,19 +65,11 @@ auto_aim_capturer: invert_image: false rls_tau_sec: 10.0 -value_broadcaster: +auto_aim_ui: ros__parameters: - forward_list: - - /gimbal/pitch/angle - - /gimbal/pitch/velocity - # - /chassis/left_front_joint/physical_angle - # - /chassis/left_front_joint/physical_velocity - # - /chassis/left_back_joint/physical_angle - # - /chassis/left_back_joint/physical_velocity - # - /chassis/right_back_joint/physical_angle - # - /chassis/right_back_joint/physical_velocity - # - /chassis/right_front_joint/physical_angle - # - /chassis/right_front_joint/physical_velocity + offset_x: 0.0 + offset_y: -0.08 + offset_z: 0.0 deformable_infantry: ros__parameters: @@ -149,9 +162,8 @@ gimbal_controller: pitch_velocity_ki: 0.0 pitch_velocity_kd: 0.0 - # pitch_gravity_ff_gain: 5.95 - pitch_gravity_ff_gain: 0.0 - pitch_gravity_ff_phase: 0.66 + pitch_gravity_ff_gain: 4.302 + pitch_gravity_ff_phase: 0.589 pitch_torque_control: true @@ -167,8 +179,8 @@ friction_wheel_controller: heat_controller: ros__parameters: - heat_per_shot: 10 - reserved_heat: 15 + heat_per_shot: 10000 + reserved_heat: 15000 bullet_feeder_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 44fda9043..e11533881 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -47,6 +47,8 @@ + + diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp index 8e01acd86..4ec4d7dfb 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp @@ -1,9 +1,9 @@ #include +#include #include #include #include #include -#include #include #include @@ -34,14 +34,10 @@ class AutoAimUi register_input("/tf", tf_, true); register_input("/auto_aim/robot_center", robot_center_, true); register_input("/auto_aim/should_shoot", should_shoot_, true); - register_input("/auto_aim/single_shoot", single_shoot_, true); } void update() override { - const auto type = *single_shoot_ ? "RUNE" : "ARMOR"; - if (!robot_center_->allFinite() || robot_center_->isZero()) { - set_distance_text(type, std::nullopt); hide_all(); return; } @@ -51,7 +47,6 @@ class AutoAimUi || point.x() >= kScreenW || point.x() < 0 // || point.y() >= kScreenH || point.y() < 0 // ) { - set_distance_text(type, std::nullopt); hide_all(); return; } @@ -65,12 +60,19 @@ class AutoAimUi { const auto distance = robot_center_->norm(); if (!std::isfinite(distance)) { - set_distance_text(type, std::nullopt); - hide_all(); + target_distance_indicator_.set_visible(false); return; } - set_distance_text(type, distance); + target_distance_text_index_ ^= 1u; + auto& text = target_distance_text_[target_distance_text_index_]; + text.clear(); + std::format_to(std::back_inserter(text), "{:.1f}m", distance); + if (text.size() > kMaxTextLength) + text.resize(kMaxTextLength); + + target_distance_indicator_.set_value(text.c_str()); + target_distance_indicator_.set_visible(true); } center_ring_.set_color(color); @@ -146,18 +148,17 @@ class AutoAimUi InputInterface tf_; InputInterface robot_center_; InputInterface should_shoot_; - InputInterface single_shoot_; Circle center_ring_{Shape::Color::GREEN, 2, 0, 0, 5, 5, false}; Line cross_top_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false}; Line cross_bottom_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false}; Line cross_left_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false}; Line cross_right_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false}; - - std::string target_distance_text_{"unknown"}; Text target_distance_indicator_{ - Shape::Color::GREEN, 15, 2, kScreenW / 2 + 34, kScreenH / 2 + 24, "unknown", false, + Shape::Color::GREEN, 20, 2, kScreenW / 2 + 34, kScreenH / 2 + 24, "", false, }; + std::array target_distance_text_{}; + std::size_t target_distance_text_index_ = 0; void hide_all() { center_ring_.set_visible(false); @@ -165,20 +166,7 @@ class AutoAimUi cross_bottom_.set_visible(false); cross_left_.set_visible(false); cross_right_.set_visible(false); - } - - void set_distance_text(const char* type, std::optional distance) { - auto& text = target_distance_text_; - text.resize(kMaxTextLength); - std::ranges::fill(text, ' '); - - if (distance) - std::format_to(std::ranges::begin(text), "{} | {:.1f}m\0", type, *distance); - else - std::format_to(std::ranges::begin(text), "{} | NONE\0", type); - - target_distance_indicator_.set_value(text.data()); - target_distance_indicator_.set_visible(true); + target_distance_indicator_.set_visible(false); } Eigen::Vector2d reproject(const Eigen::Vector3d& center) const { diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp index b5d640d34..3da6f172e 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp @@ -3,10 +3,8 @@ #include #include #include -#include #include -#include #include #include #include @@ -82,7 +80,6 @@ class DeformableInfantry register_input("/remote/mouse", mouse_); register_input("/remote/keyboard", keyboard_); - register_input("/auto_aim/robot_center", auto_aim_robot_center_, false); register_input("/referee/game/stage", game_stage_); @@ -97,7 +94,6 @@ class DeformableInfantry update_chassis_direction_indicator(); update_deformable_chassis_leg_arcs(); update_ctrl_ui(); - update_auto_aim_feedback(); status_ring_.update_bullet_allowance(*robot_bullet_allowance_); const double friction_wheel_speed = @@ -137,25 +133,6 @@ class DeformableInfantry return; } - void update_auto_aim_feedback() { - if (!auto_aim_robot_center_.ready() || !auto_aim_robot_center_->allFinite()) { - target_distance_indicator_.set_visible(false); - return; - } - - const double distance = auto_aim_robot_center_->norm(); - if (!std::isfinite(distance)) { - target_distance_indicator_.set_visible(false); - return; - } - - target_distance_text_index_ ^= 1u; - auto& text = target_distance_text_[target_distance_text_index_]; - std::snprintf(text.data(), text.size(), "%.1fm", distance); - target_distance_indicator_.set_value(text.data()); - target_distance_indicator_.set_visible(true); - } - void update_chassis_direction_indicator() { auto chassis_mode = *chassis_mode_; @@ -241,7 +218,6 @@ class DeformableInfantry InputInterface mouse_; InputInterface keyboard_; - InputInterface auto_aim_robot_center_; InputInterface game_stage_; @@ -254,10 +230,6 @@ class DeformableInfantry Arc chassis_direction_indicator_; DeformableChassisLegArcs deformable_chassis_leg_arcs_; - Text target_distance_indicator_{Shape::Color::GREEN, 20, 2, x_center + 34, - y_center + 24, "", false}; - std::array, 2> target_distance_text_{}; - size_t target_distance_text_index_ = 0; AnimatedToggle ctrl_transition_{}; uint16_t crosshair_base_x_ = 0; From e9567fb58777060d1b36bf1c265b22a3c82fa3c0 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Wed, 15 Jul 2026 02:56:02 +0800 Subject: [PATCH 17/86] feat: Add some utils --- .../rmcs_core/src/referee/app/ui/auto_aim.cpp | 20 +++++++++---------- .../rmcs_utility/rclcpp/node_mixin.hpp | 10 ---------- 2 files changed, 9 insertions(+), 21 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp index 4ec4d7dfb..3f206c909 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp @@ -1,5 +1,4 @@ #include -#include #include #include #include @@ -64,14 +63,13 @@ class AutoAimUi return; } - target_distance_text_index_ ^= 1u; - auto& text = target_distance_text_[target_distance_text_index_]; - text.clear(); - std::format_to(std::back_inserter(text), "{:.1f}m", distance); - if (text.size() > kMaxTextLength) - text.resize(kMaxTextLength); + auto& text = target_distance_text_; + text.resize(kMaxTextLength); + std::ranges::fill(text, ' '); - target_distance_indicator_.set_value(text.c_str()); + std::format_to(std::ranges::begin(text), "{:.1f}m\0", distance); + + target_distance_indicator_.set_value(text.data()); target_distance_indicator_.set_visible(true); } @@ -154,11 +152,11 @@ class AutoAimUi Line cross_bottom_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false}; Line cross_left_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false}; Line cross_right_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false}; + + std::string target_distance_text_{"unknown"}; Text target_distance_indicator_{ - Shape::Color::GREEN, 20, 2, kScreenW / 2 + 34, kScreenH / 2 + 24, "", false, + Shape::Color::GREEN, 15, 2, kScreenW / 2 + 34, kScreenH / 2 + 24, "unknown", false, }; - std::array target_distance_text_{}; - std::size_t target_distance_text_index_ = 0; void hide_all() { center_ring_.set_visible(false); diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/rclcpp/node_mixin.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/rclcpp/node_mixin.hpp index 24eb14d19..0a0369976 100644 --- a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/rclcpp/node_mixin.hpp +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/rclcpp/node_mixin.hpp @@ -1,16 +1,11 @@ #pragma once #include -#include namespace rmcs_utility { struct NodeMixin { using node = NodeMixin; - static constexpr auto options() noexcept { - return rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true); - } - template auto info(this const Self& self, std::format_string fmt, Args&&... args) -> void { auto text = std::format(fmt, std::forward(args)...); @@ -44,11 +39,6 @@ struct NodeMixin { requires std::convertible_to { dst = self.template get_parameter_or(name, fallback); } - - template - auto param_or(this const auto& self, const std::string& name, const T& fallback) -> T { - return self.template get_parameter_or(name, fallback); - } }; } // namespace rmcs_utility From 21eb0e4a6edba7d48a130f76b42a704dedb8eaa6 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Wed, 15 Jul 2026 20:48:36 +0800 Subject: [PATCH 18/86] chore: Update config --- .../src/rmcs_bringup/config/deformable-infantry-omni.yaml | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 687c6e985..e70bdf141 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -31,7 +31,7 @@ rmcs_executor: - rmcs_core::controller::chassis::DeformableJointController -> rb_joint_controller - rmcs_core::controller::chassis::DeformableJointController -> rf_joint_controller - - rmcs::AutoAimComponent + - rmcs::AutoAimComponent -> auto_aim_component - rmcs::AutoAimCapturerComponent -> auto_aim_capturer # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster @@ -56,6 +56,12 @@ value_broadcaster: - /gimbal/pitch/angle - /gimbal/pitch/velocity +auto_aim_component: + ros__parameters: + # 手动开火开关，若开启，右键自瞄并锁定后，还需要按下鼠标左键 + # 或者左拨杆向下确认才能发射 + manual_shoot: true + auto_aim_capturer: ros__parameters: camera_name: "" From e29942e8bbbe77ddc8e4b11d45c72c7d1eaae03c Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Fri, 17 Jul 2026 05:20:10 +0800 Subject: [PATCH 19/86] chore: Update script parameter and robot config --- .script/scan-remote | 245 ++++++++++++------ .../rmcs_bringup/config/auto_aim_test.yaml | 36 ++- .../config/deformable-infantry-omni.yaml | 25 +- 3 files changed, 205 insertions(+), 101 deletions(-) diff --git a/.script/scan-remote b/.script/scan-remote index 3e752f5d4..bb2483e8a 100755 --- a/.script/scan-remote +++ b/.script/scan-remote @@ -6,7 +6,6 @@ import json import os import re import select -import shutil import socket import subprocess import sys @@ -19,10 +18,10 @@ from colorama import Fore, Style SSH_PORT = 2022 SSH_USER = "root" -CONNECT_TIMEOUT = 0.75 +CONNECT_TIMEOUT = 1.0 BANNER_TIMEOUT = 0.35 SSH_PROBE_TIMEOUT = 2.0 -DEFAULT_SCAN_SEGMENTS = range(1, 6) +DEFAULT_192_168_SEGMENTS = range(1, 11) SKIP_PREFIXES = ("lo", "docker", "br-", "veth", "zt", "tailscale") SPINNER_FRAMES = "⠋⠙⠹⠸⠼⠴⠦⠧⠇⠏" REFRESH_INTERVAL = 0.08 @@ -49,41 +48,47 @@ class RawTerminal: def __exit__(self, *_): termios.tcsetattr(self._fd, termios.TCSANOW, self._old) - _SIMPLE_KEYS = { - b"\r": "enter", b"\n": "enter", - b"\t": "next", b"s": "next", b"S": "next", b"\x0e": "next", - b"w": "prev", b"W": "prev", b"\x10": "prev", - b"\x7f": "backspace", b"\x08": "backspace", - } - _ESCAPE_DIRS = {b"A": "prev", b"B": "next", b"Z": "prev"} - @staticmethod def read_key(): - if not RawTerminal.key_ready(0.02): - return None raw = sys.stdin.buffer.raw.read(1) - if raw in (b"q", b"Q"): - os._exit(130) - if raw in RawTerminal._SIMPLE_KEYS: - return RawTerminal._SIMPLE_KEYS[raw] + if raw == b"\x03": + raise KeyboardInterrupt + if raw in (b"\r", b"\n"): + return "enter" + if raw == b"\t": + return "next" + if raw in (b"w", b"W"): + return "prev" + if raw in (b"s", b"S"): + return "next" + if raw == b"\x0e": + return "next" + if raw == b"\x10": + return "prev" + if raw in (b"\x7f", b"\x08"): + return "backspace" if raw == b"\x1b": - return RawTerminal._read_escape() + if not RawTerminal.key_ready(0.02): + return "quit" + nxt = sys.stdin.buffer.raw.read(1) + if nxt != b"[": + return "escape" + if not RawTerminal.key_ready(0.02): + return "escape" + direction = sys.stdin.buffer.raw.read(1) + if direction == b"A": + return "prev" + if direction == b"B": + return "next" + if direction == b"Z": + return "prev" + return "escape" + if raw in (b"q", b"Q"): + return "quit" if len(raw) == 1 and 32 <= raw[0] <= 126: return raw.decode() return None - @staticmethod - def _read_escape(): - if not RawTerminal.key_ready(0.02): - os._exit(130) - nxt = sys.stdin.buffer.raw.read(1) - if nxt != b"[" or not RawTerminal.key_ready(0.02): - os._exit(130) - direction = sys.stdin.buffer.raw.read(1) - if direction in RawTerminal._ESCAPE_DIRS: - return RawTerminal._ESCAPE_DIRS[direction] - os._exit(130) - @staticmethod def key_ready(timeout): ready, _, _ = select.select([sys.stdin], [], [], timeout) @@ -183,11 +188,13 @@ def default_networks(): def expanded_networks(interface): ip = interface.ip - prefix = ".".join(str(part) for part in ip.packed[:2]) - return [ - ipaddress.ip_network(f"{prefix}.{segment}.0/24") - for segment in DEFAULT_SCAN_SEGMENTS - ] + if ip.packed[0] == 192 and ip.packed[1] == 168: + return [ + ipaddress.ip_network(f"192.168.{segment}.0/24") + for segment in DEFAULT_192_168_SEGMENTS + ] + + return [ipaddress.ip_network(f"{ip}/24", strict=False)] def dedupe_networks(networks): @@ -392,8 +399,8 @@ def scan_all_networks_until_selected( ) selected_result = None - pool = concurrent.futures.ThreadPoolExecutor(max_workers=max_workers) - try: + + with concurrent.futures.ThreadPoolExecutor(max_workers=max_workers) as pool: future_to_network = { pool.submit(probe_host, host): network for network, host in iter_interleaved_targets(network_targets) @@ -401,7 +408,7 @@ def scan_all_networks_until_selected( pending = set(future_to_network) while pending: - if on_key is not None: + if on_key is not None and RawTerminal.key_ready(0.02): key = RawTerminal.read_key() if key is not None: selected_result = on_key(key) @@ -439,8 +446,6 @@ def scan_all_networks_until_selected( if selected_result is not None: pool.shutdown(wait=False, cancel_futures=True) return sort_results(results), selected_result - finally: - pool.shutdown(wait=False, cancel_futures=True) return sort_results(results), None @@ -463,6 +468,35 @@ def iter_interleaved_targets(network_targets): pending = next_pending +def scan_network(network, on_progress=None, on_found=None): + targets = host_candidates([network]) + max_workers = min(128, max(8, len(targets))) + scanned = 0 + found = 0 + results = [] + + if on_progress is not None: + on_progress(network, scanned, len(targets), found, False) + + with concurrent.futures.ThreadPoolExecutor(max_workers=max_workers) as pool: + futures = [pool.submit(probe_host, host) for host in targets] + for future in concurrent.futures.as_completed(futures): + scanned += 1 + result = future.result() + if result is not None: + found += 1 + results.append(result) + if on_found is not None: + on_found(network, result) + if on_progress is not None: + on_progress(network, scanned, len(targets), found, False) + + if on_progress is not None: + on_progress(network, scanned, len(targets), found, True) + + return sort_results(results) + + def sort_results(results): return sorted( results, @@ -492,7 +526,6 @@ class ProgressView: self._thread = None self._selection = None self._prompt = None - self._interactive_render = sys.stdout.isatty() and os.getenv("TERM") != "dumb" def start(self): self._thread = threading.Thread(target=self._refresh_loop, daemon=True) @@ -522,9 +555,6 @@ class ProgressView: self.state[key]["results"].append(result) def render(self): - if not self._interactive_render: - return - with self._lock: lines = [] frame = SPINNER_FRAMES[self._frame % len(SPINNER_FRAMES)] @@ -589,38 +619,17 @@ class ProgressView: lines.append("") lines.append(self._prompt) - width = max(20, shutil.get_terminal_size((120, 24)).columns) - lines = [self._fit_line(line, width) for line in lines] - if self._rendered_lines: - sys.stdout.write(f"\r\033[{self._rendered_lines}A") + sys.stdout.write(f"\033[{self._rendered_lines}F") for line in lines: - sys.stdout.write("\033[2K\r") + sys.stdout.write("\033[2K") sys.stdout.write(line) sys.stdout.write("\n") for _ in range(max(0, self._rendered_lines - len(lines))): - sys.stdout.write("\033[2K\r\n") + sys.stdout.write("\033[2K\n") sys.stdout.flush() self._rendered_lines = len(lines) - @staticmethod - def _fit_line(line, width): - result = [] - visible = 0 - index = 0 - while index < len(line) and visible < width - 1: - if line[index] == "\033": - end = line.find("m", index) - if end == -1: - break - result.append(line[index:end + 1]) - index = end + 1 - continue - result.append(line[index]) - visible += 1 - index += 1 - return "".join(result) + Style.RESET_ALL - def set_selection(self, network, ip, prompt): with self._lock: self._selection = (network, ip) @@ -713,6 +722,92 @@ def print_results(results, json_mode, interactive_mode): print(format_table(results)) +def interactive_select_remote(view): + candidates = view.flatten_results() + if not candidates: + return None + + selected = 0 + confirm_mode = False + confirm_buf = "" + + def render_prompt(): + network, result = candidates[selected] + if confirm_mode: + prompt = ( + f"Set {Fore.GREEN}{result['ip']}{Style.RESET_ALL} as " + f"{Fore.CYAN}remote{Style.RESET_ALL}? [Y/n] {confirm_buf}" + ) + else: + prompt = ( + "Select result with Tab/Shift+Tab/↑/↓/W/S/Ctrl+N/Ctrl+P, " + "press Enter to continue, q/Esc/Ctrl+C to exit." + ) + view.set_selection(network, result["ip"], prompt) + + def confirm_choice(): + _, result = candidates[selected] + set_remote(result["ip"]) + view.clear_prompt() + print( + f"Successfully set remote host to " + f"{Fore.LIGHTGREEN_EX}{result['ip']}{Style.RESET_ALL}." + ) + return result["ip"] + + render_prompt() + with RawTerminal(): + while True: + key = RawTerminal.read_key() + if key is None: + continue + + if confirm_mode: + if key == "enter": + answer = confirm_buf.strip().lower() + if answer in ("", "y", "yes"): + return confirm_choice() + if answer in ("n", "no"): + confirm_mode = False + confirm_buf = "" + render_prompt() + continue + if key == "backspace": + confirm_buf = confirm_buf[:-1] + render_prompt() + continue + if key == "escape": + confirm_mode = False + confirm_buf = "" + render_prompt() + continue + if key in ("next", "prev"): + confirm_mode = False + confirm_buf = "" + elif isinstance(key, str) and len(key) == 1: + confirm_buf += key + render_prompt() + continue + + if key == "next": + selected = (selected + 1) % len(candidates) + confirm_mode = False + confirm_buf = "" + render_prompt() + continue + if key == "prev": + selected = (selected - 1) % len(candidates) + confirm_mode = False + confirm_buf = "" + render_prompt() + continue + if key == "enter": + confirm_mode = True + confirm_buf = "" + render_prompt() + continue + + def interactive_scan_and_select(networks): view = ProgressView(networks) selected = {"index": 0, "key": None} @@ -739,19 +834,20 @@ def interactive_scan_and_select(networks): view.set_selection( None, None, - "Scanning... press q/Esc to exit. Selection becomes available once a result appears.", + "Scanning... press q/Esc/Ctrl+C to exit. Selection becomes available once a result appears.", ) return sync_selection(candidates) network, result = candidates[selected["index"]] + selected["key"] = (network, result["ip"]) scan_done = all(view.state[network_name]["done"] for network_name in view.networks) prompt = ( "Scan complete. Select with Tab/Shift+Tab/↑/↓/W/S/Ctrl+N/Ctrl+P, " - "press Enter to set selected remote, q/Esc to exit." + "press Enter to set selected remote, q/Esc/Ctrl+C to exit." if scan_done - else "Select with Tab/Shift+Tab/↑/↓/W/S/Ctrl+N/Ctrl+P, press Enter to set immediately, q/Esc to exit." + else "Select with Tab/Shift+Tab/↑/↓/W/S/Ctrl+N/Ctrl+P, press Enter to set immediately, q/Esc/Ctrl+C to exit." ) view.set_selection( network, @@ -760,6 +856,9 @@ def interactive_scan_and_select(networks): ) def on_key(key): + if key in ("quit", "escape"): + raise KeyboardInterrupt + candidates = view.flatten_results() if not candidates: return None @@ -787,8 +886,6 @@ def interactive_scan_and_select(networks): with RawTerminal(): while True: key = RawTerminal.read_key() - if key is None: - continue chosen = on_key(key) if chosen is not None: return chosen @@ -845,4 +942,4 @@ if __name__ == "__main__": try: sys.exit(main(sys.argv[1:])) except KeyboardInterrupt: - os._exit(130) + sys.exit(1) diff --git a/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml b/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml index 7f812fa52..e1a8a1e45 100644 --- a/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml @@ -4,17 +4,17 @@ rmcs_executor: components: # - rmcs::AutoAimPlayerComponent -> auto_aim_player - rmcs::AutoAimVideoPlayerComponent -> auto_aim_video_player - # - rmcs::AutoAimRecorderComponent -> auto_aim_recorder + - rmcs::AutoAimRecorderComponent -> auto_aim_recorder - rmcs::AutoAimComponent -> auto_aim_component auto_aim_player: ros__parameters: - input_path: "/workspaces/data/autoaim/robot/blue_fast_track/" + input_path: "/workspaces/data/autoaim/2026-07-11_22-39-35/" loop_play: true auto_aim_video_player: ros__parameters: - input_path: "/workspaces/data/autoaim/静止看前哨站.avi" + input_path: "/workspaces/data/autoaim/robot/rotate.avi" framerate: 80.0 loop_play: true @@ -25,28 +25,24 @@ auto_aim_recorder: flush_every_n_frames: 64 max_duration_seconds: 0 max_videos_size_gb: 0.0 - auto_record: false auto_aim_component: ros__parameters: - dangerous_fallback: "red" + # 手动开火开关，若开启，右键自瞄并锁定后，还需要按下鼠标左键 + # 或者左拨杆向下确认才能发射 manual_shoot: false - enable_rune: true - track_ids: [HERO, ENGINEER, INFANTRY_3, INFANTRY_4, SENTRY, OUTPOST] camera_translation: [0., 0., 0.] fire_control: - bullet_speed: 22.5 - shoot_delay: 0.0 - offset_yaw: 0.0 - offset_pitch: 0.0 - attack_window: 60.0 - degraded_angle_speed: 12.0 - window_redundancy: 0.8 - window_hysteresis: 0.2 - attack_preaim: false + bullet_speed: 23.5 # m/s + shoot_delay: 0.04 # s + + offset_yaw: -0.0 # degree + offset_pitch: +0.0 # degree + + attack_window: 120.0 # degree (total window) + + is_lazy_gimbal: false require_stable_command: true - yaw_tolerance: 0.07 - pitch_tolerance: 0.04 - rune_idle_duration: 0.4 - rune_shoot_duration: 0.2 + yaw_tolerance: 0.07 # m，横向单边容差 + pitch_tolerance: 0.04 # m，纵向单边容差 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index e70bdf141..0aab3f570 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -56,12 +56,6 @@ value_broadcaster: - /gimbal/pitch/angle - /gimbal/pitch/velocity -auto_aim_component: - ros__parameters: - # 手动开火开关，若开启，右键自瞄并锁定后，还需要按下鼠标左键 - # 或者左拨杆向下确认才能发射 - manual_shoot: true - auto_aim_capturer: ros__parameters: camera_name: "" @@ -70,6 +64,23 @@ auto_aim_capturer: framerate: 120.0 invert_image: false rls_tau_sec: 10.0 + use_hardware_sync: true + delay_ms: 6.5 + +auto_aim_component: + ros__parameters: + manual_shoot: true + camera_translation: [0.058, -0.08, 0.0] + fire_control: + bullet_speed: 22.5 + shoot_delay: 0.04 + offset_yaw: -0.0 + offset_pitch: +0.0 + attack_window: 120.0 + is_lazy_gimbal: false + require_stable_command: true + yaw_tolerance: 0.07 + pitch_tolerance: 0.04 auto_aim_ui: ros__parameters: @@ -152,7 +163,7 @@ gimbal_controller: lower_limit: 0.15707 # 9 deg ctrl_hold_pitch_target_angle: 0.0 - yaw_angle_kp: 30.0 + yaw_angle_kp: 25.0 yaw_angle_ki: 0.0 yaw_angle_kd: 0.0 From 1ef9a19b8451c729bbee790682edae0b35517e6c Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Fri, 17 Jul 2026 09:45:17 +0800 Subject: [PATCH 20/86] chore: Update config --- .../rmcs_bringup/config/auto_aim_test.yaml | 24 +++++++++---------- .../config/deformable-infantry-omni.yaml | 12 +++++++--- 2 files changed, 20 insertions(+), 16 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml b/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml index e1a8a1e45..a3b31ca3e 100644 --- a/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml @@ -9,7 +9,7 @@ rmcs_executor: auto_aim_player: ros__parameters: - input_path: "/workspaces/data/autoaim/2026-07-11_22-39-35/" + input_path: "/workspaces/data/autoaim/自家小符/" loop_play: true auto_aim_video_player: @@ -28,21 +28,19 @@ auto_aim_recorder: auto_aim_component: ros__parameters: - # 手动开火开关，若开启，右键自瞄并锁定后，还需要按下鼠标左键 - # 或者左拨杆向下确认才能发射 + dangerous_fallback: "red" manual_shoot: false camera_translation: [0., 0., 0.] fire_control: - bullet_speed: 23.5 # m/s - shoot_delay: 0.04 # s - - offset_yaw: -0.0 # degree - offset_pitch: +0.0 # degree - - attack_window: 120.0 # degree (total window) - + bullet_speed: 23.5 + shoot_delay: 0.0 + offset_yaw: 0.0 + offset_pitch: 0.0 + attack_window: 120.0 + window_hysteresis: 0.2 is_lazy_gimbal: false + attack_preaim: false require_stable_command: true - yaw_tolerance: 0.07 # m，横向单边容差 - pitch_tolerance: 0.04 # m，纵向单边容差 + yaw_tolerance: 0.07 + pitch_tolerance: 0.04 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 0aab3f570..baa6d7b96 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -69,16 +69,22 @@ auto_aim_capturer: auto_aim_component: ros__parameters: + # WARN: 危险！可选 red | blue，生效后裁判系统 ID 缺席时 + # 将强行绑定为对应阵营哨兵，仅供调试，严禁比赛启用。 + # 留空或填 unknow 表示禁用。 + dangerous_fallback: "" manual_shoot: true camera_translation: [0.058, -0.08, 0.0] fire_control: bullet_speed: 22.5 - shoot_delay: 0.04 + shoot_delay: 0.1 offset_yaw: -0.0 offset_pitch: +0.0 attack_window: 120.0 + window_hysteresis: 0.2 is_lazy_gimbal: false - require_stable_command: true + attack_preaim: false + require_stable_command: false yaw_tolerance: 0.07 pitch_tolerance: 0.04 @@ -163,7 +169,7 @@ gimbal_controller: lower_limit: 0.15707 # 9 deg ctrl_hold_pitch_target_angle: 0.0 - yaw_angle_kp: 25.0 + yaw_angle_kp: 15.0 yaw_angle_ki: 0.0 yaw_angle_kd: 0.0 From 29a87d85b4ed68f605bfefe62dfc168717f3a777 Mon Sep 17 00:00:00 2001 From: qzhhhi Date: Sun, 12 Jul 2026 02:19:02 +0800 Subject: [PATCH 21/86] feat: success buff --- rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/camera_frame.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/camera_frame.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/camera_frame.hpp index f728f138e..42749f0b4 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/camera_frame.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/camera_frame.hpp @@ -21,7 +21,6 @@ struct CameraFrame { std::array data; Eigen::Quaterniond imu_snapshot; - Eigen::Vector3d gyro_body = Eigen::Vector3d::Zero(); std::chrono::steady_clock::time_point exposure_timestamp; std::chrono::steady_clock::time_point image_reception_timestamp; From ecb48beb6084e9ff9fe05712dbd9d84565e5e124 Mon Sep 17 00:00:00 2001 From: dwx5 <1591215786@qq.com> Date: Thu, 4 Jun 2026 21:08:50 +0800 Subject: [PATCH 22/86] update after merge main --- .../controller/gimbal/dual_yaw_controller.cpp | 248 ++------------- .../gimbal/hero_gimbal_controller.cpp | 111 ++++--- .../controller/shooting/heat_controller.cpp | 19 +- .../controller/shooting/putter_controller.cpp | 70 +++-- .../steering-hero-little-six-friction.cpp | 290 ++++++++++-------- .../src/rmcs_core/src/referee/app/ui/hero.cpp | 89 +++++- 6 files changed, 399 insertions(+), 428 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp index 84f8c0862..56596d598 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp @@ -1,6 +1,5 @@ #include -#include #include #include @@ -40,22 +39,18 @@ class DualYawController register_input("/gimbal/top_yaw/velocity", top_yaw_velocity_); register_input("/gimbal/bottom_yaw/angle", bottom_yaw_angle_); register_input("/gimbal/bottom_yaw/velocity", bottom_yaw_velocity_); - register_input("/gimbal/bottom_yaw/raw_angle", bottom_yaw_raw_angle_); register_input("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); register_input("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_); - register_input("/gimbal/yaw_brake/velocity", yaw_brake_velocity_); register_input("/gimbal/mode", gimbal_mode_); register_input("/gimbal/yaw/control_angle_error", control_angle_error_); register_input("/gimbal/yaw/control_angle_shift", control_angle_shift_, false); - register_input("/gimbal/bottom_yaw/torque", bottom_yaw_torque_); register_output("/gimbal/top_yaw/control_torque", top_yaw_control_torque_, 0.0); register_output("/gimbal/bottom_yaw/control_torque", bottom_yaw_control_torque_, 0.0); register_output("/gimbal/bottom_yaw/control_angle", bottom_yaw_control_angle_, nan_); register_output("/gimbal/top_yaw/control_angle_shift", top_yaw_control_angle_shift_, nan_); - register_output("/gimbal/yaw_brake/control_torque", yaw_brake_control_torque_, nan_); status_component_ = create_partner_component(get_component_name() + "_status"); @@ -72,203 +67,38 @@ class DualYawController void update() override { const auto mode = *gimbal_mode_; - const bool entering_encoder = mode == rmcs_msgs::GimbalMode::ENCODER - && last_gimbal_mode_ != rmcs_msgs::GimbalMode::ENCODER; + if (mode == rmcs_msgs::GimbalMode::ENCODER) { + const bool entering_encoder = last_gimbal_mode_ != rmcs_msgs::GimbalMode::ENCODER; - const bool leaving_encoder = mode != rmcs_msgs::GimbalMode::ENCODER - && last_gimbal_mode_ == rmcs_msgs::GimbalMode::ENCODER; - - if (entering_encoder) { - top_yaw_angle_pid_.reset(); - top_yaw_velocity_pid_.reset(); - bottom_yaw_angle_pid_.reset(); - bottom_yaw_velocity_pid_.reset(); - - yaw_brake_engage_count_ = 0; - yaw_brake_release_elapsed_count_ = 0; - yaw_brake_release_stop_count_ = 0; - yaw_brake_release_seen_motion_ = false; - encoder_state_ = EncoderState::AlignTargetRawAngle; - } - - if (leaving_encoder) { - top_yaw_angle_pid_.reset(); - top_yaw_velocity_pid_.reset(); - bottom_yaw_angle_pid_.reset(); - bottom_yaw_velocity_pid_.reset(); - - if (encoder_state_ == EncoderState::EngageYawBrake - || encoder_state_ == EncoderState::BrakeLocked) { - yaw_brake_release_elapsed_count_ = 0; - yaw_brake_release_stop_count_ = 0; - yaw_brake_release_seen_motion_ = false; - encoder_state_ = EncoderState::ReleaseYawBrake; - } else { - yaw_brake_engage_count_ = 0; - yaw_brake_release_elapsed_count_ = 0; - yaw_brake_release_stop_count_ = 0; - yaw_brake_release_seen_motion_ = false; - *yaw_brake_control_torque_ = nan_; - encoder_state_ = EncoderState::Idle; + if (entering_encoder) { + if (std::isfinite(*bottom_yaw_angle_)) { + bottom_yaw_encoder_angle_ = *bottom_yaw_angle_; + bottom_yaw_encoder_locked_ = true; + bottom_yaw_angle_pid_.reset(); + bottom_yaw_velocity_pid_.reset(); + } else { + bottom_yaw_encoder_angle_ = nan_; + bottom_yaw_encoder_locked_ = false; + } } - } - if (mode == rmcs_msgs::GimbalMode::ENCODER && encoder_state_ == EncoderState::Idle) { - top_yaw_angle_pid_.reset(); - top_yaw_velocity_pid_.reset(); - bottom_yaw_angle_pid_.reset(); - bottom_yaw_velocity_pid_.reset(); - - yaw_brake_engage_count_ = 0; - yaw_brake_release_elapsed_count_ = 0; - yaw_brake_release_stop_count_ = 0; - yaw_brake_release_seen_motion_ = false; - encoder_state_ = EncoderState::AlignTargetRawAngle; - } - // RCLCPP_INFO(get_logger(), "bottom_yaw_raw_angle: %ld", *bottom_yaw_raw_angle_); - // RCLCPP_INFO(get_logger(), "bottom_yaw_control_torque: %f", *bottom_yaw_control_torque_); - // RCLCPP_INFO(get_logger(), "bottom_yaw_torque: %f", *bottom_yaw_torque_); - // RCLCPP_INFO( - // get_logger(), "encoder_state: %s", - // encoder_state_ == EncoderState::Idle ? "Idle" - // : encoder_state_ == EncoderState::AlignTargetRawAngle ? "AlignTargetRawAngle" - // : encoder_state_ == EncoderState::EngageYawBrake ? "EngageYawBrake" - // : encoder_state_ == EncoderState::BrakeLocked ? "BrakeLocked" - // : "ReleaseYawBrake"); - const bool hold_encoder_for_brake_release = encoder_state_ == EncoderState::ReleaseYawBrake; - - if (mode == rmcs_msgs::GimbalMode::ENCODER || hold_encoder_for_brake_release) { *top_yaw_control_torque_ = nan_; *bottom_yaw_control_angle_ = nan_; + *top_yaw_control_angle_shift_ = *control_angle_shift_; - auto wrap_raw_delta = [](int64_t diff) -> int64_t { - diff %= kBottomYawRawAngleModulus; - if (diff <= -(kBottomYawRawAngleModulus / 2)) - diff += kBottomYawRawAngleModulus; - else if (diff > (kBottomYawRawAngleModulus / 2)) - diff -= kBottomYawRawAngleModulus; - return diff; - }; - - if (encoder_state_ == EncoderState::AlignTargetRawAngle) { - *top_yaw_control_angle_shift_ = 0.0; - *yaw_brake_control_torque_ = nan_; - - if (!bottom_yaw_raw_angle_.ready()) { - *bottom_yaw_control_torque_ = nan_; - } else { - const int64_t raw_error_count = - wrap_raw_delta(kEncoderBottomYawTargetRawAngle - *bottom_yaw_raw_angle_); - const int64_t raw_error_abs = - raw_error_count >= 0 ? raw_error_count : -raw_error_count; - - const bool bottom_yaw_aligned = - raw_error_abs <= kEncoderBottomYawTargetToleranceRawAngle - && std::abs(*bottom_yaw_velocity_) - <= kEncoderBottomYawLockVelocityThreshold; - - if (bottom_yaw_aligned) { - *bottom_yaw_control_torque_ = nan_; - bottom_yaw_angle_pid_.reset(); - bottom_yaw_velocity_pid_.reset(); - yaw_brake_engage_count_ = 0; - encoder_state_ = EncoderState::EngageYawBrake; - } else { - const double raw_error_angle = - kEncoderBottomYawRawAngleErrorSign - * static_cast(raw_error_count) - / static_cast(kBottomYawRawAngleModulus) * 2.0 - * std::numbers::pi; - - const double target_velocity = - bottom_yaw_angle_pid_.update(raw_error_angle); - const double velocity_error = target_velocity - *bottom_yaw_velocity_; - double control_torque = bottom_yaw_velocity_pid_.update(velocity_error); - - constexpr double kBreakawayTorque = 2.5; - constexpr double kBreakawayVelocityThreshold = 0.10; - constexpr int64_t kBreakawayErrorThreshold = 80; - - if (std::abs(*bottom_yaw_velocity_) < kBreakawayVelocityThreshold - && std::abs(raw_error_count) > kBreakawayErrorThreshold - && std::abs(control_torque) < kBreakawayTorque) { - control_torque = - velocity_error >= 0.0 ? kBreakawayTorque : -kBreakawayTorque; - } - - *bottom_yaw_control_torque_ = control_torque; - } - } - - } else if (encoder_state_ == EncoderState::EngageYawBrake) { - *top_yaw_control_angle_shift_ = 0.0; - *bottom_yaw_control_torque_ = nan_; - *yaw_brake_control_torque_ = kYawBrakeEngageTorque; - - if (std::abs(*yaw_brake_velocity_) < kYawBrakeEngageVelocityThreshold) { - ++yaw_brake_engage_count_; - } else { - yaw_brake_engage_count_ = 0; - } - - if (yaw_brake_engage_count_ >= kYawBrakeEngageConfirmCount) { - yaw_brake_engage_count_ = 0; - *yaw_brake_control_torque_ = nan_; - encoder_state_ = EncoderState::BrakeLocked; - } - - } else if (encoder_state_ == EncoderState::BrakeLocked) { - constexpr double kBrakeLockedHoldTorque = 0.6; - *bottom_yaw_control_torque_ = kBrakeLockedHoldTorque; - *yaw_brake_control_torque_ = nan_; - *top_yaw_control_angle_shift_ = *control_angle_shift_; - - } else if (encoder_state_ == EncoderState::ReleaseYawBrake) { - *top_yaw_control_angle_shift_ = 0.0; - *bottom_yaw_control_torque_ = nan_; - *yaw_brake_control_torque_ = kYawBrakeReleaseTorque; - - ++yaw_brake_release_elapsed_count_; - - if (std::abs(*yaw_brake_velocity_) > kYawBrakeReleaseMotionVelocityThreshold) { - yaw_brake_release_seen_motion_ = true; - } - - if (yaw_brake_release_seen_motion_ - && std::abs(*yaw_brake_velocity_) < kYawBrakeReleaseStopVelocityThreshold) { - ++yaw_brake_release_stop_count_; - } else { - yaw_brake_release_stop_count_ = 0; - } - - if (yaw_brake_release_stop_count_ >= kYawBrakeReleaseStopConfirmCount) { - yaw_brake_engage_count_ = 0; - yaw_brake_release_elapsed_count_ = 0; - yaw_brake_release_stop_count_ = 0; - yaw_brake_release_seen_motion_ = false; - *yaw_brake_control_torque_ = nan_; - encoder_state_ = EncoderState::Idle; - } else if (yaw_brake_release_elapsed_count_ >= kYawBrakeReleaseTimeoutCount) { - RCLCPP_WARN( - get_logger(), "Yaw brake release timed out, stop applying reverse torque " - "for protection."); - yaw_brake_engage_count_ = 0; - yaw_brake_release_elapsed_count_ = 0; - yaw_brake_release_stop_count_ = 0; - yaw_brake_release_seen_motion_ = false; - *yaw_brake_control_torque_ = nan_; - encoder_state_ = EncoderState::Idle; - } + if (bottom_yaw_encoder_locked_) { + double err = std::remainder( + bottom_yaw_encoder_angle_ - *bottom_yaw_angle_, 2.0 * std::numbers::pi); + double target_velocity = bottom_yaw_angle_pid_.update(err); + *bottom_yaw_control_torque_ = + bottom_yaw_velocity_pid_.update(target_velocity - bottom_yaw_velocity_imu()); } else { - *top_yaw_control_angle_shift_ = 0.0; *bottom_yaw_control_torque_ = nan_; - *yaw_brake_control_torque_ = nan_; } + // *bottom_yaw_control_torque_ = nan_; } else { - *yaw_brake_control_torque_ = nan_; - *top_yaw_control_torque_ = top_yaw_velocity_pid_.update( top_yaw_angle_pid_.update(*control_angle_error_) - *gimbal_yaw_velocity_imu_); @@ -278,41 +108,20 @@ class DualYawController *bottom_yaw_control_angle_ = nan_; *top_yaw_control_angle_shift_ = nan_; + bottom_yaw_encoder_locked_ = false; } last_gimbal_mode_ = mode; } private: - enum class EncoderState { - Idle, - AlignTargetRawAngle, - EngageYawBrake, - BrakeLocked, - ReleaseYawBrake - }; static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - static constexpr int64_t kBottomYawRawAngleModulus = 1 << 16; - static constexpr int64_t kEncoderBottomYawTargetRawAngle = 2434; - static constexpr int64_t kEncoderBottomYawTargetToleranceRawAngle = 80; - static constexpr double kEncoderBottomYawLockVelocityThreshold = 0.15; - static constexpr double kEncoderBottomYawRawAngleErrorSign = -1.0; - - static constexpr double kYawBrakeEngageTorque = -0.3; - static constexpr double kYawBrakeEngageVelocityThreshold = 0.1; - static constexpr int kYawBrakeEngageConfirmCount = 50; - - static constexpr double kYawBrakeReleaseTorque = 0.3; - static constexpr double kYawBrakeReleaseMotionVelocityThreshold = 0.2; - static constexpr double kYawBrakeReleaseStopVelocityThreshold = 0.08; - static constexpr int kYawBrakeReleaseStopConfirmCount = 20; - static constexpr int kYawBrakeReleaseTimeoutCount = 400; - double bottom_yaw_control_error() { if (!std::isfinite(*top_yaw_angle_) || !std::isfinite(*control_angle_error_)) return nan_; + // Avoid relying on top_yaw_angle in [0, 2pi) and control_angle_error in [-pi, pi]. constexpr double alignment = 2 * std::numbers::pi; double err = std::fmod(*top_yaw_angle_ + *control_angle_error_ + std::numbers::pi, alignment); @@ -326,31 +135,24 @@ class DualYawController InputInterface top_yaw_angle_, top_yaw_velocity_; InputInterface bottom_yaw_angle_, bottom_yaw_velocity_; - InputInterface bottom_yaw_raw_angle_; InputInterface gimbal_yaw_velocity_imu_, chassis_yaw_velocity_imu_; - InputInterface yaw_brake_velocity_; InputInterface gimbal_mode_; InputInterface control_angle_error_, control_angle_shift_; - InputInterface bottom_yaw_torque_; pid::PidCalculator top_yaw_angle_pid_, top_yaw_velocity_pid_; pid::PidCalculator bottom_yaw_angle_pid_, bottom_yaw_velocity_pid_; OutputInterface top_yaw_control_torque_; OutputInterface bottom_yaw_control_torque_; + OutputInterface bottom_yaw_control_angle_; OutputInterface top_yaw_control_angle_shift_; - OutputInterface yaw_brake_control_torque_; rmcs_msgs::GimbalMode last_gimbal_mode_ = rmcs_msgs::GimbalMode::IMU; - EncoderState encoder_state_ = EncoderState::Idle; - - int yaw_brake_engage_count_ = 0; - int yaw_brake_release_elapsed_count_ = 0; - int yaw_brake_release_stop_count_ = 0; - bool yaw_brake_release_seen_motion_ = false; + bool bottom_yaw_encoder_locked_ = false; + double bottom_yaw_encoder_angle_ = nan_; class DualYawStatus : public rmcs_executor::Component { public: diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp index f3f8d1310..2cd9be969 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp @@ -24,7 +24,11 @@ class HeroGimbalController HeroGimbalController() : Node( get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , upper_limit_(get_parameter("upper_limit").as_double()) + , lower_limit_(get_parameter("lower_limit").as_double()) + , imu_gimbal_solver(*this, upper_limit_, lower_limit_) + , encoder_gimbal_solver(*this, upper_limit_, lower_limit_) { register_input("/remote/joystick/left", joystick_left_); register_input("/remote/switch/left", switch_left_); @@ -33,8 +37,13 @@ class HeroGimbalController register_input("/remote/mouse", mouse_); register_input("/remote/keyboard", keyboard_); - register_input("/auto_aim/should_control", auto_aim_should_control_, false); - register_input("/auto_aim/control_direction", auto_aim_control_direction_, false); + register_input("/gimbal/auto_aim/control_direction", auto_aim_control_direction_, false); + register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); + register_input("/gimbal/pitch/raw_angle", gimbal_pitch_raw_angle_); + register_input("/gimbal/yaw_brake/angle", gimbal_yaw_brake_angle_); + register_input("/gimbal/yaw_brake/torque", gimbal_yaw_brake_torque_); + register_input("/gimbal/yaw_brake/velocity", gimbal_yaw_brake_velocity_); + // 锁住时复方向 register_input("/tf", tf_); register_output("/gimbal/mode", gimbal_mode_, rmcs_msgs::GimbalMode::IMU); @@ -43,13 +52,13 @@ class HeroGimbalController register_output("/gimbal/pitch/control_angle_error", pitch_angle_error_, nan_); register_output("/gimbal/yaw/control_angle_shift", yaw_control_angle_shift_, nan_); register_output("/gimbal/pitch/control_angle", pitch_control_angle_, nan_); + register_output("/gimbal/yaw_brake/control_torque", yaw_brake_control_torque_, nan_); } void update() override { const auto& switch_left = *switch_left_; const auto& switch_right = *switch_right_; - // RCLCPP_INFO(get_logger(), "pitch %f", *gimbal_pitch_angle_); do { using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) @@ -58,6 +67,7 @@ class HeroGimbalController break; } + // RCLCPP_INFO(get_logger(), "yaw_brake_torque: %f", *gimbal_yaw_brake_torque_); if (!last_keyboard_.e && keyboard_->e) { if (gimbal_mode_keyboard_ == GimbalMode::IMU) { encoder_init_pitch_ = keyboard_->ctrl ? kCtrlEInitPitch : kEInitPitch; @@ -75,7 +85,7 @@ class HeroGimbalController } *gimbal_mode_ = gimbal_mode_keyboard_; - //*gimbal_mode_ = switch_right == Switch::UP ? GimbalMode::ENCODER : GimbalMode::IMU; + *gimbal_mode_ = switch_right == Switch::UP ? GimbalMode::ENCODER : GimbalMode::IMU; if (*gimbal_mode_ == GimbalMode::IMU) { auto angle_error = switch_encoder_to_imu_by_c ? enter_imu_hold_current_pose() @@ -83,12 +93,12 @@ class HeroGimbalController *yaw_angle_error_ = angle_error.yaw_angle_error; *pitch_angle_error_ = angle_error.pitch_angle_error; - encoder_gimbal_solver_.update(PreciseTwoAxisGimbalSolver::SetDisabled{}); + encoder_gimbal_solver.update(PreciseTwoAxisGimbalSolver::SetDisabled{}); *yaw_control_angle_shift_ = nan_; *pitch_control_angle_ = nan_; } else { - imu_gimbal_solver_.update(TwoAxisGimbalSolver::SetDisabled{}); + imu_gimbal_solver.update(TwoAxisGimbalSolver::SetDisabled{}); *yaw_angle_error_ = nan_; *pitch_angle_error_ = nan_; @@ -102,8 +112,8 @@ class HeroGimbalController } void reset_all_control() { - imu_gimbal_solver_.update(TwoAxisGimbalSolver::SetDisabled{}); - encoder_gimbal_solver_.update(PreciseTwoAxisGimbalSolver::SetDisabled{}); + imu_gimbal_solver.update(TwoAxisGimbalSolver::SetDisabled{}); + encoder_gimbal_solver.update(PreciseTwoAxisGimbalSolver::SetDisabled{}); gimbal_mode_keyboard_ = rmcs_msgs::GimbalMode::IMU; *gimbal_mode_ = rmcs_msgs::GimbalMode::IMU; @@ -112,23 +122,38 @@ class HeroGimbalController *pitch_angle_error_ = nan_; *yaw_control_angle_shift_ = nan_; *pitch_control_angle_ = nan_; + *yaw_brake_control_torque_ = nan_; + + yaw_brake_count_ = 0; + yaw_brake_locked_ = false; + } + + void yaw_brake_update() { + if (std::abs(*yaw_brake_control_torque_) > 0.01 + && std::abs(*gimbal_yaw_brake_velocity_) < 0.1) { + yaw_brake_count_++; + } else { + yaw_brake_count_ = 0; + } + + if (yaw_brake_count_ > 50) { + yaw_brake_count_ = 0; + yaw_brake_locked_ = true; + *yaw_brake_control_torque_ = nan_; + } } TwoAxisGimbalSolver::AngleError update_imu_control() { - const auto auto_aim_requested = mouse_->right || *switch_right_ == rmcs_msgs::Switch::UP; - const auto should_control = auto_aim_should_control_.ready() && *auto_aim_should_control_; - const auto valid_control = auto_aim_control_direction_.ready() - && auto_aim_control_direction_->allFinite() - && !auto_aim_control_direction_->isZero(); - - if (auto_aim_requested && should_control && valid_control) { - return imu_gimbal_solver_.update( + if (auto_aim_control_direction_.ready() + && (mouse_->right || *switch_right_ == rmcs_msgs::Switch::UP) + && !auto_aim_control_direction_->isZero()) { + return imu_gimbal_solver.update( TwoAxisGimbalSolver::SetControlDirection{ OdomImu::DirectionVector{*auto_aim_control_direction_}}); } - if (!imu_gimbal_solver_.enabled()) - return imu_gimbal_solver_.update(TwoAxisGimbalSolver::SetToLevel{}); + if (!imu_gimbal_solver.enabled()) + return imu_gimbal_solver.update(TwoAxisGimbalSolver::SetToLevel{}); constexpr double joystick_sensitivity = 0.006; constexpr double mouse_sensitivity = 0.5; @@ -138,7 +163,7 @@ class HeroGimbalController double pitch_shift = -joystick_sensitivity * joystick_left_->x() + mouse_sensitivity * mouse_velocity_->x(); - return imu_gimbal_solver_.update( + return imu_gimbal_solver.update( TwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); } @@ -149,26 +174,32 @@ class HeroGimbalController auto current_direction = fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); - return imu_gimbal_solver_.update( + return imu_gimbal_solver.update( TwoAxisGimbalSolver::SetControlDirection{OdomImu::DirectionVector{*current_direction}}); } PreciseTwoAxisGimbalSolver::ControlAngle update_encoder_control() { - if (!encoder_gimbal_solver_.enabled()) { - return encoder_gimbal_solver_.update( + if (!encoder_gimbal_solver.enabled()) { + return encoder_gimbal_solver.update( PreciseTwoAxisGimbalSolver::SetControlPitch{encoder_init_pitch_}); } constexpr double mouse_yaw_sensitivity = 0.5 * 0.114; constexpr double mouse_pitch_sensitivity = 0.5 * 0.095; - constexpr double joystick_sensitivity = 0.006 * 0.02; + constexpr double joystick_sensitivity = 0.006 * 0.05; double yaw_shift = joystick_sensitivity * joystick_left_->y() + mouse_yaw_sensitivity * mouse_velocity_->y(); double pitch_shift = -joystick_sensitivity * joystick_left_->x() + mouse_pitch_sensitivity * mouse_velocity_->x(); - return encoder_gimbal_solver_.update( + if (!yaw_brake_locked_) { + *yaw_brake_control_torque_ = -0.3; + } + + yaw_brake_update(); + + return encoder_gimbal_solver.update( PreciseTwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); } @@ -177,7 +208,6 @@ class HeroGimbalController static constexpr double kEInitPitch = -0.346584; // Initial angle for standalone E. static constexpr double kCtrlEInitPitch = -0.471795; // Initial angle for Ctrl+E. - double encoder_init_pitch_ = kEInitPitch; InputInterface joystick_left_; InputInterface switch_right_; @@ -188,35 +218,32 @@ class HeroGimbalController rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); - InputInterface auto_aim_should_control_; InputInterface auto_aim_control_direction_; + InputInterface gimbal_pitch_angle_; + InputInterface gimbal_pitch_raw_angle_; + InputInterface gimbal_yaw_brake_angle_; + InputInterface gimbal_yaw_brake_torque_; + InputInterface gimbal_yaw_brake_velocity_; InputInterface tf_; rmcs_msgs::GimbalMode gimbal_mode_keyboard_ = rmcs_msgs::GimbalMode::IMU; OutputInterface gimbal_mode_; + const double upper_limit_, lower_limit_; + TwoAxisGimbalSolver imu_gimbal_solver; + PreciseTwoAxisGimbalSolver encoder_gimbal_solver; + OutputInterface yaw_angle_error_, pitch_angle_error_; OutputInterface yaw_control_angle_shift_, pitch_control_angle_; + OutputInterface yaw_brake_control_torque_; - struct SimpleComponent : Component { - auto update() -> void override {} - }; - std::shared_ptr imu_gimbal_solver_component_ = - create_partner_component("imu_gimbal_solver"); - std::shared_ptr encoder_gimbal_solver_component_ = - create_partner_component("encoder_gimbal_solver"); - - const double upper_limit_{get_parameter("upper_limit").as_double()}; - const double lower_limit_{get_parameter("lower_limit").as_double()}; - - TwoAxisGimbalSolver imu_gimbal_solver_ = { - *imu_gimbal_solver_component_, upper_limit_, lower_limit_}; - PreciseTwoAxisGimbalSolver encoder_gimbal_solver_ = { - *encoder_gimbal_solver_component_, upper_limit_, lower_limit_}; + int yaw_brake_count_ = 0; + bool yaw_brake_locked_ = false; }; } // namespace rmcs_core::controller::gimbal #include + PLUGINLIB_EXPORT_CLASS( rmcs_core::controller::gimbal::HeroGimbalController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp index c6901fbcb..2bd06a780 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp @@ -28,11 +28,21 @@ class HeatController void update() override { shooter_heat_ = std::max(0, shooter_heat_ - *shooter_cooling_); - if (*bullet_fired_) - shooter_heat_ += heat_per_shot + 10; + if (*bullet_fired_ && !bullet_fired_false_) { + shooter_heat_ += heat_per_shot; + } + bullet_fired_false_ = *bullet_fired_; + + if (++cooling_settlement_tick_ >= kCoolingSettlementTicks) { + cooling_settlement_tick_ = 0; + shooter_heat_ = std::max( + 0, shooter_heat_ - *shooter_cooling_ * kCoolingPerSettlementScale); + } *control_bullet_allowance_ = std::max( 0, (*shooter_heat_limit_ - shooter_heat_ - reserved_heat) / heat_per_shot); + + *shooting_heat_ = static_cast(shooter_heat_); } private: @@ -44,7 +54,12 @@ class HeatController const int64_t heat_per_shot; const int64_t reserved_heat; + int cooling_settlement_tick_ = 0; + static constexpr int kCoolingSettlementTicks = 100; + static constexpr int kCoolingPerSettlementScale = 100; + bool bullet_fired_false_ = false; int64_t shooter_heat_ = 0; + OutputInterface shooting_heat_; OutputInterface control_bullet_allowance_; }; diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp index eadb30281..5874a49a0 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp @@ -75,17 +75,14 @@ class PutterController register_output("/gimbal/shoot/delay_ms", shoot_delay_ms_, nan_); // auto_aim - register_input("/auto_aim/should_shoot", should_shoot_, false); + register_input("/gimbal/auto_aim/fire_control", fire_control_, false); register_output("/gimbal/shooter/mode", shoot_mode_, rmcs_msgs::ShootMode::SINGLE); register_output("/gimbal/shooter/condiction", shoot_condiction_); register_output("/gimbal/shooter/preloaded_ready", preloaded_ready_, false); } - void before_updating() override { - if (!should_shoot_.ready()) - should_shoot_.bind_directly(false); - } + ~PutterController() {} void update() override { const auto switch_right = *switch_right_; @@ -147,8 +144,11 @@ class PutterController || (last_switch_left_ == rmcs_msgs::Switch::MIDDLE && switch_left == rmcs_msgs::Switch::DOWN); + // const bool auto_fire_now = (switch_right == Switch::UP) && + // (*fire_control_); const bool auto_fire_now = - (switch_right == Switch::UP || mouse.right) && *should_shoot_; + (switch_right == Switch::UP || (mouse.right && mouse.left)) + && (*fire_control_); const bool auto_trigger_emergence = mouse.right && (click_count_ >= 2); @@ -181,26 +181,34 @@ class PutterController if (shoot_stage_ == ShootStage::SHOOTING) { // Firing state: detect whether the bullet has been fired. - // if (*bullet_fired_ && !shooted) { - // RCLCPP_INFO(get_logger(), "DETECT: Bullet fired!"); - // shooted = true; - // } + if (*bullet_fired_ && !shooted) { + RCLCPP_INFO(get_logger(), "DETECT: Bullet fired!"); + shooted = true; + } - // if (*putter_angle_ - putter_startpoint >= putter_stroke_ && !shooted) { - // RCLCPP_INFO(get_logger(), "DETECT: Putter stroke completed!"); - // shooted = true; - // } + if (*putter_angle_ - putter_startpoint >= putter_stroke_ && !shooted) { + RCLCPP_INFO(get_logger(), "DETECT: Putter stroke completed!"); + shooted = true; + } + + update_putter_jam_detection(); if (shooted) { // Bullet fired: return the putter. - *putter_control_torque_ = - putter_return_velocity_pid_.update(-50. - *putter_velocity_); - putter_timeout_detection(); + const auto angle_err = putter_startpoint - *putter_angle_; + if (angle_err > -0.1) { + *putter_control_torque_ = 0.; + set_preloading(); + shooted = false; + } else { + *putter_control_torque_ = + putter_return_velocity_pid_.update(-80. - *putter_velocity_); + putter_timeout_detection(); + } } else { // Bullet not fired yet: continue advancing. *putter_control_torque_ = - putter_return_velocity_pid_.update(120. - *putter_velocity_); - update_putter_jam_detection(); + putter_return_velocity_pid_.update(60. - *putter_velocity_); } } } else { @@ -278,19 +286,22 @@ class PutterController // If the photoelectric sensor was not triggered, treat it as a simple jam, // reverse briefly, then continue until stall. locked_detect_count_ = 0; - enter_reverse_protection(); + enter_jam_protection(); } } void update_putter_jam_detection() { - if (std::abs(*putter_velocity_) > 0.1 || std::isnan(*putter_control_torque_)) { + if ((*putter_control_torque_ > -0.03 && shoot_stage_ == ShootStage::PRELOADING) + || (*putter_control_torque_ < 0.05 && shoot_stage_ == ShootStage::SHOOTING) + || std::isnan(*putter_control_torque_)) { putter_faulty_count_ = 0; - } else { - putter_faulty_count_++; + return; } // Accumulate a fault count when the torque is abnormal. - if (putter_faulty_count_ >= 50) { + if (putter_faulty_count_ < 500) + ++putter_faulty_count_; + else { putter_faulty_count_ = 0; if (shoot_stage_ != ShootStage::SHOOTING) { // Stall detected outside the firing state: the putter is in position, @@ -299,7 +310,7 @@ class PutterController putter_startpoint = *putter_angle_; } else { // Stall detected during firing: treat the bullet as fired. - RCLCPP_INFO(get_logger(), "DETECT: Putter freezed"); + RCLCPP_INFO(get_logger(), "DETECT: Putter jammed"); shooted = true; } } @@ -310,7 +321,7 @@ class PutterController // treat it as finished and move to the next state. if (shoot_stage_ == ShootStage::SHOOTING) { if (shooted) { - if (putter_timeout_count_ < 400) + if (putter_timeout_count_ < 1600) ++putter_timeout_count_; else { putter_timeout_count_ = 0; @@ -322,11 +333,12 @@ class PutterController } } - void enter_reverse_protection() { + void enter_jam_protection() { locked_detect_count_ = 0; bullet_feeder_faulty_count_ = 0; bullet_feeder_reverse_end_ = 400; bullet_feeder_velocity_pid_.reset(); + // RCLCPP_INFO(get_logger(), "Jammed!"); } static constexpr double nan_ = @@ -383,7 +395,7 @@ class PutterController OutputInterface shoot_delay_ms_; - InputInterface should_shoot_; + InputInterface fire_control_; std::chrono::steady_clock::time_point last_fire_time_{}; std::chrono::steady_clock::time_point last_click_time_{}; int click_count_ = 0; @@ -403,4 +415,4 @@ class PutterController #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::PutterController, rmcs_executor::Component) +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::PutterController, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp index 98851e89d..698fa16bc 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp @@ -12,8 +12,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -200,13 +200,14 @@ class SteeringHeroLittle }; std::shared_ptr command_component_; - class TopBoard final : public librmcs::board::RmcsBoardLite::Callback { + class TopBoard final : private librmcs::agent::RmcsBoardLite { public: friend class SteeringHeroLittle; explicit TopBoard( SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, std::string_view board_serial = {}) - : logger_(steering_hero.get_logger()) + : librmcs::agent::RmcsBoardLite(board_serial) + , logger_(steering_hero.get_logger()) // , can0_receive_rate_counter_(logger_, "bottom/can0") // , can1_receive_rate_counter_(logger_, "bottom/can1") // , can2_receive_rate_counter_(logger_, "bottom/can2") @@ -289,8 +290,6 @@ class SteeringHeroLittle return std::make_tuple(-y, x, z); }); - - board_ = std::make_unique(*this, board_serial); } TopBoard(const TopBoard&) = delete; @@ -341,27 +340,27 @@ class SteeringHeroLittle } void command_update() { - auto builder = board_->start_transmit(); + auto builder = start_transmit(); if (std::isfinite(gimbal_pitch_motor_.control_angle())) - builder.can_transmit(Spec::kCans.kCan0, { + builder.can0_transmit({ .can_id = 0x142, .can_data = gimbal_pitch_motor_ .generate_angle_command(gimbal_pitch_motor_.control_angle()) .as_bytes(), }); else - builder.can_transmit(Spec::kCans.kCan0, { + builder.can0_transmit({ .can_id = 0x142, .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), }); // Used to distinguish pitch encoder control from IMU control. - builder.can_transmit(Spec::kCans.kCan0, { + builder.can0_transmit({ .can_id = 0x141, .can_data = gimbal_top_yaw_motor_.generate_command().as_bytes(), }); - builder.can_transmit(Spec::kCans.kCan1, { + builder.can1_transmit({ .can_id = 0x200, .can_data = device::CanPacket8{ @@ -373,7 +372,7 @@ class SteeringHeroLittle .as_bytes(), }); - builder.can_transmit(Spec::kCans.kCan1, { + builder.can1_transmit({ .can_id = 0x1FF, .can_data = device::CanPacket8{ @@ -385,7 +384,7 @@ class SteeringHeroLittle .as_bytes(), }); - builder.can_transmit(Spec::kCans.kCan2, { + builder.can2_transmit({ .can_id = 0x143, .can_data = gimbal_player_viewer_motor_ @@ -393,20 +392,20 @@ class SteeringHeroLittle .as_bytes(), }); - builder.can_transmit(Spec::kCans.kCan3, { + builder.can3_transmit({ .can_id = 0x142, .can_data = gimbal_bullet_feeder_.generate_torque_command().as_bytes(), }); builder.gpio_digital_read( - Spec::kGpios[2], + librmcs::spec::rmcs_board_lite::kGpioDescriptors[2], { .period_ms = 20, .pull = librmcs::data::GpioPull::kUp, }); builder.gpio_digital_read( - Spec::kGpios[3], + librmcs::spec::rmcs_board_lite::kGpioDescriptors[3], { .period_ms = 20, .pull = librmcs::data::GpioPull::kUp, @@ -414,47 +413,61 @@ class SteeringHeroLittle } private: - void can_receive_callback(const Spec::Can& can, const View::Can& data) override { + void can0_receive_callback(const librmcs::data::CanDataView& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; auto can_id = data.can_id; - if (can == Spec::kCans.kCan0) { - // can0_receive_rate_counter_.record(can_id); - if (can_id == 0x141) { - gimbal_top_yaw_motor_.store_status(data.can_data); - } else if (can_id == 0x142) { - gimbal_pitch_motor_.store_status(data.can_data); - } - } else if (can == Spec::kCans.kCan1) { - // can1_receive_rate_counter_.record(can_id); - if (can_id == 0x201) { - gimbal_friction_wheels_[0].store_status(data.can_data); - } else if (can_id == 0x202) { - gimbal_friction_wheels_[1].store_status(data.can_data); - } else if (can_id == 0x203) { - gimbal_friction_wheels_[2].store_status(data.can_data); - } else if (can_id == 0x204) { - gimbal_friction_wheels_[3].store_status(data.can_data); - } else if (can_id == 0x205) { - putter_motor_.store_status(data.can_data); - } else if (can_id == 0x206) { - gimbal_scope_motor_.store_status(data.can_data); - } - } else if (can == Spec::kCans.kCan2) { - // can2_receive_rate_counter_.record(can_id); - if (can_id == 0x143) { - gimbal_player_viewer_motor_.store_status(data.can_data); - } - } else if (can == Spec::kCans.kCan3) { - // can3_receive_rate_counter_.record(can_id); - if (can_id == 0x142) { - gimbal_bullet_feeder_.store_status(data.can_data); - } + // can0_receive_rate_counter_.record(can_id); + if (can_id == 0x141) { + gimbal_top_yaw_motor_.store_status(data.can_data); + } else if (can_id == 0x142) { + gimbal_pitch_motor_.store_status(data.can_data); + } + } + + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + // can1_receive_rate_counter_.record(can_id); + if (can_id == 0x201) { + gimbal_friction_wheels_[0].store_status(data.can_data); + } else if (can_id == 0x202) { + gimbal_friction_wheels_[1].store_status(data.can_data); + } else if (can_id == 0x203) { + gimbal_friction_wheels_[2].store_status(data.can_data); + } else if (can_id == 0x204) { + gimbal_friction_wheels_[3].store_status(data.can_data); + } else if (can_id == 0x205) { + putter_motor_.store_status(data.can_data); + } else if (can_id == 0x206) { + gimbal_scope_motor_.store_status(data.can_data); + } + } + + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + // can2_receive_rate_counter_.record(can_id); + if (can_id == 0x143) { + gimbal_player_viewer_motor_.store_status(data.can_data); + } + } + + void can3_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + // can3_receive_rate_counter_.record(can_id); + if (can_id == 0x142) { + gimbal_bullet_feeder_.store_status(data.can_data); } } void gpio_digital_read_result_callback( - const Spec::Gpio& gpio, const View::GpioDigital& data) override { + const librmcs::spec::rmcs_board_lite::GpioDescriptor& gpio, + const librmcs::data::GpioDigitalDataView& data) override { if (gpio.channel_index == 2) { photoelectric_sensor_status_atomic.store(data.high); } else if (gpio.channel_index == 3) { @@ -462,11 +475,12 @@ class SteeringHeroLittle } } - void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { imu_.store_accelerometer_status(data.x, data.y, data.z); } - void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { imu_.store_gyroscope_status(data.x, data.y, data.z); } @@ -496,16 +510,17 @@ class SteeringHeroLittle OutputInterface camera_capturer_trigger_timestamp_; std::atomic photoelectric_sensor_status_atomic{false}; std::atomic grayscale_sensor_status_atomic{false}; - std::unique_ptr board_; }; - class BottomBoard final : public librmcs::board::RmcsBoardLite::Callback { + class BottomBoard final : private librmcs::agent::RmcsBoardLite { public: friend class SteeringHeroLittle; explicit BottomBoard( SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, std::string_view board_serial = {}) - : logger_(steering_hero.get_logger()) + : librmcs::agent::RmcsBoardLite( + board_serial, {.dangerously_skip_version_checks = false}) + , logger_(steering_hero.get_logger()) // , can0_receive_rate_counter_(logger_, "bottom/can0") // , can1_receive_rate_counter_(logger_, "bottom/can1") // , can2_receive_rate_counter_(logger_, "bottom/can2") @@ -529,6 +544,7 @@ class SteeringHeroLittle , chassis_back_climber_motor_( {steering_hero, steering_hero_command, "/chassis/climber/left_back_motor"}, {steering_hero, steering_hero_command, "/chassis/climber/right_back_motor"}) + , yaw_brake_motor_(steering_hero, steering_hero_command, "/gimbal/yaw_brake") , gimbal_bottom_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/bottom_yaw") { // chassis_steering_motors_[0].configure( @@ -588,6 +604,8 @@ class SteeringHeroLittle .enable_multi_turn_angle() .set_reduction_ratio(19.)); + yaw_brake_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.set_reduction_ratio(1.)); gimbal_bottom_yaw_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} .set_reversed() @@ -602,8 +620,7 @@ class SteeringHeroLittle [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); }; referee_serial_->write = [this](const std::byte* buffer, size_t size) { - board_->start_transmit().uart_transmit( - Spec::kUarts.kUart0, + start_transmit().uart0_transmit( {.uart_data = std::span{buffer, size}}); return size; }; @@ -615,10 +632,6 @@ class SteeringHeroLittle steering_hero.register_output( "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); steering_hero.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); - - board_ = std::make_unique( - *this, board_serial, - librmcs::board::AdvancedOptions{}); } BottomBoard(const BottomBoard&) = delete; @@ -651,13 +664,24 @@ class SteeringHeroLittle for (auto& motor : chassis_steering_motors_) motor.update_status(); + yaw_brake_motor_.update_status(); gimbal_bottom_yaw_motor_.update_status(); + + if (++count_ == 500) { + for (int i = 0; i < 8; ++i) { + if (check[i] == 0) { + // RCLCPP_WARN(logger_, "can id 0x%03X missing", i + 0x201); + } + } + std::fill_n(check, 8, 0); + count_ = 0; + } } void command_update() { - auto builder = board_->start_transmit(); + auto builder = start_transmit(); - builder.can_transmit(Spec::kCans.kCan0, { + builder.can0_transmit({ .can_id = 0x200, .can_data = device::CanPacket8{ @@ -669,7 +693,7 @@ class SteeringHeroLittle .as_bytes(), }); - builder.can_transmit(Spec::kCans.kCan0, { + builder.can0_transmit({ .can_id = 0x1FE, .can_data = device::CanPacket8{ @@ -681,7 +705,7 @@ class SteeringHeroLittle .as_bytes(), }); - builder.can_transmit(Spec::kCans.kCan1, { + builder.can1_transmit({ .can_id = 0x200, .can_data = device::CanPacket8{ @@ -693,7 +717,7 @@ class SteeringHeroLittle .as_bytes(), }); - builder.can_transmit(Spec::kCans.kCan1, { + builder.can1_transmit({ .can_id = 0x1FE, .can_data = device::CanPacket8{ @@ -705,24 +729,24 @@ class SteeringHeroLittle .as_bytes(), }); - builder.can_transmit(Spec::kCans.kCan3, { + builder.can3_transmit({ .can_id = 0x200, .can_data = device::CanPacket8{ device::CanPacket8::PaddingQuarter{}, chassis_back_climber_motor_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, + yaw_brake_motor_.generate_command(), chassis_back_climber_motor_[0].generate_command(), } .as_bytes(), }); - builder.can_transmit(Spec::kCans.kCan3, { + builder.can3_transmit({ .can_id = 0x141, .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), }); - builder.can_transmit(Spec::kCans.kCan2, { + builder.can2_transmit({ .can_id = 0x200, .can_data = device::CanPacket8{ @@ -736,69 +760,88 @@ class SteeringHeroLittle } private: - void can_receive_callback(const Spec::Can& can, const View::Can& data) override { + void can0_receive_callback(const librmcs::data::CanDataView& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; auto can_id = data.can_id; - if (can == Spec::kCans.kCan0) { - // can0_receive_rate_counter_.record(can_id); - if (can_id == 0x201) { - chassis_wheel_motors_[0].store_status(data.can_data); - } else if (can_id == 0x202) { - chassis_wheel_motors_[1].store_status(data.can_data); - } else if (can_id == 0x205) { - chassis_steering_motors_[1].store_status(data.can_data); - } else if (can_id == 0x208) { - chassis_steering_motors_[0].store_status(data.can_data); - } - } else if (can == Spec::kCans.kCan1) { - // can1_receive_rate_counter_.record(can_id); - if (can_id == 0x203) { - chassis_wheel_motors_[2].store_status(data.can_data); - } else if (can_id == 0x204) { - chassis_wheel_motors_[3].store_status(data.can_data); - } else if (can_id == 0x207) { - chassis_steering_motors_[2].store_status(data.can_data); - } else if (can_id == 0x206) { - chassis_steering_motors_[3].store_status(data.can_data); - } else if (can_id == 0x300) { - supercap_.store_status(data.can_data); - } - } else if (can == Spec::kCans.kCan2) { - // can2_receive_rate_counter_.record(can_id); - if (can_id == 0x201) { - chassis_front_climber_motor_[0].store_status(data.can_data); - } else if (can_id == 0x203) { - chassis_front_climber_motor_[1].store_status(data.can_data); - } - } else if (can == Spec::kCans.kCan3) { - // can3_receive_rate_counter_.record(can_id); - if (can_id == 0x202) { - chassis_back_climber_motor_[1].store_status(data.can_data); - } else if (can_id == 0x204) { - chassis_back_climber_motor_[0].store_status(data.can_data); - } else if (can_id == 0x141) { - gimbal_bottom_yaw_motor_.store_status(data.can_data); - } + // can0_receive_rate_counter_.record(can_id); + check[can_id - 0x201] = 1; + if (can_id == 0x201) { + chassis_wheel_motors_[0].store_status(data.can_data); + } else if (can_id == 0x202) { + chassis_wheel_motors_[1].store_status(data.can_data); + } else if (can_id == 0x205) { + chassis_steering_motors_[1].store_status(data.can_data); + } else if (can_id == 0x208) { + chassis_steering_motors_[0].store_status(data.can_data); + } + } + + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + // can1_receive_rate_counter_.record(can_id); + if (can_id != 0x300) { + check[can_id - 0x201] = 1; + } + if (can_id == 0x203) { + chassis_wheel_motors_[2].store_status(data.can_data); + } else if (can_id == 0x204) { + chassis_wheel_motors_[3].store_status(data.can_data); + } else if (can_id == 0x207) { + chassis_steering_motors_[2].store_status(data.can_data); + } else if (can_id == 0x206) { + chassis_steering_motors_[3].store_status(data.can_data); + } else if (can_id == 0x300) { + supercap_.store_status(data.can_data); } } - void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { - if (uart == Spec::kUarts.kUart0) { - const std::byte* ptr = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, - data.uart_data.size()); - } else if (uart == Spec::kUarts.kDbus) { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) + return; + auto can_id = data.can_id; + // can2_receive_rate_counter_.record(can_id); + if (can_id == 0x201) { + chassis_front_climber_motor_[0].store_status(data.can_data); + } else if (can_id == 0x203) { + chassis_front_climber_motor_[1].store_status(data.can_data); } } - void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { + void can3_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + // can3_receive_rate_counter_.record(can_id); + if (can_id == 0x202) { + chassis_back_climber_motor_[1].store_status(data.can_data); + } else if (can_id == 0x204) { + chassis_back_climber_motor_[0].store_status(data.can_data); + } else if (can_id == 0x203) { + yaw_brake_motor_.store_status(data.can_data); + } else if (can_id == 0x141) { + gimbal_bottom_yaw_motor_.store_status(data.can_data); + } + } + + void uart0_receive_callback(const librmcs::data::UartDataView& data) override { + const std::byte* ptr = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, data.uart_data.size()); + } + + void dbus_receive_callback(const librmcs::data::UartDataView& data) override { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + } + + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { imu_.store_accelerometer_status(data.x, data.y, data.z); } - void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { imu_.store_gyroscope_status(data.x, data.y, data.z); } @@ -808,6 +851,9 @@ class SteeringHeroLittle // CanReceiveRateCounter can2_receive_rate_counter_; // CanReceiveRateCounter can3_receive_rate_counter_; + int count_ = 0; + int check[10] = {0}; + device::Bmi088 imu_; device::Dr16 dr16_; device::Supercap supercap_; @@ -816,6 +862,7 @@ class SteeringHeroLittle device::DjiMotor chassis_wheel_motors_[4]; device::DjiMotor chassis_front_climber_motor_[2]; device::DjiMotor chassis_back_climber_motor_[2]; + device::DjiMotor yaw_brake_motor_; device::LkMotor gimbal_bottom_yaw_motor_; rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; @@ -825,7 +872,6 @@ class SteeringHeroLittle OutputInterface powermeter_charge_power_limit_; OutputInterface chassis_yaw_velocity_imu_; OutputInterface chassis_pitch_imu_; - std::unique_ptr board_; }; OutputInterface tf_; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp index 6fe24a0c1..2ab4bd450 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp @@ -42,6 +42,10 @@ class Hero , bottom_yaw_angle_number_( Shape::Color::YELLOW, 20, 5, x_center + 270, y_center - 65, 0.0, false) , time_reminder_(Shape::Color::PINK, 50, 5, x_center + 150, y_center + 65, 0, false) + // , bullet_allowance_label_( + // Shape::Color::YELLOW, 18, 3, x_center - 300, y_center + 270, "bullet", false) + // , bullet_allowance_number_( + // Shape::Color::YELLOW, 20, 5, x_center - 170, y_center + 270, 0, false) , bullet_allowance_number_( Shape::Color::YELLOW, 20, 5, x_center - 220, y_center + 270, 0, false) , friction_profile_number_( @@ -80,9 +84,9 @@ class Hero register_input("/gimbal/control_bullet_allowance/limited_by_heat", robot_bullet_allowance_); register_input( - "/gimbal/first_back_friction/control_velocity", back_friction_control_velocity_); - register_input("/gimbal/first_back_friction/velocity", back_friction_velocity_); - register_input("/gimbal/first_front_friction/velocity", front_friction_velocity_); + "/gimbal/first_left_friction/control_velocity", left_friction_control_velocity_); + register_input("/gimbal/first_left_friction/velocity", left_friction_velocity_); + register_input("/gimbal/first_right_friction/velocity", right_friction_velocity_); register_input("/gimbal/friction_profile_1_active", friction_profile_1_active_, false); // register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); @@ -93,6 +97,9 @@ class Hero register_input("/gimbal/bottom_yaw/raw_angle", bottom_yaw_raw_angle_); // register_input("/gimbal/auto_aim/laser_distance", laser_distance_); + register_input("/gimbal/shooter/condiction", shoot_condiction_); + + register_input("/gimbal/shooter/mode", shoot_mode_); register_input("/gimbal/shooter/preloaded_ready", shooter_preloaded_ready_, false); // register_input("/gimbal/scope/active", is_scope_active_); @@ -100,11 +107,16 @@ class Hero register_input("/remote/mouse", mouse_); register_input("/referee/game/stage", game_stage_); + + // register_input("/gimbal/auto_aim/fire_control", auto_aim_fire_control_, false); + // register_input("/gimbal/auto_aim/target_confidence", auto_aim_target_confidence_, false); } void update() override { update_normal_ui(); + // update_bullet_allowance(); // update_sniper_ui(); + // update_state_word(); // if (*is_scope_active_) { // set_normal_ui_visible(false); @@ -134,6 +146,7 @@ class Hero yaw_angle_number_.set_visible(value); pitch_angle_number_.set_visible(value); bottom_yaw_angle_number_.set_visible(value); + // bullet_allowance_label_.set_visible(value); bullet_allowance_number_.set_visible(value); friction_profile_number_.set_visible(value); // center_green_line_.set_visible(value); @@ -210,10 +223,16 @@ class Hero friction_profile_indicator_[3].set_x2(box_left); friction_profile_indicator_[3].set_y2(box_bottom); status_ring_.update_friction_wheel_speed( - std::min(*back_friction_velocity_, *front_friction_velocity_), - *back_friction_control_velocity_ > 0); + std::min(*left_friction_velocity_, *right_friction_velocity_), + *left_friction_control_velocity_ > 0); status_ring_.update_supercap(*supercap_voltage_, true); status_ring_.update_battery_power(*chassis_voltage_); + // const bool auto_aim_locked = auto_aim_fire_control_.ready() && *auto_aim_fire_control_; + // const double target_confidence_value = + // auto_aim_target_confidence_.ready() ? *auto_aim_target_confidence_ : 0.0; + + // status_ring_.update_auto_aim_feedback(auto_aim_locked, target_confidence_value); + // update_static_status_ring(); last_keyboard_ = *keyboard_; } @@ -304,6 +323,50 @@ class Hero return; } + void update_static_status_ring() { + auto auto_aim_enable = mouse_->right == 1; + auto precise_enable = *shoot_mode_ == rmcs_msgs::ShootMode::PRECISE; + + status_ring_.update_static_parts({auto_aim_enable, precise_enable}); + } + + // void update_bullet_allowance() { + + // std::string text = "BULLET : " + std::to_string(max(0,*robot_bullet_allowance_)); + // char* allow = text.data(); + // auto color = Shape::Color::YELLOW; + + // bullet_allowance_number_.set_value(allow); + // bullet_allowance_number_.set_font_size(14); + // bullet_allowance_number_.set_color(color); + // bullet_allowance_number_.set_visible(true); + // bullet_allowance_number_.set_xy(x_center - 240, y_center + 288); + // } + + void update_state_word() { + + const char* text = "OK"; + auto color = Shape::Color::GREEN; + + if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::FRICTION_WAITING) { + text = " WAITING "; + } else if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::SHOOT) { + text = " SHOOT "; + } else if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::FIRED) { + text = " FIRED "; + } else if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::JAM) { + text = " JAM "; + } else if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::PRELOADING) { + text = "PRELOADING"; + } + + state_word_.set_value(text); + state_word_.set_font_size(30); + state_word_.set_color(color); + state_word_.set_visible(true); + state_word_.set_xy(x_center - 800, y_center + 200); + } + void update_chassis_direction_indicator() { auto chassis_mode = *chassis_mode_; @@ -318,7 +381,7 @@ class Hero return static_cast(degrees); }; // chassis_direction_indicator_.set_color( - // chassis_mode == rmcs_msgs::ChassisMode::SPIN_FAST ? Shape::Color::GREEN + // chassis_mode == rmcs_msgs::ChassisMode::SPIN ? Shape::Color::GREEN // : Shape::Color::PINK); // chassis_direction_indicator_.set_angle(to_referee_angle(*chassis_angle_), 30); const bool left_track_active = @@ -418,9 +481,9 @@ class Hero InputInterface robot_bullet_allowance_; - InputInterface back_friction_control_velocity_; - InputInterface back_friction_velocity_; - InputInterface front_friction_velocity_; + InputInterface left_friction_control_velocity_; + InputInterface left_friction_velocity_; + InputInterface right_friction_velocity_; InputInterface friction_profile_1_active_; InputInterface mouse_; @@ -436,6 +499,8 @@ class Hero InputInterface bottom_yaw_angle_; // InputInterface laser_distance_; + InputInterface shoot_mode_; + InputInterface shoot_condiction_; InputInterface shooter_preloaded_ready_; // InputInterface is_scope_active_; @@ -453,6 +518,7 @@ class Hero Text state_word_; Integer time_reminder_; + // Text bullet_allowance_label_; Integer bullet_allowance_number_; Integer friction_profile_number_; Line friction_profile_indicator_[4]; @@ -461,10 +527,13 @@ class Hero bool bottom_yaw_tracking_enabled_ = false; double bottom_yaw_anchor_angle_rad_ = 0.0; + + // InputInterface auto_aim_fire_control_; + // InputInterface auto_aim_target_confidence_; }; } // namespace rmcs_core::referee::app::ui #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::Hero, rmcs_executor::Component) +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::Hero, rmcs_executor::Component) \ No newline at end of file From c6bcd4e19d24c305cae9e1f1d35fcbbab86a453c Mon Sep 17 00:00:00 2001 From: dwx5 <1591215786@qq.com> Date: Sat, 6 Jun 2026 20:11:57 +0800 Subject: [PATCH 23/86] add yaw brake --- .../controller/gimbal/dual_yaw_controller.cpp | 248 ++++++++++++++++-- .../gimbal/hero_gimbal_controller.cpp | 39 +-- 2 files changed, 224 insertions(+), 63 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp index 56596d598..84f8c0862 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp @@ -1,5 +1,6 @@ #include +#include #include #include @@ -39,18 +40,22 @@ class DualYawController register_input("/gimbal/top_yaw/velocity", top_yaw_velocity_); register_input("/gimbal/bottom_yaw/angle", bottom_yaw_angle_); register_input("/gimbal/bottom_yaw/velocity", bottom_yaw_velocity_); + register_input("/gimbal/bottom_yaw/raw_angle", bottom_yaw_raw_angle_); register_input("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); register_input("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_); + register_input("/gimbal/yaw_brake/velocity", yaw_brake_velocity_); register_input("/gimbal/mode", gimbal_mode_); register_input("/gimbal/yaw/control_angle_error", control_angle_error_); register_input("/gimbal/yaw/control_angle_shift", control_angle_shift_, false); + register_input("/gimbal/bottom_yaw/torque", bottom_yaw_torque_); register_output("/gimbal/top_yaw/control_torque", top_yaw_control_torque_, 0.0); register_output("/gimbal/bottom_yaw/control_torque", bottom_yaw_control_torque_, 0.0); register_output("/gimbal/bottom_yaw/control_angle", bottom_yaw_control_angle_, nan_); register_output("/gimbal/top_yaw/control_angle_shift", top_yaw_control_angle_shift_, nan_); + register_output("/gimbal/yaw_brake/control_torque", yaw_brake_control_torque_, nan_); status_component_ = create_partner_component(get_component_name() + "_status"); @@ -67,38 +72,203 @@ class DualYawController void update() override { const auto mode = *gimbal_mode_; - if (mode == rmcs_msgs::GimbalMode::ENCODER) { - const bool entering_encoder = last_gimbal_mode_ != rmcs_msgs::GimbalMode::ENCODER; + const bool entering_encoder = mode == rmcs_msgs::GimbalMode::ENCODER + && last_gimbal_mode_ != rmcs_msgs::GimbalMode::ENCODER; - if (entering_encoder) { - if (std::isfinite(*bottom_yaw_angle_)) { - bottom_yaw_encoder_angle_ = *bottom_yaw_angle_; - bottom_yaw_encoder_locked_ = true; - bottom_yaw_angle_pid_.reset(); - bottom_yaw_velocity_pid_.reset(); - } else { - bottom_yaw_encoder_angle_ = nan_; - bottom_yaw_encoder_locked_ = false; - } + const bool leaving_encoder = mode != rmcs_msgs::GimbalMode::ENCODER + && last_gimbal_mode_ == rmcs_msgs::GimbalMode::ENCODER; + + if (entering_encoder) { + top_yaw_angle_pid_.reset(); + top_yaw_velocity_pid_.reset(); + bottom_yaw_angle_pid_.reset(); + bottom_yaw_velocity_pid_.reset(); + + yaw_brake_engage_count_ = 0; + yaw_brake_release_elapsed_count_ = 0; + yaw_brake_release_stop_count_ = 0; + yaw_brake_release_seen_motion_ = false; + encoder_state_ = EncoderState::AlignTargetRawAngle; + } + + if (leaving_encoder) { + top_yaw_angle_pid_.reset(); + top_yaw_velocity_pid_.reset(); + bottom_yaw_angle_pid_.reset(); + bottom_yaw_velocity_pid_.reset(); + + if (encoder_state_ == EncoderState::EngageYawBrake + || encoder_state_ == EncoderState::BrakeLocked) { + yaw_brake_release_elapsed_count_ = 0; + yaw_brake_release_stop_count_ = 0; + yaw_brake_release_seen_motion_ = false; + encoder_state_ = EncoderState::ReleaseYawBrake; + } else { + yaw_brake_engage_count_ = 0; + yaw_brake_release_elapsed_count_ = 0; + yaw_brake_release_stop_count_ = 0; + yaw_brake_release_seen_motion_ = false; + *yaw_brake_control_torque_ = nan_; + encoder_state_ = EncoderState::Idle; } + } + if (mode == rmcs_msgs::GimbalMode::ENCODER && encoder_state_ == EncoderState::Idle) { + top_yaw_angle_pid_.reset(); + top_yaw_velocity_pid_.reset(); + bottom_yaw_angle_pid_.reset(); + bottom_yaw_velocity_pid_.reset(); + + yaw_brake_engage_count_ = 0; + yaw_brake_release_elapsed_count_ = 0; + yaw_brake_release_stop_count_ = 0; + yaw_brake_release_seen_motion_ = false; + encoder_state_ = EncoderState::AlignTargetRawAngle; + } + // RCLCPP_INFO(get_logger(), "bottom_yaw_raw_angle: %ld", *bottom_yaw_raw_angle_); + // RCLCPP_INFO(get_logger(), "bottom_yaw_control_torque: %f", *bottom_yaw_control_torque_); + // RCLCPP_INFO(get_logger(), "bottom_yaw_torque: %f", *bottom_yaw_torque_); + // RCLCPP_INFO( + // get_logger(), "encoder_state: %s", + // encoder_state_ == EncoderState::Idle ? "Idle" + // : encoder_state_ == EncoderState::AlignTargetRawAngle ? "AlignTargetRawAngle" + // : encoder_state_ == EncoderState::EngageYawBrake ? "EngageYawBrake" + // : encoder_state_ == EncoderState::BrakeLocked ? "BrakeLocked" + // : "ReleaseYawBrake"); + const bool hold_encoder_for_brake_release = encoder_state_ == EncoderState::ReleaseYawBrake; + + if (mode == rmcs_msgs::GimbalMode::ENCODER || hold_encoder_for_brake_release) { *top_yaw_control_torque_ = nan_; *bottom_yaw_control_angle_ = nan_; - *top_yaw_control_angle_shift_ = *control_angle_shift_; - if (bottom_yaw_encoder_locked_) { - double err = std::remainder( - bottom_yaw_encoder_angle_ - *bottom_yaw_angle_, 2.0 * std::numbers::pi); + auto wrap_raw_delta = [](int64_t diff) -> int64_t { + diff %= kBottomYawRawAngleModulus; + if (diff <= -(kBottomYawRawAngleModulus / 2)) + diff += kBottomYawRawAngleModulus; + else if (diff > (kBottomYawRawAngleModulus / 2)) + diff -= kBottomYawRawAngleModulus; + return diff; + }; + + if (encoder_state_ == EncoderState::AlignTargetRawAngle) { + *top_yaw_control_angle_shift_ = 0.0; + *yaw_brake_control_torque_ = nan_; + + if (!bottom_yaw_raw_angle_.ready()) { + *bottom_yaw_control_torque_ = nan_; + } else { + const int64_t raw_error_count = + wrap_raw_delta(kEncoderBottomYawTargetRawAngle - *bottom_yaw_raw_angle_); + const int64_t raw_error_abs = + raw_error_count >= 0 ? raw_error_count : -raw_error_count; + + const bool bottom_yaw_aligned = + raw_error_abs <= kEncoderBottomYawTargetToleranceRawAngle + && std::abs(*bottom_yaw_velocity_) + <= kEncoderBottomYawLockVelocityThreshold; + + if (bottom_yaw_aligned) { + *bottom_yaw_control_torque_ = nan_; + bottom_yaw_angle_pid_.reset(); + bottom_yaw_velocity_pid_.reset(); + yaw_brake_engage_count_ = 0; + encoder_state_ = EncoderState::EngageYawBrake; + } else { + const double raw_error_angle = + kEncoderBottomYawRawAngleErrorSign + * static_cast(raw_error_count) + / static_cast(kBottomYawRawAngleModulus) * 2.0 + * std::numbers::pi; + + const double target_velocity = + bottom_yaw_angle_pid_.update(raw_error_angle); + const double velocity_error = target_velocity - *bottom_yaw_velocity_; + double control_torque = bottom_yaw_velocity_pid_.update(velocity_error); + + constexpr double kBreakawayTorque = 2.5; + constexpr double kBreakawayVelocityThreshold = 0.10; + constexpr int64_t kBreakawayErrorThreshold = 80; + + if (std::abs(*bottom_yaw_velocity_) < kBreakawayVelocityThreshold + && std::abs(raw_error_count) > kBreakawayErrorThreshold + && std::abs(control_torque) < kBreakawayTorque) { + control_torque = + velocity_error >= 0.0 ? kBreakawayTorque : -kBreakawayTorque; + } + + *bottom_yaw_control_torque_ = control_torque; + } + } + + } else if (encoder_state_ == EncoderState::EngageYawBrake) { + *top_yaw_control_angle_shift_ = 0.0; + *bottom_yaw_control_torque_ = nan_; + *yaw_brake_control_torque_ = kYawBrakeEngageTorque; + + if (std::abs(*yaw_brake_velocity_) < kYawBrakeEngageVelocityThreshold) { + ++yaw_brake_engage_count_; + } else { + yaw_brake_engage_count_ = 0; + } + + if (yaw_brake_engage_count_ >= kYawBrakeEngageConfirmCount) { + yaw_brake_engage_count_ = 0; + *yaw_brake_control_torque_ = nan_; + encoder_state_ = EncoderState::BrakeLocked; + } + + } else if (encoder_state_ == EncoderState::BrakeLocked) { + constexpr double kBrakeLockedHoldTorque = 0.6; + *bottom_yaw_control_torque_ = kBrakeLockedHoldTorque; + *yaw_brake_control_torque_ = nan_; + *top_yaw_control_angle_shift_ = *control_angle_shift_; + + } else if (encoder_state_ == EncoderState::ReleaseYawBrake) { + *top_yaw_control_angle_shift_ = 0.0; + *bottom_yaw_control_torque_ = nan_; + *yaw_brake_control_torque_ = kYawBrakeReleaseTorque; + + ++yaw_brake_release_elapsed_count_; + + if (std::abs(*yaw_brake_velocity_) > kYawBrakeReleaseMotionVelocityThreshold) { + yaw_brake_release_seen_motion_ = true; + } + + if (yaw_brake_release_seen_motion_ + && std::abs(*yaw_brake_velocity_) < kYawBrakeReleaseStopVelocityThreshold) { + ++yaw_brake_release_stop_count_; + } else { + yaw_brake_release_stop_count_ = 0; + } + + if (yaw_brake_release_stop_count_ >= kYawBrakeReleaseStopConfirmCount) { + yaw_brake_engage_count_ = 0; + yaw_brake_release_elapsed_count_ = 0; + yaw_brake_release_stop_count_ = 0; + yaw_brake_release_seen_motion_ = false; + *yaw_brake_control_torque_ = nan_; + encoder_state_ = EncoderState::Idle; + } else if (yaw_brake_release_elapsed_count_ >= kYawBrakeReleaseTimeoutCount) { + RCLCPP_WARN( + get_logger(), "Yaw brake release timed out, stop applying reverse torque " + "for protection."); + yaw_brake_engage_count_ = 0; + yaw_brake_release_elapsed_count_ = 0; + yaw_brake_release_stop_count_ = 0; + yaw_brake_release_seen_motion_ = false; + *yaw_brake_control_torque_ = nan_; + encoder_state_ = EncoderState::Idle; + } - double target_velocity = bottom_yaw_angle_pid_.update(err); - *bottom_yaw_control_torque_ = - bottom_yaw_velocity_pid_.update(target_velocity - bottom_yaw_velocity_imu()); } else { + *top_yaw_control_angle_shift_ = 0.0; *bottom_yaw_control_torque_ = nan_; + *yaw_brake_control_torque_ = nan_; } - // *bottom_yaw_control_torque_ = nan_; } else { + *yaw_brake_control_torque_ = nan_; + *top_yaw_control_torque_ = top_yaw_velocity_pid_.update( top_yaw_angle_pid_.update(*control_angle_error_) - *gimbal_yaw_velocity_imu_); @@ -108,20 +278,41 @@ class DualYawController *bottom_yaw_control_angle_ = nan_; *top_yaw_control_angle_shift_ = nan_; - bottom_yaw_encoder_locked_ = false; } last_gimbal_mode_ = mode; } private: + enum class EncoderState { + Idle, + AlignTargetRawAngle, + EngageYawBrake, + BrakeLocked, + ReleaseYawBrake + }; static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr int64_t kBottomYawRawAngleModulus = 1 << 16; + static constexpr int64_t kEncoderBottomYawTargetRawAngle = 2434; + static constexpr int64_t kEncoderBottomYawTargetToleranceRawAngle = 80; + static constexpr double kEncoderBottomYawLockVelocityThreshold = 0.15; + static constexpr double kEncoderBottomYawRawAngleErrorSign = -1.0; + + static constexpr double kYawBrakeEngageTorque = -0.3; + static constexpr double kYawBrakeEngageVelocityThreshold = 0.1; + static constexpr int kYawBrakeEngageConfirmCount = 50; + + static constexpr double kYawBrakeReleaseTorque = 0.3; + static constexpr double kYawBrakeReleaseMotionVelocityThreshold = 0.2; + static constexpr double kYawBrakeReleaseStopVelocityThreshold = 0.08; + static constexpr int kYawBrakeReleaseStopConfirmCount = 20; + static constexpr int kYawBrakeReleaseTimeoutCount = 400; + double bottom_yaw_control_error() { if (!std::isfinite(*top_yaw_angle_) || !std::isfinite(*control_angle_error_)) return nan_; - // Avoid relying on top_yaw_angle in [0, 2pi) and control_angle_error in [-pi, pi]. constexpr double alignment = 2 * std::numbers::pi; double err = std::fmod(*top_yaw_angle_ + *control_angle_error_ + std::numbers::pi, alignment); @@ -135,24 +326,31 @@ class DualYawController InputInterface top_yaw_angle_, top_yaw_velocity_; InputInterface bottom_yaw_angle_, bottom_yaw_velocity_; + InputInterface bottom_yaw_raw_angle_; InputInterface gimbal_yaw_velocity_imu_, chassis_yaw_velocity_imu_; + InputInterface yaw_brake_velocity_; InputInterface gimbal_mode_; InputInterface control_angle_error_, control_angle_shift_; + InputInterface bottom_yaw_torque_; pid::PidCalculator top_yaw_angle_pid_, top_yaw_velocity_pid_; pid::PidCalculator bottom_yaw_angle_pid_, bottom_yaw_velocity_pid_; OutputInterface top_yaw_control_torque_; OutputInterface bottom_yaw_control_torque_; - OutputInterface bottom_yaw_control_angle_; OutputInterface top_yaw_control_angle_shift_; + OutputInterface yaw_brake_control_torque_; rmcs_msgs::GimbalMode last_gimbal_mode_ = rmcs_msgs::GimbalMode::IMU; - bool bottom_yaw_encoder_locked_ = false; - double bottom_yaw_encoder_angle_ = nan_; + EncoderState encoder_state_ = EncoderState::Idle; + + int yaw_brake_engage_count_ = 0; + int yaw_brake_release_elapsed_count_ = 0; + int yaw_brake_release_stop_count_ = 0; + bool yaw_brake_release_seen_motion_ = false; class DualYawStatus : public rmcs_executor::Component { public: diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp index 2cd9be969..6dcffe1c8 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp @@ -40,10 +40,6 @@ class HeroGimbalController register_input("/gimbal/auto_aim/control_direction", auto_aim_control_direction_, false); register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); register_input("/gimbal/pitch/raw_angle", gimbal_pitch_raw_angle_); - register_input("/gimbal/yaw_brake/angle", gimbal_yaw_brake_angle_); - register_input("/gimbal/yaw_brake/torque", gimbal_yaw_brake_torque_); - register_input("/gimbal/yaw_brake/velocity", gimbal_yaw_brake_velocity_); - // 锁住时复方向 register_input("/tf", tf_); register_output("/gimbal/mode", gimbal_mode_, rmcs_msgs::GimbalMode::IMU); @@ -52,7 +48,6 @@ class HeroGimbalController register_output("/gimbal/pitch/control_angle_error", pitch_angle_error_, nan_); register_output("/gimbal/yaw/control_angle_shift", yaw_control_angle_shift_, nan_); register_output("/gimbal/pitch/control_angle", pitch_control_angle_, nan_); - register_output("/gimbal/yaw_brake/control_torque", yaw_brake_control_torque_, nan_); } void update() override { @@ -67,7 +62,6 @@ class HeroGimbalController break; } - // RCLCPP_INFO(get_logger(), "yaw_brake_torque: %f", *gimbal_yaw_brake_torque_); if (!last_keyboard_.e && keyboard_->e) { if (gimbal_mode_keyboard_ == GimbalMode::IMU) { encoder_init_pitch_ = keyboard_->ctrl ? kCtrlEInitPitch : kEInitPitch; @@ -122,25 +116,6 @@ class HeroGimbalController *pitch_angle_error_ = nan_; *yaw_control_angle_shift_ = nan_; *pitch_control_angle_ = nan_; - *yaw_brake_control_torque_ = nan_; - - yaw_brake_count_ = 0; - yaw_brake_locked_ = false; - } - - void yaw_brake_update() { - if (std::abs(*yaw_brake_control_torque_) > 0.01 - && std::abs(*gimbal_yaw_brake_velocity_) < 0.1) { - yaw_brake_count_++; - } else { - yaw_brake_count_ = 0; - } - - if (yaw_brake_count_ > 50) { - yaw_brake_count_ = 0; - yaw_brake_locked_ = true; - *yaw_brake_control_torque_ = nan_; - } } TwoAxisGimbalSolver::AngleError update_imu_control() { @@ -193,12 +168,6 @@ class HeroGimbalController double pitch_shift = -joystick_sensitivity * joystick_left_->x() + mouse_pitch_sensitivity * mouse_velocity_->x(); - if (!yaw_brake_locked_) { - *yaw_brake_control_torque_ = -0.3; - } - - yaw_brake_update(); - return encoder_gimbal_solver.update( PreciseTwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); } @@ -208,6 +177,7 @@ class HeroGimbalController static constexpr double kEInitPitch = -0.346584; // Initial angle for standalone E. static constexpr double kCtrlEInitPitch = -0.471795; // Initial angle for Ctrl+E. + double encoder_init_pitch_ = kEInitPitch; InputInterface joystick_left_; InputInterface switch_right_; @@ -221,9 +191,6 @@ class HeroGimbalController InputInterface auto_aim_control_direction_; InputInterface gimbal_pitch_angle_; InputInterface gimbal_pitch_raw_angle_; - InputInterface gimbal_yaw_brake_angle_; - InputInterface gimbal_yaw_brake_torque_; - InputInterface gimbal_yaw_brake_velocity_; InputInterface tf_; rmcs_msgs::GimbalMode gimbal_mode_keyboard_ = rmcs_msgs::GimbalMode::IMU; @@ -235,10 +202,6 @@ class HeroGimbalController OutputInterface yaw_angle_error_, pitch_angle_error_; OutputInterface yaw_control_angle_shift_, pitch_control_angle_; - OutputInterface yaw_brake_control_torque_; - - int yaw_brake_count_ = 0; - bool yaw_brake_locked_ = false; }; } // namespace rmcs_core::controller::gimbal From f46891c0afee1db43596a933261d73088d91a2e9 Mon Sep 17 00:00:00 2001 From: dwx5 <1591215786@qq.com> Date: Wed, 1 Jul 2026 20:39:00 +0800 Subject: [PATCH 24/86] six-friction --- librmcs-firmware-rmcs_board-lite-3.2.0.dfu | Bin 0 -> 127612 bytes .../steering-hero-little-six-friction.yaml | 420 ++-- .../chassis/chassis_climber_controller.cpp | 8 +- .../gimbal/hero_gimbal_controller.cpp | 5 +- .../controller/shooting/heat_controller.cpp | 19 +- .../controller/shooting/putter_controller.cpp | 32 +- .../steering-hero-little-six--friction.cpp | 874 ++++++++ .../steering-hero-little-six-friction.cpp | 1782 +++++++++-------- .../src/rmcs_core/src/referee/app/ui/hero.cpp | 69 - 9 files changed, 1958 insertions(+), 1251 deletions(-) create mode 100644 librmcs-firmware-rmcs_board-lite-3.2.0.dfu create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six--friction.cpp diff --git a/librmcs-firmware-rmcs_board-lite-3.2.0.dfu b/librmcs-firmware-rmcs_board-lite-3.2.0.dfu new file mode 100644 index 0000000000000000000000000000000000000000..d0efc11e5e286ee649e90efbb3d8d9fe0ef9b9d3 GIT binary patch literal 127612 zcmeFa3s_X;)&TtO*|YZ^P+)L#)6_<`B619oDW|Mx2AJ8XrDP~p)-iX2ol9w^T}~#< zW@bR;qFWfOER+=TD>Wt@(*w$FhSU^qU}fG$!8#qm3!t4N{OjEVdCAUq&i_5nfBx?| z-{Z5Hz2ECv>s@QT>s{}9Uz3ub@U;ih8B%7=)=)E@!76GWU!+DcYv*3spom$djzRWV zCPOlxjStCe-`4bp_HF9lw{KhU$M$U{liIiGAA?_`q(Ep$9%cTewc`?Y%j1|>WsJTyGiKQ7;2 z@7CXpq5pcFTkHSY`$NW1Ns*C`$RH=<*Flys;|+ndiw)5c!qYUeq;cgfG4=vQMq5V7 zG98oFH4ZbqN8Phyasi|p>p%@|jqr;G-|+Zq=@^tD^}J=OQdeecuvCSLICd-JqG zIS1}vtb%g#o9EBoU!f+n6z7zlHUx&a0vFPILTZQ94|hd0M(vH>AA4Xz)5ODZN8_7K z$0vU?^~Cg(&3nG7y2u=8!Resz3!LPbJt;&kI#p<4u5jft@tPZ$KhXEbQAx||3LkhV7rNb)qfWLS9*I{()0RF z{15#H;xEj7Xk(C~aRh%w$Hc@n z*78y|q%S{?uIDc*VPYg}%^J~4BgNiDkPmg%Ox#Pl-Nf`HVIbpQJcVU!N_L43`O^#E z(+2N}+!|4)cQm{7Qdd{N^&g6-NVMkKU=xR0Q1+BeV|+bDO{}%6u{Au$Fo?(P9pzao zQMkZ8%p@J#PQ>hEJ87bTuUcM1ucpJ39KqWnClm=J74>*+WWqX=MDf%FYrJb>ZFIeT z4{k?u&mcbBlxd1g$TWIWG8k{mq>(}QC+wfn*i0Hj5&k<`{;QM!^77wQ{FhHh$D3K3 zp=e;8#l+fH6$KzQiD8aS;_?{8zLAXTgO z+>dE9H%S=6sQb%7dcI8K1MB%KPN4At(w#(H<`>tIWh&xU`Z;9!Ie(_#_h)elpx{q_ zG6l(vDk9SucMAy~X z#}fKZI-)CscqfHgy>ygphE;b^lGg#r9*~vsngx-;3-uey)=}ycK2#STadOGo{(lFO@;3_E}%E_a`$a5qtx2oHY5xQ zAhf+Lg!Yw#Xx!HGgFQh3J&u+ZJsUh@4r@=H;|VPL$>C;HPUwlX9{9}>hkt9ycszPk0Ev%^z_a}bUjz_r-zJ-pwy0Ktw?zHAOXLGqkT8pI<9lD zXH7tlc4o7l4VyKG4V^j1qh|i5HPI>|n^8;GR4Rd?HO|*@zf7{QQWb%925V)KXHO5C zUc~IQ|2e_3;_Egs(xo6HY0^3RUZi7?TCy=6- z27bwD7$T4n1~}1;5L{y3bxsosDW1X39^GQ-kD^i}3aMgU_HDBFp*hy{y=m6;q0>X4 zR~?3kVp7W8SL=2pu<4;_N1Y6%XiFp`MI=VnA!;5#(hBYT6v*gk)aszik;6$7F8hS+ z>g=Hj1LK1dJnLF|gw{~Pv-(<(r-SO{Mv?>U$#1P!3P=W%zdEWutKBkCF ziT6}P??#D3SAM1hX>jL0<@k!$jZNZt`CVTe_X$LzeYG*o-6 z6^}?sC{D?7wk{35;l#N2_X%*Q@7nKb=0bnJ|0*wdEay+(L4$$Llb8l~?P(oz`sbeh zHCkt$|K^T6ic0FE{5j&;N4$0{b5bKYv`vMFgt3kx#0xgvTH*{7E&Zc|rZbsmpGP;n&4wlsPl`g4%J)-~ z;M1b&vBXi_`%ziwZ))ryd7B^^Yk@wo^f)<)S1;YqfRX=p7y(#y)jYa zmyiEl_+Id;FXz%8a6J$)!8##rv}<%kq&0GGTy0$VW9G-6d;xpYoi0U&WVQZvf~~?D zZQX1!S14H&N+51+b7i!HUi^&N&N9!v1 z_-Tiy&P{lAYL2tNOq}b{`_DUp=7G=b(XCLS@cok;r@E%s(+48<&a7nzOvxc8>9PiP zP?IGeq>z{~sEFBM&q^4Yq7BuIF_^CBXS&0VzF=Cv~?MOKKa9 z!j7k;IDX;>$lpKkXx34wdnC$fm%5juRr|!z1BkV5GGYMQYtdN-c9`u_x01lQz68#l zBIi!IB{xy78zt9WA?L36Id^^bsvo7FCL>{mFA_)ZlXLg|oJ;utE((wOq0YjqeJ{@Z+&u%C4W?^A06LynopkT$dnX+3F38`g=WSq$MwWl61( zQAsmZgmw|U)18WV($SiRgljIevq-q=Lc-r#6*odx-Q6szFgpYv&(F z!W-piVzKaAITBvI;J-AamN}FkXEA4758YXG$vL7{3Zdr|UUXO+^%kStV++)V9$Dy7 zZi(!KmJa0!*V5$vpy$7}s&0fjtZDi;oi|Ld^QkDz*M>GDZ8P9dd>lDaJCUO;l#pAS z0j-4rUKodFhR`m7Z?aMme|w962MmI7ix>yIy1R+Pv3%iL8QN(Vu9iX1Z3r-+9M&@Z zq1>F^*MoN!eeZlTWT(B;iSem~cgiW8Ij&}l={dqs6>{hrkmL+SjvXNchAGn+=wtz6 zEyEZbGg^_Oh$fEZvL`9R(R=|pT3n=~7$}{39%<7<2}dKP(y}zr0n+oXDOe~i&148n zjb>sCP`{ZFu5Op9(QME*yGSjMVTaP$jX<>-`NA8aM6_LaEtC)zIcYETrDH$`SXFA7 zP=h0hRIyKygw`MM&JHG}nf`>9PE$B6^Ax6{3g{t(Fr-x!vduIRT_pU$jbK!#`m40T z6mhx8^X9S3o>2GY8=<2B|1CwI?tU{Q+P-jirZJNrUq#hM*(@d@y%OD+8Eda7oM7Kx zD7q=2AcwlqAAd4uY_M zcw;!;6CV*-q2FwC=sJ*t+NRf1CHi%_2Ev@-w2>J$r>znHGEU~pBd>Q3I#L)~d&Cxe zcwcwh^rzt6Tk0tbb@&Jqd>EHXSbY91bH+mpqdp2h8F3;!u_E!xXbqB%B@%3LJ(4;* zmr1FOu<8y*;Z1Re70rsH?CWjIq~lkK@%t__(#%8zWBLX=OGO;ZXG7`jzpt;8?93ci3?-h3f-@MAvDk=ya*{JGzv^-8TX!H`J;A=(G1uC;7=jlgyVykk>l`X z!g29=D0PHb7+MA6E>NVxNdeziIuV$vX0@8NhoA`u;u_$kCFXqZD$lX58>w+NmOQY$nIEH|;5akMc68)ia|c{|V`W}*4%JjVmukhZM>5lMte zTKqaXb4eiSFm_28qCuZFqK%s+X3yV+saO1CQzfQ}blB4M?7g}%(#cdk+ucpP)yx)0 zBZvEdCetWJ!syYC^TCAYJ$KLG_E(F^(crVNUF^j=X=0NPFGv^BTZb)-U%8g{C)DvQ z3A%~&=(Qnf=uoboNlP~yGN%cHyQo#X*5n$YO>IXGTZB%aw95`tMj~pWC!Q-I$R?kI_3$uWe zSi%ypih0wyIyHP|#Vl=`Th@e6%jy8 zC2&(vCtc}U3UlS@WrZaZvJdsJI$K|VRMMMHD(TDSn<*2rD|g>WjThr=!7#?TMgdhS zJj1Ee(&BEkD#?=*e8xe6?%#19x$Mz4pGr>weOjVy2)sVPUtjjtoMh+ELOX3=IDifS z*;)tIh)yQ5Lm1Ghbn@`)--(`oUl{``^xkg+@k&9#!oC`A9i=3S>nOQKMlk-O`fAKk;TrfG*I4kf zx5f^rK@&Ub{Nx&Q2H`Kiz8ZQ8*TCPnM)L2yHMsJ&5FG`6zE9dh0_9pL0DlMe)jCb! zTKF5+>PYwEt?Uqd0dJf9i=1w%L!deVqMP{o@Z3jjK~9xQcPeVDV*LFJBq_YQJO?X@;TBvg*EB}1gPy>-Ki8ts zIm%Sv(v3prg5)-EeW}%if@;J~ZeJZe5J|A7cIjQIH39);eYvqr5Er#w4Ub0L#1HV9 zucJbUzWmLQKN6sfq5~(v*uw9K_5$@nm~DY#A;ct^3fm@QSc|7QY2{U;Y2kat2aBiS zzXjiLn`s4bFN}a69!(g7w$tVe9h6R)hN%qW0X?N0Xe=rA1H8oBi$3Q*njjD9XIb#NE?^;_|AkgekLu zj4+!0^2k!Rf!t^_I37$x(vC31J{Sx>vV-vR9#f^-)qL3?bnz>KX^k+(5Gl*{5Agp1+#WJh>J9G+_ zR#8N5qRq*tV(LWDour9I8^D4lH`xqp&0J86A}(7{&_jW?dTtA&7xL*4jDscZk$rFP zt1c2Z(&iC>gBx&=G1z3f3kK4JU<7tIA-qqAJ7~b*V%iWHgMp>tw_~vNJ1+)gBtyj} z)Y?HB%oRmkNsE~qJ6}J(JmqN_hYy%A8HXY!Z0MBrC1fN;iM+}n6fncZ(b)#!T_$Mg z6iZrd5m`=~#hEq*7r{_45=77u5ik?XfJGlhGs;O9V-SMrAWRDpXpzzq^eyS45j)eA z#3|eg5x;lQ3J*%}PAeFB?f@$-EWc*QS$Q}59<;DNCgkO$e zP9^3g%t_=%-XI!zf~DQWP{YL96TI0zh_{%M_>Xx`*@DX;?w)Qm_^ZqMp$6bc8=L?a*{*FRJVl!kiic3aY_WzG_z$7mQqml^ZV zTBpx}evIR5+dH;ahx3Dy$Y>jx@~kC&yWH+&J@*N1=xtNrRA}bUWOVpmd2~Gu2;VHB)|{)g^3`s)*Zrx70X%| zYZ}t!R(yVB&5@=fGTt^?1M{pNBu`(3=}pP>4q<|1dJkrTZlQNM(0emOi88&z5N>G$ z{Z$_9UM@R&aIg!f_tTdIn(nj;1U;y~bzE9t;T!W9a2GiW^hpEg6RVpDGsM+~@e2ED z&?hgI=z`!n4@)89$A!A{H(o?OsF zZ;STfY`-*o|v^|C2;B@!vb-+Ge^L$}7M0wk6U(LR-oi zyCKE|G{+YX@9%R}z)7GX3ahBJiC!H!s$WMQs^X&EFkfFgvZH09JYSFT%A0qt^Ul_H zaL&)Ob(o5lb zeF1-UR`=Hf&3^jcGaImGFLjf}zx@I{V_r>zbC#WwbGF#%t51EryFUl_b?Upmz9{I6 z%5~nUmp}cyUoRJ8y?nkc=@z}5N1JZf%U{U$+ba?Q-X znqJ*(&bU=OQzkgG0lZP%a=gVmPOFUEdrkx^`WO*xAQ3b$_)Q;ZLaZwoy_C15Xz*D< zq@yss1Uo$u%cL-b9!sUg+R4$3IeSX+k>f{5ozl?!(2T1N_c)%v+qR+#eaG|c;t!0iO%tv1BMvmsqQQ1Nkb^pdBE$!3` zUp0nMwJ_ty$GKwb+fVnpDnQ?dn=T~ak=Q^J`U`P`Z|n&gcz13ZApq*0JF18tz< znJ4nGCr2D*HJk;{<+T=}m>No@$I7}=9OW`>k@Y1x%4Co%Haf08%)mv{`mj`m9cH9o z$iCo91@%jRiBV#m3!JL-a%uUI3rG67)H{=Sx%8_VFOPb@Y&`4nr<-qb>4)!oxwQN# zvYe;AT>9~@!LU+fXlS4AYxmTJQ~m9Fb+otLj~fbQ-uE16JnT`>hi|rfuf4C`-;(8S z#@nus1LuEQiT~Ef@8fsjIWp5!xr;Cz`wVcQGfkFVV97$-Mh8eA*rOK5oqFoy>V7@7 zuC9s~erTI>yJotnUmAR{X8M8`F0?)G)l63K^#8ieczew|t;XyrrFk$v<}=7A3ABcR zO3!X-Y;n-%kqGNFudTQ_dviYMgvZG(CNs%d3=aQlJ-aCyu@geTR%FqoJg^iawD~V1 z$|NjnohJB`zhSB1_&CrQr$jts`6OkhM#-`Z_M2?yP=;Y2k)tUeS-Ta}$$0#QB6=*g z;;?oDPmBsy_A0K3QHiEd&=w*@-LiHo5+kgJC-S+XHWg?`6XqkV1+ess4J}}cfJLH; ztDiNcUlTlZn-w6asgeEjf4@d}qu-WHlA$Q1@9&4=+P6DS;oe1v@WpYQq)7)>WG#j5 zrnxlM*k9%y%A-tITexgDEm)|ypY0{tK57B`$VP*GR3Fv{k7f6RPs%vI2XJGY2h&6Q zH9F=E;1lJJeb@GpCg`o4A;`Kmf?Cey!d!||qO%RsjF6<#T&EJ#LwX|umFA@@+2&CY zQS2?VDksqFx17s|FMwK{8j9FZ3;0c22=4thr4ZmB!;MwJ458Bd@j^hW8Z0{^kQeCI z3D`I6I6n^wYbZ52Dhcd68GZ`GKWmzop2n&NjWYbJ=n0rwEZ9yttunp9_HH`0t&UsG zfIbXCYhc7^(2(RNdAwG)Dd&OpfGrHhEN*mR3kB>4awCl0mYZY8gEs6RyH%B{8CVbA z_ZO@MMWDuDH3PupGY+#Rd z5?p>68sXwT0qnP^^kYoIE6qp;-WrxRF?AZl+=lSo}toA zMI>Gwmp2;XLBDM>FLr|6q^D%-WLOo}Y7gx49H4$8H+m@vnm`d|uq@}YTNGiiLT1SR z**LD=OWReuz;@np9A;*S?_;>Iy1pR}uohL{y!^^n z6%eZ~g0|c76xPP#Mr`Z3K^wzb?4xUF%xB+)NZUh@tO2uHyjl=6ljl3?DEO1SeQtq1 zOG16TeR?$_NPQT#P5ex`d7i1w`&aFqVDi-;2tR4Lt^tcUwN^D*2nDu%>g*M zl{@$coSrYK)}LD`5B1?T_d_~fwy(de`MDLod1vKb8A@p7;4W_~yboE%_#CSw$5{*> zI^bKZ%cyj2?0G|!9r#e>KI%osK9{Xqums5HM#HNxs%7RgU;J`TG*ti6(jNaAVS2jK zWApzBbLf!5IM-Xyd%L3ov|sqN>{M~9TLD%^G;e;%d8cfJ^~u(%YTyy&3l3OcsU|t_ zj8VA@c!XZJE7SB+)txg&%}z0uB8b57{qzCsvy93bQwMQ9KXfTs8;)!~9m&3)N(HtF zVoOBQbF&R*yO=t%-*-9qs_eT=#JwFf4l=}Gv*E-7gKBM$ zuHXioL!~N}ULcv_r2~|x3M|r=$c-l9TqQp5RGB5r6UN;(ncg8Ah_tnrSS(u2Jtvu3a7xv;A! z>_kw)PMct*gY@n(yD{bCUED~eVyNLns@Wc!BzvaHfNwz`%6hS+&1~U0)ipgCSr712 ze5vXJXv+&&Dm6-y3s;ihBP^5W#;|&Xxo&WsaBoY91-MD2ueHmb$C24+aplmbF)e@& z21B<@B{dV%Po<$pXCEcqFtfNAh~>$aEz3ZWeZ4yX(~FE~#2(2(*dK}K;aZ!8!mK*ckyBY}!c%TS4K>&2&IhM7EXH2?5W`6H9&Cy{O&nr$eB7R`2y z56Qnq)Y^i(u{SWz6=s+P_A=(sIGCG(23x?BfPI`WLsLAL-KL+N7?*vc>Jj7o#OHQD zVwexs+tBBdX8+MCrj8;oZWe|c+zC|rzG}V!^TV6Fa=?lRGk|180GWOTms9m2J@aJM?Tsza0?_c7*3$`bUTV7Hmf>Z64+!ga67>Ve#!v%X){- zqS7%27*fWZlhkKNd=hFfR*DhXhU;f{+Fo)j4UwQcOR3`HUF}W1*J6`M{zU@Q7gO{` zC^z;R;Pl6;DwF5lmW$a5nCEml12$RtA_Ih zzVmAU-fKYf8-e#a^54Ca_cGhOE5|pIzL2@^!A8t|o|SZ~-o5Y(5oT{s&hS$hf1uDO zEry-IE$Ogo%12dSV0vPYj}H_(tMqUa0D2sI2r^y?bzfZ(ix6vGmdTSo&HIz5Ug%OW*uGSo+5O_0GQ5 zdhlMz8+!06gY-SPNA4+$J^}bwW9&vaJfwez#K6o|xCF-N)ERkX$0d?+c9P@G6;r>f z#GX~k>sf`!qcPQn!0`GEgN_Fs0gps0>4@4jHMC2z*+$Fb^>t0!>^*~tT`UX4f(uf>V#$#y-x8VJ*T-Z2i3!}3oo9bcuBtG9FWvd=`lvQdVmS}`-5t$*nZG`lp5#Nt6+o8BZkXy}2 zu(qkh(s)FcDL~h#2&v}^lI&BFbi_gIv};2bM1qwKc0yascYDRMc!c-biu^wn<+!J| z77gB0```n|fr}ava$#lyjpq+>HdlyXk!)x{ek(4$3#<5&i^wrbH6ptXY{(J0JeNm- z-;DxKAv_Vl-m;8%;gt-vprh_1qW81ZjO0vX?16Xo zhBOX$EvbL2wmm~L7U$ud_6$Vp@EH+CO%9s2VJh4S5r25v+@x1cl8Sj)_^C|owEg8t z$=3a(L)WPQw*|dQx+@&=&`MV( zuk#NLeZi=OJ-^4@TqF|?HieIc@f4T!#2$#>e-kdS5oxZR_JmV zDW^4JStEix4V2U9MnAX>Y-0_}ygp|&Q(XJk{P$hBzsJ=&%lHn(VZ~8Jb9=^uM=^{5 zTY!usU?jCMDz3cCUo1008ksR@nq|eUShFNQJJSs(Pcr^~QfC|A;iO>%c-RfizJ@w? zQS`zpe3}HLDJJS?BSfL||%wrNC{jj-@! z5{-o7E(L~bHN`Ug>}dP!gs=pu*-2Ebs9_)qCZs7c&WY}E2atn=gqF;yfdKY2~dGtrKBWzD3*lQz$4Mhy62_6sp<4)H=$x@J#ncGpYnO#)Z z>$hB+mWCWITBReER}1PKZD|^?^kuFV7?pwx4-Rx}1bI4tUIXtVzz(=8ADc6vr@*gw z8{ib05y_I+-5s#jt*BSH!i^P1tW_kY0l|s|NwftO#|s}`0PYJ#<2KQ1A*2?ZqvP^m=r-s;)-3u4e zEmIFFem*^Vd6LLbNCHfRl}tqVv4|s+4^97S=DugXeBM2`A+7FrH5X6%@KISpVOs=H z26ZCxHx|Y08RQs>6n|qREiZ0fBi=_T$oq^6@o@{84I1D@3skg3nY{rGEkDQ zQUNg)&k6TX!&kyCGjP4W$jke2_njP=X~kc-ztevZf+@Adh~Lo*!o8F~ev9!DhEV>N7dNco zmahQ|LrHru@E1J&aGk`e3ynzor#ad#4wsoyUpy~$GL=iyyrg2jm)G?miB^}D9j06`n1I6 z#8E&s9oPwAU-7@_f3LTHoG zZ0$}jY-8I0Cy>o(^}=PFERT=+zRsIH-;L_?AhzI_V{yLfZmHkzx*KHcD<9XZRP9ae zpY%}#ID;^?7c5XceWk%%cRJt7jX9Oe#bNI`N@}o2}qmOr=kfG>!byKs%>m4-=Cu zHjY-1jl4o_Al>N-@?fZfj0Ugp0Uhk>rz&)?YTBR*SA{by-MSDWtqC}HKtRMOd%&Ni zjR>~}zQF)uFW5IqkaBc68OK@U`I_7c^EG8b1qR{XgCIj8XxtVGX50g4>z2+i&%6FT z1rXSf<~2T7rM=rW$M^I*pQ;Ny5P+8;^z)q5nd8VQ##4+uZ8b)3IiC_F`s z8~IMy;%maXQv(h$b5nZko9)u6Y{dRH8%fXG(9)^w3=v5)NP?Xy5~UfT1ee)Olx8vn zXKo`(&25MiTZmFI9b)0#1eaBYAcg^P7B@O!x*dxN2PW>1-)m|d;`-F6_#on-UE1rvKlJt`0^wRUX~f zVRT`v<}1|sS~cEQq}WfXF$Ffsw;kRPqk(J%{zi0HdK zsoDBcjag#oRIF`9-lC}5C#D(|LUs#!{0RrMLvInc*oS^48aFC7+j1Jj8S@l^<(OJ5 zw-LGXz#EYojXmwz4=u#9JU)v|;?|_uQgM=G*k@DhGeoumwzhjTG zv&~k$!+XErKt$8lIm9GrzT?Tv4o;9A!w^A)D}q{B&b6W~Y(>ZUrImA2 z9&$dkkUn`IUttq$=jria<(!Y#@wg;22iMa*h2P2ZWH7v^;`h<#$xsFbpcpvYbRImZ zCWxJYGgl$oBDJY8wmY9#5J|JTdOhw``;$A82+S*L?M znz<%gY2kLmHsj)gw3$3XDVB4@`~>vi%ECSTn&$J`3*MRG*1B4<9G$6&wtD z9U0<=Ao3FE7odMv*i`R~B)o|)`WPz1$-a7eS8?_T%-+(d2{p8Hw zS&XTm^)E*+xhkw4AJs+Y42&(fp8#9uO#Hry>c<>oim{gaS>Aj_f|*USHZPH^UCXp( zxGy(B(JXk6y?wdSSLK4gO7qXa&e9(1k zZP`T`BRDA&AOy7{?)|i^oZGN#C>MNGLC)tf#KzmKvsY5Z}rdp z!RYt(_mkXT!Q2Fy-)*X;-u}`N^k?$+*Oj%=@z3xCMDMFT8T64~@2Nq%Zoc#542Nan0??G2k5(&NxTv zY-9#GEdscnuhN#C*NA*55&{Flv5(Nl4QFrg8bw3$6OeEZ{xu!}V@ZA{Sc;R6uM;Qcu+Rd1GWV1~{MD>YYaXEuZHkm{@iJ{EUdkuSDUd9%@L>mQaN^*zK8Zw99L!1+ zaokw4ex}M0%RA-y8=t(=GN<)oxdj>CgMEgD;ds8{Stis+C^!!LSm09wjgR-T;Ov0^ z<>a|3hg0I8#`>D==8@-K<^+CYh(Dh4@`Go{g_H2D@C;^7nqj4HCp`8gt)i)n@JL1Q z>n1!O(&t5!6HdNGoDX`+Q`~ia)>EF~^m9JDF%M);BNFbb%r(7wAP#O72!Y!WqU+li zpkcAoYU4K;DISj)7yeuQ;i;Ghgsk&Q*nP7cQ#fgK?szTsaTLd6+Og@QQE~0kDrtKV z%tuZ7>+i0j7dvsO;=Q+)f)k3hFI2(Uog3)BHJyjrhDyLh=?P*EgMALsup;<6C6xvC z&|uYGP^|zhh{lzwU{&IP)yZ-2BKr5>JWp124=Z-|vWphUcGtT_%V}1mi6}`508e5x ziM06#4Gu*f5?;=y$BiSQg@eRa?kTW%Nbn>c>^(&PRJ6HBYzTo>Mk`{2m%!?c8u)cS ztkIY+^Gh?jkY@nh%FfJ&vz%bbq=F@rNy93|g*bC7Dm8bZwN-JTN#kL}H(1QYc=h2i zQ|AtYR*g*xH*L1ga7xy9kmtkgmw#>E^{zM`s)V(d;h}}#kza--dh&77&L9guYq8O~ z2XSeI>)6W4gPrVX(^6AY}+^<`qPk8CM8 zn9uciXJaWZEb4~)u9_Ybe&4OaXL@lxzkCP19ZhX^eBI{juuLFsP!Zo`u7&&HdMdJV za)o8x^o089N8@V2A9e3)p6yijXl*)!!*UMYpfcCy5SsJd})SF zFEJ4arITz5Ogu2)KyhhU}#+=@Ccj&X{{ z5Txrwy@Qwy)c3hx=dvvpJqPWTLc0|GrgMS|d}_6d;;8-62Y_2IRG8qL2Onj%zaR>6ZGiGA=F(AG%s;J>Tt35)koo*q`!-OIl526zOrQ7!feWW!pa z7%czC#7LOi^M)I=`8$yCLiNCL(O7ne6I;h!80>i$`|yW@5C1aw@I{`27z?SJ2@qbk zE52LFp+Y2Gy{cgCFCnQ-sen_dEjEC?6JS4&rdqdKd?>&__tufE*(E}Bt*#sOu2yb< zJFyz_rKA0I3K2yacTcEQ*rOVw_j2e;Pv`4sd!b{ zUXZ*bDvM1Fg13Stj&zH6AESU7OBf;V{)fIIKYkoerTOT^kv>qS=~Y&FzE|A1GFy9p z2GS}s^z3WObF5OS(JluK@pR$mEuJ7r;j}K0p1xd_Dz4%5#F}1!| zSjU7*OjfVB#=Ba|S?Q=(sy11Ll}a_sjMPY!QnR*1Y++DNKGs`;okP3}a5o3r1$#XfhH#9wY7X`G z+pUsc*57>~0)VypWoUgjq0)fQdmP7GeZ^5(Jq~6Faf1PL^_lOni+y`wUzc!L%Jjm5 zmPm&!#1PwPa)B=}x!}oolL)wn^W=T?Jrzj~sugb4iCcGr;zm9Jx^h)+tc437M#Z|? zlbfc-g3P@v@78dU^?&Cg&i%bTc>(O5S5e|4kBli+#(T+a_SHfzAc5xHk`iAjNztFuHtix3J@2MkS~mHc)7 z*ZXFt2ru1G358|E&=ld^Rk-_P5;|)?kwBLDA#BkAUC9)7)^(reuqztLAvhK`#!|u#U@EPgMEae zHW+iTX#igA@BH{6hC^$$!+juK=y&#=b~VHTQ9dsyYy3R-OzNZ@InXj;n&H%i;xz|< zB6md|1Gd-+M`%an!uPgDbnD-A#1&#b^3HQ*aGQj(XWlMZV{L(`t0b1RAaXQ`gnKW_ zT5D?r%=d+@XzQpeVV&1;j)%I~BQf2*>sCl`mVGs|Lr(VK|{(y=Q1wkMMW6cg9K^GQn~Ly9JM(-t(81p8oc;`}*WhVDD6a z|HVJ||9*nt*R5c?Uq|d<8=AP**N62POk=M&_y|(5Ju`7FIobyW?$ZIpxPAADp>^Db zu{nO*xeWueE!7eOT76C1>09+QnRqY1HWijr+f z1>cTM6O@3t)6w0f2T01NfaI=n9bSqJJCG=B3A||XSay%d(iBb+0R;5boPpy_9@rbzHd4j&qB*m zXMfB1j^64b;X^8@sg~R{fFce&Uj+U`*o7USNJ+IYrTHyAczn1`1C-*XDN4#Hwaf%v zX_NORTVQXpJ-OpP@i+UC5A@Gv{GK33?T1;jecQ64U&r;?{}$I5&IY+LX1PCOTJp;6 zm}+LYfnUS-)ARovwivHF@r7O#Le6ONMv!NX@@z$Nc*^P& z#nYE3&7FLBa{QEvDdCSBVbzsVm*Wg_uc|6=y|BB$^;{)o{CfAB3!iBufosB#MjY-ndF4WW_{qrQwrCJ#49>w44gWf)SjF%6`lp{ z8K)oqJdEIdOrIHb6uN3YVLl0KttEqNUQsWYrPwc*-+y)zzT;0~reyYOcp;OC1#eMS z8+t`EXSl`0LLT_tk>UUs$RKVIgBIPdj0r#R>SX6zpkZyk$=Iij{TOf){a8WS4fReB z-kc>t4nzu|xRs#mRxFN&VJxw@Q z1}&mt&e!3jwXOTZpTF(X2D;)jt|2iM9KO+oQi=e+z{$x>Jg^}x?`Lg{dJK~&QT-w75!QOmb!e(~(lw0l@W!OTE^;!CGWkS>)3^|b|B<6qK+>m*qaP>17ZY$sS-l_E7WqTe+PYWN z9dC(zjr`X1w#c8MoKRB-aYW0R&sH>mjovx3l_yP?D11w>XbPmS=aW1^ze*p|f+ne{pnT0L0_fy>Y_nmg$jk?Ua{;fw$jU4!2bK#$YMI z7D#{G&3az3SN@*__D)a4cu2Oa#C|ENI@l*e>W<%8couT-y>0j|m7O_olM85Vc)+^| zzP?vKE1BBPXT@-zE{tTN&i zb;11{N+FV9jg?4*C6d2U5-Ix4&79&YQq?;cp7`dtaNYj2&~{>$!`6y)R(*%?Lf0%u zTPw<4A-vDbbTY>SHru}t{Y=8Vu9=QD-H774qjl}u+FLj0suE!==cu)+L?<5rurniH zwpO38JduK>5aueCS*HbZBW-{-V9<@dtKeK9l*}wSo6v16R@V3j3qDG(1>u{9IRCnoD4x!*Ow|*DI=5j!=5dngi=qiR3Zaw z*2K4yK_1TqGO!+_xgkm#PHND2bpU#6OIy!^WZh_V?*3#6BI9sse?;8-&x6zw@1MjY z*Ds6U6`$xV!>)2w4+hoCNT7f=q#CH14 zSlrX~-e}yxwXdkRaY62Kz1N0KqyXE5ay@HJ^xEpl>KM_jL_74@QV#;lRdkR1*YBI}2)_2U}z;`PbmZ!mUvb@E@Z zIvD`Btiky1$E%ZDarKVw&9Q}BvdaIW#AAyw#&OoLE>mKBS2G@#j@xc8zI}y~Trldk z6-u&t(A`%k|ICWaQ$#)Z&#h6wHYL1v=glz|`3Qw4JM{s|cq6D?-|>{^L*+%#9UIHT zKtf|;X5+{;(56rHeLtH6w%&D^ZDJJE<9LSPGHe7s@r$(xUTd>izNsukfQK50-o0OoCoU85B+O66CxxaD&6a~8J+%9Wh@%r6~7F>wu)JfUa; z3n&}<&E_;$v*o$1N%5@nVchG0^ZnWpb~7IlvUD&%mHY{(V~m{hKrdKEgUw;-X6bE; zIB5IcPg&|5>|!D{Sf3@2{g>pE$#`{}qE1p~Q0%}xRD`8HSu=qv`50N0t9(3bvwD{& zbb=j!vQBoywUf}v*U9O=JB~7=Q*scipty?^QU=Q z(=p^QHX?`Ryu#7A1Jz_Y8iI-Y@^D>k-gcx-YD5MzZY}l$t^Y-ZR%}38V*|lH?@wr_ zZb7d~TGFF%xYCe(>ukai*fo0@tbu0a$iARpgP%gUJ?Xd#?!MiDTt&EN>DYtlzFevB z4fH@>jrp^@2XeL4c?H`%6Wy1`7DJ!Vt~3L@k_`=gkg&%V10>~eQs5ZUCe+CgrOB{q zRnW^*P{%kAIT}?2zMWK?6h>%`6oFg)IM0yzK&~TclMIj43~;zY;HF(i68LujLg|>5 zsLH7>6dPdgvyN7A@g4a*-01Su(}COc@Dg0b#Uelr{WumI0~nTsJ~>X;wYSNP!Xka zdICP%0IjfZT!A`q#bg45NQhi>Ox!!W5%{*%PFO*&*2Ps7>Zc@1(NzWK|03c7;G}CnJw$Z{k#K_(#JW*<#1y5}RqTWMf7pBX@F=QmZ+KT% zs;U!164K-X6R}A_0vQFQHF0>4na-uVIw*)2h&rff4TNz<0Xcx4GiZ`ZrxP$F&?EsB zMUaHyRn+u>_HrQi?x+|*A&E1HV4DPWG~9B5A>sS&s%~zgGjnFn_nq%~&L7oXRr^wV z?Y;KeYp=ETT9~(J*T1gQfXBG<14A$=I=74mGLkVS4z>gLCxfJka}0U9R9$jy7H5RO&*nsMMIjg4w67jgeefz2Cd!v(kGYap*^6D^^FiXa-*cD4?B`U5^RXJI66%*4-wEFA z=qPq;D5S=B`Z>|Lg5nK@Br#N#8pmDaLY1n$KOr^tRdSJ(M+t=taK4`p4ZVsF4TaSB z&LB1ckg0NG3bzKcB1P+b#W~>`kFqJtwYNl9 z>u&#>=l(B!a}+x$rkt@hTY8Q@vYo!k<&{Ky%i#!8XDVi;`tLs_^XfF+!PD-L$*Bi7 z(;2JA0;Kyjxk!lf&sZ{fg*s!QUH9*$3bYyPHDqSy*VTM-e_ce=u7meAEkQl+)$w<* zjv#dpHA~w=k+&9!x&J{9&n&u6EC}My?-#4Eui1ucJFeGp?Fizx?N|QqrI}j0PMfLQ zu!lNAXR0}3nQtWbJO4a2zqK$hPw(co1$I#8$= z6Y37?KYn}%m5cWKDJ#->oa>7{m{4XMHO+&qBY6{W0UF5i+YJBi$D|yI@7F)f0<3GrDTngtpDo{El!g(rII%RCEDlzL0DYF3V!?uJIk z5IT9#3AY4d?m2aR*pgcfY5lazb`zCX(WH+^Fm@B)T(z56>EBJjE9xk1j)wT>=rpd! z{MKA{YY67(G;NNWCr?+rA(*2f>Kq+)*&O{)ouk*@E6Oi$Bksc-4Z$3p?VqC|m*(h8 zkVIC6V2)0^sM)E!q}ef>Bb21T-f3lE?^O21z+Z!w?wx)N+K0Xgz^d(wDXb^%z7p0& zsaM0Qc$2Pxn9A#~4e{<{KLM|*$GcM7?eVT0qZVuM(<67+ieHR!b*b1hteHQE)pu;|9C z`zNrc{7GDR=^wy_$z==G9usm0sjTGOC0R+?7w!Lae3*O{yw_h1@Bf?l;Gp>MO7KzO z!xj}Ej1dbi;ll>t!qS}@K4{or2qotsQ^E?Q(FcQ2caF+d{#q~ny;xB;`pRC4xbEs+ z`X^w;WiuuqOS&{)AWN8WEecV)>#xE3QxszQ_dlc%`@^q<)%^RbVg1`N;*HLqpb(c) z#A{Iq78C;UBSK`h|2<&EMinbIYgqA?m14!KDwPOe#iOM_PHz<`$8WDiISc{HahZL9+5Gz}WK!n3 z1y{?Y0yy%M+Fq<8f15PwS~zmz=gQA1XFKyO~Dirg-YdWFU<(pT2^P^@CZ zyU_Jo+_-ileRWI3o=UBrtm*#G`l`e>~{`Tt#&@@PI*lT%)_88MSz zUD->y(vSDjPuPq&{}G&cD}?r*Y5|~1WkVV4bjl{3{zqV))pa$bbML$o(xbQiIHW(0 z4_p61e6W86Y^YSR;RXNt5!BJRp|+!O>rQC=feq8$z=Q%VZKked8mID^!FL?H3=b%e z`S;U?i+C`cA9Z!_4jlAh2H*W78iU#s13%EJA6e|{V8EB*QDeT8;ESEA8M;`b(eIITt3^s1z4gqz|s z-33%1(R9(GCIx!+jZ`ZP&%;zB+yr|X*aA>(JNH>0^vv}%H{H8xdVB5dq*w5zk_7}F zwTyLUcOg7CR2ACXrFsHCI&9wsr8?XuzP8M!Z1LEfQ=$otsqV_Z*X&gKFNm9-!~45z zV%2P$vnB>7^;b(YP2Bv5O?k6bau=btEqckRyKuqq%NvN>4t)>Gr{LdJ+LOk2qg9%nUw?)Hd{MTkfc;5w=Vn}o4f0E`b>btNaR*eJiia{K9wD~n?eXHzN z(fGY2#y<;BwNVyv;;c+@yf>4^X#)(IDT*Ev!+r~3$ix|)OeH=eGX=2F_B&Y(7G-=< z2D_EZpgl(IH%%0ts={=(T1ZzWwPc8|J)0?3eQ$BHLl@XLEg6Q?(Kt2aNq6Npq_I1s zbTR(kOwqU`(^-SKFs3Ln1z&%}Yzkj!roC)1#gM_+6;UgqU8^59BfdEMq*=T%Hba~^ zkRfi{nl5hd#1}m2VpUrPyo9BTn{(2sf1{;~r`4xCgWgTtYBU4~OQ!C`F}%0+h{D74 zMp7p5oSYjR>FgUxkJ zrh2QHB(uf}k0#Oa+3f6Nx)j)&!wSM^dRk!4mfpTn4$+5(3Bwe*T!?GFH|TvU9+ovYO7LJL{i}oVG8^qF@BRV?GEH14_GMb2J(_-0N! zy@FOO?`IA@?>at!(=p}v9{dtAQHY+EbtDwlLbZR+sx7uQ?=D6(vEH5Vdqdnf|AO-% z{CBIHS33_D;tUCGB|Q%kTwKqoINBD1g-4r7O7|3q5XRuW~ z?ByNsu+^NQY_q~2yf@wO%P8VZ1x}^Lz;g|BduhO`qK-6RmA)lSd>z=dV_v$r4cN3j z8dwEvsuD7A3VWHJBq>e@ui5lJ4LvRF`kh6#ISTd2QON%v=V%MD79j%PUBhS4QD*=%eHN$}vwSFD{Y8xCn7m^JKQUkdb3MhqKKUOmHXLT*b%? z7xB>ILQhee(y)suw)|$bDZ5LRc;;ou1v)aMY_N>_WZC}o*5nUUTb^y2xOa40^uehf z;OjJcg7HgBL|WkdVuUsAMS5m&sz+BrPfR8qo_J*QT@BJF)~9Uw4@cfRe1=mm%?ZBi z-TEmwPqg_SdXkCyMRt}yg)a)B50zqUsjWQT%XvCo&Ao<0@B(rOUO?IvcnMerJyjJj z31^jbU9j>_(;wqyhnKq?eqU7M&=28X{8<2cUsUHHg`PP}p%3nTqE@TN_pdVcORIQ>(KXXkq<2mgkA?!zIXr((9%oMyQv$$0kP#BMeU$K|-ph z?^Vkp-QF>_(<%DNse2Wv3AX$8o-wPI7$Hf8WR==@>RQK0{(4HMwz6J{c`8%BL5OoM zFHj|})E=5zwZ>tMbU@xoJ=>go(G^uU{>>`4+12JT(=)1v`jE~W&t8xHU^~&<@+?!E zI1VS(a&K_mmx*(Nh0M#LnJ2tBM+j}=E@t=2F}8YsE#&9|7cZHj>&EXYhrhD;vgU0W zuUegUiAye=>Pchpojql056S3{b|&bTI-6y1?$NpFDtN$xAJ(UItooP|vJzcy{O z8fLqhTUwk;1!kRe`>He{Qi!xsI1wQj`>Zn0c=JZ+Co=(Odeg)P!1ZjTb-Y=%`JbHo z+uO{F1$I!V@y>R|GJ@>CgZ_^t^!FC^H&gw+PyPKhehrSaiU`1GaIC3N;e&N=Jp7?U z+v>-ND^M@`t_lzbf;La}bQD3Af6EE>m%DB=1A2oKL#z)A||M4A)+lw1m{@=&h) z$IOFKw9P6HHMZ6&CX|AQI&visHP)MHcn5r-pJ$@0ldCj5O>6&^{F2HmfiDsh`ZXS@ zTCh;Q=_)?S@=xTGwsmKU+jCU!vd=6w0Fx<=qZB6vG;T!3m`q*E5?Ht6th8h(f>tl7 zgMd#A>mZMoQu?F_dQdpkp8Z25QO|s$(jO=2&uY*gQ}q7vld8RDoVGV(1qol=&5qSW zBBjksYWJsyuGI<})RWuu4Xs~+mfYZA%Aan!UZVDQx2aZsh!90IkDHrqsx4lntmnty z*LZ1mTIo6X$wxEeFV4R9A=Pze&)i%t4$08kJ}H54c%5?eg>Y=XB$-ty_fbAFn{Az_!IXskJ;p4AxxuF zKQ@bxeWT7{N{{_>INif>Q=58n!~aNYIw-}fH2HaH(PcE5&S2w5Q&c(ydK|^YmNk#h zFa$~5tJckSP%4}g#t#3McqzUx&#KMjI%7Pg3LCYVy!Vc)=CMVk$rhC+TU45Cp){G5 zmScW{X1x-s(JYN7qpee}NwWep41MyWt5)#0SFc$=-A7-a`vINP@Wrb_^!L*__#f?= zY=&RMDOjzv-Z>fuPj&wQox2?;Ny^{fU;oDLAER@ni0E=@zjy_md;K}9vSlCkU)p|A znaJAI{o;;CtjeSe+doL>jQ85a_|sW*zxcm&PNQ?$I>cYGpD(qEo9Osh)&2YxK$|3e}&I(0iW&ZS@g>J`!Z{OlFv>L@YxaEjaTs5 zAEtUHw)7a^nhjq}0Y2L}Go$98#%JGi8J``1^QUPueAOKP0iTT(tY^^;)b9Bw`D|_d zQu*w-EBNdd&}ZXJ+mygcZj89~pTlRTt7}#OD*_O*`Qmzqlmti{vURyQRm88uzNLD!Z10PpZ(z|w z%_O!*tReE4D>aRr~RV$sYg zQ&cQc*FWQl(eQZm53GM5zrDZVcJTRUchgx(=j6_}_t&p+Ir;TELvJz(dg>l)pneBOq1bpK=NZ+QT zo*Ea>)F4(Yy24Xqz{^c&nnJxau5X=sxsMx}@CneJMh+kfVJvK%TVNYos9Lz)a&OwM zI~$(Z2@Y0w(d!|;>N~sjyj6LombU#sE#L<({eAG`4CrXu1MCj;0J!0u^=?3JDCD!L zG-23$$e6Mb)p{#m#5TfzrRslF@hsOV@IR`+|0ugpLVs!gN8{Pe`prD`;$xCeGt|TE z$E)QRTPI!t<8^#|z;5rBd!P7^|5BSBLgeLbZvO1Y+l*GK9&r^<4%!^!Z!;kAd&IqF zn=4xrE^qUgb53}?srpcrxpAr&+*dR&xNlM~xD~Br(wp#t3%~R5f@|M5M0ah@A$Y;9 zTuu6@{|I=&-Tv|)GT1l6h*MWv=M1VP^7(tmf7tToW_ZD^_>!6QhnG=4cnJ3s4hU_T z)~pKZ3HKgIy{I?bFAB*Ze5J02{eBoc;jU)33Ut6kGZXiuP&*r8N>t-C8{YSP^0LE=cOyx^n=c4%;UiWZYPXxKmC1j}TM0j{2aZp6B+g>WRor?wx3-zTv#^ zIw!%eUBocS$-zI|(~04FHQE=A(M|oF$qvMK#MzV@X}1-ch;aF;$T1j@96EgX%QxAOdAk{9Qy*HI7sO?gH*2!O0@+D|j5|O`>7`x?@a(!A{+Dj6A z`oJq$`3DNx<@bWNurk!|1r2T5y?&U~Tq-Y=&?ZC7<3ww(e0@l$_Wi)uv)pzIe0J5A zNW~W4Dd{gxZYsP%z#Cji@A(I6htao-6E8TqUts@(x9d8;541ne4ZCaIs9^9*EQR`V zZ)=9&aU|GHarcsrFE(rKaM-Fla{A#qcvRVtu2_Neb=+7r$+nQ zf!#mtr|lPRjyvnN=a_^|W}FjTKs_F>Cu@ zPUh0n;vHU`iG0#->3#c$x*@$4+CR%B-H^?y>}a<9-MS%TEabVzCFle*#U0nu4f!o5 z)H9GNOW9ll*E}DZ!W7k7z#(+<9@+=6){my=6NACGOLJwulW3gWVgivV1MtD;SjNb2i?V56yA@Ht zix9g7`ST^36P^dsIZ&Oepfe70=jRq>TRw|To7H~xSA5R}oqfWcB2B-C8Ez0#Zgpo{ zGp9D8H|}MmTi^*w^ERVKf{VxvAO4+Nh&a7UTy#apwcUkJ&^yAe57B zB(-5&d?2nIU9k|uQKFVEN_DB#2`{bUwq=k{S|w)4K&M4a@~V~s zz}xi8@D{rN_VoPRfW?u{3can{@j7i|4yxoB}F(;vuO% zCQ={Cxej&NOgOo^qQt;whB$@HpyCgT8rKN&Dlb_PLsP#yD{c~)HG*%0cS!Y#QV?_> zjmbpxJ!EFu{WIfcer=`R=3eujX$*Lo)0NTLOxuh-;JxCFh^VTzU$Er`W^wwpdm}%W zvrxQIyz{7^>xynf4_!*AKMMx$SuY1#7NHwwd9R!W9|5~+=?}t+$8xn zbPd(ul=xdWaF&{x^nH?NBs>m`M^x>exnDGIwoI{1)^D&PhsasZ*6u;x6LIx7bCwSf z5iP!HT3rh4#i%tH?@4HYZIA;06^cBpn3#f#&4vdinDa8@%=Vz+LHU)wKaagYbJ7xj zJtvLYxHeOlMd>BwnU=gIO?C^$uP{QOWfFFIM4u=%k4wd*9P^!C&uF%iz|Vy zam94q>j1@rS5_fSRs@4fAZgCt6cUB~%B{q-+#6!xhc5W$b;ZJiPp&h}!Id|&RtHzS zir$N)j@a*7vyb(TCg3sd+Cq*KM13NPcOtBC2FQ+5Q}`aU%dv+Tmh8LGQ0~&7hHrIf zDw_qI;7&xpr;vC)Yjp#EVD)3dtkU)MYlJnN@0Ee`F5xsIU*`)pIfif=8SA?+fPGI4 zYzHKE6a;Ib6NB0uBhT!r7`t2{}sbQVfl z4MQGQtpRyagCiR?c!<2j%hJ4xnsZzFQc{r-%4FC0zQhpl#0PRU^-+bg(kh% zEDPu-VcO|O+ua7xjcoe*(c`mg~6*h%kYo35J*;}U<=~Kp=Wq_@O5zmWhZEKpRz> z!&9o&2dYKif@&dmdW91+TBTZN+c_5x$_Gl+Q0e4BwP+1PZ7i*U*4%)+eyVl4ouzpx z)oQ35DNwsB`I&aic2F{WYbvPLfGQ=RR10hHbVE_$^uN+`3;U*Cv*(`o-1)yfr>&Gf zf^(1ejs2(e+}}J?{|Doof1Lr&Wdi3i{|e4M(Kq6nJy+n#{O`cIn8E+KJ%^pTpW5e1 zyZ3Jl0Qr;6DLCElg z8Z)A7*IhWN?aU_H^K(xpZu))ks=RMEYLMvZ~b0A<0ihd)kBx3a?+9r@9!_Bp1U$N_!_B*J6<{b4Bv{= zq?@{ra`UjaKMJYGE7L`H%99M}|6Dn`SLfzK&%BhL!k@U1k^^4{f-v(WqKCfJ0B-jT zJO}g(C+PW_xJGUsEt_X~Vp0k>c)`GRJ_5h_i78PGgS~oLo@II>d=$WY<8P1-i(_u* z%sk68i2;A5d6q2P-;DR=c)t;Orr>@n?%%+B6SEZF5$w2w&m^uBZ`Vx9voMylNlxyq zyHYF>XL=&${(Ffv*lNc&$eE23^Q;VZ5GhYh&%Bgjs04R7N8n|w7KTdj zjaGrDdDsL&k6o}ng}a3rw$j;+701}GH*km0@+qE2Z*qGG&g$`FXgYoC>=eSBm6i#J zp>0G@+%&~XYe5auofR>}x0fHHwVg7beDOdjtx1zO1X|$B76t+}2F^Yms3Gq=L_|DMu3%p4 z*9xhSf}3VF+aM32wWkykgWIB0`#U8DvTpmg#W}9#tw#77tG470MpL`W-?^sS%HEF_B-1t`!$^47ut{aHv(JjtjuEA-4lGWki)T4SeF z3i+Q1v>y3?H|%1b6Tel*E8DJ0p>C4o`TZmOeepY!x7!$tUO*2l>kZdAoXb1IHn%xFrk{e@T zYSZp*Zn{=FggrEEefq08vG5>+(oOt1cOo4T0lCU=-7M8ByI0u-S9Y&%v74?CphCHb zp_|L%Kh-lP@A~4YO<&(iV@{t^ZHYO-G^tZVGAfg)7@$SVz})ND%hG(!{2DKXnR6!+rXGwAWe$$cj>%X1N2s zadLN_q<;H~9^n182(?$$!u6j}syz;NtH+Vr^lq*5(un-JKg2&G3onfb#Y~-}9I~L4 zM*^d9%1obQ9?`}|dvf@n40N6#rm_5G7hh9|$aiErFoT>)ZCdk-&C-fmr%)H}i|IW- z(LMz>(_8-!;-GuJMU=t58(i7Vx)iBv7W^u{vl;6cIkT(MwX0kAb-w-PJn3#X{buze zJ8J{q&d;TH!Xt0E4YN7MDafz3Yc5d1}fVS_*Q~e9!FJ&}oX= zG#*6zjJUbFOp~=H6`oEcbxUy2;Pg0BC-=MgJ$AY?XSaH@Zg18+6Zebm*^fa^n*90p zN|(D`w?Tn7LY>R))$J;G?K(u>tK3t88tRrLpceeg|K3l03)+U{bspmTwR`B4nPNBn z2E}12PxO85KL4GYz6pHmfwPo$P)eax@wrp{xHz#Px$dT4D^yX%Y-zlh*VzfB1-xi=dXKMpg z$Ccf$)AC$O573JqvgoF3*T7lN*3vH>!!Kh^!!Kw34I)RK5ai?4#4T{ADHYPEX>@&Y z4(3ESGzWrU#28Obn+H26#N!cP!1X$=G7f&_IbBEnOU*6KVqvpRTz!a$1&4I`xnex0 zn=@;D+Ws`G^7=W4oXLfuPOJ@wO7oztD@z52#3U2nZ~KSb4w8#mnbNOwM$aa@72mPG zpbalD?`qV$zty4 znc1`3PBHP^cKGP7CZ>>GWKS{TfvlQc_{YK-_D>-7-S@sr;`DFta}_4CCU2YOU0&)o zh6(bz>qg6q|ExE0jxLB<(Tw!n&r@D4!Gp6JeX3xstU?+@Dr@Zi9aq^44pPlWaum>Up z(#UCRAJqNo)E$rAIR+RgA1jQx-{wS=qgNH?MN>{9bAQW-{d4xuxsUa}MsP-_fB6~t ztF8z?F8!-XY6r#EIh@@Nmh~3kOvbSn<*@cJM0pHt4}GSG^{yksRy|aD|G^DX++M}o z1ZnP9knA3WeBwXI;Hu4n(j3Gno)OGvrC}WFJwsNkOy@r+Jy7cOK!a9cfvwc?3DH8_ z-n&bkmC;z&<2q)V6;BhxdNUcrGQRGmJC)}9n3;7m%~b+ccjJN8@$gXpS?Qd*IWrG5 zJ1hGq%-KKF3=eAO2V+*OK0H(L6zEuQ5bV50=xC`f0W)9Mz<*tOsPr-6fWPD)P_i2( z(PCY(caGUv=^j6)jWFqp;|D z|3vYr@-~_4C!q)Ex;YG2QlTwSW+@lMNK7-gNgjq$52~etE|yYt9kc{38Tbyjit&!w zCX;p@{fd3nVBi}k5%YR5x5SpGfHYOZjLSk;OB;f^Z*% z{2cP*%FjKV_O5qbn*3$Ej;>FN7jd7w<%F@8th#+wyikK3z@gHAsh?s82io4JbM^J< z49WdBIG2NyPmTulI8eHw+c_JjH&vdp-W_bEc@^(C)N?Cg8!bd>SxVTJS-|mmkhpOq zQg|7)hGB+4N7$W1oCC2WMDOaxDd?wdgy#pN>HZ*iWy#7FF$YQ;_!X-TmByn)+y~HA zp`N&`BnR7UN2F26=Gqf&uG=<2emOu6eG8tum7^_L-gg7rh_l24eK*tyZ0k7ktXjX) z0!b(8Hmv`iQU7B=88w-iEU+TghXbY3U)n@iK-b zCh|e)gRB9k^>*l81KU&>3{mUx4+xfWb3Z7J`&9AnB%!~;TspZT>PzA45?6ma^Koh1 zpJq0W@W%a$u{gVzlej-=eD>LFPBNhHoqPmN9bGo6*L|y&WOYoH22Y_ua#e zFVxB3<79srtB<=UBBOpmgJ6i}vt0c#%z@Il#-+2J&HhoDm+EdUsw?{SbVm$mHWIz9KSOx0CRngiaYB# za=4z_??f2kl`%rh_2bi;P9m5 zX>4F6V>waME0NzpBK6RWGpAV7f~Se|)KerZ%(UQsVt95nzT3ixRbh;CU?BWvfn()< zYQ@^gursoWYsMC%PY4@ecZ7*{J3o`)=SoJp*rmw~U6$!Bq`BFMJ6t z=x3!S>~699)MF3Vr)jP!CMe?z;YFFwf&|!{Cu0^^NsXSx7&<#z$SJ|_^lfVYKaN<2 z8f|!tFIjn@^pAWBZzQv^b}mNUsMkpQIWX#UCmmOR*pdQyQd&$T|IF&$s~%dtzgf*w z9rtyL`;`RBqd`xJP@0C6D9oZad{{fMpLSP09F0gW1FqyYUCM3PS?cw3U*=qxHIM+% zNV0;VeKuy1z#hss30-En;t(+u_Q8XRS5Kqwh@0B=ydZAw*0a@u9@3Tw)6#Y_{A&&K z8Tj`?q-VUD_nB{+Fl30vu5LCzL|f)c*#xP;I8cr9k19C$7Y7u;sZemqEb#UGAqrU#0+A<)!*I_-Osk%2}kQ60mryjT6h2Dj}2|&*|Aj zi=Iue>)qWy%fN2wcnBYyZ%Xddg$Qo!tKIJHA@KYyz&HNfV8}8mx103u=xB?w^5w2> zohgxWgDy{-(Uk4g!MeK8%9}WWp*+3Gveihh5Dp`=b9;!>zIFS14w<-v?250acj)?3 ztu!rZ+S+dn<7hoDL16s#%xc!9mS0^HBOR!v)<3TbVtj9$I2Km|uJO2%aNUS&A}(X^ zXrzxu`e>w&M*3)^k4E}vq>ok-?J>BI7AJC44m1LJM<|KSqmXwL{(pu%_~(Asdtope z<#2DoJ>b9|gOkME?rFf?M9Y}8;g3qaFCqs<$}+}TDP?K*oRRWi6fbp6n_=hFkFl|g z*(}+GR6P6stsfFbn#XISdFVa7J}|yH!jR5WBXKzfVfk%=QZ=*`-ug?8c~ru_{G|G= z@mg8bqkXGk?Zt1crKRwE>09oL-$Fd0EbY6Nc*=7Edc+p8#QTm2v|WG$ZCC47VVXc; z6324rA#TLk7g18ZqjKoLeetUA5Sh_FitfQ+!!YgSi|Bduntha-%YXm*#kb%2?<}#- z%JwXLKNjD|y;r>->D8Vbv}H))Mx+P~=;~QI1MqDNYK?*2NAD;qy`t&rGt%(niGq%h zgRlBN`2DXovWZ+A&Ms6~=Q_KEP@E~gAh+{~BAu9Jh)&6B01u5icV<&@xx0%O7(DUkRc$((j(lR$Hvro--lv zwto4b1_^x+NrSU}DLjeqN2Cr@Oh3GbSmpx4pWz#RygOT}_U+HkmUi~Lyj4a}kU07! z!}7#_aZ`63W^&vZX%>~O@pQD`UIX(R{g_-A4echdPi(3A*>FK?3O>>&Ywv6427*L4TfNN|}kbw(gAGJdA zU=Hu#=DWu+MmIS56u}7TTePvNJfWhh0^C4E@B%S%oi4Ov=HXlFo~{$4g4h+aM8i5A zG%O>6bRv(7z-M_pt`V>Cb2{PtDtScMh%|YH zK{U<_VYk@9o1G?dNLZ}Wx|3W#W6WLhiX)=o;S9VV3U`K+Am3N}{^AP%o=tns^jtS% z^7499P7jF}4EJU*W0YrELyTjBDJKW~EvTs>Jv#t**IfNhT&Owo40FwIH)025Je=V|q+P}$_qMBe-V8ja`$PkaHvG=;Ese|4pm!Z# zfU%B(mWl3Xqtu>qb#)l?Y(5h9XS&0^UXtaQaJoKWtn;mE#Fa4u)tFrDK?;?xwcABw zx}e8s|6S@c+W+V=+FQoX5IPco8z;(qqr$pW2>pHNixvK?J}1yVMPHyFeBr|PXfuPI zF7~q)260qa=d(jBMrMhDOl3_u?F=24bit&wAQldKAt!Q#GZD?M9@8(GnvXkL{h$`4!w{IhCdl8X2c6_?sZn1i%G^E#!Y8p0GXN$Df*x)g2e6Z!D zn@Wm~omLLnV8e7&a5_3K(Dl&Q-FJ@lEFdeQoiWzAzB=KY(gt{i6E_S-(|9+&?&<+2 zVrg#{43+A>+tGW0?!GIq?}k7Yi+*K+!39{X)g+T;5J_@?rm#rb}QB`PE69HZF)p2)gw}- zEliB3|3w^I-NLg~dO_K0qw5tsAr1tw<7^y8kk10gbv#I%*a4r``Y>Pc*Fr6oRoUug%`M9d|n%0|-d#TE)(&xtp?DKzmJkDi^b z<)?cEsOz?cA?+*(OgUMULBb%Ix-v8GT^ys`Q84n|AjvFzOU8R zu9f#cVW)t!dGE7;OKD%AN*)o(!aRsQ({W;=Gv=Ev_+{IsE3x`IyRfzuK|E>AdcZpFy&byELYOMqgcZQprSG%e+jLnPq&R`4!uPJ)?ruR(pDv$W)8AH(*vOzX zINIe2Lp$65lg?}d2mM}*Wv;J5_)pe9x8b`GdVk;7?e8gV+h9eAQxI-J)yX?|M&rc1 z?C&xhJeW32xP4WyadA+`GbvRmJs2lm@@BvOR!R6-R~{SD*H&juASeXwZ^*t zBVSPa`LgNd*0O!R@b+{2r*E+C-{%WyKUa45tJbnl_usw$Qy+_`S#Mj*+V{`e-(L1W z*`M}5u>Vg!efz-rht_}5@leMX@1(ucIFL5bn4f#Fah)Ynz&V}Xb>4&c-a+rWtaTqf zP^~UIofrv+TN>NpAp-@Zj%sIOwWk%Jfe}wJ;#I8o^lNT z2xp~8uuU8r&A>wnb6A?=`R5(#ibVPuBAO3g&w}y zCja_)2#b1Y&x&JtM%m^i!H!`zF(DfFbft)9e8YsZ$~HLdwTVf1T6TN@En%WCJMk1Q z^rH+j(k4#Clb${ax-r<}N3$q{xHr)(%J-s#?q=F{y!Gl(JN^Y0wbNWI>SaM|Pp^L@ z_DIJg_aAvDAsEf1sH$%WpJl=c^H`sCv&D@<%wixBon(NhYdx< z&Tz$$37Dlz`-vRKMJSUz#AJw=u)<(kf>RgaN3F9{m1I4kXtIQW8`r}8iCs1$F!H>g1n6HW)Ypl1*_@6m~` z;i}rBQ?d&o4c((-r8Y*fbTQ9jcDF%t&to6d%}i7E9MD>row3~vm6W;$I#emYrr&5r znFL~bn!bx+oa@_ga)KC<2c-o@vCfCb!+zYypzJd8nBJ||5f4YxT)k0D+Whpek4sIE z`@YAk+5-=-9|rBr6i^<~csdB2vy?!5YrQ=bYlG=*KZ9Lk3+-w7E5V?&pr2FttNXc9 zh)}%fXBbPejcN~voDN%mSr5Z!%z?oDn_Hgfomb0Dqu==2O?Tl54ZLSZ0X?V1jw_f} zW$!uKQ@!G>b|Q;YB{;=7hwCZDv!%TNU21;rc85NVmaNO5`}Q3LJx6P+hRu%$--A7H zZqMXN$Nt3(x!r)X_snyU@=f@cx;XfsZs6#BT;0Ez?bfr9JJGlFKd$bUrV&kXX37D_ z)oI_Qd+RySe=<1XnpB`q<6Gh7=xFUWn|auL`VHzcb>AQFH|x#t4z*m!6>sbu!K%_+ z`fndWdDPw!Jx6cYqnuUS8lW=TG`H+qtZ5(tk2*qhQ7Y% z()lM!?=D^6SmcFu0Fg&kBjt_!?G+)yIX+mysk26QYcQds_{%pb?4PLk0en#VU`@_o z%q~PRr}kZAB>8v!5%AOtS?25yH|9}KmJhaUU+LQFHm>Q8O;ZxRqwJ9GvfHK+r}I`~ zcyS*188*lzEMN?yvq{;X);yX1vfO_pcG~_Ktz3pHxGgOun903o@57$Qn&vkoeDZi* zP0_9mFFe0uP-V=I1N{M%uV58ozk+Q+1q%R}D>8&3CypDImST_H47R#b|c zh1_UC+*FyDUO6+v)mOxrN@f$od|p4ZQGB^Q5AO<=F($i}F)d2~&($?yW@BH4sdQwT z{Jb9e*_b@Bsya{HR+%equg(=;Z_jlJOVDZ|sBgU?DjR!01F;tR)`a7lc)gr7AlAXm1#HyU! zUF(O<7q`XaC0DFjAa3{OdMaqEqlNU!Inbr{kDt>h2Zccgw*xajggm%jOe#WDyCQI+Q5pGV3$halyp7^h`JxM8E1Axo+t4 z^O{sM0)1+cyD;>GK`?^T>;N}sX-*p+IQdKEW02 zbPhhC^{Pd(LExD@c#$BwF`Mjyuf%m^NrLG+;-5x<_=XNo)ALnAD(8vP0ybu}`O7O|ej zU5A~Db;UKlG$O4Cb_`Xd2)$9IyY`n@ufVMMbpJ+U=;E4j&iV(vq)%IyAq$7dI!O*cq`#u=^Sg~M zYnu_;4YYmmQ+l47Qf!vWZ>^yxKqrM~A;ET8+l?m$pKZpKK?72}#iothS+OKkQZ{ncb7UIFU*<(f4Zmw?MD589d{g4u*YGVw5A*Xk#MUC=sP2hp>ofzy8;7Hfk!YgKj_LXSzvQgv2-J0 zEUDDHFWGYTI4_CnU1OjwJBY!-&C`k*w}0<3yYCe=YD zR@fjCyH$>D*PV@W?t}!u-up}WD6Ho#`MCzjC`~CxiAjKc<#M43(rdHtU;Br2Wpq}^ zxt%(!Ye&_X1xvd~@W-&84BC><6EVLJeF&R0qwmG;^AFV;9F7YQsXOE8?Rhcy_8^sf ze)fe9cEb}Z(PbIM%aJ|hzOS2MMZkK;QGWx(Rb6eo9#K%~Szd-KJWj!wxnT-gGD59 zI`eio#(YmH2e&f#Lq@8ZIn_?rWk#B6r>zA$rh zOtH|OF=WTsAdOlsd<7gaPW3E}pCUC#_t@lRokVH#61lgF?6IfYn{Nfj;E2CZYZvW5 zmsQX+*5Qr&Gt%ue7KU-z6lqNFGj6uM8DmRFVpune@!?hldrz4em4~E|P#ESuv!a*U zPP2y+3GG$dzTzZJIe5B+)0NR$N@^AKKF!Zq*EHWHW^X0zEDMp(bQ4!%uW>^uouN2I6QZRnvyq-n z)7z2WZ!Fi+Z|z4cmg`HODWNvkn2+~k4Wcz6=k`JqKVYV(o8;vK#FQY6xqdZ$y7fak zs`BIBAi2AnXy5S-b`P;_^U3=3iFPY*$#*%tx@1^&_mdvuD|Yl-TNR|#;Bp1*x6v;7 zlsAYMI18n<8%o9UK6Uiz>2Ni5KIFvu<7SrxUKnYQ)++5)1dNhJZlqtN@A7kLdEbHV z^PmX+@)TQ=3fNf`hSvz+%4TO07vF2%Q3UN4py(y8glgl))%2MQ=6ADdsJ%i`o8mltvHS}sG`6ScjIHW>ph>n4`3U|<&Qa})OhCG%@ z&&T9rx6!Hf!n{s0vV_{u)71A0*VLK{JvysB88OA~I1YTW*V%sh%>v7kssvsbhNh@$_tJMH8?Jfwjl;~db48~I?_ey z=^o6r@*d;9F3@PJQ-CD~MwyPdd_iD&!9ULH`XFJ`PS7}M`8JdncvhcwvHX{tty65Y z9H*zF&kMeT>Ih#}lCGDt8fpq>rYVrLAh5SM!Zu`kej-2(IOpG?XEv&+W*KdilbTvJ!pFnrEQ&N&Ge1p^`$ zTTBE>rll>ZFgjxzgycX(YQ3RW+o@;NX}@W$eq$Zy8J+^kNeH5dQ6qiq3}V&F(>h89 zGTPB_O#l@U6x8v8MxfeS(BV>4i<+jeqo{cK8PB?R$09 zTPKw5?JI;DXs`Au2{cWpox-ocqr(fq_%D1V>8DtR!92LA`J zs&FjCMDeQzDN!hAXEzY#`5!6kfKpSLLO$&E+HyLy!BB(tS=Sh*MDE1HvCC_@C}S!&jcj@#%T;N~qS8H|_!`@fkD7rAXDPdO-qJCnfNR%3DosYUOQy(;<%T%rz+ zQs$uq6zXwQAiWA8-_Ne=J+ih?uEl;~uYmTxh1^w+)2}LBV>A+9w=Mx@i0-z-Cu2r8 zq4v|@HqK$1We};z=k_oeE{8>wje6Mmb&G43RVgi2i^WLJ0j-rjF<$~wc)vG}lK+o> z#t_pq=d!Kh>=jNg(2(m2Ym)u0f4t0p4#?y&;GYLJ@88^dv=StN@-(@AyPlfkg@w`3 zNW=9py^+0J@ktWsX8Oj%ym}V+(-zd-K7lz_2HCvu9sSnY$QKIium`XXaTI^+4$*9T z%Ry~hnQ~wAeeFKE7IA8Dn{&x}wXim*WD@?53`dVEZ@-^N92kXy_gGp~-xD)-)e)nb zUIJ^g_xXNzjBHU9Xh1iulb`qMhB;#{)9#%~VM77@R(hPiMQIu9RYo)R>aZn88x5xf zzmSu-0WtzX{V?x;4cVR2KUO2gpJdVdbt1kIjB`Nyq~G$22(ae>Tqq9j*ISvE@erE_ zHi>SD!z9YK8cjyKsmFU77k{sZ5gn|b6K{Loy(D>7t-3DWMcKU2y3KP+!M0>&Vh8~| z{D;PaxGy(5=oFBN1>b34Dz*i9j%*{~ z`PUbSXxrP)v2x@OO2JG?Ipc(EsX^J4ua+$5lyyxFnC(o&S#nJf7Y+Qzf5`7ZQ?u)G zl|u|ZZJWTKZL@@Et!uA&<67zIl~=6Blo6|?ag)sdR1DJBUcCd}i>B%4M1W_xO)Sz{ zav=|0Ryt%eNP!vk>dgp^vVh+)_NL_zJQW}|aIca4-3OEH?!>8kl5@>BxvSULYjbVB zdOTZg=y7Kot7H5%tizLSdj!&_%`+k3(dv};l*+Vd{VqK-=l<&}mk(-mX;EGtkVB$} zsti0-?FUy+g~)F=C))I1iu@jkO5;SZ&xI>KmzijQGap3m&2#uiJri(TMKSI<;0YLs zdL}-f4cYVReVs^GM2e7?+h~Vxyx4TFhnbsA2M@9Ao^;@I>7MOuUm|F6W#ZE@BG`$o zzD#OmYrxx=xCMAk5gF2xNPxia7KL{Jf%j@5K(+75)w|xSULq(}!~y&a!E7kx*^Y5= zC5AY^r*G1oBJQwPl zoPvBvgjV2_v;=@}R&U9hSAtIfC7mcn4n3ac_+JlEc8E~%=1PH?4i+Wjaa&`>_t*n! z(A3+hZT~uEM=PMV&y*%Un0}7hhHZdNfjIIjyaGKcWHao0vvv6jyFpt`qW(ivZ<>QT zfBOM*r1jzk!nMob(vke*0|e?=MA5D}s6kYnc14Dt@<;<7gqK>j;(UVaPUFaQ?LQGy zw9fiL2k=c-(9$9ML35-FcpxWQEm(Bcjxg}doJu|?QG|oehzNS7b@}t&wrA-)D=P=> z>|Y6_6DSX(YM#S?)5G|1;PDLhs{;Bo7y&Q)V>)DHH5=7?L{aG^^gwBdNhe>FtQG{I z+?Q?`aD0bY#_|yG8*2cFVmZE`=Vi!oxUHveweTF~a13r;t~~}hP_kw8s$RfI^lITn z3FN+==^00V-Maj>V?U%<)#;u{&*Xq_t1DCiEgIxKUKmU!W5ON}%l7%;cwdfi`0B~E zEgDHXAzHkb485a+%%(VFd8T7BKnod8x2LKAFFKn+$BEFpMd8;$`K~8=uRTP-x4t@G zoFek}P0+3uC>sf57ZkC42}Z#S*ISp5REoI0VE+vvfP%rt7k+m!%6%{=_zCR9-jeGM zUvJ;1T9-Fg8n_SwaHM-O0#Y1-KBjf~s!GwKz2pf_2D?p+koVl}x#IKgHvgxc8;V5U z@hsphDAb_c3;lyk4hc91TbIk$#CT`m$CU=?DGm7TzU-ZyfMeMjv-jPqnAYXR-@m^& z_I?Fu!)_68!`mo;OiWmGs|6YCpEtmn6q%`)3U7P?ahTN5`Oo8(CXbjtZ{QN3htbf3 zQugZB=OlAq@Jyj^&9aMh)#M4C$Kz_XvvpqT5uFBICQ?t+$LdsT`{LB?%tVNJvVJi52=@EqzK#}g)1gOE zge+huc;hZAR0Vbs*1$X?pY>Uwh>IXT*U_nbcn8dc0EmS8B{6dkJ}uJm&vY>8c`0(| z!KsBHp|Bl{irJEcdw7tulleYaACcQazbD(Ko|-i0rm$L=3_H2mHSv%$m(nS>$`b!X zsHPtvWYf`aCX9Hy5O%h@_++e?p8_#JP`AK%S4uj@OBFIvGUf3cctCBNy`p@@K0-`T z+q5TnFBOE zvj*ng1f;3=!wtG=FuDmm!n4o!t~nDzzeX3JZV@|O~MFWSDLc!Nh=}tloDy5125RV z@hX-k5lZq?nntQuP>@4YdXP{tu%8Fd5)b{G$H7jNVb+^&3Lu5WCe*C2>g~LKcntSc z#&(_5>npr|kEgr)*3}A>hoW1-O2CDJXR*?F*vPLPqMl^H$i-C;=Ms*M)$EQzofE8C z**!3lEV03RwJ)3X-oXk6&#|Q800(D#>xjKyC$ggKZL`7JH8QhK4H*;oXS$#Ba%HV4 z_fpIRZK(iw$j;TtIc3$3+5DbvrI&|k&AD#_E$^&F4W$UY1mp|L=S7>g!x^V*`(Ef& z3-Dd1^HG_(>z~v%v>M_Yl=QXWB{e`)TpNQ|b#O3Jc3}gdw0T?QU~Si#=J4;jql58G z+wru{^PBE|9PdQrkRU0^7?Tn7`p>#akH6%;{lH+P9jrtsBZQ=gbPKWfPhL~g3&7H%1lACy(2oIdtxHaTo|*fid-GUZ51;UsBH#!B1#Xr zY&V|6xDZxBpo@dt9LO2wS4||G#+GG9j=23Lw!0(QOC!)*-Ue9?e5-oyB86PQHlMkc z%S`~<+>;C$TE}5cqIz@g{-1d$ai>B1npYN!Ho-3KsJCqVk# zmCISaAhp!4>2@E?*ISiF!I1#)tE+lDN6+N~#;#U)sSqR*pEc7R>4{)34j=u#4^X!6 zh4Ngku>vJp8uxd*2B3aw*LE&fx1s{>clyTR`ss2`&QZED)BY1l-rZ+(O`LGpXn-9M z>CH-l7IIOYIk&g-?5*%Fm?xk?@Y%*;e~?nHIUwMjuot7Yz|T!|!n^yi?;nW7y-(cE z9PQf9vzPyOZ~t-yc@4^5_=QilJ_r_CoClci0QcaivEW2lPNWrbQHL-#L)5O&snWZs z2mh%gjy5pdyW^O6>u23DALrN)_Cy}GiRz(t@LPeJU0R4tSp>6l6~qS6;CT-ba}a|f zf~X4LNVCr?a@~aYG@fiXxi5PSp@0*5_T{jo!hk$%16a>kB1g9m?hKHfn$$tgb;4Rd zFy1;qO9CC|XZvZa?0|iW4h=J1{0q zj&`>2+N|I+Pc#|;DQc(2;QV~ns(~?#8R}W+1wubt*fRay3%)PVQ>$E1x)Mw>!K^{; ze(w%^h8j|t$*e)Sx*B$eq;`{c{eZRO`PV={wKiyN5n@tNaxf0?%>gEo0TZc!iPR{< zpq&m91B`)g(f9ZU9Pi5pk3+!mz8nW&;tBq-?;PO!6V{7!#D#Bf4my+PF$WjgxIWI2 zWo}GW8u$w2ma=*6XJC^;dJzHs_#~Ff z<2Ic`ok{{|1qBUSxOzeJKlZig0>IL-hA7|?cA{=eT8Q$5tdbE|?o`+g60a2&xTl=j zF{ijmZL(Dzw1o27WR>j&d07tXz;GzZ?h?Z|-^o|nr79D_ixbj@(^bQguA^JKJKy~HyQyd(JC zST({|q;7)~zae#toVb?QWA;aaz087ZHu$s>_yrjZS$F^^i8j!Zt{#X){SbJUl`!PR znsIb+lX|C>eqY?%^5HI^6R$N)>FTXodoo^&U*nf!CEnaIR>A>(78jFcVTrL9!MozS zyEO0Jldn{(1|rwCJX4PrQ1lsF1T?P&+6Q`ZMS1gELvDM}zoBk9aPw~=rtdx4(k_LG?Y>jcLT^a2-w@^S~3y zPLFI8oK-VPN{nUmnF^E#^KahDg{=!`FU7kpoS;pWU^#_f5wXt?y$uR)dnD3FH=J@I zqaE7JQ`TU6Z(6pemRb1ik^tTYzT8lM`O^BfY6rd>719PQ-kPum#Zx4qh=9K-@b@(Q zJp+GGJ49uGTag#`oR==)@AaIQE);$Lowv+iZbTuqi1Gak_wax3i1IW->4Wen)U)mevo=~5NyC?OR}PnB?^+QPowSTX|iZ* z!cM+`Cif&`snw~+ZsKo3>b+K4Zw=cBQM!)_W%I{gtmvS{v2ocKkVGm1K;rHzJUf9a z^|pP~Az$X%EbSgRA=RIANcE&P%PB3HQGz_6OaSl5+|Pe-y*6lG+?Y<^5R;M~UE}jK zJ8UfKLCr-N(8C0ohP<#f#iygy3$xRZ zC!UE!eVidCKV5B%{qlg2Ay{4+$pk250}#8FpDwrXQ>2nbbq0tu&CzBEr5}NR$=m#& z$|Mn*4B_K~143ZpNG8t=!2JPRL>1O_F?m*xVwzuiXi11h*vdCCV#p}?VFx9yQn5SW z97Qwu^xa8ct_i7KUYj9oiIhqI3Ra60Lbk~=s$z#Vo`B*0nLF&}n52lRqmGNUmz+y9 z{7y6YkljhIc3!N#=O{I6G#01cD2h#k$Y-_rO0wrqX2@{F5?Uu@#)RHx2(gi*Q(Uce zpe!(Bow6kQ@^K^3&=vi~`RAEy8SQFIoH0Hhw6B>g(H6E}Ze?32bqQwnwr(08fsA$H zEm}Tr82H?=hiFew@33g8s6R@6E;(S5n`Q&wZCP(+-xtTH@fO{vLnG?eg6g7Lvv)|?yB9I zp9OmLyzxs$VDs|IYuBa!8~EePZUHw;O_{N=zeZ&B< zpX1Lv7_?{IgZEKHeBL3T;1?NHPpGKelr<4r7ws$TejI1E*N2KcS`WmXL5#S$dvxFU z%ExNcae;rAB}tB7^*4Ytm<%mEZc_;dA^R}wrc%L2g{7H&<0p>ZhP;6ggiQ{BO@bC# zSuNy-s(@2=4r+3b+~_1x+ojr!O~Lb14fz zZ<8F6q!!d$fYLdnHIQDqNgh7 zMK9d7kXbsIwp!n=*UhP%6P>;*otb-o#VQzykJ%glwMSt!nE_JuCnp>L)4`$AqdkxX zt{o&t92c@*?1iN#QRIU~n)E|hJ_cO+N*#0f-ZyHBoW85Hq)X1_nygIO!{I=Pm1l~% z(@~Nph$qL00A^YNha!3ftMfNNM3C9*PGt`T7B3xrT3dNQT4V zw6^ghp&2HyFw$|Y)hcc?j;H|!QYHQbo+!1YV?GC z^=;#jz`f?Mvhx_vNa917D-6G9n1WgQ<`F{L6QVh67V>jkG;3!aL`1`Lr&l-y7Q9d4 zjlc={BVbvHA-Wt^>HAD+Chr;mZ*XDrVcR7q&G9Kv0~ZLCcEcfq`uUBC;W&;zpVkI5 z>hoHRnMeJM=@s9YcqEH@WS7oNvNsO< z@vmc@D(3AcJp@Rof%8}|nU?iB!umjChtv8z3x~h?5p?5Mouj4o4nc>QiLcAu;bAtM zlj{V({m#;*f{w3CAO{LRuN8FP^>LYpUi1UJ1N>)vW7b}bqqgO%W|NEwC!Wlrrx%g$ z9{W*?%%W4Y)WgjfZH01T`ceg%1~w;CwG&J27#ksxwvm=DPl?YgF7%`fKaM&9{a9A- z+_X;LvL|h(Ww%y-KBIYA!y7K9J-T)N>BUVDiR2ei19GKUxjD{QZmHZHUMsK>H4z%n zrc*_3!$B!wVR5w(Yl`!VzLfdxAV-!hPZ7cM;zGW@pNY^IbT5msv^}AP_`FBPyez&Z zECXQ&);zxfDnb*?<)jpE=6ClHDJfXr#|8FVV!w!#NvZt8A*ki#j-0f&#aYYvT`$ul zoI`}#D4(xA`m&eL(E`^Hbn{KVT0O*~Q9*pdsbbH=&W;f6Bi&*Vw@{2xo251jvoEJd zTme{-kCm5b9}3b%4wPe~FJl%=lwC8xt5+Z^jaJAup@b-#@_haqSk;(FMrN8W=D&io zE3)AnVJ4gAvpXirq|?N+O01@iVbqRZ(0aav^WX%qMhf*7yQ?yC$Lw5FL^75_4!kEh zq*ehuOyC+zBQ%m^>mXzsp|!-8nuw%;TC^td7jc)`;>rIJ)+b~8Am5THiQm>0G?m*& zipA-gfI7YbHaD89TjDM67E7v%&%|5o13gq$c;!yqsVj(M-~(vy=%G9VU61#2{L@2T z9#eE`i6$bIclQ7<2JDU>ew-DqVxp6R8luhZruQYK!hAvdi;<-~FP zsa4B05e=8RY=(nO!#OzO*rT1YuJkn8c;9PJ*lcP( z(A3o6v+%CGvikBn7nOBZI7LmyR)ykR%bCk>>>#XL67Rt35xS=y@Z(np7CFF75Bq!6 zr=lexqN?3Urjc`V=8)+*>7?E&=>&Sw6@u2XP$p^E9+0xL8fTTk%@VxlTZs&`AN*TG z44nHkg4)d)TK;!k3~vg+c%q@9-iWHKDFSBCp2~mq(bIGqp5y*wn{VrK)Yt0;pXFC( z9va4aDzu(&x*m+#K;h}bCr>^2)E#-(0g-Z3F{di%Xc?X#q5u-c^VZNK8LOdrp(jYj zZlgVWdIqO(p77g)Q`5GTz#Ep&>lp`c*sBg0#|>;y;2EfQXi}9BzzczdK#xobsEbG$ z35%$UNCnQ+U^tjYK=gU(Fu^bC3&Ng*z7hCS*9jH-JV}mD(jc~=j-E+XitK(u5H~{2 z)(sG&y_NpPS9h0#4+^CCq5uls(8T-D?)&`Y3Ig7mCsomNZ#clJBfjgG#;G!6(mWLV zH7%8G8Xm8O)z|aX-8<5`E1(7`<*^0a`Mw5ENX4BeHKZ+tmHE=BY-7iGkBt4Y5$f|i zbGth*?Svzo^Gv?oJtb|Y52{Fyf$oN{Iwn=E&d&i_26zw;^U%M4c#FqK-}2-I+yeL` zbGlRo=}8ARV;c~IKWS1`PWlXI;M`azoB=RNu5A`_Z4NWeE3OcKWlw&QVq|&l?H+&n zN*BcS*M-TohWh@|4AV$Rt}P-JaNBU<3oAQ38qT5B*}_F{;UCAaDz;d&Wv-pQ3mQbo z__D_#+7OOdup6f(d1b$#S!aRoYmS}rjC^$Yf3EkcXF2($-!AZRaZN<8F&h-g?b z_U>O`@pOIkc)vX&6=9T_`$w@X%2E8b0p_^TS6_xH*CvV;!+uCg@)6PiMYb5|8z>If ztzUuUCsr{ZCGpmDRt|cecA+=2L^DZ)mU`I-F$B)`={1VTe*mw}+qXPLV3*f*9fLN+ zp6r1KV=WAOu>*O2f!-n--g0>S10G;)hjdoqDe#5;)vGE-NV;aUvi%eJTm6Jpd~2d( z1K?kvQ=*09$fXW!A$YSfS$b9?>FqmtX|}Fax01WhZL2z8wY%<0-BL$e%>L4xuY9MT z3JY#W=5=mywrvd6?CJa*xXR9>)7Pm_IanflBuw3`Zm8Y}Ht&tbr_%inxD1_WXYGL&y|CRJhCX z!IL|jPoc$vIK!kjliGmfh}prcssVgfp&raQRJN-+r`oWk(0YMMI&70&_`a@)nqbEB z`#X>9+{;sUCr@!l3qJk9m>(8u53n6ePgcmfNuIByqeSB-$L?e)+}{iHd_DYF@o7Fp z-ICfHj_K=^pXeJi+;Z>`Q`V%;m7lqGjX4{AYJUCV+GW)@uXJC&bMc$@($5Zj8h<+B zvbq^?*>rgGOe1|aN!#1`=R4cEhO=;Dsdrs90hNNU*f<|u(Ohq2)pCf20__0&MZ#Y+ zO#~!a&MZ%wpuJL!kv4nkLC>Px&RuyGGpuCf^X*cQUt}K*r$toFZ*s7GAF4J!AV5Z- z=4V>g#QRyzL+{I$H9>|~)jLYkGtMvIKIedaqt%k8h5gVtpD)2i?ONjfKt-gO$a2n} z)&}kaZ81Aach=6&6u5}m`&_YmJ?ATFdnKaQ_r2v6_{MK`)CBDj;%fr@`D0<#%+!+y zbP$gcR1*{LhwUb^eXpqAxdWPquqyQtG7a^&^IO?OwkkIPuh|51)qSkW#-!p;#3vf|} z`AD_8E;2&CU}Ar1a7|VSb?>$B8A*XcVL&;8v%6sSs5%$>+{kCwh{=>78Wy0LWn;SXm^aL(P8>~xx zRVD)On}4qHORt`w)tv_&3RAkI%#!wGY+ZWq*iH_usN{g(QI$5CDJGeR%$d0I2N$X) z-+}nnm?Tx^Z9?_-9niIewammdyeiA-2J7^4Z){Xt(|Ll&GEl$zwR#-PMqyt6*xR|Z0dXH*W{uXW9#{S7~ z-E14`nOu{}f6NBS3znI%4C`QP+e~z8^3dS=nSZ~Txjs{U-oaKqI&M?9%M^#% zb1i)9*A_Nd3SXxz2y@BXY(1rX5gS>_@wIHUEV`fI$1~G>?Xn-)*Yrp3R`A+ik2*#=mi@)CeWt6>?}tcYm{5GQyf!H+u3Xl?iOT&L#X`$ zYN`ZkLJSp|q4yJDr*5-oEp4)aFsNmcT%e&x69n^AX5dLJS1X=iqz3y29%Wy90{y@C z%$xVmCpf5)1@w>lzFTZ+^p>`ov9E)X(6>>$&`9uG0>w{isl&FL3jo&13*Zb?Hi3a? zI-Dn_ng!5vA)t+BT7m9C^L6@h+mOR=!LFXt>37;T48m1c>AG(`hQHszp0*8jOc-|H zw+n_`6Y%?Hv$qCz0W(&E8)|U-@gF(FzBZV;lLZSt=m0zpmZsfl8|;QUJ4!u%%q`Rd zt~QHYk48r#JOS;Gj~A3`Cur$_@Tk^IMRovaXwBq(h&E;6d>|W{v=>`XbMg`Xgm(-b z=Gp}thbFYKw@P_4JB`$nX*ap%0@-Ppr}_!nRw3(WU_W@ko0TLl_z@Gd?ZO7d1Z|VB zLh+VU$oCX5H{}JZ%`#@2=y-uaHw4)sd7F#ye02LUWZnhX1P1kt6^;e{Fvd(<^o~-G zg}Fjj@LK~X(mHjCgbN;NRDmNJK35`!dM3!zdDf3YsOFn+?!PE26XZIjWrB7?jM9sbuGrEg zE9>lesYyI|KIlw0g$bY|KN6&@!h|hQMexfqJCbQTrhkIx%ich2c%m_T_j){k^6SRZIl55Y z+p)@g=nugAzT7jvq#JTyi>w9Vgd+PkK?fW zpX~!}nwSn$pO1BT0tW7n^>iIwwxev1^5akk_AEyis~d?^*5NGj7y|nd|M=`~w9+!g zDK9`;DssXMnh3anzBdW0q3?Z+(o&kK7q*-=M5O^eHBM7*mfn7{v-NT5xD$U9MN3I5 z-AKGdOBp|=x9`29K-0R*fUQ2tZk|c1TWYwbeA(u3@b*wz)D4J9tN7t4*i*h%Zjp&; zksK%$EFa^QrxA~OY&m$jwoG`>T->9)h}hf~ShGlJo}it1@p8e9=VoWtaH`cC$rW3r z-@wjF#51?VUhWPylPMxawvjk<4DQ`<&Zf@Zvk~%+9_{fg>U`{-ZMvl|vPhla8HmH8ukKrztH14swo{ zzWUbw!)|_?M4TwH%>)fP!Pm*L`er?+th?<5TgAe8q}BWYb`If$y`YE4(fj(9^*r`# zLyRZ6q@V1PZ3rPkM^-~7I|63A$UEDS=iE5y*>(Fq(j^Uhird7E$}zddl2YvU$ttXN zJxDJPH1DKbPSK|+^&jc|<_x~RdU^YD-HOT;tEai9WeCU9Q0j)5w#*HU)cI9MFTftX zeiUV4upk@jNBK6e4DSG2ap&4E6U}H{GN;FDi@t(*Sdh}p=_|pTpMGm?=U=+{tbQU$ zV`u8X&$ENrk$fe&d{;=-@~YcTwCNPrdb0W-ha{_*4uXHi1SR2ZE#7(l?0&G#R~l?c zS|`7uj=Q&Eh;vv7=dcOhFW;a3?m2}#iX`eAoy#FdBw|l#+|rw*#q2Myevx{*T>DeF`oKdT z;oA26zrtl{T#|kzX$glG{@uyDs|#vTUPmb%>&E?wF)hxRV1YOk=pyE8$20zlzMV#mO?HVTYv4@yJvBv)=hV(eyq{DYk-}H^g9c8SFVI=j%Pc>H z&?mi>`1>GyUlEjQrR&CcL+8m})UJiUl3}#FjggA77*Y_68}4i50$L?~y9$IBaJYo5dql;x? z%V@ME_ES4(2uf`jms}>q>af&eVZ|)aQ>V-r{jnYwb-1zOlg8VQGIP=zi2dyEA!^tp zZ6Y|^;Pq6x6ohvo2(N?b?Rc%hYbl-2@=Yw{wuku2grlQ`x-+xFU*g2D3$Ql?Yc- z)&f^yL{7J0$Z^wbM0J)^25aIf?UwG-K<2WzGhr>RmT%&1}r_kJ_Mzs8) zzCEK^bciMmu?&gcq4M+K7n(YQY&JJp)RuDdNn@NTLYF>UWsD`kUmjxY<6UVl#p{0B zM&v|a8%H2g4fe|!;;A)e-s7gN<8+Xh_R>_DXfbIOQb?6*4fwITA;$1nq%2uRJP$gF zP!Yd48+c3e=(?xx)|SsB8pm|&I3B)Snv0xsddmi%sKeR%Y-=dIpHz=Opi$Q0m$b06wz z4Q)}NOBX?ml!W-8uqO8;wy}6TVZA;cgW+2@tgI&T{vKpVTR)D+Rd5~(vQL<(_a^3%RNOE{aY8xsg+ zemPjNbIvA$oDVS+yS&~#2i&7Mx|d#38m*UNHM~VC5ydu<{L@kj_+88O0YzLHq5RRj z^>MrxpYn}GC!7fcyXYV6rVZ09MA4U4Iw)qoa8IR}b_ld9xYM={g%fg%^-{Q|_*AJ7 zZJIA!P$`Wd-3hds-y$XQH014Q(*d}@r^>6sFUpMf;Dp4V&iLro!$y6J=#ZF{pFf=> zTew)WCq-V2HXNosqq!_!<`+(Y-Al9>wA4|*jMtS}emdtnr#ylGb%8dqjZQ10B@@gr zUT4gVYK_zRmD}N@$Sr0}&?}8{z%AVH%I`X{anFi2|gw_*{TNIIY@Es{) z?H58x&#jKfRx{X$IldcWg13G?Q8wfng_F7Eev!FnQVQEs+pRR?6LKnjR6X1L0QGVo1`Zel=FQf^%IO5k1e&6qLM zH^ad8;FZwcS3J>?zTudL^?v=_8x0jZH#5Rt>wNP*L&?%*3K`o|Wy7WPoEBtL|=WZ;txqjuI!-V58E9C`}YU6OW8nmxJD zKE9H$LpvYYs?E?k`H}4pKWaOeQIzx&S&pnpmmn{k@{@QD^+3agpk#qvI6t?KKwGY? zgG)NY@eSa)&Vr9zM~{Z&!audL5Sh;L=@4TJ+DldD5S*cmUeWR>5mPDj9C^_gWDYHQ7P`VK`?UZGU zs>4CxmrDy!r(|D!3vmZ^ieef^?cmF(%5*?naF%22Em{)w7nPsHSp==`Og-irS{5_f z<@$1c{AP$``0A%zyzfQd%+pM>p4UUBttP_S-~VBm<*1>!3eN9{RX6jrAM>na{^_Xr zo4ToI)^onT+rx$+1FS0ZAfYH5CT2FTPf>hkAyhAC6SP?Isf9RJWi_7vd)W>}WHCth z^F%7}g+j}lBJ#hz{x4{Iqh8Mt%DrB7in2?fiTjP@+>7@)fQO<}u+~*h)L%j-?Y}D` z4-t8DCW?>#e3cXJrE$jRf|zUQ@mDlU`DqPg5czEo`a$Rg&F z6S-zEebo_>ChJ(FL7s-zOSCPISaNP3H-m%}$@dO=b<}p?aNCmYnLznA2jQUxAA_HG zHqMMq*RR6rp{$Clp`zl!OKgS;tbDHR9MCrj7MBmfdkEsv_}BXgR5SLyg1w`va$^>3 z7?|Mmw0vou{A&DV=6du$^p`R^=3iLcw(Q&+XIGwCed?XY_uQp_Kk&DvzcwF%%q_;; z2C9y7#euHj2zA^AKJyUg?4H2?c2H8G;RdZ2AhNyVhj8Jzz5Kio6I*&5PuJTce25u5i31U%ulX- z+1}F)y10$xusb0ITeh+x#7Ss9gy3`B(_~BwMW*uG-P7pQ(9dvgc(yyfrCGs&Z-jJ! zP=e5aTqMew8+VL?n-WcW<)J-2oRUXEZ8|De=zTzg_jrVmmXiLT=vm2zt6rPTB;$6@4ms zSLM>q2hYm0f6od5-Rn1I0(C^7cRZj<>5q)i-8<=x_i>+yJK#c`Y<9$m?0Bb+%es zT_U#>BxjHR&|~ttFej&ipJO6bes?IDpWlf7BbufI%3M(=RQ_L4GJpRO{eO)(K$~k0 zBY+y*D9`f#0Zfkmrb%XENA}|iN735Q6O|mz%1mKIxwn+Z0_(16=+RUi|6V&&nRYxM zeA+qC`;DMi+|Jtaj}mNCbxEm*w0szz83+=zrdu4#sn|OdWa7&2A%56qy!kbd2ME!- z4w*0!&QouPb3l(%Wu|iuPsXQ@RsLU47n~&MBUK7Om)PXRE7*PFCNN(o^TADC%}&2`C
bj{~vylXg>J^9q>jW(W!y9@edF2NWMVBtd=b5%8<1KZqqFUj)tk zQ7X4CO^KugYxA0pXM{)AAbp?wT=EhY6cB&&%?+)3h>|q zjj{TZ4M)Fw4q9&3VSd=gKGoXBOm02F)ed$4LH&v6YR3bW(WH4}2fq|jPJ(tmX?M{R z*%)ipL1)yVi3vma`}$T>M_pj(6aK-Gb@#@h?fL)Z9b+6$)nV1b4R zJz+lR#~=y^enHFC_ZQ=Q&|1O0amG=wuu?k95f@x%*no45_Yjuty#phSf$M6XEL{tX=E5^{MyYDm}GzOfUx)Y`>8M56SE2RHEUuB zp}vC9u*cYX{52nA>%6~z1v?W!Ylxad0AK3-^X)Q=$B(`viuMO-@zj_Gpyf}!r7`oh zMz03LrOsXdZq;zMQpwl5I?cH8xhG@%99BuV^$R^uUnt8H;Uv`AVC-1=* zy%hi{L0{vSy-&_w?4yPk4njW}`U<}+o5Uw8q1;+KJPV*N^UF%$Srh7!A!Z9&qv*?6 zR|edltySg&elCihtB1G(v{ynIC%=rIT*|3_`v~Gy1D-CQYqgyLUupJf94-5hgG_`q znty)?&au#wMLi<18d5qa%b_1nNP=7}+s9B!v^6Hj%vMs*Z5K%jF4E2e@^(%4ic*}#|uW>K? z`4?c{Wao)c(s}m_PoVg+N}R31Sr=^lA7&lUG|W&x@x>94r_GFPqnHe@m9ZtvcNAWx z{!I1(wB-6hOVG}SexPTdW{|(!_d#j~&0IU2E%U+Phed__iasJs%P$-dQ*|<00~&z= z$^pGpyV`udzu2j9OeX9${>8ODpA0SR-g9y+U z|EWhT*M*)Q8v&So48LZGI%kjp>{AM{J&@p@8U#pjWJd`64zWnpd1T`5O_kuS5HScl zj=m4eZJs}kJlbC==Png`%K z%H%OOW3XHzQ}V~!jRWg_U#jcwbF*Dph5TwaoL_*vh~G6rP?_MNTG(^I_rwRem-}|U zdDuJOI$&g-9Tssx{E5>LavXn}<^SKi;`^CGZ>wF|;RY6P8XV~5`YG21Ht1n>^A2mtFR zkl`g2v0I*rvsY~c-|QrHJ2f3fa%X8GmFUIhCq>RDt-G=HG^gVOh|4m$-d@}HLcP_o zm^x95XL1}i&p7J!suiMK zW#0}yhTO$s8Mw&sGI-Mf5{3jM;@^K8$UiknBm!B^!dB~|K25^>?=2smD%V^B9XzSU zU9e1Ygvxo<^ECU=Cz&W9GUMI@tm}{tY9sgRAX|NQ*3?oVFzs{M^? zlvTox@VzopXXY1li21H@BrW1YMO4-Tffc!jM(Z6`$J@4Iv zVF!QyCgIsUZbX&Gi0I^*yJ1#m^5GuXZ$(m*s@LZQ-+HjdiZJsbR z8c@^a7_K0L^$ns!m{Bvs1&Qjt8S594zW)|++&I&OK4kmd<-%g2x9ZJ$rDaxqK18)*ezeeU>4UU)^q2MeImhSB1xT#mt#JkX+6Zc2z&*ars6 z9+l8+^_xd!EronQ^J4Y!M$DB_T+8pwl&bveG`xQuaT#=mZwNbQ@%~Zat(li?>dRP$ zH?osxG1Wq~>szVw`$&?>y<*Tp#JVm6F!bjB+)OJILd>`U<3mc_GHjc?!N`W-OcS^} z;=RLYb69gIp6#Z}GjnuNIuX<~f9N*0AS!!l`f)T&6vYiwkGz9+HA#e?KJ~Je_&q>8HGkQtSZ04(3O5Hl+Qx#^j z>WLv{wyRq%IqYUm1pZjMToD4yru{^N5ixT$B!D{Lz-de*k+RfNZ$%sM7}%%~jgYI52``FMNH@;aDun{=Ee3 zVY$(!fS@;kem|`Bghf3xbRgZbV#r|BIe!sZ@sn~bui9=nX;;=nlJjPWG<6XM;5h~t z&ch8c zcOEW?+pULvMGFn|F7drFFHq-QaM&I76vmXwPpeGzl;srK1WB+qqQ)6#NIB?_d#kP& zD2*E~#Df*7wQ!WIi!_yHexZjMv8XaSBJEsTq;R8BP@DLbE~(7aF$!9? z)y;slH5zMJ3{h#D{wYS%!YP52ed|e$Y6(P1f$wEPA^-ol;G|8kxN!U$UwJr*n{|&? z+Q2G7@*?o}V^xqZKx&@U$nWUz=Jp0(I>;6ec8T2=N!6DUI8Sf6OyflLpfj<6%|CtbD>KeC zS*-B2s9p-$NCt@FMC&g@i6$eRl3{_o@6s*@Efr~{#H_?sBj4Ah7phf(H;Ql3(k3Im zC0`+Lqi1fuct#26j8e|QS6z;gegh{(*l6IRfqZcfW#FJ?jD&+6cNl|!zt+HTf=n0H z2NJiZ117+4d1(9%9Y549ZURmJ2@-r(7}<;v?;c#X>20D?6P>av<^Ix@%UYMse!cv4 z`SR_{sTKF<>IEk4Euf(cOHPWAzSKdcTLWW>RSr76%PI4xM@98h2qEy}AG8sxvO|E~Sz7S~AD<_nId@ zj`fSXUJ_`*e94&zvq0vrC9N!{UbY2%=I2@i2ukhZyJMv?>0Ic;+ey~USPk|oqTxMQ z1I9ZHv3|rjsY-fC!=H=wS4n3id1IM7pZTJ!Ag~cVqpj_k5R1Ng>Wh+#<%m8zuA9TLRbRu+v*ujI}9=T1b@lKV3rn{^hLkk&R(mc#Gh!a z5OKq!S>ZA^W`z8TUo(R&2agcFP_KUJDj&9oM99eW;J}=@IIdB@m}OCg3Z(t+=R|P8 z^Uj=f&%zlGCumic*rZbQ2I*XTO=-lD$6l`kpK3k9xAl*+8*LiC=4BRg6Jm~{!X64v zBsBMkX%4)jDZaRv@ z5nn$?902UtEybFw1j95vAx-1W0g@*$LZ%mh5&i>=ANK$UeU7kzpOQae|N0SOfAlrM z|0#fwy)7jVeJZ4Ha?)w%90F(0&rI@|{2oI@B#b#&t`^%}55T(56K*lh?yj|PGI+{Q z9EMi>z>}N*IFYn=Aat0S#~%J-tbG`u`&i0A%@p`dLywKMGrsUyN5LBP0POkA52$nv z_*c=ep4|OcFCUGh;)@^I@vGGTL~JcHf-SK>A{F&|lNw;Z9Z7Q+PYF(luBm%KL3|;} z?*J`f+FK5L)164s{wCBnZUs(2*}XAm1CIr*Tv+-t(vcwcT(Lb`@x@*a_hWtxXzHk< zt;>@pC8>6Iz{#yFX@|@Sld6|j2h*Na6<`O8NLr;cxM<);Io)6rhc(12x6(3{41djl zWXMt;qQiUM{EBt+uZYTM;y_KtHS5AE3C1uUYno2c&Xyxy4>r z7s-qKh$6A*dGMwOyNu$n_;Hh`E71LqWBBPMf|3}64n?KVR6faxSA=6+qIy{9JauGf;!{GkH3MqEK>KzmPNqJ zx4{{dJpoeiCH`-c&khXWH~PLUu9XV|)A>G&B*R>4-T^km5VRrwarm(ewA%wAu)cKP zB2;1v?7Hi~Cnybfk!A&09gDoB3^7t-e{HDN$I+OypYk;1XEY-O@=Hz=o(GmegpAg$ z3yILovTSD?;k2S-PEVlU1K+rDNm-d_?J^v!8yzlG9PWR-$;nTgK?sk{RpNmoYXV{(PLt z7t^26{&Eo0JoDo0E4ph5uGgNuuR(iHQgAqUPh5(k!A~tH7QLWkf_s#|06rr-_av!z z0;TF8zQV;{Vu?u^d_By2^QzGz&xO4av*QHs+^JDyJHYBQ=A+}Y@0VHP^5czmaXXQ< zln>bfxRid1^51+@hLDv=i#-#gMt`^iK-p&9I{Ag@Hk9I-4xb$VhFfVlY>YQnTFCl{ z*NQx|;!t+m3#Sk4NiFI*067QlPUS=Br*U*qPEY)UC>yRt8O=UPUFJWJ8d)myYnpdR zU%J`g_D4B=_Zq3)v{}P%*$g?Ev5o{+Q6e8O9VGQ~IDZjjbts3jy3F_#fzo=$zewvi zHXq*k|4UjI{P%;juKmB4)}Q`Alh!l8lh!l5(z?SZtqUNnS3PqLrFH9Ol-4)=e@p9C z&-^2;aZFl2)-xup2Yx55(_)m>m;P^L^5MRe#CzOq%|k ztS+4TcRRH)>HYgJ*Z$*@WB#H>uXhjwrzj~VRuGD?dS7hCm}iKbW`1}o%~X)3Ap(c) zt$JgdV+~M|nJrk};=AhE8gbrtbfMrbIC%}{p}dZ9S7JmG%9oAC+e zuu397-iID|sE=@DLj90uDk&H}60PQ*O>mk4z^sj9T#9Fh(xu+a+O%#z_T%N?M9An3 zrKP$?Ec1&b^2~{d*K&HX7O=DZN5na7-*AwfTjW{c@Z}9guPC3#N_49-|8PE;LZ^)^ z4bY{`9tq*EiHV@u8y)bT(Lni*1T4wuW!&vXUx&%Bq7xwo9-hKV|`~>{{hS(tRcr?aa zDlHK847FGwKh&XV_?02_7I=ifIm_N}SEgVTdU@LHv}ggdEN6X|1QAtZEl+(eE7K4L zzM7M}h_NrmAb|U4vGw0Hj5x4`tTG=2n3F``P-Z-Ai8uPV(5hJx=@E6<_TJ6M&PJpj zfeg{Ebgr2u0iHC|3xrN~Ysp_JtyaP7B8Eni_B5b+P3 zJzA)Uliu}l#PYPU+$Q*ZYUJE=Sxc`1264oqYs!owi1$Kzg7w$S6{DtOpHNTm(pO&H zL2nedyJ}2ZlC=nX=*x^I8{!DsoxY?9q~;VcFAU(hDlMgv-GX z9d^9&I<(#2D51eddJxtS#1F0%Ba7iIT&$;R#DS7voIt_OAMxW`>)!It{eO+4doB9k zMbRORd|$d;NHD>ig2=l|asRhp^k$EC{hxm0Yvb6zZG%4W@8IgqezW7>wSpQxALSz5 zB30nbdAs*w60hwf{+7fq>?D>fB){@u6Xg)ZD0r?B)?JCDq6G9**BItEsYnk+a1zgQ zr0gZdwZFvQg#q)FrW{=~#Iin_NfBMd4$TIR|4j#z$Z8^KR<*bz_4yhfB~7Z5*%C>9 z2}=O9Ap^u7O_Ap}4e3i1CDX|2b3dfP@t-0MRyYFY2{5yxVLU;X{>;GO9?kyiGV{U5 z;k8@Ba(eK)y$*zIjJl^gx1~+Fyd!DL=55s~i%UatMV^aklj=OLMgB7aplvcm13l&1 z;&r^f zeZtq3f)5VziUXHT*UF6;(f^_(!L{MpaveXP_Rc7nGm?*M;gmSggbmJHy>L&>?kZB- zzEBEv+~;Pk87|zk5h5TVI}eo53`2%BJex>84>k;o;e}HR^7kmEw zJNm?qUcWngpU-|W!`0>Lp549Y9Xe*x|AC$U2IZ3aUTFAEH^%AsE5Yoe=+#f9>u-sD z)bxLZPw$$lk&@}>&3nD-$r{OMqxUcl;K_d1 z-aY1%{Tt`eoT-hL;o#TGF!#m8G7#dcEG$Oo!w(@#Mss>zA*XAW+%?A3i6!5(}oRugO z^!f#y{yzMV03QM$0iOa^Rw>0Nh#!KqxizQui^7kGUr}|;?k2&2FQT?7K zS#9kQRmr2a`XddEAyEx@#BkV%Yt!VQtS$2d#pTP2TEbzJ#vf85n$#Iq`T9|OP;R8D z1i{Wk5UeDEFdJ#6)EDqRwK+0P5M)Kx+@tx09kSNK*Yi_TrHzDTL8uGMR~m`n^V2pZ zA~!2ifR`1mfDeY0W`q#8AA56OX4Wom<~Ak=o28Lm_6 zt*SHxT19soN;S5mLCa7rGmCl1=t|yBLJ3H&4NBg>{Q5;j5Xwa*#&2%}21HF1gbMu5 z41nmbx2uu13Y~-YmGY3%BsZsM5Z*n8H+snEh1aEsM&tB*|7mQMiziJIgnFqpEUBW_ zrb;R5o36vAsJY!M3o>=R6cwH}CMB6O!g8oaR%l=S*7%sQObtc+q44*bjc<_;pWiP9 zB{kG4X_ESV?jillNi({lAkYpO1GZN&(F4QsCP>r!O2%I9=ws-CDCWhC*6H zt0)H>L!H*zC8CL%?B~->^)!Vbl;L-dnM@Em(1WCguqdm&lRc&Zd)lY-b;j~w`tX=@ zl1Gb8Nk)l!`+RIRGA5f8Q&_@$=1!LoYbSHzg=xo_SW?a8*5os4^l6p78PkVFBT099 zyWG7~4F(o_);Tj}F5u}BrB+m$CHqj5d`h59R017xKx-L&x@{uTX~3#jrg|kz8G?|Y zo~imJ)va&!!64*mH^v~NChfASwTZ#B9X)Q$5(G(ALu!Lx3P0L$9}p}rp-KGmO9W&PqRE& zsGiy4V8I+$n*Lm`8SOGJ(pVuy{Hh$*LTV&o?Ca$-HeMt&r8x~5O_=%GrtRjTKW67o zHz-mML7+`y-orIeLyE!Bo^Bf=lBUT@GwzD|SO)Dh?!)S>`!K642{&PV9E&l+W*Wzg zVH7_`HN%?PfO}7S(2pJUp?-96<)J^P=9qezYcO1nN(6T-4dIT~271eET~mE*Q)6M^ zF@8@`$N1%gwTkFt{W=XGeT zbFPk*OuC{Z;rh!s*CU~)!$@aPluuinL>K@92{3QD+bBDn?^pFer9%mC9 z{Q;?o+N6=e0F^WG)B)GTGJ0GsR^z{2rdQYVt(dw(em>HMnOq7;@HE;!lTk2n6Y6RM>vR^sTh!j$EPP9j;sNf4mjc@Sq`YF z(;!Ql1|n6OAr53lh&S&+$0p)*a8Bg2WEZF30Ne|#2Q~tmfLTBQ*af@{ya~Jyd;**V z2L6rHF914$*+4z84p4V<`b(BR0msyy16c$t#?hAXXA$BeI%p*h zPIthEaM&>0&>g(37lEKl2pL2MIUGD$>2&IifCkh80x$;X}6gschZ( z*bNPSjgYqgaqF1cZJPqys7syt+n2Tdy5r&6_iR^a&AJ|G+Q z`5|y4UR55@lxHG0{d4a5ZKiXy z)RZH&m$|4^@*f+#)$d&(CYe;F&9xC~U4O|)Ik@l*IRBpWI zqE<-nd+UogLf)0XYsY@baicz4np;k+a*_=PZn=6~dpYT@Zf@QAr*bm&*Vi9j@NT(x z_4dw-x1L{7^YNkmhcCLS;_lby=6~|`1`#e{;z2b@c-&j`O`C$b=6@79F{zn{g z3PAO7IQh$8_Vm2yH2zQnjgavvY}CHv7YEip_R9JxV;|o1`enyvPM-Skrxhm#{juc! zwX65Ko?1AvJhUa?&S^Q9@OE(1U@I|s+u+2I)f+6k&j^kb!E`7y0+%Atc%ByayACHD zC%6;N6LdHyP zCR0YITGB_ae;S<2Yl-6qhvP@NoG~U0gKf*&o*vGKHOGkp9N(8@^TPJkhLb(2<#^PM z@fm=btP`F3OMn~4biEpGbm|WQB92GK>GP7}tF3sNzQ7+#PxahDqMnDhw(R^3|E5IU zi+91|<^_Il=Pm}bZoEU#FPM^I5V>NK8RbG>VT;gkSwD1uZtfvldA?>iGPMxkB zDId0exuy5!x4^L=RetM`b|27a`P=&9Bz<9$PS;rUe767mB)u+4ud(#r@}#L>IOx51B?O20AqkL zz!+c*Fa{U{i~+^~V}LQh7+?%A1{ed30mcAhfHA-rU<@z@7z2y}#sFi0F~AsL3@`>5 z1K*4RZ!7*+0C*J1X985;252~@?_5*et2lM~P9)WhcVwa4@2JxFsu13LMI3!clfB;v z5>6xZiLYlJ$T2H=4t~!Eh5}~;EHDNb1B?O20AqkLz!+c*Fa{U{i~+^~V}LQh7+?%A z1{ed30mcAhARPmJe*UayMF$}h`mGxJt&QAt88SO#fHA-rU<@z@7z2y}#sFi0F~AsL z3@`>51B?O20AqkLz!+c*Fb2RtrsvNze#0uf|J;=wrSCmo1`G$VZ2}970mcAhfHA-r zU<@z@7z2y}#sFi0F~AsL3@`>51B?O20AqkLz!*r&fQE0EJOnI)2mOA@ar!Njuv34; z(lb4u*6&?ME~2@6&x%rUrb7$LvJNOl#^2@lRSuvGuAeyd=dE(s`mPM+szRC@0LoEs z`HhG7UhF^Vj*ftxa%}KS?E_&a?kIiEkL^EW_MxzMxuc_CquW>Z`sc&0yQ4JkY+w`4 zbpY_-nA+&~YUdI%*uQ#O&nc~fK^m?OhXI~Rkx^f2(9mW@XdvILS z?jqOTfqkv-=JdmBuqXFgPQQIU_P%}qd#G;W^gA|VAJs=Wef3sOulO^k*J2OnsybiQ z)kB?EJp0_Qsz>~xc)TM}_w=n5 friction_wheel_controller - rmcs_core::controller::shooting::HeroHeatController -> heat_controller - rmcs_core::controller::shooting::PutterController -> bullet_feeder_controller - - rmcs_core::controller::pid::PidController -> first_back_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> first_front_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> second_back_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> second_front_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> third_back_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> third_front_friction_velocity_pid_controller - - rmcs_core::controller::shooting::ShootingRecorder -> shooting_recorder + - rmcs_core::controller::pid::PidController -> first_left_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> first_right_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> second_left_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> second_right_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> third_right_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> third_left_friction_velocity_pid_controller + # - rmcs_core::controller::shooting::ShootingRecorder -> shooting_recorder - rmcs_core::controller::chassis::SteeringWheelStatus -> steering_wheel_status - rmcs_core::controller::chassis::HeroChassisController -> chassis_controller @@ -35,95 +35,44 @@ rmcs_executor: - rmcs_core::controller::chassis::HeroSteeringWheelController -> steering_wheel_controller - rmcs_core::controller::chassis::ChassisClimberController -> climber_controller - - rmcs::AutoAimCapturerComponent -> auto_aim_capturer - - rmcs::AutoAimComponent -> auto_aim_component - - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui + # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer + # - rmcs_auto_aim::AutoAimController -> auto_aim_controller - # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster + # - sp_vision_25::bridge::HeroAutoAimBridge -> hero_auto_aim_bridge + # - rmcs_core::controller::identification::SweptFrequencyController -> pitch_swept_frequency_controller # - rmcs_core::controller::identification::StaticTorqueTestController -> pitch_static_torque_test_controller # - rmcs_core::controller::identification::SweptFrequencyController -> top_yaw_swept_frequency_controller # - rmcs_core::controller::identification::SweptFrequencyController -> bottom_yaw_swept_frequency_controller - # - rmcs_core::controller::identification::SweptFrequencyController -> first_front_friction_swept_frequency_controller - # - rmcs_core::controller::identification::SweptFrequencyController -> second_front_friction_swept_frequency_controller - # - rmcs_core::controller::identification::SweptFrequencyController -> third_front_friction_swept_frequency_controller - # - rmcs_core::controller::identification::SweptFrequencyController -> first_back_friction_swept_frequency_controller - # - rmcs_core::controller::identification::SweptFrequencyController -> second_back_friction_swept_frequency_controller - # - rmcs_core::controller::identification::SweptFrequencyController -> third_back_friction_swept_frequency_controller hero_hardware: ros__parameters: board_serial_top_board: "AF-8AE3-4EC1-03C3-C494-88FE-2DC4-3018-0298" serial_bottom_rmcs_board: "AF-60BB-7484-FA24-F3FC-399B-454D-22FA-1B1D" - bottom_yaw_motor_zero_point: 35110 - pitch_motor_zero_point: 23653 - top_yaw_motor_zero_point: 48525 - viewer_motor_zero_point: 31940 + bottom_yaw_motor_zero_point: 34622 + pitch_motor_zero_point: 23251 + top_yaw_motor_zero_point: 48181 + viewer_motor_zero_point: 31940 external_imu_port: /dev/ttyUSB0 bullet_feeder_motor_zero_point: 60480 #39045 - left_front_zero_point: 5790 - right_front_zero_point: 5114 - left_back_zero_point: 7868 - right_back_zero_point: 1640 - -auto_aim_capturer: - ros__parameters: - camera_name: "" - exposure_us: 2000.0 - gain: 8.0 - framerate: 80.0 - invert_image: false - rls_tau_sec: 10.0 - use_hardware_sync: false - delay_ms: 6.5 - -auto_aim_component: - ros__parameters: - # WARN: 危险！可选 red | blue，生效后裁判系统 ID 缺席时 - # 将强行绑定为对应阵营哨兵，仅供调试，严禁比赛启用。 - # 留空或填 unknow 表示禁用。 - dangerous_fallback: "" - manual_shoot: false - enable_rune: false - camera_translation: [0.25, 0.0, -0.05] - fire_control: - bullet_speed: 11.5 - shoot_delay: 0.1 - offset_yaw: 0.0 - offset_pitch: 0.0 - attack_window: 120.0 - degraded_angle_speed: 12.0 - window_redundancy: 0.8 - window_hysteresis: 0.2 - attack_preaim: false - require_stable_command: false - yaw_tolerance: 0.07 - pitch_tolerance: 0.04 - rune_idle_duration: 0.4 - rune_shoot_duration: 0.2 - -auto_aim_ui: - ros__parameters: - offset_x: 0.0 - offset_y: 0.0 - offset_z: 0.0 + left_front_zero_point: 5826 + right_front_zero_point: 5095 + left_back_zero_point: 7804 + right_back_zero_point: 1750 value_broadcaster: ros__parameters: forward_list: - # - /gimbal/top_yaw/control_angle - # - /chassis/climber/left_front_motor/torque - # - /chassis/climber/right_front_motor/torque - # - /chassis/left_front_steering/torque - # - /chassis/left_back_steering/torque - # - /chassis/right_front_steering/torque - # - /chassis/right_back_steering/torque - - /chassis/left_front_steering/velocity - - /chassis/left_back_steering/velocity - - /chassis/right_front_steering/velocity - - /chassis/right_back_steering/velocity + - /gimbal/top_yaw/control_angle + - /chassis/climber/left_front_motor/torque + - /chassis/climber/right_front_motor/torque + - /chassis/left_front_steering/torque + - /chassis/left_back_steering/torque + - /chassis/right_front_steering/torque + - /chassis/right_back_steering/torque # - /shoot/heat12707 # - /chassis/power # - /referee/chassis/power @@ -134,55 +83,52 @@ value_broadcaster: # - /chassis/climber/front/actual_power_estimate # - /chassis/steering_wheel/actual_power_estimate # - /gimbal/putter/velocity - - /gimbal/first_front_friction/velocity - - /gimbal/first_back_friction/velocity - - /gimbal/second_front_friction/velocity - - /gimbal/second_back_friction/velocity - - /gimbal/third_front_friction/velocity - - /gimbal/third_back_friction/velocity - - /gimbal/first_front_friction/control_torque - - /gimbal/first_back_friction/control_torque - - /gimbal/second_front_friction/control_torque - - /gimbal/second_back_friction/control_torque - - /gimbal/third_front_friction/control_torque - - /gimbal/third_back_friction/control_torque + - /gimbal/first_left_friction/velocity + - /gimbal/first_right_friction/velocity + - /gimbal/second_left_friction/velocity + - /gimbal/second_right_friction/velocity + # - /gimbal/first_left_friction/control_torque + # - /gimbal/first_second_friction/control_torque + # - /gimbal/second_left_friction/control_torque + # - /gimbal/second_right_friction/control_torque # - /gimbal/bottom_yaw/torque # - /gimbal/bottom_yaw/angle # - /gimbal/top_yaw/angle # - /gimbal/pitch/angle # - /chassis/power - # - /chassis/supercap/voltage + - /chassis/supercap/voltage # - /chassis/control_power_limit # - /referee/chassis/power_limit # - /referee/chassis/buffer_energy # - /chassis/climber/front/control_power_limit # - /chassis/climber/front/power_demand_estimate # - /chassis/climber/front/actual_power_estimate - # - /chassis/steering_wheel/actual_power_estimate + # - /chassis/steering_wheel/actual_power_estimate # - /gimbal/bullet_feeder/torque # - /gimbal/bullet_feeder/control_torque - # - /gimbal/putter/angle - # - /gimbal/putter/velocity - # - /gimbal/putter/torque - # - /gimbal/putter/control_torque + - /gimbal/putter/angle + - /gimbal/putter/velocity + - /gimbal/putter/torque + - /gimbal/putter/control_torque # - /gimbal/bullet_feeder/velocity # - /gimbal/bullet_feeder/angle + climber_controller: ros__parameters: - front_climber_velocity: 22.0 + front_climber_velocity: 20.0 back_climber_velocity: 30.0 auto_climb_support_retract_velocity_fast: 70.0 auto_climb_support_retract_velocity_slow: 20.0 - auto_climb_approach_chassis_velocity: 2.0 - auto_climb_support_deploy_chassis_velocity: 0.4 + auto_climb_approach_chassis_velocity: 1.8 + auto_climb_support_deploy_chassis_velocity: 0.3 auto_climb_support_retract_chassis_velocity: 0.15 auto_climb_dash_chassis_velocity: 3.0 first_stair_dash_leveled_pitch_threshold: 0.05 second_stair_dash_leveled_pitch_threshold: -0.09 sync_coefficient: 0.2 first_stair_approach_pitch: 0.517 - second_stair_approach_pitch: 0.37 #0.365 + second_stair_approach_pitch: 0.365 front_kp: 1.0 front_ki: 0.0 front_kd: 0.5 @@ -206,23 +152,23 @@ gimbal_controller: dual_yaw_controller: ros__parameters: - top_yaw_angle_kp: 14.0 # 30.2 + top_yaw_angle_kp: 14.0 #30.2 top_yaw_angle_ki: 0.0 top_yaw_angle_kd: 0.0 - top_yaw_velocity_kp: 9.37 # 11.0 - top_yaw_velocity_ki: 0.00033 # 0.00029 + top_yaw_velocity_kp: 9.37 #11.0 + top_yaw_velocity_ki: 0.00033 #0.00029 top_yaw_velocity_kd: 0.0 top_yaw_velocity_integral_min: -2500.0 top_yaw_velocity_integral_max: 2500.0 - bottom_yaw_angle_kp: 10.0 # 18.4 + bottom_yaw_angle_kp: 10.0 #18.4 bottom_yaw_angle_ki: 0.0 bottom_yaw_angle_kd: 0.0 - bottom_yaw_velocity_kp: 2.00 # 2.81 - bottom_yaw_velocity_ki: 0.000071 # 0.00028 + bottom_yaw_velocity_kp: 2.00 #2.81 + bottom_yaw_velocity_ki: 0.000071 #0.00028 bottom_yaw_velocity_kd: 0.0 bottom_yaw_velocity_integral_min: -2500.0 bottom_yaw_velocity_integral_max: 2500.0 - + pitch_angle_pid_controller: ros__parameters: measurement: /gimbal/pitch/control_angle_error @@ -244,8 +190,8 @@ pitch_velocity_pid_controller: gimbal_player_viewer_controller: ros__parameters: - upper_limit: 0.415996 - lower_limit: -0.066441 + upper_limit: 0.415996 + lower_limit: -0.066441 viewer_angle_pid_controller: ros__parameters: @@ -258,7 +204,7 @@ viewer_angle_pid_controller: bullet_feeder_controller: ros__parameters: bullet_feeder_velocity_kp: 5.0 - bullet_feeder_velocity_ki: 0.1 + bullet_feeder_velocity_ki: 0.1 #1.1 bullet_feeder_velocity_kd: 0.0 bullet_feeder_velocity_integral_min: 0.0 bullet_feeder_velocity_integral_max: 60.0 @@ -275,32 +221,26 @@ bullet_feeder_controller: friction_wheel_controller: ros__parameters: friction_wheels: - - /gimbal/first_front_friction - - /gimbal/second_front_friction - - /gimbal/third_front_friction - - /gimbal/first_back_friction - - /gimbal/second_back_friction - - /gimbal/third_back_friction + - /gimbal/first_left_friction + - /gimbal/second_left_friction + - /gimbal/third_left_friction + - /gimbal/first_right_friction + - /gimbal/second_right_friction + - /gimbal/third_right_friction friction_velocities_profile_0: - - 390.0 - - 390.0 - - 390.0 - - 480.0 - - 480.0 - - 480.0 - # - 527.0 - # - 527.0 - # - 527.0 - # - 435.0 - # - 435.0 - # - 435.0 + - 360.0 + - 360.0 + - 360.0 + - 523.0 + - 523.0 + - 523.0 friction_velocities_profile_1: - - 527.0 - - 527.0 - - 527.0 - - 435.0 - - 435.0 - - 435.0 + - 360.0 + - 360.0 + - 360.0 + - 523.0 + - 523.0 + - 523.0 friction_soft_start_stop_time: 1.0 heat_controller: @@ -311,63 +251,63 @@ heat_controller: shooting_recorder: ros__parameters: friction_wheel_count: 6 - aim_velocity: 16.15 + aim_velocity: 11.8 log_mode: 1 # 1: trigger, 2: timing -first_front_friction_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/first_front_friction/velocity - setpoint: /gimbal/first_front_friction/control_velocity - control: /gimbal/first_front_friction/control_torque - kp: 0.006233371 - ki: 0.00 #0.00003 - kd: 0.000001 - -second_front_friction_velocity_pid_controller: +first_left_friction_velocity_pid_controller: ros__parameters: - measurement: /gimbal/second_front_friction/velocity - setpoint: /gimbal/second_front_friction/control_velocity - control: /gimbal/second_front_friction/control_torque - kp: 0.006035661 - ki: 0.00 #0.00003 - kd: 0.000001 - -third_front_friction_velocity_pid_controller: + measurement: /gimbal/first_left_friction/velocity + setpoint: /gimbal/first_left_friction/control_velocity + control: /gimbal/first_left_friction/control_torque + kp: 0.006 + ki: 0.00 + kd: 0.00008 + +first_right_friction_velocity_pid_controller: ros__parameters: - measurement: /gimbal/third_front_friction/velocity - setpoint: /gimbal/third_front_friction/control_velocity - control: /gimbal/third_front_friction/control_torque - kp: 0.006192421 - ki: 0.00 #0.00003 - kd: 0.000001 - -first_back_friction_velocity_pid_controller: + measurement: /gimbal/first_right_friction/velocity + setpoint: /gimbal/first_right_friction/control_velocity + control: /gimbal/first_right_friction/control_torque + kp: 0.006 + ki: 0.00 + kd: 0.00008 + +second_left_friction_velocity_pid_controller: ros__parameters: - measurement: /gimbal/first_back_friction/velocity - setpoint: /gimbal/first_back_friction/control_velocity - control: /gimbal/first_back_friction/control_torque - kp: 0.007749503 + measurement: /gimbal/second_left_friction/velocity + setpoint: /gimbal/second_left_friction/control_velocity + control: /gimbal/second_left_friction/control_torque + kp: 0.006 ki: 0.00 - kd: 0.00003 + kd: 0.00008 -second_back_friction_velocity_pid_controller: +second_right_friction_velocity_pid_controller: ros__parameters: - measurement: /gimbal/second_back_friction/velocity - setpoint: /gimbal/second_back_friction/control_velocity - control: /gimbal/second_back_friction/control_torque - kp: 0.007795934 + measurement: /gimbal/second_right_friction/velocity + setpoint: /gimbal/second_right_friction/control_velocity + control: /gimbal/second_right_friction/control_torque + kp: 0.006 ki: 0.00 - kd: 0.00003 + kd: 0.00008 -third_back_friction_velocity_pid_controller: +third_left_friction_velocity_pid_controller: ros__parameters: - measurement: /gimbal/third_back_friction/velocity - setpoint: /gimbal/third_back_friction/control_velocity - control: /gimbal/third_back_friction/control_torque - kp: 0.007760993 + measurement: /gimbal/third_left_friction/velocity + setpoint: /gimbal/third_left_friction/control_velocity + control: /gimbal/third_left_friction/control_torque + kp: 0.006 ki: 0.00 - kd: 0.00003 + kd: 0.00008 +third_right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/third_right_friction/velocity + setpoint: /gimbal/third_right_friction/control_velocity + control: /gimbal/third_right_friction/control_torque + kp: 0.006 + ki: 0.00 + kd: 0.00016 + steering_wheel_status: ros__parameters: vehicle_radius: 0.286378 @@ -384,6 +324,52 @@ steering_wheel_controller: k2: 3.082190e-03 no_load_power: 11.37 +auto_aim_controller: + ros__parameters: + # capture + use_video: false # If true, use video stream instead of camera. + video_path: "/workspaces/RMCS/rmcs_ws/resources/1.avi" + exposure_time: 1 + invert_image: false + # identifier + armor_model_path: "/models/mlp.onnx" + # pnp + fx: 1.722231837421459e+03 + fy: 1.724876404292754e+03 + cx: 7.013056440882832e+02 + cy: 5.645821718351237e+02 + k1: -0.064232403853946 + k2: -0.087667493884102 + k3: 0.792381808294582 + # tracker + armor_predict_duration: 500 + # controller + gimbal_predict_duration: 100 + yaw_error: 0. + pitch_error: 0. + shoot_velocity: 28.0 + predict_sec: 0.095 + # etc + buff_predict_duration: 200 + buff_model_path: "/models/buff_nocolor_v6.onnx" + omni_exposure: 1000.0 + record_fps: 120 + debug: false # Setup in actual using.Debug mode is used when referee is not ready + debug_color: 0 # 0 For blue while 1 for red. mine + debug_robot_id: 4 + debug_buff_mode: false + record: false + raw_img_pub: false # Set false in actual use + image_viewer_type: 2 + +hero_auto_aim_bridge: + ros__parameters: + config_file: "configs/standard3.yaml" + bullet_speed_fallback: 11.4 + result_timeout: 0.1 # 0.08 + debug: false + + pitch_swept_frequency_controller: ros__parameters: target: /gimbal/pitch @@ -393,7 +379,7 @@ pitch_swept_frequency_controller: start_freq: 0.1 end_freq: 10.0 duration: 60.0 - amplitude: 6.8 + amplitude: 10.0 pid: true setpoint: 0.0 @@ -409,9 +395,9 @@ pitch_static_torque_test_controller: ros__parameters: target: /gimbal/pitch - interval_angle: 0.02 + interval_angle: 0.05 wait_time: 1.5 - border_clip: 0.02 + border_clip: 0.05 position_kp: 12.0 position_ki: 0.0 @@ -455,75 +441,3 @@ bottom_yaw_swept_frequency_controller: end_freq: 4.0 duration: 80.0 amplitude: 1.0 - -first_front_friction_swept_frequency_controller: - ros__parameters: - target: /gimbal/first_front_friction - sweep: true - pid: false - logarithmic: true - start_freq: 1.0 - end_freq: 45.0 - duration: 60.0 - amplitude: 0.08 - dc_offset: 0.01 - -second_front_friction_swept_frequency_controller: - ros__parameters: - target: /gimbal/second_front_friction - sweep: true - pid: false - logarithmic: true - start_freq: 1.0 - end_freq: 45.0 - duration: 60.0 - amplitude: 0.08 - dc_offset: 0.04 - -third_front_friction_swept_frequency_controller: - ros__parameters: - target: /gimbal/third_front_friction - sweep: true - pid: false - logarithmic: true - start_freq: 1.0 - end_freq: 45.0 - duration: 60.0 - amplitude: 0.08 - dc_offset: 0.02 - -first_back_friction_swept_frequency_controller: - ros__parameters: - target: /gimbal/first_back_friction - sweep: true - pid: false - logarithmic: true - start_freq: 1.0 - end_freq: 45.0 - duration: 60.0 - amplitude: 0.08 - dc_offset: 0.021 - -second_back_friction_swept_frequency_controller: - ros__parameters: - target: /gimbal/second_back_friction - sweep: true - pid: false - logarithmic: true - start_freq: 1.0 - end_freq: 45.0 - duration: 60.0 - amplitude: 0.08 - dc_offset: 0.029 - -third_back_friction_swept_frequency_controller: - ros__parameters: - target: /gimbal/third_back_friction - sweep: true - pid: false - logarithmic: true - start_freq: 1.0 - end_freq: 45.0 - duration: 40.0 - amplitude: 0.08 - dc_offset: 0.00 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp index 840c23c2f..39f5d833a 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp @@ -297,7 +297,9 @@ class ChassisClimberController AutoClimbControl update_manual_support_control(const rmcs_msgs::Keyboard& keyboard) { AutoClimbControl control; - if (keyboard.b) { + if (keyboard.b || *rotary_knob_switch_ == rmcs_msgs::Switch::UP) { + manual_support_retracting_ = false; + manual_support_retract_block_count_ = 0; back_climber_zero_velocity_hold_ = false; control.back_climber_velocity = climber_back_control_velocity_abs_; return control; @@ -714,8 +716,8 @@ class ChassisClimberController std::shared_ptr front_power_limiter_; - double back_climber_retract_first_torque_ = 8.0; - double back_climber_retract_second_torque_ = 0.5; + double back_climber_retract_first_torque_ = 10.0; + double back_climber_retract_second_torque_ = 1.0; int back_climber_recover_count = 0; }; } // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp index 6dcffe1c8..a4afd5640 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp @@ -54,6 +54,7 @@ class HeroGimbalController const auto& switch_left = *switch_left_; const auto& switch_right = *switch_right_; + // RCLCPP_INFO(get_logger(), "pitch %f", *gimbal_pitch_angle_); do { using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) @@ -175,8 +176,8 @@ class HeroGimbalController private: static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - static constexpr double kEInitPitch = -0.346584; // Initial angle for standalone E. - static constexpr double kCtrlEInitPitch = -0.471795; // Initial angle for Ctrl+E. + static constexpr double kEInitPitch = -0.20; // Initial angle for standalone E. + static constexpr double kCtrlEInitPitch = -0.20; // Initial angle for Ctrl+E. double encoder_init_pitch_ = kEInitPitch; InputInterface joystick_left_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp index 2bd06a780..c6901fbcb 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp @@ -28,21 +28,11 @@ class HeatController void update() override { shooter_heat_ = std::max(0, shooter_heat_ - *shooter_cooling_); - if (*bullet_fired_ && !bullet_fired_false_) { - shooter_heat_ += heat_per_shot; - } - bullet_fired_false_ = *bullet_fired_; - - if (++cooling_settlement_tick_ >= kCoolingSettlementTicks) { - cooling_settlement_tick_ = 0; - shooter_heat_ = std::max( - 0, shooter_heat_ - *shooter_cooling_ * kCoolingPerSettlementScale); - } + if (*bullet_fired_) + shooter_heat_ += heat_per_shot + 10; *control_bullet_allowance_ = std::max( 0, (*shooter_heat_limit_ - shooter_heat_ - reserved_heat) / heat_per_shot); - - *shooting_heat_ = static_cast(shooter_heat_); } private: @@ -54,12 +44,7 @@ class HeatController const int64_t heat_per_shot; const int64_t reserved_heat; - int cooling_settlement_tick_ = 0; - static constexpr int kCoolingSettlementTicks = 100; - static constexpr int kCoolingPerSettlementScale = 100; - bool bullet_fired_false_ = false; int64_t shooter_heat_ = 0; - OutputInterface shooting_heat_; OutputInterface control_bullet_allowance_; }; diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp index 5874a49a0..c565c1757 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp @@ -186,12 +186,10 @@ class PutterController shooted = true; } - if (*putter_angle_ - putter_startpoint >= putter_stroke_ && !shooted) { - RCLCPP_INFO(get_logger(), "DETECT: Putter stroke completed!"); - shooted = true; - } - - update_putter_jam_detection(); + // if (*putter_angle_ - putter_startpoint >= putter_stroke_ && !shooted) { + // RCLCPP_INFO(get_logger(), "DETECT: Putter stroke completed!"); + // shooted = true; + // } if (shooted) { // Bullet fired: return the putter. @@ -208,7 +206,9 @@ class PutterController } else { // Bullet not fired yet: continue advancing. *putter_control_torque_ = - putter_return_velocity_pid_.update(60. - *putter_velocity_); + putter_return_velocity_pid_.update(80. - *putter_velocity_); + + update_putter_jam_detection(); } } } else { @@ -286,22 +286,19 @@ class PutterController // If the photoelectric sensor was not triggered, treat it as a simple jam, // reverse briefly, then continue until stall. locked_detect_count_ = 0; - enter_jam_protection(); + enter_reverse_protection(); } } void update_putter_jam_detection() { - if ((*putter_control_torque_ > -0.03 && shoot_stage_ == ShootStage::PRELOADING) - || (*putter_control_torque_ < 0.05 && shoot_stage_ == ShootStage::SHOOTING) - || std::isnan(*putter_control_torque_)) { + if (std::abs(*putter_velocity_) > 0.1 || std::isnan(*putter_control_torque_)) { putter_faulty_count_ = 0; - return; + } else { + putter_faulty_count_++; } // Accumulate a fault count when the torque is abnormal. - if (putter_faulty_count_ < 500) - ++putter_faulty_count_; - else { + if (putter_faulty_count_ >= 50) { putter_faulty_count_ = 0; if (shoot_stage_ != ShootStage::SHOOTING) { // Stall detected outside the firing state: the putter is in position, @@ -310,7 +307,7 @@ class PutterController putter_startpoint = *putter_angle_; } else { // Stall detected during firing: treat the bullet as fired. - RCLCPP_INFO(get_logger(), "DETECT: Putter jammed"); + RCLCPP_INFO(get_logger(), "DETECT: Putter freezed"); shooted = true; } } @@ -333,12 +330,11 @@ class PutterController } } - void enter_jam_protection() { + void enter_reverse_protection() { locked_detect_count_ = 0; bullet_feeder_faulty_count_ = 0; bullet_feeder_reverse_end_ = 400; bullet_feeder_velocity_pid_.reset(); - // RCLCPP_INFO(get_logger(), "Jammed!"); } static constexpr double nan_ = diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six--friction.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six--friction.cpp new file mode 100644 index 000000000..9239a858c --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six--friction.cpp @@ -0,0 +1,874 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hardware/device/bmi088.hpp" +#include "hardware/device/can_packet.hpp" +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/lk_motor.hpp" +#include "hardware/device/supercap.hpp" + +namespace rmcs_core::hardware { + +class CanReceiveRateCounter { +public: + explicit CanReceiveRateCounter(rclcpp::Logger logger, std::string_view channel_name) + : logger_(std::move(logger)) + , channel_name_(channel_name) {} + + void record(std::uint32_t can_id) { + const auto now = Clock::now(); + + std::lock_guard lock{mutex_}; + auto& status = statuses_[can_id]; + ++status.receive_count; + status.last_receive_time = now; + + report_if_due(now); + } + + void report_if_due() { + const auto now = Clock::now(); + + std::lock_guard lock{mutex_}; + report_if_due(now); + } + +private: + using Clock = std::chrono::steady_clock; + + struct Status { + std::size_t receive_count{0}; + Clock::time_point last_receive_time{}; + }; + + void report_if_due(Clock::time_point now) { + if (statuses_.empty()) + return; + + if (last_report_time_ == Clock::time_point{}) { + last_report_time_ = now; + return; + } + + const auto elapsed = now - last_report_time_; + if (elapsed < kReportInterval) + return; + + const auto elapsed_seconds = std::chrono::duration(elapsed).count(); + for (auto& [can_id, status] : statuses_) { + const bool attached = now - status.last_receive_time <= kMissTimeout; + RCLCPP_INFO( + logger_, "[can rx] %.*s id=0x%03X rate=%.1fHz status=%s", + static_cast(channel_name_.size()), channel_name_.data(), + static_cast(can_id), + static_cast(status.receive_count) / elapsed_seconds, + attached ? "attach" : "miss"); + status.receive_count = 0; + } + + last_report_time_ = now; + } + + static constexpr std::chrono::milliseconds kReportInterval{1000}; + static constexpr std::chrono::milliseconds kMissTimeout{1000}; + + rclcpp::Logger logger_; + std::string_view channel_name_; + std::mutex mutex_; + Clock::time_point last_report_time_{}; + std::map statuses_; +}; + +class SteeringHeroLittle + : public rmcs_executor::Component + , public rclcpp::Node { +public: + SteeringHeroLittle() + : Node( + get_component_name(), + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) + , command_component_( + create_partner_component( + get_component_name() + "_command", *this)) { + + register_output("/tf", tf_); + + gimbal_calibrate_subscription_ = create_subscription( + "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { + gimbal_calibrate_subscription_callback(std::move(msg)); + }); + + top_board_ = std::make_unique( + *this, *command_component_, get_parameter("board_serial_top_board").as_string()); + + bottom_board_ = std::make_unique( + *this, *command_component_, get_parameter("serial_bottom_rmcs_board").as_string()); + + tf_->set_transform( + Eigen::Translation3d{0.06603, 0.0, 0.082}); + } + + SteeringHeroLittle(const SteeringHeroLittle&) = delete; + SteeringHeroLittle& operator=(const SteeringHeroLittle&) = delete; + SteeringHeroLittle(SteeringHeroLittle&&) = delete; + SteeringHeroLittle& operator=(SteeringHeroLittle&&) = delete; + + ~SteeringHeroLittle() override = default; + + void update() override { + top_board_->update(); + bottom_board_->update(); + + tf_->set_state( + bottom_board_->gimbal_bottom_yaw_motor_.angle() + + top_board_->gimbal_top_yaw_motor_.angle()); + tf_->set_state( + top_board_->gimbal_pitch_motor_.angle()); + } + + void command_update() { + top_board_->command_update(); + bottom_board_->command_update(); + } + +private: + void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New bottom yaw offset: %ld", + bottom_board_->gimbal_bottom_yaw_motor_.calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New pitch offset: %ld", + top_board_->gimbal_pitch_motor_.calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New player viewer offset: %ld", + top_board_->gimbal_player_viewer_motor_.calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New top yaw offset: %ld", + top_board_->gimbal_top_yaw_motor_.calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New bullet feeder offset: %ld", + top_board_->gimbal_bullet_feeder_.calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] left front steering offset: %d", + bottom_board_->chassis_steering_motors_[0].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] right front steering offset: %d", + bottom_board_->chassis_steering_motors_[1].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] left back steering offset: %d", + bottom_board_->chassis_steering_motors_[2].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] right back steering offset: %d", + bottom_board_->chassis_steering_motors_[3].calibrate_zero_point()); + } + + class SteeringHeroLittleCommand : public rmcs_executor::Component { + public: + explicit SteeringHeroLittleCommand(SteeringHeroLittle& hero) + : hero_(hero) {} + + void update() override { hero_.command_update(); } + + SteeringHeroLittle& hero_; + }; + std::shared_ptr command_component_; + + class TopBoard final : private librmcs::agent::RmcsBoardLite { + public: + friend class SteeringHeroLittle; + explicit TopBoard( + SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, + std::string_view board_serial = {}) + : librmcs::agent::RmcsBoardLite(board_serial) + , logger_(steering_hero.get_logger()) + // , can0_receive_rate_counter_(logger_, "bottom/can0") + // , can1_receive_rate_counter_(logger_, "bottom/can1") + // , can2_receive_rate_counter_(logger_, "bottom/can2") + // , can3_receive_rate_counter_(logger_, "bottom/can3") + , tf_(steering_hero.tf_) + , imu_(1000, 0.2, 0.0) + , gimbal_top_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/top_yaw") + , gimbal_pitch_motor_(steering_hero, steering_hero_command, "/gimbal/pitch") + , gimbal_friction_wheels_( + {steering_hero, steering_hero_command, "/gimbal/first_right_friction"}, + {steering_hero, steering_hero_command, "/gimbal/first_left_friction"}, + {steering_hero, steering_hero_command, "/gimbal/second_right_friction"}, + {steering_hero, steering_hero_command, "/gimbal/second_left_friction"}, + {steering_hero, steering_hero_command, "/gimbal/third_right_friction"}, + {steering_hero, steering_hero_command, "/gimbal/third_left_friction"}) + , gimbal_bullet_feeder_(steering_hero, steering_hero_command, "/gimbal/bullet_feeder") + , putter_motor_(steering_hero, steering_hero_command, "/gimbal/putter") + , gimbal_scope_motor_(steering_hero, steering_hero_command, "/gimbal/scope") + , gimbal_player_viewer_motor_( + steering_hero, steering_hero_command, "/gimbal/player_viewer") { + + gimbal_top_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} + .enable_multi_turn_angle() + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("top_yaw_motor_zero_point").as_int()))); + gimbal_pitch_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("pitch_motor_zero_point").as_int())) + .enable_multi_turn_angle()); + gimbal_friction_wheels_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(1.)); + gimbal_friction_wheels_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(1.)); + gimbal_friction_wheels_[2].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + gimbal_friction_wheels_[3].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + gimbal_friction_wheels_[4].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(1.)); + gimbal_friction_wheels_[5].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(1.)); + gimbal_bullet_feeder_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("bullet_feeder_motor_zero_point").as_int())) + .set_reversed() + .enable_multi_turn_angle()); + putter_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reduction_ratio(1.) + .enable_multi_turn_angle()); + gimbal_scope_motor_.configure(device::DjiMotor::Config{device::DjiMotor::Type::kM2006}); + gimbal_player_viewer_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("viewer_motor_zero_point").as_int())) + .set_reversed() + .enable_multi_turn_angle()); + + steering_hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); + steering_hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + + steering_hero.register_output( + "/gimbal/photoelectric_sensor", photoelectric_sensor_status_, false); + steering_hero.register_output( + "/gimbal/grayscale_sensor", grayscale_sensor_status_, false); + steering_hero.register_output( + "/auto_aim/image_capturer/timestamp", camera_capturer_trigger_timestamp_, 0); + steering_hero.register_output( + "/auto_aim/image_capturer/trigger", camera_capturer_trigger_, 0); + + imu_.set_coordinate_mapping([](double x, double y, double z) { + // Get the mapping with the following code. + // The rotation angle must be an exact multiple of 90 degrees, otherwise + // use a matrix. + + return std::make_tuple(y, -x, z); + }); + } + + TopBoard(const TopBoard&) = delete; + TopBoard& operator=(const TopBoard&) = delete; + TopBoard(TopBoard&&) = delete; + TopBoard& operator=(TopBoard&&) = delete; + + ~TopBoard() final = default; + + void update() { + // can0_receive_rate_counter_.report_if_due(); + // can1_receive_rate_counter_.report_if_due(); + // can2_receive_rate_counter_.report_if_due(); + // can3_receive_rate_counter_.report_if_due(); + + imu_.update_status(); + Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; + + tf_->set_transform( + gimbal_imu_pose.conjugate()); + + *gimbal_yaw_velocity_imu_ = imu_.gz(); + *gimbal_pitch_velocity_imu_ = imu_.gy(); + + gimbal_top_yaw_motor_.update_status(); + gimbal_pitch_motor_.update_status(); + tf_->set_state( + gimbal_pitch_motor_.angle()); + + for (auto& motor : gimbal_friction_wheels_) + motor.update_status(); + + gimbal_bullet_feeder_.update_status(); + putter_motor_.update_status(); + + gimbal_player_viewer_motor_.update_status(); + tf_->set_state( + gimbal_player_viewer_motor_.angle()); + + gimbal_scope_motor_.update_status(); + + if (last_camera_capturer_trigger_timestamp_ != *camera_capturer_trigger_timestamp_) + *camera_capturer_trigger_ = true; + last_camera_capturer_trigger_timestamp_ = *camera_capturer_trigger_timestamp_; + + *photoelectric_sensor_status_ = photoelectric_sensor_status_atomic.load(); + *grayscale_sensor_status_ = grayscale_sensor_status_atomic.load(); + } + + void command_update() { + auto builder = start_transmit(); + + if (std::isfinite(gimbal_pitch_motor_.control_angle())) + builder.can0_transmit({ + .can_id = 0x143, + .can_data = gimbal_pitch_motor_ + .generate_angle_command(gimbal_pitch_motor_.control_angle()) + .as_bytes(), + }); + else + builder.can0_transmit({ + .can_id = 0x143, + .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), + }); // Used to distinguish pitch encoder control from IMU control. + + builder.can0_transmit({ + .can_id = 0x141, + .can_data = gimbal_top_yaw_motor_.generate_command().as_bytes(), + }); + + builder.can0_transmit({ + .can_id = 0x142, + .can_data = gimbal_bullet_feeder_.generate_torque_command().as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_friction_wheels_[0].generate_command(), + gimbal_friction_wheels_[1].generate_command(), + gimbal_friction_wheels_[2].generate_command(), + gimbal_friction_wheels_[3].generate_command(), + } + .as_bytes(), + }); + + builder.can2_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_friction_wheels_[4].generate_command(), + gimbal_friction_wheels_[5].generate_command(), + putter_motor_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + builder.gpio_digital_read( + librmcs::spec::rmcs_board_lite::kGpioDescriptors[2], + { + .period_ms = 20, + .pull = librmcs::data::GpioPull::kUp, + }); + } + + private: + void can0_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + // can0_receive_rate_counter_.record(can_id); + if (can_id == 0x141) { + gimbal_top_yaw_motor_.store_status(data.can_data); + } else if (can_id == 0x143) { + gimbal_pitch_motor_.store_status(data.can_data); + } else if (can_id == 0x142) { + gimbal_bullet_feeder_.store_status(data.can_data); + } + } + + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + // can1_receive_rate_counter_.record(can_id); + if (can_id == 0x201) { + gimbal_friction_wheels_[0].store_status(data.can_data); + } else if (can_id == 0x202) { + gimbal_friction_wheels_[1].store_status(data.can_data); + } else if (can_id == 0x203) { + gimbal_friction_wheels_[2].store_status(data.can_data); + } else if (can_id == 0x204) { + gimbal_friction_wheels_[3].store_status(data.can_data); + } + } + + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + // can2_receive_rate_counter_.record(can_id); + if (can_id == 0x203) { + putter_motor_.store_status(data.can_data); + } else if (can_id == 0x201) { + gimbal_friction_wheels_[4].store_status(data.can_data); + } else if (can_id == 0x202) { + gimbal_friction_wheels_[5].store_status(data.can_data); + } + } + + void gpio_digital_read_result_callback( + const librmcs::spec::rmcs_board_lite::GpioDescriptor& gpio, + const librmcs::data::GpioDigitalDataView& data) override { + if (gpio.channel_index == 2) { + photoelectric_sensor_status_atomic.store(data.high); + } + } + + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + imu_.store_accelerometer_status(data.x, data.y, data.z); + } + + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); + } + + rclcpp::Logger logger_; + // CanReceiveRateCounter can0_receive_rate_counter_; + // CanReceiveRateCounter can1_receive_rate_counter_; + // CanReceiveRateCounter can2_receive_rate_counter_; + // CanReceiveRateCounter can3_receive_rate_counter_; + OutputInterface& tf_; + + std::time_t last_camera_capturer_trigger_timestamp_{0}; + + device::Bmi088 imu_; + device::LkMotor gimbal_top_yaw_motor_; + device::LkMotor gimbal_pitch_motor_; + device::DjiMotor gimbal_friction_wheels_[6]; + device::LkMotor gimbal_bullet_feeder_; + device::DjiMotor putter_motor_; + device::DjiMotor gimbal_scope_motor_; + device::LkMotor gimbal_player_viewer_motor_; + + OutputInterface gimbal_yaw_velocity_imu_; + OutputInterface gimbal_pitch_velocity_imu_; + OutputInterface photoelectric_sensor_status_; + OutputInterface grayscale_sensor_status_; + OutputInterface camera_capturer_trigger_; + OutputInterface camera_capturer_trigger_timestamp_; + std::atomic photoelectric_sensor_status_atomic{false}; + std::atomic grayscale_sensor_status_atomic{false}; + }; + + class BottomBoard final : private librmcs::agent::RmcsBoardLite { + public: + friend class SteeringHeroLittle; + explicit BottomBoard( + SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, + std::string_view board_serial = {}) + : librmcs::agent::RmcsBoardLite( + board_serial, {.dangerously_skip_version_checks = false}) + , logger_(steering_hero.get_logger()) + // , can0_receive_rate_counter_(logger_, "bottom/can0") + // , can1_receive_rate_counter_(logger_, "bottom/can1") + // , can2_receive_rate_counter_(logger_, "bottom/can2") + // , can3_receive_rate_counter_(logger_, "bottom/can3") + , imu_(1000, 0.2, 0.0) + , dr16_(steering_hero) + , supercap_(steering_hero, steering_hero_command) + , chassis_steering_motors_( + {steering_hero, steering_hero_command, "/chassis/left_front_steering"}, + {steering_hero, steering_hero_command, "/chassis/right_front_steering"}, + {steering_hero, steering_hero_command, "/chassis/left_back_steering"}, + {steering_hero, steering_hero_command, "/chassis/right_back_steering"}) + , chassis_wheel_motors_( + {steering_hero, steering_hero_command, "/chassis/left_front_wheel"}, + {steering_hero, steering_hero_command, "/chassis/right_front_wheel"}, + {steering_hero, steering_hero_command, "/chassis/left_back_wheel"}, + {steering_hero, steering_hero_command, "/chassis/right_back_wheel"}) + , chassis_front_climber_motor_( + {steering_hero, steering_hero_command, "/chassis/climber/left_front_motor"}, + {steering_hero, steering_hero_command, "/chassis/climber/right_front_motor"}) + , chassis_back_climber_motor_( + {steering_hero, steering_hero_command, "/chassis/climber/left_back_motor"}, + {steering_hero, steering_hero_command, "/chassis/climber/right_back_motor"}) + , yaw_brake_motor_(steering_hero, steering_hero_command, "/gimbal/yaw_brake") + , gimbal_bottom_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/bottom_yaw") { + // + chassis_steering_motors_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("left_front_zero_point").as_int())) + .set_reversed()); + chassis_steering_motors_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("right_front_zero_point").as_int())) + .set_reversed()); + chassis_steering_motors_[2].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("left_back_zero_point").as_int())) + .set_reversed()); + chassis_steering_motors_[3].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("right_back_zero_point").as_int())) + .set_reversed()); + + chassis_wheel_motors_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_wheel_motors_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_wheel_motors_[2].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_wheel_motors_[3].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_front_climber_motor_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(19.)); + chassis_front_climber_motor_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + chassis_back_climber_motor_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .enable_multi_turn_angle() + .set_reduction_ratio(19.)); + chassis_back_climber_motor_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .enable_multi_turn_angle() + .set_reduction_ratio(19.)); + + yaw_brake_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.set_reduction_ratio(1.)); + gimbal_bottom_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} + .set_reversed() + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("bottom_yaw_motor_zero_point").as_int()))); + + steering_hero.register_output("/referee/serial", referee_serial_); + referee_serial_->read = [this](std::byte* buffer, size_t size) { + return referee_ring_buffer_receive_.pop_front_n( + + [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) { + start_transmit().uart0_transmit( + {.uart_data = std::span{buffer, size}}); + return size; + }; + steering_hero.register_output( + "/chassis/powermeter/control_enable", powermeter_control_enabled_, false); + steering_hero.register_output( + "/chassis/powermeter/charge_power_limit", powermeter_charge_power_limit_, 0.); + + steering_hero.register_output( + "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); + steering_hero.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); + } + + BottomBoard(const BottomBoard&) = delete; + BottomBoard& operator=(const BottomBoard&) = delete; + BottomBoard(BottomBoard&&) = delete; + BottomBoard& operator=(BottomBoard&&) = delete; + + ~BottomBoard() final = default; + + void update() { + // can0_receive_rate_counter_.report_if_due(); + // can1_receive_rate_counter_.report_if_due(); + // can2_receive_rate_counter_.report_if_due(); + // can3_receive_rate_counter_.report_if_due(); + + imu_.update_status(); + dr16_.update_status(); + supercap_.update_status(); + + *chassis_yaw_velocity_imu_ = imu_.gz(); + *chassis_pitch_imu_ = -std::asin(2.0 * (imu_.q0() * imu_.q2() - imu_.q3() * imu_.q1())); + + chassis_front_climber_motor_[0].update_status(); + chassis_front_climber_motor_[1].update_status(); + chassis_back_climber_motor_[0].update_status(); + chassis_back_climber_motor_[1].update_status(); + + for (auto& motor : chassis_wheel_motors_) + motor.update_status(); + for (auto& motor : chassis_steering_motors_) + motor.update_status(); + + yaw_brake_motor_.update_status(); + gimbal_bottom_yaw_motor_.update_status(); + + if (++count_ == 500) { + for (int i = 0; i < 8; ++i) { + if (check[i] == 0) { + RCLCPP_WARN(logger_, "can id 0x%03X missing", i + 0x201); + } + } + std::fill_n(check, 8, 0); + count_ = 0; + } + } + + void command_update() { + auto builder = start_transmit(); + + builder.can0_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[0].generate_command(), + chassis_wheel_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + builder.can0_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + chassis_steering_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + chassis_steering_motors_[0].generate_command(), + } + .as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + chassis_wheel_motors_[2].generate_command(), + chassis_wheel_motors_[3].generate_command(), + } + .as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + chassis_steering_motors_[3].generate_command(), + chassis_steering_motors_[2].generate_command(), + supercap_.generate_command(), + } + .as_bytes(), + }); + + builder.can3_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + chassis_back_climber_motor_[1].generate_command(), + yaw_brake_motor_.generate_command(), + chassis_back_climber_motor_[0].generate_command(), + } + .as_bytes(), + }); + + builder.can3_transmit({ + .can_id = 0x141, + .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), + }); + + builder.can2_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_front_climber_motor_[0].generate_command(), + device::CanPacket8::PaddingQuarter{}, + chassis_front_climber_motor_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + } + + private: + void can0_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + // can0_receive_rate_counter_.record(can_id); + check[can_id - 0x201] = 1; + if (can_id == 0x201) { + chassis_wheel_motors_[0].store_status(data.can_data); + } else if (can_id == 0x202) { + chassis_wheel_motors_[1].store_status(data.can_data); + } else if (can_id == 0x205) { + chassis_steering_motors_[1].store_status(data.can_data); + } else if (can_id == 0x208) { + chassis_steering_motors_[0].store_status(data.can_data); + } + } + + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + // can1_receive_rate_counter_.record(can_id); + if (can_id != 0x300) { + check[can_id - 0x201] = 1; + } + if (can_id == 0x203) { + chassis_wheel_motors_[2].store_status(data.can_data); + } else if (can_id == 0x204) { + chassis_wheel_motors_[3].store_status(data.can_data); + } else if (can_id == 0x207) { + chassis_steering_motors_[2].store_status(data.can_data); + } else if (can_id == 0x206) { + chassis_steering_motors_[3].store_status(data.can_data); + } else if (can_id == 0x300) { + supercap_.store_status(data.can_data); + } + } + + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) + return; + auto can_id = data.can_id; + // can2_receive_rate_counter_.record(can_id); + if (can_id == 0x201) { + chassis_front_climber_motor_[0].store_status(data.can_data); + } else if (can_id == 0x203) { + chassis_front_climber_motor_[1].store_status(data.can_data); + } + } + + void can3_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + // can3_receive_rate_counter_.record(can_id); + if (can_id == 0x202) { + chassis_back_climber_motor_[1].store_status(data.can_data); + } else if (can_id == 0x204) { + chassis_back_climber_motor_[0].store_status(data.can_data); + } else if (can_id == 0x203) { + yaw_brake_motor_.store_status(data.can_data); + } else if (can_id == 0x141) { + gimbal_bottom_yaw_motor_.store_status(data.can_data); + } + } + + void uart0_receive_callback(const librmcs::data::UartDataView& data) override { + const std::byte* ptr = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, data.uart_data.size()); + } + + void dbus_receive_callback(const librmcs::data::UartDataView& data) override { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + } + + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + imu_.store_accelerometer_status(data.x, data.y, data.z); + } + + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); + } + + rclcpp::Logger logger_; + // CanReceiveRateCounter can0_receive_rate_counter_; + // CanReceiveRateCounter can1_receive_rate_counter_; + // CanReceiveRateCounter can2_receive_rate_counter_; + // CanReceiveRateCounter can3_receive_rate_counter_; + + int count_ = 0; + int check[10] = {0}; + + device::Bmi088 imu_; + device::Dr16 dr16_; + device::Supercap supercap_; + + device::DjiMotor chassis_steering_motors_[4]; + device::DjiMotor chassis_wheel_motors_[4]; + device::DjiMotor chassis_front_climber_motor_[2]; + device::DjiMotor chassis_back_climber_motor_[2]; + device::DjiMotor yaw_brake_motor_; + device::LkMotor gimbal_bottom_yaw_motor_; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; + + OutputInterface referee_serial_; + OutputInterface powermeter_control_enabled_; + OutputInterface powermeter_charge_power_limit_; + OutputInterface chassis_yaw_velocity_imu_; + OutputInterface chassis_pitch_imu_; + }; + + OutputInterface tf_; + + rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; + + std::shared_ptr top_board_; + std::shared_ptr bottom_board_; +}; + +} // namespace rmcs_core::hardware + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringHeroLittle, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp index 698fa16bc..b9b26ddec 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp @@ -1,889 +1,893 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "hardware/device/bmi088.hpp" -#include "hardware/device/can_packet.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/lk_motor.hpp" -#include "hardware/device/supercap.hpp" - -namespace rmcs_core::hardware { - -class CanReceiveRateCounter { -public: - explicit CanReceiveRateCounter(rclcpp::Logger logger, std::string_view channel_name) - : logger_(std::move(logger)) - , channel_name_(channel_name) {} - - void record(std::uint32_t can_id) { - const auto now = Clock::now(); - - std::lock_guard lock{mutex_}; - auto& status = statuses_[can_id]; - ++status.receive_count; - status.last_receive_time = now; - - report_if_due(now); - } - - void report_if_due() { - const auto now = Clock::now(); - - std::lock_guard lock{mutex_}; - report_if_due(now); - } - -private: - using Clock = std::chrono::steady_clock; - - struct Status { - std::size_t receive_count{0}; - Clock::time_point last_receive_time{}; - }; - - void report_if_due(Clock::time_point now) { - if (statuses_.empty()) - return; - - if (last_report_time_ == Clock::time_point{}) { - last_report_time_ = now; - return; - } - - const auto elapsed = now - last_report_time_; - if (elapsed < kReportInterval) - return; - - const auto elapsed_seconds = std::chrono::duration(elapsed).count(); - for (auto& [can_id, status] : statuses_) { - const bool attached = now - status.last_receive_time <= kMissTimeout; - RCLCPP_INFO( - logger_, "[can rx] %.*s id=0x%03X rate=%.1fHz status=%s", - static_cast(channel_name_.size()), channel_name_.data(), - static_cast(can_id), - static_cast(status.receive_count) / elapsed_seconds, - attached ? "attach" : "miss"); - status.receive_count = 0; - } - - last_report_time_ = now; - } - - static constexpr std::chrono::milliseconds kReportInterval{1000}; - static constexpr std::chrono::milliseconds kMissTimeout{1000}; - - rclcpp::Logger logger_; - std::string_view channel_name_; - std::mutex mutex_; - Clock::time_point last_report_time_{}; - std::map statuses_; -}; - -class SteeringHeroLittle - : public rmcs_executor::Component - , public rclcpp::Node { -public: - SteeringHeroLittle() - : Node( - get_component_name(), - rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) - , command_component_( - create_partner_component( - get_component_name() + "_command", *this)) { - - register_output("/tf", tf_); - - gimbal_calibrate_subscription_ = create_subscription( - "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - gimbal_calibrate_subscription_callback(std::move(msg)); - }); - - top_board_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_top_board").as_string()); - - bottom_board_ = std::make_unique( - *this, *command_component_, get_parameter("serial_bottom_rmcs_board").as_string()); - - tf_->set_transform( - Eigen::Translation3d{0.06603, 0.0, 0.082}); - } - - SteeringHeroLittle(const SteeringHeroLittle&) = delete; - SteeringHeroLittle& operator=(const SteeringHeroLittle&) = delete; - SteeringHeroLittle(SteeringHeroLittle&&) = delete; - SteeringHeroLittle& operator=(SteeringHeroLittle&&) = delete; - - ~SteeringHeroLittle() override = default; - - void update() override { - top_board_->update(); - bottom_board_->update(); - - tf_->set_state( - bottom_board_->gimbal_bottom_yaw_motor_.angle() - + top_board_->gimbal_top_yaw_motor_.angle()); - tf_->set_state( - top_board_->gimbal_pitch_motor_.angle()); - } - - void command_update() { - top_board_->command_update(); - bottom_board_->command_update(); - } - -private: - void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New bottom yaw offset: %ld", - bottom_board_->gimbal_bottom_yaw_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New pitch offset: %ld", - top_board_->gimbal_pitch_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New player viewer offset: %ld", - top_board_->gimbal_player_viewer_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New top yaw offset: %ld", - top_board_->gimbal_top_yaw_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New bullet feeder offset: %ld", - top_board_->gimbal_bullet_feeder_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] left front steering offset: %d", - bottom_board_->chassis_steering_motors_[0].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] right front steering offset: %d", - bottom_board_->chassis_steering_motors_[1].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] left back steering offset: %d", - bottom_board_->chassis_steering_motors_[2].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] right back steering offset: %d", - bottom_board_->chassis_steering_motors_[3].calibrate_zero_point()); - } - - class SteeringHeroLittleCommand : public rmcs_executor::Component { - public: - explicit SteeringHeroLittleCommand(SteeringHeroLittle& hero) - : hero_(hero) {} - - void update() override { hero_.command_update(); } - - SteeringHeroLittle& hero_; - }; - std::shared_ptr command_component_; - - class TopBoard final : private librmcs::agent::RmcsBoardLite { - public: - friend class SteeringHeroLittle; - explicit TopBoard( - SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, - std::string_view board_serial = {}) - : librmcs::agent::RmcsBoardLite(board_serial) - , logger_(steering_hero.get_logger()) - // , can0_receive_rate_counter_(logger_, "bottom/can0") - // , can1_receive_rate_counter_(logger_, "bottom/can1") - // , can2_receive_rate_counter_(logger_, "bottom/can2") - // , can3_receive_rate_counter_(logger_, "bottom/can3") - , tf_(steering_hero.tf_) - , imu_(1000, 0.2, 0.0) - , gimbal_top_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/top_yaw") - , gimbal_pitch_motor_(steering_hero, steering_hero_command, "/gimbal/pitch") - , gimbal_friction_wheels_( - {steering_hero, steering_hero_command, "/gimbal/first_right_friction"}, - {steering_hero, steering_hero_command, "/gimbal/first_left_friction"}, - {steering_hero, steering_hero_command, "/gimbal/second_right_friction"}, - {steering_hero, steering_hero_command, "/gimbal/second_left_friction"}) - , gimbal_bullet_feeder_(steering_hero, steering_hero_command, "/gimbal/bullet_feeder") - , putter_motor_(steering_hero, steering_hero_command, "/gimbal/putter") - , gimbal_scope_motor_(steering_hero, steering_hero_command, "/gimbal/scope") - , gimbal_player_viewer_motor_( - steering_hero, steering_hero_command, "/gimbal/player_viewer") { - - gimbal_top_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} - .enable_multi_turn_angle() - .set_encoder_zero_point( - static_cast( - steering_hero.get_parameter("top_yaw_motor_zero_point").as_int()))); - gimbal_pitch_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} - .set_encoder_zero_point( - static_cast( - steering_hero.get_parameter("pitch_motor_zero_point").as_int())) - .enable_multi_turn_angle()); - gimbal_friction_wheels_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); - gimbal_friction_wheels_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(1.)); - gimbal_friction_wheels_[2].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); - gimbal_friction_wheels_[3].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(1.)); - gimbal_bullet_feeder_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} - .set_encoder_zero_point( - static_cast( - steering_hero.get_parameter("bullet_feeder_motor_zero_point").as_int())) - .set_reversed() - .enable_multi_turn_angle()); - putter_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(1.) - .enable_multi_turn_angle()); - gimbal_scope_motor_.configure(device::DjiMotor::Config{device::DjiMotor::Type::kM2006}); - gimbal_player_viewer_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} - .set_encoder_zero_point( - static_cast( - steering_hero.get_parameter("viewer_motor_zero_point").as_int())) - .set_reversed() - .enable_multi_turn_angle()); - - steering_hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); - steering_hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); - - steering_hero.register_output( - "/gimbal/photoelectric_sensor", photoelectric_sensor_status_, false); - steering_hero.register_output( - "/gimbal/grayscale_sensor", grayscale_sensor_status_, false); - steering_hero.register_output( - "/auto_aim/image_capturer/timestamp", camera_capturer_trigger_timestamp_, 0); - steering_hero.register_output( - "/auto_aim/image_capturer/trigger", camera_capturer_trigger_, 0); - - imu_.set_coordinate_mapping([](double x, double y, double z) { - // Get the mapping with the following code. - // The rotation angle must be an exact multiple of 90 degrees, otherwise - // use a matrix. - - return std::make_tuple(-y, x, z); - }); - } - - TopBoard(const TopBoard&) = delete; - TopBoard& operator=(const TopBoard&) = delete; - TopBoard(TopBoard&&) = delete; - TopBoard& operator=(TopBoard&&) = delete; - - ~TopBoard() final = default; - - void update() { - // can0_receive_rate_counter_.report_if_due(); - // can1_receive_rate_counter_.report_if_due(); - // can2_receive_rate_counter_.report_if_due(); - // can3_receive_rate_counter_.report_if_due(); - - imu_.update_status(); - Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; - - tf_->set_transform( - gimbal_imu_pose.conjugate()); - - *gimbal_yaw_velocity_imu_ = imu_.gz(); - *gimbal_pitch_velocity_imu_ = imu_.gy(); - - gimbal_top_yaw_motor_.update_status(); - gimbal_pitch_motor_.update_status(); - tf_->set_state( - gimbal_pitch_motor_.angle()); - - for (auto& motor : gimbal_friction_wheels_) - motor.update_status(); - - gimbal_bullet_feeder_.update_status(); - putter_motor_.update_status(); - - gimbal_player_viewer_motor_.update_status(); - tf_->set_state( - gimbal_player_viewer_motor_.angle()); - - gimbal_scope_motor_.update_status(); - - if (last_camera_capturer_trigger_timestamp_ != *camera_capturer_trigger_timestamp_) - *camera_capturer_trigger_ = true; - last_camera_capturer_trigger_timestamp_ = *camera_capturer_trigger_timestamp_; - - *photoelectric_sensor_status_ = photoelectric_sensor_status_atomic.load(); - *grayscale_sensor_status_ = grayscale_sensor_status_atomic.load(); - } - - void command_update() { - auto builder = start_transmit(); - - if (std::isfinite(gimbal_pitch_motor_.control_angle())) - builder.can0_transmit({ - .can_id = 0x142, - .can_data = gimbal_pitch_motor_ - .generate_angle_command(gimbal_pitch_motor_.control_angle()) - .as_bytes(), - }); - else - builder.can0_transmit({ - .can_id = 0x142, - .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), - }); // Used to distinguish pitch encoder control from IMU control. - - builder.can0_transmit({ - .can_id = 0x141, - .can_data = gimbal_top_yaw_motor_.generate_command().as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_friction_wheels_[0].generate_command(), - gimbal_friction_wheels_[1].generate_command(), - gimbal_friction_wheels_[2].generate_command(), - gimbal_friction_wheels_[3].generate_command(), - } - .as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x1FF, - .can_data = - device::CanPacket8{ - putter_motor_.generate_command(), - gimbal_scope_motor_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x143, - .can_data = - gimbal_player_viewer_motor_ - .generate_velocity_command(gimbal_player_viewer_motor_.control_velocity()) - .as_bytes(), - }); - - builder.can3_transmit({ - .can_id = 0x142, - .can_data = gimbal_bullet_feeder_.generate_torque_command().as_bytes(), - }); - - builder.gpio_digital_read( - librmcs::spec::rmcs_board_lite::kGpioDescriptors[2], - { - .period_ms = 20, - .pull = librmcs::data::GpioPull::kUp, - }); - - builder.gpio_digital_read( - librmcs::spec::rmcs_board_lite::kGpioDescriptors[3], - { - .period_ms = 20, - .pull = librmcs::data::GpioPull::kUp, - }); - } - - private: - void can0_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can0_receive_rate_counter_.record(can_id); - if (can_id == 0x141) { - gimbal_top_yaw_motor_.store_status(data.can_data); - } else if (can_id == 0x142) { - gimbal_pitch_motor_.store_status(data.can_data); - } - } - - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can1_receive_rate_counter_.record(can_id); - if (can_id == 0x201) { - gimbal_friction_wheels_[0].store_status(data.can_data); - } else if (can_id == 0x202) { - gimbal_friction_wheels_[1].store_status(data.can_data); - } else if (can_id == 0x203) { - gimbal_friction_wheels_[2].store_status(data.can_data); - } else if (can_id == 0x204) { - gimbal_friction_wheels_[3].store_status(data.can_data); - } else if (can_id == 0x205) { - putter_motor_.store_status(data.can_data); - } else if (can_id == 0x206) { - gimbal_scope_motor_.store_status(data.can_data); - } - } - - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can2_receive_rate_counter_.record(can_id); - if (can_id == 0x143) { - gimbal_player_viewer_motor_.store_status(data.can_data); - } - } - - void can3_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can3_receive_rate_counter_.record(can_id); - if (can_id == 0x142) { - gimbal_bullet_feeder_.store_status(data.can_data); - } - } - - void gpio_digital_read_result_callback( - const librmcs::spec::rmcs_board_lite::GpioDescriptor& gpio, - const librmcs::data::GpioDigitalDataView& data) override { - if (gpio.channel_index == 2) { - photoelectric_sensor_status_atomic.store(data.high); - } else if (gpio.channel_index == 3) { - grayscale_sensor_status_atomic.store(!data.high); - } - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - - rclcpp::Logger logger_; - // CanReceiveRateCounter can0_receive_rate_counter_; - // CanReceiveRateCounter can1_receive_rate_counter_; - // CanReceiveRateCounter can2_receive_rate_counter_; - // CanReceiveRateCounter can3_receive_rate_counter_; - OutputInterface& tf_; - - std::time_t last_camera_capturer_trigger_timestamp_{0}; - - device::Bmi088 imu_; - device::LkMotor gimbal_top_yaw_motor_; - device::LkMotor gimbal_pitch_motor_; - device::DjiMotor gimbal_friction_wheels_[4]; - device::LkMotor gimbal_bullet_feeder_; - device::DjiMotor putter_motor_; - device::DjiMotor gimbal_scope_motor_; - device::LkMotor gimbal_player_viewer_motor_; - - OutputInterface gimbal_yaw_velocity_imu_; - OutputInterface gimbal_pitch_velocity_imu_; - OutputInterface photoelectric_sensor_status_; - OutputInterface grayscale_sensor_status_; - OutputInterface camera_capturer_trigger_; - OutputInterface camera_capturer_trigger_timestamp_; - std::atomic photoelectric_sensor_status_atomic{false}; - std::atomic grayscale_sensor_status_atomic{false}; - }; - - class BottomBoard final : private librmcs::agent::RmcsBoardLite { - public: - friend class SteeringHeroLittle; - explicit BottomBoard( - SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, - std::string_view board_serial = {}) - : librmcs::agent::RmcsBoardLite( - board_serial, {.dangerously_skip_version_checks = false}) - , logger_(steering_hero.get_logger()) - // , can0_receive_rate_counter_(logger_, "bottom/can0") - // , can1_receive_rate_counter_(logger_, "bottom/can1") - // , can2_receive_rate_counter_(logger_, "bottom/can2") - // , can3_receive_rate_counter_(logger_, "bottom/can3") - , imu_(1000, 0.2, 0.0) - , dr16_(steering_hero) - , supercap_(steering_hero, steering_hero_command) - , chassis_steering_motors_( - {steering_hero, steering_hero_command, "/chassis/left_front_steering"}, - {steering_hero, steering_hero_command, "/chassis/right_front_steering"}, - {steering_hero, steering_hero_command, "/chassis/left_back_steering"}, - {steering_hero, steering_hero_command, "/chassis/right_back_steering"}) - , chassis_wheel_motors_( - {steering_hero, steering_hero_command, "/chassis/left_front_wheel"}, - {steering_hero, steering_hero_command, "/chassis/right_front_wheel"}, - {steering_hero, steering_hero_command, "/chassis/left_back_wheel"}, - {steering_hero, steering_hero_command, "/chassis/right_back_wheel"}) - , chassis_front_climber_motor_( - {steering_hero, steering_hero_command, "/chassis/climber/left_front_motor"}, - {steering_hero, steering_hero_command, "/chassis/climber/right_front_motor"}) - , chassis_back_climber_motor_( - {steering_hero, steering_hero_command, "/chassis/climber/left_back_motor"}, - {steering_hero, steering_hero_command, "/chassis/climber/right_back_motor"}) - , yaw_brake_motor_(steering_hero, steering_hero_command, "/gimbal/yaw_brake") - , gimbal_bottom_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/bottom_yaw") { - // - chassis_steering_motors_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast( - steering_hero.get_parameter("left_front_zero_point").as_int())) - .set_reversed()); - chassis_steering_motors_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast( - steering_hero.get_parameter("right_front_zero_point").as_int())) - .set_reversed()); - chassis_steering_motors_[2].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast( - steering_hero.get_parameter("left_back_zero_point").as_int())) - .set_reversed()); - chassis_steering_motors_[3].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast( - steering_hero.get_parameter("right_back_zero_point").as_int())) - .set_reversed()); - - chassis_wheel_motors_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)); - chassis_wheel_motors_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)); - chassis_wheel_motors_[2].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)); - chassis_wheel_motors_[3].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)); - chassis_front_climber_motor_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(19.)); - chassis_front_climber_motor_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - chassis_back_climber_motor_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .enable_multi_turn_angle() - .set_reduction_ratio(19.)); - chassis_back_climber_motor_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .enable_multi_turn_angle() - .set_reduction_ratio(19.)); - - yaw_brake_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.set_reduction_ratio(1.)); - gimbal_bottom_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} - .set_reversed() - .set_encoder_zero_point( - static_cast( - steering_hero.get_parameter("bottom_yaw_motor_zero_point").as_int()))); - - steering_hero.register_output("/referee/serial", referee_serial_); - referee_serial_->read = [this](std::byte* buffer, size_t size) { - return referee_ring_buffer_receive_.pop_front_n( - - [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); - }; - referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart0_transmit( - {.uart_data = std::span{buffer, size}}); - return size; - }; - steering_hero.register_output( - "/chassis/powermeter/control_enable", powermeter_control_enabled_, false); - steering_hero.register_output( - "/chassis/powermeter/charge_power_limit", powermeter_charge_power_limit_, 0.); - - steering_hero.register_output( - "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - steering_hero.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); - } - - BottomBoard(const BottomBoard&) = delete; - BottomBoard& operator=(const BottomBoard&) = delete; - BottomBoard(BottomBoard&&) = delete; - BottomBoard& operator=(BottomBoard&&) = delete; - - ~BottomBoard() final = default; - - void update() { - // can0_receive_rate_counter_.report_if_due(); - // can1_receive_rate_counter_.report_if_due(); - // can2_receive_rate_counter_.report_if_due(); - // can3_receive_rate_counter_.report_if_due(); - - imu_.update_status(); - dr16_.update_status(); - supercap_.update_status(); - - *chassis_yaw_velocity_imu_ = imu_.gz(); - *chassis_pitch_imu_ = -std::asin(2.0 * (imu_.q0() * imu_.q2() - imu_.q3() * imu_.q1())); - - chassis_front_climber_motor_[0].update_status(); - chassis_front_climber_motor_[1].update_status(); - chassis_back_climber_motor_[0].update_status(); - chassis_back_climber_motor_[1].update_status(); - - for (auto& motor : chassis_wheel_motors_) - motor.update_status(); - for (auto& motor : chassis_steering_motors_) - motor.update_status(); - - yaw_brake_motor_.update_status(); - gimbal_bottom_yaw_motor_.update_status(); - - if (++count_ == 500) { - for (int i = 0; i < 8; ++i) { - if (check[i] == 0) { - // RCLCPP_WARN(logger_, "can id 0x%03X missing", i + 0x201); - } - } - std::fill_n(check, 8, 0); - count_ = 0; - } - } - - void command_update() { - auto builder = start_transmit(); - - builder.can0_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), - chassis_wheel_motors_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - - builder.can0_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steering_motors_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - chassis_steering_motors_[0].generate_command(), - } - .as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - chassis_wheel_motors_[2].generate_command(), - chassis_wheel_motors_[3].generate_command(), - } - .as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - chassis_steering_motors_[3].generate_command(), - chassis_steering_motors_[2].generate_command(), - supercap_.generate_command(), - } - .as_bytes(), - }); - - builder.can3_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - chassis_back_climber_motor_[1].generate_command(), - yaw_brake_motor_.generate_command(), - chassis_back_climber_motor_[0].generate_command(), - } - .as_bytes(), - }); - - builder.can3_transmit({ - .can_id = 0x141, - .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_front_climber_motor_[0].generate_command(), - device::CanPacket8::PaddingQuarter{}, - chassis_front_climber_motor_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - } - - private: - void can0_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can0_receive_rate_counter_.record(can_id); - check[can_id - 0x201] = 1; - if (can_id == 0x201) { - chassis_wheel_motors_[0].store_status(data.can_data); - } else if (can_id == 0x202) { - chassis_wheel_motors_[1].store_status(data.can_data); - } else if (can_id == 0x205) { - chassis_steering_motors_[1].store_status(data.can_data); - } else if (can_id == 0x208) { - chassis_steering_motors_[0].store_status(data.can_data); - } - } - - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can1_receive_rate_counter_.record(can_id); - if (can_id != 0x300) { - check[can_id - 0x201] = 1; - } - if (can_id == 0x203) { - chassis_wheel_motors_[2].store_status(data.can_data); - } else if (can_id == 0x204) { - chassis_wheel_motors_[3].store_status(data.can_data); - } else if (can_id == 0x207) { - chassis_steering_motors_[2].store_status(data.can_data); - } else if (can_id == 0x206) { - chassis_steering_motors_[3].store_status(data.can_data); - } else if (can_id == 0x300) { - supercap_.store_status(data.can_data); - } - } - - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - auto can_id = data.can_id; - // can2_receive_rate_counter_.record(can_id); - if (can_id == 0x201) { - chassis_front_climber_motor_[0].store_status(data.can_data); - } else if (can_id == 0x203) { - chassis_front_climber_motor_[1].store_status(data.can_data); - } - } - - void can3_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can3_receive_rate_counter_.record(can_id); - if (can_id == 0x202) { - chassis_back_climber_motor_[1].store_status(data.can_data); - } else if (can_id == 0x204) { - chassis_back_climber_motor_[0].store_status(data.can_data); - } else if (can_id == 0x203) { - yaw_brake_motor_.store_status(data.can_data); - } else if (can_id == 0x141) { - gimbal_bottom_yaw_motor_.store_status(data.can_data); - } - } - - void uart0_receive_callback(const librmcs::data::UartDataView& data) override { - const std::byte* ptr = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, data.uart_data.size()); - } - - void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - - rclcpp::Logger logger_; - // CanReceiveRateCounter can0_receive_rate_counter_; - // CanReceiveRateCounter can1_receive_rate_counter_; - // CanReceiveRateCounter can2_receive_rate_counter_; - // CanReceiveRateCounter can3_receive_rate_counter_; - - int count_ = 0; - int check[10] = {0}; - - device::Bmi088 imu_; - device::Dr16 dr16_; - device::Supercap supercap_; - - device::DjiMotor chassis_steering_motors_[4]; - device::DjiMotor chassis_wheel_motors_[4]; - device::DjiMotor chassis_front_climber_motor_[2]; - device::DjiMotor chassis_back_climber_motor_[2]; - device::DjiMotor yaw_brake_motor_; - device::LkMotor gimbal_bottom_yaw_motor_; - - rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; - - OutputInterface referee_serial_; - OutputInterface powermeter_control_enabled_; - OutputInterface powermeter_charge_power_limit_; - OutputInterface chassis_yaw_velocity_imu_; - OutputInterface chassis_pitch_imu_; - }; - - OutputInterface tf_; - - rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; - - std::shared_ptr top_board_; - std::shared_ptr bottom_board_; -}; - -} // namespace rmcs_core::hardware - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringHeroLittle, rmcs_executor::Component) +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include + +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include + +// #include "hardware/device/bmi088.hpp" +// #include "hardware/device/can_packet.hpp" +// #include "hardware/device/dji_motor.hpp" +// #include "hardware/device/dr16.hpp" +// #include "hardware/device/lk_motor.hpp" +// #include "hardware/device/supercap.hpp" + +// namespace rmcs_core::hardware { + +// class CanReceiveRateCounter { +// public: +// explicit CanReceiveRateCounter(rclcpp::Logger logger, std::string_view channel_name) +// : logger_(std::move(logger)) +// , channel_name_(channel_name) {} + +// void record(std::uint32_t can_id) { +// const auto now = Clock::now(); + +// std::lock_guard lock{mutex_}; +// auto& status = statuses_[can_id]; +// ++status.receive_count; +// status.last_receive_time = now; + +// report_if_due(now); +// } + +// void report_if_due() { +// const auto now = Clock::now(); + +// std::lock_guard lock{mutex_}; +// report_if_due(now); +// } + +// private: +// using Clock = std::chrono::steady_clock; + +// struct Status { +// std::size_t receive_count{0}; +// Clock::time_point last_receive_time{}; +// }; + +// void report_if_due(Clock::time_point now) { +// if (statuses_.empty()) +// return; + +// if (last_report_time_ == Clock::time_point{}) { +// last_report_time_ = now; +// return; +// } + +// const auto elapsed = now - last_report_time_; +// if (elapsed < kReportInterval) +// return; + +// const auto elapsed_seconds = std::chrono::duration(elapsed).count(); +// for (auto& [can_id, status] : statuses_) { +// const bool attached = now - status.last_receive_time <= kMissTimeout; +// RCLCPP_INFO( +// logger_, "[can rx] %.*s id=0x%03X rate=%.1fHz status=%s", +// static_cast(channel_name_.size()), channel_name_.data(), +// static_cast(can_id), +// static_cast(status.receive_count) / elapsed_seconds, +// attached ? "attach" : "miss"); +// status.receive_count = 0; +// } + +// last_report_time_ = now; +// } + +// static constexpr std::chrono::milliseconds kReportInterval{1000}; +// static constexpr std::chrono::milliseconds kMissTimeout{1000}; + +// rclcpp::Logger logger_; +// std::string_view channel_name_; +// std::mutex mutex_; +// Clock::time_point last_report_time_{}; +// std::map statuses_; +// }; + +// class SteeringHeroLittle +// : public rmcs_executor::Component +// , public rclcpp::Node { +// public: +// SteeringHeroLittle() +// : Node( +// get_component_name(), +// rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) +// , command_component_( +// create_partner_component( +// get_component_name() + "_command", *this)) { + +// register_output("/tf", tf_); + +// gimbal_calibrate_subscription_ = create_subscription( +// "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { +// gimbal_calibrate_subscription_callback(std::move(msg)); +// }); + +// top_board_ = std::make_unique( +// *this, *command_component_, get_parameter("board_serial_top_board").as_string()); + +// bottom_board_ = std::make_unique( +// *this, *command_component_, get_parameter("serial_bottom_rmcs_board").as_string()); + +// tf_->set_transform( +// Eigen::Translation3d{0.06603, 0.0, 0.082}); +// } + +// SteeringHeroLittle(const SteeringHeroLittle&) = delete; +// SteeringHeroLittle& operator=(const SteeringHeroLittle&) = delete; +// SteeringHeroLittle(SteeringHeroLittle&&) = delete; +// SteeringHeroLittle& operator=(SteeringHeroLittle&&) = delete; + +// ~SteeringHeroLittle() override = default; + +// void update() override { +// top_board_->update(); +// bottom_board_->update(); + +// tf_->set_state( +// bottom_board_->gimbal_bottom_yaw_motor_.angle() +// + top_board_->gimbal_top_yaw_motor_.angle()); +// tf_->set_state( +// top_board_->gimbal_pitch_motor_.angle()); +// } + +// void command_update() { +// top_board_->command_update(); +// bottom_board_->command_update(); +// } + +// private: +// void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { +// RCLCPP_INFO( +// get_logger(), "[gimbal calibration] New bottom yaw offset: %ld", +// bottom_board_->gimbal_bottom_yaw_motor_.calibrate_zero_point()); +// RCLCPP_INFO( +// get_logger(), "[gimbal calibration] New pitch offset: %ld", +// top_board_->gimbal_pitch_motor_.calibrate_zero_point()); +// RCLCPP_INFO( +// get_logger(), "[gimbal calibration] New player viewer offset: %ld", +// top_board_->gimbal_player_viewer_motor_.calibrate_zero_point()); +// RCLCPP_INFO( +// get_logger(), "[gimbal calibration] New top yaw offset: %ld", +// top_board_->gimbal_top_yaw_motor_.calibrate_zero_point()); +// RCLCPP_INFO( +// get_logger(), "[gimbal calibration] New bullet feeder offset: %ld", +// top_board_->gimbal_bullet_feeder_.calibrate_zero_point()); +// RCLCPP_INFO( +// get_logger(), "[chassis calibration] left front steering offset: %d", +// bottom_board_->chassis_steering_motors_[0].calibrate_zero_point()); +// RCLCPP_INFO( +// get_logger(), "[chassis calibration] right front steering offset: %d", +// bottom_board_->chassis_steering_motors_[1].calibrate_zero_point()); +// RCLCPP_INFO( +// get_logger(), "[chassis calibration] left back steering offset: %d", +// bottom_board_->chassis_steering_motors_[2].calibrate_zero_point()); +// RCLCPP_INFO( +// get_logger(), "[chassis calibration] right back steering offset: %d", +// bottom_board_->chassis_steering_motors_[3].calibrate_zero_point()); +// } + +// class SteeringHeroLittleCommand : public rmcs_executor::Component { +// public: +// explicit SteeringHeroLittleCommand(SteeringHeroLittle& hero) +// : hero_(hero) {} + +// void update() override { hero_.command_update(); } + +// SteeringHeroLittle& hero_; +// }; +// std::shared_ptr command_component_; + +// class TopBoard final : private librmcs::agent::RmcsBoardLite { +// public: +// friend class SteeringHeroLittle; +// explicit TopBoard( +// SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, +// std::string_view board_serial = {}) +// : librmcs::agent::RmcsBoardLite(board_serial) +// , logger_(steering_hero.get_logger()) +// // , can0_receive_rate_counter_(logger_, "bottom/can0") +// // , can1_receive_rate_counter_(logger_, "bottom/can1") +// // , can2_receive_rate_counter_(logger_, "bottom/can2") +// // , can3_receive_rate_counter_(logger_, "bottom/can3") +// , tf_(steering_hero.tf_) +// , imu_(1000, 0.2, 0.0) +// , gimbal_top_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/top_yaw") +// , gimbal_pitch_motor_(steering_hero, steering_hero_command, "/gimbal/pitch") +// , gimbal_friction_wheels_( +// {steering_hero, steering_hero_command, "/gimbal/first_right_friction"}, +// {steering_hero, steering_hero_command, "/gimbal/first_left_friction"}, +// {steering_hero, steering_hero_command, "/gimbal/second_right_friction"}, +// {steering_hero, steering_hero_command, "/gimbal/second_left_friction"}) +// , gimbal_bullet_feeder_(steering_hero, steering_hero_command, +// "/gimbal/bullet_feeder") , putter_motor_(steering_hero, steering_hero_command, +// "/gimbal/putter") , gimbal_scope_motor_(steering_hero, steering_hero_command, +// "/gimbal/scope") , gimbal_player_viewer_motor_( +// steering_hero, steering_hero_command, "/gimbal/player_viewer") { + +// gimbal_top_yaw_motor_.configure( +// device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} +// .enable_multi_turn_angle() +// .set_encoder_zero_point( +// static_cast( +// steering_hero.get_parameter("top_yaw_motor_zero_point").as_int()))); +// gimbal_pitch_motor_.configure( +// device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} +// .set_encoder_zero_point( +// static_cast( +// steering_hero.get_parameter("pitch_motor_zero_point").as_int())) +// .enable_multi_turn_angle()); +// gimbal_friction_wheels_[0].configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); +// gimbal_friction_wheels_[1].configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kM3508} +// .set_reversed() +// .set_reduction_ratio(1.)); +// gimbal_friction_wheels_[2].configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); +// gimbal_friction_wheels_[3].configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kM3508} +// .set_reversed() +// .set_reduction_ratio(1.)); +// gimbal_bullet_feeder_.configure( +// device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} +// .set_encoder_zero_point( +// static_cast( +// steering_hero.get_parameter("bullet_feeder_motor_zero_point").as_int())) +// .set_reversed() +// .enable_multi_turn_angle()); +// putter_motor_.configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kM3508} +// .set_reduction_ratio(1.) +// .enable_multi_turn_angle()); +// gimbal_scope_motor_.configure(device::DjiMotor::Config{device::DjiMotor::Type::kM2006}); +// gimbal_player_viewer_motor_.configure( +// device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} +// .set_encoder_zero_point( +// static_cast( +// steering_hero.get_parameter("viewer_motor_zero_point").as_int())) +// .set_reversed() +// .enable_multi_turn_angle()); + +// steering_hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); +// steering_hero.register_output("/gimbal/pitch/velocity_imu", +// gimbal_pitch_velocity_imu_); + +// steering_hero.register_output( +// "/gimbal/photoelectric_sensor", photoelectric_sensor_status_, false); +// steering_hero.register_output( +// "/gimbal/grayscale_sensor", grayscale_sensor_status_, false); +// steering_hero.register_output( +// "/auto_aim/image_capturer/timestamp", camera_capturer_trigger_timestamp_, 0); +// steering_hero.register_output( +// "/auto_aim/image_capturer/trigger", camera_capturer_trigger_, 0); + +// imu_.set_coordinate_mapping([](double x, double y, double z) { +// // Get the mapping with the following code. +// // The rotation angle must be an exact multiple of 90 degrees, otherwise +// // use a matrix. + +// return std::make_tuple(-y, x, z); +// }); +// } + +// TopBoard(const TopBoard&) = delete; +// TopBoard& operator=(const TopBoard&) = delete; +// TopBoard(TopBoard&&) = delete; +// TopBoard& operator=(TopBoard&&) = delete; + +// ~TopBoard() final = default; + +// void update() { +// // can0_receive_rate_counter_.report_if_due(); +// // can1_receive_rate_counter_.report_if_due(); +// // can2_receive_rate_counter_.report_if_due(); +// // can3_receive_rate_counter_.report_if_due(); + +// imu_.update_status(); +// Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; + +// tf_->set_transform( +// gimbal_imu_pose.conjugate()); + +// *gimbal_yaw_velocity_imu_ = imu_.gz(); +// *gimbal_pitch_velocity_imu_ = imu_.gy(); + +// gimbal_top_yaw_motor_.update_status(); +// gimbal_pitch_motor_.update_status(); +// tf_->set_state( +// gimbal_pitch_motor_.angle()); + +// for (auto& motor : gimbal_friction_wheels_) +// motor.update_status(); + +// gimbal_bullet_feeder_.update_status(); +// putter_motor_.update_status(); + +// gimbal_player_viewer_motor_.update_status(); +// tf_->set_state( +// gimbal_player_viewer_motor_.angle()); + +// gimbal_scope_motor_.update_status(); + +// if (last_camera_capturer_trigger_timestamp_ != *camera_capturer_trigger_timestamp_) +// *camera_capturer_trigger_ = true; +// last_camera_capturer_trigger_timestamp_ = *camera_capturer_trigger_timestamp_; + +// *photoelectric_sensor_status_ = photoelectric_sensor_status_atomic.load(); +// *grayscale_sensor_status_ = grayscale_sensor_status_atomic.load(); +// } + +// void command_update() { +// auto builder = start_transmit(); + +// if (std::isfinite(gimbal_pitch_motor_.control_angle())) +// builder.can0_transmit({ +// .can_id = 0x142, +// .can_data = gimbal_pitch_motor_ +// .generate_angle_command(gimbal_pitch_motor_.control_angle()) +// .as_bytes(), +// }); +// else +// builder.can0_transmit({ +// .can_id = 0x142, +// .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), +// }); // Used to distinguish pitch encoder control from IMU control. + +// builder.can0_transmit({ +// .can_id = 0x141, +// .can_data = gimbal_top_yaw_motor_.generate_command().as_bytes(), +// }); + +// builder.can1_transmit({ +// .can_id = 0x200, +// .can_data = +// device::CanPacket8{ +// gimbal_friction_wheels_[0].generate_command(), +// gimbal_friction_wheels_[1].generate_command(), +// gimbal_friction_wheels_[2].generate_command(), +// gimbal_friction_wheels_[3].generate_command(), +// } +// .as_bytes(), +// }); + +// builder.can1_transmit({ +// .can_id = 0x1FF, +// .can_data = +// device::CanPacket8{ +// putter_motor_.generate_command(), +// gimbal_scope_motor_.generate_command(), +// device::CanPacket8::PaddingQuarter{}, +// device::CanPacket8::PaddingQuarter{}, +// } +// .as_bytes(), +// }); + +// builder.can2_transmit({ +// .can_id = 0x143, +// .can_data = +// gimbal_player_viewer_motor_ +// .generate_velocity_command(gimbal_player_viewer_motor_.control_velocity()) +// .as_bytes(), +// }); + +// builder.can3_transmit({ +// .can_id = 0x142, +// .can_data = gimbal_bullet_feeder_.generate_torque_command().as_bytes(), +// }); + +// builder.gpio_digital_read( +// librmcs::spec::rmcs_board_lite::kGpioDescriptors[2], +// { +// .period_ms = 20, +// .pull = librmcs::data::GpioPull::kUp, +// }); + +// builder.gpio_digital_read( +// librmcs::spec::rmcs_board_lite::kGpioDescriptors[3], +// { +// .period_ms = 20, +// .pull = librmcs::data::GpioPull::kUp, +// }); +// } + +// private: +// void can0_receive_callback(const librmcs::data::CanDataView& data) override { +// if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] +// return; +// auto can_id = data.can_id; +// // can0_receive_rate_counter_.record(can_id); +// if (can_id == 0x141) { +// gimbal_top_yaw_motor_.store_status(data.can_data); +// } else if (can_id == 0x142) { +// gimbal_pitch_motor_.store_status(data.can_data); +// } +// } + +// void can1_receive_callback(const librmcs::data::CanDataView& data) override { +// if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] +// return; +// auto can_id = data.can_id; +// // can1_receive_rate_counter_.record(can_id); +// if (can_id == 0x201) { +// gimbal_friction_wheels_[0].store_status(data.can_data); +// } else if (can_id == 0x202) { +// gimbal_friction_wheels_[1].store_status(data.can_data); +// } else if (can_id == 0x203) { +// gimbal_friction_wheels_[2].store_status(data.can_data); +// } else if (can_id == 0x204) { +// gimbal_friction_wheels_[3].store_status(data.can_data); +// } else if (can_id == 0x205) { +// putter_motor_.store_status(data.can_data); +// } else if (can_id == 0x206) { +// gimbal_scope_motor_.store_status(data.can_data); +// } +// } + +// void can2_receive_callback(const librmcs::data::CanDataView& data) override { +// if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] +// return; +// auto can_id = data.can_id; +// // can2_receive_rate_counter_.record(can_id); +// if (can_id == 0x143) { +// gimbal_player_viewer_motor_.store_status(data.can_data); +// } +// } + +// void can3_receive_callback(const librmcs::data::CanDataView& data) override { +// if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] +// return; +// auto can_id = data.can_id; +// // can3_receive_rate_counter_.record(can_id); +// if (can_id == 0x142) { +// gimbal_bullet_feeder_.store_status(data.can_data); +// } +// } + +// void gpio_digital_read_result_callback( +// const librmcs::spec::rmcs_board_lite::GpioDescriptor& gpio, +// const librmcs::data::GpioDigitalDataView& data) override { +// if (gpio.channel_index == 2) { +// photoelectric_sensor_status_atomic.store(data.high); +// } else if (gpio.channel_index == 3) { +// grayscale_sensor_status_atomic.store(!data.high); +// } +// } + +// void accelerometer_receive_callback( +// const librmcs::data::AccelerometerDataView& data) override { +// imu_.store_accelerometer_status(data.x, data.y, data.z); +// } + +// void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { +// imu_.store_gyroscope_status(data.x, data.y, data.z); +// } + +// rclcpp::Logger logger_; +// // CanReceiveRateCounter can0_receive_rate_counter_; +// // CanReceiveRateCounter can1_receive_rate_counter_; +// // CanReceiveRateCounter can2_receive_rate_counter_; +// // CanReceiveRateCounter can3_receive_rate_counter_; +// OutputInterface& tf_; + +// std::time_t last_camera_capturer_trigger_timestamp_{0}; + +// device::Bmi088 imu_; +// device::LkMotor gimbal_top_yaw_motor_; +// device::LkMotor gimbal_pitch_motor_; +// device::DjiMotor gimbal_friction_wheels_[4]; +// device::LkMotor gimbal_bullet_feeder_; +// device::DjiMotor putter_motor_; +// device::DjiMotor gimbal_scope_motor_; +// device::LkMotor gimbal_player_viewer_motor_; + +// OutputInterface gimbal_yaw_velocity_imu_; +// OutputInterface gimbal_pitch_velocity_imu_; +// OutputInterface photoelectric_sensor_status_; +// OutputInterface grayscale_sensor_status_; +// OutputInterface camera_capturer_trigger_; +// OutputInterface camera_capturer_trigger_timestamp_; +// std::atomic photoelectric_sensor_status_atomic{false}; +// std::atomic grayscale_sensor_status_atomic{false}; +// }; + +// class BottomBoard final : private librmcs::agent::RmcsBoardLite { +// public: +// friend class SteeringHeroLittle; +// explicit BottomBoard( +// SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, +// std::string_view board_serial = {}) +// : librmcs::agent::RmcsBoardLite( +// board_serial, {.dangerously_skip_version_checks = false}) +// , logger_(steering_hero.get_logger()) +// // , can0_receive_rate_counter_(logger_, "bottom/can0") +// // , can1_receive_rate_counter_(logger_, "bottom/can1") +// // , can2_receive_rate_counter_(logger_, "bottom/can2") +// // , can3_receive_rate_counter_(logger_, "bottom/can3") +// , imu_(1000, 0.2, 0.0) +// , dr16_(steering_hero) +// , supercap_(steering_hero, steering_hero_command) +// , chassis_steering_motors_( +// {steering_hero, steering_hero_command, "/chassis/left_front_steering"}, +// {steering_hero, steering_hero_command, "/chassis/right_front_steering"}, +// {steering_hero, steering_hero_command, "/chassis/left_back_steering"}, +// {steering_hero, steering_hero_command, "/chassis/right_back_steering"}) +// , chassis_wheel_motors_( +// {steering_hero, steering_hero_command, "/chassis/left_front_wheel"}, +// {steering_hero, steering_hero_command, "/chassis/right_front_wheel"}, +// {steering_hero, steering_hero_command, "/chassis/left_back_wheel"}, +// {steering_hero, steering_hero_command, "/chassis/right_back_wheel"}) +// , chassis_front_climber_motor_( +// {steering_hero, steering_hero_command, "/chassis/climber/left_front_motor"}, +// {steering_hero, steering_hero_command, "/chassis/climber/right_front_motor"}) +// , chassis_back_climber_motor_( +// {steering_hero, steering_hero_command, "/chassis/climber/left_back_motor"}, +// {steering_hero, steering_hero_command, "/chassis/climber/right_back_motor"}) +// , yaw_brake_motor_(steering_hero, steering_hero_command, "/gimbal/yaw_brake") +// , gimbal_bottom_yaw_motor_(steering_hero, steering_hero_command, +// "/gimbal/bottom_yaw") { +// // +// chassis_steering_motors_[0].configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} +// .set_encoder_zero_point( +// static_cast( +// steering_hero.get_parameter("left_front_zero_point").as_int())) +// .set_reversed()); +// chassis_steering_motors_[1].configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} +// .set_encoder_zero_point( +// static_cast( +// steering_hero.get_parameter("right_front_zero_point").as_int())) +// .set_reversed()); +// chassis_steering_motors_[2].configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} +// .set_encoder_zero_point( +// static_cast( +// steering_hero.get_parameter("left_back_zero_point").as_int())) +// .set_reversed()); +// chassis_steering_motors_[3].configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} +// .set_encoder_zero_point( +// static_cast( +// steering_hero.get_parameter("right_back_zero_point").as_int())) +// .set_reversed()); + +// chassis_wheel_motors_[0].configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kM3508} +// .set_reversed() +// .set_reduction_ratio(2232. / 169.)); +// chassis_wheel_motors_[1].configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kM3508} +// .set_reversed() +// .set_reduction_ratio(2232. / 169.)); +// chassis_wheel_motors_[2].configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kM3508} +// .set_reversed() +// .set_reduction_ratio(2232. / 169.)); +// chassis_wheel_motors_[3].configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kM3508} +// .set_reversed() +// .set_reduction_ratio(2232. / 169.)); +// chassis_front_climber_motor_[0].configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kM3508} +// .set_reversed() +// .set_reduction_ratio(19.)); +// chassis_front_climber_motor_[1].configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); +// chassis_back_climber_motor_[0].configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kM3508} +// .enable_multi_turn_angle() +// .set_reduction_ratio(19.)); +// chassis_back_climber_motor_[1].configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kM3508} +// .set_reversed() +// .enable_multi_turn_angle() +// .set_reduction_ratio(19.)); + +// yaw_brake_motor_.configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.set_reduction_ratio(1.)); +// gimbal_bottom_yaw_motor_.configure( +// device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} +// .set_reversed() +// .set_encoder_zero_point( +// static_cast( +// steering_hero.get_parameter("bottom_yaw_motor_zero_point").as_int()))); + +// steering_hero.register_output("/referee/serial", referee_serial_); +// referee_serial_->read = [this](std::byte* buffer, size_t size) { +// return referee_ring_buffer_receive_.pop_front_n( + +// [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); +// }; +// referee_serial_->write = [this](const std::byte* buffer, size_t size) { +// start_transmit().uart0_transmit( +// {.uart_data = std::span{buffer, size}}); +// return size; +// }; +// steering_hero.register_output( +// "/chassis/powermeter/control_enable", powermeter_control_enabled_, false); +// steering_hero.register_output( +// "/chassis/powermeter/charge_power_limit", powermeter_charge_power_limit_, 0.); + +// steering_hero.register_output( +// "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); +// steering_hero.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); +// } + +// BottomBoard(const BottomBoard&) = delete; +// BottomBoard& operator=(const BottomBoard&) = delete; +// BottomBoard(BottomBoard&&) = delete; +// BottomBoard& operator=(BottomBoard&&) = delete; + +// ~BottomBoard() final = default; + +// void update() { +// // can0_receive_rate_counter_.report_if_due(); +// // can1_receive_rate_counter_.report_if_due(); +// // can2_receive_rate_counter_.report_if_due(); +// // can3_receive_rate_counter_.report_if_due(); + +// imu_.update_status(); +// dr16_.update_status(); +// supercap_.update_status(); + +// *chassis_yaw_velocity_imu_ = imu_.gz(); +// *chassis_pitch_imu_ = -std::asin(2.0 * (imu_.q0() * imu_.q2() - imu_.q3() * +// imu_.q1())); + +// chassis_front_climber_motor_[0].update_status(); +// chassis_front_climber_motor_[1].update_status(); +// chassis_back_climber_motor_[0].update_status(); +// chassis_back_climber_motor_[1].update_status(); + +// for (auto& motor : chassis_wheel_motors_) +// motor.update_status(); +// for (auto& motor : chassis_steering_motors_) +// motor.update_status(); + +// yaw_brake_motor_.update_status(); +// gimbal_bottom_yaw_motor_.update_status(); + +// if (++count_ == 500) { +// for (int i = 0; i < 8; ++i) { +// if (check[i] == 0) { +// RCLCPP_WARN(logger_, "can id 0x%03X missing", i + 0x201); +// } +// } +// std::fill_n(check, 8, 0); +// count_ = 0; +// } +// } + +// void command_update() { +// auto builder = start_transmit(); + +// builder.can0_transmit({ +// .can_id = 0x200, +// .can_data = +// device::CanPacket8{ +// chassis_wheel_motors_[0].generate_command(), +// chassis_wheel_motors_[1].generate_command(), +// device::CanPacket8::PaddingQuarter{}, +// device::CanPacket8::PaddingQuarter{}, +// } +// .as_bytes(), +// }); + +// builder.can0_transmit({ +// .can_id = 0x1FE, +// .can_data = +// device::CanPacket8{ +// chassis_steering_motors_[1].generate_command(), +// device::CanPacket8::PaddingQuarter{}, +// device::CanPacket8::PaddingQuarter{}, +// chassis_steering_motors_[0].generate_command(), +// } +// .as_bytes(), +// }); + +// builder.can1_transmit({ +// .can_id = 0x200, +// .can_data = +// device::CanPacket8{ +// device::CanPacket8::PaddingQuarter{}, +// device::CanPacket8::PaddingQuarter{}, +// chassis_wheel_motors_[2].generate_command(), +// chassis_wheel_motors_[3].generate_command(), +// } +// .as_bytes(), +// }); + +// builder.can1_transmit({ +// .can_id = 0x1FE, +// .can_data = +// device::CanPacket8{ +// device::CanPacket8::PaddingQuarter{}, +// chassis_steering_motors_[3].generate_command(), +// chassis_steering_motors_[2].generate_command(), +// supercap_.generate_command(), +// } +// .as_bytes(), +// }); + +// builder.can3_transmit({ +// .can_id = 0x200, +// .can_data = +// device::CanPacket8{ +// device::CanPacket8::PaddingQuarter{}, +// chassis_back_climber_motor_[1].generate_command(), +// yaw_brake_motor_.generate_command(), +// chassis_back_climber_motor_[0].generate_command(), +// } +// .as_bytes(), +// }); + +// builder.can3_transmit({ +// .can_id = 0x141, +// .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), +// }); + +// builder.can2_transmit({ +// .can_id = 0x200, +// .can_data = +// device::CanPacket8{ +// chassis_front_climber_motor_[0].generate_command(), +// device::CanPacket8::PaddingQuarter{}, +// chassis_front_climber_motor_[1].generate_command(), +// device::CanPacket8::PaddingQuarter{}, +// } +// .as_bytes(), +// }); +// } + +// private: +// void can0_receive_callback(const librmcs::data::CanDataView& data) override { +// if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] +// return; +// auto can_id = data.can_id; +// // can0_receive_rate_counter_.record(can_id); +// check[can_id - 0x201] = 1; +// if (can_id == 0x201) { +// chassis_wheel_motors_[0].store_status(data.can_data); +// } else if (can_id == 0x202) { +// chassis_wheel_motors_[1].store_status(data.can_data); +// } else if (can_id == 0x205) { +// chassis_steering_motors_[1].store_status(data.can_data); +// } else if (can_id == 0x208) { +// chassis_steering_motors_[0].store_status(data.can_data); +// } +// } + +// void can1_receive_callback(const librmcs::data::CanDataView& data) override { +// if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] +// return; +// auto can_id = data.can_id; +// // can1_receive_rate_counter_.record(can_id); +// if (can_id != 0x300) { +// check[can_id - 0x201] = 1; +// } +// if (can_id == 0x203) { +// chassis_wheel_motors_[2].store_status(data.can_data); +// } else if (can_id == 0x204) { +// chassis_wheel_motors_[3].store_status(data.can_data); +// } else if (can_id == 0x207) { +// chassis_steering_motors_[2].store_status(data.can_data); +// } else if (can_id == 0x206) { +// chassis_steering_motors_[3].store_status(data.can_data); +// } else if (can_id == 0x300) { +// supercap_.store_status(data.can_data); +// } +// } + +// void can2_receive_callback(const librmcs::data::CanDataView& data) override { +// if (data.is_extended_can_id || data.is_remote_transmission) +// return; +// auto can_id = data.can_id; +// // can2_receive_rate_counter_.record(can_id); +// if (can_id == 0x201) { +// chassis_front_climber_motor_[0].store_status(data.can_data); +// } else if (can_id == 0x203) { +// chassis_front_climber_motor_[1].store_status(data.can_data); +// } +// } + +// void can3_receive_callback(const librmcs::data::CanDataView& data) override { +// if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] +// return; +// auto can_id = data.can_id; +// // can3_receive_rate_counter_.record(can_id); +// if (can_id == 0x202) { +// chassis_back_climber_motor_[1].store_status(data.can_data); +// } else if (can_id == 0x204) { +// chassis_back_climber_motor_[0].store_status(data.can_data); +// } else if (can_id == 0x203) { +// yaw_brake_motor_.store_status(data.can_data); +// } else if (can_id == 0x141) { +// gimbal_bottom_yaw_motor_.store_status(data.can_data); +// } +// } + +// void uart0_receive_callback(const librmcs::data::UartDataView& data) override { +// const std::byte* ptr = data.uart_data.data(); +// referee_ring_buffer_receive_.emplace_back_n( +// [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, +// data.uart_data.size()); +// } + +// void dbus_receive_callback(const librmcs::data::UartDataView& data) override { +// dr16_.store_status(data.uart_data.data(), data.uart_data.size()); +// } + +// void accelerometer_receive_callback( +// const librmcs::data::AccelerometerDataView& data) override { +// imu_.store_accelerometer_status(data.x, data.y, data.z); +// } + +// void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { +// imu_.store_gyroscope_status(data.x, data.y, data.z); +// } + +// rclcpp::Logger logger_; +// // CanReceiveRateCounter can0_receive_rate_counter_; +// // CanReceiveRateCounter can1_receive_rate_counter_; +// // CanReceiveRateCounter can2_receive_rate_counter_; +// // CanReceiveRateCounter can3_receive_rate_counter_; + +// int count_ = 0; +// int check[10] = {0}; + +// device::Bmi088 imu_; +// device::Dr16 dr16_; +// device::Supercap supercap_; + +// device::DjiMotor chassis_steering_motors_[4]; +// device::DjiMotor chassis_wheel_motors_[4]; +// device::DjiMotor chassis_front_climber_motor_[2]; +// device::DjiMotor chassis_back_climber_motor_[2]; +// device::DjiMotor yaw_brake_motor_; +// device::LkMotor gimbal_bottom_yaw_motor_; + +// rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; + +// OutputInterface referee_serial_; +// OutputInterface powermeter_control_enabled_; +// OutputInterface powermeter_charge_power_limit_; +// OutputInterface chassis_yaw_velocity_imu_; +// OutputInterface chassis_pitch_imu_; +// }; + +// OutputInterface tf_; + +// rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; + +// std::shared_ptr top_board_; +// std::shared_ptr bottom_board_; +// }; + +// } // namespace rmcs_core::hardware + +// #include + +// PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringHeroLittle, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp index 2ab4bd450..d27338afd 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp @@ -42,10 +42,6 @@ class Hero , bottom_yaw_angle_number_( Shape::Color::YELLOW, 20, 5, x_center + 270, y_center - 65, 0.0, false) , time_reminder_(Shape::Color::PINK, 50, 5, x_center + 150, y_center + 65, 0, false) - // , bullet_allowance_label_( - // Shape::Color::YELLOW, 18, 3, x_center - 300, y_center + 270, "bullet", false) - // , bullet_allowance_number_( - // Shape::Color::YELLOW, 20, 5, x_center - 170, y_center + 270, 0, false) , bullet_allowance_number_( Shape::Color::YELLOW, 20, 5, x_center - 220, y_center + 270, 0, false) , friction_profile_number_( @@ -97,9 +93,6 @@ class Hero register_input("/gimbal/bottom_yaw/raw_angle", bottom_yaw_raw_angle_); // register_input("/gimbal/auto_aim/laser_distance", laser_distance_); - register_input("/gimbal/shooter/condiction", shoot_condiction_); - - register_input("/gimbal/shooter/mode", shoot_mode_); register_input("/gimbal/shooter/preloaded_ready", shooter_preloaded_ready_, false); // register_input("/gimbal/scope/active", is_scope_active_); @@ -107,16 +100,11 @@ class Hero register_input("/remote/mouse", mouse_); register_input("/referee/game/stage", game_stage_); - - // register_input("/gimbal/auto_aim/fire_control", auto_aim_fire_control_, false); - // register_input("/gimbal/auto_aim/target_confidence", auto_aim_target_confidence_, false); } void update() override { update_normal_ui(); - // update_bullet_allowance(); // update_sniper_ui(); - // update_state_word(); // if (*is_scope_active_) { // set_normal_ui_visible(false); @@ -146,7 +134,6 @@ class Hero yaw_angle_number_.set_visible(value); pitch_angle_number_.set_visible(value); bottom_yaw_angle_number_.set_visible(value); - // bullet_allowance_label_.set_visible(value); bullet_allowance_number_.set_visible(value); friction_profile_number_.set_visible(value); // center_green_line_.set_visible(value); @@ -227,12 +214,6 @@ class Hero *left_friction_control_velocity_ > 0); status_ring_.update_supercap(*supercap_voltage_, true); status_ring_.update_battery_power(*chassis_voltage_); - // const bool auto_aim_locked = auto_aim_fire_control_.ready() && *auto_aim_fire_control_; - // const double target_confidence_value = - // auto_aim_target_confidence_.ready() ? *auto_aim_target_confidence_ : 0.0; - - // status_ring_.update_auto_aim_feedback(auto_aim_locked, target_confidence_value); - // update_static_status_ring(); last_keyboard_ = *keyboard_; } @@ -323,50 +304,6 @@ class Hero return; } - void update_static_status_ring() { - auto auto_aim_enable = mouse_->right == 1; - auto precise_enable = *shoot_mode_ == rmcs_msgs::ShootMode::PRECISE; - - status_ring_.update_static_parts({auto_aim_enable, precise_enable}); - } - - // void update_bullet_allowance() { - - // std::string text = "BULLET : " + std::to_string(max(0,*robot_bullet_allowance_)); - // char* allow = text.data(); - // auto color = Shape::Color::YELLOW; - - // bullet_allowance_number_.set_value(allow); - // bullet_allowance_number_.set_font_size(14); - // bullet_allowance_number_.set_color(color); - // bullet_allowance_number_.set_visible(true); - // bullet_allowance_number_.set_xy(x_center - 240, y_center + 288); - // } - - void update_state_word() { - - const char* text = "OK"; - auto color = Shape::Color::GREEN; - - if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::FRICTION_WAITING) { - text = " WAITING "; - } else if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::SHOOT) { - text = " SHOOT "; - } else if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::FIRED) { - text = " FIRED "; - } else if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::JAM) { - text = " JAM "; - } else if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::PRELOADING) { - text = "PRELOADING"; - } - - state_word_.set_value(text); - state_word_.set_font_size(30); - state_word_.set_color(color); - state_word_.set_visible(true); - state_word_.set_xy(x_center - 800, y_center + 200); - } - void update_chassis_direction_indicator() { auto chassis_mode = *chassis_mode_; @@ -499,8 +436,6 @@ class Hero InputInterface bottom_yaw_angle_; // InputInterface laser_distance_; - InputInterface shoot_mode_; - InputInterface shoot_condiction_; InputInterface shooter_preloaded_ready_; // InputInterface is_scope_active_; @@ -518,7 +453,6 @@ class Hero Text state_word_; Integer time_reminder_; - // Text bullet_allowance_label_; Integer bullet_allowance_number_; Integer friction_profile_number_; Line friction_profile_indicator_[4]; @@ -527,9 +461,6 @@ class Hero bool bottom_yaw_tracking_enabled_ = false; double bottom_yaw_anchor_angle_rad_ = 0.0; - - // InputInterface auto_aim_fire_control_; - // InputInterface auto_aim_target_confidence_; }; } // namespace rmcs_core::referee::app::ui From 5084c0111fd1fff8c244a1af0a0af43cedff3325 Mon Sep 17 00:00:00 2001 From: dwx5 <1591215786@qq.com> Date: Thu, 2 Jul 2026 22:10:14 +0800 Subject: [PATCH 25/86] =?UTF-8?q?1=E3=80=81lk=5Fmotor:=20multiple=20angle?= =?UTF-8?q?=20dynamically=20transform=202=E3=80=81interfere=20clean=203?= =?UTF-8?q?=E3=80=81six=5Ffriction:=20rename=20and=20reset=20parameter=204?= =?UTF-8?q?=E3=80=81putter=20return?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- librmcs-firmware-rmcs_board-lite-3.2.0.dfu | Bin 127612 -> 0 bytes .../steering-hero-little-six-friction.yaml | 154 +++++++++--------- .../gimbal/hero_gimbal_controller.cpp | 6 +- .../hero_friction_wheel_controller.cpp | 4 +- .../controller/shooting/putter_controller.cpp | 26 +-- .../steering-hero-little-six--friction.cpp | 27 ++- .../src/rmcs_core/src/referee/app/ui/hero.cpp | 16 +- 7 files changed, 119 insertions(+), 114 deletions(-) delete mode 100644 librmcs-firmware-rmcs_board-lite-3.2.0.dfu diff --git a/librmcs-firmware-rmcs_board-lite-3.2.0.dfu b/librmcs-firmware-rmcs_board-lite-3.2.0.dfu deleted file mode 100644 index d0efc11e5e286ee649e90efbb3d8d9fe0ef9b9d3..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 127612 zcmeFa3s_X;)&TtO*|YZ^P+)L#)6_<`B619oDW|Mx2AJ8XrDP~p)-iX2ol9w^T}~#< zW@bR;qFWfOER+=TD>Wt@(*w$FhSU^qU}fG$!8#qm3!t4N{OjEVdCAUq&i_5nfBx?| z-{Z5Hz2ECv>s@QT>s{}9Uz3ub@U;ih8B%7=)=)E@!76GWU!+DcYv*3spom$djzRWV zCPOlxjStCe-`4bp_HF9lw{KhU$M$U{liIiGAA?_`q(Ep$9%cTewc`?Y%j1|>WsJTyGiKQ7;2 z@7CXpq5pcFTkHSY`$NW1Ns*C`$RH=<*Flys;|+ndiw)5c!qYUeq;cgfG4=vQMq5V7 zG98oFH4ZbqN8Phyasi|p>p%@|jqr;G-|+Zq=@^tD^}J=OQdeecuvCSLICd-JqG zIS1}vtb%g#o9EBoU!f+n6z7zlHUx&a0vFPILTZQ94|hd0M(vH>AA4Xz)5ODZN8_7K z$0vU?^~Cg(&3nG7y2u=8!Resz3!LPbJt;&kI#p<4u5jft@tPZ$KhXEbQAx||3LkhV7rNb)qfWLS9*I{()0RF z{15#H;xEj7Xk(C~aRh%w$Hc@n z*78y|q%S{?uIDc*VPYg}%^J~4BgNiDkPmg%Ox#Pl-Nf`HVIbpQJcVU!N_L43`O^#E z(+2N}+!|4)cQm{7Qdd{N^&g6-NVMkKU=xR0Q1+BeV|+bDO{}%6u{Au$Fo?(P9pzao zQMkZ8%p@J#PQ>hEJ87bTuUcM1ucpJ39KqWnClm=J74>*+WWqX=MDf%FYrJb>ZFIeT z4{k?u&mcbBlxd1g$TWIWG8k{mq>(}QC+wfn*i0Hj5&k<`{;QM!^77wQ{FhHh$D3K3 zp=e;8#l+fH6$KzQiD8aS;_?{8zLAXTgO z+>dE9H%S=6sQb%7dcI8K1MB%KPN4At(w#(H<`>tIWh&xU`Z;9!Ie(_#_h)elpx{q_ zG6l(vDk9SucMAy~X z#}fKZI-)CscqfHgy>ygphE;b^lGg#r9*~vsngx-;3-uey)=}ycK2#STadOGo{(lFO@;3_E}%E_a`$a5qtx2oHY5xQ zAhf+Lg!Yw#Xx!HGgFQh3J&u+ZJsUh@4r@=H;|VPL$>C;HPUwlX9{9}>hkt9ycszPk0Ev%^z_a}bUjz_r-zJ-pwy0Ktw?zHAOXLGqkT8pI<9lD zXH7tlc4o7l4VyKG4V^j1qh|i5HPI>|n^8;GR4Rd?HO|*@zf7{QQWb%925V)KXHO5C zUc~IQ|2e_3;_Egs(xo6HY0^3RUZi7?TCy=6- z27bwD7$T4n1~}1;5L{y3bxsosDW1X39^GQ-kD^i}3aMgU_HDBFp*hy{y=m6;q0>X4 zR~?3kVp7W8SL=2pu<4;_N1Y6%XiFp`MI=VnA!;5#(hBYT6v*gk)aszik;6$7F8hS+ z>g=Hj1LK1dJnLF|gw{~Pv-(<(r-SO{Mv?>U$#1P!3P=W%zdEWutKBkCF ziT6}P??#D3SAM1hX>jL0<@k!$jZNZt`CVTe_X$LzeYG*o-6 z6^}?sC{D?7wk{35;l#N2_X%*Q@7nKb=0bnJ|0*wdEay+(L4$$Llb8l~?P(oz`sbeh zHCkt$|K^T6ic0FE{5j&;N4$0{b5bKYv`vMFgt3kx#0xgvTH*{7E&Zc|rZbsmpGP;n&4wlsPl`g4%J)-~ z;M1b&vBXi_`%ziwZ))ryd7B^^Yk@wo^f)<)S1;YqfRX=p7y(#y)jYa zmyiEl_+Id;FXz%8a6J$)!8##rv}<%kq&0GGTy0$VW9G-6d;xpYoi0U&WVQZvf~~?D zZQX1!S14H&N+51+b7i!HUi^&N&N9!v1 z_-Tiy&P{lAYL2tNOq}b{`_DUp=7G=b(XCLS@cok;r@E%s(+48<&a7nzOvxc8>9PiP zP?IGeq>z{~sEFBM&q^4Yq7BuIF_^CBXS&0VzF=Cv~?MOKKa9 z!j7k;IDX;>$lpKkXx34wdnC$fm%5juRr|!z1BkV5GGYMQYtdN-c9`u_x01lQz68#l zBIi!IB{xy78zt9WA?L36Id^^bsvo7FCL>{mFA_)ZlXLg|oJ;utE((wOq0YjqeJ{@Z+&u%C4W?^A06LynopkT$dnX+3F38`g=WSq$MwWl61( zQAsmZgmw|U)18WV($SiRgljIevq-q=Lc-r#6*odx-Q6szFgpYv&(F z!W-piVzKaAITBvI;J-AamN}FkXEA4758YXG$vL7{3Zdr|UUXO+^%kStV++)V9$Dy7 zZi(!KmJa0!*V5$vpy$7}s&0fjtZDi;oi|Ld^QkDz*M>GDZ8P9dd>lDaJCUO;l#pAS z0j-4rUKodFhR`m7Z?aMme|w962MmI7ix>yIy1R+Pv3%iL8QN(Vu9iX1Z3r-+9M&@Z zq1>F^*MoN!eeZlTWT(B;iSem~cgiW8Ij&}l={dqs6>{hrkmL+SjvXNchAGn+=wtz6 zEyEZbGg^_Oh$fEZvL`9R(R=|pT3n=~7$}{39%<7<2}dKP(y}zr0n+oXDOe~i&148n zjb>sCP`{ZFu5Op9(QME*yGSjMVTaP$jX<>-`NA8aM6_LaEtC)zIcYETrDH$`SXFA7 zP=h0hRIyKygw`MM&JHG}nf`>9PE$B6^Ax6{3g{t(Fr-x!vduIRT_pU$jbK!#`m40T z6mhx8^X9S3o>2GY8=<2B|1CwI?tU{Q+P-jirZJNrUq#hM*(@d@y%OD+8Eda7oM7Kx zD7q=2AcwlqAAd4uY_M zcw;!;6CV*-q2FwC=sJ*t+NRf1CHi%_2Ev@-w2>J$r>znHGEU~pBd>Q3I#L)~d&Cxe zcwcwh^rzt6Tk0tbb@&Jqd>EHXSbY91bH+mpqdp2h8F3;!u_E!xXbqB%B@%3LJ(4;* zmr1FOu<8y*;Z1Re70rsH?CWjIq~lkK@%t__(#%8zWBLX=OGO;ZXG7`jzpt;8?93ci3?-h3f-@MAvDk=ya*{JGzv^-8TX!H`J;A=(G1uC;7=jlgyVykk>l`X z!g29=D0PHb7+MA6E>NVxNdeziIuV$vX0@8NhoA`u;u_$kCFXqZD$lX58>w+NmOQY$nIEH|;5akMc68)ia|c{|V`W}*4%JjVmukhZM>5lMte zTKqaXb4eiSFm_28qCuZFqK%s+X3yV+saO1CQzfQ}blB4M?7g}%(#cdk+ucpP)yx)0 zBZvEdCetWJ!syYC^TCAYJ$KLG_E(F^(crVNUF^j=X=0NPFGv^BTZb)-U%8g{C)DvQ z3A%~&=(Qnf=uoboNlP~yGN%cHyQo#X*5n$YO>IXGTZB%aw95`tMj~pWC!Q-I$R?kI_3$uWe zSi%ypih0wyIyHP|#Vl=`Th@e6%jy8 zC2&(vCtc}U3UlS@WrZaZvJdsJI$K|VRMMMHD(TDSn<*2rD|g>WjThr=!7#?TMgdhS zJj1Ee(&BEkD#?=*e8xe6?%#19x$Mz4pGr>weOjVy2)sVPUtjjtoMh+ELOX3=IDifS z*;)tIh)yQ5Lm1Ghbn@`)--(`oUl{``^xkg+@k&9#!oC`A9i=3S>nOQKMlk-O`fAKk;TrfG*I4kf zx5f^rK@&Ub{Nx&Q2H`Kiz8ZQ8*TCPnM)L2yHMsJ&5FG`6zE9dh0_9pL0DlMe)jCb! zTKF5+>PYwEt?Uqd0dJf9i=1w%L!deVqMP{o@Z3jjK~9xQcPeVDV*LFJBq_YQJO?X@;TBvg*EB}1gPy>-Ki8ts zIm%Sv(v3prg5)-EeW}%if@;J~ZeJZe5J|A7cIjQIH39);eYvqr5Er#w4Ub0L#1HV9 zucJbUzWmLQKN6sfq5~(v*uw9K_5$@nm~DY#A;ct^3fm@QSc|7QY2{U;Y2kat2aBiS zzXjiLn`s4bFN}a69!(g7w$tVe9h6R)hN%qW0X?N0Xe=rA1H8oBi$3Q*njjD9XIb#NE?^;_|AkgekLu zj4+!0^2k!Rf!t^_I37$x(vC31J{Sx>vV-vR9#f^-)qL3?bnz>KX^k+(5Gl*{5Agp1+#WJh>J9G+_ zR#8N5qRq*tV(LWDour9I8^D4lH`xqp&0J86A}(7{&_jW?dTtA&7xL*4jDscZk$rFP zt1c2Z(&iC>gBx&=G1z3f3kK4JU<7tIA-qqAJ7~b*V%iWHgMp>tw_~vNJ1+)gBtyj} z)Y?HB%oRmkNsE~qJ6}J(JmqN_hYy%A8HXY!Z0MBrC1fN;iM+}n6fncZ(b)#!T_$Mg z6iZrd5m`=~#hEq*7r{_45=77u5ik?XfJGlhGs;O9V-SMrAWRDpXpzzq^eyS45j)eA z#3|eg5x;lQ3J*%}PAeFB?f@$-EWc*QS$Q}59<;DNCgkO$e zP9^3g%t_=%-XI!zf~DQWP{YL96TI0zh_{%M_>Xx`*@DX;?w)Qm_^ZqMp$6bc8=L?a*{*FRJVl!kiic3aY_WzG_z$7mQqml^ZV zTBpx}evIR5+dH;ahx3Dy$Y>jx@~kC&yWH+&J@*N1=xtNrRA}bUWOVpmd2~Gu2;VHB)|{)g^3`s)*Zrx70X%| zYZ}t!R(yVB&5@=fGTt^?1M{pNBu`(3=}pP>4q<|1dJkrTZlQNM(0emOi88&z5N>G$ z{Z$_9UM@R&aIg!f_tTdIn(nj;1U;y~bzE9t;T!W9a2GiW^hpEg6RVpDGsM+~@e2ED z&?hgI=z`!n4@)89$A!A{H(o?OsF zZ;STfY`-*o|v^|C2;B@!vb-+Ge^L$}7M0wk6U(LR-oi zyCKE|G{+YX@9%R}z)7GX3ahBJiC!H!s$WMQs^X&EFkfFgvZH09JYSFT%A0qt^Ul_H zaL&)Ob(o5lb zeF1-UR`=Hf&3^jcGaImGFLjf}zx@I{V_r>zbC#WwbGF#%t51EryFUl_b?Upmz9{I6 z%5~nUmp}cyUoRJ8y?nkc=@z}5N1JZf%U{U$+ba?Q-X znqJ*(&bU=OQzkgG0lZP%a=gVmPOFUEdrkx^`WO*xAQ3b$_)Q;ZLaZwoy_C15Xz*D< zq@yss1Uo$u%cL-b9!sUg+R4$3IeSX+k>f{5ozl?!(2T1N_c)%v+qR+#eaG|c;t!0iO%tv1BMvmsqQQ1Nkb^pdBE$!3` zUp0nMwJ_ty$GKwb+fVnpDnQ?dn=T~ak=Q^J`U`P`Z|n&gcz13ZApq*0JF18tz< znJ4nGCr2D*HJk;{<+T=}m>No@$I7}=9OW`>k@Y1x%4Co%Haf08%)mv{`mj`m9cH9o z$iCo91@%jRiBV#m3!JL-a%uUI3rG67)H{=Sx%8_VFOPb@Y&`4nr<-qb>4)!oxwQN# zvYe;AT>9~@!LU+fXlS4AYxmTJQ~m9Fb+otLj~fbQ-uE16JnT`>hi|rfuf4C`-;(8S z#@nus1LuEQiT~Ef@8fsjIWp5!xr;Cz`wVcQGfkFVV97$-Mh8eA*rOK5oqFoy>V7@7 zuC9s~erTI>yJotnUmAR{X8M8`F0?)G)l63K^#8ieczew|t;XyrrFk$v<}=7A3ABcR zO3!X-Y;n-%kqGNFudTQ_dviYMgvZG(CNs%d3=aQlJ-aCyu@geTR%FqoJg^iawD~V1 z$|NjnohJB`zhSB1_&CrQr$jts`6OkhM#-`Z_M2?yP=;Y2k)tUeS-Ta}$$0#QB6=*g z;;?oDPmBsy_A0K3QHiEd&=w*@-LiHo5+kgJC-S+XHWg?`6XqkV1+ess4J}}cfJLH; ztDiNcUlTlZn-w6asgeEjf4@d}qu-WHlA$Q1@9&4=+P6DS;oe1v@WpYQq)7)>WG#j5 zrnxlM*k9%y%A-tITexgDEm)|ypY0{tK57B`$VP*GR3Fv{k7f6RPs%vI2XJGY2h&6Q zH9F=E;1lJJeb@GpCg`o4A;`Kmf?Cey!d!||qO%RsjF6<#T&EJ#LwX|umFA@@+2&CY zQS2?VDksqFx17s|FMwK{8j9FZ3;0c22=4thr4ZmB!;MwJ458Bd@j^hW8Z0{^kQeCI z3D`I6I6n^wYbZ52Dhcd68GZ`GKWmzop2n&NjWYbJ=n0rwEZ9yttunp9_HH`0t&UsG zfIbXCYhc7^(2(RNdAwG)Dd&OpfGrHhEN*mR3kB>4awCl0mYZY8gEs6RyH%B{8CVbA z_ZO@MMWDuDH3PupGY+#Rd z5?p>68sXwT0qnP^^kYoIE6qp;-WrxRF?AZl+=lSo}toA zMI>Gwmp2;XLBDM>FLr|6q^D%-WLOo}Y7gx49H4$8H+m@vnm`d|uq@}YTNGiiLT1SR z**LD=OWReuz;@np9A;*S?_;>Iy1pR}uohL{y!^^n z6%eZ~g0|c76xPP#Mr`Z3K^wzb?4xUF%xB+)NZUh@tO2uHyjl=6ljl3?DEO1SeQtq1 zOG16TeR?$_NPQT#P5ex`d7i1w`&aFqVDi-;2tR4Lt^tcUwN^D*2nDu%>g*M zl{@$coSrYK)}LD`5B1?T_d_~fwy(de`MDLod1vKb8A@p7;4W_~yboE%_#CSw$5{*> zI^bKZ%cyj2?0G|!9r#e>KI%osK9{Xqums5HM#HNxs%7RgU;J`TG*ti6(jNaAVS2jK zWApzBbLf!5IM-Xyd%L3ov|sqN>{M~9TLD%^G;e;%d8cfJ^~u(%YTyy&3l3OcsU|t_ zj8VA@c!XZJE7SB+)txg&%}z0uB8b57{qzCsvy93bQwMQ9KXfTs8;)!~9m&3)N(HtF zVoOBQbF&R*yO=t%-*-9qs_eT=#JwFf4l=}Gv*E-7gKBM$ zuHXioL!~N}ULcv_r2~|x3M|r=$c-l9TqQp5RGB5r6UN;(ncg8Ah_tnrSS(u2Jtvu3a7xv;A! z>_kw)PMct*gY@n(yD{bCUED~eVyNLns@Wc!BzvaHfNwz`%6hS+&1~U0)ipgCSr712 ze5vXJXv+&&Dm6-y3s;ihBP^5W#;|&Xxo&WsaBoY91-MD2ueHmb$C24+aplmbF)e@& z21B<@B{dV%Po<$pXCEcqFtfNAh~>$aEz3ZWeZ4yX(~FE~#2(2(*dK}K;aZ!8!mK*ckyBY}!c%TS4K>&2&IhM7EXH2?5W`6H9&Cy{O&nr$eB7R`2y z56Qnq)Y^i(u{SWz6=s+P_A=(sIGCG(23x?BfPI`WLsLAL-KL+N7?*vc>Jj7o#OHQD zVwexs+tBBdX8+MCrj8;oZWe|c+zC|rzG}V!^TV6Fa=?lRGk|180GWOTms9m2J@aJM?Tsza0?_c7*3$`bUTV7Hmf>Z64+!ga67>Ve#!v%X){- zqS7%27*fWZlhkKNd=hFfR*DhXhU;f{+Fo)j4UwQcOR3`HUF}W1*J6`M{zU@Q7gO{` zC^z;R;Pl6;DwF5lmW$a5nCEml12$RtA_Ih zzVmAU-fKYf8-e#a^54Ca_cGhOE5|pIzL2@^!A8t|o|SZ~-o5Y(5oT{s&hS$hf1uDO zEry-IE$Ogo%12dSV0vPYj}H_(tMqUa0D2sI2r^y?bzfZ(ix6vGmdTSo&HIz5Ug%OW*uGSo+5O_0GQ5 zdhlMz8+!06gY-SPNA4+$J^}bwW9&vaJfwez#K6o|xCF-N)ERkX$0d?+c9P@G6;r>f z#GX~k>sf`!qcPQn!0`GEgN_Fs0gps0>4@4jHMC2z*+$Fb^>t0!>^*~tT`UX4f(uf>V#$#y-x8VJ*T-Z2i3!}3oo9bcuBtG9FWvd=`lvQdVmS}`-5t$*nZG`lp5#Nt6+o8BZkXy}2 zu(qkh(s)FcDL~h#2&v}^lI&BFbi_gIv};2bM1qwKc0yascYDRMc!c-biu^wn<+!J| z77gB0```n|fr}ava$#lyjpq+>HdlyXk!)x{ek(4$3#<5&i^wrbH6ptXY{(J0JeNm- z-;DxKAv_Vl-m;8%;gt-vprh_1qW81ZjO0vX?16Xo zhBOX$EvbL2wmm~L7U$ud_6$Vp@EH+CO%9s2VJh4S5r25v+@x1cl8Sj)_^C|owEg8t z$=3a(L)WPQw*|dQx+@&=&`MV( zuk#NLeZi=OJ-^4@TqF|?HieIc@f4T!#2$#>e-kdS5oxZR_JmV zDW^4JStEix4V2U9MnAX>Y-0_}ygp|&Q(XJk{P$hBzsJ=&%lHn(VZ~8Jb9=^uM=^{5 zTY!usU?jCMDz3cCUo1008ksR@nq|eUShFNQJJSs(Pcr^~QfC|A;iO>%c-RfizJ@w? zQS`zpe3}HLDJJS?BSfL||%wrNC{jj-@! z5{-o7E(L~bHN`Ug>}dP!gs=pu*-2Ebs9_)qCZs7c&WY}E2atn=gqF;yfdKY2~dGtrKBWzD3*lQz$4Mhy62_6sp<4)H=$x@J#ncGpYnO#)Z z>$hB+mWCWITBReER}1PKZD|^?^kuFV7?pwx4-Rx}1bI4tUIXtVzz(=8ADc6vr@*gw z8{ib05y_I+-5s#jt*BSH!i^P1tW_kY0l|s|NwftO#|s}`0PYJ#<2KQ1A*2?ZqvP^m=r-s;)-3u4e zEmIFFem*^Vd6LLbNCHfRl}tqVv4|s+4^97S=DugXeBM2`A+7FrH5X6%@KISpVOs=H z26ZCxHx|Y08RQs>6n|qREiZ0fBi=_T$oq^6@o@{84I1D@3skg3nY{rGEkDQ zQUNg)&k6TX!&kyCGjP4W$jke2_njP=X~kc-ztevZf+@Adh~Lo*!o8F~ev9!DhEV>N7dNco zmahQ|LrHru@E1J&aGk`e3ynzor#ad#4wsoyUpy~$GL=iyyrg2jm)G?miB^}D9j06`n1I6 z#8E&s9oPwAU-7@_f3LTHoG zZ0$}jY-8I0Cy>o(^}=PFERT=+zRsIH-;L_?AhzI_V{yLfZmHkzx*KHcD<9XZRP9ae zpY%}#ID;^?7c5XceWk%%cRJt7jX9Oe#bNI`N@}o2}qmOr=kfG>!byKs%>m4-=Cu zHjY-1jl4o_Al>N-@?fZfj0Ugp0Uhk>rz&)?YTBR*SA{by-MSDWtqC}HKtRMOd%&Ni zjR>~}zQF)uFW5IqkaBc68OK@U`I_7c^EG8b1qR{XgCIj8XxtVGX50g4>z2+i&%6FT z1rXSf<~2T7rM=rW$M^I*pQ;Ny5P+8;^z)q5nd8VQ##4+uZ8b)3IiC_F`s z8~IMy;%maXQv(h$b5nZko9)u6Y{dRH8%fXG(9)^w3=v5)NP?Xy5~UfT1ee)Olx8vn zXKo`(&25MiTZmFI9b)0#1eaBYAcg^P7B@O!x*dxN2PW>1-)m|d;`-F6_#on-UE1rvKlJt`0^wRUX~f zVRT`v<}1|sS~cEQq}WfXF$Ffsw;kRPqk(J%{zi0HdK zsoDBcjag#oRIF`9-lC}5C#D(|LUs#!{0RrMLvInc*oS^48aFC7+j1Jj8S@l^<(OJ5 zw-LGXz#EYojXmwz4=u#9JU)v|;?|_uQgM=G*k@DhGeoumwzhjTG zv&~k$!+XErKt$8lIm9GrzT?Tv4o;9A!w^A)D}q{B&b6W~Y(>ZUrImA2 z9&$dkkUn`IUttq$=jria<(!Y#@wg;22iMa*h2P2ZWH7v^;`h<#$xsFbpcpvYbRImZ zCWxJYGgl$oBDJY8wmY9#5J|JTdOhw``;$A82+S*L?M znz<%gY2kLmHsj)gw3$3XDVB4@`~>vi%ECSTn&$J`3*MRG*1B4<9G$6&wtD z9U0<=Ao3FE7odMv*i`R~B)o|)`WPz1$-a7eS8?_T%-+(d2{p8Hw zS&XTm^)E*+xhkw4AJs+Y42&(fp8#9uO#Hry>c<>oim{gaS>Aj_f|*USHZPH^UCXp( zxGy(B(JXk6y?wdSSLK4gO7qXa&e9(1k zZP`T`BRDA&AOy7{?)|i^oZGN#C>MNGLC)tf#KzmKvsY5Z}rdp z!RYt(_mkXT!Q2Fy-)*X;-u}`N^k?$+*Oj%=@z3xCMDMFT8T64~@2Nq%Zoc#542Nan0??G2k5(&NxTv zY-9#GEdscnuhN#C*NA*55&{Flv5(Nl4QFrg8bw3$6OeEZ{xu!}V@ZA{Sc;R6uM;Qcu+Rd1GWV1~{MD>YYaXEuZHkm{@iJ{EUdkuSDUd9%@L>mQaN^*zK8Zw99L!1+ zaokw4ex}M0%RA-y8=t(=GN<)oxdj>CgMEgD;ds8{Stis+C^!!LSm09wjgR-T;Ov0^ z<>a|3hg0I8#`>D==8@-K<^+CYh(Dh4@`Go{g_H2D@C;^7nqj4HCp`8gt)i)n@JL1Q z>n1!O(&t5!6HdNGoDX`+Q`~ia)>EF~^m9JDF%M);BNFbb%r(7wAP#O72!Y!WqU+li zpkcAoYU4K;DISj)7yeuQ;i;Ghgsk&Q*nP7cQ#fgK?szTsaTLd6+Og@QQE~0kDrtKV z%tuZ7>+i0j7dvsO;=Q+)f)k3hFI2(Uog3)BHJyjrhDyLh=?P*EgMALsup;<6C6xvC z&|uYGP^|zhh{lzwU{&IP)yZ-2BKr5>JWp124=Z-|vWphUcGtT_%V}1mi6}`508e5x ziM06#4Gu*f5?;=y$BiSQg@eRa?kTW%Nbn>c>^(&PRJ6HBYzTo>Mk`{2m%!?c8u)cS ztkIY+^Gh?jkY@nh%FfJ&vz%bbq=F@rNy93|g*bC7Dm8bZwN-JTN#kL}H(1QYc=h2i zQ|AtYR*g*xH*L1ga7xy9kmtkgmw#>E^{zM`s)V(d;h}}#kza--dh&77&L9guYq8O~ z2XSeI>)6W4gPrVX(^6AY}+^<`qPk8CM8 zn9uciXJaWZEb4~)u9_Ybe&4OaXL@lxzkCP19ZhX^eBI{juuLFsP!Zo`u7&&HdMdJV za)o8x^o089N8@V2A9e3)p6yijXl*)!!*UMYpfcCy5SsJd})SF zFEJ4arITz5Ogu2)KyhhU}#+=@Ccj&X{{ z5Txrwy@Qwy)c3hx=dvvpJqPWTLc0|GrgMS|d}_6d;;8-62Y_2IRG8qL2Onj%zaR>6ZGiGA=F(AG%s;J>Tt35)koo*q`!-OIl526zOrQ7!feWW!pa z7%czC#7LOi^M)I=`8$yCLiNCL(O7ne6I;h!80>i$`|yW@5C1aw@I{`27z?SJ2@qbk zE52LFp+Y2Gy{cgCFCnQ-sen_dEjEC?6JS4&rdqdKd?>&__tufE*(E}Bt*#sOu2yb< zJFyz_rKA0I3K2yacTcEQ*rOVw_j2e;Pv`4sd!b{ zUXZ*bDvM1Fg13Stj&zH6AESU7OBf;V{)fIIKYkoerTOT^kv>qS=~Y&FzE|A1GFy9p z2GS}s^z3WObF5OS(JluK@pR$mEuJ7r;j}K0p1xd_Dz4%5#F}1!| zSjU7*OjfVB#=Ba|S?Q=(sy11Ll}a_sjMPY!QnR*1Y++DNKGs`;okP3}a5o3r1$#XfhH#9wY7X`G z+pUsc*57>~0)VypWoUgjq0)fQdmP7GeZ^5(Jq~6Faf1PL^_lOni+y`wUzc!L%Jjm5 zmPm&!#1PwPa)B=}x!}oolL)wn^W=T?Jrzj~sugb4iCcGr;zm9Jx^h)+tc437M#Z|? zlbfc-g3P@v@78dU^?&Cg&i%bTc>(O5S5e|4kBli+#(T+a_SHfzAc5xHk`iAjNztFuHtix3J@2MkS~mHc)7 z*ZXFt2ru1G358|E&=ld^Rk-_P5;|)?kwBLDA#BkAUC9)7)^(reuqztLAvhK`#!|u#U@EPgMEae zHW+iTX#igA@BH{6hC^$$!+juK=y&#=b~VHTQ9dsyYy3R-OzNZ@InXj;n&H%i;xz|< zB6md|1Gd-+M`%an!uPgDbnD-A#1&#b^3HQ*aGQj(XWlMZV{L(`t0b1RAaXQ`gnKW_ zT5D?r%=d+@XzQpeVV&1;j)%I~BQf2*>sCl`mVGs|Lr(VK|{(y=Q1wkMMW6cg9K^GQn~Ly9JM(-t(81p8oc;`}*WhVDD6a z|HVJ||9*nt*R5c?Uq|d<8=AP**N62POk=M&_y|(5Ju`7FIobyW?$ZIpxPAADp>^Db zu{nO*xeWueE!7eOT76C1>09+QnRqY1HWijr+f z1>cTM6O@3t)6w0f2T01NfaI=n9bSqJJCG=B3A||XSay%d(iBb+0R;5boPpy_9@rbzHd4j&qB*m zXMfB1j^64b;X^8@sg~R{fFce&Uj+U`*o7USNJ+IYrTHyAczn1`1C-*XDN4#Hwaf%v zX_NORTVQXpJ-OpP@i+UC5A@Gv{GK33?T1;jecQ64U&r;?{}$I5&IY+LX1PCOTJp;6 zm}+LYfnUS-)ARovwivHF@r7O#Le6ONMv!NX@@z$Nc*^P& z#nYE3&7FLBa{QEvDdCSBVbzsVm*Wg_uc|6=y|BB$^;{)o{CfAB3!iBufosB#MjY-ndF4WW_{qrQwrCJ#49>w44gWf)SjF%6`lp{ z8K)oqJdEIdOrIHb6uN3YVLl0KttEqNUQsWYrPwc*-+y)zzT;0~reyYOcp;OC1#eMS z8+t`EXSl`0LLT_tk>UUs$RKVIgBIPdj0r#R>SX6zpkZyk$=Iij{TOf){a8WS4fReB z-kc>t4nzu|xRs#mRxFN&VJxw@Q z1}&mt&e!3jwXOTZpTF(X2D;)jt|2iM9KO+oQi=e+z{$x>Jg^}x?`Lg{dJK~&QT-w75!QOmb!e(~(lw0l@W!OTE^;!CGWkS>)3^|b|B<6qK+>m*qaP>17ZY$sS-l_E7WqTe+PYWN z9dC(zjr`X1w#c8MoKRB-aYW0R&sH>mjovx3l_yP?D11w>XbPmS=aW1^ze*p|f+ne{pnT0L0_fy>Y_nmg$jk?Ua{;fw$jU4!2bK#$YMI z7D#{G&3az3SN@*__D)a4cu2Oa#C|ENI@l*e>W<%8couT-y>0j|m7O_olM85Vc)+^| zzP?vKE1BBPXT@-zE{tTN&i zb;11{N+FV9jg?4*C6d2U5-Ix4&79&YQq?;cp7`dtaNYj2&~{>$!`6y)R(*%?Lf0%u zTPw<4A-vDbbTY>SHru}t{Y=8Vu9=QD-H774qjl}u+FLj0suE!==cu)+L?<5rurniH zwpO38JduK>5aueCS*HbZBW-{-V9<@dtKeK9l*}wSo6v16R@V3j3qDG(1>u{9IRCnoD4x!*Ow|*DI=5j!=5dngi=qiR3Zaw z*2K4yK_1TqGO!+_xgkm#PHND2bpU#6OIy!^WZh_V?*3#6BI9sse?;8-&x6zw@1MjY z*Ds6U6`$xV!>)2w4+hoCNT7f=q#CH14 zSlrX~-e}yxwXdkRaY62Kz1N0KqyXE5ay@HJ^xEpl>KM_jL_74@QV#;lRdkR1*YBI}2)_2U}z;`PbmZ!mUvb@E@Z zIvD`Btiky1$E%ZDarKVw&9Q}BvdaIW#AAyw#&OoLE>mKBS2G@#j@xc8zI}y~Trldk z6-u&t(A`%k|ICWaQ$#)Z&#h6wHYL1v=glz|`3Qw4JM{s|cq6D?-|>{^L*+%#9UIHT zKtf|;X5+{;(56rHeLtH6w%&D^ZDJJE<9LSPGHe7s@r$(xUTd>izNsukfQK50-o0OoCoU85B+O66CxxaD&6a~8J+%9Wh@%r6~7F>wu)JfUa; z3n&}<&E_;$v*o$1N%5@nVchG0^ZnWpb~7IlvUD&%mHY{(V~m{hKrdKEgUw;-X6bE; zIB5IcPg&|5>|!D{Sf3@2{g>pE$#`{}qE1p~Q0%}xRD`8HSu=qv`50N0t9(3bvwD{& zbb=j!vQBoywUf}v*U9O=JB~7=Q*scipty?^QU=Q z(=p^QHX?`Ryu#7A1Jz_Y8iI-Y@^D>k-gcx-YD5MzZY}l$t^Y-ZR%}38V*|lH?@wr_ zZb7d~TGFF%xYCe(>ukai*fo0@tbu0a$iARpgP%gUJ?Xd#?!MiDTt&EN>DYtlzFevB z4fH@>jrp^@2XeL4c?H`%6Wy1`7DJ!Vt~3L@k_`=gkg&%V10>~eQs5ZUCe+CgrOB{q zRnW^*P{%kAIT}?2zMWK?6h>%`6oFg)IM0yzK&~TclMIj43~;zY;HF(i68LujLg|>5 zsLH7>6dPdgvyN7A@g4a*-01Su(}COc@Dg0b#Uelr{WumI0~nTsJ~>X;wYSNP!Xka zdICP%0IjfZT!A`q#bg45NQhi>Ox!!W5%{*%PFO*&*2Ps7>Zc@1(NzWK|03c7;G}CnJw$Z{k#K_(#JW*<#1y5}RqTWMf7pBX@F=QmZ+KT% zs;U!164K-X6R}A_0vQFQHF0>4na-uVIw*)2h&rff4TNz<0Xcx4GiZ`ZrxP$F&?EsB zMUaHyRn+u>_HrQi?x+|*A&E1HV4DPWG~9B5A>sS&s%~zgGjnFn_nq%~&L7oXRr^wV z?Y;KeYp=ETT9~(J*T1gQfXBG<14A$=I=74mGLkVS4z>gLCxfJka}0U9R9$jy7H5RO&*nsMMIjg4w67jgeefz2Cd!v(kGYap*^6D^^FiXa-*cD4?B`U5^RXJI66%*4-wEFA z=qPq;D5S=B`Z>|Lg5nK@Br#N#8pmDaLY1n$KOr^tRdSJ(M+t=taK4`p4ZVsF4TaSB z&LB1ckg0NG3bzKcB1P+b#W~>`kFqJtwYNl9 z>u&#>=l(B!a}+x$rkt@hTY8Q@vYo!k<&{Ky%i#!8XDVi;`tLs_^XfF+!PD-L$*Bi7 z(;2JA0;Kyjxk!lf&sZ{fg*s!QUH9*$3bYyPHDqSy*VTM-e_ce=u7meAEkQl+)$w<* zjv#dpHA~w=k+&9!x&J{9&n&u6EC}My?-#4Eui1ucJFeGp?Fizx?N|QqrI}j0PMfLQ zu!lNAXR0}3nQtWbJO4a2zqK$hPw(co1$I#8$= z6Y37?KYn}%m5cWKDJ#->oa>7{m{4XMHO+&qBY6{W0UF5i+YJBi$D|yI@7F)f0<3GrDTngtpDo{El!g(rII%RCEDlzL0DYF3V!?uJIk z5IT9#3AY4d?m2aR*pgcfY5lazb`zCX(WH+^Fm@B)T(z56>EBJjE9xk1j)wT>=rpd! z{MKA{YY67(G;NNWCr?+rA(*2f>Kq+)*&O{)ouk*@E6Oi$Bksc-4Z$3p?VqC|m*(h8 zkVIC6V2)0^sM)E!q}ef>Bb21T-f3lE?^O21z+Z!w?wx)N+K0Xgz^d(wDXb^%z7p0& zsaM0Qc$2Pxn9A#~4e{<{KLM|*$GcM7?eVT0qZVuM(<67+ieHR!b*b1hteHQE)pu;|9C z`zNrc{7GDR=^wy_$z==G9usm0sjTGOC0R+?7w!Lae3*O{yw_h1@Bf?l;Gp>MO7KzO z!xj}Ej1dbi;ll>t!qS}@K4{or2qotsQ^E?Q(FcQ2caF+d{#q~ny;xB;`pRC4xbEs+ z`X^w;WiuuqOS&{)AWN8WEecV)>#xE3QxszQ_dlc%`@^q<)%^RbVg1`N;*HLqpb(c) z#A{Iq78C;UBSK`h|2<&EMinbIYgqA?m14!KDwPOe#iOM_PHz<`$8WDiISc{HahZL9+5Gz}WK!n3 z1y{?Y0yy%M+Fq<8f15PwS~zmz=gQA1XFKyO~Dirg-YdWFU<(pT2^P^@CZ zyU_Jo+_-ileRWI3o=UBrtm*#G`l`e>~{`Tt#&@@PI*lT%)_88MSz zUD->y(vSDjPuPq&{}G&cD}?r*Y5|~1WkVV4bjl{3{zqV))pa$bbML$o(xbQiIHW(0 z4_p61e6W86Y^YSR;RXNt5!BJRp|+!O>rQC=feq8$z=Q%VZKked8mID^!FL?H3=b%e z`S;U?i+C`cA9Z!_4jlAh2H*W78iU#s13%EJA6e|{V8EB*QDeT8;ESEA8M;`b(eIITt3^s1z4gqz|s z-33%1(R9(GCIx!+jZ`ZP&%;zB+yr|X*aA>(JNH>0^vv}%H{H8xdVB5dq*w5zk_7}F zwTyLUcOg7CR2ACXrFsHCI&9wsr8?XuzP8M!Z1LEfQ=$otsqV_Z*X&gKFNm9-!~45z zV%2P$vnB>7^;b(YP2Bv5O?k6bau=btEqckRyKuqq%NvN>4t)>Gr{LdJ+LOk2qg9%nUw?)Hd{MTkfc;5w=Vn}o4f0E`b>btNaR*eJiia{K9wD~n?eXHzN z(fGY2#y<;BwNVyv;;c+@yf>4^X#)(IDT*Ev!+r~3$ix|)OeH=eGX=2F_B&Y(7G-=< z2D_EZpgl(IH%%0ts={=(T1ZzWwPc8|J)0?3eQ$BHLl@XLEg6Q?(Kt2aNq6Npq_I1s zbTR(kOwqU`(^-SKFs3Ln1z&%}Yzkj!roC)1#gM_+6;UgqU8^59BfdEMq*=T%Hba~^ zkRfi{nl5hd#1}m2VpUrPyo9BTn{(2sf1{;~r`4xCgWgTtYBU4~OQ!C`F}%0+h{D74 zMp7p5oSYjR>FgUxkJ zrh2QHB(uf}k0#Oa+3f6Nx)j)&!wSM^dRk!4mfpTn4$+5(3Bwe*T!?GFH|TvU9+ovYO7LJL{i}oVG8^qF@BRV?GEH14_GMb2J(_-0N! zy@FOO?`IA@?>at!(=p}v9{dtAQHY+EbtDwlLbZR+sx7uQ?=D6(vEH5Vdqdnf|AO-% z{CBIHS33_D;tUCGB|Q%kTwKqoINBD1g-4r7O7|3q5XRuW~ z?ByNsu+^NQY_q~2yf@wO%P8VZ1x}^Lz;g|BduhO`qK-6RmA)lSd>z=dV_v$r4cN3j z8dwEvsuD7A3VWHJBq>e@ui5lJ4LvRF`kh6#ISTd2QON%v=V%MD79j%PUBhS4QD*=%eHN$}vwSFD{Y8xCn7m^JKQUkdb3MhqKKUOmHXLT*b%? z7xB>ILQhee(y)suw)|$bDZ5LRc;;ou1v)aMY_N>_WZC}o*5nUUTb^y2xOa40^uehf z;OjJcg7HgBL|WkdVuUsAMS5m&sz+BrPfR8qo_J*QT@BJF)~9Uw4@cfRe1=mm%?ZBi z-TEmwPqg_SdXkCyMRt}yg)a)B50zqUsjWQT%XvCo&Ao<0@B(rOUO?IvcnMerJyjJj z31^jbU9j>_(;wqyhnKq?eqU7M&=28X{8<2cUsUHHg`PP}p%3nTqE@TN_pdVcORIQ>(KXXkq<2mgkA?!zIXr((9%oMyQv$$0kP#BMeU$K|-ph z?^Vkp-QF>_(<%DNse2Wv3AX$8o-wPI7$Hf8WR==@>RQK0{(4HMwz6J{c`8%BL5OoM zFHj|})E=5zwZ>tMbU@xoJ=>go(G^uU{>>`4+12JT(=)1v`jE~W&t8xHU^~&<@+?!E zI1VS(a&K_mmx*(Nh0M#LnJ2tBM+j}=E@t=2F}8YsE#&9|7cZHj>&EXYhrhD;vgU0W zuUegUiAye=>Pchpojql056S3{b|&bTI-6y1?$NpFDtN$xAJ(UItooP|vJzcy{O z8fLqhTUwk;1!kRe`>He{Qi!xsI1wQj`>Zn0c=JZ+Co=(Odeg)P!1ZjTb-Y=%`JbHo z+uO{F1$I!V@y>R|GJ@>CgZ_^t^!FC^H&gw+PyPKhehrSaiU`1GaIC3N;e&N=Jp7?U z+v>-ND^M@`t_lzbf;La}bQD3Af6EE>m%DB=1A2oKL#z)A||M4A)+lw1m{@=&h) z$IOFKw9P6HHMZ6&CX|AQI&visHP)MHcn5r-pJ$@0ldCj5O>6&^{F2HmfiDsh`ZXS@ zTCh;Q=_)?S@=xTGwsmKU+jCU!vd=6w0Fx<=qZB6vG;T!3m`q*E5?Ht6th8h(f>tl7 zgMd#A>mZMoQu?F_dQdpkp8Z25QO|s$(jO=2&uY*gQ}q7vld8RDoVGV(1qol=&5qSW zBBjksYWJsyuGI<})RWuu4Xs~+mfYZA%Aan!UZVDQx2aZsh!90IkDHrqsx4lntmnty z*LZ1mTIo6X$wxEeFV4R9A=Pze&)i%t4$08kJ}H54c%5?eg>Y=XB$-ty_fbAFn{Az_!IXskJ;p4AxxuF zKQ@bxeWT7{N{{_>INif>Q=58n!~aNYIw-}fH2HaH(PcE5&S2w5Q&c(ydK|^YmNk#h zFa$~5tJckSP%4}g#t#3McqzUx&#KMjI%7Pg3LCYVy!Vc)=CMVk$rhC+TU45Cp){G5 zmScW{X1x-s(JYN7qpee}NwWep41MyWt5)#0SFc$=-A7-a`vINP@Wrb_^!L*__#f?= zY=&RMDOjzv-Z>fuPj&wQox2?;Ny^{fU;oDLAER@ni0E=@zjy_md;K}9vSlCkU)p|A znaJAI{o;;CtjeSe+doL>jQ85a_|sW*zxcm&PNQ?$I>cYGpD(qEo9Osh)&2YxK$|3e}&I(0iW&ZS@g>J`!Z{OlFv>L@YxaEjaTs5 zAEtUHw)7a^nhjq}0Y2L}Go$98#%JGi8J``1^QUPueAOKP0iTT(tY^^;)b9Bw`D|_d zQu*w-EBNdd&}ZXJ+mygcZj89~pTlRTt7}#OD*_O*`Qmzqlmti{vURyQRm88uzNLD!Z10PpZ(z|w z%_O!*tReE4D>aRr~RV$sYg zQ&cQc*FWQl(eQZm53GM5zrDZVcJTRUchgx(=j6_}_t&p+Ir;TELvJz(dg>l)pneBOq1bpK=NZ+QT zo*Ea>)F4(Yy24Xqz{^c&nnJxau5X=sxsMx}@CneJMh+kfVJvK%TVNYos9Lz)a&OwM zI~$(Z2@Y0w(d!|;>N~sjyj6LombU#sE#L<({eAG`4CrXu1MCj;0J!0u^=?3JDCD!L zG-23$$e6Mb)p{#m#5TfzrRslF@hsOV@IR`+|0ugpLVs!gN8{Pe`prD`;$xCeGt|TE z$E)QRTPI!t<8^#|z;5rBd!P7^|5BSBLgeLbZvO1Y+l*GK9&r^<4%!^!Z!;kAd&IqF zn=4xrE^qUgb53}?srpcrxpAr&+*dR&xNlM~xD~Br(wp#t3%~R5f@|M5M0ah@A$Y;9 zTuu6@{|I=&-Tv|)GT1l6h*MWv=M1VP^7(tmf7tToW_ZD^_>!6QhnG=4cnJ3s4hU_T z)~pKZ3HKgIy{I?bFAB*Ze5J02{eBoc;jU)33Ut6kGZXiuP&*r8N>t-C8{YSP^0LE=cOyx^n=c4%;UiWZYPXxKmC1j}TM0j{2aZp6B+g>WRor?wx3-zTv#^ zIw!%eUBocS$-zI|(~04FHQE=A(M|oF$qvMK#MzV@X}1-ch;aF;$T1j@96EgX%QxAOdAk{9Qy*HI7sO?gH*2!O0@+D|j5|O`>7`x?@a(!A{+Dj6A z`oJq$`3DNx<@bWNurk!|1r2T5y?&U~Tq-Y=&?ZC7<3ww(e0@l$_Wi)uv)pzIe0J5A zNW~W4Dd{gxZYsP%z#Cji@A(I6htao-6E8TqUts@(x9d8;541ne4ZCaIs9^9*EQR`V zZ)=9&aU|GHarcsrFE(rKaM-Fla{A#qcvRVtu2_Neb=+7r$+nQ zf!#mtr|lPRjyvnN=a_^|W}FjTKs_F>Cu@ zPUh0n;vHU`iG0#->3#c$x*@$4+CR%B-H^?y>}a<9-MS%TEabVzCFle*#U0nu4f!o5 z)H9GNOW9ll*E}DZ!W7k7z#(+<9@+=6){my=6NACGOLJwulW3gWVgivV1MtD;SjNb2i?V56yA@Ht zix9g7`ST^36P^dsIZ&Oepfe70=jRq>TRw|To7H~xSA5R}oqfWcB2B-C8Ez0#Zgpo{ zGp9D8H|}MmTi^*w^ERVKf{VxvAO4+Nh&a7UTy#apwcUkJ&^yAe57B zB(-5&d?2nIU9k|uQKFVEN_DB#2`{bUwq=k{S|w)4K&M4a@~V~s zz}xi8@D{rN_VoPRfW?u{3can{@j7i|4yxoB}F(;vuO% zCQ={Cxej&NOgOo^qQt;whB$@HpyCgT8rKN&Dlb_PLsP#yD{c~)HG*%0cS!Y#QV?_> zjmbpxJ!EFu{WIfcer=`R=3eujX$*Lo)0NTLOxuh-;JxCFh^VTzU$Er`W^wwpdm}%W zvrxQIyz{7^>xynf4_!*AKMMx$SuY1#7NHwwd9R!W9|5~+=?}t+$8xn zbPd(ul=xdWaF&{x^nH?NBs>m`M^x>exnDGIwoI{1)^D&PhsasZ*6u;x6LIx7bCwSf z5iP!HT3rh4#i%tH?@4HYZIA;06^cBpn3#f#&4vdinDa8@%=Vz+LHU)wKaagYbJ7xj zJtvLYxHeOlMd>BwnU=gIO?C^$uP{QOWfFFIM4u=%k4wd*9P^!C&uF%iz|Vy zam94q>j1@rS5_fSRs@4fAZgCt6cUB~%B{q-+#6!xhc5W$b;ZJiPp&h}!Id|&RtHzS zir$N)j@a*7vyb(TCg3sd+Cq*KM13NPcOtBC2FQ+5Q}`aU%dv+Tmh8LGQ0~&7hHrIf zDw_qI;7&xpr;vC)Yjp#EVD)3dtkU)MYlJnN@0Ee`F5xsIU*`)pIfif=8SA?+fPGI4 zYzHKE6a;Ib6NB0uBhT!r7`t2{}sbQVfl z4MQGQtpRyagCiR?c!<2j%hJ4xnsZzFQc{r-%4FC0zQhpl#0PRU^-+bg(kh% zEDPu-VcO|O+ua7xjcoe*(c`mg~6*h%kYo35J*;}U<=~Kp=Wq_@O5zmWhZEKpRz> z!&9o&2dYKif@&dmdW91+TBTZN+c_5x$_Gl+Q0e4BwP+1PZ7i*U*4%)+eyVl4ouzpx z)oQ35DNwsB`I&aic2F{WYbvPLfGQ=RR10hHbVE_$^uN+`3;U*Cv*(`o-1)yfr>&Gf zf^(1ejs2(e+}}J?{|Doof1Lr&Wdi3i{|e4M(Kq6nJy+n#{O`cIn8E+KJ%^pTpW5e1 zyZ3Jl0Qr;6DLCElg z8Z)A7*IhWN?aU_H^K(xpZu))ks=RMEYLMvZ~b0A<0ihd)kBx3a?+9r@9!_Bp1U$N_!_B*J6<{b4Bv{= zq?@{ra`UjaKMJYGE7L`H%99M}|6Dn`SLfzK&%BhL!k@U1k^^4{f-v(WqKCfJ0B-jT zJO}g(C+PW_xJGUsEt_X~Vp0k>c)`GRJ_5h_i78PGgS~oLo@II>d=$WY<8P1-i(_u* z%sk68i2;A5d6q2P-;DR=c)t;Orr>@n?%%+B6SEZF5$w2w&m^uBZ`Vx9voMylNlxyq zyHYF>XL=&${(Ffv*lNc&$eE23^Q;VZ5GhYh&%Bgjs04R7N8n|w7KTdj zjaGrDdDsL&k6o}ng}a3rw$j;+701}GH*km0@+qE2Z*qGG&g$`FXgYoC>=eSBm6i#J zp>0G@+%&~XYe5auofR>}x0fHHwVg7beDOdjtx1zO1X|$B76t+}2F^Yms3Gq=L_|DMu3%p4 z*9xhSf}3VF+aM32wWkykgWIB0`#U8DvTpmg#W}9#tw#77tG470MpL`W-?^sS%HEF_B-1t`!$^47ut{aHv(JjtjuEA-4lGWki)T4SeF z3i+Q1v>y3?H|%1b6Tel*E8DJ0p>C4o`TZmOeepY!x7!$tUO*2l>kZdAoXb1IHn%xFrk{e@T zYSZp*Zn{=FggrEEefq08vG5>+(oOt1cOo4T0lCU=-7M8ByI0u-S9Y&%v74?CphCHb zp_|L%Kh-lP@A~4YO<&(iV@{t^ZHYO-G^tZVGAfg)7@$SVz})ND%hG(!{2DKXnR6!+rXGwAWe$$cj>%X1N2s zadLN_q<;H~9^n182(?$$!u6j}syz;NtH+Vr^lq*5(un-JKg2&G3onfb#Y~-}9I~L4 zM*^d9%1obQ9?`}|dvf@n40N6#rm_5G7hh9|$aiErFoT>)ZCdk-&C-fmr%)H}i|IW- z(LMz>(_8-!;-GuJMU=t58(i7Vx)iBv7W^u{vl;6cIkT(MwX0kAb-w-PJn3#X{buze zJ8J{q&d;TH!Xt0E4YN7MDafz3Yc5d1}fVS_*Q~e9!FJ&}oX= zG#*6zjJUbFOp~=H6`oEcbxUy2;Pg0BC-=MgJ$AY?XSaH@Zg18+6Zebm*^fa^n*90p zN|(D`w?Tn7LY>R))$J;G?K(u>tK3t88tRrLpceeg|K3l03)+U{bspmTwR`B4nPNBn z2E}12PxO85KL4GYz6pHmfwPo$P)eax@wrp{xHz#Px$dT4D^yX%Y-zlh*VzfB1-xi=dXKMpg z$Ccf$)AC$O573JqvgoF3*T7lN*3vH>!!Kh^!!Kw34I)RK5ai?4#4T{ADHYPEX>@&Y z4(3ESGzWrU#28Obn+H26#N!cP!1X$=G7f&_IbBEnOU*6KVqvpRTz!a$1&4I`xnex0 zn=@;D+Ws`G^7=W4oXLfuPOJ@wO7oztD@z52#3U2nZ~KSb4w8#mnbNOwM$aa@72mPG zpbalD?`qV$zty4 znc1`3PBHP^cKGP7CZ>>GWKS{TfvlQc_{YK-_D>-7-S@sr;`DFta}_4CCU2YOU0&)o zh6(bz>qg6q|ExE0jxLB<(Tw!n&r@D4!Gp6JeX3xstU?+@Dr@Zi9aq^44pPlWaum>Up z(#UCRAJqNo)E$rAIR+RgA1jQx-{wS=qgNH?MN>{9bAQW-{d4xuxsUa}MsP-_fB6~t ztF8z?F8!-XY6r#EIh@@Nmh~3kOvbSn<*@cJM0pHt4}GSG^{yksRy|aD|G^DX++M}o z1ZnP9knA3WeBwXI;Hu4n(j3Gno)OGvrC}WFJwsNkOy@r+Jy7cOK!a9cfvwc?3DH8_ z-n&bkmC;z&<2q)V6;BhxdNUcrGQRGmJC)}9n3;7m%~b+ccjJN8@$gXpS?Qd*IWrG5 zJ1hGq%-KKF3=eAO2V+*OK0H(L6zEuQ5bV50=xC`f0W)9Mz<*tOsPr-6fWPD)P_i2( z(PCY(caGUv=^j6)jWFqp;|D z|3vYr@-~_4C!q)Ex;YG2QlTwSW+@lMNK7-gNgjq$52~etE|yYt9kc{38Tbyjit&!w zCX;p@{fd3nVBi}k5%YR5x5SpGfHYOZjLSk;OB;f^Z*% z{2cP*%FjKV_O5qbn*3$Ej;>FN7jd7w<%F@8th#+wyikK3z@gHAsh?s82io4JbM^J< z49WdBIG2NyPmTulI8eHw+c_JjH&vdp-W_bEc@^(C)N?Cg8!bd>SxVTJS-|mmkhpOq zQg|7)hGB+4N7$W1oCC2WMDOaxDd?wdgy#pN>HZ*iWy#7FF$YQ;_!X-TmByn)+y~HA zp`N&`BnR7UN2F26=Gqf&uG=<2emOu6eG8tum7^_L-gg7rh_l24eK*tyZ0k7ktXjX) z0!b(8Hmv`iQU7B=88w-iEU+TghXbY3U)n@iK-b zCh|e)gRB9k^>*l81KU&>3{mUx4+xfWb3Z7J`&9AnB%!~;TspZT>PzA45?6ma^Koh1 zpJq0W@W%a$u{gVzlej-=eD>LFPBNhHoqPmN9bGo6*L|y&WOYoH22Y_ua#e zFVxB3<79srtB<=UBBOpmgJ6i}vt0c#%z@Il#-+2J&HhoDm+EdUsw?{SbVm$mHWIz9KSOx0CRngiaYB# za=4z_??f2kl`%rh_2bi;P9m5 zX>4F6V>waME0NzpBK6RWGpAV7f~Se|)KerZ%(UQsVt95nzT3ixRbh;CU?BWvfn()< zYQ@^gursoWYsMC%PY4@ecZ7*{J3o`)=SoJp*rmw~U6$!Bq`BFMJ6t z=x3!S>~699)MF3Vr)jP!CMe?z;YFFwf&|!{Cu0^^NsXSx7&<#z$SJ|_^lfVYKaN<2 z8f|!tFIjn@^pAWBZzQv^b}mNUsMkpQIWX#UCmmOR*pdQyQd&$T|IF&$s~%dtzgf*w z9rtyL`;`RBqd`xJP@0C6D9oZad{{fMpLSP09F0gW1FqyYUCM3PS?cw3U*=qxHIM+% zNV0;VeKuy1z#hss30-En;t(+u_Q8XRS5Kqwh@0B=ydZAw*0a@u9@3Tw)6#Y_{A&&K z8Tj`?q-VUD_nB{+Fl30vu5LCzL|f)c*#xP;I8cr9k19C$7Y7u;sZemqEb#UGAqrU#0+A<)!*I_-Osk%2}kQ60mryjT6h2Dj}2|&*|Aj zi=Iue>)qWy%fN2wcnBYyZ%Xddg$Qo!tKIJHA@KYyz&HNfV8}8mx103u=xB?w^5w2> zohgxWgDy{-(Uk4g!MeK8%9}WWp*+3Gveihh5Dp`=b9;!>zIFS14w<-v?250acj)?3 ztu!rZ+S+dn<7hoDL16s#%xc!9mS0^HBOR!v)<3TbVtj9$I2Km|uJO2%aNUS&A}(X^ zXrzxu`e>w&M*3)^k4E}vq>ok-?J>BI7AJC44m1LJM<|KSqmXwL{(pu%_~(Asdtope z<#2DoJ>b9|gOkME?rFf?M9Y}8;g3qaFCqs<$}+}TDP?K*oRRWi6fbp6n_=hFkFl|g z*(}+GR6P6stsfFbn#XISdFVa7J}|yH!jR5WBXKzfVfk%=QZ=*`-ug?8c~ru_{G|G= z@mg8bqkXGk?Zt1crKRwE>09oL-$Fd0EbY6Nc*=7Edc+p8#QTm2v|WG$ZCC47VVXc; z6324rA#TLk7g18ZqjKoLeetUA5Sh_FitfQ+!!YgSi|Bduntha-%YXm*#kb%2?<}#- z%JwXLKNjD|y;r>->D8Vbv}H))Mx+P~=;~QI1MqDNYK?*2NAD;qy`t&rGt%(niGq%h zgRlBN`2DXovWZ+A&Ms6~=Q_KEP@E~gAh+{~BAu9Jh)&6B01u5icV<&@xx0%O7(DUkRc$((j(lR$Hvro--lv zwto4b1_^x+NrSU}DLjeqN2Cr@Oh3GbSmpx4pWz#RygOT}_U+HkmUi~Lyj4a}kU07! z!}7#_aZ`63W^&vZX%>~O@pQD`UIX(R{g_-A4echdPi(3A*>FK?3O>>&Ywv6427*L4TfNN|}kbw(gAGJdA zU=Hu#=DWu+MmIS56u}7TTePvNJfWhh0^C4E@B%S%oi4Ov=HXlFo~{$4g4h+aM8i5A zG%O>6bRv(7z-M_pt`V>Cb2{PtDtScMh%|YH zK{U<_VYk@9o1G?dNLZ}Wx|3W#W6WLhiX)=o;S9VV3U`K+Am3N}{^AP%o=tns^jtS% z^7499P7jF}4EJU*W0YrELyTjBDJKW~EvTs>Jv#t**IfNhT&Owo40FwIH)025Je=V|q+P}$_qMBe-V8ja`$PkaHvG=;Ese|4pm!Z# zfU%B(mWl3Xqtu>qb#)l?Y(5h9XS&0^UXtaQaJoKWtn;mE#Fa4u)tFrDK?;?xwcABw zx}e8s|6S@c+W+V=+FQoX5IPco8z;(qqr$pW2>pHNixvK?J}1yVMPHyFeBr|PXfuPI zF7~q)260qa=d(jBMrMhDOl3_u?F=24bit&wAQldKAt!Q#GZD?M9@8(GnvXkL{h$`4!w{IhCdl8X2c6_?sZn1i%G^E#!Y8p0GXN$Df*x)g2e6Z!D zn@Wm~omLLnV8e7&a5_3K(Dl&Q-FJ@lEFdeQoiWzAzB=KY(gt{i6E_S-(|9+&?&<+2 zVrg#{43+A>+tGW0?!GIq?}k7Yi+*K+!39{X)g+T;5J_@?rm#rb}QB`PE69HZF)p2)gw}- zEliB3|3w^I-NLg~dO_K0qw5tsAr1tw<7^y8kk10gbv#I%*a4r``Y>Pc*Fr6oRoUug%`M9d|n%0|-d#TE)(&xtp?DKzmJkDi^b z<)?cEsOz?cA?+*(OgUMULBb%Ix-v8GT^ys`Q84n|AjvFzOU8R zu9f#cVW)t!dGE7;OKD%AN*)o(!aRsQ({W;=Gv=Ev_+{IsE3x`IyRfzuK|E>AdcZpFy&byELYOMqgcZQprSG%e+jLnPq&R`4!uPJ)?ruR(pDv$W)8AH(*vOzX zINIe2Lp$65lg?}d2mM}*Wv;J5_)pe9x8b`GdVk;7?e8gV+h9eAQxI-J)yX?|M&rc1 z?C&xhJeW32xP4WyadA+`GbvRmJs2lm@@BvOR!R6-R~{SD*H&juASeXwZ^*t zBVSPa`LgNd*0O!R@b+{2r*E+C-{%WyKUa45tJbnl_usw$Qy+_`S#Mj*+V{`e-(L1W z*`M}5u>Vg!efz-rht_}5@leMX@1(ucIFL5bn4f#Fah)Ynz&V}Xb>4&c-a+rWtaTqf zP^~UIofrv+TN>NpAp-@Zj%sIOwWk%Jfe}wJ;#I8o^lNT z2xp~8uuU8r&A>wnb6A?=`R5(#ibVPuBAO3g&w}y zCja_)2#b1Y&x&JtM%m^i!H!`zF(DfFbft)9e8YsZ$~HLdwTVf1T6TN@En%WCJMk1Q z^rH+j(k4#Clb${ax-r<}N3$q{xHr)(%J-s#?q=F{y!Gl(JN^Y0wbNWI>SaM|Pp^L@ z_DIJg_aAvDAsEf1sH$%WpJl=c^H`sCv&D@<%wixBon(NhYdx< z&Tz$$37Dlz`-vRKMJSUz#AJw=u)<(kf>RgaN3F9{m1I4kXtIQW8`r}8iCs1$F!H>g1n6HW)Ypl1*_@6m~` z;i}rBQ?d&o4c((-r8Y*fbTQ9jcDF%t&to6d%}i7E9MD>row3~vm6W;$I#emYrr&5r znFL~bn!bx+oa@_ga)KC<2c-o@vCfCb!+zYypzJd8nBJ||5f4YxT)k0D+Whpek4sIE z`@YAk+5-=-9|rBr6i^<~csdB2vy?!5YrQ=bYlG=*KZ9Lk3+-w7E5V?&pr2FttNXc9 zh)}%fXBbPejcN~voDN%mSr5Z!%z?oDn_Hgfomb0Dqu==2O?Tl54ZLSZ0X?V1jw_f} zW$!uKQ@!G>b|Q;YB{;=7hwCZDv!%TNU21;rc85NVmaNO5`}Q3LJx6P+hRu%$--A7H zZqMXN$Nt3(x!r)X_snyU@=f@cx;XfsZs6#BT;0Ez?bfr9JJGlFKd$bUrV&kXX37D_ z)oI_Qd+RySe=<1XnpB`q<6Gh7=xFUWn|auL`VHzcb>AQFH|x#t4z*m!6>sbu!K%_+ z`fndWdDPw!Jx6cYqnuUS8lW=TG`H+qtZ5(tk2*qhQ7Y% z()lM!?=D^6SmcFu0Fg&kBjt_!?G+)yIX+mysk26QYcQds_{%pb?4PLk0en#VU`@_o z%q~PRr}kZAB>8v!5%AOtS?25yH|9}KmJhaUU+LQFHm>Q8O;ZxRqwJ9GvfHK+r}I`~ zcyS*188*lzEMN?yvq{;X);yX1vfO_pcG~_Ktz3pHxGgOun903o@57$Qn&vkoeDZi* zP0_9mFFe0uP-V=I1N{M%uV58ozk+Q+1q%R}D>8&3CypDImST_H47R#b|c zh1_UC+*FyDUO6+v)mOxrN@f$od|p4ZQGB^Q5AO<=F($i}F)d2~&($?yW@BH4sdQwT z{Jb9e*_b@Bsya{HR+%equg(=;Z_jlJOVDZ|sBgU?DjR!01F;tR)`a7lc)gr7AlAXm1#HyU! zUF(O<7q`XaC0DFjAa3{OdMaqEqlNU!Inbr{kDt>h2Zccgw*xajggm%jOe#WDyCQI+Q5pGV3$halyp7^h`JxM8E1Axo+t4 z^O{sM0)1+cyD;>GK`?^T>;N}sX-*p+IQdKEW02 zbPhhC^{Pd(LExD@c#$BwF`Mjyuf%m^NrLG+;-5x<_=XNo)ALnAD(8vP0ybu}`O7O|ej zU5A~Db;UKlG$O4Cb_`Xd2)$9IyY`n@ufVMMbpJ+U=;E4j&iV(vq)%IyAq$7dI!O*cq`#u=^Sg~M zYnu_;4YYmmQ+l47Qf!vWZ>^yxKqrM~A;ET8+l?m$pKZpKK?72}#iothS+OKkQZ{ncb7UIFU*<(f4Zmw?MD589d{g4u*YGVw5A*Xk#MUC=sP2hp>ofzy8;7Hfk!YgKj_LXSzvQgv2-J0 zEUDDHFWGYTI4_CnU1OjwJBY!-&C`k*w}0<3yYCe=YD zR@fjCyH$>D*PV@W?t}!u-up}WD6Ho#`MCzjC`~CxiAjKc<#M43(rdHtU;Br2Wpq}^ zxt%(!Ye&_X1xvd~@W-&84BC><6EVLJeF&R0qwmG;^AFV;9F7YQsXOE8?Rhcy_8^sf ze)fe9cEb}Z(PbIM%aJ|hzOS2MMZkK;QGWx(Rb6eo9#K%~Szd-KJWj!wxnT-gGD59 zI`eio#(YmH2e&f#Lq@8ZIn_?rWk#B6r>zA$rh zOtH|OF=WTsAdOlsd<7gaPW3E}pCUC#_t@lRokVH#61lgF?6IfYn{Nfj;E2CZYZvW5 zmsQX+*5Qr&Gt%ue7KU-z6lqNFGj6uM8DmRFVpune@!?hldrz4em4~E|P#ESuv!a*U zPP2y+3GG$dzTzZJIe5B+)0NR$N@^AKKF!Zq*EHWHW^X0zEDMp(bQ4!%uW>^uouN2I6QZRnvyq-n z)7z2WZ!Fi+Z|z4cmg`HODWNvkn2+~k4Wcz6=k`JqKVYV(o8;vK#FQY6xqdZ$y7fak zs`BIBAi2AnXy5S-b`P;_^U3=3iFPY*$#*%tx@1^&_mdvuD|Yl-TNR|#;Bp1*x6v;7 zlsAYMI18n<8%o9UK6Uiz>2Ni5KIFvu<7SrxUKnYQ)++5)1dNhJZlqtN@A7kLdEbHV z^PmX+@)TQ=3fNf`hSvz+%4TO07vF2%Q3UN4py(y8glgl))%2MQ=6ADdsJ%i`o8mltvHS}sG`6ScjIHW>ph>n4`3U|<&Qa})OhCG%@ z&&T9rx6!Hf!n{s0vV_{u)71A0*VLK{JvysB88OA~I1YTW*V%sh%>v7kssvsbhNh@$_tJMH8?Jfwjl;~db48~I?_ey z=^o6r@*d;9F3@PJQ-CD~MwyPdd_iD&!9ULH`XFJ`PS7}M`8JdncvhcwvHX{tty65Y z9H*zF&kMeT>Ih#}lCGDt8fpq>rYVrLAh5SM!Zu`kej-2(IOpG?XEv&+W*KdilbTvJ!pFnrEQ&N&Ge1p^`$ zTTBE>rll>ZFgjxzgycX(YQ3RW+o@;NX}@W$eq$Zy8J+^kNeH5dQ6qiq3}V&F(>h89 zGTPB_O#l@U6x8v8MxfeS(BV>4i<+jeqo{cK8PB?R$09 zTPKw5?JI;DXs`Au2{cWpox-ocqr(fq_%D1V>8DtR!92LA`J zs&FjCMDeQzDN!hAXEzY#`5!6kfKpSLLO$&E+HyLy!BB(tS=Sh*MDE1HvCC_@C}S!&jcj@#%T;N~qS8H|_!`@fkD7rAXDPdO-qJCnfNR%3DosYUOQy(;<%T%rz+ zQs$uq6zXwQAiWA8-_Ne=J+ih?uEl;~uYmTxh1^w+)2}LBV>A+9w=Mx@i0-z-Cu2r8 zq4v|@HqK$1We};z=k_oeE{8>wje6Mmb&G43RVgi2i^WLJ0j-rjF<$~wc)vG}lK+o> z#t_pq=d!Kh>=jNg(2(m2Ym)u0f4t0p4#?y&;GYLJ@88^dv=StN@-(@AyPlfkg@w`3 zNW=9py^+0J@ktWsX8Oj%ym}V+(-zd-K7lz_2HCvu9sSnY$QKIium`XXaTI^+4$*9T z%Ry~hnQ~wAeeFKE7IA8Dn{&x}wXim*WD@?53`dVEZ@-^N92kXy_gGp~-xD)-)e)nb zUIJ^g_xXNzjBHU9Xh1iulb`qMhB;#{)9#%~VM77@R(hPiMQIu9RYo)R>aZn88x5xf zzmSu-0WtzX{V?x;4cVR2KUO2gpJdVdbt1kIjB`Nyq~G$22(ae>Tqq9j*ISvE@erE_ zHi>SD!z9YK8cjyKsmFU77k{sZ5gn|b6K{Loy(D>7t-3DWMcKU2y3KP+!M0>&Vh8~| z{D;PaxGy(5=oFBN1>b34Dz*i9j%*{~ z`PUbSXxrP)v2x@OO2JG?Ipc(EsX^J4ua+$5lyyxFnC(o&S#nJf7Y+Qzf5`7ZQ?u)G zl|u|ZZJWTKZL@@Et!uA&<67zIl~=6Blo6|?ag)sdR1DJBUcCd}i>B%4M1W_xO)Sz{ zav=|0Ryt%eNP!vk>dgp^vVh+)_NL_zJQW}|aIca4-3OEH?!>8kl5@>BxvSULYjbVB zdOTZg=y7Kot7H5%tizLSdj!&_%`+k3(dv};l*+Vd{VqK-=l<&}mk(-mX;EGtkVB$} zsti0-?FUy+g~)F=C))I1iu@jkO5;SZ&xI>KmzijQGap3m&2#uiJri(TMKSI<;0YLs zdL}-f4cYVReVs^GM2e7?+h~Vxyx4TFhnbsA2M@9Ao^;@I>7MOuUm|F6W#ZE@BG`$o zzD#OmYrxx=xCMAk5gF2xNPxia7KL{Jf%j@5K(+75)w|xSULq(}!~y&a!E7kx*^Y5= zC5AY^r*G1oBJQwPl zoPvBvgjV2_v;=@}R&U9hSAtIfC7mcn4n3ac_+JlEc8E~%=1PH?4i+Wjaa&`>_t*n! z(A3+hZT~uEM=PMV&y*%Un0}7hhHZdNfjIIjyaGKcWHao0vvv6jyFpt`qW(ivZ<>QT zfBOM*r1jzk!nMob(vke*0|e?=MA5D}s6kYnc14Dt@<;<7gqK>j;(UVaPUFaQ?LQGy zw9fiL2k=c-(9$9ML35-FcpxWQEm(Bcjxg}doJu|?QG|oehzNS7b@}t&wrA-)D=P=> z>|Y6_6DSX(YM#S?)5G|1;PDLhs{;Bo7y&Q)V>)DHH5=7?L{aG^^gwBdNhe>FtQG{I z+?Q?`aD0bY#_|yG8*2cFVmZE`=Vi!oxUHveweTF~a13r;t~~}hP_kw8s$RfI^lITn z3FN+==^00V-Maj>V?U%<)#;u{&*Xq_t1DCiEgIxKUKmU!W5ON}%l7%;cwdfi`0B~E zEgDHXAzHkb485a+%%(VFd8T7BKnod8x2LKAFFKn+$BEFpMd8;$`K~8=uRTP-x4t@G zoFek}P0+3uC>sf57ZkC42}Z#S*ISp5REoI0VE+vvfP%rt7k+m!%6%{=_zCR9-jeGM zUvJ;1T9-Fg8n_SwaHM-O0#Y1-KBjf~s!GwKz2pf_2D?p+koVl}x#IKgHvgxc8;V5U z@hsphDAb_c3;lyk4hc91TbIk$#CT`m$CU=?DGm7TzU-ZyfMeMjv-jPqnAYXR-@m^& z_I?Fu!)_68!`mo;OiWmGs|6YCpEtmn6q%`)3U7P?ahTN5`Oo8(CXbjtZ{QN3htbf3 zQugZB=OlAq@Jyj^&9aMh)#M4C$Kz_XvvpqT5uFBICQ?t+$LdsT`{LB?%tVNJvVJi52=@EqzK#}g)1gOE zge+huc;hZAR0Vbs*1$X?pY>Uwh>IXT*U_nbcn8dc0EmS8B{6dkJ}uJm&vY>8c`0(| z!KsBHp|Bl{irJEcdw7tulleYaACcQazbD(Ko|-i0rm$L=3_H2mHSv%$m(nS>$`b!X zsHPtvWYf`aCX9Hy5O%h@_++e?p8_#JP`AK%S4uj@OBFIvGUf3cctCBNy`p@@K0-`T z+q5TnFBOE zvj*ng1f;3=!wtG=FuDmm!n4o!t~nDzzeX3JZV@|O~MFWSDLc!Nh=}tloDy5125RV z@hX-k5lZq?nntQuP>@4YdXP{tu%8Fd5)b{G$H7jNVb+^&3Lu5WCe*C2>g~LKcntSc z#&(_5>npr|kEgr)*3}A>hoW1-O2CDJXR*?F*vPLPqMl^H$i-C;=Ms*M)$EQzofE8C z**!3lEV03RwJ)3X-oXk6&#|Q800(D#>xjKyC$ggKZL`7JH8QhK4H*;oXS$#Ba%HV4 z_fpIRZK(iw$j;TtIc3$3+5DbvrI&|k&AD#_E$^&F4W$UY1mp|L=S7>g!x^V*`(Ef& z3-Dd1^HG_(>z~v%v>M_Yl=QXWB{e`)TpNQ|b#O3Jc3}gdw0T?QU~Si#=J4;jql58G z+wru{^PBE|9PdQrkRU0^7?Tn7`p>#akH6%;{lH+P9jrtsBZQ=gbPKWfPhL~g3&7H%1lACy(2oIdtxHaTo|*fid-GUZ51;UsBH#!B1#Xr zY&V|6xDZxBpo@dt9LO2wS4||G#+GG9j=23Lw!0(QOC!)*-Ue9?e5-oyB86PQHlMkc z%S`~<+>;C$TE}5cqIz@g{-1d$ai>B1npYN!Ho-3KsJCqVk# zmCISaAhp!4>2@E?*ISiF!I1#)tE+lDN6+N~#;#U)sSqR*pEc7R>4{)34j=u#4^X!6 zh4Ngku>vJp8uxd*2B3aw*LE&fx1s{>clyTR`ss2`&QZED)BY1l-rZ+(O`LGpXn-9M z>CH-l7IIOYIk&g-?5*%Fm?xk?@Y%*;e~?nHIUwMjuot7Yz|T!|!n^yi?;nW7y-(cE z9PQf9vzPyOZ~t-yc@4^5_=QilJ_r_CoClci0QcaivEW2lPNWrbQHL-#L)5O&snWZs z2mh%gjy5pdyW^O6>u23DALrN)_Cy}GiRz(t@LPeJU0R4tSp>6l6~qS6;CT-ba}a|f zf~X4LNVCr?a@~aYG@fiXxi5PSp@0*5_T{jo!hk$%16a>kB1g9m?hKHfn$$tgb;4Rd zFy1;qO9CC|XZvZa?0|iW4h=J1{0q zj&`>2+N|I+Pc#|;DQc(2;QV~ns(~?#8R}W+1wubt*fRay3%)PVQ>$E1x)Mw>!K^{; ze(w%^h8j|t$*e)Sx*B$eq;`{c{eZRO`PV={wKiyN5n@tNaxf0?%>gEo0TZc!iPR{< zpq&m91B`)g(f9ZU9Pi5pk3+!mz8nW&;tBq-?;PO!6V{7!#D#Bf4my+PF$WjgxIWI2 zWo}GW8u$w2ma=*6XJC^;dJzHs_#~Ff z<2Ic`ok{{|1qBUSxOzeJKlZig0>IL-hA7|?cA{=eT8Q$5tdbE|?o`+g60a2&xTl=j zF{ijmZL(Dzw1o27WR>j&d07tXz;GzZ?h?Z|-^o|nr79D_ixbj@(^bQguA^JKJKy~HyQyd(JC zST({|q;7)~zae#toVb?QWA;aaz087ZHu$s>_yrjZS$F^^i8j!Zt{#X){SbJUl`!PR znsIb+lX|C>eqY?%^5HI^6R$N)>FTXodoo^&U*nf!CEnaIR>A>(78jFcVTrL9!MozS zyEO0Jldn{(1|rwCJX4PrQ1lsF1T?P&+6Q`ZMS1gELvDM}zoBk9aPw~=rtdx4(k_LG?Y>jcLT^a2-w@^S~3y zPLFI8oK-VPN{nUmnF^E#^KahDg{=!`FU7kpoS;pWU^#_f5wXt?y$uR)dnD3FH=J@I zqaE7JQ`TU6Z(6pemRb1ik^tTYzT8lM`O^BfY6rd>719PQ-kPum#Zx4qh=9K-@b@(Q zJp+GGJ49uGTag#`oR==)@AaIQE);$Lowv+iZbTuqi1Gak_wax3i1IW->4Wen)U)mevo=~5NyC?OR}PnB?^+QPowSTX|iZ* z!cM+`Cif&`snw~+ZsKo3>b+K4Zw=cBQM!)_W%I{gtmvS{v2ocKkVGm1K;rHzJUf9a z^|pP~Az$X%EbSgRA=RIANcE&P%PB3HQGz_6OaSl5+|Pe-y*6lG+?Y<^5R;M~UE}jK zJ8UfKLCr-N(8C0ohP<#f#iygy3$xRZ zC!UE!eVidCKV5B%{qlg2Ay{4+$pk250}#8FpDwrXQ>2nbbq0tu&CzBEr5}NR$=m#& z$|Mn*4B_K~143ZpNG8t=!2JPRL>1O_F?m*xVwzuiXi11h*vdCCV#p}?VFx9yQn5SW z97Qwu^xa8ct_i7KUYj9oiIhqI3Ra60Lbk~=s$z#Vo`B*0nLF&}n52lRqmGNUmz+y9 z{7y6YkljhIc3!N#=O{I6G#01cD2h#k$Y-_rO0wrqX2@{F5?Uu@#)RHx2(gi*Q(Uce zpe!(Bow6kQ@^K^3&=vi~`RAEy8SQFIoH0Hhw6B>g(H6E}Ze?32bqQwnwr(08fsA$H zEm}Tr82H?=hiFew@33g8s6R@6E;(S5n`Q&wZCP(+-xtTH@fO{vLnG?eg6g7Lvv)|?yB9I zp9OmLyzxs$VDs|IYuBa!8~EePZUHw;O_{N=zeZ&B< zpX1Lv7_?{IgZEKHeBL3T;1?NHPpGKelr<4r7ws$TejI1E*N2KcS`WmXL5#S$dvxFU z%ExNcae;rAB}tB7^*4Ytm<%mEZc_;dA^R}wrc%L2g{7H&<0p>ZhP;6ggiQ{BO@bC# zSuNy-s(@2=4r+3b+~_1x+ojr!O~Lb14fz zZ<8F6q!!d$fYLdnHIQDqNgh7 zMK9d7kXbsIwp!n=*UhP%6P>;*otb-o#VQzykJ%glwMSt!nE_JuCnp>L)4`$AqdkxX zt{o&t92c@*?1iN#QRIU~n)E|hJ_cO+N*#0f-ZyHBoW85Hq)X1_nygIO!{I=Pm1l~% z(@~Nph$qL00A^YNha!3ftMfNNM3C9*PGt`T7B3xrT3dNQT4V zw6^ghp&2HyFw$|Y)hcc?j;H|!QYHQbo+!1YV?GC z^=;#jz`f?Mvhx_vNa917D-6G9n1WgQ<`F{L6QVh67V>jkG;3!aL`1`Lr&l-y7Q9d4 zjlc={BVbvHA-Wt^>HAD+Chr;mZ*XDrVcR7q&G9Kv0~ZLCcEcfq`uUBC;W&;zpVkI5 z>hoHRnMeJM=@s9YcqEH@WS7oNvNsO< z@vmc@D(3AcJp@Rof%8}|nU?iB!umjChtv8z3x~h?5p?5Mouj4o4nc>QiLcAu;bAtM zlj{V({m#;*f{w3CAO{LRuN8FP^>LYpUi1UJ1N>)vW7b}bqqgO%W|NEwC!Wlrrx%g$ z9{W*?%%W4Y)WgjfZH01T`ceg%1~w;CwG&J27#ksxwvm=DPl?YgF7%`fKaM&9{a9A- z+_X;LvL|h(Ww%y-KBIYA!y7K9J-T)N>BUVDiR2ei19GKUxjD{QZmHZHUMsK>H4z%n zrc*_3!$B!wVR5w(Yl`!VzLfdxAV-!hPZ7cM;zGW@pNY^IbT5msv^}AP_`FBPyez&Z zECXQ&);zxfDnb*?<)jpE=6ClHDJfXr#|8FVV!w!#NvZt8A*ki#j-0f&#aYYvT`$ul zoI`}#D4(xA`m&eL(E`^Hbn{KVT0O*~Q9*pdsbbH=&W;f6Bi&*Vw@{2xo251jvoEJd zTme{-kCm5b9}3b%4wPe~FJl%=lwC8xt5+Z^jaJAup@b-#@_haqSk;(FMrN8W=D&io zE3)AnVJ4gAvpXirq|?N+O01@iVbqRZ(0aav^WX%qMhf*7yQ?yC$Lw5FL^75_4!kEh zq*ehuOyC+zBQ%m^>mXzsp|!-8nuw%;TC^td7jc)`;>rIJ)+b~8Am5THiQm>0G?m*& zipA-gfI7YbHaD89TjDM67E7v%&%|5o13gq$c;!yqsVj(M-~(vy=%G9VU61#2{L@2T z9#eE`i6$bIclQ7<2JDU>ew-DqVxp6R8luhZruQYK!hAvdi;<-~FP zsa4B05e=8RY=(nO!#OzO*rT1YuJkn8c;9PJ*lcP( z(A3o6v+%CGvikBn7nOBZI7LmyR)ykR%bCk>>>#XL67Rt35xS=y@Z(np7CFF75Bq!6 zr=lexqN?3Urjc`V=8)+*>7?E&=>&Sw6@u2XP$p^E9+0xL8fTTk%@VxlTZs&`AN*TG z44nHkg4)d)TK;!k3~vg+c%q@9-iWHKDFSBCp2~mq(bIGqp5y*wn{VrK)Yt0;pXFC( z9va4aDzu(&x*m+#K;h}bCr>^2)E#-(0g-Z3F{di%Xc?X#q5u-c^VZNK8LOdrp(jYj zZlgVWdIqO(p77g)Q`5GTz#Ep&>lp`c*sBg0#|>;y;2EfQXi}9BzzczdK#xobsEbG$ z35%$UNCnQ+U^tjYK=gU(Fu^bC3&Ng*z7hCS*9jH-JV}mD(jc~=j-E+XitK(u5H~{2 z)(sG&y_NpPS9h0#4+^CCq5uls(8T-D?)&`Y3Ig7mCsomNZ#clJBfjgG#;G!6(mWLV zH7%8G8Xm8O)z|aX-8<5`E1(7`<*^0a`Mw5ENX4BeHKZ+tmHE=BY-7iGkBt4Y5$f|i zbGth*?Svzo^Gv?oJtb|Y52{Fyf$oN{Iwn=E&d&i_26zw;^U%M4c#FqK-}2-I+yeL` zbGlRo=}8ARV;c~IKWS1`PWlXI;M`azoB=RNu5A`_Z4NWeE3OcKWlw&QVq|&l?H+&n zN*BcS*M-TohWh@|4AV$Rt}P-JaNBU<3oAQ38qT5B*}_F{;UCAaDz;d&Wv-pQ3mQbo z__D_#+7OOdup6f(d1b$#S!aRoYmS}rjC^$Yf3EkcXF2($-!AZRaZN<8F&h-g?b z_U>O`@pOIkc)vX&6=9T_`$w@X%2E8b0p_^TS6_xH*CvV;!+uCg@)6PiMYb5|8z>If ztzUuUCsr{ZCGpmDRt|cecA+=2L^DZ)mU`I-F$B)`={1VTe*mw}+qXPLV3*f*9fLN+ zp6r1KV=WAOu>*O2f!-n--g0>S10G;)hjdoqDe#5;)vGE-NV;aUvi%eJTm6Jpd~2d( z1K?kvQ=*09$fXW!A$YSfS$b9?>FqmtX|}Fax01WhZL2z8wY%<0-BL$e%>L4xuY9MT z3JY#W=5=mywrvd6?CJa*xXR9>)7Pm_IanflBuw3`Zm8Y}Ht&tbr_%inxD1_WXYGL&y|CRJhCX z!IL|jPoc$vIK!kjliGmfh}prcssVgfp&raQRJN-+r`oWk(0YMMI&70&_`a@)nqbEB z`#X>9+{;sUCr@!l3qJk9m>(8u53n6ePgcmfNuIByqeSB-$L?e)+}{iHd_DYF@o7Fp z-ICfHj_K=^pXeJi+;Z>`Q`V%;m7lqGjX4{AYJUCV+GW)@uXJC&bMc$@($5Zj8h<+B zvbq^?*>rgGOe1|aN!#1`=R4cEhO=;Dsdrs90hNNU*f<|u(Ohq2)pCf20__0&MZ#Y+ zO#~!a&MZ%wpuJL!kv4nkLC>Px&RuyGGpuCf^X*cQUt}K*r$toFZ*s7GAF4J!AV5Z- z=4V>g#QRyzL+{I$H9>|~)jLYkGtMvIKIedaqt%k8h5gVtpD)2i?ONjfKt-gO$a2n} z)&}kaZ81Aach=6&6u5}m`&_YmJ?ATFdnKaQ_r2v6_{MK`)CBDj;%fr@`D0<#%+!+y zbP$gcR1*{LhwUb^eXpqAxdWPquqyQtG7a^&^IO?OwkkIPuh|51)qSkW#-!p;#3vf|} z`AD_8E;2&CU}Ar1a7|VSb?>$B8A*XcVL&;8v%6sSs5%$>+{kCwh{=>78Wy0LWn;SXm^aL(P8>~xx zRVD)On}4qHORt`w)tv_&3RAkI%#!wGY+ZWq*iH_usN{g(QI$5CDJGeR%$d0I2N$X) z-+}nnm?Tx^Z9?_-9niIewammdyeiA-2J7^4Z){Xt(|Ll&GEl$zwR#-PMqyt6*xR|Z0dXH*W{uXW9#{S7~ z-E14`nOu{}f6NBS3znI%4C`QP+e~z8^3dS=nSZ~Txjs{U-oaKqI&M?9%M^#% zb1i)9*A_Nd3SXxz2y@BXY(1rX5gS>_@wIHUEV`fI$1~G>?Xn-)*Yrp3R`A+ik2*#=mi@)CeWt6>?}tcYm{5GQyf!H+u3Xl?iOT&L#X`$ zYN`ZkLJSp|q4yJDr*5-oEp4)aFsNmcT%e&x69n^AX5dLJS1X=iqz3y29%Wy90{y@C z%$xVmCpf5)1@w>lzFTZ+^p>`ov9E)X(6>>$&`9uG0>w{isl&FL3jo&13*Zb?Hi3a? zI-Dn_ng!5vA)t+BT7m9C^L6@h+mOR=!LFXt>37;T48m1c>AG(`hQHszp0*8jOc-|H zw+n_`6Y%?Hv$qCz0W(&E8)|U-@gF(FzBZV;lLZSt=m0zpmZsfl8|;QUJ4!u%%q`Rd zt~QHYk48r#JOS;Gj~A3`Cur$_@Tk^IMRovaXwBq(h&E;6d>|W{v=>`XbMg`Xgm(-b z=Gp}thbFYKw@P_4JB`$nX*ap%0@-Ppr}_!nRw3(WU_W@ko0TLl_z@Gd?ZO7d1Z|VB zLh+VU$oCX5H{}JZ%`#@2=y-uaHw4)sd7F#ye02LUWZnhX1P1kt6^;e{Fvd(<^o~-G zg}Fjj@LK~X(mHjCgbN;NRDmNJK35`!dM3!zdDf3YsOFn+?!PE26XZIjWrB7?jM9sbuGrEg zE9>lesYyI|KIlw0g$bY|KN6&@!h|hQMexfqJCbQTrhkIx%ich2c%m_T_j){k^6SRZIl55Y z+p)@g=nugAzT7jvq#JTyi>w9Vgd+PkK?fW zpX~!}nwSn$pO1BT0tW7n^>iIwwxev1^5akk_AEyis~d?^*5NGj7y|nd|M=`~w9+!g zDK9`;DssXMnh3anzBdW0q3?Z+(o&kK7q*-=M5O^eHBM7*mfn7{v-NT5xD$U9MN3I5 z-AKGdOBp|=x9`29K-0R*fUQ2tZk|c1TWYwbeA(u3@b*wz)D4J9tN7t4*i*h%Zjp&; zksK%$EFa^QrxA~OY&m$jwoG`>T->9)h}hf~ShGlJo}it1@p8e9=VoWtaH`cC$rW3r z-@wjF#51?VUhWPylPMxawvjk<4DQ`<&Zf@Zvk~%+9_{fg>U`{-ZMvl|vPhla8HmH8ukKrztH14swo{ zzWUbw!)|_?M4TwH%>)fP!Pm*L`er?+th?<5TgAe8q}BWYb`If$y`YE4(fj(9^*r`# zLyRZ6q@V1PZ3rPkM^-~7I|63A$UEDS=iE5y*>(Fq(j^Uhird7E$}zddl2YvU$ttXN zJxDJPH1DKbPSK|+^&jc|<_x~RdU^YD-HOT;tEai9WeCU9Q0j)5w#*HU)cI9MFTftX zeiUV4upk@jNBK6e4DSG2ap&4E6U}H{GN;FDi@t(*Sdh}p=_|pTpMGm?=U=+{tbQU$ zV`u8X&$ENrk$fe&d{;=-@~YcTwCNPrdb0W-ha{_*4uXHi1SR2ZE#7(l?0&G#R~l?c zS|`7uj=Q&Eh;vv7=dcOhFW;a3?m2}#iX`eAoy#FdBw|l#+|rw*#q2Myevx{*T>DeF`oKdT z;oA26zrtl{T#|kzX$glG{@uyDs|#vTUPmb%>&E?wF)hxRV1YOk=pyE8$20zlzMV#mO?HVTYv4@yJvBv)=hV(eyq{DYk-}H^g9c8SFVI=j%Pc>H z&?mi>`1>GyUlEjQrR&CcL+8m})UJiUl3}#FjggA77*Y_68}4i50$L?~y9$IBaJYo5dql;x? z%V@ME_ES4(2uf`jms}>q>af&eVZ|)aQ>V-r{jnYwb-1zOlg8VQGIP=zi2dyEA!^tp zZ6Y|^;Pq6x6ohvo2(N?b?Rc%hYbl-2@=Yw{wuku2grlQ`x-+xFU*g2D3$Ql?Yc- z)&f^yL{7J0$Z^wbM0J)^25aIf?UwG-K<2WzGhr>RmT%&1}r_kJ_Mzs8) zzCEK^bciMmu?&gcq4M+K7n(YQY&JJp)RuDdNn@NTLYF>UWsD`kUmjxY<6UVl#p{0B zM&v|a8%H2g4fe|!;;A)e-s7gN<8+Xh_R>_DXfbIOQb?6*4fwITA;$1nq%2uRJP$gF zP!Yd48+c3e=(?xx)|SsB8pm|&I3B)Snv0xsddmi%sKeR%Y-=dIpHz=Opi$Q0m$b06wz z4Q)}NOBX?ml!W-8uqO8;wy}6TVZA;cgW+2@tgI&T{vKpVTR)D+Rd5~(vQL<(_a^3%RNOE{aY8xsg+ zemPjNbIvA$oDVS+yS&~#2i&7Mx|d#38m*UNHM~VC5ydu<{L@kj_+88O0YzLHq5RRj z^>MrxpYn}GC!7fcyXYV6rVZ09MA4U4Iw)qoa8IR}b_ld9xYM={g%fg%^-{Q|_*AJ7 zZJIA!P$`Wd-3hds-y$XQH014Q(*d}@r^>6sFUpMf;Dp4V&iLro!$y6J=#ZF{pFf=> zTew)WCq-V2HXNosqq!_!<`+(Y-Al9>wA4|*jMtS}emdtnr#ylGb%8dqjZQ10B@@gr zUT4gVYK_zRmD}N@$Sr0}&?}8{z%AVH%I`X{anFi2|gw_*{TNIIY@Es{) z?H58x&#jKfRx{X$IldcWg13G?Q8wfng_F7Eev!FnQVQEs+pRR?6LKnjR6X1L0QGVo1`Zel=FQf^%IO5k1e&6qLM zH^ad8;FZwcS3J>?zTudL^?v=_8x0jZH#5Rt>wNP*L&?%*3K`o|Wy7WPoEBtL|=WZ;txqjuI!-V58E9C`}YU6OW8nmxJD zKE9H$LpvYYs?E?k`H}4pKWaOeQIzx&S&pnpmmn{k@{@QD^+3agpk#qvI6t?KKwGY? zgG)NY@eSa)&Vr9zM~{Z&!audL5Sh;L=@4TJ+DldD5S*cmUeWR>5mPDj9C^_gWDYHQ7P`VK`?UZGU zs>4CxmrDy!r(|D!3vmZ^ieef^?cmF(%5*?naF%22Em{)w7nPsHSp==`Og-irS{5_f z<@$1c{AP$``0A%zyzfQd%+pM>p4UUBttP_S-~VBm<*1>!3eN9{RX6jrAM>na{^_Xr zo4ToI)^onT+rx$+1FS0ZAfYH5CT2FTPf>hkAyhAC6SP?Isf9RJWi_7vd)W>}WHCth z^F%7}g+j}lBJ#hz{x4{Iqh8Mt%DrB7in2?fiTjP@+>7@)fQO<}u+~*h)L%j-?Y}D` z4-t8DCW?>#e3cXJrE$jRf|zUQ@mDlU`DqPg5czEo`a$Rg&F z6S-zEebo_>ChJ(FL7s-zOSCPISaNP3H-m%}$@dO=b<}p?aNCmYnLznA2jQUxAA_HG zHqMMq*RR6rp{$Clp`zl!OKgS;tbDHR9MCrj7MBmfdkEsv_}BXgR5SLyg1w`va$^>3 z7?|Mmw0vou{A&DV=6du$^p`R^=3iLcw(Q&+XIGwCed?XY_uQp_Kk&DvzcwF%%q_;; z2C9y7#euHj2zA^AKJyUg?4H2?c2H8G;RdZ2AhNyVhj8Jzz5Kio6I*&5PuJTce25u5i31U%ulX- z+1}F)y10$xusb0ITeh+x#7Ss9gy3`B(_~BwMW*uG-P7pQ(9dvgc(yyfrCGs&Z-jJ! zP=e5aTqMew8+VL?n-WcW<)J-2oRUXEZ8|De=zTzg_jrVmmXiLT=vm2zt6rPTB;$6@4ms zSLM>q2hYm0f6od5-Rn1I0(C^7cRZj<>5q)i-8<=x_i>+yJK#c`Y<9$m?0Bb+%es zT_U#>BxjHR&|~ttFej&ipJO6bes?IDpWlf7BbufI%3M(=RQ_L4GJpRO{eO)(K$~k0 zBY+y*D9`f#0Zfkmrb%XENA}|iN735Q6O|mz%1mKIxwn+Z0_(16=+RUi|6V&&nRYxM zeA+qC`;DMi+|Jtaj}mNCbxEm*w0szz83+=zrdu4#sn|OdWa7&2A%56qy!kbd2ME!- z4w*0!&QouPb3l(%Wu|iuPsXQ@RsLU47n~&MBUK7Om)PXRE7*PFCNN(o^TADC%}&2`C
bj{~vylXg>J^9q>jW(W!y9@edF2NWMVBtd=b5%8<1KZqqFUj)tk zQ7X4CO^KugYxA0pXM{)AAbp?wT=EhY6cB&&%?+)3h>|q zjj{TZ4M)Fw4q9&3VSd=gKGoXBOm02F)ed$4LH&v6YR3bW(WH4}2fq|jPJ(tmX?M{R z*%)ipL1)yVi3vma`}$T>M_pj(6aK-Gb@#@h?fL)Z9b+6$)nV1b4R zJz+lR#~=y^enHFC_ZQ=Q&|1O0amG=wuu?k95f@x%*no45_Yjuty#phSf$M6XEL{tX=E5^{MyYDm}GzOfUx)Y`>8M56SE2RHEUuB zp}vC9u*cYX{52nA>%6~z1v?W!Ylxad0AK3-^X)Q=$B(`viuMO-@zj_Gpyf}!r7`oh zMz03LrOsXdZq;zMQpwl5I?cH8xhG@%99BuV^$R^uUnt8H;Uv`AVC-1=* zy%hi{L0{vSy-&_w?4yPk4njW}`U<}+o5Uw8q1;+KJPV*N^UF%$Srh7!A!Z9&qv*?6 zR|edltySg&elCihtB1G(v{ynIC%=rIT*|3_`v~Gy1D-CQYqgyLUupJf94-5hgG_`q znty)?&au#wMLi<18d5qa%b_1nNP=7}+s9B!v^6Hj%vMs*Z5K%jF4E2e@^(%4ic*}#|uW>K? z`4?c{Wao)c(s}m_PoVg+N}R31Sr=^lA7&lUG|W&x@x>94r_GFPqnHe@m9ZtvcNAWx z{!I1(wB-6hOVG}SexPTdW{|(!_d#j~&0IU2E%U+Phed__iasJs%P$-dQ*|<00~&z= z$^pGpyV`udzu2j9OeX9${>8ODpA0SR-g9y+U z|EWhT*M*)Q8v&So48LZGI%kjp>{AM{J&@p@8U#pjWJd`64zWnpd1T`5O_kuS5HScl zj=m4eZJs}kJlbC==Png`%K z%H%OOW3XHzQ}V~!jRWg_U#jcwbF*Dph5TwaoL_*vh~G6rP?_MNTG(^I_rwRem-}|U zdDuJOI$&g-9Tssx{E5>LavXn}<^SKi;`^CGZ>wF|;RY6P8XV~5`YG21Ht1n>^A2mtFR zkl`g2v0I*rvsY~c-|QrHJ2f3fa%X8GmFUIhCq>RDt-G=HG^gVOh|4m$-d@}HLcP_o zm^x95XL1}i&p7J!suiMK zW#0}yhTO$s8Mw&sGI-Mf5{3jM;@^K8$UiknBm!B^!dB~|K25^>?=2smD%V^B9XzSU zU9e1Ygvxo<^ECU=Cz&W9GUMI@tm}{tY9sgRAX|NQ*3?oVFzs{M^? zlvTox@VzopXXY1li21H@BrW1YMO4-Tffc!jM(Z6`$J@4Iv zVF!QyCgIsUZbX&Gi0I^*yJ1#m^5GuXZ$(m*s@LZQ-+HjdiZJsbR z8c@^a7_K0L^$ns!m{Bvs1&Qjt8S594zW)|++&I&OK4kmd<-%g2x9ZJ$rDaxqK18)*ezeeU>4UU)^q2MeImhSB1xT#mt#JkX+6Zc2z&*ars6 z9+l8+^_xd!EronQ^J4Y!M$DB_T+8pwl&bveG`xQuaT#=mZwNbQ@%~Zat(li?>dRP$ zH?osxG1Wq~>szVw`$&?>y<*Tp#JVm6F!bjB+)OJILd>`U<3mc_GHjc?!N`W-OcS^} z;=RLYb69gIp6#Z}GjnuNIuX<~f9N*0AS!!l`f)T&6vYiwkGz9+HA#e?KJ~Je_&q>8HGkQtSZ04(3O5Hl+Qx#^j z>WLv{wyRq%IqYUm1pZjMToD4yru{^N5ixT$B!D{Lz-de*k+RfNZ$%sM7}%%~jgYI52``FMNH@;aDun{=Ee3 zVY$(!fS@;kem|`Bghf3xbRgZbV#r|BIe!sZ@sn~bui9=nX;;=nlJjPWG<6XM;5h~t z&ch8c zcOEW?+pULvMGFn|F7drFFHq-QaM&I76vmXwPpeGzl;srK1WB+qqQ)6#NIB?_d#kP& zD2*E~#Df*7wQ!WIi!_yHexZjMv8XaSBJEsTq;R8BP@DLbE~(7aF$!9? z)y;slH5zMJ3{h#D{wYS%!YP52ed|e$Y6(P1f$wEPA^-ol;G|8kxN!U$UwJr*n{|&? z+Q2G7@*?o}V^xqZKx&@U$nWUz=Jp0(I>;6ec8T2=N!6DUI8Sf6OyflLpfj<6%|CtbD>KeC zS*-B2s9p-$NCt@FMC&g@i6$eRl3{_o@6s*@Efr~{#H_?sBj4Ah7phf(H;Ql3(k3Im zC0`+Lqi1fuct#26j8e|QS6z;gegh{(*l6IRfqZcfW#FJ?jD&+6cNl|!zt+HTf=n0H z2NJiZ117+4d1(9%9Y549ZURmJ2@-r(7}<;v?;c#X>20D?6P>av<^Ix@%UYMse!cv4 z`SR_{sTKF<>IEk4Euf(cOHPWAzSKdcTLWW>RSr76%PI4xM@98h2qEy}AG8sxvO|E~Sz7S~AD<_nId@ zj`fSXUJ_`*e94&zvq0vrC9N!{UbY2%=I2@i2ukhZyJMv?>0Ic;+ey~USPk|oqTxMQ z1I9ZHv3|rjsY-fC!=H=wS4n3id1IM7pZTJ!Ag~cVqpj_k5R1Ng>Wh+#<%m8zuA9TLRbRu+v*ujI}9=T1b@lKV3rn{^hLkk&R(mc#Gh!a z5OKq!S>ZA^W`z8TUo(R&2agcFP_KUJDj&9oM99eW;J}=@IIdB@m}OCg3Z(t+=R|P8 z^Uj=f&%zlGCumic*rZbQ2I*XTO=-lD$6l`kpK3k9xAl*+8*LiC=4BRg6Jm~{!X64v zBsBMkX%4)jDZaRv@ z5nn$?902UtEybFw1j95vAx-1W0g@*$LZ%mh5&i>=ANK$UeU7kzpOQae|N0SOfAlrM z|0#fwy)7jVeJZ4Ha?)w%90F(0&rI@|{2oI@B#b#&t`^%}55T(56K*lh?yj|PGI+{Q z9EMi>z>}N*IFYn=Aat0S#~%J-tbG`u`&i0A%@p`dLywKMGrsUyN5LBP0POkA52$nv z_*c=ep4|OcFCUGh;)@^I@vGGTL~JcHf-SK>A{F&|lNw;Z9Z7Q+PYF(luBm%KL3|;} z?*J`f+FK5L)164s{wCBnZUs(2*}XAm1CIr*Tv+-t(vcwcT(Lb`@x@*a_hWtxXzHk< zt;>@pC8>6Iz{#yFX@|@Sld6|j2h*Na6<`O8NLr;cxM<);Io)6rhc(12x6(3{41djl zWXMt;qQiUM{EBt+uZYTM;y_KtHS5AE3C1uUYno2c&Xyxy4>r z7s-qKh$6A*dGMwOyNu$n_;Hh`E71LqWBBPMf|3}64n?KVR6faxSA=6+qIy{9JauGf;!{GkH3MqEK>KzmPNqJ zx4{{dJpoeiCH`-c&khXWH~PLUu9XV|)A>G&B*R>4-T^km5VRrwarm(ewA%wAu)cKP zB2;1v?7Hi~Cnybfk!A&09gDoB3^7t-e{HDN$I+OypYk;1XEY-O@=Hz=o(GmegpAg$ z3yILovTSD?;k2S-PEVlU1K+rDNm-d_?J^v!8yzlG9PWR-$;nTgK?sk{RpNmoYXV{(PLt z7t^26{&Eo0JoDo0E4ph5uGgNuuR(iHQgAqUPh5(k!A~tH7QLWkf_s#|06rr-_av!z z0;TF8zQV;{Vu?u^d_By2^QzGz&xO4av*QHs+^JDyJHYBQ=A+}Y@0VHP^5czmaXXQ< zln>bfxRid1^51+@hLDv=i#-#gMt`^iK-p&9I{Ag@Hk9I-4xb$VhFfVlY>YQnTFCl{ z*NQx|;!t+m3#Sk4NiFI*067QlPUS=Br*U*qPEY)UC>yRt8O=UPUFJWJ8d)myYnpdR zU%J`g_D4B=_Zq3)v{}P%*$g?Ev5o{+Q6e8O9VGQ~IDZjjbts3jy3F_#fzo=$zewvi zHXq*k|4UjI{P%;juKmB4)}Q`Alh!l8lh!l5(z?SZtqUNnS3PqLrFH9Ol-4)=e@p9C z&-^2;aZFl2)-xup2Yx55(_)m>m;P^L^5MRe#CzOq%|k ztS+4TcRRH)>HYgJ*Z$*@WB#H>uXhjwrzj~VRuGD?dS7hCm}iKbW`1}o%~X)3Ap(c) zt$JgdV+~M|nJrk};=AhE8gbrtbfMrbIC%}{p}dZ9S7JmG%9oAC+e zuu397-iID|sE=@DLj90uDk&H}60PQ*O>mk4z^sj9T#9Fh(xu+a+O%#z_T%N?M9An3 zrKP$?Ec1&b^2~{d*K&HX7O=DZN5na7-*AwfTjW{c@Z}9guPC3#N_49-|8PE;LZ^)^ z4bY{`9tq*EiHV@u8y)bT(Lni*1T4wuW!&vXUx&%Bq7xwo9-hKV|`~>{{hS(tRcr?aa zDlHK847FGwKh&XV_?02_7I=ifIm_N}SEgVTdU@LHv}ggdEN6X|1QAtZEl+(eE7K4L zzM7M}h_NrmAb|U4vGw0Hj5x4`tTG=2n3F``P-Z-Ai8uPV(5hJx=@E6<_TJ6M&PJpj zfeg{Ebgr2u0iHC|3xrN~Ysp_JtyaP7B8Eni_B5b+P3 zJzA)Uliu}l#PYPU+$Q*ZYUJE=Sxc`1264oqYs!owi1$Kzg7w$S6{DtOpHNTm(pO&H zL2nedyJ}2ZlC=nX=*x^I8{!DsoxY?9q~;VcFAU(hDlMgv-GX z9d^9&I<(#2D51eddJxtS#1F0%Ba7iIT&$;R#DS7voIt_OAMxW`>)!It{eO+4doB9k zMbRORd|$d;NHD>ig2=l|asRhp^k$EC{hxm0Yvb6zZG%4W@8IgqezW7>wSpQxALSz5 zB30nbdAs*w60hwf{+7fq>?D>fB){@u6Xg)ZD0r?B)?JCDq6G9**BItEsYnk+a1zgQ zr0gZdwZFvQg#q)FrW{=~#Iin_NfBMd4$TIR|4j#z$Z8^KR<*bz_4yhfB~7Z5*%C>9 z2}=O9Ap^u7O_Ap}4e3i1CDX|2b3dfP@t-0MRyYFY2{5yxVLU;X{>;GO9?kyiGV{U5 z;k8@Ba(eK)y$*zIjJl^gx1~+Fyd!DL=55s~i%UatMV^aklj=OLMgB7aplvcm13l&1 z;&r^f zeZtq3f)5VziUXHT*UF6;(f^_(!L{MpaveXP_Rc7nGm?*M;gmSggbmJHy>L&>?kZB- zzEBEv+~;Pk87|zk5h5TVI}eo53`2%BJex>84>k;o;e}HR^7kmEw zJNm?qUcWngpU-|W!`0>Lp549Y9Xe*x|AC$U2IZ3aUTFAEH^%AsE5Yoe=+#f9>u-sD z)bxLZPw$$lk&@}>&3nD-$r{OMqxUcl;K_d1 z-aY1%{Tt`eoT-hL;o#TGF!#m8G7#dcEG$Oo!w(@#Mss>zA*XAW+%?A3i6!5(}oRugO z^!f#y{yzMV03QM$0iOa^Rw>0Nh#!KqxizQui^7kGUr}|;?k2&2FQT?7K zS#9kQRmr2a`XddEAyEx@#BkV%Yt!VQtS$2d#pTP2TEbzJ#vf85n$#Iq`T9|OP;R8D z1i{Wk5UeDEFdJ#6)EDqRwK+0P5M)Kx+@tx09kSNK*Yi_TrHzDTL8uGMR~m`n^V2pZ zA~!2ifR`1mfDeY0W`q#8AA56OX4Wom<~Ak=o28Lm_6 zt*SHxT19soN;S5mLCa7rGmCl1=t|yBLJ3H&4NBg>{Q5;j5Xwa*#&2%}21HF1gbMu5 z41nmbx2uu13Y~-YmGY3%BsZsM5Z*n8H+snEh1aEsM&tB*|7mQMiziJIgnFqpEUBW_ zrb;R5o36vAsJY!M3o>=R6cwH}CMB6O!g8oaR%l=S*7%sQObtc+q44*bjc<_;pWiP9 zB{kG4X_ESV?jillNi({lAkYpO1GZN&(F4QsCP>r!O2%I9=ws-CDCWhC*6H zt0)H>L!H*zC8CL%?B~->^)!Vbl;L-dnM@Em(1WCguqdm&lRc&Zd)lY-b;j~w`tX=@ zl1Gb8Nk)l!`+RIRGA5f8Q&_@$=1!LoYbSHzg=xo_SW?a8*5os4^l6p78PkVFBT099 zyWG7~4F(o_);Tj}F5u}BrB+m$CHqj5d`h59R017xKx-L&x@{uTX~3#jrg|kz8G?|Y zo~imJ)va&!!64*mH^v~NChfASwTZ#B9X)Q$5(G(ALu!Lx3P0L$9}p}rp-KGmO9W&PqRE& zsGiy4V8I+$n*Lm`8SOGJ(pVuy{Hh$*LTV&o?Ca$-HeMt&r8x~5O_=%GrtRjTKW67o zHz-mML7+`y-orIeLyE!Bo^Bf=lBUT@GwzD|SO)Dh?!)S>`!K642{&PV9E&l+W*Wzg zVH7_`HN%?PfO}7S(2pJUp?-96<)J^P=9qezYcO1nN(6T-4dIT~271eET~mE*Q)6M^ zF@8@`$N1%gwTkFt{W=XGeT zbFPk*OuC{Z;rh!s*CU~)!$@aPluuinL>K@92{3QD+bBDn?^pFer9%mC9 z{Q;?o+N6=e0F^WG)B)GTGJ0GsR^z{2rdQYVt(dw(em>HMnOq7;@HE;!lTk2n6Y6RM>vR^sTh!j$EPP9j;sNf4mjc@Sq`YF z(;!Ql1|n6OAr53lh&S&+$0p)*a8Bg2WEZF30Ne|#2Q~tmfLTBQ*af@{ya~Jyd;**V z2L6rHF914$*+4z84p4V<`b(BR0msyy16c$t#?hAXXA$BeI%p*h zPIthEaM&>0&>g(37lEKl2pL2MIUGD$>2&IifCkh80x$;X}6gschZ( z*bNPSjgYqgaqF1cZJPqys7syt+n2Tdy5r&6_iR^a&AJ|G+Q z`5|y4UR55@lxHG0{d4a5ZKiXy z)RZH&m$|4^@*f+#)$d&(CYe;F&9xC~U4O|)Ik@l*IRBpWI zqE<-nd+UogLf)0XYsY@baicz4np;k+a*_=PZn=6~dpYT@Zf@QAr*bm&*Vi9j@NT(x z_4dw-x1L{7^YNkmhcCLS;_lby=6~|`1`#e{;z2b@c-&j`O`C$b=6@79F{zn{g z3PAO7IQh$8_Vm2yH2zQnjgavvY}CHv7YEip_R9JxV;|o1`enyvPM-Skrxhm#{juc! zwX65Ko?1AvJhUa?&S^Q9@OE(1U@I|s+u+2I)f+6k&j^kb!E`7y0+%Atc%ByayACHD zC%6;N6LdHyP zCR0YITGB_ae;S<2Yl-6qhvP@NoG~U0gKf*&o*vGKHOGkp9N(8@^TPJkhLb(2<#^PM z@fm=btP`F3OMn~4biEpGbm|WQB92GK>GP7}tF3sNzQ7+#PxahDqMnDhw(R^3|E5IU zi+91|<^_Il=Pm}bZoEU#FPM^I5V>NK8RbG>VT;gkSwD1uZtfvldA?>iGPMxkB zDId0exuy5!x4^L=RetM`b|27a`P=&9Bz<9$PS;rUe767mB)u+4ud(#r@}#L>IOx51B?O20AqkL zz!+c*Fa{U{i~+^~V}LQh7+?%A1{ed30mcAhfHA-rU<@z@7z2y}#sFi0F~AsL3@`>5 z1K*4RZ!7*+0C*J1X985;252~@?_5*et2lM~P9)WhcVwa4@2JxFsu13LMI3!clfB;v z5>6xZiLYlJ$T2H=4t~!Eh5}~;EHDNb1B?O20AqkLz!+c*Fa{U{i~+^~V}LQh7+?%A z1{ed30mcAhARPmJe*UayMF$}h`mGxJt&QAt88SO#fHA-rU<@z@7z2y}#sFi0F~AsL z3@`>51B?O20AqkLz!+c*Fb2RtrsvNze#0uf|J;=wrSCmo1`G$VZ2}970mcAhfHA-r zU<@z@7z2y}#sFi0F~AsL3@`>51B?O20AqkLz!*r&fQE0EJOnI)2mOA@ar!Njuv34; z(lb4u*6&?ME~2@6&x%rUrb7$LvJNOl#^2@lRSuvGuAeyd=dE(s`mPM+szRC@0LoEs z`HhG7UhF^Vj*ftxa%}KS?E_&a?kIiEkL^EW_MxzMxuc_CquW>Z`sc&0yQ4JkY+w`4 zbpY_-nA+&~YUdI%*uQ#O&nc~fK^m?OhXI~Rkx^f2(9mW@XdvILS z?jqOTfqkv-=JdmBuqXFgPQQIU_P%}qd#G;W^gA|VAJs=Wef3sOulO^k*J2OnsybiQ z)kB?EJp0_Qsz>~xc)TM}_w=n5 friction_wheel_controller - rmcs_core::controller::shooting::HeroHeatController -> heat_controller - rmcs_core::controller::shooting::PutterController -> bullet_feeder_controller - - rmcs_core::controller::pid::PidController -> first_left_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> first_right_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> second_left_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> second_right_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> third_right_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> third_left_friction_velocity_pid_controller - # - rmcs_core::controller::shooting::ShootingRecorder -> shooting_recorder + - rmcs_core::controller::pid::PidController -> first_back_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> first_front_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> second_back_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> second_front_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> third_back_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> third_front_friction_velocity_pid_controller + - rmcs_core::controller::shooting::ShootingRecorder -> shooting_recorder - rmcs_core::controller::chassis::SteeringWheelStatus -> steering_wheel_status - rmcs_core::controller::chassis::HeroChassisController -> chassis_controller @@ -39,7 +39,7 @@ rmcs_executor: # - rmcs_auto_aim::AutoAimController -> auto_aim_controller - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster - # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster + #- rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster # - sp_vision_25::bridge::HeroAutoAimBridge -> hero_auto_aim_bridge @@ -66,13 +66,13 @@ hero_hardware: value_broadcaster: ros__parameters: forward_list: - - /gimbal/top_yaw/control_angle - - /chassis/climber/left_front_motor/torque - - /chassis/climber/right_front_motor/torque - - /chassis/left_front_steering/torque - - /chassis/left_back_steering/torque - - /chassis/right_front_steering/torque - - /chassis/right_back_steering/torque + # - /gimbal/top_yaw/control_angle + # - /chassis/climber/left_front_motor/torque + # - /chassis/climber/right_front_motor/torque + # - /chassis/left_front_steering/torque + # - /chassis/left_back_steering/torque + # - /chassis/right_front_steering/torque + # - /chassis/right_back_steering/torque # - /shoot/heat12707 # - /chassis/power # - /referee/chassis/power @@ -83,20 +83,18 @@ value_broadcaster: # - /chassis/climber/front/actual_power_estimate # - /chassis/steering_wheel/actual_power_estimate # - /gimbal/putter/velocity - - /gimbal/first_left_friction/velocity - - /gimbal/first_right_friction/velocity - - /gimbal/second_left_friction/velocity - - /gimbal/second_right_friction/velocity - # - /gimbal/first_left_friction/control_torque - # - /gimbal/first_second_friction/control_torque - # - /gimbal/second_left_friction/control_torque - # - /gimbal/second_right_friction/control_torque + - /gimbal/first_front_friction/velocity + - /gimbal/first_back_friction/velocity + - /gimbal/second_front_friction/velocity + - /gimbal/second_back_friction/velocity + - /gimbal/third_front_friction/velocity + - /gimbal/third_back_friction/velocity # - /gimbal/bottom_yaw/torque # - /gimbal/bottom_yaw/angle # - /gimbal/top_yaw/angle # - /gimbal/pitch/angle # - /chassis/power - - /chassis/supercap/voltage + # - /chassis/supercap/voltage # - /chassis/control_power_limit # - /referee/chassis/power_limit # - /referee/chassis/buffer_energy @@ -106,10 +104,10 @@ value_broadcaster: # - /chassis/steering_wheel/actual_power_estimate # - /gimbal/bullet_feeder/torque # - /gimbal/bullet_feeder/control_torque - - /gimbal/putter/angle - - /gimbal/putter/velocity - - /gimbal/putter/torque - - /gimbal/putter/control_torque + # - /gimbal/putter/angle + # - /gimbal/putter/velocity + # - /gimbal/putter/torque + # - /gimbal/putter/control_torque # - /gimbal/bullet_feeder/velocity # - /gimbal/bullet_feeder/angle @@ -204,7 +202,7 @@ viewer_angle_pid_controller: bullet_feeder_controller: ros__parameters: bullet_feeder_velocity_kp: 5.0 - bullet_feeder_velocity_ki: 0.1 #1.1 + bullet_feeder_velocity_ki: 0.1 bullet_feeder_velocity_kd: 0.0 bullet_feeder_velocity_integral_min: 0.0 bullet_feeder_velocity_integral_max: 60.0 @@ -221,26 +219,26 @@ bullet_feeder_controller: friction_wheel_controller: ros__parameters: friction_wheels: - - /gimbal/first_left_friction - - /gimbal/second_left_friction - - /gimbal/third_left_friction - - /gimbal/first_right_friction - - /gimbal/second_right_friction - - /gimbal/third_right_friction + - /gimbal/first_front_friction + - /gimbal/second_front_friction + - /gimbal/third_front_friction + - /gimbal/first_back_friction + - /gimbal/second_back_friction + - /gimbal/third_back_friction friction_velocities_profile_0: - - 360.0 - - 360.0 - - 360.0 - - 523.0 - - 523.0 - - 523.0 + - 532.0 + - 532.0 + - 532.0 + - 543.0 + - 543.0 + - 543.0 friction_velocities_profile_1: - - 360.0 - - 360.0 - - 360.0 - - 523.0 - - 523.0 - - 523.0 + - 408.0 + - 408.0 + - 408.0 + - 550.0 + - 550.0 + - 550.0 friction_soft_start_stop_time: 1.0 heat_controller: @@ -254,59 +252,59 @@ shooting_recorder: aim_velocity: 11.8 log_mode: 1 # 1: trigger, 2: timing -first_left_friction_velocity_pid_controller: +first_front_friction_velocity_pid_controller: ros__parameters: - measurement: /gimbal/first_left_friction/velocity - setpoint: /gimbal/first_left_friction/control_velocity - control: /gimbal/first_left_friction/control_torque + measurement: /gimbal/first_front_friction/velocity + setpoint: /gimbal/first_front_friction/control_velocity + control: /gimbal/first_front_friction/control_torque kp: 0.006 ki: 0.00 kd: 0.00008 -first_right_friction_velocity_pid_controller: +second_front_friction_velocity_pid_controller: ros__parameters: - measurement: /gimbal/first_right_friction/velocity - setpoint: /gimbal/first_right_friction/control_velocity - control: /gimbal/first_right_friction/control_torque + measurement: /gimbal/second_front_friction/velocity + setpoint: /gimbal/second_front_friction/control_velocity + control: /gimbal/second_front_friction/control_torque kp: 0.006 ki: 0.00 kd: 0.00008 -second_left_friction_velocity_pid_controller: +third_front_friction_velocity_pid_controller: ros__parameters: - measurement: /gimbal/second_left_friction/velocity - setpoint: /gimbal/second_left_friction/control_velocity - control: /gimbal/second_left_friction/control_torque + measurement: /gimbal/third_front_friction/velocity + setpoint: /gimbal/third_front_friction/control_velocity + control: /gimbal/third_front_friction/control_torque kp: 0.006 ki: 0.00 - kd: 0.00008 + kd: 0.0008 -second_right_friction_velocity_pid_controller: +first_back_friction_velocity_pid_controller: ros__parameters: - measurement: /gimbal/second_right_friction/velocity - setpoint: /gimbal/second_right_friction/control_velocity - control: /gimbal/second_right_friction/control_torque - kp: 0.006 + measurement: /gimbal/first_back_friction/velocity + setpoint: /gimbal/first_back_friction/control_velocity + control: /gimbal/first_back_friction/control_torque + kp: 0.004 ki: 0.00 - kd: 0.00008 + kd: 0.00006 -third_left_friction_velocity_pid_controller: +second_back_friction_velocity_pid_controller: ros__parameters: - measurement: /gimbal/third_left_friction/velocity - setpoint: /gimbal/third_left_friction/control_velocity - control: /gimbal/third_left_friction/control_torque - kp: 0.006 + measurement: /gimbal/second_back_friction/velocity + setpoint: /gimbal/second_back_friction/control_velocity + control: /gimbal/second_back_friction/control_torque + kp: 0.004 ki: 0.00 - kd: 0.00008 + kd: 0.00006 -third_right_friction_velocity_pid_controller: +third_back_friction_velocity_pid_controller: ros__parameters: - measurement: /gimbal/third_right_friction/velocity - setpoint: /gimbal/third_right_friction/control_velocity - control: /gimbal/third_right_friction/control_torque - kp: 0.006 + measurement: /gimbal/third_back_friction/velocity + setpoint: /gimbal/third_back_friction/control_velocity + control: /gimbal/third_back_friction/control_torque + kp: 0.004 ki: 0.00 - kd: 0.00016 + kd: 0.00006 steering_wheel_status: ros__parameters: diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp index a4afd5640..6326d627f 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp @@ -162,7 +162,7 @@ class HeroGimbalController constexpr double mouse_yaw_sensitivity = 0.5 * 0.114; constexpr double mouse_pitch_sensitivity = 0.5 * 0.095; - constexpr double joystick_sensitivity = 0.006 * 0.05; + constexpr double joystick_sensitivity = 0.006 * 0.02; double yaw_shift = joystick_sensitivity * joystick_left_->y() + mouse_yaw_sensitivity * mouse_velocity_->y(); @@ -176,8 +176,8 @@ class HeroGimbalController private: static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - static constexpr double kEInitPitch = -0.20; // Initial angle for standalone E. - static constexpr double kCtrlEInitPitch = -0.20; // Initial angle for Ctrl+E. + static constexpr double kEInitPitch = -0.346584; // Initial angle for standalone E. + static constexpr double kCtrlEInitPitch = -0.471795; // Initial angle for Ctrl+E. double encoder_init_pitch_ = kEInitPitch; InputInterface joystick_left_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_friction_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_friction_wheel_controller.cpp index 1656d2276..f434c5ec1 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_friction_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_friction_wheel_controller.cpp @@ -193,7 +193,7 @@ class HeroFrictionWheelController bool detect_bullet_fire() { bool fired = false; if (!std::isnan(last_primary_friction_velocity_)) { - double differential = *friction_velocities_[0] - last_primary_friction_velocity_; + double differential = *friction_velocities_[1] - last_primary_friction_velocity_; if (differential < 0.1) primary_friction_velocity_decrease_integral_ += differential; else { @@ -204,7 +204,7 @@ class HeroFrictionWheelController primary_friction_velocity_decrease_integral_ = 0; } } - last_primary_friction_velocity_ = *friction_velocities_[0]; + last_primary_friction_velocity_ = *friction_velocities_[1]; return fired; } diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp index c565c1757..c7f13b6fd 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp @@ -75,7 +75,7 @@ class PutterController register_output("/gimbal/shoot/delay_ms", shoot_delay_ms_, nan_); // auto_aim - register_input("/gimbal/auto_aim/fire_control", fire_control_, false); + // register_input("/gimbal/auto_aim/fire_control", fire_control_, false); register_output("/gimbal/shooter/mode", shoot_mode_, rmcs_msgs::ShootMode::SINGLE); register_output("/gimbal/shooter/condiction", shoot_condiction_); @@ -146,9 +146,9 @@ class PutterController // const bool auto_fire_now = (switch_right == Switch::UP) && // (*fire_control_); - const bool auto_fire_now = - (switch_right == Switch::UP || (mouse.right && mouse.left)) - && (*fire_control_); + const bool auto_fire_now = false; + // (switch_right == Switch::UP || (mouse.right && mouse.left)) + // && (*fire_control_); const bool auto_trigger_emergence = mouse.right && (click_count_ >= 2); @@ -193,21 +193,13 @@ class PutterController if (shooted) { // Bullet fired: return the putter. - const auto angle_err = putter_startpoint - *putter_angle_; - if (angle_err > -0.1) { - *putter_control_torque_ = 0.; - set_preloading(); - shooted = false; - } else { - *putter_control_torque_ = - putter_return_velocity_pid_.update(-80. - *putter_velocity_); - putter_timeout_detection(); - } + *putter_control_torque_ = + putter_return_velocity_pid_.update(-50. - *putter_velocity_); + putter_timeout_detection(); } else { // Bullet not fired yet: continue advancing. *putter_control_torque_ = - putter_return_velocity_pid_.update(80. - *putter_velocity_); - + putter_return_velocity_pid_.update(120. - *putter_velocity_); update_putter_jam_detection(); } } @@ -318,7 +310,7 @@ class PutterController // treat it as finished and move to the next state. if (shoot_stage_ == ShootStage::SHOOTING) { if (shooted) { - if (putter_timeout_count_ < 1600) + if (putter_timeout_count_ < 400) ++putter_timeout_count_; else { putter_timeout_count_ = 0; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six--friction.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six--friction.cpp index 9239a858c..0e50770a1 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six--friction.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six--friction.cpp @@ -217,12 +217,12 @@ class SteeringHeroLittle , gimbal_top_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/top_yaw") , gimbal_pitch_motor_(steering_hero, steering_hero_command, "/gimbal/pitch") , gimbal_friction_wheels_( - {steering_hero, steering_hero_command, "/gimbal/first_right_friction"}, - {steering_hero, steering_hero_command, "/gimbal/first_left_friction"}, - {steering_hero, steering_hero_command, "/gimbal/second_right_friction"}, - {steering_hero, steering_hero_command, "/gimbal/second_left_friction"}, - {steering_hero, steering_hero_command, "/gimbal/third_right_friction"}, - {steering_hero, steering_hero_command, "/gimbal/third_left_friction"}) + {steering_hero, steering_hero_command, "/gimbal/first_front_friction"}, + {steering_hero, steering_hero_command, "/gimbal/first_back_friction"}, + {steering_hero, steering_hero_command, "/gimbal/second_front_friction"}, + {steering_hero, steering_hero_command, "/gimbal/second_back_friction"}, + {steering_hero, steering_hero_command, "/gimbal/third_front_friction"}, + {steering_hero, steering_hero_command, "/gimbal/third_back_friction"}) , gimbal_bullet_feeder_(steering_hero, steering_hero_command, "/gimbal/bullet_feeder") , putter_motor_(steering_hero, steering_hero_command, "/gimbal/putter") , gimbal_scope_motor_(steering_hero, steering_hero_command, "/gimbal/scope") @@ -347,6 +347,16 @@ class SteeringHeroLittle *photoelectric_sensor_status_ = photoelectric_sensor_status_atomic.load(); *grayscale_sensor_status_ = grayscale_sensor_status_atomic.load(); + + if (++count_ == 500) { + for (int i = 0; i < 6; ++i) { + if (friciton_detect[i] == 0) { + RCLCPP_WARN(logger_, "can id 0x%03X missing", i + 0x201); + } + } + std::fill_n(friciton_detect, 6, 0); + count_ = 0; + } } void command_update() { @@ -427,6 +437,7 @@ class SteeringHeroLittle return; auto can_id = data.can_id; // can1_receive_rate_counter_.record(can_id); + friciton_detect[can_id - 0x201] = 1; if (can_id == 0x201) { gimbal_friction_wheels_[0].store_status(data.can_data); } else if (can_id == 0x202) { @@ -447,8 +458,10 @@ class SteeringHeroLittle putter_motor_.store_status(data.can_data); } else if (can_id == 0x201) { gimbal_friction_wheels_[4].store_status(data.can_data); + friciton_detect[4] = 1; } else if (can_id == 0x202) { gimbal_friction_wheels_[5].store_status(data.can_data); + friciton_detect[5] = 1; } } @@ -477,6 +490,8 @@ class SteeringHeroLittle OutputInterface& tf_; std::time_t last_camera_capturer_trigger_timestamp_{0}; + int count_ = 0; + int friciton_detect[6]; device::Bmi088 imu_; device::LkMotor gimbal_top_yaw_motor_; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp index d27338afd..4dda2cede 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp @@ -80,9 +80,9 @@ class Hero register_input("/gimbal/control_bullet_allowance/limited_by_heat", robot_bullet_allowance_); register_input( - "/gimbal/first_left_friction/control_velocity", left_friction_control_velocity_); - register_input("/gimbal/first_left_friction/velocity", left_friction_velocity_); - register_input("/gimbal/first_right_friction/velocity", right_friction_velocity_); + "/gimbal/first_back_friction/control_velocity", back_friction_control_velocity_); + register_input("/gimbal/first_back_friction/velocity", back_friction_velocity_); + register_input("/gimbal/first_front_friction/velocity", front_friction_velocity_); register_input("/gimbal/friction_profile_1_active", friction_profile_1_active_, false); // register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); @@ -210,8 +210,8 @@ class Hero friction_profile_indicator_[3].set_x2(box_left); friction_profile_indicator_[3].set_y2(box_bottom); status_ring_.update_friction_wheel_speed( - std::min(*left_friction_velocity_, *right_friction_velocity_), - *left_friction_control_velocity_ > 0); + std::min(*back_friction_velocity_, *front_friction_velocity_), + *back_friction_control_velocity_ > 0); status_ring_.update_supercap(*supercap_voltage_, true); status_ring_.update_battery_power(*chassis_voltage_); last_keyboard_ = *keyboard_; @@ -418,9 +418,9 @@ class Hero InputInterface robot_bullet_allowance_; - InputInterface left_friction_control_velocity_; - InputInterface left_friction_velocity_; - InputInterface right_friction_velocity_; + InputInterface back_friction_control_velocity_; + InputInterface back_friction_velocity_; + InputInterface front_friction_velocity_; InputInterface friction_profile_1_active_; InputInterface mouse_; From c6751a7cff08b0b1b3f8716f2e9e07348f1e5d97 Mon Sep 17 00:00:00 2001 From: qzhhhi Date: Fri, 22 May 2026 13:21:21 +0800 Subject: [PATCH 26/86] =?UTF-8?q?vt13=20test=20(rmcs=5Fboard=20need=20?= =?UTF-8?q?=EF=BC=9A=20chore(rmcs=5Fboard):=20Raise=20UART0=20baudrate=20f?= =?UTF-8?q?or=20temporary=20debugging=20by=20qzhhhi=20=C2=B7=20Pull=20Requ?= =?UTF-8?q?est=20#55=20=C2=B7=20Allia)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../hardware/deformable-infantry-omni-b.cpp | 852 ------------------ .../src/hardware/deformable-infantry-omni.cpp | 825 ----------------- .../rmcs_core/src/hardware/device/dr16.hpp | 312 +++---- .../src/hardware/device/remote_control.hpp | 142 +-- .../rmcs_core/src/hardware/device/vt13.hpp | 68 +- rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp | 490 ---------- .../steering-hero-little-six--friction.cpp | 16 +- .../steering-hero-little-six-friction.cpp | 17 +- 8 files changed, 245 insertions(+), 2477 deletions(-) delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp deleted file mode 100644 index 0c1694081..000000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ /dev/null @@ -1,852 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include - -#include "hardware/device/bmi088.hpp" -#include "hardware/device/can_packet.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/lk_motor.hpp" -#include "hardware/device/supercap.hpp" - -namespace rmcs_core::hardware { - -using Clock = std::chrono::steady_clock; - -class DeformableInfantryOmniB - : public rmcs_executor::Component - , public rclcpp::Node { -public: - DeformableInfantryOmniB() - : Node( - get_component_name(), - rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) - , deformable_infantry_command_( - create_partner_component( - get_component_name() + "_command", *this)) { - using namespace rmcs_description; - - register_input("/predefined/timestamp", timestamp_); - register_output("/tf", tf_); - register_output( - "/auto_aim/camera_transform", camera_transform_, Eigen::Isometry3d::Identity()); - register_output("/auto_aim/barrel_direction", barrel_direction_, Eigen::Vector3d::UnitX()); - register_output("/auto_aim/yaw_velocity", auto_aim_yaw_velocity_, 0.0); - - tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); - - // For command: remote-status - using Srv = std_srvs::srv::Trigger; - status_service_ = create_service( - "/rmcs/service/robot_status", - [this](const Srv::Request::SharedPtr&, const Srv::Response::SharedPtr& response) { - status_service_callback(response); - }); - - std::string serial_filter_imu; - get_parameter_or("serial_filter_imu", serial_filter_imu, std::string{}); - - rmcs_board_lite = std::make_unique( - *this, *deformable_infantry_command_, - get_parameter("serial_filter_rmcs_board").as_string()); - top_board_ = std::make_unique( - *this, *deformable_infantry_command_, - get_parameter("serial_filter_top_board").as_string(), !serial_filter_imu.empty()); - if (!serial_filter_imu.empty()) - imu_board_ = std::make_unique(*this, serial_filter_imu); - } - - ~DeformableInfantryOmniB() override = default; - - void before_updating() override { - top_board_->request_hard_sync_read(); - next_hard_sync_log_time_ = Clock::now() + std::chrono::seconds(1); - } - - void update() override { - rmcs_board_lite->update(); - top_board_->update(); - if (imu_board_) - imu_board_->update(); - - using namespace rmcs_description; - *camera_transform_ = fast_tf::lookup_transform(*tf_); - *barrel_direction_ = - *fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); - *auto_aim_yaw_velocity_ = top_board_->gimbal_yaw_velocity(); - } - - void command_update() { - const bool even = ((cmd_tick_++ & 1u) == 0u); - rmcs_board_lite->command_update(even); - top_board_->command_update(); - } - -private: - class DeformableInfantryOmniBCommand; - class BottomBoard; - class ImuBoard; - class TopBoard; - - class DeformableInfantryOmniBCommand : public rmcs_executor::Component { - public: - explicit DeformableInfantryOmniBCommand(DeformableInfantryOmniB& deformableInfantry) - : deformableInfantry(deformableInfantry) {} - - void update() override { deformableInfantry.command_update(); } - - DeformableInfantryOmniB& deformableInfantry; - }; - - struct BottomBoard final : public librmcs::board::RmcsBoardLite::Callback { - public: - static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - - explicit BottomBoard( - DeformableInfantryOmniB& deformableInfantry, - DeformableInfantryOmniBCommand& deformableInfantry_command, - const std::string& serial_filter = {}) - : deformable_infantry_{deformableInfantry} - , command_{deformableInfantry_command} - , tf_{deformableInfantry.tf_} { - - deformableInfantry.register_output("/referee/serial", referee_serial_); - referee_serial_->read = [this](std::byte* buffer, size_t size) { - return referee_ring_buffer_receive_.pop_front_n( - [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); - }; - referee_serial_->write = [this](const std::byte* buffer, size_t size) { - board_->start_transmit().uart_transmit( - Spec::kUarts.kUart0, - {.uart_data = std::span{buffer, size}}); - return size; - }; - - gimbal_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( - static_cast( - deformableInfantry.get_parameter("yaw_motor_zero_point").as_int()))); - - for (auto& motor : chassis_wheel_motors_) - motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(13.0) - .enable_multi_turn_angle() - .set_reversed()); - - // V2: LK MG5010 i36 direct-drive joint motors, built-in encoder zero point - for (auto& motor : chassis_joint_motors_) - motor.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei36} - .set_reversed() - .enable_multi_turn_angle()); - - imu_.set_coordinate_mapping([](double x, double y, double z) { - // Keep the existing chassis yaw axis mapping explicit until the bottom-board IMU - // installation is re-validated on hardware. - return std::make_tuple(-y, x, z); - }); - - gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); - - deformableInfantry.register_output( - "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - deformableInfantry.register_output("/chassis/imu/pitch", chassis_imu_pitch_, 0.0); - deformableInfantry.register_output("/chassis/imu/roll", chassis_imu_roll_, 0.0); - deformableInfantry.register_output( - "/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, 0.0); - deformableInfantry.register_output( - "/chassis/imu/roll_rate", chassis_imu_roll_rate_, 0.0); - deformableInfantry.register_output( - "/chassis/left_front_joint/physical_angle", left_front_joint_physical_angle_, nan_); - deformableInfantry.register_output( - "/chassis/left_back_joint/physical_angle", left_back_joint_physical_angle_, nan_); - deformableInfantry.register_output( - "/chassis/right_back_joint/physical_angle", right_back_joint_physical_angle_, nan_); - deformableInfantry.register_output( - "/chassis/right_front_joint/physical_angle", right_front_joint_physical_angle_, - nan_); - deformableInfantry.register_output( - "/chassis/left_front_joint/physical_velocity", left_front_joint_physical_velocity_, - nan_); - deformableInfantry.register_output( - "/chassis/left_back_joint/physical_velocity", left_back_joint_physical_velocity_, - nan_); - deformableInfantry.register_output( - "/chassis/right_back_joint/physical_velocity", right_back_joint_physical_velocity_, - nan_); - deformableInfantry.register_output( - "/chassis/right_front_joint/physical_velocity", - right_front_joint_physical_velocity_, nan_); - deformableInfantry.register_output("/chassis/encoder/alpha", encoder_alpha_, nan_); - deformableInfantry.register_output( - "/chassis/encoder/alpha_dot", encoder_alpha_dot_, nan_); - deformableInfantry.register_output("/chassis/radius", radius_, default_radius_); - - deformableInfantry.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); - deformableInfantry.get_parameter_or( - "debug_log_wheel_motor", debug_log_wheel_motor_, false); - deformableInfantry.get_parameter_or( - "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); - - auto options = librmcs::board::AdvancedOptions{}; - options.dangerously_skip_version_checks = true; - board_ = std::make_unique( - *this, serial_filter, - options); - } - - void update() { - imu_.update_status(); - *chassis_yaw_velocity_imu_ = imu_.gz(); - { - const double q0 = imu_.q0(); - const double q1 = imu_.q1(); - const double q2 = imu_.q2(); - const double q3 = imu_.q3(); - - double sin_pitch = 2.0 * (q0 * q2 - q3 * q1); - sin_pitch = std::clamp(sin_pitch, -1.0, 1.0); - - const double standard_pitch = std::asin(sin_pitch); - const double standard_roll = - std::atan2(2.0 * (q0 * q1 + q2 * q3), 1.0 - 2.0 * (q1 * q1 + q2 * q2)); - - // Export chassis attitude using the requested convention: - // pitch < 0 when the front is higher, roll > 0 when the left side is higher. - *chassis_imu_pitch_ = -standard_pitch; - *chassis_imu_roll_ = standard_roll; - *chassis_imu_pitch_rate_ = -imu_.gy(); - *chassis_imu_roll_rate_ = imu_.gx(); - } - - for (auto& motor : chassis_wheel_motors_) - motor.update_status(); - for (auto& motor : chassis_joint_motors_) - motor.update_status(); - - update_joint_physical_feedback_( - 0, left_front_joint_physical_angle_, left_front_joint_physical_velocity_); - update_joint_physical_feedback_( - 1, left_back_joint_physical_angle_, left_back_joint_physical_velocity_); - update_joint_physical_feedback_( - 2, right_back_joint_physical_angle_, right_back_joint_physical_velocity_); - update_joint_physical_feedback_( - 3, right_front_joint_physical_angle_, right_front_joint_physical_velocity_); - - update_geometry_feedback_(); - if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) - log_chassis_feedback_once_per_second_(); - - dr16_.update_status(); - gimbal_yaw_motor_.update_status(); - if (supercap_status_received_.load(std::memory_order_relaxed)) - supercap_.update_status(); - if (debug_log_supercap_) - log_supercap_feedback_once_per_second_(); - gimbal_bullet_feeder_.update_status(); - - tf_->set_state( - gimbal_yaw_motor_.angle()); - } - - void command_update(bool even) { - auto builder = board_->start_transmit(); - if (even) { - builder.can_transmit(Spec::kCans.kCan0, { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can_transmit(Spec::kCans.kCan1, { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can_transmit(Spec::kCans.kCan2, { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[2].generate_command(), - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can_transmit(Spec::kCans.kCan3, { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[3].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can_transmit(Spec::kCans.kCan2, { - .can_id = 0x142, - .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes(), - }); - builder.can_transmit(Spec::kCans.kCan1, { - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - } - .as_bytes(), - }); - } else { - builder.can_transmit(Spec::kCans.kCan0, { - .can_id = 0x141, - .can_data = chassis_joint_motors_[0].generate_command().as_bytes(), - }); - builder.can_transmit(Spec::kCans.kCan1, { - .can_id = 0x141, - .can_data = chassis_joint_motors_[1].generate_command().as_bytes(), - }); - builder.can_transmit(Spec::kCans.kCan2, { - .can_id = 0x141, - .can_data = chassis_joint_motors_[2].generate_command().as_bytes(), - }); - builder.can_transmit(Spec::kCans.kCan3, { - .can_id = 0x141, - .can_data = chassis_joint_motors_[3].generate_command().as_bytes(), - }); - } - } - - DeformableInfantryOmniB& deformable_infantry_; - rmcs_executor::Component& command_; - - static constexpr double joint_zero_physical_angle_rad_ = 62.5 * std::numbers::pi / 180.0; - static constexpr double chassis_radius_base_ = 0.2341741; - static constexpr double rod_length_ = 0.150; - static constexpr double default_radius_ = 0.5 * rod_length_ + chassis_radius_base_; - - static double to_physical_angle_(double motor_angle) { - return joint_zero_physical_angle_rad_ - motor_angle; - } - - static double to_physical_velocity_(double motor_velocity) { return -motor_velocity; } - - void update_joint_physical_feedback_( - size_t index, OutputInterface& angle_output, - OutputInterface& velocity_output) { - if (!joint_status_received_[index].load(std::memory_order_relaxed)) { - *angle_output = nan_; - *velocity_output = nan_; - return; - } - - *angle_output = to_physical_angle_(chassis_joint_motors_[index].angle()); - *velocity_output = to_physical_velocity_(chassis_joint_motors_[index].velocity()); - } - - void update_geometry_feedback_() { - const Eigen::Vector4d alpha_rad{ - *left_front_joint_physical_angle_, *left_back_joint_physical_angle_, - *right_back_joint_physical_angle_, *right_front_joint_physical_angle_}; - const Eigen::Vector4d alpha_dot_rad{ - *left_front_joint_physical_velocity_, *left_back_joint_physical_velocity_, - *right_back_joint_physical_velocity_, *right_front_joint_physical_velocity_}; - - if (!alpha_rad.array().isFinite().all() || !alpha_dot_rad.array().isFinite().all()) { - *encoder_alpha_ = nan_; - *encoder_alpha_dot_ = nan_; - *radius_ = default_radius_; - RCLCPP_WARN_THROTTLE( - deformable_infantry_.get_logger(), *deformable_infantry_.get_clock(), 1000, - "deformable joint feedback invalid, fallback chassis radius to default %.3f m", - default_radius_); - return; - } - - *encoder_alpha_ = alpha_rad.mean(); - *encoder_alpha_dot_ = alpha_dot_rad.mean(); - *radius_ = (chassis_radius_base_ + rod_length_ * alpha_rad.array().cos()).mean(); - } - - void log_chassis_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_chassis_feedback_log_time_) - return; - - const auto wheel_rx = [this](size_t index) { - return wheel_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - const auto joint_rx = [this](size_t index) { - return joint_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - - if (debug_log_wheel_motor_) { - RCLCPP_INFO( - deformable_infantry_.get_logger(), - "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " - "rx=[%c %c %c %c]", - chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), - chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), - chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), - chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), wheel_rx(0), - wheel_rx(1), wheel_rx(2), wheel_rx(3)); - } - - if (debug_log_deformable_joint_motor_) { - RCLCPP_INFO( - deformable_infantry_.get_logger(), - "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "rx=[%c %c %c %c]", - *left_front_joint_physical_angle_, *left_back_joint_physical_angle_, - *right_back_joint_physical_angle_, *right_front_joint_physical_angle_, - *left_front_joint_physical_velocity_, *left_back_joint_physical_velocity_, - *right_back_joint_physical_velocity_, *right_front_joint_physical_velocity_, - joint_rx(0), joint_rx(1), joint_rx(2), joint_rx(3)); - } - - next_chassis_feedback_log_time_ = now + std::chrono::seconds(1); - } - - void log_supercap_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_supercap_feedback_log_time_) - return; - - const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); - auto supercap_raw_packet = latest_supercap_status_.load(std::memory_order_relaxed); - const auto supercap_raw_bytes = supercap_raw_packet.as_bytes(); - - RCLCPP_INFO( - deformable_infantry_.get_logger(), - "[supercap] can1 rx=%c id=0x300 enabled=%d supercap_v=% .3f chassis_v=% .3f " - "power=% .3f raw=[%02X %02X %02X %02X %02X %02X %02X %02X]", - supercap_rx ? 'Y' : 'N', supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, - supercap_rx ? supercap_.supercap_voltage() : nan_, - supercap_rx ? supercap_.chassis_voltage() : nan_, - supercap_rx ? supercap_.chassis_power() : nan_, - std::to_integer(supercap_raw_bytes[0]), - std::to_integer(supercap_raw_bytes[1]), - std::to_integer(supercap_raw_bytes[2]), - std::to_integer(supercap_raw_bytes[3]), - std::to_integer(supercap_raw_bytes[4]), - std::to_integer(supercap_raw_bytes[5]), - std::to_integer(supercap_raw_bytes[6]), - std::to_integer(supercap_raw_bytes[7])); - - next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); - } - - void can_receive_callback(const Spec::Can& can, const View::Can& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (can == Spec::kCans.kCan0) { - if (data.can_id == 0x201) { - chassis_wheel_motors_[0].store_status(data.can_data); - wheel_status_received_[0].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[0].store_status(data.can_data); - joint_status_received_[0].store(true, std::memory_order_relaxed); - } - } else if (can == Spec::kCans.kCan1) { - if (data.can_id == 0x201) { - chassis_wheel_motors_[1].store_status(data.can_data); - wheel_status_received_[1].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[1].store_status(data.can_data); - joint_status_received_[1].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x300) { - if (data.can_data.size() == 8) - latest_supercap_status_.store( - device::CanPacket8{data.can_data}, std::memory_order_relaxed); - supercap_.store_status(data.can_data); - supercap_status_received_.store(true, std::memory_order_relaxed); - } - } else if (can == Spec::kCans.kCan2) { - if (data.can_id == 0x201) { - chassis_wheel_motors_[2].store_status(data.can_data); - wheel_status_received_[2].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[2].store_status(data.can_data); - joint_status_received_[2].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x142) { - gimbal_yaw_motor_.store_status(data.can_data); - } else if (data.can_id == 0x203) { - gimbal_bullet_feeder_.store_status(data.can_data); - } - } else if (can == Spec::kCans.kCan3) { - if (data.can_id == 0x201) { - chassis_wheel_motors_[3].store_status(data.can_data); - wheel_status_received_[3].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[3].store_status(data.can_data); - joint_status_received_[3].store(true, std::memory_order_relaxed); - } - } - } - - void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { - if (uart == Spec::kUarts.kDbus) { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); - } else if (uart == Spec::kUarts.kUart0) { - const std::byte* ptr = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, - data.uart_data.size()); - } - } - - void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const View::ImuGyroscope& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - - OutputInterface& tf_; - - device::Bmi088 imu_{1000, 0.2, 0.0}; - device::LkMotor gimbal_yaw_motor_{deformable_infantry_, command_, "/gimbal/yaw"}; - device::Dr16 dr16_{deformable_infantry_}; - - device::DjiMotor chassis_wheel_motors_[4]{ - device::DjiMotor{deformable_infantry_, command_, "/chassis/left_front_wheel"}, - device::DjiMotor{deformable_infantry_, command_, "/chassis/left_back_wheel"}, - device::DjiMotor{deformable_infantry_, command_, "/chassis/right_back_wheel"}, - device::DjiMotor{deformable_infantry_, command_, "/chassis/right_front_wheel"}, - }; - device::LkMotor chassis_joint_motors_[4]{ - device::LkMotor{deformable_infantry_, command_, "/chassis/left_front_joint"}, - device::LkMotor{deformable_infantry_, command_, "/chassis/left_back_joint"}, - device::LkMotor{deformable_infantry_, command_, "/chassis/right_back_joint"}, - device::LkMotor{deformable_infantry_, command_, "/chassis/right_front_joint"}, - }; - - std::atomic wheel_status_received_[4] = {false, false, false, false}; - std::atomic joint_status_received_[4] = {false, false, false, false}; - bool debug_log_supercap_ = false; - bool debug_log_wheel_motor_ = false; - bool debug_log_deformable_joint_motor_ = false; - Clock::time_point next_chassis_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - Clock::time_point next_supercap_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - device::Supercap supercap_{deformable_infantry_, command_}; - std::atomic latest_supercap_status_{device::CanPacket8{uint64_t{0}}}; - std::atomic supercap_status_received_{false}; - device::DjiMotor gimbal_bullet_feeder_{ - deformable_infantry_, command_, "/gimbal/bullet_feeder"}; - - rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; - OutputInterface referee_serial_; - - OutputInterface chassis_yaw_velocity_imu_; - OutputInterface chassis_imu_pitch_; - OutputInterface chassis_imu_roll_; - OutputInterface chassis_imu_pitch_rate_; - OutputInterface chassis_imu_roll_rate_; - OutputInterface left_front_joint_physical_angle_; - OutputInterface left_back_joint_physical_angle_; - OutputInterface right_back_joint_physical_angle_; - OutputInterface right_front_joint_physical_angle_; - OutputInterface left_front_joint_physical_velocity_; - OutputInterface left_back_joint_physical_velocity_; - OutputInterface right_back_joint_physical_velocity_; - OutputInterface right_front_joint_physical_velocity_; - OutputInterface encoder_alpha_; - OutputInterface encoder_alpha_dot_; - OutputInterface radius_; - - std::unique_ptr board_; - }; - - struct ImuBoard final : public librmcs::board::RmcsBoardLite::Callback { - public: - explicit ImuBoard( - DeformableInfantryOmniB& deformableInfantry, const std::string& serial_filter = {}) - : tf_{deformableInfantry.tf_} - , bmi088_{1000, 0.2, 0.0} { - - deformableInfantry.register_output( - "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); - - bmi088_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(x, z, -y); }); - - auto options = librmcs::board::AdvancedOptions{}; - options.dangerously_skip_version_checks = true; - board_ = std::make_unique( - *this, serial_filter, - options); - } - - void update() { - bmi088_.update_status(); - Eigen::Quaterniond const gimbal_imu_pose{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - - tf_->set_transform( - gimbal_imu_pose.conjugate()); - - *gimbal_pitch_velocity_imu_ = bmi088_.gy(); - } - - void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { - (void)uart; - (void)data; - } - - void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const View::ImuGyroscope& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); - } - - OutputInterface& tf_; - OutputInterface gimbal_pitch_velocity_imu_; - - device::Bmi088 bmi088_; - - std::unique_ptr board_; - }; - - struct TopBoard final : public librmcs::board::RmcsBoardLite::Callback { - public: - explicit TopBoard( - DeformableInfantryOmniB& deformableInfantry, - DeformableInfantryOmniBCommand& deformableInfantry_command, - const std::string& serial_filter = {}, bool has_external_imu_board = false) - : has_external_imu_board_(has_external_imu_board) - , tf_(deformableInfantry.tf_) - , bmi088_(1000, 0.2, 0.0) - , gimbal_pitch_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/pitch") - , gimbal_left_friction_( - deformableInfantry, deformableInfantry_command, "/gimbal/left_friction") - , gimbal_right_friction_( - deformableInfantry, deformableInfantry_command, "/gimbal/right_friction") - , scope_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/scope") { - - gimbal_pitch_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} - .set_reversed() - .set_encoder_zero_point( - static_cast( - deformableInfantry.get_parameter("pitch_motor_zero_point").as_int()))); - - gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(1.) - .set_reversed()); - gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); - - scope_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); - - deformableInfantry.register_output( - "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - if (!has_external_imu_board_) - deformableInfantry.register_output( - "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_encoder_); - - bmi088_.set_coordinate_mapping([](double x, double y, double z) { - // Top board BMI088 maps to gimbal frame as (-x, -y, z). - return std::make_tuple(-x, -y, z); - }); - - auto options = librmcs::board::AdvancedOptions{}; - options.dangerously_skip_version_checks = true; - board_ = std::make_unique( - *this, serial_filter, - options); - } - - [[nodiscard]] auto gimbal_yaw_velocity() const -> double { - return *gimbal_yaw_velocity_bmi088_; - } - - void request_hard_sync_read() { - // RMCS-lite top board variant currently has no GPIO hard-sync request - // path. - } - - void update() { - bmi088_.update_status(); - - gimbal_pitch_motor_.update_status(); - gimbal_left_friction_.update_status(); - gimbal_right_friction_.update_status(); - scope_motor_.update_status(); - - const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); - - *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); - if (!has_external_imu_board_) { - Eigen::Quaterniond const odom_imu_to_yaw_link{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - Eigen::Quaterniond const yaw_link_to_odom_imu = odom_imu_to_yaw_link.conjugate(); - Eigen::Quaterniond pitch_link_to_odom_imu = - Eigen::Quaterniond{ - Eigen::AngleAxisd{-pitch_encoder_angle, Eigen::Vector3d::UnitY()}} - * yaw_link_to_odom_imu; - pitch_link_to_odom_imu.normalize(); - - *gimbal_pitch_velocity_encoder_ = gimbal_pitch_motor_.velocity(); - // The BMI088 is mounted on the yaw link. fast_tf stores PitchLink -> - // OdomImu, so use the encoder pitch from the TF tree to move the - // yaw-link pose back into PitchLink. - tf_->set_transform( - pitch_link_to_odom_imu); - } - - tf_->set_state( - pitch_encoder_angle); - } - - void command_update() { - auto builder = board_->start_transmit(); - builder.can_transmit(Spec::kCans.kCan0, { - .can_id = 0x141, - .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), - }); - - builder.can_transmit(Spec::kCans.kCan1, { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_left_friction_.generate_command(), - gimbal_right_friction_.generate_command(), - scope_motor_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - } - - void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { - (void)uart; - (void)data; - } - - void can_receive_callback(const Spec::Can& can, const View::Can& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - if (can == Spec::kCans.kCan0) { - if (data.can_id == 0x141) - gimbal_pitch_motor_.store_status(data.can_data); - } else if (can == Spec::kCans.kCan1) { - if (data.can_id == 0x201) - gimbal_left_friction_.store_status(data.can_data); - else if (data.can_id == 0x202) - gimbal_right_friction_.store_status(data.can_data); - else if (data.can_id == 0x203) - scope_motor_.store_status(data.can_data); - } - } - - void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const View::ImuGyroscope& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); - } - - bool has_external_imu_board_ = false; - OutputInterface& tf_; - - OutputInterface gimbal_yaw_velocity_bmi088_; - OutputInterface gimbal_pitch_velocity_encoder_; - - device::Bmi088 bmi088_; - device::LkMotor gimbal_pitch_motor_; - device::DjiMotor gimbal_left_friction_; - device::DjiMotor gimbal_right_friction_; - device::DjiMotor scope_motor_; - - std::unique_ptr board_; - }; - - auto status_service_callback(const std::shared_ptr& response) - -> void { - response->success = true; - - auto feedback_message = std::ostringstream{}; - auto text = [&](std::format_string format, Args&&... args) { - std::println(feedback_message, format, std::forward(args)...); - }; - - text("Gimbal Status"); - text("- Yaw: {}", rmcs_board_lite->gimbal_yaw_motor_.last_raw_angle()); - text("- Pitch: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); - - text("Chassis Status"); - text("- left front: {}", rmcs_board_lite->chassis_joint_motors_[0].last_raw_angle()); - text("- left back: {}", rmcs_board_lite->chassis_joint_motors_[1].last_raw_angle()); - text("- right back: {}", rmcs_board_lite->chassis_joint_motors_[2].last_raw_angle()); - text("- right front: {}", rmcs_board_lite->chassis_joint_motors_[3].last_raw_angle()); - - response->message = feedback_message.str(); - } - - OutputInterface tf_; - OutputInterface camera_transform_; - OutputInterface barrel_direction_; - OutputInterface auto_aim_yaw_velocity_; - InputInterface timestamp_; - std::atomic hard_sync_pending_{false}; - Clock::time_point next_hard_sync_log_time_{}; - - std::shared_ptr deformable_infantry_command_; - std::unique_ptr rmcs_board_lite; - std::unique_ptr imu_board_; - std::unique_ptr top_board_; - - std::shared_ptr> status_service_; - uint32_t cmd_tick_ = 0; -}; - -} // namespace rmcs_core::hardware - -#include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DeformableInfantryOmniB, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp deleted file mode 100644 index 5c3d5956d..000000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ /dev/null @@ -1,825 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include "hardware/device/bmi088.hpp" -#include "hardware/device/bmi088_ekf.hpp" -#include "hardware/device/board_clock_lifter.hpp" -#include "hardware/device/can_packet.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/lk_motor.hpp" -#include "hardware/device/supercap.hpp" - -namespace rmcs_core::hardware { - -using Clock = std::chrono::steady_clock; - -class DeformableInfantryOmni - : public rmcs_executor::Component - , public rclcpp::Node { -public: - DeformableInfantryOmni() - : Node( - get_component_name(), - rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) - , command_(create_partner_component(get_component_name() + "_command", *this)) { - using namespace rmcs_description; - - register_input("/predefined/timestamp", timestamp_); - register_output("/tf", tf_); - register_output( - "/auto_aim/camera_transform", camera_transform_, Eigen::Isometry3d::Identity()); - register_output("/auto_aim/barrel_direction", barrel_direction_, Eigen::Vector3d::UnitX()); - register_output("/auto_aim/yaw_velocity", auto_aim_yaw_velocity_, 0.0); - - tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); - - bottom_board_ = std::make_unique( - *this, *command_, get_parameter("serial_filter_bottom_board").as_string()); - top_board_ = std::make_unique( - *this, *command_, get_parameter("serial_filter_top_board").as_string()); - - // For command: remote-status - using Srv = std_srvs::srv::Trigger; - status_service_ = create_service( - "/rmcs/service/robot_status", - [this](const Srv::Request::SharedPtr&, const Srv::Response::SharedPtr& response) { - status_service_callback(response); - }); - } - - ~DeformableInfantryOmni() override = default; - - void before_updating() override { top_board_->request_hard_sync_read(); } - - void update() override { - bottom_board_->update(); - top_board_->update(); - - using namespace rmcs_description; - *camera_transform_ = fast_tf::lookup_transform(*tf_); - *barrel_direction_ = - *fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); - *auto_aim_yaw_velocity_ = top_board_->gimbal_yaw_velocity(); - } - - void command_update() { - const bool even = ((cmd_tick_++ & 1u) == 0u); - bottom_board_->command_update(even); - top_board_->command_update(); - } - -private: - static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); - static constexpr auto kLeftFront = 0; - static constexpr auto kLeftBack = 1; - static constexpr auto kRightBack = 2; - static constexpr auto kRightFront = 3; - static constexpr auto kJointName = std::array{ - "left_front", - "left_back", - "right_back", - "right_front", - }; - - class Command : public rmcs_executor::Component { - public: - explicit Command(DeformableInfantryOmni& deformableInfantry) - : deformableInfantry(deformableInfantry) {} - - void update() override { deformableInfantry.command_update(); } - - DeformableInfantryOmni& deformableInfantry; - }; - - struct BottomBoard final : public librmcs::board::RmcsBoardLite::Callback { - public: - explicit BottomBoard( - DeformableInfantryOmni& status, rmcs_executor::Component& command, - const std::string& serial_filter = {}) - : status_{status} - , command_{command} { - - status.register_output("/referee/serial", referee_serial_); - referee_serial_->read = [this](std::byte* buffer, size_t size) { - return referee_ring_buffer_receive_.pop_front_n( - [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); - }; - referee_serial_->write = [this](const std::byte* buffer, size_t size) { - board_->start_transmit().uart_transmit( - Spec::kUarts.kUart0, {.uart_data = std::span{buffer, size}}); - return size; - }; - - gimbal_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( - static_cast(status.get_parameter("yaw_motor_zero_point").as_int()))); - - for (auto& motor : chassis_wheel_motors_) - motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(19.0) - .enable_multi_turn_angle()); - - for (auto& motor : chassis_joint_motors_) - motor.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei36} - .set_reversed() - .enable_multi_turn_angle()); - - imu_.set_coordinate_mapping([](double x, double y, double z) { - // Keep the existing chassis yaw axis mapping explicit until the bottom-board IMU - // installation is re-validated on hardware. - return std::make_tuple(-y, x, z); - }); - - gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); - - status.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - status.register_output("/chassis/imu/pitch", chassis_imu_pitch_, 0.0); - status.register_output("/chassis/imu/roll", chassis_imu_roll_, 0.0); - status.register_output("/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, 0.0); - status.register_output("/chassis/imu/roll_rate", chassis_imu_roll_rate_, 0.0); - for (size_t i = 0; i < 4; ++i) { - status.register_output( - std::format( - "/chassis/{}_joint/physical_angle", DeformableInfantryOmni::kJointName[i]), - joint_physical_angle_[i], kNaN); - status.register_output( - std::format( - "/chassis/{}_joint/physical_velocity", - DeformableInfantryOmni::kJointName[i]), - joint_physical_velocity_[i], kNaN); - } - status.register_output("/chassis/encoder/alpha", encoder_alpha_, kNaN); - status.register_output("/chassis/encoder/alpha_dot", encoder_alpha_dot_, kNaN); - status.register_output("/chassis/radius", radius_, kDefaultRadius); - - status.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); - status.get_parameter_or("debug_log_wheel_motor", debug_log_wheel_motor_, false); - status.get_parameter_or( - "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); - - auto options = librmcs::board::AdvancedOptions{}; - options.dangerously_skip_version_checks = true; - board_ = std::make_unique(*this, serial_filter, options); - } - - void update() { - imu_.update_status(); - *chassis_yaw_velocity_imu_ = imu_.gz(); - { - const double q0 = imu_.q0(); - const double q1 = imu_.q1(); - const double q2 = imu_.q2(); - const double q3 = imu_.q3(); - - double sin_pitch = 2.0 * (q0 * q2 - q3 * q1); - sin_pitch = std::clamp(sin_pitch, -1.0, 1.0); - - const double standard_pitch = std::asin(sin_pitch); - const double standard_roll = - std::atan2(2.0 * (q0 * q1 + q2 * q3), 1.0 - 2.0 * (q1 * q1 + q2 * q2)); - - // Export chassis attitude using the requested convention: - // pitch < 0 when the front is higher, roll > 0 when the left side is higher. - *chassis_imu_pitch_ = -standard_pitch; - *chassis_imu_roll_ = standard_roll; - *chassis_imu_pitch_rate_ = -imu_.gy(); - *chassis_imu_roll_rate_ = imu_.gx(); - } - - for (auto& motor : chassis_wheel_motors_) - motor.update_status(); - for (auto& motor : chassis_joint_motors_) - motor.update_status(); - - for (size_t i = 0; i < 4; ++i) - update_joint_physical_feedback_( - i, joint_physical_angle_[i], joint_physical_velocity_[i]); - - update_geometry_feedback_(); - if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) - log_chassis_feedback_once_per_second_(); - - dr16_.update_status(); - gimbal_yaw_motor_.update_status(); - if (supercap_status_received_.load(std::memory_order_relaxed)) - supercap_.update_status(); - if (debug_log_supercap_) - log_supercap_feedback_once_per_second_(); - gimbal_bullet_feeder_.update_status(); - - tf_->set_state( - gimbal_yaw_motor_.angle()); - } - - void command_update(bool even) { - auto builder = board_->start_transmit(); - if (even) { - builder.can_transmit( - Spec::kCans.kCan0, - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[kLeftFront].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan1, - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[kLeftBack].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan2, - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[kRightBack].generate_command(), - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan3, - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[kRightFront].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan2, - { - .can_id = 0x142, - .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan1, // - { - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - } - .as_bytes(), - }); - } else { - for (size_t i = 0; i < 4; ++i) { - switch (i) { - case kLeftFront: - builder.can_transmit( - Spec::kCans.kCan0, - { - .can_id = 0x141, - .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), - }); - break; - case kLeftBack: - builder.can_transmit( - Spec::kCans.kCan1, - { - .can_id = 0x141, - .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), - }); - break; - case kRightBack: - builder.can_transmit( - Spec::kCans.kCan2, - { - .can_id = 0x141, - .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), - }); - break; - case kRightFront: - builder.can_transmit( - Spec::kCans.kCan3, - { - .can_id = 0x141, - .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), - }); - break; - default: break; - } - } - } - } - - static constexpr double kJointZeroPhysicalAngleRad = 62.5 * std::numbers::pi / 180.0; - static constexpr double kChassisRadiusBase = 0.2341741; - static constexpr double kRodLength = 0.150; - static constexpr double kDefaultRadius = kChassisRadiusBase + kRodLength; - - DeformableInfantryOmni& status_; - Component& command_; - - std::unique_ptr board_; - - // Interfaces - - OutputInterface& tf_{status_.tf_}; - - OutputInterface chassis_yaw_velocity_imu_; - OutputInterface chassis_imu_pitch_; - OutputInterface chassis_imu_roll_; - OutputInterface chassis_imu_pitch_rate_; - OutputInterface chassis_imu_roll_rate_; - - std::array, 4> joint_physical_angle_; - std::array, 4> joint_physical_velocity_; - - OutputInterface encoder_alpha_; - OutputInterface encoder_alpha_dot_; - OutputInterface radius_; - - rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; - OutputInterface referee_serial_; - - // State - - std::atomic wheel_status_received_[4] = {false, false, false, false}; - std::atomic joint_status_received_[4] = {false, false, false, false}; - - bool debug_log_supercap_ = false; - bool debug_log_wheel_motor_ = false; - bool debug_log_deformable_joint_motor_ = false; - - Clock::time_point next_chassis_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - Clock::time_point next_supercap_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - - // Device - - device::Bmi088 imu_{1000, 0.2, 0.0}; - device::LkMotor gimbal_yaw_motor_{status_, command_, "/gimbal/yaw"}; - device::Dr16 dr16_{status_}; - - device::DjiMotor chassis_wheel_motors_[4]{ - device::DjiMotor{status_, command_, "/chassis/left_front_wheel"}, - device::DjiMotor{status_, command_, "/chassis/left_back_wheel"}, - device::DjiMotor{status_, command_, "/chassis/right_back_wheel"}, - device::DjiMotor{status_, command_, "/chassis/right_front_wheel"}, - }; - device::LkMotor chassis_joint_motors_[4]{ - device::LkMotor{status_, command_, "/chassis/left_front_joint"}, - device::LkMotor{status_, command_, "/chassis/left_back_joint"}, - device::LkMotor{status_, command_, "/chassis/right_back_joint"}, - device::LkMotor{status_, command_, "/chassis/right_front_joint"}, - }; - - std::atomic latest_supercap_status_{device::CanPacket8{uint64_t{0}}}; - std::atomic supercap_status_received_{false}; - device::Supercap supercap_{status_, command_}; - - device::DjiMotor gimbal_bullet_feeder_{status_, command_, "/gimbal/bullet_feeder"}; - - void process_chassis_can_receive_(size_t index, const View::Can& data) { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[index].store_status(data.can_data); - wheel_status_received_[index].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[index].store_status(data.can_data); - joint_status_received_[index].store(true, std::memory_order_relaxed); - } - } - - void update_joint_physical_feedback_( - size_t index, OutputInterface& angle_output, - OutputInterface& velocity_output) { - - if (!joint_status_received_[index].load(std::memory_order_relaxed)) { - *angle_output = kNaN; - *velocity_output = kNaN; - return; - } - - const auto to_physical_angle = [](double motor_angle) { - return kJointZeroPhysicalAngleRad - motor_angle; - }; - const auto to_physical_velocity = [](double motor_velocity) { return -motor_velocity; }; - - *angle_output = to_physical_angle(chassis_joint_motors_[index].angle()); - *velocity_output = to_physical_velocity(chassis_joint_motors_[index].velocity()); - } - - void update_geometry_feedback_() { - const Eigen::Vector4d alpha_rad{ - *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], - *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront]}; - const Eigen::Vector4d alpha_dot_rad{ - *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], - *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront]}; - - if (!alpha_rad.array().isFinite().all() || !alpha_dot_rad.array().isFinite().all()) { - *encoder_alpha_ = kNaN; - *encoder_alpha_dot_ = kNaN; - *radius_ = kDefaultRadius; - RCLCPP_WARN_THROTTLE( - status_.get_logger(), *status_.get_clock(), 1000, - "deformable joint feedback invalid, fallback chassis radius to default %.3f m", - kDefaultRadius); - return; - } - - *encoder_alpha_ = alpha_rad.mean(); - *encoder_alpha_dot_ = alpha_dot_rad.mean(); - *radius_ = (kChassisRadiusBase + kRodLength * alpha_rad.array().cos()).mean(); - } - - void log_chassis_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_chassis_feedback_log_time_) - return; - - const auto wheel_rx = [this](size_t index) { - return wheel_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - const auto joint_rx = [this](size_t index) { - return joint_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - - if (debug_log_wheel_motor_) { - std::string wheel_rx_str; - for (size_t i = 0; i < 4; ++i) { - if (i > 0) - wheel_rx_str.push_back(' '); - wheel_rx_str.push_back(wheel_rx(i)); - } - RCLCPP_INFO( - status_.get_logger(), - "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " - "rx=[%s]", - chassis_wheel_motors_[kLeftFront].angle(), - chassis_wheel_motors_[kLeftBack].angle(), - chassis_wheel_motors_[kRightBack].angle(), - chassis_wheel_motors_[kRightFront].angle(), - chassis_wheel_motors_[kLeftFront].angle(), - chassis_wheel_motors_[kLeftBack].angle(), - chassis_wheel_motors_[kRightBack].angle(), - chassis_wheel_motors_[kRightFront].angle(), wheel_rx_str.c_str()); - } - - if (debug_log_deformable_joint_motor_) { - std::string joint_rx_str; - for (size_t i = 0; i < 4; ++i) { - if (i > 0) - joint_rx_str.push_back(' '); - joint_rx_str.push_back(joint_rx(i)); - } - RCLCPP_INFO( - status_.get_logger(), - "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "rx=[%s]", - *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], - *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront], - *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], - *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront], - joint_rx_str.c_str()); - } - - next_chassis_feedback_log_time_ = now + std::chrono::seconds(1); - } - - void log_supercap_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_supercap_feedback_log_time_) - return; - - const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); - auto supercap_raw_packet = latest_supercap_status_.load(std::memory_order_relaxed); - const auto supercap_raw_bytes = supercap_raw_packet.as_bytes(); - - RCLCPP_INFO( - status_.get_logger(), - "[supercap] can1 rx=%c id=0x300 enabled=%d supercap_v=% .3f chassis_v=% .3f " - "power=% .3f raw=[%02X %02X %02X %02X %02X %02X %02X %02X]", - supercap_rx ? 'Y' : 'N', supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, - supercap_rx ? supercap_.supercap_voltage() : kNaN, - supercap_rx ? supercap_.chassis_voltage() : kNaN, - supercap_rx ? supercap_.chassis_power() : kNaN, - std::to_integer(supercap_raw_bytes[0]), - std::to_integer(supercap_raw_bytes[1]), - std::to_integer(supercap_raw_bytes[2]), - std::to_integer(supercap_raw_bytes[3]), - std::to_integer(supercap_raw_bytes[4]), - std::to_integer(supercap_raw_bytes[5]), - std::to_integer(supercap_raw_bytes[6]), - std::to_integer(supercap_raw_bytes[7])); - - next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); - } - - void can_receive_callback(const Spec::Can& can, const View::Can& data) override { - if (can == Spec::kCans.kCan0) { - process_chassis_can_receive_(0, data); - } else if (can == Spec::kCans.kCan1) { - process_chassis_can_receive_(1, data); - if (!data.is_extended_can_id && !data.is_remote_transmission - && data.can_id == 0x300) { - if (data.can_data.size() == 8) - latest_supercap_status_.store( - device::CanPacket8{data.can_data}, std::memory_order_relaxed); - supercap_.store_status(data.can_data); - supercap_status_received_.store(true, std::memory_order_relaxed); - } - } else if (can == Spec::kCans.kCan2) { - process_chassis_can_receive_(2, data); - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x142) - gimbal_yaw_motor_.store_status(data.can_data); - else if (data.can_id == 0x203) - gimbal_bullet_feeder_.store_status(data.can_data); - } else if (can == Spec::kCans.kCan3) { - process_chassis_can_receive_(3, data); - } - } - - void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { - if (uart == Spec::kUarts.kDbus) { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); - } else if (uart == Spec::kUarts.kUart0) { - const std::byte* ptr = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, - data.uart_data.size()); - } - } - - void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const View::ImuGyroscope& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - }; - - struct TopBoard final : public librmcs::board::RmcsBoardLite::Callback { - public: - explicit TopBoard( - DeformableInfantryOmni& status, rmcs_executor::Component& command, - const std::string& serial_filter = {}) - : tf_{status.tf_} - , bmi088_{device::Bmi088Ekf::Config{ - .body_to_sensor = (Eigen::Matrix3d() << 1, 0, 0, 0, 0, -1, 0, 1, 0).finished()}} - , gimbal_pitch_motor_(status, command, "/gimbal/pitch") - , gimbal_left_friction_(status, command, "/gimbal/left_friction") - , gimbal_right_friction_(status, command, "/gimbal/right_friction") { - - gimbal_pitch_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} - .set_reversed() - .set_encoder_zero_point( - static_cast(status.get_parameter("pitch_motor_zero_point").as_int()))); - - gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(1.) - .set_reversed()); - gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); - - status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); - status.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); - status.register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); - - auto options = librmcs::board::AdvancedOptions{}; - options.dangerously_skip_version_checks = true; - board_ = std::make_unique(*this, serial_filter, options); - - board_->start_transmit().gpio_digital_read( - Spec::kGpios.kUart1Rx, { - .period_ms = 0, - .asap = false, - .rising_edge = false, - .falling_edge = true, - .capture_timestamp = true, - .pull = librmcs::data::GpioPull::kUp, - }); - } - - ~TopBoard() override = default; - - [[nodiscard]] auto gimbal_yaw_velocity() const -> double { - return *gimbal_yaw_velocity_bmi088_; - } - - void request_hard_sync_read() { - // RMCS-lite top board variant currently has no GPIO hard-sync request - // path. - } - - void update() { - gimbal_pitch_motor_.update_status(); - gimbal_left_friction_.update_status(); - gimbal_right_friction_.update_status(); - - const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); - - if (auto snapshot = bmi088_.snapshot()) { - *gimbal_pitch_velocity_bmi088_ = snapshot->gyro_body.y(); - *gimbal_yaw_velocity_bmi088_ = snapshot->gyro_body.z(); - tf_->set_transform( - snapshot->orientation.conjugate()); - } - - tf_->set_state( - pitch_encoder_angle); - } - - void command_update() const { - auto builder = board_->start_transmit(); - builder.can_transmit( - Spec::kCans.kCan0, - { - .can_id = 0x141, - .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan1, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_left_friction_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan2, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - gimbal_right_friction_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - } - - void can_receive_callback(const Spec::Can& can, const View::Can& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - if (can == Spec::kCans.kCan0) { - if (data.can_id == 0x141) - gimbal_pitch_motor_.store_status(data.can_data); - } else if (can == Spec::kCans.kCan1) { - if (data.can_id == 0x201) - gimbal_left_friction_.store_status(data.can_data); - } else if (can == Spec::kCans.kCan2) { - if (data.can_id == 0x202) - gimbal_right_friction_.store_status(data.can_data); - } - } - - void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { - const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); - bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); - } - - void gyroscope_receive_callback(const View::ImuGyroscope& data) override { - const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); - if (!timestamp.has_value()) - return; - auto snapshot = - bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); - if (snapshot) - imu_snapshot_output_.emit(*snapshot); - } - - void gpio_digital_read_result_callback( - const Spec::Gpio& gpio, const View::GpioDigital& data) override { - if (gpio != Spec::kGpios.kUart1Rx) - return; - if (!data.timestamp_quarter_us) - return; - - const auto timestamp = board_clock_lifter_.lift_timestamp(*data.timestamp_quarter_us); - if (!timestamp.has_value()) - return; - - camera_signal_output_.emit(*timestamp); - } - - OutputInterface& tf_; - OutputInterface gimbal_yaw_velocity_bmi088_; - OutputInterface gimbal_pitch_velocity_bmi088_; - - EventOutputInterface imu_snapshot_output_; - EventOutputInterface camera_signal_output_; - - device::Bmi088Ekf bmi088_; - device::BoardClockLifter board_clock_lifter_; - device::LkMotor gimbal_pitch_motor_; - device::DjiMotor gimbal_left_friction_; - device::DjiMotor gimbal_right_friction_; - - std::unique_ptr board_; - }; - - auto status_service_callback(const std::shared_ptr& response) - -> void { - response->success = true; - - auto feedback_message = std::ostringstream{}; - auto text = [&](std::format_string format, Args&&... args) { - std::println(feedback_message, format, std::forward(args)...); - }; - - text("Gimbal Status"); - text("- Yaw: {}", bottom_board_->gimbal_yaw_motor_.last_raw_angle()); - text("- Pitch: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); - - text("Chassis Status"); - constexpr auto kPosition = - std::array{"left front", "left back", "right back", "right front"}; - constexpr auto kMaxLength = - std::ranges::max_element(kPosition, {}, &std::string_view::size)->size(); - - for (auto&& [index, motor] : - std::views::zip(kPosition, bottom_board_->chassis_joint_motors_)) { - text("- {:{}}: {}", index, kMaxLength, motor.last_raw_angle()); - } - - response->message = feedback_message.str(); - } - - OutputInterface tf_; - OutputInterface camera_transform_; - OutputInterface barrel_direction_; - OutputInterface auto_aim_yaw_velocity_; - InputInterface timestamp_; - - std::unique_ptr bottom_board_; - std::unique_ptr top_board_; - - std::shared_ptr command_; - uint32_t cmd_tick_ = 0; - - std::shared_ptr> status_service_; -}; - -} // namespace rmcs_core::hardware - -#include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DeformableInfantryOmni, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp index 51895755b..cd0a8fbc2 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp @@ -1,12 +1,12 @@ #pragma once -#include -#include -#include - #include #include #include +#include +#include +#include +#include #include #include @@ -19,231 +19,176 @@ class Dr16 { public: Dr16() = default; - void store_status(const std::byte* uart_data, size_t uart_data_length) { - if (uart_data_length != 6 + 8 + 4) + void store_status(std::span uart_data) { + if (uart_data.size() != kStatusSize) return; - // Avoid using reinterpret_cast here because it does not account for pointer alignment. - // Dr16DataPart structures are aligned, and using reinterpret_cast on potentially unaligned - // uart_data can cause undefined behavior on architectures that enforce strict alignment - // requirements (e.g., ARM). - // Directly accessing unaligned memory through a casted pointer can lead to crashes, - // inefficiencies, or incorrect data reads. Instead, std::memcpy safely copies the data from - // unaligned memory to properly aligned structures without violating alignment or strict - // aliasing rules. + last_receive_time_.store(Clock::now(), std::memory_order_relaxed); + + auto* cursor = uart_data.data(); uint64_t part1{}; - std::memcpy(&part1, uart_data, 6); - uart_data += 6; + std::memcpy(&part1, cursor, kPart1Size); + cursor += kPart1Size; data_part1_.store(part1, std::memory_order::relaxed); uint64_t part2{}; - std::memcpy(&part2, uart_data, 8); - uart_data += 8; + std::memcpy(&part2, cursor, kPart2Size); + cursor += kPart2Size; data_part2_.store(part2, std::memory_order::relaxed); uint32_t part3{}; - std::memcpy(&part3, uart_data, 4); - uart_data += 4; + std::memcpy(&part3, cursor, kPart3Size); data_part3_.store(part3, std::memory_order::relaxed); - - last_remote_control_received_at_ = Clock::now(); - valid_ = true; } void update_status() { - const auto now = Clock::now(); - refresh_validity(now); - if (!valid_) - return; - - auto part1 alignas(uint64_t) = - std::bit_cast(data_part1_.load(std::memory_order::relaxed)); + const auto raw_part1 = data_part1_.load(std::memory_order::relaxed); + const auto part1 = std::bit_cast(raw_part1); - auto channel_to_double = [](int32_t value) { - value -= 1024; - if (-660 <= value && value <= 660) - return value / 660.0; - return 0.0; + joystick_right_ = { + channel_to_double(static_cast(part1.joystick_channel1)), + -channel_to_double(static_cast(part1.joystick_channel0)), + }; + joystick_left_ = { + channel_to_double(static_cast(part1.joystick_channel3)), + -channel_to_double(static_cast(part1.joystick_channel2)), }; - joystick_right_.y = -channel_to_double(static_cast(part1.joystick_channel0)); - joystick_right_.x = channel_to_double(static_cast(part1.joystick_channel1)); - joystick_left_.y = -channel_to_double(static_cast(part1.joystick_channel2)); - joystick_left_.x = channel_to_double(static_cast(part1.joystick_channel3)); - - switch_right_ = static_cast(part1.switch_right); - switch_left_ = static_cast(part1.switch_left); - - auto part2 alignas(uint64_t) = - std::bit_cast(data_part2_.load(std::memory_order::relaxed)); - mouse_velocity_.x = -part2.mouse_velocity_y / 32768.0; - mouse_velocity_.y = -part2.mouse_velocity_x / 32768.0; + switch_right_ = static_cast(part1.switch_right); + switch_left_ = static_cast(part1.switch_left); - mouse_wheel_ = -part2.mouse_velocity_z / 32768.0; + const auto raw_part2 = data_part2_.load(std::memory_order::relaxed); + const auto part2 = std::bit_cast(raw_part2); - mouse_.left = part2.mouse_left; - mouse_.right = part2.mouse_right; + mouse_velocity_ = { + -static_cast(part2.mouse_velocity_y) / 32768.0, + -static_cast(part2.mouse_velocity_x) / 32768.0, + }; + mouse_wheel_ = -static_cast(part2.mouse_velocity_z) / 32768.0; + mouse_ = { + .left = part2.mouse_left, + .right = part2.mouse_right, + }; - auto part3 alignas(uint32_t) = - std::bit_cast(data_part3_.load(std::memory_order::relaxed)); + const auto raw_part3 = data_part3_.load(std::memory_order::relaxed); + const auto part3 = std::bit_cast(raw_part3); - keyboard_ = part3.keyboard; + keyboard_ = std::bit_cast(part3.keyboard); rotary_knob_ = channel_to_double(part3.rotary_knob); - update_rotary_knob_switch(); } - struct Vector { - constexpr static Vector zero() { return {.x = 0, .y = 0}; } - double x, y; - }; - - enum class Switch : uint8_t { kUnknown = 0, kUp = 1, kDown = 2, kMiddle = 3 }; - - struct [[gnu::packed]] Mouse { - constexpr static Mouse zero() { - constexpr uint8_t zero = 0; - return std::bit_cast(zero); - } - - bool left : 1; - bool right : 1; - }; - static_assert(sizeof(Mouse) == 1); + [[nodiscard]] const Eigen::Vector2d& joystick_right() const noexcept { return joystick_right_; } - struct [[gnu::packed]] Keyboard { - constexpr static Keyboard zero() { - constexpr uint16_t zero = 0; - return std::bit_cast(zero); - } - - bool w : 1; - bool s : 1; - bool a : 1; - bool d : 1; - bool shift : 1; - bool ctrl : 1; - bool q : 1; - bool e : 1; - bool r : 1; - bool f : 1; - bool g : 1; - bool z : 1; - bool x : 1; - bool c : 1; - bool v : 1; - bool b : 1; - }; - static_assert(sizeof(Keyboard) == 2); - - Eigen::Vector2d joystick_right() const { return to_eigen_vector(joystick_right_); } - Eigen::Vector2d joystick_left() const { return to_eigen_vector(joystick_left_); } - - rmcs_msgs::Switch switch_right() const { - return std::bit_cast(switch_right_); + [[nodiscard]] bool valid() const noexcept { + const auto last_receive_time = last_receive_time_.load(std::memory_order_relaxed); + return last_receive_time != TimePoint::min() + && Clock::now() - last_receive_time <= kFreshTimeout; } - rmcs_msgs::Switch switch_left() const { return std::bit_cast(switch_left_); } - Eigen::Vector2d mouse_velocity() const { return to_eigen_vector(mouse_velocity_); } + [[nodiscard]] const Eigen::Vector2d& joystick_left() const noexcept { return joystick_left_; } - rmcs_msgs::Mouse mouse() const { return std::bit_cast(mouse_); } - rmcs_msgs::Keyboard keyboard() const { return std::bit_cast(keyboard_); } + [[nodiscard]] rmcs_msgs::Switch switch_right() const noexcept { return switch_right_; } - rmcs_msgs::Switch rotary_knob_switch() const { return rotary_knob_switch_; } + [[nodiscard]] rmcs_msgs::Switch switch_left() const noexcept { return switch_left_; } - bool valid() const noexcept { return valid_; } + [[nodiscard]] const Eigen::Vector2d& mouse_velocity() const noexcept { return mouse_velocity_; } - void set_timeout_enabled(bool enabled) { timeout_enabled_ = enabled; } + [[nodiscard]] rmcs_msgs::Mouse mouse() const noexcept { return mouse_; } - double rotary_knob() const { return rotary_knob_; } + [[nodiscard]] rmcs_msgs::Keyboard keyboard() const noexcept { return keyboard_; } - double mouse_wheel() const { return mouse_wheel_; } + [[nodiscard]] double rotary_knob() const noexcept { return rotary_knob_; } -private: - static Eigen::Vector2d to_eigen_vector(Vector vector) { return {vector.x, vector.y}; } + [[nodiscard]] double mouse_wheel() const noexcept { return mouse_wheel_; } - void update_rotary_knob_switch() { - constexpr double divider = 0.7, anti_shake_shift = 0.05; - double upper_divider = divider, lower_divider = -divider; - - auto switch_value = rotary_knob_switch_; - if (switch_value == rmcs_msgs::Switch::UP) - upper_divider -= anti_shake_shift, lower_divider -= anti_shake_shift; - else if (switch_value == rmcs_msgs::Switch::MIDDLE) - upper_divider += anti_shake_shift, lower_divider -= anti_shake_shift; - else if (switch_value == rmcs_msgs::Switch::DOWN) - upper_divider += anti_shake_shift, lower_divider += anti_shake_shift; - - const auto knob_value = -rotary_knob_; - if (knob_value > upper_divider) { - switch_value = rmcs_msgs::Switch::UP; - } else if (knob_value < lower_divider) { - switch_value = rmcs_msgs::Switch::DOWN; - } else { - switch_value = rmcs_msgs::Switch::MIDDLE; - } - rotary_knob_switch_ = switch_value; + [[nodiscard]] rmcs_msgs::Switch rotary_knob_switch() const noexcept { + return rotary_knob_switch_; } +private: using Clock = std::chrono::steady_clock; using TimePoint = Clock::time_point; static constexpr auto kFreshTimeout = std::chrono::milliseconds(500); - void refresh_validity(const TimePoint now) { - if (!timeout_enabled_ || !valid_ || now - last_remote_control_received_at_ <= kFreshTimeout) - return; - - reset_remote_control_state(); - valid_ = false; - } - - void reset_remote_control_state() { - joystick_right_ = Vector::zero(); - joystick_left_ = Vector::zero(); - switch_right_ = Switch::kUnknown; - switch_left_ = Switch::kUnknown; - mouse_velocity_ = Vector::zero(); - mouse_wheel_ = 0.0; - mouse_ = Mouse::zero(); - keyboard_ = Keyboard::zero(); - rotary_knob_ = 0.0; - rotary_knob_switch_ = rmcs_msgs::Switch::UNKNOWN; - } + static constexpr std::size_t kPart1Size = 6; + static constexpr std::size_t kPart2Size = 8; + static constexpr std::size_t kPart3Size = 4; + static constexpr std::size_t kStatusSize = kPart1Size + kPart2Size + kPart3Size; struct [[gnu::packed]] Dr16DataPart1 { uint64_t joystick_channel0 : 11; uint64_t joystick_channel1 : 11; uint64_t joystick_channel2 : 11; uint64_t joystick_channel3 : 11; - uint64_t switch_right : 2; - uint64_t switch_left : 2; - + uint64_t switch_left : 2; uint64_t padding : 16; }; static_assert(sizeof(Dr16DataPart1) == 8); - std::atomic data_part1_{std::bit_cast(Dr16DataPart1{ - .joystick_channel0 = 1024, - .joystick_channel1 = 1024, - .joystick_channel2 = 1024, - .joystick_channel3 = 1024, - .switch_right = static_cast(Switch::kUnknown), - .switch_left = static_cast(Switch::kUnknown), - .padding = 0, - })}; - static_assert(decltype(data_part1_)::is_always_lock_free); struct [[gnu::packed]] Dr16DataPart2 { int16_t mouse_velocity_x; int16_t mouse_velocity_y; int16_t mouse_velocity_z; - bool mouse_left; bool mouse_right; }; static_assert(sizeof(Dr16DataPart2) == 8); + + struct [[gnu::packed]] Dr16DataPart3 { + uint16_t keyboard; + uint16_t rotary_knob; + }; + static_assert(sizeof(Dr16DataPart3) == 4); + + static double channel_to_double(int32_t value) { + value -= 1024; + if (-660 <= value && value <= 660) + return value / 660.0; + return 0.0; + } + + void update_rotary_knob_switch() { + constexpr double divider = 0.7; + constexpr double anti_shake_shift = 0.05; + + double upper_divider = divider; + double lower_divider = -divider; + if (rotary_knob_switch_ == rmcs_msgs::Switch::UP) { + upper_divider -= anti_shake_shift; + lower_divider -= anti_shake_shift; + } else if (rotary_knob_switch_ == rmcs_msgs::Switch::MIDDLE) { + upper_divider += anti_shake_shift; + lower_divider -= anti_shake_shift; + } else if (rotary_knob_switch_ == rmcs_msgs::Switch::DOWN) { + upper_divider += anti_shake_shift; + lower_divider += anti_shake_shift; + } + + const auto knob_value = -rotary_knob_; + if (knob_value > upper_divider) { + rotary_knob_switch_ = rmcs_msgs::Switch::UP; + } else if (knob_value < lower_divider) { + rotary_knob_switch_ = rmcs_msgs::Switch::DOWN; + } else { + rotary_knob_switch_ = rmcs_msgs::Switch::MIDDLE; + } + } + + std::atomic data_part1_{std::bit_cast(Dr16DataPart1{ + .joystick_channel0 = 1024, + .joystick_channel1 = 1024, + .joystick_channel2 = 1024, + .joystick_channel3 = 1024, + .switch_right = static_cast(rmcs_msgs::Switch::UNKNOWN), + .switch_left = static_cast(rmcs_msgs::Switch::UNKNOWN), + .padding = 0, + })}; + static_assert(decltype(data_part1_)::is_always_lock_free); + std::atomic data_part2_{std::bit_cast(Dr16DataPart2{ .mouse_velocity_x = 0, .mouse_velocity_y = 0, @@ -253,34 +198,27 @@ class Dr16 { })}; static_assert(decltype(data_part2_)::is_always_lock_free); - struct [[gnu::packed]] Dr16DataPart3 { - Keyboard keyboard; - uint16_t rotary_knob; - }; - static_assert(sizeof(Dr16DataPart3) == 4); - std::atomic data_part3_ = {std::bit_cast(Dr16DataPart3{ - .keyboard = Keyboard::zero(), + std::atomic data_part3_{std::bit_cast(Dr16DataPart3{ + .keyboard = 0, .rotary_knob = 0, })}; static_assert(decltype(data_part3_)::is_always_lock_free); - Vector joystick_right_ = Vector::zero(); - Vector joystick_left_ = Vector::zero(); + std::atomic last_receive_time_{TimePoint::min()}; - Switch switch_right_ = Switch::kUnknown; - Switch switch_left_ = Switch::kUnknown; + Eigen::Vector2d joystick_right_ = Eigen::Vector2d::Zero(); + Eigen::Vector2d joystick_left_ = Eigen::Vector2d::Zero(); + Eigen::Vector2d mouse_velocity_ = Eigen::Vector2d::Zero(); - Vector mouse_velocity_ = Vector::zero(); - double mouse_wheel_ = 0.0; + rmcs_msgs::Switch switch_right_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Switch switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Switch rotary_knob_switch_ = rmcs_msgs::Switch::UNKNOWN; - Mouse mouse_ = Mouse::zero(); - Keyboard keyboard_ = Keyboard::zero(); + rmcs_msgs::Mouse mouse_ = rmcs_msgs::Mouse::zero(); + rmcs_msgs::Keyboard keyboard_ = rmcs_msgs::Keyboard::zero(); double rotary_knob_ = 0.0; - rmcs_msgs::Switch rotary_knob_switch_ = rmcs_msgs::Switch::UNKNOWN; - TimePoint last_remote_control_received_at_ = TimePoint::min(); - bool valid_ = false; - bool timeout_enabled_ = true; + double mouse_wheel_ = 0.0; }; -} // namespace rmcs_core::hardware::device +} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp index 689436bc6..236f3821b 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp @@ -13,17 +13,11 @@ namespace rmcs_core::hardware::device { -/* -遥控输入仲裁： -- vt13 valid S挡：vt13主控 | 比赛用 -- vt13 valid C挡：等同于dr16双下 | 疯车救车 -- 其他情况：dr16主控；dr16无效则进入空安全态 -- 旋钮始终来自 dr16，dr16 无效则清零 -*/ - class RemoteControl { public: - explicit RemoteControl(rmcs_executor::Component& component) { + RemoteControl(rmcs_executor::Component& component, Dr16& dr16, Vt13& vt13) + : dr16_(dr16) + , vt13_(vt13) { component.register_output( "/remote/joystick/right", joystick_right_output_, Eigen::Vector2d::Zero()); component.register_output( @@ -47,116 +41,52 @@ class RemoteControl { "/remote/keyboard", keyboard_output_, rmcs_msgs::Keyboard::zero()); } - void register_dr16(Dr16* dr16) { dr16_ = dr16; } - void register_vt13(Vt13* vt13) { vt13_ = vt13; } - void update() { - update_timeout_interlock(); - - const auto control_source = select_control_source(); - const auto snapshot = build_snapshot(control_source); + if (dr16_.valid() || !vt13_.valid() || vt13_.mode_switch() == Vt13::ModeSwitch::kNormal) { + *switch_right_output_ = dr16_.switch_right(); + *switch_left_output_ = dr16_.switch_left(); - *joystick_right_output_ = snapshot.joystick_right; - *joystick_left_output_ = snapshot.joystick_left; + *joystick_right_output_ = dr16_.joystick_right(); + *joystick_left_output_ = dr16_.joystick_left(); - *switch_right_output_ = snapshot.switch_right; - *switch_left_output_ = snapshot.switch_left; + *mouse_velocity_output_ = dr16_.mouse_velocity(); + *mouse_wheel_output_ = dr16_.mouse_wheel(); - *mouse_velocity_output_ = snapshot.mouse_velocity; - *mouse_wheel_output_ = snapshot.mouse_wheel; + *mouse_output_ = dr16_.mouse(); + *keyboard_output_ = dr16_.keyboard(); + } else if (vt13_.mode_switch() == Vt13::ModeSwitch::kCine) { + *switch_right_output_ = rmcs_msgs::Switch::DOWN; + *switch_left_output_ = rmcs_msgs::Switch::DOWN; - *mouse_output_ = snapshot.mouse; - *keyboard_output_ = snapshot.keyboard; + *joystick_right_output_ = Eigen::Vector2d::Zero(); + *joystick_left_output_ = Eigen::Vector2d::Zero(); - if (dr16_ && dr16_->valid()) { - *rotary_knob_output_ = dr16_->rotary_knob(); - *rotary_knob_switch_output_ = dr16_->rotary_knob_switch(); - } else { - *rotary_knob_output_ = 0.0; - *rotary_knob_switch_output_ = rmcs_msgs::Switch::UNKNOWN; - } - } + *mouse_velocity_output_ = Eigen::Vector2d::Zero(); + *mouse_wheel_output_ = 0; -private: - enum class ControlSource { - kDr16, - kVt13Sport, - kCineSafe, - kInvalidSafe, - }; - - struct Snapshot { - Eigen::Vector2d joystick_right = Eigen::Vector2d::Zero(); - Eigen::Vector2d joystick_left = Eigen::Vector2d::Zero(); - - rmcs_msgs::Switch switch_right = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Switch switch_left = rmcs_msgs::Switch::UNKNOWN; - - Eigen::Vector2d mouse_velocity = Eigen::Vector2d::Zero(); - double mouse_wheel = 0.0; - - rmcs_msgs::Mouse mouse = rmcs_msgs::Mouse::zero(); - rmcs_msgs::Keyboard keyboard = rmcs_msgs::Keyboard::zero(); - }; - - // 超时互锁：仅当对方 valid 时本设备才允许超时失效，保证至少一路不失效 - auto update_timeout_interlock() const -> void { - const auto dr16_ok = dr16_ && dr16_->valid(); - const auto vt13_ok = vt13_ && vt13_->valid(); - if (dr16_) - dr16_->set_timeout_enabled(vt13_ok); - if (vt13_) - vt13_->set_timeout_enabled(dr16_ok); - } + *mouse_output_ = rmcs_msgs::Mouse::zero(); + *keyboard_output_ = rmcs_msgs::Keyboard::zero(); + } else if (vt13_.mode_switch() == Vt13::ModeSwitch::kSport) { + *switch_right_output_ = rmcs_msgs::Switch::MIDDLE; + *switch_left_output_ = rmcs_msgs::Switch::MIDDLE; - ControlSource select_control_source() const { - if (vt13_ && vt13_->valid()) { - switch (vt13_->mode_switch()) { - case Vt13::ModeSwitch::kSport: return ControlSource::kVt13Sport; - case Vt13::ModeSwitch::kCine: return ControlSource::kCineSafe; - case Vt13::ModeSwitch::kNormal: - case Vt13::ModeSwitch::kUnknown: break; - } - } + *joystick_right_output_ = vt13_.joystick_right(); + *joystick_left_output_ = vt13_.joystick_left(); - return (dr16_ && dr16_->valid()) ? ControlSource::kDr16 : ControlSource::kInvalidSafe; - } + *mouse_velocity_output_ = vt13_.mouse_velocity(); + *mouse_wheel_output_ = vt13_.mouse_wheel(); - Snapshot build_snapshot(ControlSource source) const { - Snapshot snapshot{}; - switch (source) { - case ControlSource::kDr16: - snapshot.joystick_right = dr16_->joystick_right(); - snapshot.joystick_left = dr16_->joystick_left(); - snapshot.switch_right = dr16_->switch_right(); - snapshot.switch_left = dr16_->switch_left(); - snapshot.mouse_velocity = dr16_->mouse_velocity(); - snapshot.mouse_wheel = dr16_->mouse_wheel(); - snapshot.mouse = dr16_->mouse(); - snapshot.keyboard = dr16_->keyboard(); - break; - case ControlSource::kVt13Sport: - snapshot.joystick_right = vt13_->joystick_right(); - snapshot.joystick_left = vt13_->joystick_left(); - snapshot.switch_right = rmcs_msgs::Switch::MIDDLE; - snapshot.switch_left = rmcs_msgs::Switch::MIDDLE; - snapshot.mouse_velocity = vt13_->mouse_velocity(); - snapshot.mouse_wheel = vt13_->mouse_wheel(); - snapshot.mouse = vt13_->mouse(); - snapshot.keyboard = vt13_->keyboard(); - break; - case ControlSource::kCineSafe: - snapshot.switch_right = rmcs_msgs::Switch::DOWN; - snapshot.switch_left = rmcs_msgs::Switch::DOWN; - break; - case ControlSource::kInvalidSafe: break; + *mouse_output_ = vt13_.mouse(); + *keyboard_output_ = vt13_.keyboard(); } - return snapshot; + *rotary_knob_output_ = dr16_.rotary_knob(); + *rotary_knob_switch_output_ = dr16_.rotary_knob_switch(); } - Dr16* dr16_{nullptr}; - Vt13* vt13_{nullptr}; +private: + Dr16& dr16_; + Vt13& vt13_; rmcs_executor::Component::OutputInterface joystick_right_output_; rmcs_executor::Component::OutputInterface joystick_left_output_; @@ -174,4 +104,4 @@ class RemoteControl { rmcs_executor::Component::OutputInterface keyboard_output_; }; -} // namespace rmcs_core::hardware::device +} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp index 327999820..168ffa52c 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp @@ -92,21 +92,20 @@ class Vt13 { } refresh_validity(now); + maybe_log_statistics(now); } - ModeSwitch mode_switch() const noexcept { return mode_switch_; } - bool valid() const noexcept { return valid_; } + [[nodiscard]] ModeSwitch mode_switch() const noexcept { return mode_switch_; } + [[nodiscard]] bool valid() const noexcept { return valid_; } - void set_timeout_enabled(bool enabled) { timeout_enabled_ = enabled; } + [[nodiscard]] const Eigen::Vector2d& joystick_left() const noexcept { return joystick_left_; } + [[nodiscard]] const Eigen::Vector2d& joystick_right() const noexcept { return joystick_right_; } - const Eigen::Vector2d& joystick_left() const noexcept { return joystick_left_; } - const Eigen::Vector2d& joystick_right() const noexcept { return joystick_right_; } + [[nodiscard]] const Eigen::Vector2d& mouse_velocity() const noexcept { return mouse_velocity_; } + [[nodiscard]] double mouse_wheel() const noexcept { return mouse_wheel_; } - const Eigen::Vector2d& mouse_velocity() const noexcept { return mouse_velocity_; } - double mouse_wheel() const noexcept { return mouse_wheel_; } - - rmcs_msgs::Mouse mouse() const noexcept { return mouse_; } - rmcs_msgs::Keyboard keyboard() const noexcept { return keyboard_; } + [[nodiscard]] rmcs_msgs::Mouse mouse() const noexcept { return mouse_; } + [[nodiscard]] rmcs_msgs::Keyboard keyboard() const noexcept { return keyboard_; } private: using Clock = std::chrono::steady_clock; @@ -280,13 +279,57 @@ class Vt13 { } void refresh_validity(const TimePoint now) { - if (!timeout_enabled_ || !valid_ || now - last_remote_control_received_at_ <= kFreshTimeout) + if (!valid_ || now - last_remote_control_received_at_ <= kFreshTimeout) return; reset_remote_control_state(); valid_ = false; } + void maybe_log_statistics(const TimePoint now) { + if (last_statistics_log_time_ == TimePoint::min()) { + last_statistics_log_time_ = now; + return; + } + + const auto elapsed = now - last_statistics_log_time_; + if (elapsed < kStatisticsLogInterval) + return; + + const auto readable = data_buffer_.readable(); + const auto store_calls = store_calls_.exchange(0, std::memory_order_relaxed); + const auto received_bytes = received_bytes_.exchange(0, std::memory_order_relaxed); + const auto overflow_count = overflow_count_.exchange(0, std::memory_order_relaxed); + const auto overflow_dropped_bytes = + overflow_dropped_bytes_.exchange(0, std::memory_order_relaxed); + const auto elapsed_seconds = std::chrono::duration(elapsed).count(); + + /* RCLCPP_INFO( + logger_, + "VT13 stats: rx=%.1f Hz %.1f B/s remote_ok=%zu verify_fail=%zu remote_bad_header=%zu " + "remote_bad_crc=%zu referee_discarded=%zu referee_bad_crc8=%zu referee_oversize=%zu " + "unknown_prefix=%zu overflow=%llu dropped=%llu readable=%zu peak=%zu valid=%s", + static_cast(store_calls) / elapsed_seconds, + static_cast(received_bytes) / elapsed_seconds, remote_success_count_, + verification_failures_, remote_bad_header_count_, remote_bad_crc_count_, + referee_discarded_count_, referee_bad_crc8_count_, referee_oversize_count_, + unknown_prefix_count_, static_cast(overflow_count), + static_cast(overflow_dropped_bytes), readable, peak_readable_, + valid_ ? "true" : "false"); + */ + + remote_success_count_ = 0; + verification_failures_ = 0; + remote_bad_header_count_ = 0; + remote_bad_crc_count_ = 0; + referee_discarded_count_ = 0; + referee_bad_crc8_count_ = 0; + referee_oversize_count_ = 0; + unknown_prefix_count_ = 0; + peak_readable_ = readable; + last_statistics_log_time_ = now; + } + void reset_remote_control_state() { mode_switch_ = ModeSwitch::kUnknown; joystick_left_ = Eigen::Vector2d::Zero(); @@ -318,7 +361,6 @@ class Vt13 { TimePoint last_statistics_log_time_ = TimePoint::min(); bool valid_ = false; - bool timeout_enabled_ = true; std::size_t peak_readable_ = 0; std::size_t remote_success_count_ = 0; std::size_t verification_failures_ = 0; @@ -341,4 +383,4 @@ class Vt13 { rmcs_msgs::Keyboard keyboard_ = rmcs_msgs::Keyboard::zero(); }; -} // namespace rmcs_core::hardware::device +} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp deleted file mode 100644 index 9b6aa67fd..000000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp +++ /dev/null @@ -1,490 +0,0 @@ -#include "hardware/device/bmi088.hpp" -#include "hardware/device/can_packet.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/lk_motor.hpp" -#include "hardware/device/supercap.hpp" - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace rmcs_core::hardware { - -class Sentry - : public rmcs_executor::Component - , public rclcpp::Node { -public: - Sentry() - : Node( - get_component_name(), - rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) { - - register_input("/predefined/timestamp", timestamp_); - register_output("/tf", tf_); - - // For command: remote-status - using Srv = std_srvs::srv::Trigger; - status_service_ = create_service( - "/rmcs/service/robot_status", - [this](const Srv::Request::SharedPtr&, const Srv::Response::SharedPtr& response) { - status_service_callback(response); - }); - - top_board_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_top_board").as_string()); - - bottom_board_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_bottom_board").as_string()); - - gimbal_board_ = - std::make_unique(get_parameter("board_serial_gimbal_board").as_string()); - - tf_->set_transform( - Eigen::Translation3d{0.08, 0.0, 0.0}); - tf_->set_transform( - Eigen::Translation3d{0.07128, 0.0, 0.0481}); - } - - auto update() -> void override { - top_board_->update(); - bottom_board_->update(); - gimbal_board_->update(); - tf_->set_transform( - gimbal_board_->imu_pose().conjugate()); - } - -private: - class GimbalBoard final : public librmcs::board::CBoard::Callback { - public: - explicit GimbalBoard(std::string_view board_serial = {}) { - bmi088_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(y, -x, z); }); - board_ = std::make_unique(*this, board_serial); - } - - GimbalBoard(const GimbalBoard&) = delete; - GimbalBoard& operator=(const GimbalBoard&) = delete; - GimbalBoard(GimbalBoard&&) = delete; - GimbalBoard& operator=(GimbalBoard&&) = delete; - - ~GimbalBoard() override = default; - - auto update() -> void { - bmi088_.update_status(); - imu_pose_ = Eigen::Quaterniond{bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - } - - auto imu_pose() const -> Eigen::Quaterniond { return imu_pose_; } - - private: - void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const View::ImuGyroscope& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); - } - - device::Bmi088 bmi088_{1000, 0.2, 0.0}; - Eigen::Quaterniond imu_pose_ = Eigen::Quaterniond::Identity(); - std::unique_ptr board_; - }; - - class TopBoard final : public librmcs::board::RmcsBoardLite::Callback { - friend class Sentry; - - public: - explicit TopBoard( - Sentry& sentry, rmcs_executor::Component& sentry_command, - std::string_view board_serial = {}, - librmcs::board::AdvancedOptions options = {}) - : tf_(sentry.tf_) - , bmi088_(1000, 0.2, 0.0) - , gimbal_pitch_motor_(sentry, sentry_command, "/gimbal/pitch") - , gimbal_top_yaw_motor_(sentry, sentry_command, "/gimbal/top_yaw") - , gimbal_bullet_feeder_(sentry, sentry_command, "/gimbal/bullet_feeder") - , gimbal_left_friction_(sentry, sentry_command, "/gimbal/left_friction") - , gimbal_right_friction_(sentry, sentry_command, "/gimbal/right_friction") { - gimbal_pitch_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( - static_cast(sentry.get_parameter("pitch_motor_zero_point").as_int()))); - - gimbal_top_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( - static_cast(sentry.get_parameter("top_yaw_motor_zero_point").as_int()))); - - gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .enable_multi_turn_angle() - .set_reversed() - .set_reduction_ratio(19 * 2)); - - gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); - gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(1.) - .set_reversed()); - - sentry.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - sentry.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); - - bmi088_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(-x, -y, z); }); - - board_ = std::make_unique(*this, board_serial, options); - } - - auto update() -> void { - gimbal_top_yaw_motor_.update_status(); - gimbal_pitch_motor_.update_status(); - - const auto pitch_angle = - std::remainder(gimbal_pitch_motor_.angle(), 2.0 * std::numbers::pi_v); - - bmi088_.update_status(); - const Eigen::Quaterniond gimbal_bmi088_pose{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - - tf_->set_transform( - gimbal_bmi088_pose.conjugate()); - - *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); - *gimbal_pitch_velocity_bmi088_ = bmi088_.gy(); - - gimbal_bullet_feeder_.update_status(); - gimbal_left_friction_.update_status(); - gimbal_right_friction_.update_status(); - - tf_->set_state( - gimbal_top_yaw_motor_.angle()); - tf_->set_state(pitch_angle); - } - - auto command_update() -> void { - auto builder = board_->start_transmit(); - - builder.can_transmit(Spec::kCans.kCan0, { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_right_friction_.generate_command(), - gimbal_left_friction_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - } - .as_bytes(), - }); - - builder.can_transmit(Spec::kCans.kCan3, { - .can_id = 0x141, - .can_data = gimbal_top_yaw_motor_.generate_torque_command().as_bytes(), - }); - - builder.can_transmit(Spec::kCans.kCan2, { - .can_id = 0x141, - .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), - }); - } - - private: - void can_receive_callback(const Spec::Can& can, const View::Can& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - if (can == Spec::kCans.kCan0) { - if (can_id == 0x202) { - gimbal_left_friction_.store_status(data.can_data); - } else if (can_id == 0x201) { - gimbal_right_friction_.store_status(data.can_data); - } else if (can_id == 0x204) { - gimbal_bullet_feeder_.store_status(data.can_data); - } - } else if (can == Spec::kCans.kCan2) { - if (can_id == 0x141) - gimbal_pitch_motor_.store_status(data.can_data); - } else if (can == Spec::kCans.kCan3) { - if (can_id == 0x141) - gimbal_top_yaw_motor_.store_status(data.can_data); - } - } - - void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const View::ImuGyroscope& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); - } - - OutputInterface& tf_; - - OutputInterface gimbal_yaw_velocity_bmi088_; - OutputInterface gimbal_pitch_velocity_bmi088_; - - device::Bmi088 bmi088_; - device::LkMotor gimbal_pitch_motor_; - device::LkMotor gimbal_top_yaw_motor_; - device::DjiMotor gimbal_bullet_feeder_; - - device::DjiMotor gimbal_left_friction_; - device::DjiMotor gimbal_right_friction_; - std::unique_ptr board_; - }; - - class BottomBoard final : public librmcs::board::CBoard::Callback { - friend class Sentry; - - public: - explicit BottomBoard( - Sentry& sentry, rmcs_executor::Component& sentry_command, - std::string_view board_serial = {}) - : imu_(1000, 0.2, 0.0) - , tf_(sentry.tf_) - , dr16_(sentry) - , gimbal_bottom_yaw_motor_(sentry, sentry_command, "/gimbal/bottom_yaw") - , chassis_wheel_motors_( - {sentry, sentry_command, "/chassis/left_front_wheel"}, - {sentry, sentry_command, "/chassis/left_back_wheel"}, - {sentry, sentry_command, "/chassis/right_back_wheel"}, - {sentry, sentry_command, "/chassis/right_front_wheel"}) - , chassis_steer_motors_( - {sentry, sentry_command, "/chassis/left_front_steering"}, - {sentry, sentry_command, "/chassis/left_back_steering"}, - {sentry, sentry_command, "/chassis/right_back_steering"}, - {sentry, sentry_command, "/chassis/right_front_steering"}) - , supercap_(sentry, sentry_command) { - sentry.register_output("/referee/serial", referee_serial_); - - referee_serial_->read = [this](std::byte* buffer, size_t size) { - return referee_ring_buffer_receive_.pop_front_n( - [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); - }; - referee_serial_->write = [this](const std::byte* buffer, size_t size) { - board_->start_transmit().uart_transmit( - Spec::kUarts.kUart1, - {.uart_data = std::span{buffer, size}}); - return size; - }; - - const auto zero_point = sentry.get_parameter("bottom_yaw_motor_zero_point").as_int(); - gimbal_bottom_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} - .set_reversed() - .set_encoder_zero_point(static_cast(zero_point))); - - for (auto& motor : chassis_wheel_motors_) { - motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(11.) - .enable_multi_turn_angle() - .set_reversed()); - } - - constexpr auto kSteerNames = std::array{ - "right_back_zero_point", - "right_front_zero_point", - "left_front_zero_point", - "left_back_zero_point", - }; - for (auto&& [motor, name] : std::views::zip(chassis_steer_motors_, kSteerNames)) { - const auto zero_point = sentry.get_parameter(name).as_int(); - motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point(static_cast(zero_point)) - .enable_multi_turn_angle()); - } - - sentry.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - - board_ = std::make_unique(*this, board_serial); - } - - auto update() -> void { - imu_.update_status(); - *chassis_yaw_velocity_imu_ = imu_.gz(); - supercap_.update_status(); - - for (auto& motor : chassis_wheel_motors_) - motor.update_status(); - for (auto& motor : chassis_steer_motors_) - motor.update_status(); - - dr16_.update_status(); - gimbal_bottom_yaw_motor_.update_status(); - tf_->set_state( - gimbal_bottom_yaw_motor_.angle()); - } - - auto command_update() -> void { - using namespace device; - - auto builder = board_->start_transmit(); - builder.can_transmit(Spec::kCans.kCan1, { - .can_id = 0x141, - .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), - }); - - auto cache = CanPacket8{}; - auto generate = [&](std::uint32_t id, std::ranges::range auto& motors, auto... args) { - auto command = [&](T arg) { - if constexpr (std::same_as) { - return arg; - } else { - const auto valid = arg >= 0 && arg < 4; - return valid ? motors[arg].generate_command() - : CanPacket8::PaddingQuarter{}; - } - }; - cache = CanPacket8{command(args)...}; - return librmcs::data::CanDataView{.can_id = id, .can_data = cache.as_bytes()}; - }; - - if (can_transmission_mode_) { - builder.can_transmit(Spec::kCans.kCan1, generate(0x200, chassis_wheel_motors_, 1, 0, -1, -1)) - .can_transmit(Spec::kCans.kCan2, generate(0x200, chassis_wheel_motors_, -1, 2, -1, 3)); - } else { - builder.can_transmit(Spec::kCans.kCan1, generate(0x1FE, chassis_steer_motors_, 1, 0, -1, -1)) - .can_transmit(Spec::kCans.kCan2, generate( - 0x1FE, chassis_steer_motors_, 2, 3, -1, supercap_.generate_command())); - } - can_transmission_mode_ = !can_transmission_mode_; - } - - private: - void can_receive_callback(const Spec::Can& can, const View::Can& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - if (can == Spec::kCans.kCan1) { - if (can_id == 0x201) - chassis_wheel_motors_[1].store_status(data.can_data); - else if (can_id == 0x202) - chassis_wheel_motors_[0].store_status(data.can_data); - else if (can_id == 0x205) - chassis_steer_motors_[1].store_status(data.can_data); - else if (can_id == 0x206) - chassis_steer_motors_[0].store_status(data.can_data); - else if (can_id == 0x141) - gimbal_bottom_yaw_motor_.store_status(data.can_data); - } else if (can == Spec::kCans.kCan2) { - if (can_id == 0x202) - chassis_wheel_motors_[2].store_status(data.can_data); - else if (can_id == 0x204) - chassis_wheel_motors_[3].store_status(data.can_data); - else if (can_id == 0x205) - chassis_steer_motors_[2].store_status(data.can_data); - else if (can_id == 0x206) - chassis_steer_motors_[3].store_status(data.can_data); - else if (can_id == 0x300) - supercap_.store_status(data.can_data); - } - } - - void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { - if (uart == Spec::kUarts.kDbus) { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); - } else if (uart == Spec::kUarts.kUart1) { - const auto* uart_data = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&uart_data](std::byte* storage) noexcept { *storage = *uart_data++; }, - data.uart_data.size()); - } - } - - void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const View::ImuGyroscope& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - - bool can_transmission_mode_ = true; - device::Bmi088 imu_; - OutputInterface& tf_; - - device::Dr16 dr16_; - device::LkMotor gimbal_bottom_yaw_motor_; - device::DjiMotor chassis_wheel_motors_[4]; - device::DjiMotor chassis_steer_motors_[4]; - device::Supercap supercap_; - - rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; - OutputInterface referee_serial_; - OutputInterface chassis_yaw_velocity_imu_; - std::unique_ptr board_; - }; - - struct CommandTransmitter : public rmcs_executor::Component { - std::function fn; - - template - explicit CommandTransmitter(Fn&& fn) - : fn{std::forward(fn)} {} - - auto update() -> void override { fn(); } - }; - - auto status_service_callback(const std::shared_ptr& response) - -> void { - response->success = true; - - auto feedback_message = std::ostringstream{}; - auto text = [&](std::format_string format, Args&&... args) { - std::println(feedback_message, format, std::forward(args)...); - }; - - text("Gimbal Status"); - text("- Bottom Yaw: {}", bottom_board_->gimbal_bottom_yaw_motor_.last_raw_angle()); - text("- Top Yaw: {}", top_board_->gimbal_top_yaw_motor_.last_raw_angle()); - text("- Pitch Angle: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); - - text("Chassis Status"); - constexpr auto kPosition = - std::array{"right back", "right front", "left front", "left back"}; - constexpr auto kMaxLength = - std::ranges::max_element(kPosition, {}, &std::string_view::size)->size(); - - for (auto&& [index, motor] : - std::views::zip(kPosition, bottom_board_->chassis_steer_motors_)) { - text("- {:{}}: {}", index, kMaxLength, motor.last_raw_angle()); - } - - response->message = feedback_message.str(); - } - - auto command_update() -> void { - top_board_->command_update(); - bottom_board_->command_update(); - } - std::shared_ptr command_component_{ - create_partner_component( - get_component_name() + "_command", [this] { command_update(); })}; - - InputInterface timestamp_; - OutputInterface tf_; - - std::unique_ptr gimbal_board_; - std::unique_ptr top_board_; - std::unique_ptr bottom_board_; - - std::shared_ptr> status_service_; -}; - -} // namespace rmcs_core::hardware - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Sentry, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six--friction.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six--friction.cpp index 0e50770a1..fd4ccb9b7 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six--friction.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six--friction.cpp @@ -32,7 +32,9 @@ #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" +#include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" +#include "hardware/device/vt13.hpp" namespace rmcs_core::hardware { @@ -131,6 +133,9 @@ class SteeringHeroLittle bottom_board_ = std::make_unique( *this, *command_component_, get_parameter("serial_bottom_rmcs_board").as_string()); + remote_control_ = + std::make_unique(*this, bottom_board_->dr16_, top_board_->vt13_); + tf_->set_transform( Eigen::Translation3d{0.06603, 0.0, 0.082}); } @@ -145,6 +150,7 @@ class SteeringHeroLittle void update() override { top_board_->update(); bottom_board_->update(); + remote_control_->update(); tf_->set_state( bottom_board_->gimbal_bottom_yaw_motor_.angle() @@ -316,6 +322,7 @@ class SteeringHeroLittle // can3_receive_rate_counter_.report_if_due(); imu_.update_status(); + vt13_.update_status(); Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; tf_->set_transform( @@ -482,6 +489,10 @@ class SteeringHeroLittle imu_.store_gyroscope_status(data.x, data.y, data.z); } + void uart0_receive_callback(const librmcs::data::UartDataView& data) override { + vt13_.store_status(data.uart_data); + } + rclcpp::Logger logger_; // CanReceiveRateCounter can0_receive_rate_counter_; // CanReceiveRateCounter can1_receive_rate_counter_; @@ -494,6 +505,7 @@ class SteeringHeroLittle int friciton_detect[6]; device::Bmi088 imu_; + device::Vt13 vt13_; device::LkMotor gimbal_top_yaw_motor_; device::LkMotor gimbal_pitch_motor_; device::DjiMotor gimbal_friction_wheels_[6]; @@ -526,7 +538,6 @@ class SteeringHeroLittle // , can2_receive_rate_counter_(logger_, "bottom/can2") // , can3_receive_rate_counter_(logger_, "bottom/can3") , imu_(1000, 0.2, 0.0) - , dr16_(steering_hero) , supercap_(steering_hero, steering_hero_command) , chassis_steering_motors_( {steering_hero, steering_hero_command, "/chassis/left_front_steering"}, @@ -833,7 +844,7 @@ class SteeringHeroLittle } void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + dr16_.store_status(data.uart_data); } void accelerometer_receive_callback( @@ -880,6 +891,7 @@ class SteeringHeroLittle std::shared_ptr top_board_; std::shared_ptr bottom_board_; + std::unique_ptr remote_control_; }; } // namespace rmcs_core::hardware diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp index b9b26ddec..91d70545a 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp @@ -32,7 +32,9 @@ // #include "hardware/device/dji_motor.hpp" // #include "hardware/device/dr16.hpp" // #include "hardware/device/lk_motor.hpp" +// #include "hardware/device/remote_control.hpp" // #include "hardware/device/supercap.hpp" +// #include "hardware/device/vt13.hpp" // namespace rmcs_core::hardware { @@ -131,6 +133,10 @@ // bottom_board_ = std::make_unique( // *this, *command_component_, get_parameter("serial_bottom_rmcs_board").as_string()); +// remote_control_ = +// std::make_unique(*this, bottom_board_->dr16_, +// top_board_->vt13_); + // tf_->set_transform( // Eigen::Translation3d{0.06603, 0.0, 0.082}); // } @@ -145,6 +151,7 @@ // void update() override { // top_board_->update(); // bottom_board_->update(); +// remote_control_->update(); // tf_->set_state( // bottom_board_->gimbal_bottom_yaw_motor_.angle() @@ -307,6 +314,7 @@ // // can3_receive_rate_counter_.report_if_due(); // imu_.update_status(); +// vt13_.update_status(); // Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; // tf_->set_transform( @@ -485,6 +493,10 @@ // imu_.store_gyroscope_status(data.x, data.y, data.z); // } +// void uart0_receive_callback(const librmcs::data::UartDataView& data) override { +// vt13_.store_status(data.uart_data); +// } + // rclcpp::Logger logger_; // // CanReceiveRateCounter can0_receive_rate_counter_; // // CanReceiveRateCounter can1_receive_rate_counter_; @@ -495,6 +507,7 @@ // std::time_t last_camera_capturer_trigger_timestamp_{0}; // device::Bmi088 imu_; +// device::Vt13 vt13_; // device::LkMotor gimbal_top_yaw_motor_; // device::LkMotor gimbal_pitch_motor_; // device::DjiMotor gimbal_friction_wheels_[4]; @@ -527,7 +540,6 @@ // // , can2_receive_rate_counter_(logger_, "bottom/can2") // // , can3_receive_rate_counter_(logger_, "bottom/can3") // , imu_(1000, 0.2, 0.0) -// , dr16_(steering_hero) // , supercap_(steering_hero, steering_hero_command) // , chassis_steering_motors_( // {steering_hero, steering_hero_command, "/chassis/left_front_steering"}, @@ -837,7 +849,7 @@ // } // void dbus_receive_callback(const librmcs::data::UartDataView& data) override { -// dr16_.store_status(data.uart_data.data(), data.uart_data.size()); +// dr16_.store_status(data.uart_data); // } // void accelerometer_receive_callback( @@ -884,6 +896,7 @@ // std::shared_ptr top_board_; // std::shared_ptr bottom_board_; +// std::unique_ptr remote_control_; // }; // } // namespace rmcs_core::hardware From fea28b4fb262d55b96b38bf1e4da6af2e64f6274 Mon Sep 17 00:00:00 2001 From: dwx5 <1591215786@qq.com> Date: Wed, 8 Jul 2026 23:39:08 +0800 Subject: [PATCH 27/86] friction_sweep and parameter changes --- .../steering-hero-little-six-friction.yaml | 134 ++++++++++++++---- rmcs_ws/src/rmcs_core/plugins.xml | 4 + .../controller/shooting/putter_controller.cpp | 8 +- .../steering-hero-little-six--friction.cpp | 12 +- 4 files changed, 127 insertions(+), 31 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml index c14b1d314..b58584b46 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml @@ -47,14 +47,20 @@ rmcs_executor: # - rmcs_core::controller::identification::StaticTorqueTestController -> pitch_static_torque_test_controller # - rmcs_core::controller::identification::SweptFrequencyController -> top_yaw_swept_frequency_controller # - rmcs_core::controller::identification::SweptFrequencyController -> bottom_yaw_swept_frequency_controller + # - rmcs_core::controller::identification::SweptFrequencyController -> first_front_friction_swept_frequency_controller + # - rmcs_core::controller::identification::SweptFrequencyController -> second_front_friction_swept_frequency_controller + # - rmcs_core::controller::identification::SweptFrequencyController -> third_front_friction_swept_frequency_controller + # - rmcs_core::controller::identification::SweptFrequencyController -> first_back_friction_swept_frequency_controller + # - rmcs_core::controller::identification::SweptFrequencyController -> second_back_friction_swept_frequency_controller + # - rmcs_core::controller::identification::SweptFrequencyController -> third_back_friction_swept_frequency_controller hero_hardware: ros__parameters: board_serial_top_board: "AF-8AE3-4EC1-03C3-C494-88FE-2DC4-3018-0298" serial_bottom_rmcs_board: "AF-60BB-7484-FA24-F3FC-399B-454D-22FA-1B1D" bottom_yaw_motor_zero_point: 34622 - pitch_motor_zero_point: 23251 - top_yaw_motor_zero_point: 48181 + pitch_motor_zero_point: 23653 + top_yaw_motor_zero_point: 48150 viewer_motor_zero_point: 31940 external_imu_port: /dev/ttyUSB0 bullet_feeder_motor_zero_point: 60480 #39045 @@ -89,6 +95,12 @@ value_broadcaster: - /gimbal/second_back_friction/velocity - /gimbal/third_front_friction/velocity - /gimbal/third_back_friction/velocity + - /gimbal/first_front_friction/control_torque + - /gimbal/first_back_friction/control_torque + - /gimbal/second_front_friction/control_torque + - /gimbal/second_back_friction/control_torque + - /gimbal/third_front_friction/control_torque + - /gimbal/third_back_friction/control_torque # - /gimbal/bottom_yaw/torque # - /gimbal/bottom_yaw/angle # - /gimbal/top_yaw/angle @@ -105,7 +117,7 @@ value_broadcaster: # - /gimbal/bullet_feeder/torque # - /gimbal/bullet_feeder/control_torque # - /gimbal/putter/angle - # - /gimbal/putter/velocity + - /gimbal/putter/velocity # - /gimbal/putter/torque # - /gimbal/putter/control_torque # - /gimbal/bullet_feeder/velocity @@ -226,12 +238,12 @@ friction_wheel_controller: - /gimbal/second_back_friction - /gimbal/third_back_friction friction_velocities_profile_0: - - 532.0 - - 532.0 - - 532.0 - - 543.0 - - 543.0 - - 543.0 + - 530.0 + - 530.0 + - 530.0 + - 454.0 + - 454.0 + - 454.0 friction_velocities_profile_1: - 408.0 - 408.0 @@ -249,7 +261,7 @@ heat_controller: shooting_recorder: ros__parameters: friction_wheel_count: 6 - aim_velocity: 11.8 + aim_velocity: 16.15 log_mode: 1 # 1: trigger, 2: timing first_front_friction_velocity_pid_controller: @@ -257,54 +269,54 @@ first_front_friction_velocity_pid_controller: measurement: /gimbal/first_front_friction/velocity setpoint: /gimbal/first_front_friction/control_velocity control: /gimbal/first_front_friction/control_torque - kp: 0.006 - ki: 0.00 - kd: 0.00008 + kp: 0.006233371 + ki: 0.00003 + kd: 0.000001 second_front_friction_velocity_pid_controller: ros__parameters: measurement: /gimbal/second_front_friction/velocity setpoint: /gimbal/second_front_friction/control_velocity control: /gimbal/second_front_friction/control_torque - kp: 0.006 - ki: 0.00 - kd: 0.00008 + kp: 0.006035661 + ki: 0.00008 + kd: 0.000001 third_front_friction_velocity_pid_controller: ros__parameters: measurement: /gimbal/third_front_friction/velocity setpoint: /gimbal/third_front_friction/control_velocity control: /gimbal/third_front_friction/control_torque - kp: 0.006 - ki: 0.00 - kd: 0.0008 + kp: 0.006192421 + ki: 0.00003 + kd: 0.000001 first_back_friction_velocity_pid_controller: ros__parameters: measurement: /gimbal/first_back_friction/velocity setpoint: /gimbal/first_back_friction/control_velocity control: /gimbal/first_back_friction/control_torque - kp: 0.004 + kp: 0.007749503 ki: 0.00 - kd: 0.00006 + kd: 0.00003 second_back_friction_velocity_pid_controller: ros__parameters: measurement: /gimbal/second_back_friction/velocity setpoint: /gimbal/second_back_friction/control_velocity control: /gimbal/second_back_friction/control_torque - kp: 0.004 + kp: 0.0085 #0.007795934 ki: 0.00 - kd: 0.00006 + kd: 0.00003 third_back_friction_velocity_pid_controller: ros__parameters: measurement: /gimbal/third_back_friction/velocity setpoint: /gimbal/third_back_friction/control_velocity control: /gimbal/third_back_friction/control_torque - kp: 0.004 + kp: 0.007760993 ki: 0.00 - kd: 0.00006 + kd: 0.00003 steering_wheel_status: ros__parameters: @@ -439,3 +451,75 @@ bottom_yaw_swept_frequency_controller: end_freq: 4.0 duration: 80.0 amplitude: 1.0 + +first_front_friction_swept_frequency_controller: + ros__parameters: + target: /gimbal/first_front_friction + sweep: true + pid: false + logarithmic: true + start_freq: 1.0 + end_freq: 45.0 + duration: 60.0 + amplitude: 0.08 + dc_offset: 0.01 + +second_front_friction_swept_frequency_controller: + ros__parameters: + target: /gimbal/second_front_friction + sweep: true + pid: false + logarithmic: true + start_freq: 1.0 + end_freq: 45.0 + duration: 60.0 + amplitude: 0.08 + dc_offset: 0.04 + +third_front_friction_swept_frequency_controller: + ros__parameters: + target: /gimbal/third_front_friction + sweep: true + pid: false + logarithmic: true + start_freq: 1.0 + end_freq: 45.0 + duration: 60.0 + amplitude: 0.08 + dc_offset: 0.02 + +first_back_friction_swept_frequency_controller: + ros__parameters: + target: /gimbal/first_back_friction + sweep: true + pid: false + logarithmic: true + start_freq: 1.0 + end_freq: 45.0 + duration: 60.0 + amplitude: 0.08 + dc_offset: 0.021 + +second_back_friction_swept_frequency_controller: + ros__parameters: + target: /gimbal/second_back_friction + sweep: true + pid: false + logarithmic: true + start_freq: 1.0 + end_freq: 45.0 + duration: 60.0 + amplitude: 0.08 + dc_offset: 0.029 + +third_back_friction_swept_frequency_controller: + ros__parameters: + target: /gimbal/third_back_friction + sweep: true + pid: false + logarithmic: true + start_freq: 1.0 + end_freq: 45.0 + duration: 40.0 + amplitude: 0.08 + dc_offset: 0.00 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index e11533881..6133fac06 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -59,4 +59,8 @@ + + + + diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp index c7f13b6fd..0b1a5749f 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp @@ -181,10 +181,10 @@ class PutterController if (shoot_stage_ == ShootStage::SHOOTING) { // Firing state: detect whether the bullet has been fired. - if (*bullet_fired_ && !shooted) { - RCLCPP_INFO(get_logger(), "DETECT: Bullet fired!"); - shooted = true; - } + // if (*bullet_fired_ && !shooted) { + // RCLCPP_INFO(get_logger(), "DETECT: Bullet fired!"); + // shooted = true; + // } // if (*putter_angle_ - putter_startpoint >= putter_stroke_ && !shooted) { // RCLCPP_INFO(get_logger(), "DETECT: Putter stroke completed!"); diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six--friction.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six--friction.cpp index fd4ccb9b7..6ac803de1 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six--friction.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six--friction.cpp @@ -355,13 +355,19 @@ class SteeringHeroLittle *photoelectric_sensor_status_ = photoelectric_sensor_status_atomic.load(); *grayscale_sensor_status_ = grayscale_sensor_status_atomic.load(); - if (++count_ == 500) { + if (++count_ == 250) { for (int i = 0; i < 6; ++i) { if (friciton_detect[i] == 0) { RCLCPP_WARN(logger_, "can id 0x%03X missing", i + 0x201); } } std::fill_n(friciton_detect, 6, 0); + for (int i = 0; i < 3; ++i) { + if (can0_detect[i] == 0) { + RCLCPP_WARN(logger_, "can id 0x%03X missing", i + 0x141); + } + } + std::fill_n(can0_detect, 3, 0); count_ = 0; } } @@ -430,6 +436,7 @@ class SteeringHeroLittle return; auto can_id = data.can_id; // can0_receive_rate_counter_.record(can_id); + can0_detect[can_id - 0x141] = 1; if (can_id == 0x141) { gimbal_top_yaw_motor_.store_status(data.can_data); } else if (can_id == 0x143) { @@ -503,6 +510,7 @@ class SteeringHeroLittle std::time_t last_camera_capturer_trigger_timestamp_{0}; int count_ = 0; int friciton_detect[6]; + int can0_detect[3]; device::Bmi088 imu_; device::Vt13 vt13_; @@ -678,7 +686,7 @@ class SteeringHeroLittle yaw_brake_motor_.update_status(); gimbal_bottom_yaw_motor_.update_status(); - if (++count_ == 500) { + if (++count_ == 250) { for (int i = 0; i < 8; ++i) { if (check[i] == 0) { RCLCPP_WARN(logger_, "can id 0x%03X missing", i + 0x201); From 56e87d5b29cb7a2c637cafe6c8c94fd61cf47400 Mon Sep 17 00:00:00 2001 From: dwx5 <1591215786@qq.com> Date: Wed, 15 Jul 2026 15:16:50 +0800 Subject: [PATCH 28/86] stable --- .../steering-hero-little-six-friction.yaml | 48 ++++++++++--------- .../chassis/chassis_climber_controller.cpp | 2 + .../gimbal/hero_gimbal_controller.cpp | 2 +- .../rmcs_core/src/hardware/device/vt13.hpp | 6 +-- .../steering-hero-little-six--friction.cpp | 6 +-- 5 files changed, 35 insertions(+), 29 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml index b58584b46..b710cb88b 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml @@ -58,16 +58,16 @@ hero_hardware: ros__parameters: board_serial_top_board: "AF-8AE3-4EC1-03C3-C494-88FE-2DC4-3018-0298" serial_bottom_rmcs_board: "AF-60BB-7484-FA24-F3FC-399B-454D-22FA-1B1D" - bottom_yaw_motor_zero_point: 34622 + bottom_yaw_motor_zero_point: 35110 pitch_motor_zero_point: 23653 - top_yaw_motor_zero_point: 48150 + top_yaw_motor_zero_point: 48525 viewer_motor_zero_point: 31940 external_imu_port: /dev/ttyUSB0 bullet_feeder_motor_zero_point: 60480 #39045 - left_front_zero_point: 5826 - right_front_zero_point: 5095 - left_back_zero_point: 7804 - right_back_zero_point: 1750 + left_front_zero_point: 5790 + right_front_zero_point: 5114 + left_back_zero_point: 7868 + right_back_zero_point: 1640 value_broadcaster: ros__parameters: @@ -79,6 +79,10 @@ value_broadcaster: # - /chassis/left_back_steering/torque # - /chassis/right_front_steering/torque # - /chassis/right_back_steering/torque + - /chassis/left_front_steering/velocity + - /chassis/left_back_steering/velocity + - /chassis/right_front_steering/velocity + - /chassis/right_back_steering/velocity # - /shoot/heat12707 # - /chassis/power # - /referee/chassis/power @@ -117,7 +121,7 @@ value_broadcaster: # - /gimbal/bullet_feeder/torque # - /gimbal/bullet_feeder/control_torque # - /gimbal/putter/angle - - /gimbal/putter/velocity + # - /gimbal/putter/velocity # - /gimbal/putter/torque # - /gimbal/putter/control_torque # - /gimbal/bullet_feeder/velocity @@ -126,19 +130,19 @@ value_broadcaster: climber_controller: ros__parameters: - front_climber_velocity: 20.0 + front_climber_velocity: 22.0 back_climber_velocity: 30.0 auto_climb_support_retract_velocity_fast: 70.0 auto_climb_support_retract_velocity_slow: 20.0 - auto_climb_approach_chassis_velocity: 1.8 - auto_climb_support_deploy_chassis_velocity: 0.3 + auto_climb_approach_chassis_velocity: 2.0 + auto_climb_support_deploy_chassis_velocity: 0.4 auto_climb_support_retract_chassis_velocity: 0.15 auto_climb_dash_chassis_velocity: 3.0 first_stair_dash_leveled_pitch_threshold: 0.05 second_stair_dash_leveled_pitch_threshold: -0.09 sync_coefficient: 0.2 first_stair_approach_pitch: 0.517 - second_stair_approach_pitch: 0.365 + second_stair_approach_pitch: 0.37 #0.365 front_kp: 1.0 front_ki: 0.0 front_kd: 0.5 @@ -238,12 +242,12 @@ friction_wheel_controller: - /gimbal/second_back_friction - /gimbal/third_back_friction friction_velocities_profile_0: - - 530.0 - - 530.0 - - 530.0 - - 454.0 - - 454.0 - - 454.0 + - 527.0 #530.0 + - 527.0 #530.0 + - 527.0 #530.0 + - 435.0 #454.0 + - 435.0 #454.0 + - 435.0 #454.0 friction_velocities_profile_1: - 408.0 - 408.0 @@ -279,7 +283,7 @@ second_front_friction_velocity_pid_controller: setpoint: /gimbal/second_front_friction/control_velocity control: /gimbal/second_front_friction/control_torque kp: 0.006035661 - ki: 0.00008 + ki: 0.00003 kd: 0.000001 third_front_friction_velocity_pid_controller: @@ -305,7 +309,7 @@ second_back_friction_velocity_pid_controller: measurement: /gimbal/second_back_friction/velocity setpoint: /gimbal/second_back_friction/control_velocity control: /gimbal/second_back_friction/control_torque - kp: 0.0085 #0.007795934 + kp: 0.007795934 ki: 0.00 kd: 0.00003 @@ -389,7 +393,7 @@ pitch_swept_frequency_controller: start_freq: 0.1 end_freq: 10.0 duration: 60.0 - amplitude: 10.0 + amplitude: 6.8 pid: true setpoint: 0.0 @@ -405,9 +409,9 @@ pitch_static_torque_test_controller: ros__parameters: target: /gimbal/pitch - interval_angle: 0.05 + interval_angle: 0.02 wait_time: 1.5 - border_clip: 0.05 + border_clip: 0.02 position_kp: 12.0 position_ki: 0.0 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp index 39f5d833a..3355b468e 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp @@ -119,6 +119,8 @@ class ChassisClimberController auto keyboard = *keyboard_; auto rotary_knob_switch = *rotary_knob_switch_; + // RCLCPP_INFO(get_logger(), "%f", *chassis_pitch_imu_); + bool rotary_knob_to_down = (last_rotary_knob_switch_ != Switch::DOWN && rotary_knob_switch == Switch::DOWN); bool rotary_knob_from_down = diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp index 6326d627f..1093dc007 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp @@ -80,7 +80,7 @@ class HeroGimbalController } *gimbal_mode_ = gimbal_mode_keyboard_; - *gimbal_mode_ = switch_right == Switch::UP ? GimbalMode::ENCODER : GimbalMode::IMU; + //*gimbal_mode_ = switch_right == Switch::UP ? GimbalMode::ENCODER : GimbalMode::IMU; if (*gimbal_mode_ == GimbalMode::IMU) { auto angle_error = switch_encoder_to_imu_by_c ? enter_imu_hold_current_pose() diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp index 168ffa52c..53802e75b 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp @@ -70,9 +70,9 @@ class Vt13 { else { unknown_prefix_count_++; if (should_log_verification_failure(now)) { - RCLCPP_WARN( - logger_, "VT13 unknown prefix: front=0x%02x readable=%zu", - std::to_integer(front), readable); + // RCLCPP_WARN( + // logger_, "VT13 unknown prefix: front=0x%02x readable=%zu", + // std::to_integer(front), readable); } } diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six--friction.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six--friction.cpp index 6ac803de1..3b4711690 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six--friction.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six--friction.cpp @@ -358,13 +358,13 @@ class SteeringHeroLittle if (++count_ == 250) { for (int i = 0; i < 6; ++i) { if (friciton_detect[i] == 0) { - RCLCPP_WARN(logger_, "can id 0x%03X missing", i + 0x201); + RCLCPP_WARN(logger_, "friction can id 0x%03X missing", i + 0x201); } } std::fill_n(friciton_detect, 6, 0); for (int i = 0; i < 3; ++i) { if (can0_detect[i] == 0) { - RCLCPP_WARN(logger_, "can id 0x%03X missing", i + 0x141); + RCLCPP_WARN(logger_, "top board can id 0x%03X missing", i + 0x141); } } std::fill_n(can0_detect, 3, 0); @@ -689,7 +689,7 @@ class SteeringHeroLittle if (++count_ == 250) { for (int i = 0; i < 8; ++i) { if (check[i] == 0) { - RCLCPP_WARN(logger_, "can id 0x%03X missing", i + 0x201); + RCLCPP_WARN(logger_, "bottom board can id 0x%03X missing", i + 0x201); } } std::fill_n(check, 8, 0); From 0504ae833de4e64149f2dbbe87ad30de25945fd1 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Wed, 15 Jul 2026 18:47:45 +0800 Subject: [PATCH 29/86] wip: Rebase with main branch --- .../steering-hero-little-six-friction.yaml | 180 +- .../gimbal/hero_gimbal_controller.cpp | 55 +- .../rmcs_core/src/hardware/device/dr16.hpp | 334 +-- .../src/hardware/device/remote_control.hpp | 107 - .../rmcs_core/src/hardware/device/vt13.hpp | 386 ---- .../steering-hero-little-six--friction.cpp | 909 --------- .../steering-hero-little-six-friction.cpp | 1803 ++++++++--------- 7 files changed, 1218 insertions(+), 2556 deletions(-) delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six--friction.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml index b710cb88b..5ecf58744 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml @@ -38,10 +38,10 @@ rmcs_executor: # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer # - rmcs_auto_aim::AutoAimController -> auto_aim_controller - - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster - #- rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster + # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster - # - sp_vision_25::bridge::HeroAutoAimBridge -> hero_auto_aim_bridge + # - sp_vision_25::bridge::HeroAutoAimBridge -> hero_auto_aim_bridge # - rmcs_core::controller::identification::SweptFrequencyController -> pitch_swept_frequency_controller # - rmcs_core::controller::identification::StaticTorqueTestController -> pitch_static_torque_test_controller @@ -61,12 +61,12 @@ hero_hardware: bottom_yaw_motor_zero_point: 35110 pitch_motor_zero_point: 23653 top_yaw_motor_zero_point: 48525 - viewer_motor_zero_point: 31940 + viewer_motor_zero_point: 31940 external_imu_port: /dev/ttyUSB0 bullet_feeder_motor_zero_point: 60480 #39045 left_front_zero_point: 5790 right_front_zero_point: 5114 - left_back_zero_point: 7868 + left_back_zero_point: 7868 right_back_zero_point: 1640 value_broadcaster: @@ -117,7 +117,7 @@ value_broadcaster: # - /chassis/climber/front/control_power_limit # - /chassis/climber/front/power_demand_estimate # - /chassis/climber/front/actual_power_estimate - # - /chassis/steering_wheel/actual_power_estimate + # - /chassis/steering_wheel/actual_power_estimate # - /gimbal/bullet_feeder/torque # - /gimbal/bullet_feeder/control_torque # - /gimbal/putter/angle @@ -127,7 +127,6 @@ value_broadcaster: # - /gimbal/bullet_feeder/velocity # - /gimbal/bullet_feeder/angle - climber_controller: ros__parameters: front_climber_velocity: 22.0 @@ -142,7 +141,7 @@ climber_controller: second_stair_dash_leveled_pitch_threshold: -0.09 sync_coefficient: 0.2 first_stair_approach_pitch: 0.517 - second_stair_approach_pitch: 0.37 #0.365 + second_stair_approach_pitch: 0.37 #0.365 front_kp: 1.0 front_ki: 0.0 front_kd: 0.5 @@ -177,12 +176,12 @@ dual_yaw_controller: bottom_yaw_angle_kp: 10.0 #18.4 bottom_yaw_angle_ki: 0.0 bottom_yaw_angle_kd: 0.0 - bottom_yaw_velocity_kp: 2.00 #2.81 + bottom_yaw_velocity_kp: 2.00 #2.81 bottom_yaw_velocity_ki: 0.000071 #0.00028 bottom_yaw_velocity_kd: 0.0 bottom_yaw_velocity_integral_min: -2500.0 bottom_yaw_velocity_integral_max: 2500.0 - + pitch_angle_pid_controller: ros__parameters: measurement: /gimbal/pitch/control_angle_error @@ -204,8 +203,8 @@ pitch_velocity_pid_controller: gimbal_player_viewer_controller: ros__parameters: - upper_limit: 0.415996 - lower_limit: -0.066441 + upper_limit: 0.415996 + lower_limit: -0.066441 viewer_angle_pid_controller: ros__parameters: @@ -218,7 +217,7 @@ viewer_angle_pid_controller: bullet_feeder_controller: ros__parameters: bullet_feeder_velocity_kp: 5.0 - bullet_feeder_velocity_ki: 0.1 + bullet_feeder_velocity_ki: 0.1 bullet_feeder_velocity_kd: 0.0 bullet_feeder_velocity_integral_min: 0.0 bullet_feeder_velocity_integral_max: 60.0 @@ -242,10 +241,10 @@ friction_wheel_controller: - /gimbal/second_back_friction - /gimbal/third_back_friction friction_velocities_profile_0: - - 527.0 #530.0 - - 527.0 #530.0 - - 527.0 #530.0 - - 435.0 #454.0 + - 527.0 #530.0 + - 527.0 #530.0 + - 527.0 #530.0 + - 435.0 #454.0 - 435.0 #454.0 - 435.0 #454.0 friction_velocities_profile_1: @@ -273,7 +272,7 @@ first_front_friction_velocity_pid_controller: measurement: /gimbal/first_front_friction/velocity setpoint: /gimbal/first_front_friction/control_velocity control: /gimbal/first_front_friction/control_torque - kp: 0.006233371 + kp: 0.006233371 ki: 0.00003 kd: 0.000001 @@ -282,7 +281,7 @@ second_front_friction_velocity_pid_controller: measurement: /gimbal/second_front_friction/velocity setpoint: /gimbal/second_front_friction/control_velocity control: /gimbal/second_front_friction/control_torque - kp: 0.006035661 + kp: 0.006035661 ki: 0.00003 kd: 0.000001 @@ -291,7 +290,7 @@ third_front_friction_velocity_pid_controller: measurement: /gimbal/third_front_friction/velocity setpoint: /gimbal/third_front_friction/control_velocity control: /gimbal/third_front_friction/control_torque - kp: 0.006192421 + kp: 0.006192421 ki: 0.00003 kd: 0.000001 @@ -300,7 +299,7 @@ first_back_friction_velocity_pid_controller: measurement: /gimbal/first_back_friction/velocity setpoint: /gimbal/first_back_friction/control_velocity control: /gimbal/first_back_friction/control_torque - kp: 0.007749503 + kp: 0.007749503 ki: 0.00 kd: 0.00003 @@ -309,7 +308,7 @@ second_back_friction_velocity_pid_controller: measurement: /gimbal/second_back_friction/velocity setpoint: /gimbal/second_back_friction/control_velocity control: /gimbal/second_back_friction/control_torque - kp: 0.007795934 + kp: 0.007795934 ki: 0.00 kd: 0.00003 @@ -318,10 +317,10 @@ third_back_friction_velocity_pid_controller: measurement: /gimbal/third_back_friction/velocity setpoint: /gimbal/third_back_friction/control_velocity control: /gimbal/third_back_friction/control_torque - kp: 0.007760993 + kp: 0.007760993 ki: 0.00 kd: 0.00003 - + steering_wheel_status: ros__parameters: vehicle_radius: 0.286378 @@ -376,13 +375,12 @@ auto_aim_controller: raw_img_pub: false # Set false in actual use image_viewer_type: 2 -hero_auto_aim_bridge: - ros__parameters: - config_file: "configs/standard3.yaml" - bullet_speed_fallback: 11.4 - result_timeout: 0.1 # 0.08 - debug: false - +hero_auto_aim_bridge: + ros__parameters: + config_file: "configs/standard3.yaml" + bullet_speed_fallback: 11.4 + result_timeout: 0.1 # 0.08 + debug: false pitch_swept_frequency_controller: ros__parameters: @@ -457,73 +455,73 @@ bottom_yaw_swept_frequency_controller: amplitude: 1.0 first_front_friction_swept_frequency_controller: - ros__parameters: - target: /gimbal/first_front_friction - sweep: true - pid: false - logarithmic: true - start_freq: 1.0 - end_freq: 45.0 - duration: 60.0 - amplitude: 0.08 - dc_offset: 0.01 + ros__parameters: + target: /gimbal/first_front_friction + sweep: true + pid: false + logarithmic: true + start_freq: 1.0 + end_freq: 45.0 + duration: 60.0 + amplitude: 0.08 + dc_offset: 0.01 second_front_friction_swept_frequency_controller: - ros__parameters: - target: /gimbal/second_front_friction - sweep: true - pid: false - logarithmic: true - start_freq: 1.0 - end_freq: 45.0 - duration: 60.0 - amplitude: 0.08 - dc_offset: 0.04 + ros__parameters: + target: /gimbal/second_front_friction + sweep: true + pid: false + logarithmic: true + start_freq: 1.0 + end_freq: 45.0 + duration: 60.0 + amplitude: 0.08 + dc_offset: 0.04 third_front_friction_swept_frequency_controller: - ros__parameters: - target: /gimbal/third_front_friction - sweep: true - pid: false - logarithmic: true - start_freq: 1.0 - end_freq: 45.0 - duration: 60.0 - amplitude: 0.08 - dc_offset: 0.02 + ros__parameters: + target: /gimbal/third_front_friction + sweep: true + pid: false + logarithmic: true + start_freq: 1.0 + end_freq: 45.0 + duration: 60.0 + amplitude: 0.08 + dc_offset: 0.02 first_back_friction_swept_frequency_controller: - ros__parameters: - target: /gimbal/first_back_friction - sweep: true - pid: false - logarithmic: true - start_freq: 1.0 - end_freq: 45.0 - duration: 60.0 - amplitude: 0.08 - dc_offset: 0.021 + ros__parameters: + target: /gimbal/first_back_friction + sweep: true + pid: false + logarithmic: true + start_freq: 1.0 + end_freq: 45.0 + duration: 60.0 + amplitude: 0.08 + dc_offset: 0.021 second_back_friction_swept_frequency_controller: - ros__parameters: - target: /gimbal/second_back_friction - sweep: true - pid: false - logarithmic: true - start_freq: 1.0 - end_freq: 45.0 - duration: 60.0 - amplitude: 0.08 - dc_offset: 0.029 + ros__parameters: + target: /gimbal/second_back_friction + sweep: true + pid: false + logarithmic: true + start_freq: 1.0 + end_freq: 45.0 + duration: 60.0 + amplitude: 0.08 + dc_offset: 0.029 third_back_friction_swept_frequency_controller: - ros__parameters: - target: /gimbal/third_back_friction - sweep: true - pid: false - logarithmic: true - start_freq: 1.0 - end_freq: 45.0 - duration: 40.0 - amplitude: 0.08 - dc_offset: 0.00 + ros__parameters: + target: /gimbal/third_back_friction + sweep: true + pid: false + logarithmic: true + start_freq: 1.0 + end_freq: 45.0 + duration: 40.0 + amplitude: 0.08 + dc_offset: 0.00 diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp index 1093dc007..dc2ffd927 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp @@ -24,11 +24,7 @@ class HeroGimbalController HeroGimbalController() : Node( get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , upper_limit_(get_parameter("upper_limit").as_double()) - , lower_limit_(get_parameter("lower_limit").as_double()) - , imu_gimbal_solver(*this, upper_limit_, lower_limit_) - , encoder_gimbal_solver(*this, upper_limit_, lower_limit_) { + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { register_input("/remote/joystick/left", joystick_left_); register_input("/remote/switch/left", switch_left_); @@ -38,8 +34,6 @@ class HeroGimbalController register_input("/remote/keyboard", keyboard_); register_input("/gimbal/auto_aim/control_direction", auto_aim_control_direction_, false); - register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); - register_input("/gimbal/pitch/raw_angle", gimbal_pitch_raw_angle_); register_input("/tf", tf_); register_output("/gimbal/mode", gimbal_mode_, rmcs_msgs::GimbalMode::IMU); @@ -88,12 +82,12 @@ class HeroGimbalController *yaw_angle_error_ = angle_error.yaw_angle_error; *pitch_angle_error_ = angle_error.pitch_angle_error; - encoder_gimbal_solver.update(PreciseTwoAxisGimbalSolver::SetDisabled{}); + encoder_gimbal_solver_.update(PreciseTwoAxisGimbalSolver::SetDisabled{}); *yaw_control_angle_shift_ = nan_; *pitch_control_angle_ = nan_; } else { - imu_gimbal_solver.update(TwoAxisGimbalSolver::SetDisabled{}); + imu_gimbal_solver_.update(TwoAxisGimbalSolver::SetDisabled{}); *yaw_angle_error_ = nan_; *pitch_angle_error_ = nan_; @@ -107,8 +101,8 @@ class HeroGimbalController } void reset_all_control() { - imu_gimbal_solver.update(TwoAxisGimbalSolver::SetDisabled{}); - encoder_gimbal_solver.update(PreciseTwoAxisGimbalSolver::SetDisabled{}); + imu_gimbal_solver_.update(TwoAxisGimbalSolver::SetDisabled{}); + encoder_gimbal_solver_.update(PreciseTwoAxisGimbalSolver::SetDisabled{}); gimbal_mode_keyboard_ = rmcs_msgs::GimbalMode::IMU; *gimbal_mode_ = rmcs_msgs::GimbalMode::IMU; @@ -123,13 +117,13 @@ class HeroGimbalController if (auto_aim_control_direction_.ready() && (mouse_->right || *switch_right_ == rmcs_msgs::Switch::UP) && !auto_aim_control_direction_->isZero()) { - return imu_gimbal_solver.update( + return imu_gimbal_solver_.update( TwoAxisGimbalSolver::SetControlDirection{ OdomImu::DirectionVector{*auto_aim_control_direction_}}); } - if (!imu_gimbal_solver.enabled()) - return imu_gimbal_solver.update(TwoAxisGimbalSolver::SetToLevel{}); + if (!imu_gimbal_solver_.enabled()) + return imu_gimbal_solver_.update(TwoAxisGimbalSolver::SetToLevel{}); constexpr double joystick_sensitivity = 0.006; constexpr double mouse_sensitivity = 0.5; @@ -139,7 +133,7 @@ class HeroGimbalController double pitch_shift = -joystick_sensitivity * joystick_left_->x() + mouse_sensitivity * mouse_velocity_->x(); - return imu_gimbal_solver.update( + return imu_gimbal_solver_.update( TwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); } @@ -150,13 +144,13 @@ class HeroGimbalController auto current_direction = fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); - return imu_gimbal_solver.update( + return imu_gimbal_solver_.update( TwoAxisGimbalSolver::SetControlDirection{OdomImu::DirectionVector{*current_direction}}); } PreciseTwoAxisGimbalSolver::ControlAngle update_encoder_control() { - if (!encoder_gimbal_solver.enabled()) { - return encoder_gimbal_solver.update( + if (!encoder_gimbal_solver_.enabled()) { + return encoder_gimbal_solver_.update( PreciseTwoAxisGimbalSolver::SetControlPitch{encoder_init_pitch_}); } @@ -169,7 +163,7 @@ class HeroGimbalController double pitch_shift = -joystick_sensitivity * joystick_left_->x() + mouse_pitch_sensitivity * mouse_velocity_->x(); - return encoder_gimbal_solver.update( + return encoder_gimbal_solver_.update( PreciseTwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); } @@ -190,24 +184,33 @@ class HeroGimbalController rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); InputInterface auto_aim_control_direction_; - InputInterface gimbal_pitch_angle_; - InputInterface gimbal_pitch_raw_angle_; InputInterface tf_; rmcs_msgs::GimbalMode gimbal_mode_keyboard_ = rmcs_msgs::GimbalMode::IMU; OutputInterface gimbal_mode_; - const double upper_limit_, lower_limit_; - TwoAxisGimbalSolver imu_gimbal_solver; - PreciseTwoAxisGimbalSolver encoder_gimbal_solver; - OutputInterface yaw_angle_error_, pitch_angle_error_; OutputInterface yaw_control_angle_shift_, pitch_control_angle_; + + struct SimpleComponent : Component { + auto update() -> void override {} + }; + std::shared_ptr imu_gimbal_solver_component_ = + create_partner_component("imu_gimbal_solver"); + std::shared_ptr encoder_gimbal_solver_component_ = + create_partner_component("encoder_gimbal_solver"); + + const double upper_limit_{get_parameter("upper_limit").as_double()}; + const double lower_limit_{get_parameter("lower_limit").as_double()}; + + TwoAxisGimbalSolver imu_gimbal_solver_ = { + *imu_gimbal_solver_component_, upper_limit_, lower_limit_}; + PreciseTwoAxisGimbalSolver encoder_gimbal_solver_ = { + *encoder_gimbal_solver_component_, upper_limit_, lower_limit_}; }; } // namespace rmcs_core::controller::gimbal #include - PLUGINLIB_EXPORT_CLASS( rmcs_core::controller::gimbal::HeroGimbalController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp index cd0a8fbc2..75152b179 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp @@ -1,14 +1,16 @@ #pragma once -#include -#include -#include #include #include #include -#include + +#include +#include #include +#include +#include +#include #include #include #include @@ -17,178 +19,230 @@ namespace rmcs_core::hardware::device { class Dr16 { public: - Dr16() = default; + explicit Dr16(rmcs_executor::Component& component) { + component.register_output( + "/remote/joystick/right", joystick_right_output_, Eigen::Vector2d::Zero()); + component.register_output( + "/remote/joystick/left", joystick_left_output_, Eigen::Vector2d::Zero()); + + component.register_output( + "/remote/switch/right", switch_right_output_, rmcs_msgs::Switch::UNKNOWN); + component.register_output( + "/remote/switch/left", switch_left_output_, rmcs_msgs::Switch::UNKNOWN); + + component.register_output( + "/remote/mouse/velocity", mouse_velocity_output_, Eigen::Vector2d::Zero()); + component.register_output("/remote/mouse/mouse_wheel", mouse_wheel_output_); + + component.register_output("/remote/mouse", mouse_output_); + std::memset(&*mouse_output_, 0, sizeof(*mouse_output_)); + component.register_output("/remote/keyboard", keyboard_output_); + std::memset(&*keyboard_output_, 0, sizeof(*keyboard_output_)); + + component.register_output("/remote/rotary_knob", rotary_knob_output_); + + // Simulate the rotary knob as a switch, with anti-shake algorithm. + component.register_output( + "/remote/rotary_knob_switch", rotary_knob_switch_output_, rmcs_msgs::Switch::UNKNOWN); + } - void store_status(std::span uart_data) { - if (uart_data.size() != kStatusSize) + void store_status(const std::byte* uart_data, size_t uart_data_length) { + if (uart_data_length != 6 + 8 + 4) return; - last_receive_time_.store(Clock::now(), std::memory_order_relaxed); - - auto* cursor = uart_data.data(); + // Avoid using reinterpret_cast here because it does not account for pointer alignment. + // Dr16DataPart structures are aligned, and using reinterpret_cast on potentially unaligned + // uart_data can cause undefined behavior on architectures that enforce strict alignment + // requirements (e.g., ARM). + // Directly accessing unaligned memory through a casted pointer can lead to crashes, + // inefficiencies, or incorrect data reads. Instead, std::memcpy safely copies the data from + // unaligned memory to properly aligned structures without violating alignment or strict + // aliasing rules. uint64_t part1{}; - std::memcpy(&part1, cursor, kPart1Size); - cursor += kPart1Size; + std::memcpy(&part1, uart_data, 6); + uart_data += 6; data_part1_.store(part1, std::memory_order::relaxed); uint64_t part2{}; - std::memcpy(&part2, cursor, kPart2Size); - cursor += kPart2Size; + std::memcpy(&part2, uart_data, 8); + uart_data += 8; data_part2_.store(part2, std::memory_order::relaxed); uint32_t part3{}; - std::memcpy(&part3, cursor, kPart3Size); + std::memcpy(&part3, uart_data, 4); + uart_data += 4; data_part3_.store(part3, std::memory_order::relaxed); } void update_status() { - const auto raw_part1 = data_part1_.load(std::memory_order::relaxed); - const auto part1 = std::bit_cast(raw_part1); - - joystick_right_ = { - channel_to_double(static_cast(part1.joystick_channel1)), - -channel_to_double(static_cast(part1.joystick_channel0)), - }; - joystick_left_ = { - channel_to_double(static_cast(part1.joystick_channel3)), - -channel_to_double(static_cast(part1.joystick_channel2)), + auto part1 alignas(uint64_t) = + std::bit_cast(data_part1_.load(std::memory_order::relaxed)); + + auto channel_to_double = [](int32_t value) { + value -= 1024; + if (-660 <= value && value <= 660) + return value / 660.0; + return 0.0; }; + joystick_right_.y = -channel_to_double(static_cast(part1.joystick_channel0)); + joystick_right_.x = channel_to_double(static_cast(part1.joystick_channel1)); + joystick_left_.y = -channel_to_double(static_cast(part1.joystick_channel2)); + joystick_left_.x = channel_to_double(static_cast(part1.joystick_channel3)); - switch_right_ = static_cast(part1.switch_right); - switch_left_ = static_cast(part1.switch_left); + switch_right_ = static_cast(part1.switch_right); + switch_left_ = static_cast(part1.switch_left); - const auto raw_part2 = data_part2_.load(std::memory_order::relaxed); - const auto part2 = std::bit_cast(raw_part2); + auto part2 alignas(uint64_t) = + std::bit_cast(data_part2_.load(std::memory_order::relaxed)); - mouse_velocity_ = { - -static_cast(part2.mouse_velocity_y) / 32768.0, - -static_cast(part2.mouse_velocity_x) / 32768.0, - }; - mouse_wheel_ = -static_cast(part2.mouse_velocity_z) / 32768.0; - mouse_ = { - .left = part2.mouse_left, - .right = part2.mouse_right, - }; + mouse_velocity_.x = -part2.mouse_velocity_y / 32768.0; + mouse_velocity_.y = -part2.mouse_velocity_x / 32768.0; + + mouse_wheel_ = -part2.mouse_velocity_z / 32768.0; + + mouse_.left = part2.mouse_left; + mouse_.right = part2.mouse_right; - const auto raw_part3 = data_part3_.load(std::memory_order::relaxed); - const auto part3 = std::bit_cast(raw_part3); + auto part3 alignas(uint32_t) = + std::bit_cast(data_part3_.load(std::memory_order::relaxed)); - keyboard_ = std::bit_cast(part3.keyboard); + keyboard_ = part3.keyboard; rotary_knob_ = channel_to_double(part3.rotary_knob); - update_rotary_knob_switch(); - } - [[nodiscard]] const Eigen::Vector2d& joystick_right() const noexcept { return joystick_right_; } + *joystick_right_output_ = joystick_right(); + *joystick_left_output_ = joystick_left(); - [[nodiscard]] bool valid() const noexcept { - const auto last_receive_time = last_receive_time_.load(std::memory_order_relaxed); - return last_receive_time != TimePoint::min() - && Clock::now() - last_receive_time <= kFreshTimeout; - } + *switch_right_output_ = switch_right(); + *switch_left_output_ = switch_left(); - [[nodiscard]] const Eigen::Vector2d& joystick_left() const noexcept { return joystick_left_; } + *mouse_velocity_output_ = mouse_velocity(); + *mouse_wheel_output_ = mouse_wheel(); - [[nodiscard]] rmcs_msgs::Switch switch_right() const noexcept { return switch_right_; } + *mouse_output_ = mouse(); + *keyboard_output_ = keyboard(); - [[nodiscard]] rmcs_msgs::Switch switch_left() const noexcept { return switch_left_; } + *rotary_knob_output_ = rotary_knob(); + update_rotary_knob_switch(); + } - [[nodiscard]] const Eigen::Vector2d& mouse_velocity() const noexcept { return mouse_velocity_; } + struct Vector { + constexpr static Vector zero() { return {.x = 0, .y = 0}; } + double x, y; + }; - [[nodiscard]] rmcs_msgs::Mouse mouse() const noexcept { return mouse_; } + enum class Switch : uint8_t { kUnknown = 0, kUp = 1, kDown = 2, kMiddle = 3 }; - [[nodiscard]] rmcs_msgs::Keyboard keyboard() const noexcept { return keyboard_; } + struct [[gnu::packed]] Mouse { + constexpr static Mouse zero() { + constexpr uint8_t zero = 0; + return std::bit_cast(zero); + } - [[nodiscard]] double rotary_knob() const noexcept { return rotary_knob_; } + bool left : 1; + bool right : 1; + }; + static_assert(sizeof(Mouse) == 1); - [[nodiscard]] double mouse_wheel() const noexcept { return mouse_wheel_; } + struct [[gnu::packed]] Keyboard { + constexpr static Keyboard zero() { + constexpr uint16_t zero = 0; + return std::bit_cast(zero); + } - [[nodiscard]] rmcs_msgs::Switch rotary_knob_switch() const noexcept { - return rotary_knob_switch_; - } + bool w : 1; + bool s : 1; + bool a : 1; + bool d : 1; + bool shift : 1; + bool ctrl : 1; + bool q : 1; + bool e : 1; + bool r : 1; + bool f : 1; + bool g : 1; + bool z : 1; + bool x : 1; + bool c : 1; + bool v : 1; + bool b : 1; + }; + static_assert(sizeof(Keyboard) == 2); -private: - using Clock = std::chrono::steady_clock; - using TimePoint = Clock::time_point; + Eigen::Vector2d joystick_right() const { return to_eigen_vector(joystick_right_); } + Eigen::Vector2d joystick_left() const { return to_eigen_vector(joystick_left_); } - static constexpr auto kFreshTimeout = std::chrono::milliseconds(500); + rmcs_msgs::Switch switch_right() const { + return std::bit_cast(switch_right_); + } + rmcs_msgs::Switch switch_left() const { return std::bit_cast(switch_left_); } - static constexpr std::size_t kPart1Size = 6; - static constexpr std::size_t kPart2Size = 8; - static constexpr std::size_t kPart3Size = 4; - static constexpr std::size_t kStatusSize = kPart1Size + kPart2Size + kPart3Size; + Eigen::Vector2d mouse_velocity() const { return to_eigen_vector(mouse_velocity_); } - struct [[gnu::packed]] Dr16DataPart1 { - uint64_t joystick_channel0 : 11; - uint64_t joystick_channel1 : 11; - uint64_t joystick_channel2 : 11; - uint64_t joystick_channel3 : 11; - uint64_t switch_right : 2; - uint64_t switch_left : 2; - uint64_t padding : 16; - }; - static_assert(sizeof(Dr16DataPart1) == 8); + rmcs_msgs::Mouse mouse() const { return std::bit_cast(mouse_); } + rmcs_msgs::Keyboard keyboard() const { return std::bit_cast(keyboard_); } - struct [[gnu::packed]] Dr16DataPart2 { - int16_t mouse_velocity_x; - int16_t mouse_velocity_y; - int16_t mouse_velocity_z; - bool mouse_left; - bool mouse_right; - }; - static_assert(sizeof(Dr16DataPart2) == 8); + double rotary_knob() const { return rotary_knob_; } - struct [[gnu::packed]] Dr16DataPart3 { - uint16_t keyboard; - uint16_t rotary_knob; - }; - static_assert(sizeof(Dr16DataPart3) == 4); + double mouse_wheel() const { return mouse_wheel_; } - static double channel_to_double(int32_t value) { - value -= 1024; - if (-660 <= value && value <= 660) - return value / 660.0; - return 0.0; - } +private: + static Eigen::Vector2d to_eigen_vector(Vector vector) { return {vector.x, vector.y}; } void update_rotary_knob_switch() { - constexpr double divider = 0.7; - constexpr double anti_shake_shift = 0.05; - - double upper_divider = divider; - double lower_divider = -divider; - if (rotary_knob_switch_ == rmcs_msgs::Switch::UP) { - upper_divider -= anti_shake_shift; - lower_divider -= anti_shake_shift; - } else if (rotary_knob_switch_ == rmcs_msgs::Switch::MIDDLE) { - upper_divider += anti_shake_shift; - lower_divider -= anti_shake_shift; - } else if (rotary_knob_switch_ == rmcs_msgs::Switch::DOWN) { - upper_divider += anti_shake_shift; - lower_divider += anti_shake_shift; - } - - const auto knob_value = -rotary_knob_; + constexpr double divider = 0.7, anti_shake_shift = 0.05; + double upper_divider = divider, lower_divider = -divider; + + auto& switch_value = *rotary_knob_switch_output_; + if (switch_value == rmcs_msgs::Switch::UP) + upper_divider -= anti_shake_shift, lower_divider -= anti_shake_shift; + else if (switch_value == rmcs_msgs::Switch::MIDDLE) + upper_divider += anti_shake_shift, lower_divider -= anti_shake_shift; + else if (switch_value == rmcs_msgs::Switch::DOWN) + upper_divider += anti_shake_shift, lower_divider += anti_shake_shift; + + const auto knob_value = -*rotary_knob_output_; if (knob_value > upper_divider) { - rotary_knob_switch_ = rmcs_msgs::Switch::UP; + switch_value = rmcs_msgs::Switch::UP; } else if (knob_value < lower_divider) { - rotary_knob_switch_ = rmcs_msgs::Switch::DOWN; + switch_value = rmcs_msgs::Switch::DOWN; } else { - rotary_knob_switch_ = rmcs_msgs::Switch::MIDDLE; + switch_value = rmcs_msgs::Switch::MIDDLE; } } + struct [[gnu::packed]] Dr16DataPart1 { + uint64_t joystick_channel0 : 11; + uint64_t joystick_channel1 : 11; + uint64_t joystick_channel2 : 11; + uint64_t joystick_channel3 : 11; + + uint64_t switch_right : 2; + uint64_t switch_left : 2; + + uint64_t padding : 16; + }; + static_assert(sizeof(Dr16DataPart1) == 8); std::atomic data_part1_{std::bit_cast(Dr16DataPart1{ .joystick_channel0 = 1024, .joystick_channel1 = 1024, .joystick_channel2 = 1024, .joystick_channel3 = 1024, - .switch_right = static_cast(rmcs_msgs::Switch::UNKNOWN), - .switch_left = static_cast(rmcs_msgs::Switch::UNKNOWN), + .switch_right = static_cast(Switch::kUnknown), + .switch_left = static_cast(Switch::kUnknown), .padding = 0, })}; static_assert(decltype(data_part1_)::is_always_lock_free); + struct [[gnu::packed]] Dr16DataPart2 { + int16_t mouse_velocity_x; + int16_t mouse_velocity_y; + int16_t mouse_velocity_z; + + bool mouse_left; + bool mouse_right; + }; + static_assert(sizeof(Dr16DataPart2) == 8); std::atomic data_part2_{std::bit_cast(Dr16DataPart2{ .mouse_velocity_x = 0, .mouse_velocity_y = 0, @@ -198,27 +252,45 @@ class Dr16 { })}; static_assert(decltype(data_part2_)::is_always_lock_free); - std::atomic data_part3_{std::bit_cast(Dr16DataPart3{ - .keyboard = 0, + struct [[gnu::packed]] Dr16DataPart3 { + Keyboard keyboard; + uint16_t rotary_knob; + }; + static_assert(sizeof(Dr16DataPart3) == 4); + std::atomic data_part3_ = {std::bit_cast(Dr16DataPart3{ + .keyboard = Keyboard::zero(), .rotary_knob = 0, })}; static_assert(decltype(data_part3_)::is_always_lock_free); - std::atomic last_receive_time_{TimePoint::min()}; + Vector joystick_right_ = Vector::zero(); + Vector joystick_left_ = Vector::zero(); - Eigen::Vector2d joystick_right_ = Eigen::Vector2d::Zero(); - Eigen::Vector2d joystick_left_ = Eigen::Vector2d::Zero(); - Eigen::Vector2d mouse_velocity_ = Eigen::Vector2d::Zero(); + Switch switch_right_ = Switch::kUnknown; + Switch switch_left_ = Switch::kUnknown; - rmcs_msgs::Switch switch_right_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Switch switch_left_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Switch rotary_knob_switch_ = rmcs_msgs::Switch::UNKNOWN; + Vector mouse_velocity_ = Vector::zero(); - rmcs_msgs::Mouse mouse_ = rmcs_msgs::Mouse::zero(); - rmcs_msgs::Keyboard keyboard_ = rmcs_msgs::Keyboard::zero(); + Mouse mouse_ = Mouse::zero(); + Keyboard keyboard_ = Keyboard::zero(); double rotary_knob_ = 0.0; double mouse_wheel_ = 0.0; + + rmcs_executor::Component::OutputInterface joystick_right_output_; + rmcs_executor::Component::OutputInterface joystick_left_output_; + + rmcs_executor::Component::OutputInterface switch_right_output_; + rmcs_executor::Component::OutputInterface switch_left_output_; + + rmcs_executor::Component::OutputInterface mouse_velocity_output_; + rmcs_executor::Component::OutputInterface mouse_wheel_output_; + + rmcs_executor::Component::OutputInterface mouse_output_; + rmcs_executor::Component::OutputInterface keyboard_output_; + + rmcs_executor::Component::OutputInterface rotary_knob_output_; + rmcs_executor::Component::OutputInterface rotary_knob_switch_output_; }; -} // namespace rmcs_core::hardware::device \ No newline at end of file +} // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp deleted file mode 100644 index 236f3821b..000000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp +++ /dev/null @@ -1,107 +0,0 @@ -#pragma once - -#include - -#include -#include -#include -#include -#include - -#include "hardware/device/dr16.hpp" -#include "hardware/device/vt13.hpp" - -namespace rmcs_core::hardware::device { - -class RemoteControl { -public: - RemoteControl(rmcs_executor::Component& component, Dr16& dr16, Vt13& vt13) - : dr16_(dr16) - , vt13_(vt13) { - component.register_output( - "/remote/joystick/right", joystick_right_output_, Eigen::Vector2d::Zero()); - component.register_output( - "/remote/joystick/left", joystick_left_output_, Eigen::Vector2d::Zero()); - - component.register_output( - "/remote/switch/right", switch_right_output_, rmcs_msgs::Switch::UNKNOWN); - component.register_output( - "/remote/switch/left", switch_left_output_, rmcs_msgs::Switch::UNKNOWN); - - component.register_output("/remote/rotary_knob", rotary_knob_output_, 0.0); - component.register_output( - "/remote/rotary_knob_switch", rotary_knob_switch_output_, rmcs_msgs::Switch::UNKNOWN); - - component.register_output( - "/remote/mouse/velocity", mouse_velocity_output_, Eigen::Vector2d::Zero()); - component.register_output("/remote/mouse/mouse_wheel", mouse_wheel_output_, 0.0); - - component.register_output("/remote/mouse", mouse_output_, rmcs_msgs::Mouse::zero()); - component.register_output( - "/remote/keyboard", keyboard_output_, rmcs_msgs::Keyboard::zero()); - } - - void update() { - if (dr16_.valid() || !vt13_.valid() || vt13_.mode_switch() == Vt13::ModeSwitch::kNormal) { - *switch_right_output_ = dr16_.switch_right(); - *switch_left_output_ = dr16_.switch_left(); - - *joystick_right_output_ = dr16_.joystick_right(); - *joystick_left_output_ = dr16_.joystick_left(); - - *mouse_velocity_output_ = dr16_.mouse_velocity(); - *mouse_wheel_output_ = dr16_.mouse_wheel(); - - *mouse_output_ = dr16_.mouse(); - *keyboard_output_ = dr16_.keyboard(); - } else if (vt13_.mode_switch() == Vt13::ModeSwitch::kCine) { - *switch_right_output_ = rmcs_msgs::Switch::DOWN; - *switch_left_output_ = rmcs_msgs::Switch::DOWN; - - *joystick_right_output_ = Eigen::Vector2d::Zero(); - *joystick_left_output_ = Eigen::Vector2d::Zero(); - - *mouse_velocity_output_ = Eigen::Vector2d::Zero(); - *mouse_wheel_output_ = 0; - - *mouse_output_ = rmcs_msgs::Mouse::zero(); - *keyboard_output_ = rmcs_msgs::Keyboard::zero(); - } else if (vt13_.mode_switch() == Vt13::ModeSwitch::kSport) { - *switch_right_output_ = rmcs_msgs::Switch::MIDDLE; - *switch_left_output_ = rmcs_msgs::Switch::MIDDLE; - - *joystick_right_output_ = vt13_.joystick_right(); - *joystick_left_output_ = vt13_.joystick_left(); - - *mouse_velocity_output_ = vt13_.mouse_velocity(); - *mouse_wheel_output_ = vt13_.mouse_wheel(); - - *mouse_output_ = vt13_.mouse(); - *keyboard_output_ = vt13_.keyboard(); - } - - *rotary_knob_output_ = dr16_.rotary_knob(); - *rotary_knob_switch_output_ = dr16_.rotary_knob_switch(); - } - -private: - Dr16& dr16_; - Vt13& vt13_; - - rmcs_executor::Component::OutputInterface joystick_right_output_; - rmcs_executor::Component::OutputInterface joystick_left_output_; - - rmcs_executor::Component::OutputInterface switch_right_output_; - rmcs_executor::Component::OutputInterface switch_left_output_; - - rmcs_executor::Component::OutputInterface rotary_knob_output_; - rmcs_executor::Component::OutputInterface rotary_knob_switch_output_; - - rmcs_executor::Component::OutputInterface mouse_velocity_output_; - rmcs_executor::Component::OutputInterface mouse_wheel_output_; - - rmcs_executor::Component::OutputInterface mouse_output_; - rmcs_executor::Component::OutputInterface keyboard_output_; -}; - -} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp deleted file mode 100644 index 53802e75b..000000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp +++ /dev/null @@ -1,386 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace rmcs_core::hardware::device { - -class Vt13 { -public: - enum class ModeSwitch : uint8_t { - kUnknown = 0, - kCine = 1, - kNormal = 2, - kSport = 3, - }; - - Vt13() = default; - - void store_status(std::span uart_data) { - store_calls_.fetch_add(1, std::memory_order_relaxed); - received_bytes_.fetch_add(uart_data.size(), std::memory_order_relaxed); - - const auto written = data_buffer_.emplace_back_n( - [iter = uart_data.cbegin()](std::byte* storage) mutable noexcept { - *storage = *iter++; - }, - uart_data.size()); - if (written != uart_data.size()) { - const auto dropped = uart_data.size() - written; - overflow_count_.fetch_add(1, std::memory_order_relaxed); - overflow_dropped_bytes_.fetch_add(dropped, std::memory_order_relaxed); - if (should_log_overflow()) { - RCLCPP_WARN( - logger_, "VT13 input buffer overflow: dropped %zu of %zu bytes", dropped, - uart_data.size()); - } - } - } - - void update_status() { - const auto now = Clock::now(); - auto readable = data_buffer_.readable(); - peak_readable_ = std::max(peak_readable_, readable); - - while (readable) { - ReadResult result = VerificationFailed{}; - - const std::byte front = *data_buffer_.peek_front(); - if (front == std::byte{0xa9}) - result = read_remote_control_data(readable, now); - else if (front == std::byte(0xa5)) - result = read_referee_style_data(readable, now); - else { - unknown_prefix_count_++; - if (should_log_verification_failure(now)) { - // RCLCPP_WARN( - // logger_, "VT13 unknown prefix: front=0x%02x readable=%zu", - // std::to_integer(front), readable); - } - } - - if (std::holds_alternative(result)) { - break; - } - if (std::holds_alternative(result)) { - verification_failures_++; - data_buffer_.pop_front([](std::byte&&) noexcept {}); - readable--; - continue; - } - if (std::holds_alternative(result)) { - readable -= std::get(result).read; - continue; - } - } - - refresh_validity(now); - maybe_log_statistics(now); - } - - [[nodiscard]] ModeSwitch mode_switch() const noexcept { return mode_switch_; } - [[nodiscard]] bool valid() const noexcept { return valid_; } - - [[nodiscard]] const Eigen::Vector2d& joystick_left() const noexcept { return joystick_left_; } - [[nodiscard]] const Eigen::Vector2d& joystick_right() const noexcept { return joystick_right_; } - - [[nodiscard]] const Eigen::Vector2d& mouse_velocity() const noexcept { return mouse_velocity_; } - [[nodiscard]] double mouse_wheel() const noexcept { return mouse_wheel_; } - - [[nodiscard]] rmcs_msgs::Mouse mouse() const noexcept { return mouse_; } - [[nodiscard]] rmcs_msgs::Keyboard keyboard() const noexcept { return keyboard_; } - -private: - using Clock = std::chrono::steady_clock; - using TimePoint = Clock::time_point; - - static constexpr auto kFreshTimeout = std::chrono::milliseconds(500); - static constexpr auto kVerificationLogInterval = std::chrono::seconds(1); - static constexpr auto kOverflowLogInterval = std::chrono::seconds(1); - static constexpr auto kStatisticsLogInterval = std::chrono::seconds(5); - static constexpr std::size_t kRefereeFrameMaxSize = 256; - - struct Incomplete {}; - struct VerificationFailed {}; - struct Success { - std::size_t read; - }; - using ReadResult = std::variant; - - struct [[gnu::packed]] RemoteControlData { - static constexpr uint16_t kHeaderMagic = 0x53a9; - - uint16_t header; - - uint16_t joystick_channel0 : 11; - uint16_t joystick_channel1 : 11; - uint16_t joystick_channel2 : 11; - uint16_t joystick_channel3 : 11; - - uint8_t mode_switch : 2; - uint8_t pause_button : 1; - uint8_t left_custom_button : 1; - uint8_t right_custom_button : 1; - uint16_t dial : 11; - uint8_t trigger : 1; - uint8_t padding1 : 3; - - int16_t mouse_velocity_x; - int16_t mouse_velocity_y; - int16_t mouse_velocity_z; - uint8_t mouse_left : 2; - uint8_t mouse_right : 2; - uint8_t mouse_middle : 2; - uint8_t padding2 : 2; - - uint16_t keyboard; - - uint16_t crc16; - }; - - struct [[gnu::packed]] RefereeFrameHeader { - uint8_t sof; - uint16_t data_length; - uint8_t seq; - uint8_t crc8; - }; - - ReadResult read_remote_control_data(const std::size_t readable, const TimePoint now) { - if (readable < sizeof(RemoteControlData)) - return Incomplete{}; - - RemoteControlData data; - data_buffer_.peek_front_n( - [dst = reinterpret_cast(&data)](std::byte src) mutable noexcept { - *dst++ = src; - }, - sizeof(RemoteControlData)); - - if (data.header != RemoteControlData::kHeaderMagic) { - remote_bad_header_count_++; - if (should_log_verification_failure(now)) { - RCLCPP_WARN( - logger_, "VT13 remote control header invalid: header=0x%04x readable=%zu", - data.header, readable); - } - return VerificationFailed{}; - } - if (!rmcs_utility::dji_crc::verify_crc16(data)) { - remote_bad_crc_count_++; - if (should_log_verification_failure(now)) - RCLCPP_WARN(logger_, "VT13 remote control crc16 invalid: readable=%zu", readable); - return VerificationFailed{}; - } - - data_buffer_.pop_front_n([](std::byte&&) noexcept {}, sizeof(RemoteControlData)); - - update_remote_control_data(data); - valid_ = true; - last_remote_control_received_at_ = now; - remote_success_count_++; - return Success{sizeof(RemoteControlData)}; - } - - void update_remote_control_data(const RemoteControlData& data) { - mode_switch_ = static_cast(data.mode_switch + 1); - - joystick_right_ = { - channel_to_double(static_cast(data.joystick_channel1)), - -channel_to_double(static_cast(data.joystick_channel0)), - }; - joystick_left_ = { - channel_to_double(static_cast(data.joystick_channel2)), - -channel_to_double(static_cast(data.joystick_channel3)), - }; - - mouse_velocity_ = { - -data.mouse_velocity_y / 32768.0, - -data.mouse_velocity_x / 32768.0, - }; - mouse_wheel_ = -static_cast(data.mouse_velocity_z) / 32768.0; - - mouse_ = { - .left = static_cast(data.mouse_left), - .right = static_cast(data.mouse_right), - }; - keyboard_ = std::bit_cast(data.keyboard); - } - - ReadResult read_referee_style_data(const std::size_t readable, const TimePoint now) { - if (readable < sizeof(RefereeFrameHeader)) - return Incomplete{}; - - RefereeFrameHeader header; - data_buffer_.peek_front_n( - [dst = reinterpret_cast(&header)](std::byte src) mutable noexcept { - *dst++ = src; - }, - sizeof(RefereeFrameHeader)); - - if (!rmcs_utility::dji_crc::verify_crc8(header)) { - referee_bad_crc8_count_++; - if (should_log_verification_failure(now)) - RCLCPP_WARN(logger_, "VT13 referee header crc8 invalid: readable=%zu", readable); - return VerificationFailed{}; - } - - const std::size_t total_frame_size = - sizeof(RefereeFrameHeader) + 2 + header.data_length + 2; - if (total_frame_size > kRefereeFrameMaxSize) { - referee_oversize_count_++; - if (should_log_verification_failure(now)) { - RCLCPP_WARN( - logger_, "VT13 referee frame oversized: data_length=%u total=%zu readable=%zu", - header.data_length, total_frame_size, readable); - } - return VerificationFailed{}; - } - if (readable < total_frame_size) - return Incomplete{}; - - data_buffer_.pop_front_n([](std::byte&&) noexcept {}, total_frame_size); - referee_discarded_count_++; - return Success{total_frame_size}; - } - - bool should_log_verification_failure(const TimePoint now) { - if (last_verification_log_time_ != TimePoint::min() - && now - last_verification_log_time_ < kVerificationLogInterval) - return false; - last_verification_log_time_ = now; - return true; - } - - bool should_log_overflow() { - const auto now = Clock::now(); - if (last_overflow_log_time_ != TimePoint::min() - && now - last_overflow_log_time_ < kOverflowLogInterval) - return false; - - last_overflow_log_time_ = now; - return true; - } - - void refresh_validity(const TimePoint now) { - if (!valid_ || now - last_remote_control_received_at_ <= kFreshTimeout) - return; - - reset_remote_control_state(); - valid_ = false; - } - - void maybe_log_statistics(const TimePoint now) { - if (last_statistics_log_time_ == TimePoint::min()) { - last_statistics_log_time_ = now; - return; - } - - const auto elapsed = now - last_statistics_log_time_; - if (elapsed < kStatisticsLogInterval) - return; - - const auto readable = data_buffer_.readable(); - const auto store_calls = store_calls_.exchange(0, std::memory_order_relaxed); - const auto received_bytes = received_bytes_.exchange(0, std::memory_order_relaxed); - const auto overflow_count = overflow_count_.exchange(0, std::memory_order_relaxed); - const auto overflow_dropped_bytes = - overflow_dropped_bytes_.exchange(0, std::memory_order_relaxed); - const auto elapsed_seconds = std::chrono::duration(elapsed).count(); - - /* RCLCPP_INFO( - logger_, - "VT13 stats: rx=%.1f Hz %.1f B/s remote_ok=%zu verify_fail=%zu remote_bad_header=%zu " - "remote_bad_crc=%zu referee_discarded=%zu referee_bad_crc8=%zu referee_oversize=%zu " - "unknown_prefix=%zu overflow=%llu dropped=%llu readable=%zu peak=%zu valid=%s", - static_cast(store_calls) / elapsed_seconds, - static_cast(received_bytes) / elapsed_seconds, remote_success_count_, - verification_failures_, remote_bad_header_count_, remote_bad_crc_count_, - referee_discarded_count_, referee_bad_crc8_count_, referee_oversize_count_, - unknown_prefix_count_, static_cast(overflow_count), - static_cast(overflow_dropped_bytes), readable, peak_readable_, - valid_ ? "true" : "false"); - */ - - remote_success_count_ = 0; - verification_failures_ = 0; - remote_bad_header_count_ = 0; - remote_bad_crc_count_ = 0; - referee_discarded_count_ = 0; - referee_bad_crc8_count_ = 0; - referee_oversize_count_ = 0; - unknown_prefix_count_ = 0; - peak_readable_ = readable; - last_statistics_log_time_ = now; - } - - void reset_remote_control_state() { - mode_switch_ = ModeSwitch::kUnknown; - joystick_left_ = Eigen::Vector2d::Zero(); - joystick_right_ = Eigen::Vector2d::Zero(); - mouse_velocity_ = Eigen::Vector2d::Zero(); - mouse_wheel_ = 0; - mouse_ = rmcs_msgs::Mouse::zero(); - keyboard_ = rmcs_msgs::Keyboard::zero(); - } - - static double channel_to_double(int32_t value) { - value -= 1024; - if (-660 <= value && value <= 660) - return value / 660.0; - return 0.0; - } - - rclcpp::Logger logger_ = rclcpp::get_logger("vt13"); - rmcs_utility::RingBuffer data_buffer_{1024}; - - std::atomic store_calls_{0}; - std::atomic received_bytes_{0}; - std::atomic overflow_count_{0}; - std::atomic overflow_dropped_bytes_{0}; - - TimePoint last_remote_control_received_at_ = TimePoint::min(); - TimePoint last_verification_log_time_ = TimePoint::min(); - TimePoint last_overflow_log_time_ = TimePoint::min(); - TimePoint last_statistics_log_time_ = TimePoint::min(); - - bool valid_ = false; - std::size_t peak_readable_ = 0; - std::size_t remote_success_count_ = 0; - std::size_t verification_failures_ = 0; - std::size_t remote_bad_header_count_ = 0; - std::size_t remote_bad_crc_count_ = 0; - std::size_t referee_discarded_count_ = 0; - std::size_t referee_bad_crc8_count_ = 0; - std::size_t referee_oversize_count_ = 0; - std::size_t unknown_prefix_count_ = 0; - - ModeSwitch mode_switch_ = ModeSwitch::kUnknown; - - Eigen::Vector2d joystick_left_ = Eigen::Vector2d::Zero(); - Eigen::Vector2d joystick_right_ = Eigen::Vector2d::Zero(); - - Eigen::Vector2d mouse_velocity_ = Eigen::Vector2d::Zero(); - double mouse_wheel_ = 0; - - rmcs_msgs::Mouse mouse_ = rmcs_msgs::Mouse::zero(); - rmcs_msgs::Keyboard keyboard_ = rmcs_msgs::Keyboard::zero(); -}; - -} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six--friction.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six--friction.cpp deleted file mode 100644 index 3b4711690..000000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six--friction.cpp +++ /dev/null @@ -1,909 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "hardware/device/bmi088.hpp" -#include "hardware/device/can_packet.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/lk_motor.hpp" -#include "hardware/device/remote_control.hpp" -#include "hardware/device/supercap.hpp" -#include "hardware/device/vt13.hpp" - -namespace rmcs_core::hardware { - -class CanReceiveRateCounter { -public: - explicit CanReceiveRateCounter(rclcpp::Logger logger, std::string_view channel_name) - : logger_(std::move(logger)) - , channel_name_(channel_name) {} - - void record(std::uint32_t can_id) { - const auto now = Clock::now(); - - std::lock_guard lock{mutex_}; - auto& status = statuses_[can_id]; - ++status.receive_count; - status.last_receive_time = now; - - report_if_due(now); - } - - void report_if_due() { - const auto now = Clock::now(); - - std::lock_guard lock{mutex_}; - report_if_due(now); - } - -private: - using Clock = std::chrono::steady_clock; - - struct Status { - std::size_t receive_count{0}; - Clock::time_point last_receive_time{}; - }; - - void report_if_due(Clock::time_point now) { - if (statuses_.empty()) - return; - - if (last_report_time_ == Clock::time_point{}) { - last_report_time_ = now; - return; - } - - const auto elapsed = now - last_report_time_; - if (elapsed < kReportInterval) - return; - - const auto elapsed_seconds = std::chrono::duration(elapsed).count(); - for (auto& [can_id, status] : statuses_) { - const bool attached = now - status.last_receive_time <= kMissTimeout; - RCLCPP_INFO( - logger_, "[can rx] %.*s id=0x%03X rate=%.1fHz status=%s", - static_cast(channel_name_.size()), channel_name_.data(), - static_cast(can_id), - static_cast(status.receive_count) / elapsed_seconds, - attached ? "attach" : "miss"); - status.receive_count = 0; - } - - last_report_time_ = now; - } - - static constexpr std::chrono::milliseconds kReportInterval{1000}; - static constexpr std::chrono::milliseconds kMissTimeout{1000}; - - rclcpp::Logger logger_; - std::string_view channel_name_; - std::mutex mutex_; - Clock::time_point last_report_time_{}; - std::map statuses_; -}; - -class SteeringHeroLittle - : public rmcs_executor::Component - , public rclcpp::Node { -public: - SteeringHeroLittle() - : Node( - get_component_name(), - rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) - , command_component_( - create_partner_component( - get_component_name() + "_command", *this)) { - - register_output("/tf", tf_); - - gimbal_calibrate_subscription_ = create_subscription( - "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - gimbal_calibrate_subscription_callback(std::move(msg)); - }); - - top_board_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_top_board").as_string()); - - bottom_board_ = std::make_unique( - *this, *command_component_, get_parameter("serial_bottom_rmcs_board").as_string()); - - remote_control_ = - std::make_unique(*this, bottom_board_->dr16_, top_board_->vt13_); - - tf_->set_transform( - Eigen::Translation3d{0.06603, 0.0, 0.082}); - } - - SteeringHeroLittle(const SteeringHeroLittle&) = delete; - SteeringHeroLittle& operator=(const SteeringHeroLittle&) = delete; - SteeringHeroLittle(SteeringHeroLittle&&) = delete; - SteeringHeroLittle& operator=(SteeringHeroLittle&&) = delete; - - ~SteeringHeroLittle() override = default; - - void update() override { - top_board_->update(); - bottom_board_->update(); - remote_control_->update(); - - tf_->set_state( - bottom_board_->gimbal_bottom_yaw_motor_.angle() - + top_board_->gimbal_top_yaw_motor_.angle()); - tf_->set_state( - top_board_->gimbal_pitch_motor_.angle()); - } - - void command_update() { - top_board_->command_update(); - bottom_board_->command_update(); - } - -private: - void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New bottom yaw offset: %ld", - bottom_board_->gimbal_bottom_yaw_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New pitch offset: %ld", - top_board_->gimbal_pitch_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New player viewer offset: %ld", - top_board_->gimbal_player_viewer_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New top yaw offset: %ld", - top_board_->gimbal_top_yaw_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New bullet feeder offset: %ld", - top_board_->gimbal_bullet_feeder_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] left front steering offset: %d", - bottom_board_->chassis_steering_motors_[0].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] right front steering offset: %d", - bottom_board_->chassis_steering_motors_[1].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] left back steering offset: %d", - bottom_board_->chassis_steering_motors_[2].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] right back steering offset: %d", - bottom_board_->chassis_steering_motors_[3].calibrate_zero_point()); - } - - class SteeringHeroLittleCommand : public rmcs_executor::Component { - public: - explicit SteeringHeroLittleCommand(SteeringHeroLittle& hero) - : hero_(hero) {} - - void update() override { hero_.command_update(); } - - SteeringHeroLittle& hero_; - }; - std::shared_ptr command_component_; - - class TopBoard final : private librmcs::agent::RmcsBoardLite { - public: - friend class SteeringHeroLittle; - explicit TopBoard( - SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, - std::string_view board_serial = {}) - : librmcs::agent::RmcsBoardLite(board_serial) - , logger_(steering_hero.get_logger()) - // , can0_receive_rate_counter_(logger_, "bottom/can0") - // , can1_receive_rate_counter_(logger_, "bottom/can1") - // , can2_receive_rate_counter_(logger_, "bottom/can2") - // , can3_receive_rate_counter_(logger_, "bottom/can3") - , tf_(steering_hero.tf_) - , imu_(1000, 0.2, 0.0) - , gimbal_top_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/top_yaw") - , gimbal_pitch_motor_(steering_hero, steering_hero_command, "/gimbal/pitch") - , gimbal_friction_wheels_( - {steering_hero, steering_hero_command, "/gimbal/first_front_friction"}, - {steering_hero, steering_hero_command, "/gimbal/first_back_friction"}, - {steering_hero, steering_hero_command, "/gimbal/second_front_friction"}, - {steering_hero, steering_hero_command, "/gimbal/second_back_friction"}, - {steering_hero, steering_hero_command, "/gimbal/third_front_friction"}, - {steering_hero, steering_hero_command, "/gimbal/third_back_friction"}) - , gimbal_bullet_feeder_(steering_hero, steering_hero_command, "/gimbal/bullet_feeder") - , putter_motor_(steering_hero, steering_hero_command, "/gimbal/putter") - , gimbal_scope_motor_(steering_hero, steering_hero_command, "/gimbal/scope") - , gimbal_player_viewer_motor_( - steering_hero, steering_hero_command, "/gimbal/player_viewer") { - - gimbal_top_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} - .enable_multi_turn_angle() - .set_encoder_zero_point( - static_cast( - steering_hero.get_parameter("top_yaw_motor_zero_point").as_int()))); - gimbal_pitch_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} - .set_encoder_zero_point( - static_cast( - steering_hero.get_parameter("pitch_motor_zero_point").as_int())) - .enable_multi_turn_angle()); - gimbal_friction_wheels_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(1.)); - gimbal_friction_wheels_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(1.)); - gimbal_friction_wheels_[2].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); - gimbal_friction_wheels_[3].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); - gimbal_friction_wheels_[4].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(1.)); - gimbal_friction_wheels_[5].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(1.)); - gimbal_bullet_feeder_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} - .set_encoder_zero_point( - static_cast( - steering_hero.get_parameter("bullet_feeder_motor_zero_point").as_int())) - .set_reversed() - .enable_multi_turn_angle()); - putter_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(1.) - .enable_multi_turn_angle()); - gimbal_scope_motor_.configure(device::DjiMotor::Config{device::DjiMotor::Type::kM2006}); - gimbal_player_viewer_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} - .set_encoder_zero_point( - static_cast( - steering_hero.get_parameter("viewer_motor_zero_point").as_int())) - .set_reversed() - .enable_multi_turn_angle()); - - steering_hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); - steering_hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); - - steering_hero.register_output( - "/gimbal/photoelectric_sensor", photoelectric_sensor_status_, false); - steering_hero.register_output( - "/gimbal/grayscale_sensor", grayscale_sensor_status_, false); - steering_hero.register_output( - "/auto_aim/image_capturer/timestamp", camera_capturer_trigger_timestamp_, 0); - steering_hero.register_output( - "/auto_aim/image_capturer/trigger", camera_capturer_trigger_, 0); - - imu_.set_coordinate_mapping([](double x, double y, double z) { - // Get the mapping with the following code. - // The rotation angle must be an exact multiple of 90 degrees, otherwise - // use a matrix. - - return std::make_tuple(y, -x, z); - }); - } - - TopBoard(const TopBoard&) = delete; - TopBoard& operator=(const TopBoard&) = delete; - TopBoard(TopBoard&&) = delete; - TopBoard& operator=(TopBoard&&) = delete; - - ~TopBoard() final = default; - - void update() { - // can0_receive_rate_counter_.report_if_due(); - // can1_receive_rate_counter_.report_if_due(); - // can2_receive_rate_counter_.report_if_due(); - // can3_receive_rate_counter_.report_if_due(); - - imu_.update_status(); - vt13_.update_status(); - Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; - - tf_->set_transform( - gimbal_imu_pose.conjugate()); - - *gimbal_yaw_velocity_imu_ = imu_.gz(); - *gimbal_pitch_velocity_imu_ = imu_.gy(); - - gimbal_top_yaw_motor_.update_status(); - gimbal_pitch_motor_.update_status(); - tf_->set_state( - gimbal_pitch_motor_.angle()); - - for (auto& motor : gimbal_friction_wheels_) - motor.update_status(); - - gimbal_bullet_feeder_.update_status(); - putter_motor_.update_status(); - - gimbal_player_viewer_motor_.update_status(); - tf_->set_state( - gimbal_player_viewer_motor_.angle()); - - gimbal_scope_motor_.update_status(); - - if (last_camera_capturer_trigger_timestamp_ != *camera_capturer_trigger_timestamp_) - *camera_capturer_trigger_ = true; - last_camera_capturer_trigger_timestamp_ = *camera_capturer_trigger_timestamp_; - - *photoelectric_sensor_status_ = photoelectric_sensor_status_atomic.load(); - *grayscale_sensor_status_ = grayscale_sensor_status_atomic.load(); - - if (++count_ == 250) { - for (int i = 0; i < 6; ++i) { - if (friciton_detect[i] == 0) { - RCLCPP_WARN(logger_, "friction can id 0x%03X missing", i + 0x201); - } - } - std::fill_n(friciton_detect, 6, 0); - for (int i = 0; i < 3; ++i) { - if (can0_detect[i] == 0) { - RCLCPP_WARN(logger_, "top board can id 0x%03X missing", i + 0x141); - } - } - std::fill_n(can0_detect, 3, 0); - count_ = 0; - } - } - - void command_update() { - auto builder = start_transmit(); - - if (std::isfinite(gimbal_pitch_motor_.control_angle())) - builder.can0_transmit({ - .can_id = 0x143, - .can_data = gimbal_pitch_motor_ - .generate_angle_command(gimbal_pitch_motor_.control_angle()) - .as_bytes(), - }); - else - builder.can0_transmit({ - .can_id = 0x143, - .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), - }); // Used to distinguish pitch encoder control from IMU control. - - builder.can0_transmit({ - .can_id = 0x141, - .can_data = gimbal_top_yaw_motor_.generate_command().as_bytes(), - }); - - builder.can0_transmit({ - .can_id = 0x142, - .can_data = gimbal_bullet_feeder_.generate_torque_command().as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_friction_wheels_[0].generate_command(), - gimbal_friction_wheels_[1].generate_command(), - gimbal_friction_wheels_[2].generate_command(), - gimbal_friction_wheels_[3].generate_command(), - } - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_friction_wheels_[4].generate_command(), - gimbal_friction_wheels_[5].generate_command(), - putter_motor_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - - builder.gpio_digital_read( - librmcs::spec::rmcs_board_lite::kGpioDescriptors[2], - { - .period_ms = 20, - .pull = librmcs::data::GpioPull::kUp, - }); - } - - private: - void can0_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can0_receive_rate_counter_.record(can_id); - can0_detect[can_id - 0x141] = 1; - if (can_id == 0x141) { - gimbal_top_yaw_motor_.store_status(data.can_data); - } else if (can_id == 0x143) { - gimbal_pitch_motor_.store_status(data.can_data); - } else if (can_id == 0x142) { - gimbal_bullet_feeder_.store_status(data.can_data); - } - } - - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can1_receive_rate_counter_.record(can_id); - friciton_detect[can_id - 0x201] = 1; - if (can_id == 0x201) { - gimbal_friction_wheels_[0].store_status(data.can_data); - } else if (can_id == 0x202) { - gimbal_friction_wheels_[1].store_status(data.can_data); - } else if (can_id == 0x203) { - gimbal_friction_wheels_[2].store_status(data.can_data); - } else if (can_id == 0x204) { - gimbal_friction_wheels_[3].store_status(data.can_data); - } - } - - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can2_receive_rate_counter_.record(can_id); - if (can_id == 0x203) { - putter_motor_.store_status(data.can_data); - } else if (can_id == 0x201) { - gimbal_friction_wheels_[4].store_status(data.can_data); - friciton_detect[4] = 1; - } else if (can_id == 0x202) { - gimbal_friction_wheels_[5].store_status(data.can_data); - friciton_detect[5] = 1; - } - } - - void gpio_digital_read_result_callback( - const librmcs::spec::rmcs_board_lite::GpioDescriptor& gpio, - const librmcs::data::GpioDigitalDataView& data) override { - if (gpio.channel_index == 2) { - photoelectric_sensor_status_atomic.store(data.high); - } - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - - void uart0_receive_callback(const librmcs::data::UartDataView& data) override { - vt13_.store_status(data.uart_data); - } - - rclcpp::Logger logger_; - // CanReceiveRateCounter can0_receive_rate_counter_; - // CanReceiveRateCounter can1_receive_rate_counter_; - // CanReceiveRateCounter can2_receive_rate_counter_; - // CanReceiveRateCounter can3_receive_rate_counter_; - OutputInterface& tf_; - - std::time_t last_camera_capturer_trigger_timestamp_{0}; - int count_ = 0; - int friciton_detect[6]; - int can0_detect[3]; - - device::Bmi088 imu_; - device::Vt13 vt13_; - device::LkMotor gimbal_top_yaw_motor_; - device::LkMotor gimbal_pitch_motor_; - device::DjiMotor gimbal_friction_wheels_[6]; - device::LkMotor gimbal_bullet_feeder_; - device::DjiMotor putter_motor_; - device::DjiMotor gimbal_scope_motor_; - device::LkMotor gimbal_player_viewer_motor_; - - OutputInterface gimbal_yaw_velocity_imu_; - OutputInterface gimbal_pitch_velocity_imu_; - OutputInterface photoelectric_sensor_status_; - OutputInterface grayscale_sensor_status_; - OutputInterface camera_capturer_trigger_; - OutputInterface camera_capturer_trigger_timestamp_; - std::atomic photoelectric_sensor_status_atomic{false}; - std::atomic grayscale_sensor_status_atomic{false}; - }; - - class BottomBoard final : private librmcs::agent::RmcsBoardLite { - public: - friend class SteeringHeroLittle; - explicit BottomBoard( - SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, - std::string_view board_serial = {}) - : librmcs::agent::RmcsBoardLite( - board_serial, {.dangerously_skip_version_checks = false}) - , logger_(steering_hero.get_logger()) - // , can0_receive_rate_counter_(logger_, "bottom/can0") - // , can1_receive_rate_counter_(logger_, "bottom/can1") - // , can2_receive_rate_counter_(logger_, "bottom/can2") - // , can3_receive_rate_counter_(logger_, "bottom/can3") - , imu_(1000, 0.2, 0.0) - , supercap_(steering_hero, steering_hero_command) - , chassis_steering_motors_( - {steering_hero, steering_hero_command, "/chassis/left_front_steering"}, - {steering_hero, steering_hero_command, "/chassis/right_front_steering"}, - {steering_hero, steering_hero_command, "/chassis/left_back_steering"}, - {steering_hero, steering_hero_command, "/chassis/right_back_steering"}) - , chassis_wheel_motors_( - {steering_hero, steering_hero_command, "/chassis/left_front_wheel"}, - {steering_hero, steering_hero_command, "/chassis/right_front_wheel"}, - {steering_hero, steering_hero_command, "/chassis/left_back_wheel"}, - {steering_hero, steering_hero_command, "/chassis/right_back_wheel"}) - , chassis_front_climber_motor_( - {steering_hero, steering_hero_command, "/chassis/climber/left_front_motor"}, - {steering_hero, steering_hero_command, "/chassis/climber/right_front_motor"}) - , chassis_back_climber_motor_( - {steering_hero, steering_hero_command, "/chassis/climber/left_back_motor"}, - {steering_hero, steering_hero_command, "/chassis/climber/right_back_motor"}) - , yaw_brake_motor_(steering_hero, steering_hero_command, "/gimbal/yaw_brake") - , gimbal_bottom_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/bottom_yaw") { - // - chassis_steering_motors_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast( - steering_hero.get_parameter("left_front_zero_point").as_int())) - .set_reversed()); - chassis_steering_motors_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast( - steering_hero.get_parameter("right_front_zero_point").as_int())) - .set_reversed()); - chassis_steering_motors_[2].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast( - steering_hero.get_parameter("left_back_zero_point").as_int())) - .set_reversed()); - chassis_steering_motors_[3].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast( - steering_hero.get_parameter("right_back_zero_point").as_int())) - .set_reversed()); - - chassis_wheel_motors_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)); - chassis_wheel_motors_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)); - chassis_wheel_motors_[2].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)); - chassis_wheel_motors_[3].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)); - chassis_front_climber_motor_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(19.)); - chassis_front_climber_motor_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - chassis_back_climber_motor_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .enable_multi_turn_angle() - .set_reduction_ratio(19.)); - chassis_back_climber_motor_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .enable_multi_turn_angle() - .set_reduction_ratio(19.)); - - yaw_brake_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.set_reduction_ratio(1.)); - gimbal_bottom_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} - .set_reversed() - .set_encoder_zero_point( - static_cast( - steering_hero.get_parameter("bottom_yaw_motor_zero_point").as_int()))); - - steering_hero.register_output("/referee/serial", referee_serial_); - referee_serial_->read = [this](std::byte* buffer, size_t size) { - return referee_ring_buffer_receive_.pop_front_n( - - [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); - }; - referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart0_transmit( - {.uart_data = std::span{buffer, size}}); - return size; - }; - steering_hero.register_output( - "/chassis/powermeter/control_enable", powermeter_control_enabled_, false); - steering_hero.register_output( - "/chassis/powermeter/charge_power_limit", powermeter_charge_power_limit_, 0.); - - steering_hero.register_output( - "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - steering_hero.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); - } - - BottomBoard(const BottomBoard&) = delete; - BottomBoard& operator=(const BottomBoard&) = delete; - BottomBoard(BottomBoard&&) = delete; - BottomBoard& operator=(BottomBoard&&) = delete; - - ~BottomBoard() final = default; - - void update() { - // can0_receive_rate_counter_.report_if_due(); - // can1_receive_rate_counter_.report_if_due(); - // can2_receive_rate_counter_.report_if_due(); - // can3_receive_rate_counter_.report_if_due(); - - imu_.update_status(); - dr16_.update_status(); - supercap_.update_status(); - - *chassis_yaw_velocity_imu_ = imu_.gz(); - *chassis_pitch_imu_ = -std::asin(2.0 * (imu_.q0() * imu_.q2() - imu_.q3() * imu_.q1())); - - chassis_front_climber_motor_[0].update_status(); - chassis_front_climber_motor_[1].update_status(); - chassis_back_climber_motor_[0].update_status(); - chassis_back_climber_motor_[1].update_status(); - - for (auto& motor : chassis_wheel_motors_) - motor.update_status(); - for (auto& motor : chassis_steering_motors_) - motor.update_status(); - - yaw_brake_motor_.update_status(); - gimbal_bottom_yaw_motor_.update_status(); - - if (++count_ == 250) { - for (int i = 0; i < 8; ++i) { - if (check[i] == 0) { - RCLCPP_WARN(logger_, "bottom board can id 0x%03X missing", i + 0x201); - } - } - std::fill_n(check, 8, 0); - count_ = 0; - } - } - - void command_update() { - auto builder = start_transmit(); - - builder.can0_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), - chassis_wheel_motors_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - - builder.can0_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steering_motors_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - chassis_steering_motors_[0].generate_command(), - } - .as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - chassis_wheel_motors_[2].generate_command(), - chassis_wheel_motors_[3].generate_command(), - } - .as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - chassis_steering_motors_[3].generate_command(), - chassis_steering_motors_[2].generate_command(), - supercap_.generate_command(), - } - .as_bytes(), - }); - - builder.can3_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - chassis_back_climber_motor_[1].generate_command(), - yaw_brake_motor_.generate_command(), - chassis_back_climber_motor_[0].generate_command(), - } - .as_bytes(), - }); - - builder.can3_transmit({ - .can_id = 0x141, - .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_front_climber_motor_[0].generate_command(), - device::CanPacket8::PaddingQuarter{}, - chassis_front_climber_motor_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - } - - private: - void can0_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can0_receive_rate_counter_.record(can_id); - check[can_id - 0x201] = 1; - if (can_id == 0x201) { - chassis_wheel_motors_[0].store_status(data.can_data); - } else if (can_id == 0x202) { - chassis_wheel_motors_[1].store_status(data.can_data); - } else if (can_id == 0x205) { - chassis_steering_motors_[1].store_status(data.can_data); - } else if (can_id == 0x208) { - chassis_steering_motors_[0].store_status(data.can_data); - } - } - - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can1_receive_rate_counter_.record(can_id); - if (can_id != 0x300) { - check[can_id - 0x201] = 1; - } - if (can_id == 0x203) { - chassis_wheel_motors_[2].store_status(data.can_data); - } else if (can_id == 0x204) { - chassis_wheel_motors_[3].store_status(data.can_data); - } else if (can_id == 0x207) { - chassis_steering_motors_[2].store_status(data.can_data); - } else if (can_id == 0x206) { - chassis_steering_motors_[3].store_status(data.can_data); - } else if (can_id == 0x300) { - supercap_.store_status(data.can_data); - } - } - - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - auto can_id = data.can_id; - // can2_receive_rate_counter_.record(can_id); - if (can_id == 0x201) { - chassis_front_climber_motor_[0].store_status(data.can_data); - } else if (can_id == 0x203) { - chassis_front_climber_motor_[1].store_status(data.can_data); - } - } - - void can3_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can3_receive_rate_counter_.record(can_id); - if (can_id == 0x202) { - chassis_back_climber_motor_[1].store_status(data.can_data); - } else if (can_id == 0x204) { - chassis_back_climber_motor_[0].store_status(data.can_data); - } else if (can_id == 0x203) { - yaw_brake_motor_.store_status(data.can_data); - } else if (can_id == 0x141) { - gimbal_bottom_yaw_motor_.store_status(data.can_data); - } - } - - void uart0_receive_callback(const librmcs::data::UartDataView& data) override { - const std::byte* ptr = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, data.uart_data.size()); - } - - void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - - rclcpp::Logger logger_; - // CanReceiveRateCounter can0_receive_rate_counter_; - // CanReceiveRateCounter can1_receive_rate_counter_; - // CanReceiveRateCounter can2_receive_rate_counter_; - // CanReceiveRateCounter can3_receive_rate_counter_; - - int count_ = 0; - int check[10] = {0}; - - device::Bmi088 imu_; - device::Dr16 dr16_; - device::Supercap supercap_; - - device::DjiMotor chassis_steering_motors_[4]; - device::DjiMotor chassis_wheel_motors_[4]; - device::DjiMotor chassis_front_climber_motor_[2]; - device::DjiMotor chassis_back_climber_motor_[2]; - device::DjiMotor yaw_brake_motor_; - device::LkMotor gimbal_bottom_yaw_motor_; - - rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; - - OutputInterface referee_serial_; - OutputInterface powermeter_control_enabled_; - OutputInterface powermeter_charge_power_limit_; - OutputInterface chassis_yaw_velocity_imu_; - OutputInterface chassis_pitch_imu_; - }; - - OutputInterface tf_; - - rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; - - std::shared_ptr top_board_; - std::shared_ptr bottom_board_; - std::unique_ptr remote_control_; -}; - -} // namespace rmcs_core::hardware - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringHeroLittle, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp index 91d70545a..eb23cdbb9 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp @@ -1,906 +1,897 @@ -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include - -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include - -// #include "hardware/device/bmi088.hpp" -// #include "hardware/device/can_packet.hpp" -// #include "hardware/device/dji_motor.hpp" -// #include "hardware/device/dr16.hpp" -// #include "hardware/device/lk_motor.hpp" -// #include "hardware/device/remote_control.hpp" -// #include "hardware/device/supercap.hpp" -// #include "hardware/device/vt13.hpp" - -// namespace rmcs_core::hardware { - -// class CanReceiveRateCounter { -// public: -// explicit CanReceiveRateCounter(rclcpp::Logger logger, std::string_view channel_name) -// : logger_(std::move(logger)) -// , channel_name_(channel_name) {} - -// void record(std::uint32_t can_id) { -// const auto now = Clock::now(); - -// std::lock_guard lock{mutex_}; -// auto& status = statuses_[can_id]; -// ++status.receive_count; -// status.last_receive_time = now; - -// report_if_due(now); -// } - -// void report_if_due() { -// const auto now = Clock::now(); - -// std::lock_guard lock{mutex_}; -// report_if_due(now); -// } - -// private: -// using Clock = std::chrono::steady_clock; - -// struct Status { -// std::size_t receive_count{0}; -// Clock::time_point last_receive_time{}; -// }; - -// void report_if_due(Clock::time_point now) { -// if (statuses_.empty()) -// return; - -// if (last_report_time_ == Clock::time_point{}) { -// last_report_time_ = now; -// return; -// } - -// const auto elapsed = now - last_report_time_; -// if (elapsed < kReportInterval) -// return; - -// const auto elapsed_seconds = std::chrono::duration(elapsed).count(); -// for (auto& [can_id, status] : statuses_) { -// const bool attached = now - status.last_receive_time <= kMissTimeout; -// RCLCPP_INFO( -// logger_, "[can rx] %.*s id=0x%03X rate=%.1fHz status=%s", -// static_cast(channel_name_.size()), channel_name_.data(), -// static_cast(can_id), -// static_cast(status.receive_count) / elapsed_seconds, -// attached ? "attach" : "miss"); -// status.receive_count = 0; -// } - -// last_report_time_ = now; -// } - -// static constexpr std::chrono::milliseconds kReportInterval{1000}; -// static constexpr std::chrono::milliseconds kMissTimeout{1000}; - -// rclcpp::Logger logger_; -// std::string_view channel_name_; -// std::mutex mutex_; -// Clock::time_point last_report_time_{}; -// std::map statuses_; -// }; - -// class SteeringHeroLittle -// : public rmcs_executor::Component -// , public rclcpp::Node { -// public: -// SteeringHeroLittle() -// : Node( -// get_component_name(), -// rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) -// , command_component_( -// create_partner_component( -// get_component_name() + "_command", *this)) { - -// register_output("/tf", tf_); - -// gimbal_calibrate_subscription_ = create_subscription( -// "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { -// gimbal_calibrate_subscription_callback(std::move(msg)); -// }); - -// top_board_ = std::make_unique( -// *this, *command_component_, get_parameter("board_serial_top_board").as_string()); - -// bottom_board_ = std::make_unique( -// *this, *command_component_, get_parameter("serial_bottom_rmcs_board").as_string()); - -// remote_control_ = -// std::make_unique(*this, bottom_board_->dr16_, -// top_board_->vt13_); - -// tf_->set_transform( -// Eigen::Translation3d{0.06603, 0.0, 0.082}); -// } - -// SteeringHeroLittle(const SteeringHeroLittle&) = delete; -// SteeringHeroLittle& operator=(const SteeringHeroLittle&) = delete; -// SteeringHeroLittle(SteeringHeroLittle&&) = delete; -// SteeringHeroLittle& operator=(SteeringHeroLittle&&) = delete; - -// ~SteeringHeroLittle() override = default; - -// void update() override { -// top_board_->update(); -// bottom_board_->update(); -// remote_control_->update(); - -// tf_->set_state( -// bottom_board_->gimbal_bottom_yaw_motor_.angle() -// + top_board_->gimbal_top_yaw_motor_.angle()); -// tf_->set_state( -// top_board_->gimbal_pitch_motor_.angle()); -// } - -// void command_update() { -// top_board_->command_update(); -// bottom_board_->command_update(); -// } - -// private: -// void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { -// RCLCPP_INFO( -// get_logger(), "[gimbal calibration] New bottom yaw offset: %ld", -// bottom_board_->gimbal_bottom_yaw_motor_.calibrate_zero_point()); -// RCLCPP_INFO( -// get_logger(), "[gimbal calibration] New pitch offset: %ld", -// top_board_->gimbal_pitch_motor_.calibrate_zero_point()); -// RCLCPP_INFO( -// get_logger(), "[gimbal calibration] New player viewer offset: %ld", -// top_board_->gimbal_player_viewer_motor_.calibrate_zero_point()); -// RCLCPP_INFO( -// get_logger(), "[gimbal calibration] New top yaw offset: %ld", -// top_board_->gimbal_top_yaw_motor_.calibrate_zero_point()); -// RCLCPP_INFO( -// get_logger(), "[gimbal calibration] New bullet feeder offset: %ld", -// top_board_->gimbal_bullet_feeder_.calibrate_zero_point()); -// RCLCPP_INFO( -// get_logger(), "[chassis calibration] left front steering offset: %d", -// bottom_board_->chassis_steering_motors_[0].calibrate_zero_point()); -// RCLCPP_INFO( -// get_logger(), "[chassis calibration] right front steering offset: %d", -// bottom_board_->chassis_steering_motors_[1].calibrate_zero_point()); -// RCLCPP_INFO( -// get_logger(), "[chassis calibration] left back steering offset: %d", -// bottom_board_->chassis_steering_motors_[2].calibrate_zero_point()); -// RCLCPP_INFO( -// get_logger(), "[chassis calibration] right back steering offset: %d", -// bottom_board_->chassis_steering_motors_[3].calibrate_zero_point()); -// } - -// class SteeringHeroLittleCommand : public rmcs_executor::Component { -// public: -// explicit SteeringHeroLittleCommand(SteeringHeroLittle& hero) -// : hero_(hero) {} - -// void update() override { hero_.command_update(); } - -// SteeringHeroLittle& hero_; -// }; -// std::shared_ptr command_component_; - -// class TopBoard final : private librmcs::agent::RmcsBoardLite { -// public: -// friend class SteeringHeroLittle; -// explicit TopBoard( -// SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, -// std::string_view board_serial = {}) -// : librmcs::agent::RmcsBoardLite(board_serial) -// , logger_(steering_hero.get_logger()) -// // , can0_receive_rate_counter_(logger_, "bottom/can0") -// // , can1_receive_rate_counter_(logger_, "bottom/can1") -// // , can2_receive_rate_counter_(logger_, "bottom/can2") -// // , can3_receive_rate_counter_(logger_, "bottom/can3") -// , tf_(steering_hero.tf_) -// , imu_(1000, 0.2, 0.0) -// , gimbal_top_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/top_yaw") -// , gimbal_pitch_motor_(steering_hero, steering_hero_command, "/gimbal/pitch") -// , gimbal_friction_wheels_( -// {steering_hero, steering_hero_command, "/gimbal/first_right_friction"}, -// {steering_hero, steering_hero_command, "/gimbal/first_left_friction"}, -// {steering_hero, steering_hero_command, "/gimbal/second_right_friction"}, -// {steering_hero, steering_hero_command, "/gimbal/second_left_friction"}) -// , gimbal_bullet_feeder_(steering_hero, steering_hero_command, -// "/gimbal/bullet_feeder") , putter_motor_(steering_hero, steering_hero_command, -// "/gimbal/putter") , gimbal_scope_motor_(steering_hero, steering_hero_command, -// "/gimbal/scope") , gimbal_player_viewer_motor_( -// steering_hero, steering_hero_command, "/gimbal/player_viewer") { - -// gimbal_top_yaw_motor_.configure( -// device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} -// .enable_multi_turn_angle() -// .set_encoder_zero_point( -// static_cast( -// steering_hero.get_parameter("top_yaw_motor_zero_point").as_int()))); -// gimbal_pitch_motor_.configure( -// device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} -// .set_encoder_zero_point( -// static_cast( -// steering_hero.get_parameter("pitch_motor_zero_point").as_int())) -// .enable_multi_turn_angle()); -// gimbal_friction_wheels_[0].configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); -// gimbal_friction_wheels_[1].configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508} -// .set_reversed() -// .set_reduction_ratio(1.)); -// gimbal_friction_wheels_[2].configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); -// gimbal_friction_wheels_[3].configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508} -// .set_reversed() -// .set_reduction_ratio(1.)); -// gimbal_bullet_feeder_.configure( -// device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} -// .set_encoder_zero_point( -// static_cast( -// steering_hero.get_parameter("bullet_feeder_motor_zero_point").as_int())) -// .set_reversed() -// .enable_multi_turn_angle()); -// putter_motor_.configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508} -// .set_reduction_ratio(1.) -// .enable_multi_turn_angle()); -// gimbal_scope_motor_.configure(device::DjiMotor::Config{device::DjiMotor::Type::kM2006}); -// gimbal_player_viewer_motor_.configure( -// device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} -// .set_encoder_zero_point( -// static_cast( -// steering_hero.get_parameter("viewer_motor_zero_point").as_int())) -// .set_reversed() -// .enable_multi_turn_angle()); - -// steering_hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); -// steering_hero.register_output("/gimbal/pitch/velocity_imu", -// gimbal_pitch_velocity_imu_); - -// steering_hero.register_output( -// "/gimbal/photoelectric_sensor", photoelectric_sensor_status_, false); -// steering_hero.register_output( -// "/gimbal/grayscale_sensor", grayscale_sensor_status_, false); -// steering_hero.register_output( -// "/auto_aim/image_capturer/timestamp", camera_capturer_trigger_timestamp_, 0); -// steering_hero.register_output( -// "/auto_aim/image_capturer/trigger", camera_capturer_trigger_, 0); - -// imu_.set_coordinate_mapping([](double x, double y, double z) { -// // Get the mapping with the following code. -// // The rotation angle must be an exact multiple of 90 degrees, otherwise -// // use a matrix. - -// return std::make_tuple(-y, x, z); -// }); -// } - -// TopBoard(const TopBoard&) = delete; -// TopBoard& operator=(const TopBoard&) = delete; -// TopBoard(TopBoard&&) = delete; -// TopBoard& operator=(TopBoard&&) = delete; - -// ~TopBoard() final = default; - -// void update() { -// // can0_receive_rate_counter_.report_if_due(); -// // can1_receive_rate_counter_.report_if_due(); -// // can2_receive_rate_counter_.report_if_due(); -// // can3_receive_rate_counter_.report_if_due(); - -// imu_.update_status(); -// vt13_.update_status(); -// Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; - -// tf_->set_transform( -// gimbal_imu_pose.conjugate()); - -// *gimbal_yaw_velocity_imu_ = imu_.gz(); -// *gimbal_pitch_velocity_imu_ = imu_.gy(); - -// gimbal_top_yaw_motor_.update_status(); -// gimbal_pitch_motor_.update_status(); -// tf_->set_state( -// gimbal_pitch_motor_.angle()); - -// for (auto& motor : gimbal_friction_wheels_) -// motor.update_status(); - -// gimbal_bullet_feeder_.update_status(); -// putter_motor_.update_status(); - -// gimbal_player_viewer_motor_.update_status(); -// tf_->set_state( -// gimbal_player_viewer_motor_.angle()); - -// gimbal_scope_motor_.update_status(); - -// if (last_camera_capturer_trigger_timestamp_ != *camera_capturer_trigger_timestamp_) -// *camera_capturer_trigger_ = true; -// last_camera_capturer_trigger_timestamp_ = *camera_capturer_trigger_timestamp_; - -// *photoelectric_sensor_status_ = photoelectric_sensor_status_atomic.load(); -// *grayscale_sensor_status_ = grayscale_sensor_status_atomic.load(); -// } - -// void command_update() { -// auto builder = start_transmit(); - -// if (std::isfinite(gimbal_pitch_motor_.control_angle())) -// builder.can0_transmit({ -// .can_id = 0x142, -// .can_data = gimbal_pitch_motor_ -// .generate_angle_command(gimbal_pitch_motor_.control_angle()) -// .as_bytes(), -// }); -// else -// builder.can0_transmit({ -// .can_id = 0x142, -// .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), -// }); // Used to distinguish pitch encoder control from IMU control. - -// builder.can0_transmit({ -// .can_id = 0x141, -// .can_data = gimbal_top_yaw_motor_.generate_command().as_bytes(), -// }); - -// builder.can1_transmit({ -// .can_id = 0x200, -// .can_data = -// device::CanPacket8{ -// gimbal_friction_wheels_[0].generate_command(), -// gimbal_friction_wheels_[1].generate_command(), -// gimbal_friction_wheels_[2].generate_command(), -// gimbal_friction_wheels_[3].generate_command(), -// } -// .as_bytes(), -// }); - -// builder.can1_transmit({ -// .can_id = 0x1FF, -// .can_data = -// device::CanPacket8{ -// putter_motor_.generate_command(), -// gimbal_scope_motor_.generate_command(), -// device::CanPacket8::PaddingQuarter{}, -// device::CanPacket8::PaddingQuarter{}, -// } -// .as_bytes(), -// }); - -// builder.can2_transmit({ -// .can_id = 0x143, -// .can_data = -// gimbal_player_viewer_motor_ -// .generate_velocity_command(gimbal_player_viewer_motor_.control_velocity()) -// .as_bytes(), -// }); - -// builder.can3_transmit({ -// .can_id = 0x142, -// .can_data = gimbal_bullet_feeder_.generate_torque_command().as_bytes(), -// }); - -// builder.gpio_digital_read( -// librmcs::spec::rmcs_board_lite::kGpioDescriptors[2], -// { -// .period_ms = 20, -// .pull = librmcs::data::GpioPull::kUp, -// }); - -// builder.gpio_digital_read( -// librmcs::spec::rmcs_board_lite::kGpioDescriptors[3], -// { -// .period_ms = 20, -// .pull = librmcs::data::GpioPull::kUp, -// }); -// } - -// private: -// void can0_receive_callback(const librmcs::data::CanDataView& data) override { -// if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] -// return; -// auto can_id = data.can_id; -// // can0_receive_rate_counter_.record(can_id); -// if (can_id == 0x141) { -// gimbal_top_yaw_motor_.store_status(data.can_data); -// } else if (can_id == 0x142) { -// gimbal_pitch_motor_.store_status(data.can_data); -// } -// } - -// void can1_receive_callback(const librmcs::data::CanDataView& data) override { -// if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] -// return; -// auto can_id = data.can_id; -// // can1_receive_rate_counter_.record(can_id); -// if (can_id == 0x201) { -// gimbal_friction_wheels_[0].store_status(data.can_data); -// } else if (can_id == 0x202) { -// gimbal_friction_wheels_[1].store_status(data.can_data); -// } else if (can_id == 0x203) { -// gimbal_friction_wheels_[2].store_status(data.can_data); -// } else if (can_id == 0x204) { -// gimbal_friction_wheels_[3].store_status(data.can_data); -// } else if (can_id == 0x205) { -// putter_motor_.store_status(data.can_data); -// } else if (can_id == 0x206) { -// gimbal_scope_motor_.store_status(data.can_data); -// } -// } - -// void can2_receive_callback(const librmcs::data::CanDataView& data) override { -// if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] -// return; -// auto can_id = data.can_id; -// // can2_receive_rate_counter_.record(can_id); -// if (can_id == 0x143) { -// gimbal_player_viewer_motor_.store_status(data.can_data); -// } -// } - -// void can3_receive_callback(const librmcs::data::CanDataView& data) override { -// if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] -// return; -// auto can_id = data.can_id; -// // can3_receive_rate_counter_.record(can_id); -// if (can_id == 0x142) { -// gimbal_bullet_feeder_.store_status(data.can_data); -// } -// } - -// void gpio_digital_read_result_callback( -// const librmcs::spec::rmcs_board_lite::GpioDescriptor& gpio, -// const librmcs::data::GpioDigitalDataView& data) override { -// if (gpio.channel_index == 2) { -// photoelectric_sensor_status_atomic.store(data.high); -// } else if (gpio.channel_index == 3) { -// grayscale_sensor_status_atomic.store(!data.high); -// } -// } - -// void accelerometer_receive_callback( -// const librmcs::data::AccelerometerDataView& data) override { -// imu_.store_accelerometer_status(data.x, data.y, data.z); -// } - -// void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { -// imu_.store_gyroscope_status(data.x, data.y, data.z); -// } - -// void uart0_receive_callback(const librmcs::data::UartDataView& data) override { -// vt13_.store_status(data.uart_data); -// } - -// rclcpp::Logger logger_; -// // CanReceiveRateCounter can0_receive_rate_counter_; -// // CanReceiveRateCounter can1_receive_rate_counter_; -// // CanReceiveRateCounter can2_receive_rate_counter_; -// // CanReceiveRateCounter can3_receive_rate_counter_; -// OutputInterface& tf_; - -// std::time_t last_camera_capturer_trigger_timestamp_{0}; - -// device::Bmi088 imu_; -// device::Vt13 vt13_; -// device::LkMotor gimbal_top_yaw_motor_; -// device::LkMotor gimbal_pitch_motor_; -// device::DjiMotor gimbal_friction_wheels_[4]; -// device::LkMotor gimbal_bullet_feeder_; -// device::DjiMotor putter_motor_; -// device::DjiMotor gimbal_scope_motor_; -// device::LkMotor gimbal_player_viewer_motor_; - -// OutputInterface gimbal_yaw_velocity_imu_; -// OutputInterface gimbal_pitch_velocity_imu_; -// OutputInterface photoelectric_sensor_status_; -// OutputInterface grayscale_sensor_status_; -// OutputInterface camera_capturer_trigger_; -// OutputInterface camera_capturer_trigger_timestamp_; -// std::atomic photoelectric_sensor_status_atomic{false}; -// std::atomic grayscale_sensor_status_atomic{false}; -// }; - -// class BottomBoard final : private librmcs::agent::RmcsBoardLite { -// public: -// friend class SteeringHeroLittle; -// explicit BottomBoard( -// SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, -// std::string_view board_serial = {}) -// : librmcs::agent::RmcsBoardLite( -// board_serial, {.dangerously_skip_version_checks = false}) -// , logger_(steering_hero.get_logger()) -// // , can0_receive_rate_counter_(logger_, "bottom/can0") -// // , can1_receive_rate_counter_(logger_, "bottom/can1") -// // , can2_receive_rate_counter_(logger_, "bottom/can2") -// // , can3_receive_rate_counter_(logger_, "bottom/can3") -// , imu_(1000, 0.2, 0.0) -// , supercap_(steering_hero, steering_hero_command) -// , chassis_steering_motors_( -// {steering_hero, steering_hero_command, "/chassis/left_front_steering"}, -// {steering_hero, steering_hero_command, "/chassis/right_front_steering"}, -// {steering_hero, steering_hero_command, "/chassis/left_back_steering"}, -// {steering_hero, steering_hero_command, "/chassis/right_back_steering"}) -// , chassis_wheel_motors_( -// {steering_hero, steering_hero_command, "/chassis/left_front_wheel"}, -// {steering_hero, steering_hero_command, "/chassis/right_front_wheel"}, -// {steering_hero, steering_hero_command, "/chassis/left_back_wheel"}, -// {steering_hero, steering_hero_command, "/chassis/right_back_wheel"}) -// , chassis_front_climber_motor_( -// {steering_hero, steering_hero_command, "/chassis/climber/left_front_motor"}, -// {steering_hero, steering_hero_command, "/chassis/climber/right_front_motor"}) -// , chassis_back_climber_motor_( -// {steering_hero, steering_hero_command, "/chassis/climber/left_back_motor"}, -// {steering_hero, steering_hero_command, "/chassis/climber/right_back_motor"}) -// , yaw_brake_motor_(steering_hero, steering_hero_command, "/gimbal/yaw_brake") -// , gimbal_bottom_yaw_motor_(steering_hero, steering_hero_command, -// "/gimbal/bottom_yaw") { -// // -// chassis_steering_motors_[0].configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} -// .set_encoder_zero_point( -// static_cast( -// steering_hero.get_parameter("left_front_zero_point").as_int())) -// .set_reversed()); -// chassis_steering_motors_[1].configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} -// .set_encoder_zero_point( -// static_cast( -// steering_hero.get_parameter("right_front_zero_point").as_int())) -// .set_reversed()); -// chassis_steering_motors_[2].configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} -// .set_encoder_zero_point( -// static_cast( -// steering_hero.get_parameter("left_back_zero_point").as_int())) -// .set_reversed()); -// chassis_steering_motors_[3].configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} -// .set_encoder_zero_point( -// static_cast( -// steering_hero.get_parameter("right_back_zero_point").as_int())) -// .set_reversed()); - -// chassis_wheel_motors_[0].configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508} -// .set_reversed() -// .set_reduction_ratio(2232. / 169.)); -// chassis_wheel_motors_[1].configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508} -// .set_reversed() -// .set_reduction_ratio(2232. / 169.)); -// chassis_wheel_motors_[2].configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508} -// .set_reversed() -// .set_reduction_ratio(2232. / 169.)); -// chassis_wheel_motors_[3].configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508} -// .set_reversed() -// .set_reduction_ratio(2232. / 169.)); -// chassis_front_climber_motor_[0].configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508} -// .set_reversed() -// .set_reduction_ratio(19.)); -// chassis_front_climber_motor_[1].configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); -// chassis_back_climber_motor_[0].configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508} -// .enable_multi_turn_angle() -// .set_reduction_ratio(19.)); -// chassis_back_climber_motor_[1].configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508} -// .set_reversed() -// .enable_multi_turn_angle() -// .set_reduction_ratio(19.)); - -// yaw_brake_motor_.configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.set_reduction_ratio(1.)); -// gimbal_bottom_yaw_motor_.configure( -// device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} -// .set_reversed() -// .set_encoder_zero_point( -// static_cast( -// steering_hero.get_parameter("bottom_yaw_motor_zero_point").as_int()))); - -// steering_hero.register_output("/referee/serial", referee_serial_); -// referee_serial_->read = [this](std::byte* buffer, size_t size) { -// return referee_ring_buffer_receive_.pop_front_n( - -// [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); -// }; -// referee_serial_->write = [this](const std::byte* buffer, size_t size) { -// start_transmit().uart0_transmit( -// {.uart_data = std::span{buffer, size}}); -// return size; -// }; -// steering_hero.register_output( -// "/chassis/powermeter/control_enable", powermeter_control_enabled_, false); -// steering_hero.register_output( -// "/chassis/powermeter/charge_power_limit", powermeter_charge_power_limit_, 0.); - -// steering_hero.register_output( -// "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); -// steering_hero.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); -// } - -// BottomBoard(const BottomBoard&) = delete; -// BottomBoard& operator=(const BottomBoard&) = delete; -// BottomBoard(BottomBoard&&) = delete; -// BottomBoard& operator=(BottomBoard&&) = delete; - -// ~BottomBoard() final = default; - -// void update() { -// // can0_receive_rate_counter_.report_if_due(); -// // can1_receive_rate_counter_.report_if_due(); -// // can2_receive_rate_counter_.report_if_due(); -// // can3_receive_rate_counter_.report_if_due(); - -// imu_.update_status(); -// dr16_.update_status(); -// supercap_.update_status(); - -// *chassis_yaw_velocity_imu_ = imu_.gz(); -// *chassis_pitch_imu_ = -std::asin(2.0 * (imu_.q0() * imu_.q2() - imu_.q3() * -// imu_.q1())); - -// chassis_front_climber_motor_[0].update_status(); -// chassis_front_climber_motor_[1].update_status(); -// chassis_back_climber_motor_[0].update_status(); -// chassis_back_climber_motor_[1].update_status(); - -// for (auto& motor : chassis_wheel_motors_) -// motor.update_status(); -// for (auto& motor : chassis_steering_motors_) -// motor.update_status(); - -// yaw_brake_motor_.update_status(); -// gimbal_bottom_yaw_motor_.update_status(); - -// if (++count_ == 500) { -// for (int i = 0; i < 8; ++i) { -// if (check[i] == 0) { -// RCLCPP_WARN(logger_, "can id 0x%03X missing", i + 0x201); -// } -// } -// std::fill_n(check, 8, 0); -// count_ = 0; -// } -// } - -// void command_update() { -// auto builder = start_transmit(); - -// builder.can0_transmit({ -// .can_id = 0x200, -// .can_data = -// device::CanPacket8{ -// chassis_wheel_motors_[0].generate_command(), -// chassis_wheel_motors_[1].generate_command(), -// device::CanPacket8::PaddingQuarter{}, -// device::CanPacket8::PaddingQuarter{}, -// } -// .as_bytes(), -// }); - -// builder.can0_transmit({ -// .can_id = 0x1FE, -// .can_data = -// device::CanPacket8{ -// chassis_steering_motors_[1].generate_command(), -// device::CanPacket8::PaddingQuarter{}, -// device::CanPacket8::PaddingQuarter{}, -// chassis_steering_motors_[0].generate_command(), -// } -// .as_bytes(), -// }); - -// builder.can1_transmit({ -// .can_id = 0x200, -// .can_data = -// device::CanPacket8{ -// device::CanPacket8::PaddingQuarter{}, -// device::CanPacket8::PaddingQuarter{}, -// chassis_wheel_motors_[2].generate_command(), -// chassis_wheel_motors_[3].generate_command(), -// } -// .as_bytes(), -// }); - -// builder.can1_transmit({ -// .can_id = 0x1FE, -// .can_data = -// device::CanPacket8{ -// device::CanPacket8::PaddingQuarter{}, -// chassis_steering_motors_[3].generate_command(), -// chassis_steering_motors_[2].generate_command(), -// supercap_.generate_command(), -// } -// .as_bytes(), -// }); - -// builder.can3_transmit({ -// .can_id = 0x200, -// .can_data = -// device::CanPacket8{ -// device::CanPacket8::PaddingQuarter{}, -// chassis_back_climber_motor_[1].generate_command(), -// yaw_brake_motor_.generate_command(), -// chassis_back_climber_motor_[0].generate_command(), -// } -// .as_bytes(), -// }); - -// builder.can3_transmit({ -// .can_id = 0x141, -// .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), -// }); - -// builder.can2_transmit({ -// .can_id = 0x200, -// .can_data = -// device::CanPacket8{ -// chassis_front_climber_motor_[0].generate_command(), -// device::CanPacket8::PaddingQuarter{}, -// chassis_front_climber_motor_[1].generate_command(), -// device::CanPacket8::PaddingQuarter{}, -// } -// .as_bytes(), -// }); -// } - -// private: -// void can0_receive_callback(const librmcs::data::CanDataView& data) override { -// if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] -// return; -// auto can_id = data.can_id; -// // can0_receive_rate_counter_.record(can_id); -// check[can_id - 0x201] = 1; -// if (can_id == 0x201) { -// chassis_wheel_motors_[0].store_status(data.can_data); -// } else if (can_id == 0x202) { -// chassis_wheel_motors_[1].store_status(data.can_data); -// } else if (can_id == 0x205) { -// chassis_steering_motors_[1].store_status(data.can_data); -// } else if (can_id == 0x208) { -// chassis_steering_motors_[0].store_status(data.can_data); -// } -// } - -// void can1_receive_callback(const librmcs::data::CanDataView& data) override { -// if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] -// return; -// auto can_id = data.can_id; -// // can1_receive_rate_counter_.record(can_id); -// if (can_id != 0x300) { -// check[can_id - 0x201] = 1; -// } -// if (can_id == 0x203) { -// chassis_wheel_motors_[2].store_status(data.can_data); -// } else if (can_id == 0x204) { -// chassis_wheel_motors_[3].store_status(data.can_data); -// } else if (can_id == 0x207) { -// chassis_steering_motors_[2].store_status(data.can_data); -// } else if (can_id == 0x206) { -// chassis_steering_motors_[3].store_status(data.can_data); -// } else if (can_id == 0x300) { -// supercap_.store_status(data.can_data); -// } -// } - -// void can2_receive_callback(const librmcs::data::CanDataView& data) override { -// if (data.is_extended_can_id || data.is_remote_transmission) -// return; -// auto can_id = data.can_id; -// // can2_receive_rate_counter_.record(can_id); -// if (can_id == 0x201) { -// chassis_front_climber_motor_[0].store_status(data.can_data); -// } else if (can_id == 0x203) { -// chassis_front_climber_motor_[1].store_status(data.can_data); -// } -// } - -// void can3_receive_callback(const librmcs::data::CanDataView& data) override { -// if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] -// return; -// auto can_id = data.can_id; -// // can3_receive_rate_counter_.record(can_id); -// if (can_id == 0x202) { -// chassis_back_climber_motor_[1].store_status(data.can_data); -// } else if (can_id == 0x204) { -// chassis_back_climber_motor_[0].store_status(data.can_data); -// } else if (can_id == 0x203) { -// yaw_brake_motor_.store_status(data.can_data); -// } else if (can_id == 0x141) { -// gimbal_bottom_yaw_motor_.store_status(data.can_data); -// } -// } - -// void uart0_receive_callback(const librmcs::data::UartDataView& data) override { -// const std::byte* ptr = data.uart_data.data(); -// referee_ring_buffer_receive_.emplace_back_n( -// [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, -// data.uart_data.size()); -// } - -// void dbus_receive_callback(const librmcs::data::UartDataView& data) override { -// dr16_.store_status(data.uart_data); -// } - -// void accelerometer_receive_callback( -// const librmcs::data::AccelerometerDataView& data) override { -// imu_.store_accelerometer_status(data.x, data.y, data.z); -// } - -// void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { -// imu_.store_gyroscope_status(data.x, data.y, data.z); -// } - -// rclcpp::Logger logger_; -// // CanReceiveRateCounter can0_receive_rate_counter_; -// // CanReceiveRateCounter can1_receive_rate_counter_; -// // CanReceiveRateCounter can2_receive_rate_counter_; -// // CanReceiveRateCounter can3_receive_rate_counter_; - -// int count_ = 0; -// int check[10] = {0}; - -// device::Bmi088 imu_; -// device::Dr16 dr16_; -// device::Supercap supercap_; - -// device::DjiMotor chassis_steering_motors_[4]; -// device::DjiMotor chassis_wheel_motors_[4]; -// device::DjiMotor chassis_front_climber_motor_[2]; -// device::DjiMotor chassis_back_climber_motor_[2]; -// device::DjiMotor yaw_brake_motor_; -// device::LkMotor gimbal_bottom_yaw_motor_; - -// rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; - -// OutputInterface referee_serial_; -// OutputInterface powermeter_control_enabled_; -// OutputInterface powermeter_charge_power_limit_; -// OutputInterface chassis_yaw_velocity_imu_; -// OutputInterface chassis_pitch_imu_; -// }; - -// OutputInterface tf_; - -// rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; - -// std::shared_ptr top_board_; -// std::shared_ptr bottom_board_; -// std::unique_ptr remote_control_; -// }; - -// } // namespace rmcs_core::hardware - -// #include - -// PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringHeroLittle, rmcs_executor::Component) +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hardware/device/bmi088.hpp" +#include "hardware/device/can_packet.hpp" +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/lk_motor.hpp" +#include "hardware/device/supercap.hpp" + +namespace rmcs_core::hardware { + +class CanReceiveRateCounter { +public: + explicit CanReceiveRateCounter(rclcpp::Logger logger, std::string_view channel_name) + : logger_(std::move(logger)) + , channel_name_(channel_name) {} + + void record(std::uint32_t can_id) { + const auto now = Clock::now(); + + std::lock_guard lock{mutex_}; + auto& status = statuses_[can_id]; + ++status.receive_count; + status.last_receive_time = now; + + report_if_due(now); + } + + void report_if_due() { + const auto now = Clock::now(); + + std::lock_guard lock{mutex_}; + report_if_due(now); + } + +private: + using Clock = std::chrono::steady_clock; + + struct Status { + std::size_t receive_count{0}; + Clock::time_point last_receive_time{}; + }; + + void report_if_due(Clock::time_point now) { + if (statuses_.empty()) + return; + + if (last_report_time_ == Clock::time_point{}) { + last_report_time_ = now; + return; + } + + const auto elapsed = now - last_report_time_; + if (elapsed < kReportInterval) + return; + + const auto elapsed_seconds = std::chrono::duration(elapsed).count(); + for (auto& [can_id, status] : statuses_) { + const bool attached = now - status.last_receive_time <= kMissTimeout; + RCLCPP_INFO( + logger_, "[can rx] %.*s id=0x%03X rate=%.1fHz status=%s", + static_cast(channel_name_.size()), channel_name_.data(), + static_cast(can_id), + static_cast(status.receive_count) / elapsed_seconds, + attached ? "attach" : "miss"); + status.receive_count = 0; + } + + last_report_time_ = now; + } + + static constexpr std::chrono::milliseconds kReportInterval{1000}; + static constexpr std::chrono::milliseconds kMissTimeout{1000}; + + rclcpp::Logger logger_; + std::string_view channel_name_; + std::mutex mutex_; + Clock::time_point last_report_time_{}; + std::map statuses_; +}; + +class SteeringHeroLittle + : public rmcs_executor::Component + , public rclcpp::Node { +public: + SteeringHeroLittle() + : Node( + get_component_name(), + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) + , command_component_( + create_partner_component( + get_component_name() + "_command", *this)) { + + register_output("/tf", tf_); + + gimbal_calibrate_subscription_ = create_subscription( + "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { + gimbal_calibrate_subscription_callback(std::move(msg)); + }); + + top_board_ = std::make_unique( + *this, *command_component_, get_parameter("board_serial_top_board").as_string()); + + bottom_board_ = std::make_unique( + *this, *command_component_, get_parameter("serial_bottom_rmcs_board").as_string()); + + tf_->set_transform( + Eigen::Translation3d{0.22, 0.0, -0.05}); + } + + SteeringHeroLittle(const SteeringHeroLittle&) = delete; + SteeringHeroLittle& operator=(const SteeringHeroLittle&) = delete; + SteeringHeroLittle(SteeringHeroLittle&&) = delete; + SteeringHeroLittle& operator=(SteeringHeroLittle&&) = delete; + + ~SteeringHeroLittle() override = default; + + void update() override { + top_board_->update(); + bottom_board_->update(); + + tf_->set_state( + bottom_board_->gimbal_bottom_yaw_motor_.angle() + + top_board_->gimbal_top_yaw_motor_.angle()); + tf_->set_state( + top_board_->gimbal_pitch_motor_.angle()); + } + + void command_update() { + top_board_->command_update(); + bottom_board_->command_update(); + } + +private: + void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New bottom yaw offset: %ld", + bottom_board_->gimbal_bottom_yaw_motor_.calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New pitch offset: %ld", + top_board_->gimbal_pitch_motor_.calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New player viewer offset: %ld", + top_board_->gimbal_player_viewer_motor_.calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New top yaw offset: %ld", + top_board_->gimbal_top_yaw_motor_.calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New bullet feeder offset: %ld", + top_board_->gimbal_bullet_feeder_.calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] left front steering offset: %d", + bottom_board_->chassis_steering_motors_[0].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] right front steering offset: %d", + bottom_board_->chassis_steering_motors_[1].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] left back steering offset: %d", + bottom_board_->chassis_steering_motors_[2].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] right back steering offset: %d", + bottom_board_->chassis_steering_motors_[3].calibrate_zero_point()); + } + + class SteeringHeroLittleCommand : public rmcs_executor::Component { + public: + explicit SteeringHeroLittleCommand(SteeringHeroLittle& hero) + : hero_(hero) {} + + void update() override { hero_.command_update(); } + + SteeringHeroLittle& hero_; + }; + std::shared_ptr command_component_; + + class TopBoard final : private librmcs::agent::RmcsBoardLite { + public: + friend class SteeringHeroLittle; + explicit TopBoard( + SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, + std::string_view board_serial = {}) + : librmcs::agent::RmcsBoardLite(board_serial) + , logger_(steering_hero.get_logger()) + // , can0_receive_rate_counter_(logger_, "bottom/can0") + // , can1_receive_rate_counter_(logger_, "bottom/can1") + // , can2_receive_rate_counter_(logger_, "bottom/can2") + // , can3_receive_rate_counter_(logger_, "bottom/can3") + , tf_(steering_hero.tf_) + , imu_(1000, 0.2, 0.0) + , gimbal_top_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/top_yaw") + , gimbal_pitch_motor_(steering_hero, steering_hero_command, "/gimbal/pitch") + , gimbal_friction_wheels_( + {steering_hero, steering_hero_command, "/gimbal/first_front_friction"}, + {steering_hero, steering_hero_command, "/gimbal/first_back_friction"}, + {steering_hero, steering_hero_command, "/gimbal/second_front_friction"}, + {steering_hero, steering_hero_command, "/gimbal/second_back_friction"}, + {steering_hero, steering_hero_command, "/gimbal/third_front_friction"}, + {steering_hero, steering_hero_command, "/gimbal/third_back_friction"}) + , gimbal_bullet_feeder_(steering_hero, steering_hero_command, "/gimbal/bullet_feeder") + , putter_motor_(steering_hero, steering_hero_command, "/gimbal/putter") + , gimbal_scope_motor_(steering_hero, steering_hero_command, "/gimbal/scope") + , gimbal_player_viewer_motor_( + steering_hero, steering_hero_command, "/gimbal/player_viewer") { + + gimbal_top_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} + .enable_multi_turn_angle() + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("top_yaw_motor_zero_point").as_int()))); + gimbal_pitch_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("pitch_motor_zero_point").as_int())) + .enable_multi_turn_angle()); + gimbal_friction_wheels_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(1.)); + gimbal_friction_wheels_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(1.)); + gimbal_friction_wheels_[2].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + gimbal_friction_wheels_[3].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + gimbal_friction_wheels_[4].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(1.)); + gimbal_friction_wheels_[5].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(1.)); + gimbal_bullet_feeder_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("bullet_feeder_motor_zero_point").as_int())) + .set_reversed() + .enable_multi_turn_angle()); + putter_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reduction_ratio(1.) + .enable_multi_turn_angle()); + gimbal_scope_motor_.configure(device::DjiMotor::Config{device::DjiMotor::Type::kM2006}); + gimbal_player_viewer_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("viewer_motor_zero_point").as_int())) + .set_reversed() + .enable_multi_turn_angle()); + + steering_hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); + steering_hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + + steering_hero.register_output( + "/gimbal/photoelectric_sensor", photoelectric_sensor_status_, false); + steering_hero.register_output( + "/gimbal/grayscale_sensor", grayscale_sensor_status_, false); + steering_hero.register_output( + "/auto_aim/image_capturer/timestamp", camera_capturer_trigger_timestamp_, 0); + steering_hero.register_output( + "/auto_aim/image_capturer/trigger", camera_capturer_trigger_, 0); + + imu_.set_coordinate_mapping([](double x, double y, double z) { + // Get the mapping with the following code. + // The rotation angle must be an exact multiple of 90 degrees, otherwise + // use a matrix. + + return std::make_tuple(y, -x, z); + }); + } + + TopBoard(const TopBoard&) = delete; + TopBoard& operator=(const TopBoard&) = delete; + TopBoard(TopBoard&&) = delete; + TopBoard& operator=(TopBoard&&) = delete; + + ~TopBoard() final = default; + + void update() { + // can0_receive_rate_counter_.report_if_due(); + // can1_receive_rate_counter_.report_if_due(); + // can2_receive_rate_counter_.report_if_due(); + // can3_receive_rate_counter_.report_if_due(); + + imu_.update_status(); + Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; + + tf_->set_transform( + gimbal_imu_pose.conjugate()); + + *gimbal_yaw_velocity_imu_ = imu_.gz(); + *gimbal_pitch_velocity_imu_ = imu_.gy(); + + gimbal_top_yaw_motor_.update_status(); + gimbal_pitch_motor_.update_status(); + tf_->set_state( + gimbal_pitch_motor_.angle()); + + for (auto& motor : gimbal_friction_wheels_) + motor.update_status(); + + gimbal_bullet_feeder_.update_status(); + putter_motor_.update_status(); + + gimbal_player_viewer_motor_.update_status(); + tf_->set_state( + gimbal_player_viewer_motor_.angle()); + + gimbal_scope_motor_.update_status(); + + if (last_camera_capturer_trigger_timestamp_ != *camera_capturer_trigger_timestamp_) + *camera_capturer_trigger_ = true; + last_camera_capturer_trigger_timestamp_ = *camera_capturer_trigger_timestamp_; + + *photoelectric_sensor_status_ = photoelectric_sensor_status_atomic.load(); + *grayscale_sensor_status_ = grayscale_sensor_status_atomic.load(); + + if (++count_ == 250) { + for (int i = 0; i < 6; ++i) { + if (friciton_detect[i] == 0) { + RCLCPP_WARN(logger_, "friction can id 0x%03X missing", i + 0x201); + } + } + std::fill_n(friciton_detect, 6, 0); + for (int i = 0; i < 3; ++i) { + if (can0_detect[i] == 0) { + RCLCPP_WARN(logger_, "top board can id 0x%03X missing", i + 0x141); + } + } + std::fill_n(can0_detect, 3, 0); + count_ = 0; + } + } + + void command_update() { + auto builder = start_transmit(); + + if (std::isfinite(gimbal_pitch_motor_.control_angle())) + builder.can0_transmit({ + .can_id = 0x143, + .can_data = gimbal_pitch_motor_ + .generate_angle_command(gimbal_pitch_motor_.control_angle()) + .as_bytes(), + }); + else + builder.can0_transmit({ + .can_id = 0x143, + .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), + }); // Used to distinguish pitch encoder control from IMU control. + + builder.can0_transmit({ + .can_id = 0x141, + .can_data = gimbal_top_yaw_motor_.generate_command().as_bytes(), + }); + + builder.can0_transmit({ + .can_id = 0x142, + .can_data = gimbal_bullet_feeder_.generate_torque_command().as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_friction_wheels_[0].generate_command(), + gimbal_friction_wheels_[1].generate_command(), + gimbal_friction_wheels_[2].generate_command(), + gimbal_friction_wheels_[3].generate_command(), + } + .as_bytes(), + }); + + builder.can2_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_friction_wheels_[4].generate_command(), + gimbal_friction_wheels_[5].generate_command(), + putter_motor_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + builder.gpio_digital_read( + librmcs::spec::rmcs_board_lite::kGpioDescriptors[2], + { + .period_ms = 20, + .pull = librmcs::data::GpioPull::kUp, + }); + } + + private: + void can0_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + // can0_receive_rate_counter_.record(can_id); + can0_detect[can_id - 0x141] = 1; + if (can_id == 0x141) { + gimbal_top_yaw_motor_.store_status(data.can_data); + } else if (can_id == 0x143) { + gimbal_pitch_motor_.store_status(data.can_data); + } else if (can_id == 0x142) { + gimbal_bullet_feeder_.store_status(data.can_data); + } + } + + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + // can1_receive_rate_counter_.record(can_id); + friciton_detect[can_id - 0x201] = 1; + if (can_id == 0x201) { + gimbal_friction_wheels_[0].store_status(data.can_data); + } else if (can_id == 0x202) { + gimbal_friction_wheels_[1].store_status(data.can_data); + } else if (can_id == 0x203) { + gimbal_friction_wheels_[2].store_status(data.can_data); + } else if (can_id == 0x204) { + gimbal_friction_wheels_[3].store_status(data.can_data); + } + } + + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + // can2_receive_rate_counter_.record(can_id); + if (can_id == 0x203) { + putter_motor_.store_status(data.can_data); + } else if (can_id == 0x201) { + gimbal_friction_wheels_[4].store_status(data.can_data); + friciton_detect[4] = 1; + } else if (can_id == 0x202) { + gimbal_friction_wheels_[5].store_status(data.can_data); + friciton_detect[5] = 1; + } + } + + void gpio_digital_read_result_callback( + const librmcs::spec::rmcs_board_lite::GpioDescriptor& gpio, + const librmcs::data::GpioDigitalDataView& data) override { + if (gpio.channel_index == 2) { + photoelectric_sensor_status_atomic.store(data.high); + } + } + + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + imu_.store_accelerometer_status(data.x, data.y, data.z); + } + + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); + } + + rclcpp::Logger logger_; + // CanReceiveRateCounter can0_receive_rate_counter_; + // CanReceiveRateCounter can1_receive_rate_counter_; + // CanReceiveRateCounter can2_receive_rate_counter_; + // CanReceiveRateCounter can3_receive_rate_counter_; + OutputInterface& tf_; + + std::time_t last_camera_capturer_trigger_timestamp_{0}; + int count_ = 0; + int friciton_detect[6]; + int can0_detect[3]; + + device::Bmi088 imu_; + device::LkMotor gimbal_top_yaw_motor_; + device::LkMotor gimbal_pitch_motor_; + device::DjiMotor gimbal_friction_wheels_[6]; + device::LkMotor gimbal_bullet_feeder_; + device::DjiMotor putter_motor_; + device::DjiMotor gimbal_scope_motor_; + device::LkMotor gimbal_player_viewer_motor_; + + OutputInterface gimbal_yaw_velocity_imu_; + OutputInterface gimbal_pitch_velocity_imu_; + OutputInterface photoelectric_sensor_status_; + OutputInterface grayscale_sensor_status_; + OutputInterface camera_capturer_trigger_; + OutputInterface camera_capturer_trigger_timestamp_; + std::atomic photoelectric_sensor_status_atomic{false}; + std::atomic grayscale_sensor_status_atomic{false}; + }; + + class BottomBoard final : private librmcs::agent::RmcsBoardLite { + public: + friend class SteeringHeroLittle; + explicit BottomBoard( + SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, + std::string_view board_serial = {}) + : librmcs::agent::RmcsBoardLite( + board_serial, {.dangerously_skip_version_checks = false}) + , logger_(steering_hero.get_logger()) + // , can0_receive_rate_counter_(logger_, "bottom/can0") + // , can1_receive_rate_counter_(logger_, "bottom/can1") + // , can2_receive_rate_counter_(logger_, "bottom/can2") + // , can3_receive_rate_counter_(logger_, "bottom/can3") + , imu_(1000, 0.2, 0.0) + , dr16_(steering_hero) + , supercap_(steering_hero, steering_hero_command) + , chassis_steering_motors_( + {steering_hero, steering_hero_command, "/chassis/left_front_steering"}, + {steering_hero, steering_hero_command, "/chassis/right_front_steering"}, + {steering_hero, steering_hero_command, "/chassis/left_back_steering"}, + {steering_hero, steering_hero_command, "/chassis/right_back_steering"}) + , chassis_wheel_motors_( + {steering_hero, steering_hero_command, "/chassis/left_front_wheel"}, + {steering_hero, steering_hero_command, "/chassis/right_front_wheel"}, + {steering_hero, steering_hero_command, "/chassis/left_back_wheel"}, + {steering_hero, steering_hero_command, "/chassis/right_back_wheel"}) + , chassis_front_climber_motor_( + {steering_hero, steering_hero_command, "/chassis/climber/left_front_motor"}, + {steering_hero, steering_hero_command, "/chassis/climber/right_front_motor"}) + , chassis_back_climber_motor_( + {steering_hero, steering_hero_command, "/chassis/climber/left_back_motor"}, + {steering_hero, steering_hero_command, "/chassis/climber/right_back_motor"}) + , yaw_brake_motor_(steering_hero, steering_hero_command, "/gimbal/yaw_brake") + , gimbal_bottom_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/bottom_yaw") { + // + chassis_steering_motors_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("left_front_zero_point").as_int())) + .set_reversed()); + chassis_steering_motors_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("right_front_zero_point").as_int())) + .set_reversed()); + chassis_steering_motors_[2].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("left_back_zero_point").as_int())) + .set_reversed()); + chassis_steering_motors_[3].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("right_back_zero_point").as_int())) + .set_reversed()); + + chassis_wheel_motors_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_wheel_motors_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_wheel_motors_[2].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_wheel_motors_[3].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_front_climber_motor_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(19.)); + chassis_front_climber_motor_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + chassis_back_climber_motor_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .enable_multi_turn_angle() + .set_reduction_ratio(19.)); + chassis_back_climber_motor_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .enable_multi_turn_angle() + .set_reduction_ratio(19.)); + + yaw_brake_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.set_reduction_ratio(1.)); + gimbal_bottom_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} + .set_reversed() + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("bottom_yaw_motor_zero_point").as_int()))); + + steering_hero.register_output("/referee/serial", referee_serial_); + referee_serial_->read = [this](std::byte* buffer, size_t size) { + return referee_ring_buffer_receive_.pop_front_n( + + [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) { + start_transmit().uart0_transmit( + {.uart_data = std::span{buffer, size}}); + return size; + }; + steering_hero.register_output( + "/chassis/powermeter/control_enable", powermeter_control_enabled_, false); + steering_hero.register_output( + "/chassis/powermeter/charge_power_limit", powermeter_charge_power_limit_, 0.); + + steering_hero.register_output( + "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); + steering_hero.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); + } + + BottomBoard(const BottomBoard&) = delete; + BottomBoard& operator=(const BottomBoard&) = delete; + BottomBoard(BottomBoard&&) = delete; + BottomBoard& operator=(BottomBoard&&) = delete; + + ~BottomBoard() final = default; + + void update() { + // can0_receive_rate_counter_.report_if_due(); + // can1_receive_rate_counter_.report_if_due(); + // can2_receive_rate_counter_.report_if_due(); + // can3_receive_rate_counter_.report_if_due(); + + imu_.update_status(); + dr16_.update_status(); + supercap_.update_status(); + + *chassis_yaw_velocity_imu_ = imu_.gz(); + *chassis_pitch_imu_ = -std::asin(2.0 * (imu_.q0() * imu_.q2() - imu_.q3() * imu_.q1())); + + chassis_front_climber_motor_[0].update_status(); + chassis_front_climber_motor_[1].update_status(); + chassis_back_climber_motor_[0].update_status(); + chassis_back_climber_motor_[1].update_status(); + + for (auto& motor : chassis_wheel_motors_) + motor.update_status(); + for (auto& motor : chassis_steering_motors_) + motor.update_status(); + + yaw_brake_motor_.update_status(); + gimbal_bottom_yaw_motor_.update_status(); + + if (++count_ == 250) { + for (int i = 0; i < 8; ++i) { + if (check[i] == 0) { + RCLCPP_WARN(logger_, "bottom board can id 0x%03X missing", i + 0x201); + } + } + std::fill_n(check, 8, 0); + count_ = 0; + } + } + + void command_update() { + auto builder = start_transmit(); + + builder.can0_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[0].generate_command(), + chassis_wheel_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + builder.can0_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + chassis_steering_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + chassis_steering_motors_[0].generate_command(), + } + .as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + chassis_wheel_motors_[2].generate_command(), + chassis_wheel_motors_[3].generate_command(), + } + .as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + chassis_steering_motors_[3].generate_command(), + chassis_steering_motors_[2].generate_command(), + supercap_.generate_command(), + } + .as_bytes(), + }); + + builder.can3_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + chassis_back_climber_motor_[1].generate_command(), + yaw_brake_motor_.generate_command(), + chassis_back_climber_motor_[0].generate_command(), + } + .as_bytes(), + }); + + builder.can3_transmit({ + .can_id = 0x141, + .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), + }); + + builder.can2_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_front_climber_motor_[0].generate_command(), + device::CanPacket8::PaddingQuarter{}, + chassis_front_climber_motor_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + } + + private: + void can0_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + // can0_receive_rate_counter_.record(can_id); + check[can_id - 0x201] = 1; + if (can_id == 0x201) { + chassis_wheel_motors_[0].store_status(data.can_data); + } else if (can_id == 0x202) { + chassis_wheel_motors_[1].store_status(data.can_data); + } else if (can_id == 0x205) { + chassis_steering_motors_[1].store_status(data.can_data); + } else if (can_id == 0x208) { + chassis_steering_motors_[0].store_status(data.can_data); + } + } + + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + // can1_receive_rate_counter_.record(can_id); + if (can_id != 0x300) { + check[can_id - 0x201] = 1; + } + if (can_id == 0x203) { + chassis_wheel_motors_[2].store_status(data.can_data); + } else if (can_id == 0x204) { + chassis_wheel_motors_[3].store_status(data.can_data); + } else if (can_id == 0x207) { + chassis_steering_motors_[2].store_status(data.can_data); + } else if (can_id == 0x206) { + chassis_steering_motors_[3].store_status(data.can_data); + } else if (can_id == 0x300) { + supercap_.store_status(data.can_data); + } + } + + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) + return; + auto can_id = data.can_id; + // can2_receive_rate_counter_.record(can_id); + if (can_id == 0x201) { + chassis_front_climber_motor_[0].store_status(data.can_data); + } else if (can_id == 0x203) { + chassis_front_climber_motor_[1].store_status(data.can_data); + } + } + + void can3_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + // can3_receive_rate_counter_.record(can_id); + if (can_id == 0x202) { + chassis_back_climber_motor_[1].store_status(data.can_data); + } else if (can_id == 0x204) { + chassis_back_climber_motor_[0].store_status(data.can_data); + } else if (can_id == 0x203) { + yaw_brake_motor_.store_status(data.can_data); + } else if (can_id == 0x141) { + gimbal_bottom_yaw_motor_.store_status(data.can_data); + } + } + + void uart0_receive_callback(const librmcs::data::UartDataView& data) override { + const std::byte* ptr = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, data.uart_data.size()); + } + + void dbus_receive_callback(const librmcs::data::UartDataView& data) override { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + } + + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + imu_.store_accelerometer_status(data.x, data.y, data.z); + } + + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); + } + + rclcpp::Logger logger_; + // CanReceiveRateCounter can0_receive_rate_counter_; + // CanReceiveRateCounter can1_receive_rate_counter_; + // CanReceiveRateCounter can2_receive_rate_counter_; + // CanReceiveRateCounter can3_receive_rate_counter_; + + int count_ = 0; + int check[10] = {0}; + + device::Bmi088 imu_; + device::Dr16 dr16_; + device::Supercap supercap_; + + device::DjiMotor chassis_steering_motors_[4]; + device::DjiMotor chassis_wheel_motors_[4]; + device::DjiMotor chassis_front_climber_motor_[2]; + device::DjiMotor chassis_back_climber_motor_[2]; + device::DjiMotor yaw_brake_motor_; + device::LkMotor gimbal_bottom_yaw_motor_; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; + + OutputInterface referee_serial_; + OutputInterface powermeter_control_enabled_; + OutputInterface powermeter_charge_power_limit_; + OutputInterface chassis_yaw_velocity_imu_; + OutputInterface chassis_pitch_imu_; + }; + + OutputInterface tf_; + + rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; + + std::shared_ptr top_board_; + std::shared_ptr bottom_board_; +}; + +} // namespace rmcs_core::hardware + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringHeroLittle, rmcs_executor::Component) From 82b90ae0fbd7f55105ee92d14ad5fb15ea2d7e3e Mon Sep 17 00:00:00 2001 From: dwx5 <1591215786@qq.com> Date: Wed, 15 Jul 2026 22:32:11 +0800 Subject: [PATCH 30/86] parameter changes for 12m/s --- .../steering-hero-little-six-friction.yaml | 36 +++++++++++-------- 1 file changed, 21 insertions(+), 15 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml index 5ecf58744..2b8197023 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml @@ -241,19 +241,25 @@ friction_wheel_controller: - /gimbal/second_back_friction - /gimbal/third_back_friction friction_velocities_profile_0: - - 527.0 #530.0 - - 527.0 #530.0 - - 527.0 #530.0 - - 435.0 #454.0 - - 435.0 #454.0 - - 435.0 #454.0 + - 390.0 + - 390.0 + - 390.0 + - 480.0 + - 480.0 + - 480.0 + # - 527.0 + # - 527.0 + # - 527.0 + # - 435.0 + # - 435.0 + # - 435.0 friction_velocities_profile_1: - - 408.0 - - 408.0 - - 408.0 - - 550.0 - - 550.0 - - 550.0 + - 527.0 + - 527.0 + - 527.0 + - 435.0 + - 435.0 + - 435.0 friction_soft_start_stop_time: 1.0 heat_controller: @@ -273,7 +279,7 @@ first_front_friction_velocity_pid_controller: setpoint: /gimbal/first_front_friction/control_velocity control: /gimbal/first_front_friction/control_torque kp: 0.006233371 - ki: 0.00003 + ki: 0.00 #0.00003 kd: 0.000001 second_front_friction_velocity_pid_controller: @@ -282,7 +288,7 @@ second_front_friction_velocity_pid_controller: setpoint: /gimbal/second_front_friction/control_velocity control: /gimbal/second_front_friction/control_torque kp: 0.006035661 - ki: 0.00003 + ki: 0.00 #0.00003 kd: 0.000001 third_front_friction_velocity_pid_controller: @@ -291,7 +297,7 @@ third_front_friction_velocity_pid_controller: setpoint: /gimbal/third_front_friction/control_velocity control: /gimbal/third_front_friction/control_torque kp: 0.006192421 - ki: 0.00003 + ki: 0.00 #0.00003 kd: 0.000001 first_back_friction_velocity_pid_controller: From 996764797756f1b5076376124f6bbc59632d7c02 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Wed, 15 Jul 2026 19:14:09 +0800 Subject: [PATCH 31/86] wip: Adapt auto aim for hardware and clean config --- .../steering-hero-little-six-friction.yaml | 5 - .../steering-hero-little-six-friction.cpp | 139 ++++++++++-------- 2 files changed, 81 insertions(+), 63 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml index 2b8197023..d8c2c07ed 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml @@ -35,14 +35,9 @@ rmcs_executor: - rmcs_core::controller::chassis::HeroSteeringWheelController -> steering_wheel_controller - rmcs_core::controller::chassis::ChassisClimberController -> climber_controller - # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer - # - rmcs_auto_aim::AutoAimController -> auto_aim_controller - # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster - # - sp_vision_25::bridge::HeroAutoAimBridge -> hero_auto_aim_bridge - # - rmcs_core::controller::identification::SweptFrequencyController -> pitch_swept_frequency_controller # - rmcs_core::controller::identification::StaticTorqueTestController -> pitch_static_torque_test_controller # - rmcs_core::controller::identification::SweptFrequencyController -> top_yaw_swept_frequency_controller diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp index eb23cdbb9..6ba2f8a76 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -23,11 +24,15 @@ #include #include #include +#include +#include #include #include #include #include "hardware/device/bmi088.hpp" +#include "hardware/device/bmi088_ekf.hpp" +#include "hardware/device/board_clock_lifter.hpp" #include "hardware/device/can_packet.hpp" #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" @@ -120,6 +125,11 @@ class SteeringHeroLittle register_output("/tf", tf_); + register_output( + "/auto_aim/camera_transform", camera_transform_, Eigen::Isometry3d::Identity()); + register_output("/auto_aim/barrel_direction", barrel_direction_, Eigen::Vector3d::UnitX()); + register_output("/auto_aim/yaw_velocity", auto_aim_yaw_velocity_, 0.0); + gimbal_calibrate_subscription_ = create_subscription( "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { gimbal_calibrate_subscription_callback(std::move(msg)); @@ -151,6 +161,12 @@ class SteeringHeroLittle + top_board_->gimbal_top_yaw_motor_.angle()); tf_->set_state( top_board_->gimbal_pitch_motor_.angle()); + + using namespace rmcs_description; + *camera_transform_ = fast_tf::lookup_transform(*tf_); + *barrel_direction_ = + *fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + *auto_aim_yaw_velocity_ = top_board_->gimbal_yaw_velocity(); } void command_update() { @@ -201,19 +217,19 @@ class SteeringHeroLittle std::shared_ptr command_component_; class TopBoard final : private librmcs::agent::RmcsBoardLite { - public: friend class SteeringHeroLittle; + + public: explicit TopBoard( SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, std::string_view board_serial = {}) : librmcs::agent::RmcsBoardLite(board_serial) , logger_(steering_hero.get_logger()) - // , can0_receive_rate_counter_(logger_, "bottom/can0") - // , can1_receive_rate_counter_(logger_, "bottom/can1") - // , can2_receive_rate_counter_(logger_, "bottom/can2") - // , can3_receive_rate_counter_(logger_, "bottom/can3") , tf_(steering_hero.tf_) - , imu_(1000, 0.2, 0.0) + , bmi088_{device::Bmi088Ekf::Config{ + .body_to_sensor = + Eigen::AngleAxisd{std::numbers::pi / 2.0, Eigen::Vector3d::UnitZ()} + .toRotationMatrix()}} , gimbal_top_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/top_yaw") , gimbal_pitch_motor_(steering_hero, steering_hero_command, "/gimbal/pitch") , gimbal_friction_wheels_( @@ -284,45 +300,29 @@ class SteeringHeroLittle steering_hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); steering_hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + steering_hero.register_output( + "/gimbal/auto_aim/exposure_signal", camera_signal_output_); + steering_hero.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); + steering_hero.register_output( "/gimbal/photoelectric_sensor", photoelectric_sensor_status_, false); steering_hero.register_output( "/gimbal/grayscale_sensor", grayscale_sensor_status_, false); - steering_hero.register_output( - "/auto_aim/image_capturer/timestamp", camera_capturer_trigger_timestamp_, 0); - steering_hero.register_output( - "/auto_aim/image_capturer/trigger", camera_capturer_trigger_, 0); - - imu_.set_coordinate_mapping([](double x, double y, double z) { - // Get the mapping with the following code. - // The rotation angle must be an exact multiple of 90 degrees, otherwise - // use a matrix. - - return std::make_tuple(y, -x, z); - }); } - TopBoard(const TopBoard&) = delete; - TopBoard& operator=(const TopBoard&) = delete; - TopBoard(TopBoard&&) = delete; - TopBoard& operator=(TopBoard&&) = delete; - - ~TopBoard() final = default; - void update() { // can0_receive_rate_counter_.report_if_due(); // can1_receive_rate_counter_.report_if_due(); // can2_receive_rate_counter_.report_if_due(); // can3_receive_rate_counter_.report_if_due(); - imu_.update_status(); - Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; - - tf_->set_transform( - gimbal_imu_pose.conjugate()); + if (auto snapshot = bmi088_.snapshot()) { + tf_->set_transform( + snapshot->orientation.conjugate()); - *gimbal_yaw_velocity_imu_ = imu_.gz(); - *gimbal_pitch_velocity_imu_ = imu_.gy(); + *gimbal_yaw_velocity_imu_ = snapshot->gyro_body.z(); + *gimbal_pitch_velocity_imu_ = snapshot->gyro_body.y(); + } gimbal_top_yaw_motor_.update_status(); gimbal_pitch_motor_.update_status(); @@ -341,10 +341,6 @@ class SteeringHeroLittle gimbal_scope_motor_.update_status(); - if (last_camera_capturer_trigger_timestamp_ != *camera_capturer_trigger_timestamp_) - *camera_capturer_trigger_ = true; - last_camera_capturer_trigger_timestamp_ = *camera_capturer_trigger_timestamp_; - *photoelectric_sensor_status_ = photoelectric_sensor_status_atomic.load(); *grayscale_sensor_status_ = grayscale_sensor_status_atomic.load(); @@ -415,12 +411,23 @@ class SteeringHeroLittle .as_bytes(), }); - builder.gpio_digital_read( - librmcs::spec::rmcs_board_lite::kGpioDescriptors[2], - { - .period_ms = 20, - .pull = librmcs::data::GpioPull::kUp, - }); + builder + .gpio_digital_read( + librmcs::spec::rmcs_board_lite::kGpioDescriptors.kUart1Rx, + { + .period_ms = 20, + .pull = librmcs::data::GpioPull::kUp, + }) + .gpio_digital_read( + librmcs::spec::rmcs_board_lite::kGpioDescriptors.kUart1Tx, + { + .period_ms = 0, + .asap = false, + .rising_edge = false, + .falling_edge = true, + .capture_timestamp = true, + .pull = librmcs::data::GpioPull::kUp, + }); } private: @@ -475,18 +482,37 @@ class SteeringHeroLittle void gpio_digital_read_result_callback( const librmcs::spec::rmcs_board_lite::GpioDescriptor& gpio, const librmcs::data::GpioDigitalDataView& data) override { - if (gpio.channel_index == 2) { + if (gpio == librmcs::spec::rmcs_board_lite::kGpioDescriptors.kUart1Rx) { photoelectric_sensor_status_atomic.store(data.high); + } else if (gpio == librmcs::spec::rmcs_board_lite::kGpioDescriptors.kUart1Tx) { + if (!data.timestamp_quarter_us) + return; + const auto timestamp = + board_clock_lifter_.lift_timestamp(*data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + camera_signal_output_.emit(*timestamp); } } void accelerometer_receive_callback( const librmcs::data::AccelerometerDataView& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); + const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); + bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); } void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); + const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + + if (auto snapshot = + bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp)) + imu_snapshot_output_.emit(*snapshot); + } + + [[nodiscard]] auto gimbal_yaw_velocity() const -> double { + return *gimbal_yaw_velocity_imu_; } rclcpp::Logger logger_; @@ -496,12 +522,12 @@ class SteeringHeroLittle // CanReceiveRateCounter can3_receive_rate_counter_; OutputInterface& tf_; - std::time_t last_camera_capturer_trigger_timestamp_{0}; int count_ = 0; int friciton_detect[6]; int can0_detect[3]; - device::Bmi088 imu_; + device::Bmi088Ekf bmi088_; + device::BoardClockLifter board_clock_lifter_; device::LkMotor gimbal_top_yaw_motor_; device::LkMotor gimbal_pitch_motor_; device::DjiMotor gimbal_friction_wheels_[6]; @@ -514,15 +540,16 @@ class SteeringHeroLittle OutputInterface gimbal_pitch_velocity_imu_; OutputInterface photoelectric_sensor_status_; OutputInterface grayscale_sensor_status_; - OutputInterface camera_capturer_trigger_; - OutputInterface camera_capturer_trigger_timestamp_; + EventOutputInterface camera_signal_output_; + EventOutputInterface imu_snapshot_output_; std::atomic photoelectric_sensor_status_atomic{false}; std::atomic grayscale_sensor_status_atomic{false}; }; class BottomBoard final : private librmcs::agent::RmcsBoardLite { - public: friend class SteeringHeroLittle; + + public: explicit BottomBoard( SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, std::string_view board_serial = {}) @@ -642,13 +669,6 @@ class SteeringHeroLittle steering_hero.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); } - BottomBoard(const BottomBoard&) = delete; - BottomBoard& operator=(const BottomBoard&) = delete; - BottomBoard(BottomBoard&&) = delete; - BottomBoard& operator=(BottomBoard&&) = delete; - - ~BottomBoard() final = default; - void update() { // can0_receive_rate_counter_.report_if_due(); // can1_receive_rate_counter_.report_if_due(); @@ -884,6 +904,10 @@ class SteeringHeroLittle OutputInterface tf_; + OutputInterface camera_transform_; + OutputInterface barrel_direction_; + OutputInterface auto_aim_yaw_velocity_; + rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; std::shared_ptr top_board_; @@ -893,5 +917,4 @@ class SteeringHeroLittle } // namespace rmcs_core::hardware #include - PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringHeroLittle, rmcs_executor::Component) From f123fa627f21668b936f15022701c8d4a9fc4a53 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Wed, 15 Jul 2026 21:12:29 +0800 Subject: [PATCH 32/86] wip: Adapt auto aim control --- .../rmcs_bringup/config/auto_aim_test.yaml | 19 +----- .../steering-hero-little-six-friction.yaml | 61 +++++-------------- .../gimbal/hero_gimbal_controller.cpp | 7 +-- .../controller/shooting/putter_controller.cpp | 18 +++--- 4 files changed, 30 insertions(+), 75 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml b/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml index a3b31ca3e..ebafa2e92 100644 --- a/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml @@ -9,13 +9,13 @@ rmcs_executor: auto_aim_player: ros__parameters: - input_path: "/workspaces/data/autoaim/自家小符/" + input_path: "/workspaces/data/autoaim/2026-07-11_22-39-06/" loop_play: true auto_aim_video_player: ros__parameters: input_path: "/workspaces/data/autoaim/robot/rotate.avi" - framerate: 80.0 + framerate: 40.0 loop_play: true auto_aim_recorder: @@ -28,19 +28,4 @@ auto_aim_recorder: auto_aim_component: ros__parameters: - dangerous_fallback: "red" manual_shoot: false - camera_translation: [0., 0., 0.] - - fire_control: - bullet_speed: 23.5 - shoot_delay: 0.0 - offset_yaw: 0.0 - offset_pitch: 0.0 - attack_window: 120.0 - window_hysteresis: 0.2 - is_lazy_gimbal: false - attack_preaim: false - require_stable_command: true - yaw_tolerance: 0.07 - pitch_tolerance: 0.04 diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml index d8c2c07ed..c0d91b916 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml @@ -35,6 +35,9 @@ rmcs_executor: - rmcs_core::controller::chassis::HeroSteeringWheelController -> steering_wheel_controller - rmcs_core::controller::chassis::ChassisClimberController -> climber_controller + - rmcs::AutoAimCapturerComponent -> auto_aim_capturer + - rmcs::AutoAimComponent -> auto_aim_component + # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster @@ -64,6 +67,19 @@ hero_hardware: left_back_zero_point: 7868 right_back_zero_point: 1640 +auto_aim_capturer: + ros__parameters: + camera_name: "" + exposure_us: 2000.0 + gain: 16.9807 + framerate: 249.1 + invert_image: false + rls_tau_sec: 10.0 + +auto_aim_component: + ros__parameters: + manual_shoot: false + value_broadcaster: ros__parameters: forward_list: @@ -338,51 +354,6 @@ steering_wheel_controller: k2: 3.082190e-03 no_load_power: 11.37 -auto_aim_controller: - ros__parameters: - # capture - use_video: false # If true, use video stream instead of camera. - video_path: "/workspaces/RMCS/rmcs_ws/resources/1.avi" - exposure_time: 1 - invert_image: false - # identifier - armor_model_path: "/models/mlp.onnx" - # pnp - fx: 1.722231837421459e+03 - fy: 1.724876404292754e+03 - cx: 7.013056440882832e+02 - cy: 5.645821718351237e+02 - k1: -0.064232403853946 - k2: -0.087667493884102 - k3: 0.792381808294582 - # tracker - armor_predict_duration: 500 - # controller - gimbal_predict_duration: 100 - yaw_error: 0. - pitch_error: 0. - shoot_velocity: 28.0 - predict_sec: 0.095 - # etc - buff_predict_duration: 200 - buff_model_path: "/models/buff_nocolor_v6.onnx" - omni_exposure: 1000.0 - record_fps: 120 - debug: false # Setup in actual using.Debug mode is used when referee is not ready - debug_color: 0 # 0 For blue while 1 for red. mine - debug_robot_id: 4 - debug_buff_mode: false - record: false - raw_img_pub: false # Set false in actual use - image_viewer_type: 2 - -hero_auto_aim_bridge: - ros__parameters: - config_file: "configs/standard3.yaml" - bullet_speed_fallback: 11.4 - result_timeout: 0.1 # 0.08 - debug: false - pitch_swept_frequency_controller: ros__parameters: target: /gimbal/pitch diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp index dc2ffd927..03deb0bef 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp @@ -33,7 +33,7 @@ class HeroGimbalController register_input("/remote/mouse", mouse_); register_input("/remote/keyboard", keyboard_); - register_input("/gimbal/auto_aim/control_direction", auto_aim_control_direction_, false); + register_input("/auto_aim/control_direction", auto_aim_control_direction_, false); register_input("/tf", tf_); register_output("/gimbal/mode", gimbal_mode_, rmcs_msgs::GimbalMode::IMU); @@ -114,9 +114,8 @@ class HeroGimbalController } TwoAxisGimbalSolver::AngleError update_imu_control() { - if (auto_aim_control_direction_.ready() - && (mouse_->right || *switch_right_ == rmcs_msgs::Switch::UP) - && !auto_aim_control_direction_->isZero()) { + if (auto_aim_control_direction_.ready() && !auto_aim_control_direction_->isZero() + && (mouse_->right || *switch_right_ == rmcs_msgs::Switch::UP)) { return imu_gimbal_solver_.update( TwoAxisGimbalSolver::SetControlDirection{ OdomImu::DirectionVector{*auto_aim_control_direction_}}); diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp index 0b1a5749f..eadb30281 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp @@ -75,14 +75,17 @@ class PutterController register_output("/gimbal/shoot/delay_ms", shoot_delay_ms_, nan_); // auto_aim - // register_input("/gimbal/auto_aim/fire_control", fire_control_, false); + register_input("/auto_aim/should_shoot", should_shoot_, false); register_output("/gimbal/shooter/mode", shoot_mode_, rmcs_msgs::ShootMode::SINGLE); register_output("/gimbal/shooter/condiction", shoot_condiction_); register_output("/gimbal/shooter/preloaded_ready", preloaded_ready_, false); } - ~PutterController() {} + void before_updating() override { + if (!should_shoot_.ready()) + should_shoot_.bind_directly(false); + } void update() override { const auto switch_right = *switch_right_; @@ -144,11 +147,8 @@ class PutterController || (last_switch_left_ == rmcs_msgs::Switch::MIDDLE && switch_left == rmcs_msgs::Switch::DOWN); - // const bool auto_fire_now = (switch_right == Switch::UP) && - // (*fire_control_); - const bool auto_fire_now = false; - // (switch_right == Switch::UP || (mouse.right && mouse.left)) - // && (*fire_control_); + const bool auto_fire_now = + (switch_right == Switch::UP || mouse.right) && *should_shoot_; const bool auto_trigger_emergence = mouse.right && (click_count_ >= 2); @@ -383,7 +383,7 @@ class PutterController OutputInterface shoot_delay_ms_; - InputInterface fire_control_; + InputInterface should_shoot_; std::chrono::steady_clock::time_point last_fire_time_{}; std::chrono::steady_clock::time_point last_click_time_{}; int click_count_ = 0; @@ -403,4 +403,4 @@ class PutterController #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::PutterController, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::PutterController, rmcs_executor::Component) From 9d338b4158c363cc4c52b57b5f6420e7c7420df5 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Thu, 16 Jul 2026 00:41:59 +0800 Subject: [PATCH 33/86] chore: Modify hero config --- .../steering-hero-little-six-friction.yaml | 48 +++++++++++-------- .../steering-hero-little-six-friction.cpp | 3 +- 2 files changed, 30 insertions(+), 21 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml index c0d91b916..67928053d 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml @@ -8,6 +8,7 @@ rmcs_executor: - rmcs_core::referee::command::Interaction -> referee_interaction - rmcs_core::referee::command::interaction::Ui -> referee_ui - rmcs_core::referee::app::ui::Hero -> referee_ui_hero + - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui - rmcs_core::referee::Command -> referee_command - rmcs_core::controller::gimbal::HeroGimbalController -> gimbal_controller @@ -71,15 +72,22 @@ auto_aim_capturer: ros__parameters: camera_name: "" exposure_us: 2000.0 - gain: 16.9807 - framerate: 249.1 + gain: 8.0 + framerate: 80.0 invert_image: false rls_tau_sec: 10.0 + use_hardware_sync: false auto_aim_component: ros__parameters: manual_shoot: false +auto_aim_ui: + ros__parameters: + offset_x: 0.000 + offset_y: 0.000 + offset_z: 0.000 + value_broadcaster: ros__parameters: forward_list: @@ -176,19 +184,19 @@ gimbal_controller: dual_yaw_controller: ros__parameters: - top_yaw_angle_kp: 14.0 #30.2 + top_yaw_angle_kp: 14.0 # 30.2 top_yaw_angle_ki: 0.0 top_yaw_angle_kd: 0.0 - top_yaw_velocity_kp: 9.37 #11.0 - top_yaw_velocity_ki: 0.00033 #0.00029 + top_yaw_velocity_kp: 9.37 # 11.0 + top_yaw_velocity_ki: 0.00033 # 0.00029 top_yaw_velocity_kd: 0.0 top_yaw_velocity_integral_min: -2500.0 top_yaw_velocity_integral_max: 2500.0 - bottom_yaw_angle_kp: 10.0 #18.4 + bottom_yaw_angle_kp: 10.0 # 18.4 bottom_yaw_angle_ki: 0.0 bottom_yaw_angle_kd: 0.0 - bottom_yaw_velocity_kp: 2.00 #2.81 - bottom_yaw_velocity_ki: 0.000071 #0.00028 + bottom_yaw_velocity_kp: 2.00 # 2.81 + bottom_yaw_velocity_ki: 0.000071 # 0.00028 bottom_yaw_velocity_kd: 0.0 bottom_yaw_velocity_integral_min: -2500.0 bottom_yaw_velocity_integral_max: 2500.0 @@ -258,19 +266,19 @@ friction_wheel_controller: - 480.0 - 480.0 - 480.0 - # - 527.0 - # - 527.0 - # - 527.0 - # - 435.0 - # - 435.0 - # - 435.0 + # - 527.0 + # - 527.0 + # - 527.0 + # - 435.0 + # - 435.0 + # - 435.0 friction_velocities_profile_1: - - 527.0 - - 527.0 - - 527.0 - - 435.0 - - 435.0 - - 435.0 + - 527.0 + - 527.0 + - 527.0 + - 435.0 + - 435.0 + - 435.0 friction_soft_start_stop_time: 1.0 heat_controller: diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp index 6ba2f8a76..cdc0e6630 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp @@ -482,7 +482,8 @@ class SteeringHeroLittle void gpio_digital_read_result_callback( const librmcs::spec::rmcs_board_lite::GpioDescriptor& gpio, const librmcs::data::GpioDigitalDataView& data) override { - if (gpio == librmcs::spec::rmcs_board_lite::kGpioDescriptors.kUart1Rx) { + + /* */ if (gpio == librmcs::spec::rmcs_board_lite::kGpioDescriptors.kUart1Rx) { photoelectric_sensor_status_atomic.store(data.high); } else if (gpio == librmcs::spec::rmcs_board_lite::kGpioDescriptors.kUart1Tx) { if (!data.timestamp_quarter_us) From 1a7d3d96402b3d1e01454061fb21f178b05c9809 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sat, 18 Jul 2026 08:55:39 +0800 Subject: [PATCH 34/86] wip: Merge hero and infantry 3/4, remove unused robot --- .../config/deformable-infantry-omni-b.yaml | 121 ++- .../config/deformable-infantry-omni.yaml | 16 +- rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 296 ------ rmcs_ws/src/rmcs_core/plugins.xml | 3 - .../hardware/deformable-infantry-omni-b.cpp | 857 +++++++++++++++++ .../src/hardware/deformable-infantry-omni.cpp | 861 ++++++++++++++++++ rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 57 +- .../rmcs_core/src/hardware/omni_infantry.cpp | 103 ++- .../steering-hero-little-six-friction.cpp | 566 ++++++------ 9 files changed, 2172 insertions(+), 708 deletions(-) delete mode 100644 rmcs_ws/src/rmcs_bringup/config/sentry.yaml create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 5f1c382f6..db46b0666 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -30,35 +30,76 @@ rmcs_executor: - rmcs_core::controller::chassis::DeformableJointController -> rb_joint_controller - rmcs_core::controller::chassis::DeformableJointController -> rf_joint_controller - # - rmcs::AutoAimComponent - # - rmcs::AutoAimCapturerComponent -> auto_aim_capturer + - rmcs::AutoAimComponent -> auto_aim_component + - rmcs::AutoAimCapturerComponent -> auto_aim_capturer + - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + # - rmcs_core::debug::ValueCollector -> value_collector + +value_collector: + ros__parameters: + csv_path: "/tmp/pitch_.csv" + signals: + - /gimbal/pitch/angle + - /gimbal/pitch/velocity + - /gimbal/pitch/velocity_imu + - /gimbal/pitch/angle_error + - /gimbal/pitch/control_torque + - /gimbal/pitch/control_velocity + write_interval: 5 + flush_interval: 1000 + +value_broadcaster: + ros__parameters: + forward_list: + - /gimbal/pitch/angle + - /gimbal/pitch/velocity auto_aim_capturer: ros__parameters: camera_name: "" exposure_us: 2000.0 - gain: 16.9807 - framerate: 249.1 + gain: 8.0 + framerate: 120.0 invert_image: false rls_tau_sec: 10.0 + use_hardware_sync: false + delay_ms: 6.5 -value_broadcaster: +auto_aim_component: ros__parameters: - forward_list: - - /gimbal/yaw/angle - - /gimbal/yaw/velocity + # WARN: 危险！可选 red | blue，生效后裁判系统 ID 缺席时 + # 将强行绑定为对应阵营哨兵，仅供调试，严禁比赛启用。 + # 留空或填 unknow 表示禁用。 + dangerous_fallback: "" + manual_shoot: true + camera_translation: [0.058, -0.08, 0.0] + fire_control: + bullet_speed: 22.5 + shoot_delay: 0.1 + offset_yaw: -0.0 + offset_pitch: +0.0 + attack_window: 120.0 + window_hysteresis: 0.2 + is_lazy_gimbal: false + attack_preaim: false + require_stable_command: false + yaw_tolerance: 0.07 + pitch_tolerance: 0.04 + +auto_aim_ui: + ros__parameters: + offset_x: 0.0 + offset_y: -0.08 + offset_z: 0.0 deformable_infantry: ros__parameters: - serial_filter_rmcs_board: "AF-73B2-E8A1-A544-79ED-5BDA-D088-7F21-A6A6" - serial_filter_top_board: "AF-ABAC-786D-1B53-99F6-00A2-42A6-AA95-9D69" - serial_filter_imu: "AF-C26A-0C9C-CF41-3E3C-1596-524B-7527-5744" - left_front_zero_point: 7173 - left_back_zero_point: 5167 - right_back_zero_point: 3098 - right_front_zero_point: 6485 + serial_filter_bottom_board: "AF-73B2-E8A1-A544-79ED-5BDA-D088-7F21-A6A6" + serial_filter_top_board: "AF-C26A-0C9C-CF41-3E3C-1596-524B-7527-5744" + chassis_radius: 0.2341741 + rod_length: 0.140 yaw_motor_zero_point: 57900 pitch_motor_zero_point: 56354 debug_log_supercap: false @@ -68,17 +109,17 @@ deformable_infantry: chassis_controller: ros__parameters: # Deploy geometry / chassis-owned joint intent - min_angle: 8.0 - max_angle: 58.0 + min_angle: 5.0 + max_angle: 59.0 active_suspension_enable: true spin_ratio: 1.0 deformable_suspension: ros__parameters: # IMU attitude correction at min-angle stance. - active_suspension_pitch_outer_kp: 8.0 - active_suspension_pitch_outer_ki: 0.35 - active_suspension_pitch_outer_kd: 0.28 + active_suspension_pitch_outer_kp: 12.0 + active_suspension_pitch_outer_ki: 0.02 + active_suspension_pitch_outer_kd: 0.0 active_suspension_pitch_outer_integral_min: -2.0 active_suspension_pitch_outer_integral_max: 2.0 active_suspension_pitch_outer_output_min: -3.0 @@ -92,9 +133,9 @@ deformable_suspension: active_suspension_pitch_inner_output_min: -0.785 active_suspension_pitch_inner_output_max: 0.785 - active_suspension_roll_outer_kp: 8.0 - active_suspension_roll_outer_ki: 0.35 - active_suspension_roll_outer_kd: 0.28 + active_suspension_roll_outer_kp: 12.0 + active_suspension_roll_outer_ki: 0.02 + active_suspension_roll_outer_kd: 0.0 active_suspension_roll_outer_integral_min: -2.0 active_suspension_roll_outer_integral_max: 2.0 active_suspension_roll_outer_output_min: -3.0 @@ -122,10 +163,11 @@ deformable_suspension: gimbal_controller: ros__parameters: - upper_limit: -0.65 # -35 deg - lower_limit: 0.05 # 6 deg + upper_limit: -0.47123 # -27 deg + lower_limit: 0.10 # 8 deg + ctrl_hold_pitch_target_angle: 0.0 - yaw_angle_kp: 30.0 + yaw_angle_kp: 15.0 yaw_angle_ki: 0.0 yaw_angle_kd: 0.0 @@ -133,14 +175,17 @@ gimbal_controller: yaw_velocity_ki: 0.0 yaw_velocity_kd: 0.0 - pitch_angle_kp: 25.0 - pitch_angle_ki: 0.0 - pitch_angle_kd: 0.0 + pitch_angle_kp: 35.0 + pitch_angle_ki: 0.02 + pitch_angle_kd: 0.3 - pitch_velocity_kp: 2.2 + pitch_velocity_kp: 2.0 pitch_velocity_ki: 0.0 pitch_velocity_kd: 0.0 + pitch_gravity_ff_gain: 4.302 + pitch_gravity_ff_phase: 0.589 + pitch_torque_control: true friction_wheel_controller: @@ -156,7 +201,7 @@ friction_wheel_controller: heat_controller: ros__parameters: heat_per_shot: 10000 - reserved_heat: 5000 + reserved_heat: 15000 bullet_feeder_controller: ros__parameters: @@ -198,11 +243,9 @@ bullet_feeder_velocity_pid_controller: deformable_chassis_controller: ros__parameters: - mass: 23.0 + mass: 25.5 moment_of_inertia: 1.0 - chassis_radius: 0.2341741 - rod_length: 0.150 - wheel_radius: 0.07 + wheel_radius: 0.075 friction_coefficient: 6.6 k1: 2.958580e+00 k2: 3.082190e-03 @@ -210,12 +253,10 @@ deformable_chassis_controller: lf_joint_controller: ros__parameters: - # Joint-local servo inputs produced by chassis intent generation measurement_angle: /chassis/left_front_joint/physical_angle setpoint_angle: /chassis/left_front_joint/target_physical_angle setpoint_velocity: /chassis/left_front_joint/target_physical_velocity control: /chassis/left_front_joint/control_torque - dt: 0.001 b0: -1.0 kt: 1.0 @@ -232,9 +273,9 @@ lf_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 + lb_joint_controller: ros__parameters: - # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/left_back_joint/physical_angle setpoint_angle: /chassis/left_back_joint/target_physical_angle setpoint_velocity: /chassis/left_back_joint/target_physical_velocity @@ -255,9 +296,9 @@ lb_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 + rb_joint_controller: ros__parameters: - # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_back_joint/physical_angle setpoint_angle: /chassis/right_back_joint/target_physical_angle setpoint_velocity: /chassis/right_back_joint/target_physical_velocity @@ -278,9 +319,9 @@ rb_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 + rf_joint_controller: ros__parameters: - # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_front_joint/physical_angle setpoint_angle: /chassis/right_front_joint/target_physical_angle setpoint_velocity: /chassis/right_front_joint/target_physical_velocity diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index baa6d7b96..cb2131048 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -10,7 +10,6 @@ rmcs_executor: - rmcs_core::referee::command::Interaction -> referee_interaction - rmcs_core::referee::command::interaction::Ui -> referee_ui - rmcs_core::referee::app::ui::DeformableInfantry -> referee_ui_infantry - - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui - rmcs_core::controller::gimbal::DeformableInfantryGimbalController -> gimbal_controller @@ -33,6 +32,7 @@ rmcs_executor: - rmcs::AutoAimComponent -> auto_aim_component - rmcs::AutoAimCapturerComponent -> auto_aim_capturer + - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster # - rmcs_core::debug::ValueCollector -> value_collector @@ -78,8 +78,8 @@ auto_aim_component: fire_control: bullet_speed: 22.5 shoot_delay: 0.1 - offset_yaw: -0.0 - offset_pitch: +0.0 + offset_yaw: +0.1 + offset_pitch: -0.6 attack_window: 120.0 window_hysteresis: 0.2 is_lazy_gimbal: false @@ -98,10 +98,8 @@ deformable_infantry: ros__parameters: serial_filter_bottom_board: "AF-23FB-EE32-B892-1302-AE70-D640-7B4E-0CBF" serial_filter_top_board: "AF-7A42-07AA-D181-0356-7715-6D7C-4C65-5762" - left_front_zero_point: 374 - left_back_zero_point: 5801 - right_back_zero_point: 7817 - right_front_zero_point: 7136 + chassis_radius: 0.2341741 + rod_length: 0.140 yaw_motor_zero_point: 43365 pitch_motor_zero_point: 6432 debug_log_supercap: false @@ -166,7 +164,7 @@ deformable_suspension: gimbal_controller: ros__parameters: upper_limit: -0.47123 # -27 deg - lower_limit: 0.15707 # 9 deg + lower_limit: 0.10 # 8 deg ctrl_hold_pitch_target_angle: 0.0 yaw_angle_kp: 15.0 @@ -247,8 +245,6 @@ deformable_chassis_controller: ros__parameters: mass: 25.5 moment_of_inertia: 1.0 - chassis_radius: 0.2341741 - rod_length: 0.140 wheel_radius: 0.075 friction_coefficient: 6.6 k1: 2.958580e+00 diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml deleted file mode 100644 index c4c0bd3aa..000000000 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ /dev/null @@ -1,296 +0,0 @@ -rmcs_executor: - ros__parameters: - update_rate: 1000.0 - components: - - rmcs_core::hardware::Sentry -> sentry_hardware - - - rmcs_core::referee::Status -> referee_status - - rmcs_core::referee::Command -> referee_command - - rmcs_core::referee::command::Interaction -> referee_interaction - - rmcs_core::referee::command::interaction::SentryDecision -> referee_sentry_decision - - rmcs_core::referee::command::interaction::Ui -> referee_ui - - - rmcs_core::controller::gimbal::EccentricDualYaw -> gimbal_controller - - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller - - rmcs_core::controller::shooting::HeatController -> heat_controller - - rmcs_core::controller::shooting::BulletFeederController17mm -> bullet_feeder_controller - - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller - - rmcs_core::controller::chassis::SentryClimber -> sentry_climber - - rmcs_core::controller::chassis::ChassisController -> chassis_controller - - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller - - # - rmcs::navigation::Navigation -> rmcs_navigation - - - rmcs::AutoAimRecorderComponent -> auto_aim_recorder - - rmcs::AutoAimCapturerComponent -> auto_aim_capturer - - rmcs::AutoAimComponent -> auto_aim_component - - # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster - # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster - -rmcs_navigation: - ros__parameters: - command_vel_name: "/cmd_vel" - endpoint: "rmuc" - enable_goal_topic_forward: true - -auto_aim_capturer: - ros__parameters: - camera_name: "" - exposure_us: 1500.0 - gain: 8.0 - framerate: 120.0 - invert_image: true - rls_tau_sec: 10.0 - use_hardware_sync: true - delay_ms: 6.5 - -auto_aim_recorder: - ros__parameters: - output_path: "/tmp/autoaim/records" - queue_depth: 16 - flush_every_n_frames: 64 - max_duration_seconds: 300 - max_videos_size_gb: 100.0 - auto_record: false - -auto_aim_component: - ros__parameters: - manual_shoot: true - enable_rune: true - # track_ids: [OUTPOST, BASE] - track_ids: [HERO, ENGINEER, INFANTRY_3, INFANTRY_4, SENTRY, OUTPOST, BASE] - camera_translation: [0.07128, 0.0, 0.0481] - fire_control: - bullet_speed: 22.5 - shoot_delay: 0.05 - offset_yaw: -0.3 - offset_pitch: -0.1 - attack_window: 80.0 - degraded_angle_speed: 12.0 - window_redundancy: 0.8 - window_hysteresis: 0.2 - attack_preaim: false - require_stable_command: false - yaw_tolerance: 0.07 - pitch_tolerance: 0.04 - rune_idle_duration: 0.4 - rune_shoot_duration: 0.2 - -value_broadcaster: - ros__parameters: - forward_list: - - /gimbal/yaw/velocity_imu - - /gimbal/pitch/velocity_imu - -sentry_hardware: - ros__parameters: - board_serial_bottom_board: "af-b4e5" - board_serial_gimbal_board: "af-8b8b" - - pitch_motor_zero_point: 12453 - - bottom_yaw_motor_zero_point: 54253 - top_yaw_motor_zero_point: 32736 - - left_front_zero_point: 5776 - left_back_zero_point: 3784 - right_back_zero_point: 3048 - right_front_zero_point: 5135 - -gimbal_controller: - ros__parameters: - upper_limit: -0.65 - lower_limit: 0.36 - - top_yaw_angle_kp: 30.0 - top_yaw_angle_ki: 0.008 - top_yaw_angle_kd: 0.005 - top_yaw_velocity_kp: 2.160 - top_yaw_velocity_ki: 0.0 - top_yaw_velocity_kd: 0.0 - - bottom_yaw_angle_kp: 15.0 - bottom_yaw_angle_ki: 0.01 - bottom_yaw_angle_kd: 0.0 - bottom_yaw_velocity_kp: 2.75 - bottom_yaw_velocity_ki: 0.00125 - bottom_yaw_velocity_kd: 0.0 - - pitch_angle_kp: 35.0 - pitch_angle_ki: 0.01 - pitch_angle_kd: 0.0 - pitch_velocity_kp: 2.5 - pitch_velocity_ki: 0.01 - pitch_velocity_kd: 0.0 - - top_yaw_angle_integral_min: -187.0 - top_yaw_angle_integral_max: 187.0 - top_yaw_velocity_integral_min: -2400.0 - top_yaw_velocity_integral_max: 2400.0 - - bottom_yaw_angle_integral_min: -150.0 - bottom_yaw_angle_integral_max: 150.0 - bottom_yaw_velocity_integral_min: -2400.0 - bottom_yaw_velocity_integral_max: 2400.0 - - pitch_angle_integral_min: -150.0 - pitch_angle_integral_max: 150.0 - pitch_velocity_integral_min: -2400.0 - pitch_velocity_integral_max: 2400.0 - - top_yaw_velocity_ff_gain: 1.0 - top_yaw_ff_cutoff_hz: 10.0 - top_yaw_ff_max: 2.0 - top_yaw_ff_jump_threshold: 0.01 - -chassis_controller: - ros__parameters: - angular_velocity_max: 10.0 - translational_velocity_max: 10.0 - following_velocity_kp: 7.0 - following_velocity_ki: 0.0 - following_velocity_kd: 0.0 - -sentry_climber: - ros__parameters: - track_group: - speed_rush: 20.0 - kp: 1.0 - ki: 0.0 - kd: 0.5 - sync_coefficient: 0.2 - power_estimate_bias: 0.0 - power_estimate_k_tau2: 1.0 - power_estimate_k_mech: 1.0 - stick_group: - speed_drop: 30.0 - speed_rise: 60.0 - rise_torque_limit: 1.5 - land_speed_begin: 100.0 - land_speed_final: 10.0 - land_duration: 0.5 - land_torque_limit: 8.0 - blocked_torque_threshold: 0.1 - blocked_speed_threshold: 0.1 - kp: 1.0 - ki: 0.0 - kd: 0.0 - sync_coefficient: 0.2 - hold_torque: 0.01 - block_hold: 0.05 - align: - err: 0.18 - w: 0.2 - hold: 0.05 - timeout: 15.0 - climb: - approach_pitch: 0.585 - leveled_pitch: 0.05 - approach_vx: 1.2 - deploy_vx: 0.3 - dash_vx: 3.0 - retract_vx: 0.0 - dash_min: 0.1 - dash_duration: 0.8 - stick_timeout: 8.0 - approach_timeout: 6.0 - land: - dash_vx: 1.0 - soft_vx: 0.3 - land_pitch: 0.15 - land_delay: 0.2 - stick_timeout: 8.0 - soft_timeout: 3.0 - settle_timeout: 8.0 - leave_vx: 0.3 - leave_duration: 1.0 - -friction_wheel_controller: - ros__parameters: - friction_wheels: - - /gimbal/left_friction - - /gimbal/right_friction - friction_velocities: - - 575.0 - - 575.0 - friction_soft_start_stop_time: 1.0 - -heat_controller: - ros__parameters: - heat_per_shot: 10000 - reserved_heat: 30000 - -bullet_feeder_controller: - ros__parameters: - bullets_per_feeder_turn: 9.0 - shot_frequency: 28.0 - safe_shot_frequency: 10.0 - eject_frequency: 15.0 - eject_time: 0.15 - deep_eject_frequency: 15.0 - deep_eject_time: 0.20 - single_shot_max_stop_delay: 2.0 - -left_friction_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/left_friction/velocity - setpoint: /gimbal/left_friction/control_velocity - control: /gimbal/left_friction/control_torque - kp: 0.003436926 - ki: 0.00 - kd: 0.009373434 - -right_friction_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/right_friction/velocity - setpoint: /gimbal/right_friction/control_velocity - control: /gimbal/right_friction/control_torque - kp: 0.003436926 - ki: 0.00 - kd: 0.009373434 - -bullet_feeder_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/bullet_feeder/velocity - setpoint: /gimbal/bullet_feeder/control_velocity - control: /gimbal/bullet_feeder/control_torque - kp: 0.283 - ki: 0.0 - kd: 0.0 - -steering_wheel_controller: - ros__parameters: - mess: 22.0 - moment_of_inertia: 0.77852676 - vehicle_radius: 0.26870058 - wheel_radius: 0.055 - friction_coefficient: 0.666 - k1: 2.958580e+00 - k2: 3.082190e-03 - no_load_power: 11.37 - - chassis_translation_kp: 20.0 - chassis_translation_ki: 0.00 - chassis_translation_kd: 0.00 - chassis_translation_integral_limit: 200.0 - - chassis_angular_velocity_kp: 8.0 - chassis_angular_velocity_ki: 0.005 - chassis_angular_velocity_kd: 1.0 - chassis_angular_velocity_integral_limit: 200.0 - - steering_velocity_kp: 0.15 - steering_velocity_ki: 0.0 - steering_velocity_kd: 0.0 - - steering_angle_kp: 30.0 - steering_angle_ki: 0.0 - steering_angle_kd: 0.0 - - wheel_velocity_kp: 0.5 - wheel_velocity_ki: 0.0 - wheel_velocity_kd: 0.0 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 6133fac06..4f402aaa2 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -1,10 +1,7 @@ - - - diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp new file mode 100644 index 000000000..1183c9b1f --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -0,0 +1,857 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "hardware/device/bmi088.hpp" +#include "hardware/device/bmi088_ekf.hpp" +#include "hardware/device/board_clock_lifter.hpp" +#include "hardware/device/can_packet.hpp" +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/lk_motor.hpp" +#include "hardware/device/supercap.hpp" +#include "hardware/util/status_monitor.hpp" + +namespace rmcs_core::hardware { + +using Clock = std::chrono::steady_clock; + +class DeformableInfantryOmniB + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DeformableInfantryOmniB() + : Node( + get_component_name(), + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) + , command_(create_partner_component(get_component_name() + "_command", *this)) { + using namespace rmcs_description; + + register_input("/predefined/timestamp", timestamp_); + register_output("/tf", tf_); + register_output( + "/auto_aim/camera_transform", camera_transform_, Eigen::Isometry3d::Identity()); + register_output("/auto_aim/barrel_direction", barrel_direction_, Eigen::Vector3d::UnitX()); + register_output("/auto_aim/yaw_velocity", auto_aim_yaw_velocity_, 0.0); + + tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); + + bottom_board_ = std::make_unique( + *this, *command_, get_parameter("serial_filter_bottom_board").as_string()); + top_board_ = std::make_unique( + *this, *command_, get_parameter("serial_filter_top_board").as_string()); + + // For command: remote-status + using Srv = std_srvs::srv::Trigger; + status_service_ = create_service( + "/rmcs/service/robot_status", + [this](const Srv::Request::SharedPtr&, const Srv::Response::SharedPtr& response) { + status_service_callback(response); + }); + } + + ~DeformableInfantryOmniB() override = default; + + void before_updating() override { top_board_->request_hard_sync_read(); } + + void update() override { + bottom_board_->update(); + top_board_->update(); + + using namespace rmcs_description; + *camera_transform_ = fast_tf::lookup_transform(*tf_); + *barrel_direction_ = + *fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + *auto_aim_yaw_velocity_ = top_board_->gimbal_yaw_velocity(); + } + + void command_update() { + const bool even = ((cmd_tick_++ & 1u) == 0u); + bottom_board_->command_update(even); + top_board_->command_update(); + } + +private: + static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); + static constexpr auto kLeftFront = 0; + static constexpr auto kLeftBack = 1; + static constexpr auto kRightBack = 2; + static constexpr auto kRightFront = 3; + static constexpr auto kJointName = std::array{ + "left_front", + "left_back", + "right_back", + "right_front", + }; + + class Command : public Component { + public: + explicit Command(DeformableInfantryOmniB& deformableInfantry) + : deformableInfantry(deformableInfantry) {} + + void update() override { deformableInfantry.command_update(); } + + DeformableInfantryOmniB& deformableInfantry; + }; + + struct TopBoard final : public librmcs::board::RmcsBoardLite::Callback { + public: + explicit TopBoard( + DeformableInfantryOmniB& status, Component& command, + const std::string& serial_filter = {}) + : tf_{status.tf_} + , bmi088_{device::Bmi088Ekf::Config{ + .body_to_sensor = + Eigen::AngleAxisd{std::numbers::pi / 2.0, Eigen::Vector3d::UnitX()} + .toRotationMatrix()}} + , gimbal_pitch_motor_(status, command, "/gimbal/pitch") + , gimbal_left_friction_(status, command, "/gimbal/left_friction") + , gimbal_right_friction_(status, command, "/gimbal/right_friction") { + + gimbal_pitch_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} + .set_reversed() + .set_encoder_zero_point( + static_cast(status.get_parameter("pitch_motor_zero_point").as_int()))); + + gimbal_left_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} + .set_reduction_ratio(1.) + .set_reversed()); + gimbal_right_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2}.set_reduction_ratio( + 1.)); + + status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); + status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); + status.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); + status.register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); + + auto options = librmcs::board::AdvancedOptions{}; + options.dangerously_skip_version_checks = true; + board_ = std::make_unique(*this, serial_filter, options); + + board_->start_transmit().gpio_digital_read( + Spec::kGpios.kUart1Rx, // + { + .period_ms = 0, + .asap = false, + .rising_edge = false, + .falling_edge = true, + .capture_timestamp = true, + .pull = librmcs::data::GpioPull::kUp, + }); + } + + ~TopBoard() override = default; + + [[nodiscard]] auto gimbal_yaw_velocity() const -> double { + return *gimbal_yaw_velocity_bmi088_; + } + + void request_hard_sync_read() { + // RMCS-lite top board variant currently has no GPIO hard-sync request + // path. + } + + void update() { + gimbal_pitch_motor_.update_status(); + gimbal_left_friction_.update_status(); + gimbal_right_friction_.update_status(); + + const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); + + if (auto snapshot = bmi088_.snapshot()) { + *gimbal_pitch_velocity_bmi088_ = snapshot->gyro_body.y(); + *gimbal_yaw_velocity_bmi088_ = snapshot->gyro_body.z(); + tf_->set_transform( + snapshot->orientation.conjugate()); + } + + tf_->set_state( + pitch_encoder_angle); + } + + void command_update() const { + auto builder = board_->start_transmit(); + { + auto packet = gimbal_pitch_motor_.generate_torque_command(); + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x141, + .can_data = packet.as_bytes(), + }); + } + { + auto packet = device::CanPacket8{uint64_t{0}}; + packet << gimbal_right_friction_; + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = gimbal_right_friction_.send_id(), + .can_data = packet.as_bytes(), + }); + } + { + auto packet = device::CanPacket8{uint64_t{0}}; + packet << gimbal_left_friction_; + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = gimbal_left_friction_.send_id(), + .can_data = packet.as_bytes(), + }); + } + } + + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + if (can == Spec::kCans.kCan0) { + if (data.can_id == 0x141) + gimbal_pitch_motor_.store_status(data.can_data); + monitor_.tick("Top::Can0", data.can_id); + } else if (can == Spec::kCans.kCan1) { + gimbal_right_friction_.match_then_store_status(data.can_id, data.can_data); + monitor_.tick("Top::Can1", data.can_id); + } else if (can == Spec::kCans.kCan2) { + gimbal_left_friction_.match_then_store_status(data.can_id, data.can_data); + monitor_.tick("Top::Can2", data.can_id); + } + } + + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { + const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); + bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); + monitor_.tick("Top::Imu", "Acc"); + } + + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); + monitor_.tick("Top::Imu", "Gyr"); + if (!timestamp.has_value()) + return; + auto snapshot = + bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); + if (snapshot) + imu_snapshot_output_.emit(*snapshot); + } + + void gpio_digital_read_result_callback( + const Spec::Gpio& gpio, const View::GpioDigital& data) override { + if (gpio != Spec::kGpios.kUart1Rx) + return; + if (!data.timestamp_quarter_us) + return; + + const auto timestamp = board_clock_lifter_.lift_timestamp(*data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + + camera_signal_output_.emit(*timestamp); + monitor_.tick("Top::CameraSync", "Active"); + } + + auto status() const -> std::vector { return monitor_.text(); } + + OutputInterface& tf_; + OutputInterface gimbal_yaw_velocity_bmi088_; + OutputInterface gimbal_pitch_velocity_bmi088_; + + EventOutputInterface imu_snapshot_output_; + EventOutputInterface camera_signal_output_; + + device::Bmi088Ekf bmi088_; + device::BoardClockLifter board_clock_lifter_; + device::LkMotor gimbal_pitch_motor_; + device::DjiMotor gimbal_left_friction_; + device::DjiMotor gimbal_right_friction_; + + StatusMonitor monitor_{}; + std::unique_ptr board_; + }; + + struct BottomBoard final : public librmcs::board::RmcsBoardLite::Callback { + public: + explicit BottomBoard( + DeformableInfantryOmniB& status, Component& command, + const std::string& serial_filter = {}) + : status_{status} + , command_{command} + , kChassisRadiusBase(status.get_parameter("chassis_radius").as_double()) + , kRodLength(status.get_parameter("rod_length").as_double()) + , kDefaultRadius(kChassisRadiusBase + kRodLength) { + + status.register_output("/referee/serial", referee_serial_); + referee_serial_->read = [this](std::byte* buffer, size_t size) { + return referee_ring_buffer_receive_.pop_front_n( + [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) { + board_->start_transmit().uart_transmit( + Spec::kUarts.kUart0, {.uart_data = std::span{buffer, size}}); + return size; + }; + + gimbal_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( + static_cast(status.get_parameter("yaw_motor_zero_point").as_int()))); + + for (auto& motor : chassis_wheel_motors_) + motor.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} + .set_reversed() + .set_reduction_ratio(13.0) + .enable_multi_turn_angle()); + + for (auto& motor : chassis_joint_motors_) + motor.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei36} + .set_reversed() + .enable_multi_turn_angle()); + + imu_.set_coordinate_mapping([](double x, double y, double z) { + // Keep the existing chassis yaw axis mapping explicit until the bottom-board IMU + // installation is re-validated on hardware. + return std::make_tuple(-y, x, z); + }); + + gimbal_bullet_feeder_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM2006, 3} + .enable_multi_turn_angle()); + + status.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); + status.register_output("/chassis/imu/pitch", chassis_imu_pitch_, 0.0); + status.register_output("/chassis/imu/roll", chassis_imu_roll_, 0.0); + status.register_output("/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, 0.0); + status.register_output("/chassis/imu/roll_rate", chassis_imu_roll_rate_, 0.0); + for (size_t i = 0; i < 4; ++i) { + status.register_output( + std::format( + "/chassis/{}_joint/physical_angle", DeformableInfantryOmniB::kJointName[i]), + joint_physical_angle_[i], kNaN); + status.register_output( + std::format( + "/chassis/{}_joint/physical_velocity", + DeformableInfantryOmniB::kJointName[i]), + joint_physical_velocity_[i], kNaN); + } + status.register_output("/chassis/encoder/alpha", encoder_alpha_, kNaN); + status.register_output("/chassis/encoder/alpha_dot", encoder_alpha_dot_, kNaN); + status.register_output("/chassis/radius", radius_, kDefaultRadius); + + status.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); + status.get_parameter_or("debug_log_wheel_motor", debug_log_wheel_motor_, false); + status.get_parameter_or( + "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); + + auto options = librmcs::board::AdvancedOptions{}; + options.dangerously_skip_version_checks = true; + board_ = std::make_unique(*this, serial_filter, options); + } + + void update() { + imu_.update_status(); + *chassis_yaw_velocity_imu_ = imu_.gz(); + { + const double q0 = imu_.q0(); + const double q1 = imu_.q1(); + const double q2 = imu_.q2(); + const double q3 = imu_.q3(); + + double sin_pitch = 2.0 * (q0 * q2 - q3 * q1); + sin_pitch = std::clamp(sin_pitch, -1.0, 1.0); + + const double standard_pitch = std::asin(sin_pitch); + const double standard_roll = + std::atan2(2.0 * (q0 * q1 + q2 * q3), 1.0 - 2.0 * (q1 * q1 + q2 * q2)); + + // Export chassis attitude using the requested convention: + // pitch < 0 when the front is higher, roll > 0 when the left side is higher. + *chassis_imu_pitch_ = -standard_pitch; + *chassis_imu_roll_ = standard_roll; + *chassis_imu_pitch_rate_ = -imu_.gy(); + *chassis_imu_roll_rate_ = imu_.gx(); + } + + for (auto& motor : chassis_wheel_motors_) + motor.update_status(); + for (auto& motor : chassis_joint_motors_) + motor.update_status(); + + for (size_t i = 0; i < 4; ++i) + update_joint_physical_feedback_( + i, joint_physical_angle_[i], joint_physical_velocity_[i]); + + update_geometry_feedback_(); + if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) + log_chassis_feedback_once_per_second_(); + + dr16_.update_status(); + gimbal_yaw_motor_.update_status(); + if (supercap_status_received_.load(std::memory_order_relaxed)) + supercap_.update_status(); + if (debug_log_supercap_) + log_supercap_feedback_once_per_second_(); + gimbal_bullet_feeder_.update_status(); + + tf_->set_state( + gimbal_yaw_motor_.angle()); + } + + void command_update(bool even) { + auto builder = board_->start_transmit(); + if (even) { + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kLeftFront].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kLeftBack].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kRightBack].generate_command(), + device::CanPacket8::PaddingQuarter{}, + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan3, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kRightFront].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = 0x142, + .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + } + .as_bytes(), + }); + } else { + for (size_t i = 0; i < 4; ++i) { + switch (i) { + case kLeftFront: + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + case kLeftBack: + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + case kRightBack: + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + case kRightFront: + builder.can_transmit( + Spec::kCans.kCan3, // + { + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + default: break; + } + } + } + } + + static constexpr double kJointZeroPhysicalAngleRad = 62.5 * std::numbers::pi / 180.0; + + DeformableInfantryOmniB& status_; + Component& command_; + + std::unique_ptr board_; + + // Interfaces + + OutputInterface& tf_{status_.tf_}; + + OutputInterface chassis_yaw_velocity_imu_; + OutputInterface chassis_imu_pitch_; + OutputInterface chassis_imu_roll_; + OutputInterface chassis_imu_pitch_rate_; + OutputInterface chassis_imu_roll_rate_; + + std::array, 4> joint_physical_angle_; + std::array, 4> joint_physical_velocity_; + + OutputInterface encoder_alpha_; + OutputInterface encoder_alpha_dot_; + OutputInterface radius_; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; + OutputInterface referee_serial_; + + // State + + std::atomic wheel_status_received_[4] = {false, false, false, false}; + std::atomic joint_status_received_[4] = {false, false, false, false}; + + bool debug_log_supercap_ = false; + bool debug_log_wheel_motor_ = false; + bool debug_log_deformable_joint_motor_ = false; + + const double kChassisRadiusBase; + const double kRodLength; + const double kDefaultRadius; + + Clock::time_point next_chassis_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; + Clock::time_point next_supercap_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; + + // Device + + device::Bmi088 imu_{1000, 0.2, 0.0}; + device::LkMotor gimbal_yaw_motor_{status_, command_, "/gimbal/yaw"}; + device::Dr16 dr16_{status_}; + + device::DjiMotor chassis_wheel_motors_[4]{ + device::DjiMotor{status_, command_, "/chassis/left_front_wheel"}, + device::DjiMotor{status_, command_, "/chassis/left_back_wheel"}, + device::DjiMotor{status_, command_, "/chassis/right_back_wheel"}, + device::DjiMotor{status_, command_, "/chassis/right_front_wheel"}, + }; + device::LkMotor chassis_joint_motors_[4]{ + device::LkMotor{status_, command_, "/chassis/left_front_joint"}, + device::LkMotor{status_, command_, "/chassis/left_back_joint"}, + device::LkMotor{status_, command_, "/chassis/right_back_joint"}, + device::LkMotor{status_, command_, "/chassis/right_front_joint"}, + }; + + std::atomic latest_supercap_status_{device::CanPacket8{uint64_t{0}}}; + std::atomic supercap_status_received_{false}; + device::Supercap supercap_{status_, command_}; + + device::DjiMotor gimbal_bullet_feeder_{status_, command_, "/gimbal/bullet_feeder"}; + + void process_chassis_can_receive_(size_t index, const View::Can& data) { + if (data.is_extended_can_id || data.is_remote_transmission) + return; + if (data.can_id == 0x201) { + chassis_wheel_motors_[index].store_status(data.can_data); + wheel_status_received_[index].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x141) { + chassis_joint_motors_[index].store_status(data.can_data); + joint_status_received_[index].store(true, std::memory_order_relaxed); + } + } + + void update_joint_physical_feedback_( + size_t index, OutputInterface& angle_output, + OutputInterface& velocity_output) { + + if (!joint_status_received_[index].load(std::memory_order_relaxed)) { + *angle_output = kNaN; + *velocity_output = kNaN; + return; + } + + const auto to_physical_angle = [](double motor_angle) { + return kJointZeroPhysicalAngleRad - motor_angle; + }; + const auto to_physical_velocity = [](double motor_velocity) { return -motor_velocity; }; + + *angle_output = to_physical_angle(chassis_joint_motors_[index].angle()); + *velocity_output = to_physical_velocity(chassis_joint_motors_[index].velocity()); + } + + void update_geometry_feedback_() { + const Eigen::Vector4d alpha_rad{ + *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], + *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront]}; + const Eigen::Vector4d alpha_dot_rad{ + *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], + *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront]}; + + if (!alpha_rad.array().isFinite().all() || !alpha_dot_rad.array().isFinite().all()) { + *encoder_alpha_ = kNaN; + *encoder_alpha_dot_ = kNaN; + *radius_ = kDefaultRadius; + RCLCPP_WARN_THROTTLE( + status_.get_logger(), *status_.get_clock(), 1000, + "deformable joint feedback invalid, fallback chassis radius to default %.3f m", + kDefaultRadius); + return; + } + + *encoder_alpha_ = alpha_rad.mean(); + *encoder_alpha_dot_ = alpha_dot_rad.mean(); + *radius_ = (kChassisRadiusBase + kRodLength * alpha_rad.array().cos()).mean(); + } + + void log_chassis_feedback_once_per_second_() { + const auto now = Clock::now(); + if (now < next_chassis_feedback_log_time_) + return; + + const auto wheel_rx = [this](size_t index) { + return wheel_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; + }; + const auto joint_rx = [this](size_t index) { + return joint_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; + }; + + if (debug_log_wheel_motor_) { + std::string wheel_rx_str; + for (size_t i = 0; i < 4; ++i) { + if (i > 0) + wheel_rx_str.push_back(' '); + wheel_rx_str.push_back(wheel_rx(i)); + } + RCLCPP_INFO( + status_.get_logger(), + "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " + "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " + "rx=[%s]", + chassis_wheel_motors_[kLeftFront].angle(), + chassis_wheel_motors_[kLeftBack].angle(), + chassis_wheel_motors_[kRightBack].angle(), + chassis_wheel_motors_[kRightFront].angle(), + chassis_wheel_motors_[kLeftFront].angle(), + chassis_wheel_motors_[kLeftBack].angle(), + chassis_wheel_motors_[kRightBack].angle(), + chassis_wheel_motors_[kRightFront].angle(), wheel_rx_str.c_str()); + } + + if (debug_log_deformable_joint_motor_) { + std::string joint_rx_str; + for (size_t i = 0; i < 4; ++i) { + if (i > 0) + joint_rx_str.push_back(' '); + joint_rx_str.push_back(joint_rx(i)); + } + RCLCPP_INFO( + status_.get_logger(), + "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " + "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " + "rx=[%s]", + *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], + *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront], + *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], + *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront], + joint_rx_str.c_str()); + } + + next_chassis_feedback_log_time_ = now + std::chrono::seconds(1); + } + + void log_supercap_feedback_once_per_second_() { + const auto now = Clock::now(); + if (now < next_supercap_feedback_log_time_) + return; + + const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); + auto supercap_raw_packet = latest_supercap_status_.load(std::memory_order_relaxed); + const auto supercap_raw_bytes = supercap_raw_packet.as_bytes(); + + RCLCPP_INFO( + status_.get_logger(), + "[supercap] can1 rx=%c id=0x300 enabled=%d supercap_v=% .3f chassis_v=% .3f " + "power=% .3f raw=[%02X %02X %02X %02X %02X %02X %02X %02X]", + supercap_rx ? 'Y' : 'N', supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, + supercap_rx ? supercap_.supercap_voltage() : kNaN, + supercap_rx ? supercap_.chassis_voltage() : kNaN, + supercap_rx ? supercap_.chassis_power() : kNaN, + std::to_integer(supercap_raw_bytes[0]), + std::to_integer(supercap_raw_bytes[1]), + std::to_integer(supercap_raw_bytes[2]), + std::to_integer(supercap_raw_bytes[3]), + std::to_integer(supercap_raw_bytes[4]), + std::to_integer(supercap_raw_bytes[5]), + std::to_integer(supercap_raw_bytes[6]), + std::to_integer(supercap_raw_bytes[7])); + + next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); + } + + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) + return; + if (can == Spec::kCans.kCan0) { + process_chassis_can_receive_(0, data); + monitor_.tick("Bottom::Can0", data.can_id); + } else if (can == Spec::kCans.kCan1) { + process_chassis_can_receive_(1, data); + if (!data.is_extended_can_id && !data.is_remote_transmission + && data.can_id == 0x300) { + if (data.can_data.size() == 8) + latest_supercap_status_.store( + device::CanPacket8{data.can_data}, std::memory_order_relaxed); + supercap_.store_status(data.can_data); + supercap_status_received_.store(true, std::memory_order_relaxed); + } + monitor_.tick("Bottom::Can1", data.can_id); + } else if (can == Spec::kCans.kCan2) { + process_chassis_can_receive_(2, data); + if (data.is_extended_can_id || data.is_remote_transmission) + return; + if (data.can_id == 0x142) + gimbal_yaw_motor_.store_status(data.can_data); + else if (data.can_id == 0x203) + gimbal_bullet_feeder_.store_status(data.can_data); + monitor_.tick("Bottom::Can2", data.can_id); + } else if (can == Spec::kCans.kCan3) { + process_chassis_can_receive_(3, data); + monitor_.tick("Bottom::Can3", data.can_id); + } + } + + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + if (uart == Spec::kUarts.kDbus) { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + monitor_.tick("Bottom::Dbus", "Active"); + } else if (uart == Spec::kUarts.kUart0) { + const std::byte* ptr = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, + data.uart_data.size()); + monitor_.tick("Bottom::Uart0", "Active"); + } + } + + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { + imu_.store_accelerometer_status(data.x, data.y, data.z); + monitor_.tick("Bottom::Imu", "Acc"); + } + + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); + monitor_.tick("Bottom::Imu", "Gyr"); + } + + auto status() const -> std::vector { return monitor_.text(); } + + StatusMonitor monitor_{}; + }; + + auto status_service_callback(const std::shared_ptr& response) + -> void { + response->success = true; + + auto feedback_message = std::ostringstream{}; + auto text = [&](std::format_string format, Args&&... args) { + std::println(feedback_message, format, std::forward(args)...); + }; + + text(" yaw_motor_zero_point: {}", bottom_board_->gimbal_yaw_motor_.last_raw_angle()); + text(" pitch_motor_zero_point: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); + constexpr auto kPosition = + std::array{"left_front", "left_back", "right_back", "right_front"}; + + text(""); + for (auto&& [index, motor] : + std::views::zip(kPosition, bottom_board_->chassis_joint_motors_)) { + text(" {}_zero_point: {}", index, motor.last_raw_angle()); + } + + text("\nBottomBoard Status:"); + for (const auto& line : bottom_board_->status()) + text("> {}", line); + + text("\nTopBoard Status:"); + for (const auto& line : top_board_->status()) + text("> {}", line); + + response->message = feedback_message.str(); + } + + OutputInterface tf_; + OutputInterface camera_transform_; + OutputInterface barrel_direction_; + OutputInterface auto_aim_yaw_velocity_; + InputInterface timestamp_; + + std::unique_ptr bottom_board_; + std::unique_ptr top_board_; + + std::shared_ptr command_; + uint32_t cmd_tick_ = 0; + + std::shared_ptr> status_service_; +}; + +} // namespace rmcs_core::hardware + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DeformableInfantryOmniB, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp new file mode 100644 index 000000000..6e9242e28 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -0,0 +1,861 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "hardware/device/bmi088.hpp" +#include "hardware/device/bmi088_ekf.hpp" +#include "hardware/device/board_clock_lifter.hpp" +#include "hardware/device/can_packet.hpp" +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/lk_motor.hpp" +#include "hardware/device/supercap.hpp" +#include "hardware/util/status_monitor.hpp" + +namespace rmcs_core::hardware { + +using Clock = std::chrono::steady_clock; + +class DeformableInfantryOmni + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DeformableInfantryOmni() + : Node( + get_component_name(), + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) + , command_(create_partner_component(get_component_name() + "_command", *this)) { + using namespace rmcs_description; + + register_input("/predefined/timestamp", timestamp_); + register_output("/tf", tf_); + register_output( + "/auto_aim/camera_transform", camera_transform_, Eigen::Isometry3d::Identity()); + register_output("/auto_aim/barrel_direction", barrel_direction_, Eigen::Vector3d::UnitX()); + register_output("/auto_aim/yaw_velocity", auto_aim_yaw_velocity_, 0.0); + + tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); + + bottom_board_ = std::make_unique( + *this, *command_, get_parameter("serial_filter_bottom_board").as_string()); + top_board_ = std::make_unique( + *this, *command_, get_parameter("serial_filter_top_board").as_string()); + + // For command: remote-status + using Srv = std_srvs::srv::Trigger; + status_service_ = create_service( + "/rmcs/service/robot_status", + [this](const Srv::Request::SharedPtr&, const Srv::Response::SharedPtr& response) { + status_service_callback(response); + }); + } + + ~DeformableInfantryOmni() override = default; + + void before_updating() override { top_board_->request_hard_sync_read(); } + + void update() override { + bottom_board_->update(); + top_board_->update(); + + using namespace rmcs_description; + *camera_transform_ = fast_tf::lookup_transform(*tf_); + *barrel_direction_ = + *fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + *auto_aim_yaw_velocity_ = top_board_->gimbal_yaw_velocity(); + } + + void command_update() { + const bool even = ((cmd_tick_++ & 1u) == 0u); + bottom_board_->command_update(even); + top_board_->command_update(); + } + +private: + static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); + static constexpr auto kLeftFront = 0; + static constexpr auto kLeftBack = 1; + static constexpr auto kRightBack = 2; + static constexpr auto kRightFront = 3; + static constexpr auto kJointName = std::array{ + "left_front", + "left_back", + "right_back", + "right_front", + }; + + class Command : public rmcs_executor::Component { + public: + explicit Command(DeformableInfantryOmni& deformableInfantry) + : deformableInfantry(deformableInfantry) {} + + void update() override { deformableInfantry.command_update(); } + + DeformableInfantryOmni& deformableInfantry; + }; + + struct BottomBoard final : public librmcs::board::RmcsBoardLite::Callback { + public: + explicit BottomBoard( + DeformableInfantryOmni& status, rmcs_executor::Component& command, + const std::string& serial_filter = {}) + : status_{status} + , command_{command} + , kChassisRadiusBase(status.get_parameter("chassis_radius").as_double()) + , kRodLength(status.get_parameter("rod_length").as_double()) + , kDefaultRadius(kChassisRadiusBase + kRodLength) { + + status.register_output("/referee/serial", referee_serial_); + referee_serial_->read = [this](std::byte* buffer, size_t size) { + return referee_ring_buffer_receive_.pop_front_n( + [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) { + board_->start_transmit().uart_transmit( + Spec::kUarts.kUart0, {.uart_data = std::span{buffer, size}}); + return size; + }; + + gimbal_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( + static_cast(status.get_parameter("yaw_motor_zero_point").as_int()))); + + for (auto& motor : chassis_wheel_motors_) + motor.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} + .set_reduction_ratio(19.0) + .enable_multi_turn_angle()); + + for (auto& motor : chassis_joint_motors_) + motor.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei36} + .set_reversed() + .enable_multi_turn_angle()); + + imu_.set_coordinate_mapping([](double x, double y, double z) { + // Keep the existing chassis yaw axis mapping explicit until the bottom-board IMU + // installation is re-validated on hardware. + return std::make_tuple(-y, x, z); + }); + + gimbal_bullet_feeder_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM2006, 3} + .enable_multi_turn_angle()); + + status.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); + status.register_output("/chassis/imu/pitch", chassis_imu_pitch_, 0.0); + status.register_output("/chassis/imu/roll", chassis_imu_roll_, 0.0); + status.register_output("/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, 0.0); + status.register_output("/chassis/imu/roll_rate", chassis_imu_roll_rate_, 0.0); + for (size_t i = 0; i < 4; ++i) { + status.register_output( + std::format( + "/chassis/{}_joint/physical_angle", DeformableInfantryOmni::kJointName[i]), + joint_physical_angle_[i], kNaN); + status.register_output( + std::format( + "/chassis/{}_joint/physical_velocity", + DeformableInfantryOmni::kJointName[i]), + joint_physical_velocity_[i], kNaN); + } + status.register_output("/chassis/encoder/alpha", encoder_alpha_, kNaN); + status.register_output("/chassis/encoder/alpha_dot", encoder_alpha_dot_, kNaN); + status.register_output("/chassis/radius", radius_, kDefaultRadius); + + status.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); + status.get_parameter_or("debug_log_wheel_motor", debug_log_wheel_motor_, false); + status.get_parameter_or( + "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); + + auto options = librmcs::board::AdvancedOptions{}; + options.dangerously_skip_version_checks = true; + board_ = std::make_unique(*this, serial_filter, options); + } + + void update() { + imu_.update_status(); + *chassis_yaw_velocity_imu_ = imu_.gz(); + { + const double q0 = imu_.q0(); + const double q1 = imu_.q1(); + const double q2 = imu_.q2(); + const double q3 = imu_.q3(); + + double sin_pitch = 2.0 * (q0 * q2 - q3 * q1); + sin_pitch = std::clamp(sin_pitch, -1.0, 1.0); + + const double standard_pitch = std::asin(sin_pitch); + const double standard_roll = + std::atan2(2.0 * (q0 * q1 + q2 * q3), 1.0 - 2.0 * (q1 * q1 + q2 * q2)); + + // Export chassis attitude using the requested convention: + // pitch < 0 when the front is higher, roll > 0 when the left side is higher. + *chassis_imu_pitch_ = -standard_pitch; + *chassis_imu_roll_ = standard_roll; + *chassis_imu_pitch_rate_ = -imu_.gy(); + *chassis_imu_roll_rate_ = imu_.gx(); + } + + for (auto& motor : chassis_wheel_motors_) + motor.update_status(); + for (auto& motor : chassis_joint_motors_) + motor.update_status(); + + for (size_t i = 0; i < 4; ++i) + update_joint_physical_feedback_( + i, joint_physical_angle_[i], joint_physical_velocity_[i]); + + update_geometry_feedback_(); + if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) + log_chassis_feedback_once_per_second_(); + + dr16_.update_status(); + gimbal_yaw_motor_.update_status(); + if (supercap_status_received_.load(std::memory_order_relaxed)) + supercap_.update_status(); + if (debug_log_supercap_) + log_supercap_feedback_once_per_second_(); + gimbal_bullet_feeder_.update_status(); + + tf_->set_state( + gimbal_yaw_motor_.angle()); + } + + void command_update(bool even) { + auto builder = board_->start_transmit(); + if (even) { + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kLeftFront].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kLeftBack].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kRightBack].generate_command(), + device::CanPacket8::PaddingQuarter{}, + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan3, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kRightFront].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = 0x142, + .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + } + .as_bytes(), + }); + } else { + for (size_t i = 0; i < 4; ++i) { + switch (i) { + case kLeftFront: + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + case kLeftBack: + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + case kRightBack: + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + case kRightFront: + builder.can_transmit( + Spec::kCans.kCan3, // + { + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + default: break; + } + } + } + } + + static constexpr double kJointZeroPhysicalAngleRad = 62.5 * std::numbers::pi / 180.0; + + DeformableInfantryOmni& status_; + Component& command_; + + std::unique_ptr board_; + + // Interfaces + + OutputInterface& tf_{status_.tf_}; + + OutputInterface chassis_yaw_velocity_imu_; + OutputInterface chassis_imu_pitch_; + OutputInterface chassis_imu_roll_; + OutputInterface chassis_imu_pitch_rate_; + OutputInterface chassis_imu_roll_rate_; + + std::array, 4> joint_physical_angle_; + std::array, 4> joint_physical_velocity_; + + OutputInterface encoder_alpha_; + OutputInterface encoder_alpha_dot_; + OutputInterface radius_; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; + OutputInterface referee_serial_; + + // State + + std::atomic wheel_status_received_[4] = {false, false, false, false}; + std::atomic joint_status_received_[4] = {false, false, false, false}; + + bool debug_log_supercap_ = false; + bool debug_log_wheel_motor_ = false; + bool debug_log_deformable_joint_motor_ = false; + + const double kChassisRadiusBase; + const double kRodLength; + const double kDefaultRadius; + + Clock::time_point next_chassis_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; + Clock::time_point next_supercap_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; + + // Device + + device::Bmi088 imu_{1000, 0.2, 0.0}; + device::LkMotor gimbal_yaw_motor_{status_, command_, "/gimbal/yaw"}; + device::Dr16 dr16_{status_}; + + device::DjiMotor chassis_wheel_motors_[4]{ + device::DjiMotor{status_, command_, "/chassis/left_front_wheel"}, + device::DjiMotor{status_, command_, "/chassis/left_back_wheel"}, + device::DjiMotor{status_, command_, "/chassis/right_back_wheel"}, + device::DjiMotor{status_, command_, "/chassis/right_front_wheel"}, + }; + device::LkMotor chassis_joint_motors_[4]{ + device::LkMotor{status_, command_, "/chassis/left_front_joint"}, + device::LkMotor{status_, command_, "/chassis/left_back_joint"}, + device::LkMotor{status_, command_, "/chassis/right_back_joint"}, + device::LkMotor{status_, command_, "/chassis/right_front_joint"}, + }; + + std::atomic latest_supercap_status_{device::CanPacket8{uint64_t{0}}}; + std::atomic supercap_status_received_{false}; + device::Supercap supercap_{status_, command_}; + + device::DjiMotor gimbal_bullet_feeder_{status_, command_, "/gimbal/bullet_feeder"}; + + void process_chassis_can_receive_(size_t index, const View::Can& data) { + if (data.is_extended_can_id || data.is_remote_transmission) + return; + if (data.can_id == 0x201) { + chassis_wheel_motors_[index].store_status(data.can_data); + wheel_status_received_[index].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x141) { + chassis_joint_motors_[index].store_status(data.can_data); + joint_status_received_[index].store(true, std::memory_order_relaxed); + } + } + + void update_joint_physical_feedback_( + size_t index, OutputInterface& angle_output, + OutputInterface& velocity_output) { + + if (!joint_status_received_[index].load(std::memory_order_relaxed)) { + *angle_output = kNaN; + *velocity_output = kNaN; + return; + } + + const auto to_physical_angle = [](double motor_angle) { + return kJointZeroPhysicalAngleRad - motor_angle; + }; + const auto to_physical_velocity = [](double motor_velocity) { return -motor_velocity; }; + + *angle_output = to_physical_angle(chassis_joint_motors_[index].angle()); + *velocity_output = to_physical_velocity(chassis_joint_motors_[index].velocity()); + } + + void update_geometry_feedback_() { + const Eigen::Vector4d alpha_rad{ + *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], + *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront]}; + const Eigen::Vector4d alpha_dot_rad{ + *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], + *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront]}; + + if (!alpha_rad.array().isFinite().all() || !alpha_dot_rad.array().isFinite().all()) { + *encoder_alpha_ = kNaN; + *encoder_alpha_dot_ = kNaN; + *radius_ = kDefaultRadius; + RCLCPP_WARN_THROTTLE( + status_.get_logger(), *status_.get_clock(), 1000, + "deformable joint feedback invalid, fallback chassis radius to default %.3f m", + kDefaultRadius); + return; + } + + *encoder_alpha_ = alpha_rad.mean(); + *encoder_alpha_dot_ = alpha_dot_rad.mean(); + *radius_ = (kChassisRadiusBase + kRodLength * alpha_rad.array().cos()).mean(); + } + + void log_chassis_feedback_once_per_second_() { + const auto now = Clock::now(); + if (now < next_chassis_feedback_log_time_) + return; + + const auto wheel_rx = [this](size_t index) { + return wheel_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; + }; + const auto joint_rx = [this](size_t index) { + return joint_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; + }; + + if (debug_log_wheel_motor_) { + std::string wheel_rx_str; + for (size_t i = 0; i < 4; ++i) { + if (i > 0) + wheel_rx_str.push_back(' '); + wheel_rx_str.push_back(wheel_rx(i)); + } + RCLCPP_INFO( + status_.get_logger(), + "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " + "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " + "rx=[%s]", + chassis_wheel_motors_[kLeftFront].angle(), + chassis_wheel_motors_[kLeftBack].angle(), + chassis_wheel_motors_[kRightBack].angle(), + chassis_wheel_motors_[kRightFront].angle(), + chassis_wheel_motors_[kLeftFront].angle(), + chassis_wheel_motors_[kLeftBack].angle(), + chassis_wheel_motors_[kRightBack].angle(), + chassis_wheel_motors_[kRightFront].angle(), wheel_rx_str.c_str()); + } + + if (debug_log_deformable_joint_motor_) { + std::string joint_rx_str; + for (size_t i = 0; i < 4; ++i) { + if (i > 0) + joint_rx_str.push_back(' '); + joint_rx_str.push_back(joint_rx(i)); + } + RCLCPP_INFO( + status_.get_logger(), + "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " + "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " + "rx=[%s]", + *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], + *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront], + *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], + *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront], + joint_rx_str.c_str()); + } + + next_chassis_feedback_log_time_ = now + std::chrono::seconds(1); + } + + void log_supercap_feedback_once_per_second_() { + const auto now = Clock::now(); + if (now < next_supercap_feedback_log_time_) + return; + + const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); + auto supercap_raw_packet = latest_supercap_status_.load(std::memory_order_relaxed); + const auto supercap_raw_bytes = supercap_raw_packet.as_bytes(); + + RCLCPP_INFO( + status_.get_logger(), + "[supercap] can1 rx=%c id=0x300 enabled=%d supercap_v=% .3f chassis_v=% .3f " + "power=% .3f raw=[%02X %02X %02X %02X %02X %02X %02X %02X]", + supercap_rx ? 'Y' : 'N', supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, + supercap_rx ? supercap_.supercap_voltage() : kNaN, + supercap_rx ? supercap_.chassis_voltage() : kNaN, + supercap_rx ? supercap_.chassis_power() : kNaN, + std::to_integer(supercap_raw_bytes[0]), + std::to_integer(supercap_raw_bytes[1]), + std::to_integer(supercap_raw_bytes[2]), + std::to_integer(supercap_raw_bytes[3]), + std::to_integer(supercap_raw_bytes[4]), + std::to_integer(supercap_raw_bytes[5]), + std::to_integer(supercap_raw_bytes[6]), + std::to_integer(supercap_raw_bytes[7])); + + next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); + } + + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) + return; + if (can == Spec::kCans.kCan0) { + process_chassis_can_receive_(0, data); + monitor_.tick("Bottom::Can0", data.can_id); + } else if (can == Spec::kCans.kCan1) { + process_chassis_can_receive_(1, data); + if (!data.is_extended_can_id && !data.is_remote_transmission + && data.can_id == 0x300) { + if (data.can_data.size() == 8) + latest_supercap_status_.store( + device::CanPacket8{data.can_data}, std::memory_order_relaxed); + supercap_.store_status(data.can_data); + supercap_status_received_.store(true, std::memory_order_relaxed); + } + monitor_.tick("Bottom::Can1", data.can_id); + } else if (can == Spec::kCans.kCan2) { + process_chassis_can_receive_(2, data); + if (data.is_extended_can_id || data.is_remote_transmission) + return; + if (data.can_id == 0x142) + gimbal_yaw_motor_.store_status(data.can_data); + else if (data.can_id == 0x203) + gimbal_bullet_feeder_.store_status(data.can_data); + monitor_.tick("Bottom::Can2", data.can_id); + } else if (can == Spec::kCans.kCan3) { + process_chassis_can_receive_(3, data); + monitor_.tick("Bottom::Can3", data.can_id); + } + } + + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + if (uart == Spec::kUarts.kDbus) { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + monitor_.tick("Bottom::Dbus", "Active"); + } else if (uart == Spec::kUarts.kUart0) { + const std::byte* ptr = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, + data.uart_data.size()); + monitor_.tick("Bottom::Uart0", "Active"); + } + } + + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { + imu_.store_accelerometer_status(data.x, data.y, data.z); + monitor_.tick("Bottom::Imu", "Acc"); + } + + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); + monitor_.tick("Bottom::Imu", "Gyr"); + } + + auto status() const -> std::vector { return monitor_.text(); } + + StatusMonitor monitor_{}; + }; + + struct TopBoard final : public librmcs::board::RmcsBoardLite::Callback { + public: + explicit TopBoard( + DeformableInfantryOmni& status, rmcs_executor::Component& command, + const std::string& serial_filter = {}) + : tf_{status.tf_} + , bmi088_{device::Bmi088Ekf::Config{ + .body_to_sensor = + Eigen::AngleAxisd{std::numbers::pi / 2.0, Eigen::Vector3d::UnitX()} + .toRotationMatrix()}} + , gimbal_pitch_motor_(status, command, "/gimbal/pitch") + , gimbal_left_friction_(status, command, "/gimbal/left_friction") + , gimbal_right_friction_(status, command, "/gimbal/right_friction") { + + gimbal_pitch_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} + .set_reversed() + .set_encoder_zero_point( + static_cast(status.get_parameter("pitch_motor_zero_point").as_int()))); + + gimbal_left_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} + .set_reduction_ratio(1.) + .set_reversed()); + gimbal_right_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2}.set_reduction_ratio( + 1.)); + + status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); + status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); + status.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); + status.register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); + + auto options = librmcs::board::AdvancedOptions{}; + options.dangerously_skip_version_checks = true; + board_ = std::make_unique(*this, serial_filter, options); + + board_->start_transmit().gpio_digital_read( + Spec::kGpios.kUart1Rx, { + .period_ms = 0, + .asap = false, + .rising_edge = false, + .falling_edge = true, + .capture_timestamp = true, + .pull = librmcs::data::GpioPull::kUp, + }); + } + + ~TopBoard() override = default; + + [[nodiscard]] auto gimbal_yaw_velocity() const -> double { + return *gimbal_yaw_velocity_bmi088_; + } + + void request_hard_sync_read() { + // RMCS-lite top board variant currently has no GPIO hard-sync request + // path. + } + + void update() { + gimbal_pitch_motor_.update_status(); + gimbal_left_friction_.update_status(); + gimbal_right_friction_.update_status(); + + const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); + + if (auto snapshot = bmi088_.snapshot()) { + *gimbal_pitch_velocity_bmi088_ = snapshot->gyro_body.y(); + *gimbal_yaw_velocity_bmi088_ = snapshot->gyro_body.z(); + tf_->set_transform( + snapshot->orientation.conjugate()); + } + + tf_->set_state( + pitch_encoder_angle); + } + + void command_update() const { + auto builder = board_->start_transmit(); + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x141, + .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_left_friction_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + gimbal_right_friction_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + } + + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + if (can == Spec::kCans.kCan0) { + if (data.can_id == 0x141) + gimbal_pitch_motor_.store_status(data.can_data); + monitor_.tick("Top::Can0", data.can_id); + } else if (can == Spec::kCans.kCan1) { + if (data.can_id == 0x201) + gimbal_left_friction_.store_status(data.can_data); + monitor_.tick("Top::Can1", data.can_id); + } else if (can == Spec::kCans.kCan2) { + if (data.can_id == 0x202) + gimbal_right_friction_.store_status(data.can_data); + monitor_.tick("Top::Can2", data.can_id); + } + } + + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { + const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); + bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); + monitor_.tick("Top::Imu", "Acc"); + } + + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); + monitor_.tick("Top::Imu", "Gyr"); + if (!timestamp.has_value()) + return; + auto snapshot = + bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); + if (snapshot) + imu_snapshot_output_.emit(*snapshot); + } + + void gpio_digital_read_result_callback( + const Spec::Gpio& gpio, const View::GpioDigital& data) override { + if (gpio != Spec::kGpios.kUart1Rx) + return; + if (!data.timestamp_quarter_us) + return; + + const auto timestamp = board_clock_lifter_.lift_timestamp(*data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + + camera_signal_output_.emit(*timestamp); + monitor_.tick("Top::CameraSync", "Active"); + } + + auto status() const -> std::vector { return monitor_.text(); } + + OutputInterface& tf_; + OutputInterface gimbal_yaw_velocity_bmi088_; + OutputInterface gimbal_pitch_velocity_bmi088_; + + EventOutputInterface imu_snapshot_output_; + EventOutputInterface camera_signal_output_; + + device::Bmi088Ekf bmi088_; + device::BoardClockLifter board_clock_lifter_; + device::LkMotor gimbal_pitch_motor_; + device::DjiMotor gimbal_left_friction_; + device::DjiMotor gimbal_right_friction_; + + StatusMonitor monitor_{}; + std::unique_ptr board_; + }; + + auto status_service_callback(const std::shared_ptr& response) + -> void { + response->success = true; + + auto feedback_message = std::ostringstream{}; + auto text = [&](std::format_string format, Args&&... args) { + std::println(feedback_message, format, std::forward(args)...); + }; + + text(" yaw_motor_zero_point: {}", bottom_board_->gimbal_yaw_motor_.last_raw_angle()); + text(" pitch_motor_zero_point: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); + constexpr auto kPosition = + std::array{"left_front", "left_back", "right_back", "right_front"}; + + text(""); + for (auto&& [index, motor] : + std::views::zip(kPosition, bottom_board_->chassis_joint_motors_)) { + text(" {}_zero_point: {}", index, motor.last_raw_angle()); + } + + text("\nBottomBoard Status:"); + for (const auto& line : bottom_board_->status()) + text("> {}", line); + + text("\nTopBoard Status:"); + for (const auto& line : top_board_->status()) + text("> {}", line); + + response->message = feedback_message.str(); + } + + OutputInterface tf_; + OutputInterface camera_transform_; + OutputInterface barrel_direction_; + OutputInterface auto_aim_yaw_velocity_; + InputInterface timestamp_; + + std::unique_ptr bottom_board_; + std::unique_ptr top_board_; + + std::shared_ptr command_; + uint32_t cmd_tick_ = 0; + + std::shared_ptr> status_service_; +}; + +} // namespace rmcs_core::hardware + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DeformableInfantryOmni, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index df2c270e8..e0595c16b 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -42,13 +42,13 @@ class Flight device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( static_cast(get_parameter("pitch_motor_zero_point").as_int()))); gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 3} .set_reversed() .set_reduction_ratio(1.0)); gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.0)); + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 4}.set_reduction_ratio(1.0)); gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); + device::DjiMotor::Config{device::DjiMotor::Type::kM2006, 1}.enable_multi_turn_angle()); bmi088_.set_coordinate_mapping( [](double x, double y, double z) { return std::tuple{y, z, x}; }); @@ -74,8 +74,7 @@ class Flight }; referee_serial_->write = [this](const std::byte* buffer, size_t size) { board_->start_transmit().uart_transmit( - Spec::kUarts.kUart1, - {.uart_data = std::span{buffer, size}}); + Spec::kUarts.kUart1, {.uart_data = std::span{buffer, size}}); return size; }; @@ -106,33 +105,37 @@ class Flight auto builder = board_->start_transmit(); builder .can_transmit( - Spec::kCans.kCan0, - {.can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - gimbal_left_friction_.generate_command(), - gimbal_right_friction_.generate_command(), - } - .as_bytes()}) + Spec::kCans.kCan0, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + gimbal_left_friction_.generate_command(), + gimbal_right_friction_.generate_command(), + } + .as_bytes(), + }) .can_transmit( - Spec::kCans.kCan1, - {.can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes()}) + Spec::kCans.kCan1, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }) .can_transmit( - Spec::kCans.kCan2, + Spec::kCans.kCan2, // {.can_id = 0x141, .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes()}) .can_transmit( - Spec::kCans.kCan3, + Spec::kCans.kCan3, // {.can_id = 0x142, .can_data = gimbal_pitch_motor_.generate_command().as_bytes()}); } diff --git a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp index a678b9ae2..62420a43b 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp @@ -57,7 +57,7 @@ class OmniInfantry for (auto& motor : chassis_wheel_motors_) motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} .set_reversed() .set_reduction_ratio(13.) .enable_multi_turn_angle()); @@ -74,13 +74,13 @@ class OmniInfantry static_cast(get_parameter("pitch_motor_zero_point").as_int()))); gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1}.set_reduction_ratio(1.)); gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2} .set_reversed() .set_reduction_ratio(1.)); gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); + device::DjiMotor::Config{device::DjiMotor::Type::kM2006, 2}.enable_multi_turn_angle()); register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); @@ -92,15 +92,14 @@ class OmniInfantry *this, get_parameter("board_serial").as_string()); board_->start_transmit().gpio_digital_read( - Spec::kGpios.kUart0Tx, - { - .period_ms = 0, - .asap = false, - .rising_edge = false, - .falling_edge = true, - .capture_timestamp = true, - .pull = librmcs::data::GpioPull::kUp, - }); + Spec::kGpios.kUart0Tx, { + .period_ms = 0, + .asap = false, + .rising_edge = false, + .falling_edge = true, + .capture_timestamp = true, + .pull = librmcs::data::GpioPull::kUp, + }); using namespace rmcs_description; // NOLINT(google-build-using-namespace) tf_->set_transform(Eigen::Translation3d{0.06603, 0.0, 0.082}); @@ -130,8 +129,7 @@ class OmniInfantry }; referee_serial_->write = [this](const std::byte* buffer, size_t size) { board_->start_transmit().uart_transmit( - Spec::kUarts.kUart1, - {.uart_data = std::span{buffer, size}}); + Spec::kUarts.kUart1, {.uart_data = std::span{buffer, size}}); return size; }; } @@ -154,50 +152,55 @@ class OmniInfantry auto builder = board_->start_transmit(); builder.can_transmit( - Spec::kCans.kCan1, - {.can_id = 0x1FE, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - } - .as_bytes()}); + Spec::kCans.kCan1, // + { + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + } + .as_bytes(), + }); builder.can_transmit( - Spec::kCans.kCan1, - {.can_id = 0x145, - .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes()}); + Spec::kCans.kCan1, // + {.can_id = 0x145, .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes()}); builder.can_transmit( - Spec::kCans.kCan1, - {.can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), - chassis_wheel_motors_[1].generate_command(), - chassis_wheel_motors_[2].generate_command(), - chassis_wheel_motors_[3].generate_command(), - } - .as_bytes()}); + Spec::kCans.kCan1, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[0].generate_command(), + chassis_wheel_motors_[1].generate_command(), + chassis_wheel_motors_[2].generate_command(), + chassis_wheel_motors_[3].generate_command(), + } + .as_bytes(), + }); builder.can_transmit( - Spec::kCans.kCan2, - {.can_id = 0x142, + Spec::kCans.kCan2, // + {.can_id = 0x142, .can_data = gimbal_pitch_motor_.generate_velocity_command().as_bytes()}); builder.can_transmit( - Spec::kCans.kCan2, - {.can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - gimbal_left_friction_.generate_command(), - gimbal_right_friction_.generate_command(), - } - .as_bytes()}); + Spec::kCans.kCan2, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + gimbal_bullet_feeder_.generate_command(), + gimbal_left_friction_.generate_command(), + gimbal_right_friction_.generate_command(), + } + .as_bytes(), + }); } private: diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp index cdc0e6630..f20588c23 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp @@ -13,8 +13,7 @@ #include #include -#include -#include +#include #include #include #include @@ -216,15 +215,11 @@ class SteeringHeroLittle }; std::shared_ptr command_component_; - class TopBoard final : private librmcs::agent::RmcsBoardLite { - friend class SteeringHeroLittle; - - public: + struct TopBoard final : librmcs::board::RmcsBoardLite::Callback { explicit TopBoard( SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, std::string_view board_serial = {}) - : librmcs::agent::RmcsBoardLite(board_serial) - , logger_(steering_hero.get_logger()) + : logger_(steering_hero.get_logger()) , tf_(steering_hero.tf_) , bmi088_{device::Bmi088Ekf::Config{ .body_to_sensor = @@ -258,23 +253,25 @@ class SteeringHeroLittle steering_hero.get_parameter("pitch_motor_zero_point").as_int())) .enable_multi_turn_angle()); gimbal_friction_wheels_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} .set_reversed() .set_reduction_ratio(1.)); gimbal_friction_wheels_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2} .set_reversed() .set_reduction_ratio(1.)); gimbal_friction_wheels_[2].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 3}.set_reduction_ratio( + 1.)); gimbal_friction_wheels_[3].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 4}.set_reduction_ratio( + 1.)); gimbal_friction_wheels_[4].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} .set_reversed() .set_reduction_ratio(1.)); gimbal_friction_wheels_[5].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2} .set_reversed() .set_reduction_ratio(1.)); gimbal_bullet_feeder_.configure( @@ -285,10 +282,11 @@ class SteeringHeroLittle .set_reversed() .enable_multi_turn_angle()); putter_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 3} .set_reduction_ratio(1.) .enable_multi_turn_angle()); - gimbal_scope_motor_.configure(device::DjiMotor::Config{device::DjiMotor::Type::kM2006}); + gimbal_scope_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM2006, 4}); gimbal_player_viewer_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} .set_encoder_zero_point( @@ -308,6 +306,8 @@ class SteeringHeroLittle "/gimbal/photoelectric_sensor", photoelectric_sensor_status_, false); steering_hero.register_output( "/gimbal/grayscale_sensor", grayscale_sensor_status_, false); + + board_ = std::make_unique(*this, board_serial); } void update() { @@ -362,130 +362,131 @@ class SteeringHeroLittle } void command_update() { - auto builder = start_transmit(); + auto builder = board_->start_transmit(); if (std::isfinite(gimbal_pitch_motor_.control_angle())) - builder.can0_transmit({ - .can_id = 0x143, - .can_data = gimbal_pitch_motor_ - .generate_angle_command(gimbal_pitch_motor_.control_angle()) - .as_bytes(), - }); + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x143, + .can_data = gimbal_pitch_motor_ + .generate_angle_command(gimbal_pitch_motor_.control_angle()) + .as_bytes(), + }); else - builder.can0_transmit({ - .can_id = 0x143, - .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), - }); // Used to distinguish pitch encoder control from IMU control. - - builder.can0_transmit({ - .can_id = 0x141, - .can_data = gimbal_top_yaw_motor_.generate_command().as_bytes(), - }); + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x143, + .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), + }); // Used to distinguish pitch encoder control from IMU control. + + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x141, + .can_data = gimbal_top_yaw_motor_.generate_command().as_bytes(), + }); - builder.can0_transmit({ - .can_id = 0x142, - .can_data = gimbal_bullet_feeder_.generate_torque_command().as_bytes(), - }); + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x142, + .can_data = gimbal_bullet_feeder_.generate_torque_command().as_bytes(), + }); - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_friction_wheels_[0].generate_command(), - gimbal_friction_wheels_[1].generate_command(), - gimbal_friction_wheels_[2].generate_command(), - gimbal_friction_wheels_[3].generate_command(), - } - .as_bytes(), - }); + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_friction_wheels_[0].generate_command(), + gimbal_friction_wheels_[1].generate_command(), + gimbal_friction_wheels_[2].generate_command(), + gimbal_friction_wheels_[3].generate_command(), + } + .as_bytes(), + }); - builder.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_friction_wheels_[4].generate_command(), - gimbal_friction_wheels_[5].generate_command(), - putter_motor_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_friction_wheels_[4].generate_command(), + gimbal_friction_wheels_[5].generate_command(), + putter_motor_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); builder .gpio_digital_read( - librmcs::spec::rmcs_board_lite::kGpioDescriptors.kUart1Rx, + Spec::kGpios.kUart1Rx, { .period_ms = 20, .pull = librmcs::data::GpioPull::kUp, }) .gpio_digital_read( - librmcs::spec::rmcs_board_lite::kGpioDescriptors.kUart1Tx, - { - .period_ms = 0, - .asap = false, - .rising_edge = false, - .falling_edge = true, - .capture_timestamp = true, - .pull = librmcs::data::GpioPull::kUp, - }); - } - - private: - void can0_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can0_receive_rate_counter_.record(can_id); - can0_detect[can_id - 0x141] = 1; - if (can_id == 0x141) { - gimbal_top_yaw_motor_.store_status(data.can_data); - } else if (can_id == 0x143) { - gimbal_pitch_motor_.store_status(data.can_data); - } else if (can_id == 0x142) { - gimbal_bullet_feeder_.store_status(data.can_data); - } - } - - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can1_receive_rate_counter_.record(can_id); - friciton_detect[can_id - 0x201] = 1; - if (can_id == 0x201) { - gimbal_friction_wheels_[0].store_status(data.can_data); - } else if (can_id == 0x202) { - gimbal_friction_wheels_[1].store_status(data.can_data); - } else if (can_id == 0x203) { - gimbal_friction_wheels_[2].store_status(data.can_data); - } else if (can_id == 0x204) { - gimbal_friction_wheels_[3].store_status(data.can_data); - } + Spec::kGpios.kUart1Tx, { + .period_ms = 0, + .asap = false, + .rising_edge = false, + .falling_edge = true, + .capture_timestamp = true, + .pull = librmcs::data::GpioPull::kUp, + }); } - void can2_receive_callback(const librmcs::data::CanDataView& data) override { + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; auto can_id = data.can_id; - // can2_receive_rate_counter_.record(can_id); - if (can_id == 0x203) { - putter_motor_.store_status(data.can_data); - } else if (can_id == 0x201) { - gimbal_friction_wheels_[4].store_status(data.can_data); - friciton_detect[4] = 1; - } else if (can_id == 0x202) { - gimbal_friction_wheels_[5].store_status(data.can_data); - friciton_detect[5] = 1; + if (can == Spec::kCans.kCan0) { + // can0_receive_rate_counter_.record(can_id); + can0_detect[can_id - 0x141] = 1; + if (can_id == 0x141) { + gimbal_top_yaw_motor_.store_status(data.can_data); + } else if (can_id == 0x143) { + gimbal_pitch_motor_.store_status(data.can_data); + } else if (can_id == 0x142) { + gimbal_bullet_feeder_.store_status(data.can_data); + } + } else if (can == Spec::kCans.kCan1) { + // can1_receive_rate_counter_.record(can_id); + friciton_detect[can_id - 0x201] = 1; + if (can_id == 0x201) { + gimbal_friction_wheels_[0].store_status(data.can_data); + } else if (can_id == 0x202) { + gimbal_friction_wheels_[1].store_status(data.can_data); + } else if (can_id == 0x203) { + gimbal_friction_wheels_[2].store_status(data.can_data); + } else if (can_id == 0x204) { + gimbal_friction_wheels_[3].store_status(data.can_data); + } + } else if (can == Spec::kCans.kCan2) { + // can2_receive_rate_counter_.record(can_id); + if (can_id == 0x203) { + putter_motor_.store_status(data.can_data); + } else if (can_id == 0x201) { + gimbal_friction_wheels_[4].store_status(data.can_data); + friciton_detect[4] = 1; + } else if (can_id == 0x202) { + gimbal_friction_wheels_[5].store_status(data.can_data); + friciton_detect[5] = 1; + } } } void gpio_digital_read_result_callback( - const librmcs::spec::rmcs_board_lite::GpioDescriptor& gpio, - const librmcs::data::GpioDigitalDataView& data) override { + const Spec::Gpio& gpio, const View::GpioDigital& data) override { - /* */ if (gpio == librmcs::spec::rmcs_board_lite::kGpioDescriptors.kUart1Rx) { + /* */ if (gpio == Spec::kGpios.kUart1Rx) { photoelectric_sensor_status_atomic.store(data.high); - } else if (gpio == librmcs::spec::rmcs_board_lite::kGpioDescriptors.kUart1Tx) { + } else if (gpio == Spec::kGpios.kUart1Tx) { if (!data.timestamp_quarter_us) return; const auto timestamp = @@ -496,13 +497,12 @@ class SteeringHeroLittle } } - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); } - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); if (!timestamp.has_value()) return; @@ -545,18 +545,15 @@ class SteeringHeroLittle EventOutputInterface imu_snapshot_output_; std::atomic photoelectric_sensor_status_atomic{false}; std::atomic grayscale_sensor_status_atomic{false}; - }; - class BottomBoard final : private librmcs::agent::RmcsBoardLite { - friend class SteeringHeroLittle; + std::unique_ptr board_; + }; - public: + struct BottomBoard final : librmcs::board::RmcsBoardLite::Callback { explicit BottomBoard( SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, std::string_view board_serial = {}) - : librmcs::agent::RmcsBoardLite( - board_serial, {.dangerously_skip_version_checks = false}) - , logger_(steering_hero.get_logger()) + : logger_(steering_hero.get_logger()) // , can0_receive_rate_counter_(logger_, "bottom/can0") // , can1_receive_rate_counter_(logger_, "bottom/can1") // , can2_receive_rate_counter_(logger_, "bottom/can2") @@ -584,64 +581,66 @@ class SteeringHeroLittle , gimbal_bottom_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/bottom_yaw") { // chassis_steering_motors_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020, 4} .set_encoder_zero_point( static_cast( steering_hero.get_parameter("left_front_zero_point").as_int())) .set_reversed()); chassis_steering_motors_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020, 1} .set_encoder_zero_point( static_cast( steering_hero.get_parameter("right_front_zero_point").as_int())) .set_reversed()); chassis_steering_motors_[2].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020, 3} .set_encoder_zero_point( static_cast( steering_hero.get_parameter("left_back_zero_point").as_int())) .set_reversed()); chassis_steering_motors_[3].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020, 2} .set_encoder_zero_point( static_cast( steering_hero.get_parameter("right_back_zero_point").as_int())) .set_reversed()); chassis_wheel_motors_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 4} .set_reversed() .set_reduction_ratio(2232. / 169.)); chassis_wheel_motors_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} .set_reversed() .set_reduction_ratio(2232. / 169.)); chassis_wheel_motors_[2].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 3} .set_reversed() .set_reduction_ratio(2232. / 169.)); chassis_wheel_motors_[3].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 4} .set_reversed() .set_reduction_ratio(2232. / 169.)); chassis_front_climber_motor_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} .set_reversed() .set_reduction_ratio(19.)); chassis_front_climber_motor_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2}.set_reduction_ratio( + 19.)); chassis_back_climber_motor_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2} .enable_multi_turn_angle() .set_reduction_ratio(19.)); chassis_back_climber_motor_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 4} .set_reversed() .enable_multi_turn_angle() .set_reduction_ratio(19.)); yaw_brake_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.set_reduction_ratio(1.)); + device::DjiMotor::Config{device::DjiMotor::Type::kM2006, 3}.set_reduction_ratio( + 1.)); gimbal_bottom_yaw_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} .set_reversed() @@ -656,8 +655,8 @@ class SteeringHeroLittle [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); }; referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart0_transmit( - {.uart_data = std::span{buffer, size}}); + board_->start_transmit().uart_transmit( + Spec::kUarts.kUart0, {.uart_data = std::span{buffer, size}}); return size; }; steering_hero.register_output( @@ -668,6 +667,8 @@ class SteeringHeroLittle steering_hero.register_output( "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); steering_hero.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); + + board_ = std::make_unique(*this, board_serial); } void update() { @@ -708,169 +709,168 @@ class SteeringHeroLittle } void command_update() { - auto builder = start_transmit(); - - builder.can0_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), - chassis_wheel_motors_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - - builder.can0_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steering_motors_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - chassis_steering_motors_[0].generate_command(), - } - .as_bytes(), - }); + auto builder = board_->start_transmit(); + + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[0].generate_command(), + chassis_wheel_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - chassis_wheel_motors_[2].generate_command(), - chassis_wheel_motors_[3].generate_command(), - } - .as_bytes(), - }); + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + chassis_steering_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + chassis_steering_motors_[0].generate_command(), + } + .as_bytes(), + }); - builder.can1_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - chassis_steering_motors_[3].generate_command(), - chassis_steering_motors_[2].generate_command(), - supercap_.generate_command(), - } - .as_bytes(), - }); + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + chassis_wheel_motors_[2].generate_command(), + chassis_wheel_motors_[3].generate_command(), + } + .as_bytes(), + }); - builder.can3_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - chassis_back_climber_motor_[1].generate_command(), - yaw_brake_motor_.generate_command(), - chassis_back_climber_motor_[0].generate_command(), - } - .as_bytes(), - }); + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + chassis_steering_motors_[3].generate_command(), + chassis_steering_motors_[2].generate_command(), + supercap_.generate_command(), + } + .as_bytes(), + }); - builder.can3_transmit({ - .can_id = 0x141, - .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), - }); + builder.can_transmit( + Spec::kCans.kCan3, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + chassis_back_climber_motor_[1].generate_command(), + yaw_brake_motor_.generate_command(), + chassis_back_climber_motor_[0].generate_command(), + } + .as_bytes(), + }); - builder.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_front_climber_motor_[0].generate_command(), - device::CanPacket8::PaddingQuarter{}, - chassis_front_climber_motor_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - } + builder.can_transmit( + Spec::kCans.kCan3, // + { + .can_id = 0x141, + .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), + }); - private: - void can0_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can0_receive_rate_counter_.record(can_id); - check[can_id - 0x201] = 1; - if (can_id == 0x201) { - chassis_wheel_motors_[0].store_status(data.can_data); - } else if (can_id == 0x202) { - chassis_wheel_motors_[1].store_status(data.can_data); - } else if (can_id == 0x205) { - chassis_steering_motors_[1].store_status(data.can_data); - } else if (can_id == 0x208) { - chassis_steering_motors_[0].store_status(data.can_data); - } + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_front_climber_motor_[0].generate_command(), + device::CanPacket8::PaddingQuarter{}, + chassis_front_climber_motor_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); } - void can1_receive_callback(const librmcs::data::CanDataView& data) override { + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; auto can_id = data.can_id; - // can1_receive_rate_counter_.record(can_id); - if (can_id != 0x300) { + if (can == Spec::kCans.kCan0) { + // can0_receive_rate_counter_.record(can_id); check[can_id - 0x201] = 1; - } - if (can_id == 0x203) { - chassis_wheel_motors_[2].store_status(data.can_data); - } else if (can_id == 0x204) { - chassis_wheel_motors_[3].store_status(data.can_data); - } else if (can_id == 0x207) { - chassis_steering_motors_[2].store_status(data.can_data); - } else if (can_id == 0x206) { - chassis_steering_motors_[3].store_status(data.can_data); - } else if (can_id == 0x300) { - supercap_.store_status(data.can_data); - } - } - - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - auto can_id = data.can_id; - // can2_receive_rate_counter_.record(can_id); - if (can_id == 0x201) { - chassis_front_climber_motor_[0].store_status(data.can_data); - } else if (can_id == 0x203) { - chassis_front_climber_motor_[1].store_status(data.can_data); + if (can_id == 0x201) { + chassis_wheel_motors_[0].store_status(data.can_data); + } else if (can_id == 0x202) { + chassis_wheel_motors_[1].store_status(data.can_data); + } else if (can_id == 0x205) { + chassis_steering_motors_[1].store_status(data.can_data); + } else if (can_id == 0x208) { + chassis_steering_motors_[0].store_status(data.can_data); + } + } else if (can == Spec::kCans.kCan1) { + // can1_receive_rate_counter_.record(can_id); + if (can_id != 0x300) + check[can_id - 0x201] = 1; + if (can_id == 0x203) { + chassis_wheel_motors_[2].store_status(data.can_data); + } else if (can_id == 0x204) { + chassis_wheel_motors_[3].store_status(data.can_data); + } else if (can_id == 0x207) { + chassis_steering_motors_[2].store_status(data.can_data); + } else if (can_id == 0x206) { + chassis_steering_motors_[3].store_status(data.can_data); + } else if (can_id == 0x300) { + supercap_.store_status(data.can_data); + } + } else if (can == Spec::kCans.kCan2) { + // can2_receive_rate_counter_.record(can_id); + if (can_id == 0x201) { + chassis_front_climber_motor_[0].store_status(data.can_data); + } else if (can_id == 0x203) { + chassis_front_climber_motor_[1].store_status(data.can_data); + } + } else if (can == Spec::kCans.kCan3) { + // can3_receive_rate_counter_.record(can_id); + if (can_id == 0x202) { + chassis_back_climber_motor_[1].store_status(data.can_data); + } else if (can_id == 0x204) { + chassis_back_climber_motor_[0].store_status(data.can_data); + } else if (can_id == 0x203) { + yaw_brake_motor_.store_status(data.can_data); + } else if (can_id == 0x141) { + gimbal_bottom_yaw_motor_.store_status(data.can_data); + } } } - void can3_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - // can3_receive_rate_counter_.record(can_id); - if (can_id == 0x202) { - chassis_back_climber_motor_[1].store_status(data.can_data); - } else if (can_id == 0x204) { - chassis_back_climber_motor_[0].store_status(data.can_data); - } else if (can_id == 0x203) { - yaw_brake_motor_.store_status(data.can_data); - } else if (can_id == 0x141) { - gimbal_bottom_yaw_motor_.store_status(data.can_data); + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + if (uart == Spec::kUarts.kUart0) { + const std::byte* ptr = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, + data.uart_data.size()); + } else if (uart == Spec::kUarts.kDbus) { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); } } - void uart0_receive_callback(const librmcs::data::UartDataView& data) override { - const std::byte* ptr = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, data.uart_data.size()); - } - - void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { imu_.store_accelerometer_status(data.x, data.y, data.z); } - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { imu_.store_gyroscope_status(data.x, data.y, data.z); } @@ -901,6 +901,8 @@ class SteeringHeroLittle OutputInterface powermeter_charge_power_limit_; OutputInterface chassis_yaw_velocity_imu_; OutputInterface chassis_pitch_imu_; + + std::unique_ptr board_; }; OutputInterface tf_; From e5772c204caa005d07bbabfa36f8c77657ea8c71 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sat, 18 Jul 2026 10:37:57 +0800 Subject: [PATCH 35/86] fix: Correct hero friction order --- .../controller/shooting/hero_friction_wheel_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_friction_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_friction_wheel_controller.cpp index f434c5ec1..1656d2276 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_friction_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_friction_wheel_controller.cpp @@ -193,7 +193,7 @@ class HeroFrictionWheelController bool detect_bullet_fire() { bool fired = false; if (!std::isnan(last_primary_friction_velocity_)) { - double differential = *friction_velocities_[1] - last_primary_friction_velocity_; + double differential = *friction_velocities_[0] - last_primary_friction_velocity_; if (differential < 0.1) primary_friction_velocity_decrease_integral_ += differential; else { @@ -204,7 +204,7 @@ class HeroFrictionWheelController primary_friction_velocity_decrease_integral_ = 0; } } - last_primary_friction_velocity_ = *friction_velocities_[1]; + last_primary_friction_velocity_ = *friction_velocities_[0]; return fired; } From c6793a3879db50d55c4cb64f68e8e9804c808127 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sat, 18 Jul 2026 11:20:01 +0800 Subject: [PATCH 36/86] fix(hero): Update auto-aim config and should_control gating --- .../steering-hero-little-six-friction.yaml | 24 ++++++++++++++++--- .../gimbal/hero_gimbal_controller.cpp | 11 +++++++-- 2 files changed, 30 insertions(+), 5 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml index 67928053d..1f4534803 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml @@ -77,16 +77,34 @@ auto_aim_capturer: invert_image: false rls_tau_sec: 10.0 use_hardware_sync: false + delay_ms: 6.5 auto_aim_component: ros__parameters: + # WARN: 危险！可选 red | blue，生效后裁判系统 ID 缺席时 + # 将强行绑定为对应阵营哨兵，仅供调试，严禁比赛启用。 + # 留空或填 unknow 表示禁用。 + dangerous_fallback: "" manual_shoot: false + camera_translation: [0.25, 0.0, -0.05] + fire_control: + bullet_speed: 11.5 + shoot_delay: 0.1 + offset_yaw: 0.0 + offset_pitch: 0.0 + attack_window: 120.0 + window_hysteresis: 0.2 + is_lazy_gimbal: false + attack_preaim: false + require_stable_command: false + yaw_tolerance: 0.07 + pitch_tolerance: 0.04 auto_aim_ui: ros__parameters: - offset_x: 0.000 - offset_y: 0.000 - offset_z: 0.000 + offset_x: 0.0 + offset_y: 0.0 + offset_z: 0.0 value_broadcaster: ros__parameters: diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp index 03deb0bef..f3f8d1310 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp @@ -33,6 +33,7 @@ class HeroGimbalController register_input("/remote/mouse", mouse_); register_input("/remote/keyboard", keyboard_); + register_input("/auto_aim/should_control", auto_aim_should_control_, false); register_input("/auto_aim/control_direction", auto_aim_control_direction_, false); register_input("/tf", tf_); @@ -114,8 +115,13 @@ class HeroGimbalController } TwoAxisGimbalSolver::AngleError update_imu_control() { - if (auto_aim_control_direction_.ready() && !auto_aim_control_direction_->isZero() - && (mouse_->right || *switch_right_ == rmcs_msgs::Switch::UP)) { + const auto auto_aim_requested = mouse_->right || *switch_right_ == rmcs_msgs::Switch::UP; + const auto should_control = auto_aim_should_control_.ready() && *auto_aim_should_control_; + const auto valid_control = auto_aim_control_direction_.ready() + && auto_aim_control_direction_->allFinite() + && !auto_aim_control_direction_->isZero(); + + if (auto_aim_requested && should_control && valid_control) { return imu_gimbal_solver_.update( TwoAxisGimbalSolver::SetControlDirection{ OdomImu::DirectionVector{*auto_aim_control_direction_}}); @@ -182,6 +188,7 @@ class HeroGimbalController rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + InputInterface auto_aim_should_control_; InputInterface auto_aim_control_direction_; InputInterface tf_; From c2b74e8ad1665cdd3782bdef9142048033230601 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sat, 18 Jul 2026 17:29:59 +0800 Subject: [PATCH 37/86] merge(robot): Merge sentry impl into this branch --- .script/host/rmcs | 14 +- .script/local-context | 17 +- .script/scan-remote | 249 ++----- rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 243 +++++++ rmcs_ws/src/rmcs_core/plugins.xml | 1 + .../chassis/chassis_climber_controller.cpp | 10 +- .../controller/chassis/chassis_controller.cpp | 168 ++--- .../chassis/chassis_power_controller.cpp | 9 +- .../controller/chassis/deformable_chassis.cpp | 2 +- .../controller/chassis/deformable_mode.hpp | 8 +- .../chassis/hero_chassis_controller.cpp | 1 - .../controller/gimbal/eccentric_dual_yaw.cpp | 122 ++-- .../gimbal/eccentric_dual_yaw_solver.hpp | 5 - rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp | 636 ++++++++++++++++++ .../referee/app/ui/deformable_infantry_ui.cpp | 2 +- .../src/rmcs_core/src/referee/app/ui/hero.cpp | 4 +- .../rmcs_core/src/referee/app/ui/infantry.cpp | 2 +- .../include/rmcs_msgs/chassis_mode.hpp | 9 +- .../rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp | 2 - 19 files changed, 1061 insertions(+), 443 deletions(-) create mode 100644 rmcs_ws/src/rmcs_bringup/config/sentry.yaml create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp diff --git a/.script/host/rmcs b/.script/host/rmcs index 8d8696d9f..918b209f7 100755 --- a/.script/host/rmcs +++ b/.script/host/rmcs @@ -11,7 +11,7 @@ readonly RMCS_PATH="/workspaces/RMCS/" function show_help() { local project_dir="$1" local service="$2" - echo "Usage: $(basename "$0") [path] [zsh|recreate|n|nvim|neovide|vim|ide|ai]" + echo "Usage: $(basename "$0") [path] [zsh|recreate|n|nvim|neovide|vim|ide]" echo " Project dir: $project_dir" echo " Service: $service" } @@ -70,15 +70,6 @@ function rmcs_recreate() { docker compose up -d --force-recreate } -function rmcs_ai() { - local service="$1" - local agent="${RMCS_AGENT:-opencode}" - echo "Starting container and launching Agent ($agent)..." - setup_container - docker compose exec -u "$DEVELOPER_NAME" -w "$RMCS_PATH" "$service" \ - zsh -ic "exec ${agent}" -} - function main() { local project_dir command @@ -109,9 +100,6 @@ function main() { n | nvim | neovide | vim | ide) rmcs_nvim "$service" ;; - ai) - rmcs_ai "$service" - ;; *) show_help "$project_dir" "$service" ;; diff --git a/.script/local-context b/.script/local-context index 7ffa214b6..af72cd280 100755 --- a/.script/local-context +++ b/.script/local-context @@ -2,7 +2,7 @@ set -euo pipefail -BASE="/tmp/rmcs-navigation/context" +TOPIC="/rmcs_navigation/context/mock" usage() { cat <<'EOF' @@ -73,17 +73,12 @@ else fi fi -fifo="${BASE}/${key}" -if [[ ! -p "${fifo}" ]]; then - echo "Context FIFO not found: ${fifo}" >&2 - echo "Start rmcs-navigation first so /tmp/rmcs-navigation/context/ is created." >&2 - exit 1 -fi - +payload="${key}: ${value}" echo "[local-context] key: ${key}" echo "[local-context] value: ${value}" -echo "[local-context] fifo: ${fifo}" +echo "[local-context] topic: ${TOPIC}" +echo "[local-context] payload(to publish): ${payload}" -printf '%s' "${value}" >"${fifo}" +ros2 topic pub -1 "$TOPIC" std_msgs/msg/String "{data: '${payload}'}" -echo "Wrote ${key}=${value} to ${fifo}" +echo "Published ${key}=${value} to local topic ${TOPIC}" diff --git a/.script/scan-remote b/.script/scan-remote index bb2483e8a..e26bc9cdc 100755 --- a/.script/scan-remote +++ b/.script/scan-remote @@ -6,6 +6,7 @@ import json import os import re import select +import shutil import socket import subprocess import sys @@ -18,10 +19,10 @@ from colorama import Fore, Style SSH_PORT = 2022 SSH_USER = "root" -CONNECT_TIMEOUT = 1.0 +CONNECT_TIMEOUT = 0.5 BANNER_TIMEOUT = 0.35 SSH_PROBE_TIMEOUT = 2.0 -DEFAULT_192_168_SEGMENTS = range(1, 11) +DEFAULT_SCAN_SEGMENTS = range(1, 6) SKIP_PREFIXES = ("lo", "docker", "br-", "veth", "zt", "tailscale") SPINNER_FRAMES = "⠋⠙⠹⠸⠼⠴⠦⠧⠇⠏" REFRESH_INTERVAL = 0.08 @@ -48,47 +49,41 @@ class RawTerminal: def __exit__(self, *_): termios.tcsetattr(self._fd, termios.TCSANOW, self._old) + _SIMPLE_KEYS = { + b"\r": "enter", b"\n": "enter", + b"\t": "next", b"s": "next", b"S": "next", b"\x0e": "next", + b"w": "prev", b"W": "prev", b"\x10": "prev", + b"\x7f": "backspace", b"\x08": "backspace", + } + _ESCAPE_DIRS = {b"A": "prev", b"B": "next", b"Z": "prev"} + @staticmethod def read_key(): + if not RawTerminal.key_ready(0.02): + return None raw = sys.stdin.buffer.raw.read(1) - if raw == b"\x03": - raise KeyboardInterrupt - if raw in (b"\r", b"\n"): - return "enter" - if raw == b"\t": - return "next" - if raw in (b"w", b"W"): - return "prev" - if raw in (b"s", b"S"): - return "next" - if raw == b"\x0e": - return "next" - if raw == b"\x10": - return "prev" - if raw in (b"\x7f", b"\x08"): - return "backspace" - if raw == b"\x1b": - if not RawTerminal.key_ready(0.02): - return "quit" - nxt = sys.stdin.buffer.raw.read(1) - if nxt != b"[": - return "escape" - if not RawTerminal.key_ready(0.02): - return "escape" - direction = sys.stdin.buffer.raw.read(1) - if direction == b"A": - return "prev" - if direction == b"B": - return "next" - if direction == b"Z": - return "prev" - return "escape" if raw in (b"q", b"Q"): - return "quit" + os._exit(130) + if raw in RawTerminal._SIMPLE_KEYS: + return RawTerminal._SIMPLE_KEYS[raw] + if raw == b"\x1b": + return RawTerminal._read_escape() if len(raw) == 1 and 32 <= raw[0] <= 126: return raw.decode() return None + @staticmethod + def _read_escape(): + if not RawTerminal.key_ready(0.02): + os._exit(130) + nxt = sys.stdin.buffer.raw.read(1) + if nxt != b"[" or not RawTerminal.key_ready(0.02): + os._exit(130) + direction = sys.stdin.buffer.raw.read(1) + if direction in RawTerminal._ESCAPE_DIRS: + return RawTerminal._ESCAPE_DIRS[direction] + os._exit(130) + @staticmethod def key_ready(timeout): ready, _, _ = select.select([sys.stdin], [], [], timeout) @@ -188,13 +183,11 @@ def default_networks(): def expanded_networks(interface): ip = interface.ip - if ip.packed[0] == 192 and ip.packed[1] == 168: - return [ - ipaddress.ip_network(f"192.168.{segment}.0/24") - for segment in DEFAULT_192_168_SEGMENTS - ] - - return [ipaddress.ip_network(f"{ip}/24", strict=False)] + prefix = ".".join(str(part) for part in ip.packed[:2]) + return [ + ipaddress.ip_network(f"{prefix}.{segment}.0/24") + for segment in DEFAULT_SCAN_SEGMENTS + ] def dedupe_networks(networks): @@ -399,8 +392,8 @@ def scan_all_networks_until_selected( ) selected_result = None - - with concurrent.futures.ThreadPoolExecutor(max_workers=max_workers) as pool: + pool = concurrent.futures.ThreadPoolExecutor(max_workers=max_workers) + try: future_to_network = { pool.submit(probe_host, host): network for network, host in iter_interleaved_targets(network_targets) @@ -408,7 +401,7 @@ def scan_all_networks_until_selected( pending = set(future_to_network) while pending: - if on_key is not None and RawTerminal.key_ready(0.02): + if on_key is not None: key = RawTerminal.read_key() if key is not None: selected_result = on_key(key) @@ -446,6 +439,8 @@ def scan_all_networks_until_selected( if selected_result is not None: pool.shutdown(wait=False, cancel_futures=True) return sort_results(results), selected_result + finally: + pool.shutdown(wait=False, cancel_futures=True) return sort_results(results), None @@ -468,35 +463,6 @@ def iter_interleaved_targets(network_targets): pending = next_pending -def scan_network(network, on_progress=None, on_found=None): - targets = host_candidates([network]) - max_workers = min(128, max(8, len(targets))) - scanned = 0 - found = 0 - results = [] - - if on_progress is not None: - on_progress(network, scanned, len(targets), found, False) - - with concurrent.futures.ThreadPoolExecutor(max_workers=max_workers) as pool: - futures = [pool.submit(probe_host, host) for host in targets] - for future in concurrent.futures.as_completed(futures): - scanned += 1 - result = future.result() - if result is not None: - found += 1 - results.append(result) - if on_found is not None: - on_found(network, result) - if on_progress is not None: - on_progress(network, scanned, len(targets), found, False) - - if on_progress is not None: - on_progress(network, scanned, len(targets), found, True) - - return sort_results(results) - - def sort_results(results): return sorted( results, @@ -526,6 +492,7 @@ class ProgressView: self._thread = None self._selection = None self._prompt = None + self._interactive_render = sys.stdout.isatty() and os.getenv("TERM") != "dumb" def start(self): self._thread = threading.Thread(target=self._refresh_loop, daemon=True) @@ -555,6 +522,9 @@ class ProgressView: self.state[key]["results"].append(result) def render(self): + if not self._interactive_render: + return + with self._lock: lines = [] frame = SPINNER_FRAMES[self._frame % len(SPINNER_FRAMES)] @@ -619,17 +589,38 @@ class ProgressView: lines.append("") lines.append(self._prompt) + width = max(20, shutil.get_terminal_size((120, 24)).columns) + lines = [self._fit_line(line, width) for line in lines] + if self._rendered_lines: - sys.stdout.write(f"\033[{self._rendered_lines}F") + sys.stdout.write(f"\r\033[{self._rendered_lines}A") for line in lines: - sys.stdout.write("\033[2K") + sys.stdout.write("\033[2K\r") sys.stdout.write(line) sys.stdout.write("\n") for _ in range(max(0, self._rendered_lines - len(lines))): - sys.stdout.write("\033[2K\n") + sys.stdout.write("\033[2K\r\n") sys.stdout.flush() self._rendered_lines = len(lines) + @staticmethod + def _fit_line(line, width): + result = [] + visible = 0 + index = 0 + while index < len(line) and visible < width - 1: + if line[index] == "\033": + end = line.find("m", index) + if end == -1: + break + result.append(line[index:end + 1]) + index = end + 1 + continue + result.append(line[index]) + visible += 1 + index += 1 + return "".join(result) + Style.RESET_ALL + def set_selection(self, network, ip, prompt): with self._lock: self._selection = (network, ip) @@ -722,92 +713,6 @@ def print_results(results, json_mode, interactive_mode): print(format_table(results)) -def interactive_select_remote(view): - candidates = view.flatten_results() - if not candidates: - return None - - selected = 0 - confirm_mode = False - confirm_buf = "" - - def render_prompt(): - network, result = candidates[selected] - if confirm_mode: - prompt = ( - f"Set {Fore.GREEN}{result['ip']}{Style.RESET_ALL} as " - f"{Fore.CYAN}remote{Style.RESET_ALL}? [Y/n] {confirm_buf}" - ) - else: - prompt = ( - "Select result with Tab/Shift+Tab/↑/↓/W/S/Ctrl+N/Ctrl+P, " - "press Enter to continue, q/Esc/Ctrl+C to exit." - ) - view.set_selection(network, result["ip"], prompt) - - def confirm_choice(): - _, result = candidates[selected] - set_remote(result["ip"]) - view.clear_prompt() - print( - f"Successfully set remote host to " - f"{Fore.LIGHTGREEN_EX}{result['ip']}{Style.RESET_ALL}." - ) - return result["ip"] - - render_prompt() - with RawTerminal(): - while True: - key = RawTerminal.read_key() - if key is None: - continue - - if confirm_mode: - if key == "enter": - answer = confirm_buf.strip().lower() - if answer in ("", "y", "yes"): - return confirm_choice() - if answer in ("n", "no"): - confirm_mode = False - confirm_buf = "" - render_prompt() - continue - if key == "backspace": - confirm_buf = confirm_buf[:-1] - render_prompt() - continue - if key == "escape": - confirm_mode = False - confirm_buf = "" - render_prompt() - continue - if key in ("next", "prev"): - confirm_mode = False - confirm_buf = "" - elif isinstance(key, str) and len(key) == 1: - confirm_buf += key - render_prompt() - continue - - if key == "next": - selected = (selected + 1) % len(candidates) - confirm_mode = False - confirm_buf = "" - render_prompt() - continue - if key == "prev": - selected = (selected - 1) % len(candidates) - confirm_mode = False - confirm_buf = "" - render_prompt() - continue - if key == "enter": - confirm_mode = True - confirm_buf = "" - render_prompt() - continue - - def interactive_scan_and_select(networks): view = ProgressView(networks) selected = {"index": 0, "key": None} @@ -834,20 +739,19 @@ def interactive_scan_and_select(networks): view.set_selection( None, None, - "Scanning... press q/Esc/Ctrl+C to exit. Selection becomes available once a result appears.", + "Scanning... press q/Esc to exit. Selection becomes available once a result appears.", ) return sync_selection(candidates) network, result = candidates[selected["index"]] - selected["key"] = (network, result["ip"]) scan_done = all(view.state[network_name]["done"] for network_name in view.networks) prompt = ( "Scan complete. Select with Tab/Shift+Tab/↑/↓/W/S/Ctrl+N/Ctrl+P, " - "press Enter to set selected remote, q/Esc/Ctrl+C to exit." + "press Enter to set selected remote, q/Esc to exit." if scan_done - else "Select with Tab/Shift+Tab/↑/↓/W/S/Ctrl+N/Ctrl+P, press Enter to set immediately, q/Esc/Ctrl+C to exit." + else "Select with Tab/Shift+Tab/↑/↓/W/S/Ctrl+N/Ctrl+P, press Enter to set immediately, q/Esc to exit." ) view.set_selection( network, @@ -856,9 +760,6 @@ def interactive_scan_and_select(networks): ) def on_key(key): - if key in ("quit", "escape"): - raise KeyboardInterrupt - candidates = view.flatten_results() if not candidates: return None @@ -867,14 +768,10 @@ def interactive_scan_and_select(networks): if key == "next": selected["index"] = (selected["index"] + 1) % len(candidates) - network, result = candidates[selected["index"]] - selected["key"] = (network, result["ip"]) render_prompt() return None if key == "prev": selected["index"] = (selected["index"] - 1) % len(candidates) - network, result = candidates[selected["index"]] - selected["key"] = (network, result["ip"]) render_prompt() return None if key == "enter": @@ -886,6 +783,8 @@ def interactive_scan_and_select(networks): with RawTerminal(): while True: key = RawTerminal.read_key() + if key is None: + continue chosen = on_key(key) if chosen is not None: return chosen @@ -942,4 +841,4 @@ if __name__ == "__main__": try: sys.exit(main(sys.argv[1:])) except KeyboardInterrupt: - sys.exit(1) + os._exit(130) diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml new file mode 100644 index 000000000..6e679cabc --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -0,0 +1,243 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::Sentry -> sentry_hardware + + - rmcs_core::referee::Status -> referee_status + - rmcs_core::referee::Command -> referee_command + - rmcs_core::referee::command::Interaction -> referee_interaction + # - rmcs_core::referee::command::interaction::SentryDecision -> referee_sentry_decision + - rmcs_core::referee::command::interaction::Ui -> referee_ui + + - rmcs_core::controller::gimbal::EccentricDualYaw -> gimbal_controller + - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller + - rmcs_core::controller::shooting::HeatController -> heat_controller + - rmcs_core::controller::shooting::BulletFeederController17mm -> bullet_feeder_controller + - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller + - rmcs_core::controller::chassis::ChassisClimberController -> climber_controller + - rmcs_core::controller::chassis::ChassisController -> chassis_controller + - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller + - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller + + # - rmcs::navigation::Navigation -> rmcs_navigation + + # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster + + - rmcs::AutoAimCapturerComponent -> auto_aim_capturer + # - rmcs::AutoAimPlayerComponent -> auto_aim_player + # - rmcs::AutoAimRecorderComponent -> auto_aim_recorder + - rmcs::AutoAimComponent -> auto_aim_component + +auto_aim_capturer: + ros__parameters: + camera_name: "" + exposure_us: 3000.0 + gain: 8.0 + framerate: 120.0 + invert_image: true + rls_tau_sec: 10.0 + use_hardware_sync: true + delay_ms: 6.5 + +auto_aim_recorder: + ros__parameters: + output_path: "/tmp/autoaim/records" + queue_depth: 16 + flush_every_n_frames: 64 + max_duration_seconds: 0 + max_videos_size_gb: 0.0 + +auto_aim_component: + ros__parameters: + # WARN: 危险！可选 red | blue，生效后裁判系统 ID 缺席时 + # 将强行绑定为对应阵营哨兵，仅供调试，严禁比赛启用。 + # 留空或填 unknow 表示禁用。 + dangerous_fallback: "" + manual_shoot: true + camera_translation: [0.07128, 0.0, 0.0481] + fire_control: + bullet_speed: 22.5 + shoot_delay: 0.1 + offset_yaw: -1.0 + offset_pitch: +0.0 + attack_window: 120.0 + window_hysteresis: 0.2 + is_lazy_gimbal: false + attack_preaim: false + require_stable_command: false + yaw_tolerance: 0.07 + pitch_tolerance: 0.04 + +value_broadcaster: + ros__parameters: + forward_list: + - /gimbal/yaw/velocity_imu + - /gimbal/pitch/velocity_imu + +# The positive direction is the one that battery exists +sentry_hardware: + ros__parameters: + board_serial_bottom_board: "af-b4e5" + board_serial_gimbal_board: "af-8b8b" + + pitch_motor_zero_point: 17241 + + bottom_yaw_motor_zero_point: 54253 + top_yaw_motor_zero_point: 32736 + + left_front_zero_point: 5776 + left_back_zero_point: 3784 + right_back_zero_point: 3048 + right_front_zero_point: 5135 + +rmcs_navigation: + ros__parameters: + command_vel_name: "/cmd_vel" + mock_context: false + endpoint: "test" # otaku | main | test + enable_goal_topic_forward: true + +gimbal_controller: + ros__parameters: + upper_limit: -0.65 + lower_limit: 0.36 + + top_yaw_angle_kp: 35.0 + top_yaw_angle_ki: 0.008 + top_yaw_angle_kd: 0.005 + top_yaw_velocity_kp: 2.160 + top_yaw_velocity_ki: 0.00 + top_yaw_velocity_kd: 0.0 + + bottom_yaw_angle_kp: 16.0 + bottom_yaw_angle_ki: 0.01 + bottom_yaw_angle_kd: 0.0 + bottom_yaw_velocity_kp: 2.75 + bottom_yaw_velocity_ki: 0.00125 + bottom_yaw_velocity_kd: 0.0 + + pitch_angle_kp: 32.0 + pitch_angle_ki: 0.01 + pitch_angle_kd: 0.0 + pitch_velocity_kp: 2.5 + pitch_velocity_ki: 0.0 + pitch_velocity_kd: 0.0 + +chassis_controller: + ros__parameters: + following_velocity_kp: 7.0 + following_velocity_ki: 0.0 + following_velocity_kd: 0.0 + +climber_controller: + ros__parameters: + front_climber_velocity: 20.0 + back_climber_velocity: 30.0 + auto_climb_support_retract_velocity_fast: 60.0 + auto_climb_support_retract_velocity_slow: 20.0 + auto_climb_approach_chassis_velocity: 1.0 + auto_climb_support_deploy_chassis_velocity: 0.3 + auto_climb_support_retract_chassis_velocity: 0.3 + auto_climb_dash_chassis_velocity: 3.0 + first_stair_dash_leveled_pitch_threshold: 0.05 + second_stair_dash_leveled_pitch_threshold: -0.09 + sync_coefficient: 0.2 + first_stair_approach_pitch: 0.585 + second_stair_approach_pitch: 0.365 + front_kp: 1.0 + front_ki: 0.0 + front_kd: 0.5 + front_power_estimate_bias: 0.0 + front_power_estimate_k_tau2: 1.0 + front_power_estimate_k_mech: 1.0 + back_kp: 0.5 + back_ki: 0.0 + back_kd: 0.0 + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/left_friction + - /gimbal/right_friction + friction_velocities: + - 575.0 + - 575.0 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 10000 + reserved_heat: 30000 + +bullet_feeder_controller: + ros__parameters: + bullets_per_feeder_turn: 9.0 + shot_frequency: 28.0 + safe_shot_frequency: 10.0 + eject_frequency: 15.0 + eject_time: 0.15 + deep_eject_frequency: 15.0 + deep_eject_time: 0.20 + single_shot_max_stop_delay: 2.0 + +left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/left_friction/velocity + setpoint: /gimbal/left_friction/control_velocity + control: /gimbal/left_friction/control_torque + kp: 0.003436926 + ki: 0.00 + kd: 0.009373434 + +right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/right_friction/velocity + setpoint: /gimbal/right_friction/control_velocity + control: /gimbal/right_friction/control_torque + kp: 0.003436926 + ki: 0.00 + kd: 0.009373434 + +bullet_feeder_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/bullet_feeder/velocity + setpoint: /gimbal/bullet_feeder/control_velocity + control: /gimbal/bullet_feeder/control_torque + kp: 0.283 + ki: 0.0 + kd: 0.0 + +steering_wheel_controller: + ros__parameters: + mess: 22.0 + moment_of_inertia: 0.77852676 + vehicle_radius: 0.26870058 + wheel_radius: 0.055 + friction_coefficient: 0.666 + k1: 2.958580e+00 + k2: 3.082190e-03 + no_load_power: 11.37 + + chassis_translation_kp: 20.0 + chassis_translation_ki: 0.001 + chassis_translation_kd: 0.00 + + chassis_angular_velocity_kp: 8.0 + chassis_angular_velocity_ki: 0.0 + chassis_angular_velocity_kd: 1.0 + + steering_velocity_kp: 0.15 + steering_velocity_ki: 0.0 + steering_velocity_kd: 0.0 + + steering_angle_kp: 30.0 + steering_angle_ki: 0.0 + steering_angle_kd: 0.0 + + wheel_velocity_kp: 0.5 + wheel_velocity_ki: 0.0 + wheel_velocity_kd: 0.0 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 4f402aaa2..1803a1d48 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -4,6 +4,7 @@ + diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp index 3355b468e..840c23c2f 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp @@ -119,8 +119,6 @@ class ChassisClimberController auto keyboard = *keyboard_; auto rotary_knob_switch = *rotary_knob_switch_; - // RCLCPP_INFO(get_logger(), "%f", *chassis_pitch_imu_); - bool rotary_knob_to_down = (last_rotary_knob_switch_ != Switch::DOWN && rotary_knob_switch == Switch::DOWN); bool rotary_knob_from_down = @@ -299,9 +297,7 @@ class ChassisClimberController AutoClimbControl update_manual_support_control(const rmcs_msgs::Keyboard& keyboard) { AutoClimbControl control; - if (keyboard.b || *rotary_knob_switch_ == rmcs_msgs::Switch::UP) { - manual_support_retracting_ = false; - manual_support_retract_block_count_ = 0; + if (keyboard.b) { back_climber_zero_velocity_hold_ = false; control.back_climber_velocity = climber_back_control_velocity_abs_; return control; @@ -718,8 +714,8 @@ class ChassisClimberController std::shared_ptr front_power_limiter_; - double back_climber_retract_first_torque_ = 10.0; - double back_climber_retract_second_torque_ = 1.0; + double back_climber_retract_first_torque_ = 8.0; + double back_climber_retract_second_torque_ = 0.5; int back_climber_recover_count = 0; }; } // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp index 81f6bbaf3..db740632b 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp @@ -1,43 +1,42 @@ #include "controller/pid/pid_calculator.hpp" -#include -#include - #include #include #include #include #include #include +#include #include -#include namespace rmcs_core::controller::chassis { class ChassisController : public rmcs_executor::Component - , public rclcpp::Node - , public rmcs_utility::NodeMixin { + , public rclcpp::Node { public: ChassisController() - : Node{get_component_name(), node::options()} { + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} { - following_velocity_controller_.output_max = +angular_velocity_max; + following_velocity_controller_.output_max = angular_velocity_max; following_velocity_controller_.output_min = -angular_velocity_max; register_input("/remote/joystick/right", joystick_right_); + register_input("/remote/joystick/left", joystick_left_); register_input("/remote/switch/right", switch_right_); register_input("/remote/switch/left", switch_left_); + register_input("/remote/mouse/velocity", mouse_velocity_); + register_input("/remote/mouse", mouse_); register_input("/remote/keyboard", keyboard_); + register_input("/remote/rotary_knob", rotary_knob_); register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); - register_input("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, false); - - register_input("/chassis/climber/direction", chassis_climb_direction_, false); - register_input("/chassis/climber/speed", chassis_climb_speed_, false); - register_input("/chassis/climber/measure_yaw", chassis_measure_yaw_, false); + register_input("/chassis/velocity", chassis_velocity_, false); + register_input("/chassis/climbing_forward_velocity", climbing_forward_velocity_, false); register_input("/rmcs_navigation/enable_control", navigation_enable_control_, false); register_input("/rmcs_navigation/chassis_velocity", navigation_command_velocity_, false); @@ -52,21 +51,16 @@ class ChassisController void before_updating() override { if (!gimbal_yaw_angle_.ready()) { gimbal_yaw_angle_.make_and_bind_directly(0.0); - node::warn("Failed to fetch \"/gimbal/yaw/angle\". Set to 0.0."); + RCLCPP_WARN(get_logger(), "Failed to fetch \"/gimbal/yaw/angle\". Set to 0.0."); } if (!gimbal_yaw_angle_error_.ready()) { gimbal_yaw_angle_error_.make_and_bind_directly(0.0); - node::warn("Failed to fetch \"/gimbal/yaw/control_angle_error\". Set to 0.0."); + RCLCPP_WARN( + get_logger(), "Failed to fetch \"/gimbal/yaw/control_angle_error\". Set to 0.0."); } - if (!chassis_climb_direction_.ready()) { - chassis_climb_direction_.make_and_bind_directly(kNaN); - } - if (!chassis_climb_speed_.ready()) { - chassis_climb_speed_.make_and_bind_directly(kNaN); - } - if (!chassis_measure_yaw_.ready()) { - chassis_measure_yaw_.make_and_bind_directly(kNaN); + if (!climbing_forward_velocity_.ready()) { + climbing_forward_velocity_.make_and_bind_directly(kNaN); } if (!navigation_enable_control_.ready()) { @@ -83,9 +77,9 @@ class ChassisController void update() override { using namespace rmcs_msgs; - const auto switch_right = *switch_right_; - const auto switch_left = *switch_left_; - const auto keyboard = *keyboard_; + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; do { if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) @@ -125,13 +119,6 @@ class ChassisController mode = *navigation_chassis_behavior_; } - if (climb_active()) { - mode = ChassisMode::CLIMB; - } else if (mode == ChassisMode::CLIMB) { - mode = ChassisMode::AUTO; - } - - update_spin_stuck_watchdog(mode); *mode_ = mode; } @@ -146,45 +133,8 @@ class ChassisController void reset_all_controls() { *mode_ = rmcs_msgs::ChassisMode::ALIGNMENT; *chassis_control_velocity_ = {kNaN, kNaN, kNaN}; - - spin_stuck_count_ = 0; - spin_reverse_cooldown_ = 0; - following_velocity_controller_.reset(); } - auto update_spin_stuck_watchdog(rmcs_msgs::ChassisMode mode) -> void { - constexpr auto kSpinStuckConfirmTicks = std::size_t{300}; - constexpr auto kSpinReverseCooldownTicks = std::size_t{1000}; - constexpr auto kSpinStuckAngularVelocityRatio = double{0.2}; - - using rmcs_msgs::ChassisMode; - - if (spin_reverse_cooldown_ > 0) { - --spin_reverse_cooldown_; - spin_stuck_count_ = 0; - return; - } - - if (!rmcs_msgs::is_spining(mode) || !chassis_yaw_velocity_imu_.ready()) { - spin_stuck_count_ = 0; - return; - } - - const auto expected = (mode == ChassisMode::SPIN_FAST ? 0.6 : 0.3) * angular_velocity_max; - if (std::abs(*chassis_yaw_velocity_imu_) >= kSpinStuckAngularVelocityRatio * expected) { - spin_stuck_count_ = 0; - return; - } - - if (++spin_stuck_count_ < kSpinStuckConfirmTicks) - return; - - spinning_forward_ = !spinning_forward_; - spin_reverse_cooldown_ = kSpinReverseCooldownTicks; - spin_stuck_count_ = 0; - - node::warn("Spin stuck detected, reverse spinning direction."); - } void update_velocity_control() { auto translational_velocity = update_translational_velocity_control(); auto angular_velocity = update_angular_velocity_control(); @@ -192,38 +142,14 @@ class ChassisController chassis_control_velocity_->vector << translational_velocity, angular_velocity; } - auto climb_active() const -> bool { - return std::isfinite(*chassis_climb_direction_) && std::isfinite(*chassis_climb_speed_) - && std::isfinite(*chassis_measure_yaw_); - } - - static auto normalize_signed_angle(double angle) noexcept { - constexpr auto kTwoPi = 2.0 * std::numbers::pi; - while (angle >= std::numbers::pi) - angle -= kTwoPi; - while (angle < -std::numbers::pi) - angle += kTwoPi; - return angle; - } - Eigen::Vector2d update_translational_velocity_control() { - using namespace rmcs_msgs; - - if (*mode_ == ChassisMode::CLIMB) { - // speed 以底盘正向 direction 为正向：上坡为正前进，下坡为负倒车 - return {*chassis_climb_speed_, 0.0}; - } + if (!std::isnan(*climbing_forward_velocity_)) + return {*climbing_forward_velocity_, 0.0}; if (*navigation_enable_control_) { const auto command = *navigation_command_velocity_; - if (command.array().isFinite().all()) { - Eigen::Vector2d superimposed = - command + *joystick_right_ * translational_velocity_max; - if (superimposed.norm() > translational_velocity_max) - superimposed *= translational_velocity_max / superimposed.norm(); - - return Eigen::Rotation2Dd{*gimbal_yaw_angle_} * superimposed; - } + if (command.array().isFinite().all()) + return Eigen::Rotation2Dd{*gimbal_yaw_angle_} * command; } auto keyboard = *keyboard_; @@ -244,6 +170,17 @@ class ChassisController double angular_velocity = 0.0; double chassis_control_angle = kNaN; + if (!std::isnan(*climbing_forward_velocity_)) { + double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + if (err > std::numbers::pi) + err -= 2 * std::numbers::pi; + angular_velocity = following_velocity_controller_.update(err); + + *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; + *chassis_control_angle_ = chassis_control_angle; + return angular_velocity; + } + using namespace rmcs_msgs; switch (*mode_) { case ChassisMode::AUTO: break; @@ -299,17 +236,6 @@ class ChassisController angular_velocity = following_velocity_controller_.update(err); } break; - - case ChassisMode::CLIMB: { - chassis_control_angle = *chassis_climb_direction_; - - const auto err = normalize_signed_angle(chassis_control_angle - *chassis_measure_yaw_); - angular_velocity = following_velocity_controller_.update(err); - - *chassis_angle_ = *chassis_measure_yaw_; - *chassis_control_angle_ = chassis_control_angle; - return angular_velocity; - } } *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; @@ -334,13 +260,17 @@ class ChassisController static constexpr double kInf = std::numeric_limits::infinity(); static constexpr double kNaN = std::numeric_limits::quiet_NaN(); - const double translational_velocity_max{node::param_or("translational_velocity_max", 10.0)}; - const double angular_velocity_max{node::param_or("angular_velocity_max", 16.0)}; + const double translational_velocity_max{get_parameter_or("translational_velocity_max", 10.0)}; + const double angular_velocity_max{get_parameter_or("angular_velocity_max", 16.0)}; InputInterface joystick_right_; + InputInterface joystick_left_; InputInterface switch_right_; InputInterface switch_left_; + InputInterface mouse_velocity_; + InputInterface mouse_; InputInterface keyboard_; + InputInterface rotary_knob_; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; @@ -349,10 +279,8 @@ class ChassisController InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; OutputInterface chassis_angle_, chassis_control_angle_; - InputInterface chassis_yaw_velocity_imu_; - InputInterface chassis_climb_direction_; - InputInterface chassis_climb_speed_; - InputInterface chassis_measure_yaw_; + InputInterface chassis_velocity_; + InputInterface climbing_forward_velocity_; InputInterface navigation_enable_control_; InputInterface navigation_command_velocity_; @@ -360,14 +288,10 @@ class ChassisController OutputInterface mode_; bool spinning_forward_ = true; - - std::size_t spin_stuck_count_ = 0; - std::size_t spin_reverse_cooldown_ = 0; - pid::PidCalculator following_velocity_controller_{ - node::param_or("following_velocity_kp", 8.0), - node::param_or("following_velocity_ki", 0.0), - node::param_or("following_velocity_kd", 0.0), + get_parameter_or("following_velocity_kp", 8.0), + get_parameter_or("following_velocity_ki", 0.0), + get_parameter_or("following_velocity_kd", 0.0), }; OutputInterface chassis_control_velocity_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp index f9e8e83a9..7c701f633 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp @@ -30,7 +30,6 @@ class ChassisPowerController register_input("/chassis/power", chassis_power_); register_input("/chassis/supercap/voltage", supercap_voltage_); register_input("/chassis/supercap/enabled", supercap_enabled_); - register_input("/rmcs_navigation/enable_supercap", navigation_supercap_, false); register_input("/referee/chassis/power_limit", chassis_power_limit_referee_); register_input("/referee/chassis/buffer_energy", chassis_buffer_energy_referee_); @@ -111,12 +110,9 @@ class ChassisPowerController void update_control_power_limit() { double power_limit; - const auto navigation_supercap_boost = - navigation_supercap_.ready() && *navigation_supercap_; - - if ((boost_mode_ || navigation_supercap_boost) && *supercap_enabled_) + if (boost_mode_ && *supercap_enabled_) power_limit = - rmcs_msgs::is_powered(*mode_) ? inf_ : *chassis_power_limit_referee_ + 80.0; + rmcs_msgs::need_power(*mode_) ? inf_ : *chassis_power_limit_referee_ + 80.0; else power_limit = *chassis_power_limit_referee_; chassis_power_limit_expected_ = power_limit; @@ -163,7 +159,6 @@ class ChassisPowerController InputInterface supercap_voltage_; InputInterface supercap_enabled_; - InputInterface navigation_supercap_; InputInterface chassis_power_limit_referee_; InputInterface chassis_buffer_energy_referee_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index 9cea6ef5b..a8688a6d3 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -182,7 +182,7 @@ class DeformableChassis switch (*mode_) { case rmcs_msgs::ChassisMode::AUTO: break; - case rmcs_msgs::ChassisMode::SPIN: { + case rmcs_msgs::ChassisMode::SPIN_FAST: { bool forward = joint_mode_mgr_.spinning_forward(); angular_velocity = spin_ratio_ * (forward ? angular_velocity_max_ : -angular_velocity_max_); diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp index 923a9ff2c..ca9aa4d64 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp @@ -148,17 +148,17 @@ class DeformableChassisModeManager { if (last_switch_right_ == rmcs_msgs::Switch::MIDDLE && switch_right == rmcs_msgs::Switch::DOWN) { - if (next_mode == rmcs_msgs::ChassisMode::SPIN) { + if (next_mode == rmcs_msgs::ChassisMode::SPIN_FAST) { next_mode = rmcs_msgs::ChassisMode::STEP_DOWN; } else { - next_mode = rmcs_msgs::ChassisMode::SPIN; + next_mode = rmcs_msgs::ChassisMode::SPIN_FAST; joint_posture_state_.spinning_forward = !joint_posture_state_.spinning_forward; } } else if (!last_keyboard_.c && keyboard.c) { - if (next_mode == rmcs_msgs::ChassisMode::SPIN) { + if (next_mode == rmcs_msgs::ChassisMode::SPIN_FAST) { next_mode = rmcs_msgs::ChassisMode::AUTO; } else { - next_mode = rmcs_msgs::ChassisMode::SPIN; + next_mode = rmcs_msgs::ChassisMode::SPIN_FAST; joint_posture_state_.spinning_forward = !joint_posture_state_.spinning_forward; } } else if (!last_keyboard_.z && keyboard.z) { diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp index 9291886ed..8fd643e54 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp @@ -203,7 +203,6 @@ class HeroChassisController } break; case rmcs_msgs::ChassisMode::ALIGNMENT: [[fallthrough]]; case rmcs_msgs::ChassisMode::ALIGNMENT_POWERED: [[fallthrough]]; - case rmcs_msgs::ChassisMode::CLIMB: [[fallthrough]]; case rmcs_msgs::ChassisMode::LAUNCH_RAMP: { double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp index 526853c05..0f2734ec1 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp @@ -18,49 +18,6 @@ namespace rmcs_core::controller::gimbal { using namespace rmcs_description; -// 对 top 关节自瞄参考角做差分+低通滤波的目标角速度前馈， -// 补偿斜坡跟踪滞后；切板跳变时清零。 -struct YawRateFeedforward { - double gain = 1.0; - double cutoff_hz = 15.0; - double max_rate = 6.0; - double jump_threshold = 0.05; - - auto update(double azimuth, std::chrono::steady_clock::time_point now) -> double { - if (std::isfinite(prev_azimuth_)) { - const auto dt = std::chrono::duration(now - prev_timestamp_).count(); - const auto delta = limit_rad(azimuth - prev_azimuth_); - if (std::abs(delta) > jump_threshold) { - filtered_rate_ = 0.0; - } else if (dt > kMinDt) { - const auto raw = delta / dt; - const auto alpha = dt / (dt + 1.0 / (2.0 * std::numbers::pi * cutoff_hz)); - filtered_rate_ += alpha * (raw - filtered_rate_); - } - } - prev_azimuth_ = azimuth; - prev_timestamp_ = now; - return gain * std::clamp(filtered_rate_, -max_rate, max_rate); - } - -private: - static constexpr double kNaN = std::numeric_limits::quiet_NaN(); - static constexpr double kMinDt = 1e-6; - - static auto limit_rad(double angle) -> double { - constexpr double kPi = std::numbers::pi_v; - while (angle > kPi) - angle -= 2.0 * kPi; - while (angle <= -kPi) - angle += 2.0 * kPi; - return angle; - } - - double prev_azimuth_ = kNaN; - double filtered_rate_ = 0.0; - std::chrono::steady_clock::time_point prev_timestamp_{}; -}; - class EccentricDualYaw : public rmcs_executor::Component , public rclcpp::Node { @@ -68,12 +25,7 @@ class EccentricDualYaw EccentricDualYaw() : Node{ get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} { - get_parameter_or("top_yaw_velocity_ff_gain", top_yaw_ff_.gain, 1.0); - get_parameter_or("top_yaw_ff_cutoff_hz", top_yaw_ff_.cutoff_hz, 15.0); - get_parameter_or("top_yaw_ff_max", top_yaw_ff_.max_rate, 6.0); - get_parameter_or("top_yaw_ff_jump_threshold", top_yaw_ff_.jump_threshold, 0.05); - } + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} {} auto before_updating() -> void override { if (!input_.navigation_enable_control.ready()) { @@ -108,9 +60,7 @@ class EccentricDualYaw upper_limit_, lower_limit_, }); - const auto top_yaw_ff = - top_yaw_ff_.update(solver_.top_target_azimuth(), *input_.timestamp); - apply_control(error.bottom_yaw, error.top_yaw, error.pitch, top_yaw_ff); + apply_control(error.bottom_yaw, error.top_yaw, error.pitch); const auto [_, cur_pitch] = current_barrel_yaw_pitch(); stored_bottom_yaw_target_ = limit_rad(current_bottom_world_yaw() + error.bottom_yaw); @@ -119,35 +69,44 @@ class EccentricDualYaw return; } - const auto yaw_shift = +kJoystickSensitivity * input_.joystick_left->y() - + kMouseSensitivity * input_.mouse_velocity->y(); - const auto pitch_shift = -kJoystickSensitivity * input_.joystick_left->x() - - kMouseSensitivity * input_.mouse_velocity->x(); - - auto nav_yshift = double{0.}; - auto nav_pshift = double{0.}; + // 导航控制。 if (input_.enable_navigation()) { - constexpr auto kGimbalFree = std::numeric_limits::min(); - const auto& toward = *input_.navigation_toward; - if (toward.x() == kGimbalFree && toward.y() == kGimbalFree) { - enter_disabled_state(); - return; - } - if (std::isfinite(toward.x())) - nav_yshift = limit_rad(toward.x() - stored_bottom_yaw_target_); - if (std::isfinite(toward.y())) - nav_pshift = limit_rad( - std::clamp(toward.y(), upper_limit_, lower_limit_) - stored_pitch_target_); + const auto error = solver_.update( + EccentricDualYawSolver::Navigation{ + *input_.top_yaw_angle, + *input_.navigation_toward, + current_bottom_world_yaw(), + actual_yaw_pitch.second, + stored_bottom_yaw_target_, + stored_pitch_target_, + upper_limit_, + lower_limit_, + }); + apply_control(error.bottom_yaw, error.top_yaw, error.pitch); + + stored_bottom_yaw_target_ = limit_rad(current_bottom_world_yaw() + error.bottom_yaw); + stored_pitch_target_ = std::clamp( + limit_rad(actual_yaw_pitch.second + error.pitch), upper_limit_, lower_limit_); + return; } - stored_bottom_yaw_target_ = limit_rad(stored_bottom_yaw_target_ + nav_yshift + yaw_shift); - stored_pitch_target_ = - std::clamp(stored_pitch_target_ + nav_pshift + pitch_shift, upper_limit_, lower_limit_); + // 手动控制。 + { + const double yaw_shift = kJoystickSensitivity * input_.joystick_left->y() + + kMouseSensitivity * input_.mouse_velocity->y(); + const double pitch_shift = -kJoystickSensitivity * input_.joystick_left->x() + - kMouseSensitivity * input_.mouse_velocity->x(); + + stored_bottom_yaw_target_ = limit_rad(stored_bottom_yaw_target_ + yaw_shift); + stored_pitch_target_ = + std::clamp(stored_pitch_target_ + pitch_shift, upper_limit_, lower_limit_); + + const double bottom_yaw_error = + limit_rad(stored_bottom_yaw_target_ - current_bottom_world_yaw()); + const double pitch_error = limit_rad(stored_pitch_target_ - actual_yaw_pitch.second); - apply_control( - limit_rad(+stored_bottom_yaw_target_ - current_bottom_world_yaw()), - limit_rad(-*input_.top_yaw_angle), - limit_rad(+stored_pitch_target_ - actual_yaw_pitch.second)); + apply_control(bottom_yaw_error, limit_rad(-*input_.top_yaw_angle), pitch_error); + } } private: @@ -163,8 +122,6 @@ class EccentricDualYaw EccentricDualYawSolver solver_; - YawRateFeedforward top_yaw_ff_; - struct Input { explicit Input(rmcs_executor::Component& component) { component.register_input("/remote/joystick/left", joystick_left); @@ -350,15 +307,12 @@ class EccentricDualYaw return std::atan2(vector.y(), vector.x()); } - auto apply_control( - double bottom_yaw_error, double top_yaw_error, double pitch_error, - double top_yaw_feedforward = 0.0) -> void { + auto apply_control(double bottom_yaw_error, double top_yaw_error, double pitch_error) -> void { const auto current_bottom_velocity = *input_.bottom_yaw_velocity + *input_.chassis_yaw_velocity_imu; const auto bottom_velocity_ref = bottom_yaw_angle_pid_.update(bottom_yaw_error); - const auto top_velocity_ref = - top_yaw_angle_pid_.update(top_yaw_error) + top_yaw_feedforward; + const auto top_velocity_ref = top_yaw_angle_pid_.update(top_yaw_error); const auto pitch_velocity_ref = pitch_angle_pid_.update(pitch_error); *output_.top_yaw_control_torque = diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw_solver.hpp index 41409120f..7f9d2fd07 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw_solver.hpp @@ -29,9 +29,6 @@ class EccentricDualYawSolver { auto update(const Operation& op) -> Error { return op.update(*this); } auto enabled() const -> bool { return enabled_; } - // top 关节的自瞄参考角（desired_top），供角速度前馈差分使用 - auto top_target_azimuth() const -> double { return top_target_azimuth_; } - class SetDisabled : public Operation { private: auto update(EccentricDualYawSolver& s) const -> Error override { @@ -83,7 +80,6 @@ class EccentricDualYawSolver { const double bottom_error = limit_rad(center_azimuth - current_btm); const double desired_top = limit_rad(barrel_azimuth - center_azimuth); const double top_error = limit_rad(desired_top - current_top); - s.top_target_azimuth_ = desired_top; const double desired_pitch = std::clamp(barrel_pitch, upper_, lower_); const double pitch_error = limit_rad(desired_pitch - current_brl); @@ -154,7 +150,6 @@ class EccentricDualYawSolver { } bool enabled_ = false; - double top_target_azimuth_ = kNaN_; }; } // namespace rmcs_core::controller::gimbal diff --git a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp new file mode 100644 index 000000000..dd92f1a4d --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp @@ -0,0 +1,636 @@ +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hardware/device/bmi088_ekf.hpp" +#include "hardware/device/board_clock_lifter.hpp" +#include "hardware/device/can_packet.hpp" +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/lk_motor.hpp" +#include "hardware/device/supercap.hpp" +#include "hardware/util/status_monitor.hpp" + +namespace rmcs_core::hardware { + +class Sentry + : public rmcs_executor::Component + , public rclcpp::Node { + + static constexpr auto kPosition = + std::array{"left_front", "left_back", "right_back", "right_front"}; + +public: + Sentry() + : Node( + get_component_name(), + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) { + + register_input("/predefined/timestamp", timestamp_); + + register_output("/tf", tf_); + register_output("/auto_aim/camera_transform", camera_transform_); + register_output("/auto_aim/barrel_direction", barrel_direction_); + register_output( + "/auto_aim/yaw_velocity", yaw_velocity_, std::numeric_limits::quiet_NaN()); + + // 提供 remote-status 命令服务。 + using Srv = std_srvs::srv::Trigger; + status_service_ = create_service( + "/rmcs/service/robot_status", + [this](const Srv::Request::SharedPtr&, const Srv::Response::SharedPtr& response) { + status_service_callback(response); + }); + + gimbal_board_ = std::make_unique( + *this, *command_component_, get_parameter("board_serial_gimbal_board").as_string()); + + chassis_board_ = std::make_unique( + *this, *command_component_, get_parameter("board_serial_bottom_board").as_string()); + + tf_->set_transform( + Eigen::Translation3d{0.08, 0.0, 0.0}); + tf_->set_transform( + Eigen::Translation3d{0.07128, 0.0, 0.0481}); + } + + void update() override { + gimbal_board_->update(); + chassis_board_->update(); + + using namespace rmcs_description; + *camera_transform_ = fast_tf::lookup_transform(*tf_); + *barrel_direction_ = *fast_tf::cast( + PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + *yaw_velocity_ = gimbal_board_->yaw_velocity(); + } + +private: + class GimbalBoard final : public librmcs::board::RmcsBoardLite::Callback { + public: + explicit GimbalBoard( + Sentry& sentry, rmcs_executor::Component& sentry_command, + std::string_view board_serial = {}) + : tf_(sentry.tf_) + , gimbal_pitch_motor_(sentry, sentry_command, "/gimbal/pitch") + , gimbal_top_yaw_motor_(sentry, sentry_command, "/gimbal/top_yaw") + , gimbal_bullet_feeder_(sentry, sentry_command, "/gimbal/bullet_feeder") + , gimbal_left_friction_(sentry, sentry_command, "/gimbal/left_friction") + , gimbal_right_friction_(sentry, sentry_command, "/gimbal/right_friction") { + + using namespace device; + + auto zero_point = int{0}; + sentry.get_parameter("pitch_motor_zero_point", zero_point); + gimbal_pitch_motor_.configure( + LkMotor::Config{LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point(zero_point)); + + sentry.get_parameter("top_yaw_motor_zero_point", zero_point); + gimbal_top_yaw_motor_.configure( + LkMotor::Config{LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point(zero_point)); + + gimbal_bullet_feeder_.configure( + DjiMotor::Config{DjiMotor::Type::kM3508, 4} + .enable_multi_turn_angle() + .set_reversed() + .set_reduction_ratio(19 * 2)); + + gimbal_left_friction_.configure( + DjiMotor::Config{DjiMotor::Type::kM3508, 2}.set_reduction_ratio(1.)); + gimbal_right_friction_.configure( + DjiMotor::Config{DjiMotor::Type::kM3508, 1}.set_reduction_ratio(1.).set_reversed()); + + sentry.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_, 0.0); + sentry.register_output( + "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_, 0.0); + + sentry.register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); + sentry.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); + + board_ = std::make_unique(*this, board_serial); + board_->start_transmit().gpio_digital_read( + Spec::kGpios.kUart1Rx, { + .period_ms = 0, + .asap = false, + .rising_edge = false, + .falling_edge = true, + .capture_timestamp = true, + .pull = librmcs::data::GpioPull::kUp, + }); + } + + auto yaw_velocity() const -> double { return *gimbal_yaw_velocity_bmi088_; } + + auto status() const -> std::vector { return monitor_.text(); } + + void update() { + gimbal_bullet_feeder_.update_status(); + gimbal_left_friction_.update_status(); + gimbal_right_friction_.update_status(); + + gimbal_top_yaw_motor_.update_status(); + tf_->set_state( + gimbal_top_yaw_motor_.angle()); + + gimbal_pitch_motor_.update_status(); + const auto pitch_angle = + std::remainder(gimbal_pitch_motor_.angle(), 2.0 * std::numbers::pi); + tf_->set_state(pitch_angle); + + if (const auto snapshot = bmi088_.snapshot()) { + tf_->set_transform( + snapshot->orientation.conjugate()); + + *gimbal_yaw_velocity_bmi088_ = snapshot->gyro_body.z(); + *gimbal_pitch_velocity_bmi088_ = snapshot->gyro_body.y(); + } + } + + void command_update() const { + board_->start_transmit() + .can_transmit( + Spec::kCans.kCan0, + { + .can_id = gimbal_right_friction_.send_id(), + .can_data = + device::CanPacket8{ + gimbal_right_friction_.generate_command(), + gimbal_left_friction_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + gimbal_bullet_feeder_.generate_command(), + } + .as_bytes(), + }) + .can_transmit( + Spec::kCans.kCan3, + { + .can_id = 0x141, + .can_data = gimbal_top_yaw_motor_.generate_torque_command().as_bytes(), + }) + .can_transmit( + Spec::kCans.kCan3, + { + .can_id = 0x142, + .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), + }); + } + + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + + const auto& can_id = data.can_id; + const auto& can_data = data.can_data; + + if (can == Spec::kCans.kCan0) { + /*^^*/ gimbal_left_friction_.match_then_store_status(can_id, can_data) + || gimbal_right_friction_.match_then_store_status(can_id, can_data) + || gimbal_bullet_feeder_.match_then_store_status(can_id, can_data); + + monitor_.tick("Gimbal::Can0", can_id); + + } else if (can == Spec::kCans.kCan3) { + /*^^*/ if (can_id == 0x141) { + gimbal_top_yaw_motor_.store_status(can_data); + } else if (can_id == 0x142) { + gimbal_pitch_motor_.store_status(can_data); + } + + monitor_.tick("Gimbal::Can3", can_id); + } + } + + void gpio_digital_read_result_callback( + const Spec::Gpio& gpio, const View::GpioDigital& data) override { + if (!data.timestamp_quarter_us) + return; + + if (gpio == Spec::kGpios.kUart1Rx) { + if (data.high) + return; + + const auto timestamp = + board_clock_lifter_.lift_timestamp(*data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + + camera_signal_output_.emit(*timestamp); + } + } + + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { + const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); + bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); + monitor_.tick("Gimbal::Imu", "Acc"); + } + + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + monitor_.tick("Gimbal::Imu", "Gyr"); + const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + + auto snapshot = + bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); + if (!snapshot) + return; + + imu_snapshot_output_.emit(*snapshot); + } + + OutputInterface& tf_; + + OutputInterface gimbal_yaw_velocity_bmi088_; + OutputInterface gimbal_pitch_velocity_bmi088_; + + EventOutputInterface camera_signal_output_; + EventOutputInterface imu_snapshot_output_; + + device::Bmi088Ekf bmi088_{device::Bmi088Ekf::Config{ + .body_to_sensor = + Eigen::AngleAxisd{std::numbers::pi, Eigen::Vector3d::UnitZ()}.toRotationMatrix(), + }}; + device::BoardClockLifter board_clock_lifter_; + + device::LkMotor gimbal_pitch_motor_; + device::LkMotor gimbal_top_yaw_motor_; + device::DjiMotor gimbal_bullet_feeder_; + + device::DjiMotor gimbal_left_friction_; + device::DjiMotor gimbal_right_friction_; + + StatusMonitor monitor_{}; + std::unique_ptr board_; + }; + + class ChassisBoard final : public librmcs::board::RmcsBoardLite::Callback { + public: + explicit ChassisBoard( + Sentry& sentry, rmcs_executor::Component& sentry_command, + std::string_view board_serial = {}) + : tf_(sentry.tf_) + , dr16_(sentry) + , gimbal_bottom_yaw_motor_(sentry, sentry_command, "/gimbal/bottom_yaw") + , chassis_wheel_motors_( + {sentry, sentry_command, "/chassis/left_front_wheel"}, + {sentry, sentry_command, "/chassis/left_back_wheel"}, + {sentry, sentry_command, "/chassis/right_back_wheel"}, + {sentry, sentry_command, "/chassis/right_front_wheel"}) + , chassis_steer_motors_( + {sentry, sentry_command, "/chassis/left_front_steering"}, + {sentry, sentry_command, "/chassis/left_back_steering"}, + {sentry, sentry_command, "/chassis/right_back_steering"}, + {sentry, sentry_command, "/chassis/right_front_steering"}) + , chassis_front_climber_motor_( + {sentry, sentry_command, "/chassis/climber/left_front_motor"}, + {sentry, sentry_command, "/chassis/climber/right_front_motor"}) + , chassis_back_climber_motor_( + {sentry, sentry_command, "/chassis/climber/left_back_motor"}, + {sentry, sentry_command, "/chassis/climber/right_back_motor"}) + , supercap_(sentry, sentry_command) { + + using namespace device; + + sentry.register_output("/referee/serial", referee_serial_); + sentry.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0.0); + sentry.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); + + referee_serial_->read = [this](std::byte* buffer, size_t size) { + return referee_ring_buffer_receive_.pop_front_n( + [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) { + board_->start_transmit().uart_transmit( + Spec::kUarts.kUart0, {.uart_data = std::span{buffer, size}}); + return size; + }; + + const auto zero_point = sentry.get_parameter("bottom_yaw_motor_zero_point").as_int(); + gimbal_bottom_yaw_motor_.configure( + LkMotor::Config{LkMotor::Type::kMG6012Ei8}.set_reversed().set_encoder_zero_point( + static_cast(zero_point))); + + constexpr auto kWheelIds = std::array{2, 1, 2, 4}; + for (auto&& [motor, id] : std::views::zip(chassis_wheel_motors_, kWheelIds)) { + motor.configure( + DjiMotor::Config{DjiMotor::Type::kM3508, id} + .set_reduction_ratio(11.) + .enable_multi_turn_angle() + .set_reversed()); + } + + constexpr auto kSteerIds = std::array{2, 1, 1, 2}; + for (auto&& [motor, name, id] : + std::views::zip(chassis_steer_motors_, kPosition, kSteerIds)) { + const auto zero_point = + sentry.get_parameter(std::string{name} + "_zero_point").as_int(); + motor.configure( + DjiMotor::Config{DjiMotor::Type::kGM6020, id} + .set_reversed() + .set_encoder_zero_point(static_cast(zero_point)) + .enable_multi_turn_angle()); + } + + chassis_front_climber_motor_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1}.set_reduction_ratio( + 19.)); + chassis_front_climber_motor_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2} + .set_reversed() + .set_reduction_ratio(19.)); + chassis_back_climber_motor_[0].configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} + .set_reversed() + .enable_multi_turn_angle()); + chassis_back_climber_motor_[1].configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} + .enable_multi_turn_angle()); + + board_ = std::make_unique(*this, board_serial); + } + + auto status() const -> std::vector { return monitor_.text(); } + + void update() { + gimbal_bottom_yaw_motor_.update_status(); + dr16_.update_status(); + supercap_.update_status(); + + for (auto& motor : chassis_wheel_motors_) + motor.update_status(); + for (auto& motor : chassis_steer_motors_) + motor.update_status(); + + chassis_front_climber_motor_[0].update_status(); + chassis_front_climber_motor_[1].update_status(); + chassis_back_climber_motor_[0].update_status(); + chassis_back_climber_motor_[1].update_status(); + + tf_->set_state( + gimbal_bottom_yaw_motor_.angle()); + + if (const auto snapshot = bmi088_.snapshot()) { + const auto& q = snapshot->orientation; + *chassis_pitch_imu_ = -std::asin(2.0 * (q.w() * q.y() - q.z() * q.x())); + *chassis_yaw_velocity_imu_ = snapshot->gyro_body.z(); + } + } + + void command_update() { + using namespace device; + + auto cache = CanPacket8{}; + auto generate = [&](const auto& motors, // + CanPacket8::Quarter extra = {}, std::size_t slot = 4) { + auto slots = std::array{}; + slots.fill(CanPacket8::PaddingQuarter{}); + std::uint32_t can_id = 0; + for (auto& motor : motors) { + slots[(motor.id() - 1) % 4] = motor.generate_command(); + if (can_id == 0) + can_id = motor.send_id(); + } + if (slot < 4) + slots[slot] = extra; + cache = CanPacket8{slots[0], slots[1], slots[2], slots[3]}; + return librmcs::data::CanDataView{.can_id = can_id, .can_data = cache.as_bytes()}; + }; + + auto supercap_package = CanPacket8{ + CanPacket8::PaddingQuarter{}, + CanPacket8::PaddingQuarter{}, + CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + }; + auto bottom_yaw_package = gimbal_bottom_yaw_motor_.generate_command(); + + board_->start_transmit() + .can_transmit( + Spec::kCans.kCan0, + { + .can_id = 0x142, + .can_data = chassis_back_climber_motor_[0].generate_command().as_bytes(), + }) + .can_transmit( + Spec::kCans.kCan0, + { + .can_id = 0x143, + .can_data = chassis_back_climber_motor_[1].generate_command().as_bytes(), + }) + + .can_transmit( + Spec::kCans.kCan1, generate(std::views::counted(chassis_wheel_motors_, 2))) + .can_transmit( + Spec::kCans.kCan2, generate(std::views::counted(chassis_wheel_motors_ + 2, 2))) + + .can_transmit( + Spec::kCans.kCan1, generate(std::views::counted(chassis_steer_motors_, 2))) + .can_transmit( + Spec::kCans.kCan2, generate(std::views::counted(chassis_steer_motors_ + 2, 2))) + + .can_transmit( + Spec::kCans.kCan3, {.can_id = 0x1fe, .can_data = supercap_package.as_bytes()}) + .can_transmit( + Spec::kCans.kCan3, + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_front_climber_motor_[0].generate_command(), + chassis_front_climber_motor_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }) + .can_transmit( + Spec::kCans.kCan3, + {.can_id = 0x141, .can_data = bottom_yaw_package.as_bytes()}); + } + + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + + const auto& can_id = data.can_id; + const auto& can_data = data.can_data; + + if (can == Spec::kCans.kCan0) { + if (can_id == 0x142) { + chassis_back_climber_motor_[0].store_status(data.can_data); + } else if (can_id == 0x143) { + chassis_back_climber_motor_[1].store_status(data.can_data); + } + + monitor_.tick("Chassis::Can0", can_id); + + } else if (can == Spec::kCans.kCan1) { + /*^^*/ chassis_wheel_motors_[0].match_then_store_status(can_id, can_data) + || chassis_wheel_motors_[1].match_then_store_status(can_id, can_data) + + || chassis_steer_motors_[0].match_then_store_status(can_id, can_data) + || chassis_steer_motors_[1].match_then_store_status(can_id, can_data); + + monitor_.tick("Chassis::Can1", can_id); + + } else if (can == Spec::kCans.kCan2) { + /*^^*/ chassis_wheel_motors_[2].match_then_store_status(can_id, can_data) + || chassis_wheel_motors_[3].match_then_store_status(can_id, can_data) + + || chassis_steer_motors_[2].match_then_store_status(can_id, can_data) + || chassis_steer_motors_[3].match_then_store_status(can_id, can_data); + + monitor_.tick("Chassis::Can2", can_id); + + } else if (can == Spec::kCans.kCan3) { + if (can_id == 0x300) { + supercap_.store_status(can_data); + } else if (can_id == 0x141) { + gimbal_bottom_yaw_motor_.store_status(data.can_data); + } else { + /*^^*/ chassis_front_climber_motor_[0].match_then_store_status(can_id, can_data) + || chassis_front_climber_motor_[1].match_then_store_status( + can_id, can_data); + } + + monitor_.tick("Chassis::Can3", can_id); + } + } + + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + if (uart == Spec::kUarts.kDbus) { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + monitor_.tick("Chassis::Dbus", "Active"); + } else if (uart == Spec::kUarts.kUart0) { + const auto* uart_data = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&uart_data](std::byte* storage) noexcept { *storage = *uart_data++; }, + data.uart_data.size()); + monitor_.tick("Chassis::Uart0", "Active"); + } + } + + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { + const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); + bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); + monitor_.tick("Chassis::Imu", "Acc"); + } + + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + monitor_.tick("Chassis::Imu", "Gyr"); + const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + + auto snapshot = + bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); + if (!snapshot) + return; + } + + device::Bmi088Ekf bmi088_; + device::BoardClockLifter board_clock_lifter_; + OutputInterface& tf_; + + device::Dr16 dr16_; + device::LkMotor gimbal_bottom_yaw_motor_; + device::DjiMotor chassis_wheel_motors_[4]; + device::DjiMotor chassis_steer_motors_[4]; + + device::DjiMotor chassis_front_climber_motor_[2]; + device::LkMotor chassis_back_climber_motor_[2]; + + device::Supercap supercap_; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; + OutputInterface referee_serial_; + OutputInterface chassis_yaw_velocity_imu_; + OutputInterface chassis_pitch_imu_; + + StatusMonitor monitor_{}; + std::unique_ptr board_; + }; + + struct CommandTransmitter : public rmcs_executor::Component { + std::function fn; + + template + explicit CommandTransmitter(Fn&& fn) + : fn{std::forward(fn)} {} + + void update() override { fn(); } + }; + + void + status_service_callback(const std::shared_ptr& response) { + response->success = true; + + auto feedback_message = std::ostringstream{}; + auto text = [&](std::format_string format, Args&&... args) { + std::println(feedback_message, format, std::forward(args)...); + }; + + text( + " bottom_yaw_motor_zero_point: {}", + chassis_board_->gimbal_bottom_yaw_motor_.last_raw_angle()); + text(" pitch_motor_zero_point: {}", gimbal_board_->gimbal_pitch_motor_.last_raw_angle()); + text( + " top_yaw_motor_zero_point: {}", + gimbal_board_->gimbal_top_yaw_motor_.last_raw_angle()); + + text(""); + for (auto&& [index, motor] : + std::views::zip(kPosition, chassis_board_->chassis_steer_motors_)) { + text(" {}_zero_point: {}", index, motor.last_raw_angle()); + } + + text("\nGimbalBoard Status:"); + for (const auto& line : gimbal_board_->status()) { + text("> {}", line); + } + + text("\nChassisBoard Status:"); + for (const auto& line : chassis_board_->status()) { + text("> {}", line); + } + + response->message = feedback_message.str(); + } + + void command_update() { + gimbal_board_->command_update(); + chassis_board_->command_update(); + } + std::shared_ptr command_component_{ + create_partner_component( + get_component_name() + "_command", [this] { command_update(); })}; + + InputInterface timestamp_; + OutputInterface tf_; + + OutputInterface camera_transform_; + OutputInterface barrel_direction_; + OutputInterface yaw_velocity_; + + std::unique_ptr gimbal_board_; + std::unique_ptr chassis_board_; + + std::shared_ptr> status_service_; +}; + +} // namespace rmcs_core::hardware + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Sentry, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp index 3da6f172e..b5e7640e1 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp @@ -142,7 +142,7 @@ class DeformableInfantry static Shape::Color chassis_direction_indicator_color(rmcs_msgs::ChassisMode mode) { switch (mode) { - case rmcs_msgs::ChassisMode::SPIN: return Shape::Color::GREEN; + case rmcs_msgs::ChassisMode::SPIN_FAST: return Shape::Color::GREEN; case rmcs_msgs::ChassisMode::AUTO: return Shape::Color::CYAN; case rmcs_msgs::ChassisMode::STEP_DOWN: return Shape::Color::PINK; default: return Shape::Color::WHITE; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp index 4dda2cede..6fe24a0c1 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp @@ -318,7 +318,7 @@ class Hero return static_cast(degrees); }; // chassis_direction_indicator_.set_color( - // chassis_mode == rmcs_msgs::ChassisMode::SPIN ? Shape::Color::GREEN + // chassis_mode == rmcs_msgs::ChassisMode::SPIN_FAST ? Shape::Color::GREEN // : Shape::Color::PINK); // chassis_direction_indicator_.set_angle(to_referee_angle(*chassis_angle_), 30); const bool left_track_active = @@ -467,4 +467,4 @@ class Hero #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::Hero, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::Hero, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp index d78f8add4..f284b57a1 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp @@ -107,7 +107,7 @@ class Infantry }; chassis_direction_indicator_.set_color( chassis_mode == rmcs_msgs::ChassisMode::SPIN_FAST ? Shape::Color::GREEN - : Shape::Color::PINK); + : Shape::Color::PINK); chassis_direction_indicator_.set_angle(to_referee_angle(*chassis_angle_), 30); bool chassis_control_direction_indicator_visible = false; diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp index a279b92c7..182b95cc7 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp @@ -12,15 +12,10 @@ enum class ChassisMode : uint8_t { LAUNCH_RAMP, ALIGNMENT, ALIGNMENT_POWERED, - CLIMB, }; -constexpr auto is_powered(ChassisMode mode) noexcept { - return mode == ChassisMode::ALIGNMENT_POWERED || mode == ChassisMode::LAUNCH_RAMP - || mode == ChassisMode::CLIMB; -} -constexpr auto is_spining(ChassisMode mode) noexcept { - return mode == ChassisMode::SPIN_SLOW || mode == ChassisMode::SPIN_FAST; +constexpr auto need_power(ChassisMode mode) noexcept { + return mode == ChassisMode::ALIGNMENT_POWERED || mode == ChassisMode::LAUNCH_RAMP; } } // namespace rmcs_msgs diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp index 1cccd175a..b30d59be4 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp @@ -20,7 +20,6 @@ #include "mouse.hpp" // IWYU pragma: export #include "robot_color.hpp" // IWYU pragma: export #include "robot_id.hpp" // IWYU pragma: export -#include "sentry_event.hpp" // IWYU pragma: export #include "serial_interface.hpp" // IWYU pragma: export #include "shoot_mode.hpp" // IWYU pragma: export #include "shoot_status.hpp" // IWYU pragma: export @@ -50,7 +49,6 @@ constexpr auto to_string(ChassisMode mode) noexcept -> const char* { case ChassisMode::SPIN_SLOW: return "SPIN_SLOW"; case ChassisMode::ALIGNMENT: return "ALIGNMENT"; case ChassisMode::ALIGNMENT_POWERED: return "ALIGNMENT_POWERED"; - case ChassisMode::CLIMB: return "CLIMB"; } return "INVALID"; } From fb9518c00a2f4f43185ce0789c01eeed1c68c7de Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sat, 18 Jul 2026 17:55:26 +0800 Subject: [PATCH 38/86] fix: No response of switching item in scan-remote --- .script/scan-remote | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/.script/scan-remote b/.script/scan-remote index e26bc9cdc..3e752f5d4 100755 --- a/.script/scan-remote +++ b/.script/scan-remote @@ -19,7 +19,7 @@ from colorama import Fore, Style SSH_PORT = 2022 SSH_USER = "root" -CONNECT_TIMEOUT = 0.5 +CONNECT_TIMEOUT = 0.75 BANNER_TIMEOUT = 0.35 SSH_PROBE_TIMEOUT = 2.0 DEFAULT_SCAN_SEGMENTS = range(1, 6) @@ -768,10 +768,14 @@ def interactive_scan_and_select(networks): if key == "next": selected["index"] = (selected["index"] + 1) % len(candidates) + network, result = candidates[selected["index"]] + selected["key"] = (network, result["ip"]) render_prompt() return None if key == "prev": selected["index"] = (selected["index"] - 1) % len(candidates) + network, result = candidates[selected["index"]] + selected["key"] = (network, result["ip"]) render_prompt() return None if key == "enter": From 1cc332c18a26678c777ba0e240f2a837ae698fb4 Mon Sep 17 00:00:00 2001 From: FloatPigeon Date: Sun, 19 Jul 2026 20:11:54 +0800 Subject: [PATCH 39/86] feat: Add VT13 remote control and dual control arbitration (#93) --- .../steering-hero-little-six-friction.yaml | 2 +- .../hardware/deformable-infantry-omni-b.cpp | 27 +- .../src/hardware/deformable-infantry-omni.cpp | 9 +- .../rmcs_core/src/hardware/device/dr16.hpp | 107 +++-- .../src/hardware/device/remote_control.hpp | 165 ++++++++ .../rmcs_core/src/hardware/device/vt13.hpp | 386 ++++++++++++++++++ rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 8 +- .../rmcs_core/src/hardware/omni_infantry.cpp | 9 +- rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp | 9 +- .../steering-hero-little-six-friction.cpp | 29 +- 10 files changed, 681 insertions(+), 70 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml index 1f4534803..5ef17bf21 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml @@ -8,7 +8,6 @@ rmcs_executor: - rmcs_core::referee::command::Interaction -> referee_interaction - rmcs_core::referee::command::interaction::Ui -> referee_ui - rmcs_core::referee::app::ui::Hero -> referee_ui_hero - - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui - rmcs_core::referee::Command -> referee_command - rmcs_core::controller::gimbal::HeroGimbalController -> gimbal_controller @@ -38,6 +37,7 @@ rmcs_executor: - rmcs::AutoAimCapturerComponent -> auto_aim_capturer - rmcs::AutoAimComponent -> auto_aim_component + - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index 1183c9b1f..8efa589ba 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -31,7 +31,9 @@ #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" +#include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" +#include "hardware/device/vt13.hpp" #include "hardware/util/status_monitor.hpp" namespace rmcs_core::hardware { @@ -58,6 +60,8 @@ class DeformableInfantryOmniB tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); + remote_control_ = std::make_unique(*this); + bottom_board_ = std::make_unique( *this, *command_, get_parameter("serial_filter_bottom_board").as_string()); top_board_ = std::make_unique( @@ -79,6 +83,7 @@ class DeformableInfantryOmniB void update() override { bottom_board_->update(); top_board_->update(); + remote_control_->update(); using namespace rmcs_description; *camera_transform_ = fast_tf::lookup_transform(*tf_); @@ -121,7 +126,8 @@ class DeformableInfantryOmniB explicit TopBoard( DeformableInfantryOmniB& status, Component& command, const std::string& serial_filter = {}) - : tf_{status.tf_} + : status_{status} + , tf_{status.tf_} , bmi088_{device::Bmi088Ekf::Config{ .body_to_sensor = Eigen::AngleAxisd{std::numbers::pi / 2.0, Eigen::Vector3d::UnitX()} @@ -163,8 +169,11 @@ class DeformableInfantryOmniB .capture_timestamp = true, .pull = librmcs::data::GpioPull::kUp, }); - } + board_->start_transmit().uart_config(Spec::kUarts.kUart0, {.baudrate = 921600}); + + status_.remote_control_->register_vt13(&vt13_); + } ~TopBoard() override = default; [[nodiscard]] auto gimbal_yaw_velocity() const -> double { @@ -177,6 +186,7 @@ class DeformableInfantryOmniB } void update() { + vt13_.update_status(); gimbal_pitch_motor_.update_status(); gimbal_left_friction_.update_status(); gimbal_right_friction_.update_status(); @@ -243,6 +253,11 @@ class DeformableInfantryOmniB } } + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + if (uart == Spec::kUarts.kUart0) + vt13_.store_status(data.uart_data); + } + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); @@ -277,6 +292,7 @@ class DeformableInfantryOmniB auto status() const -> std::vector { return monitor_.text(); } + DeformableInfantryOmniB& status_; OutputInterface& tf_; OutputInterface gimbal_yaw_velocity_bmi088_; OutputInterface gimbal_pitch_velocity_bmi088_; @@ -286,6 +302,7 @@ class DeformableInfantryOmniB device::Bmi088Ekf bmi088_; device::BoardClockLifter board_clock_lifter_; + device::Vt13 vt13_; device::LkMotor gimbal_pitch_motor_; device::DjiMotor gimbal_left_friction_; device::DjiMotor gimbal_right_friction_; @@ -371,8 +388,9 @@ class DeformableInfantryOmniB auto options = librmcs::board::AdvancedOptions{}; options.dangerously_skip_version_checks = true; board_ = std::make_unique(*this, serial_filter, options); - } + status_.remote_control_->register_dr16(&dr16_); + } void update() { imu_.update_status(); *chassis_yaw_velocity_imu_ = imu_.gz(); @@ -584,7 +602,7 @@ class DeformableInfantryOmniB device::Bmi088 imu_{1000, 0.2, 0.0}; device::LkMotor gimbal_yaw_motor_{status_, command_, "/gimbal/yaw"}; - device::Dr16 dr16_{status_}; + device::Dr16 dr16_; device::DjiMotor chassis_wheel_motors_[4]{ device::DjiMotor{status_, command_, "/chassis/left_front_wheel"}, @@ -844,6 +862,7 @@ class DeformableInfantryOmniB std::unique_ptr bottom_board_; std::unique_ptr top_board_; + std::unique_ptr remote_control_; std::shared_ptr command_; uint32_t cmd_tick_ = 0; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index 6e9242e28..83c40c97b 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -32,6 +32,7 @@ #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" +#include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" #include "hardware/util/status_monitor.hpp" @@ -59,6 +60,8 @@ class DeformableInfantryOmni tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); + remote_control_ = std::make_unique(*this); + bottom_board_ = std::make_unique( *this, *command_, get_parameter("serial_filter_bottom_board").as_string()); top_board_ = std::make_unique( @@ -80,6 +83,7 @@ class DeformableInfantryOmni void update() override { bottom_board_->update(); top_board_->update(); + remote_control_->update(); using namespace rmcs_description; *camera_transform_ = fast_tf::lookup_transform(*tf_); @@ -193,6 +197,8 @@ class DeformableInfantryOmni auto options = librmcs::board::AdvancedOptions{}; options.dangerously_skip_version_checks = true; board_ = std::make_unique(*this, serial_filter, options); + + status_.remote_control_->register_dr16(&dr16_); } void update() { @@ -406,7 +412,7 @@ class DeformableInfantryOmni device::Bmi088 imu_{1000, 0.2, 0.0}; device::LkMotor gimbal_yaw_motor_{status_, command_, "/gimbal/yaw"}; - device::Dr16 dr16_{status_}; + device::Dr16 dr16_{}; device::DjiMotor chassis_wheel_motors_[4]{ device::DjiMotor{status_, command_, "/chassis/left_front_wheel"}, @@ -848,6 +854,7 @@ class DeformableInfantryOmni std::unique_ptr bottom_board_; std::unique_ptr top_board_; + std::unique_ptr remote_control_; std::shared_ptr command_; uint32_t cmd_tick_ = 0; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp index 75152b179..ba0ec4fac 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp @@ -6,11 +6,9 @@ #include #include +#include #include -#include -#include -#include #include #include #include @@ -19,32 +17,7 @@ namespace rmcs_core::hardware::device { class Dr16 { public: - explicit Dr16(rmcs_executor::Component& component) { - component.register_output( - "/remote/joystick/right", joystick_right_output_, Eigen::Vector2d::Zero()); - component.register_output( - "/remote/joystick/left", joystick_left_output_, Eigen::Vector2d::Zero()); - - component.register_output( - "/remote/switch/right", switch_right_output_, rmcs_msgs::Switch::UNKNOWN); - component.register_output( - "/remote/switch/left", switch_left_output_, rmcs_msgs::Switch::UNKNOWN); - - component.register_output( - "/remote/mouse/velocity", mouse_velocity_output_, Eigen::Vector2d::Zero()); - component.register_output("/remote/mouse/mouse_wheel", mouse_wheel_output_); - - component.register_output("/remote/mouse", mouse_output_); - std::memset(&*mouse_output_, 0, sizeof(*mouse_output_)); - component.register_output("/remote/keyboard", keyboard_output_); - std::memset(&*keyboard_output_, 0, sizeof(*keyboard_output_)); - - component.register_output("/remote/rotary_knob", rotary_knob_output_); - - // Simulate the rotary knob as a switch, with anti-shake algorithm. - component.register_output( - "/remote/rotary_knob_switch", rotary_knob_switch_output_, rmcs_msgs::Switch::UNKNOWN); - } + Dr16() = default; void store_status(const std::byte* uart_data, size_t uart_data_length) { if (uart_data_length != 6 + 8 + 4) @@ -73,9 +46,17 @@ class Dr16 { std::memcpy(&part3, uart_data, 4); uart_data += 4; data_part3_.store(part3, std::memory_order::relaxed); + + last_remote_control_received_at_ = Clock::now(); + valid_ = true; } void update_status() { + const auto now = Clock::now(); + refresh_validity(now); + if (!valid_) + return; + auto part1 alignas(uint64_t) = std::bit_cast(data_part1_.load(std::memory_order::relaxed)); @@ -110,19 +91,6 @@ class Dr16 { keyboard_ = part3.keyboard; rotary_knob_ = channel_to_double(part3.rotary_knob); - *joystick_right_output_ = joystick_right(); - *joystick_left_output_ = joystick_left(); - - *switch_right_output_ = switch_right(); - *switch_left_output_ = switch_left(); - - *mouse_velocity_output_ = mouse_velocity(); - *mouse_wheel_output_ = mouse_wheel(); - - *mouse_output_ = mouse(); - *keyboard_output_ = keyboard(); - - *rotary_knob_output_ = rotary_knob(); update_rotary_knob_switch(); } @@ -182,6 +150,10 @@ class Dr16 { rmcs_msgs::Mouse mouse() const { return std::bit_cast(mouse_); } rmcs_msgs::Keyboard keyboard() const { return std::bit_cast(keyboard_); } + rmcs_msgs::Switch rotary_knob_switch() const { return rotary_knob_switch_; } + + bool valid() const noexcept { return valid_; } + double rotary_knob() const { return rotary_knob_; } double mouse_wheel() const { return mouse_wheel_; } @@ -193,7 +165,7 @@ class Dr16 { constexpr double divider = 0.7, anti_shake_shift = 0.05; double upper_divider = divider, lower_divider = -divider; - auto& switch_value = *rotary_knob_switch_output_; + auto switch_value = rotary_knob_switch_; if (switch_value == rmcs_msgs::Switch::UP) upper_divider -= anti_shake_shift, lower_divider -= anti_shake_shift; else if (switch_value == rmcs_msgs::Switch::MIDDLE) @@ -201,7 +173,7 @@ class Dr16 { else if (switch_value == rmcs_msgs::Switch::DOWN) upper_divider += anti_shake_shift, lower_divider += anti_shake_shift; - const auto knob_value = -*rotary_knob_output_; + const auto knob_value = -rotary_knob_; if (knob_value > upper_divider) { switch_value = rmcs_msgs::Switch::UP; } else if (knob_value < lower_divider) { @@ -209,6 +181,33 @@ class Dr16 { } else { switch_value = rmcs_msgs::Switch::MIDDLE; } + rotary_knob_switch_ = switch_value; + } + + using Clock = std::chrono::steady_clock; + using TimePoint = Clock::time_point; + + static constexpr auto kFreshTimeout = std::chrono::milliseconds(500); + + void refresh_validity(const TimePoint now) { + if (!valid_ || now - last_remote_control_received_at_ <= kFreshTimeout) + return; + + reset_remote_control_state(); + valid_ = false; + } + + void reset_remote_control_state() { + joystick_right_ = Vector::zero(); + joystick_left_ = Vector::zero(); + switch_right_ = Switch::kUnknown; + switch_left_ = Switch::kUnknown; + mouse_velocity_ = Vector::zero(); + mouse_wheel_ = 0.0; + mouse_ = Mouse::zero(); + keyboard_ = Keyboard::zero(); + rotary_knob_ = 0.0; + rotary_knob_switch_ = rmcs_msgs::Switch::UNKNOWN; } struct [[gnu::packed]] Dr16DataPart1 { @@ -270,27 +269,15 @@ class Dr16 { Switch switch_left_ = Switch::kUnknown; Vector mouse_velocity_ = Vector::zero(); + double mouse_wheel_ = 0.0; Mouse mouse_ = Mouse::zero(); Keyboard keyboard_ = Keyboard::zero(); double rotary_knob_ = 0.0; - double mouse_wheel_ = 0.0; - - rmcs_executor::Component::OutputInterface joystick_right_output_; - rmcs_executor::Component::OutputInterface joystick_left_output_; - - rmcs_executor::Component::OutputInterface switch_right_output_; - rmcs_executor::Component::OutputInterface switch_left_output_; - - rmcs_executor::Component::OutputInterface mouse_velocity_output_; - rmcs_executor::Component::OutputInterface mouse_wheel_output_; - - rmcs_executor::Component::OutputInterface mouse_output_; - rmcs_executor::Component::OutputInterface keyboard_output_; - - rmcs_executor::Component::OutputInterface rotary_knob_output_; - rmcs_executor::Component::OutputInterface rotary_knob_switch_output_; + rmcs_msgs::Switch rotary_knob_switch_ = rmcs_msgs::Switch::UNKNOWN; + TimePoint last_remote_control_received_at_ = TimePoint::min(); + bool valid_ = false; }; } // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp new file mode 100644 index 000000000..c61b874e7 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp @@ -0,0 +1,165 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include "hardware/device/dr16.hpp" +#include "hardware/device/vt13.hpp" + +namespace rmcs_core::hardware::device { + +/* +遥控输入仲裁： +- vt13 valid S挡：vt13主控 | 比赛用 +- vt13 valid C挡：等同于dr16双下 | 疯车救车 +- 其他情况：dr16主控；dr16无效则进入空安全态 +- 旋钮始终来自 dr16，dr16 无效则清零 +*/ + +class RemoteControl { +public: + explicit RemoteControl(rmcs_executor::Component& component) { + component.register_output( + "/remote/joystick/right", joystick_right_output_, Eigen::Vector2d::Zero()); + component.register_output( + "/remote/joystick/left", joystick_left_output_, Eigen::Vector2d::Zero()); + + component.register_output( + "/remote/switch/right", switch_right_output_, rmcs_msgs::Switch::UNKNOWN); + component.register_output( + "/remote/switch/left", switch_left_output_, rmcs_msgs::Switch::UNKNOWN); + + component.register_output("/remote/rotary_knob", rotary_knob_output_, 0.0); + component.register_output( + "/remote/rotary_knob_switch", rotary_knob_switch_output_, rmcs_msgs::Switch::UNKNOWN); + + component.register_output( + "/remote/mouse/velocity", mouse_velocity_output_, Eigen::Vector2d::Zero()); + component.register_output("/remote/mouse/mouse_wheel", mouse_wheel_output_, 0.0); + + component.register_output("/remote/mouse", mouse_output_, rmcs_msgs::Mouse::zero()); + component.register_output( + "/remote/keyboard", keyboard_output_, rmcs_msgs::Keyboard::zero()); + } + + void register_dr16(Dr16* dr16) { dr16_ = dr16; } + void register_vt13(Vt13* vt13) { vt13_ = vt13; } + + void update() { + const auto control_source = select_control_source(); + const auto snapshot = build_snapshot(control_source); + + *joystick_right_output_ = snapshot.joystick_right; + *joystick_left_output_ = snapshot.joystick_left; + + *switch_right_output_ = snapshot.switch_right; + *switch_left_output_ = snapshot.switch_left; + + *mouse_velocity_output_ = snapshot.mouse_velocity; + *mouse_wheel_output_ = snapshot.mouse_wheel; + + *mouse_output_ = snapshot.mouse; + *keyboard_output_ = snapshot.keyboard; + + if (dr16_ && dr16_->valid()) { + *rotary_knob_output_ = dr16_->rotary_knob(); + *rotary_knob_switch_output_ = dr16_->rotary_knob_switch(); + } else { + *rotary_knob_output_ = 0.0; + *rotary_knob_switch_output_ = rmcs_msgs::Switch::UNKNOWN; + } + } + +private: + enum class ControlSource { + kDr16, + kVt13Sport, + kCineSafe, + kInvalidSafe, + }; + + struct Snapshot { + Eigen::Vector2d joystick_right = Eigen::Vector2d::Zero(); + Eigen::Vector2d joystick_left = Eigen::Vector2d::Zero(); + + rmcs_msgs::Switch switch_right = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Switch switch_left = rmcs_msgs::Switch::UNKNOWN; + + Eigen::Vector2d mouse_velocity = Eigen::Vector2d::Zero(); + double mouse_wheel = 0.0; + + rmcs_msgs::Mouse mouse = rmcs_msgs::Mouse::zero(); + rmcs_msgs::Keyboard keyboard = rmcs_msgs::Keyboard::zero(); + }; + + ControlSource select_control_source() const { + if (vt13_ && vt13_->valid()) { + switch (vt13_->mode_switch()) { + case Vt13::ModeSwitch::kSport: return ControlSource::kVt13Sport; + case Vt13::ModeSwitch::kCine: return ControlSource::kCineSafe; + case Vt13::ModeSwitch::kNormal: + case Vt13::ModeSwitch::kUnknown: break; + } + } + + return (dr16_ && dr16_->valid()) ? ControlSource::kDr16 : ControlSource::kInvalidSafe; + } + + Snapshot build_snapshot(ControlSource source) const { + Snapshot snapshot{}; + switch (source) { + case ControlSource::kDr16: + snapshot.joystick_right = dr16_->joystick_right(); + snapshot.joystick_left = dr16_->joystick_left(); + snapshot.switch_right = dr16_->switch_right(); + snapshot.switch_left = dr16_->switch_left(); + snapshot.mouse_velocity = dr16_->mouse_velocity(); + snapshot.mouse_wheel = dr16_->mouse_wheel(); + snapshot.mouse = dr16_->mouse(); + snapshot.keyboard = dr16_->keyboard(); + break; + case ControlSource::kVt13Sport: + snapshot.joystick_right = vt13_->joystick_right(); + snapshot.joystick_left = vt13_->joystick_left(); + snapshot.switch_right = rmcs_msgs::Switch::MIDDLE; + snapshot.switch_left = rmcs_msgs::Switch::MIDDLE; + snapshot.mouse_velocity = vt13_->mouse_velocity(); + snapshot.mouse_wheel = vt13_->mouse_wheel(); + snapshot.mouse = vt13_->mouse(); + snapshot.keyboard = vt13_->keyboard(); + break; + case ControlSource::kCineSafe: + snapshot.switch_right = rmcs_msgs::Switch::DOWN; + snapshot.switch_left = rmcs_msgs::Switch::DOWN; + break; + case ControlSource::kInvalidSafe: break; + } + + return snapshot; + } + + Dr16* dr16_{nullptr}; + Vt13* vt13_{nullptr}; + + rmcs_executor::Component::OutputInterface joystick_right_output_; + rmcs_executor::Component::OutputInterface joystick_left_output_; + + rmcs_executor::Component::OutputInterface switch_right_output_; + rmcs_executor::Component::OutputInterface switch_left_output_; + + rmcs_executor::Component::OutputInterface rotary_knob_output_; + rmcs_executor::Component::OutputInterface rotary_knob_switch_output_; + + rmcs_executor::Component::OutputInterface mouse_velocity_output_; + rmcs_executor::Component::OutputInterface mouse_wheel_output_; + + rmcs_executor::Component::OutputInterface mouse_output_; + rmcs_executor::Component::OutputInterface keyboard_output_; +}; + +} // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp new file mode 100644 index 000000000..619fe4581 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp @@ -0,0 +1,386 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware::device { + +class Vt13 { +public: + enum class ModeSwitch : uint8_t { + kUnknown = 0, + kCine = 1, + kNormal = 2, + kSport = 3, + }; + + Vt13() = default; + + void store_status(std::span uart_data) { + store_calls_.fetch_add(1, std::memory_order_relaxed); + received_bytes_.fetch_add(uart_data.size(), std::memory_order_relaxed); + + const auto written = data_buffer_.emplace_back_n( + [iter = uart_data.cbegin()](std::byte* storage) mutable noexcept { + *storage = *iter++; + }, + uart_data.size()); + if (written != uart_data.size()) { + const auto dropped = uart_data.size() - written; + overflow_count_.fetch_add(1, std::memory_order_relaxed); + overflow_dropped_bytes_.fetch_add(dropped, std::memory_order_relaxed); + if (should_log_overflow()) { + RCLCPP_WARN( + logger_, "VT13 input buffer overflow: dropped %zu of %zu bytes", dropped, + uart_data.size()); + } + } + } + + void update_status() { + const auto now = Clock::now(); + auto readable = data_buffer_.readable(); + peak_readable_ = std::max(peak_readable_, readable); + + while (readable) { + ReadResult result = VerificationFailed{}; + + const std::byte front = *data_buffer_.peek_front(); + if (front == std::byte{0xa9}) + result = read_remote_control_data(readable, now); + else if (front == std::byte(0xa5)) + result = read_referee_style_data(readable, now); + else { + unknown_prefix_count_++; + if (should_log_verification_failure(now)) { + RCLCPP_WARN( + logger_, "VT13 unknown prefix: front=0x%02x readable=%zu", + std::to_integer(front), readable); + } + } + + if (std::holds_alternative(result)) { + break; + } + if (std::holds_alternative(result)) { + verification_failures_++; + data_buffer_.pop_front([](std::byte&&) noexcept {}); + readable--; + continue; + } + if (std::holds_alternative(result)) { + readable -= std::get(result).read; + continue; + } + } + + refresh_validity(now); + maybe_log_statistics(now); + } + + ModeSwitch mode_switch() const noexcept { return mode_switch_; } + bool valid() const noexcept { return valid_; } + + const Eigen::Vector2d& joystick_left() const noexcept { return joystick_left_; } + const Eigen::Vector2d& joystick_right() const noexcept { return joystick_right_; } + + const Eigen::Vector2d& mouse_velocity() const noexcept { return mouse_velocity_; } + double mouse_wheel() const noexcept { return mouse_wheel_; } + + rmcs_msgs::Mouse mouse() const noexcept { return mouse_; } + rmcs_msgs::Keyboard keyboard() const noexcept { return keyboard_; } + +private: + using Clock = std::chrono::steady_clock; + using TimePoint = Clock::time_point; + + static constexpr auto kFreshTimeout = std::chrono::milliseconds(500); + static constexpr auto kVerificationLogInterval = std::chrono::seconds(1); + static constexpr auto kOverflowLogInterval = std::chrono::seconds(1); + static constexpr auto kStatisticsLogInterval = std::chrono::seconds(5); + static constexpr std::size_t kRefereeFrameMaxSize = 256; + + struct Incomplete {}; + struct VerificationFailed {}; + struct Success { + std::size_t read; + }; + using ReadResult = std::variant; + + struct [[gnu::packed]] RemoteControlData { + static constexpr uint16_t kHeaderMagic = 0x53a9; + + uint16_t header; + + uint16_t joystick_channel0 : 11; + uint16_t joystick_channel1 : 11; + uint16_t joystick_channel2 : 11; + uint16_t joystick_channel3 : 11; + + uint8_t mode_switch : 2; + uint8_t pause_button : 1; + uint8_t left_custom_button : 1; + uint8_t right_custom_button : 1; + uint16_t dial : 11; + uint8_t trigger : 1; + uint8_t padding1 : 3; + + int16_t mouse_velocity_x; + int16_t mouse_velocity_y; + int16_t mouse_velocity_z; + uint8_t mouse_left : 2; + uint8_t mouse_right : 2; + uint8_t mouse_middle : 2; + uint8_t padding2 : 2; + + uint16_t keyboard; + + uint16_t crc16; + }; + + struct [[gnu::packed]] RefereeFrameHeader { + uint8_t sof; + uint16_t data_length; + uint8_t seq; + uint8_t crc8; + }; + + ReadResult read_remote_control_data(const std::size_t readable, const TimePoint now) { + if (readable < sizeof(RemoteControlData)) + return Incomplete{}; + + RemoteControlData data; + data_buffer_.peek_front_n( + [dst = reinterpret_cast(&data)](std::byte src) mutable noexcept { + *dst++ = src; + }, + sizeof(RemoteControlData)); + + if (data.header != RemoteControlData::kHeaderMagic) { + remote_bad_header_count_++; + if (should_log_verification_failure(now)) { + RCLCPP_WARN( + logger_, "VT13 remote control header invalid: header=0x%04x readable=%zu", + data.header, readable); + } + return VerificationFailed{}; + } + if (!rmcs_utility::dji_crc::verify_crc16(data)) { + remote_bad_crc_count_++; + if (should_log_verification_failure(now)) + RCLCPP_WARN(logger_, "VT13 remote control crc16 invalid: readable=%zu", readable); + return VerificationFailed{}; + } + + data_buffer_.pop_front_n([](std::byte&&) noexcept {}, sizeof(RemoteControlData)); + + update_remote_control_data(data); + valid_ = true; + last_remote_control_received_at_ = now; + remote_success_count_++; + return Success{sizeof(RemoteControlData)}; + } + + void update_remote_control_data(const RemoteControlData& data) { + mode_switch_ = static_cast(data.mode_switch + 1); + + joystick_right_ = { + channel_to_double(static_cast(data.joystick_channel1)), + -channel_to_double(static_cast(data.joystick_channel0)), + }; + joystick_left_ = { + channel_to_double(static_cast(data.joystick_channel2)), + -channel_to_double(static_cast(data.joystick_channel3)), + }; + + mouse_velocity_ = { + -data.mouse_velocity_y / 32768.0, + -data.mouse_velocity_x / 32768.0, + }; + mouse_wheel_ = -static_cast(data.mouse_velocity_z) / 32768.0; + + mouse_ = { + .left = static_cast(data.mouse_left), + .right = static_cast(data.mouse_right), + }; + keyboard_ = std::bit_cast(data.keyboard); + } + + ReadResult read_referee_style_data(const std::size_t readable, const TimePoint now) { + if (readable < sizeof(RefereeFrameHeader)) + return Incomplete{}; + + RefereeFrameHeader header; + data_buffer_.peek_front_n( + [dst = reinterpret_cast(&header)](std::byte src) mutable noexcept { + *dst++ = src; + }, + sizeof(RefereeFrameHeader)); + + if (!rmcs_utility::dji_crc::verify_crc8(header)) { + referee_bad_crc8_count_++; + if (should_log_verification_failure(now)) + RCLCPP_WARN(logger_, "VT13 referee header crc8 invalid: readable=%zu", readable); + return VerificationFailed{}; + } + + const std::size_t total_frame_size = + sizeof(RefereeFrameHeader) + 2 + header.data_length + 2; + if (total_frame_size > kRefereeFrameMaxSize) { + referee_oversize_count_++; + if (should_log_verification_failure(now)) { + RCLCPP_WARN( + logger_, "VT13 referee frame oversized: data_length=%u total=%zu readable=%zu", + header.data_length, total_frame_size, readable); + } + return VerificationFailed{}; + } + if (readable < total_frame_size) + return Incomplete{}; + + data_buffer_.pop_front_n([](std::byte&&) noexcept {}, total_frame_size); + referee_discarded_count_++; + return Success{total_frame_size}; + } + + bool should_log_verification_failure(const TimePoint now) { + if (last_verification_log_time_ != TimePoint::min() + && now - last_verification_log_time_ < kVerificationLogInterval) + return false; + last_verification_log_time_ = now; + return true; + } + + bool should_log_overflow() { + const auto now = Clock::now(); + if (last_overflow_log_time_ != TimePoint::min() + && now - last_overflow_log_time_ < kOverflowLogInterval) + return false; + + last_overflow_log_time_ = now; + return true; + } + + void refresh_validity(const TimePoint now) { + if (!valid_ || now - last_remote_control_received_at_ <= kFreshTimeout) + return; + + reset_remote_control_state(); + valid_ = false; + } + + void maybe_log_statistics(const TimePoint now) { + if (last_statistics_log_time_ == TimePoint::min()) { + last_statistics_log_time_ = now; + return; + } + + const auto elapsed = now - last_statistics_log_time_; + if (elapsed < kStatisticsLogInterval) + return; + + const auto readable = data_buffer_.readable(); + const auto store_calls = store_calls_.exchange(0, std::memory_order_relaxed); + const auto received_bytes = received_bytes_.exchange(0, std::memory_order_relaxed); + const auto overflow_count = overflow_count_.exchange(0, std::memory_order_relaxed); + const auto overflow_dropped_bytes = + overflow_dropped_bytes_.exchange(0, std::memory_order_relaxed); + const auto elapsed_seconds = std::chrono::duration(elapsed).count(); + + /* RCLCPP_INFO( + logger_, + "VT13 stats: rx=%.1f Hz %.1f B/s remote_ok=%zu verify_fail=%zu remote_bad_header=%zu " + "remote_bad_crc=%zu referee_discarded=%zu referee_bad_crc8=%zu referee_oversize=%zu " + "unknown_prefix=%zu overflow=%llu dropped=%llu readable=%zu peak=%zu valid=%s", + static_cast(store_calls) / elapsed_seconds, + static_cast(received_bytes) / elapsed_seconds, remote_success_count_, + verification_failures_, remote_bad_header_count_, remote_bad_crc_count_, + referee_discarded_count_, referee_bad_crc8_count_, referee_oversize_count_, + unknown_prefix_count_, static_cast(overflow_count), + static_cast(overflow_dropped_bytes), readable, peak_readable_, + valid_ ? "true" : "false"); + */ + + remote_success_count_ = 0; + verification_failures_ = 0; + remote_bad_header_count_ = 0; + remote_bad_crc_count_ = 0; + referee_discarded_count_ = 0; + referee_bad_crc8_count_ = 0; + referee_oversize_count_ = 0; + unknown_prefix_count_ = 0; + peak_readable_ = readable; + last_statistics_log_time_ = now; + } + + void reset_remote_control_state() { + mode_switch_ = ModeSwitch::kUnknown; + joystick_left_ = Eigen::Vector2d::Zero(); + joystick_right_ = Eigen::Vector2d::Zero(); + mouse_velocity_ = Eigen::Vector2d::Zero(); + mouse_wheel_ = 0; + mouse_ = rmcs_msgs::Mouse::zero(); + keyboard_ = rmcs_msgs::Keyboard::zero(); + } + + static double channel_to_double(int32_t value) { + value -= 1024; + if (-660 <= value && value <= 660) + return value / 660.0; + return 0.0; + } + + rclcpp::Logger logger_ = rclcpp::get_logger("vt13"); + rmcs_utility::RingBuffer data_buffer_{1024}; + + std::atomic store_calls_{0}; + std::atomic received_bytes_{0}; + std::atomic overflow_count_{0}; + std::atomic overflow_dropped_bytes_{0}; + + TimePoint last_remote_control_received_at_ = TimePoint::min(); + TimePoint last_verification_log_time_ = TimePoint::min(); + TimePoint last_overflow_log_time_ = TimePoint::min(); + TimePoint last_statistics_log_time_ = TimePoint::min(); + + bool valid_ = false; + std::size_t peak_readable_ = 0; + std::size_t remote_success_count_ = 0; + std::size_t verification_failures_ = 0; + std::size_t remote_bad_header_count_ = 0; + std::size_t remote_bad_crc_count_ = 0; + std::size_t referee_discarded_count_ = 0; + std::size_t referee_bad_crc8_count_ = 0; + std::size_t referee_oversize_count_ = 0; + std::size_t unknown_prefix_count_ = 0; + + ModeSwitch mode_switch_ = ModeSwitch::kUnknown; + + Eigen::Vector2d joystick_left_ = Eigen::Vector2d::Zero(); + Eigen::Vector2d joystick_right_ = Eigen::Vector2d::Zero(); + + Eigen::Vector2d mouse_velocity_ = Eigen::Vector2d::Zero(); + double mouse_wheel_ = 0; + + rmcs_msgs::Mouse mouse_ = rmcs_msgs::Mouse::zero(); + rmcs_msgs::Keyboard keyboard_ = rmcs_msgs::Keyboard::zero(); +}; + +} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index e0595c16b..d1c5b214f 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -19,6 +19,7 @@ #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" +#include "hardware/device/remote_control.hpp" #include "librmcs/board/rmcs_board_lite.hpp" namespace rmcs_core::hardware { @@ -78,6 +79,9 @@ class Flight return size; }; + remote_control_ = std::make_unique(*this); + remote_control_->register_dr16(&dr16_); + status_service_ = create_service( "/rmcs/service/robot_status", [this]( @@ -94,6 +98,7 @@ class Flight update_motors(); update_imu(); dr16_.update_status(); + remote_control_->update(); using namespace rmcs_description; *camera_transform_ = fast_tf::lookup_transform(*tf_); @@ -251,7 +256,8 @@ class Flight device::DjiMotor gimbal_right_friction_{*this, *command_component_, "/gimbal/right_friction"}; device::DjiMotor gimbal_bullet_feeder_{*this, *command_component_, "/gimbal/bullet_feeder"}; - device::Dr16 dr16_{*this}; + device::Dr16 dr16_; + std::unique_ptr remote_control_; device::Bmi088 bmi088_{1000.0, 0.2, 0.00}; OutputInterface gimbal_yaw_velocity_imu_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp index 62420a43b..b9589e4c8 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp @@ -26,6 +26,7 @@ #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" +#include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" namespace rmcs_core::hardware { @@ -53,7 +54,8 @@ class OmniInfantry , gimbal_left_friction_(*this, *infantry_command_, "/gimbal/left_friction") , gimbal_right_friction_(*this, *infantry_command_, "/gimbal/right_friction") , gimbal_bullet_feeder_(*this, *infantry_command_, "/gimbal/bullet_feeder") - , dr16_{*this} { + , dr16_{} + { for (auto& motor : chassis_wheel_motors_) motor.configure( @@ -132,6 +134,9 @@ class OmniInfantry Spec::kUarts.kUart1, {.uart_data = std::span{buffer, size}}); return size; }; + + remote_control_ = std::make_unique(*this); + remote_control_->register_dr16(&dr16_); } OmniInfantry(const OmniInfantry&) = delete; @@ -145,6 +150,7 @@ class OmniInfantry update_motors(); update_imu(); dr16_.update_status(); + remote_control_->update(); supercap_.update_status(); } @@ -350,6 +356,7 @@ class OmniInfantry device::DjiMotor gimbal_bullet_feeder_; device::Dr16 dr16_; + std::unique_ptr remote_control_; device::Bmi088Ekf bmi088_; device::BoardClockLifter board_clock_lifter_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp index dd92f1a4d..4233d7c76 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp @@ -19,6 +19,7 @@ #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" +#include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" #include "hardware/util/status_monitor.hpp" @@ -53,6 +54,8 @@ class Sentry status_service_callback(response); }); + remote_control_ = std::make_unique(*this); + gimbal_board_ = std::make_unique( *this, *command_component_, get_parameter("board_serial_gimbal_board").as_string()); @@ -68,6 +71,7 @@ class Sentry void update() override { gimbal_board_->update(); chassis_board_->update(); + remote_control_->update(); using namespace rmcs_description; *camera_transform_ = fast_tf::lookup_transform(*tf_); @@ -280,7 +284,7 @@ class Sentry Sentry& sentry, rmcs_executor::Component& sentry_command, std::string_view board_serial = {}) : tf_(sentry.tf_) - , dr16_(sentry) + , dr16_{} , gimbal_bottom_yaw_motor_(sentry, sentry_command, "/gimbal/bottom_yaw") , chassis_wheel_motors_( {sentry, sentry_command, "/chassis/left_front_wheel"}, @@ -358,6 +362,8 @@ class Sentry .enable_multi_turn_angle()); board_ = std::make_unique(*this, board_serial); + + sentry.remote_control_->register_dr16(&dr16_); } auto status() const -> std::vector { return monitor_.text(); } @@ -625,6 +631,7 @@ class Sentry std::unique_ptr gimbal_board_; std::unique_ptr chassis_board_; + std::unique_ptr remote_control_; std::shared_ptr> status_service_; }; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp index f20588c23..b216fa6eb 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp @@ -36,7 +36,9 @@ #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" +#include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" +#include "hardware/device/vt13.hpp" namespace rmcs_core::hardware { @@ -134,6 +136,8 @@ class SteeringHeroLittle gimbal_calibrate_subscription_callback(std::move(msg)); }); + remote_control_ = std::make_unique(*this); + top_board_ = std::make_unique( *this, *command_component_, get_parameter("board_serial_top_board").as_string()); @@ -154,6 +158,7 @@ class SteeringHeroLittle void update() override { top_board_->update(); bottom_board_->update(); + remote_control_->update(); tf_->set_state( bottom_board_->gimbal_bottom_yaw_motor_.angle() @@ -308,6 +313,10 @@ class SteeringHeroLittle "/gimbal/grayscale_sensor", grayscale_sensor_status_, false); board_ = std::make_unique(*this, board_serial); + + board_->start_transmit().uart_config(Spec::kUarts.kUart0, {.baudrate = 921600}); + + steering_hero.remote_control_->register_vt13(&vt13_); } void update() { @@ -316,6 +325,8 @@ class SteeringHeroLittle // can2_receive_rate_counter_.report_if_due(); // can3_receive_rate_counter_.report_if_due(); + vt13_.update_status(); + if (auto snapshot = bmi088_.snapshot()) { tf_->set_transform( snapshot->orientation.conjugate()); @@ -481,6 +492,12 @@ class SteeringHeroLittle } } + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + if (uart == Spec::kUarts.kUart0) { + vt13_.store_status(data.uart_data); + } + } + void gpio_digital_read_result_callback( const Spec::Gpio& gpio, const View::GpioDigital& data) override { @@ -516,6 +533,9 @@ class SteeringHeroLittle return *gimbal_yaw_velocity_imu_; } + [[nodiscard]] device::Vt13& vt13() noexcept { return vt13_; } + [[nodiscard]] const device::Vt13& vt13() const noexcept { return vt13_; } + rclcpp::Logger logger_; // CanReceiveRateCounter can0_receive_rate_counter_; // CanReceiveRateCounter can1_receive_rate_counter_; @@ -529,6 +549,7 @@ class SteeringHeroLittle device::Bmi088Ekf bmi088_; device::BoardClockLifter board_clock_lifter_; + device::Vt13 vt13_; device::LkMotor gimbal_top_yaw_motor_; device::LkMotor gimbal_pitch_motor_; device::DjiMotor gimbal_friction_wheels_[6]; @@ -559,7 +580,7 @@ class SteeringHeroLittle // , can2_receive_rate_counter_(logger_, "bottom/can2") // , can3_receive_rate_counter_(logger_, "bottom/can3") , imu_(1000, 0.2, 0.0) - , dr16_(steering_hero) + , dr16_{} , supercap_(steering_hero, steering_hero_command) , chassis_steering_motors_( {steering_hero, steering_hero_command, "/chassis/left_front_steering"}, @@ -669,6 +690,8 @@ class SteeringHeroLittle steering_hero.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); board_ = std::make_unique(*this, board_serial); + + steering_hero.remote_control_->register_dr16(&dr16_); } void update() { @@ -866,6 +889,9 @@ class SteeringHeroLittle } } + [[nodiscard]] device::Dr16& dr16() noexcept { return dr16_; } + [[nodiscard]] const device::Dr16& dr16() const noexcept { return dr16_; } + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { imu_.store_accelerometer_status(data.x, data.y, data.z); } @@ -915,6 +941,7 @@ class SteeringHeroLittle std::shared_ptr top_board_; std::shared_ptr bottom_board_; + std::unique_ptr remote_control_; }; } // namespace rmcs_core::hardware From efd99d7a5c92b43fb539b355e784f3ad457fd924 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sun, 19 Jul 2026 20:18:47 +0800 Subject: [PATCH 40/86] chore: Clean up code and apply format --- .../controller/chassis/deformable_chassis.cpp | 16 +-- .../chassis/deformable_joint_controller.cpp | 4 +- .../controller/chassis/deformable_mode.hpp | 8 +- .../deformable_omni_wheel_controller.cpp | 30 +++--- .../chassis/deformable_suspension.cpp | 97 +++++++++---------- .../shooting/friction_wheel_controller.cpp | 1 - .../rmcs_core/src/hardware/device/vt13.hpp | 47 +-------- .../rmcs_core/src/hardware/omni_infantry.cpp | 3 +- .../rmcs_core/src/referee/app/ui/infantry.cpp | 2 +- 9 files changed, 81 insertions(+), 127 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index a8688a6d3..397400cfe 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -53,14 +53,18 @@ class DeformableChassis register_output("/chassis/pitch_lock_active", pitch_lock_active_, false); register_output("/chassis/active_suspension/active", active_suspension_active_, false); register_output("/chassis/deformable/low_prone_active", low_prone_active_, false); - register_output("/chassis/deformable/symmetric_posture_target", symmetric_posture_target_, true); + register_output( + "/chassis/deformable/symmetric_posture_target", symmetric_posture_target_, true); register_output("/chassis/deformable/correction_inverted", correction_inverted_, false); - register_output("/chassis/deformable/min_angle_deg", min_angle_deg_, joint_mode_mgr_.min_angle()); - register_output("/chassis/deformable/max_angle_deg", max_angle_deg_, joint_mode_mgr_.max_angle()); register_output( - "/chassis/deformable/suspension_reference_angle_deg", - suspension_reference_angle_deg_, joint_mode_mgr_.suspension_reference_angle_deg()); - register_output("/chassis/deformable/reset_count", deformable_reset_count_, static_cast(0)); + "/chassis/deformable/min_angle_deg", min_angle_deg_, joint_mode_mgr_.min_angle()); + register_output( + "/chassis/deformable/max_angle_deg", max_angle_deg_, joint_mode_mgr_.max_angle()); + register_output( + "/chassis/deformable/suspension_reference_angle_deg", suspension_reference_angle_deg_, + joint_mode_mgr_.suspension_reference_angle_deg()); + register_output( + "/chassis/deformable/reset_count", deformable_reset_count_, static_cast(0)); for (size_t i = 0; i < kJointCount; ++i) { register_output( fmt::format("/chassis/deformable/{}_joint/posture_target_angle", kJointName[i]), diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp index 38bb7ec1c..88bfb2041 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp @@ -102,8 +102,8 @@ class DeformableJointController config_.eso.beta1 = load_parameter_or(*this, "eso_beta1", 3.0 * config_.eso.w0); config_.eso.beta2 = load_parameter_or(*this, "eso_beta2", 3.0 * config_.eso.w0 * config_.eso.w0); - config_.eso.beta3 = load_parameter_or( - *this, "eso_beta3", config_.eso.w0 * config_.eso.w0 * config_.eso.w0); + config_.eso.beta3 = + load_parameter_or(*this, "eso_beta3", config_.eso.w0 * config_.eso.w0 * config_.eso.w0); config_.eso.z3_limit = load_parameter_or(*this, "eso_z3_limit", 1e9); config_.nlesf.k1 = load_parameter_or(*this, "k1", 50.0); diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp index ca9aa4d64..dac11d423 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp @@ -82,8 +82,7 @@ class DeformableChassisModeManager { joint_posture_state_.ctrl_low_prone_active = keyboard.ctrl; joint_posture_state_.low_prone_active = joint_posture_state_.ctrl_low_prone_active || low_prone_enabled_by_toggle_; - joint_posture_state_.pitch_lock_active = - joint_posture_state_.ctrl_low_prone_active; + joint_posture_state_.pitch_lock_active = joint_posture_state_.ctrl_low_prone_active; update_suspension_mode_from_inputs_(switch_left, switch_right, keyboard, rotary_knob); update_posture_target_from_inputs_(switch_left, switch_right, keyboard, rotary_knob, dt); @@ -248,9 +247,8 @@ class DeformableChassisModeManager { if (posture_toggle_requested) { if (joint_posture_state_.suspension_active) { active_suspension_base_angle_ = - (std::abs(active_suspension_base_angle_ - max_angle_) < 1e-6) - ? min_angle_ - : max_angle_; + (std::abs(active_suspension_base_angle_ - max_angle_) < 1e-6) ? min_angle_ + : max_angle_; current_target_angle_ = active_suspension_base_angle_; apply_symmetric_target_ = true; joint_current_target_angle_.fill(current_target_angle_); diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp index 74bc3d6b7..81b7b230d 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp @@ -91,7 +91,7 @@ class DeformableOmniWheelController } private: - static constexpr size_t kWheelCount = 4; + static constexpr size_t kWheelCount = 4; static constexpr const char* kWheelName[] = { "left_front", "left_back", @@ -99,7 +99,7 @@ class DeformableOmniWheelController "right_front", }; static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - static constexpr double g_ = 9.81; + static constexpr double g_ = 9.81; struct ChassisControlTorque { Eigen::Vector2d torque; @@ -113,7 +113,7 @@ class DeformableOmniWheelController Eigen::Vector3d calculate_chassis_velocity(const Eigen::Vector4d& wheel_velocities) const { const auto& [w1, w2, w3, w4] = wheel_velocities; - const double a_plus_b = std::numbers::sqrt2 * std::max(*chassis_radius_, 1e-6); + const double a_plus_b = std::numbers::sqrt2 * std::max(*chassis_radius_, 1e-6); Eigen::Vector3d velocity; velocity.x() = -w1 - w2 + w3 + w4; velocity.y() = w1 - w2 - w3 + w4; @@ -133,16 +133,16 @@ class DeformableOmniWheelController result.torque.x() = translational_torque.norm(); const double a_plus_b = std::numbers::sqrt2 * std::max(*chassis_radius_, 1e-6); - result.torque.y() = (-std::numbers::sqrt2 / 4 * wheel_radius_) - * (moment_of_inertia_ / a_plus_b) - * angular_velocity_pid_calculator_.update(chassis_velocity_error[2]); + result.torque.y() = (-std::numbers::sqrt2 / 4 * wheel_radius_) + * (moment_of_inertia_ / a_plus_b) + * angular_velocity_pid_calculator_.update(chassis_velocity_error[2]); Eigen::Vector2d translational_torque_direction; if (result.torque.x() > 0) translational_torque_direction = translational_torque / result.torque.x(); else translational_torque_direction = Eigen::Vector2d::UnitX(); - auto& [x, y] = translational_torque_direction; + auto& [x, y] = translational_torque_direction; result.lambda = {-x + y, -x - y}; return result; @@ -167,13 +167,13 @@ class DeformableOmniWheelController const Eigen::Vector4d& wheel_pid_torques) const { const auto& [w1, w2, w3, w4] = wheel_velocities; - const auto& [x_max, y_max] = chassis_control_torque.torque; - const double y_sign = y_max > 0 ? 1.0 : -1.0; + const auto& [x_max, y_max] = chassis_control_torque.torque; + const double y_sign = y_max > 0 ? 1.0 : -1.0; const auto& [lambda_1, lambda_2] = chassis_control_torque.lambda; const auto& [t1, t2, t3, t4] = wheel_pid_torques; - const double rhombus_top = (friction_coefficient_ * mass_ * g_ * wheel_radius_) / 4; + const double rhombus_top = (friction_coefficient_ * mass_ * g_ * wheel_radius_) / 4; const double rhombus_right = rhombus_top / std::max(std::abs(lambda_1), std::abs(lambda_2)); const double a = 4 * k1_; @@ -189,14 +189,14 @@ class DeformableOmniWheelController Eigen::Vector2d result = Eigen::Vector2d::Constant(nan_); if (com_height_ > 1e-6) { - const double dir_x = -(lambda_1 + lambda_2) / 2.0; - const double dir_y = (lambda_1 - lambda_2) / 2.0; - const double coeff = -com_height_ / (std::numbers::sqrt2 * wheel_radius_); + const double dir_x = -(lambda_1 + lambda_2) / 2.0; + const double dir_y = (lambda_1 - lambda_2) / 2.0; + const double coeff = -com_height_ / (std::numbers::sqrt2 * wheel_radius_); const double gamma_1 = coeff * (+dir_x / chassis_radius_x_ + dir_y / chassis_radius_y_); const double gamma_2 = coeff * (-dir_x / chassis_radius_x_ + dir_y / chassis_radius_y_); const double force_to_torque = friction_coefficient_ * wheel_radius_; - const double rhs = force_to_torque * mass_ * g_ / 4.0; + const double rhs = force_to_torque * mass_ * g_ / 4.0; const std::vector half_planes = { {lambda_1 - force_to_torque * gamma_1, y_sign, rhs}, {-lambda_1 - force_to_torque * gamma_1, -y_sign, rhs}, @@ -223,7 +223,7 @@ class DeformableOmniWheelController static Eigen::Vector4d calculate_wheel_control_torques( ChassisControlTorque chassis_control_torque, Eigen::Vector4d wheel_pid_torques) { const auto& [lambda_1, lambda_2] = chassis_control_torque.lambda; - Eigen::Vector4d wheel_torques = { + Eigen::Vector4d wheel_torques = { +lambda_1 * chassis_control_torque.torque.x(), +lambda_2 * chassis_control_torque.torque.x(), -lambda_1 * chassis_control_torque.torque.x(), diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp index 3f562ba01..6da008098 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp @@ -3,8 +3,8 @@ #include #include #include -#include #include +#include #include #include @@ -29,14 +29,12 @@ class DeformableSuspension register_input("/chassis/active_suspension/active", active_suspension_active_); register_input("/chassis/deformable/reset_count", reset_count_, false); register_input("/chassis/deformable/low_prone_active", low_prone_active_); - register_input( - "/chassis/deformable/symmetric_posture_target", symmetric_posture_target_); + register_input("/chassis/deformable/symmetric_posture_target", symmetric_posture_target_); register_input("/chassis/deformable/correction_inverted", correction_inverted_); register_input("/chassis/deformable/min_angle_deg", min_angle_deg_); register_input("/chassis/deformable/max_angle_deg", max_angle_deg_); register_input( - "/chassis/deformable/suspension_reference_angle_deg", - suspension_reference_angle_deg_); + "/chassis/deformable/suspension_reference_angle_deg", suspension_reference_angle_deg_); register_input("/chassis/imu/pitch", chassis_imu_pitch_, false); register_input("/chassis/imu/roll", chassis_imu_roll_, false); @@ -54,12 +52,10 @@ class DeformableSuspension std::string{"/chassis/"} + kJointName[i] + "_joint/target_physical_angle", joint_target_angle_[i], nan_); register_output( - std::string{"/chassis/"} + kJointName[i] - + "_joint/target_physical_velocity", + std::string{"/chassis/"} + kJointName[i] + "_joint/target_physical_velocity", joint_target_velocity_[i], nan_); register_output( - std::string{"/chassis/"} + kJointName[i] - + "_joint/target_physical_acceleration", + std::string{"/chassis/"} + kJointName[i] + "_joint/target_physical_acceleration", joint_target_acceleration_[i], nan_); register_output( std::string{"/chassis/"} + kJointName[i] + "_joint/control_angle_error", @@ -115,9 +111,9 @@ class DeformableSuspension copy_joint_angle_states_(joint_angle_states); update_suspension_state_( *chassis_imu_pitch_ - pitch_offset_value_, *chassis_imu_roll_ - roll_offset_value_, - filtered_pitch_rate, filtered_roll_rate, *active_suspension_active_, - *low_prone_active_, *min_angle_deg_, *max_angle_deg_, - *suspension_reference_angle_deg_, *correction_inverted_, joint_angle_states, dt); + filtered_pitch_rate, filtered_roll_rate, *active_suspension_active_, *low_prone_active_, + *min_angle_deg_, *max_angle_deg_, *suspension_reference_angle_deg_, + *correction_inverted_, joint_angle_states, dt); const auto target_angles_rad = compute_joint_trajectory_targets_( posture_target_angles_rad, *active_suspension_active_, *low_prone_active_, @@ -159,9 +155,9 @@ class DeformableSuspension } void load_pid_( - const std::string& prefix, pid::PidCalculator& pid, double kp_default, - double ki_default, double kd_default, double integral_min_default, - double integral_max_default, double output_min_default, double output_max_default) { + const std::string& prefix, pid::PidCalculator& pid, double kp_default, double ki_default, + double kd_default, double integral_min_default, double integral_max_default, + double output_min_default, double output_max_default) { pid.kp = get_parameter_or(prefix + "kp", kp_default); pid.ki = get_parameter_or(prefix + "ki", ki_default); pid.kd = get_parameter_or(prefix + "kd", kd_default); @@ -173,47 +169,51 @@ class DeformableSuspension void load_config_() { joint_target_vel_limit_ = std::max( - deg_to_rad_(std::abs(get_parameter_or("target_physical_velocity_limit", 180.0))), - 1e-6); + deg_to_rad_(std::abs(get_parameter_or("target_physical_velocity_limit", 180.0))), 1e-6); joint_target_acc_limit_ = std::max( deg_to_rad_(std::abs(get_parameter_or("target_physical_acceleration_limit", 720.0))), 1e-6); suspension_target_vel_limit_ = std::max( - deg_to_rad_(std::abs(get_parameter_or( - "active_suspension_target_velocity_limit_deg", - get_parameter_or("target_physical_velocity_limit", 180.0)))), + deg_to_rad_( + std::abs(get_parameter_or( + "active_suspension_target_velocity_limit_deg", + get_parameter_or("target_physical_velocity_limit", 180.0)))), 1e-6); suspension_target_acc_limit_ = std::max( - deg_to_rad_(std::abs(get_parameter_or( - "active_suspension_target_acceleration_limit_deg", - get_parameter_or("target_physical_acceleration_limit", 720.0)))), + deg_to_rad_( + std::abs(get_parameter_or( + "active_suspension_target_acceleration_limit_deg", + get_parameter_or("target_physical_acceleration_limit", 720.0)))), 1e-6); load_pid_( - "active_suspension_pitch_outer_", pitch_outer_pid_, 8.0, 0.35, 0.28, -2.0, 2.0, - -3.0, 3.0); + "active_suspension_pitch_outer_", pitch_outer_pid_, 8.0, 0.35, 0.28, -2.0, 2.0, -3.0, + 3.0); load_pid_( - "active_suspension_pitch_inner_", pitch_inner_pid_, 2.0, 0.0, 0.0, -1.0, 1.0, - -0.785, 0.785); + "active_suspension_pitch_inner_", pitch_inner_pid_, 2.0, 0.0, 0.0, -1.0, 1.0, -0.785, + 0.785); load_pid_( - "active_suspension_roll_outer_", roll_outer_pid_, 8.0, 0.35, 0.28, -2.0, 2.0, - -3.0, 3.0); + "active_suspension_roll_outer_", roll_outer_pid_, 8.0, 0.35, 0.28, -2.0, 2.0, -3.0, + 3.0); load_pid_( - "active_suspension_roll_inner_", roll_inner_pid_, 2.0, 0.0, 0.0, -1.0, 1.0, - -0.785, 0.785); + "active_suspension_roll_inner_", roll_inner_pid_, 2.0, 0.0, 0.0, -1.0, 1.0, -0.785, + 0.785); active_correction_vel_limit_ = std::max( - deg_to_rad_(std::abs( - get_parameter_or("active_suspension_correction_velocity_limit_deg", 720.0))), + deg_to_rad_( + std::abs( + get_parameter_or("active_suspension_correction_velocity_limit_deg", 720.0))), 1e-6); active_correction_acc_limit_ = std::max( - deg_to_rad_(std::abs( - get_parameter_or("active_suspension_correction_acceleration_limit_deg", 3600.0))), + deg_to_rad_( + std::abs(get_parameter_or( + "active_suspension_correction_acceleration_limit_deg", 3600.0))), 1e-6); - active_rate_lpf_cutoff_hz_ = std::max( - get_parameter_or("active_suspension_rate_lpf_cutoff_hz", 10.0), 1e-6); + active_rate_lpf_cutoff_hz_ = + std::max(get_parameter_or("active_suspension_rate_lpf_cutoff_hz", 10.0), 1e-6); - calibration_wait_time_ = std::max(get_parameter_or("chassis_imu_calibration_wait_s", 2.0), 0.0); + calibration_wait_time_ = + std::max(get_parameter_or("chassis_imu_calibration_wait_s", 2.0), 0.0); calibration_sample_time_ = std::max(get_parameter_or("chassis_imu_calibration_sample_s", 3.0), 1e-6); } @@ -366,8 +366,7 @@ class DeformableSuspension const double right_roll_contribution = std::max(roll_diff, 0.0); correction_target_rad_[kLeftFront] = -(front_pitch_contribution + left_roll_contribution); - correction_target_rad_[kLeftBack] = - -(back_pitch_contribution + left_roll_contribution); + correction_target_rad_[kLeftBack] = -(back_pitch_contribution + left_roll_contribution); correction_target_rad_[kRightBack] = -(back_pitch_contribution + right_roll_contribution); correction_target_rad_[kRightFront] = @@ -380,7 +379,8 @@ class DeformableSuspension correction_target_rad_[kLeftFront] = front_pitch_contribution + left_roll_contribution; correction_target_rad_[kLeftBack] = back_pitch_contribution + left_roll_contribution; correction_target_rad_[kRightBack] = back_pitch_contribution + right_roll_contribution; - correction_target_rad_[kRightFront] = front_pitch_contribution + right_roll_contribution; + correction_target_rad_[kRightFront] = + front_pitch_contribution + right_roll_contribution; } } @@ -392,10 +392,10 @@ class DeformableSuspension const double min_susp_rad = deg_to_rad_(min_angle_deg - 5.0); for (size_t i = 0; i < kJointCount; ++i) { - const double base_angle = std::isfinite(base_joint_angles[i]) - ? base_joint_angles[i] - : (low_prone_override_active ? min_susp_rad - : deg_to_rad_(base_angle_deg)); + const double base_angle = + std::isfinite(base_joint_angles[i]) + ? base_joint_angles[i] + : (low_prone_override_active ? min_susp_rad : deg_to_rad_(base_angle_deg)); const double correction_min = min_susp_rad - base_angle; const double correction_max = max_target_rad - base_angle; @@ -419,7 +419,8 @@ class DeformableSuspension std::clamp(velocity_error / dt, -correction_acc_limit, correction_acc_limit); velocity_state += acceleration_state * dt; - velocity_state = std::clamp(velocity_state, -correction_vel_limit, correction_vel_limit); + velocity_state = + std::clamp(velocity_state, -correction_vel_limit, correction_vel_limit); angle_state += velocity_state * dt; const double next_error = target - angle_state; @@ -552,9 +553,7 @@ class DeformableSuspension } } - void publish_nan_joint_targets_() { - reset_all_controls_(); - } + void publish_nan_joint_targets_() { reset_all_controls_(); } InputInterface update_rate_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp index f0b277f1c..76beb6868 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp @@ -97,7 +97,6 @@ class FrictionWheelController last_switch_left_ = switch_left; last_keyboard_ = keyboard; } - } private: diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp index 619fe4581..4272bfda1 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp @@ -92,7 +92,6 @@ class Vt13 { } refresh_validity(now); - maybe_log_statistics(now); } ModeSwitch mode_switch() const noexcept { return mode_switch_; } @@ -286,50 +285,6 @@ class Vt13 { valid_ = false; } - void maybe_log_statistics(const TimePoint now) { - if (last_statistics_log_time_ == TimePoint::min()) { - last_statistics_log_time_ = now; - return; - } - - const auto elapsed = now - last_statistics_log_time_; - if (elapsed < kStatisticsLogInterval) - return; - - const auto readable = data_buffer_.readable(); - const auto store_calls = store_calls_.exchange(0, std::memory_order_relaxed); - const auto received_bytes = received_bytes_.exchange(0, std::memory_order_relaxed); - const auto overflow_count = overflow_count_.exchange(0, std::memory_order_relaxed); - const auto overflow_dropped_bytes = - overflow_dropped_bytes_.exchange(0, std::memory_order_relaxed); - const auto elapsed_seconds = std::chrono::duration(elapsed).count(); - - /* RCLCPP_INFO( - logger_, - "VT13 stats: rx=%.1f Hz %.1f B/s remote_ok=%zu verify_fail=%zu remote_bad_header=%zu " - "remote_bad_crc=%zu referee_discarded=%zu referee_bad_crc8=%zu referee_oversize=%zu " - "unknown_prefix=%zu overflow=%llu dropped=%llu readable=%zu peak=%zu valid=%s", - static_cast(store_calls) / elapsed_seconds, - static_cast(received_bytes) / elapsed_seconds, remote_success_count_, - verification_failures_, remote_bad_header_count_, remote_bad_crc_count_, - referee_discarded_count_, referee_bad_crc8_count_, referee_oversize_count_, - unknown_prefix_count_, static_cast(overflow_count), - static_cast(overflow_dropped_bytes), readable, peak_readable_, - valid_ ? "true" : "false"); - */ - - remote_success_count_ = 0; - verification_failures_ = 0; - remote_bad_header_count_ = 0; - remote_bad_crc_count_ = 0; - referee_discarded_count_ = 0; - referee_bad_crc8_count_ = 0; - referee_oversize_count_ = 0; - unknown_prefix_count_ = 0; - peak_readable_ = readable; - last_statistics_log_time_ = now; - } - void reset_remote_control_state() { mode_switch_ = ModeSwitch::kUnknown; joystick_left_ = Eigen::Vector2d::Zero(); @@ -383,4 +338,4 @@ class Vt13 { rmcs_msgs::Keyboard keyboard_ = rmcs_msgs::Keyboard::zero(); }; -} // namespace rmcs_core::hardware::device \ No newline at end of file +} // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp index b9589e4c8..b8c5c4af9 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp @@ -54,8 +54,7 @@ class OmniInfantry , gimbal_left_friction_(*this, *infantry_command_, "/gimbal/left_friction") , gimbal_right_friction_(*this, *infantry_command_, "/gimbal/right_friction") , gimbal_bullet_feeder_(*this, *infantry_command_, "/gimbal/bullet_feeder") - , dr16_{} - { + , dr16_{} { for (auto& motor : chassis_wheel_motors_) motor.configure( diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp index f284b57a1..d78f8add4 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp @@ -107,7 +107,7 @@ class Infantry }; chassis_direction_indicator_.set_color( chassis_mode == rmcs_msgs::ChassisMode::SPIN_FAST ? Shape::Color::GREEN - : Shape::Color::PINK); + : Shape::Color::PINK); chassis_direction_indicator_.set_angle(to_referee_angle(*chassis_angle_), 30); bool chassis_control_direction_indicator_visible = false; From 1e67576e124bf141d190953d9c7bc23a9abee41a Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Mon, 20 Jul 2026 23:13:59 +0800 Subject: [PATCH 41/86] feat: Refine auto-aim fire control and bringup configs --- .../rmcs_bringup/config/auto_aim_test.yaml | 26 +++++++++++--- .../config/deformable-infantry-omni-b.yaml | 13 +++---- .../config/deformable-infantry-omni.yaml | 13 +++---- rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 7 ++-- .../steering-hero-little-six-friction.yaml | 1 + .../rmcs_core/src/referee/app/ui/auto_aim.cpp | 34 +++++++++++++------ 6 files changed, 62 insertions(+), 32 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml b/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml index ebafa2e92..c57bc0dd9 100644 --- a/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml @@ -2,20 +2,20 @@ rmcs_executor: ros__parameters: update_rate: 1000.0 components: - # - rmcs::AutoAimPlayerComponent -> auto_aim_player - - rmcs::AutoAimVideoPlayerComponent -> auto_aim_video_player - - rmcs::AutoAimRecorderComponent -> auto_aim_recorder + - rmcs::AutoAimPlayerComponent -> auto_aim_player + # - rmcs::AutoAimVideoPlayerComponent -> auto_aim_video_player + # - rmcs::AutoAimRecorderComponent -> auto_aim_recorder - rmcs::AutoAimComponent -> auto_aim_component auto_aim_player: ros__parameters: - input_path: "/workspaces/data/autoaim/2026-07-11_22-39-06/" + input_path: "/workspaces/data/autoaim/自家大符/" loop_play: true auto_aim_video_player: ros__parameters: input_path: "/workspaces/data/autoaim/robot/rotate.avi" - framerate: 40.0 + framerate: 80.0 loop_play: true auto_aim_recorder: @@ -28,4 +28,20 @@ auto_aim_recorder: auto_aim_component: ros__parameters: + dangerous_fallback: "red" manual_shoot: false + camera_translation: [0., 0., 0.] + + fire_control: + bullet_speed: 22.5 + shoot_delay: 0.0 + offset_yaw: 0.0 + offset_pitch: 0.0 + attack_window: 120.0 + degraded_angle_speed: 12.0 + window_hysteresis: 0.2 + is_lazy_gimbal: false + attack_preaim: false + require_stable_command: true + yaw_tolerance: 0.07 + pitch_tolerance: 0.04 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index db46b0666..100f1a144 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -59,7 +59,7 @@ value_broadcaster: auto_aim_capturer: ros__parameters: camera_name: "" - exposure_us: 2000.0 + exposure_us: 4000.0 gain: 8.0 framerate: 120.0 invert_image: false @@ -77,10 +77,11 @@ auto_aim_component: camera_translation: [0.058, -0.08, 0.0] fire_control: bullet_speed: 22.5 - shoot_delay: 0.1 - offset_yaw: -0.0 - offset_pitch: +0.0 - attack_window: 120.0 + shoot_delay: 0.04 + offset_yaw: +2.5 + offset_pitch: +0.5 + attack_window: 80.0 + degraded_angle_speed: 12.0 window_hysteresis: 0.2 is_lazy_gimbal: false attack_preaim: false @@ -91,7 +92,7 @@ auto_aim_component: auto_aim_ui: ros__parameters: offset_x: 0.0 - offset_y: -0.08 + offset_y: +0.08 offset_z: 0.0 deformable_infantry: diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index cb2131048..a83b79cec 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -59,7 +59,7 @@ value_broadcaster: auto_aim_capturer: ros__parameters: camera_name: "" - exposure_us: 2000.0 + exposure_us: 4000.0 gain: 8.0 framerate: 120.0 invert_image: false @@ -77,10 +77,11 @@ auto_aim_component: camera_translation: [0.058, -0.08, 0.0] fire_control: bullet_speed: 22.5 - shoot_delay: 0.1 + shoot_delay: 0.04 offset_yaw: +0.1 - offset_pitch: -0.6 - attack_window: 120.0 + offset_pitch: -0.4 + attack_window: 80.0 + degraded_angle_speed: 12.0 window_hysteresis: 0.2 is_lazy_gimbal: false attack_preaim: false @@ -167,11 +168,11 @@ gimbal_controller: lower_limit: 0.10 # 8 deg ctrl_hold_pitch_target_angle: 0.0 - yaw_angle_kp: 15.0 + yaw_angle_kp: 10.0 yaw_angle_ki: 0.0 yaw_angle_kd: 0.0 - yaw_velocity_kp: 15.0 + yaw_velocity_kp: 10.0 yaw_velocity_ki: 0.0 yaw_velocity_kd: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index 6e679cabc..a1885365e 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -53,10 +53,6 @@ auto_aim_recorder: auto_aim_component: ros__parameters: - # WARN: 危险！可选 red | blue，生效后裁判系统 ID 缺席时 - # 将强行绑定为对应阵营哨兵，仅供调试，严禁比赛启用。 - # 留空或填 unknow 表示禁用。 - dangerous_fallback: "" manual_shoot: true camera_translation: [0.07128, 0.0, 0.0481] fire_control: @@ -65,6 +61,7 @@ auto_aim_component: offset_yaw: -1.0 offset_pitch: +0.0 attack_window: 120.0 + degraded_angle_speed: 12.0 window_hysteresis: 0.2 is_lazy_gimbal: false attack_preaim: false @@ -84,7 +81,7 @@ sentry_hardware: board_serial_bottom_board: "af-b4e5" board_serial_gimbal_board: "af-8b8b" - pitch_motor_zero_point: 17241 + pitch_motor_zero_point: 12453 bottom_yaw_motor_zero_point: 54253 top_yaw_motor_zero_point: 32736 diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml index 5ef17bf21..2c5c01ec0 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml @@ -93,6 +93,7 @@ auto_aim_component: offset_yaw: 0.0 offset_pitch: 0.0 attack_window: 120.0 + degraded_angle_speed: 12.0 window_hysteresis: 0.2 is_lazy_gimbal: false attack_preaim: false diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp index 3f206c909..8e01acd86 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include @@ -33,10 +34,14 @@ class AutoAimUi register_input("/tf", tf_, true); register_input("/auto_aim/robot_center", robot_center_, true); register_input("/auto_aim/should_shoot", should_shoot_, true); + register_input("/auto_aim/single_shoot", single_shoot_, true); } void update() override { + const auto type = *single_shoot_ ? "RUNE" : "ARMOR"; + if (!robot_center_->allFinite() || robot_center_->isZero()) { + set_distance_text(type, std::nullopt); hide_all(); return; } @@ -46,6 +51,7 @@ class AutoAimUi || point.x() >= kScreenW || point.x() < 0 // || point.y() >= kScreenH || point.y() < 0 // ) { + set_distance_text(type, std::nullopt); hide_all(); return; } @@ -59,18 +65,12 @@ class AutoAimUi { const auto distance = robot_center_->norm(); if (!std::isfinite(distance)) { - target_distance_indicator_.set_visible(false); + set_distance_text(type, std::nullopt); + hide_all(); return; } - auto& text = target_distance_text_; - text.resize(kMaxTextLength); - std::ranges::fill(text, ' '); - - std::format_to(std::ranges::begin(text), "{:.1f}m\0", distance); - - target_distance_indicator_.set_value(text.data()); - target_distance_indicator_.set_visible(true); + set_distance_text(type, distance); } center_ring_.set_color(color); @@ -146,6 +146,7 @@ class AutoAimUi InputInterface tf_; InputInterface robot_center_; InputInterface should_shoot_; + InputInterface single_shoot_; Circle center_ring_{Shape::Color::GREEN, 2, 0, 0, 5, 5, false}; Line cross_top_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false}; @@ -164,7 +165,20 @@ class AutoAimUi cross_bottom_.set_visible(false); cross_left_.set_visible(false); cross_right_.set_visible(false); - target_distance_indicator_.set_visible(false); + } + + void set_distance_text(const char* type, std::optional distance) { + auto& text = target_distance_text_; + text.resize(kMaxTextLength); + std::ranges::fill(text, ' '); + + if (distance) + std::format_to(std::ranges::begin(text), "{} | {:.1f}m\0", type, *distance); + else + std::format_to(std::ranges::begin(text), "{} | NONE\0", type); + + target_distance_indicator_.set_value(text.data()); + target_distance_indicator_.set_visible(true); } Eigen::Vector2d reproject(const Eigen::Vector3d& center) const { From 318c1ca2674f2b1e36138904987cb105391c6fc6 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Wed, 22 Jul 2026 02:12:15 +0800 Subject: [PATCH 42/86] wip: Adjust 17mm bullet feeder behavior, improve rmcs host util and update auto_aim --- .gitmodules | 3 --- .script/host/rmcs | 14 ++++++++++- rmcs_ws/src/rmcs_auto_aim_v2 | 1 - .../rmcs_bringup/config/auto_aim_test.yaml | 9 ++++--- .../config/deformable-infantry-omni-b.yaml | 1 - .../config/deformable-infantry-omni.yaml | 1 - .../rmcs_bringup/config/navigation_test.yaml | 11 --------- rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 24 +++++++++---------- .../steering-hero-little-six-friction.yaml | 1 - 9 files changed, 28 insertions(+), 37 deletions(-) delete mode 160000 rmcs_ws/src/rmcs_auto_aim_v2 delete mode 100644 rmcs_ws/src/rmcs_bringup/config/navigation_test.yaml diff --git a/.gitmodules b/.gitmodules index de2c6a1cb..61e5f08fb 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,3 @@ [submodule "rmcs_ws/src/fast_tf"] path = rmcs_ws/src/fast_tf url = https://github.com/qzhhhi/FastTF.git -[submodule "rmcs_ws/src/rmcs_auto_aim_v2"] - path = rmcs_ws/src/rmcs_auto_aim_v2 - url = https://github.com/Alliance-Algorithm/rmcs_auto_aim_v2.git diff --git a/.script/host/rmcs b/.script/host/rmcs index 918b209f7..8d8696d9f 100755 --- a/.script/host/rmcs +++ b/.script/host/rmcs @@ -11,7 +11,7 @@ readonly RMCS_PATH="/workspaces/RMCS/" function show_help() { local project_dir="$1" local service="$2" - echo "Usage: $(basename "$0") [path] [zsh|recreate|n|nvim|neovide|vim|ide]" + echo "Usage: $(basename "$0") [path] [zsh|recreate|n|nvim|neovide|vim|ide|ai]" echo " Project dir: $project_dir" echo " Service: $service" } @@ -70,6 +70,15 @@ function rmcs_recreate() { docker compose up -d --force-recreate } +function rmcs_ai() { + local service="$1" + local agent="${RMCS_AGENT:-opencode}" + echo "Starting container and launching Agent ($agent)..." + setup_container + docker compose exec -u "$DEVELOPER_NAME" -w "$RMCS_PATH" "$service" \ + zsh -ic "exec ${agent}" +} + function main() { local project_dir command @@ -100,6 +109,9 @@ function main() { n | nvim | neovide | vim | ide) rmcs_nvim "$service" ;; + ai) + rmcs_ai "$service" + ;; *) show_help "$project_dir" "$service" ;; diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 deleted file mode 160000 index a261f18c4..000000000 --- a/rmcs_ws/src/rmcs_auto_aim_v2 +++ /dev/null @@ -1 +0,0 @@ -Subproject commit a261f18c453d26bea7f08335c0daebb4cdda4b36 diff --git a/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml b/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml index c57bc0dd9..b110fcfd1 100644 --- a/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml @@ -2,8 +2,8 @@ rmcs_executor: ros__parameters: update_rate: 1000.0 components: - - rmcs::AutoAimPlayerComponent -> auto_aim_player - # - rmcs::AutoAimVideoPlayerComponent -> auto_aim_video_player + # - rmcs::AutoAimPlayerComponent -> auto_aim_player + - rmcs::AutoAimVideoPlayerComponent -> auto_aim_video_player # - rmcs::AutoAimRecorderComponent -> auto_aim_recorder - rmcs::AutoAimComponent -> auto_aim_component @@ -14,7 +14,7 @@ auto_aim_player: auto_aim_video_player: ros__parameters: - input_path: "/workspaces/data/autoaim/robot/rotate.avi" + input_path: "/workspaces/data/autoaim/静止看前哨站.avi" framerate: 80.0 loop_play: true @@ -37,10 +37,9 @@ auto_aim_component: shoot_delay: 0.0 offset_yaw: 0.0 offset_pitch: 0.0 - attack_window: 120.0 + attack_window: 60.0 degraded_angle_speed: 12.0 window_hysteresis: 0.2 - is_lazy_gimbal: false attack_preaim: false require_stable_command: true yaw_tolerance: 0.07 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 100f1a144..2ff00181a 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -83,7 +83,6 @@ auto_aim_component: attack_window: 80.0 degraded_angle_speed: 12.0 window_hysteresis: 0.2 - is_lazy_gimbal: false attack_preaim: false require_stable_command: false yaw_tolerance: 0.07 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index a83b79cec..9c26fb78b 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -83,7 +83,6 @@ auto_aim_component: attack_window: 80.0 degraded_angle_speed: 12.0 window_hysteresis: 0.2 - is_lazy_gimbal: false attack_preaim: false require_stable_command: false yaw_tolerance: 0.07 diff --git a/rmcs_ws/src/rmcs_bringup/config/navigation_test.yaml b/rmcs_ws/src/rmcs_bringup/config/navigation_test.yaml deleted file mode 100644 index 52e1e8c42..000000000 --- a/rmcs_ws/src/rmcs_bringup/config/navigation_test.yaml +++ /dev/null @@ -1,11 +0,0 @@ -rmcs_executor: - ros__parameters: - update_rate: 1000.0 - components: - - rmcs::navigation::Navigation -> rmcs_navigation - -rmcs_navigation: - ros__parameters: - command_vel_name: "/cmd_vel" - endpoint: "rmuc" - enable_goal_topic_forward: true diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index a1885365e..76ad8c1b8 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -22,15 +22,21 @@ rmcs_executor: - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller - # - rmcs::navigation::Navigation -> rmcs_navigation + - rmcs::navigation::Navigation -> rmcs_navigation + + # - rmcs::AutoAimRecorderComponent -> auto_aim_recorder + # - rmcs::AutoAimCapturerComponent -> auto_aim_capturer + - rmcs::AutoAimComponent -> auto_aim_component # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster - - rmcs::AutoAimCapturerComponent -> auto_aim_capturer - # - rmcs::AutoAimPlayerComponent -> auto_aim_player - # - rmcs::AutoAimRecorderComponent -> auto_aim_recorder - - rmcs::AutoAimComponent -> auto_aim_component +rmcs_navigation: + ros__parameters: + command_vel_name: "/cmd_vel" + mock_context: false + endpoint: "train" + enable_goal_topic_forward: true auto_aim_capturer: ros__parameters: @@ -63,7 +69,6 @@ auto_aim_component: attack_window: 120.0 degraded_angle_speed: 12.0 window_hysteresis: 0.2 - is_lazy_gimbal: false attack_preaim: false require_stable_command: false yaw_tolerance: 0.07 @@ -91,13 +96,6 @@ sentry_hardware: right_back_zero_point: 3048 right_front_zero_point: 5135 -rmcs_navigation: - ros__parameters: - command_vel_name: "/cmd_vel" - mock_context: false - endpoint: "test" # otaku | main | test - enable_goal_topic_forward: true - gimbal_controller: ros__parameters: upper_limit: -0.65 diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml index 2c5c01ec0..e745a3eba 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml @@ -95,7 +95,6 @@ auto_aim_component: attack_window: 120.0 degraded_angle_speed: 12.0 window_hysteresis: 0.2 - is_lazy_gimbal: false attack_preaim: false require_stable_command: false yaw_tolerance: 0.07 From 057da3f7aaa651a7304c3b887a5f5ea3d91a15aa Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Wed, 22 Jul 2026 03:12:17 +0800 Subject: [PATCH 43/86] wip: Develop sentry navigation --- .script/local-context | 17 +++++++++++------ rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 1 - 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/.script/local-context b/.script/local-context index af72cd280..7ffa214b6 100755 --- a/.script/local-context +++ b/.script/local-context @@ -2,7 +2,7 @@ set -euo pipefail -TOPIC="/rmcs_navigation/context/mock" +BASE="/tmp/rmcs-navigation/context" usage() { cat <<'EOF' @@ -73,12 +73,17 @@ else fi fi -payload="${key}: ${value}" +fifo="${BASE}/${key}" +if [[ ! -p "${fifo}" ]]; then + echo "Context FIFO not found: ${fifo}" >&2 + echo "Start rmcs-navigation first so /tmp/rmcs-navigation/context/ is created." >&2 + exit 1 +fi + echo "[local-context] key: ${key}" echo "[local-context] value: ${value}" -echo "[local-context] topic: ${TOPIC}" -echo "[local-context] payload(to publish): ${payload}" +echo "[local-context] fifo: ${fifo}" -ros2 topic pub -1 "$TOPIC" std_msgs/msg/String "{data: '${payload}'}" +printf '%s' "${value}" >"${fifo}" -echo "Published ${key}=${value} to local topic ${TOPIC}" +echo "Wrote ${key}=${value} to ${fifo}" diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index 76ad8c1b8..3565cc9b3 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -34,7 +34,6 @@ rmcs_executor: rmcs_navigation: ros__parameters: command_vel_name: "/cmd_vel" - mock_context: false endpoint: "train" enable_goal_topic_forward: true From 152aed1792f874ddc19ea2f5a7051b27c19b1c41 Mon Sep 17 00:00:00 2001 From: creeper5820 <131014151+creeper5820@users.noreply.github.com> Date: Mon, 27 Jul 2026 22:04:24 +0800 Subject: [PATCH 44/86] dev(robot): Sentry update (#96) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * feat: Add spin stuck detection for chassis and develop nav gimbal control * feat: Getter for node mixin * wip: Chassis stuck detection * feat: Support nav fusion control with joystick * feat: Update remote control timeout logic * chore: Update sentry config * build: Add detect path to update-image workflow * chore: Update auto aim v2 * wip: Fill basic context and utils for climber * refactor: Climber for sentry * feat(climber): add kRetracted state for stick auto-retract with stall detection - StickGroup: add kRetracted state with speed_rise + rise_torque_limit + hold_torque - kRetracted performs PID ascent, switches to constant hold_torque (0.25Nm) on stall - release_chassis() now sets track→kFree + stick→kRetracted for safe idle state - Add hold_torque param (default 0.25) to YAML config * wip: Cleanup code * feat: Adapt climb request from navigation * chore: Update auto aim v2 * wip: Improve climber --------- Co-authored-by: zlq040222 <1542498005@qq.com> --- rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 93 ++- rmcs_ws/src/rmcs_core/plugins.xml | 1 + .../controller/chassis/chassis_controller.cpp | 174 +++-- .../chassis/chassis_power_controller.cpp | 2 +- .../chassis/hero_chassis_controller.cpp | 1 + .../src/controller/chassis/sentry_climber.cpp | 654 ------------------ .../controller/gimbal/eccentric_dual_yaw.cpp | 59 +- .../rmcs_core/src/hardware/device/dr16.hpp | 5 +- .../src/hardware/device/remote_control.hpp | 12 + .../rmcs_core/src/hardware/device/vt13.hpp | 5 +- rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp | 11 +- .../include/rmcs_msgs/chassis_mode.hpp | 9 +- .../rmcs_utility/rclcpp/node_mixin.hpp | 10 + 13 files changed, 263 insertions(+), 773 deletions(-) delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index 3565cc9b3..de6f840c8 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -17,16 +17,16 @@ rmcs_executor: - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller - - rmcs_core::controller::chassis::ChassisClimberController -> climber_controller + - rmcs_core::controller::chassis::SentryClimber -> sentry_climber - rmcs_core::controller::chassis::ChassisController -> chassis_controller - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller - - rmcs::navigation::Navigation -> rmcs_navigation + # - rmcs::navigation::Navigation -> rmcs_navigation # - rmcs::AutoAimRecorderComponent -> auto_aim_recorder # - rmcs::AutoAimCapturerComponent -> auto_aim_capturer - - rmcs::AutoAimComponent -> auto_aim_component + # - rmcs::AutoAimComponent -> auto_aim_component # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster @@ -34,13 +34,13 @@ rmcs_executor: rmcs_navigation: ros__parameters: command_vel_name: "/cmd_vel" - endpoint: "train" + endpoint: "otaku" enable_goal_topic_forward: true auto_aim_capturer: ros__parameters: camera_name: "" - exposure_us: 3000.0 + exposure_us: 2000.0 gain: 8.0 framerate: 120.0 invert_image: true @@ -59,12 +59,13 @@ auto_aim_recorder: auto_aim_component: ros__parameters: manual_shoot: true + enable_rune: true camera_translation: [0.07128, 0.0, 0.0481] fire_control: bullet_speed: 22.5 shoot_delay: 0.1 - offset_yaw: -1.0 - offset_pitch: +0.0 + offset_yaw: -0.5 + offset_pitch: +1.0 attack_window: 120.0 degraded_angle_speed: 12.0 window_hysteresis: 0.2 @@ -79,7 +80,6 @@ value_broadcaster: - /gimbal/yaw/velocity_imu - /gimbal/pitch/velocity_imu -# The positive direction is the one that battery exists sentry_hardware: ros__parameters: board_serial_bottom_board: "af-b4e5" @@ -123,34 +123,63 @@ gimbal_controller: chassis_controller: ros__parameters: + angular_velocity_max: 10.0 + translational_velocity_max: 10.0 following_velocity_kp: 7.0 following_velocity_ki: 0.0 following_velocity_kd: 0.0 -climber_controller: - ros__parameters: - front_climber_velocity: 20.0 - back_climber_velocity: 30.0 - auto_climb_support_retract_velocity_fast: 60.0 - auto_climb_support_retract_velocity_slow: 20.0 - auto_climb_approach_chassis_velocity: 1.0 - auto_climb_support_deploy_chassis_velocity: 0.3 - auto_climb_support_retract_chassis_velocity: 0.3 - auto_climb_dash_chassis_velocity: 3.0 - first_stair_dash_leveled_pitch_threshold: 0.05 - second_stair_dash_leveled_pitch_threshold: -0.09 - sync_coefficient: 0.2 - first_stair_approach_pitch: 0.585 - second_stair_approach_pitch: 0.365 - front_kp: 1.0 - front_ki: 0.0 - front_kd: 0.5 - front_power_estimate_bias: 0.0 - front_power_estimate_k_tau2: 1.0 - front_power_estimate_k_mech: 1.0 - back_kp: 0.5 - back_ki: 0.0 - back_kd: 0.0 +sentry_climber: + ros__parameters: + track_group: + speed_rush: 20.0 + kp: 1.0 + ki: 0.0 + kd: 0.5 + sync_coefficient: 0.2 + power_estimate_bias: 0.0 + power_estimate_k_tau2: 1.0 + power_estimate_k_mech: 1.0 + stick_group: + speed_drop: 30.0 + speed_rise: 60.0 + rise_torque_limit: 1.2 + land_speed_begin: 100.0 + land_speed_final: 10.0 + land_duration: 0.5 + land_torque_limit: 8.0 + blocked_torque_threshold: 0.1 + blocked_speed_threshold: 0.1 + kp: 1.0 + ki: 0.0 + kd: 0.0 + sync_coefficient: 0.2 + hold_torque: 0.01 + block_hold: 0.05 + align: + err: 0.20 + w: 0.2 + hold: 0.05 + timeout: 15.0 + climb: + approach_pitch: 0.585 + leveled_pitch: 0.05 + approach_vx: 1.2 + deploy_vx: 0.3 + dash_vx: 3.0 + retract_vx: 0.3 + dash_min: 0.1 + dash_duration: 0.8 + stick_timeout: 8.0 + approach_timeout: 8.0 + land: + dash_vx: 1.0 + soft_vx: 0.4 + land_pitch: 0.15 + land_delay: 0.2 + stick_timeout: 8.0 + soft_timeout: 3.0 + settle_timeout: 8.0 friction_wheel_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 1803a1d48..95777e34d 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -16,6 +16,7 @@ + diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp index db740632b..88d033483 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp @@ -1,42 +1,43 @@ #include "controller/pid/pid_calculator.hpp" +#include +#include + #include #include #include #include #include #include -#include #include +#include namespace rmcs_core::controller::chassis { class ChassisController : public rmcs_executor::Component - , public rclcpp::Node { + , public rclcpp::Node + , public rmcs_utility::NodeMixin { public: ChassisController() - : Node{ - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} { + : Node{get_component_name(), node::options()} { - following_velocity_controller_.output_max = angular_velocity_max; + following_velocity_controller_.output_max = +angular_velocity_max; following_velocity_controller_.output_min = -angular_velocity_max; register_input("/remote/joystick/right", joystick_right_); - register_input("/remote/joystick/left", joystick_left_); register_input("/remote/switch/right", switch_right_); register_input("/remote/switch/left", switch_left_); - register_input("/remote/mouse/velocity", mouse_velocity_); - register_input("/remote/mouse", mouse_); register_input("/remote/keyboard", keyboard_); - register_input("/remote/rotary_knob", rotary_knob_); register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); - register_input("/chassis/velocity", chassis_velocity_, false); - register_input("/chassis/climbing_forward_velocity", climbing_forward_velocity_, false); + register_input("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, false); + + register_input("/chassis/climber/direction", chassis_climb_direction_, false); + register_input("/chassis/climber/speed", chassis_climb_speed_, false); + register_input("/chassis/climber/measure_yaw", chassis_measure_yaw_, false); register_input("/rmcs_navigation/enable_control", navigation_enable_control_, false); register_input("/rmcs_navigation/chassis_velocity", navigation_command_velocity_, false); @@ -51,16 +52,21 @@ class ChassisController void before_updating() override { if (!gimbal_yaw_angle_.ready()) { gimbal_yaw_angle_.make_and_bind_directly(0.0); - RCLCPP_WARN(get_logger(), "Failed to fetch \"/gimbal/yaw/angle\". Set to 0.0."); + node::warn("Failed to fetch \"/gimbal/yaw/angle\". Set to 0.0."); } if (!gimbal_yaw_angle_error_.ready()) { gimbal_yaw_angle_error_.make_and_bind_directly(0.0); - RCLCPP_WARN( - get_logger(), "Failed to fetch \"/gimbal/yaw/control_angle_error\". Set to 0.0."); + node::warn("Failed to fetch \"/gimbal/yaw/control_angle_error\". Set to 0.0."); } - if (!climbing_forward_velocity_.ready()) { - climbing_forward_velocity_.make_and_bind_directly(kNaN); + if (!chassis_climb_direction_.ready()) { + chassis_climb_direction_.make_and_bind_directly(kNaN); + } + if (!chassis_climb_speed_.ready()) { + chassis_climb_speed_.make_and_bind_directly(kNaN); + } + if (!chassis_measure_yaw_.ready()) { + chassis_measure_yaw_.make_and_bind_directly(kNaN); } if (!navigation_enable_control_.ready()) { @@ -77,9 +83,9 @@ class ChassisController void update() override { using namespace rmcs_msgs; - auto switch_right = *switch_right_; - auto switch_left = *switch_left_; - auto keyboard = *keyboard_; + const auto switch_right = *switch_right_; + const auto switch_left = *switch_left_; + const auto keyboard = *keyboard_; do { if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) @@ -119,6 +125,13 @@ class ChassisController mode = *navigation_chassis_behavior_; } + if (climb_active()) { + mode = ChassisMode::CLIMB; + } else if (mode == ChassisMode::CLIMB) { + mode = ChassisMode::AUTO; + } + + update_spin_stuck_watchdog(mode); *mode_ = mode; } @@ -133,8 +146,50 @@ class ChassisController void reset_all_controls() { *mode_ = rmcs_msgs::ChassisMode::ALIGNMENT; *chassis_control_velocity_ = {kNaN, kNaN, kNaN}; + + spin_stuck_count_ = 0; + spin_recovery_count_ = 0; + following_velocity_controller_.reset(); } + auto update_spin_stuck_watchdog(rmcs_msgs::ChassisMode& mode) -> void { + constexpr auto kSpinStuckConfirmTicks = std::size_t{300}; + constexpr auto kSpinRecoveryTicks = std::size_t{1000}; + constexpr auto kSpinStuckAngularVelocityRatio = double{0.2}; + + using rmcs_msgs::ChassisMode; + + if (spin_recovery_count_ > 0) { + mode = ChassisMode::ALIGNMENT_POWERED; + + if (--spin_recovery_count_ == 0) + mode = mode_before_watchdog_; + + spin_stuck_count_ = 0; + return; + } + + if (!rmcs_msgs::is_spining(mode) || !chassis_yaw_velocity_imu_.ready()) { + spin_stuck_count_ = 0; + return; + } + + const auto expected = (mode == ChassisMode::SPIN_FAST ? 0.6 : 0.3) * angular_velocity_max; + if (std::abs(*chassis_yaw_velocity_imu_) >= kSpinStuckAngularVelocityRatio * expected) { + spin_stuck_count_ = 0; + return; + } + + if (++spin_stuck_count_ < kSpinStuckConfirmTicks) + return; + + mode_before_watchdog_ = mode; + mode = ChassisMode::ALIGNMENT_POWERED; + spin_recovery_count_ = kSpinRecoveryTicks; + spin_stuck_count_ = 0; + + node::warn("Spin stuck detected, disable spinning for 1s."); + } void update_velocity_control() { auto translational_velocity = update_translational_velocity_control(); auto angular_velocity = update_angular_velocity_control(); @@ -142,14 +197,38 @@ class ChassisController chassis_control_velocity_->vector << translational_velocity, angular_velocity; } + auto climb_active() const -> bool { + return std::isfinite(*chassis_climb_direction_) && std::isfinite(*chassis_climb_speed_) + && std::isfinite(*chassis_measure_yaw_); + } + + static auto normalize_signed_angle(double angle) noexcept { + constexpr auto kTwoPi = 2.0 * std::numbers::pi; + while (angle >= std::numbers::pi) + angle -= kTwoPi; + while (angle < -std::numbers::pi) + angle += kTwoPi; + return angle; + } + Eigen::Vector2d update_translational_velocity_control() { - if (!std::isnan(*climbing_forward_velocity_)) - return {*climbing_forward_velocity_, 0.0}; + using namespace rmcs_msgs; + + if (*mode_ == ChassisMode::CLIMB) { + // speed 以底盘正向 direction 为正向：上坡为正前进，下坡为负倒车 + return {*chassis_climb_speed_, 0.0}; + } if (*navigation_enable_control_) { const auto command = *navigation_command_velocity_; - if (command.array().isFinite().all()) - return Eigen::Rotation2Dd{*gimbal_yaw_angle_} * command; + if (command.array().isFinite().all()) { + Eigen::Vector2d superimposed = + command + *joystick_right_ * translational_velocity_max; + if (superimposed.norm() > translational_velocity_max) + superimposed *= translational_velocity_max / superimposed.norm(); + + return Eigen::Rotation2Dd{*gimbal_yaw_angle_} * superimposed; + } } auto keyboard = *keyboard_; @@ -170,17 +249,6 @@ class ChassisController double angular_velocity = 0.0; double chassis_control_angle = kNaN; - if (!std::isnan(*climbing_forward_velocity_)) { - double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); - if (err > std::numbers::pi) - err -= 2 * std::numbers::pi; - angular_velocity = following_velocity_controller_.update(err); - - *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; - *chassis_control_angle_ = chassis_control_angle; - return angular_velocity; - } - using namespace rmcs_msgs; switch (*mode_) { case ChassisMode::AUTO: break; @@ -236,6 +304,17 @@ class ChassisController angular_velocity = following_velocity_controller_.update(err); } break; + + case ChassisMode::CLIMB: { + chassis_control_angle = *chassis_climb_direction_; + + const auto err = normalize_signed_angle(chassis_control_angle - *chassis_measure_yaw_); + angular_velocity = following_velocity_controller_.update(err); + + *chassis_angle_ = *chassis_measure_yaw_; + *chassis_control_angle_ = chassis_control_angle; + return angular_velocity; + } } *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; @@ -260,17 +339,13 @@ class ChassisController static constexpr double kInf = std::numeric_limits::infinity(); static constexpr double kNaN = std::numeric_limits::quiet_NaN(); - const double translational_velocity_max{get_parameter_or("translational_velocity_max", 10.0)}; - const double angular_velocity_max{get_parameter_or("angular_velocity_max", 16.0)}; + const double translational_velocity_max{node::param_or("translational_velocity_max", 10.0)}; + const double angular_velocity_max{node::param_or("angular_velocity_max", 16.0)}; InputInterface joystick_right_; - InputInterface joystick_left_; InputInterface switch_right_; InputInterface switch_left_; - InputInterface mouse_velocity_; - InputInterface mouse_; InputInterface keyboard_; - InputInterface rotary_knob_; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; @@ -279,8 +354,10 @@ class ChassisController InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; OutputInterface chassis_angle_, chassis_control_angle_; - InputInterface chassis_velocity_; - InputInterface climbing_forward_velocity_; + InputInterface chassis_yaw_velocity_imu_; + InputInterface chassis_climb_direction_; + InputInterface chassis_climb_speed_; + InputInterface chassis_measure_yaw_; InputInterface navigation_enable_control_; InputInterface navigation_command_velocity_; @@ -288,10 +365,15 @@ class ChassisController OutputInterface mode_; bool spinning_forward_ = true; + + std::size_t spin_stuck_count_ = 0; + std::size_t spin_recovery_count_ = 0; + rmcs_msgs::ChassisMode mode_before_watchdog_ = rmcs_msgs::ChassisMode::AUTO; + pid::PidCalculator following_velocity_controller_{ - get_parameter_or("following_velocity_kp", 8.0), - get_parameter_or("following_velocity_ki", 0.0), - get_parameter_or("following_velocity_kd", 0.0), + node::param_or("following_velocity_kp", 8.0), + node::param_or("following_velocity_ki", 0.0), + node::param_or("following_velocity_kd", 0.0), }; OutputInterface chassis_control_velocity_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp index 7c701f633..b4bad88fb 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp @@ -112,7 +112,7 @@ class ChassisPowerController if (boost_mode_ && *supercap_enabled_) power_limit = - rmcs_msgs::need_power(*mode_) ? inf_ : *chassis_power_limit_referee_ + 80.0; + rmcs_msgs::is_powered(*mode_) ? inf_ : *chassis_power_limit_referee_ + 80.0; else power_limit = *chassis_power_limit_referee_; chassis_power_limit_expected_ = power_limit; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp index 8fd643e54..9291886ed 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp @@ -203,6 +203,7 @@ class HeroChassisController } break; case rmcs_msgs::ChassisMode::ALIGNMENT: [[fallthrough]]; case rmcs_msgs::ChassisMode::ALIGNMENT_POWERED: [[fallthrough]]; + case rmcs_msgs::ChassisMode::CLIMB: [[fallthrough]]; case rmcs_msgs::ChassisMode::LAUNCH_RAMP: { double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp deleted file mode 100644 index f57cc995e..000000000 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp +++ /dev/null @@ -1,654 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include "controller/chassis/climber/co_schduler.hpp" -#include "controller/chassis/climber/stick_group.hpp" -#include "controller/chassis/climber/track_group.hpp" - -namespace rmcs_core::controller::chassis { - -class SentryClimber - : public rmcs_executor::Component - , public rclcpp::Node - , public rmcs_utility::NodeMixin { - - static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); - - using TrackState = climber::TrackGroup::State; - using StickState = climber::StickGroup::State; - - struct Config { - climber::TrackGroup::Config track; - climber::StickGroup::Config stick; - - struct Align { - double err; - double w; - double hold; - double timeout; - } align; - - struct Climb { - double approach_pitch; - double leveled_pitch; - double approach_vx; - double deploy_vx; - double dash_vx; - double retract_vx; - double dash_min; - double dash_duration; - double stick_timeout; - double approach_timeout; - } climb; - - struct Land { - double dash_vx; - double soft_vx; - double land_pitch; - double land_delay; - double stick_timeout; - double soft_timeout; - double settle_timeout; - double leave_vx; - double leave_duration; - } land; - - double block_hold; - - template - static auto load(ParamOr&& param_or) -> Config { - return Config{ - .track = - { - .speed_rush = param_or("track_group.speed_rush", 20.0), - .kp = param_or("track_group.kp", 1.0), - .ki = param_or("track_group.ki", 0.0), - .kd = param_or("track_group.kd", 0.5), - .sync_coefficient = param_or("track_group.sync_coefficient", 0.2), - .power_estimate_bias = param_or("track_group.power_estimate_bias", 0.0), - .power_estimate_k_tau2 = param_or("track_group.power_estimate_k_tau2", 1.0), - .power_estimate_k_mech = param_or("track_group.power_estimate_k_mech", 1.0), - }, - .stick = - { - .speed_drop = param_or("stick_group.speed_drop", 30.0), - .speed_rise = param_or("stick_group.speed_rise", 60.0), - .rise_torque_limit = param_or("stick_group.rise_torque_limit", 0.5), - .land_speed_begin = param_or("stick_group.land_speed_begin", 30.0), - .land_speed_final = param_or("stick_group.land_speed_final", 2.0), - .land_duration = param_or("stick_group.land_duration", 0.5), - .land_torque_limit = param_or("stick_group.land_torque_limit", 8.0), - .blocked_torque_threshold = - param_or("stick_group.blocked_torque_threshold", 0.1), - .blocked_speed_threshold = - param_or("stick_group.blocked_speed_threshold", 0.1), - .kp = param_or("stick_group.kp", 0.5), - .ki = param_or("stick_group.ki", 0.0), - .kd = param_or("stick_group.kd", 0.0), - .sync_coefficient = param_or("stick_group.sync_coefficient", 0.2), - .hold_torque = param_or("stick_group.hold_torque", 0.01), - }, - .align = - { - .err = param_or("align.err", 0.10), - .w = param_or("align.w", 0.2), - .hold = param_or("align.hold", 0.05), - .timeout = param_or("align.timeout", 15.0), - }, - .climb = - { - .approach_pitch = param_or("climb.approach_pitch", 0.585), - .leveled_pitch = param_or("climb.leveled_pitch", 0.05), - .approach_vx = param_or("climb.approach_vx", 1.2), - .deploy_vx = param_or("climb.deploy_vx", 0.3), - .dash_vx = param_or("climb.dash_vx", 3.0), - .retract_vx = param_or("climb.retract_vx", 0.3), - .dash_min = param_or("climb.dash_min", 0.1), - .dash_duration = param_or("climb.dash_duration", 3.0), - .stick_timeout = param_or("climb.stick_timeout", 8.0), - .approach_timeout = param_or("climb.approach_timeout", 8.0), - }, - .land = - { - .dash_vx = param_or("land.dash_vx", 0.8), - .soft_vx = param_or("land.soft_vx", 0.4), - .land_pitch = param_or("land.land_pitch", 0.15), - .land_delay = param_or("land.land_delay", 0.2), - .stick_timeout = param_or("land.stick_timeout", 8.0), - .soft_timeout = param_or("land.soft_timeout", 3.0), - .settle_timeout = param_or("land.settle_timeout", 8.0), - .leave_vx = param_or("land.leave_vx", 0.5), - .leave_duration = param_or("land.leave_duration", 1.0), - }, - .block_hold = param_or("block_hold", 0.05), - }; - } - }; - - // 仅输入与派生；不负责 output - struct Context { - InputInterface l_switch; - InputInterface r_switch; - InputInterface keyboard; - InputInterface rotary_knob; - - InputInterface nav_cross_direction; - InputInterface nav_is_climb; - - InputInterface chassis_pitch; - InputInterface chassis_yaw_rate; - InputInterface measure_yaw; - - static constexpr auto normalize_angle(double angle) noexcept { - while (angle >= std::numbers::pi) - angle -= 2.0 * std::numbers::pi; - while (angle < -std::numbers::pi) - angle += 2.0 * std::numbers::pi; - return angle; - } - - auto bind(Component& component) noexcept { - // 当 nav_cross_direction 发生 isnan -> !isnan 的变化时，视作一次跨越地形事件请求 - // 反之，会立刻取消请求，终止当前事件 - component.register_input( - "/rmcs_navigation/request/cross_direction", nav_cross_direction, false); - component.register_input("/rmcs_navigation/request/is_climb", nav_is_climb, false); - - component.register_input("/remote/switch/left", l_switch, false); - component.register_input("/remote/switch/right", r_switch, false); - component.register_input("/remote/keyboard", keyboard, false); - component.register_input("/remote/rotary_knob_switch", rotary_knob, false); - - component.register_input("/chassis/pitch_imu", chassis_pitch, false); - component.register_input("/chassis/yaw/velocity_imu", chassis_yaw_rate, false); - component.register_input("/chassis/climber/measure_yaw", measure_yaw, false); - } - - auto load_fallback(std::invocable auto&& handler) { - using namespace rmcs_msgs; - - const auto ensure_bind = - [&](InputInterface& input, T default_value, std::string_view name) { - if (input.ready() == false) { - input.make_and_bind_directly(default_value); - std::invoke(handler, name); - } - }; - - ensure_bind(nav_cross_direction, kNaN, "nav_cross_direction"); - ensure_bind(nav_is_climb, false, "nav_is_climb"); - - ensure_bind(l_switch, Switch::UNKNOWN, "l_switch"); - ensure_bind(r_switch, Switch::UNKNOWN, "r_switch"); - ensure_bind(keyboard, Keyboard::zero(), "keyboard"); - ensure_bind(rotary_knob, Switch::UNKNOWN, "rotary_knob"); - - ensure_bind(chassis_pitch, 0.0, "chassis_pitch"); - ensure_bind(chassis_yaw_rate, 0.0, "chassis_yaw_rate"); - ensure_bind(measure_yaw, kNaN, "measure_yaw"); - } - - auto is_estop() const { - using namespace rmcs_msgs; - const auto l = *l_switch; - const auto r = *r_switch; - return l == Switch::UNKNOWN || r == Switch::UNKNOWN - || (l == Switch::DOWN && r == Switch::DOWN); - } - - auto align_error(double goal) const noexcept { - return normalize_angle(*measure_yaw - goal); - } - - auto wait_align( - double goal, double err_limit, double w_limit, std::chrono::steady_clock::duration hold, - std::chrono::steady_clock::duration timeout) const { - constexpr auto kSinceInit = std::optional{}; - return CoSchduler::WaitUntil{ - .monitor = - [=, this, hold_since = kSinceInit]() mutable { - if (!std::isfinite(*measure_yaw)) - return false; - - const auto stable = std::abs(align_error(goal)) < err_limit - && std::abs(*chassis_yaw_rate) < w_limit; - - const auto now = std::chrono::steady_clock::now(); - if (stable) { - if (!hold_since.has_value()) - hold_since = now; - else if (now - *hold_since >= hold) - return true; - } else { - hold_since.reset(); - } - return false; - }, - .timeout = timeout, - }; - } - - } context; - - OutputInterface chassis_track_direction; // 以履带方向为正向 - OutputInterface chassis_climb_speed; // 正向为基准的速度值 - OutputInterface - chassis_climb_status; // 事件进度: 0=空闲, 1=成功, -1=失败, (0,1)阶段小数 - - // /chassis/climber/status 阶段编码：(0, 0.55) 上台阶，[0.55, 1) 下台阶 - static constexpr double kStatusClimbAlign = 0.1; - static constexpr double kStatusClimbApproach = 0.2; - static constexpr double kStatusClimbDeploy = 0.3; - static constexpr double kStatusClimbDash = 0.4; - static constexpr double kStatusClimbRetract = 0.5; - static constexpr double kStatusLandAlign = 0.6; - static constexpr double kStatusLandDash = 0.7; - static constexpr double kStatusLandSettle = 0.75; - static constexpr double kStatusLandSoft = 0.8; - static constexpr double kStatusLandFinal = 0.9; - static constexpr double kStatusLandLeave = 0.95; - - struct SimpleComponent : public rmcs_executor::Component { - std::function fn; - - template - explicit SimpleComponent(Fn&& fn) - : fn{std::forward(fn)} {} - - auto update() -> void override { fn(); } - }; - - std::shared_ptr output_component{ - create_partner_component( - get_component_name() + "_output", [this] { std::ignore = this; }), - }; - - std::unique_ptr track_group; - std::unique_ptr stick_group; - CoSchduler schduler; - - Config config; - CoSchduler::Handle task_handler; - - static constexpr auto seconds_to_duration(double seconds) noexcept { - return std::chrono::duration_cast( - std::chrono::duration{seconds}); - } - - auto release_climber() noexcept { - *chassis_track_direction = kNaN; - *chassis_climb_speed = kNaN; - track_group->set_state(TrackState::kFree); - stick_group->set_state(// - context.is_estop() ? StickState::kFree : StickState::kKeep); - } - - auto wait_block(std::chrono::steady_clock::duration timeout) { - constexpr auto kSinceInit = std::optional{}; - const auto hold = seconds_to_duration(config.block_hold); - return CoSchduler::WaitUntil{ - .monitor = - [this, hold, hold_since = kSinceInit]() mutable { - const auto now = std::chrono::steady_clock::now(); - if (stick_group->get_block()) { - if (!hold_since.has_value()) - hold_since = now; - else if (now - *hold_since >= hold) - return true; - } else { - hold_since.reset(); - } - return false; - }, - .timeout = timeout, - }; - } - - auto spin_context() -> CoSchduler::Task { - using namespace rmcs_msgs; - - auto last_keyboard = Keyboard::zero(); - auto last_rotary = Switch::UNKNOWN; - - auto last_nav_cross_dir = kNaN; - - const auto cancel_task = [this] { - if (!task_handler.done()) { - task_handler.cancel(); - task_handler = {}; - } - *chassis_climb_status = 0.0; - release_climber(); - }; - - while (true) { - const auto keyboard = *context.keyboard; - const auto rotary = *context.rotary_knob; - - const auto nav_cross_dir = *context.nav_cross_direction; - const auto nav_is_climb = *context.nav_is_climb; - - const auto nav_request = - !std::isfinite(last_nav_cross_dir) && std::isfinite(nav_cross_dir); - const auto nav_canceled = - std::isfinite(last_nav_cross_dir) && !std::isfinite(nav_cross_dir); - - const auto step_direction = std::isfinite(*context.nav_cross_direction) - ? *context.nav_cross_direction - : *context.measure_yaw; - - const auto stop_intent = - (last_rotary != Switch::MIDDLE && rotary == Switch::MIDDLE) || nav_canceled; - const auto land_intent = (last_rotary != Switch::DOWN && rotary == Switch::DOWN) - || (nav_request && !nav_is_climb); - const auto rise_intent = (last_rotary != Switch::UP && rotary == Switch::UP) - || (last_keyboard.g == false && keyboard.g == true) - || (nav_request && nav_is_climb); - - do { - if (context.is_estop() || stop_intent) { - cancel_task(); - break; - } - if (rise_intent) { - if (!task_handler.done()) { - cancel_task(); - } else if (!std::isfinite(*context.measure_yaw)) { - *chassis_climb_status = -1; - node::error("climb start rejected: measure_yaw invalid"); - } else { - task_handler = schduler.append(climb(step_direction)); - } - break; - } - if (land_intent) { - if (!task_handler.done()) { - cancel_task(); - } else if (!std::isfinite(*context.measure_yaw)) { - *chassis_climb_status = -1; - node::error("land start rejected: measure_yaw invalid"); - } else { - task_handler = schduler.append(land(step_direction)); - } - break; - } - } while (false); - - if (!context.is_estop() && task_handler.done()) - release_climber(); - - last_keyboard = keyboard; - last_rotary = rotary; - - last_nav_cross_dir = nav_cross_dir; - - co_await CoSchduler::Tick{}; - } - } - - auto spin_groups() -> CoSchduler::Task { - while (true) { - track_group->spin_once(); - stick_group->spin_once(); - co_await CoSchduler::Tick{}; - } - } - - auto climb(double direction) -> CoSchduler::Task { - using namespace std::chrono_literals; - - *chassis_climb_status = kStatusClimbAlign; - - node::info("Climb start, direction={:.3f}", direction); - *chassis_track_direction = direction; - - // [] 将底盘与台阶方向对齐 - track_group->set_state(TrackState::kHold); - stick_group->set_state(StickState::kHold); - *chassis_climb_speed = 0.0; - { - const auto t0 = std::chrono::steady_clock::now(); - const auto timed_out = co_await context.wait_align( - direction, config.align.err, config.align.w, seconds_to_duration(config.align.hold), - seconds_to_duration(config.align.timeout)); - if (timed_out || !std::isfinite(*context.measure_yaw)) { - node::warn("climb ALIGN failed"); - release_climber(); - *chassis_climb_status = -1; - co_return; - } - const auto elapsed = - std::chrono::duration(std::chrono::steady_clock::now() - t0); - node::info( - "climb ALIGN done: err={:.3f}, took={:.3f}s", context.align_error(direction), - elapsed.count()); - } - - // [] 冲向台阶，开启履带，让底盘沿着台阶边缘上升，直到倾斜到一定角度 - *chassis_climb_status = kStatusClimbApproach; - track_group->set_state(TrackState::kRush); - stick_group->set_state(StickState::kHold); - *chassis_climb_speed = config.climb.approach_vx; - { - auto count = std::size_t{0}; - auto timeout = bool{false}; - do { - if (timeout) { - *chassis_climb_speed = -config.climb.approach_vx; - co_await CoSchduler::Sleep{500ms}; - - *chassis_climb_speed = +config.climb.approach_vx; - } - - timeout = co_await CoSchduler::WaitUntil{ - .monitor = - [this] { return *context.chassis_pitch > config.climb.approach_pitch; }, - .timeout = seconds_to_duration(config.climb.approach_timeout), - }; - if (timeout) - node::warn("climb APPROACH timeout, retry"); - - if (count++ > 2) { - node::error("上台阶彻底失败"); - release_climber(); - *chassis_climb_status = -1; - co_return; - } - } while (timeout); - } - - // [] 伸出撑杆，同时慢速向台阶方向前进 - *chassis_climb_status = kStatusClimbDeploy; - track_group->set_state(TrackState::kHold); - stick_group->set_state(StickState::kDrop); - *chassis_climb_speed = config.climb.deploy_vx; - { - const auto timed_out = - co_await wait_block(seconds_to_duration(config.climb.stick_timeout)); - if (timed_out) - node::warn("climb DEPLOY stick timeout, continue"); - } - - // [] 撑杆已完全伸出，全力冲上台阶，保持一定时间间隔 - *chassis_climb_status = kStatusClimbDash; - track_group->set_state(TrackState::kHold); - stick_group->set_state(StickState::kHold); - *chassis_climb_speed = config.climb.dash_vx; - co_await CoSchduler::Sleep{seconds_to_duration(config.climb.dash_duration)}; - - // [] 上台阶完毕，收回撑杆 - *chassis_climb_status = kStatusClimbRetract; - track_group->set_state(TrackState::kHold); - stick_group->set_state(StickState::kRise); - *chassis_climb_speed = config.climb.retract_vx; - { - const auto timed_out = - co_await wait_block(seconds_to_duration(config.climb.stick_timeout)); - if (timed_out) - node::warn("climb RETRACT stick timeout, continue"); - } - - *chassis_climb_speed = config.climb.dash_vx; - co_await CoSchduler::Sleep{500ms}; - - *chassis_climb_status = 1.0; - release_climber(); - } - - auto land(double direction) -> CoSchduler::Task { - *chassis_climb_status = kStatusLandAlign; - - node::info("Land start, direction={:.3f}", direction); - *chassis_track_direction = direction + std::numbers::pi; - - // [] 底盘对齐方向，准备下台阶 - track_group->set_state(TrackState::kHold); - stick_group->set_state(StickState::kHold); - *chassis_climb_speed = 0.0; - { - const auto timed_out = co_await context.wait_align( - direction + std::numbers::pi, config.align.err, config.align.w, - seconds_to_duration(config.align.hold), seconds_to_duration(config.align.timeout)); - if (timed_out) { - node::warn("land ALIGN failed"); - release_climber(); - *chassis_climb_status = -1; - co_return; - } - } - - // [] 伸出撑杆，以较快速度冲下台阶 - *chassis_climb_status = kStatusLandDash; - track_group->set_state(TrackState::kHold); - stick_group->set_state(StickState::kDrop); - *chassis_climb_speed = -config.land.dash_vx; - { - const auto timed_out = - co_await wait_block(seconds_to_duration(config.land.stick_timeout)); - if (timed_out) - node::warn("land DEPLOY stick timeout, continue"); - } - - // [] 保持撑杆伸出，直到撑杆从台阶落下，底盘倾角低于某个阈值，趋近水平 - *chassis_climb_status = kStatusLandSettle; - track_group->set_state(TrackState::kHold); - stick_group->set_state(StickState::kDrop); - *chassis_climb_speed = -config.land.dash_vx; - { - const auto timed_out = co_await CoSchduler::WaitUntil{ - .monitor = - [this] { return std::abs(*context.chassis_pitch) < config.land.land_pitch; }, - .timeout = seconds_to_duration(config.land.settle_timeout), - }; - if (timed_out) - node::warn("land SETTLE timeout, continue"); - } - using namespace std::chrono_literals; - co_await CoSchduler::Sleep{seconds_to_duration(config.land.land_delay)}; - - // [] 撑杆按照速度曲线收回，减少落地震动，并缓慢前进，让履带顺着台阶落下 - *chassis_climb_status = kStatusLandSoft; - track_group->set_state(TrackState::kHold); - stick_group->set_state(StickState::kLand); - *chassis_climb_speed = -config.land.soft_vx; - co_await CoSchduler::Sleep{seconds_to_duration(config.stick.land_duration)}; - { - const auto timed_out = - co_await wait_block(seconds_to_duration(config.land.soft_timeout)); - if (timed_out) - node::warn("land SOFT stick timeout, continue"); - } - - // [] 等待完全落地，底盘倾角趋近水平 - { - const auto timed_out = co_await CoSchduler::WaitUntil{ - .monitor = - [this] { return std::abs(*context.chassis_pitch) < config.land.land_pitch; }, - .timeout = seconds_to_duration(config.land.settle_timeout), - }; - if (timed_out) - node::warn("land SETTLE timeout, continue"); - } - - // [] 完全收回撑杆，结束下台阶 - *chassis_climb_status = kStatusLandFinal; - track_group->set_state(TrackState::kFree); - stick_group->set_state(StickState::kRise); - *chassis_climb_speed = kNaN; - { - const auto timed_out = - co_await wait_block(seconds_to_duration(config.land.stick_timeout)); - if (timed_out) - node::warn("land FINAL stick timeout, continue"); - } - - // [] 撑杆收回后，以一定速度向前（驶离台阶方向）运动一段时间 - *chassis_climb_status = kStatusLandLeave; - track_group->set_state(TrackState::kHold); - stick_group->set_state(StickState::kHold); - *chassis_climb_speed = -config.land.leave_vx; - co_await CoSchduler::Sleep{seconds_to_duration(config.land.leave_duration)}; - - *chassis_climb_status = 1.0; - release_climber(); - } - -public: - SentryClimber() - : Node{get_component_name(), node::options()} { - const auto read_parameter = [this](std::string_view name, double fallback) { - return node::param_or(std::string{name}, fallback); - }; - - config = Config::load(read_parameter); - - track_group = std::make_unique(*this, config.track); - stick_group = std::make_unique(*this, config.stick); - - context.bind(*this); - - // 底盘契约输出挂在 partner 上，保证更新序在主逻辑之后对下游可见 - output_component->register_output( - "/chassis/climber/direction", chassis_track_direction, kNaN); - output_component->register_output("/chassis/climber/speed", chassis_climb_speed, kNaN); - output_component->register_output("/chassis/climber/status", chassis_climb_status, 0.0); - - schduler.append(spin_context()); - schduler.append(spin_groups()); - } - - auto before_updating() -> void override { - context.load_fallback([this](std::string_view name) { - node::warn("Failed to fetch input '{}'. Bind to fallback.", name); - }); - } - - auto update() -> void override { - try { - schduler.spin_once(); - } catch (const std::exception& e) { - node::error("climber routine exception: {}", e.what()); - task_handler.cancel(); - task_handler = {}; - track_group->set_state(climber::TrackGroup::State::kFree); - stick_group->set_state(climber::StickGroup::State::kFree); - release_climber(); - } - } -}; - -} // namespace rmcs_core::controller::chassis - -#include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::SentryClimber, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp index 0f2734ec1..bea900342 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp @@ -69,44 +69,35 @@ class EccentricDualYaw return; } - // 导航控制。 - if (input_.enable_navigation()) { - const auto error = solver_.update( - EccentricDualYawSolver::Navigation{ - *input_.top_yaw_angle, - *input_.navigation_toward, - current_bottom_world_yaw(), - actual_yaw_pitch.second, - stored_bottom_yaw_target_, - stored_pitch_target_, - upper_limit_, - lower_limit_, - }); - apply_control(error.bottom_yaw, error.top_yaw, error.pitch); + const auto yaw_shift = +kJoystickSensitivity * input_.joystick_left->y() + + kMouseSensitivity * input_.mouse_velocity->y(); + const auto pitch_shift = -kJoystickSensitivity * input_.joystick_left->x() + - kMouseSensitivity * input_.mouse_velocity->x(); - stored_bottom_yaw_target_ = limit_rad(current_bottom_world_yaw() + error.bottom_yaw); - stored_pitch_target_ = std::clamp( - limit_rad(actual_yaw_pitch.second + error.pitch), upper_limit_, lower_limit_); - return; + auto nav_yshift = double{0.}; + auto nav_pshift = double{0.}; + if (input_.enable_navigation()) { + constexpr auto kGimbalFree = std::numeric_limits::min(); + const auto& toward = *input_.navigation_toward; + if (toward.x() == kGimbalFree && toward.y() == kGimbalFree) { + enter_disabled_state(); + return; + } + if (std::isfinite(toward.x())) + nav_yshift = limit_rad(toward.x() - stored_bottom_yaw_target_); + if (std::isfinite(toward.y())) + nav_pshift = limit_rad( + std::clamp(toward.y(), upper_limit_, lower_limit_) - stored_pitch_target_); } - // 手动控制。 - { - const double yaw_shift = kJoystickSensitivity * input_.joystick_left->y() - + kMouseSensitivity * input_.mouse_velocity->y(); - const double pitch_shift = -kJoystickSensitivity * input_.joystick_left->x() - - kMouseSensitivity * input_.mouse_velocity->x(); - - stored_bottom_yaw_target_ = limit_rad(stored_bottom_yaw_target_ + yaw_shift); - stored_pitch_target_ = - std::clamp(stored_pitch_target_ + pitch_shift, upper_limit_, lower_limit_); - - const double bottom_yaw_error = - limit_rad(stored_bottom_yaw_target_ - current_bottom_world_yaw()); - const double pitch_error = limit_rad(stored_pitch_target_ - actual_yaw_pitch.second); + stored_bottom_yaw_target_ = limit_rad(stored_bottom_yaw_target_ + nav_yshift + yaw_shift); + stored_pitch_target_ = + std::clamp(stored_pitch_target_ + nav_pshift + pitch_shift, upper_limit_, lower_limit_); - apply_control(bottom_yaw_error, limit_rad(-*input_.top_yaw_angle), pitch_error); - } + apply_control( + limit_rad(+stored_bottom_yaw_target_ - current_bottom_world_yaw()), + limit_rad(-*input_.top_yaw_angle), + limit_rad(+stored_pitch_target_ - actual_yaw_pitch.second)); } private: diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp index ba0ec4fac..51895755b 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp @@ -154,6 +154,8 @@ class Dr16 { bool valid() const noexcept { return valid_; } + void set_timeout_enabled(bool enabled) { timeout_enabled_ = enabled; } + double rotary_knob() const { return rotary_knob_; } double mouse_wheel() const { return mouse_wheel_; } @@ -190,7 +192,7 @@ class Dr16 { static constexpr auto kFreshTimeout = std::chrono::milliseconds(500); void refresh_validity(const TimePoint now) { - if (!valid_ || now - last_remote_control_received_at_ <= kFreshTimeout) + if (!timeout_enabled_ || !valid_ || now - last_remote_control_received_at_ <= kFreshTimeout) return; reset_remote_control_state(); @@ -278,6 +280,7 @@ class Dr16 { rmcs_msgs::Switch rotary_knob_switch_ = rmcs_msgs::Switch::UNKNOWN; TimePoint last_remote_control_received_at_ = TimePoint::min(); bool valid_ = false; + bool timeout_enabled_ = true; }; } // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp index c61b874e7..689436bc6 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp @@ -51,6 +51,8 @@ class RemoteControl { void register_vt13(Vt13* vt13) { vt13_ = vt13; } void update() { + update_timeout_interlock(); + const auto control_source = select_control_source(); const auto snapshot = build_snapshot(control_source); @@ -97,6 +99,16 @@ class RemoteControl { rmcs_msgs::Keyboard keyboard = rmcs_msgs::Keyboard::zero(); }; + // 超时互锁：仅当对方 valid 时本设备才允许超时失效，保证至少一路不失效 + auto update_timeout_interlock() const -> void { + const auto dr16_ok = dr16_ && dr16_->valid(); + const auto vt13_ok = vt13_ && vt13_->valid(); + if (dr16_) + dr16_->set_timeout_enabled(vt13_ok); + if (vt13_) + vt13_->set_timeout_enabled(dr16_ok); + } + ControlSource select_control_source() const { if (vt13_ && vt13_->valid()) { switch (vt13_->mode_switch()) { diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp index 4272bfda1..327999820 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp @@ -97,6 +97,8 @@ class Vt13 { ModeSwitch mode_switch() const noexcept { return mode_switch_; } bool valid() const noexcept { return valid_; } + void set_timeout_enabled(bool enabled) { timeout_enabled_ = enabled; } + const Eigen::Vector2d& joystick_left() const noexcept { return joystick_left_; } const Eigen::Vector2d& joystick_right() const noexcept { return joystick_right_; } @@ -278,7 +280,7 @@ class Vt13 { } void refresh_validity(const TimePoint now) { - if (!valid_ || now - last_remote_control_received_at_ <= kFreshTimeout) + if (!timeout_enabled_ || !valid_ || now - last_remote_control_received_at_ <= kFreshTimeout) return; reset_remote_control_state(); @@ -316,6 +318,7 @@ class Vt13 { TimePoint last_statistics_log_time_ = TimePoint::min(); bool valid_ = false; + bool timeout_enabled_ = true; std::size_t peak_readable_ = 0; std::size_t remote_success_count_ = 0; std::size_t verification_failures_ = 0; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp index 4233d7c76..fecbe501f 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp @@ -38,13 +38,15 @@ class Sentry get_component_name(), rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) { + constexpr auto kNaN = std::numeric_limits::quiet_NaN(); + register_input("/predefined/timestamp", timestamp_); register_output("/tf", tf_); + register_output("/chassis/climber/measure_yaw", chassis_measure_yaw_, kNaN); register_output("/auto_aim/camera_transform", camera_transform_); register_output("/auto_aim/barrel_direction", barrel_direction_); - register_output( - "/auto_aim/yaw_velocity", yaw_velocity_, std::numeric_limits::quiet_NaN()); + register_output("/auto_aim/yaw_velocity", yaw_velocity_, kNaN); // 提供 remote-status 命令服务。 using Srv = std_srvs::srv::Trigger; @@ -78,6 +80,10 @@ class Sentry *barrel_direction_ = *fast_tf::cast( PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); *yaw_velocity_ = gimbal_board_->yaw_velocity(); + + const auto chassis_direction = + fast_tf::cast(BaseLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + *chassis_measure_yaw_ = std::atan2(chassis_direction->y(), chassis_direction->x()); } private: @@ -628,6 +634,7 @@ class Sentry OutputInterface camera_transform_; OutputInterface barrel_direction_; OutputInterface yaw_velocity_; + OutputInterface chassis_measure_yaw_; std::unique_ptr gimbal_board_; std::unique_ptr chassis_board_; diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp index 182b95cc7..a279b92c7 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp @@ -12,10 +12,15 @@ enum class ChassisMode : uint8_t { LAUNCH_RAMP, ALIGNMENT, ALIGNMENT_POWERED, + CLIMB, }; -constexpr auto need_power(ChassisMode mode) noexcept { - return mode == ChassisMode::ALIGNMENT_POWERED || mode == ChassisMode::LAUNCH_RAMP; +constexpr auto is_powered(ChassisMode mode) noexcept { + return mode == ChassisMode::ALIGNMENT_POWERED || mode == ChassisMode::LAUNCH_RAMP + || mode == ChassisMode::CLIMB; +} +constexpr auto is_spining(ChassisMode mode) noexcept { + return mode == ChassisMode::SPIN_SLOW || mode == ChassisMode::SPIN_FAST; } } // namespace rmcs_msgs diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/rclcpp/node_mixin.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/rclcpp/node_mixin.hpp index 0a0369976..24eb14d19 100644 --- a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/rclcpp/node_mixin.hpp +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/rclcpp/node_mixin.hpp @@ -1,11 +1,16 @@ #pragma once #include +#include namespace rmcs_utility { struct NodeMixin { using node = NodeMixin; + static constexpr auto options() noexcept { + return rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true); + } + template auto info(this const Self& self, std::format_string fmt, Args&&... args) -> void { auto text = std::format(fmt, std::forward(args)...); @@ -39,6 +44,11 @@ struct NodeMixin { requires std::convertible_to { dst = self.template get_parameter_or(name, fallback); } + + template + auto param_or(this const auto& self, const std::string& name, const T& fallback) -> T { + return self.template get_parameter_or(name, fallback); + } }; } // namespace rmcs_utility From 5a61c94bd040bbdec7ae2f48bfacd581aac6da87 Mon Sep 17 00:00:00 2001 From: noskillzheng <3515964992@qq.com> Date: Mon, 27 Jul 2026 23:46:19 +0800 Subject: [PATCH 45/86] wip(robots): Flight development (#94) * chore: Clean up code * feat: Add mavlink to dockerfile * feat: Adapt autoaim on flight * feat: Add flight odin support by mavlink component * fix: Add px4_serial output (forgot in flight.cpp) * chore: Update autoaim offset * chore: Add submodule odin and hikcamera * fix: Add autoaim param degraded_angle_speed,refactor:odin management in px4_vision_bridge * chore: Remove hikcamera and odin_ros_driver submodules * chore: Apply format * chore: Remove unused cmake config --------- Co-authored-by: creeper5820 --- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 215 ------------------ rmcs_ws/src/rmcs_core/plugins.xml | 2 + rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 64 ++++-- 3 files changed, 41 insertions(+), 240 deletions(-) delete mode 100644 rmcs_ws/src/rmcs_bringup/config/flight.yaml diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml deleted file mode 100644 index 04bb643e9..000000000 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ /dev/null @@ -1,215 +0,0 @@ -rmcs_executor: - ros__parameters: - update_rate: 1000.0 - components: - - rmcs_core::hardware::Flight -> flight_hardware - - rmcs_core::hardware::Px4VisionBridge -> px4_vision_bridge - - - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller - - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller - - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller - - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller - - - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller - - rmcs_core::controller::shooting::HeatController -> heat_controller - - rmcs_core::controller::shooting::BulletFeederController17mm -> bullet_feeder_controller - - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller - - - rmcs_core::referee::command::Interaction -> referee_interaction - - rmcs_core::referee::Command -> referee_command - - rmcs_core::referee::Status -> referee_status - - rmcs_core::referee::command::interaction::Ui -> referee_ui - - rmcs_core::referee::app::ui::Flight -> referee_ui_flight - - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui - - # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster - # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster - - - rmcs::AutoAimCapturerComponent -> auto_aim_capturer - # - rmcs::AutoAimPlayerComponent -> auto_aim_player - - rmcs::AutoAimRecorderComponent -> auto_aim_recorder - - rmcs::AutoAimComponent -> auto_aim_component - -auto_aim_capturer: - ros__parameters: - camera_name: "" - exposure_us: 3000.0 - gain: 8.0 - framerate: 120.0 - invert_image: false - rls_tau_sec: 10.0 - use_hardware_sync: false - delay_ms: 6.5 - -auto_aim_recorder: - ros__parameters: - output_path: "/tmp/autoaim/records" - queue_depth: 16 - flush_every_n_frames: 64 - max_duration_seconds: 0 - max_videos_size_gb: 0.0 - -auto_aim_component: - ros__parameters: - # WARN: 危险！可选 red | blue，生效后裁判系统 ID 缺席时 - # 将强行绑定为对应阵营哨兵，仅供调试，严禁比赛启用。 - # 留空或填 unknow 表示禁用。 - dangerous_fallback: "" - manual_shoot: true - enable_rune: false - camera_translation: [0.10238, 0.0, 0.05286] - fire_control: - bullet_speed: 22.5 - shoot_delay: 0.05 - offset_yaw: +1.3 #越大越左 - offset_pitch: +1.7 #越大越下 - attack_window: 80.0 - degraded_angle_speed: 12.0 - window_redundancy: 0.8 - window_hysteresis: 0.2 - is_lazy_gimbal: false - attack_preaim: false - require_stable_command: false - yaw_tolerance: 0.07 - pitch_tolerance: 0.04 - rune_idle_duration: 0.4 - rune_shoot_duration: 0.2 - -px4_vision_bridge: - ros__parameters: - source_topic: /odin1/odometry_highfreq - system_id: 1 # 与 PX4 MAV_SYS_ID 一致 - component_id: 197 # MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY - max_send_rate_hz: 50.0 - # 挂载角(ZYX, rad): 传感器系->机体系, 初值 roll=π pitch=π/2 yaw=0 - mount_rpy: [3.14159265358979, 1.5707963267949, 0.0] - # Odin1 自启看门狗: 里程计断流超过 timeout 秒且距上次拉起超过 cooldown 秒 - # 才执行 tmux-launch.sh; 数据正常时不会重启 Odin1 (保住 SLAM 预热) - odin_autostart: true - odin_watchdog_timeout: 10.0 - odin_restart_cooldown: 60.0 # 需覆盖 SLAM 预热 30~60s - -value_broadcaster: - ros__parameters: - forward_list: - - /gimbal/pitch/angle - - /gimbal/pitch/velocity - - /gimbal/pitch/torque - - /gimbal/pitch/control_angle_error - - /gimbal/pitch/control_velocity - - /gimbal/pitch/velocity_imu - - - /gimbal/yaw/angle - - /gimbal/yaw/velocity - - /gimbal/yaw/torque - - /gimbal/yaw/control_torque - - /gimbal/yaw/control_angle_error - - /gimbal/yaw/velocity_imu - - /gimbal/bullet_feeder/velocity - -tf_broadcaster: - ros__parameters: - tf: /tf - -flight_hardware: - ros__parameters: - board_serial: "AF-7C58-5458-E731-9F74-1F9C-CAFD-30AF-9C09" - yaw_motor_zero_point: 11720 - pitch_motor_zero_point: 18578 - -referee_status: - ros__parameters: - path: /dev/tty0 - -gimbal_controller: - ros__parameters: - upper_limit: -0.39518 # -0.39518 rad ≈ -22.6° - lower_limit: 0.7 # 0.7 rad ≈ 40.1° - yaw_lower_limit: 0.1745 - yaw_upper_limit: 2.5708 - -yaw_angle_pid_controller: - ros__parameters: - measurement: /gimbal/yaw/control_angle_error - control: /gimbal/yaw/control_velocity - kp: 15.0 - ki: 0.0 - kd: 0.0 - -yaw_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/yaw/velocity_imu - setpoint: /gimbal/yaw/control_velocity - control: /gimbal/yaw/control_torque - kp: 6.0 - ki: 0.0 - kd: 0.0 - -pitch_angle_pid_controller: - ros__parameters: - measurement: /gimbal/pitch/control_angle_error - control: /gimbal/pitch/control_velocity - kp: 10.0 - ki: 0.0 - kd: 0.0 - -friction_wheel_controller: - ros__parameters: - friction_wheels: - - /gimbal/left_friction - - /gimbal/right_friction - friction_velocities: - - 620.0 - - 620.0 - friction_soft_start_stop_time: 1.0 - -heat_controller: - ros__parameters: - heat_per_shot: 10000 - reserved_heat: 15000 - -bullet_feeder_controller: - ros__parameters: - bullets_per_feeder_turn: 8.0 - shot_frequency: 20.0 - safe_shot_frequency: 10.0 - eject_frequency: 10.0 - eject_time: 0.05 - deep_eject_frequency: 5.0 - deep_eject_time: 0.2 - single_shot_max_stop_delay: 2.0 - -left_friction_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/left_friction/velocity - setpoint: /gimbal/left_friction/control_velocity - control: /gimbal/left_friction/control_torque - kp: 0.003436926 - ki: 0.00 - kd: 0.009373434 - -right_friction_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/right_friction/velocity - setpoint: /gimbal/right_friction/control_velocity - control: /gimbal/right_friction/control_torque - kp: 0.003436926 - ki: 0.00 - kd: 0.009373434 - -bullet_feeder_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/bullet_feeder/velocity - setpoint: /gimbal/bullet_feeder/control_velocity - control: /gimbal/bullet_feeder/control_torque - kp: 0.583 - ki: 0.0 - kd: 0.0 - -auto_aim_ui: - ros__parameters: - offset_x: 0.000 - offset_y: 0.000 - offset_z: 0.000 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 95777e34d..a9845583b 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -5,6 +5,8 @@ + + diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index d1c5b214f..754807075 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -2,7 +2,6 @@ #include #include #include -#include #include #include @@ -10,11 +9,14 @@ #include #include #include +#include +#include #include #include #include -#include "hardware/device/bmi088.hpp" +#include "hardware/device/bmi088_ekf.hpp" +#include "hardware/device/board_clock_lifter.hpp" #include "hardware/device/can_packet.hpp" #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" @@ -51,9 +53,6 @@ class Flight gimbal_bullet_feeder_.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM2006, 1}.enable_multi_turn_angle()); - bmi088_.set_coordinate_mapping( - [](double x, double y, double z) { return std::tuple{y, z, x}; }); - using namespace rmcs_description; constexpr auto kCameraPostionX = 0.10238; @@ -61,12 +60,11 @@ class Flight tf_->set_transform( Eigen::Translation3d{kCameraPostionX, 0.0, kCameraPostionZ}); - register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); - register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_, 0.0); + register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_, 0.0); register_output("/tf", tf_); - register_output("/auto_aim/camera_transform", camera_transform_); - register_output("/auto_aim/barrel_direction", barrel_direction_); + register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); register_output("/referee/serial", referee_serial_); referee_serial_->read = [this](std::byte* buffer, size_t size) { @@ -79,6 +77,14 @@ class Flight return size; }; + register_output("/px4/serial", px4_serial_); + px4_serial_->read = [](std::byte*, size_t) { return size_t{0}; }; + px4_serial_->write = [this](const std::byte* buffer, size_t size) { + board_->start_transmit().uart_transmit( + Spec::kUarts.kUart0, {.uart_data = std::span{buffer, size}}); + return size; + }; + remote_control_ = std::make_unique(*this); remote_control_->register_dr16(&dr16_); @@ -99,11 +105,6 @@ class Flight update_imu(); dr16_.update_status(); remote_control_->update(); - - using namespace rmcs_description; - *camera_transform_ = fast_tf::lookup_transform(*tf_); - *barrel_direction_ = - *fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); } void command_update() { @@ -162,13 +163,12 @@ class Flight void update_imu() { using namespace rmcs_description; - bmi088_.update_status(); - const auto gimbal_imu_pose = - Eigen::Quaterniond{bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - tf_->set_transform(gimbal_imu_pose.conjugate()); + if (const auto snapshot = bmi088_.snapshot()) { + tf_->set_transform(snapshot->orientation.conjugate()); - *gimbal_yaw_velocity_imu_ = bmi088_.gz(); - *gimbal_pitch_velocity_imu_ = bmi088_.gy(); + *gimbal_yaw_velocity_imu_ = snapshot->gyro_body.z(); + *gimbal_pitch_velocity_imu_ = snapshot->gyro_body.y(); + } } void @@ -225,11 +225,21 @@ class Flight } void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); + const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); + bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); } void gyroscope_receive_callback(const View::ImuGyroscope& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); + const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + + const auto snapshot = + bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); + if (!snapshot) + return; + + imu_snapshot_output_.emit(*snapshot); } private: @@ -258,15 +268,19 @@ class Flight device::Dr16 dr16_; std::unique_ptr remote_control_; - device::Bmi088 bmi088_{1000.0, 0.2, 0.00}; + // 等价于旧 Bmi088 的坐标映射 (x, y, z) -> (y, z, x)：body = body_to_sensor^T * sensor + device::Bmi088Ekf bmi088_{device::Bmi088Ekf::Config{ + .body_to_sensor = (Eigen::Matrix3d{} << 0, 0, 1, 1, 0, 0, 0, 1, 0).finished(), + }}; + device::BoardClockLifter board_clock_lifter_; OutputInterface gimbal_yaw_velocity_imu_; OutputInterface gimbal_pitch_velocity_imu_; OutputInterface tf_; OutputInterface referee_serial_; + OutputInterface px4_serial_; - OutputInterface camera_transform_; - OutputInterface barrel_direction_; + EventOutputInterface imu_snapshot_output_; rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; }; From 4c49549c4e94b09bf8b19359013f4a5f5d9406ae Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Mon, 27 Jul 2026 22:32:32 +0800 Subject: [PATCH 46/86] chore: Add optional flag for robots and complete rmcs_msgs format --- rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml | 1 + rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml | 1 + .../rmcs_bringup/config/steering-hero-little-six-friction.yaml | 1 + rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp | 1 + 4 files changed, 4 insertions(+) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 2ff00181a..7c1649453 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -74,6 +74,7 @@ auto_aim_component: # 留空或填 unknow 表示禁用。 dangerous_fallback: "" manual_shoot: true + enable_rune: true camera_translation: [0.058, -0.08, 0.0] fire_control: bullet_speed: 22.5 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 9c26fb78b..95edf145b 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -74,6 +74,7 @@ auto_aim_component: # 留空或填 unknow 表示禁用。 dangerous_fallback: "" manual_shoot: true + enable_rune: true camera_translation: [0.058, -0.08, 0.0] fire_control: bullet_speed: 22.5 diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml index e745a3eba..0f2dd6982 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml @@ -86,6 +86,7 @@ auto_aim_component: # 留空或填 unknow 表示禁用。 dangerous_fallback: "" manual_shoot: false + enable_rune: false camera_translation: [0.25, 0.0, -0.05] fire_control: bullet_speed: 11.5 diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp index b30d59be4..9694a65d4 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp @@ -49,6 +49,7 @@ constexpr auto to_string(ChassisMode mode) noexcept -> const char* { case ChassisMode::SPIN_SLOW: return "SPIN_SLOW"; case ChassisMode::ALIGNMENT: return "ALIGNMENT"; case ChassisMode::ALIGNMENT_POWERED: return "ALIGNMENT_POWERED"; + case ChassisMode::CLIMB: return "CLIMB"; } return "INVALID"; } From d89b0f245853ed36e39a4038378038d8e8ee6463 Mon Sep 17 00:00:00 2001 From: zlq04222 <1542498005@qq.com> Date: Tue, 28 Jul 2026 04:34:32 +0800 Subject: [PATCH 47/86] wip(robots): Sentry development (#97) * wip: Develop sentry climb and decision * feat: Enable navigation and add referee enemy status outputs - Enable SentryDecision and Navigation plugins in sentry config - Parameterize steering wheel controller PID gains - Add enemy outpost/base hp and damage difference outputs * wip: Clean up pr impl and update auto aim v2 --------- Co-authored-by: creeper5820 --- .../rmcs_bringup/config/auto_aim_test.yaml | 8 +- rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 23 +- rmcs_ws/src/rmcs_core/plugins.xml | 1 + .../command/interaction/sentry_decision.cpp | 243 ------------------ .../rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp | 1 + 5 files changed, 19 insertions(+), 257 deletions(-) delete mode 100644 rmcs_ws/src/rmcs_core/src/referee/command/interaction/sentry_decision.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml b/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml index b110fcfd1..566d2d90c 100644 --- a/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml @@ -2,14 +2,14 @@ rmcs_executor: ros__parameters: update_rate: 1000.0 components: - # - rmcs::AutoAimPlayerComponent -> auto_aim_player - - rmcs::AutoAimVideoPlayerComponent -> auto_aim_video_player + - rmcs::AutoAimPlayerComponent -> auto_aim_player + # - rmcs::AutoAimVideoPlayerComponent -> auto_aim_video_player # - rmcs::AutoAimRecorderComponent -> auto_aim_recorder - rmcs::AutoAimComponent -> auto_aim_component auto_aim_player: ros__parameters: - input_path: "/workspaces/data/autoaim/自家大符/" + input_path: "/workspaces/data/autoaim/robot/blue_fast_track/" loop_play: true auto_aim_video_player: @@ -25,11 +25,13 @@ auto_aim_recorder: flush_every_n_frames: 64 max_duration_seconds: 0 max_videos_size_gb: 0.0 + auto_record: false auto_aim_component: ros__parameters: dangerous_fallback: "red" manual_shoot: false + enable_rune: true camera_translation: [0., 0., 0.] fire_control: diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index de6f840c8..a4b85a3a8 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -7,7 +7,7 @@ rmcs_executor: - rmcs_core::referee::Status -> referee_status - rmcs_core::referee::Command -> referee_command - rmcs_core::referee::command::Interaction -> referee_interaction - # - rmcs_core::referee::command::interaction::SentryDecision -> referee_sentry_decision + - rmcs_core::referee::command::interaction::SentryDecision -> referee_sentry_decision - rmcs_core::referee::command::interaction::Ui -> referee_ui - rmcs_core::controller::gimbal::EccentricDualYaw -> gimbal_controller @@ -24,9 +24,9 @@ rmcs_executor: # - rmcs::navigation::Navigation -> rmcs_navigation - # - rmcs::AutoAimRecorderComponent -> auto_aim_recorder - # - rmcs::AutoAimCapturerComponent -> auto_aim_capturer - # - rmcs::AutoAimComponent -> auto_aim_component + - rmcs::AutoAimRecorderComponent -> auto_aim_recorder + - rmcs::AutoAimCapturerComponent -> auto_aim_capturer + - rmcs::AutoAimComponent -> auto_aim_component # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster @@ -34,7 +34,7 @@ rmcs_executor: rmcs_navigation: ros__parameters: command_vel_name: "/cmd_vel" - endpoint: "otaku" + endpoint: "train" enable_goal_topic_forward: true auto_aim_capturer: @@ -53,8 +53,9 @@ auto_aim_recorder: output_path: "/tmp/autoaim/records" queue_depth: 16 flush_every_n_frames: 64 - max_duration_seconds: 0 - max_videos_size_gb: 0.0 + max_duration_seconds: 300 + max_videos_size_gb: 100.0 + auto_record: false auto_aim_component: ros__parameters: @@ -63,10 +64,10 @@ auto_aim_component: camera_translation: [0.07128, 0.0, 0.0481] fire_control: bullet_speed: 22.5 - shoot_delay: 0.1 - offset_yaw: -0.5 - offset_pitch: +1.0 - attack_window: 120.0 + shoot_delay: 0.02 + offset_yaw: -0.3 + offset_pitch: +0.9 + attack_window: 80.0 degraded_angle_speed: 12.0 window_hysteresis: 0.2 attack_preaim: false diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index a9845583b..a066759ae 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -54,6 +54,7 @@ + diff --git a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/sentry_decision.cpp b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/sentry_decision.cpp deleted file mode 100644 index ec4403fa4..000000000 --- a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/sentry_decision.cpp +++ /dev/null @@ -1,243 +0,0 @@ -#include "referee/command/field.hpp" -#include "referee/command/interaction/header.hpp" -#include "referee/status/field.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace rmcs_core::referee::command::interaction { - -class SentryDecision - : public rmcs_executor::Component - , public rclcpp::Node { -public: - using Command = status::SentryCommand; - using Posture = Command::Posture; - using SentryEvent = rmcs_msgs::SentryEvent; - using EventCounts = std::unordered_map; - using Clock = std::chrono::steady_clock; - - InputInterface sentry_events_; - InputInterface robot_id_; - InputInterface sentry_posture_fb_; - InputInterface robot_hp_fb_; - InputInterface energy_core_status_; - InputInterface can_rebirth_free_; - - OutputInterface sentry_decision_field_; - - Header header_{}; - Command command_{}; - - EventCounts cached_events_; - std::unordered_set requests_; - std::unordered_map pose_targets_; - std::unordered_set logged_events_; - std::uint8_t last_fb_posture_ = 3; - bool last_can_rebirth_free_ = false; - - static inline const auto kPoseEvents = std::unordered_set{ - SentryEvent::SWITCH_POSE_ATTACK, - SentryEvent::SWITCH_POSE_DEFENSE, - SentryEvent::SWITCH_POSE_MOVE, - SentryEvent::SWITCH_POSE_POWERED_ATTACK, - SentryEvent::SWITCH_POSE_POWERED_DEFENSE, - SentryEvent::SWITCH_POSE_POWERED_MOVE, - }; - - static auto to_posture(SentryEvent event) -> Posture { - switch (event) { - case SentryEvent::SWITCH_POSE_ATTACK: return Posture::ATTACK; - case SentryEvent::SWITCH_POSE_DEFENSE: return Posture::DEFENSE; - case SentryEvent::SWITCH_POSE_MOVE: return Posture::MOVE; - case SentryEvent::SWITCH_POSE_POWERED_ATTACK: return Posture::POWERED_ATTACK; - case SentryEvent::SWITCH_POSE_POWERED_DEFENSE: return Posture::POWERED_DEFENSE; - case SentryEvent::SWITCH_POSE_POWERED_MOVE: return Posture::POWERED_MOVE; - default: return Posture::UNKNOWN; - } - } - - static constexpr auto kEventPriority = std::array{ - SentryEvent::CONFIRM_REBIRTH, - SentryEvent::CONFIRM_INSTANT_REBIRTH, - SentryEvent::SWITCH_POSE_ATTACK, - SentryEvent::SWITCH_POSE_DEFENSE, - SentryEvent::SWITCH_POSE_MOVE, - SentryEvent::SWITCH_POSE_POWERED_ATTACK, - SentryEvent::SWITCH_POSE_POWERED_DEFENSE, - SentryEvent::SWITCH_POSE_POWERED_MOVE, - SentryEvent::EXCHANGE_AMMO_SUPPLY_POINT, - SentryEvent::EXCHANGE_AMMO_REMOTE, - SentryEvent::EXCHANGE_HP_REMOTE, - SentryEvent::ACTIVATE_ENERGY_CORE, - }; - - SentryDecision() - : Node{ - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} { - - register_input("/referee/id", robot_id_); - register_input("/rmcs_navigation/sentry_events", sentry_events_, false); - register_input("/referee/sentry/posture", sentry_posture_fb_, false); - register_input("/referee/current_hp", robot_hp_fb_, false); - register_input( - "/referee/event/ally_big_energy_activation_status", energy_core_status_, false); - register_input("/referee/sentry/can_rebirth_free", can_rebirth_free_, false); - - register_output("/referee/command/interaction/sentry_decision", sentry_decision_field_); - } - - auto before_updating() -> void override { - if (!sentry_events_.ready()) - sentry_events_.make_and_bind_directly(); - if (!sentry_posture_fb_.ready()) - sentry_posture_fb_.make_and_bind_directly(uint8_t{3}); - if (!robot_hp_fb_.ready()) - robot_hp_fb_.make_and_bind_directly(uint16_t{0}); - if (!energy_core_status_.ready()) - energy_core_status_.make_and_bind_directly(uint8_t{0}); - if (!can_rebirth_free_.ready()) - can_rebirth_free_.make_and_bind_directly(false); - } - - auto update() -> void override { - if (*robot_id_ == rmcs_msgs::RobotId::UNKNOWN) { - *sentry_decision_field_ = Field{}; - return; - } - - detect_new_events(); - - const auto can_rebirth_free = *can_rebirth_free_; - if (can_rebirth_free && !last_can_rebirth_free_) { - requests_.insert(SentryEvent::CONFIRM_REBIRTH); - } - last_can_rebirth_free_ = can_rebirth_free; - - consume_one_event(); - verify_feedback(); - } - -private: - auto detect_new_events() -> void { - const auto& input = *sentry_events_; - - for (const auto event : kEventPriority) { - auto input_it = input.find(event); - auto input_count = (input_it != input.end()) ? input_it->second : uint16_t{0}; - auto cache_count = cached_events_[event]; - - if (cache_count != input_count) { - if (kPoseEvents.contains(event)) { - for (const auto rm : kPoseEvents) - requests_.erase(rm); - } - requests_.insert(event); - cached_events_[event] = input_count; - } - } - } - - auto consume_one_event() -> void { - if (requests_.empty()) { - *sentry_decision_field_ = Field{}; - return; - } - - const auto id = rmcs_msgs::FullRobotId{*robot_id_}; - header_.command_id = 0x0120; - header_.sender_id = id; - header_.receiver_id = rmcs_msgs::FullRobotId::REFEREE_SERVER; - - for (const auto event : kEventPriority) { - if (!requests_.contains(event)) - continue; - - command_ = Command{}; - - if (kPoseEvents.contains(event)) { - command_.posture = to_posture(event); - pose_targets_[event] = to_posture(event); - } else if (event == SentryEvent::CONFIRM_REBIRTH) { - command_.rebirth_confirm = 1; - } else if (event == SentryEvent::CONFIRM_INSTANT_REBIRTH) { - command_.instant_rebirth_confirm = 1; - } else if (event == SentryEvent::EXCHANGE_AMMO_SUPPLY_POINT) { - command_.ammo_exchange = 1; - } else if (event == SentryEvent::EXCHANGE_AMMO_REMOTE) { - command_.remote_ammo_request = 1; - } else if (event == SentryEvent::EXCHANGE_HP_REMOTE) { - command_.remote_hp_request = 1; - } else if (event == SentryEvent::ACTIVATE_ENERGY_CORE) { - command_.energy_core_confirm = 1; - } - - *sentry_decision_field_ = MAKE_FIELD(header_, command_); - - if (kPoseEvents.contains(event)) { - if (!logged_events_.contains(event)) { - RCLCPP_INFO( - get_logger(), "Sentry pose command: %d", - std::to_underlying(command_.posture)); - logged_events_.insert(event); - } - } - break; - } - } - - auto verify_feedback() -> void { - const auto fb_posture_id = *sentry_posture_fb_; - const auto fb_hp = *robot_hp_fb_; - const auto energy_core_status = *energy_core_status_; - - if (fb_posture_id != last_fb_posture_) { - RCLCPP_INFO( - get_logger(), "Sentry posture feedback: %d → %d", last_fb_posture_, fb_posture_id); - last_fb_posture_ = fb_posture_id; - } - - auto to_erase = std::vector{}; - for (const auto event : requests_) { - if (kPoseEvents.contains(event)) { - auto it = pose_targets_.find(event); - if (it != pose_targets_.end() - && static_cast(it->second) == fb_posture_id) { - to_erase.push_back(event); - pose_targets_.erase(it); - logged_events_.erase(event); - } - } else if ( - event == SentryEvent::CONFIRM_REBIRTH - || event == SentryEvent::CONFIRM_INSTANT_REBIRTH) { - if (fb_hp > 0) { - to_erase.push_back(event); - } - } else if (event == SentryEvent::ACTIVATE_ENERGY_CORE) { - // FIXME: 加 5s 超时销毁 - if (energy_core_status != 0) { - to_erase.push_back(event); - } - } else { - to_erase.push_back(event); - } - } - - for (const auto event : to_erase) - requests_.erase(event); - } -}; - -} // namespace rmcs_core::referee::command::interaction - -#include -PLUGINLIB_EXPORT_CLASS( - rmcs_core::referee::command::interaction::SentryDecision, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp index 9694a65d4..1cccd175a 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp @@ -20,6 +20,7 @@ #include "mouse.hpp" // IWYU pragma: export #include "robot_color.hpp" // IWYU pragma: export #include "robot_id.hpp" // IWYU pragma: export +#include "sentry_event.hpp" // IWYU pragma: export #include "serial_interface.hpp" // IWYU pragma: export #include "shoot_mode.hpp" // IWYU pragma: export #include "shoot_status.hpp" // IWYU pragma: export From e1ebf4eddf68af1f7db7d7acacfa3d2059bf0bc2 Mon Sep 17 00:00:00 2001 From: zlq04222 <1542498005@qq.com> Date: Tue, 28 Jul 2026 17:27:19 +0800 Subject: [PATCH 48/86] wip(robots): Sentry development (#98) * wip(robots): Enable sentry navigation, add navigation supercap boost and refine climber status * wip(robots): Add leave phase after sentry climber landing --- rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 20 ++++++++++--------- .../chassis/chassis_power_controller.cpp | 7 ++++++- 2 files changed, 17 insertions(+), 10 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index a4b85a3a8..2dd12af46 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -22,7 +22,7 @@ rmcs_executor: - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller - # - rmcs::navigation::Navigation -> rmcs_navigation + - rmcs::navigation::Navigation -> rmcs_navigation - rmcs::AutoAimRecorderComponent -> auto_aim_recorder - rmcs::AutoAimCapturerComponent -> auto_aim_capturer @@ -40,7 +40,7 @@ rmcs_navigation: auto_aim_capturer: ros__parameters: camera_name: "" - exposure_us: 2000.0 + exposure_us: 1000.0 gain: 8.0 framerate: 120.0 invert_image: true @@ -60,7 +60,7 @@ auto_aim_recorder: auto_aim_component: ros__parameters: manual_shoot: true - enable_rune: true + enable_rune: false camera_translation: [0.07128, 0.0, 0.0481] fire_control: bullet_speed: 22.5 @@ -68,7 +68,7 @@ auto_aim_component: offset_yaw: -0.3 offset_pitch: +0.9 attack_window: 80.0 - degraded_angle_speed: 12.0 + degraded_angle_speed: 10.0 window_hysteresis: 0.2 attack_preaim: false require_stable_command: false @@ -144,7 +144,7 @@ sentry_climber: stick_group: speed_drop: 30.0 speed_rise: 60.0 - rise_torque_limit: 1.2 + rise_torque_limit: 1.5 land_speed_begin: 100.0 land_speed_final: 10.0 land_duration: 0.5 @@ -158,7 +158,7 @@ sentry_climber: hold_torque: 0.01 block_hold: 0.05 align: - err: 0.20 + err: 0.18 w: 0.2 hold: 0.05 timeout: 15.0 @@ -168,7 +168,7 @@ sentry_climber: approach_vx: 1.2 deploy_vx: 0.3 dash_vx: 3.0 - retract_vx: 0.3 + retract_vx: 0.0 dash_min: 0.1 dash_duration: 0.8 stick_timeout: 8.0 @@ -181,6 +181,8 @@ sentry_climber: stick_timeout: 8.0 soft_timeout: 3.0 settle_timeout: 8.0 + leave_vx: 0.3 + leave_duration: 1.0 friction_wheel_controller: ros__parameters: @@ -247,11 +249,11 @@ steering_wheel_controller: no_load_power: 11.37 chassis_translation_kp: 20.0 - chassis_translation_ki: 0.001 + chassis_translation_ki: 0.0001 chassis_translation_kd: 0.00 chassis_angular_velocity_kp: 8.0 - chassis_angular_velocity_ki: 0.0 + chassis_angular_velocity_ki: 0.005 chassis_angular_velocity_kd: 1.0 steering_velocity_kp: 0.15 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp index b4bad88fb..f9e8e83a9 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp @@ -30,6 +30,7 @@ class ChassisPowerController register_input("/chassis/power", chassis_power_); register_input("/chassis/supercap/voltage", supercap_voltage_); register_input("/chassis/supercap/enabled", supercap_enabled_); + register_input("/rmcs_navigation/enable_supercap", navigation_supercap_, false); register_input("/referee/chassis/power_limit", chassis_power_limit_referee_); register_input("/referee/chassis/buffer_energy", chassis_buffer_energy_referee_); @@ -110,7 +111,10 @@ class ChassisPowerController void update_control_power_limit() { double power_limit; - if (boost_mode_ && *supercap_enabled_) + const auto navigation_supercap_boost = + navigation_supercap_.ready() && *navigation_supercap_; + + if ((boost_mode_ || navigation_supercap_boost) && *supercap_enabled_) power_limit = rmcs_msgs::is_powered(*mode_) ? inf_ : *chassis_power_limit_referee_ + 80.0; else @@ -159,6 +163,7 @@ class ChassisPowerController InputInterface supercap_voltage_; InputInterface supercap_enabled_; + InputInterface navigation_supercap_; InputInterface chassis_power_limit_referee_; InputInterface chassis_buffer_energy_referee_; From 32aa7c24edde2768264a39298524a32ce8364388 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Tue, 28 Jul 2026 20:08:44 +0800 Subject: [PATCH 49/86] wip: Sentry climber and stuck detection while rotating --- rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 10 ++++---- .../controller/chassis/chassis_controller.cpp | 24 +++++++------------ 2 files changed, 14 insertions(+), 20 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index 2dd12af46..a7abac03c 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -40,7 +40,7 @@ rmcs_navigation: auto_aim_capturer: ros__parameters: camera_name: "" - exposure_us: 1000.0 + exposure_us: 3000.0 gain: 8.0 framerate: 120.0 invert_image: true @@ -60,7 +60,7 @@ auto_aim_recorder: auto_aim_component: ros__parameters: manual_shoot: true - enable_rune: false + enable_rune: true camera_translation: [0.07128, 0.0, 0.0481] fire_control: bullet_speed: 22.5 @@ -172,10 +172,10 @@ sentry_climber: dash_min: 0.1 dash_duration: 0.8 stick_timeout: 8.0 - approach_timeout: 8.0 + approach_timeout: 6.0 land: dash_vx: 1.0 - soft_vx: 0.4 + soft_vx: 0.3 land_pitch: 0.15 land_delay: 0.2 stick_timeout: 8.0 @@ -249,7 +249,7 @@ steering_wheel_controller: no_load_power: 11.37 chassis_translation_kp: 20.0 - chassis_translation_ki: 0.0001 + chassis_translation_ki: 0.00 chassis_translation_kd: 0.00 chassis_angular_velocity_kp: 8.0 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp index 88d033483..81f6bbaf3 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp @@ -148,23 +148,19 @@ class ChassisController *chassis_control_velocity_ = {kNaN, kNaN, kNaN}; spin_stuck_count_ = 0; - spin_recovery_count_ = 0; + spin_reverse_cooldown_ = 0; following_velocity_controller_.reset(); } - auto update_spin_stuck_watchdog(rmcs_msgs::ChassisMode& mode) -> void { + auto update_spin_stuck_watchdog(rmcs_msgs::ChassisMode mode) -> void { constexpr auto kSpinStuckConfirmTicks = std::size_t{300}; - constexpr auto kSpinRecoveryTicks = std::size_t{1000}; + constexpr auto kSpinReverseCooldownTicks = std::size_t{1000}; constexpr auto kSpinStuckAngularVelocityRatio = double{0.2}; using rmcs_msgs::ChassisMode; - if (spin_recovery_count_ > 0) { - mode = ChassisMode::ALIGNMENT_POWERED; - - if (--spin_recovery_count_ == 0) - mode = mode_before_watchdog_; - + if (spin_reverse_cooldown_ > 0) { + --spin_reverse_cooldown_; spin_stuck_count_ = 0; return; } @@ -183,12 +179,11 @@ class ChassisController if (++spin_stuck_count_ < kSpinStuckConfirmTicks) return; - mode_before_watchdog_ = mode; - mode = ChassisMode::ALIGNMENT_POWERED; - spin_recovery_count_ = kSpinRecoveryTicks; + spinning_forward_ = !spinning_forward_; + spin_reverse_cooldown_ = kSpinReverseCooldownTicks; spin_stuck_count_ = 0; - node::warn("Spin stuck detected, disable spinning for 1s."); + node::warn("Spin stuck detected, reverse spinning direction."); } void update_velocity_control() { auto translational_velocity = update_translational_velocity_control(); @@ -367,8 +362,7 @@ class ChassisController bool spinning_forward_ = true; std::size_t spin_stuck_count_ = 0; - std::size_t spin_recovery_count_ = 0; - rmcs_msgs::ChassisMode mode_before_watchdog_ = rmcs_msgs::ChassisMode::AUTO; + std::size_t spin_reverse_cooldown_ = 0; pid::PidCalculator following_velocity_controller_{ node::param_or("following_velocity_kp", 8.0), From dd7c807ff7081c728116c3012939c191c8da2dcc Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Fri, 31 Jul 2026 23:10:35 +0800 Subject: [PATCH 50/86] wip: Adapt auto aim config --- rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml | 5 +++-- .../config/deformable-infantry-omni-b.yaml | 1 + .../config/deformable-infantry-omni.yaml | 1 + rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 13 +++++++------ .../config/steering-hero-little-six-friction.yaml | 1 + 5 files changed, 13 insertions(+), 8 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml b/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml index 566d2d90c..3c02425ea 100644 --- a/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml @@ -2,8 +2,8 @@ rmcs_executor: ros__parameters: update_rate: 1000.0 components: - - rmcs::AutoAimPlayerComponent -> auto_aim_player - # - rmcs::AutoAimVideoPlayerComponent -> auto_aim_video_player + # - rmcs::AutoAimPlayerComponent -> auto_aim_player + - rmcs::AutoAimVideoPlayerComponent -> auto_aim_video_player # - rmcs::AutoAimRecorderComponent -> auto_aim_recorder - rmcs::AutoAimComponent -> auto_aim_component @@ -41,6 +41,7 @@ auto_aim_component: offset_pitch: 0.0 attack_window: 60.0 degraded_angle_speed: 12.0 + window_redundancy: 0.8 window_hysteresis: 0.2 attack_preaim: false require_stable_command: true diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 7c1649453..f7c20cbb5 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -83,6 +83,7 @@ auto_aim_component: offset_pitch: +0.5 attack_window: 80.0 degraded_angle_speed: 12.0 + window_redundancy: 0.8 window_hysteresis: 0.2 attack_preaim: false require_stable_command: false diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 95edf145b..3cff304fd 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -83,6 +83,7 @@ auto_aim_component: offset_pitch: -0.4 attack_window: 80.0 degraded_angle_speed: 12.0 + window_redundancy: 0.8 window_hysteresis: 0.2 attack_preaim: false require_stable_command: false diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index a7abac03c..257b00059 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -22,7 +22,7 @@ rmcs_executor: - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller - - rmcs::navigation::Navigation -> rmcs_navigation + # - rmcs::navigation::Navigation -> rmcs_navigation - rmcs::AutoAimRecorderComponent -> auto_aim_recorder - rmcs::AutoAimCapturerComponent -> auto_aim_capturer @@ -64,11 +64,12 @@ auto_aim_component: camera_translation: [0.07128, 0.0, 0.0481] fire_control: bullet_speed: 22.5 - shoot_delay: 0.02 + shoot_delay: 0.05 offset_yaw: -0.3 - offset_pitch: +0.9 + offset_pitch: +0.1 attack_window: 80.0 - degraded_angle_speed: 10.0 + degraded_angle_speed: 12.0 + window_redundancy: 0.8 window_hysteresis: 0.2 attack_preaim: false require_stable_command: false @@ -101,14 +102,14 @@ gimbal_controller: upper_limit: -0.65 lower_limit: 0.36 - top_yaw_angle_kp: 35.0 + top_yaw_angle_kp: 25.0 top_yaw_angle_ki: 0.008 top_yaw_angle_kd: 0.005 top_yaw_velocity_kp: 2.160 top_yaw_velocity_ki: 0.00 top_yaw_velocity_kd: 0.0 - bottom_yaw_angle_kp: 16.0 + bottom_yaw_angle_kp: 12.0 bottom_yaw_angle_ki: 0.01 bottom_yaw_angle_kd: 0.0 bottom_yaw_velocity_kp: 2.75 diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml index 0f2dd6982..a6af9a993 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml @@ -95,6 +95,7 @@ auto_aim_component: offset_pitch: 0.0 attack_window: 120.0 degraded_angle_speed: 12.0 + window_redundancy: 0.8 window_hysteresis: 0.2 attack_preaim: false require_stable_command: false From 6d3b85771ca17393c142a9afd28c21b09e939871 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sat, 1 Aug 2026 00:05:39 +0800 Subject: [PATCH 51/86] chore: Modify sentry param --- rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index 257b00059..760b41920 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -66,7 +66,7 @@ auto_aim_component: bullet_speed: 22.5 shoot_delay: 0.05 offset_yaw: -0.3 - offset_pitch: +0.1 + offset_pitch: -0.05 attack_window: 80.0 degraded_angle_speed: 12.0 window_redundancy: 0.8 From 1b236dd73768aefcea32b5271bb1fbe8510b5a92 Mon Sep 17 00:00:00 2001 From: zlq04222 <1542498005@qq.com> Date: Sat, 1 Aug 2026 03:38:27 +0800 Subject: [PATCH 52/86] chore: Tune sentry gimbal PID params and add steering wheel integral limits (#99) --- rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 23 ++++++++++++++++++--- 1 file changed, 20 insertions(+), 3 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index 760b41920..3db6f9bec 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -41,7 +41,7 @@ auto_aim_capturer: ros__parameters: camera_name: "" exposure_us: 3000.0 - gain: 8.0 + gain: 10.0 framerate: 120.0 invert_image: true rls_tau_sec: 10.0 @@ -102,14 +102,14 @@ gimbal_controller: upper_limit: -0.65 lower_limit: 0.36 - top_yaw_angle_kp: 25.0 + top_yaw_angle_kp: 30.0 top_yaw_angle_ki: 0.008 top_yaw_angle_kd: 0.005 top_yaw_velocity_kp: 2.160 top_yaw_velocity_ki: 0.00 top_yaw_velocity_kd: 0.0 - bottom_yaw_angle_kp: 12.0 + bottom_yaw_angle_kp: 15.0 bottom_yaw_angle_ki: 0.01 bottom_yaw_angle_kd: 0.0 bottom_yaw_velocity_kp: 2.75 @@ -123,6 +123,21 @@ gimbal_controller: pitch_velocity_ki: 0.0 pitch_velocity_kd: 0.0 + top_yaw_angle_integral_min: -187.0 + top_yaw_angle_integral_max: 187.0 + top_yaw_velocity_integral_min: -2400.0 + top_yaw_velocity_integral_max: 2400.0 + + bottom_yaw_angle_integral_min: -150.0 + bottom_yaw_angle_integral_max: 150.0 + bottom_yaw_velocity_integral_min: -2400.0 + bottom_yaw_velocity_integral_max: 2400.0 + + pitch_angle_integral_min: -150.0 + pitch_angle_integral_max: 150.0 + pitch_velocity_integral_min: -2400.0 + pitch_velocity_integral_max: 2400.0 + chassis_controller: ros__parameters: angular_velocity_max: 10.0 @@ -252,10 +267,12 @@ steering_wheel_controller: chassis_translation_kp: 20.0 chassis_translation_ki: 0.00 chassis_translation_kd: 0.00 + chassis_translation_integral_limit: 200.0 chassis_angular_velocity_kp: 8.0 chassis_angular_velocity_ki: 0.005 chassis_angular_velocity_kd: 1.0 + chassis_angular_velocity_integral_limit: 200.0 steering_velocity_kp: 0.15 steering_velocity_ki: 0.0 From 80e27055871b384bd10f1c5bcd99431b9685ae85 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sat, 1 Aug 2026 04:03:48 +0800 Subject: [PATCH 53/86] feat: Add top yaw velocity feedforward to reduce sentry auto-aim tracking lag --- rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 15 +++-- .../controller/gimbal/eccentric_dual_yaw.cpp | 63 +++++++++++++++++-- .../gimbal/eccentric_dual_yaw_solver.hpp | 5 ++ 3 files changed, 74 insertions(+), 9 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index 3db6f9bec..cf1143791 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -41,7 +41,7 @@ auto_aim_capturer: ros__parameters: camera_name: "" exposure_us: 3000.0 - gain: 10.0 + gain: 12.0 framerate: 120.0 invert_image: true rls_tau_sec: 10.0 @@ -66,7 +66,7 @@ auto_aim_component: bullet_speed: 22.5 shoot_delay: 0.05 offset_yaw: -0.3 - offset_pitch: -0.05 + offset_pitch: -0.1 attack_window: 80.0 degraded_angle_speed: 12.0 window_redundancy: 0.8 @@ -106,7 +106,7 @@ gimbal_controller: top_yaw_angle_ki: 0.008 top_yaw_angle_kd: 0.005 top_yaw_velocity_kp: 2.160 - top_yaw_velocity_ki: 0.00 + top_yaw_velocity_ki: 0.0 top_yaw_velocity_kd: 0.0 bottom_yaw_angle_kp: 15.0 @@ -116,11 +116,11 @@ gimbal_controller: bottom_yaw_velocity_ki: 0.00125 bottom_yaw_velocity_kd: 0.0 - pitch_angle_kp: 32.0 + pitch_angle_kp: 35.0 pitch_angle_ki: 0.01 pitch_angle_kd: 0.0 pitch_velocity_kp: 2.5 - pitch_velocity_ki: 0.0 + pitch_velocity_ki: 0.01 pitch_velocity_kd: 0.0 top_yaw_angle_integral_min: -187.0 @@ -138,6 +138,11 @@ gimbal_controller: pitch_velocity_integral_min: -2400.0 pitch_velocity_integral_max: 2400.0 + top_yaw_velocity_ff_gain: 1.0 + top_yaw_ff_cutoff_hz: 10.0 + top_yaw_ff_max: 2.0 + top_yaw_ff_jump_threshold: 0.01 + chassis_controller: ros__parameters: angular_velocity_max: 10.0 diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp index bea900342..526853c05 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp @@ -18,6 +18,49 @@ namespace rmcs_core::controller::gimbal { using namespace rmcs_description; +// 对 top 关节自瞄参考角做差分+低通滤波的目标角速度前馈， +// 补偿斜坡跟踪滞后；切板跳变时清零。 +struct YawRateFeedforward { + double gain = 1.0; + double cutoff_hz = 15.0; + double max_rate = 6.0; + double jump_threshold = 0.05; + + auto update(double azimuth, std::chrono::steady_clock::time_point now) -> double { + if (std::isfinite(prev_azimuth_)) { + const auto dt = std::chrono::duration(now - prev_timestamp_).count(); + const auto delta = limit_rad(azimuth - prev_azimuth_); + if (std::abs(delta) > jump_threshold) { + filtered_rate_ = 0.0; + } else if (dt > kMinDt) { + const auto raw = delta / dt; + const auto alpha = dt / (dt + 1.0 / (2.0 * std::numbers::pi * cutoff_hz)); + filtered_rate_ += alpha * (raw - filtered_rate_); + } + } + prev_azimuth_ = azimuth; + prev_timestamp_ = now; + return gain * std::clamp(filtered_rate_, -max_rate, max_rate); + } + +private: + static constexpr double kNaN = std::numeric_limits::quiet_NaN(); + static constexpr double kMinDt = 1e-6; + + static auto limit_rad(double angle) -> double { + constexpr double kPi = std::numbers::pi_v; + while (angle > kPi) + angle -= 2.0 * kPi; + while (angle <= -kPi) + angle += 2.0 * kPi; + return angle; + } + + double prev_azimuth_ = kNaN; + double filtered_rate_ = 0.0; + std::chrono::steady_clock::time_point prev_timestamp_{}; +}; + class EccentricDualYaw : public rmcs_executor::Component , public rclcpp::Node { @@ -25,7 +68,12 @@ class EccentricDualYaw EccentricDualYaw() : Node{ get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} {} + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} { + get_parameter_or("top_yaw_velocity_ff_gain", top_yaw_ff_.gain, 1.0); + get_parameter_or("top_yaw_ff_cutoff_hz", top_yaw_ff_.cutoff_hz, 15.0); + get_parameter_or("top_yaw_ff_max", top_yaw_ff_.max_rate, 6.0); + get_parameter_or("top_yaw_ff_jump_threshold", top_yaw_ff_.jump_threshold, 0.05); + } auto before_updating() -> void override { if (!input_.navigation_enable_control.ready()) { @@ -60,7 +108,9 @@ class EccentricDualYaw upper_limit_, lower_limit_, }); - apply_control(error.bottom_yaw, error.top_yaw, error.pitch); + const auto top_yaw_ff = + top_yaw_ff_.update(solver_.top_target_azimuth(), *input_.timestamp); + apply_control(error.bottom_yaw, error.top_yaw, error.pitch, top_yaw_ff); const auto [_, cur_pitch] = current_barrel_yaw_pitch(); stored_bottom_yaw_target_ = limit_rad(current_bottom_world_yaw() + error.bottom_yaw); @@ -113,6 +163,8 @@ class EccentricDualYaw EccentricDualYawSolver solver_; + YawRateFeedforward top_yaw_ff_; + struct Input { explicit Input(rmcs_executor::Component& component) { component.register_input("/remote/joystick/left", joystick_left); @@ -298,12 +350,15 @@ class EccentricDualYaw return std::atan2(vector.y(), vector.x()); } - auto apply_control(double bottom_yaw_error, double top_yaw_error, double pitch_error) -> void { + auto apply_control( + double bottom_yaw_error, double top_yaw_error, double pitch_error, + double top_yaw_feedforward = 0.0) -> void { const auto current_bottom_velocity = *input_.bottom_yaw_velocity + *input_.chassis_yaw_velocity_imu; const auto bottom_velocity_ref = bottom_yaw_angle_pid_.update(bottom_yaw_error); - const auto top_velocity_ref = top_yaw_angle_pid_.update(top_yaw_error); + const auto top_velocity_ref = + top_yaw_angle_pid_.update(top_yaw_error) + top_yaw_feedforward; const auto pitch_velocity_ref = pitch_angle_pid_.update(pitch_error); *output_.top_yaw_control_torque = diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw_solver.hpp index 7f9d2fd07..41409120f 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw_solver.hpp @@ -29,6 +29,9 @@ class EccentricDualYawSolver { auto update(const Operation& op) -> Error { return op.update(*this); } auto enabled() const -> bool { return enabled_; } + // top 关节的自瞄参考角（desired_top），供角速度前馈差分使用 + auto top_target_azimuth() const -> double { return top_target_azimuth_; } + class SetDisabled : public Operation { private: auto update(EccentricDualYawSolver& s) const -> Error override { @@ -80,6 +83,7 @@ class EccentricDualYawSolver { const double bottom_error = limit_rad(center_azimuth - current_btm); const double desired_top = limit_rad(barrel_azimuth - center_azimuth); const double top_error = limit_rad(desired_top - current_top); + s.top_target_azimuth_ = desired_top; const double desired_pitch = std::clamp(barrel_pitch, upper_, lower_); const double pitch_error = limit_rad(desired_pitch - current_brl); @@ -150,6 +154,7 @@ class EccentricDualYawSolver { } bool enabled_ = false; + double top_target_azimuth_ = kNaN_; }; } // namespace rmcs_core::controller::gimbal From e0161cce1a1783efe2c8463fa4d4dabc8b8e3c8f Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sun, 2 Aug 2026 20:20:07 +0800 Subject: [PATCH 54/86] wip: Modify sentry config --- rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index cf1143791..24862238d 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -34,14 +34,14 @@ rmcs_executor: rmcs_navigation: ros__parameters: command_vel_name: "/cmd_vel" - endpoint: "train" + endpoint: "rmuc" enable_goal_topic_forward: true auto_aim_capturer: ros__parameters: camera_name: "" - exposure_us: 3000.0 - gain: 12.0 + exposure_us: 1500.0 + gain: 8.0 framerate: 120.0 invert_image: true rls_tau_sec: 10.0 From a1d894a1e72d1016a66ce5c498970b66098d050e Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sun, 2 Aug 2026 22:33:14 +0800 Subject: [PATCH 55/86] chore: Update configuration --- rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml | 1 + rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 2 ++ 2 files changed, 3 insertions(+) diff --git a/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml b/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml index 3c02425ea..544a2b0e8 100644 --- a/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml @@ -32,6 +32,7 @@ auto_aim_component: dangerous_fallback: "red" manual_shoot: false enable_rune: true + track_ids: [HERO, ENGINEER, INFANTRY_3, INFANTRY_4, SENTRY, OUTPOST] camera_translation: [0., 0., 0.] fire_control: diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index 24862238d..d216bf864 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -61,6 +61,8 @@ auto_aim_component: ros__parameters: manual_shoot: true enable_rune: true + # track_ids: [OUTPOST, BASE] + track_ids: [HERO, ENGINEER, INFANTRY_3, INFANTRY_4, SENTRY, OUTPOST, BASE] camera_translation: [0.07128, 0.0, 0.0481] fire_control: bullet_speed: 22.5 From 51f3fc86d22c7c6d88a5f1d1e39e168e15463c4d Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sun, 2 Aug 2026 22:53:43 +0800 Subject: [PATCH 56/86] chore: Update configuration --- rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml | 2 ++ rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml | 2 ++ rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml | 2 ++ rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 2 ++ .../rmcs_bringup/config/steering-hero-little-six-friction.yaml | 2 ++ 5 files changed, 10 insertions(+) diff --git a/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml b/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml index 544a2b0e8..7f812fa52 100644 --- a/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml @@ -48,3 +48,5 @@ auto_aim_component: require_stable_command: true yaw_tolerance: 0.07 pitch_tolerance: 0.04 + rune_idle_duration: 0.4 + rune_shoot_duration: 0.2 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index f7c20cbb5..aa952607c 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -89,6 +89,8 @@ auto_aim_component: require_stable_command: false yaw_tolerance: 0.07 pitch_tolerance: 0.04 + rune_idle_duration: 0.4 + rune_shoot_duration: 0.2 auto_aim_ui: ros__parameters: diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 3cff304fd..272d4f091 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -89,6 +89,8 @@ auto_aim_component: require_stable_command: false yaw_tolerance: 0.07 pitch_tolerance: 0.04 + rune_idle_duration: 0.4 + rune_shoot_duration: 0.2 auto_aim_ui: ros__parameters: diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index d216bf864..c4c0bd3aa 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -77,6 +77,8 @@ auto_aim_component: require_stable_command: false yaw_tolerance: 0.07 pitch_tolerance: 0.04 + rune_idle_duration: 0.4 + rune_shoot_duration: 0.2 value_broadcaster: ros__parameters: diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml index a6af9a993..fb58305a2 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml @@ -101,6 +101,8 @@ auto_aim_component: require_stable_command: false yaw_tolerance: 0.07 pitch_tolerance: 0.04 + rune_idle_duration: 0.4 + rune_shoot_duration: 0.2 auto_aim_ui: ros__parameters: From 7a765ccd785e0a1801246042ce14e4d5b6c4bbe4 Mon Sep 17 00:00:00 2001 From: qzhhhi Date: Tue, 4 Aug 2026 02:21:20 +0800 Subject: [PATCH 57/86] fix: rune not tracking, bump auto aim --- rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml | 11 ++++++----- rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 2 +- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml b/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml index 7f812fa52..db9e59e70 100644 --- a/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml @@ -2,14 +2,14 @@ rmcs_executor: ros__parameters: update_rate: 1000.0 components: - # - rmcs::AutoAimPlayerComponent -> auto_aim_player - - rmcs::AutoAimVideoPlayerComponent -> auto_aim_video_player + - rmcs::AutoAimPlayerComponent -> auto_aim_player + # - rmcs::AutoAimVideoPlayerComponent -> auto_aim_video_player # - rmcs::AutoAimRecorderComponent -> auto_aim_recorder - rmcs::AutoAimComponent -> auto_aim_component auto_aim_player: ros__parameters: - input_path: "/workspaces/data/autoaim/robot/blue_fast_track/" + input_path: "/workspaces/RMCS/develop_ws/record/26uc-train/2026-08-03_20-51-11" loop_play: true auto_aim_video_player: @@ -29,10 +29,10 @@ auto_aim_recorder: auto_aim_component: ros__parameters: - dangerous_fallback: "red" + dangerous_fallback: "blue" manual_shoot: false enable_rune: true - track_ids: [HERO, ENGINEER, INFANTRY_3, INFANTRY_4, SENTRY, OUTPOST] + track_ids: [HERO, ENGINEER, INFANTRY_3, INFANTRY_4, SENTRY, OUTPOST, RUNE] camera_translation: [0., 0., 0.] fire_control: @@ -50,3 +50,4 @@ auto_aim_component: pitch_tolerance: 0.04 rune_idle_duration: 0.4 rune_shoot_duration: 0.2 + # is_lazy_gimbal: false diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index c4c0bd3aa..3f8d323e9 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -62,7 +62,7 @@ auto_aim_component: manual_shoot: true enable_rune: true # track_ids: [OUTPOST, BASE] - track_ids: [HERO, ENGINEER, INFANTRY_3, INFANTRY_4, SENTRY, OUTPOST, BASE] + track_ids: [HERO, ENGINEER, INFANTRY_3, INFANTRY_4, SENTRY, OUTPOST, BASE, RUNE] camera_translation: [0.07128, 0.0, 0.0481] fire_control: bullet_speed: 22.5 From 7093809011f155e8efefcdeb1e76a7dbfed470f6 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Sat, 25 Jul 2026 12:10:50 +0800 Subject: [PATCH 58/86] feat(deformable-infantry): rebase omni hardware onto dev/robots --- .../config/deformable-infantry-omni-b.yaml | 14 +- .../config/deformable-infantry-omni-c.yaml | 352 +++++++ .../config/deformable-infantry-omni.yaml | 14 +- rmcs_ws/src/rmcs_core/plugins.xml | 1 + .../hardware/deformable-infantry-omni-b.cpp | 14 +- .../hardware/deformable-infantry-omni-c.cpp | 875 ++++++++++++++++++ .../src/hardware/deformable-infantry-omni.cpp | 383 ++++---- 7 files changed, 1452 insertions(+), 201 deletions(-) create mode 100644 rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index aa952607c..c674be36e 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -10,6 +10,7 @@ rmcs_executor: - rmcs_core::referee::command::Interaction -> referee_interaction - rmcs_core::referee::command::interaction::Ui -> referee_ui - rmcs_core::referee::app::ui::DeformableInfantry -> referee_ui_infantry + - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui - rmcs_core::controller::gimbal::DeformableInfantryGimbalController -> gimbal_controller @@ -85,6 +86,7 @@ auto_aim_component: degraded_angle_speed: 12.0 window_redundancy: 0.8 window_hysteresis: 0.2 + is_lazy_gimbal: false attack_preaim: false require_stable_command: false yaw_tolerance: 0.07 @@ -97,6 +99,12 @@ auto_aim_ui: offset_x: 0.0 offset_y: +0.08 offset_z: 0.0 + warn_distance_m: 2.0 + +referee_ui_infantry: + ros__parameters: + crosshair_offset_x: -2 + crosshair_offset_y: -30 deformable_infantry: ros__parameters: @@ -113,10 +121,10 @@ deformable_infantry: chassis_controller: ros__parameters: # Deploy geometry / chassis-owned joint intent - min_angle: 5.0 + min_angle: 4.0 max_angle: 59.0 active_suspension_enable: true - spin_ratio: 1.0 + wireless_charging_offset_deg: 135.0 deformable_suspension: ros__parameters: @@ -175,7 +183,7 @@ gimbal_controller: yaw_angle_ki: 0.0 yaw_angle_kd: 0.0 - yaw_velocity_kp: 15.0 + yaw_velocity_kp: 13.0 yaw_velocity_ki: 0.0 yaw_velocity_kd: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml new file mode 100644 index 000000000..ea5a4cf78 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml @@ -0,0 +1,352 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::DeformableInfantryOmniC -> deformable_infantry + + - rmcs_core::referee::Status -> referee_status + - rmcs_core::referee::Command -> referee_command + + - rmcs_core::referee::command::Interaction -> referee_interaction + - rmcs_core::referee::command::interaction::Ui -> referee_ui + - rmcs_core::referee::app::ui::DeformableInfantry -> referee_ui_infantry + # - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui + + - rmcs_core::controller::gimbal::DeformableInfantryGimbalController -> gimbal_controller + + - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller + - rmcs_core::controller::shooting::HeatController -> heat_controller + - rmcs_core::controller::shooting::BulletFeederController17mm -> bullet_feeder_controller + - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller + + - rmcs_core::controller::chassis::DeformableChassis -> chassis_controller + - rmcs_core::controller::chassis::DeformableSuspension -> deformable_suspension + - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller + - rmcs_core::controller::chassis::DeformableOmniWheelController -> deformable_chassis_controller + + - rmcs_core::controller::chassis::DeformableJointController -> lf_joint_controller + - rmcs_core::controller::chassis::DeformableJointController -> lb_joint_controller + - rmcs_core::controller::chassis::DeformableJointController -> rb_joint_controller + - rmcs_core::controller::chassis::DeformableJointController -> rf_joint_controller + + # - rmcs::AutoAimComponent -> auto_aim_component + # - rmcs::AutoAimCapturerComponent -> auto_aim_capturer + # - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui + + # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + # - rmcs_core::debug::ValueCollector -> value_collector + +value_collector: + ros__parameters: + csv_path: "/tmp/pitch_.csv" + signals: + - /gimbal/pitch/angle + - /gimbal/pitch/velocity + - /gimbal/pitch/velocity_imu + - /gimbal/pitch/angle_error + - /gimbal/pitch/control_torque + - /gimbal/pitch/control_velocity + write_interval: 5 + flush_interval: 1000 + +value_broadcaster: + ros__parameters: + forward_list: + - /gimbal/pitch/angle + - /gimbal/pitch/velocity + +auto_aim_capturer: + ros__parameters: + camera_name: "" + exposure_us: 4000.0 + gain: 8.0 + framerate: 120.0 + invert_image: false + rls_tau_sec: 10.0 + use_hardware_sync: false + delay_ms: 6.5 + +auto_aim_component: + ros__parameters: + # WARN: 危险！可选 red | blue，生效后裁判系统 ID 缺席时 + # 将强行绑定为对应阵营哨兵，仅供调试，严禁比赛启用。 + # 留空或填 unknow 表示禁用。 + dangerous_fallback: "" + manual_shoot: true + camera_translation: [0.058, -0.08, 0.0] + fire_control: + bullet_speed: 22.5 + shoot_delay: 0.04 + offset_yaw: +2.5 + offset_pitch: +0.5 + attack_window: 80.0 + degraded_angle_speed: 12.0 + window_hysteresis: 0.2 + is_lazy_gimbal: false + attack_preaim: false + require_stable_command: false + yaw_tolerance: 0.07 + pitch_tolerance: 0.04 + +auto_aim_ui: + ros__parameters: + offset_x: 0.0 + offset_y: +0.08 + offset_z: 0.0 + warn_distance_m: 2.0 + +referee_ui_infantry: + ros__parameters: + crosshair_offset_x: -2 + crosshair_offset_y: -30 + +deformable_infantry: + ros__parameters: + serial_filter_bottom_board: "AF-C1C3-DFE8-40A8-FC4B-B853-6ED7-AC9F-1DED" + serial_filter_top_board: "AF-ABAC-786D-1B53-99F6-00A2-42A6-AA95-9D69" + chassis_radius: 0.2341741 + rod_length: 0.140 + yaw_motor_zero_point: 5881 + pitch_motor_zero_point: 32214 + debug_log_supercap: false + debug_log_wheel_motor: false + debug_log_deformable_joint_motor: false + +chassis_controller: + ros__parameters: + # Deploy geometry / chassis-owned joint intent + min_angle: 4.0 + max_angle: 59.0 + active_suspension_enable: true + wireless_charging_offset_deg: 135.0 + +deformable_suspension: + ros__parameters: + # IMU attitude correction at min-angle stance. + active_suspension_pitch_outer_kp: 12.0 + active_suspension_pitch_outer_ki: 0.02 + active_suspension_pitch_outer_kd: 0.0 + active_suspension_pitch_outer_integral_min: -2.0 + active_suspension_pitch_outer_integral_max: 2.0 + active_suspension_pitch_outer_output_min: -3.0 + active_suspension_pitch_outer_output_max: 3.0 + + active_suspension_pitch_inner_kp: 0.45 + active_suspension_pitch_inner_ki: 0.0 + active_suspension_pitch_inner_kd: 0.0 + active_suspension_pitch_inner_integral_min: -1.0 + active_suspension_pitch_inner_integral_max: 1.0 + active_suspension_pitch_inner_output_min: -0.785 + active_suspension_pitch_inner_output_max: 0.785 + + active_suspension_roll_outer_kp: 12.0 + active_suspension_roll_outer_ki: 0.02 + active_suspension_roll_outer_kd: 0.0 + active_suspension_roll_outer_integral_min: -2.0 + active_suspension_roll_outer_integral_max: 2.0 + active_suspension_roll_outer_output_min: -3.0 + active_suspension_roll_outer_output_max: 3.0 + + active_suspension_roll_inner_kp: 0.45 + active_suspension_roll_inner_ki: 0.0 + active_suspension_roll_inner_kd: 0.0 + active_suspension_roll_inner_integral_min: -1.0 + active_suspension_roll_inner_integral_max: 1.0 + active_suspension_roll_inner_output_min: -0.785 + active_suspension_roll_inner_output_max: 0.785 + + # Chassis-owned joint intent trajectory limits while attitude correction is active. + active_suspension_target_velocity_limit_deg: 80.0 + active_suspension_target_acceleration_limit_deg: 360.0 + active_suspension_correction_velocity_limit_deg: 720.0 + active_suspension_correction_acceleration_limit_deg: 3600.0 + active_suspension_rate_lpf_cutoff_hz: 10.0 + + # Automatic IMU mounting-error calibration. + # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. + chassis_imu_calibration_wait_s: 2.0 + chassis_imu_calibration_sample_s: 3.0 + +gimbal_controller: + ros__parameters: + upper_limit: -0.47123 # -27 deg + lower_limit: 0.10 # 8 deg + ctrl_hold_pitch_target_angle: 0.0 + + yaw_angle_kp: 10.0 + yaw_angle_ki: 0.0 + yaw_angle_kd: 0.0 + + yaw_velocity_kp: 13.0 + yaw_velocity_ki: 0.0 + yaw_velocity_kd: 0.0 + + pitch_angle_kp: 35.0 + pitch_angle_ki: 0.02 + pitch_angle_kd: 0.3 + + pitch_velocity_kp: 2.0 + pitch_velocity_ki: 0.0 + pitch_velocity_kd: 0.0 + + pitch_gravity_ff_gain: 4.302 + pitch_gravity_ff_phase: 0.589 + + pitch_torque_control: true + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/left_friction + - /gimbal/right_friction + friction_velocities: + - 580.0 + - 580.0 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 10000 + reserved_heat: 15000 + +bullet_feeder_controller: + ros__parameters: + bullets_per_feeder_turn: 8.0 + shot_frequency: 30.0 + safe_shot_frequency: 10.0 + eject_frequency: 10.0 + eject_time: 0.05 + deep_eject_frequency: 5.0 + deep_eject_time: 0.2 + single_shot_max_stop_delay: 2.0 + +left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/left_friction/velocity + setpoint: /gimbal/left_friction/control_velocity + control: /gimbal/left_friction/control_torque + kp: 0.003436926 + ki: 0.00 + kd: 0.009373434 + +right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/right_friction/velocity + setpoint: /gimbal/right_friction/control_velocity + control: /gimbal/right_friction/control_torque + kp: 0.003436926 + ki: 0.00 + kd: 0.009373434 + +bullet_feeder_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/bullet_feeder/velocity + setpoint: /gimbal/bullet_feeder/control_velocity + control: /gimbal/bullet_feeder/control_torque + kp: 1.5 + ki: 0.0 + kd: 0.0 + +deformable_chassis_controller: + ros__parameters: + mass: 25.5 + moment_of_inertia: 1.0 + wheel_radius: 0.075 + friction_coefficient: 6.6 + k1: 2.958580e+00 + k2: 3.082190e-03 + no_load_power: 11.37 + +lf_joint_controller: + ros__parameters: + measurement_angle: /chassis/left_front_joint/physical_angle + setpoint_angle: /chassis/left_front_joint/target_physical_angle + setpoint_velocity: /chassis/left_front_joint/target_physical_velocity + control: /chassis/left_front_joint/control_torque + dt: 0.001 + b0: -1.0 + kt: 1.0 + td_h: 0.001 + td_r: 50.0 + eso_w0: 250.0 + eso_auto_beta: true + k1: 30.0 + k2: 17.0 + alpha1: 0.75 + alpha2: 0.7 + delta: 0.02 + u_min: -200.0 + u_max: 200.0 + output_min: -200.0 + output_max: 200.0 + +lb_joint_controller: + ros__parameters: + measurement_angle: /chassis/left_back_joint/physical_angle + setpoint_angle: /chassis/left_back_joint/target_physical_angle + setpoint_velocity: /chassis/left_back_joint/target_physical_velocity + control: /chassis/left_back_joint/control_torque + dt: 0.001 + b0: -1.0 + kt: 1.0 + td_h: 0.001 + td_r: 50.0 + eso_w0: 250.0 + eso_auto_beta: true + k1: 30.0 + k2: 17.0 + alpha1: 0.75 + alpha2: 0.7 + delta: 0.02 + u_min: -200.0 + u_max: 200.0 + output_min: -200.0 + output_max: 200.0 + +rb_joint_controller: + ros__parameters: + measurement_angle: /chassis/right_back_joint/physical_angle + setpoint_angle: /chassis/right_back_joint/target_physical_angle + setpoint_velocity: /chassis/right_back_joint/target_physical_velocity + control: /chassis/right_back_joint/control_torque + dt: 0.001 + b0: -1.0 + kt: 1.0 + td_h: 0.001 + td_r: 50.0 + eso_w0: 250.0 + eso_auto_beta: true + k1: 30.0 + k2: 17.0 + alpha1: 0.75 + alpha2: 0.7 + delta: 0.02 + u_min: -200.0 + u_max: 200.0 + output_min: -200.0 + output_max: 200.0 + +rf_joint_controller: + ros__parameters: + measurement_angle: /chassis/right_front_joint/physical_angle + setpoint_angle: /chassis/right_front_joint/target_physical_angle + setpoint_velocity: /chassis/right_front_joint/target_physical_velocity + control: /chassis/right_front_joint/control_torque + dt: 0.001 + b0: -1.0 + kt: 1.0 + td_h: 0.001 + td_r: 50.0 + eso_w0: 250.0 + eso_auto_beta: true + k1: 30.0 + k2: 17.0 + alpha1: 0.75 + alpha2: 0.7 + delta: 0.02 + u_min: -200.0 + u_max: 200.0 + output_min: -200.0 + output_max: 200.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 272d4f091..0d3a4afb1 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -64,7 +64,7 @@ auto_aim_capturer: framerate: 120.0 invert_image: false rls_tau_sec: 10.0 - use_hardware_sync: true + use_hardware_sync: false delay_ms: 6.5 auto_aim_component: @@ -97,6 +97,12 @@ auto_aim_ui: offset_x: 0.0 offset_y: -0.08 offset_z: 0.0 + warn_distance_m: 2.0 + +referee_ui_infantry: + ros__parameters: + crosshair_offset_x: -2 + crosshair_offset_y: -30 deformable_infantry: ros__parameters: @@ -113,10 +119,10 @@ deformable_infantry: chassis_controller: ros__parameters: # Deploy geometry / chassis-owned joint intent - min_angle: 5.0 + min_angle: 4.0 max_angle: 59.0 active_suspension_enable: true - spin_ratio: 1.0 + wireless_charging_offset_deg: -45.0 deformable_suspension: ros__parameters: @@ -175,7 +181,7 @@ gimbal_controller: yaw_angle_ki: 0.0 yaw_angle_kd: 0.0 - yaw_velocity_kp: 10.0 + yaw_velocity_kp: 13.0 yaw_velocity_ki: 0.0 yaw_velocity_kd: 0.0 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index a066759ae..ac41a1087 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -4,6 +4,7 @@ + diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index 8efa589ba..884ebf695 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -217,21 +217,21 @@ class DeformableInfantryOmniB } { auto packet = device::CanPacket8{uint64_t{0}}; - packet << gimbal_right_friction_; + packet << gimbal_left_friction_; builder.can_transmit( Spec::kCans.kCan1, // { - .can_id = gimbal_right_friction_.send_id(), + .can_id = gimbal_left_friction_.send_id(), .can_data = packet.as_bytes(), }); } { auto packet = device::CanPacket8{uint64_t{0}}; - packet << gimbal_left_friction_; + packet << gimbal_right_friction_; builder.can_transmit( Spec::kCans.kCan2, // { - .can_id = gimbal_left_friction_.send_id(), + .can_id = gimbal_right_friction_.send_id(), .can_data = packet.as_bytes(), }); } @@ -245,10 +245,10 @@ class DeformableInfantryOmniB gimbal_pitch_motor_.store_status(data.can_data); monitor_.tick("Top::Can0", data.can_id); } else if (can == Spec::kCans.kCan1) { - gimbal_right_friction_.match_then_store_status(data.can_id, data.can_data); + gimbal_left_friction_.match_then_store_status(data.can_id, data.can_data); monitor_.tick("Top::Can1", data.can_id); } else if (can == Spec::kCans.kCan2) { - gimbal_left_friction_.match_then_store_status(data.can_id, data.can_data); + gimbal_right_friction_.match_then_store_status(data.can_id, data.can_data); monitor_.tick("Top::Can2", data.can_id); } } @@ -873,4 +873,4 @@ class DeformableInfantryOmniB } // namespace rmcs_core::hardware #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DeformableInfantryOmniB, rmcs_executor::Component) +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DeformableInfantryOmniB, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp new file mode 100644 index 000000000..860c16c13 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp @@ -0,0 +1,875 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "hardware/device/bmi088.hpp" +#include "hardware/device/bmi088_ekf.hpp" +#include "hardware/device/board_clock_lifter.hpp" +#include "hardware/device/can_packet.hpp" +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/lk_motor.hpp" +#include "hardware/device/remote_control.hpp" +#include "hardware/device/supercap.hpp" +#include "hardware/device/vt13.hpp" +#include "hardware/util/status_monitor.hpp" + +namespace rmcs_core::hardware { + +using Clock = std::chrono::steady_clock; + +class DeformableInfantryOmniC + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DeformableInfantryOmniC() + : Node( + get_component_name(), + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) + , command_(create_partner_component(get_component_name() + "_command", *this)) { + using namespace rmcs_description; + + register_input("/predefined/timestamp", timestamp_); + register_output("/tf", tf_); + register_output( + "/auto_aim/camera_transform", camera_transform_, Eigen::Isometry3d::Identity()); + register_output("/auto_aim/barrel_direction", barrel_direction_, Eigen::Vector3d::UnitX()); + register_output("/auto_aim/yaw_velocity", auto_aim_yaw_velocity_, 0.0); + + tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); + + remote_control_ = std::make_unique(*this); + + bottom_board_ = std::make_unique( + *this, *command_, get_parameter("serial_filter_bottom_board").as_string()); + top_board_ = std::make_unique( + *this, *command_, get_parameter("serial_filter_top_board").as_string()); + + // For command: remote-status + using Srv = std_srvs::srv::Trigger; + status_service_ = create_service( + "/rmcs/service/robot_status", + [this](const Srv::Request::SharedPtr&, const Srv::Response::SharedPtr& response) { + status_service_callback(response); + }); + } + + ~DeformableInfantryOmniC() override = default; + + void before_updating() override { top_board_->request_hard_sync_read(); } + + void update() override { + bottom_board_->update(); + top_board_->update(); + remote_control_->update(); + + using namespace rmcs_description; + *camera_transform_ = fast_tf::lookup_transform(*tf_); + *barrel_direction_ = + *fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + *auto_aim_yaw_velocity_ = top_board_->gimbal_yaw_velocity(); + } + + void command_update() { + const bool even = ((cmd_tick_++ & 1u) == 0u); + bottom_board_->command_update(even); + top_board_->command_update(); + } + +private: + static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); + static constexpr auto kLeftFront = 0; + static constexpr auto kLeftBack = 1; + static constexpr auto kRightBack = 2; + static constexpr auto kRightFront = 3; + static constexpr auto kJointName = std::array{ + "left_front", + "left_back", + "right_back", + "right_front", + }; + + class Command : public Component { + public: + explicit Command(DeformableInfantryOmniC& deformableInfantry) + : deformableInfantry(deformableInfantry) {} + + void update() override { deformableInfantry.command_update(); } + + DeformableInfantryOmniC& deformableInfantry; + }; + + struct TopBoard final : public librmcs::board::RmcsBoardLite::Callback { + public: + explicit TopBoard( + DeformableInfantryOmniC& status, Component& command, + const std::string& serial_filter = {}) + : status_{status} + , tf_{status.tf_} + , bmi088_{device::Bmi088Ekf::Config{ + .body_to_sensor = + Eigen::AngleAxisd{std::numbers::pi / 2.0, Eigen::Vector3d::UnitX()} + .toRotationMatrix()}} + , gimbal_pitch_motor_(status, command, "/gimbal/pitch") + , gimbal_left_friction_(status, command, "/gimbal/left_friction") + , gimbal_right_friction_(status, command, "/gimbal/right_friction") { + + gimbal_pitch_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} + .set_reversed() + .set_encoder_zero_point( + static_cast(status.get_parameter("pitch_motor_zero_point").as_int()))); + + gimbal_left_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} + .set_reduction_ratio(1.) + .set_reversed()); + gimbal_right_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2}.set_reduction_ratio( + 1.)); + + status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); + status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); + status.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); + status.register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); + + auto options = librmcs::board::AdvancedOptions{}; + options.dangerously_skip_version_checks = true; + board_ = std::make_unique(*this, serial_filter, options); + + board_->start_transmit().gpio_digital_read( + Spec::kGpios.kUart1Rx, // + { + .period_ms = 0, + .asap = false, + .rising_edge = false, + .falling_edge = true, + .capture_timestamp = true, + .pull = librmcs::data::GpioPull::kUp, + }); + + board_->start_transmit().uart_config(Spec::kUarts.kUart0, {.baudrate = 921600}); + + status_.remote_control_->register_vt13(&vt13_); + } + + ~TopBoard() override = default; + + [[nodiscard]] auto gimbal_yaw_velocity() const -> double { + return *gimbal_yaw_velocity_bmi088_; + } + + void request_hard_sync_read() { + // RMCS-lite top board variant currently has no GPIO hard-sync request + // path. + } + + void update() { + vt13_.update_status(); + gimbal_pitch_motor_.update_status(); + gimbal_left_friction_.update_status(); + gimbal_right_friction_.update_status(); + + const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); + + if (auto snapshot = bmi088_.snapshot()) { + *gimbal_pitch_velocity_bmi088_ = snapshot->gyro_body.y(); + *gimbal_yaw_velocity_bmi088_ = snapshot->gyro_body.z(); + tf_->set_transform( + snapshot->orientation.conjugate()); + } + + tf_->set_state( + pitch_encoder_angle); + } + + void command_update() const { + auto builder = board_->start_transmit(); + { + auto packet = gimbal_pitch_motor_.generate_torque_command(); + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x141, + .can_data = packet.as_bytes(), + }); + } + { + auto packet = device::CanPacket8{uint64_t{0}}; + packet << gimbal_left_friction_; + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = gimbal_left_friction_.send_id(), + .can_data = packet.as_bytes(), + }); + } + { + auto packet = device::CanPacket8{uint64_t{0}}; + packet << gimbal_right_friction_; + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = gimbal_right_friction_.send_id(), + .can_data = packet.as_bytes(), + }); + } + } + + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + if (can == Spec::kCans.kCan0) { + if (data.can_id == 0x141) + gimbal_pitch_motor_.store_status(data.can_data); + monitor_.tick("Top::Can0", data.can_id); + } else if (can == Spec::kCans.kCan1) { + gimbal_left_friction_.match_then_store_status(data.can_id, data.can_data); + monitor_.tick("Top::Can1", data.can_id); + } else if (can == Spec::kCans.kCan2) { + gimbal_right_friction_.match_then_store_status(data.can_id, data.can_data); + monitor_.tick("Top::Can2", data.can_id); + } + } + + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + if (uart == Spec::kUarts.kUart0) + vt13_.store_status(data.uart_data); + } + + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { + const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); + bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); + monitor_.tick("Top::Imu", "Acc"); + } + + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); + monitor_.tick("Top::Imu", "Gyr"); + if (!timestamp.has_value()) + return; + auto snapshot = + bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); + if (snapshot) + imu_snapshot_output_.emit(*snapshot); + } + + void gpio_digital_read_result_callback( + const Spec::Gpio& gpio, const View::GpioDigital& data) override { + if (gpio != Spec::kGpios.kUart1Rx) + return; + if (!data.timestamp_quarter_us) + return; + + const auto timestamp = board_clock_lifter_.lift_timestamp(*data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + + camera_signal_output_.emit(*timestamp); + monitor_.tick("Top::CameraSync", "Active"); + } + + auto status() const -> std::vector { return monitor_.text(); } + + DeformableInfantryOmniC& status_; + OutputInterface& tf_; + OutputInterface gimbal_yaw_velocity_bmi088_; + OutputInterface gimbal_pitch_velocity_bmi088_; + + EventOutputInterface imu_snapshot_output_; + EventOutputInterface camera_signal_output_; + + device::Bmi088Ekf bmi088_; + device::BoardClockLifter board_clock_lifter_; + device::Vt13 vt13_; + device::LkMotor gimbal_pitch_motor_; + device::DjiMotor gimbal_left_friction_; + device::DjiMotor gimbal_right_friction_; + + StatusMonitor monitor_{}; + std::unique_ptr board_; + }; + + struct BottomBoard final : public librmcs::board::RmcsBoardLite::Callback { + public: + explicit BottomBoard( + DeformableInfantryOmniC& status, Component& command, + const std::string& serial_filter = {}) + : status_{status} + , command_{command} + , kChassisRadiusBase(status.get_parameter("chassis_radius").as_double()) + , kRodLength(status.get_parameter("rod_length").as_double()) + , kDefaultRadius(kChassisRadiusBase + kRodLength) { + + status.register_output("/referee/serial", referee_serial_); + referee_serial_->read = [this](std::byte* buffer, size_t size) { + return referee_ring_buffer_receive_.pop_front_n( + [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) { + board_->start_transmit().uart_transmit( + Spec::kUarts.kUart0, {.uart_data = std::span{buffer, size}}); + return size; + }; + + gimbal_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( + static_cast(status.get_parameter("yaw_motor_zero_point").as_int()))); + + for (auto& motor : chassis_wheel_motors_) + motor.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} + .set_reduction_ratio(19.0) + .enable_multi_turn_angle()); + + for (auto& motor : chassis_joint_motors_) + motor.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei36} + .set_reversed() + .enable_multi_turn_angle()); + + imu_.set_coordinate_mapping([](double x, double y, double z) { + // Keep the existing chassis yaw axis mapping explicit until the bottom-board IMU + // installation is re-validated on hardware. + return std::make_tuple(-y, x, z); + }); + + gimbal_bullet_feeder_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM2006, 3} + .enable_multi_turn_angle()); + + status.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); + status.register_output("/chassis/imu/pitch", chassis_imu_pitch_, 0.0); + status.register_output("/chassis/imu/roll", chassis_imu_roll_, 0.0); + status.register_output("/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, 0.0); + status.register_output("/chassis/imu/roll_rate", chassis_imu_roll_rate_, 0.0); + for (size_t i = 0; i < 4; ++i) { + status.register_output( + std::format( + "/chassis/{}_joint/physical_angle", DeformableInfantryOmniC::kJointName[i]), + joint_physical_angle_[i], kNaN); + status.register_output( + std::format( + "/chassis/{}_joint/physical_velocity", + DeformableInfantryOmniC::kJointName[i]), + joint_physical_velocity_[i], kNaN); + } + status.register_output("/chassis/encoder/alpha", encoder_alpha_, kNaN); + status.register_output("/chassis/encoder/alpha_dot", encoder_alpha_dot_, kNaN); + status.register_output("/chassis/radius", radius_, kDefaultRadius); + + status.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); + status.get_parameter_or("debug_log_wheel_motor", debug_log_wheel_motor_, false); + status.get_parameter_or( + "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); + + auto options = librmcs::board::AdvancedOptions{}; + options.dangerously_skip_version_checks = true; + board_ = std::make_unique(*this, serial_filter, options); + + status_.remote_control_->register_dr16(&dr16_); + } + + void update() { + imu_.update_status(); + *chassis_yaw_velocity_imu_ = imu_.gz(); + { + const double q0 = imu_.q0(); + const double q1 = imu_.q1(); + const double q2 = imu_.q2(); + const double q3 = imu_.q3(); + + double sin_pitch = 2.0 * (q0 * q2 - q3 * q1); + sin_pitch = std::clamp(sin_pitch, -1.0, 1.0); + + const double standard_pitch = std::asin(sin_pitch); + const double standard_roll = + std::atan2(2.0 * (q0 * q1 + q2 * q3), 1.0 - 2.0 * (q1 * q1 + q2 * q2)); + + // Export chassis attitude using the requested convention: + // pitch < 0 when the front is higher, roll > 0 when the left side is higher. + *chassis_imu_pitch_ = -standard_pitch; + *chassis_imu_roll_ = standard_roll; + *chassis_imu_pitch_rate_ = -imu_.gy(); + *chassis_imu_roll_rate_ = imu_.gx(); + } + + for (auto& motor : chassis_wheel_motors_) + motor.update_status(); + for (auto& motor : chassis_joint_motors_) + motor.update_status(); + + for (size_t i = 0; i < 4; ++i) + update_joint_physical_feedback_( + i, joint_physical_angle_[i], joint_physical_velocity_[i]); + + update_geometry_feedback_(); + if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) + log_chassis_feedback_once_per_second_(); + + dr16_.update_status(); + gimbal_yaw_motor_.update_status(); + if (supercap_status_received_.load(std::memory_order_relaxed)) + supercap_.update_status(); + if (debug_log_supercap_) + log_supercap_feedback_once_per_second_(); + gimbal_bullet_feeder_.update_status(); + + tf_->set_state( + gimbal_yaw_motor_.angle()); + } + + void command_update(bool even) { + auto builder = board_->start_transmit(); + if (even) { + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kLeftFront].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kLeftBack].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kRightBack].generate_command(), + device::CanPacket8::PaddingQuarter{}, + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan3, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kRightFront].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = 0x142, + .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + } + .as_bytes(), + }); + } else { + for (size_t i = 0; i < 4; ++i) { + switch (i) { + case kLeftFront: + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + case kLeftBack: + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + case kRightBack: + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + case kRightFront: + builder.can_transmit( + Spec::kCans.kCan3, // + { + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + default: break; + } + } + } + } + + static constexpr double kJointZeroPhysicalAngleRad = 62.5 * std::numbers::pi / 180.0; + + DeformableInfantryOmniC& status_; + Component& command_; + + std::unique_ptr board_; + + // Interfaces + + OutputInterface& tf_{status_.tf_}; + + OutputInterface chassis_yaw_velocity_imu_; + OutputInterface chassis_imu_pitch_; + OutputInterface chassis_imu_roll_; + OutputInterface chassis_imu_pitch_rate_; + OutputInterface chassis_imu_roll_rate_; + + std::array, 4> joint_physical_angle_; + std::array, 4> joint_physical_velocity_; + + OutputInterface encoder_alpha_; + OutputInterface encoder_alpha_dot_; + OutputInterface radius_; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; + OutputInterface referee_serial_; + + // State + + std::atomic wheel_status_received_[4] = {false, false, false, false}; + std::atomic joint_status_received_[4] = {false, false, false, false}; + + bool debug_log_supercap_ = false; + bool debug_log_wheel_motor_ = false; + bool debug_log_deformable_joint_motor_ = false; + + const double kChassisRadiusBase; + const double kRodLength; + const double kDefaultRadius; + + Clock::time_point next_chassis_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; + Clock::time_point next_supercap_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; + + // Device + + device::Bmi088 imu_{1000, 0.2, 0.0}; + device::LkMotor gimbal_yaw_motor_{status_, command_, "/gimbal/yaw"}; + device::Dr16 dr16_; + + device::DjiMotor chassis_wheel_motors_[4]{ + device::DjiMotor{status_, command_, "/chassis/left_front_wheel"}, + device::DjiMotor{status_, command_, "/chassis/left_back_wheel"}, + device::DjiMotor{status_, command_, "/chassis/right_back_wheel"}, + device::DjiMotor{status_, command_, "/chassis/right_front_wheel"}, + }; + device::LkMotor chassis_joint_motors_[4]{ + device::LkMotor{status_, command_, "/chassis/left_front_joint"}, + device::LkMotor{status_, command_, "/chassis/left_back_joint"}, + device::LkMotor{status_, command_, "/chassis/right_back_joint"}, + device::LkMotor{status_, command_, "/chassis/right_front_joint"}, + }; + + std::atomic latest_supercap_status_{device::CanPacket8{uint64_t{0}}}; + std::atomic supercap_status_received_{false}; + device::Supercap supercap_{status_, command_}; + + device::DjiMotor gimbal_bullet_feeder_{status_, command_, "/gimbal/bullet_feeder"}; + + void process_chassis_can_receive_(size_t index, const View::Can& data) { + if (data.is_extended_can_id || data.is_remote_transmission) + return; + if (data.can_id == 0x201) { + chassis_wheel_motors_[index].store_status(data.can_data); + wheel_status_received_[index].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x141) { + chassis_joint_motors_[index].store_status(data.can_data); + joint_status_received_[index].store(true, std::memory_order_relaxed); + } + } + + void update_joint_physical_feedback_( + size_t index, OutputInterface& angle_output, + OutputInterface& velocity_output) { + + if (!joint_status_received_[index].load(std::memory_order_relaxed)) { + *angle_output = kNaN; + *velocity_output = kNaN; + return; + } + + const auto to_physical_angle = [](double motor_angle) { + return kJointZeroPhysicalAngleRad - motor_angle; + }; + const auto to_physical_velocity = [](double motor_velocity) { return -motor_velocity; }; + + *angle_output = to_physical_angle(chassis_joint_motors_[index].angle()); + *velocity_output = to_physical_velocity(chassis_joint_motors_[index].velocity()); + } + + void update_geometry_feedback_() { + const Eigen::Vector4d alpha_rad{ + *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], + *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront]}; + const Eigen::Vector4d alpha_dot_rad{ + *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], + *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront]}; + + if (!alpha_rad.array().isFinite().all() || !alpha_dot_rad.array().isFinite().all()) { + *encoder_alpha_ = kNaN; + *encoder_alpha_dot_ = kNaN; + *radius_ = kDefaultRadius; + RCLCPP_WARN_THROTTLE( + status_.get_logger(), *status_.get_clock(), 1000, + "deformable joint feedback invalid, fallback chassis radius to default %.3f m", + kDefaultRadius); + return; + } + + *encoder_alpha_ = alpha_rad.mean(); + *encoder_alpha_dot_ = alpha_dot_rad.mean(); + *radius_ = (kChassisRadiusBase + kRodLength * alpha_rad.array().cos()).mean(); + } + + void log_chassis_feedback_once_per_second_() { + const auto now = Clock::now(); + if (now < next_chassis_feedback_log_time_) + return; + + const auto wheel_rx = [this](size_t index) { + return wheel_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; + }; + const auto joint_rx = [this](size_t index) { + return joint_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; + }; + + if (debug_log_wheel_motor_) { + std::string wheel_rx_str; + for (size_t i = 0; i < 4; ++i) { + if (i > 0) + wheel_rx_str.push_back(' '); + wheel_rx_str.push_back(wheel_rx(i)); + } + RCLCPP_INFO( + status_.get_logger(), + "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " + "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " + "rx=[%s]", + chassis_wheel_motors_[kLeftFront].angle(), + chassis_wheel_motors_[kLeftBack].angle(), + chassis_wheel_motors_[kRightBack].angle(), + chassis_wheel_motors_[kRightFront].angle(), + chassis_wheel_motors_[kLeftFront].angle(), + chassis_wheel_motors_[kLeftBack].angle(), + chassis_wheel_motors_[kRightBack].angle(), + chassis_wheel_motors_[kRightFront].angle(), wheel_rx_str.c_str()); + } + + if (debug_log_deformable_joint_motor_) { + std::string joint_rx_str; + for (size_t i = 0; i < 4; ++i) { + if (i > 0) + joint_rx_str.push_back(' '); + joint_rx_str.push_back(joint_rx(i)); + } + RCLCPP_INFO( + status_.get_logger(), + "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " + "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " + "rx=[%s]", + *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], + *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront], + *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], + *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront], + joint_rx_str.c_str()); + } + + next_chassis_feedback_log_time_ = now + std::chrono::seconds(1); + } + + void log_supercap_feedback_once_per_second_() { + const auto now = Clock::now(); + if (now < next_supercap_feedback_log_time_) + return; + + const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); + auto supercap_raw_packet = latest_supercap_status_.load(std::memory_order_relaxed); + const auto supercap_raw_bytes = supercap_raw_packet.as_bytes(); + + RCLCPP_INFO( + status_.get_logger(), + "[supercap] can1 rx=%c id=0x300 enabled=%d supercap_v=% .3f chassis_v=% .3f " + "power=% .3f raw=[%02X %02X %02X %02X %02X %02X %02X %02X]", + supercap_rx ? 'Y' : 'N', supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, + supercap_rx ? supercap_.supercap_voltage() : kNaN, + supercap_rx ? supercap_.chassis_voltage() : kNaN, + supercap_rx ? supercap_.chassis_power() : kNaN, + std::to_integer(supercap_raw_bytes[0]), + std::to_integer(supercap_raw_bytes[1]), + std::to_integer(supercap_raw_bytes[2]), + std::to_integer(supercap_raw_bytes[3]), + std::to_integer(supercap_raw_bytes[4]), + std::to_integer(supercap_raw_bytes[5]), + std::to_integer(supercap_raw_bytes[6]), + std::to_integer(supercap_raw_bytes[7])); + + next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); + } + + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { + if (can == Spec::kCans.kCan0) { + process_chassis_can_receive_(0, data); + monitor_.tick("Bottom::Can0", data.can_id); + } else if (can == Spec::kCans.kCan1) { + process_chassis_can_receive_(1, data); + if (!data.is_extended_can_id && !data.is_remote_transmission + && data.can_id == 0x300) { + if (data.can_data.size() == 8) + latest_supercap_status_.store( + device::CanPacket8{data.can_data}, std::memory_order_relaxed); + supercap_.store_status(data.can_data); + supercap_status_received_.store(true, std::memory_order_relaxed); + } + monitor_.tick("Bottom::Can1", data.can_id); + } else if (can == Spec::kCans.kCan2) { + process_chassis_can_receive_(2, data); + if (data.is_extended_can_id || data.is_remote_transmission) + return; + if (data.can_id == 0x142) + gimbal_yaw_motor_.store_status(data.can_data); + else if (data.can_id == 0x203) + gimbal_bullet_feeder_.store_status(data.can_data); + monitor_.tick("Bottom::Can2", data.can_id); + } else if (can == Spec::kCans.kCan3) { + process_chassis_can_receive_(3, data); + monitor_.tick("Bottom::Can3", data.can_id); + } + } + + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + if (uart == Spec::kUarts.kDbus) { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + monitor_.tick("Bottom::Dbus", "Active"); + } else if (uart == Spec::kUarts.kUart0) { + const std::byte* ptr = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, + data.uart_data.size()); + monitor_.tick("Bottom::Uart0", "Active"); + } + } + + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { + imu_.store_accelerometer_status(data.x, data.y, data.z); + monitor_.tick("Bottom::Imu", "Acc"); + } + + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); + monitor_.tick("Bottom::Imu", "Gyr"); + } + + auto status() const -> std::vector { return monitor_.text(); } + + StatusMonitor monitor_{}; + }; + + auto status_service_callback(const std::shared_ptr& response) + -> void { + response->success = true; + + auto feedback_message = std::ostringstream{}; + auto text = [&](std::format_string format, Args&&... args) { + std::println(feedback_message, format, std::forward(args)...); + }; + + text(" yaw_motor_zero_point: {}", bottom_board_->gimbal_yaw_motor_.last_raw_angle()); + text(" pitch_motor_zero_point: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); + constexpr auto kPosition = + std::array{"left_front", "left_back", "right_back", "right_front"}; + + text(""); + for (auto&& [index, motor] : + std::views::zip(kPosition, bottom_board_->chassis_joint_motors_)) { + text(" {}_zero_point: {}", index, motor.last_raw_angle()); + } + + text("\nBottomBoard Status:"); + for (const auto& line : bottom_board_->status()) + text("> {}", line); + + text("\nTopBoard Status:"); + for (const auto& line : top_board_->status()) + text("> {}", line); + + response->message = feedback_message.str(); + } + + OutputInterface tf_; + OutputInterface camera_transform_; + OutputInterface barrel_direction_; + OutputInterface auto_aim_yaw_velocity_; + InputInterface timestamp_; + + std::unique_ptr bottom_board_; + std::unique_ptr top_board_; + std::unique_ptr remote_control_; + + std::shared_ptr command_; + uint32_t cmd_tick_ = 0; + + std::shared_ptr> status_service_; +}; + +} // namespace rmcs_core::hardware + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DeformableInfantryOmniC, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index 83c40c97b..557ba36ad 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -13,7 +13,6 @@ #include #include -#include #include #include @@ -34,6 +33,7 @@ #include "hardware/device/lk_motor.hpp" #include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" +#include "hardware/device/vt13.hpp" #include "hardware/util/status_monitor.hpp" namespace rmcs_core::hardware { @@ -111,7 +111,7 @@ class DeformableInfantryOmni "right_front", }; - class Command : public rmcs_executor::Component { + class Command : public Component { public: explicit Command(DeformableInfantryOmni& deformableInfantry) : deformableInfantry(deformableInfantry) {} @@ -121,10 +121,201 @@ class DeformableInfantryOmni DeformableInfantryOmni& deformableInfantry; }; + struct TopBoard final : public librmcs::board::RmcsBoardLite::Callback { + public: + explicit TopBoard( + DeformableInfantryOmni& status, Component& command, + const std::string& serial_filter = {}) + : status_{status} + , tf_{status.tf_} + , bmi088_{device::Bmi088Ekf::Config{ + .body_to_sensor = + Eigen::AngleAxisd{std::numbers::pi / 2.0, Eigen::Vector3d::UnitX()} + .toRotationMatrix()}} + , gimbal_pitch_motor_(status, command, "/gimbal/pitch") + , gimbal_left_friction_(status, command, "/gimbal/left_friction") + , gimbal_right_friction_(status, command, "/gimbal/right_friction") { + + gimbal_pitch_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} + .set_reversed() + .set_encoder_zero_point( + static_cast(status.get_parameter("pitch_motor_zero_point").as_int()))); + + gimbal_left_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} + .set_reduction_ratio(1.) + .set_reversed()); + gimbal_right_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2}.set_reduction_ratio( + 1.)); + + status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); + status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); + status.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); + status.register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); + + auto options = librmcs::board::AdvancedOptions{}; + options.dangerously_skip_version_checks = true; + board_ = std::make_unique(*this, serial_filter, options); + + board_->start_transmit().gpio_digital_read( + Spec::kGpios.kUart1Rx, // + { + .period_ms = 0, + .asap = false, + .rising_edge = false, + .falling_edge = true, + .capture_timestamp = true, + .pull = librmcs::data::GpioPull::kUp, + }); + + board_->start_transmit().uart_config(Spec::kUarts.kUart0, {.baudrate = 921600}); + + status_.remote_control_->register_vt13(&vt13_); + } + + ~TopBoard() override = default; + + [[nodiscard]] auto gimbal_yaw_velocity() const -> double { + return *gimbal_yaw_velocity_bmi088_; + } + + void request_hard_sync_read() { + // RMCS-lite top board variant currently has no GPIO hard-sync request + // path. + } + + void update() { + vt13_.update_status(); + gimbal_pitch_motor_.update_status(); + gimbal_left_friction_.update_status(); + gimbal_right_friction_.update_status(); + + const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); + + if (auto snapshot = bmi088_.snapshot()) { + *gimbal_pitch_velocity_bmi088_ = snapshot->gyro_body.y(); + *gimbal_yaw_velocity_bmi088_ = snapshot->gyro_body.z(); + tf_->set_transform( + snapshot->orientation.conjugate()); + } + + tf_->set_state( + pitch_encoder_angle); + } + + void command_update() const { + auto builder = board_->start_transmit(); + { + auto packet = gimbal_pitch_motor_.generate_torque_command(); + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x141, + .can_data = packet.as_bytes(), + }); + } + { + auto packet = device::CanPacket8{uint64_t{0}}; + packet << gimbal_left_friction_; + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = gimbal_left_friction_.send_id(), + .can_data = packet.as_bytes(), + }); + } + { + auto packet = device::CanPacket8{uint64_t{0}}; + packet << gimbal_right_friction_; + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = gimbal_right_friction_.send_id(), + .can_data = packet.as_bytes(), + }); + } + } + + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + if (can == Spec::kCans.kCan0) { + if (data.can_id == 0x141) + gimbal_pitch_motor_.store_status(data.can_data); + monitor_.tick("Top::Can0", data.can_id); + } else if (can == Spec::kCans.kCan1) { + gimbal_left_friction_.match_then_store_status(data.can_id, data.can_data); + monitor_.tick("Top::Can1", data.can_id); + } else if (can == Spec::kCans.kCan2) { + gimbal_right_friction_.match_then_store_status(data.can_id, data.can_data); + monitor_.tick("Top::Can2", data.can_id); + } + } + + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + if (uart == Spec::kUarts.kUart0) + vt13_.store_status(data.uart_data); + } + + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { + const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); + bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); + monitor_.tick("Top::Imu", "Acc"); + } + + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); + monitor_.tick("Top::Imu", "Gyr"); + if (!timestamp.has_value()) + return; + auto snapshot = + bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); + if (snapshot) + imu_snapshot_output_.emit(*snapshot); + } + + void gpio_digital_read_result_callback( + const Spec::Gpio& gpio, const View::GpioDigital& data) override { + if (gpio != Spec::kGpios.kUart1Rx) + return; + if (!data.timestamp_quarter_us) + return; + + const auto timestamp = board_clock_lifter_.lift_timestamp(*data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + + camera_signal_output_.emit(*timestamp); + monitor_.tick("Top::CameraSync", "Active"); + } + + auto status() const -> std::vector { return monitor_.text(); } + + DeformableInfantryOmni& status_; + OutputInterface& tf_; + OutputInterface gimbal_yaw_velocity_bmi088_; + OutputInterface gimbal_pitch_velocity_bmi088_; + + EventOutputInterface imu_snapshot_output_; + EventOutputInterface camera_signal_output_; + + device::Bmi088Ekf bmi088_; + device::BoardClockLifter board_clock_lifter_; + device::Vt13 vt13_; + device::LkMotor gimbal_pitch_motor_; + device::DjiMotor gimbal_left_friction_; + device::DjiMotor gimbal_right_friction_; + + StatusMonitor monitor_{}; + std::unique_ptr board_; + }; + struct BottomBoard final : public librmcs::board::RmcsBoardLite::Callback { public: explicit BottomBoard( - DeformableInfantryOmni& status, rmcs_executor::Component& command, + DeformableInfantryOmni& status, Component& command, const std::string& serial_filter = {}) : status_{status} , command_{command} @@ -412,7 +603,7 @@ class DeformableInfantryOmni device::Bmi088 imu_{1000, 0.2, 0.0}; device::LkMotor gimbal_yaw_motor_{status_, command_, "/gimbal/yaw"}; - device::Dr16 dr16_{}; + device::Dr16 dr16_; device::DjiMotor chassis_wheel_motors_[4]{ device::DjiMotor{status_, command_, "/chassis/left_front_wheel"}, @@ -633,188 +824,6 @@ class DeformableInfantryOmni StatusMonitor monitor_{}; }; - struct TopBoard final : public librmcs::board::RmcsBoardLite::Callback { - public: - explicit TopBoard( - DeformableInfantryOmni& status, rmcs_executor::Component& command, - const std::string& serial_filter = {}) - : tf_{status.tf_} - , bmi088_{device::Bmi088Ekf::Config{ - .body_to_sensor = - Eigen::AngleAxisd{std::numbers::pi / 2.0, Eigen::Vector3d::UnitX()} - .toRotationMatrix()}} - , gimbal_pitch_motor_(status, command, "/gimbal/pitch") - , gimbal_left_friction_(status, command, "/gimbal/left_friction") - , gimbal_right_friction_(status, command, "/gimbal/right_friction") { - - gimbal_pitch_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} - .set_reversed() - .set_encoder_zero_point( - static_cast(status.get_parameter("pitch_motor_zero_point").as_int()))); - - gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} - .set_reduction_ratio(1.) - .set_reversed()); - gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2}.set_reduction_ratio( - 1.)); - - status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); - status.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); - status.register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); - - auto options = librmcs::board::AdvancedOptions{}; - options.dangerously_skip_version_checks = true; - board_ = std::make_unique(*this, serial_filter, options); - - board_->start_transmit().gpio_digital_read( - Spec::kGpios.kUart1Rx, { - .period_ms = 0, - .asap = false, - .rising_edge = false, - .falling_edge = true, - .capture_timestamp = true, - .pull = librmcs::data::GpioPull::kUp, - }); - } - - ~TopBoard() override = default; - - [[nodiscard]] auto gimbal_yaw_velocity() const -> double { - return *gimbal_yaw_velocity_bmi088_; - } - - void request_hard_sync_read() { - // RMCS-lite top board variant currently has no GPIO hard-sync request - // path. - } - - void update() { - gimbal_pitch_motor_.update_status(); - gimbal_left_friction_.update_status(); - gimbal_right_friction_.update_status(); - - const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); - - if (auto snapshot = bmi088_.snapshot()) { - *gimbal_pitch_velocity_bmi088_ = snapshot->gyro_body.y(); - *gimbal_yaw_velocity_bmi088_ = snapshot->gyro_body.z(); - tf_->set_transform( - snapshot->orientation.conjugate()); - } - - tf_->set_state( - pitch_encoder_angle); - } - - void command_update() const { - auto builder = board_->start_transmit(); - builder.can_transmit( - Spec::kCans.kCan0, // - { - .can_id = 0x141, - .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan1, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_left_friction_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan2, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - gimbal_right_friction_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - } - - void can_receive_callback(const Spec::Can& can, const View::Can& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - if (can == Spec::kCans.kCan0) { - if (data.can_id == 0x141) - gimbal_pitch_motor_.store_status(data.can_data); - monitor_.tick("Top::Can0", data.can_id); - } else if (can == Spec::kCans.kCan1) { - if (data.can_id == 0x201) - gimbal_left_friction_.store_status(data.can_data); - monitor_.tick("Top::Can1", data.can_id); - } else if (can == Spec::kCans.kCan2) { - if (data.can_id == 0x202) - gimbal_right_friction_.store_status(data.can_data); - monitor_.tick("Top::Can2", data.can_id); - } - } - - void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { - const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); - bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); - monitor_.tick("Top::Imu", "Acc"); - } - - void gyroscope_receive_callback(const View::ImuGyroscope& data) override { - const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); - monitor_.tick("Top::Imu", "Gyr"); - if (!timestamp.has_value()) - return; - auto snapshot = - bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); - if (snapshot) - imu_snapshot_output_.emit(*snapshot); - } - - void gpio_digital_read_result_callback( - const Spec::Gpio& gpio, const View::GpioDigital& data) override { - if (gpio != Spec::kGpios.kUart1Rx) - return; - if (!data.timestamp_quarter_us) - return; - - const auto timestamp = board_clock_lifter_.lift_timestamp(*data.timestamp_quarter_us); - if (!timestamp.has_value()) - return; - - camera_signal_output_.emit(*timestamp); - monitor_.tick("Top::CameraSync", "Active"); - } - - auto status() const -> std::vector { return monitor_.text(); } - - OutputInterface& tf_; - OutputInterface gimbal_yaw_velocity_bmi088_; - OutputInterface gimbal_pitch_velocity_bmi088_; - - EventOutputInterface imu_snapshot_output_; - EventOutputInterface camera_signal_output_; - - device::Bmi088Ekf bmi088_; - device::BoardClockLifter board_clock_lifter_; - device::LkMotor gimbal_pitch_motor_; - device::DjiMotor gimbal_left_friction_; - device::DjiMotor gimbal_right_friction_; - - StatusMonitor monitor_{}; - std::unique_ptr board_; - }; - auto status_service_callback(const std::shared_ptr& response) -> void { response->success = true; @@ -865,4 +874,4 @@ class DeformableInfantryOmni } // namespace rmcs_core::hardware #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DeformableInfantryOmni, rmcs_executor::Component) +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DeformableInfantryOmni, rmcs_executor::Component) \ No newline at end of file From 7937abfc2485f588b1294035f3e9cefb409121d3 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Sat, 25 Jul 2026 12:11:02 +0800 Subject: [PATCH 59/86] feat(chassis): rebase wireless charging mode onto dev/robots --- .../controller/chassis/deformable_chassis.cpp | 36 +++++++++++++++++-- .../controller/chassis/deformable_mode.hpp | 4 +++ .../chassis/hero_chassis_controller.cpp | 9 +++++ .../include/rmcs_msgs/chassis_mode.hpp | 1 + .../rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp | 1 + 5 files changed, 48 insertions(+), 3 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index 397400cfe..c961bb1c7 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -30,7 +30,8 @@ class DeformableChassis get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) , following_velocity_controller_(10.0, 0.0, 0.0) - , spin_ratio_(std::clamp(get_parameter_or("spin_ratio", 0.6), 0.0, 1.0)) + , wireless_charging_offset_rad_( + deg_to_rad(get_parameter_or("wireless_charging_offset_deg", 135.0))) , joint_mode_mgr_(*this) { following_velocity_controller_.output_max = angular_velocity_max_; @@ -189,7 +190,7 @@ class DeformableChassis case rmcs_msgs::ChassisMode::SPIN_FAST: { bool forward = joint_mode_mgr_.spinning_forward(); angular_velocity = - spin_ratio_ * (forward ? angular_velocity_max_ : -angular_velocity_max_); + forward ? angular_velocity_max_ : -angular_velocity_max_; angular_velocity = std::clamp(angular_velocity, -angular_velocity_max_, angular_velocity_max_); } break; @@ -209,6 +210,19 @@ class DeformableChassis angular_velocity = following_velocity_controller_.update(chassis_angle_error); } break; + case rmcs_msgs::ChassisMode::WIRELESS_CHARGING: { + double chassis_angle_error = + calculate_unsigned_chassis_angle_error(chassis_control_angle); + + chassis_control_angle = normalize_positive_angle( + chassis_control_angle + wireless_charging_offset_rad_); + chassis_angle_error = normalize_positive_angle( + chassis_angle_error + wireless_charging_offset_rad_); + chassis_angle_error = normalize_signed_angle(chassis_angle_error); + + angular_velocity = following_velocity_controller_.update(chassis_angle_error); + } break; + default: break; } @@ -232,6 +246,22 @@ class DeformableChassis static double deg_to_rad(double deg) { return deg * std::numbers::pi / 180.0; } + static double normalize_positive_angle(double angle) { + constexpr double full_turn = 2 * std::numbers::pi; + while (angle >= full_turn) + angle -= full_turn; + while (angle < 0.0) + angle += full_turn; + return angle; + } + + static double normalize_signed_angle(double angle) { + angle = normalize_positive_angle(angle); + if (angle > std::numbers::pi) + angle -= 2 * std::numbers::pi; + return angle; + } + void publish_joint_posture_targets_() { std::array targets_deg{}; joint_mode_mgr_.copy_joint_posture_target_deg(targets_deg); @@ -271,7 +301,7 @@ class DeformableChassis std::array, kJointCount> joint_posture_target_angle_rad_; pid::PidCalculator following_velocity_controller_; - const double spin_ratio_; + const double wireless_charging_offset_rad_; DeformableChassisModeManager joint_mode_mgr_; }; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp index dac11d423..f7677d187 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp @@ -164,6 +164,10 @@ class DeformableChassisModeManager { next_mode = next_mode == rmcs_msgs::ChassisMode::STEP_DOWN ? rmcs_msgs::ChassisMode::AUTO : rmcs_msgs::ChassisMode::STEP_DOWN; + } else if (!last_keyboard_.x && keyboard.x) { + next_mode = next_mode == rmcs_msgs::ChassisMode::WIRELESS_CHARGING + ? rmcs_msgs::ChassisMode::AUTO + : rmcs_msgs::ChassisMode::WIRELESS_CHARGING; } joint_posture_state_.mode = next_mode; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp index 9291886ed..7fc3c075b 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp @@ -216,6 +216,15 @@ class HeroChassisController angular_velocity = following_velocity_controller_.update(err); } break; + case rmcs_msgs::ChassisMode::WIRELESS_CHARGING: { + constexpr double offset = std::numbers::pi / 4; + double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + + chassis_control_angle = normalize_positive_angle(chassis_control_angle + offset); + err = normalize_signed_angle(err + offset); + + angular_velocity = following_velocity_controller_.update(err); + } break; } *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; *chassis_control_angle_ = chassis_control_angle; diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp index a279b92c7..87599fb7f 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp @@ -12,6 +12,7 @@ enum class ChassisMode : uint8_t { LAUNCH_RAMP, ALIGNMENT, ALIGNMENT_POWERED, + WIRELESS_CHARGING, CLIMB, }; diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp index 1cccd175a..456e1f672 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp @@ -50,6 +50,7 @@ constexpr auto to_string(ChassisMode mode) noexcept -> const char* { case ChassisMode::SPIN_SLOW: return "SPIN_SLOW"; case ChassisMode::ALIGNMENT: return "ALIGNMENT"; case ChassisMode::ALIGNMENT_POWERED: return "ALIGNMENT_POWERED"; + case ChassisMode::WIRELESS_CHARGING: return "WIRELESS_CHARGING"; case ChassisMode::CLIMB: return "CLIMB"; } return "INVALID"; From 4694b139dc8f07a28a78c0650281c9cfc2be7538 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Mon, 27 Jul 2026 00:13:08 +0800 Subject: [PATCH 60/86] feat: Add static assertions for data structure sizes --- .../src/referee/app/ui/shape/shape.hpp | 4 ++ .../referee/command/interaction/header.hpp | 1 + rmcs_ws/src/rmcs_core/src/referee/status.cpp | 66 ++++++++++++++----- .../rmcs_core/src/referee/status/field.hpp | 7 ++ .../include/rmcs_msgs/full_robot_id.hpp | 8 +-- .../rmcs_msgs/include/rmcs_msgs/robot_id.hpp | 12 ++-- 6 files changed, 70 insertions(+), 28 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp index a0bb9b3b1..5891905df 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp @@ -182,6 +182,10 @@ class Shape uint16_t details_e : 11; } part3; }; + static_assert(sizeof(DescriptionField::Part1) == 4); + static_assert(sizeof(DescriptionField::Part2) == 4); + static_assert(sizeof(DescriptionField::Part3) == 4); + static_assert(sizeof(DescriptionField) == 15); void set_modified() { // Optimization: Assume the modification does not exist when invisible. diff --git a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/header.hpp b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/header.hpp index 97223d51b..bd271a5ed 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/header.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/header.hpp @@ -11,5 +11,6 @@ struct __attribute__((packed)) Header { uint16_t sender_id; uint16_t receiver_id; }; +static_assert(sizeof(Header) == 6); } // namespace rmcs_core::referee::command::interaction \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/referee/status.cpp b/rmcs_ws/src/rmcs_core/src/referee/status.cpp index c0a7db62e..4768ad474 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status.cpp @@ -156,6 +156,19 @@ class Status } private: + template + bool read_frame_data(const char* name, T& data) { + if (frame_.header.data_length < sizeof(T)) { + RCLCPP_WARN( + logger_, "%s length invalid: %u", name, + static_cast(frame_.header.data_length)); + return false; + } + + std::memcpy(&data, frame_.body.data, sizeof(data)); + return true; + } + void process_frame() { auto command_id = frame_.body.command_id; if (command_id == 0x0001) @@ -185,7 +198,9 @@ class Status } void update_game_status() { - auto& data = reinterpret_cast(frame_.body.data); + GameStatus data; + if (!read_frame_data("Game status", data)) + return; *game_stage_ = static_cast(data.game_progress); *stage_remain_time_ = data.stage_remain_time; @@ -198,7 +213,9 @@ class Status } void update_event_data() { - auto& data = reinterpret_cast(frame_.body.data); + EventData data; + if (!read_frame_data("Event data", data)) + return; *ally_small_energy_activation_status_ = data.ally_small_energy_activation_status; *ally_big_energy_activation_status_ = data.ally_big_energy_activation_status; @@ -206,13 +223,18 @@ class Status } void update_dart_info() { - auto& data = reinterpret_cast(frame_.body.data); + DartInfo data; + if (!read_frame_data("Dart info", data)) + return; *dart_latest_hit_target_total_count_ = data.latest_hit_target_total_count; } void update_game_robot_hp() { - auto& data = reinterpret_cast(frame_.body.data); + GameRobotHp data; + if (!read_frame_data("Game robot hp", data)) + return; + *robots_hp_ = data; *ally_hero_hp_ = data.ally_1_robot_hp; *ally_engineer_hp_ = data.ally_2_robot_hp; @@ -226,13 +248,15 @@ class Status } void update_robot_status() { + RobotStatus data; + if (!read_frame_data("Robot status", data)) + return; + if (*game_stage_ == rmcs_msgs::GameStage::STARTED) robot_status_watchdog_.reset(60'000); else robot_status_watchdog_.reset(5'000); - auto& data = reinterpret_cast(frame_.body.data); - *robot_current_hp_ = data.current_hp; *robot_id_ = static_cast(data.robot_id); *robot_shooter_cooling_ = data.shooter_barrel_cooling_value; @@ -247,14 +271,20 @@ class Status } void update_power_heat_data() { + PowerHeatData data; + if (!read_frame_data("Power heat data", data)) + return; + power_heat_data_watchdog_.reset(3'000); - auto& data = reinterpret_cast(frame_.body.data); *robot_buffer_energy_ = static_cast(data.buffer_energy); } void update_robot_position() { - auto& data = reinterpret_cast(frame_.body.data); + RobotPosition data; + if (!read_frame_data("Robot position", data)) + return; + *robot_position_x_ = data.x; *robot_position_y_ = data.y; *robot_position_angle_ = data.angle; @@ -263,7 +293,10 @@ class Status void update_hurt_data() {} void update_shoot_data() { - auto& data = reinterpret_cast(frame_.body.data); + ShootData data; + if (!read_frame_data("Shoot data", data)) + return; + *robot_initial_speed_ = data.initial_speed; const auto now = std::chrono::high_resolution_clock::now(); @@ -271,7 +304,10 @@ class Status } void update_bullet_allowance() { - auto& data = reinterpret_cast(frame_.body.data); + BulletAllowance data; + if (!read_frame_data("Bullet allowance", data)) + return; + *robot_bullet_allowance_ = data.projectile_allowance_17mm; *robot_42mm_bullet_allowance_ = data.projectile_allowance_42mm; *remaining_gold_coin_ = data.remaining_gold_coin; @@ -290,15 +326,9 @@ class Status } void update_map_command() { - if (frame_.header.data_length < sizeof(MapCommand)) { - RCLCPP_WARN( - logger_, "Map command length invalid: %u", - static_cast(frame_.header.data_length)); - return; - } - MapCommand data; - std::memcpy(&data, frame_.body.data, sizeof(data)); + if (!read_frame_data("Map command", data)) + return; *map_command_target_position_x_ = data.target_position_x; *map_command_target_position_y_ = data.target_position_y; diff --git a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp index ad5e21619..68472badc 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp @@ -10,6 +10,7 @@ struct __attribute__((packed)) GameStatus { uint16_t stage_remain_time; uint64_t sync_timestamp; }; +static_assert(sizeof(GameStatus) == 11); struct __attribute__((packed)) GameRobotHp { uint16_t ally_1_robot_hp; @@ -23,6 +24,7 @@ struct __attribute__((packed)) GameRobotHp { uint16_t enemy_outpost_hp; uint16_t enemy_base_hp; }; +static_assert(sizeof(GameRobotHp) == 20); struct __attribute__((packed)) EventData { std::uint32_t ally_supply_zone_occupied : 1 = 0; @@ -75,17 +77,20 @@ struct __attribute__((packed)) PowerHeatData { uint16_t shooter_17mm_barrel_heat; uint16_t shooter_42mm_barrel_heat; }; +static_assert(sizeof(PowerHeatData) == 14); struct __attribute__((packed)) RobotPosition { float x; float y; float angle; }; +static_assert(sizeof(RobotPosition) == 12); struct __attribute__((packed)) HurtData { uint8_t armor_id : 4; uint8_t reason : 4; }; +static_assert(sizeof(HurtData) == 1); struct __attribute__((packed)) ShootData { uint8_t bullet_type; @@ -93,6 +98,7 @@ struct __attribute__((packed)) ShootData { uint8_t launching_frequency; float initial_speed; }; +static_assert(sizeof(ShootData) == 7); struct __attribute__((packed)) BulletAllowance { uint16_t projectile_allowance_17mm; @@ -100,6 +106,7 @@ struct __attribute__((packed)) BulletAllowance { uint16_t remaining_gold_coin; uint16_t projectile_allowance_fortress; }; +static_assert(sizeof(BulletAllowance) == 8); struct __attribute__((packed)) MapCommand { float target_position_x; diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/full_robot_id.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/full_robot_id.hpp index 4cdc6d5d4..f7f4459a5 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/full_robot_id.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/full_robot_id.hpp @@ -21,8 +21,8 @@ class FullRobotId { RED_SENTRY = 7, RED_DART = 8, RED_RADAR = 9, - RED_OUTPOST = 10, - RED_BASE = 11, + RED_BASE = 10, + RED_OUTPOST = 11, BLUE_HERO = 101, BLUE_ENGINEER = 102, @@ -33,8 +33,8 @@ class FullRobotId { BLUE_SENTRY = 107, BLUE_DART = 108, BLUE_RADAR = 109, - BLUE_OUTPOST = 110, - BLUE_BASE = 111, + BLUE_BASE = 110, + BLUE_OUTPOST = 111, RED_HERO_CLIENT = 0x0101, RED_ENGINEER_CLIENT = 0x0102, diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_id.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_id.hpp index 2c82568ba..f596e6668 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_id.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_id.hpp @@ -16,8 +16,8 @@ enum class ArmorID : uint16_t { Sentry = 7, Dart = 8, Radar = 9, - Outpost = 10, - Base = 11, + Base = 10, + Outpost = 11, }; class RobotId { @@ -34,8 +34,8 @@ class RobotId { RED_SENTRY = 7, RED_DART = 8, RED_RADAR = 9, - RED_OUTPOST = 10, - RED_BASE = 11, + RED_BASE = 10, + RED_OUTPOST = 11, BLUE_HERO = 101, BLUE_ENGINEER = 102, @@ -46,8 +46,8 @@ class RobotId { BLUE_SENTRY = 107, BLUE_DART = 108, BLUE_RADAR = 109, - BLUE_OUTPOST = 110, - BLUE_BASE = 111, + BLUE_BASE = 110, + BLUE_OUTPOST = 111, }; constexpr RobotId() From ed8b5a9ce50734ca99694a136145278d61fa6204 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Tue, 28 Jul 2026 00:17:36 +0800 Subject: [PATCH 61/86] feat(chassis): update wireless charging mode parameters --- .../config/deformable-infantry-omni-b.yaml | 4 +- .../config/deformable-infantry-omni-c.yaml | 4 +- .../config/deformable-infantry-omni.yaml | 4 +- .../controller/chassis/chassis_controller.cpp | 8 +++ .../chassis/hero_chassis_controller.cpp | 9 --- .../src/referee/app/ui/shape/shape.hpp | 4 -- .../referee/command/interaction/header.hpp | 1 - rmcs_ws/src/rmcs_core/src/referee/status.cpp | 66 +++++-------------- .../rmcs_core/src/referee/status/field.hpp | 12 +--- .../include/rmcs_msgs/full_robot_id.hpp | 8 +-- .../rmcs_msgs/include/rmcs_msgs/robot_id.hpp | 12 ++-- 11 files changed, 43 insertions(+), 89 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index c674be36e..152566a36 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -162,8 +162,8 @@ deformable_suspension: active_suspension_roll_inner_output_max: 0.785 # Chassis-owned joint intent trajectory limits while attitude correction is active. - active_suspension_target_velocity_limit_deg: 80.0 - active_suspension_target_acceleration_limit_deg: 360.0 + active_suspension_target_velocity_limit_deg: 150.0 + active_suspension_target_acceleration_limit_deg: 600.0 active_suspension_correction_velocity_limit_deg: 720.0 active_suspension_correction_acceleration_limit_deg: 3600.0 active_suspension_rate_lpf_cutoff_hz: 10.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml index ea5a4cf78..2a5d4dedf 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml @@ -158,8 +158,8 @@ deformable_suspension: active_suspension_roll_inner_output_max: 0.785 # Chassis-owned joint intent trajectory limits while attitude correction is active. - active_suspension_target_velocity_limit_deg: 80.0 - active_suspension_target_acceleration_limit_deg: 360.0 + active_suspension_target_velocity_limit_deg: 150.0 + active_suspension_target_acceleration_limit_deg: 600.0 active_suspension_correction_velocity_limit_deg: 720.0 active_suspension_correction_acceleration_limit_deg: 3600.0 active_suspension_rate_lpf_cutoff_hz: 10.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 0d3a4afb1..9d0c3259f 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -160,8 +160,8 @@ deformable_suspension: active_suspension_roll_inner_output_max: 0.785 # Chassis-owned joint intent trajectory limits while attitude correction is active. - active_suspension_target_velocity_limit_deg: 80.0 - active_suspension_target_acceleration_limit_deg: 360.0 + active_suspension_target_velocity_limit_deg: 150.0 + active_suspension_target_acceleration_limit_deg: 600.0 active_suspension_correction_velocity_limit_deg: 720.0 active_suspension_correction_acceleration_limit_deg: 3600.0 active_suspension_rate_lpf_cutoff_hz: 10.0 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp index 81f6bbaf3..6ac4dc4b8 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp @@ -299,7 +299,15 @@ class ChassisController angular_velocity = following_velocity_controller_.update(err); } break; + case ChassisMode::WIRELESS_CHARGING: { + constexpr double offset = std::numbers::pi / 4; + double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + + chassis_control_angle = chassis_control_angle + offset; + err = normalize_signed_angle(err + offset); + angular_velocity = following_velocity_controller_.update(err); + } break; case ChassisMode::CLIMB: { chassis_control_angle = *chassis_climb_direction_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp index 7fc3c075b..9291886ed 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp @@ -216,15 +216,6 @@ class HeroChassisController angular_velocity = following_velocity_controller_.update(err); } break; - case rmcs_msgs::ChassisMode::WIRELESS_CHARGING: { - constexpr double offset = std::numbers::pi / 4; - double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); - - chassis_control_angle = normalize_positive_angle(chassis_control_angle + offset); - err = normalize_signed_angle(err + offset); - - angular_velocity = following_velocity_controller_.update(err); - } break; } *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; *chassis_control_angle_ = chassis_control_angle; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp index 5891905df..a0bb9b3b1 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp @@ -182,10 +182,6 @@ class Shape uint16_t details_e : 11; } part3; }; - static_assert(sizeof(DescriptionField::Part1) == 4); - static_assert(sizeof(DescriptionField::Part2) == 4); - static_assert(sizeof(DescriptionField::Part3) == 4); - static_assert(sizeof(DescriptionField) == 15); void set_modified() { // Optimization: Assume the modification does not exist when invisible. diff --git a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/header.hpp b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/header.hpp index bd271a5ed..97223d51b 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/header.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/header.hpp @@ -11,6 +11,5 @@ struct __attribute__((packed)) Header { uint16_t sender_id; uint16_t receiver_id; }; -static_assert(sizeof(Header) == 6); } // namespace rmcs_core::referee::command::interaction \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/referee/status.cpp b/rmcs_ws/src/rmcs_core/src/referee/status.cpp index 4768ad474..c0a7db62e 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status.cpp @@ -156,19 +156,6 @@ class Status } private: - template - bool read_frame_data(const char* name, T& data) { - if (frame_.header.data_length < sizeof(T)) { - RCLCPP_WARN( - logger_, "%s length invalid: %u", name, - static_cast(frame_.header.data_length)); - return false; - } - - std::memcpy(&data, frame_.body.data, sizeof(data)); - return true; - } - void process_frame() { auto command_id = frame_.body.command_id; if (command_id == 0x0001) @@ -198,9 +185,7 @@ class Status } void update_game_status() { - GameStatus data; - if (!read_frame_data("Game status", data)) - return; + auto& data = reinterpret_cast(frame_.body.data); *game_stage_ = static_cast(data.game_progress); *stage_remain_time_ = data.stage_remain_time; @@ -213,9 +198,7 @@ class Status } void update_event_data() { - EventData data; - if (!read_frame_data("Event data", data)) - return; + auto& data = reinterpret_cast(frame_.body.data); *ally_small_energy_activation_status_ = data.ally_small_energy_activation_status; *ally_big_energy_activation_status_ = data.ally_big_energy_activation_status; @@ -223,18 +206,13 @@ class Status } void update_dart_info() { - DartInfo data; - if (!read_frame_data("Dart info", data)) - return; + auto& data = reinterpret_cast(frame_.body.data); *dart_latest_hit_target_total_count_ = data.latest_hit_target_total_count; } void update_game_robot_hp() { - GameRobotHp data; - if (!read_frame_data("Game robot hp", data)) - return; - + auto& data = reinterpret_cast(frame_.body.data); *robots_hp_ = data; *ally_hero_hp_ = data.ally_1_robot_hp; *ally_engineer_hp_ = data.ally_2_robot_hp; @@ -248,15 +226,13 @@ class Status } void update_robot_status() { - RobotStatus data; - if (!read_frame_data("Robot status", data)) - return; - if (*game_stage_ == rmcs_msgs::GameStage::STARTED) robot_status_watchdog_.reset(60'000); else robot_status_watchdog_.reset(5'000); + auto& data = reinterpret_cast(frame_.body.data); + *robot_current_hp_ = data.current_hp; *robot_id_ = static_cast(data.robot_id); *robot_shooter_cooling_ = data.shooter_barrel_cooling_value; @@ -271,20 +247,14 @@ class Status } void update_power_heat_data() { - PowerHeatData data; - if (!read_frame_data("Power heat data", data)) - return; - power_heat_data_watchdog_.reset(3'000); + auto& data = reinterpret_cast(frame_.body.data); *robot_buffer_energy_ = static_cast(data.buffer_energy); } void update_robot_position() { - RobotPosition data; - if (!read_frame_data("Robot position", data)) - return; - + auto& data = reinterpret_cast(frame_.body.data); *robot_position_x_ = data.x; *robot_position_y_ = data.y; *robot_position_angle_ = data.angle; @@ -293,10 +263,7 @@ class Status void update_hurt_data() {} void update_shoot_data() { - ShootData data; - if (!read_frame_data("Shoot data", data)) - return; - + auto& data = reinterpret_cast(frame_.body.data); *robot_initial_speed_ = data.initial_speed; const auto now = std::chrono::high_resolution_clock::now(); @@ -304,10 +271,7 @@ class Status } void update_bullet_allowance() { - BulletAllowance data; - if (!read_frame_data("Bullet allowance", data)) - return; - + auto& data = reinterpret_cast(frame_.body.data); *robot_bullet_allowance_ = data.projectile_allowance_17mm; *robot_42mm_bullet_allowance_ = data.projectile_allowance_42mm; *remaining_gold_coin_ = data.remaining_gold_coin; @@ -326,9 +290,15 @@ class Status } void update_map_command() { - MapCommand data; - if (!read_frame_data("Map command", data)) + if (frame_.header.data_length < sizeof(MapCommand)) { + RCLCPP_WARN( + logger_, "Map command length invalid: %u", + static_cast(frame_.header.data_length)); return; + } + + MapCommand data; + std::memcpy(&data, frame_.body.data, sizeof(data)); *map_command_target_position_x_ = data.target_position_x; *map_command_target_position_y_ = data.target_position_y; diff --git a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp index 68472badc..fc7efa63a 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp @@ -10,21 +10,17 @@ struct __attribute__((packed)) GameStatus { uint16_t stage_remain_time; uint64_t sync_timestamp; }; -static_assert(sizeof(GameStatus) == 11); struct __attribute__((packed)) GameRobotHp { uint16_t ally_1_robot_hp; uint16_t ally_2_robot_hp; uint16_t ally_3_robot_hp; uint16_t ally_4_robot_hp; - int16_t damage_difference; + uint16_t reserved; uint16_t ally_7_robot_hp; uint16_t ally_outpost_hp; uint16_t ally_base_hp; - uint16_t enemy_outpost_hp; - uint16_t enemy_base_hp; }; -static_assert(sizeof(GameRobotHp) == 20); struct __attribute__((packed)) EventData { std::uint32_t ally_supply_zone_occupied : 1 = 0; @@ -67,7 +63,6 @@ struct __attribute__((packed)) RobotStatus { std::uint8_t power_management_shooter_output : 1 = 0; std::uint8_t reserved : 5 = 0; }; -static_assert(sizeof(RobotStatus) == 17); struct __attribute__((packed)) PowerHeatData { uint16_t reserved_1; @@ -77,20 +72,17 @@ struct __attribute__((packed)) PowerHeatData { uint16_t shooter_17mm_barrel_heat; uint16_t shooter_42mm_barrel_heat; }; -static_assert(sizeof(PowerHeatData) == 14); struct __attribute__((packed)) RobotPosition { float x; float y; float angle; }; -static_assert(sizeof(RobotPosition) == 12); struct __attribute__((packed)) HurtData { uint8_t armor_id : 4; uint8_t reason : 4; }; -static_assert(sizeof(HurtData) == 1); struct __attribute__((packed)) ShootData { uint8_t bullet_type; @@ -98,7 +90,6 @@ struct __attribute__((packed)) ShootData { uint8_t launching_frequency; float initial_speed; }; -static_assert(sizeof(ShootData) == 7); struct __attribute__((packed)) BulletAllowance { uint16_t projectile_allowance_17mm; @@ -106,7 +97,6 @@ struct __attribute__((packed)) BulletAllowance { uint16_t remaining_gold_coin; uint16_t projectile_allowance_fortress; }; -static_assert(sizeof(BulletAllowance) == 8); struct __attribute__((packed)) MapCommand { float target_position_x; diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/full_robot_id.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/full_robot_id.hpp index f7f4459a5..4cdc6d5d4 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/full_robot_id.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/full_robot_id.hpp @@ -21,8 +21,8 @@ class FullRobotId { RED_SENTRY = 7, RED_DART = 8, RED_RADAR = 9, - RED_BASE = 10, - RED_OUTPOST = 11, + RED_OUTPOST = 10, + RED_BASE = 11, BLUE_HERO = 101, BLUE_ENGINEER = 102, @@ -33,8 +33,8 @@ class FullRobotId { BLUE_SENTRY = 107, BLUE_DART = 108, BLUE_RADAR = 109, - BLUE_BASE = 110, - BLUE_OUTPOST = 111, + BLUE_OUTPOST = 110, + BLUE_BASE = 111, RED_HERO_CLIENT = 0x0101, RED_ENGINEER_CLIENT = 0x0102, diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_id.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_id.hpp index f596e6668..2c82568ba 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_id.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_id.hpp @@ -16,8 +16,8 @@ enum class ArmorID : uint16_t { Sentry = 7, Dart = 8, Radar = 9, - Base = 10, - Outpost = 11, + Outpost = 10, + Base = 11, }; class RobotId { @@ -34,8 +34,8 @@ class RobotId { RED_SENTRY = 7, RED_DART = 8, RED_RADAR = 9, - RED_BASE = 10, - RED_OUTPOST = 11, + RED_OUTPOST = 10, + RED_BASE = 11, BLUE_HERO = 101, BLUE_ENGINEER = 102, @@ -46,8 +46,8 @@ class RobotId { BLUE_SENTRY = 107, BLUE_DART = 108, BLUE_RADAR = 109, - BLUE_BASE = 110, - BLUE_OUTPOST = 111, + BLUE_OUTPOST = 110, + BLUE_BASE = 111, }; constexpr RobotId() From f1ff8f4581cd2f943adcdfc72a48d4b1980903cf Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Tue, 28 Jul 2026 02:30:52 +0800 Subject: [PATCH 62/86] feat(deformable): add wireless charging joint posture with offset-directed leg lifting - X/WIRELESS_CHARGING mode now raises the offset-aligned leg and its diagonal counterpart, with Q toggling between hardcoded high/low charging poses - Leg azimuths unified to CCW [0,360): LF=45, LB=135, RB=225, RF=315 - wireless_charging_offset_deg is now a required parameter (no default) - Offset direction corrected (subtract in chassis follow) - Offset ownership moved to DeformableChassisModeManager; chassis consumes via getter - Add WIRELESS_CHARGING to hero chassis fallthrough chain - Add BLACK UI color for WIRELESS_CHARGING mode in deformable referee UI --- .../config/deformable-infantry-omni-b.yaml | 2 +- .../config/deformable-infantry-omni.yaml | 2 +- .../controller/chassis/chassis_controller.cpp | 10 +- .../controller/chassis/deformable_chassis.cpp | 9 +- .../controller/chassis/deformable_mode.hpp | 123 +++++++++++++++++- .../chassis/hero_chassis_controller.cpp | 1 + .../referee/app/ui/deformable_infantry_ui.cpp | 1 + 7 files changed, 128 insertions(+), 20 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 152566a36..70ca8417f 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -124,7 +124,7 @@ chassis_controller: min_angle: 4.0 max_angle: 59.0 active_suspension_enable: true - wireless_charging_offset_deg: 135.0 + wireless_charging_offset_deg: 225.0 deformable_suspension: ros__parameters: diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 9d0c3259f..d49c24e15 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -122,7 +122,7 @@ chassis_controller: min_angle: 4.0 max_angle: 59.0 active_suspension_enable: true - wireless_charging_offset_deg: -45.0 + wireless_charging_offset_deg: 45.0 deformable_suspension: ros__parameters: diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp index 6ac4dc4b8..63e5698dc 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp @@ -299,15 +299,7 @@ class ChassisController angular_velocity = following_velocity_controller_.update(err); } break; - case ChassisMode::WIRELESS_CHARGING: { - constexpr double offset = std::numbers::pi / 4; - double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); - - chassis_control_angle = chassis_control_angle + offset; - err = normalize_signed_angle(err + offset); - - angular_velocity = following_velocity_controller_.update(err); - } break; + case ChassisMode::WIRELESS_CHARGING: [[fallthrough]]; case ChassisMode::CLIMB: { chassis_control_angle = *chassis_climb_direction_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index c961bb1c7..2f424b14b 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -30,8 +30,6 @@ class DeformableChassis get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) , following_velocity_controller_(10.0, 0.0, 0.0) - , wireless_charging_offset_rad_( - deg_to_rad(get_parameter_or("wireless_charging_offset_deg", 135.0))) , joint_mode_mgr_(*this) { following_velocity_controller_.output_max = angular_velocity_max_; @@ -211,13 +209,15 @@ class DeformableChassis } break; case rmcs_msgs::ChassisMode::WIRELESS_CHARGING: { + const double wireless_charging_offset_rad = + joint_mode_mgr_.wireless_charging_offset_rad(); double chassis_angle_error = calculate_unsigned_chassis_angle_error(chassis_control_angle); chassis_control_angle = normalize_positive_angle( - chassis_control_angle + wireless_charging_offset_rad_); + chassis_control_angle - wireless_charging_offset_rad); chassis_angle_error = normalize_positive_angle( - chassis_angle_error + wireless_charging_offset_rad_); + chassis_angle_error - wireless_charging_offset_rad); chassis_angle_error = normalize_signed_angle(chassis_angle_error); angular_velocity = following_velocity_controller_.update(chassis_angle_error); @@ -301,7 +301,6 @@ class DeformableChassis std::array, kJointCount> joint_posture_target_angle_rad_; pid::PidCalculator following_velocity_controller_; - const double wireless_charging_offset_rad_; DeformableChassisModeManager joint_mode_mgr_; }; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp index f7677d187..90efa38ff 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp @@ -40,7 +40,12 @@ class DeformableChassisModeManager { std::clamp( node.get_parameter_or("active_suspension_base_angle", max_angle_), min_angle_ - 5.0, max_angle_)) - , suspension_enable_(node.get_parameter_or("active_suspension_enable", false)) { + , suspension_enable_(node.get_parameter_or("active_suspension_enable", false)) + , wireless_charging_offset_deg_(normalize_ccw_deg_( + node.get_parameter("wireless_charging_offset_deg").as_double())) + , wireless_charging_offset_rad_(deg_to_rad_(wireless_charging_offset_deg_)) + , charging_raised_leg_index_( + select_charging_raised_leg_index_(wireless_charging_offset_deg_)) { current_target_angle_ = max_angle_; joint_current_target_angle_.fill(max_angle_); update_joint_posture_state_(false); @@ -64,6 +69,8 @@ class DeformableChassisModeManager { apply_symmetric_target_ = true; suspension_enabled_by_toggle_ = false; low_prone_enabled_by_toggle_ = false; + charging_posture_high_ = true; + before_wireless_charging_ = false; last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; last_keyboard_ = rmcs_msgs::Keyboard::zero(); @@ -77,6 +84,7 @@ class DeformableChassisModeManager { const rmcs_msgs::Keyboard& keyboard, double rotary_knob, double dt) { update_mode_from_inputs_(switch_left, switch_right, keyboard); + update_wireless_charging_posture_transition_(); update_low_prone_toggle_from_inputs_(switch_left, switch_right); joint_posture_state_.ctrl_low_prone_active = keyboard.ctrl; @@ -110,6 +118,8 @@ class DeformableChassisModeManager { double min_angle() const { return min_angle_; } double max_angle() const { return max_angle_; } double max_angle_rad() const { return deg_to_rad_(max_angle_); } + double wireless_charging_offset_deg() const { return wireless_charging_offset_deg_; } + double wireless_charging_offset_rad() const { return wireless_charging_offset_rad_; } double active_suspension_min_angle_rad() const { return deg_to_rad_(min_angle_ - 5.0); } @@ -125,8 +135,51 @@ class DeformableChassisModeManager { static constexpr size_t kRightFront = 3; static constexpr size_t kJointCount = 4; + // Hardcoded wireless-charging postures: raised leg ~5cm higher (L=140mm). + static constexpr double kChargingHighSupportDeg = 59.0; + static constexpr double kChargingHighRaisedDeg = 35.0; + static constexpr double kChargingLowSupportDeg = 25.0; + static constexpr double kChargingLowRaisedDeg = 8.0; + + // Leg azimuths in chassis frame (deg), counterclockwise from +X: LF → LB → RB → RF. + static constexpr std::array kLegAzimuthDeg = { + 45.0, + 135.0, + 225.0, + 315.0, + }; + static double deg_to_rad_(double deg) { return deg * std::numbers::pi / 180.0; } + static double normalize_ccw_deg_(double deg) { + double normalized = std::fmod(deg, 360.0); + if (normalized < 0.0) + normalized += 360.0; + return normalized; + } + + static double absolute_angle_distance_deg_(double a, double b) { + double delta = std::abs(normalize_ccw_deg_(a) - normalize_ccw_deg_(b)); + if (delta > 180.0) + delta = 360.0 - delta; + return delta; + } + + static size_t select_charging_raised_leg_index_(double wireless_charging_offset_deg) { + const double offset_ccw = normalize_ccw_deg_(wireless_charging_offset_deg); + size_t best_index = kLeftFront; + double best_distance = absolute_angle_distance_deg_(offset_ccw, kLegAzimuthDeg[kLeftFront]); + for (size_t i = 1; i < kJointCount; ++i) { + const double distance = + absolute_angle_distance_deg_(offset_ccw, kLegAzimuthDeg[i]); + if (distance < best_distance) { + best_distance = distance; + best_index = i; + } + } + return best_index; + } + static bool symmetric_joint_target_requested_(const std::array& joint_target_deg) { constexpr double epsilon = 1e-6; @@ -135,6 +188,54 @@ class DeformableChassisModeManager { }); } + double posture_reference_angle_deg_() const { + if (apply_symmetric_target_) + return current_target_angle_; + + double sum = 0.0; + for (double angle_deg : joint_current_target_angle_) + sum += angle_deg; + return sum / static_cast(kJointCount); + } + + bool is_posture_high_() const { + const double midpoint = (min_angle_ + max_angle_) / 2.0; + return posture_reference_angle_deg_() >= midpoint; + } + + void apply_wireless_charging_posture_(bool high) { + const double support_deg = high ? kChargingHighSupportDeg : kChargingLowSupportDeg; + const double raised_deg = high ? kChargingHighRaisedDeg : kChargingLowRaisedDeg; + const size_t diagonal_index = (charging_raised_leg_index_ + 2) % kJointCount; + + current_target_angle_ = support_deg; + apply_symmetric_target_ = false; + joint_current_target_angle_.fill(support_deg); + joint_current_target_angle_[charging_raised_leg_index_] = raised_deg; + joint_current_target_angle_[diagonal_index] = raised_deg; + charging_posture_high_ = high; + } + + void apply_symmetric_posture_from_high_(bool high) { + current_target_angle_ = high ? max_angle_ : min_angle_; + apply_symmetric_target_ = true; + joint_current_target_angle_.fill(current_target_angle_); + } + + void update_wireless_charging_posture_transition_() { + const bool now_wireless_charging_ = + joint_posture_state_.mode == rmcs_msgs::ChassisMode::WIRELESS_CHARGING; + + if (now_wireless_charging_ && !before_wireless_charging_) { + charging_posture_high_ = is_posture_high_(); + apply_wireless_charging_posture_(charging_posture_high_); + } else if (!now_wireless_charging_ && before_wireless_charging_) { + apply_symmetric_posture_from_high_(charging_posture_high_); + } + + before_wireless_charging_ = now_wireless_charging_; + } + void update_mode_from_inputs_( rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, const rmcs_msgs::Keyboard& keyboard) { @@ -241,13 +342,22 @@ class DeformableChassisModeManager { remote_joint_posture_rotary_mode && rotary_knob_up_edge_(rotary_knob); const bool front_high_rear_low = !last_keyboard_.b && keyboard.b; const bool front_low_rear_high = !last_keyboard_.g && keyboard.g; + const bool posture_toggle_requested = + remote_posture_toggle_condition || keyboard_posture_toggle_condition; + + if (joint_posture_state_.mode == rmcs_msgs::ChassisMode::WIRELESS_CHARGING) { + if (posture_toggle_requested) + apply_wireless_charging_posture_(!charging_posture_high_); + else + apply_wireless_charging_posture_(charging_posture_high_); + + last_rotary_knob_ = rotary_knob; + return; + } if (apply_symmetric_target_) joint_current_target_angle_.fill(current_target_angle_); - const bool posture_toggle_requested = - remote_posture_toggle_condition || keyboard_posture_toggle_condition; - if (posture_toggle_requested) { if (joint_posture_state_.suspension_active) { active_suspension_base_angle_ = @@ -320,12 +430,17 @@ class DeformableChassisModeManager { double max_angle_; double active_suspension_base_angle_; bool suspension_enable_; + double wireless_charging_offset_deg_; + double wireless_charging_offset_rad_; + size_t charging_raised_leg_index_; double current_target_angle_; std::array joint_current_target_angle_; bool apply_symmetric_target_ = true; bool suspension_enabled_by_toggle_ = false; bool low_prone_enabled_by_toggle_ = false; + bool charging_posture_high_ = true; + bool before_wireless_charging_ = false; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp index 9291886ed..7e22197c0 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp @@ -203,6 +203,7 @@ class HeroChassisController } break; case rmcs_msgs::ChassisMode::ALIGNMENT: [[fallthrough]]; case rmcs_msgs::ChassisMode::ALIGNMENT_POWERED: [[fallthrough]]; + case rmcs_msgs::ChassisMode::WIRELESS_CHARGING: [[fallthrough]]; case rmcs_msgs::ChassisMode::CLIMB: [[fallthrough]]; case rmcs_msgs::ChassisMode::LAUNCH_RAMP: { double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp index b5e7640e1..a91679a40 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp @@ -145,6 +145,7 @@ class DeformableInfantry case rmcs_msgs::ChassisMode::SPIN_FAST: return Shape::Color::GREEN; case rmcs_msgs::ChassisMode::AUTO: return Shape::Color::CYAN; case rmcs_msgs::ChassisMode::STEP_DOWN: return Shape::Color::PINK; + case rmcs_msgs::ChassisMode::WIRELESS_CHARGING: return Shape::Color::BLACK; default: return Shape::Color::WHITE; } } From 1c5cc367cff7daf6d2e1d902d6a062da414b1b69 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Tue, 28 Jul 2026 16:47:04 +0800 Subject: [PATCH 63/86] feat(statue): fix field GameRobotHp --- rmcs_ws/src/rmcs_core/src/referee/status/field.hpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp index fc7efa63a..ad5e21619 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp @@ -16,10 +16,12 @@ struct __attribute__((packed)) GameRobotHp { uint16_t ally_2_robot_hp; uint16_t ally_3_robot_hp; uint16_t ally_4_robot_hp; - uint16_t reserved; + int16_t damage_difference; uint16_t ally_7_robot_hp; uint16_t ally_outpost_hp; uint16_t ally_base_hp; + uint16_t enemy_outpost_hp; + uint16_t enemy_base_hp; }; struct __attribute__((packed)) EventData { @@ -63,6 +65,7 @@ struct __attribute__((packed)) RobotStatus { std::uint8_t power_management_shooter_output : 1 = 0; std::uint8_t reserved : 5 = 0; }; +static_assert(sizeof(RobotStatus) == 17); struct __attribute__((packed)) PowerHeatData { uint16_t reserved_1; From 7d01e58b18dea2aa04456c5cae80c0e5fb901029 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Fri, 31 Jul 2026 02:10:13 +0800 Subject: [PATCH 64/86] feat(chassis): update spin recovery logic and enhance climbing behavior --- .../config/deformable-infantry-omni-b.yaml | 10 +--------- .../rmcs_bringup/config/deformable-infantry-omni.yaml | 10 ++-------- .../src/controller/chassis/chassis_controller.cpp | 3 ++- 3 files changed, 5 insertions(+), 18 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 70ca8417f..e50c12998 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -10,7 +10,6 @@ rmcs_executor: - rmcs_core::referee::command::Interaction -> referee_interaction - rmcs_core::referee::command::interaction::Ui -> referee_ui - rmcs_core::referee::app::ui::DeformableInfantry -> referee_ui_infantry - - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui - rmcs_core::controller::gimbal::DeformableInfantryGimbalController -> gimbal_controller @@ -86,7 +85,6 @@ auto_aim_component: degraded_angle_speed: 12.0 window_redundancy: 0.8 window_hysteresis: 0.2 - is_lazy_gimbal: false attack_preaim: false require_stable_command: false yaw_tolerance: 0.07 @@ -99,12 +97,6 @@ auto_aim_ui: offset_x: 0.0 offset_y: +0.08 offset_z: 0.0 - warn_distance_m: 2.0 - -referee_ui_infantry: - ros__parameters: - crosshair_offset_x: -2 - crosshair_offset_y: -30 deformable_infantry: ros__parameters: @@ -183,7 +175,7 @@ gimbal_controller: yaw_angle_ki: 0.0 yaw_angle_kd: 0.0 - yaw_velocity_kp: 13.0 + yaw_velocity_kp: 15.0 yaw_velocity_ki: 0.0 yaw_velocity_kd: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index d49c24e15..cd2a63dd3 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -64,7 +64,7 @@ auto_aim_capturer: framerate: 120.0 invert_image: false rls_tau_sec: 10.0 - use_hardware_sync: false + use_hardware_sync: true delay_ms: 6.5 auto_aim_component: @@ -97,12 +97,6 @@ auto_aim_ui: offset_x: 0.0 offset_y: -0.08 offset_z: 0.0 - warn_distance_m: 2.0 - -referee_ui_infantry: - ros__parameters: - crosshair_offset_x: -2 - crosshair_offset_y: -30 deformable_infantry: ros__parameters: @@ -181,7 +175,7 @@ gimbal_controller: yaw_angle_ki: 0.0 yaw_angle_kd: 0.0 - yaw_velocity_kp: 13.0 + yaw_velocity_kp: 10.0 yaw_velocity_ki: 0.0 yaw_velocity_kd: 0.0 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp index 63e5698dc..c4e220d01 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp @@ -259,6 +259,7 @@ class ChassisController // @NOTE: Align With 4 Sides case ChassisMode::ALIGNMENT_POWERED: [[fallthrough]]; + case ChassisMode::WIRELESS_CHARGING: [[fallthrough]]; case ChassisMode::ALIGNMENT: { const auto speed = chassis_control_velocity_->vector.head<2>(); const auto line1 = Eigen::Vector2d{speed.x(), 0}; @@ -299,7 +300,7 @@ class ChassisController angular_velocity = following_velocity_controller_.update(err); } break; - case ChassisMode::WIRELESS_CHARGING: [[fallthrough]]; + case ChassisMode::CLIMB: { chassis_control_angle = *chassis_climb_direction_; From 491f75175e6de55861958a15f72310582c52e9fd Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Fri, 31 Jul 2026 08:55:00 +0800 Subject: [PATCH 65/86] chore(deformable): adjust auto aim parameters for improved performance --- .../config/deformable-infantry-omni-b.yaml | 11 +++++----- .../config/deformable-infantry-omni-c.yaml | 22 ++++++------------- .../config/deformable-infantry-omni.yaml | 7 +++--- 3 files changed, 15 insertions(+), 25 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index e50c12998..5eec04577 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -60,7 +60,7 @@ auto_aim_capturer: ros__parameters: camera_name: "" exposure_us: 4000.0 - gain: 8.0 + gain: 10.0 framerate: 120.0 invert_image: false rls_tau_sec: 10.0 @@ -74,13 +74,12 @@ auto_aim_component: # 留空或填 unknow 表示禁用。 dangerous_fallback: "" manual_shoot: true - enable_rune: true camera_translation: [0.058, -0.08, 0.0] fire_control: bullet_speed: 22.5 shoot_delay: 0.04 - offset_yaw: +2.5 - offset_pitch: +0.5 + offset_yaw: +2.4 + offset_pitch: +0.3 attack_window: 80.0 degraded_angle_speed: 12.0 window_redundancy: 0.8 @@ -171,11 +170,11 @@ gimbal_controller: lower_limit: 0.10 # 8 deg ctrl_hold_pitch_target_angle: 0.0 - yaw_angle_kp: 15.0 + yaw_angle_kp: 10.0 yaw_angle_ki: 0.0 yaw_angle_kd: 0.0 - yaw_velocity_kp: 15.0 + yaw_velocity_kp: 13.0 yaw_velocity_ki: 0.0 yaw_velocity_kd: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml index 2a5d4dedf..d342b6266 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml @@ -10,7 +10,6 @@ rmcs_executor: - rmcs_core::referee::command::Interaction -> referee_interaction - rmcs_core::referee::command::interaction::Ui -> referee_ui - rmcs_core::referee::app::ui::DeformableInfantry -> referee_ui_infantry - # - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui - rmcs_core::controller::gimbal::DeformableInfantryGimbalController -> gimbal_controller @@ -31,9 +30,9 @@ rmcs_executor: - rmcs_core::controller::chassis::DeformableJointController -> rb_joint_controller - rmcs_core::controller::chassis::DeformableJointController -> rf_joint_controller - # - rmcs::AutoAimComponent -> auto_aim_component - # - rmcs::AutoAimCapturerComponent -> auto_aim_capturer - # - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui + - rmcs::AutoAimComponent -> auto_aim_component + - rmcs::AutoAimCapturerComponent -> auto_aim_capturer + - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster # - rmcs_core::debug::ValueCollector -> value_collector @@ -61,7 +60,7 @@ auto_aim_capturer: ros__parameters: camera_name: "" exposure_us: 4000.0 - gain: 8.0 + gain: 10.0 framerate: 120.0 invert_image: false rls_tau_sec: 10.0 @@ -79,12 +78,11 @@ auto_aim_component: fire_control: bullet_speed: 22.5 shoot_delay: 0.04 - offset_yaw: +2.5 - offset_pitch: +0.5 + offset_yaw: +1.0 + offset_pitch: -0.5 attack_window: 80.0 degraded_angle_speed: 12.0 window_hysteresis: 0.2 - is_lazy_gimbal: false attack_preaim: false require_stable_command: false yaw_tolerance: 0.07 @@ -93,14 +91,8 @@ auto_aim_component: auto_aim_ui: ros__parameters: offset_x: 0.0 - offset_y: +0.08 + offset_y: 0.0 offset_z: 0.0 - warn_distance_m: 2.0 - -referee_ui_infantry: - ros__parameters: - crosshair_offset_x: -2 - crosshair_offset_y: -30 deformable_infantry: ros__parameters: diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index cd2a63dd3..92038625e 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -60,11 +60,11 @@ auto_aim_capturer: ros__parameters: camera_name: "" exposure_us: 4000.0 - gain: 8.0 + gain: 10.0 framerate: 120.0 invert_image: false rls_tau_sec: 10.0 - use_hardware_sync: true + use_hardware_sync: false delay_ms: 6.5 auto_aim_component: @@ -74,7 +74,6 @@ auto_aim_component: # 留空或填 unknow 表示禁用。 dangerous_fallback: "" manual_shoot: true - enable_rune: true camera_translation: [0.058, -0.08, 0.0] fire_control: bullet_speed: 22.5 @@ -175,7 +174,7 @@ gimbal_controller: yaw_angle_ki: 0.0 yaw_angle_kd: 0.0 - yaw_velocity_kp: 10.0 + yaw_velocity_kp: 13.0 yaw_velocity_ki: 0.0 yaw_velocity_kd: 0.0 From f8dabd66d0b8f219104cc514bd4fb31bd8332fbb Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Sat, 1 Aug 2026 03:45:22 +0800 Subject: [PATCH 66/86] fix(yaml): adjust auto aim component pitch --- rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml | 2 +- rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 5eec04577..81bab0e82 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -79,7 +79,7 @@ auto_aim_component: bullet_speed: 22.5 shoot_delay: 0.04 offset_yaw: +2.4 - offset_pitch: +0.3 + offset_pitch: +0.1 attack_window: 80.0 degraded_angle_speed: 12.0 window_redundancy: 0.8 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml index d342b6266..1cb13f9b4 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml @@ -91,7 +91,7 @@ auto_aim_component: auto_aim_ui: ros__parameters: offset_x: 0.0 - offset_y: 0.0 + offset_y: -0.08 offset_z: 0.0 deformable_infantry: From d987ef5b53d2e318e11e6808013f07e45df41f7f Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Sat, 1 Aug 2026 23:33:15 +0800 Subject: [PATCH 67/86] feat(chassis): enhance wireless charging parameters and logic --- .../config/deformable-infantry-omni-b.yaml | 7 ++- .../config/deformable-infantry-omni-c.yaml | 7 ++- .../config/deformable-infantry-omni.yaml | 7 ++- .../controller/chassis/deformable_chassis.cpp | 14 ++++- .../controller/chassis/deformable_mode.hpp | 57 +++++++++++++------ 5 files changed, 70 insertions(+), 22 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 81bab0e82..2679c91a3 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -111,11 +111,16 @@ deformable_infantry: chassis_controller: ros__parameters: - # Deploy geometry / chassis-owned joint intent min_angle: 4.0 max_angle: 59.0 active_suspension_enable: true wireless_charging_offset_deg: 225.0 + charging_high_support_deg: 59.0 + charging_high_raised_deg: 25.0 + charging_low_support_deg: 23.0 + charging_low_raised_deg: 8.0 + wireless_charging_speed_limit: 0.6 + wireless_charging_angular_velocity_limit: 10.0 deformable_suspension: ros__parameters: diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml index 1cb13f9b4..a364bc1bb 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml @@ -108,11 +108,16 @@ deformable_infantry: chassis_controller: ros__parameters: - # Deploy geometry / chassis-owned joint intent min_angle: 4.0 max_angle: 59.0 active_suspension_enable: true wireless_charging_offset_deg: 135.0 + charging_high_support_deg: 59.0 + charging_high_raised_deg: 35.0 + charging_low_support_deg: 20.5 + charging_low_raised_deg: 8.0 + wireless_charging_speed_limit: 0.6 + wireless_charging_angular_velocity_limit: 10.0 deformable_suspension: ros__parameters: diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 92038625e..668663cd6 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -111,11 +111,16 @@ deformable_infantry: chassis_controller: ros__parameters: - # Deploy geometry / chassis-owned joint intent min_angle: 4.0 max_angle: 59.0 active_suspension_enable: true wireless_charging_offset_deg: 45.0 + charging_high_support_deg: 59.0 + charging_high_raised_deg: 33.0 + charging_low_support_deg: 22.5 + charging_low_raised_deg: 8.0 + wireless_charging_speed_limit: 0.6 + wireless_charging_angular_velocity_limit: 10.0 deformable_suspension: ros__parameters: diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index 2f424b14b..3edec1aeb 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -30,6 +30,9 @@ class DeformableChassis get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) , following_velocity_controller_(10.0, 0.0, 0.0) + , wireless_charging_speed_limit_(get_parameter_or("wireless_charging_speed_limit", 0.2)) + , wireless_charging_angular_velocity_limit_( + get_parameter_or("wireless_charging_angular_velocity_limit", 3.0)) , joint_mode_mgr_(*this) { following_velocity_controller_.output_max = angular_velocity_max_; @@ -174,7 +177,10 @@ class DeformableChassis if (translational_velocity.norm() > 1.0) translational_velocity.normalize(); - translational_velocity *= translational_velocity_max_; + const double max_speed = + *mode_ == rmcs_msgs::ChassisMode::WIRELESS_CHARGING ? wireless_charging_speed_limit_ + : translational_velocity_max_; + translational_velocity *= max_speed; return translational_velocity; } @@ -221,6 +227,9 @@ class DeformableChassis chassis_angle_error = normalize_signed_angle(chassis_angle_error); angular_velocity = following_velocity_controller_.update(chassis_angle_error); + angular_velocity = std::clamp( + angular_velocity, -wireless_charging_angular_velocity_limit_, + wireless_charging_angular_velocity_limit_); } break; default: break; @@ -302,6 +311,9 @@ class DeformableChassis pid::PidCalculator following_velocity_controller_; + double wireless_charging_speed_limit_; + double wireless_charging_angular_velocity_limit_; + DeformableChassisModeManager joint_mode_mgr_; }; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp index 90efa38ff..e38f545ff 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp @@ -45,7 +45,15 @@ class DeformableChassisModeManager { node.get_parameter("wireless_charging_offset_deg").as_double())) , wireless_charging_offset_rad_(deg_to_rad_(wireless_charging_offset_deg_)) , charging_raised_leg_index_( - select_charging_raised_leg_index_(wireless_charging_offset_deg_)) { + select_charging_raised_leg_index_(wireless_charging_offset_deg_)) + , charging_high_support_deg_( + node.get_parameter("charging_high_support_deg").as_double()) + , charging_high_raised_deg_( + node.get_parameter("charging_high_raised_deg").as_double()) + , charging_low_support_deg_( + node.get_parameter("charging_low_support_deg").as_double()) + , charging_low_raised_deg_( + node.get_parameter("charging_low_raised_deg").as_double()) { current_target_angle_ = max_angle_; joint_current_target_angle_.fill(max_angle_); update_joint_posture_state_(false); @@ -71,6 +79,7 @@ class DeformableChassisModeManager { low_prone_enabled_by_toggle_ = false; charging_posture_high_ = true; before_wireless_charging_ = false; + wireless_charging_legs_raised_ = false; last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; last_keyboard_ = rmcs_msgs::Keyboard::zero(); @@ -135,12 +144,6 @@ class DeformableChassisModeManager { static constexpr size_t kRightFront = 3; static constexpr size_t kJointCount = 4; - // Hardcoded wireless-charging postures: raised leg ~5cm higher (L=140mm). - static constexpr double kChargingHighSupportDeg = 59.0; - static constexpr double kChargingHighRaisedDeg = 35.0; - static constexpr double kChargingLowSupportDeg = 25.0; - static constexpr double kChargingLowRaisedDeg = 8.0; - // Leg azimuths in chassis frame (deg), counterclockwise from +X: LF → LB → RB → RF. static constexpr std::array kLegAzimuthDeg = { 45.0, @@ -204,8 +207,8 @@ class DeformableChassisModeManager { } void apply_wireless_charging_posture_(bool high) { - const double support_deg = high ? kChargingHighSupportDeg : kChargingLowSupportDeg; - const double raised_deg = high ? kChargingHighRaisedDeg : kChargingLowRaisedDeg; + const double support_deg = high ? charging_high_support_deg_ : charging_low_support_deg_; + const double raised_deg = high ? charging_high_raised_deg_ : charging_low_raised_deg_; const size_t diagonal_index = (charging_raised_leg_index_ + 2) % kJointCount; current_target_angle_ = support_deg; @@ -228,9 +231,10 @@ class DeformableChassisModeManager { if (now_wireless_charging_ && !before_wireless_charging_) { charging_posture_high_ = is_posture_high_(); - apply_wireless_charging_posture_(charging_posture_high_); + wireless_charging_legs_raised_ = false; } else if (!now_wireless_charging_ && before_wireless_charging_) { apply_symmetric_posture_from_high_(charging_posture_high_); + wireless_charging_legs_raised_ = false; } before_wireless_charging_ = now_wireless_charging_; @@ -305,12 +309,16 @@ class DeformableChassisModeManager { const bool remote_active_toggle_requested = remote_suspension_rotary_mode && rotary_knob_down_edge_(rotary_knob); + const bool wireless_charging_active = + joint_posture_state_.mode == rmcs_msgs::ChassisMode::WIRELESS_CHARGING; + const bool keyboard_active_suspension_toggle_requested = !last_keyboard_.e && keyboard.e; - if (keyboard_active_suspension_toggle_requested || remote_active_toggle_requested) + if (!wireless_charging_active + && (keyboard_active_suspension_toggle_requested || remote_active_toggle_requested)) suspension_enabled_by_toggle_ = !suspension_enabled_by_toggle_; const bool active_requested = - suspension_enable_ + suspension_enable_ && !wireless_charging_active && (joint_posture_state_.low_prone_active || suspension_enabled_by_toggle_); joint_posture_state_.suspension_mode = SuspensionMode::OFF; @@ -346,13 +354,21 @@ class DeformableChassisModeManager { remote_posture_toggle_condition || keyboard_posture_toggle_condition; if (joint_posture_state_.mode == rmcs_msgs::ChassisMode::WIRELESS_CHARGING) { - if (posture_toggle_requested) - apply_wireless_charging_posture_(!charging_posture_high_); - else - apply_wireless_charging_posture_(charging_posture_high_); + const bool charging_raise_toggle_requested = !last_keyboard_.e && keyboard.e; + if (charging_raise_toggle_requested) { + wireless_charging_legs_raised_ = !wireless_charging_legs_raised_; + if (!wireless_charging_legs_raised_) + apply_symmetric_posture_from_high_(charging_posture_high_); + } - last_rotary_knob_ = rotary_knob; - return; + if (wireless_charging_legs_raised_) { + if (posture_toggle_requested) + charging_posture_high_ = !charging_posture_high_; + + apply_wireless_charging_posture_(charging_posture_high_); + last_rotary_knob_ = rotary_knob; + return; + } } if (apply_symmetric_target_) @@ -433,6 +449,10 @@ class DeformableChassisModeManager { double wireless_charging_offset_deg_; double wireless_charging_offset_rad_; size_t charging_raised_leg_index_; + double charging_high_support_deg_; + double charging_high_raised_deg_; + double charging_low_support_deg_; + double charging_low_raised_deg_; double current_target_angle_; std::array joint_current_target_angle_; @@ -441,6 +461,7 @@ class DeformableChassisModeManager { bool low_prone_enabled_by_toggle_ = false; bool charging_posture_high_ = true; bool before_wireless_charging_ = false; + bool wireless_charging_legs_raised_ = false; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); From d62a08fd57f3e38e0e5999626938334ac3206ae5 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Sun, 2 Aug 2026 12:18:33 +0800 Subject: [PATCH 68/86] fix(suspension): adjust suspension parameters for improved performance --- rmcs_ws/src/hikcamera | 1 + .../config/deformable-infantry-omni-b.yaml | 12 +++++------- .../config/deformable-infantry-omni-c.yaml | 7 +++---- .../config/deformable-infantry-omni.yaml | 6 ++---- .../src/controller/chassis/deformable_suspension.cpp | 6 +++++- 5 files changed, 16 insertions(+), 16 deletions(-) create mode 160000 rmcs_ws/src/hikcamera diff --git a/rmcs_ws/src/hikcamera b/rmcs_ws/src/hikcamera new file mode 160000 index 000000000..f0077f034 --- /dev/null +++ b/rmcs_ws/src/hikcamera @@ -0,0 +1 @@ +Subproject commit f0077f034800bcd0dde4fffeff270b733772a57e diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 2679c91a3..633ef5250 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -116,7 +116,7 @@ chassis_controller: active_suspension_enable: true wireless_charging_offset_deg: 225.0 charging_high_support_deg: 59.0 - charging_high_raised_deg: 25.0 + charging_high_raised_deg: 23.0 charging_low_support_deg: 23.0 charging_low_raised_deg: 8.0 wireless_charging_speed_limit: 0.6 @@ -124,7 +124,6 @@ chassis_controller: deformable_suspension: ros__parameters: - # IMU attitude correction at min-angle stance. active_suspension_pitch_outer_kp: 12.0 active_suspension_pitch_outer_ki: 0.02 active_suspension_pitch_outer_kd: 0.0 @@ -157,15 +156,14 @@ deformable_suspension: active_suspension_roll_inner_output_min: -0.785 active_suspension_roll_inner_output_max: 0.785 - # Chassis-owned joint intent trajectory limits while attitude correction is active. active_suspension_target_velocity_limit_deg: 150.0 active_suspension_target_acceleration_limit_deg: 600.0 active_suspension_correction_velocity_limit_deg: 720.0 active_suspension_correction_acceleration_limit_deg: 3600.0 active_suspension_rate_lpf_cutoff_hz: 10.0 - # Automatic IMU mounting-error calibration. - # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. + active_suspension_target_pitch_deg: 0.0 + chassis_imu_calibration_wait_s: 2.0 chassis_imu_calibration_sample_s: 3.0 @@ -183,11 +181,11 @@ gimbal_controller: yaw_velocity_ki: 0.0 yaw_velocity_kd: 0.0 - pitch_angle_kp: 35.0 + pitch_angle_kp: 30.0 pitch_angle_ki: 0.02 pitch_angle_kd: 0.3 - pitch_velocity_kp: 2.0 + pitch_velocity_kp: 1.8 pitch_velocity_ki: 0.0 pitch_velocity_kd: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml index a364bc1bb..9e87259cd 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml @@ -82,6 +82,7 @@ auto_aim_component: offset_pitch: -0.5 attack_window: 80.0 degraded_angle_speed: 12.0 + window_redundancy: 0.8 window_hysteresis: 0.2 attack_preaim: false require_stable_command: false @@ -121,7 +122,6 @@ chassis_controller: deformable_suspension: ros__parameters: - # IMU attitude correction at min-angle stance. active_suspension_pitch_outer_kp: 12.0 active_suspension_pitch_outer_ki: 0.02 active_suspension_pitch_outer_kd: 0.0 @@ -154,15 +154,14 @@ deformable_suspension: active_suspension_roll_inner_output_min: -0.785 active_suspension_roll_inner_output_max: 0.785 - # Chassis-owned joint intent trajectory limits while attitude correction is active. active_suspension_target_velocity_limit_deg: 150.0 active_suspension_target_acceleration_limit_deg: 600.0 active_suspension_correction_velocity_limit_deg: 720.0 active_suspension_correction_acceleration_limit_deg: 3600.0 active_suspension_rate_lpf_cutoff_hz: 10.0 - # Automatic IMU mounting-error calibration. - # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. + active_suspension_target_pitch_deg: 0.0 + chassis_imu_calibration_wait_s: 2.0 chassis_imu_calibration_sample_s: 3.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 668663cd6..4f0027072 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -124,7 +124,6 @@ chassis_controller: deformable_suspension: ros__parameters: - # IMU attitude correction at min-angle stance. active_suspension_pitch_outer_kp: 12.0 active_suspension_pitch_outer_ki: 0.02 active_suspension_pitch_outer_kd: 0.0 @@ -157,15 +156,14 @@ deformable_suspension: active_suspension_roll_inner_output_min: -0.785 active_suspension_roll_inner_output_max: 0.785 - # Chassis-owned joint intent trajectory limits while attitude correction is active. active_suspension_target_velocity_limit_deg: 150.0 active_suspension_target_acceleration_limit_deg: 600.0 active_suspension_correction_velocity_limit_deg: 720.0 active_suspension_correction_acceleration_limit_deg: 3600.0 active_suspension_rate_lpf_cutoff_hz: 10.0 - # Automatic IMU mounting-error calibration. - # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. + active_suspension_target_pitch_deg: 0.0 + chassis_imu_calibration_wait_s: 2.0 chassis_imu_calibration_sample_s: 3.0 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp index 6da008098..ee31f2205 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp @@ -211,6 +211,8 @@ class DeformableSuspension 1e-6); active_rate_lpf_cutoff_hz_ = std::max(get_parameter_or("active_suspension_rate_lpf_cutoff_hz", 10.0), 1e-6); + active_target_pitch_rad_ = + deg_to_rad_(get_parameter_or("active_suspension_target_pitch_deg", 0.0)); calibration_wait_time_ = std::max(get_parameter_or("chassis_imu_calibration_wait_s", 2.0), 0.0); @@ -451,7 +453,8 @@ class DeformableSuspension const double clamped_pitch = std::clamp(pitch, -max_attitude, max_attitude); const double clamped_roll = std::clamp(roll, -max_attitude, max_attitude); - const double pitch_outer = pitch_outer_pid_.update(-clamped_pitch); + const double pitch_outer = + pitch_outer_pid_.update(active_target_pitch_rad_ - clamped_pitch); const double roll_outer = roll_outer_pid_.update(clamped_roll); const double pitch_diff = pitch_inner_pid_.update(pitch_outer - pitch_rate); const double roll_diff = roll_inner_pid_.update(roll_outer + roll_rate); @@ -589,6 +592,7 @@ class DeformableSuspension double active_correction_vel_limit_ = 40.0; double active_correction_acc_limit_ = 200.0; double active_rate_lpf_cutoff_hz_ = 10.0; + double active_target_pitch_rad_ = 0.0; double active_rate_filter_sampling_hz_ = 0.0; double calibration_wait_time_ = 2.0; From 5565d12656400b4d7ff469f47607dcb9541541b0 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Mon, 3 Aug 2026 02:12:43 +0800 Subject: [PATCH 69/86] feat(friction_wheel): add rotary knob input and low mode functionality --- .../config/deformable-infantry-omni-b.yaml | 31 +++++++++--- .../config/deformable-infantry-omni-c.yaml | 27 ++++++++-- .../config/deformable-infantry-omni.yaml | 27 ++++++++-- .../shooting/friction_wheel_controller.cpp | 50 +++++++++++++++++-- 4 files changed, 114 insertions(+), 21 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 633ef5250..f440156a8 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -30,6 +30,7 @@ rmcs_executor: - rmcs_core::controller::chassis::DeformableJointController -> rb_joint_controller - rmcs_core::controller::chassis::DeformableJointController -> rf_joint_controller + - rmcs::AutoAimRecorderComponent -> auto_aim_recorder - rmcs::AutoAimComponent -> auto_aim_component - rmcs::AutoAimCapturerComponent -> auto_aim_capturer - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui @@ -56,6 +57,15 @@ value_broadcaster: - /gimbal/pitch/angle - /gimbal/pitch/velocity +auto_aim_recorder: + ros__parameters: + output_path: "/home/ubuntu/autoaim/recoder" + queue_depth: 16 + flush_every_n_frames: 64 + max_duration_seconds: 0 + max_videos_size_gb: 0.0 + auto_record: true + auto_aim_capturer: ros__parameters: camera_name: "" @@ -77,9 +87,9 @@ auto_aim_component: camera_translation: [0.058, -0.08, 0.0] fire_control: bullet_speed: 22.5 - shoot_delay: 0.04 + shoot_delay: 0.05 offset_yaw: +2.4 - offset_pitch: +0.1 + offset_pitch: +0.05 attack_window: 80.0 degraded_angle_speed: 12.0 window_redundancy: 0.8 @@ -88,7 +98,7 @@ auto_aim_component: require_stable_command: false yaw_tolerance: 0.07 pitch_tolerance: 0.04 - rune_idle_duration: 0.4 + rune_idle_duration: 0.7 rune_shoot_duration: 0.2 auto_aim_ui: @@ -178,12 +188,16 @@ gimbal_controller: yaw_angle_kd: 0.0 yaw_velocity_kp: 13.0 - yaw_velocity_ki: 0.0 + yaw_velocity_ki: 0.02 yaw_velocity_kd: 0.0 + yaw_velocity_integral_min: -5.0 + yaw_velocity_integral_max: 5.0 - pitch_angle_kp: 30.0 + pitch_angle_kp: 35.0 pitch_angle_ki: 0.02 - pitch_angle_kd: 0.3 + pitch_angle_kd: 0.0 + pitch_angle_integral_min: -0.5 + pitch_angle_integral_max: 0.5 pitch_velocity_kp: 1.8 pitch_velocity_ki: 0.0 @@ -202,6 +216,9 @@ friction_wheel_controller: friction_velocities: - 580.0 - 580.0 + friction_velocities_low_mode: + - 530.0 + - 530.0 friction_soft_start_stop_time: 1.0 heat_controller: @@ -212,7 +229,7 @@ heat_controller: bullet_feeder_controller: ros__parameters: bullets_per_feeder_turn: 8.0 - shot_frequency: 30.0 + shot_frequency: 15.0 safe_shot_frequency: 10.0 eject_frequency: 10.0 eject_time: 0.05 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml index 9e87259cd..e9fbdf385 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml @@ -30,6 +30,7 @@ rmcs_executor: - rmcs_core::controller::chassis::DeformableJointController -> rb_joint_controller - rmcs_core::controller::chassis::DeformableJointController -> rf_joint_controller + - rmcs::AutoAimRecorderComponent -> auto_aim_recorder - rmcs::AutoAimComponent -> auto_aim_component - rmcs::AutoAimCapturerComponent -> auto_aim_capturer - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui @@ -56,6 +57,15 @@ value_broadcaster: - /gimbal/pitch/angle - /gimbal/pitch/velocity +auto_aim_recorder: + ros__parameters: + output_path: "/home/ubuntu/autoaim/recoder" + queue_depth: 16 + flush_every_n_frames: 64 + max_duration_seconds: 0 + max_videos_size_gb: 0.0 + auto_record: true + auto_aim_capturer: ros__parameters: camera_name: "" @@ -77,9 +87,9 @@ auto_aim_component: camera_translation: [0.058, -0.08, 0.0] fire_control: bullet_speed: 22.5 - shoot_delay: 0.04 - offset_yaw: +1.0 - offset_pitch: -0.5 + shoot_delay: 0.05 + offset_yaw: +0.6 + offset_pitch: -0.2 attack_window: 80.0 degraded_angle_speed: 12.0 window_redundancy: 0.8 @@ -176,12 +186,16 @@ gimbal_controller: yaw_angle_kd: 0.0 yaw_velocity_kp: 13.0 - yaw_velocity_ki: 0.0 + yaw_velocity_ki: 0.02 yaw_velocity_kd: 0.0 + yaw_velocity_integral_min: -5.0 + yaw_velocity_integral_max: 5.0 pitch_angle_kp: 35.0 pitch_angle_ki: 0.02 pitch_angle_kd: 0.3 + pitch_angle_integral_min: -0.5 + pitch_angle_integral_max: 0.5 pitch_velocity_kp: 2.0 pitch_velocity_ki: 0.0 @@ -200,6 +214,9 @@ friction_wheel_controller: friction_velocities: - 580.0 - 580.0 + friction_velocities_low_mode: + - 530.0 + - 530.0 friction_soft_start_stop_time: 1.0 heat_controller: @@ -210,7 +227,7 @@ heat_controller: bullet_feeder_controller: ros__parameters: bullets_per_feeder_turn: 8.0 - shot_frequency: 30.0 + shot_frequency: 15.0 safe_shot_frequency: 10.0 eject_frequency: 10.0 eject_time: 0.05 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 4f0027072..7ad5a9da8 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -30,6 +30,7 @@ rmcs_executor: - rmcs_core::controller::chassis::DeformableJointController -> rb_joint_controller - rmcs_core::controller::chassis::DeformableJointController -> rf_joint_controller + - rmcs::AutoAimRecorderComponent -> auto_aim_recorder - rmcs::AutoAimComponent -> auto_aim_component - rmcs::AutoAimCapturerComponent -> auto_aim_capturer - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui @@ -56,6 +57,15 @@ value_broadcaster: - /gimbal/pitch/angle - /gimbal/pitch/velocity +auto_aim_recorder: + ros__parameters: + output_path: "/home/ubuntu/autoaim/recoder" + queue_depth: 16 + flush_every_n_frames: 64 + max_duration_seconds: 0 + max_videos_size_gb: 0.0 + auto_record: true + auto_aim_capturer: ros__parameters: camera_name: "" @@ -77,8 +87,8 @@ auto_aim_component: camera_translation: [0.058, -0.08, 0.0] fire_control: bullet_speed: 22.5 - shoot_delay: 0.04 - offset_yaw: +0.1 + shoot_delay: 0.05 + offset_yaw: +1.1 offset_pitch: -0.4 attack_window: 80.0 degraded_angle_speed: 12.0 @@ -88,7 +98,7 @@ auto_aim_component: require_stable_command: false yaw_tolerance: 0.07 pitch_tolerance: 0.04 - rune_idle_duration: 0.4 + rune_idle_duration: 0.7 rune_shoot_duration: 0.2 auto_aim_ui: @@ -178,12 +188,16 @@ gimbal_controller: yaw_angle_kd: 0.0 yaw_velocity_kp: 13.0 - yaw_velocity_ki: 0.0 + yaw_velocity_ki: 0.02 yaw_velocity_kd: 0.0 + yaw_velocity_integral_min: -5.0 + yaw_velocity_integral_max: 5.0 pitch_angle_kp: 35.0 pitch_angle_ki: 0.02 pitch_angle_kd: 0.3 + pitch_angle_integral_min: -0.5 + pitch_angle_integral_max: 0.5 pitch_velocity_kp: 2.0 pitch_velocity_ki: 0.0 @@ -202,6 +216,9 @@ friction_wheel_controller: friction_velocities: - 580.0 - 580.0 + friction_velocities_low_mode: + - 530.0 + - 530.0 friction_soft_start_stop_time: 1.0 heat_controller: @@ -212,7 +229,7 @@ heat_controller: bullet_feeder_controller: ros__parameters: bullets_per_feeder_turn: 8.0 - shot_frequency: 30.0 + shot_frequency: 15.0 safe_shot_frequency: 10.0 eject_frequency: 10.0 eject_time: 0.05 diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp index 76beb6868..92b1fdcde 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp @@ -31,6 +31,7 @@ class FrictionWheelController register_input("/remote/switch/right", switch_right_); register_input("/remote/switch/left", switch_left_); register_input("/remote/keyboard", keyboard_); + register_input("/remote/rotary_knob", rotary_knob_, false); auto friction_wheels = get_parameter("friction_wheels").as_string_array(); auto friction_working_velocities = get_parameter("friction_velocities").as_double_array(); @@ -48,6 +49,16 @@ class FrictionWheelController friction_working_velocity_outputs_ = std::make_unique[]>(friction_count_); friction_control_velocities_ = std::make_unique[]>(friction_count_); + if (has_parameter("friction_velocities_low_mode")) { + auto friction_working_velocities_low = + get_parameter("friction_velocities_low_mode").as_double_array(); + if (friction_working_velocities_low.size() == friction_count_) { + friction_working_velocities_low_ = std::make_unique(friction_count_); + for (size_t i = 0; i < friction_count_; i++) + friction_working_velocities_low_[i] = friction_working_velocities_low[i]; + low_mode_enabled_ = true; + } + } for (size_t i = 0; i < friction_count_; i++) { friction_working_velocities_[i] = friction_working_velocities[i]; register_input(friction_wheels[i] + "/velocity", friction_velocities_[i]); @@ -72,6 +83,13 @@ class FrictionWheelController const auto keyboard = *keyboard_; using namespace rmcs_msgs; + const bool knob_up = rotary_knob_.ready() && *rotary_knob_ <= -knob_edge_threshold_; + if (low_mode_enabled_ && switch_left == Switch::DOWN && switch_right == Switch::MIDDLE + && knob_up && !last_knob_up_) + toggle_low_mode(); + + last_knob_up_ = knob_up; + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { reset_all_controls(); @@ -102,6 +120,7 @@ class FrictionWheelController private: void reset_all_controls() { friction_enabled_ = false; + low_mode_active_ = false; last_primary_friction_velocity_ = nan_; primary_friction_velocity_decrease_integral_ = 0; @@ -114,9 +133,26 @@ class FrictionWheelController *friction_ready_ = *friction_jammed_ = *bullet_fired_ = false; } + void toggle_low_mode() { + low_mode_active_ = !low_mode_active_; + + if (!std::isnan(friction_soft_start_stop_percentage_)) { + double sum = 0.0; + for (size_t i = 0; i < friction_count_; i++) + sum += *friction_velocities_[i] / target_friction_velocity(i); + friction_soft_start_stop_percentage_ = + std::clamp(sum / static_cast(friction_count_), 0.0, 1.0); + } + } + + double target_friction_velocity(size_t i) const { + return low_mode_active_ ? friction_working_velocities_low_[i] + : friction_working_velocities_[i]; + } + void update_friction_working_velocity_outputs() { for (size_t i = 0; i < friction_count_; i++) - *friction_working_velocity_outputs_[i] = friction_working_velocities_[i]; + *friction_working_velocity_outputs_[i] = target_friction_velocity(i); } void update_friction_velocities() { @@ -124,7 +160,7 @@ class FrictionWheelController friction_soft_start_stop_percentage_ = 0.0; for (size_t i = 0; i < friction_count_; i++) friction_soft_start_stop_percentage_ += - *friction_velocities_[i] / friction_working_velocities_[i]; + *friction_velocities_[i] / target_friction_velocity(i); friction_soft_start_stop_percentage_ /= static_cast(friction_count_); } friction_soft_start_stop_percentage_ += @@ -134,7 +170,7 @@ class FrictionWheelController for (size_t i = 0; i < friction_count_; i++) *friction_control_velocities_[i] = - friction_soft_start_stop_percentage_ * friction_working_velocities_[i]; + friction_soft_start_stop_percentage_ * target_friction_velocity(i); } void update_friction_status() { @@ -181,7 +217,7 @@ class FrictionWheelController primary_friction_velocity_decrease_integral_ += differential; else { if (primary_friction_velocity_decrease_integral_ < -14.0 - && last_primary_friction_velocity_ < friction_working_velocities_[0] - 20.0) + && last_primary_friction_velocity_ < target_friction_velocity(0) - 20.0) fired = true; primary_friction_velocity_decrease_integral_ = 0; @@ -193,20 +229,26 @@ class FrictionWheelController } static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double knob_edge_threshold_ = 0.7; rclcpp::Logger logger_; InputInterface switch_right_; InputInterface switch_left_; InputInterface keyboard_; + InputInterface rotary_knob_; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + bool last_knob_up_ = false; size_t friction_count_; std::unique_ptr friction_working_velocities_; + std::unique_ptr friction_working_velocities_low_; + bool low_mode_enabled_ = false; + bool low_mode_active_ = false; std::unique_ptr[]> friction_velocities_; std::unique_ptr[]> friction_working_velocity_outputs_; From c9e695a608eb7dadffe9ced87d34e0b69be9cf6c Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Mon, 3 Aug 2026 14:33:06 +0800 Subject: [PATCH 70/86] Add ladar message transmit - Add referee 0x301 RobotInteractionData - Add vtm-link 0x310 CustomMsg --- .../hardware/deformable-infantry-omni-b.cpp | 15 +++- .../hardware/deformable-infantry-omni-c.cpp | 15 +++- .../src/hardware/deformable-infantry-omni.cpp | 15 +++- .../vtm-link/ladar_package_transmit.hpp | 86 +++++++++++++++++++ rmcs_ws/src/rmcs_core/src/referee/status.cpp | 31 +++++++ .../rmcs_core/src/referee/status/field.hpp | 7 ++ 6 files changed, 163 insertions(+), 6 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/vtm-link/ladar_package_transmit.hpp diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index 884ebf695..913672ef2 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -34,6 +34,7 @@ #include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" #include "hardware/device/vt13.hpp" +#include "hardware/vtm-link/ladar_package_transmit.hpp" #include "hardware/util/status_monitor.hpp" namespace rmcs_core::hardware { @@ -134,7 +135,15 @@ class DeformableInfantryOmniB .toRotationMatrix()}} , gimbal_pitch_motor_(status, command, "/gimbal/pitch") , gimbal_left_friction_(status, command, "/gimbal/left_friction") - , gimbal_right_friction_(status, command, "/gimbal/right_friction") { + , gimbal_right_friction_(status, command, "/gimbal/right_friction") + , ladar_transmit_( + command, std::chrono::milliseconds{200}, + [this](const std::byte* buffer, size_t size) { + board_->start_transmit().uart_transmit( + Spec::kUarts.kUart0, + {.uart_data = std::span{buffer, size}}); + }, + status_.get_logger()) { gimbal_pitch_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} @@ -204,7 +213,8 @@ class DeformableInfantryOmniB pitch_encoder_angle); } - void command_update() const { + void command_update() { + ladar_transmit_.command_update(); auto builder = board_->start_transmit(); { auto packet = gimbal_pitch_motor_.generate_torque_command(); @@ -308,6 +318,7 @@ class DeformableInfantryOmniB device::DjiMotor gimbal_right_friction_; StatusMonitor monitor_{}; + vtm::LadarPackageTransmit ladar_transmit_; std::unique_ptr board_; }; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp index 860c16c13..13949dfae 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp @@ -34,6 +34,7 @@ #include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" #include "hardware/device/vt13.hpp" +#include "hardware/vtm-link/ladar_package_transmit.hpp" #include "hardware/util/status_monitor.hpp" namespace rmcs_core::hardware { @@ -134,7 +135,15 @@ class DeformableInfantryOmniC .toRotationMatrix()}} , gimbal_pitch_motor_(status, command, "/gimbal/pitch") , gimbal_left_friction_(status, command, "/gimbal/left_friction") - , gimbal_right_friction_(status, command, "/gimbal/right_friction") { + , gimbal_right_friction_(status, command, "/gimbal/right_friction") + , ladar_transmit_( + command, std::chrono::milliseconds{200}, + [this](const std::byte* buffer, size_t size) { + board_->start_transmit().uart_transmit( + Spec::kUarts.kUart0, + {.uart_data = std::span{buffer, size}}); + }, + status_.get_logger()) { gimbal_pitch_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} @@ -205,7 +214,8 @@ class DeformableInfantryOmniC pitch_encoder_angle); } - void command_update() const { + void command_update() { + ladar_transmit_.command_update(); auto builder = board_->start_transmit(); { auto packet = gimbal_pitch_motor_.generate_torque_command(); @@ -309,6 +319,7 @@ class DeformableInfantryOmniC device::DjiMotor gimbal_right_friction_; StatusMonitor monitor_{}; + vtm::LadarPackageTransmit ladar_transmit_; std::unique_ptr board_; }; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index 557ba36ad..973896e65 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -34,6 +34,7 @@ #include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" #include "hardware/device/vt13.hpp" +#include "hardware/vtm-link/ladar_package_transmit.hpp" #include "hardware/util/status_monitor.hpp" namespace rmcs_core::hardware { @@ -134,7 +135,15 @@ class DeformableInfantryOmni .toRotationMatrix()}} , gimbal_pitch_motor_(status, command, "/gimbal/pitch") , gimbal_left_friction_(status, command, "/gimbal/left_friction") - , gimbal_right_friction_(status, command, "/gimbal/right_friction") { + , gimbal_right_friction_(status, command, "/gimbal/right_friction") + , ladar_transmit_( + command, std::chrono::milliseconds{200}, + [this](const std::byte* buffer, size_t size) { + board_->start_transmit().uart_transmit( + Spec::kUarts.kUart0, + {.uart_data = std::span{buffer, size}}); + }, + status_.get_logger()) { gimbal_pitch_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} @@ -205,7 +214,8 @@ class DeformableInfantryOmni pitch_encoder_angle); } - void command_update() const { + void command_update() { + ladar_transmit_.command_update(); auto builder = board_->start_transmit(); { auto packet = gimbal_pitch_motor_.generate_torque_command(); @@ -309,6 +319,7 @@ class DeformableInfantryOmni device::DjiMotor gimbal_right_friction_; StatusMonitor monitor_{}; + vtm::LadarPackageTransmit ladar_transmit_; std::unique_ptr board_; }; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/vtm-link/ladar_package_transmit.hpp b/rmcs_ws/src/rmcs_core/src/hardware/vtm-link/ladar_package_transmit.hpp new file mode 100644 index 000000000..4538ad2a0 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/vtm-link/ladar_package_transmit.hpp @@ -0,0 +1,86 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "referee/frame.hpp" + +namespace rmcs_core::hardware::vtm { + +class LadarPackageTransmit { +public: + using UartWriter = std::function; + using LidarMsgBroadcast = std::array; + + LadarPackageTransmit( + rmcs_executor::Component& component, std::chrono::milliseconds interval, + UartWriter uart_writer, rclcpp::Logger logger, + const std::string& lidar_msg_broadcast_name = + "/referee/multi_robot_communication/lidar_msg_broadcast") + : uart_writer_(std::move(uart_writer)) + , interval_(interval) + , logger_(std::move(logger)) { + component.register_input(lidar_msg_broadcast_name, lidar_msg_broadcast_, false); + } + + void command_update() { + if (!lidar_msg_broadcast_.ready()) + return; + + auto now = std::chrono::steady_clock::now(); + if (now < next_publish_time_) + return; + + publish_single_packet(*lidar_msg_broadcast_); + next_publish_time_ = now + interval_; + } + +private: + static constexpr uint16_t kLadarCmdId = 0x0310; + static constexpr size_t kLadarDataSize = 118; + static constexpr size_t kLadarPacketSize = 300; + + void publish_single_packet(const LidarMsgBroadcast& lidar_msg_broadcast) { + static constexpr size_t kHeaderSize = sizeof(referee::FrameHeader); + static constexpr size_t kCmdIdSize = sizeof(uint16_t); + static constexpr size_t kCrc16Size = sizeof(uint16_t); + static constexpr size_t kFrameSize = kHeaderSize + kCmdIdSize + kLadarPacketSize + + kCrc16Size; + + referee::Frame frame; + frame.header.sof = referee::sof_value; + frame.header.data_length = kLadarPacketSize; + frame.header.sequence = sequence_++; + frame.header.crc8 = 0; + frame.body.command_id = kLadarCmdId; + std::memcpy(frame.body.data, lidar_msg_broadcast.data(), kLadarDataSize); + std::memset( + frame.body.data + kLadarDataSize, 0, kLadarPacketSize - kLadarDataSize); + + rmcs_utility::dji_crc::append_crc8(frame.header); + rmcs_utility::dji_crc::append_crc16(&frame, kFrameSize); + + uart_writer_(reinterpret_cast(&frame), kFrameSize); + RCLCPP_DEBUG(logger_, "uart sent ladar packet"); + } + + rmcs_executor::Component::InputInterface lidar_msg_broadcast_; + UartWriter uart_writer_; + std::chrono::milliseconds interval_; + std::chrono::steady_clock::time_point next_publish_time_ = + std::chrono::steady_clock::time_point::min(); + uint8_t sequence_{0}; + rclcpp::Logger logger_; +}; + +} // namespace rmcs_core::hardware::vtm diff --git a/rmcs_ws/src/rmcs_core/src/referee/status.cpp b/rmcs_ws/src/rmcs_core/src/referee/status.cpp index c0a7db62e..3f11a3af6 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status.cpp @@ -1,3 +1,4 @@ +#include #include #include #include @@ -103,6 +104,9 @@ class Status register_output("/referee/map_command/event/source", map_command_event_source_, 0); register_output("/referee/map_command/event/timestamp", map_command_event_timestamp_, 0.0); register_output("/referee/map_command/event/sequence", map_command_event_sequence_, 0); + register_output( + "/referee/multi_robot_communication/lidar_msg_broadcast", lidar_msg_broadcast_, + LidarMsgBroadcast{}); robot_status_watchdog_.reset(5'000); } @@ -182,6 +186,8 @@ class Status update_sentry_info(); else if (command_id == 0x0303) update_map_command(); + else if (command_id == 0x0301) + update_robot_interaction_data(); } void update_game_status() { @@ -326,6 +332,28 @@ class Status *map_command_event_timestamp_ = *map_command_received_timestamp_; *map_command_event_sequence_ += 1; } + + void update_robot_interaction_data() { + if (frame_.header.data_length < sizeof(RobotInteractionData)) { + RCLCPP_WARN( + logger_, "Robot interaction data length invalid: %u", + static_cast(frame_.header.data_length)); + return; + } + + RobotInteractionData data; + std::memcpy(&data, frame_.body.data, sizeof(data)); + + if (data.sender_id != 9 && data.sender_id != 109) + return; + if (data.data_cmd_id < 0x0200 || data.data_cmd_id > 0x02ff) + return; + + LidarMsgBroadcast lidar_msg{}; + std::memcpy(lidar_msg.data(), data.user_data, lidar_msg.size()); + *lidar_msg_broadcast_ = lidar_msg; + } + // When referee system loses connection unexpectedly, // use these indicators make sure the robot safe. // Muzzle: Cooling priority with level 1 @@ -403,6 +431,9 @@ class Status OutputInterface map_command_event_sequence_; MapCommand last_map_command_{}; bool has_last_map_command_ = false; + + using LidarMsgBroadcast = std::array; + OutputInterface lidar_msg_broadcast_; }; } // namespace rmcs_core::referee diff --git a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp index ad5e21619..1f2f02821 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp @@ -158,4 +158,11 @@ struct __attribute__((packed)) SentryInfo { }; static_assert(sizeof(SentryInfo) == 14); +struct __attribute__((packed)) RobotInteractionData { + uint16_t data_cmd_id; + uint16_t sender_id; + uint16_t receiver_id; + uint8_t user_data[112]; +}; + } // namespace rmcs_core::referee::status From f0e97d96f68cca11c437c4fca06ecc141edb6581 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Tue, 4 Aug 2026 02:29:15 +0800 Subject: [PATCH 71/86] feat(deformable): simplify charging posture, add yaw packet logging - Remove high/low wireless charging posture selection - Add mouse wheel friction wheel speed adjustment (Ctrl+F) - Add CAN2 packet CSV logging (0x200/0x141/0x142) - Tune auto-aim parameters and expose debug_log_yaw_packet config --- .../config/deformable-infantry-omni-b.yaml | 32 +++-- .../config/deformable-infantry-omni-c.yaml | 29 ++-- .../config/deformable-infantry-omni.yaml | 22 +-- .../controller/chassis/deformable_mode.hpp | 113 ++-------------- .../shooting/friction_wheel_controller.cpp | 50 +++++++ .../hardware/deformable-infantry-omni-b.cpp | 126 ++++++++++++++++-- .../hardware/deformable-infantry-omni-c.cpp | 126 ++++++++++++++++-- .../src/hardware/deformable-infantry-omni.cpp | 126 ++++++++++++++++-- 8 files changed, 446 insertions(+), 178 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index f440156a8..9438cf42d 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -54,8 +54,8 @@ value_collector: value_broadcaster: ros__parameters: forward_list: - - /gimbal/pitch/angle - - /gimbal/pitch/velocity + - /gimbal/yaw/angle + - /gimbal/yaw/velocity auto_aim_recorder: ros__parameters: @@ -63,13 +63,13 @@ auto_aim_recorder: queue_depth: 16 flush_every_n_frames: 64 max_duration_seconds: 0 - max_videos_size_gb: 0.0 - auto_record: true + max_videos_size_gb: 200.0 + auto_record: false auto_aim_capturer: ros__parameters: camera_name: "" - exposure_us: 4000.0 + exposure_us: 2000.0 gain: 10.0 framerate: 120.0 invert_image: false @@ -83,21 +83,21 @@ auto_aim_component: # 将强行绑定为对应阵营哨兵，仅供调试，严禁比赛启用。 # 留空或填 unknow 表示禁用。 dangerous_fallback: "" - manual_shoot: true + manual_shoot: false camera_translation: [0.058, -0.08, 0.0] fire_control: bullet_speed: 22.5 - shoot_delay: 0.05 - offset_yaw: +2.4 - offset_pitch: +0.05 - attack_window: 80.0 + shoot_delay: 0.07 + offset_yaw: +2.0 + offset_pitch: -0.4 + attack_window: 120.0 degraded_angle_speed: 12.0 - window_redundancy: 0.8 + window_redundancy: 0.6 window_hysteresis: 0.2 attack_preaim: false require_stable_command: false - yaw_tolerance: 0.07 - pitch_tolerance: 0.04 + yaw_tolerance: 0.14 + pitch_tolerance: 0.08 rune_idle_duration: 0.7 rune_shoot_duration: 0.2 @@ -118,6 +118,8 @@ deformable_infantry: debug_log_supercap: false debug_log_wheel_motor: false debug_log_deformable_joint_motor: false + debug_log_yaw_packet: false + debug_log_yaw_packet_path: "/home/ubuntu/controller/yaw" chassis_controller: ros__parameters: @@ -125,10 +127,6 @@ chassis_controller: max_angle: 59.0 active_suspension_enable: true wireless_charging_offset_deg: 225.0 - charging_high_support_deg: 59.0 - charging_high_raised_deg: 23.0 - charging_low_support_deg: 23.0 - charging_low_raised_deg: 8.0 wireless_charging_speed_limit: 0.6 wireless_charging_angular_velocity_limit: 10.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml index e9fbdf385..d84c1fdff 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml @@ -63,13 +63,12 @@ auto_aim_recorder: queue_depth: 16 flush_every_n_frames: 64 max_duration_seconds: 0 - max_videos_size_gb: 0.0 - auto_record: true - + max_videos_size_gb: 200.0 + auto_record: false auto_aim_capturer: ros__parameters: camera_name: "" - exposure_us: 4000.0 + exposure_us: 2000.0 gain: 10.0 framerate: 120.0 invert_image: false @@ -83,21 +82,23 @@ auto_aim_component: # 将强行绑定为对应阵营哨兵，仅供调试，严禁比赛启用。 # 留空或填 unknow 表示禁用。 dangerous_fallback: "" - manual_shoot: true + manual_shoot: false camera_translation: [0.058, -0.08, 0.0] fire_control: bullet_speed: 22.5 - shoot_delay: 0.05 - offset_yaw: +0.6 + shoot_delay: 0.07 + offset_yaw: +1.05 offset_pitch: -0.2 - attack_window: 80.0 + attack_window: 120.0 degraded_angle_speed: 12.0 - window_redundancy: 0.8 + window_redundancy: 0.6 window_hysteresis: 0.2 attack_preaim: false require_stable_command: false - yaw_tolerance: 0.07 - pitch_tolerance: 0.04 + yaw_tolerance: 0.14 + pitch_tolerance: 0.08 + rune_idle_duration: 0.7 + rune_shoot_duration: 0.2 auto_aim_ui: ros__parameters: @@ -116,6 +117,8 @@ deformable_infantry: debug_log_supercap: false debug_log_wheel_motor: false debug_log_deformable_joint_motor: false + debug_log_yaw_packet: false + debug_log_yaw_packet_path: "/home/ubuntu/controller/yaw" chassis_controller: ros__parameters: @@ -123,10 +126,6 @@ chassis_controller: max_angle: 59.0 active_suspension_enable: true wireless_charging_offset_deg: 135.0 - charging_high_support_deg: 59.0 - charging_high_raised_deg: 35.0 - charging_low_support_deg: 20.5 - charging_low_raised_deg: 8.0 wireless_charging_speed_limit: 0.6 wireless_charging_angular_velocity_limit: 10.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 7ad5a9da8..9413733f5 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -63,13 +63,13 @@ auto_aim_recorder: queue_depth: 16 flush_every_n_frames: 64 max_duration_seconds: 0 - max_videos_size_gb: 0.0 - auto_record: true + max_videos_size_gb: 200.0 + auto_record: false auto_aim_capturer: ros__parameters: camera_name: "" - exposure_us: 4000.0 + exposure_us: 2000.0 gain: 10.0 framerate: 120.0 invert_image: false @@ -83,21 +83,21 @@ auto_aim_component: # 将强行绑定为对应阵营哨兵，仅供调试，严禁比赛启用。 # 留空或填 unknow 表示禁用。 dangerous_fallback: "" - manual_shoot: true + manual_shoot: false camera_translation: [0.058, -0.08, 0.0] fire_control: bullet_speed: 22.5 - shoot_delay: 0.05 - offset_yaw: +1.1 - offset_pitch: -0.4 - attack_window: 80.0 + shoot_delay: 0.07 + offset_yaw: +1.05 + offset_pitch: -0.2 + attack_window: 120.0 degraded_angle_speed: 12.0 - window_redundancy: 0.8 + window_redundancy: 0.6 window_hysteresis: 0.2 attack_preaim: false require_stable_command: false - yaw_tolerance: 0.07 - pitch_tolerance: 0.04 + yaw_tolerance: 0.14 + pitch_tolerance: 0.08 rune_idle_duration: 0.7 rune_shoot_duration: 0.2 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp index e38f545ff..8aeeb7193 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp @@ -43,17 +43,7 @@ class DeformableChassisModeManager { , suspension_enable_(node.get_parameter_or("active_suspension_enable", false)) , wireless_charging_offset_deg_(normalize_ccw_deg_( node.get_parameter("wireless_charging_offset_deg").as_double())) - , wireless_charging_offset_rad_(deg_to_rad_(wireless_charging_offset_deg_)) - , charging_raised_leg_index_( - select_charging_raised_leg_index_(wireless_charging_offset_deg_)) - , charging_high_support_deg_( - node.get_parameter("charging_high_support_deg").as_double()) - , charging_high_raised_deg_( - node.get_parameter("charging_high_raised_deg").as_double()) - , charging_low_support_deg_( - node.get_parameter("charging_low_support_deg").as_double()) - , charging_low_raised_deg_( - node.get_parameter("charging_low_raised_deg").as_double()) { + , wireless_charging_offset_rad_(deg_to_rad_(wireless_charging_offset_deg_)) { current_target_angle_ = max_angle_; joint_current_target_angle_.fill(max_angle_); update_joint_posture_state_(false); @@ -77,9 +67,7 @@ class DeformableChassisModeManager { apply_symmetric_target_ = true; suspension_enabled_by_toggle_ = false; low_prone_enabled_by_toggle_ = false; - charging_posture_high_ = true; before_wireless_charging_ = false; - wireless_charging_legs_raised_ = false; last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; last_keyboard_ = rmcs_msgs::Keyboard::zero(); @@ -144,14 +132,6 @@ class DeformableChassisModeManager { static constexpr size_t kRightFront = 3; static constexpr size_t kJointCount = 4; - // Leg azimuths in chassis frame (deg), counterclockwise from +X: LF → LB → RB → RF. - static constexpr std::array kLegAzimuthDeg = { - 45.0, - 135.0, - 225.0, - 315.0, - }; - static double deg_to_rad_(double deg) { return deg * std::numbers::pi / 180.0; } static double normalize_ccw_deg_(double deg) { @@ -161,28 +141,6 @@ class DeformableChassisModeManager { return normalized; } - static double absolute_angle_distance_deg_(double a, double b) { - double delta = std::abs(normalize_ccw_deg_(a) - normalize_ccw_deg_(b)); - if (delta > 180.0) - delta = 360.0 - delta; - return delta; - } - - static size_t select_charging_raised_leg_index_(double wireless_charging_offset_deg) { - const double offset_ccw = normalize_ccw_deg_(wireless_charging_offset_deg); - size_t best_index = kLeftFront; - double best_distance = absolute_angle_distance_deg_(offset_ccw, kLegAzimuthDeg[kLeftFront]); - for (size_t i = 1; i < kJointCount; ++i) { - const double distance = - absolute_angle_distance_deg_(offset_ccw, kLegAzimuthDeg[i]); - if (distance < best_distance) { - best_distance = distance; - best_index = i; - } - } - return best_index; - } - static bool symmetric_joint_target_requested_(const std::array& joint_target_deg) { constexpr double epsilon = 1e-6; @@ -191,50 +149,20 @@ class DeformableChassisModeManager { }); } - double posture_reference_angle_deg_() const { - if (apply_symmetric_target_) - return current_target_angle_; - - double sum = 0.0; - for (double angle_deg : joint_current_target_angle_) - sum += angle_deg; - return sum / static_cast(kJointCount); - } - - bool is_posture_high_() const { - const double midpoint = (min_angle_ + max_angle_) / 2.0; - return posture_reference_angle_deg_() >= midpoint; - } - - void apply_wireless_charging_posture_(bool high) { - const double support_deg = high ? charging_high_support_deg_ : charging_low_support_deg_; - const double raised_deg = high ? charging_high_raised_deg_ : charging_low_raised_deg_; - const size_t diagonal_index = (charging_raised_leg_index_ + 2) % kJointCount; - - current_target_angle_ = support_deg; - apply_symmetric_target_ = false; - joint_current_target_angle_.fill(support_deg); - joint_current_target_angle_[charging_raised_leg_index_] = raised_deg; - joint_current_target_angle_[diagonal_index] = raised_deg; - charging_posture_high_ = high; - } - - void apply_symmetric_posture_from_high_(bool high) { - current_target_angle_ = high ? max_angle_ : min_angle_; + void apply_wireless_charging_posture_() { + current_target_angle_ = min_angle_; apply_symmetric_target_ = true; - joint_current_target_angle_.fill(current_target_angle_); + joint_current_target_angle_.fill(min_angle_); } void update_wireless_charging_posture_transition_() { const bool now_wireless_charging_ = joint_posture_state_.mode == rmcs_msgs::ChassisMode::WIRELESS_CHARGING; - if (now_wireless_charging_ && !before_wireless_charging_) { - charging_posture_high_ = is_posture_high_(); - wireless_charging_legs_raised_ = false; - } else if (!now_wireless_charging_ && before_wireless_charging_) { - apply_symmetric_posture_from_high_(charging_posture_high_); - wireless_charging_legs_raised_ = false; + if (!now_wireless_charging_ && before_wireless_charging_) { + current_target_angle_ = max_angle_; + apply_symmetric_target_ = true; + joint_current_target_angle_.fill(max_angle_); } before_wireless_charging_ = now_wireless_charging_; @@ -354,21 +282,9 @@ class DeformableChassisModeManager { remote_posture_toggle_condition || keyboard_posture_toggle_condition; if (joint_posture_state_.mode == rmcs_msgs::ChassisMode::WIRELESS_CHARGING) { - const bool charging_raise_toggle_requested = !last_keyboard_.e && keyboard.e; - if (charging_raise_toggle_requested) { - wireless_charging_legs_raised_ = !wireless_charging_legs_raised_; - if (!wireless_charging_legs_raised_) - apply_symmetric_posture_from_high_(charging_posture_high_); - } - - if (wireless_charging_legs_raised_) { - if (posture_toggle_requested) - charging_posture_high_ = !charging_posture_high_; - - apply_wireless_charging_posture_(charging_posture_high_); - last_rotary_knob_ = rotary_knob; - return; - } + apply_wireless_charging_posture_(); + last_rotary_knob_ = rotary_knob; + return; } if (apply_symmetric_target_) @@ -448,20 +364,13 @@ class DeformableChassisModeManager { bool suspension_enable_; double wireless_charging_offset_deg_; double wireless_charging_offset_rad_; - size_t charging_raised_leg_index_; - double charging_high_support_deg_; - double charging_high_raised_deg_; - double charging_low_support_deg_; - double charging_low_raised_deg_; double current_target_angle_; std::array joint_current_target_angle_; bool apply_symmetric_target_ = true; bool suspension_enabled_by_toggle_ = false; bool low_prone_enabled_by_toggle_ = false; - bool charging_posture_high_ = true; bool before_wireless_charging_ = false; - bool wireless_charging_legs_raised_ = false; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp index 92b1fdcde..927886e4c 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp @@ -31,6 +31,7 @@ class FrictionWheelController register_input("/remote/switch/right", switch_right_); register_input("/remote/switch/left", switch_left_); register_input("/remote/keyboard", keyboard_); + register_input("/remote/mouse/mouse_wheel", mouse_wheel_); register_input("/remote/rotary_knob", rotary_knob_, false); auto friction_wheels = get_parameter("friction_wheels").as_string_array(); @@ -69,6 +70,15 @@ class FrictionWheelController friction_wheels[i] + "/control_velocity", friction_control_velocities_[i], nan_); } + friction_velocity_max_ = std::make_unique(friction_count_); + friction_velocity_min_ = std::make_unique(friction_count_); + for (size_t i = 0; i < friction_count_; i++) { + friction_velocity_max_[i] = friction_working_velocities_[i]; + friction_velocity_min_[i] = + low_mode_enabled_ ? friction_working_velocities_low_[i] + : friction_working_velocities_[i]; + } + friction_soft_start_stop_step_ = (1 / 1000.0) / get_parameter("friction_soft_start_stop_time").as_double(); @@ -97,6 +107,13 @@ class FrictionWheelController } if (switch_right != Switch::DOWN) { + if (keyboard.ctrl && keyboard.f) + update_friction_speed_by_mouse_wheel(); + else { + wheel_accumulator_ = 0.0; + wheel_tick_pending_ = false; + } + update_friction_working_velocity_outputs(); if ((!last_keyboard_.v && keyboard.v) @@ -145,6 +162,30 @@ class FrictionWheelController } } + void update_friction_speed_by_mouse_wheel() { + wheel_accumulator_ += *mouse_wheel_; + if (std::abs(*mouse_wheel_) < wheel_rest_threshold_) { + wheel_accumulator_ = 0.0; + wheel_tick_pending_ = false; + } + if (wheel_tick_pending_) + return; + if (wheel_accumulator_ > wheel_tick_threshold_) { + wheel_tick_pending_ = true; + adjust_friction_speed(friction_speed_adjust_step_); + } else if (wheel_accumulator_ < -wheel_tick_threshold_) { + wheel_tick_pending_ = true; + adjust_friction_speed(-friction_speed_adjust_step_); + } + } + + void adjust_friction_speed(double delta) { + for (size_t i = 0; i < friction_count_; i++) + friction_working_velocities_[i] = std::clamp( + friction_working_velocities_[i] + delta, friction_velocity_min_[i], + friction_velocity_max_[i]); + } + double target_friction_velocity(size_t i) const { return low_mode_active_ ? friction_working_velocities_low_[i] : friction_working_velocities_[i]; @@ -230,12 +271,16 @@ class FrictionWheelController static constexpr double nan_ = std::numeric_limits::quiet_NaN(); static constexpr double knob_edge_threshold_ = 0.7; + static constexpr double friction_speed_adjust_step_ = 5.0; + static constexpr double wheel_tick_threshold_ = 0.005; + static constexpr double wheel_rest_threshold_ = 1e-6; rclcpp::Logger logger_; InputInterface switch_right_; InputInterface switch_left_; InputInterface keyboard_; + InputInterface mouse_wheel_; InputInterface rotary_knob_; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; @@ -247,9 +292,14 @@ class FrictionWheelController std::unique_ptr friction_working_velocities_; std::unique_ptr friction_working_velocities_low_; + std::unique_ptr friction_velocity_min_; + std::unique_ptr friction_velocity_max_; bool low_mode_enabled_ = false; bool low_mode_active_ = false; + double wheel_accumulator_ = 0.0; + bool wheel_tick_pending_ = false; + std::unique_ptr[]> friction_velocities_; std::unique_ptr[]> friction_working_velocity_outputs_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index 913672ef2..af52e0ff4 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -4,10 +4,15 @@ #include #include #include +#include +#include +#include #include #include +#include #include #include +#include #include #include #include @@ -395,6 +400,12 @@ class DeformableInfantryOmniB status.get_parameter_or("debug_log_wheel_motor", debug_log_wheel_motor_, false); status.get_parameter_or( "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); + status.get_parameter_or("debug_log_yaw_packet", debug_log_yaw_packet_, false); + status.get_parameter_or( + "debug_log_yaw_packet_path", debug_log_yaw_packet_path_, + std::string{"/home/ubuntu/controller/yaw"}); + if (debug_log_yaw_packet_) + open_yaw_packet_log_files_(); auto options = librmcs::board::AdvancedOptions{}; options.dangerously_skip_version_checks = true; @@ -480,19 +491,19 @@ class DeformableInfantryOmniB } .as_bytes(), }); + auto packet_can2_200 = device::CanPacket8{ + chassis_wheel_motors_[kRightBack].generate_command(), + device::CanPacket8::PaddingQuarter{}, + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + }; builder.can_transmit( Spec::kCans.kCan2, // { .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[kRightBack].generate_command(), - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), + .can_data = packet_can2_200.as_bytes(), }); + log_can2_packet_(true, 0x200, packet_can2_200.as_bytes()); builder.can_transmit( Spec::kCans.kCan3, // { @@ -506,12 +517,14 @@ class DeformableInfantryOmniB } .as_bytes(), }); + auto packet_can2_142 = gimbal_yaw_motor_.generate_command(); builder.can_transmit( Spec::kCans.kCan2, // { .can_id = 0x142, - .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), + .can_data = packet_can2_142.as_bytes(), }); + log_can2_packet_(true, 0x142, packet_can2_142.as_bytes()); builder.can_transmit( Spec::kCans.kCan1, // { @@ -544,14 +557,17 @@ class DeformableInfantryOmniB .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), }); break; - case kRightBack: + case kRightBack: { + auto packet = chassis_joint_motors_[i].generate_command(); builder.can_transmit( Spec::kCans.kCan2, // { .can_id = 0x141, - .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + .can_data = packet.as_bytes(), }); + log_can2_packet_(true, 0x141, packet.as_bytes()); break; + } case kRightFront: builder.can_transmit( Spec::kCans.kCan3, // @@ -602,6 +618,14 @@ class DeformableInfantryOmniB bool debug_log_wheel_motor_ = false; bool debug_log_deformable_joint_motor_ = false; + bool debug_log_yaw_packet_ = false; + std::string debug_log_yaw_packet_path_; + std::ofstream can2_all_file_; + std::ofstream yaw_file_; + std::mutex log_mutex_; + size_t pending_rows_ = 0; + static constexpr size_t kYawLogFlushRows_ = 500; + const double kChassisRadiusBase; const double kRodLength; const double kDefaultRadius; @@ -774,6 +798,85 @@ class DeformableInfantryOmniB next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); } + void open_yaw_packet_log_files_() { + try { + std::filesystem::create_directories(debug_log_yaw_packet_path_); + const auto time = std::chrono::system_clock::to_time_t( + std::chrono::system_clock::now()); + auto ss = std::ostringstream{}; + ss << std::put_time(std::localtime(&time), "%Y-%m-%d_%H-%M-%S"); + const auto timestamp = ss.str(); + + can2_all_file_.open( + debug_log_yaw_packet_path_ + "/can2_all_" + timestamp + ".csv", + std::ios::out | std::ios::trunc); + yaw_file_.open( + debug_log_yaw_packet_path_ + "/yaw_" + timestamp + ".csv", + std::ios::out | std::ios::trunc); + + if (!can2_all_file_.is_open() || !yaw_file_.is_open()) { + RCLCPP_ERROR( + status_.get_logger(), + "[yaw packet log] failed to open log files under '%s'", + debug_log_yaw_packet_path_.c_str()); + debug_log_yaw_packet_ = false; + return; + } + + can2_all_file_ << "timestamp_ns,direction,can_id,d0,d1,d2,d3,d4,d5,d6,d7\n"; + yaw_file_ << "timestamp_ns,direction,can_id,d0,d1,d2,d3,d4,d5,d6,d7\n"; + can2_all_file_.flush(); + yaw_file_.flush(); + + RCLCPP_INFO( + status_.get_logger(), + "[yaw packet log] recording CAN2 frames to %s/can2_all_%s.csv and %s/yaw_%s.csv", + debug_log_yaw_packet_path_.c_str(), timestamp.c_str(), + debug_log_yaw_packet_path_.c_str(), timestamp.c_str()); + } catch (const std::exception& e) { + RCLCPP_ERROR( + status_.get_logger(), "[yaw packet log] failed to open log files: %s", + e.what()); + debug_log_yaw_packet_ = false; + } + } + + void log_can2_packet_(bool tx, uint32_t can_id, std::span data) { + if (!debug_log_yaw_packet_) + return; + + const auto timestamp = status_.now().nanoseconds(); + const auto data_bytes = [&] { + auto bytes = std::array{}; + for (size_t i = 0; i < 8; ++i) + bytes[i] = i < data.size() ? std::to_integer(data[i]) : 0; + return bytes; + }(); + + const auto write_row = [&](std::ofstream& file, bool yaw_only) { + if (!file.is_open()) + return; + if (yaw_only && can_id != 0x142) + return; + file << timestamp << ',' << (tx ? "TX" : "RX") << ",0x" << std::hex + << std::setw(8) << std::setfill('0') << can_id << std::dec; + for (const auto byte : data_bytes) + file << ",0x" << std::hex << std::setw(2) << std::setfill('0') << byte + << std::dec; + file << '\n'; + if (++pending_rows_ >= kYawLogFlushRows_) { + file.flush(); + pending_rows_ = 0; + } + }; + + { + std::lock_guard lock{log_mutex_}; + write_row(can2_all_file_, false); + write_row(yaw_file_, true); + } + } + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { if (data.is_extended_can_id || data.is_remote_transmission) return; @@ -792,6 +895,7 @@ class DeformableInfantryOmniB } monitor_.tick("Bottom::Can1", data.can_id); } else if (can == Spec::kCans.kCan2) { + log_can2_packet_(false, data.can_id, data.can_data); process_chassis_can_receive_(2, data); if (data.is_extended_can_id || data.is_remote_transmission) return; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp index 13949dfae..f9141e2ef 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp @@ -4,10 +4,15 @@ #include #include #include +#include +#include +#include #include #include +#include #include #include +#include #include #include #include @@ -395,6 +400,12 @@ class DeformableInfantryOmniC status.get_parameter_or("debug_log_wheel_motor", debug_log_wheel_motor_, false); status.get_parameter_or( "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); + status.get_parameter_or("debug_log_yaw_packet", debug_log_yaw_packet_, false); + status.get_parameter_or( + "debug_log_yaw_packet_path", debug_log_yaw_packet_path_, + std::string{"/home/ubuntu/controller/yaw"}); + if (debug_log_yaw_packet_) + open_yaw_packet_log_files_(); auto options = librmcs::board::AdvancedOptions{}; options.dangerously_skip_version_checks = true; @@ -481,19 +492,19 @@ class DeformableInfantryOmniC } .as_bytes(), }); + auto packet_can2_200 = device::CanPacket8{ + chassis_wheel_motors_[kRightBack].generate_command(), + device::CanPacket8::PaddingQuarter{}, + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + }; builder.can_transmit( Spec::kCans.kCan2, // { .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[kRightBack].generate_command(), - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), + .can_data = packet_can2_200.as_bytes(), }); + log_can2_packet_(true, 0x200, packet_can2_200.as_bytes()); builder.can_transmit( Spec::kCans.kCan3, // { @@ -507,12 +518,14 @@ class DeformableInfantryOmniC } .as_bytes(), }); + auto packet_can2_142 = gimbal_yaw_motor_.generate_command(); builder.can_transmit( Spec::kCans.kCan2, // { .can_id = 0x142, - .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), + .can_data = packet_can2_142.as_bytes(), }); + log_can2_packet_(true, 0x142, packet_can2_142.as_bytes()); builder.can_transmit( Spec::kCans.kCan1, // { @@ -545,14 +558,17 @@ class DeformableInfantryOmniC .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), }); break; - case kRightBack: + case kRightBack: { + auto packet = chassis_joint_motors_[i].generate_command(); builder.can_transmit( Spec::kCans.kCan2, // { .can_id = 0x141, - .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + .can_data = packet.as_bytes(), }); + log_can2_packet_(true, 0x141, packet.as_bytes()); break; + } case kRightFront: builder.can_transmit( Spec::kCans.kCan3, // @@ -603,6 +619,14 @@ class DeformableInfantryOmniC bool debug_log_wheel_motor_ = false; bool debug_log_deformable_joint_motor_ = false; + bool debug_log_yaw_packet_ = false; + std::string debug_log_yaw_packet_path_; + std::ofstream can2_all_file_; + std::ofstream yaw_file_; + std::mutex log_mutex_; + size_t pending_rows_ = 0; + static constexpr size_t kYawLogFlushRows_ = 500; + const double kChassisRadiusBase; const double kRodLength; const double kDefaultRadius; @@ -775,6 +799,85 @@ class DeformableInfantryOmniC next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); } + void open_yaw_packet_log_files_() { + try { + std::filesystem::create_directories(debug_log_yaw_packet_path_); + const auto time = std::chrono::system_clock::to_time_t( + std::chrono::system_clock::now()); + auto ss = std::ostringstream{}; + ss << std::put_time(std::localtime(&time), "%Y-%m-%d_%H-%M-%S"); + const auto timestamp = ss.str(); + + can2_all_file_.open( + debug_log_yaw_packet_path_ + "/can2_all_" + timestamp + ".csv", + std::ios::out | std::ios::trunc); + yaw_file_.open( + debug_log_yaw_packet_path_ + "/yaw_" + timestamp + ".csv", + std::ios::out | std::ios::trunc); + + if (!can2_all_file_.is_open() || !yaw_file_.is_open()) { + RCLCPP_ERROR( + status_.get_logger(), + "[yaw packet log] failed to open log files under '%s'", + debug_log_yaw_packet_path_.c_str()); + debug_log_yaw_packet_ = false; + return; + } + + can2_all_file_ << "timestamp_ns,direction,can_id,d0,d1,d2,d3,d4,d5,d6,d7\n"; + yaw_file_ << "timestamp_ns,direction,can_id,d0,d1,d2,d3,d4,d5,d6,d7\n"; + can2_all_file_.flush(); + yaw_file_.flush(); + + RCLCPP_INFO( + status_.get_logger(), + "[yaw packet log] recording CAN2 frames to %s/can2_all_%s.csv and %s/yaw_%s.csv", + debug_log_yaw_packet_path_.c_str(), timestamp.c_str(), + debug_log_yaw_packet_path_.c_str(), timestamp.c_str()); + } catch (const std::exception& e) { + RCLCPP_ERROR( + status_.get_logger(), "[yaw packet log] failed to open log files: %s", + e.what()); + debug_log_yaw_packet_ = false; + } + } + + void log_can2_packet_(bool tx, uint32_t can_id, std::span data) { + if (!debug_log_yaw_packet_) + return; + + const auto timestamp = status_.now().nanoseconds(); + const auto data_bytes = [&] { + auto bytes = std::array{}; + for (size_t i = 0; i < 8; ++i) + bytes[i] = i < data.size() ? std::to_integer(data[i]) : 0; + return bytes; + }(); + + const auto write_row = [&](std::ofstream& file, bool yaw_only) { + if (!file.is_open()) + return; + if (yaw_only && can_id != 0x142) + return; + file << timestamp << ',' << (tx ? "TX" : "RX") << ",0x" << std::hex + << std::setw(8) << std::setfill('0') << can_id << std::dec; + for (const auto byte : data_bytes) + file << ",0x" << std::hex << std::setw(2) << std::setfill('0') << byte + << std::dec; + file << '\n'; + if (++pending_rows_ >= kYawLogFlushRows_) { + file.flush(); + pending_rows_ = 0; + } + }; + + { + std::lock_guard lock{log_mutex_}; + write_row(can2_all_file_, false); + write_row(yaw_file_, true); + } + } + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { if (can == Spec::kCans.kCan0) { process_chassis_can_receive_(0, data); @@ -791,6 +894,7 @@ class DeformableInfantryOmniC } monitor_.tick("Bottom::Can1", data.can_id); } else if (can == Spec::kCans.kCan2) { + log_can2_packet_(false, data.can_id, data.can_data); process_chassis_can_receive_(2, data); if (data.is_extended_can_id || data.is_remote_transmission) return; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index 973896e65..19bb226b7 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -4,10 +4,15 @@ #include #include #include +#include +#include +#include #include #include +#include #include #include +#include #include #include #include @@ -395,6 +400,12 @@ class DeformableInfantryOmni status.get_parameter_or("debug_log_wheel_motor", debug_log_wheel_motor_, false); status.get_parameter_or( "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); + status.get_parameter_or("debug_log_yaw_packet", debug_log_yaw_packet_, false); + status.get_parameter_or( + "debug_log_yaw_packet_path", debug_log_yaw_packet_path_, + std::string{"/home/ubuntu/controller/yaw"}); + if (debug_log_yaw_packet_) + open_yaw_packet_log_files_(); auto options = librmcs::board::AdvancedOptions{}; options.dangerously_skip_version_checks = true; @@ -481,19 +492,19 @@ class DeformableInfantryOmni } .as_bytes(), }); + auto packet_can2_200 = device::CanPacket8{ + chassis_wheel_motors_[kRightBack].generate_command(), + device::CanPacket8::PaddingQuarter{}, + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + }; builder.can_transmit( Spec::kCans.kCan2, // { .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[kRightBack].generate_command(), - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), + .can_data = packet_can2_200.as_bytes(), }); + log_can2_packet_(true, 0x200, packet_can2_200.as_bytes()); builder.can_transmit( Spec::kCans.kCan3, // { @@ -507,12 +518,14 @@ class DeformableInfantryOmni } .as_bytes(), }); + auto packet_can2_142 = gimbal_yaw_motor_.generate_command(); builder.can_transmit( Spec::kCans.kCan2, // { .can_id = 0x142, - .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), + .can_data = packet_can2_142.as_bytes(), }); + log_can2_packet_(true, 0x142, packet_can2_142.as_bytes()); builder.can_transmit( Spec::kCans.kCan1, // { @@ -545,14 +558,17 @@ class DeformableInfantryOmni .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), }); break; - case kRightBack: + case kRightBack: { + auto packet = chassis_joint_motors_[i].generate_command(); builder.can_transmit( Spec::kCans.kCan2, // { .can_id = 0x141, - .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + .can_data = packet.as_bytes(), }); + log_can2_packet_(true, 0x141, packet.as_bytes()); break; + } case kRightFront: builder.can_transmit( Spec::kCans.kCan3, // @@ -603,6 +619,14 @@ class DeformableInfantryOmni bool debug_log_wheel_motor_ = false; bool debug_log_deformable_joint_motor_ = false; + bool debug_log_yaw_packet_ = false; + std::string debug_log_yaw_packet_path_; + std::ofstream can2_all_file_; + std::ofstream yaw_file_; + std::mutex log_mutex_; + size_t pending_rows_ = 0; + static constexpr size_t kYawLogFlushRows_ = 500; + const double kChassisRadiusBase; const double kRodLength; const double kDefaultRadius; @@ -775,6 +799,85 @@ class DeformableInfantryOmni next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); } + void open_yaw_packet_log_files_() { + try { + std::filesystem::create_directories(debug_log_yaw_packet_path_); + const auto time = std::chrono::system_clock::to_time_t( + std::chrono::system_clock::now()); + auto ss = std::ostringstream{}; + ss << std::put_time(std::localtime(&time), "%Y-%m-%d_%H-%M-%S"); + const auto timestamp = ss.str(); + + can2_all_file_.open( + debug_log_yaw_packet_path_ + "/can2_all_" + timestamp + ".csv", + std::ios::out | std::ios::trunc); + yaw_file_.open( + debug_log_yaw_packet_path_ + "/yaw_" + timestamp + ".csv", + std::ios::out | std::ios::trunc); + + if (!can2_all_file_.is_open() || !yaw_file_.is_open()) { + RCLCPP_ERROR( + status_.get_logger(), + "[yaw packet log] failed to open log files under '%s'", + debug_log_yaw_packet_path_.c_str()); + debug_log_yaw_packet_ = false; + return; + } + + can2_all_file_ << "timestamp_ns,direction,can_id,d0,d1,d2,d3,d4,d5,d6,d7\n"; + yaw_file_ << "timestamp_ns,direction,can_id,d0,d1,d2,d3,d4,d5,d6,d7\n"; + can2_all_file_.flush(); + yaw_file_.flush(); + + RCLCPP_INFO( + status_.get_logger(), + "[yaw packet log] recording CAN2 frames to %s/can2_all_%s.csv and %s/yaw_%s.csv", + debug_log_yaw_packet_path_.c_str(), timestamp.c_str(), + debug_log_yaw_packet_path_.c_str(), timestamp.c_str()); + } catch (const std::exception& e) { + RCLCPP_ERROR( + status_.get_logger(), "[yaw packet log] failed to open log files: %s", + e.what()); + debug_log_yaw_packet_ = false; + } + } + + void log_can2_packet_(bool tx, uint32_t can_id, std::span data) { + if (!debug_log_yaw_packet_) + return; + + const auto timestamp = status_.now().nanoseconds(); + const auto data_bytes = [&] { + auto bytes = std::array{}; + for (size_t i = 0; i < 8; ++i) + bytes[i] = i < data.size() ? std::to_integer(data[i]) : 0; + return bytes; + }(); + + const auto write_row = [&](std::ofstream& file, bool yaw_only) { + if (!file.is_open()) + return; + if (yaw_only && can_id != 0x142) + return; + file << timestamp << ',' << (tx ? "TX" : "RX") << ",0x" << std::hex + << std::setw(8) << std::setfill('0') << can_id << std::dec; + for (const auto byte : data_bytes) + file << ",0x" << std::hex << std::setw(2) << std::setfill('0') << byte + << std::dec; + file << '\n'; + if (++pending_rows_ >= kYawLogFlushRows_) { + file.flush(); + pending_rows_ = 0; + } + }; + + { + std::lock_guard lock{log_mutex_}; + write_row(can2_all_file_, false); + write_row(yaw_file_, true); + } + } + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { if (data.is_extended_can_id || data.is_remote_transmission) return; @@ -793,6 +896,7 @@ class DeformableInfantryOmni } monitor_.tick("Bottom::Can1", data.can_id); } else if (can == Spec::kCans.kCan2) { + log_can2_packet_(false, data.can_id, data.can_data); process_chassis_can_receive_(2, data); if (data.is_extended_can_id || data.is_remote_transmission) return; From 340ca9296d3c09e8654e685416544e0b71cf0ab8 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Tue, 4 Aug 2026 06:33:26 +0800 Subject: [PATCH 72/86] feat(suspension): delete CTRL suspension mode,delete charge special mode ,fix parameter --- .../config/deformable-infantry-omni-b.yaml | 3 ++ .../config/deformable-infantry-omni-c.yaml | 7 +++- .../config/deformable-infantry-omni.yaml | 3 ++ .../controller/chassis/deformable_mode.hpp | 38 ++----------------- .../deformable_infantry_gimbal_controller.cpp | 33 +++++++++++++++- .../hardware/deformable-infantry-omni-b.cpp | 22 ++++++++++- .../hardware/deformable-infantry-omni-c.cpp | 22 ++++++++++- .../src/hardware/deformable-infantry-omni.cpp | 22 ++++++++++- 8 files changed, 105 insertions(+), 45 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 9438cf42d..43aaf21fe 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -181,6 +181,9 @@ gimbal_controller: lower_limit: 0.10 # 8 deg ctrl_hold_pitch_target_angle: 0.0 + yaw_velocity_feedback_timeout_ms: 100 + yaw_angle_feedback_timeout_ms: 100 + yaw_angle_kp: 10.0 yaw_angle_ki: 0.0 yaw_angle_kd: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml index d84c1fdff..25d7131fd 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml @@ -87,7 +87,7 @@ auto_aim_component: fire_control: bullet_speed: 22.5 shoot_delay: 0.07 - offset_yaw: +1.05 + offset_yaw: +1.02 offset_pitch: -0.2 attack_window: 120.0 degraded_angle_speed: 12.0 @@ -112,7 +112,7 @@ deformable_infantry: serial_filter_top_board: "AF-ABAC-786D-1B53-99F6-00A2-42A6-AA95-9D69" chassis_radius: 0.2341741 rod_length: 0.140 - yaw_motor_zero_point: 5881 + yaw_motor_zero_point: 38910 pitch_motor_zero_point: 32214 debug_log_supercap: false debug_log_wheel_motor: false @@ -180,6 +180,9 @@ gimbal_controller: lower_limit: 0.10 # 8 deg ctrl_hold_pitch_target_angle: 0.0 + yaw_velocity_feedback_timeout_ms: 100 + yaw_angle_feedback_timeout_ms: 100 + yaw_angle_kp: 10.0 yaw_angle_ki: 0.0 yaw_angle_kd: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 9413733f5..314e39d97 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -183,6 +183,9 @@ gimbal_controller: lower_limit: 0.10 # 8 deg ctrl_hold_pitch_target_angle: 0.0 + yaw_velocity_feedback_timeout_ms: 100 + yaw_angle_feedback_timeout_ms: 100 + yaw_angle_kp: 10.0 yaw_angle_ki: 0.0 yaw_angle_kd: 0.0 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp index 8aeeb7193..c67154626 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp @@ -67,7 +67,6 @@ class DeformableChassisModeManager { apply_symmetric_target_ = true; suspension_enabled_by_toggle_ = false; low_prone_enabled_by_toggle_ = false; - before_wireless_charging_ = false; last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; last_keyboard_ = rmcs_msgs::Keyboard::zero(); @@ -81,7 +80,6 @@ class DeformableChassisModeManager { const rmcs_msgs::Keyboard& keyboard, double rotary_knob, double dt) { update_mode_from_inputs_(switch_left, switch_right, keyboard); - update_wireless_charging_posture_transition_(); update_low_prone_toggle_from_inputs_(switch_left, switch_right); joint_posture_state_.ctrl_low_prone_active = keyboard.ctrl; @@ -149,25 +147,6 @@ class DeformableChassisModeManager { }); } - void apply_wireless_charging_posture_() { - current_target_angle_ = min_angle_; - apply_symmetric_target_ = true; - joint_current_target_angle_.fill(min_angle_); - } - - void update_wireless_charging_posture_transition_() { - const bool now_wireless_charging_ = - joint_posture_state_.mode == rmcs_msgs::ChassisMode::WIRELESS_CHARGING; - - if (!now_wireless_charging_ && before_wireless_charging_) { - current_target_angle_ = max_angle_; - apply_symmetric_target_ = true; - joint_current_target_angle_.fill(max_angle_); - } - - before_wireless_charging_ = now_wireless_charging_; - } - void update_mode_from_inputs_( rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, const rmcs_msgs::Keyboard& keyboard) { @@ -237,17 +216,13 @@ class DeformableChassisModeManager { const bool remote_active_toggle_requested = remote_suspension_rotary_mode && rotary_knob_down_edge_(rotary_knob); - const bool wireless_charging_active = - joint_posture_state_.mode == rmcs_msgs::ChassisMode::WIRELESS_CHARGING; - const bool keyboard_active_suspension_toggle_requested = !last_keyboard_.e && keyboard.e; - if (!wireless_charging_active - && (keyboard_active_suspension_toggle_requested || remote_active_toggle_requested)) + if (keyboard_active_suspension_toggle_requested || remote_active_toggle_requested) suspension_enabled_by_toggle_ = !suspension_enabled_by_toggle_; const bool active_requested = - suspension_enable_ && !wireless_charging_active - && (joint_posture_state_.low_prone_active || suspension_enabled_by_toggle_); + suspension_enable_ + && (low_prone_enabled_by_toggle_ || suspension_enabled_by_toggle_); joint_posture_state_.suspension_mode = SuspensionMode::OFF; if (active_requested) @@ -281,12 +256,6 @@ class DeformableChassisModeManager { const bool posture_toggle_requested = remote_posture_toggle_condition || keyboard_posture_toggle_condition; - if (joint_posture_state_.mode == rmcs_msgs::ChassisMode::WIRELESS_CHARGING) { - apply_wireless_charging_posture_(); - last_rotary_knob_ = rotary_knob; - return; - } - if (apply_symmetric_target_) joint_current_target_angle_.fill(current_target_angle_); @@ -370,7 +339,6 @@ class DeformableChassisModeManager { bool apply_symmetric_target_ = true; bool suspension_enabled_by_toggle_ = false; bool low_prone_enabled_by_toggle_ = false; - bool before_wireless_charging_ = false; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp index 9396df1d1..16680c3d4 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp @@ -1,7 +1,9 @@ #include "controller/gimbal/two_axis_gimbal_solver.hpp" #include "controller/pid/pid_calculator.hpp" +#include #include +#include #include #include @@ -36,6 +38,11 @@ class DeformableInfantryGimbalController get_parameter_or("pitch_gravity_ff_gain", pitch_gravity_ff_gain_, 0.0); get_parameter_or("pitch_gravity_ff_phase", pitch_gravity_ff_phase_, 0.0); get_parameter_or("ctrl_hold_pitch_target_angle", ctrl_hold_pitch_target_angle_, 0.0); + get_parameter_or( + "yaw_velocity_feedback_timeout_ms", yaw_velocity_feedback_timeout_ms_, + std::int64_t{100}); + get_parameter_or( + "yaw_angle_feedback_timeout_ms", yaw_angle_feedback_timeout_ms_, std::int64_t{100}); } auto update() -> void override { @@ -70,13 +77,15 @@ class DeformableInfantryGimbalController if (!ctrl_hold_active_) *output_.pitch_angle_error = angle_error.pitch_angle_error; - if (!std::isfinite(angle_error.yaw_angle_error)) { + const bool yaw_feedback_stale = yaw_feedback_stale_(); + + if (yaw_feedback_stale || !std::isfinite(angle_error.yaw_angle_error)) { yaw_angle_pid_.reset(); yaw_velocity_pid_.reset(); *output_.yaw_control_torque = kNaN; } - if (std::isfinite(angle_error.yaw_angle_error)) { + if (!yaw_feedback_stale && std::isfinite(angle_error.yaw_angle_error)) { const auto yaw_velocity_ref = yaw_angle_pid_.update(angle_error.yaw_angle_error); *output_.yaw_control_torque = yaw_velocity_pid_.update(yaw_velocity_ref - *input_.yaw_velocity_imu); @@ -140,6 +149,9 @@ class DeformableInfantryGimbalController component.register_input("/auto_aim/should_control", auto_aim_should_control, false); component.register_input( "/auto_aim/control_direction", auto_aim_control_direction, false); + component.register_input( + "/gimbal/yaw/velocity_imu_timestamp", yaw_velocity_imu_timestamp, false); + component.register_input("/gimbal/yaw/angle_timestamp", yaw_angle_timestamp, false); } InputInterface joystick_left; @@ -159,6 +171,8 @@ class DeformableInfantryGimbalController InputInterface auto_aim_should_control; InputInterface auto_aim_control_direction; + InputInterface yaw_velocity_imu_timestamp; + InputInterface yaw_angle_timestamp; } input_{*this}; struct Output { @@ -191,6 +205,18 @@ class DeformableInfantryGimbalController return kDefaultDt; } + auto yaw_feedback_stale_() const -> bool { + const auto now_ns = std::chrono::steady_clock::now().time_since_epoch().count(); + if (input_.yaw_velocity_imu_timestamp.ready() + && now_ns - *input_.yaw_velocity_imu_timestamp + > yaw_velocity_feedback_timeout_ms_ * 1'000'000) + return true; + if (input_.yaw_angle_timestamp.ready() + && now_ns - *input_.yaw_angle_timestamp > yaw_angle_feedback_timeout_ms_ * 1'000'000) + return true; + return false; + } + auto manual_yaw_shift() const -> double { return joystick_sensitivity_ * input_.joystick_left->y() + mouse_sensitivity_ * input_.mouse_velocity->y(); @@ -331,6 +357,9 @@ class DeformableInfantryGimbalController get_parameter("pitch_velocity_kd").as_double(), }; + std::int64_t yaw_velocity_feedback_timeout_ms_ = 100; + std::int64_t yaw_angle_feedback_timeout_ms_ = 100; + double joystick_sensitivity_ = 0.003; double mouse_sensitivity_ = 0.5; bool pitch_torque_control_enabled_ = false; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index af52e0ff4..9a57455a4 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -166,6 +166,8 @@ class DeformableInfantryOmniB status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); + status.register_output( + "/gimbal/yaw/velocity_imu_timestamp", gimbal_yaw_velocity_imu_timestamp_); status.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); status.register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); @@ -204,6 +206,8 @@ class DeformableInfantryOmniB gimbal_pitch_motor_.update_status(); gimbal_left_friction_.update_status(); gimbal_right_friction_.update_status(); + *gimbal_yaw_velocity_imu_timestamp_ = + last_gyro_receive_ns_.load(std::memory_order::relaxed); const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); @@ -280,6 +284,8 @@ class DeformableInfantryOmniB } void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + last_gyro_receive_ns_.store( + Clock::now().time_since_epoch().count(), std::memory_order::relaxed); const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); monitor_.tick("Top::Imu", "Gyr"); if (!timestamp.has_value()) @@ -311,6 +317,9 @@ class DeformableInfantryOmniB OutputInterface& tf_; OutputInterface gimbal_yaw_velocity_bmi088_; OutputInterface gimbal_pitch_velocity_bmi088_; + OutputInterface gimbal_yaw_velocity_imu_timestamp_; + + std::atomic last_gyro_receive_ns_{Clock::now().time_since_epoch().count()}; EventOutputInterface imu_snapshot_output_; EventOutputInterface camera_signal_output_; @@ -353,6 +362,8 @@ class DeformableInfantryOmniB device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( static_cast(status.get_parameter("yaw_motor_zero_point").as_int()))); + status.register_output("/gimbal/yaw/angle_timestamp", gimbal_yaw_angle_timestamp_); + for (auto& motor : chassis_wheel_motors_) motor.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} @@ -452,6 +463,8 @@ class DeformableInfantryOmniB dr16_.update_status(); gimbal_yaw_motor_.update_status(); + *gimbal_yaw_angle_timestamp_ = + last_yaw_status_receive_ns_.load(std::memory_order::relaxed); if (supercap_status_received_.load(std::memory_order_relaxed)) supercap_.update_status(); if (debug_log_supercap_) @@ -635,6 +648,9 @@ class DeformableInfantryOmniB // Device + OutputInterface gimbal_yaw_angle_timestamp_; + std::atomic last_yaw_status_receive_ns_{Clock::now().time_since_epoch().count()}; + device::Bmi088 imu_{1000, 0.2, 0.0}; device::LkMotor gimbal_yaw_motor_{status_, command_, "/gimbal/yaw"}; device::Dr16 dr16_; @@ -899,9 +915,11 @@ class DeformableInfantryOmniB process_chassis_can_receive_(2, data); if (data.is_extended_can_id || data.is_remote_transmission) return; - if (data.can_id == 0x142) + if (data.can_id == 0x142) { gimbal_yaw_motor_.store_status(data.can_data); - else if (data.can_id == 0x203) + last_yaw_status_receive_ns_.store( + Clock::now().time_since_epoch().count(), std::memory_order::relaxed); + } else if (data.can_id == 0x203) gimbal_bullet_feeder_.store_status(data.can_data); monitor_.tick("Bottom::Can2", data.can_id); } else if (can == Spec::kCans.kCan3) { diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp index f9141e2ef..51ff819d1 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp @@ -166,6 +166,8 @@ class DeformableInfantryOmniC status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); + status.register_output( + "/gimbal/yaw/velocity_imu_timestamp", gimbal_yaw_velocity_imu_timestamp_); status.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); status.register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); @@ -205,6 +207,8 @@ class DeformableInfantryOmniC gimbal_pitch_motor_.update_status(); gimbal_left_friction_.update_status(); gimbal_right_friction_.update_status(); + *gimbal_yaw_velocity_imu_timestamp_ = + last_gyro_receive_ns_.load(std::memory_order::relaxed); const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); @@ -281,6 +285,8 @@ class DeformableInfantryOmniC } void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + last_gyro_receive_ns_.store( + Clock::now().time_since_epoch().count(), std::memory_order::relaxed); const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); monitor_.tick("Top::Imu", "Gyr"); if (!timestamp.has_value()) @@ -312,6 +318,9 @@ class DeformableInfantryOmniC OutputInterface& tf_; OutputInterface gimbal_yaw_velocity_bmi088_; OutputInterface gimbal_pitch_velocity_bmi088_; + OutputInterface gimbal_yaw_velocity_imu_timestamp_; + + std::atomic last_gyro_receive_ns_{Clock::now().time_since_epoch().count()}; EventOutputInterface imu_snapshot_output_; EventOutputInterface camera_signal_output_; @@ -354,6 +363,8 @@ class DeformableInfantryOmniC device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( static_cast(status.get_parameter("yaw_motor_zero_point").as_int()))); + status.register_output("/gimbal/yaw/angle_timestamp", gimbal_yaw_angle_timestamp_); + for (auto& motor : chassis_wheel_motors_) motor.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} @@ -453,6 +464,8 @@ class DeformableInfantryOmniC dr16_.update_status(); gimbal_yaw_motor_.update_status(); + *gimbal_yaw_angle_timestamp_ = + last_yaw_status_receive_ns_.load(std::memory_order::relaxed); if (supercap_status_received_.load(std::memory_order_relaxed)) supercap_.update_status(); if (debug_log_supercap_) @@ -636,6 +649,9 @@ class DeformableInfantryOmniC // Device + OutputInterface gimbal_yaw_angle_timestamp_; + std::atomic last_yaw_status_receive_ns_{Clock::now().time_since_epoch().count()}; + device::Bmi088 imu_{1000, 0.2, 0.0}; device::LkMotor gimbal_yaw_motor_{status_, command_, "/gimbal/yaw"}; device::Dr16 dr16_; @@ -898,9 +914,11 @@ class DeformableInfantryOmniC process_chassis_can_receive_(2, data); if (data.is_extended_can_id || data.is_remote_transmission) return; - if (data.can_id == 0x142) + if (data.can_id == 0x142) { gimbal_yaw_motor_.store_status(data.can_data); - else if (data.can_id == 0x203) + last_yaw_status_receive_ns_.store( + Clock::now().time_since_epoch().count(), std::memory_order::relaxed); + } else if (data.can_id == 0x203) gimbal_bullet_feeder_.store_status(data.can_data); monitor_.tick("Bottom::Can2", data.can_id); } else if (can == Spec::kCans.kCan3) { diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index 19bb226b7..69b648870 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -166,6 +166,8 @@ class DeformableInfantryOmni status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); + status.register_output( + "/gimbal/yaw/velocity_imu_timestamp", gimbal_yaw_velocity_imu_timestamp_); status.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); status.register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); @@ -205,6 +207,8 @@ class DeformableInfantryOmni gimbal_pitch_motor_.update_status(); gimbal_left_friction_.update_status(); gimbal_right_friction_.update_status(); + *gimbal_yaw_velocity_imu_timestamp_ = + last_gyro_receive_ns_.load(std::memory_order::relaxed); const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); @@ -281,6 +285,8 @@ class DeformableInfantryOmni } void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + last_gyro_receive_ns_.store( + Clock::now().time_since_epoch().count(), std::memory_order::relaxed); const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); monitor_.tick("Top::Imu", "Gyr"); if (!timestamp.has_value()) @@ -312,6 +318,9 @@ class DeformableInfantryOmni OutputInterface& tf_; OutputInterface gimbal_yaw_velocity_bmi088_; OutputInterface gimbal_pitch_velocity_bmi088_; + OutputInterface gimbal_yaw_velocity_imu_timestamp_; + + std::atomic last_gyro_receive_ns_{Clock::now().time_since_epoch().count()}; EventOutputInterface imu_snapshot_output_; EventOutputInterface camera_signal_output_; @@ -354,6 +363,8 @@ class DeformableInfantryOmni device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( static_cast(status.get_parameter("yaw_motor_zero_point").as_int()))); + status.register_output("/gimbal/yaw/angle_timestamp", gimbal_yaw_angle_timestamp_); + for (auto& motor : chassis_wheel_motors_) motor.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} @@ -453,6 +464,8 @@ class DeformableInfantryOmni dr16_.update_status(); gimbal_yaw_motor_.update_status(); + *gimbal_yaw_angle_timestamp_ = + last_yaw_status_receive_ns_.load(std::memory_order::relaxed); if (supercap_status_received_.load(std::memory_order_relaxed)) supercap_.update_status(); if (debug_log_supercap_) @@ -636,6 +649,9 @@ class DeformableInfantryOmni // Device + OutputInterface gimbal_yaw_angle_timestamp_; + std::atomic last_yaw_status_receive_ns_{Clock::now().time_since_epoch().count()}; + device::Bmi088 imu_{1000, 0.2, 0.0}; device::LkMotor gimbal_yaw_motor_{status_, command_, "/gimbal/yaw"}; device::Dr16 dr16_; @@ -900,9 +916,11 @@ class DeformableInfantryOmni process_chassis_can_receive_(2, data); if (data.is_extended_can_id || data.is_remote_transmission) return; - if (data.can_id == 0x142) + if (data.can_id == 0x142) { gimbal_yaw_motor_.store_status(data.can_data); - else if (data.can_id == 0x203) + last_yaw_status_receive_ns_.store( + Clock::now().time_since_epoch().count(), std::memory_order::relaxed); + } else if (data.can_id == 0x203) gimbal_bullet_feeder_.store_status(data.can_data); monitor_.tick("Bottom::Can2", data.can_id); } else if (can == Spec::kCans.kCan3) { From fafe81b9c2bbe767d3bff0ccb26756184c891f7c Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Tue, 4 Aug 2026 11:23:22 +0800 Subject: [PATCH 73/86] fix(config): adjust pitch and angle parameters for auto aim and chassis controller --- .../config/deformable-infantry-omni-b.yaml | 4 ++-- .../rmcs_bringup/config/deformable-infantry-omni.yaml | 10 +++++----- .../src/controller/chassis/deformable_mode.hpp | 4 ++-- .../src/rmcs_core/src/hardware/device/bmi088_ekf.hpp | 2 +- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 43aaf21fe..d465f732f 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -89,7 +89,7 @@ auto_aim_component: bullet_speed: 22.5 shoot_delay: 0.07 offset_yaw: +2.0 - offset_pitch: -0.4 + offset_pitch: +0.2 attack_window: 120.0 degraded_angle_speed: 12.0 window_redundancy: 0.6 @@ -123,7 +123,7 @@ deformable_infantry: chassis_controller: ros__parameters: - min_angle: 4.0 + min_angle: 3.5 max_angle: 59.0 active_suspension_enable: true wireless_charging_offset_deg: 225.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 314e39d97..b9ae078ae 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -88,8 +88,8 @@ auto_aim_component: fire_control: bullet_speed: 22.5 shoot_delay: 0.07 - offset_yaw: +1.05 - offset_pitch: -0.2 + offset_yaw: +0.5 + offset_pitch: -0.6 attack_window: 120.0 degraded_angle_speed: 12.0 window_redundancy: 0.6 @@ -121,7 +121,7 @@ deformable_infantry: chassis_controller: ros__parameters: - min_angle: 4.0 + min_angle: 3.0 max_angle: 59.0 active_suspension_enable: true wireless_charging_offset_deg: 45.0 @@ -197,8 +197,8 @@ gimbal_controller: yaw_velocity_integral_max: 5.0 pitch_angle_kp: 35.0 - pitch_angle_ki: 0.02 - pitch_angle_kd: 0.3 + pitch_angle_ki: 0.03 + pitch_angle_kd: 0.2 pitch_angle_integral_min: -0.5 pitch_angle_integral_max: 0.5 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp index c67154626..476541a02 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp @@ -221,8 +221,8 @@ class DeformableChassisModeManager { suspension_enabled_by_toggle_ = !suspension_enabled_by_toggle_; const bool active_requested = - suspension_enable_ - && (low_prone_enabled_by_toggle_ || suspension_enabled_by_toggle_); + suspension_enable_ && suspension_enabled_by_toggle_ + && !joint_posture_state_.low_prone_active; joint_posture_state_.suspension_mode = SuspensionMode::OFF; if (active_requested) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp index 83c1b2e32..33ae9bef4 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp @@ -84,7 +84,7 @@ class Bmi088Ekf { ekf_state_time_ = accel_sample_time; const auto correction = ekf_.prepare_correction(pending_accel_sample_->accel_g); - if (!correction || correction->chi_square() >= 3.0) + if (!correction || correction->chi_square() >= 16.0) break; if (!ekf_.correct(*correction)) break; From b3552ba1b251812b8e9a580665ff2d1a82ff957f Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Tue, 4 Aug 2026 11:54:37 +0800 Subject: [PATCH 74/86] feat(WIRELESS_CHARGING): enhance wireless charging mode handling and posture adjustments --- .../src/controller/chassis/deformable_mode.hpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp index 476541a02..03dd49ef5 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp @@ -177,9 +177,17 @@ class DeformableChassisModeManager { ? rmcs_msgs::ChassisMode::AUTO : rmcs_msgs::ChassisMode::STEP_DOWN; } else if (!last_keyboard_.x && keyboard.x) { - next_mode = next_mode == rmcs_msgs::ChassisMode::WIRELESS_CHARGING - ? rmcs_msgs::ChassisMode::AUTO - : rmcs_msgs::ChassisMode::WIRELESS_CHARGING; + if (next_mode == rmcs_msgs::ChassisMode::WIRELESS_CHARGING) { + next_mode = rmcs_msgs::ChassisMode::AUTO; + } else { + next_mode = rmcs_msgs::ChassisMode::WIRELESS_CHARGING; + // Entering wireless charging: force min-angle posture once, regardless of + // the current max/min posture. Q can still toggle freely afterwards. + current_target_angle_ = min_angle_; + active_suspension_base_angle_ = min_angle_; + apply_symmetric_target_ = true; + joint_current_target_angle_.fill(min_angle_); + } } joint_posture_state_.mode = next_mode; From e5e14e8e789614598aae893c749a5b27bbde54c3 Mon Sep 17 00:00:00 2001 From: qzhhhi Date: Tue, 4 Aug 2026 23:30:55 +0800 Subject: [PATCH 75/86] feat: autoaim output path --- rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml | 2 +- rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index d465f732f..20bef6613 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -59,7 +59,7 @@ value_broadcaster: auto_aim_recorder: ros__parameters: - output_path: "/home/ubuntu/autoaim/recoder" + output_path: "/autoaim/recoder" queue_depth: 16 flush_every_n_frames: 64 max_duration_seconds: 0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index b9ae078ae..198679e8a 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -59,7 +59,7 @@ value_broadcaster: auto_aim_recorder: ros__parameters: - output_path: "/home/ubuntu/autoaim/recoder" + output_path: "/autoaim/recoder" queue_depth: 16 flush_every_n_frames: 64 max_duration_seconds: 0 From e919c4faf9c7d3fea583d0fbc1b7e4c15a9c9592 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Wed, 5 Aug 2026 10:15:46 +0800 Subject: [PATCH 76/86] chore(suspension): change susension parameter --- .../config/deformable-infantry-omni-b.yaml | 14 +++--- .../config/deformable-infantry-omni-c.yaml | 15 ++++--- .../config/deformable-infantry-omni.yaml | 10 ++--- .../controller/chassis/deformable_mode.hpp | 45 +++++++++++++++++-- 4 files changed, 62 insertions(+), 22 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 20bef6613..0ceb94877 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -118,12 +118,12 @@ deformable_infantry: debug_log_supercap: false debug_log_wheel_motor: false debug_log_deformable_joint_motor: false - debug_log_yaw_packet: false + debug_log_yaw_packet: true debug_log_yaw_packet_path: "/home/ubuntu/controller/yaw" chassis_controller: ros__parameters: - min_angle: 3.5 + min_angle: 4.0 max_angle: 59.0 active_suspension_enable: true wireless_charging_offset_deg: 225.0 @@ -135,8 +135,8 @@ deformable_suspension: active_suspension_pitch_outer_kp: 12.0 active_suspension_pitch_outer_ki: 0.02 active_suspension_pitch_outer_kd: 0.0 - active_suspension_pitch_outer_integral_min: -2.0 - active_suspension_pitch_outer_integral_max: 2.0 + active_suspension_pitch_outer_integral_min: -1.0 + active_suspension_pitch_outer_integral_max: 1.0 active_suspension_pitch_outer_output_min: -3.0 active_suspension_pitch_outer_output_max: 3.0 @@ -151,8 +151,8 @@ deformable_suspension: active_suspension_roll_outer_kp: 12.0 active_suspension_roll_outer_ki: 0.02 active_suspension_roll_outer_kd: 0.0 - active_suspension_roll_outer_integral_min: -2.0 - active_suspension_roll_outer_integral_max: 2.0 + active_suspension_roll_outer_integral_min: -1.0 + active_suspension_roll_outer_integral_max: 1.0 active_suspension_roll_outer_output_min: -3.0 active_suspension_roll_outer_output_max: 3.0 @@ -177,7 +177,7 @@ deformable_suspension: gimbal_controller: ros__parameters: - upper_limit: -0.47123 # -27 deg + upper_limit: -0.60 # -27 deg lower_limit: 0.10 # 8 deg ctrl_hold_pitch_target_angle: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml index 25d7131fd..8ba07c6a3 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml @@ -59,12 +59,13 @@ value_broadcaster: auto_aim_recorder: ros__parameters: - output_path: "/home/ubuntu/autoaim/recoder" + output_path: "/autoaim/recoder" queue_depth: 16 flush_every_n_frames: 64 max_duration_seconds: 0 max_videos_size_gb: 200.0 auto_record: false + auto_aim_capturer: ros__parameters: camera_name: "" @@ -122,7 +123,7 @@ deformable_infantry: chassis_controller: ros__parameters: - min_angle: 4.0 + min_angle: 6.5 max_angle: 59.0 active_suspension_enable: true wireless_charging_offset_deg: 135.0 @@ -134,8 +135,8 @@ deformable_suspension: active_suspension_pitch_outer_kp: 12.0 active_suspension_pitch_outer_ki: 0.02 active_suspension_pitch_outer_kd: 0.0 - active_suspension_pitch_outer_integral_min: -2.0 - active_suspension_pitch_outer_integral_max: 2.0 + active_suspension_pitch_outer_integral_min: -1.0 + active_suspension_pitch_outer_integral_max: 1.0 active_suspension_pitch_outer_output_min: -3.0 active_suspension_pitch_outer_output_max: 3.0 @@ -150,8 +151,8 @@ deformable_suspension: active_suspension_roll_outer_kp: 12.0 active_suspension_roll_outer_ki: 0.02 active_suspension_roll_outer_kd: 0.0 - active_suspension_roll_outer_integral_min: -2.0 - active_suspension_roll_outer_integral_max: 2.0 + active_suspension_roll_outer_integral_min: -1.0 + active_suspension_roll_outer_integral_max: 1.0 active_suspension_roll_outer_output_min: -3.0 active_suspension_roll_outer_output_max: 3.0 @@ -176,7 +177,7 @@ deformable_suspension: gimbal_controller: ros__parameters: - upper_limit: -0.47123 # -27 deg + upper_limit: -0.60 # -27 deg lower_limit: 0.10 # 8 deg ctrl_hold_pitch_target_angle: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 198679e8a..d74f77d62 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -137,8 +137,8 @@ deformable_suspension: active_suspension_pitch_outer_kp: 12.0 active_suspension_pitch_outer_ki: 0.02 active_suspension_pitch_outer_kd: 0.0 - active_suspension_pitch_outer_integral_min: -2.0 - active_suspension_pitch_outer_integral_max: 2.0 + active_suspension_pitch_outer_integral_min: -1.0 + active_suspension_pitch_outer_integral_max: 1.0 active_suspension_pitch_outer_output_min: -3.0 active_suspension_pitch_outer_output_max: 3.0 @@ -153,8 +153,8 @@ deformable_suspension: active_suspension_roll_outer_kp: 12.0 active_suspension_roll_outer_ki: 0.02 active_suspension_roll_outer_kd: 0.0 - active_suspension_roll_outer_integral_min: -2.0 - active_suspension_roll_outer_integral_max: 2.0 + active_suspension_roll_outer_integral_min: -1.0 + active_suspension_roll_outer_integral_max: 1.0 active_suspension_roll_outer_output_min: -3.0 active_suspension_roll_outer_output_max: 3.0 @@ -179,7 +179,7 @@ deformable_suspension: gimbal_controller: ros__parameters: - upper_limit: -0.47123 # -27 deg + upper_limit: -0.60 # -27 deg lower_limit: 0.10 # 8 deg ctrl_hold_pitch_target_angle: 0.0 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp index 03dd49ef5..c0e7e528e 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp @@ -71,6 +71,8 @@ class DeformableChassisModeManager { last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; last_keyboard_ = rmcs_msgs::Keyboard::zero(); last_rotary_knob_ = 0.0; + suspension_was_active_ = false; + posture_state_saved_ = false; update_joint_posture_state_(false); } @@ -147,6 +149,24 @@ class DeformableChassisModeManager { }); } + void save_posture_state_() { + saved_current_target_angle_ = current_target_angle_; + saved_active_suspension_base_angle_ = active_suspension_base_angle_; + saved_joint_current_target_angle_ = joint_current_target_angle_; + saved_apply_symmetric_target_ = apply_symmetric_target_; + posture_state_saved_ = true; + } + + void restore_posture_state_() { + if (!posture_state_saved_) + return; + current_target_angle_ = saved_current_target_angle_; + active_suspension_base_angle_ = saved_active_suspension_base_angle_; + joint_current_target_angle_ = saved_joint_current_target_angle_; + apply_symmetric_target_ = saved_apply_symmetric_target_; + posture_state_saved_ = false; + } + void update_mode_from_inputs_( rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, const rmcs_msgs::Keyboard& keyboard) { @@ -181,6 +201,7 @@ class DeformableChassisModeManager { next_mode = rmcs_msgs::ChassisMode::AUTO; } else { next_mode = rmcs_msgs::ChassisMode::WIRELESS_CHARGING; + save_posture_state_(); // Entering wireless charging: force min-angle posture once, regardless of // the current max/min posture. Q can still toggle freely afterwards. current_target_angle_ = min_angle_; @@ -190,6 +211,12 @@ class DeformableChassisModeManager { } } + if (posture_state_saved_ + && joint_posture_state_.mode == rmcs_msgs::ChassisMode::WIRELESS_CHARGING + && next_mode != rmcs_msgs::ChassisMode::WIRELESS_CHARGING) { + restore_posture_state_(); + } + joint_posture_state_.mode = next_mode; } @@ -228,9 +255,7 @@ class DeformableChassisModeManager { if (keyboard_active_suspension_toggle_requested || remote_active_toggle_requested) suspension_enabled_by_toggle_ = !suspension_enabled_by_toggle_; - const bool active_requested = - suspension_enable_ && suspension_enabled_by_toggle_ - && !joint_posture_state_.low_prone_active; + const bool active_requested = suspension_enable_ && suspension_enabled_by_toggle_; joint_posture_state_.suspension_mode = SuspensionMode::OFF; if (active_requested) @@ -238,6 +263,13 @@ class DeformableChassisModeManager { joint_posture_state_.suspension_active = joint_posture_state_.suspension_mode == SuspensionMode::ACTIVE; + + if (joint_posture_state_.suspension_active && !suspension_was_active_) { + active_suspension_base_angle_ = current_target_angle_; + } else if (!joint_posture_state_.suspension_active && suspension_was_active_) { + current_target_angle_ = active_suspension_base_angle_; + } + suspension_was_active_ = joint_posture_state_.suspension_active; } void update_low_prone_toggle_from_inputs_( @@ -347,6 +379,13 @@ class DeformableChassisModeManager { bool apply_symmetric_target_ = true; bool suspension_enabled_by_toggle_ = false; bool low_prone_enabled_by_toggle_ = false; + bool suspension_was_active_ = false; + + bool posture_state_saved_ = false; + double saved_current_target_angle_ = 0.0; + double saved_active_suspension_base_angle_ = 0.0; + std::array saved_joint_current_target_angle_{}; + bool saved_apply_symmetric_target_ = true; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); From 725e15f00bfacf233a5b72199d21778a693a207c Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Wed, 5 Aug 2026 11:28:51 +0800 Subject: [PATCH 77/86] feat(controller): enhance gimbal and chassis control logic with new parameters and functions --- .../config/deformable-infantry-omni-b.yaml | 2 +- .../config/deformable-infantry-omni-c.yaml | 2 +- .../config/deformable-infantry-omni.yaml | 2 +- .../controller/chassis/deformable_chassis.cpp | 3 +- .../controller/chassis/deformable_mode.hpp | 88 +++++++++++++++++-- .../deformable_infantry_gimbal_controller.cpp | 2 +- 6 files changed, 85 insertions(+), 14 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 0ceb94877..f19af5a74 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -123,7 +123,7 @@ deformable_infantry: chassis_controller: ros__parameters: - min_angle: 4.0 + min_angle: 8.0 max_angle: 59.0 active_suspension_enable: true wireless_charging_offset_deg: 225.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml index 8ba07c6a3..69d6e081c 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml @@ -123,7 +123,7 @@ deformable_infantry: chassis_controller: ros__parameters: - min_angle: 6.5 + min_angle: 8.0 max_angle: 59.0 active_suspension_enable: true wireless_charging_offset_deg: 135.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index d74f77d62..6113251ed 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -121,7 +121,7 @@ deformable_infantry: chassis_controller: ros__parameters: - min_angle: 3.0 + min_angle: 8.0 max_angle: 59.0 active_suspension_enable: true wireless_charging_offset_deg: 45.0 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index 3edec1aeb..245ec44b2 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -111,7 +111,8 @@ class DeformableChassis double rotary_knob = rotary_knob_.ready() ? *rotary_knob_ : 0.0; - joint_mode_mgr_.update(switch_left, switch_right, keyboard, rotary_knob, update_dt()); + joint_mode_mgr_.update(switch_left, switch_right, keyboard, rotary_knob, update_dt(), + *gimbal_yaw_angle_); *mode_ = joint_mode_mgr_.mode(); *pitch_lock_active_ = joint_mode_mgr_.pitch_lock_active(); diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp index c0e7e528e..377fd5f98 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp @@ -67,6 +67,7 @@ class DeformableChassisModeManager { apply_symmetric_target_ = true; suspension_enabled_by_toggle_ = false; low_prone_enabled_by_toggle_ = false; + step_down_combo_active_ = false; last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; last_keyboard_ = rmcs_msgs::Keyboard::zero(); @@ -79,7 +80,7 @@ class DeformableChassisModeManager { void update( rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, - const rmcs_msgs::Keyboard& keyboard, double rotary_knob, double dt) { + const rmcs_msgs::Keyboard& keyboard, double rotary_knob, double dt, double gimbal_yaw_rad) { update_mode_from_inputs_(switch_left, switch_right, keyboard); update_low_prone_toggle_from_inputs_(switch_left, switch_right); @@ -87,10 +88,13 @@ class DeformableChassisModeManager { joint_posture_state_.ctrl_low_prone_active = keyboard.ctrl; joint_posture_state_.low_prone_active = joint_posture_state_.ctrl_low_prone_active || low_prone_enabled_by_toggle_; - joint_posture_state_.pitch_lock_active = joint_posture_state_.ctrl_low_prone_active; update_suspension_mode_from_inputs_(switch_left, switch_right, keyboard, rotary_knob); - update_posture_target_from_inputs_(switch_left, switch_right, keyboard, rotary_knob, dt); + update_posture_target_from_inputs_( + switch_left, switch_right, keyboard, rotary_knob, dt, gimbal_yaw_rad); + update_step_down_combo_from_inputs_(keyboard); + joint_posture_state_.pitch_lock_active = + joint_posture_state_.ctrl_low_prone_active || step_down_combo_active_; update_joint_posture_state_(joint_posture_state_.low_prone_active); last_switch_right_ = switch_right; @@ -141,6 +145,13 @@ class DeformableChassisModeManager { return normalized; } + static bool gimbal_faces_physical_front_(double gimbal_yaw_rad) { + if (!std::isfinite(gimbal_yaw_rad)) + return true; + const double signed_yaw = std::remainder(gimbal_yaw_rad, 2 * std::numbers::pi); + return std::abs(signed_yaw) <= std::numbers::pi / 2; + } + static bool symmetric_joint_target_requested_(const std::array& joint_target_deg) { constexpr double epsilon = 1e-6; @@ -251,7 +262,7 @@ class DeformableChassisModeManager { const bool remote_active_toggle_requested = remote_suspension_rotary_mode && rotary_knob_down_edge_(rotary_knob); - const bool keyboard_active_suspension_toggle_requested = !last_keyboard_.e && keyboard.e; + const bool keyboard_active_suspension_toggle_requested = !last_keyboard_.g && keyboard.g; if (keyboard_active_suspension_toggle_requested || remote_active_toggle_requested) suspension_enabled_by_toggle_ = !suspension_enabled_by_toggle_; @@ -282,7 +293,8 @@ class DeformableChassisModeManager { void update_posture_target_from_inputs_( rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, - const rmcs_msgs::Keyboard& keyboard, double rotary_knob, double /*dt*/) { + const rmcs_msgs::Keyboard& keyboard, double rotary_knob, double /*dt*/, + double gimbal_yaw_rad) { const bool remote_joint_posture_rotary_mode = switch_left == rmcs_msgs::Switch::MIDDLE && switch_right == rmcs_msgs::Switch::MIDDLE; @@ -292,7 +304,6 @@ class DeformableChassisModeManager { const bool remote_front_back_posture_toggle_condition = remote_joint_posture_rotary_mode && rotary_knob_up_edge_(rotary_knob); const bool front_high_rear_low = !last_keyboard_.b && keyboard.b; - const bool front_low_rear_high = !last_keyboard_.g && keyboard.g; const bool posture_toggle_requested = remote_posture_toggle_condition || keyboard_posture_toggle_condition; @@ -316,14 +327,65 @@ class DeformableChassisModeManager { } else if (remote_front_back_posture_toggle_condition) { toggle_front_back_posture_target_(); } else if (front_high_rear_low) { - apply_front_high_rear_low_target_(); - } else if (front_low_rear_high) { - apply_front_low_rear_high_target_(); + if (gimbal_faces_physical_front_(gimbal_yaw_rad)) + apply_front_high_rear_low_target_(); + else + apply_front_low_rear_high_target_(); } last_rotary_knob_ = rotary_knob; } + void update_step_down_combo_from_inputs_(const rmcs_msgs::Keyboard& keyboard) { + const bool e_held = keyboard.e; + + if (e_held && !step_down_combo_active_) { + saved_combo_mode_ = joint_posture_state_.mode; + saved_combo_suspension_enabled_ = suspension_enabled_by_toggle_; + saved_combo_current_target_angle_ = current_target_angle_; + saved_combo_active_suspension_base_angle_ = active_suspension_base_angle_; + saved_combo_joint_target_ = joint_current_target_angle_; + saved_combo_apply_symmetric_ = apply_symmetric_target_; + + step_down_combo_active_ = true; + joint_posture_state_.mode = rmcs_msgs::ChassisMode::STEP_DOWN; + joint_posture_state_.suspension_mode = SuspensionMode::ACTIVE; + joint_posture_state_.suspension_active = true; + suspension_enabled_by_toggle_ = true; + suspension_was_active_ = true; + current_target_angle_ = min_angle_ - 5.0; + active_suspension_base_angle_ = min_angle_ - 5.0; + apply_symmetric_target_ = true; + joint_current_target_angle_.fill(min_angle_ - 5.0); + update_joint_posture_state_(joint_posture_state_.low_prone_active); + return; + } + + if (!e_held && step_down_combo_active_) { + joint_posture_state_.mode = saved_combo_mode_; + suspension_enabled_by_toggle_ = saved_combo_suspension_enabled_; + suspension_was_active_ = saved_combo_suspension_enabled_ && suspension_enable_; + current_target_angle_ = saved_combo_current_target_angle_; + active_suspension_base_angle_ = saved_combo_active_suspension_base_angle_; + joint_current_target_angle_ = saved_combo_joint_target_; + apply_symmetric_target_ = saved_combo_apply_symmetric_; + step_down_combo_active_ = false; + update_joint_posture_state_(joint_posture_state_.low_prone_active); + return; + } + + if (step_down_combo_active_) { + joint_posture_state_.mode = rmcs_msgs::ChassisMode::STEP_DOWN; + joint_posture_state_.suspension_mode = SuspensionMode::ACTIVE; + joint_posture_state_.suspension_active = true; + current_target_angle_ = min_angle_ - 5.0; + active_suspension_base_angle_ = min_angle_ - 5.0; + apply_symmetric_target_ = true; + joint_current_target_angle_.fill(min_angle_ - 5.0); + update_joint_posture_state_(joint_posture_state_.low_prone_active); + } + } + bool rotary_knob_down_edge_(double rotary_knob) const { constexpr double rotary_knob_edge_threshold = 0.7; return last_rotary_knob_ < rotary_knob_edge_threshold @@ -381,6 +443,14 @@ class DeformableChassisModeManager { bool low_prone_enabled_by_toggle_ = false; bool suspension_was_active_ = false; + bool step_down_combo_active_ = false; + rmcs_msgs::ChassisMode saved_combo_mode_ = rmcs_msgs::ChassisMode::AUTO; + bool saved_combo_suspension_enabled_ = false; + double saved_combo_current_target_angle_ = 0.0; + double saved_combo_active_suspension_base_angle_ = 0.0; + std::array saved_combo_joint_target_{}; + bool saved_combo_apply_symmetric_ = true; + bool posture_state_saved_ = false; double saved_current_target_angle_ = 0.0; double saved_active_suspension_base_angle_ = 0.0; diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp index 16680c3d4..03b6909ba 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp @@ -259,7 +259,7 @@ class DeformableInfantryGimbalController suspension_on_by_switch_ = !suspension_on_by_switch_; } - pitch_lock_active_ = keyboard.ctrl || suspension_on_by_switch_; + pitch_lock_active_ = keyboard.ctrl || keyboard.e || suspension_on_by_switch_; last_switch_right_ = switch_right; } From ed96ec6ad3132d5ce507c33fb56574e4275a251d Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Wed, 5 Aug 2026 11:37:48 +0800 Subject: [PATCH 78/86] feat(chassis): update joint posture state with suspension mode handling --- .../src/rmcs_core/src/controller/chassis/deformable_mode.hpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp index 377fd5f98..981bc2d7d 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp @@ -369,6 +369,11 @@ class DeformableChassisModeManager { active_suspension_base_angle_ = saved_combo_active_suspension_base_angle_; joint_current_target_angle_ = saved_combo_joint_target_; apply_symmetric_target_ = saved_combo_apply_symmetric_; + joint_posture_state_.suspension_active = + suspension_enable_ && suspension_enabled_by_toggle_; + joint_posture_state_.suspension_mode = + joint_posture_state_.suspension_active ? SuspensionMode::ACTIVE + : SuspensionMode::OFF; step_down_combo_active_ = false; update_joint_posture_state_(joint_posture_state_.low_prone_active); return; From ec3c6c7748ba6593a7df786143085e4ad39c37a4 Mon Sep 17 00:00:00 2001 From: qzhhhi Date: Wed, 5 Aug 2026 11:41:10 +0800 Subject: [PATCH 79/86] feat: rune ff works --- .../config/deformable-infantry-omni-b.yaml | 31 +++- .../config/deformable-infantry-omni.yaml | 13 +- rmcs_ws/src/rmcs_core/plugins.xml | 1 + .../src/rmcs_core/scripts/fit_gravity_ff.py | 73 ++++++++ .../controller/chassis/deformable_chassis.cpp | 161 +++++++++++++----- .../deformable_infantry_gimbal_controller.cpp | 68 ++++++-- .../gimbal/two_axis_gimbal_solver.hpp | 10 ++ .../src/debug/gimbal_value_collector.cpp | 143 ++++++++++++++++ .../static_torque_test_controller.cpp | 76 +++------ 9 files changed, 451 insertions(+), 125 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/scripts/fit_gravity_ff.py create mode 100644 rmcs_ws/src/rmcs_core/src/debug/gimbal_value_collector.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index f19af5a74..47850c443 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -35,6 +35,8 @@ rmcs_executor: - rmcs::AutoAimCapturerComponent -> auto_aim_capturer - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui + # - rmcs_core::debug::GimbalValueCollector -> gimbal_value_collector + # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster # - rmcs_core::debug::ValueCollector -> value_collector @@ -51,6 +53,12 @@ value_collector: write_interval: 5 flush_interval: 1000 +gimbal_value_collector: + ros__parameters: + csv_path: "/tmp/gimbal_ff_.csv" + write_interval: 1 + flush_interval: 1000 + value_broadcaster: ros__parameters: forward_list: @@ -84,11 +92,12 @@ auto_aim_component: # 留空或填 unknow 表示禁用。 dangerous_fallback: "" manual_shoot: false + enable_rune: true camera_translation: [0.058, -0.08, 0.0] fire_control: bullet_speed: 22.5 shoot_delay: 0.07 - offset_yaw: +2.0 + offset_yaw: +2.7 offset_pitch: +0.2 attack_window: 120.0 degraded_angle_speed: 12.0 @@ -98,7 +107,7 @@ auto_aim_component: require_stable_command: false yaw_tolerance: 0.14 pitch_tolerance: 0.08 - rune_idle_duration: 0.7 + rune_idle_duration: 0.6 rune_shoot_duration: 0.2 auto_aim_ui: @@ -177,8 +186,8 @@ deformable_suspension: gimbal_controller: ros__parameters: - upper_limit: -0.60 # -27 deg - lower_limit: 0.10 # 8 deg + upper_limit: -0.60 + lower_limit: 0.10 ctrl_hold_pitch_target_angle: 0.0 yaw_velocity_feedback_timeout_ms: 100 @@ -204,8 +213,16 @@ gimbal_controller: pitch_velocity_ki: 0.0 pitch_velocity_kd: 0.0 - pitch_gravity_ff_gain: 4.302 - pitch_gravity_ff_phase: 0.589 + yaw_ref_velocity_gain: 1.0 + pitch_ref_velocity_gain: 1.0 + + yaw_velocity_ff_gain: 0.13 + yaw_acceleration_ff_gain: 0.18 + pitch_velocity_ff_gain: 0.378 + pitch_acceleration_ff_gain: 0.0396 + + pitch_gravity_ff_gain: 2.438 + pitch_gravity_ff_phase: 1.7254 pitch_torque_control: true @@ -261,7 +278,7 @@ bullet_feeder_velocity_pid_controller: measurement: /gimbal/bullet_feeder/velocity setpoint: /gimbal/bullet_feeder/control_velocity control: /gimbal/bullet_feeder/control_torque - kp: 1.5 + kp: 0.9 ki: 0.0 kd: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 6113251ed..5cf2e33a4 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -84,6 +84,7 @@ auto_aim_component: # 留空或填 unknow 表示禁用。 dangerous_fallback: "" manual_shoot: false + enable_rune: true camera_translation: [0.058, -0.08, 0.0] fire_control: bullet_speed: 22.5 @@ -206,8 +207,16 @@ gimbal_controller: pitch_velocity_ki: 0.0 pitch_velocity_kd: 0.0 - pitch_gravity_ff_gain: 4.302 - pitch_gravity_ff_phase: 0.589 + yaw_ref_velocity_gain: 1.0 + pitch_ref_velocity_gain: 1.0 + + yaw_velocity_ff_gain: 0.20 + yaw_acceleration_ff_gain: 0.17 + pitch_velocity_ff_gain: 0.356 + pitch_acceleration_ff_gain: 0.041 + + pitch_gravity_ff_gain: 2.575 + pitch_gravity_ff_phase: 1.7814 pitch_torque_control: true diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index ac41a1087..193dff619 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -50,6 +50,7 @@ + diff --git a/rmcs_ws/src/rmcs_core/scripts/fit_gravity_ff.py b/rmcs_ws/src/rmcs_core/scripts/fit_gravity_ff.py new file mode 100644 index 000000000..4a95bbad2 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/scripts/fit_gravity_ff.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python3 +"""Fit gravity feedforward A*sin(theta - phi) from StaticTorqueTestController CSV. + +Usage: fit_gravity_ff.py +Fits against imu_pitch_angle (authoritative) and the encoder angle (reference), +prints gain / phase / residual RMS and the phase difference between the two. +""" + +import sys + +import numpy as np +from scipy.optimize import least_squares + + +def load_csv(path): + with open(path) as f: + header = f.readline().strip().split(",") + data = np.genfromtxt(path, delimiter=",", skip_header=1) + return header, data + + +def column(header, data, name): + return data[:, header.index(name)] + + +def fit_gravity(angle, torque, with_offset=False): + def model(p): + gain, phase = p[0], p[1] + offset = p[2] if with_offset else 0.0 + return gain * np.sin(angle - phase) + offset + + result = least_squares(lambda p: model(p) - torque, [1.0, 0.0, 0.0][: 3 if with_offset else 2]) + gain, phase = result.x[0], result.x[1] + offset = result.x[2] if with_offset else 0.0 + if gain < 0: + gain, phase = -gain, phase + np.pi + phase = (phase + np.pi) % (2 * np.pi) - np.pi + rms = float(np.sqrt(np.mean((gain * np.sin(angle - phase) + offset - torque) ** 2))) + return gain, phase, offset, rms + + +def main(): + header, data = load_csv(sys.argv[1]) + torque = column(header, data, "/gimbal/pitch/torque") + velocity = column(header, data, "/gimbal/pitch/velocity") + angle_enc = column(header, data, "/gimbal/pitch/angle") + angle_imu = column(header, data, "imu_pitch_angle") + + still = np.abs(velocity) < 0.05 + if still.sum() < len(velocity): + print(f"dropped {len(velocity) - still.sum()} moving points (|v| >= 0.05 rad/s)") + + for label, angle in [("imu", angle_imu), ("encoder", angle_enc)]: + gain, phase, _, rms = fit_gravity(angle[still], torque[still]) + print( + f"[{label:7s}] gain = {gain:8.4f} phase = {phase:+8.4f} rad" + f" rms = {rms:.4f} N*m <- deploy these" + ) + gain_o, phase_o, offset_o, rms_o = fit_gravity( + angle[still], torque[still], with_offset=True + ) + print( + f"[{label:7s}] gain = {gain_o:8.4f} phase = {phase_o:+8.4f} rad" + f" offset = {offset_o:+7.4f} rms = {rms_o:.4f} N*m (diagnostic)" + ) + + gain_i, phase_i, _, _ = fit_gravity(angle_imu[still], torque[still]) + _, phase_e, _, _ = fit_gravity(angle_enc[still], torque[still]) + print(f"phase difference (imu - encoder) = {phase_i - phase_e:+.4f} rad") + + +if __name__ == "__main__": + main() diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index 245ec44b2..71669d33b 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -48,6 +48,10 @@ class DeformableChassis register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); + register_input("/auto_aim/single_shoot", auto_aim_single_shoot_, false); + register_input("/auto_aim/robot_center", auto_aim_robot_center_, false); + register_input("/tf", tf_, false); + register_output("/chassis/angle", chassis_angle_, nan_); register_output("/chassis/control_angle", chassis_control_angle_, nan_); register_output("/chassis/control_mode", mode_); @@ -125,6 +129,10 @@ class DeformableChassis *suspension_reference_angle_deg_ = joint_mode_mgr_.suspension_reference_angle_deg(); publish_joint_posture_targets_(); + update_auto_aim_override_state_(); + if (auto_aim_posture_override_) + apply_auto_aim_posture_override_(); + update_velocity_control(); } while (false); } @@ -156,6 +164,32 @@ class DeformableChassis *chassis_control_angle_ = nan_; } + void update_auto_aim_override_state_() { + auto_aim_posture_override_ = auto_aim_single_shoot_.ready() && *auto_aim_single_shoot_; + auto_aim_vector_follow_ = // + auto_aim_posture_override_ && tf_.ready() && auto_aim_robot_center_.ready() + && auto_aim_robot_center_->allFinite() && !auto_aim_robot_center_->isZero(); + } + + void apply_auto_aim_posture_override_() { + const double front_rad = deg_to_rad(joint_mode_mgr_.max_angle()); + const double back_rad = deg_to_rad(joint_mode_mgr_.min_angle()); + *joint_posture_target_angle_rad_[kLeftFront] = front_rad; + *joint_posture_target_angle_rad_[kRightFront] = front_rad; + *joint_posture_target_angle_rad_[kLeftBack] = back_rad; + *joint_posture_target_angle_rad_[kRightBack] = back_rad; + + *symmetric_posture_target_ = false; + *low_prone_active_ = false; + *active_suspension_active_ = false; + + const double min_deg = joint_mode_mgr_.min_angle(); + const double max_deg = joint_mode_mgr_.max_angle(); + const double reference_deg = (min_deg + max_deg) / 2.0; + *suspension_reference_angle_deg_ = reference_deg; + *correction_inverted_ = reference_deg > (min_deg - 5.0 + max_deg) / 2.0; + } + double update_dt() const { if (update_rate_.ready() && std::isfinite(*update_rate_) && *update_rate_ > 1e-6) return 1.0 / *update_rate_; @@ -178,9 +212,9 @@ class DeformableChassis if (translational_velocity.norm() > 1.0) translational_velocity.normalize(); - const double max_speed = - *mode_ == rmcs_msgs::ChassisMode::WIRELESS_CHARGING ? wireless_charging_speed_limit_ - : translational_velocity_max_; + const double max_speed = *mode_ == rmcs_msgs::ChassisMode::WIRELESS_CHARGING + ? wireless_charging_speed_limit_ + : translational_velocity_max_; translational_velocity *= max_speed; return translational_velocity; } @@ -189,51 +223,54 @@ class DeformableChassis double angular_velocity = 0.0; double chassis_control_angle = nan_; - switch (*mode_) { - case rmcs_msgs::ChassisMode::AUTO: break; - - case rmcs_msgs::ChassisMode::SPIN_FAST: { - bool forward = joint_mode_mgr_.spinning_forward(); - angular_velocity = - forward ? angular_velocity_max_ : -angular_velocity_max_; - angular_velocity = - std::clamp(angular_velocity, -angular_velocity_max_, angular_velocity_max_); - } break; - - case rmcs_msgs::ChassisMode::STEP_DOWN: { - double chassis_angle_error = - calculate_unsigned_chassis_angle_error(chassis_control_angle); - - constexpr double alignment = std::numbers::pi; - while (chassis_angle_error > alignment / 2) { - chassis_control_angle -= alignment; - if (chassis_control_angle < 0) - chassis_control_angle += 2 * std::numbers::pi; - chassis_angle_error -= alignment; + if (auto_aim_posture_override_) { + angular_velocity = update_auto_aim_override_angular_velocity_(chassis_control_angle); + } else { + switch (*mode_) { + case rmcs_msgs::ChassisMode::AUTO: break; + + case rmcs_msgs::ChassisMode::SPIN_FAST: { + bool forward = joint_mode_mgr_.spinning_forward(); + angular_velocity = forward ? angular_velocity_max_ : -angular_velocity_max_; + angular_velocity = + std::clamp(angular_velocity, -angular_velocity_max_, angular_velocity_max_); + } break; + + case rmcs_msgs::ChassisMode::STEP_DOWN: { + double chassis_angle_error = + calculate_unsigned_chassis_angle_error(chassis_control_angle); + + constexpr double alignment = std::numbers::pi; + while (chassis_angle_error > alignment / 2) { + chassis_control_angle -= alignment; + if (chassis_control_angle < 0) + chassis_control_angle += 2 * std::numbers::pi; + chassis_angle_error -= alignment; + } + + angular_velocity = following_velocity_controller_.update(chassis_angle_error); + } break; + + case rmcs_msgs::ChassisMode::WIRELESS_CHARGING: { + const double wireless_charging_offset_rad = + joint_mode_mgr_.wireless_charging_offset_rad(); + double chassis_angle_error = + calculate_unsigned_chassis_angle_error(chassis_control_angle); + + chassis_control_angle = + normalize_positive_angle(chassis_control_angle - wireless_charging_offset_rad); + chassis_angle_error = + normalize_positive_angle(chassis_angle_error - wireless_charging_offset_rad); + chassis_angle_error = normalize_signed_angle(chassis_angle_error); + + angular_velocity = following_velocity_controller_.update(chassis_angle_error); + angular_velocity = std::clamp( + angular_velocity, -wireless_charging_angular_velocity_limit_, + wireless_charging_angular_velocity_limit_); + } break; + + default: break; } - - angular_velocity = following_velocity_controller_.update(chassis_angle_error); - } break; - - case rmcs_msgs::ChassisMode::WIRELESS_CHARGING: { - const double wireless_charging_offset_rad = - joint_mode_mgr_.wireless_charging_offset_rad(); - double chassis_angle_error = - calculate_unsigned_chassis_angle_error(chassis_control_angle); - - chassis_control_angle = normalize_positive_angle( - chassis_control_angle - wireless_charging_offset_rad); - chassis_angle_error = normalize_positive_angle( - chassis_angle_error - wireless_charging_offset_rad); - chassis_angle_error = normalize_signed_angle(chassis_angle_error); - - angular_velocity = following_velocity_controller_.update(chassis_angle_error); - angular_velocity = std::clamp( - angular_velocity, -wireless_charging_angular_velocity_limit_, - wireless_charging_angular_velocity_limit_); - } break; - - default: break; } *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; @@ -242,6 +279,26 @@ class DeformableChassis return angular_velocity; } + double update_auto_aim_override_angular_velocity_(double& chassis_control_angle) { + double chassis_angle_error; + if (auto_aim_vector_follow_) { + const auto target_in_base = fast_tf::cast( + rmcs_description::OdomImu::Position{*auto_aim_robot_center_}, *tf_); + chassis_angle_error = target_in_base->head<2>().norm() > 1e-6 + ? std::atan2(target_in_base->y(), target_in_base->x()) + : 0.0; + chassis_control_angle = normalize_positive_angle( + 2 * std::numbers::pi - *gimbal_yaw_angle_ + chassis_angle_error); + } else { + chassis_angle_error = normalize_signed_angle( + calculate_unsigned_chassis_angle_error(chassis_control_angle)); + } + + return std::clamp( + following_velocity_controller_.update(chassis_angle_error), -angular_velocity_max_, + angular_velocity_max_); + } + double calculate_unsigned_chassis_angle_error(double& chassis_control_angle) { chassis_control_angle = *gimbal_yaw_angle_error_; if (chassis_control_angle < 0) @@ -286,6 +343,10 @@ class DeformableChassis "right_back", "right_front", }; + static constexpr size_t kLeftFront = 0; + static constexpr size_t kLeftBack = 1; + static constexpr size_t kRightBack = 2; + static constexpr size_t kRightFront = 3; InputInterface joystick_right_; InputInterface switch_right_; @@ -297,6 +358,12 @@ class DeformableChassis InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; OutputInterface chassis_angle_, chassis_control_angle_; + InputInterface auto_aim_single_shoot_; + InputInterface auto_aim_robot_center_; + InputInterface tf_; + bool auto_aim_posture_override_ = false; + bool auto_aim_vector_follow_ = false; + OutputInterface mode_; OutputInterface chassis_control_velocity_; OutputInterface pitch_lock_active_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp index 03b6909ba..f2cb7e9a3 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp @@ -37,6 +37,12 @@ class DeformableInfantryGimbalController get_parameter_or("pitch_gravity_ff_gain", pitch_gravity_ff_gain_, 0.0); get_parameter_or("pitch_gravity_ff_phase", pitch_gravity_ff_phase_, 0.0); + get_parameter_or("yaw_ref_velocity_gain", yaw_ref_velocity_gain_, 1.0); + get_parameter_or("pitch_ref_velocity_gain", pitch_ref_velocity_gain_, 1.0); + get_parameter_or("yaw_velocity_ff_gain", yaw_velocity_ff_gain_, 0.0); + get_parameter_or("yaw_acceleration_ff_gain", yaw_acceleration_ff_gain_, 0.0); + get_parameter_or("pitch_velocity_ff_gain", pitch_velocity_ff_gain_, 0.0); + get_parameter_or("pitch_acceleration_ff_gain", pitch_acceleration_ff_gain_, 0.0); get_parameter_or("ctrl_hold_pitch_target_angle", ctrl_hold_pitch_target_angle_, 0.0); get_parameter_or( "yaw_velocity_feedback_timeout_ms", yaw_velocity_feedback_timeout_ms_, @@ -78,6 +84,7 @@ class DeformableInfantryGimbalController *output_.pitch_angle_error = angle_error.pitch_angle_error; const bool yaw_feedback_stale = yaw_feedback_stale_(); + const auto trajectory_ff = trajectory_feedforward(auto_aim_active); if (yaw_feedback_stale || !std::isfinite(angle_error.yaw_angle_error)) { yaw_angle_pid_.reset(); @@ -86,9 +93,11 @@ class DeformableInfantryGimbalController } if (!yaw_feedback_stale && std::isfinite(angle_error.yaw_angle_error)) { - const auto yaw_velocity_ref = yaw_angle_pid_.update(angle_error.yaw_angle_error); + const auto yaw_velocity_ref = yaw_angle_pid_.update(angle_error.yaw_angle_error) + + trajectory_ff.yaw_ref_velocity; *output_.yaw_control_torque = - yaw_velocity_pid_.update(yaw_velocity_ref - *input_.yaw_velocity_imu); + yaw_velocity_pid_.update(yaw_velocity_ref - *input_.yaw_velocity_imu) + + trajectory_ff.yaw_velocity + trajectory_ff.yaw_acceleration; } if (!ctrl_hold_active_) { @@ -100,13 +109,15 @@ class DeformableInfantryGimbalController } else { const auto pitch_gravity_ff = pitch_gravity_feedforward(); const auto pitch_velocity_ref = - pitch_angle_pid_.update(angle_error.pitch_angle_error); + pitch_angle_pid_.update(angle_error.pitch_angle_error) + + trajectory_ff.pitch_ref_velocity; if (pitch_torque_control_enabled_) { *output_.pitch_control_velocity = kNaN; *output_.pitch_control_torque = pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity_imu) - + pitch_gravity_ff; + + pitch_gravity_ff + trajectory_ff.pitch_velocity + + trajectory_ff.pitch_acceleration; } else { pitch_velocity_pid_.reset(); *output_.pitch_control_velocity = pitch_velocity_ref; @@ -139,16 +150,15 @@ class DeformableInfantryGimbalController component.register_input("/remote/mouse", mouse); component.register_input("/predefined/update_rate", update_rate, false); - component.register_input("/gimbal/yaw/angle", yaw_angle); - component.register_input("/gimbal/yaw/velocity", yaw_velocity); component.register_input("/gimbal/pitch/angle", pitch_angle); - component.register_input("/gimbal/pitch/velocity", pitch_velocity); component.register_input("/gimbal/yaw/velocity_imu", yaw_velocity_imu); component.register_input("/gimbal/pitch/velocity_imu", pitch_velocity_imu); component.register_input("/auto_aim/should_control", auto_aim_should_control, false); component.register_input( "/auto_aim/control_direction", auto_aim_control_direction, false); + component.register_input("/auto_aim/ff_v", auto_aim_ff_v, false); + component.register_input("/auto_aim/ff_a", auto_aim_ff_a, false); component.register_input( "/gimbal/yaw/velocity_imu_timestamp", yaw_velocity_imu_timestamp, false); component.register_input("/gimbal/yaw/angle_timestamp", yaw_angle_timestamp, false); @@ -162,15 +172,14 @@ class DeformableInfantryGimbalController InputInterface mouse; InputInterface update_rate; - InputInterface yaw_angle; - InputInterface yaw_velocity; InputInterface pitch_angle; - InputInterface pitch_velocity; InputInterface yaw_velocity_imu; InputInterface pitch_velocity_imu; InputInterface auto_aim_should_control; InputInterface auto_aim_control_direction; + InputInterface auto_aim_ff_v; + InputInterface auto_aim_ff_a; InputInterface yaw_velocity_imu_timestamp; InputInterface yaw_angle_timestamp; } input_{*this}; @@ -246,9 +255,36 @@ class DeformableInfantryGimbalController auto pitch_gravity_feedforward() const -> double { if (ctrl_hold_active_) return 0.0; - if (!input_.pitch_angle.ready() || !std::isfinite(*input_.pitch_angle)) - return 0.0; - return pitch_gravity_ff_gain_ * std::sin(*input_.pitch_angle - pitch_gravity_ff_phase_); + return pitch_gravity_ff_gain_ + * std::sin(gimbal_solver_.gimbal_world_pitch() - pitch_gravity_ff_phase_); + } + + struct TrajectoryFeedforward { + double yaw_ref_velocity = 0.0; + double pitch_ref_velocity = 0.0; + double yaw_velocity = 0.0; + double yaw_acceleration = 0.0; + double pitch_velocity = 0.0; + double pitch_acceleration = 0.0; + }; + + auto trajectory_feedforward(bool auto_aim_active) const -> TrajectoryFeedforward { + if (!auto_aim_active || !input_.auto_aim_ff_v.ready() || !input_.auto_aim_ff_a.ready() + || !input_.auto_aim_ff_v->allFinite() || !input_.auto_aim_ff_a->allFinite()) + return {}; + + const auto ff_v = + gimbal_solver_.odom_to_yaw_link(OdomImu::DirectionVector{*input_.auto_aim_ff_v}); + const auto ff_a = + gimbal_solver_.odom_to_yaw_link(OdomImu::DirectionVector{*input_.auto_aim_ff_a}); + return { + .yaw_ref_velocity = yaw_ref_velocity_gain_ * ff_v->z(), + .pitch_ref_velocity = pitch_ref_velocity_gain_ * ff_v->y(), + .yaw_velocity = yaw_velocity_ff_gain_ * ff_v->z(), + .yaw_acceleration = yaw_acceleration_ff_gain_ * ff_a->z(), + .pitch_velocity = pitch_velocity_ff_gain_ * ff_v->y(), + .pitch_acceleration = pitch_acceleration_ff_gain_ * ff_a->y(), + }; } auto update_pitch_lock_state( @@ -366,6 +402,12 @@ class DeformableInfantryGimbalController double ctrl_hold_pitch_target_angle_ = 0.0; double pitch_gravity_ff_gain_ = 0.0; double pitch_gravity_ff_phase_ = 0.0; + double yaw_ref_velocity_gain_ = 1.0; + double pitch_ref_velocity_gain_ = 1.0; + double yaw_velocity_ff_gain_ = 0.0; + double yaw_acceleration_ff_gain_ = 0.0; + double pitch_velocity_ff_gain_ = 0.0; + double pitch_acceleration_ff_gain_ = 0.0; bool pitch_lock_active_ = false; bool suspension_on_by_switch_ = false; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp index 5c048d0ef..e59da7d2e 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp @@ -125,6 +125,16 @@ class TwoAxisGimbalSolver { bool enabled() const { return control_enabled_; } + double gimbal_world_pitch() const { + auto dir = + fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + return std::asin(std::clamp(dir->z(), -1.0, 1.0)); + } + + YawLink::DirectionVector odom_to_yaw_link(const OdomImu::DirectionVector& vector) const { + return fast_tf::cast(vector, *tf_); + } + private: void update_yaw_axis() { auto yaw_axis = diff --git a/rmcs_ws/src/rmcs_core/src/debug/gimbal_value_collector.cpp b/rmcs_ws/src/rmcs_core/src/debug/gimbal_value_collector.cpp new file mode 100644 index 000000000..697d4f689 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/debug/gimbal_value_collector.cpp @@ -0,0 +1,143 @@ +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::debug { + +class GimbalValueCollector + : public rmcs_executor::Component + , public rclcpp::Node + , public rmcs_utility::NodeMixin { +public: + GimbalValueCollector() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + + register_input("/tf", tf_); + + register_input("/auto_aim/should_control", auto_aim_should_control_); + register_input("/auto_aim/ff_v", auto_aim_ff_v_); + register_input("/auto_aim/ff_a", auto_aim_ff_a_); + + register_input("/gimbal/yaw/angle", yaw_angle_); + register_input("/gimbal/yaw/velocity_imu", yaw_velocity_imu_); + register_input("/gimbal/yaw/control_torque", yaw_control_torque_); + register_input("/gimbal/yaw/control_angle_error", yaw_control_angle_error_); + + register_input("/gimbal/pitch/angle", pitch_angle_); + register_input("/gimbal/pitch/velocity_imu", pitch_velocity_imu_); + register_input("/gimbal/pitch/control_torque", pitch_control_torque_); + register_input("/gimbal/pitch/control_angle_error", pitch_control_angle_error_); + + node::param("csv_path", csv_path_); + node::param("write_interval", write_interval_); + node::param("flush_interval", flush_interval_); + + { + constexpr auto kPlaceholder = std::string_view{""}; + const auto pos = csv_path_.find(kPlaceholder); + if (pos != std::string::npos) { + const auto now = std::chrono::system_clock::now(); + const auto time = std::chrono::system_clock::to_time_t(now); + + auto ss = std::ostringstream{}; + ss << std::put_time(std::localtime(&time), "%Y-%m-%d_%H-%M-%S"); + csv_path_.replace(pos, kPlaceholder.size(), ss.str()); + } + } + + csv_file_.open(csv_path_, std::ios::out | std::ios::trunc); + if (!csv_file_.is_open()) { + node::error("failed to open {}", csv_path_); + return; + } + + csv_file_ << "index,should_control" + ",pitch_angle_imu,pitch_ref_velocity,pitch_ref_acceleration" + ",yaw_ref_velocity,yaw_ref_acceleration" + ",pitch_velocity_imu,yaw_velocity_imu" + ",pitch_angle,yaw_angle" + ",pitch_control_torque,yaw_control_torque" + ",pitch_control_angle_error,yaw_control_angle_error\n"; + csv_file_.flush(); + + node::info("collecting gimbal signals to {}", csv_path_); + } + + auto update() -> void override { + if (!csv_file_.is_open()) + return; + + if (tick_++ % write_interval_ != 0) + return; + + const auto pitch_angle_imu = gimbal_world_pitch(); + const auto ff_v = fast_tf::cast( + rmcs_description::OdomImu::DirectionVector{*auto_aim_ff_v_}, *tf_); + const auto ff_a = fast_tf::cast( + rmcs_description::OdomImu::DirectionVector{*auto_aim_ff_a_}, *tf_); + + csv_file_ << sample_count_++ // + << ',' << (*auto_aim_should_control_ ? 1 : 0) // + << ',' << pitch_angle_imu // + << ',' << ff_v->y() << ',' << ff_a->y() // + << ',' << ff_v->z() << ',' << ff_a->z() // + << ',' << *pitch_velocity_imu_ << ',' << *yaw_velocity_imu_ // + << ',' << *pitch_angle_ << ',' << *yaw_angle_ // + << ',' << *pitch_control_torque_ << ',' << *yaw_control_torque_ // + << ',' << *pitch_control_angle_error_ << ',' << *yaw_control_angle_error_ // + << '\n'; + + if (sample_count_ % flush_interval_ == 0) + csv_file_.flush(); + } + +private: + auto gimbal_world_pitch() const -> double { + auto dir = fast_tf::cast( + rmcs_description::PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + return std::asin(std::clamp(dir->z(), -1.0, 1.0)); + } + + InputInterface tf_; + + InputInterface auto_aim_should_control_; + InputInterface auto_aim_ff_v_; + InputInterface auto_aim_ff_a_; + + InputInterface yaw_angle_; + InputInterface yaw_velocity_imu_; + InputInterface yaw_control_torque_; + InputInterface yaw_control_angle_error_; + + InputInterface pitch_angle_; + InputInterface pitch_velocity_imu_; + InputInterface pitch_control_torque_; + InputInterface pitch_control_angle_error_; + + std::string csv_path_; + int write_interval_ = 1; + int flush_interval_ = 1000; + + std::ofstream csv_file_; + + int tick_ = 0; + int sample_count_ = 0; +}; + +} // namespace rmcs_core::debug + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::debug::GimbalValueCollector, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/identification/static_torque_test_controller.cpp b/rmcs_ws/src/rmcs_core/src/identification/static_torque_test_controller.cpp index e537da8e8..81b33392b 100644 --- a/rmcs_ws/src/rmcs_core/src/identification/static_torque_test_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/identification/static_torque_test_controller.cpp @@ -9,10 +9,14 @@ #include #include #include +#include + +#include #include #include #include +#include #include #include #include @@ -25,7 +29,6 @@ namespace { using Clock = std::chrono::steady_clock; -constexpr auto kCenterInfoInterval = std::chrono::duration(0.5); constexpr double kRangeTolerance = 1e-9; template @@ -93,17 +96,14 @@ double wrap_to_pi(double angle) { enum class RemoteMode { kMeasureRange, - kCenterHold, kTestCommand, kIdle, }; RemoteMode decode_remote_mode(rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right) { using rmcs_msgs::Switch; - if (switch_left == Switch::MIDDLE && switch_right == Switch::DOWN) - return RemoteMode::kMeasureRange; if (switch_left == Switch::MIDDLE && switch_right == Switch::MIDDLE) - return RemoteMode::kCenterHold; + return RemoteMode::kMeasureRange; if (switch_left == Switch::MIDDLE && switch_right == Switch::UP) return RemoteMode::kTestCommand; return RemoteMode::kIdle; @@ -149,6 +149,7 @@ class StaticTorqueTestController register_input("/predefined/timestamp", timestamp_); register_input("/remote/switch/left", switch_left_); register_input("/remote/switch/right", switch_right_); + register_input("/tf", tf_); register_input(measured_torque_name_, measured_torque_); register_input(measured_velocity_name_, measured_velocity_); register_input(measured_angle_name_, measured_angle_); @@ -167,7 +168,6 @@ class StaticTorqueTestController angle_tracking_initialized_ = false; range_initialized_ = false; remote_mode_initialized_ = false; - next_center_info_time_ = Clock::time_point{}; } void update() override { @@ -178,7 +178,6 @@ class StaticTorqueTestController switch (remote_mode) { case RemoteMode::kMeasureRange: handle_measure_range(); break; - case RemoteMode::kCenterHold: handle_center_hold(mode_changed); break; case RemoteMode::kTestCommand: handle_test_command(mode_changed); break; case RemoteMode::kIdle: handle_idle(); break; } @@ -224,56 +223,11 @@ class StaticTorqueTestController range_max_ = std::max(range_max_, current_continuous_angle_); } - void handle_center_hold(bool mode_changed) { - stop_test(true); - - if (!range_initialized_) { - *control_torque_ = nan_; - return; - } - - if (mode_changed) { - position_pid_.reset(); - velocity_pid_.reset(); - next_center_info_time_ = *timestamp_; - } - - active_setpoint_ = 0.5 * (range_min_ + range_max_); - *control_torque_ = calculate_pid_output(active_setpoint_); - - maybe_log_center_hold_info(); - } - - void maybe_log_center_hold_info() { - if (*timestamp_ < next_center_info_time_) - return; - - const double control_error = active_setpoint_ - current_continuous_angle_; - RCLCPP_INFO( - get_logger(), - "Center hold error=%.6f rad, setpoint=%.6f rad, angle=%.6f rad, range=[%.6f, %.6f] rad", - control_error, wrap_to_pi(active_setpoint_), current_wrapped_angle_, - wrap_to_pi(range_min_), wrap_to_pi(range_max_)); - - do { - next_center_info_time_ += - std::chrono::duration_cast(kCenterInfoInterval); - } while (*timestamp_ >= next_center_info_time_); - } - void handle_test_command(bool mode_changed) { if (mode_changed) { - if (last_remote_mode_ == RemoteMode::kCenterHold) { - if (!start_test()) { - *control_torque_ = nan_; - return; - } - } else if (!test_setpoint_valid_) { + if (!start_test()) { *control_torque_ = nan_; return; - } else { - position_pid_.reset(); - velocity_pid_.reset(); } } @@ -334,7 +288,8 @@ class StaticTorqueTestController csv_writer_.open(path); csv_writer_.write_row( "update_count", "elapsed_s", control_torque_name_, measured_torque_name_, - measured_velocity_name_, measured_angle_name_); + measured_velocity_name_, measured_angle_name_, "imu_pitch_angle", + "imu_yaw_angle"); csv_writer_.flush(); } catch (const std::exception& exception) { const auto path_string = path.string(); @@ -364,12 +319,21 @@ class StaticTorqueTestController const double elapsed_s = std::max(0.0, std::chrono::duration(*timestamp_ - test_start_time_).count()); + const auto [imu_yaw, imu_pitch] = imu_yaw_pitch(); + csv_writer_.write_row( *update_count_, elapsed_s, *control_torque_, *measured_torque_, *measured_velocity_, - current_wrapped_angle_); + current_wrapped_angle_, imu_pitch, imu_yaw); csv_writer_.flush(); } + std::pair imu_yaw_pitch() const { + auto dir = fast_tf::cast( + rmcs_description::PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + return { + std::atan2(dir->y(), dir->x()), std::asin(std::clamp(dir->z(), -1.0, 1.0))}; + } + void finish_test_sequence() { test_active_ = false; close_csv(); @@ -430,6 +394,7 @@ class StaticTorqueTestController InputInterface timestamp_; InputInterface switch_left_; InputInterface switch_right_; + InputInterface tf_; InputInterface measured_torque_; InputInterface measured_velocity_; InputInterface measured_angle_; @@ -450,7 +415,6 @@ class StaticTorqueTestController bool remote_mode_initialized_ = false; RemoteMode last_remote_mode_ = RemoteMode::kIdle; - Clock::time_point next_center_info_time_{}; double active_setpoint_ = 0.0; bool test_setpoint_valid_ = false; From 3658f18c537d09a7851798880e63d159b8e7ceea Mon Sep 17 00:00:00 2001 From: qzhhhi Date: Wed, 5 Aug 2026 12:01:40 +0800 Subject: [PATCH 80/86] feat: revert old deformable chassis to a --- .../config/deformable-infantry-omni-b.yaml | 2 +- rmcs_ws/src/rmcs_core/plugins.xml | 1 + .../controller/chassis/deformable_chassis.cpp | 158 ++----- .../chassis/deformable_chassis_rune.cpp | 393 ++++++++++++++++++ 4 files changed, 440 insertions(+), 114 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_rune.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 47850c443..69d7da766 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -20,7 +20,7 @@ rmcs_executor: - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller - - rmcs_core::controller::chassis::DeformableChassis -> chassis_controller + - rmcs_core::controller::chassis::DeformableChassisRune -> chassis_controller - rmcs_core::controller::chassis::DeformableSuspension -> deformable_suspension - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::DeformableOmniWheelController -> deformable_chassis_controller diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 193dff619..610cf396d 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -21,6 +21,7 @@ + diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index 71669d33b..f14d29003 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -48,10 +48,6 @@ class DeformableChassis register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); - register_input("/auto_aim/single_shoot", auto_aim_single_shoot_, false); - register_input("/auto_aim/robot_center", auto_aim_robot_center_, false); - register_input("/tf", tf_, false); - register_output("/chassis/angle", chassis_angle_, nan_); register_output("/chassis/control_angle", chassis_control_angle_, nan_); register_output("/chassis/control_mode", mode_); @@ -115,8 +111,8 @@ class DeformableChassis double rotary_knob = rotary_knob_.ready() ? *rotary_knob_ : 0.0; - joint_mode_mgr_.update(switch_left, switch_right, keyboard, rotary_knob, update_dt(), - *gimbal_yaw_angle_); + joint_mode_mgr_.update( + switch_left, switch_right, keyboard, rotary_knob, update_dt(), *gimbal_yaw_angle_); *mode_ = joint_mode_mgr_.mode(); *pitch_lock_active_ = joint_mode_mgr_.pitch_lock_active(); @@ -129,10 +125,6 @@ class DeformableChassis *suspension_reference_angle_deg_ = joint_mode_mgr_.suspension_reference_angle_deg(); publish_joint_posture_targets_(); - update_auto_aim_override_state_(); - if (auto_aim_posture_override_) - apply_auto_aim_posture_override_(); - update_velocity_control(); } while (false); } @@ -164,32 +156,6 @@ class DeformableChassis *chassis_control_angle_ = nan_; } - void update_auto_aim_override_state_() { - auto_aim_posture_override_ = auto_aim_single_shoot_.ready() && *auto_aim_single_shoot_; - auto_aim_vector_follow_ = // - auto_aim_posture_override_ && tf_.ready() && auto_aim_robot_center_.ready() - && auto_aim_robot_center_->allFinite() && !auto_aim_robot_center_->isZero(); - } - - void apply_auto_aim_posture_override_() { - const double front_rad = deg_to_rad(joint_mode_mgr_.max_angle()); - const double back_rad = deg_to_rad(joint_mode_mgr_.min_angle()); - *joint_posture_target_angle_rad_[kLeftFront] = front_rad; - *joint_posture_target_angle_rad_[kRightFront] = front_rad; - *joint_posture_target_angle_rad_[kLeftBack] = back_rad; - *joint_posture_target_angle_rad_[kRightBack] = back_rad; - - *symmetric_posture_target_ = false; - *low_prone_active_ = false; - *active_suspension_active_ = false; - - const double min_deg = joint_mode_mgr_.min_angle(); - const double max_deg = joint_mode_mgr_.max_angle(); - const double reference_deg = (min_deg + max_deg) / 2.0; - *suspension_reference_angle_deg_ = reference_deg; - *correction_inverted_ = reference_deg > (min_deg - 5.0 + max_deg) / 2.0; - } - double update_dt() const { if (update_rate_.ready() && std::isfinite(*update_rate_) && *update_rate_ > 1e-6) return 1.0 / *update_rate_; @@ -223,54 +189,50 @@ class DeformableChassis double angular_velocity = 0.0; double chassis_control_angle = nan_; - if (auto_aim_posture_override_) { - angular_velocity = update_auto_aim_override_angular_velocity_(chassis_control_angle); - } else { - switch (*mode_) { - case rmcs_msgs::ChassisMode::AUTO: break; - - case rmcs_msgs::ChassisMode::SPIN_FAST: { - bool forward = joint_mode_mgr_.spinning_forward(); - angular_velocity = forward ? angular_velocity_max_ : -angular_velocity_max_; - angular_velocity = - std::clamp(angular_velocity, -angular_velocity_max_, angular_velocity_max_); - } break; - - case rmcs_msgs::ChassisMode::STEP_DOWN: { - double chassis_angle_error = - calculate_unsigned_chassis_angle_error(chassis_control_angle); - - constexpr double alignment = std::numbers::pi; - while (chassis_angle_error > alignment / 2) { - chassis_control_angle -= alignment; - if (chassis_control_angle < 0) - chassis_control_angle += 2 * std::numbers::pi; - chassis_angle_error -= alignment; - } - - angular_velocity = following_velocity_controller_.update(chassis_angle_error); - } break; - - case rmcs_msgs::ChassisMode::WIRELESS_CHARGING: { - const double wireless_charging_offset_rad = - joint_mode_mgr_.wireless_charging_offset_rad(); - double chassis_angle_error = - calculate_unsigned_chassis_angle_error(chassis_control_angle); - - chassis_control_angle = - normalize_positive_angle(chassis_control_angle - wireless_charging_offset_rad); - chassis_angle_error = - normalize_positive_angle(chassis_angle_error - wireless_charging_offset_rad); - chassis_angle_error = normalize_signed_angle(chassis_angle_error); - - angular_velocity = following_velocity_controller_.update(chassis_angle_error); - angular_velocity = std::clamp( - angular_velocity, -wireless_charging_angular_velocity_limit_, - wireless_charging_angular_velocity_limit_); - } break; - - default: break; + switch (*mode_) { + case rmcs_msgs::ChassisMode::AUTO: break; + + case rmcs_msgs::ChassisMode::SPIN_FAST: { + bool forward = joint_mode_mgr_.spinning_forward(); + angular_velocity = forward ? angular_velocity_max_ : -angular_velocity_max_; + angular_velocity = + std::clamp(angular_velocity, -angular_velocity_max_, angular_velocity_max_); + } break; + + case rmcs_msgs::ChassisMode::STEP_DOWN: { + double chassis_angle_error = + calculate_unsigned_chassis_angle_error(chassis_control_angle); + + constexpr double alignment = std::numbers::pi; + while (chassis_angle_error > alignment / 2) { + chassis_control_angle -= alignment; + if (chassis_control_angle < 0) + chassis_control_angle += 2 * std::numbers::pi; + chassis_angle_error -= alignment; } + + angular_velocity = following_velocity_controller_.update(chassis_angle_error); + } break; + + case rmcs_msgs::ChassisMode::WIRELESS_CHARGING: { + const double wireless_charging_offset_rad = + joint_mode_mgr_.wireless_charging_offset_rad(); + double chassis_angle_error = + calculate_unsigned_chassis_angle_error(chassis_control_angle); + + chassis_control_angle = + normalize_positive_angle(chassis_control_angle - wireless_charging_offset_rad); + chassis_angle_error = + normalize_positive_angle(chassis_angle_error - wireless_charging_offset_rad); + chassis_angle_error = normalize_signed_angle(chassis_angle_error); + + angular_velocity = following_velocity_controller_.update(chassis_angle_error); + angular_velocity = std::clamp( + angular_velocity, -wireless_charging_angular_velocity_limit_, + wireless_charging_angular_velocity_limit_); + } break; + + default: break; } *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; @@ -279,26 +241,6 @@ class DeformableChassis return angular_velocity; } - double update_auto_aim_override_angular_velocity_(double& chassis_control_angle) { - double chassis_angle_error; - if (auto_aim_vector_follow_) { - const auto target_in_base = fast_tf::cast( - rmcs_description::OdomImu::Position{*auto_aim_robot_center_}, *tf_); - chassis_angle_error = target_in_base->head<2>().norm() > 1e-6 - ? std::atan2(target_in_base->y(), target_in_base->x()) - : 0.0; - chassis_control_angle = normalize_positive_angle( - 2 * std::numbers::pi - *gimbal_yaw_angle_ + chassis_angle_error); - } else { - chassis_angle_error = normalize_signed_angle( - calculate_unsigned_chassis_angle_error(chassis_control_angle)); - } - - return std::clamp( - following_velocity_controller_.update(chassis_angle_error), -angular_velocity_max_, - angular_velocity_max_); - } - double calculate_unsigned_chassis_angle_error(double& chassis_control_angle) { chassis_control_angle = *gimbal_yaw_angle_error_; if (chassis_control_angle < 0) @@ -343,10 +285,6 @@ class DeformableChassis "right_back", "right_front", }; - static constexpr size_t kLeftFront = 0; - static constexpr size_t kLeftBack = 1; - static constexpr size_t kRightBack = 2; - static constexpr size_t kRightFront = 3; InputInterface joystick_right_; InputInterface switch_right_; @@ -358,12 +296,6 @@ class DeformableChassis InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; OutputInterface chassis_angle_, chassis_control_angle_; - InputInterface auto_aim_single_shoot_; - InputInterface auto_aim_robot_center_; - InputInterface tf_; - bool auto_aim_posture_override_ = false; - bool auto_aim_vector_follow_ = false; - OutputInterface mode_; OutputInterface chassis_control_velocity_; OutputInterface pitch_lock_active_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_rune.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_rune.cpp new file mode 100644 index 000000000..9f1e000fa --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_rune.cpp @@ -0,0 +1,393 @@ +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "controller/chassis/deformable_mode.hpp" +#include "controller/pid/pid_calculator.hpp" + +namespace rmcs_core::controller::chassis { + +class DeformableChassisRune + : public rmcs_executor::Component + , public rclcpp::Node { +public: + explicit DeformableChassisRune() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , following_velocity_controller_(10.0, 0.0, 0.0) + , wireless_charging_speed_limit_(get_parameter_or("wireless_charging_speed_limit", 0.2)) + , wireless_charging_angular_velocity_limit_( + get_parameter_or("wireless_charging_angular_velocity_limit", 3.0)) + , joint_mode_mgr_(*this) { + + following_velocity_controller_.output_max = angular_velocity_max_; + following_velocity_controller_.output_min = -angular_velocity_max_; + + register_input("/remote/joystick/right", joystick_right_); + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/keyboard", keyboard_); + register_input("/remote/rotary_knob", rotary_knob_); + register_input("/predefined/update_rate", update_rate_); + + register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); + register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); + + register_input("/auto_aim/single_shoot", auto_aim_single_shoot_, false); + register_input("/auto_aim/robot_center", auto_aim_robot_center_, false); + register_input("/tf", tf_, false); + + register_output("/chassis/angle", chassis_angle_, nan_); + register_output("/chassis/control_angle", chassis_control_angle_, nan_); + register_output("/chassis/control_mode", mode_); + register_output("/chassis/control_velocity", chassis_control_velocity_); + register_output("/chassis/pitch_lock_active", pitch_lock_active_, false); + register_output("/chassis/active_suspension/active", active_suspension_active_, false); + register_output("/chassis/deformable/low_prone_active", low_prone_active_, false); + register_output( + "/chassis/deformable/symmetric_posture_target", symmetric_posture_target_, true); + register_output("/chassis/deformable/correction_inverted", correction_inverted_, false); + register_output( + "/chassis/deformable/min_angle_deg", min_angle_deg_, joint_mode_mgr_.min_angle()); + register_output( + "/chassis/deformable/max_angle_deg", max_angle_deg_, joint_mode_mgr_.max_angle()); + register_output( + "/chassis/deformable/suspension_reference_angle_deg", suspension_reference_angle_deg_, + joint_mode_mgr_.suspension_reference_angle_deg()); + register_output( + "/chassis/deformable/reset_count", deformable_reset_count_, static_cast(0)); + for (size_t i = 0; i < kJointCount; ++i) { + register_output( + fmt::format("/chassis/deformable/{}_joint/posture_target_angle", kJointName[i]), + joint_posture_target_angle_rad_[i], deg_to_rad(joint_mode_mgr_.max_angle())); + } + + *mode_ = rmcs_msgs::ChassisMode::AUTO; + *pitch_lock_active_ = false; + *active_suspension_active_ = false; + *low_prone_active_ = false; + *symmetric_posture_target_ = true; + *correction_inverted_ = false; + chassis_control_velocity_->vector << nan_, nan_, nan_; + } + + void before_updating() override { + if (!gimbal_yaw_angle_.ready()) { + gimbal_yaw_angle_.make_and_bind_directly(0.0); + RCLCPP_WARN(get_logger(), "Failed to fetch \"/gimbal/yaw/angle\". Set to 0.0."); + } + if (!gimbal_yaw_angle_error_.ready()) { + gimbal_yaw_angle_error_.make_and_bind_directly(0.0); + RCLCPP_WARN( + get_logger(), "Failed to fetch \"/gimbal/yaw/control_angle_error\". " + "Set to 0.0."); + } + } + + void update() override { + using rmcs_msgs::Switch; + + const auto switch_right = *switch_right_; + const auto switch_left = *switch_left_; + const auto keyboard = *keyboard_; + + do { + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_all_controls(); + break; + } + + double rotary_knob = rotary_knob_.ready() ? *rotary_knob_ : 0.0; + + joint_mode_mgr_.update( + switch_left, switch_right, keyboard, rotary_knob, update_dt(), *gimbal_yaw_angle_); + + *mode_ = joint_mode_mgr_.mode(); + *pitch_lock_active_ = joint_mode_mgr_.pitch_lock_active(); + *active_suspension_active_ = joint_mode_mgr_.suspension_active(); + *low_prone_active_ = joint_mode_mgr_.low_prone_active(); + *symmetric_posture_target_ = joint_mode_mgr_.symmetric_posture_target(); + *correction_inverted_ = joint_mode_mgr_.correction_inverted(); + *min_angle_deg_ = joint_mode_mgr_.min_angle(); + *max_angle_deg_ = joint_mode_mgr_.max_angle(); + *suspension_reference_angle_deg_ = joint_mode_mgr_.suspension_reference_angle_deg(); + publish_joint_posture_targets_(); + + update_auto_aim_override_state_(); + if (auto_aim_posture_override_) + apply_auto_aim_posture_override_(); + + update_velocity_control(); + } while (false); + } + +private: + static constexpr size_t kJointCount = 4; + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double translational_velocity_max_ = 10.0; + static constexpr double angular_velocity_max_ = 30.0; + static constexpr double default_dt_ = 1e-3; + + void reset_all_controls() { + joint_mode_mgr_.reset(); + *deformable_reset_count_ += 1; + + *mode_ = rmcs_msgs::ChassisMode::AUTO; + *pitch_lock_active_ = false; + *active_suspension_active_ = false; + *low_prone_active_ = false; + *symmetric_posture_target_ = true; + *correction_inverted_ = false; + *min_angle_deg_ = joint_mode_mgr_.min_angle(); + *max_angle_deg_ = joint_mode_mgr_.max_angle(); + *suspension_reference_angle_deg_ = joint_mode_mgr_.suspension_reference_angle_deg(); + publish_joint_posture_targets_(); + + chassis_control_velocity_->vector << nan_, nan_, nan_; + *chassis_angle_ = nan_; + *chassis_control_angle_ = nan_; + } + + void update_auto_aim_override_state_() { + auto_aim_posture_override_ = auto_aim_single_shoot_.ready() && *auto_aim_single_shoot_; + auto_aim_vector_follow_ = // + auto_aim_posture_override_ && tf_.ready() && auto_aim_robot_center_.ready() + && auto_aim_robot_center_->allFinite() && !auto_aim_robot_center_->isZero(); + } + + void apply_auto_aim_posture_override_() { + const double front_rad = deg_to_rad(joint_mode_mgr_.max_angle()); + const double back_rad = deg_to_rad(joint_mode_mgr_.min_angle()); + *joint_posture_target_angle_rad_[kLeftFront] = front_rad; + *joint_posture_target_angle_rad_[kRightFront] = front_rad; + *joint_posture_target_angle_rad_[kLeftBack] = back_rad; + *joint_posture_target_angle_rad_[kRightBack] = back_rad; + + *symmetric_posture_target_ = false; + *low_prone_active_ = false; + *active_suspension_active_ = false; + + const double min_deg = joint_mode_mgr_.min_angle(); + const double max_deg = joint_mode_mgr_.max_angle(); + const double reference_deg = (min_deg + max_deg) / 2.0; + *suspension_reference_angle_deg_ = reference_deg; + *correction_inverted_ = reference_deg > (min_deg - 5.0 + max_deg) / 2.0; + } + + double update_dt() const { + if (update_rate_.ready() && std::isfinite(*update_rate_) && *update_rate_ > 1e-6) + return 1.0 / *update_rate_; + return default_dt_; + } + + void update_velocity_control() { + Eigen::Vector2d translational_velocity = update_translational_velocity_control(); + double angular_velocity = update_angular_velocity_control(); + chassis_control_velocity_->vector << translational_velocity, angular_velocity; + } + + Eigen::Vector2d update_translational_velocity_control() { + const auto keyboard = *keyboard_; + Eigen::Vector2d keyboard_move{keyboard.w - keyboard.s, keyboard.a - keyboard.d}; + + Eigen::Vector2d translational_velocity = + Eigen::Rotation2Dd{*gimbal_yaw_angle_} * (*joystick_right_ + keyboard_move); + + if (translational_velocity.norm() > 1.0) + translational_velocity.normalize(); + + const double max_speed = *mode_ == rmcs_msgs::ChassisMode::WIRELESS_CHARGING + ? wireless_charging_speed_limit_ + : translational_velocity_max_; + translational_velocity *= max_speed; + return translational_velocity; + } + + double update_angular_velocity_control() { + double angular_velocity = 0.0; + double chassis_control_angle = nan_; + + if (auto_aim_posture_override_) { + angular_velocity = update_auto_aim_override_angular_velocity_(chassis_control_angle); + } else { + switch (*mode_) { + case rmcs_msgs::ChassisMode::AUTO: break; + + case rmcs_msgs::ChassisMode::SPIN_FAST: { + bool forward = joint_mode_mgr_.spinning_forward(); + angular_velocity = forward ? angular_velocity_max_ : -angular_velocity_max_; + angular_velocity = + std::clamp(angular_velocity, -angular_velocity_max_, angular_velocity_max_); + } break; + + case rmcs_msgs::ChassisMode::STEP_DOWN: { + double chassis_angle_error = + calculate_unsigned_chassis_angle_error(chassis_control_angle); + + constexpr double alignment = std::numbers::pi; + while (chassis_angle_error > alignment / 2) { + chassis_control_angle -= alignment; + if (chassis_control_angle < 0) + chassis_control_angle += 2 * std::numbers::pi; + chassis_angle_error -= alignment; + } + + angular_velocity = following_velocity_controller_.update(chassis_angle_error); + } break; + + case rmcs_msgs::ChassisMode::WIRELESS_CHARGING: { + const double wireless_charging_offset_rad = + joint_mode_mgr_.wireless_charging_offset_rad(); + double chassis_angle_error = + calculate_unsigned_chassis_angle_error(chassis_control_angle); + + chassis_control_angle = + normalize_positive_angle(chassis_control_angle - wireless_charging_offset_rad); + chassis_angle_error = + normalize_positive_angle(chassis_angle_error - wireless_charging_offset_rad); + chassis_angle_error = normalize_signed_angle(chassis_angle_error); + + angular_velocity = following_velocity_controller_.update(chassis_angle_error); + angular_velocity = std::clamp( + angular_velocity, -wireless_charging_angular_velocity_limit_, + wireless_charging_angular_velocity_limit_); + } break; + + default: break; + } + } + + *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; + *chassis_control_angle_ = chassis_control_angle; + + return angular_velocity; + } + + double update_auto_aim_override_angular_velocity_(double& chassis_control_angle) { + double chassis_angle_error; + if (auto_aim_vector_follow_) { + const auto target_in_base = fast_tf::cast( + rmcs_description::OdomImu::Position{*auto_aim_robot_center_}, *tf_); + chassis_angle_error = target_in_base->head<2>().norm() > 1e-6 + ? std::atan2(target_in_base->y(), target_in_base->x()) + : 0.0; + chassis_control_angle = normalize_positive_angle( + 2 * std::numbers::pi - *gimbal_yaw_angle_ + chassis_angle_error); + } else { + chassis_angle_error = normalize_signed_angle( + calculate_unsigned_chassis_angle_error(chassis_control_angle)); + } + + return std::clamp( + following_velocity_controller_.update(chassis_angle_error), -angular_velocity_max_, + angular_velocity_max_); + } + + double calculate_unsigned_chassis_angle_error(double& chassis_control_angle) { + chassis_control_angle = *gimbal_yaw_angle_error_; + if (chassis_control_angle < 0) + chassis_control_angle += 2 * std::numbers::pi; + + double unsigned_angle_error = chassis_control_angle + *gimbal_yaw_angle_; + if (unsigned_angle_error >= 2 * std::numbers::pi) + unsigned_angle_error -= 2 * std::numbers::pi; + + return unsigned_angle_error; + } + + static double deg_to_rad(double deg) { return deg * std::numbers::pi / 180.0; } + + static double normalize_positive_angle(double angle) { + constexpr double full_turn = 2 * std::numbers::pi; + while (angle >= full_turn) + angle -= full_turn; + while (angle < 0.0) + angle += full_turn; + return angle; + } + + static double normalize_signed_angle(double angle) { + angle = normalize_positive_angle(angle); + if (angle > std::numbers::pi) + angle -= 2 * std::numbers::pi; + return angle; + } + + void publish_joint_posture_targets_() { + std::array targets_deg{}; + joint_mode_mgr_.copy_joint_posture_target_deg(targets_deg); + + for (size_t i = 0; i < kJointCount; ++i) + *joint_posture_target_angle_rad_[i] = deg_to_rad(targets_deg[i]); + } + + static constexpr const char* kJointName[] = { + "left_front", + "left_back", + "right_back", + "right_front", + }; + static constexpr size_t kLeftFront = 0; + static constexpr size_t kLeftBack = 1; + static constexpr size_t kRightBack = 2; + static constexpr size_t kRightFront = 3; + + InputInterface joystick_right_; + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface keyboard_; + InputInterface rotary_knob_; + InputInterface update_rate_; + + InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; + OutputInterface chassis_angle_, chassis_control_angle_; + + InputInterface auto_aim_single_shoot_; + InputInterface auto_aim_robot_center_; + InputInterface tf_; + bool auto_aim_posture_override_ = false; + bool auto_aim_vector_follow_ = false; + + OutputInterface mode_; + OutputInterface chassis_control_velocity_; + OutputInterface pitch_lock_active_; + OutputInterface active_suspension_active_; + OutputInterface low_prone_active_; + OutputInterface symmetric_posture_target_; + OutputInterface correction_inverted_; + OutputInterface min_angle_deg_; + OutputInterface max_angle_deg_; + OutputInterface suspension_reference_angle_deg_; + OutputInterface deformable_reset_count_; + std::array, kJointCount> joint_posture_target_angle_rad_; + + pid::PidCalculator following_velocity_controller_; + + double wireless_charging_speed_limit_; + double wireless_charging_angular_velocity_limit_; + + DeformableChassisModeManager joint_mode_mgr_; +}; + +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::DeformableChassisRune, rmcs_executor::Component) From 3b564d3c1958f645eb115d40f6cd699a8384c9a7 Mon Sep 17 00:00:00 2001 From: qzhhhi Date: Wed, 5 Aug 2026 12:13:08 +0800 Subject: [PATCH 81/86] feat: adjust para --- .../src/rmcs_bringup/config/deformable-infantry-omni-b.yaml | 4 ++-- rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 69d7da766..1441bc013 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -97,8 +97,8 @@ auto_aim_component: fire_control: bullet_speed: 22.5 shoot_delay: 0.07 - offset_yaw: +2.7 - offset_pitch: +0.2 + offset_yaw: +2.6 + offset_pitch: +0.3 attack_window: 120.0 degraded_angle_speed: 12.0 window_redundancy: 0.6 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 5cf2e33a4..a70837337 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -89,8 +89,8 @@ auto_aim_component: fire_control: bullet_speed: 22.5 shoot_delay: 0.07 - offset_yaw: +0.5 - offset_pitch: -0.6 + offset_yaw: +0.2 + offset_pitch: -0.4 attack_window: 120.0 degraded_angle_speed: 12.0 window_redundancy: 0.6 From 2e0dc41e1a3309de0d3fd71ac2b1a3c1b7e2f5de Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Thu, 6 Aug 2026 07:58:02 +0800 Subject: [PATCH 82/86] feat: update deformable chassis references and adjust auto aim parameters --- .../config/deformable-infantry-omni-b.yaml | 4 +-- .../config/deformable-infantry-omni-c.yaml | 27 ++++++++++++------- .../config/deformable-infantry-omni.yaml | 6 ++--- .../src/controller/pid/pid_calculator.hpp | 1 + 4 files changed, 24 insertions(+), 14 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 1441bc013..a2f96a8cf 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -98,7 +98,7 @@ auto_aim_component: bullet_speed: 22.5 shoot_delay: 0.07 offset_yaw: +2.6 - offset_pitch: +0.3 + offset_pitch: -0.3 attack_window: 120.0 degraded_angle_speed: 12.0 window_redundancy: 0.6 @@ -127,7 +127,7 @@ deformable_infantry: debug_log_supercap: false debug_log_wheel_motor: false debug_log_deformable_joint_motor: false - debug_log_yaw_packet: true + debug_log_yaw_packet: false debug_log_yaw_packet_path: "/home/ubuntu/controller/yaw" chassis_controller: diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml index 69d6e081c..dd95c01e4 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml @@ -20,7 +20,7 @@ rmcs_executor: - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller - - rmcs_core::controller::chassis::DeformableChassis -> chassis_controller + - rmcs_core::controller::chassis::DeformableChassisRune -> chassis_controller - rmcs_core::controller::chassis::DeformableSuspension -> deformable_suspension - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::DeformableOmniWheelController -> deformable_chassis_controller @@ -84,21 +84,22 @@ auto_aim_component: # 留空或填 unknow 表示禁用。 dangerous_fallback: "" manual_shoot: false + enable_rune: true camera_translation: [0.058, -0.08, 0.0] fire_control: bullet_speed: 22.5 shoot_delay: 0.07 - offset_yaw: +1.02 - offset_pitch: -0.2 + offset_yaw: +0.8 + offset_pitch: -1.4 attack_window: 120.0 degraded_angle_speed: 12.0 window_redundancy: 0.6 window_hysteresis: 0.2 attack_preaim: false require_stable_command: false - yaw_tolerance: 0.14 - pitch_tolerance: 0.08 - rune_idle_duration: 0.7 + yaw_tolerance: 0.21 + pitch_tolerance: 0.12 + rune_idle_duration: 0.6 rune_shoot_duration: 0.2 auto_aim_ui: @@ -204,8 +205,16 @@ gimbal_controller: pitch_velocity_ki: 0.0 pitch_velocity_kd: 0.0 - pitch_gravity_ff_gain: 4.302 - pitch_gravity_ff_phase: 0.589 + yaw_ref_velocity_gain: 1.0 + pitch_ref_velocity_gain: 1.0 + + yaw_velocity_ff_gain: 0.13 + yaw_acceleration_ff_gain: 0.18 + pitch_velocity_ff_gain: 0.378 + pitch_acceleration_ff_gain: 0.0396 + + pitch_gravity_ff_gain: 2.575 + pitch_gravity_ff_phase: 1.784 pitch_torque_control: true @@ -261,7 +270,7 @@ bullet_feeder_velocity_pid_controller: measurement: /gimbal/bullet_feeder/velocity setpoint: /gimbal/bullet_feeder/control_velocity control: /gimbal/bullet_feeder/control_torque - kp: 1.5 + kp: 0.9 ki: 0.0 kd: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index a70837337..e526a1fa1 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -14,13 +14,13 @@ rmcs_executor: - rmcs_core::controller::gimbal::DeformableInfantryGimbalController -> gimbal_controller - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller - - rmcs_core::controller::shooting::HeatController -> heat_controller + - rmcs_core::controller::shooting::HeatControllerRune -> heat_controller - rmcs_core::controller::shooting::BulletFeederController17mm -> bullet_feeder_controller - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller - - rmcs_core::controller::chassis::DeformableChassis -> chassis_controller + - rmcs_core::controller::chassis::DeformableChassisRune -> chassis_controller - rmcs_core::controller::chassis::DeformableSuspension -> deformable_suspension - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::DeformableOmniWheelController -> deformable_chassis_controller @@ -90,7 +90,7 @@ auto_aim_component: bullet_speed: 22.5 shoot_delay: 0.07 offset_yaw: +0.2 - offset_pitch: -0.4 + offset_pitch: -1.2 attack_window: 120.0 degraded_angle_speed: 12.0 window_redundancy: 0.6 diff --git a/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp b/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp index 2951f53f8..b6bf346ef 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp @@ -31,6 +31,7 @@ class PidCalculator { double update(double err) { if (!std::isfinite(err)) { + reset(); return nan; } else { double control = kp * err; From aef4a397f10369f2db30747d0f796b8918d2bfc9 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Thu, 6 Aug 2026 14:05:58 +0800 Subject: [PATCH 83/86] fix: fix HeatController instead of HeatControllerRune" --- rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index e526a1fa1..714498a00 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -14,7 +14,7 @@ rmcs_executor: - rmcs_core::controller::gimbal::DeformableInfantryGimbalController -> gimbal_controller - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller - - rmcs_core::controller::shooting::HeatControllerRune -> heat_controller + - rmcs_core::controller::shooting::HeatController -> heat_controller - rmcs_core::controller::shooting::BulletFeederController17mm -> bullet_feeder_controller - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller From 9a139dd0d0c76b2817684e14418f5878df42b7eb Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Wed, 12 Aug 2026 00:17:33 +0800 Subject: [PATCH 84/86] feat: add MVSUSB3Core and ROS mavlink header installation to bootstrap script; remove unused yaw feedback timeouts from gimbal controller and chassis files --- .../config/deformable-infantry-omni-b.yaml | 5 +- .../config/deformable-infantry-omni-c.yaml | 5 +- .../config/deformable-infantry-omni.yaml | 9 +- rmcs_ws/src/rmcs_core/plugins.xml | 1 - .../controller/chassis/deformable_chassis.cpp | 157 +++++-- .../chassis/deformable_chassis_rune.cpp | 393 ------------------ .../deformable_infantry_gimbal_controller.cpp | 34 +- .../hardware/deformable-infantry-omni-b.cpp | 18 - .../hardware/deformable-infantry-omni-c.cpp | 18 - .../src/hardware/deformable-infantry-omni.cpp | 18 - 10 files changed, 118 insertions(+), 540 deletions(-) delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_rune.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index a2f96a8cf..f949a5bb2 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -20,7 +20,7 @@ rmcs_executor: - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller - - rmcs_core::controller::chassis::DeformableChassisRune -> chassis_controller + - rmcs_core::controller::chassis::DeformableChassis -> chassis_controller - rmcs_core::controller::chassis::DeformableSuspension -> deformable_suspension - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::DeformableOmniWheelController -> deformable_chassis_controller @@ -190,9 +190,6 @@ gimbal_controller: lower_limit: 0.10 ctrl_hold_pitch_target_angle: 0.0 - yaw_velocity_feedback_timeout_ms: 100 - yaw_angle_feedback_timeout_ms: 100 - yaw_angle_kp: 10.0 yaw_angle_ki: 0.0 yaw_angle_kd: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml index dd95c01e4..87e512a27 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml @@ -20,7 +20,7 @@ rmcs_executor: - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller - - rmcs_core::controller::chassis::DeformableChassisRune -> chassis_controller + - rmcs_core::controller::chassis::DeformableChassis -> chassis_controller - rmcs_core::controller::chassis::DeformableSuspension -> deformable_suspension - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::DeformableOmniWheelController -> deformable_chassis_controller @@ -182,9 +182,6 @@ gimbal_controller: lower_limit: 0.10 # 8 deg ctrl_hold_pitch_target_angle: 0.0 - yaw_velocity_feedback_timeout_ms: 100 - yaw_angle_feedback_timeout_ms: 100 - yaw_angle_kp: 10.0 yaw_angle_ki: 0.0 yaw_angle_kd: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 714498a00..fa9d189c0 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -20,7 +20,7 @@ rmcs_executor: - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller - - rmcs_core::controller::chassis::DeformableChassisRune -> chassis_controller + - rmcs_core::controller::chassis::DeformableChassis -> chassis_controller - rmcs_core::controller::chassis::DeformableSuspension -> deformable_suspension - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::DeformableOmniWheelController -> deformable_chassis_controller @@ -126,10 +126,6 @@ chassis_controller: max_angle: 59.0 active_suspension_enable: true wireless_charging_offset_deg: 45.0 - charging_high_support_deg: 59.0 - charging_high_raised_deg: 33.0 - charging_low_support_deg: 22.5 - charging_low_raised_deg: 8.0 wireless_charging_speed_limit: 0.6 wireless_charging_angular_velocity_limit: 10.0 @@ -184,9 +180,6 @@ gimbal_controller: lower_limit: 0.10 # 8 deg ctrl_hold_pitch_target_angle: 0.0 - yaw_velocity_feedback_timeout_ms: 100 - yaw_angle_feedback_timeout_ms: 100 - yaw_angle_kp: 10.0 yaw_angle_ki: 0.0 yaw_angle_kd: 0.0 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 610cf396d..193dff619 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -21,7 +21,6 @@ - diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index f14d29003..a0a9fb6f4 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -48,6 +48,10 @@ class DeformableChassis register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); + register_input("/auto_aim/single_shoot", auto_aim_single_shoot_, false); + register_input("/auto_aim/robot_center", auto_aim_robot_center_, false); + register_input("/tf", tf_, false); + register_output("/chassis/angle", chassis_angle_, nan_); register_output("/chassis/control_angle", chassis_control_angle_, nan_); register_output("/chassis/control_mode", mode_); @@ -125,6 +129,10 @@ class DeformableChassis *suspension_reference_angle_deg_ = joint_mode_mgr_.suspension_reference_angle_deg(); publish_joint_posture_targets_(); + update_auto_aim_override_state_(); + if (auto_aim_posture_override_) + apply_auto_aim_posture_override_(); + update_velocity_control(); } while (false); } @@ -156,6 +164,32 @@ class DeformableChassis *chassis_control_angle_ = nan_; } + void update_auto_aim_override_state_() { + auto_aim_posture_override_ = auto_aim_single_shoot_.ready() && *auto_aim_single_shoot_; + auto_aim_vector_follow_ = // + auto_aim_posture_override_ && tf_.ready() && auto_aim_robot_center_.ready() + && auto_aim_robot_center_->allFinite() && !auto_aim_robot_center_->isZero(); + } + + void apply_auto_aim_posture_override_() { + const double front_rad = deg_to_rad(joint_mode_mgr_.max_angle()); + const double back_rad = deg_to_rad(joint_mode_mgr_.min_angle()); + *joint_posture_target_angle_rad_[kLeftFront] = front_rad; + *joint_posture_target_angle_rad_[kRightFront] = front_rad; + *joint_posture_target_angle_rad_[kLeftBack] = back_rad; + *joint_posture_target_angle_rad_[kRightBack] = back_rad; + + *symmetric_posture_target_ = false; + *low_prone_active_ = false; + *active_suspension_active_ = false; + + const double min_deg = joint_mode_mgr_.min_angle(); + const double max_deg = joint_mode_mgr_.max_angle(); + const double reference_deg = (min_deg + max_deg) / 2.0; + *suspension_reference_angle_deg_ = reference_deg; + *correction_inverted_ = reference_deg > (min_deg - 5.0 + max_deg) / 2.0; + } + double update_dt() const { if (update_rate_.ready() && std::isfinite(*update_rate_) && *update_rate_ > 1e-6) return 1.0 / *update_rate_; @@ -189,50 +223,54 @@ class DeformableChassis double angular_velocity = 0.0; double chassis_control_angle = nan_; - switch (*mode_) { - case rmcs_msgs::ChassisMode::AUTO: break; - - case rmcs_msgs::ChassisMode::SPIN_FAST: { - bool forward = joint_mode_mgr_.spinning_forward(); - angular_velocity = forward ? angular_velocity_max_ : -angular_velocity_max_; - angular_velocity = - std::clamp(angular_velocity, -angular_velocity_max_, angular_velocity_max_); - } break; - - case rmcs_msgs::ChassisMode::STEP_DOWN: { - double chassis_angle_error = - calculate_unsigned_chassis_angle_error(chassis_control_angle); - - constexpr double alignment = std::numbers::pi; - while (chassis_angle_error > alignment / 2) { - chassis_control_angle -= alignment; - if (chassis_control_angle < 0) - chassis_control_angle += 2 * std::numbers::pi; - chassis_angle_error -= alignment; + if (auto_aim_posture_override_) { + angular_velocity = update_auto_aim_override_angular_velocity_(chassis_control_angle); + } else { + switch (*mode_) { + case rmcs_msgs::ChassisMode::AUTO: break; + + case rmcs_msgs::ChassisMode::SPIN_FAST: { + bool forward = joint_mode_mgr_.spinning_forward(); + angular_velocity = forward ? angular_velocity_max_ : -angular_velocity_max_; + angular_velocity = + std::clamp(angular_velocity, -angular_velocity_max_, angular_velocity_max_); + } break; + + case rmcs_msgs::ChassisMode::STEP_DOWN: { + double chassis_angle_error = + calculate_unsigned_chassis_angle_error(chassis_control_angle); + + constexpr double alignment = std::numbers::pi; + while (chassis_angle_error > alignment / 2) { + chassis_control_angle -= alignment; + if (chassis_control_angle < 0) + chassis_control_angle += 2 * std::numbers::pi; + chassis_angle_error -= alignment; + } + + angular_velocity = following_velocity_controller_.update(chassis_angle_error); + } break; + + case rmcs_msgs::ChassisMode::WIRELESS_CHARGING: { + const double wireless_charging_offset_rad = + joint_mode_mgr_.wireless_charging_offset_rad(); + double chassis_angle_error = + calculate_unsigned_chassis_angle_error(chassis_control_angle); + + chassis_control_angle = + normalize_positive_angle(chassis_control_angle - wireless_charging_offset_rad); + chassis_angle_error = + normalize_positive_angle(chassis_angle_error - wireless_charging_offset_rad); + chassis_angle_error = normalize_signed_angle(chassis_angle_error); + + angular_velocity = following_velocity_controller_.update(chassis_angle_error); + angular_velocity = std::clamp( + angular_velocity, -wireless_charging_angular_velocity_limit_, + wireless_charging_angular_velocity_limit_); + } break; + + default: break; } - - angular_velocity = following_velocity_controller_.update(chassis_angle_error); - } break; - - case rmcs_msgs::ChassisMode::WIRELESS_CHARGING: { - const double wireless_charging_offset_rad = - joint_mode_mgr_.wireless_charging_offset_rad(); - double chassis_angle_error = - calculate_unsigned_chassis_angle_error(chassis_control_angle); - - chassis_control_angle = - normalize_positive_angle(chassis_control_angle - wireless_charging_offset_rad); - chassis_angle_error = - normalize_positive_angle(chassis_angle_error - wireless_charging_offset_rad); - chassis_angle_error = normalize_signed_angle(chassis_angle_error); - - angular_velocity = following_velocity_controller_.update(chassis_angle_error); - angular_velocity = std::clamp( - angular_velocity, -wireless_charging_angular_velocity_limit_, - wireless_charging_angular_velocity_limit_); - } break; - - default: break; } *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; @@ -241,6 +279,26 @@ class DeformableChassis return angular_velocity; } + double update_auto_aim_override_angular_velocity_(double& chassis_control_angle) { + double chassis_angle_error; + if (auto_aim_vector_follow_) { + const auto target_in_base = fast_tf::cast( + rmcs_description::OdomImu::Position{*auto_aim_robot_center_}, *tf_); + chassis_angle_error = target_in_base->head<2>().norm() > 1e-6 + ? std::atan2(target_in_base->y(), target_in_base->x()) + : 0.0; + chassis_control_angle = normalize_positive_angle( + 2 * std::numbers::pi - *gimbal_yaw_angle_ + chassis_angle_error); + } else { + chassis_angle_error = normalize_signed_angle( + calculate_unsigned_chassis_angle_error(chassis_control_angle)); + } + + return std::clamp( + following_velocity_controller_.update(chassis_angle_error), -angular_velocity_max_, + angular_velocity_max_); + } + double calculate_unsigned_chassis_angle_error(double& chassis_control_angle) { chassis_control_angle = *gimbal_yaw_angle_error_; if (chassis_control_angle < 0) @@ -285,6 +343,10 @@ class DeformableChassis "right_back", "right_front", }; + static constexpr size_t kLeftFront = 0; + static constexpr size_t kLeftBack = 1; + static constexpr size_t kRightBack = 2; + static constexpr size_t kRightFront = 3; InputInterface joystick_right_; InputInterface switch_right_; @@ -296,6 +358,12 @@ class DeformableChassis InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; OutputInterface chassis_angle_, chassis_control_angle_; + InputInterface auto_aim_single_shoot_; + InputInterface auto_aim_robot_center_; + InputInterface tf_; + bool auto_aim_posture_override_ = false; + bool auto_aim_vector_follow_ = false; + OutputInterface mode_; OutputInterface chassis_control_velocity_; OutputInterface pitch_lock_active_; @@ -321,4 +389,5 @@ class DeformableChassis #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::DeformableChassis, rmcs_executor::Component) +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::DeformableChassis, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_rune.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_rune.cpp deleted file mode 100644 index 9f1e000fa..000000000 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_rune.cpp +++ /dev/null @@ -1,393 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "controller/chassis/deformable_mode.hpp" -#include "controller/pid/pid_calculator.hpp" - -namespace rmcs_core::controller::chassis { - -class DeformableChassisRune - : public rmcs_executor::Component - , public rclcpp::Node { -public: - explicit DeformableChassisRune() - : Node( - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , following_velocity_controller_(10.0, 0.0, 0.0) - , wireless_charging_speed_limit_(get_parameter_or("wireless_charging_speed_limit", 0.2)) - , wireless_charging_angular_velocity_limit_( - get_parameter_or("wireless_charging_angular_velocity_limit", 3.0)) - , joint_mode_mgr_(*this) { - - following_velocity_controller_.output_max = angular_velocity_max_; - following_velocity_controller_.output_min = -angular_velocity_max_; - - register_input("/remote/joystick/right", joystick_right_); - register_input("/remote/switch/right", switch_right_); - register_input("/remote/switch/left", switch_left_); - register_input("/remote/keyboard", keyboard_); - register_input("/remote/rotary_knob", rotary_knob_); - register_input("/predefined/update_rate", update_rate_); - - register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); - register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); - - register_input("/auto_aim/single_shoot", auto_aim_single_shoot_, false); - register_input("/auto_aim/robot_center", auto_aim_robot_center_, false); - register_input("/tf", tf_, false); - - register_output("/chassis/angle", chassis_angle_, nan_); - register_output("/chassis/control_angle", chassis_control_angle_, nan_); - register_output("/chassis/control_mode", mode_); - register_output("/chassis/control_velocity", chassis_control_velocity_); - register_output("/chassis/pitch_lock_active", pitch_lock_active_, false); - register_output("/chassis/active_suspension/active", active_suspension_active_, false); - register_output("/chassis/deformable/low_prone_active", low_prone_active_, false); - register_output( - "/chassis/deformable/symmetric_posture_target", symmetric_posture_target_, true); - register_output("/chassis/deformable/correction_inverted", correction_inverted_, false); - register_output( - "/chassis/deformable/min_angle_deg", min_angle_deg_, joint_mode_mgr_.min_angle()); - register_output( - "/chassis/deformable/max_angle_deg", max_angle_deg_, joint_mode_mgr_.max_angle()); - register_output( - "/chassis/deformable/suspension_reference_angle_deg", suspension_reference_angle_deg_, - joint_mode_mgr_.suspension_reference_angle_deg()); - register_output( - "/chassis/deformable/reset_count", deformable_reset_count_, static_cast(0)); - for (size_t i = 0; i < kJointCount; ++i) { - register_output( - fmt::format("/chassis/deformable/{}_joint/posture_target_angle", kJointName[i]), - joint_posture_target_angle_rad_[i], deg_to_rad(joint_mode_mgr_.max_angle())); - } - - *mode_ = rmcs_msgs::ChassisMode::AUTO; - *pitch_lock_active_ = false; - *active_suspension_active_ = false; - *low_prone_active_ = false; - *symmetric_posture_target_ = true; - *correction_inverted_ = false; - chassis_control_velocity_->vector << nan_, nan_, nan_; - } - - void before_updating() override { - if (!gimbal_yaw_angle_.ready()) { - gimbal_yaw_angle_.make_and_bind_directly(0.0); - RCLCPP_WARN(get_logger(), "Failed to fetch \"/gimbal/yaw/angle\". Set to 0.0."); - } - if (!gimbal_yaw_angle_error_.ready()) { - gimbal_yaw_angle_error_.make_and_bind_directly(0.0); - RCLCPP_WARN( - get_logger(), "Failed to fetch \"/gimbal/yaw/control_angle_error\". " - "Set to 0.0."); - } - } - - void update() override { - using rmcs_msgs::Switch; - - const auto switch_right = *switch_right_; - const auto switch_left = *switch_left_; - const auto keyboard = *keyboard_; - - do { - if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) - || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { - reset_all_controls(); - break; - } - - double rotary_knob = rotary_knob_.ready() ? *rotary_knob_ : 0.0; - - joint_mode_mgr_.update( - switch_left, switch_right, keyboard, rotary_knob, update_dt(), *gimbal_yaw_angle_); - - *mode_ = joint_mode_mgr_.mode(); - *pitch_lock_active_ = joint_mode_mgr_.pitch_lock_active(); - *active_suspension_active_ = joint_mode_mgr_.suspension_active(); - *low_prone_active_ = joint_mode_mgr_.low_prone_active(); - *symmetric_posture_target_ = joint_mode_mgr_.symmetric_posture_target(); - *correction_inverted_ = joint_mode_mgr_.correction_inverted(); - *min_angle_deg_ = joint_mode_mgr_.min_angle(); - *max_angle_deg_ = joint_mode_mgr_.max_angle(); - *suspension_reference_angle_deg_ = joint_mode_mgr_.suspension_reference_angle_deg(); - publish_joint_posture_targets_(); - - update_auto_aim_override_state_(); - if (auto_aim_posture_override_) - apply_auto_aim_posture_override_(); - - update_velocity_control(); - } while (false); - } - -private: - static constexpr size_t kJointCount = 4; - static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - static constexpr double translational_velocity_max_ = 10.0; - static constexpr double angular_velocity_max_ = 30.0; - static constexpr double default_dt_ = 1e-3; - - void reset_all_controls() { - joint_mode_mgr_.reset(); - *deformable_reset_count_ += 1; - - *mode_ = rmcs_msgs::ChassisMode::AUTO; - *pitch_lock_active_ = false; - *active_suspension_active_ = false; - *low_prone_active_ = false; - *symmetric_posture_target_ = true; - *correction_inverted_ = false; - *min_angle_deg_ = joint_mode_mgr_.min_angle(); - *max_angle_deg_ = joint_mode_mgr_.max_angle(); - *suspension_reference_angle_deg_ = joint_mode_mgr_.suspension_reference_angle_deg(); - publish_joint_posture_targets_(); - - chassis_control_velocity_->vector << nan_, nan_, nan_; - *chassis_angle_ = nan_; - *chassis_control_angle_ = nan_; - } - - void update_auto_aim_override_state_() { - auto_aim_posture_override_ = auto_aim_single_shoot_.ready() && *auto_aim_single_shoot_; - auto_aim_vector_follow_ = // - auto_aim_posture_override_ && tf_.ready() && auto_aim_robot_center_.ready() - && auto_aim_robot_center_->allFinite() && !auto_aim_robot_center_->isZero(); - } - - void apply_auto_aim_posture_override_() { - const double front_rad = deg_to_rad(joint_mode_mgr_.max_angle()); - const double back_rad = deg_to_rad(joint_mode_mgr_.min_angle()); - *joint_posture_target_angle_rad_[kLeftFront] = front_rad; - *joint_posture_target_angle_rad_[kRightFront] = front_rad; - *joint_posture_target_angle_rad_[kLeftBack] = back_rad; - *joint_posture_target_angle_rad_[kRightBack] = back_rad; - - *symmetric_posture_target_ = false; - *low_prone_active_ = false; - *active_suspension_active_ = false; - - const double min_deg = joint_mode_mgr_.min_angle(); - const double max_deg = joint_mode_mgr_.max_angle(); - const double reference_deg = (min_deg + max_deg) / 2.0; - *suspension_reference_angle_deg_ = reference_deg; - *correction_inverted_ = reference_deg > (min_deg - 5.0 + max_deg) / 2.0; - } - - double update_dt() const { - if (update_rate_.ready() && std::isfinite(*update_rate_) && *update_rate_ > 1e-6) - return 1.0 / *update_rate_; - return default_dt_; - } - - void update_velocity_control() { - Eigen::Vector2d translational_velocity = update_translational_velocity_control(); - double angular_velocity = update_angular_velocity_control(); - chassis_control_velocity_->vector << translational_velocity, angular_velocity; - } - - Eigen::Vector2d update_translational_velocity_control() { - const auto keyboard = *keyboard_; - Eigen::Vector2d keyboard_move{keyboard.w - keyboard.s, keyboard.a - keyboard.d}; - - Eigen::Vector2d translational_velocity = - Eigen::Rotation2Dd{*gimbal_yaw_angle_} * (*joystick_right_ + keyboard_move); - - if (translational_velocity.norm() > 1.0) - translational_velocity.normalize(); - - const double max_speed = *mode_ == rmcs_msgs::ChassisMode::WIRELESS_CHARGING - ? wireless_charging_speed_limit_ - : translational_velocity_max_; - translational_velocity *= max_speed; - return translational_velocity; - } - - double update_angular_velocity_control() { - double angular_velocity = 0.0; - double chassis_control_angle = nan_; - - if (auto_aim_posture_override_) { - angular_velocity = update_auto_aim_override_angular_velocity_(chassis_control_angle); - } else { - switch (*mode_) { - case rmcs_msgs::ChassisMode::AUTO: break; - - case rmcs_msgs::ChassisMode::SPIN_FAST: { - bool forward = joint_mode_mgr_.spinning_forward(); - angular_velocity = forward ? angular_velocity_max_ : -angular_velocity_max_; - angular_velocity = - std::clamp(angular_velocity, -angular_velocity_max_, angular_velocity_max_); - } break; - - case rmcs_msgs::ChassisMode::STEP_DOWN: { - double chassis_angle_error = - calculate_unsigned_chassis_angle_error(chassis_control_angle); - - constexpr double alignment = std::numbers::pi; - while (chassis_angle_error > alignment / 2) { - chassis_control_angle -= alignment; - if (chassis_control_angle < 0) - chassis_control_angle += 2 * std::numbers::pi; - chassis_angle_error -= alignment; - } - - angular_velocity = following_velocity_controller_.update(chassis_angle_error); - } break; - - case rmcs_msgs::ChassisMode::WIRELESS_CHARGING: { - const double wireless_charging_offset_rad = - joint_mode_mgr_.wireless_charging_offset_rad(); - double chassis_angle_error = - calculate_unsigned_chassis_angle_error(chassis_control_angle); - - chassis_control_angle = - normalize_positive_angle(chassis_control_angle - wireless_charging_offset_rad); - chassis_angle_error = - normalize_positive_angle(chassis_angle_error - wireless_charging_offset_rad); - chassis_angle_error = normalize_signed_angle(chassis_angle_error); - - angular_velocity = following_velocity_controller_.update(chassis_angle_error); - angular_velocity = std::clamp( - angular_velocity, -wireless_charging_angular_velocity_limit_, - wireless_charging_angular_velocity_limit_); - } break; - - default: break; - } - } - - *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; - *chassis_control_angle_ = chassis_control_angle; - - return angular_velocity; - } - - double update_auto_aim_override_angular_velocity_(double& chassis_control_angle) { - double chassis_angle_error; - if (auto_aim_vector_follow_) { - const auto target_in_base = fast_tf::cast( - rmcs_description::OdomImu::Position{*auto_aim_robot_center_}, *tf_); - chassis_angle_error = target_in_base->head<2>().norm() > 1e-6 - ? std::atan2(target_in_base->y(), target_in_base->x()) - : 0.0; - chassis_control_angle = normalize_positive_angle( - 2 * std::numbers::pi - *gimbal_yaw_angle_ + chassis_angle_error); - } else { - chassis_angle_error = normalize_signed_angle( - calculate_unsigned_chassis_angle_error(chassis_control_angle)); - } - - return std::clamp( - following_velocity_controller_.update(chassis_angle_error), -angular_velocity_max_, - angular_velocity_max_); - } - - double calculate_unsigned_chassis_angle_error(double& chassis_control_angle) { - chassis_control_angle = *gimbal_yaw_angle_error_; - if (chassis_control_angle < 0) - chassis_control_angle += 2 * std::numbers::pi; - - double unsigned_angle_error = chassis_control_angle + *gimbal_yaw_angle_; - if (unsigned_angle_error >= 2 * std::numbers::pi) - unsigned_angle_error -= 2 * std::numbers::pi; - - return unsigned_angle_error; - } - - static double deg_to_rad(double deg) { return deg * std::numbers::pi / 180.0; } - - static double normalize_positive_angle(double angle) { - constexpr double full_turn = 2 * std::numbers::pi; - while (angle >= full_turn) - angle -= full_turn; - while (angle < 0.0) - angle += full_turn; - return angle; - } - - static double normalize_signed_angle(double angle) { - angle = normalize_positive_angle(angle); - if (angle > std::numbers::pi) - angle -= 2 * std::numbers::pi; - return angle; - } - - void publish_joint_posture_targets_() { - std::array targets_deg{}; - joint_mode_mgr_.copy_joint_posture_target_deg(targets_deg); - - for (size_t i = 0; i < kJointCount; ++i) - *joint_posture_target_angle_rad_[i] = deg_to_rad(targets_deg[i]); - } - - static constexpr const char* kJointName[] = { - "left_front", - "left_back", - "right_back", - "right_front", - }; - static constexpr size_t kLeftFront = 0; - static constexpr size_t kLeftBack = 1; - static constexpr size_t kRightBack = 2; - static constexpr size_t kRightFront = 3; - - InputInterface joystick_right_; - InputInterface switch_right_; - InputInterface switch_left_; - InputInterface keyboard_; - InputInterface rotary_knob_; - InputInterface update_rate_; - - InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; - OutputInterface chassis_angle_, chassis_control_angle_; - - InputInterface auto_aim_single_shoot_; - InputInterface auto_aim_robot_center_; - InputInterface tf_; - bool auto_aim_posture_override_ = false; - bool auto_aim_vector_follow_ = false; - - OutputInterface mode_; - OutputInterface chassis_control_velocity_; - OutputInterface pitch_lock_active_; - OutputInterface active_suspension_active_; - OutputInterface low_prone_active_; - OutputInterface symmetric_posture_target_; - OutputInterface correction_inverted_; - OutputInterface min_angle_deg_; - OutputInterface max_angle_deg_; - OutputInterface suspension_reference_angle_deg_; - OutputInterface deformable_reset_count_; - std::array, kJointCount> joint_posture_target_angle_rad_; - - pid::PidCalculator following_velocity_controller_; - - double wireless_charging_speed_limit_; - double wireless_charging_angular_velocity_limit_; - - DeformableChassisModeManager joint_mode_mgr_; -}; - -} // namespace rmcs_core::controller::chassis - -#include - -PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::chassis::DeformableChassisRune, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp index f2cb7e9a3..6690e8841 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp @@ -1,9 +1,7 @@ #include "controller/gimbal/two_axis_gimbal_solver.hpp" #include "controller/pid/pid_calculator.hpp" -#include #include -#include #include #include @@ -44,11 +42,6 @@ class DeformableInfantryGimbalController get_parameter_or("pitch_velocity_ff_gain", pitch_velocity_ff_gain_, 0.0); get_parameter_or("pitch_acceleration_ff_gain", pitch_acceleration_ff_gain_, 0.0); get_parameter_or("ctrl_hold_pitch_target_angle", ctrl_hold_pitch_target_angle_, 0.0); - get_parameter_or( - "yaw_velocity_feedback_timeout_ms", yaw_velocity_feedback_timeout_ms_, - std::int64_t{100}); - get_parameter_or( - "yaw_angle_feedback_timeout_ms", yaw_angle_feedback_timeout_ms_, std::int64_t{100}); } auto update() -> void override { @@ -83,16 +76,13 @@ class DeformableInfantryGimbalController if (!ctrl_hold_active_) *output_.pitch_angle_error = angle_error.pitch_angle_error; - const bool yaw_feedback_stale = yaw_feedback_stale_(); const auto trajectory_ff = trajectory_feedforward(auto_aim_active); - if (yaw_feedback_stale || !std::isfinite(angle_error.yaw_angle_error)) { + if (!std::isfinite(angle_error.yaw_angle_error)) { yaw_angle_pid_.reset(); yaw_velocity_pid_.reset(); *output_.yaw_control_torque = kNaN; - } - - if (!yaw_feedback_stale && std::isfinite(angle_error.yaw_angle_error)) { + } else { const auto yaw_velocity_ref = yaw_angle_pid_.update(angle_error.yaw_angle_error) + trajectory_ff.yaw_ref_velocity; *output_.yaw_control_torque = @@ -159,9 +149,6 @@ class DeformableInfantryGimbalController "/auto_aim/control_direction", auto_aim_control_direction, false); component.register_input("/auto_aim/ff_v", auto_aim_ff_v, false); component.register_input("/auto_aim/ff_a", auto_aim_ff_a, false); - component.register_input( - "/gimbal/yaw/velocity_imu_timestamp", yaw_velocity_imu_timestamp, false); - component.register_input("/gimbal/yaw/angle_timestamp", yaw_angle_timestamp, false); } InputInterface joystick_left; @@ -180,8 +167,6 @@ class DeformableInfantryGimbalController InputInterface auto_aim_control_direction; InputInterface auto_aim_ff_v; InputInterface auto_aim_ff_a; - InputInterface yaw_velocity_imu_timestamp; - InputInterface yaw_angle_timestamp; } input_{*this}; struct Output { @@ -214,18 +199,6 @@ class DeformableInfantryGimbalController return kDefaultDt; } - auto yaw_feedback_stale_() const -> bool { - const auto now_ns = std::chrono::steady_clock::now().time_since_epoch().count(); - if (input_.yaw_velocity_imu_timestamp.ready() - && now_ns - *input_.yaw_velocity_imu_timestamp - > yaw_velocity_feedback_timeout_ms_ * 1'000'000) - return true; - if (input_.yaw_angle_timestamp.ready() - && now_ns - *input_.yaw_angle_timestamp > yaw_angle_feedback_timeout_ms_ * 1'000'000) - return true; - return false; - } - auto manual_yaw_shift() const -> double { return joystick_sensitivity_ * input_.joystick_left->y() + mouse_sensitivity_ * input_.mouse_velocity->y(); @@ -393,9 +366,6 @@ class DeformableInfantryGimbalController get_parameter("pitch_velocity_kd").as_double(), }; - std::int64_t yaw_velocity_feedback_timeout_ms_ = 100; - std::int64_t yaw_angle_feedback_timeout_ms_ = 100; - double joystick_sensitivity_ = 0.003; double mouse_sensitivity_ = 0.5; bool pitch_torque_control_enabled_ = false; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index 9a57455a4..703e30e51 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -166,8 +166,6 @@ class DeformableInfantryOmniB status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); - status.register_output( - "/gimbal/yaw/velocity_imu_timestamp", gimbal_yaw_velocity_imu_timestamp_); status.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); status.register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); @@ -206,8 +204,6 @@ class DeformableInfantryOmniB gimbal_pitch_motor_.update_status(); gimbal_left_friction_.update_status(); gimbal_right_friction_.update_status(); - *gimbal_yaw_velocity_imu_timestamp_ = - last_gyro_receive_ns_.load(std::memory_order::relaxed); const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); @@ -284,8 +280,6 @@ class DeformableInfantryOmniB } void gyroscope_receive_callback(const View::ImuGyroscope& data) override { - last_gyro_receive_ns_.store( - Clock::now().time_since_epoch().count(), std::memory_order::relaxed); const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); monitor_.tick("Top::Imu", "Gyr"); if (!timestamp.has_value()) @@ -317,9 +311,6 @@ class DeformableInfantryOmniB OutputInterface& tf_; OutputInterface gimbal_yaw_velocity_bmi088_; OutputInterface gimbal_pitch_velocity_bmi088_; - OutputInterface gimbal_yaw_velocity_imu_timestamp_; - - std::atomic last_gyro_receive_ns_{Clock::now().time_since_epoch().count()}; EventOutputInterface imu_snapshot_output_; EventOutputInterface camera_signal_output_; @@ -362,8 +353,6 @@ class DeformableInfantryOmniB device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( static_cast(status.get_parameter("yaw_motor_zero_point").as_int()))); - status.register_output("/gimbal/yaw/angle_timestamp", gimbal_yaw_angle_timestamp_); - for (auto& motor : chassis_wheel_motors_) motor.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} @@ -463,8 +452,6 @@ class DeformableInfantryOmniB dr16_.update_status(); gimbal_yaw_motor_.update_status(); - *gimbal_yaw_angle_timestamp_ = - last_yaw_status_receive_ns_.load(std::memory_order::relaxed); if (supercap_status_received_.load(std::memory_order_relaxed)) supercap_.update_status(); if (debug_log_supercap_) @@ -648,9 +635,6 @@ class DeformableInfantryOmniB // Device - OutputInterface gimbal_yaw_angle_timestamp_; - std::atomic last_yaw_status_receive_ns_{Clock::now().time_since_epoch().count()}; - device::Bmi088 imu_{1000, 0.2, 0.0}; device::LkMotor gimbal_yaw_motor_{status_, command_, "/gimbal/yaw"}; device::Dr16 dr16_; @@ -917,8 +901,6 @@ class DeformableInfantryOmniB return; if (data.can_id == 0x142) { gimbal_yaw_motor_.store_status(data.can_data); - last_yaw_status_receive_ns_.store( - Clock::now().time_since_epoch().count(), std::memory_order::relaxed); } else if (data.can_id == 0x203) gimbal_bullet_feeder_.store_status(data.can_data); monitor_.tick("Bottom::Can2", data.can_id); diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp index 51ff819d1..29f22d4ee 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp @@ -166,8 +166,6 @@ class DeformableInfantryOmniC status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); - status.register_output( - "/gimbal/yaw/velocity_imu_timestamp", gimbal_yaw_velocity_imu_timestamp_); status.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); status.register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); @@ -207,8 +205,6 @@ class DeformableInfantryOmniC gimbal_pitch_motor_.update_status(); gimbal_left_friction_.update_status(); gimbal_right_friction_.update_status(); - *gimbal_yaw_velocity_imu_timestamp_ = - last_gyro_receive_ns_.load(std::memory_order::relaxed); const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); @@ -285,8 +281,6 @@ class DeformableInfantryOmniC } void gyroscope_receive_callback(const View::ImuGyroscope& data) override { - last_gyro_receive_ns_.store( - Clock::now().time_since_epoch().count(), std::memory_order::relaxed); const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); monitor_.tick("Top::Imu", "Gyr"); if (!timestamp.has_value()) @@ -318,9 +312,6 @@ class DeformableInfantryOmniC OutputInterface& tf_; OutputInterface gimbal_yaw_velocity_bmi088_; OutputInterface gimbal_pitch_velocity_bmi088_; - OutputInterface gimbal_yaw_velocity_imu_timestamp_; - - std::atomic last_gyro_receive_ns_{Clock::now().time_since_epoch().count()}; EventOutputInterface imu_snapshot_output_; EventOutputInterface camera_signal_output_; @@ -363,8 +354,6 @@ class DeformableInfantryOmniC device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( static_cast(status.get_parameter("yaw_motor_zero_point").as_int()))); - status.register_output("/gimbal/yaw/angle_timestamp", gimbal_yaw_angle_timestamp_); - for (auto& motor : chassis_wheel_motors_) motor.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} @@ -464,8 +453,6 @@ class DeformableInfantryOmniC dr16_.update_status(); gimbal_yaw_motor_.update_status(); - *gimbal_yaw_angle_timestamp_ = - last_yaw_status_receive_ns_.load(std::memory_order::relaxed); if (supercap_status_received_.load(std::memory_order_relaxed)) supercap_.update_status(); if (debug_log_supercap_) @@ -649,9 +636,6 @@ class DeformableInfantryOmniC // Device - OutputInterface gimbal_yaw_angle_timestamp_; - std::atomic last_yaw_status_receive_ns_{Clock::now().time_since_epoch().count()}; - device::Bmi088 imu_{1000, 0.2, 0.0}; device::LkMotor gimbal_yaw_motor_{status_, command_, "/gimbal/yaw"}; device::Dr16 dr16_; @@ -916,8 +900,6 @@ class DeformableInfantryOmniC return; if (data.can_id == 0x142) { gimbal_yaw_motor_.store_status(data.can_data); - last_yaw_status_receive_ns_.store( - Clock::now().time_since_epoch().count(), std::memory_order::relaxed); } else if (data.can_id == 0x203) gimbal_bullet_feeder_.store_status(data.can_data); monitor_.tick("Bottom::Can2", data.can_id); diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index 69b648870..5b0670a96 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -166,8 +166,6 @@ class DeformableInfantryOmni status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); - status.register_output( - "/gimbal/yaw/velocity_imu_timestamp", gimbal_yaw_velocity_imu_timestamp_); status.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); status.register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); @@ -207,8 +205,6 @@ class DeformableInfantryOmni gimbal_pitch_motor_.update_status(); gimbal_left_friction_.update_status(); gimbal_right_friction_.update_status(); - *gimbal_yaw_velocity_imu_timestamp_ = - last_gyro_receive_ns_.load(std::memory_order::relaxed); const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); @@ -285,8 +281,6 @@ class DeformableInfantryOmni } void gyroscope_receive_callback(const View::ImuGyroscope& data) override { - last_gyro_receive_ns_.store( - Clock::now().time_since_epoch().count(), std::memory_order::relaxed); const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); monitor_.tick("Top::Imu", "Gyr"); if (!timestamp.has_value()) @@ -318,9 +312,6 @@ class DeformableInfantryOmni OutputInterface& tf_; OutputInterface gimbal_yaw_velocity_bmi088_; OutputInterface gimbal_pitch_velocity_bmi088_; - OutputInterface gimbal_yaw_velocity_imu_timestamp_; - - std::atomic last_gyro_receive_ns_{Clock::now().time_since_epoch().count()}; EventOutputInterface imu_snapshot_output_; EventOutputInterface camera_signal_output_; @@ -363,8 +354,6 @@ class DeformableInfantryOmni device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( static_cast(status.get_parameter("yaw_motor_zero_point").as_int()))); - status.register_output("/gimbal/yaw/angle_timestamp", gimbal_yaw_angle_timestamp_); - for (auto& motor : chassis_wheel_motors_) motor.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} @@ -464,8 +453,6 @@ class DeformableInfantryOmni dr16_.update_status(); gimbal_yaw_motor_.update_status(); - *gimbal_yaw_angle_timestamp_ = - last_yaw_status_receive_ns_.load(std::memory_order::relaxed); if (supercap_status_received_.load(std::memory_order_relaxed)) supercap_.update_status(); if (debug_log_supercap_) @@ -649,9 +636,6 @@ class DeformableInfantryOmni // Device - OutputInterface gimbal_yaw_angle_timestamp_; - std::atomic last_yaw_status_receive_ns_{Clock::now().time_since_epoch().count()}; - device::Bmi088 imu_{1000, 0.2, 0.0}; device::LkMotor gimbal_yaw_motor_{status_, command_, "/gimbal/yaw"}; device::Dr16 dr16_; @@ -918,8 +902,6 @@ class DeformableInfantryOmni return; if (data.can_id == 0x142) { gimbal_yaw_motor_.store_status(data.can_data); - last_yaw_status_receive_ns_.store( - Clock::now().time_since_epoch().count(), std::memory_order::relaxed); } else if (data.can_id == 0x203) gimbal_bullet_feeder_.store_status(data.can_data); monitor_.tick("Bottom::Can2", data.can_id); From 132b2aed91d140c6ec67a8a3ab4b0be153072c75 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Wed, 12 Aug 2026 00:50:48 +0800 Subject: [PATCH 85/86] chore: remove deformable debug logging and dead code - Remove debug_log_* switches and their supercap/wheel/joint/yaw-packet logging code from the deformable-infantry-omni hardware and configs - Drop write-only leftovers (wheel_status_received_, latest_supercap_status_) whose only readers were the removed debug logs - Align omni-c can_receive_callback with omni/-b: reject extended/remote CAN frames upfront - Remove unused getters and a self-assignment no-op from deformable_mode.hpp - Document the intentionally always-true correction-inverted expression in deformable_chassis.cpp --- .../config/deformable-infantry-omni-b.yaml | 5 - .../config/deformable-infantry-omni-c.yaml | 5 - .../config/deformable-infantry-omni.yaml | 3 - .../controller/chassis/deformable_chassis.cpp | 4 + .../controller/chassis/deformable_mode.hpp | 10 +- .../hardware/deformable-infantry-omni-b.cpp | 208 ----------------- .../hardware/deformable-infantry-omni-c.cpp | 210 +----------------- .../src/hardware/deformable-infantry-omni.cpp | 208 ----------------- 8 files changed, 7 insertions(+), 646 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index f949a5bb2..6da631fc8 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -124,11 +124,6 @@ deformable_infantry: rod_length: 0.140 yaw_motor_zero_point: 57900 pitch_motor_zero_point: 56354 - debug_log_supercap: false - debug_log_wheel_motor: false - debug_log_deformable_joint_motor: false - debug_log_yaw_packet: false - debug_log_yaw_packet_path: "/home/ubuntu/controller/yaw" chassis_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml index 87e512a27..a0f5dbcd3 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml @@ -116,11 +116,6 @@ deformable_infantry: rod_length: 0.140 yaw_motor_zero_point: 38910 pitch_motor_zero_point: 32214 - debug_log_supercap: false - debug_log_wheel_motor: false - debug_log_deformable_joint_motor: false - debug_log_yaw_packet: false - debug_log_yaw_packet_path: "/home/ubuntu/controller/yaw" chassis_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index fa9d189c0..8ebccfd87 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -116,9 +116,6 @@ deformable_infantry: rod_length: 0.140 yaw_motor_zero_point: 43365 pitch_motor_zero_point: 6432 - debug_log_supercap: false - debug_log_wheel_motor: false - debug_log_deformable_joint_motor: false chassis_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index a0a9fb6f4..a1af4e889 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -187,6 +187,10 @@ class DeformableChassis const double max_deg = joint_mode_mgr_.max_angle(); const double reference_deg = (min_deg + max_deg) / 2.0; *suspension_reference_angle_deg_ = reference_deg; + // Always true: reference_deg == (min_deg + max_deg) / 2.0 + // > (min_deg - 5.0 + max_deg) / 2.0 == reference_deg - 2.5. + // Under auto-aim low-prone override the correction direction is + // intentionally always inverted. *correction_inverted_ = reference_deg > (min_deg - 5.0 + max_deg) / 2.0; } diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp index 981bc2d7d..7e3af28cc 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp @@ -114,16 +114,10 @@ class DeformableChassisModeManager { out = joint_posture_state_.joint_posture_target_deg; } - const JointPostureState& joint_posture_state() const { return joint_posture_state_; } - double min_angle() const { return min_angle_; } double max_angle() const { return max_angle_; } - double max_angle_rad() const { return deg_to_rad_(max_angle_); } - double wireless_charging_offset_deg() const { return wireless_charging_offset_deg_; } double wireless_charging_offset_rad() const { return wireless_charging_offset_rad_; } - double active_suspension_min_angle_rad() const { return deg_to_rad_(min_angle_ - 5.0); } - bool correction_inverted() const { double midpoint = (min_angle_ - 5.0 + max_angle_) / 2.0; return joint_posture_state_.suspension_reference_angle_deg > midpoint; @@ -183,10 +177,8 @@ class DeformableChassisModeManager { const rmcs_msgs::Keyboard& keyboard) { auto next_mode = joint_posture_state_.mode; - if (switch_left == rmcs_msgs::Switch::DOWN) { - joint_posture_state_.mode = next_mode; + if (switch_left == rmcs_msgs::Switch::DOWN) return; - } if (last_switch_right_ == rmcs_msgs::Switch::MIDDLE && switch_right == rmcs_msgs::Switch::DOWN) { diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index 703e30e51..5d245cbaf 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -4,12 +4,8 @@ #include #include #include -#include -#include -#include #include #include -#include #include #include #include @@ -396,17 +392,6 @@ class DeformableInfantryOmniB status.register_output("/chassis/encoder/alpha_dot", encoder_alpha_dot_, kNaN); status.register_output("/chassis/radius", radius_, kDefaultRadius); - status.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); - status.get_parameter_or("debug_log_wheel_motor", debug_log_wheel_motor_, false); - status.get_parameter_or( - "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); - status.get_parameter_or("debug_log_yaw_packet", debug_log_yaw_packet_, false); - status.get_parameter_or( - "debug_log_yaw_packet_path", debug_log_yaw_packet_path_, - std::string{"/home/ubuntu/controller/yaw"}); - if (debug_log_yaw_packet_) - open_yaw_packet_log_files_(); - auto options = librmcs::board::AdvancedOptions{}; options.dangerously_skip_version_checks = true; board_ = std::make_unique(*this, serial_filter, options); @@ -447,15 +432,11 @@ class DeformableInfantryOmniB i, joint_physical_angle_[i], joint_physical_velocity_[i]); update_geometry_feedback_(); - if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) - log_chassis_feedback_once_per_second_(); dr16_.update_status(); gimbal_yaw_motor_.update_status(); if (supercap_status_received_.load(std::memory_order_relaxed)) supercap_.update_status(); - if (debug_log_supercap_) - log_supercap_feedback_once_per_second_(); gimbal_bullet_feeder_.update_status(); tf_->set_state( @@ -503,7 +484,6 @@ class DeformableInfantryOmniB .can_id = 0x200, .can_data = packet_can2_200.as_bytes(), }); - log_can2_packet_(true, 0x200, packet_can2_200.as_bytes()); builder.can_transmit( Spec::kCans.kCan3, // { @@ -524,7 +504,6 @@ class DeformableInfantryOmniB .can_id = 0x142, .can_data = packet_can2_142.as_bytes(), }); - log_can2_packet_(true, 0x142, packet_can2_142.as_bytes()); builder.can_transmit( Spec::kCans.kCan1, // { @@ -565,7 +544,6 @@ class DeformableInfantryOmniB .can_id = 0x141, .can_data = packet.as_bytes(), }); - log_can2_packet_(true, 0x141, packet.as_bytes()); break; } case kRightFront: @@ -611,28 +589,12 @@ class DeformableInfantryOmniB // State - std::atomic wheel_status_received_[4] = {false, false, false, false}; std::atomic joint_status_received_[4] = {false, false, false, false}; - bool debug_log_supercap_ = false; - bool debug_log_wheel_motor_ = false; - bool debug_log_deformable_joint_motor_ = false; - - bool debug_log_yaw_packet_ = false; - std::string debug_log_yaw_packet_path_; - std::ofstream can2_all_file_; - std::ofstream yaw_file_; - std::mutex log_mutex_; - size_t pending_rows_ = 0; - static constexpr size_t kYawLogFlushRows_ = 500; - const double kChassisRadiusBase; const double kRodLength; const double kDefaultRadius; - Clock::time_point next_chassis_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - Clock::time_point next_supercap_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - // Device device::Bmi088 imu_{1000, 0.2, 0.0}; @@ -652,7 +614,6 @@ class DeformableInfantryOmniB device::LkMotor{status_, command_, "/chassis/right_front_joint"}, }; - std::atomic latest_supercap_status_{device::CanPacket8{uint64_t{0}}}; std::atomic supercap_status_received_{false}; device::Supercap supercap_{status_, command_}; @@ -663,7 +624,6 @@ class DeformableInfantryOmniB return; if (data.can_id == 0x201) { chassis_wheel_motors_[index].store_status(data.can_data); - wheel_status_received_[index].store(true, std::memory_order_relaxed); } else if (data.can_id == 0x141) { chassis_joint_motors_[index].store_status(data.can_data); joint_status_received_[index].store(true, std::memory_order_relaxed); @@ -713,170 +673,6 @@ class DeformableInfantryOmniB *radius_ = (kChassisRadiusBase + kRodLength * alpha_rad.array().cos()).mean(); } - void log_chassis_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_chassis_feedback_log_time_) - return; - - const auto wheel_rx = [this](size_t index) { - return wheel_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - const auto joint_rx = [this](size_t index) { - return joint_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - - if (debug_log_wheel_motor_) { - std::string wheel_rx_str; - for (size_t i = 0; i < 4; ++i) { - if (i > 0) - wheel_rx_str.push_back(' '); - wheel_rx_str.push_back(wheel_rx(i)); - } - RCLCPP_INFO( - status_.get_logger(), - "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " - "rx=[%s]", - chassis_wheel_motors_[kLeftFront].angle(), - chassis_wheel_motors_[kLeftBack].angle(), - chassis_wheel_motors_[kRightBack].angle(), - chassis_wheel_motors_[kRightFront].angle(), - chassis_wheel_motors_[kLeftFront].angle(), - chassis_wheel_motors_[kLeftBack].angle(), - chassis_wheel_motors_[kRightBack].angle(), - chassis_wheel_motors_[kRightFront].angle(), wheel_rx_str.c_str()); - } - - if (debug_log_deformable_joint_motor_) { - std::string joint_rx_str; - for (size_t i = 0; i < 4; ++i) { - if (i > 0) - joint_rx_str.push_back(' '); - joint_rx_str.push_back(joint_rx(i)); - } - RCLCPP_INFO( - status_.get_logger(), - "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "rx=[%s]", - *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], - *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront], - *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], - *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront], - joint_rx_str.c_str()); - } - - next_chassis_feedback_log_time_ = now + std::chrono::seconds(1); - } - - void log_supercap_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_supercap_feedback_log_time_) - return; - - const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); - auto supercap_raw_packet = latest_supercap_status_.load(std::memory_order_relaxed); - const auto supercap_raw_bytes = supercap_raw_packet.as_bytes(); - - RCLCPP_INFO( - status_.get_logger(), - "[supercap] can1 rx=%c id=0x300 enabled=%d supercap_v=% .3f chassis_v=% .3f " - "power=% .3f raw=[%02X %02X %02X %02X %02X %02X %02X %02X]", - supercap_rx ? 'Y' : 'N', supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, - supercap_rx ? supercap_.supercap_voltage() : kNaN, - supercap_rx ? supercap_.chassis_voltage() : kNaN, - supercap_rx ? supercap_.chassis_power() : kNaN, - std::to_integer(supercap_raw_bytes[0]), - std::to_integer(supercap_raw_bytes[1]), - std::to_integer(supercap_raw_bytes[2]), - std::to_integer(supercap_raw_bytes[3]), - std::to_integer(supercap_raw_bytes[4]), - std::to_integer(supercap_raw_bytes[5]), - std::to_integer(supercap_raw_bytes[6]), - std::to_integer(supercap_raw_bytes[7])); - - next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); - } - - void open_yaw_packet_log_files_() { - try { - std::filesystem::create_directories(debug_log_yaw_packet_path_); - const auto time = std::chrono::system_clock::to_time_t( - std::chrono::system_clock::now()); - auto ss = std::ostringstream{}; - ss << std::put_time(std::localtime(&time), "%Y-%m-%d_%H-%M-%S"); - const auto timestamp = ss.str(); - - can2_all_file_.open( - debug_log_yaw_packet_path_ + "/can2_all_" + timestamp + ".csv", - std::ios::out | std::ios::trunc); - yaw_file_.open( - debug_log_yaw_packet_path_ + "/yaw_" + timestamp + ".csv", - std::ios::out | std::ios::trunc); - - if (!can2_all_file_.is_open() || !yaw_file_.is_open()) { - RCLCPP_ERROR( - status_.get_logger(), - "[yaw packet log] failed to open log files under '%s'", - debug_log_yaw_packet_path_.c_str()); - debug_log_yaw_packet_ = false; - return; - } - - can2_all_file_ << "timestamp_ns,direction,can_id,d0,d1,d2,d3,d4,d5,d6,d7\n"; - yaw_file_ << "timestamp_ns,direction,can_id,d0,d1,d2,d3,d4,d5,d6,d7\n"; - can2_all_file_.flush(); - yaw_file_.flush(); - - RCLCPP_INFO( - status_.get_logger(), - "[yaw packet log] recording CAN2 frames to %s/can2_all_%s.csv and %s/yaw_%s.csv", - debug_log_yaw_packet_path_.c_str(), timestamp.c_str(), - debug_log_yaw_packet_path_.c_str(), timestamp.c_str()); - } catch (const std::exception& e) { - RCLCPP_ERROR( - status_.get_logger(), "[yaw packet log] failed to open log files: %s", - e.what()); - debug_log_yaw_packet_ = false; - } - } - - void log_can2_packet_(bool tx, uint32_t can_id, std::span data) { - if (!debug_log_yaw_packet_) - return; - - const auto timestamp = status_.now().nanoseconds(); - const auto data_bytes = [&] { - auto bytes = std::array{}; - for (size_t i = 0; i < 8; ++i) - bytes[i] = i < data.size() ? std::to_integer(data[i]) : 0; - return bytes; - }(); - - const auto write_row = [&](std::ofstream& file, bool yaw_only) { - if (!file.is_open()) - return; - if (yaw_only && can_id != 0x142) - return; - file << timestamp << ',' << (tx ? "TX" : "RX") << ",0x" << std::hex - << std::setw(8) << std::setfill('0') << can_id << std::dec; - for (const auto byte : data_bytes) - file << ",0x" << std::hex << std::setw(2) << std::setfill('0') << byte - << std::dec; - file << '\n'; - if (++pending_rows_ >= kYawLogFlushRows_) { - file.flush(); - pending_rows_ = 0; - } - }; - - { - std::lock_guard lock{log_mutex_}; - write_row(can2_all_file_, false); - write_row(yaw_file_, true); - } - } - void can_receive_callback(const Spec::Can& can, const View::Can& data) override { if (data.is_extended_can_id || data.is_remote_transmission) return; @@ -887,15 +683,11 @@ class DeformableInfantryOmniB process_chassis_can_receive_(1, data); if (!data.is_extended_can_id && !data.is_remote_transmission && data.can_id == 0x300) { - if (data.can_data.size() == 8) - latest_supercap_status_.store( - device::CanPacket8{data.can_data}, std::memory_order_relaxed); supercap_.store_status(data.can_data); supercap_status_received_.store(true, std::memory_order_relaxed); } monitor_.tick("Bottom::Can1", data.can_id); } else if (can == Spec::kCans.kCan2) { - log_can2_packet_(false, data.can_id, data.can_data); process_chassis_can_receive_(2, data); if (data.is_extended_can_id || data.is_remote_transmission) return; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp index 29f22d4ee..74c8b0300 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp @@ -4,12 +4,8 @@ #include #include #include -#include -#include -#include #include #include -#include #include #include #include @@ -396,17 +392,6 @@ class DeformableInfantryOmniC status.register_output("/chassis/encoder/alpha_dot", encoder_alpha_dot_, kNaN); status.register_output("/chassis/radius", radius_, kDefaultRadius); - status.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); - status.get_parameter_or("debug_log_wheel_motor", debug_log_wheel_motor_, false); - status.get_parameter_or( - "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); - status.get_parameter_or("debug_log_yaw_packet", debug_log_yaw_packet_, false); - status.get_parameter_or( - "debug_log_yaw_packet_path", debug_log_yaw_packet_path_, - std::string{"/home/ubuntu/controller/yaw"}); - if (debug_log_yaw_packet_) - open_yaw_packet_log_files_(); - auto options = librmcs::board::AdvancedOptions{}; options.dangerously_skip_version_checks = true; board_ = std::make_unique(*this, serial_filter, options); @@ -448,15 +433,11 @@ class DeformableInfantryOmniC i, joint_physical_angle_[i], joint_physical_velocity_[i]); update_geometry_feedback_(); - if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) - log_chassis_feedback_once_per_second_(); dr16_.update_status(); gimbal_yaw_motor_.update_status(); if (supercap_status_received_.load(std::memory_order_relaxed)) supercap_.update_status(); - if (debug_log_supercap_) - log_supercap_feedback_once_per_second_(); gimbal_bullet_feeder_.update_status(); tf_->set_state( @@ -504,7 +485,6 @@ class DeformableInfantryOmniC .can_id = 0x200, .can_data = packet_can2_200.as_bytes(), }); - log_can2_packet_(true, 0x200, packet_can2_200.as_bytes()); builder.can_transmit( Spec::kCans.kCan3, // { @@ -525,7 +505,6 @@ class DeformableInfantryOmniC .can_id = 0x142, .can_data = packet_can2_142.as_bytes(), }); - log_can2_packet_(true, 0x142, packet_can2_142.as_bytes()); builder.can_transmit( Spec::kCans.kCan1, // { @@ -566,7 +545,6 @@ class DeformableInfantryOmniC .can_id = 0x141, .can_data = packet.as_bytes(), }); - log_can2_packet_(true, 0x141, packet.as_bytes()); break; } case kRightFront: @@ -612,28 +590,12 @@ class DeformableInfantryOmniC // State - std::atomic wheel_status_received_[4] = {false, false, false, false}; std::atomic joint_status_received_[4] = {false, false, false, false}; - bool debug_log_supercap_ = false; - bool debug_log_wheel_motor_ = false; - bool debug_log_deformable_joint_motor_ = false; - - bool debug_log_yaw_packet_ = false; - std::string debug_log_yaw_packet_path_; - std::ofstream can2_all_file_; - std::ofstream yaw_file_; - std::mutex log_mutex_; - size_t pending_rows_ = 0; - static constexpr size_t kYawLogFlushRows_ = 500; - const double kChassisRadiusBase; const double kRodLength; const double kDefaultRadius; - Clock::time_point next_chassis_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - Clock::time_point next_supercap_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - // Device device::Bmi088 imu_{1000, 0.2, 0.0}; @@ -653,7 +615,6 @@ class DeformableInfantryOmniC device::LkMotor{status_, command_, "/chassis/right_front_joint"}, }; - std::atomic latest_supercap_status_{device::CanPacket8{uint64_t{0}}}; std::atomic supercap_status_received_{false}; device::Supercap supercap_{status_, command_}; @@ -664,7 +625,6 @@ class DeformableInfantryOmniC return; if (data.can_id == 0x201) { chassis_wheel_motors_[index].store_status(data.can_data); - wheel_status_received_[index].store(true, std::memory_order_relaxed); } else if (data.can_id == 0x141) { chassis_joint_motors_[index].store_status(data.can_data); joint_status_received_[index].store(true, std::memory_order_relaxed); @@ -714,171 +674,9 @@ class DeformableInfantryOmniC *radius_ = (kChassisRadiusBase + kRodLength * alpha_rad.array().cos()).mean(); } - void log_chassis_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_chassis_feedback_log_time_) - return; - - const auto wheel_rx = [this](size_t index) { - return wheel_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - const auto joint_rx = [this](size_t index) { - return joint_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - - if (debug_log_wheel_motor_) { - std::string wheel_rx_str; - for (size_t i = 0; i < 4; ++i) { - if (i > 0) - wheel_rx_str.push_back(' '); - wheel_rx_str.push_back(wheel_rx(i)); - } - RCLCPP_INFO( - status_.get_logger(), - "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " - "rx=[%s]", - chassis_wheel_motors_[kLeftFront].angle(), - chassis_wheel_motors_[kLeftBack].angle(), - chassis_wheel_motors_[kRightBack].angle(), - chassis_wheel_motors_[kRightFront].angle(), - chassis_wheel_motors_[kLeftFront].angle(), - chassis_wheel_motors_[kLeftBack].angle(), - chassis_wheel_motors_[kRightBack].angle(), - chassis_wheel_motors_[kRightFront].angle(), wheel_rx_str.c_str()); - } - - if (debug_log_deformable_joint_motor_) { - std::string joint_rx_str; - for (size_t i = 0; i < 4; ++i) { - if (i > 0) - joint_rx_str.push_back(' '); - joint_rx_str.push_back(joint_rx(i)); - } - RCLCPP_INFO( - status_.get_logger(), - "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "rx=[%s]", - *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], - *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront], - *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], - *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront], - joint_rx_str.c_str()); - } - - next_chassis_feedback_log_time_ = now + std::chrono::seconds(1); - } - - void log_supercap_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_supercap_feedback_log_time_) - return; - - const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); - auto supercap_raw_packet = latest_supercap_status_.load(std::memory_order_relaxed); - const auto supercap_raw_bytes = supercap_raw_packet.as_bytes(); - - RCLCPP_INFO( - status_.get_logger(), - "[supercap] can1 rx=%c id=0x300 enabled=%d supercap_v=% .3f chassis_v=% .3f " - "power=% .3f raw=[%02X %02X %02X %02X %02X %02X %02X %02X]", - supercap_rx ? 'Y' : 'N', supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, - supercap_rx ? supercap_.supercap_voltage() : kNaN, - supercap_rx ? supercap_.chassis_voltage() : kNaN, - supercap_rx ? supercap_.chassis_power() : kNaN, - std::to_integer(supercap_raw_bytes[0]), - std::to_integer(supercap_raw_bytes[1]), - std::to_integer(supercap_raw_bytes[2]), - std::to_integer(supercap_raw_bytes[3]), - std::to_integer(supercap_raw_bytes[4]), - std::to_integer(supercap_raw_bytes[5]), - std::to_integer(supercap_raw_bytes[6]), - std::to_integer(supercap_raw_bytes[7])); - - next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); - } - - void open_yaw_packet_log_files_() { - try { - std::filesystem::create_directories(debug_log_yaw_packet_path_); - const auto time = std::chrono::system_clock::to_time_t( - std::chrono::system_clock::now()); - auto ss = std::ostringstream{}; - ss << std::put_time(std::localtime(&time), "%Y-%m-%d_%H-%M-%S"); - const auto timestamp = ss.str(); - - can2_all_file_.open( - debug_log_yaw_packet_path_ + "/can2_all_" + timestamp + ".csv", - std::ios::out | std::ios::trunc); - yaw_file_.open( - debug_log_yaw_packet_path_ + "/yaw_" + timestamp + ".csv", - std::ios::out | std::ios::trunc); - - if (!can2_all_file_.is_open() || !yaw_file_.is_open()) { - RCLCPP_ERROR( - status_.get_logger(), - "[yaw packet log] failed to open log files under '%s'", - debug_log_yaw_packet_path_.c_str()); - debug_log_yaw_packet_ = false; - return; - } - - can2_all_file_ << "timestamp_ns,direction,can_id,d0,d1,d2,d3,d4,d5,d6,d7\n"; - yaw_file_ << "timestamp_ns,direction,can_id,d0,d1,d2,d3,d4,d5,d6,d7\n"; - can2_all_file_.flush(); - yaw_file_.flush(); - - RCLCPP_INFO( - status_.get_logger(), - "[yaw packet log] recording CAN2 frames to %s/can2_all_%s.csv and %s/yaw_%s.csv", - debug_log_yaw_packet_path_.c_str(), timestamp.c_str(), - debug_log_yaw_packet_path_.c_str(), timestamp.c_str()); - } catch (const std::exception& e) { - RCLCPP_ERROR( - status_.get_logger(), "[yaw packet log] failed to open log files: %s", - e.what()); - debug_log_yaw_packet_ = false; - } - } - - void log_can2_packet_(bool tx, uint32_t can_id, std::span data) { - if (!debug_log_yaw_packet_) - return; - - const auto timestamp = status_.now().nanoseconds(); - const auto data_bytes = [&] { - auto bytes = std::array{}; - for (size_t i = 0; i < 8; ++i) - bytes[i] = i < data.size() ? std::to_integer(data[i]) : 0; - return bytes; - }(); - - const auto write_row = [&](std::ofstream& file, bool yaw_only) { - if (!file.is_open()) - return; - if (yaw_only && can_id != 0x142) - return; - file << timestamp << ',' << (tx ? "TX" : "RX") << ",0x" << std::hex - << std::setw(8) << std::setfill('0') << can_id << std::dec; - for (const auto byte : data_bytes) - file << ",0x" << std::hex << std::setw(2) << std::setfill('0') << byte - << std::dec; - file << '\n'; - if (++pending_rows_ >= kYawLogFlushRows_) { - file.flush(); - pending_rows_ = 0; - } - }; - - { - std::lock_guard lock{log_mutex_}; - write_row(can2_all_file_, false); - write_row(yaw_file_, true); - } - } - void can_receive_callback(const Spec::Can& can, const View::Can& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) + return; if (can == Spec::kCans.kCan0) { process_chassis_can_receive_(0, data); monitor_.tick("Bottom::Can0", data.can_id); @@ -886,15 +684,11 @@ class DeformableInfantryOmniC process_chassis_can_receive_(1, data); if (!data.is_extended_can_id && !data.is_remote_transmission && data.can_id == 0x300) { - if (data.can_data.size() == 8) - latest_supercap_status_.store( - device::CanPacket8{data.can_data}, std::memory_order_relaxed); supercap_.store_status(data.can_data); supercap_status_received_.store(true, std::memory_order_relaxed); } monitor_.tick("Bottom::Can1", data.can_id); } else if (can == Spec::kCans.kCan2) { - log_can2_packet_(false, data.can_id, data.can_data); process_chassis_can_receive_(2, data); if (data.is_extended_can_id || data.is_remote_transmission) return; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index 5b0670a96..43ec5009a 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -4,12 +4,8 @@ #include #include #include -#include -#include -#include #include #include -#include #include #include #include @@ -396,17 +392,6 @@ class DeformableInfantryOmni status.register_output("/chassis/encoder/alpha_dot", encoder_alpha_dot_, kNaN); status.register_output("/chassis/radius", radius_, kDefaultRadius); - status.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); - status.get_parameter_or("debug_log_wheel_motor", debug_log_wheel_motor_, false); - status.get_parameter_or( - "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); - status.get_parameter_or("debug_log_yaw_packet", debug_log_yaw_packet_, false); - status.get_parameter_or( - "debug_log_yaw_packet_path", debug_log_yaw_packet_path_, - std::string{"/home/ubuntu/controller/yaw"}); - if (debug_log_yaw_packet_) - open_yaw_packet_log_files_(); - auto options = librmcs::board::AdvancedOptions{}; options.dangerously_skip_version_checks = true; board_ = std::make_unique(*this, serial_filter, options); @@ -448,15 +433,11 @@ class DeformableInfantryOmni i, joint_physical_angle_[i], joint_physical_velocity_[i]); update_geometry_feedback_(); - if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) - log_chassis_feedback_once_per_second_(); dr16_.update_status(); gimbal_yaw_motor_.update_status(); if (supercap_status_received_.load(std::memory_order_relaxed)) supercap_.update_status(); - if (debug_log_supercap_) - log_supercap_feedback_once_per_second_(); gimbal_bullet_feeder_.update_status(); tf_->set_state( @@ -504,7 +485,6 @@ class DeformableInfantryOmni .can_id = 0x200, .can_data = packet_can2_200.as_bytes(), }); - log_can2_packet_(true, 0x200, packet_can2_200.as_bytes()); builder.can_transmit( Spec::kCans.kCan3, // { @@ -525,7 +505,6 @@ class DeformableInfantryOmni .can_id = 0x142, .can_data = packet_can2_142.as_bytes(), }); - log_can2_packet_(true, 0x142, packet_can2_142.as_bytes()); builder.can_transmit( Spec::kCans.kCan1, // { @@ -566,7 +545,6 @@ class DeformableInfantryOmni .can_id = 0x141, .can_data = packet.as_bytes(), }); - log_can2_packet_(true, 0x141, packet.as_bytes()); break; } case kRightFront: @@ -612,28 +590,12 @@ class DeformableInfantryOmni // State - std::atomic wheel_status_received_[4] = {false, false, false, false}; std::atomic joint_status_received_[4] = {false, false, false, false}; - bool debug_log_supercap_ = false; - bool debug_log_wheel_motor_ = false; - bool debug_log_deformable_joint_motor_ = false; - - bool debug_log_yaw_packet_ = false; - std::string debug_log_yaw_packet_path_; - std::ofstream can2_all_file_; - std::ofstream yaw_file_; - std::mutex log_mutex_; - size_t pending_rows_ = 0; - static constexpr size_t kYawLogFlushRows_ = 500; - const double kChassisRadiusBase; const double kRodLength; const double kDefaultRadius; - Clock::time_point next_chassis_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - Clock::time_point next_supercap_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - // Device device::Bmi088 imu_{1000, 0.2, 0.0}; @@ -653,7 +615,6 @@ class DeformableInfantryOmni device::LkMotor{status_, command_, "/chassis/right_front_joint"}, }; - std::atomic latest_supercap_status_{device::CanPacket8{uint64_t{0}}}; std::atomic supercap_status_received_{false}; device::Supercap supercap_{status_, command_}; @@ -664,7 +625,6 @@ class DeformableInfantryOmni return; if (data.can_id == 0x201) { chassis_wheel_motors_[index].store_status(data.can_data); - wheel_status_received_[index].store(true, std::memory_order_relaxed); } else if (data.can_id == 0x141) { chassis_joint_motors_[index].store_status(data.can_data); joint_status_received_[index].store(true, std::memory_order_relaxed); @@ -714,170 +674,6 @@ class DeformableInfantryOmni *radius_ = (kChassisRadiusBase + kRodLength * alpha_rad.array().cos()).mean(); } - void log_chassis_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_chassis_feedback_log_time_) - return; - - const auto wheel_rx = [this](size_t index) { - return wheel_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - const auto joint_rx = [this](size_t index) { - return joint_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - - if (debug_log_wheel_motor_) { - std::string wheel_rx_str; - for (size_t i = 0; i < 4; ++i) { - if (i > 0) - wheel_rx_str.push_back(' '); - wheel_rx_str.push_back(wheel_rx(i)); - } - RCLCPP_INFO( - status_.get_logger(), - "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " - "rx=[%s]", - chassis_wheel_motors_[kLeftFront].angle(), - chassis_wheel_motors_[kLeftBack].angle(), - chassis_wheel_motors_[kRightBack].angle(), - chassis_wheel_motors_[kRightFront].angle(), - chassis_wheel_motors_[kLeftFront].angle(), - chassis_wheel_motors_[kLeftBack].angle(), - chassis_wheel_motors_[kRightBack].angle(), - chassis_wheel_motors_[kRightFront].angle(), wheel_rx_str.c_str()); - } - - if (debug_log_deformable_joint_motor_) { - std::string joint_rx_str; - for (size_t i = 0; i < 4; ++i) { - if (i > 0) - joint_rx_str.push_back(' '); - joint_rx_str.push_back(joint_rx(i)); - } - RCLCPP_INFO( - status_.get_logger(), - "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "rx=[%s]", - *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], - *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront], - *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], - *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront], - joint_rx_str.c_str()); - } - - next_chassis_feedback_log_time_ = now + std::chrono::seconds(1); - } - - void log_supercap_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_supercap_feedback_log_time_) - return; - - const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); - auto supercap_raw_packet = latest_supercap_status_.load(std::memory_order_relaxed); - const auto supercap_raw_bytes = supercap_raw_packet.as_bytes(); - - RCLCPP_INFO( - status_.get_logger(), - "[supercap] can1 rx=%c id=0x300 enabled=%d supercap_v=% .3f chassis_v=% .3f " - "power=% .3f raw=[%02X %02X %02X %02X %02X %02X %02X %02X]", - supercap_rx ? 'Y' : 'N', supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, - supercap_rx ? supercap_.supercap_voltage() : kNaN, - supercap_rx ? supercap_.chassis_voltage() : kNaN, - supercap_rx ? supercap_.chassis_power() : kNaN, - std::to_integer(supercap_raw_bytes[0]), - std::to_integer(supercap_raw_bytes[1]), - std::to_integer(supercap_raw_bytes[2]), - std::to_integer(supercap_raw_bytes[3]), - std::to_integer(supercap_raw_bytes[4]), - std::to_integer(supercap_raw_bytes[5]), - std::to_integer(supercap_raw_bytes[6]), - std::to_integer(supercap_raw_bytes[7])); - - next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); - } - - void open_yaw_packet_log_files_() { - try { - std::filesystem::create_directories(debug_log_yaw_packet_path_); - const auto time = std::chrono::system_clock::to_time_t( - std::chrono::system_clock::now()); - auto ss = std::ostringstream{}; - ss << std::put_time(std::localtime(&time), "%Y-%m-%d_%H-%M-%S"); - const auto timestamp = ss.str(); - - can2_all_file_.open( - debug_log_yaw_packet_path_ + "/can2_all_" + timestamp + ".csv", - std::ios::out | std::ios::trunc); - yaw_file_.open( - debug_log_yaw_packet_path_ + "/yaw_" + timestamp + ".csv", - std::ios::out | std::ios::trunc); - - if (!can2_all_file_.is_open() || !yaw_file_.is_open()) { - RCLCPP_ERROR( - status_.get_logger(), - "[yaw packet log] failed to open log files under '%s'", - debug_log_yaw_packet_path_.c_str()); - debug_log_yaw_packet_ = false; - return; - } - - can2_all_file_ << "timestamp_ns,direction,can_id,d0,d1,d2,d3,d4,d5,d6,d7\n"; - yaw_file_ << "timestamp_ns,direction,can_id,d0,d1,d2,d3,d4,d5,d6,d7\n"; - can2_all_file_.flush(); - yaw_file_.flush(); - - RCLCPP_INFO( - status_.get_logger(), - "[yaw packet log] recording CAN2 frames to %s/can2_all_%s.csv and %s/yaw_%s.csv", - debug_log_yaw_packet_path_.c_str(), timestamp.c_str(), - debug_log_yaw_packet_path_.c_str(), timestamp.c_str()); - } catch (const std::exception& e) { - RCLCPP_ERROR( - status_.get_logger(), "[yaw packet log] failed to open log files: %s", - e.what()); - debug_log_yaw_packet_ = false; - } - } - - void log_can2_packet_(bool tx, uint32_t can_id, std::span data) { - if (!debug_log_yaw_packet_) - return; - - const auto timestamp = status_.now().nanoseconds(); - const auto data_bytes = [&] { - auto bytes = std::array{}; - for (size_t i = 0; i < 8; ++i) - bytes[i] = i < data.size() ? std::to_integer(data[i]) : 0; - return bytes; - }(); - - const auto write_row = [&](std::ofstream& file, bool yaw_only) { - if (!file.is_open()) - return; - if (yaw_only && can_id != 0x142) - return; - file << timestamp << ',' << (tx ? "TX" : "RX") << ",0x" << std::hex - << std::setw(8) << std::setfill('0') << can_id << std::dec; - for (const auto byte : data_bytes) - file << ",0x" << std::hex << std::setw(2) << std::setfill('0') << byte - << std::dec; - file << '\n'; - if (++pending_rows_ >= kYawLogFlushRows_) { - file.flush(); - pending_rows_ = 0; - } - }; - - { - std::lock_guard lock{log_mutex_}; - write_row(can2_all_file_, false); - write_row(yaw_file_, true); - } - } - void can_receive_callback(const Spec::Can& can, const View::Can& data) override { if (data.is_extended_can_id || data.is_remote_transmission) return; @@ -888,15 +684,11 @@ class DeformableInfantryOmni process_chassis_can_receive_(1, data); if (!data.is_extended_can_id && !data.is_remote_transmission && data.can_id == 0x300) { - if (data.can_data.size() == 8) - latest_supercap_status_.store( - device::CanPacket8{data.can_data}, std::memory_order_relaxed); supercap_.store_status(data.can_data); supercap_status_received_.store(true, std::memory_order_relaxed); } monitor_.tick("Bottom::Can1", data.can_id); } else if (can == Spec::kCans.kCan2) { - log_can2_packet_(false, data.can_id, data.can_data); process_chassis_can_receive_(2, data); if (data.is_extended_can_id || data.is_remote_transmission) return; From 50a0ddf8405a297ddec8bc61c6209d941bae5fe5 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Wed, 12 Aug 2026 00:56:10 +0800 Subject: [PATCH 86/86] chore: remove unused fit_gravity_ff script and gimbal_value_collector; delete ladar_package_transmit header --- .gitmodules | 3 + rmcs_ws/src/rmcs_auto_aim_v2 | 1 + .../src/rmcs_core/scripts/fit_gravity_ff.py | 73 --------- .../src/debug/gimbal_value_collector.cpp | 143 ------------------ .../vtm-link/ladar_package_transmit.hpp | 86 ----------- 5 files changed, 4 insertions(+), 302 deletions(-) create mode 160000 rmcs_ws/src/rmcs_auto_aim_v2 delete mode 100644 rmcs_ws/src/rmcs_core/scripts/fit_gravity_ff.py delete mode 100644 rmcs_ws/src/rmcs_core/src/debug/gimbal_value_collector.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/vtm-link/ladar_package_transmit.hpp diff --git a/.gitmodules b/.gitmodules index 61e5f08fb..de2c6a1cb 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,6 @@ [submodule "rmcs_ws/src/fast_tf"] path = rmcs_ws/src/fast_tf url = https://github.com/qzhhhi/FastTF.git +[submodule "rmcs_ws/src/rmcs_auto_aim_v2"] + path = rmcs_ws/src/rmcs_auto_aim_v2 + url = https://github.com/Alliance-Algorithm/rmcs_auto_aim_v2.git diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 new file mode 160000 index 000000000..40bb1f99c --- /dev/null +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -0,0 +1 @@ +Subproject commit 40bb1f99c3f41d97330e25f7ee5fb3765825bf91 diff --git a/rmcs_ws/src/rmcs_core/scripts/fit_gravity_ff.py b/rmcs_ws/src/rmcs_core/scripts/fit_gravity_ff.py deleted file mode 100644 index 4a95bbad2..000000000 --- a/rmcs_ws/src/rmcs_core/scripts/fit_gravity_ff.py +++ /dev/null @@ -1,73 +0,0 @@ -#!/usr/bin/env python3 -"""Fit gravity feedforward A*sin(theta - phi) from StaticTorqueTestController CSV. - -Usage: fit_gravity_ff.py -Fits against imu_pitch_angle (authoritative) and the encoder angle (reference), -prints gain / phase / residual RMS and the phase difference between the two. -""" - -import sys - -import numpy as np -from scipy.optimize import least_squares - - -def load_csv(path): - with open(path) as f: - header = f.readline().strip().split(",") - data = np.genfromtxt(path, delimiter=",", skip_header=1) - return header, data - - -def column(header, data, name): - return data[:, header.index(name)] - - -def fit_gravity(angle, torque, with_offset=False): - def model(p): - gain, phase = p[0], p[1] - offset = p[2] if with_offset else 0.0 - return gain * np.sin(angle - phase) + offset - - result = least_squares(lambda p: model(p) - torque, [1.0, 0.0, 0.0][: 3 if with_offset else 2]) - gain, phase = result.x[0], result.x[1] - offset = result.x[2] if with_offset else 0.0 - if gain < 0: - gain, phase = -gain, phase + np.pi - phase = (phase + np.pi) % (2 * np.pi) - np.pi - rms = float(np.sqrt(np.mean((gain * np.sin(angle - phase) + offset - torque) ** 2))) - return gain, phase, offset, rms - - -def main(): - header, data = load_csv(sys.argv[1]) - torque = column(header, data, "/gimbal/pitch/torque") - velocity = column(header, data, "/gimbal/pitch/velocity") - angle_enc = column(header, data, "/gimbal/pitch/angle") - angle_imu = column(header, data, "imu_pitch_angle") - - still = np.abs(velocity) < 0.05 - if still.sum() < len(velocity): - print(f"dropped {len(velocity) - still.sum()} moving points (|v| >= 0.05 rad/s)") - - for label, angle in [("imu", angle_imu), ("encoder", angle_enc)]: - gain, phase, _, rms = fit_gravity(angle[still], torque[still]) - print( - f"[{label:7s}] gain = {gain:8.4f} phase = {phase:+8.4f} rad" - f" rms = {rms:.4f} N*m <- deploy these" - ) - gain_o, phase_o, offset_o, rms_o = fit_gravity( - angle[still], torque[still], with_offset=True - ) - print( - f"[{label:7s}] gain = {gain_o:8.4f} phase = {phase_o:+8.4f} rad" - f" offset = {offset_o:+7.4f} rms = {rms_o:.4f} N*m (diagnostic)" - ) - - gain_i, phase_i, _, _ = fit_gravity(angle_imu[still], torque[still]) - _, phase_e, _, _ = fit_gravity(angle_enc[still], torque[still]) - print(f"phase difference (imu - encoder) = {phase_i - phase_e:+.4f} rad") - - -if __name__ == "__main__": - main() diff --git a/rmcs_ws/src/rmcs_core/src/debug/gimbal_value_collector.cpp b/rmcs_ws/src/rmcs_core/src/debug/gimbal_value_collector.cpp deleted file mode 100644 index 697d4f689..000000000 --- a/rmcs_ws/src/rmcs_core/src/debug/gimbal_value_collector.cpp +++ /dev/null @@ -1,143 +0,0 @@ -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include - -namespace rmcs_core::debug { - -class GimbalValueCollector - : public rmcs_executor::Component - , public rclcpp::Node - , public rmcs_utility::NodeMixin { -public: - GimbalValueCollector() - : Node( - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { - - register_input("/tf", tf_); - - register_input("/auto_aim/should_control", auto_aim_should_control_); - register_input("/auto_aim/ff_v", auto_aim_ff_v_); - register_input("/auto_aim/ff_a", auto_aim_ff_a_); - - register_input("/gimbal/yaw/angle", yaw_angle_); - register_input("/gimbal/yaw/velocity_imu", yaw_velocity_imu_); - register_input("/gimbal/yaw/control_torque", yaw_control_torque_); - register_input("/gimbal/yaw/control_angle_error", yaw_control_angle_error_); - - register_input("/gimbal/pitch/angle", pitch_angle_); - register_input("/gimbal/pitch/velocity_imu", pitch_velocity_imu_); - register_input("/gimbal/pitch/control_torque", pitch_control_torque_); - register_input("/gimbal/pitch/control_angle_error", pitch_control_angle_error_); - - node::param("csv_path", csv_path_); - node::param("write_interval", write_interval_); - node::param("flush_interval", flush_interval_); - - { - constexpr auto kPlaceholder = std::string_view{""}; - const auto pos = csv_path_.find(kPlaceholder); - if (pos != std::string::npos) { - const auto now = std::chrono::system_clock::now(); - const auto time = std::chrono::system_clock::to_time_t(now); - - auto ss = std::ostringstream{}; - ss << std::put_time(std::localtime(&time), "%Y-%m-%d_%H-%M-%S"); - csv_path_.replace(pos, kPlaceholder.size(), ss.str()); - } - } - - csv_file_.open(csv_path_, std::ios::out | std::ios::trunc); - if (!csv_file_.is_open()) { - node::error("failed to open {}", csv_path_); - return; - } - - csv_file_ << "index,should_control" - ",pitch_angle_imu,pitch_ref_velocity,pitch_ref_acceleration" - ",yaw_ref_velocity,yaw_ref_acceleration" - ",pitch_velocity_imu,yaw_velocity_imu" - ",pitch_angle,yaw_angle" - ",pitch_control_torque,yaw_control_torque" - ",pitch_control_angle_error,yaw_control_angle_error\n"; - csv_file_.flush(); - - node::info("collecting gimbal signals to {}", csv_path_); - } - - auto update() -> void override { - if (!csv_file_.is_open()) - return; - - if (tick_++ % write_interval_ != 0) - return; - - const auto pitch_angle_imu = gimbal_world_pitch(); - const auto ff_v = fast_tf::cast( - rmcs_description::OdomImu::DirectionVector{*auto_aim_ff_v_}, *tf_); - const auto ff_a = fast_tf::cast( - rmcs_description::OdomImu::DirectionVector{*auto_aim_ff_a_}, *tf_); - - csv_file_ << sample_count_++ // - << ',' << (*auto_aim_should_control_ ? 1 : 0) // - << ',' << pitch_angle_imu // - << ',' << ff_v->y() << ',' << ff_a->y() // - << ',' << ff_v->z() << ',' << ff_a->z() // - << ',' << *pitch_velocity_imu_ << ',' << *yaw_velocity_imu_ // - << ',' << *pitch_angle_ << ',' << *yaw_angle_ // - << ',' << *pitch_control_torque_ << ',' << *yaw_control_torque_ // - << ',' << *pitch_control_angle_error_ << ',' << *yaw_control_angle_error_ // - << '\n'; - - if (sample_count_ % flush_interval_ == 0) - csv_file_.flush(); - } - -private: - auto gimbal_world_pitch() const -> double { - auto dir = fast_tf::cast( - rmcs_description::PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); - return std::asin(std::clamp(dir->z(), -1.0, 1.0)); - } - - InputInterface tf_; - - InputInterface auto_aim_should_control_; - InputInterface auto_aim_ff_v_; - InputInterface auto_aim_ff_a_; - - InputInterface yaw_angle_; - InputInterface yaw_velocity_imu_; - InputInterface yaw_control_torque_; - InputInterface yaw_control_angle_error_; - - InputInterface pitch_angle_; - InputInterface pitch_velocity_imu_; - InputInterface pitch_control_torque_; - InputInterface pitch_control_angle_error_; - - std::string csv_path_; - int write_interval_ = 1; - int flush_interval_ = 1000; - - std::ofstream csv_file_; - - int tick_ = 0; - int sample_count_ = 0; -}; - -} // namespace rmcs_core::debug - -#include -PLUGINLIB_EXPORT_CLASS(rmcs_core::debug::GimbalValueCollector, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/vtm-link/ladar_package_transmit.hpp b/rmcs_ws/src/rmcs_core/src/hardware/vtm-link/ladar_package_transmit.hpp deleted file mode 100644 index 4538ad2a0..000000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/vtm-link/ladar_package_transmit.hpp +++ /dev/null @@ -1,86 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "referee/frame.hpp" - -namespace rmcs_core::hardware::vtm { - -class LadarPackageTransmit { -public: - using UartWriter = std::function; - using LidarMsgBroadcast = std::array; - - LadarPackageTransmit( - rmcs_executor::Component& component, std::chrono::milliseconds interval, - UartWriter uart_writer, rclcpp::Logger logger, - const std::string& lidar_msg_broadcast_name = - "/referee/multi_robot_communication/lidar_msg_broadcast") - : uart_writer_(std::move(uart_writer)) - , interval_(interval) - , logger_(std::move(logger)) { - component.register_input(lidar_msg_broadcast_name, lidar_msg_broadcast_, false); - } - - void command_update() { - if (!lidar_msg_broadcast_.ready()) - return; - - auto now = std::chrono::steady_clock::now(); - if (now < next_publish_time_) - return; - - publish_single_packet(*lidar_msg_broadcast_); - next_publish_time_ = now + interval_; - } - -private: - static constexpr uint16_t kLadarCmdId = 0x0310; - static constexpr size_t kLadarDataSize = 118; - static constexpr size_t kLadarPacketSize = 300; - - void publish_single_packet(const LidarMsgBroadcast& lidar_msg_broadcast) { - static constexpr size_t kHeaderSize = sizeof(referee::FrameHeader); - static constexpr size_t kCmdIdSize = sizeof(uint16_t); - static constexpr size_t kCrc16Size = sizeof(uint16_t); - static constexpr size_t kFrameSize = kHeaderSize + kCmdIdSize + kLadarPacketSize - + kCrc16Size; - - referee::Frame frame; - frame.header.sof = referee::sof_value; - frame.header.data_length = kLadarPacketSize; - frame.header.sequence = sequence_++; - frame.header.crc8 = 0; - frame.body.command_id = kLadarCmdId; - std::memcpy(frame.body.data, lidar_msg_broadcast.data(), kLadarDataSize); - std::memset( - frame.body.data + kLadarDataSize, 0, kLadarPacketSize - kLadarDataSize); - - rmcs_utility::dji_crc::append_crc8(frame.header); - rmcs_utility::dji_crc::append_crc16(&frame, kFrameSize); - - uart_writer_(reinterpret_cast(&frame), kFrameSize); - RCLCPP_DEBUG(logger_, "uart sent ladar packet"); - } - - rmcs_executor::Component::InputInterface lidar_msg_broadcast_; - UartWriter uart_writer_; - std::chrono::milliseconds interval_; - std::chrono::steady_clock::time_point next_publish_time_ = - std::chrono::steady_clock::time_point::min(); - uint8_t sequence_{0}; - rclcpp::Logger logger_; -}; - -} // namespace rmcs_core::hardware::vtm