diff --git a/.devcontainer/scripts/bootstrap-tools.sh b/.devcontainer/scripts/bootstrap-tools.sh index c475d8894..804923cac 100644 --- a/.devcontainer/scripts/bootstrap-tools.sh +++ b/.devcontainer/scripts/bootstrap-tools.sh @@ -6,6 +6,71 @@ installed_tools="" skipped_tools="" manifest_path="/workspaces/RMCS/.devcontainer/.generated/host-tools.manifest" +ensure_mvsusb3core_cmake_package() { + if [ -f /opt/mvs-usb3-core/lib/cmake/MVSUSB3Core/MVSUSB3CoreConfig.cmake ]; then + return 0 + fi + + case "$(uname -m)" in + x86_64) + arch_dir=64 + ;; + aarch64) + arch_dir=aarch64 + ;; + *) + printf 'Warning: unsupported architecture for MVSUSB3Core bootstrap: %s\n' "$(uname -m)" >&2 + return 0 + ;; + esac + + sudo mkdir -p /opt/mvs-usb3-core/lib/cmake/MVSUSB3Core + sudo tee /opt/mvs-usb3-core/lib/cmake/MVSUSB3Core/MVSUSB3CoreConfig.cmake >/dev/null <&2 fi 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/.gitignore b/.gitignore index 05b12839b..39b2fb34f 100644 --- a/.gitignore +++ b/.gitignore @@ -42,10 +42,6 @@ srv/_*.py qtcreator-* *.user -/planning/cfg -/planning/docs -/planning/src - *~ # Emacs @@ -71,5 +67,15 @@ id_rsa.pub # Recording files record/ +# Docker Compose override (personal config) +docker-compose.override.yml + # For temperary development develop_ws + +# For Agent Development +.opencode/ +.claude/ +.codex/ +.mimo/ +CLAUDE.md 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/.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/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/.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/.script/template/entrypoint b/.script/template/entrypoint index e8660e7ac..49290c5c3 100755 --- a/.script/template/entrypoint +++ b/.script/template/entrypoint @@ -1,5 +1,18 @@ #!/usr/bin/bash +USBFS_MEMORY_MB="${USBFS_MEMORY_MB:-2000}" +USBFS_MEMORY_MB_PATH="/sys/module/usbcore/parameters/usbfs_memory_mb" + +if [ -w "$USBFS_MEMORY_MB_PATH" ]; then + if ! printf '%s\n' "$USBFS_MEMORY_MB" > "$USBFS_MEMORY_MB_PATH"; then + echo "Warning: failed to set $USBFS_MEMORY_MB_PATH to $USBFS_MEMORY_MB" >&2 + fi +elif [ -e "$USBFS_MEMORY_MB_PATH" ]; then + echo "Warning: $USBFS_MEMORY_MB_PATH is not writable, skipping usbfs_memory_mb setup" >&2 +else + echo "Warning: $USBFS_MEMORY_MB_PATH not found, skipping usbfs_memory_mb setup" >&2 +fi + # Remove all files in /tmp rm -rf /tmp/* @@ -14,4 +27,4 @@ if [ -f "/etc/avahi/enabled" ]; then service avahi-daemon start fi -sleep infinity \ No newline at end of file +sleep infinity 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/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_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 new file mode 160000 index 000000000..316c96edf --- /dev/null +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -0,0 +1 @@ +Subproject commit 316c96edf0a86f55118131067d6ca3a195e53629 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_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml new file mode 100644 index 000000000..f440156a8 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -0,0 +1,367 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::DeformableInfantryOmniB -> 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::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::AutoAimRecorderComponent -> auto_aim_recorder + - 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_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: "" + exposure_us: 4000.0 + gain: 10.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.05 + offset_yaw: +2.4 + offset_pitch: +0.05 + 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.7 + 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_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 + +chassis_controller: + ros__parameters: + 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: 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 + +deformable_suspension: + ros__parameters: + 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 + + 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 + + active_suspension_target_pitch_deg: 0.0 + + 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.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.0 + pitch_angle_integral_min: -0.5 + pitch_angle_integral_max: 0.5 + + pitch_velocity_kp: 1.8 + 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_velocities_low_mode: + - 530.0 + - 530.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: 15.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-steering.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml similarity index 53% rename from rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml rename to rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml index b1ec953d6..e9fbdf385 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml @@ -2,7 +2,7 @@ rmcs_executor: ros__parameters: update_rate: 1000.0 components: - - rmcs_core::hardware::DeformableInfantryV2 -> deformable_infantry + - rmcs_core::hardware::DeformableInfantryOmniC -> deformable_infantry - rmcs_core::referee::Status -> referee_status - rmcs_core::referee::Command -> referee_command @@ -21,105 +21,189 @@ 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::AutoAimRecorderComponent -> auto_aim_recorder + - 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_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: "" + exposure_us: 4000.0 + gain: 10.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.05 + offset_yaw: +0.6 + offset_pitch: -0.2 + 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 + +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_bottom_board: "AF-C1C3-DFE8-40A8-FC4B-B853-6ED7-AC9F-1DED" 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 + 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: 20.0 - max_angle: 50.0 + min_angle: 4.0 + max_angle: 59.0 active_suspension_enable: true - spin_ratio: 1.0 - - # 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 - - # 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 + 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: + 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 + + 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 + + active_suspension_target_pitch_deg: 0.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: - 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_ki: 0.0 yaw_angle_kd: 0.0 - yaw_velocity_kp: 8.0 - yaw_velocity_ki: 0.0 + yaw_velocity_kp: 13.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: 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_angle_integral_min: -0.5 + pitch_angle_integral_max: 0.5 - 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,19 +212,22 @@ friction_wheel_controller: - /gimbal/left_friction - /gimbal/right_friction friction_velocities: - - 600.0 - - 600.0 + - 580.0 + - 580.0 + friction_velocities_low_mode: + - 530.0 + - 530.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: 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 @@ -171,34 +258,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 +295,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 +317,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 +340,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 +363,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..7ad5a9da8 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,124 +30,206 @@ 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::AutoAimRecorderComponent -> auto_aim_recorder + - 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_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: "" + exposure_us: 4000.0 + gain: 10.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.05 + offset_yaw: +1.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.7 + 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 chassis_controller: ros__parameters: - # Deploy geometry / chassis-owned joint intent - min_angle: 8.0 - max_angle: 58.0 + min_angle: 4.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 - - # 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 - - # Automatic IMU mounting-error calibration. - # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. + 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: + 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 + + 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 + + active_suspension_target_pitch_deg: 0.0 + chassis_imu_calibration_wait_s: 2.0 chassis_imu_calibration_sample_s: 3.0 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_ki: 0.0 + yaw_velocity_kp: 13.0 + yaw_velocity_ki: 0.02 yaw_velocity_kd: 0.0 + yaw_velocity_integral_min: -5.0 + yaw_velocity_integral_max: 5.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 + 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.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_velocities_low_mode: + - 530.0 + - 530.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: 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 @@ -183,30 +266,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 +297,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 +319,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 +342,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 +365,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/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml new file mode 100644 index 000000000..04bb643e9 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -0,0 +1,215 @@ +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_bringup/config/mecanum-hero.yaml b/rmcs_ws/src/rmcs_bringup/config/mecanum-hero.yaml deleted file mode 100644 index 41d3572a1..000000000 --- a/rmcs_ws/src/rmcs_bringup/config/mecanum-hero.yaml +++ /dev/null @@ -1,161 +0,0 @@ -rmcs_executor: - ros__parameters: - update_rate: 1000.0 - components: - - rmcs_core::hardware::MecanumHero -> hero_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::Ui -> referee_ui - # - rmcs_core::referee::app::ui::Hero -> referee_ui_hero - - - rmcs_core::controller::gimbal::HeroGimbalController -> gimbal_controller - - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_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::BulletFeederController42mm -> bullet_feeder_controller - - rmcs_core::controller::pid::PidController -> second_left_friction_velocity_pid_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_right_friction_velocity_pid_controller - - - rmcs_core::controller::chassis::ChassisController -> chassis_controller - - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - - rmcs_core::controller::chassis::OmniWheelController -> omni_wheel_controller - -hero_hardware: - ros__parameters: - board_serial_top_board: "(TODO)" - board_serial_bottom_board: "(TODO)" - yaw_motor_zero_point: 61054 - pitch_motor_zero_point: 54062 - external_imu_port: /dev/ttyUSB0 - -gimbal_controller: - ros__parameters: - upper_limit: -0.4598 - lower_limit: 0.4362 - -yaw_angle_pid_controller: - ros__parameters: - measurement: /gimbal/yaw/control_angle_error - feedforward: -/chassis/yaw/velocity_imu - control: /gimbal/yaw/control_velocity - kp: 16.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: 16.00 - ki: 0.0 - kd: 0.0 - output_max: 10.0 - -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: - - 450.00 - - 450.00 - - 535.00 - - 535.00 - friction_soft_start_stop_time: 1.0 - -heat_controller: - ros__parameters: - heat_per_shot: 100000 - reserved_heat: 0 - -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.003436926 - ki: 0.00 - kd: 0.009373434 - -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.003436926 - ki: 0.00 - kd: 0.009373434 - -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.003436926 - ki: 0.00 - kd: 0.009373434 - -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.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.0 - ki: 0.001 - kd: 0.0 - integral_min: -1000.0 - integral_max: 1000.0 - -left_front_wheel_velocity_pid_controller: - ros__parameters: - measurement: /chassis/left_front_wheel/velocity - setpoint: /chassis/left_front_wheel/control_velocity - control: /chassis/left_front_wheel/control_torque_unrestricted - kp: 0.185 - ki: 0.00 - kd: 0.00 - -left_back_wheel_velocity_pid_controller: - ros__parameters: - measurement: /chassis/left_back_wheel/velocity - setpoint: /chassis/left_back_wheel/control_velocity - control: /chassis/left_back_wheel/control_torque_unrestricted - kp: 0.185 - ki: 0.00 - kd: 0.00 - -right_back_wheel_velocity_pid_controller: - ros__parameters: - measurement: /chassis/right_back_wheel/velocity - setpoint: /chassis/right_back_wheel/control_velocity - control: /chassis/right_back_wheel/control_torque_unrestricted - kp: 0.185 - ki: 0.00 - kd: 0.00 - -right_front_wheel_velocity_pid_controller: - ros__parameters: - measurement: /chassis/right_front_wheel/velocity - setpoint: /chassis/right_front_wheel/control_velocity - control: /chassis/right_front_wheel/control_torque_unrestricted - kp: 0.185 - ki: 0.00 - kd: 0.00 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/omni-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/omni-infantry.yaml index 6364aec7e..304407cf5 100644 --- a/rmcs_ws/src/rmcs_bringup/config/omni-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/omni-infantry.yaml @@ -53,9 +53,9 @@ yaw_velocity_pid_controller: measurement: /gimbal/yaw/velocity_imu setpoint: /gimbal/yaw/control_velocity control: /gimbal/yaw/control_torque - kp: 40.0 - ki: 0.001 - kd: 0.01 + kp: 4.4 + ki: 0.00011 + kd: 0.0011 pitch_angle_pid_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index 99c037100..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: 22.5 - 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.002495 - # 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_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-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-infantry.yaml deleted file mode 100644 index d650d044a..000000000 --- a/rmcs_ws/src/rmcs_bringup/config/steering-infantry.yaml +++ /dev/null @@ -1,128 +0,0 @@ -rmcs_executor: - ros__parameters: - update_rate: 1000.0 - components: - - rmcs_core::hardware::SteeringInfantry -> steeringInfantry_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::Ui -> referee_ui - # - rmcs_core::referee::app::ui::Infantry -> referee_ui_infantry - - - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller - - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_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::controller::chassis::ChassisController -> chassis_controller - - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller - -steeringInfantry_hardware: - ros__parameters: - board_serial_top_board: "(TODO)" - board_serial_bottom_board: "(TODO)" - yaw_motor_zero_point: 32285 - pitch_motor_zero_point: 6321 - left_front_zero_point: 7848 - left_back_zero_point: 5770 - right_back_zero_point: 2380 - right_front_zero_point: 1705 - -gimbal_controller: - ros__parameters: - upper_limit: -0.39518 - lower_limit: 0.36 - -yaw_angle_pid_controller: - ros__parameters: - measurement: /gimbal/yaw/control_angle_error - control: /gimbal/yaw/control_velocity - kp: 16.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.00 - ki: 0.0 - kd: 0.0 - -friction_wheel_controller: - ros__parameters: - friction_wheels: - - /gimbal/left_friction - - /gimbal/right_friction - friction_velocities: - - 600.0 - - 600.0 - friction_soft_start_stop_time: 1.0 - -heat_controller: - ros__parameters: - heat_per_shot: 10000 - reserved_heat: 0 - -bullet_feeder_controller: - ros__parameters: - bullets_per_feeder_turn: 10.0 - shot_frequency: 24.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: 4 - log_mode: 2 # 1: trigger, 2: timing - -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: 19.0 - moment_of_inertia: 1.0 - vehicle_radius: 0.24678 - 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/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index 36e7e5706..0ae6350ad 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -8,29 +8,30 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-packed-bitfield-compat") if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-O2 -Wall -Wextra -Wpedantic) + add_compile_options(-O2 -Wall -Wextra -Wpedantic) endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() + 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.1.0/librmcs-sdk-src-3.1.0.zip - URL_HASH SHA256=07107e251745ddb23f7b3e39edec5d6910be1a197025d167ec9849c5c80dd954 - DOWNLOAD_EXTRACT_TIMESTAMP TRUE + librmcs + 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) file(GLOB_RECURSE PROJECT_SOURCE CONFIGURE_DEPENDS - ${PROJECT_SOURCE_DIR}/src/*.cpp - ${PROJECT_SOURCE_DIR}/src/*.c + ${PROJECT_SOURCE_DIR}/src/*.cpp + ${PROJECT_SOURCE_DIR}/src/*.c ) ament_auto_add_library( - ${PROJECT_NAME} SHARED - ${PROJECT_SOURCE} + ${PROJECT_NAME} SHARED + ${PROJECT_SOURCE} ) include_directories(${PROJECT_SOURCE_DIR}/include) 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 f314d28fc..ac41a1087 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -1,44 +1,69 @@ - - + + - - - + + + + + + + + + + + + + - - + - + + + + + + + + + + + + + + + + + diff --git a/rmcs_ws/src/rmcs_core/src/broadcaster/tf_broadcaster.cpp b/rmcs_ws/src/rmcs_core/src/broadcaster/tf_broadcaster.cpp index 40dfe60a6..f3d13c700 100644 --- a/rmcs_ws/src/rmcs_core/src/broadcaster/tf_broadcaster.cpp +++ b/rmcs_ws/src/rmcs_core/src/broadcaster/tf_broadcaster.cpp @@ -26,6 +26,8 @@ class TfBroadcaster } ~TfBroadcaster() = default; + void before_updating() override { fast_tf::rcl::broadcast_all(*tf_); } + void update() override { using namespace std::chrono_literals; if (*update_count_ == 0) diff --git a/rmcs_ws/src/rmcs_core/src/broadcaster/value_broadcaster.cpp b/rmcs_ws/src/rmcs_core/src/broadcaster/value_broadcaster.cpp index c9b9a5845..ec612065f 100644 --- a/rmcs_ws/src/rmcs_core/src/broadcaster/value_broadcaster.cpp +++ b/rmcs_ws/src/rmcs_core/src/broadcaster/value_broadcaster.cpp @@ -14,15 +14,16 @@ class ValueBroadcaster : Node{get_component_name()} { declare_parameter>("forward_list", std::vector{}); parameter_subscriber_ = std::make_unique(this); - parameter_callback_ = parameter_subscriber_->add_parameter_callback( + parameter_callback_ = parameter_subscriber_->add_parameter_callback( "forward_list", [this](const rclcpp::Parameter& para) { update_forward_list(para.as_string_array()); }); } - void before_pairing( - const std::map& output_map) override { - for (const auto& [name, type] : output_map) { - if (type == typeid(double)) { + void before_pairing(const OutputInfoMap& output_map) override { + for (const auto& [name, output] : output_map) { + if (output.kind != rmcs_executor::InterfaceKind::Normal) + continue; + if (output.type.get() == typeid(double)) { forward_units_.emplace( name, std::make_unique>(this, name)); @@ -52,7 +53,7 @@ class ValueBroadcaster "unsupported or the output does not exist.", name.c_str()); } else { - auto& unit = iter->second; + auto& unit = iter->second; unit->active = true; } } @@ -70,8 +71,8 @@ class ValueBroadcaster virtual ~BasicForwarderUnit() {} virtual void activate(rclcpp::Node* node) = 0; - virtual void update() = 0; - virtual void deactivate() = 0; + virtual void update() = 0; + virtual void deactivate() = 0; bool active; }; @@ -118,4 +119,4 @@ class ValueBroadcaster #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::broadcaster::ValueBroadcaster, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::broadcaster::ValueBroadcaster, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/adrc/eso.hpp b/rmcs_ws/src/rmcs_core/src/controller/adrc/eso.hpp index e7d9d6eec..0d79b3009 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/adrc/eso.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/adrc/eso.hpp @@ -66,9 +66,7 @@ class ESO { } private: - static double clamp(double x, double lo, double hi) { - return std::max(lo, std::min(x, hi)); - } + static double clamp(double x, double lo, double hi) { return std::max(lo, std::min(x, hi)); } void sanitize_config() { constexpr double kEps = 1e-9; diff --git a/rmcs_ws/src/rmcs_core/src/controller/adrc/nlesf.hpp b/rmcs_ws/src/rmcs_core/src/controller/adrc/nlesf.hpp index c3bd67257..ad21b4fbd 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/adrc/nlesf.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/adrc/nlesf.hpp @@ -47,22 +47,22 @@ class NLESF { constexpr double kEps = 1e-9; const double safe_b0 = (std::fabs(b0) < kEps) ? ((b0 >= 0.0) ? kEps : -kEps) : b0; - const double u0 = cfg_.k1 * fal(e1, cfg_.alpha1, cfg_.delta) - + cfg_.k2 * fal(e2, cfg_.alpha2, cfg_.delta); + const double u0 = + cfg_.k1 * fal(e1, cfg_.alpha1, cfg_.delta) + cfg_.k2 * fal(e2, cfg_.alpha2, cfg_.delta); const double u = clamp((u0 - z3) / safe_b0, cfg_.u_min, cfg_.u_max); return Output{u0, u}; } private: static double sign(double x) { - if (x > 0.0) return 1.0; - if (x < 0.0) return -1.0; + if (x > 0.0) + return 1.0; + if (x < 0.0) + return -1.0; return 0.0; } - static double clamp(double x, double lo, double hi) { - return std::max(lo, std::min(x, hi)); - } + static double clamp(double x, double lo, double hi) { return std::max(lo, std::min(x, hi)); } void sanitize_config() { constexpr double kEps = 1e-9; diff --git a/rmcs_ws/src/rmcs_core/src/controller/adrc/td.hpp b/rmcs_ws/src/rmcs_core/src/controller/adrc/td.hpp index 6b14ce5f3..390d57112 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/adrc/td.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/adrc/td.hpp @@ -57,14 +57,14 @@ class TD { private: static double sign(double x) { - if (x > 0.0) return 1.0; - if (x < 0.0) return -1.0; + if (x > 0.0) + return 1.0; + if (x < 0.0) + return -1.0; return 0.0; } - static double clamp(double x, double lo, double hi) { - return std::max(lo, std::min(x, hi)); - } + static double clamp(double x, double lo, double hi) { return std::max(lo, std::min(x, hi)); } static double fhan(double x1_minus_v, double x2, double r, double h) { const double d = r * h * h; @@ -72,9 +72,7 @@ class TD { const double y = x1_minus_v + a0; const double a1 = std::sqrt(d * (d + 8.0 * std::fabs(y))); - const double a = (std::fabs(y) > d) - ? (a0 + sign(y) * (a1 - d) * 0.5) - : (a0 + y); + const double a = (std::fabs(y) > d) ? (a0 + sign(y) * (a1 - d) * 0.5) : (a0 + y); if (std::fabs(a) <= d) { return -r * a / d; 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 new file mode 100644 index 000000000..840c23c2f --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp @@ -0,0 +1,725 @@ +#include "controller/pid/matrix_pid_calculator.hpp" +#include "rmcs_msgs/keyboard.hpp" +#include "rmcs_msgs/switch.hpp" +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::controller::chassis { + +namespace { +double estimate_front_power( + double left_torque, double right_torque, double left_velocity, double right_velocity, + double bias, double k_tau2, double k_mech) { + + if (!std::isfinite(left_torque) || !std::isfinite(right_torque)) + return 0.0; + + return bias + k_tau2 * (std::pow(left_torque, 2) + std::pow(right_torque, 2)) + + k_mech + * (std::abs(left_torque * left_velocity) + std::abs(right_torque * right_velocity)); +} +} // namespace + +enum class AutoClimbState { IDLE, ALIGN, APPROACH, SUPPORT_DEPLOY, DASH, SUPPORT_RETRACT }; + +class ChassisClimberController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + ChassisClimberController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , logger_(get_logger()) + , front_velocity_pid_calculator_( + get_parameter("front_kp").as_double(), get_parameter("front_ki").as_double(), + get_parameter("front_kd").as_double()) + , back_velocity_pid_calculator_( + get_parameter("back_kp").as_double(), get_parameter("back_ki").as_double(), + get_parameter("back_kd").as_double()) { + + track_velocity_max_ = get_parameter("front_climber_velocity").as_double(); + climber_back_control_velocity_abs_ = get_parameter("back_climber_velocity").as_double(); + auto_climb_support_retract_velocity_fast_abs_ = + get_parameter("auto_climb_support_retract_velocity_fast").as_double(); + auto_climb_support_retract_velocity_slow_abs_ = + get_parameter("auto_climb_support_retract_velocity_slow").as_double(); + auto_climb_approach_chassis_velocity_ = + get_parameter("auto_climb_approach_chassis_velocity").as_double(); + auto_climb_support_deploy_chassis_velocity_ = + get_parameter("auto_climb_support_deploy_chassis_velocity").as_double(); + auto_climb_support_retract_chassis_velocity_ = + get_parameter("auto_climb_support_retract_chassis_velocity").as_double(); + auto_climb_dash_chassis_velocity_ = + get_parameter("auto_climb_dash_chassis_velocity").as_double(); + first_stair_dash_leveled_pitch_threshold_ = + get_parameter("first_stair_dash_leveled_pitch_threshold").as_double(); + second_stair_dash_leveled_pitch_threshold_ = + get_parameter("second_stair_dash_leveled_pitch_threshold").as_double(); + sync_coefficient_ = get_parameter("sync_coefficient").as_double(); + first_stair_approach_pitch_ = get_parameter("first_stair_approach_pitch").as_double(); + second_stair_approach_pitch_ = get_parameter("second_stair_approach_pitch").as_double(); + front_power_estimate_bias_ = get_parameter("front_power_estimate_bias").as_double(); + front_power_estimate_k_tau2_ = get_parameter("front_power_estimate_k_tau2").as_double(); + front_power_estimate_k_mech_ = get_parameter("front_power_estimate_k_mech").as_double(); + + register_output( + "/chassis/climber/left_front_motor/requested_control_torque", + climber_front_left_requested_control_torque_, nan_); + register_output( + "/chassis/climber/right_front_motor/requested_control_torque", + climber_front_right_requested_control_torque_, nan_); + register_output( + "/chassis/climber/left_back_motor/control_torque", climber_back_left_control_torque_, + nan_); + register_output( + "/chassis/climber/right_back_motor/control_torque", climber_back_right_control_torque_, + nan_); + register_output("/chassis/climbing_forward_velocity", climbing_forward_velocity_, nan_); + register_output("/chassis/climber/auto_climb_active", auto_climb_active_, false); + register_output( + "/chassis/climber/front/power_budget_active", front_power_budget_active_, false); + register_output( + "/chassis/climber/front/power_demand_estimate", front_power_demand_estimate_, 0.0); + + register_input("/chassis/climber/left_front_motor/velocity", climber_front_left_velocity_); + register_input( + "/chassis/climber/right_front_motor/velocity", climber_front_right_velocity_); + register_input("/chassis/climber/left_back_motor/velocity", climber_back_left_velocity_); + register_input("/chassis/climber/right_back_motor/velocity", climber_back_right_velocity_); + + register_input("/chassis/climber/left_back_motor/torque", climber_back_left_torque_); + register_input("/chassis/climber/right_back_motor/torque", climber_back_right_torque_); + + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/keyboard", keyboard_); + register_input("/remote/rotary_knob_switch", rotary_knob_switch_); + register_input("/chassis/pitch_imu", chassis_pitch_imu_); + + register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); + register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_); + register_input("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); + + front_power_limiter_ = create_partner_component( + get_component_name() + "_front_power_limiter", front_power_estimate_bias_, + front_power_estimate_k_tau2_, front_power_estimate_k_mech_); + } + + void update() override { + using namespace rmcs_msgs; + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; + auto rotary_knob_switch = *rotary_knob_switch_; + + bool rotary_knob_to_down = + (last_rotary_knob_switch_ != Switch::DOWN && rotary_knob_switch == Switch::DOWN); + bool rotary_knob_from_down = + (last_rotary_knob_switch_ == Switch::DOWN && rotary_knob_switch != Switch::DOWN); + + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_all_controls(); + } else { + handle_auto_climb_requests( + (!last_keyboard_.g && keyboard.g) || rotary_knob_to_down, rotary_knob_from_down, + rotary_knob_switch); + + if (auto_climb_state_ != AutoClimbState::IDLE) { + stop_manual_support(); + apply_climb_control(update_auto_climb_control()); + } else { + apply_climb_control(update_manual_support_control(keyboard)); + } + } + + last_keyboard_ = keyboard; + last_rotary_knob_switch_ = rotary_knob_switch; + } + +private: + struct AutoClimbControl { + double front_track_velocity = nan_; + double back_climber_velocity = nan_; + double override_chassis_vx = nan_; + }; + + class ChassisClimberFrontPowerLimiter : public rmcs_executor::Component { + public: + ChassisClimberFrontPowerLimiter(double bias, double k_tau2, double k_mech) + : front_power_estimate_bias_(bias) + , front_power_estimate_k_tau2_(k_tau2) + , front_power_estimate_k_mech_(k_mech) { + register_input( + "/chassis/climber/left_front_motor/requested_control_torque", + left_requested_control_torque_); + register_input( + "/chassis/climber/right_front_motor/requested_control_torque", + right_requested_control_torque_); + register_input("/chassis/climber/left_front_motor/velocity", left_velocity_); + register_input("/chassis/climber/right_front_motor/velocity", right_velocity_); + register_input("/chassis/climber/left_front_motor/max_torque", left_max_torque_); + register_input("/chassis/climber/right_front_motor/max_torque", right_max_torque_); + register_input("/chassis/climber/front/control_power_limit", control_power_limit_); + + register_output( + "/chassis/climber/left_front_motor/control_torque", left_control_torque_, nan_); + register_output( + "/chassis/climber/right_front_motor/control_torque", right_control_torque_, nan_); + register_output( + "/chassis/climber/front/actual_power_estimate", actual_power_estimate_, 0.0); + } + + void update() override { + const double left_requested = *left_requested_control_torque_; + const double right_requested = *right_requested_control_torque_; + + if (!std::isfinite(left_requested) || !std::isfinite(right_requested)) { + *left_control_torque_ = nan_; + *right_control_torque_ = nan_; + *actual_power_estimate_ = 0.0; + return; + } + + if (*control_power_limit_ <= 0.0) { + *left_control_torque_ = 0.0; + *right_control_torque_ = 0.0; + *actual_power_estimate_ = estimate_front_power( + *left_control_torque_, *right_control_torque_, *left_velocity_, + *right_velocity_, front_power_estimate_bias_, front_power_estimate_k_tau2_, + front_power_estimate_k_mech_); + return; + } + + const double left_torque = + std::clamp(left_requested, -*left_max_torque_, *left_max_torque_); + const double right_torque = + std::clamp(right_requested, -*right_max_torque_, *right_max_torque_); + + const double estimated_power = estimate_front_power( + left_torque, right_torque, *left_velocity_, *right_velocity_, + front_power_estimate_bias_, front_power_estimate_k_tau2_, + front_power_estimate_k_mech_); + + if (estimated_power <= *control_power_limit_ || estimated_power <= 0.0) { + *left_control_torque_ = left_torque; + *right_control_torque_ = right_torque; + *actual_power_estimate_ = estimated_power; + return; + } + + const double scale = std::clamp(*control_power_limit_ / estimated_power, 0.0, 1.0); + *left_control_torque_ = left_torque * scale; + *right_control_torque_ = right_torque * scale; + *actual_power_estimate_ = estimate_front_power( + *left_control_torque_, *right_control_torque_, *left_velocity_, *right_velocity_, + front_power_estimate_bias_, front_power_estimate_k_tau2_, + front_power_estimate_k_mech_); + } + + private: + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + + double front_power_estimate_bias_; + double front_power_estimate_k_tau2_; + double front_power_estimate_k_mech_; + + InputInterface left_requested_control_torque_; + InputInterface right_requested_control_torque_; + InputInterface left_velocity_; + InputInterface right_velocity_; + InputInterface left_max_torque_; + InputInterface right_max_torque_; + InputInterface control_power_limit_; + + OutputInterface left_control_torque_; + OutputInterface right_control_torque_; + OutputInterface actual_power_estimate_; + }; + + void handle_auto_climb_requests( + bool start_requested, bool abort_by_rotary, rmcs_msgs::Switch rotary_knob_switch) { + + if (start_requested) { + if (auto_climb_state_ == AutoClimbState::IDLE) { + start_auto_climb( + rotary_knob_switch == rmcs_msgs::Switch::UP ? "Rotary Knob" : "Keyboard G"); + } else { + abort_auto_climb("toggled again"); + } + } else if (abort_by_rotary && auto_climb_state_ != AutoClimbState::IDLE) { + abort_auto_climb("rotary knob left UP"); + } + } + + void start_auto_climb(const char* source) { + stop_manual_support(); + back_climber_zero_velocity_hold_ = false; + auto_climb_stair_index_ = 0; + auto_climb_align_stable_count_ = 0; + auto_climb_support_block_count_ = 0; + enter_auto_climb_state(AutoClimbState::ALIGN); + + RCLCPP_INFO(logger_, "Auto climb started by %s. Entering ALIGN.", source); + } + + void abort_auto_climb(const char* reason) { + stop_auto_climb(); + start_back_climber_retract("Auto climb exit"); + RCLCPP_INFO(logger_, "Auto climb aborted (%s).", reason); + } + + AutoClimbControl update_auto_climb_control() { + if (auto_climb_state_ == AutoClimbState::IDLE) + return {}; + + auto_climb_timer_++; + + switch (auto_climb_state_) { + case AutoClimbState::IDLE: return {}; + case AutoClimbState::ALIGN: return update_auto_climb_align(); + case AutoClimbState::APPROACH: return update_auto_climb_approach(); + case AutoClimbState::SUPPORT_DEPLOY: return update_auto_climb_support_deploy(); + case AutoClimbState::DASH: return update_auto_climb_dash(); + case AutoClimbState::SUPPORT_RETRACT: return update_auto_climb_support_retract(); + } + + return {}; + } + + AutoClimbControl update_manual_support_control(const rmcs_msgs::Keyboard& keyboard) { + AutoClimbControl control; + + if (keyboard.b) { + back_climber_zero_velocity_hold_ = false; + control.back_climber_velocity = climber_back_control_velocity_abs_; + return control; + } + + if (last_keyboard_.b) { + start_back_climber_retract("Manual support"); + } + + if (!manual_support_retracting_) { + if (back_climber_zero_velocity_hold_) { + control.back_climber_velocity = 0.0; + } else { + start_back_climber_retract("Auto retract"); + } + return control; + } + + if (back_climber_recover_count > 1200) { + control.back_climber_velocity = -auto_climb_support_retract_velocity_slow_abs_; + } else { + control.back_climber_velocity = -auto_climb_support_retract_velocity_fast_abs_; + } + + if (is_back_climber_blocked()) + manual_support_retract_block_count_++; + else + manual_support_retract_block_count_ = 0; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "MANUAL_SUPPORT_RETRACT: blocked_ticks=%d", + manual_support_retract_block_count_); + + if (manual_support_retract_block_count_ >= kManualSupportRetractConfirmTicks) { + stop_manual_support(); + back_climber_zero_velocity_hold_ = true; + control.back_climber_velocity = 0.0; + RCLCPP_INFO(logger_, "Manual support retract completed."); + } + + return control; + } + + AutoClimbControl update_auto_climb_align() { + AutoClimbControl control{ + .front_track_velocity = 0.0, + .back_climber_velocity = -5.0, + .override_chassis_vx = 0.0, + }; + + double gimbal_yaw_angle_error = *gimbal_yaw_angle_error_; + if (gimbal_yaw_angle_error < 0) + gimbal_yaw_angle_error += 2 * std::numbers::pi; + + double err = gimbal_yaw_angle_error + *gimbal_yaw_angle_; + while (err >= std::numbers::pi) + err -= 2 * std::numbers::pi; + while (err < -std::numbers::pi) + err += 2 * std::numbers::pi; + + double yaw_velocity = *gimbal_yaw_velocity_imu_; + bool is_aligned = std::abs(err) < kAutoClimbAlignThreshold; + bool is_stable = std::abs(yaw_velocity) < kAutoClimbAlignVelocityThreshold; + + if (is_aligned && is_stable) + auto_climb_align_stable_count_++; + else + auto_climb_align_stable_count_ = 0; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "ALIGN: err=%.3f, yaw_velocity=%.3f, stable_ticks=%d", err, + yaw_velocity, auto_climb_align_stable_count_); + + if (auto_climb_align_stable_count_ >= kAutoClimbAlignConfirmTicks) { + enter_auto_climb_state(AutoClimbState::APPROACH); + RCLCPP_INFO(logger_, "Chassis aligned. Entering APPROACH."); + } + + return control; + } + + AutoClimbControl update_auto_climb_approach() { + AutoClimbControl control{ + .front_track_velocity = track_velocity_max_, + .back_climber_velocity = -5.0, + .override_chassis_vx = auto_climb_approach_chassis_velocity_, + }; + + double pitch = *chassis_pitch_imu_; + double target_pitch = auto_climb_stair_index_ == 0 ? first_stair_approach_pitch_ + : second_stair_approach_pitch_; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "APPROACH (step %d): pitch=%.3f, target>%.3f", + auto_climb_stair_index_ + 1, pitch, target_pitch); + + if (pitch > target_pitch) { + enter_auto_climb_state(AutoClimbState::SUPPORT_DEPLOY); + RCLCPP_INFO( + logger_, "Auto climb entering SUPPORT_DEPLOY (step %d).", + auto_climb_stair_index_ + 1); + } + + return control; + } + + AutoClimbControl update_auto_climb_support_deploy() { + AutoClimbControl control{ + .front_track_velocity = 0.0, + .back_climber_velocity = climber_back_control_velocity_abs_, + .override_chassis_vx = auto_climb_support_deploy_chassis_velocity_, + }; + + if (is_back_climber_blocked()) + auto_climb_support_block_count_++; + else + auto_climb_support_block_count_ = 0; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "SUPPORT_DEPLOY (step %d): blocked_ticks=%d", + auto_climb_stair_index_ + 1, auto_climb_support_block_count_); + + if (auto_climb_support_block_count_ >= kAutoClimbSupportConfirmTicks) { + enter_auto_climb_state(AutoClimbState::DASH); + RCLCPP_INFO( + logger_, "Auto climb entering DASH (step %d).", auto_climb_stair_index_ + 1); + } + + return control; + } + + AutoClimbControl update_auto_climb_dash() { + AutoClimbControl control{ + .front_track_velocity = 0, + .back_climber_velocity = climber_back_control_velocity_abs_, + .override_chassis_vx = auto_climb_dash_chassis_velocity_, + }; + + double pitch = *chassis_pitch_imu_; + double leveled_pitch_threshold = auto_climb_stair_index_ == 0 + ? first_stair_dash_leveled_pitch_threshold_ + : second_stair_dash_leveled_pitch_threshold_; + bool is_leveled = + pitch < leveled_pitch_threshold && auto_climb_timer_ > kAutoClimbDashMinTicks; + bool timeout = auto_climb_timer_ > kAutoClimbDashTimeoutTicks; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "DASH (step %d): pitch=%.3f, threshold=%.3f, timer=%d", + auto_climb_stair_index_ + 1, pitch, leveled_pitch_threshold, auto_climb_timer_); + + if (is_leveled || timeout) { + enter_auto_climb_state(AutoClimbState::SUPPORT_RETRACT); + + if (timeout) { + RCLCPP_WARN( + logger_, "Auto climb DASH timeout on step %d. Entering SUPPORT_RETRACT.", + auto_climb_stair_index_ + 1); + } else { + RCLCPP_INFO( + logger_, "Auto climb reached platform on step %d.", + auto_climb_stair_index_ + 1); + } + } + + return control; + } + + AutoClimbControl update_auto_climb_support_retract() { + AutoClimbControl control{ + .front_track_velocity = track_velocity_max_, + .back_climber_velocity = back_climber_recover_count <= 1200 + ? -auto_climb_support_retract_velocity_fast_abs_ + : -auto_climb_support_retract_velocity_slow_abs_, + .override_chassis_vx = auto_climb_support_retract_chassis_velocity_, + }; + + if (is_back_climber_blocked()) + auto_climb_support_block_count_++; + else + auto_climb_support_block_count_ = 0; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "SUPPORT_RETRACT (step %d): blocked_ticks=%d", + auto_climb_stair_index_ + 1, auto_climb_support_block_count_); + + if (auto_climb_support_block_count_ >= kAutoClimbSupportRetractConfirmTicks) { + bool has_next_stair = auto_climb_stair_index_ + 1 < kAutoClimbMaxStairs; + + if (has_next_stair) { + auto_climb_stair_index_++; + enter_auto_climb_state(AutoClimbState::APPROACH); + RCLCPP_INFO( + logger_, "Auto climb continuing to step %d.", auto_climb_stair_index_ + 1); + } else { + int finished_steps = auto_climb_stair_index_ + 1; + stop_auto_climb(); + back_climber_zero_velocity_hold_ = true; + control.front_track_velocity = nan_; + control.back_climber_velocity = 0.0; + control.override_chassis_vx = nan_; + RCLCPP_INFO(logger_, "Auto climb completed (finished %d steps).", finished_steps); + } + } + + return control; + } + + void apply_climb_control(const AutoClimbControl& control) { + *climbing_forward_velocity_ = control.override_chassis_vx; + *auto_climb_active_ = auto_climb_state_ != AutoClimbState::IDLE; + if (back_climber_recover_count != 0) { + back_climber_recover_count--; + } + + dual_motor_sync_control( + control.front_track_velocity, *climber_front_left_velocity_, + *climber_front_right_velocity_, front_velocity_pid_calculator_, + *climber_front_left_requested_control_torque_, + *climber_front_right_requested_control_torque_); + + dual_motor_sync_control( + control.back_climber_velocity, *climber_back_left_velocity_, + *climber_back_right_velocity_, back_velocity_pid_calculator_, + *climber_back_left_control_torque_, *climber_back_right_control_torque_); + + if (back_climber_recover_count > 1200) { + limit_back_climber_retract_torque( + control.back_climber_velocity, *climber_back_left_control_torque_, + *climber_back_right_control_torque_, back_climber_retract_first_torque_); + } else { + limit_back_climber_retract_torque( + control.back_climber_velocity, *climber_back_left_control_torque_, + *climber_back_right_control_torque_, back_climber_retract_second_torque_); + } + + *front_power_budget_active_ = is_front_power_budget_active(); + *front_power_demand_estimate_ = estimate_front_power( + *climber_front_left_requested_control_torque_, + *climber_front_right_requested_control_torque_, *climber_front_left_velocity_, + *climber_front_right_velocity_, front_power_estimate_bias_, + front_power_estimate_k_tau2_, front_power_estimate_k_mech_); + } + + void reset_all_controls() { + *climber_front_left_requested_control_torque_ = nan_; + *climber_front_right_requested_control_torque_ = nan_; + *climber_back_left_control_torque_ = nan_; + *climber_back_right_control_torque_ = nan_; + *climbing_forward_velocity_ = nan_; + *auto_climb_active_ = false; + *front_power_budget_active_ = false; + *front_power_demand_estimate_ = 0.0; + stop_manual_support(); + stop_auto_climb(); + back_climber_zero_velocity_hold_ = false; + } + + void stop_manual_support() { + back_climber_recover_count = 1500; + manual_support_retracting_ = false; + manual_support_retract_block_count_ = 0; + } + + void start_back_climber_retract(const char* source) { + if (!back_climber_recover_count) { + back_climber_recover_count = 1500; + } + manual_support_retracting_ = true; + manual_support_retract_block_count_ = 0; + back_climber_zero_velocity_hold_ = false; + RCLCPP_INFO(logger_, "%s back climber retract started.", source); + } + + void stop_auto_climb() { + auto_climb_state_ = AutoClimbState::IDLE; + auto_climb_timer_ = 0; + auto_climb_stair_index_ = 0; + auto_climb_align_stable_count_ = 0; + auto_climb_support_block_count_ = 0; + } + + void enter_auto_climb_state(AutoClimbState state) { + if (state == auto_climb_state_) + return; + auto_climb_state_ = state; + auto_climb_timer_ = 0; + auto_climb_align_stable_count_ = 0; + auto_climb_support_block_count_ = 0; + } + + bool is_back_climber_blocked() const { + return (std::abs(*climber_back_left_torque_) > kBackClimberBlockedTorqueThreshold + && std::abs(*climber_back_left_velocity_) < kBackClimberBlockedVelocityThreshold) + || (std::abs(*climber_back_right_torque_) > kBackClimberBlockedTorqueThreshold + && std::abs(*climber_back_right_velocity_) < kBackClimberBlockedVelocityThreshold); + } + + bool is_front_power_budget_active() const { + return auto_climb_state_ == AutoClimbState::APPROACH + || auto_climb_state_ == AutoClimbState::SUPPORT_RETRACT; + } + + 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) const { + + if (std::isnan(setpoint)) { + left_torque_out = nan_; + right_torque_out = nan_; + return; + } + + Eigen::Vector2d setpoint_error{setpoint - left_velocity, setpoint - right_velocity}; + Eigen::Vector2d relative_velocity{ + left_velocity - right_velocity, right_velocity - left_velocity}; + + Eigen::Vector2d control_error = setpoint_error - sync_coefficient_ * relative_velocity; + auto control_torques = pid_calculator.update(control_error); + + left_torque_out = control_torques[0]; + right_torque_out = control_torques[1]; + } + + static void limit_back_climber_retract_torque( + double back_climber_velocity_setpoint, double& left_torque, double& right_torque, + double max_torque) { + + if (!std::isfinite(back_climber_velocity_setpoint) || back_climber_velocity_setpoint >= 0.0) + return; + if (!(max_torque > 0.0)) + return; + + const double peak = std::max(std::abs(left_torque), std::abs(right_torque)); + if (peak <= max_torque) + return; + + const double scale = max_torque / peak; + left_torque *= scale; + right_torque *= scale; + } + + rclcpp::Logger logger_; + + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double kAutoClimbAlignThreshold = 0.10; + static constexpr double kAutoClimbAlignVelocityThreshold = 0.2; + static constexpr double kBackClimberBlockedTorqueThreshold = 0.1; + static constexpr double kBackClimberBlockedVelocityThreshold = 0.1; + static constexpr int kAutoClimbAlignConfirmTicks = 50; + static constexpr int kAutoClimbSupportConfirmTicks = 50; + static constexpr int kAutoClimbDashMinTicks = 100; + static constexpr int kAutoClimbDashTimeoutTicks = 3000; + static constexpr int kAutoClimbSupportRetractConfirmTicks = 50; + static constexpr int kAutoClimbMaxStairs = 2; + static constexpr int kManualSupportRetractConfirmTicks = 50; + + double sync_coefficient_; + double first_stair_approach_pitch_; + double second_stair_approach_pitch_; + + double track_velocity_max_; + double climber_back_control_velocity_abs_; + double auto_climb_support_retract_velocity_fast_abs_; + double auto_climb_support_retract_velocity_slow_abs_; + double auto_climb_approach_chassis_velocity_; + double auto_climb_support_deploy_chassis_velocity_; + double auto_climb_support_retract_chassis_velocity_; + double auto_climb_dash_chassis_velocity_; + double first_stair_dash_leveled_pitch_threshold_; + double second_stair_dash_leveled_pitch_threshold_; + double front_power_estimate_bias_; + double front_power_estimate_k_tau2_; + double front_power_estimate_k_mech_; + + AutoClimbState auto_climb_state_ = AutoClimbState::IDLE; + int auto_climb_timer_ = 0; + int auto_climb_stair_index_ = 0; + int auto_climb_align_stable_count_ = 0; + int auto_climb_support_block_count_ = 0; + bool manual_support_retracting_ = false; + int manual_support_retract_block_count_ = 0; + bool back_climber_zero_velocity_hold_ = false; + + OutputInterface climber_front_left_requested_control_torque_; + OutputInterface climber_front_right_requested_control_torque_; + OutputInterface climber_back_left_control_torque_; + OutputInterface climber_back_right_control_torque_; + OutputInterface climbing_forward_velocity_; + OutputInterface auto_climb_active_; + OutputInterface front_power_budget_active_; + OutputInterface front_power_demand_estimate_; + + InputInterface climber_front_left_velocity_; + InputInterface climber_front_right_velocity_; + InputInterface climber_back_left_velocity_; + InputInterface climber_back_right_velocity_; + + InputInterface climber_back_left_torque_; + InputInterface climber_back_right_torque_; + + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface keyboard_; + InputInterface rotary_knob_switch_; + + InputInterface chassis_pitch_imu_; + InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_, gimbal_yaw_velocity_imu_; + + rmcs_msgs::Switch last_rotary_knob_switch_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + + pid::MatrixPidCalculator<2> front_velocity_pid_calculator_, back_velocity_pid_calculator_; + + std::shared_ptr front_power_limiter_; + + 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 + +#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 da70a96f8..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 @@ -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; } @@ -100,24 +139,93 @@ class ChassisController } while (false); last_switch_right_ = switch_right; - last_switch_left_ = switch_left; - last_keyboard_ = keyboard; + last_switch_left_ = switch_left; + last_keyboard_ = keyboard; } 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(); + auto angular_velocity = update_angular_velocity_control(); 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}; @@ -133,21 +241,44 @@ class ChassisController } double update_angular_velocity_control() { - double angular_velocity = 0.0; - double chassis_control_angle = nan; + double angular_velocity = 0.0; + 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::WIRELESS_CHARGING: [[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,20 +289,31 @@ 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_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; *chassis_control_angle_ = chassis_control_angle; return angular_velocity; @@ -181,48 +323,53 @@ 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; - rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); 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 +377,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 2d084f8b5..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 { @@ -51,9 +54,9 @@ class ChassisPowerController using namespace rmcs_msgs; auto switch_right = *switch_right_; - auto switch_left = *switch_left_; - auto keyboard = *keyboard_; - auto rotary_knob = *rotary_knob_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; + auto rotary_knob = *rotary_knob_; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { @@ -74,8 +77,8 @@ class ChassisPowerController // charging_power_limit = constexpr double buffer_energy_control_line = 120; // = referee + excess - constexpr double buffer_energy_base_line = 30; // = referee - constexpr double buffer_energy_dead_line = 0; // = 0 + constexpr double buffer_energy_base_line = 30; // = referee + constexpr double buffer_energy_dead_line = 0; // = 0 *supercap_charge_power_limit_ = *chassis_power_limit_referee_ @@ -91,9 +94,10 @@ class ChassisPowerController } void reset_power_control() { - virtual_buffer_energy_ = virtual_buffer_energy_limit_; - boost_mode_ = false; + 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,18 +111,20 @@ 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; // chassis_control_power_limit = constexpr double supercap_voltage_control_line = 12.5; // = supercap - constexpr double supercap_voltage_base_line = 12.0; // = referee - power_limit = *chassis_power_limit_referee_ + constexpr double supercap_voltage_base_line = 12.0; // = referee + power_limit = *chassis_power_limit_referee_ + (power_limit - *chassis_power_limit_referee_) * std::clamp( (*supercap_voltage_ - supercap_voltage_base_line) @@ -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/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/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index c9c61ace5..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 @@ -1,13 +1,12 @@ #include #include #include -#include -#include #include #include -#include +#include #include +#include #include #include @@ -15,1004 +14,311 @@ #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) + , 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) { - 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_; + const double max_speed = + *mode_ == rmcs_msgs::ChassisMode::WIRELESS_CHARGING ? wireless_charging_speed_limit_ + : translational_velocity_max_; + translational_velocity *= max_speed; + 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]; + 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 (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; - } - // --- main pipeline --- - void run_joint_intent_pipeline_() { - auto feedback = read_joint_feedback_(); + 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); - suspension_.update_imu_calibration( - trajectory_.symmetric_requested(), *chassis_imu_pitch_, *chassis_imu_roll_, - update_dt_()); + 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); - if (!trajectory_.active() - && !trajectory_.initialize_from_feedback( - feedback.motor_angles, feedback.physical_angles)) { - reset_all_controls_(); - return; + 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; } - 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(); + 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; + } - 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; + 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 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; + 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]); } - // --- member variables --- - InputInterface joystick_right_, joystick_left_; - InputInterface switch_right_, switch_left_; - InputInterface mouse_velocity_; - InputInterface mouse_; + static constexpr const char* kJointName[] = { + "left_front", + "left_back", + "right_back", + "right_front", + }; + + 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_; + + 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::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 beeb1d2f1..000000000 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_layer.hpp +++ /dev/null @@ -1,172 +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..e38f545ff --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp @@ -0,0 +1,471 @@ +#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)) + , 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()) { + 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; + 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(); + 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_wireless_charging_posture_transition_(); + 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 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; + } + +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; + + // 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; + return std::all_of(joint_target_deg.begin() + 1, joint_target_deg.end(), [&](double v) { + return std::abs(v - joint_target_deg.front()) <= epsilon; + }); + } + + 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_; + 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_(); + 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_; + } + + 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; + } 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; + } + + 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 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)) + 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_); + + 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; + const bool posture_toggle_requested = + 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; + } + } + + if (apply_symmetric_target_) + joint_current_target_angle_.fill(current_target_angle_); + + 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 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(); + 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 7ae3c1937..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) @@ -194,14 +198,14 @@ class DeformableOmniWheelController const double force_to_torque = friction_coefficient_ * wheel_radius_; 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}, {-lambda_1 - force_to_torque * gamma_1, -y_sign, rhs}, - { lambda_2 - force_to_torque * gamma_2, y_sign, rhs}, + {lambda_2 - force_to_torque * gamma_2, y_sign, rhs}, {-lambda_2 - force_to_torque * gamma_2, -y_sign, rhs}, - {-lambda_1 + force_to_torque * gamma_1, y_sign, rhs}, - { lambda_1 + force_to_torque * gamma_1, -y_sign, rhs}, - {-lambda_2 + force_to_torque * gamma_2, y_sign, rhs}, - { lambda_2 + force_to_torque * gamma_2, -y_sign, rhs}, + {-lambda_1 + force_to_torque * gamma_1, y_sign, rhs}, + {lambda_1 + force_to_torque * gamma_1, -y_sign, rhs}, + {-lambda_2 + force_to_torque * gamma_2, y_sign, rhs}, + {lambda_2 + force_to_torque * gamma_2, -y_sign, rhs}, }; result = qcp_solver_.solve( {1.0, 1.0}, {x_max, std::abs(y_max)}, half_planes, {a, b, c, d, e, f}); @@ -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..ee31f2205 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp @@ -0,0 +1,631 @@ +#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); + 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); + 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(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); + + 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_target_pitch_rad_ = 0.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 ba3b68c0c..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/chassis/hero_chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp new file mode 100644 index 000000000..7e22197c0 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp @@ -0,0 +1,310 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "controller/pid/pid_calculator.hpp" + +namespace rmcs_core::controller::chassis { + +class HeroChassisController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + HeroChassisController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , following_velocity_controller_(10.0, 0.0, 2.4) { + 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_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_); + } + + 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."); + } + chassis_velocity_feedback_ready_ = chassis_velocity_.ready(); + if (!chassis_velocity_feedback_ready_) { + chassis_velocity_.make_and_bind_directly(0.0, 0.0, 0.0); + } + } + + void update() override { + using namespace rmcs_msgs; + + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; + auto mode = *mode_; + + do { + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_all_controls(); + break; + } + + if (switch_left != Switch::DOWN) { + if (last_switch_right_ == Switch::MIDDLE && switch_right == Switch::DOWN) { + 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_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 + || (mode == rmcs_msgs::ChassisMode::STEP_DOWN + && step_down_facing_ == StepDownFacing::BACK)) { + mode = rmcs_msgs::ChassisMode::STEP_DOWN; + step_down_facing_ = StepDownFacing::FRONT; + } else { + mode = rmcs_msgs::ChassisMode::AUTO; + step_down_facing_ = StepDownFacing::FRONT; + } + } else if (!last_keyboard_.z && keyboard.z) { + if (mode != rmcs_msgs::ChassisMode::STEP_DOWN + || (mode == rmcs_msgs::ChassisMode::STEP_DOWN + && step_down_facing_ == StepDownFacing::FRONT)) { + mode = rmcs_msgs::ChassisMode::STEP_DOWN; + step_down_facing_ = StepDownFacing::BACK; + } else { + mode = rmcs_msgs::ChassisMode::AUTO; + step_down_facing_ = StepDownFacing::BACK; + } + } + *mode_ = mode; + } + + update_velocity_control(); + } while (false); + + last_switch_right_ = switch_right; + last_switch_left_ = switch_left; + last_keyboard_ = keyboard; + } + + void reset_all_controls() { + *mode_ = rmcs_msgs::ChassisMode::AUTO; + step_down_facing_ = StepDownFacing::BACK; + + *chassis_control_velocity_ = {nan, nan, nan}; + } + + void update_velocity_control() { + auto translational_velocity = update_translational_velocity_control(); + auto angular_velocity = update_angular_velocity_control(translational_velocity); + + chassis_control_velocity_->vector << translational_velocity, angular_velocity; + } + + Eigen::Vector2d update_translational_velocity_control() { + if (!std::isnan(*climbing_forward_velocity_)) { + return Eigen::Vector2d{*climbing_forward_velocity_, 0.0}; + } + + 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(); + + translational_velocity *= translational_velocity_max; + + return translational_velocity; + } + + double update_angular_velocity_control(const Eigen::Vector2d& translational_velocity) { + double angular_velocity = 0.0; + double chassis_control_angle = nan; + + if (!std::isnan(*climbing_forward_velocity_)) { + double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + + constexpr double alignment = 2 * std::numbers::pi; + if (err > alignment / 2) + err -= alignment; + + 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; + } + + switch (*mode_) { + case rmcs_msgs::ChassisMode::AUTO: { + angular_velocity = + update_following_angular_velocity(StepDownFacing::BACK, chassis_control_angle); + + // Keep AUTO rear-following gentle at low translation speed and fully enabled at max. + const double measured_translational_speed = + chassis_velocity_feedback_ready_ ? chassis_velocity_->vector.head<2>().norm() + : translational_velocity.norm(); + angular_velocity *= + std::clamp(measured_translational_speed / translational_velocity_max, 0.0, 0.3); + + angular_velocity = 0.0; + + } break; + 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; + case rmcs_msgs::ChassisMode::STEP_DOWN: { + 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::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); + + // 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; + } + *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; + *chassis_control_angle_ = chassis_control_angle; + + return angular_velocity; + } + + double calculate_unsigned_chassis_angle_error(double& chassis_control_angle) { + chassis_control_angle = normalize_positive_angle(*gimbal_yaw_angle_error_); + + // err = setpoint - measurement + // ^ ^ + // |gimbal_yaw_angle_error |chassis_angle + // ^ + // |(2pi - gimbal_yaw_angle) + double err = normalize_positive_angle(chassis_control_angle + *gimbal_yaw_angle_); + + return err; + } + +private: + enum class StepDownFacing { FRONT, BACK }; + + double update_following_angular_velocity( + StepDownFacing target_facing, double& chassis_control_angle) { + double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + if (target_facing == StepDownFacing::BACK) { + chassis_control_angle = + normalize_positive_angle(chassis_control_angle + std::numbers::pi); + err = normalize_positive_angle(err + std::numbers::pi); + } + + err = normalize_signed_angle(err); + return following_velocity_controller_.update(err); + } + + 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; + } + + static constexpr double nan = std::numeric_limits::quiet_NaN(); + + // Maximum control velocities + static constexpr double translational_velocity_max = 10.0; + static constexpr double 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; + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + + InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; + InputInterface chassis_velocity_; + InputInterface climbing_forward_velocity_; + OutputInterface chassis_angle_, chassis_control_angle_; + + OutputInterface mode_; + bool spinning_forward_ = true; + bool chassis_velocity_feedback_ready_ = false; + StepDownFacing step_down_facing_ = StepDownFacing::BACK; + pid::PidCalculator following_velocity_controller_; + + OutputInterface chassis_control_velocity_; +}; + +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::HeroChassisController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_power_controller.cpp new file mode 100644 index 000000000..ee59e1ce5 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_power_controller.cpp @@ -0,0 +1,232 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "referee/app/ui/shape/shape.hpp" + +namespace rmcs_core::controller::chassis { + +using namespace referee::app; + +class HeroChassisPowerController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + HeroChassisPowerController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + get_parameter("front_climber_power_limit_max", front_climber_power_limit_max_); + get_parameter("drive_power_limit_floor", drive_power_limit_floor_); + get_parameter("auto_climb_min_control_power_limit", auto_climb_min_control_power_limit_); + register_input("/chassis/control_mode", mode_); + + 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("/chassis/power", chassis_power_); + register_input("/chassis/supercap/voltage", supercap_voltage_); + register_input("/chassis/supercap/enabled", supercap_enabled_); + + register_input("/referee/chassis/power_limit", chassis_power_limit_referee_); + register_input("/referee/chassis/buffer_energy", chassis_buffer_energy_referee_); + register_input( + "/chassis/climber/front/power_budget_active", front_power_budget_active_, false); + register_input( + "/chassis/climber/front/power_demand_estimate", front_power_demand_estimate_, false); + register_input("/chassis/climber/auto_climb_active", auto_climb_active_, false); + + register_output("/chassis/supercap/charge_power_limit", supercap_charge_power_limit_, 0.0); + register_output("/chassis/control_power_limit", chassis_control_power_limit_, 0.0); + register_output( + "/chassis/climber/front/control_power_limit", front_climber_control_power_limit_, 0.0); + + register_output( + "/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); + } + + void before_updating() override { + if (!front_power_budget_active_.ready()) + front_power_budget_active_.make_and_bind_directly(false); + if (!front_power_demand_estimate_.ready()) + front_power_demand_estimate_.make_and_bind_directly(0.0); + if (!auto_climb_active_.ready()) + auto_climb_active_.make_and_bind_directly(false); + } + + void update() override { + update_charging_power_limit(); + + update_ui(); + + using namespace rmcs_msgs; + + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; + auto rotary_knob = *rotary_knob_; + + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_power_control(); + return; + } + + update_virtual_buffer_energy(); + + boost_mode_ = keyboard.shift || rotary_knob < -0.9 || *auto_climb_active_; + update_control_power_limit(); + } + +private: + void update_charging_power_limit() { + // Maximum excess power when buffer energy is sufficient. + constexpr double excess_power_limit = 35; + + // charging_power_limit = + constexpr double buffer_energy_control_line = 120; // = referee + excess + constexpr double buffer_energy_base_line = 30; // = referee + constexpr double buffer_energy_dead_line = 0; // = 0 + + *supercap_charge_power_limit_ = + *chassis_power_limit_referee_ + * std::clamp( + (*chassis_buffer_energy_referee_ - buffer_energy_dead_line) + / (buffer_energy_base_line - buffer_energy_dead_line), + 0.0, 1.0) + + excess_power_limit + * std::clamp( + (*chassis_buffer_energy_referee_ - buffer_energy_base_line) + / (buffer_energy_control_line - buffer_energy_base_line), + 0.0, 1.0); + } + + void reset_power_control() { + virtual_buffer_energy_ = virtual_buffer_energy_limit_; + boost_mode_ = false; + *chassis_control_power_limit_ = 0.0; + *front_climber_control_power_limit_ = 0.0; + } + + void update_virtual_buffer_energy() { + constexpr double dt = 1e-3; + virtual_buffer_energy_ += dt * (chassis_power_limit_expected_ - *chassis_power_); + virtual_buffer_energy_ = std::clamp( + virtual_buffer_energy_, 0.0, + std::min(*chassis_buffer_energy_referee_, virtual_buffer_energy_limit_)); + } + + void update_control_power_limit() { + double total_power_limit; + + if (boost_mode_ && *supercap_enabled_) + total_power_limit = *mode_ == rmcs_msgs::ChassisMode::LAUNCH_RAMP + ? inf_ + : *chassis_power_limit_referee_ + 80.0; + else + total_power_limit = *chassis_power_limit_referee_; + chassis_power_limit_expected_ = total_power_limit; + + // chassis_control_power_limit = + constexpr double supercap_voltage_control_line = 12.5; // = supercap + constexpr double supercap_voltage_base_line = 12.0; // = referee + total_power_limit = + *chassis_power_limit_referee_ + + (total_power_limit - *chassis_power_limit_referee_) + * std::clamp( + (*supercap_voltage_ - supercap_voltage_base_line) + / (supercap_voltage_control_line - supercap_voltage_base_line), + 0.0, 1.0); + + // Maximum excess power when virtual buffer energy is full. + constexpr double excess_power_limit = 0; + + total_power_limit += excess_power_limit; + total_power_limit *= virtual_buffer_energy_ / virtual_buffer_energy_limit_; + + if (*auto_climb_active_) + total_power_limit = std::max(total_power_limit, auto_climb_min_control_power_limit_); + + const auto [drive_limit, front_limit] = split_control_power_limit(total_power_limit); + *chassis_control_power_limit_ = drive_limit; + *front_climber_control_power_limit_ = front_limit; + } + + std::pair split_control_power_limit(double total_power_limit) const { + if (!*front_power_budget_active_) + return {total_power_limit, 0.0}; + + if (total_power_limit <= drive_power_limit_floor_) + return {total_power_limit, 0.0}; + + const double front_limit = std::min( + {*front_power_demand_estimate_, front_climber_power_limit_max_, + total_power_limit - drive_power_limit_floor_}); + return {total_power_limit - front_limit, front_limit}; + } + + void update_ui() { + chassis_power_ui_.set_value(static_cast(std::round(*chassis_power_))); + chassis_control_power_limit_ui_.set_value( + static_cast(std::round(*chassis_control_power_limit_))); + } + + static constexpr double inf_ = std::numeric_limits::infinity(); + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + + InputInterface mode_; + + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface keyboard_; + InputInterface rotary_knob_; + + InputInterface chassis_power_; + static constexpr double virtual_buffer_energy_limit_ = 30.0; + double virtual_buffer_energy_; + + InputInterface supercap_voltage_; + InputInterface supercap_enabled_; + + InputInterface chassis_power_limit_referee_; + InputInterface chassis_buffer_energy_referee_; + InputInterface front_power_budget_active_; + InputInterface front_power_demand_estimate_; + InputInterface auto_climb_active_; + + bool boost_mode_ = false; + OutputInterface supercap_charge_power_limit_; + double chassis_power_limit_expected_; + OutputInterface chassis_control_power_limit_; + OutputInterface front_climber_control_power_limit_; + double front_climber_power_limit_max_; + double drive_power_limit_floor_; + double auto_climb_min_control_power_limit_ = 0.0; + + OutputInterface supercap_voltage_control_line_; + OutputInterface supercap_voltage_base_line_; + OutputInterface supercap_voltage_dead_line_; + + ui::Integer chassis_power_ui_{ui::Shape::Color::WHITE, 15, 2, ui::x_center, 100, 0}; + ui::Integer chassis_control_power_limit_ui_{ + ui::Shape::Color::WHITE, 15, 2, ui::x_center, 150, 0}; +}; + +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::HeroChassisPowerController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_steering_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_steering_wheel_controller.cpp new file mode 100644 index 000000000..1db280c50 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_steering_wheel_controller.cpp @@ -0,0 +1,518 @@ +#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 HeroSteeringWheelController + : public rmcs_executor::Component + , public rclcpp::Node { + + using Formula = std::tuple; + +public: + explicit HeroSteeringWheelController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , mess_(get_parameter("mess").as_double()) + , moment_of_inertia_(get_parameter("moment_of_inertia").as_double()) + , vehicle_radius_(get_parameter("vehicle_radius").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()) + , 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) + , 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) { + + 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/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_); + register_output( + "/chassis/steering_wheel/actual_power_estimate", steering_wheel_actual_power_estimate_, + 0.0); + } + + void update() override { + if (std::isnan(chassis_control_velocity_->vector[0])) { + reset_all_controls(); + return; + } + + integral_yaw_angle_imu(); + + auto steering_status = calculate_steering_status(); + auto wheel_velocities = calculate_wheel_velocities(); + auto chassis_velocity = calculate_chassis_velocity(steering_status, wheel_velocities); + + auto chassis_status_expected = calculate_chassis_status_expected(chassis_velocity); + auto chassis_control_velocity = calculate_chassis_control_velocity(); + + auto chassis_acceleration = calculate_chassis_control_acceleration( + chassis_status_expected.velocity, chassis_control_velocity); + + double power_limit = + *power_limit_ - no_load_power_ - k2_ * wheel_velocities.array().pow(2).sum(); + + auto wheel_pid_torques = + calculate_wheel_pid_torques(steering_status, wheel_velocities, chassis_status_expected); + + auto constrained_chassis_acceleration = constrain_chassis_control_acceleration( + steering_status, wheel_velocities, chassis_acceleration, wheel_pid_torques, + power_limit); + auto filtered_chassis_acceleration = + odom_to_base_link_vector(control_acceleration_filter_.update( + base_link_to_odom_vector(constrained_chassis_acceleration))); + + auto steering_torques = calculate_steering_control_torques( + steering_status, chassis_status_expected, filtered_chassis_acceleration); + auto wheel_torques = calculate_wheel_control_torques( + steering_status, filtered_chassis_acceleration, wheel_pid_torques); + + update_control_torques(steering_torques, wheel_torques); + *steering_wheel_actual_power_estimate_ = + calculate_actual_power_estimate(wheel_velocities, wheel_torques); + update_chassis_velocity_expected(filtered_chassis_acceleration); + } + +private: + struct SteeringStatus { + Eigen::Vector4d angle, cos_angle, sin_angle; + Eigen::Vector4d velocity; + }; + + struct ChassisStatus { + Eigen::Vector3d velocity; + Eigen::Vector4d wheel_velocity_x, wheel_velocity_y; + }; + + void reset_all_controls() { + control_acceleration_filter_.reset(); + + chassis_yaw_angle_imu_ = 0.0; + chassis_velocity_expected_ = Eigen::Vector3d::Zero(); + + *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; + *steering_wheel_actual_power_estimate_ = 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); + } + + SteeringStatus calculate_steering_status() { + SteeringStatus steering_status; + + steering_status.angle = { + *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(); + + steering_status.velocity = { + *left_front_steering_velocity_, // + *left_back_steering_velocity_, // + *right_back_steering_velocity_, // + *right_front_steering_velocity_ // + }; + + return steering_status; + } + + Eigen::Vector4d calculate_wheel_velocities() { + return { + *left_front_wheel_velocity_, // + *left_back_wheel_velocity_, // + *right_back_wheel_velocity_, // + *right_front_wheel_velocity_ // + }; + } + + Eigen::Vector3d calculate_chassis_velocity( + const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities) const { + Eigen::Vector3d velocity; + double one_quarter_r = wheel_radius_ / 4.0; + velocity.x() = one_quarter_r * wheel_velocities.dot(steering_status.cos_angle); + velocity.y() = one_quarter_r * wheel_velocities.dot(steering_status.sin_angle); + velocity.z() = -one_quarter_r / vehicle_radius_ + * (-wheel_velocities[0] * steering_status.sin_angle[0] + + wheel_velocities[1] * steering_status.cos_angle[1] + + wheel_velocities[2] * steering_status.sin_angle[2] + - wheel_velocities[3] * steering_status.cos_angle[3]); + return velocity; + } + + ChassisStatus calculate_chassis_status_expected(const Eigen::Vector3d& chassis_velocity) { + auto calculate_energy = [this](const Eigen::Vector3d& velocity) { + return mess_ * velocity.head<2>().squaredNorm() + + moment_of_inertia_ * velocity.z() * velocity.z(); + }; + auto chassis_energy = calculate_energy(chassis_velocity); + auto chassis_energy_expected = calculate_energy(chassis_velocity_expected_); + if (chassis_energy_expected > chassis_energy) { + double k = std::sqrt(chassis_energy / chassis_energy_expected); + chassis_velocity_expected_ *= k; + } + + ChassisStatus chassis_status_expected; + chassis_status_expected.velocity = odom_to_base_link_vector(chassis_velocity_expected_); + + const auto& [vx, vy, vz] = chassis_status_expected.velocity; + chassis_status_expected.wheel_velocity_x = vx - vehicle_radius_ * vz * sin_varphi_.array(); + chassis_status_expected.wheel_velocity_y = vy + vehicle_radius_ * vz * cos_varphi_.array(); + + return chassis_status_expected; + } + + Eigen::Vector3d calculate_chassis_control_velocity() { + 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; + } + + Eigen::Vector3d calculate_chassis_control_acceleration( + const Eigen::Vector3d& chassis_velocity_expected, + const Eigen::Vector3d& chassis_control_velocity) { + + Eigen::Vector2d translational_control_velocity = chassis_control_velocity.head<2>(); + Eigen::Vector2d translational_velocity = chassis_velocity_expected.head<2>(); + Eigen::Vector2d translational_control_acceleration = + chassis_translational_velocity_pid_.update( + translational_control_velocity - translational_velocity); + + const double& angular_control_velocity = chassis_control_velocity[2]; + const double& angular_velocity = chassis_velocity_expected[2]; + double angular_control_acceleration = + chassis_angular_velocity_pid_.update(angular_control_velocity - angular_velocity); + + 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; + } + + Eigen::Vector4d calculate_wheel_pid_torques( + const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities, + const ChassisStatus& chassis_status_expected) { + 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); + } + + Eigen::Vector3d constrain_chassis_control_acceleration( + const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities, + const Eigen::Vector3d& chassis_acceleration, const Eigen::Vector4d& 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 rhombus_top = rhombus_right * mess_ * vehicle_radius_ / moment_of_inertia_; + + auto [a, b, c, d, e, f] = calculate_ellipse_parameters( + steering_status, wheel_velocities, translational_acceleration_direction, + angular_acceleration_direction, wheel_pid_torques); + + Eigen::Vector2d best_point = qcp_solver_.solve( + {1.0, 0.2}, {translational_acceleration_max, angular_acceleration_max}, + {rhombus_right, rhombus_top}, {a, b, c, d, e, f - power_limit}); + + Eigen::Vector3d best_acceleration; + best_acceleration << best_point.x() * translational_acceleration_direction, + best_point.y() * angular_acceleration_direction; + return best_acceleration; + } + + Eigen::Vector calculate_ellipse_parameters( + const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities, + const Eigen::Vector2d& translational_acceleration_direction, + const double& angular_acceleration_direction, const Eigen::Vector4d& wheel_torque_base) { + Eigen::Vector4d cos_alpha_minus_gamma = + steering_status.cos_angle.array() * translational_acceleration_direction.x() + + steering_status.sin_angle.array() * translational_acceleration_direction.y(); + Eigen::Vector4d sin_alpha_minus_varphi = + cos_varphi_.array() * steering_status.sin_angle.array() + - sin_varphi_.array() * steering_status.cos_angle.array(); + Eigen::Vector4d double_k1_torque_base_plus_wheel_velocities = + 2 * k1_ * wheel_torque_base.array() + wheel_velocities.array(); + + Eigen::Vector formula; + auto& [a, b, c, d, e, f] = formula; + + a = (k1_ * mess_ * mess_ * wheel_radius_ * wheel_radius_ / 16.0) + * cos_alpha_minus_gamma.array().pow(2).sum(); + b = ((k1_ * mess_ * moment_of_inertia_ * wheel_radius_ * wheel_radius_) + / (8.0 * vehicle_radius_)) + * angular_acceleration_direction + * (cos_alpha_minus_gamma.array() * sin_alpha_minus_varphi.array()).sum(); + c = ((k1_ * moment_of_inertia_ * moment_of_inertia_ * wheel_radius_ * wheel_radius_) + / (16.0 * vehicle_radius_ * vehicle_radius_)) + * sin_alpha_minus_varphi.array().pow(2).sum(); + d = (mess_ * wheel_radius_ / 4.0) + * (double_k1_torque_base_plus_wheel_velocities.array() * cos_alpha_minus_gamma.array()) + .sum(); + e = ((moment_of_inertia_ * wheel_radius_) / (4.0 * vehicle_radius_)) + * angular_acceleration_direction + * (double_k1_torque_base_plus_wheel_velocities.array() * sin_alpha_minus_varphi.array()) + .sum(); + f = (wheel_torque_base.array() + * (k1_ * wheel_torque_base.array() + wheel_velocities.array())) + .sum(); + + return formula; + } + + Eigen::Vector4d calculate_steering_control_torques( + const SteeringStatus& steering_status, const ChassisStatus& chassis_status_expected, + const Eigen::Vector3d& chassis_acceleration) { + + const auto& [vx, vy, vz] = chassis_status_expected.velocity; + const auto& [ax, ay, az] = chassis_acceleration; + + 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_velocities = + vx * ay - vy * ax - vz * (vx * vx + vy * vy) + + vehicle_radius_ * (az * vx - vz * (ax + vz * vy)) * cos_varphi_.array() + + vehicle_radius_ * (az * vy - vz * (ay - vz * vx)) * sin_varphi_.array(); + Eigen::Vector4d steering_control_angles; + + for (int i = 0; i < steering_control_velocities.size(); ++i) { + if (dot_r_squared[i] > 1e-2) { + steering_control_velocities[i] /= dot_r_squared[i]; + steering_control_angles[i] = std::atan2( + chassis_status_expected.wheel_velocity_y[i], + chassis_status_expected.wheel_velocity_x[i]); + } else { + auto x = ax - vehicle_radius_ * (az * sin_varphi_[i] + 0 * cos_varphi_[i]); + auto y = ay + vehicle_radius_ * (az * cos_varphi_[i] - 0 * sin_varphi_[i]); + if (x * x + y * y > 1e-6) { + steering_control_velocities[i] = 0.0; + steering_control_angles[i] = std::atan2(y, x); + } else { + steering_control_velocities[i] = nan_; + steering_control_angles[i] = nan_; + } + } + } + + Eigen::Vector4d steering_torques = steering_velocity_pid_.update( + steering_control_velocities + + steering_angle_pid_.update( + (steering_control_angles - 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_torques.unaryExpr([](double v) { return std::isnan(v) ? 0.0 : v; }); + } + + Eigen::Vector4d calculate_wheel_control_torques( + const SteeringStatus& steering_status, const Eigen::Vector3d& chassis_acceleration, + const Eigen::Vector4d& wheel_pid_torques) { + + const auto& [ax, ay, az] = chassis_acceleration; + Eigen::Vector4d wheel_torques = + wheel_radius_ + * (ax * mess_ * steering_status.cos_angle.array() + + ay * mess_ * steering_status.sin_angle.array() + + az * moment_of_inertia_ + * (cos_varphi_.array() * steering_status.sin_angle.array() + - sin_varphi_.array() * steering_status.cos_angle.array()) + / vehicle_radius_) + / 4.0; + + wheel_torques += wheel_pid_torques; + + return wheel_torques; + } + + double calculate_actual_power_estimate( + const Eigen::Vector4d& wheel_velocities, const Eigen::Vector4d& wheel_torques) const { + return no_load_power_ + k2_ * wheel_velocities.array().pow(2).sum() + + (wheel_torques.array() * (k1_ * wheel_torques.array() + wheel_velocities.array())) + .sum(); + } + + void update_control_torques( + const Eigen::Vector4d& steering_torques, const Eigen::Vector4d& wheel_torques) { + *left_front_steering_control_torque_ = steering_torques[0]; + *left_back_steering_control_torque_ = steering_torques[1]; + *right_back_steering_control_torque_ = steering_torques[2]; + *right_front_steering_control_torque_ = steering_torques[3]; + + *left_front_wheel_control_torque_ = wheel_torques[0]; + *left_back_wheel_control_torque_ = wheel_torques[1]; + *right_back_wheel_control_torque_ = wheel_torques[2]; + *right_front_wheel_control_torque_ = wheel_torques[3]; + } + + void update_chassis_velocity_expected(const Eigen::Vector3d& chassis_acceleration) { + auto acceleration_odom = base_link_to_odom_vector(chassis_acceleration); + chassis_velocity_expected_ += dt_ * acceleration_odom; + } + + 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; + } + + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double inf_ = std::numeric_limits::infinity(); + + static constexpr double dt_ = 1e-3; + static constexpr double g_ = 9.81; + + const double mess_; + const double moment_of_inertia_; + const double vehicle_radius_; + const double wheel_radius_; + const double friction_coefficient_; + + const double k1_, k2_, no_load_power_; + + 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 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_; + OutputInterface steering_wheel_actual_power_estimate_; + + 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_; + + const Eigen::Vector4d cos_varphi_, sin_varphi_; + + pid::MatrixPidCalculator<4> steering_velocity_pid_, steering_angle_pid_, wheel_velocity_pid_; +}; + +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::HeroSteeringWheelController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/omni_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/omni_wheel_controller.cpp index a6e181da3..fc194e4a4 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/omni_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/omni_wheel_controller.cpp @@ -131,8 +131,10 @@ class OmniWheelController const auto& [x, y, z] = chassis_velocity; constexpr double a_plus_b = chassis_radius_x_ + chassis_radius_y_; Eigen::Vector4d wheel_control_velocity = { - -x + y + a_plus_b * z, -x - y + a_plus_b * z, // - +x - y + a_plus_b * z, +x + y + a_plus_b * z, // + -x + y + a_plus_b * z, + -x - y + a_plus_b * z, // + +x - y + a_plus_b * z, + +x + y + a_plus_b * z, // }; wheel_control_velocity *= -1 / (std::numbers::sqrt2 * wheel_radius_); return wheel_velocity_pid_.update(wheel_control_velocity - wheel_velocities); diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp index aeba601a7..6c7085862 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp @@ -61,9 +61,9 @@ class QcpSolver { const std::vector& half_planes, const QuadraticConstraint& quadratic_constraint) { std::vector polygon = { - {boundary_constraint.x_max, boundary_constraint.y_max}, - { 0.0, boundary_constraint.y_max}, - { 0.0, -boundary_constraint.y_max}, + {boundary_constraint.x_max, boundary_constraint.y_max}, + {0.0, boundary_constraint.y_max}, + {0.0, -boundary_constraint.y_max}, {boundary_constraint.x_max, -boundary_constraint.y_max}, }; @@ -129,10 +129,7 @@ class QcpSolver { [&line](const Eigen::Vector2d& p1, const Eigen::Vector2d& p2) -> Eigen::Vector2d { Eigen::Vector3d line2 = Eigen::Vector3d(p1.x(), p1.y(), 1).cross(Eigen::Vector3d(p2.x(), p2.y(), 1)); - Eigen::Matrix2d matrix{ - { line.x(), line.y()}, - {line2.x(), line2.y()} - }; + Eigen::Matrix2d matrix{{line.x(), line.y()}, {line2.x(), line2.y()}}; return matrix.inverse() * Eigen::Vector2d(-line.z(), -line2.z()); }); } 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/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/chassis/steering_wheel_status.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_status.cpp new file mode 100644 index 000000000..8450b3863 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_status.cpp @@ -0,0 +1,104 @@ +#include + +#include +#include +#include +#include + +namespace rmcs_core::controller::chassis { + +class SteeringWheelStatus + : public rmcs_executor::Component + , public rclcpp::Node { +public: + explicit SteeringWheelStatus() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , vehicle_radius_(get_parameter("vehicle_radius").as_double()) + , wheel_radius_(get_parameter("wheel_radius").as_double()) { + 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_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_output("/chassis/velocity", chassis_velocity_, 0.0, 0.0, 0.0); + } + + void update() override { + auto steering_status = calculate_steering_status(); + auto wheel_velocities = calculate_wheel_velocities(); + chassis_velocity_->vector = calculate_chassis_velocity(steering_status, wheel_velocities); + } + +private: + struct SteeringStatus { + Eigen::Vector4d angle, cos_angle, sin_angle; + }; + + SteeringStatus calculate_steering_status() const { + SteeringStatus steering_status; + + steering_status.angle = { + *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(); + + return steering_status; + } + + Eigen::Vector4d calculate_wheel_velocities() const { + return { + *left_front_wheel_velocity_, // + *left_back_wheel_velocity_, // + *right_back_wheel_velocity_, // + *right_front_wheel_velocity_ // + }; + } + + Eigen::Vector3d calculate_chassis_velocity( + const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities) const { + Eigen::Vector3d velocity; + double one_quarter_r = wheel_radius_ / 4.0; + velocity.x() = one_quarter_r * wheel_velocities.dot(steering_status.cos_angle); + velocity.y() = one_quarter_r * wheel_velocities.dot(steering_status.sin_angle); + velocity.z() = -one_quarter_r / vehicle_radius_ + * (-wheel_velocities[0] * steering_status.sin_angle[0] + + wheel_velocities[1] * steering_status.cos_angle[1] + + wheel_velocities[2] * steering_status.sin_angle[2] + - wheel_velocities[3] * steering_status.cos_angle[3]); + return velocity; + } + + const double vehicle_radius_; + const double wheel_radius_; + + InputInterface left_front_steering_angle_; + InputInterface left_back_steering_angle_; + InputInterface right_back_steering_angle_; + InputInterface right_front_steering_angle_; + + InputInterface left_front_wheel_velocity_; + InputInterface left_back_wheel_velocity_; + InputInterface right_back_wheel_velocity_; + InputInterface right_front_wheel_velocity_; + + OutputInterface chassis_velocity_; +}; + +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::SteeringWheelStatus, rmcs_executor::Component) 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/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/controller/gimbal/dual_yaw_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp index 739b58592..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,9 +1,14 @@ +#include +#include +#include #include #include +#include #include #include +#include #include "controller/pid/pid_calculator.hpp" @@ -35,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/top_yaw/control_angle", top_yaw_control_torque_, 0.0); - register_output("/gimbal/bottom_yaw/control_angle_shift", 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"); @@ -61,34 +70,255 @@ class DualYawController } void update() override { - if (std::isnan(*control_angle_error_)) { + const auto mode = *gimbal_mode_; + + 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; + } + + 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_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()); - } - if (std::isnan(*control_angle_shift_)) { - *top_yaw_control_angle_ = nan_; - *bottom_yaw_control_angle_shift_ = nan_; - } else { - *top_yaw_control_angle_ = 0.0; - *bottom_yaw_control_angle_shift_ = *control_angle_shift_; + *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(); + 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() { - double err = *top_yaw_angle_ + *control_angle_error_; - if (err > std::numbers::pi) - err -= 2 * std::numbers::pi; + if (!std::isfinite(*top_yaw_angle_) || !std::isfinite(*control_angle_error_)) + return nan_; + + constexpr double alignment = 2 * std::numbers::pi; + double err = + std::fmod(*top_yaw_angle_ + *control_angle_error_ + std::numbers::pi, alignment); + if (err < 0) + err += alignment; + err -= std::numbers::pi; return err; } @@ -96,19 +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; + EncoderState encoder_state_ = EncoderState::Idle; - OutputInterface top_yaw_control_angle_; - OutputInterface bottom_yaw_control_angle_shift_; + 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/hero_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp index f20bd98fe..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 @@ -1,5 +1,7 @@ +#include #include +#include #include #include #include @@ -22,13 +24,7 @@ class HeroGimbalController HeroGimbalController() : Node( get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , imu_gimbal_solver( - *this, get_parameter("upper_limit").as_double(), - get_parameter("lower_limit").as_double()) - , encoder_gimbal_solver( - *this, get_parameter("upper_limit").as_double(), - get_parameter("lower_limit").as_double()) { + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { register_input("/remote/joystick/left", joystick_left_); register_input("/remote/switch/left", switch_left_); @@ -37,7 +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("/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); @@ -51,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) @@ -59,25 +58,37 @@ class HeroGimbalController break; } - if (!last_keyboard_.q && keyboard_->q) { - if (gimbal_mode_keyboard_ == GimbalMode::IMU) + if (!last_keyboard_.e && keyboard_->e) { + if (gimbal_mode_keyboard_ == GimbalMode::IMU) { + encoder_init_pitch_ = keyboard_->ctrl ? kCtrlEInitPitch : kEInitPitch; gimbal_mode_keyboard_ = GimbalMode::ENCODER; - else + } else { gimbal_mode_keyboard_ = GimbalMode::IMU; + } } - *gimbal_mode_ = - *switch_right_ == Switch::UP ? GimbalMode::ENCODER : gimbal_mode_keyboard_; + + 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_; @@ -91,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; @@ -104,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; @@ -121,32 +136,49 @@ class HeroGimbalController double yaw_shift = joystick_sensitivity * joystick_left_->y() + mouse_sensitivity * mouse_velocity_->y(); double pitch_shift = - -joystick_sensitivity * joystick_left_->x() - mouse_sensitivity * mouse_velocity_->x(); + -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(PreciseTwoAxisGimbalSolver::SetControlPitch{-0.31}); + if (!encoder_gimbal_solver_.enabled()) { + return encoder_gimbal_solver_.update( + PreciseTwoAxisGimbalSolver::SetControlPitch{encoder_init_pitch_}); + } - constexpr double joystick_sensitivity = 0.006 * 0.1; 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; 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(); + + 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.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_; InputInterface switch_left_; @@ -156,21 +188,35 @@ class HeroGimbalController rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + InputInterface auto_aim_should_control_; InputInterface auto_aim_control_direction_; + InputInterface tf_; rmcs_msgs::GimbalMode gimbal_mode_keyboard_ = rmcs_msgs::GimbalMode::IMU; OutputInterface gimbal_mode_; - 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) \ No newline at end of file + rmcs_core::controller::gimbal::HeroGimbalController, rmcs_executor::Component) 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 8eb7e6477..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,7 @@ class PlayerViewer register_input("/remote/mouse/mouse_wheel", mouse_wheel_); register_input("/remote/keyboard", keyboard_); - register_input("/gimbal/pitch/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_); register_output( @@ -46,18 +46,21 @@ class PlayerViewer using namespace rmcs_msgs; const auto switch_right = *switch_right_; - const auto switch_left = *switch_left_; - const auto keyboard = *keyboard_; + const auto switch_left = *switch_left_; + const auto keyboard = *keyboard_; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { reset_all_controls(); } else { - if (!last_keyboard_.e && keyboard.e) + 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_active_ = !scope_active_; + *is_scope_active_ = scope_active_; scope_viewer_reset_ = scope_active_; } @@ -72,17 +75,17 @@ class PlayerViewer *scope_control_torque_ = nan_; *viewer_control_angle_error_ = nan_; - *viewer_control_angle_ = nan_; + *viewer_control_angle_ = nan_; - *is_scope_active_ = false; - scope_active_ = false; + *is_scope_active_ = false; + scope_active_ = false; scope_viewer_reset_ = false; - viewer_reset_ = true; + viewer_reset_ = true; } void update_viewer_control() { constexpr double scope_offset_angle = 0.31; - *scope_offset_angle_ = scope_offset_angle; + *scope_offset_angle_ = scope_offset_angle; auto unit_sensitivity = [&](double sensitivity) { return (*is_scope_active_) ? sensitivity : 1.0; @@ -92,17 +95,17 @@ class PlayerViewer *viewer_delta_angle_by_mouse_wheel_ = 0.5 * *mouse_wheel_ * unit_sensitivity(0.09); if (viewer_reset_) { - *viewer_control_angle_ = upper_limit_; - viewer_reset_ = false; + *viewer_control_angle_ = viewer_init_angle_; + viewer_reset_ = false; } else { if (scope_viewer_reset_) { *viewer_control_angle_ = *scope_offset_angle_; - scope_viewer_reset_ = false; + scope_viewer_reset_ = false; } else { *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; }; @@ -115,26 +118,31 @@ class PlayerViewer *viewer_control_angle_ - norm_angle(*gimbal_player_viewer_angle_); if (scope_active_) { - *scope_control_torque_ = 0.2; + *scope_control_torque_ = 0.13; } else { - *scope_control_torque_ = -0.2; + *scope_control_torque_ = -0.13; } } private: static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - static constexpr double pi_ = std::numbers::pi; + static constexpr double pi_ = std::numbers::pi; + + // The steering-hero viewer angle limit range is [0.68, 1.17]. + 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}; const double upper_limit_, lower_limit_; + double viewer_init_angle_ = kEInitViewerAngle; InputInterface switch_right_; InputInterface switch_left_; InputInterface keyboard_; InputInterface mouse_wheel_; - InputInterface gimbal_pitch_angle_; + InputInterface gimbal_pitch_raw_angle_; InputInterface gimbal_player_viewer_angle_; OutputInterface scope_control_torque_; @@ -155,4 +163,4 @@ class PlayerViewer #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::gimbal::PlayerViewer, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::gimbal::PlayerViewer, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/precise_two_axis_gimbal_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/precise_two_axis_gimbal_solver.hpp index 9a5c29480..daa13997b 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/precise_two_axis_gimbal_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/precise_two_axis_gimbal_solver.hpp @@ -25,8 +25,48 @@ class PreciseTwoAxisGimbalSolver { : upper_limit_(upper_limit) , lower_limit_(lower_limit) { component.register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); + component.register_input("/tf", tf_); } + struct SetControlDirection : Operation { + friend class PreciseTwoAxisGimbalSolver; + + explicit SetControlDirection(OdomImu::DirectionVector target) + : target_(std::move(target)) {} + + private: + double update(PreciseTwoAxisGimbalSolver& super) const { + if (!super.tf_.ready()) { + super.control_pitch_angle_ = PreciseTwoAxisGimbalSolver::nan_; + return PreciseTwoAxisGimbalSolver::nan_; + } + + auto dir_yaw_link = fast_tf::cast(target_, *super.tf_); + Eigen::Vector3d dir = *dir_yaw_link; + + const double norm = dir.norm(); + if (norm < kEps) { + super.control_pitch_angle_ = PreciseTwoAxisGimbalSolver::nan_; + return PreciseTwoAxisGimbalSolver::nan_; + } + dir /= norm; + + const double xy_norm = std::hypot(dir.x(), dir.y()); + + // In YawLink, yaw is the increment relative to the current total yaw. + const double yaw_shift = std::atan2(dir.y(), dir.x()); + + // Mechanical pitch convention: level is 0, pitching downward is positive, so negate it. + const double pitch_angle = -std::atan2(dir.z(), std::max(xy_norm, kEps)); + + super.set_pitch_angle(pitch_angle); + return yaw_shift; + } + + static constexpr double kEps = 1e-9; + OdomImu::DirectionVector target_; + }; + struct SetDisabled : Operation { friend class PreciseTwoAxisGimbalSolver; @@ -91,10 +131,11 @@ class PreciseTwoAxisGimbalSolver { static constexpr double nan_ = std::numeric_limits::quiet_NaN(); rmcs_executor::Component::InputInterface gimbal_pitch_angle_; + rmcs_executor::Component::InputInterface tf_; const double upper_limit_, lower_limit_; double control_pitch_angle_ = nan_; }; -} // namespace rmcs_core::controller::gimbal \ No newline at end of file +} // namespace rmcs_core::controller::gimbal diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp index d27704d52..dfd361313 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp @@ -20,9 +20,11 @@ class SimpleGimbalController : Node( get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , two_axis_gimbal_solver( - *this, get_parameter("upper_limit").as_double(), - get_parameter("lower_limit").as_double()) { + , two_axis_gimbal_solver{ + *this, + get_parameter("upper_limit").as_double(), + get_parameter("lower_limit").as_double(), + } { register_input("/remote/joystick/left", joystick_left_); register_input("/remote/switch/right", switch_right_); @@ -30,10 +32,15 @@ class SimpleGimbalController register_input("/remote/mouse/velocity", mouse_velocity_); register_input("/remote/mouse", mouse_); - register_input("/gimbal/auto_aim/control_direction", auto_aim_control_direction_, false); + register_input("/auto_aim/should_control", auto_aim_should_control_, false); + register_input("/auto_aim/control_direction", auto_aim_control_direction_, false); register_output("/gimbal/yaw/control_angle_error", yaw_angle_error_, nan_); register_output("/gimbal/pitch/control_angle_error", pitch_angle_error_, nan_); + + two_axis_gimbal_solver.enable_yaw_limit( + *this, get_parameter("yaw_upper_limit").as_double(), + get_parameter("yaw_lower_limit").as_double()); } void update() override { @@ -45,18 +52,22 @@ class SimpleGimbalController TwoAxisGimbalSolver::AngleError calculate_angle_error() { auto switch_right = *switch_right_; auto switch_left = *switch_left_; - auto mouse = *mouse_; using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) return two_axis_gimbal_solver.update(TwoAxisGimbalSolver::SetDisabled()); - if (auto_aim_control_direction_.ready() && (mouse.right || switch_right == Switch::UP) - && !auto_aim_control_direction_->isZero()) + const auto auto_aim_requested = mouse_->right || switch_right == 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(); + + if (auto_aim_requested && should_control && valid_control) { return two_axis_gimbal_solver.update( TwoAxisGimbalSolver::SetControlDirection( OdomImu::DirectionVector(*auto_aim_control_direction_))); + } if (!two_axis_gimbal_solver.enabled()) return two_axis_gimbal_solver.update(TwoAxisGimbalSolver::SetToLevel()); @@ -67,7 +78,7 @@ class SimpleGimbalController double yaw_shift = joystick_sensitivity * joystick_left_->y() + mouse_sensitivity * mouse_velocity_->y(); double pitch_shift = - -joystick_sensitivity * joystick_left_->x() - mouse_sensitivity * mouse_velocity_->x(); + -joystick_sensitivity * joystick_left_->x() + mouse_sensitivity * mouse_velocity_->x(); return two_axis_gimbal_solver.update( TwoAxisGimbalSolver::SetControlShift(yaw_shift, pitch_shift)); @@ -82,6 +93,7 @@ class SimpleGimbalController InputInterface mouse_velocity_; InputInterface mouse_; + InputInterface auto_aim_should_control_; InputInterface auto_aim_control_direction_; TwoAxisGimbalSolver two_axis_gimbal_solver; @@ -94,4 +106,4 @@ class SimpleGimbalController #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::gimbal::SimpleGimbalController, rmcs_executor::Component) \ No newline at end of file + rmcs_core::controller::gimbal::SimpleGimbalController, rmcs_executor::Component) 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 c1fc772f3..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 @@ -1,7 +1,9 @@ #pragma once +#include #include #include +#include #include #include @@ -21,17 +23,20 @@ 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_); } + void enable_yaw_limit( + rmcs_executor::Component& component, double yaw_upper_limit, double yaw_lower_limit) { + yaw_cw_max_ = yaw_upper_limit; + yaw_cw_min_ = yaw_lower_limit; + component.register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); + } + class SetDisabled : public Operation { PitchLink::DirectionVector update(TwoAxisGimbalSolver& super) const override { super.control_enabled_ = false; @@ -105,14 +110,14 @@ 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_) return {nan_, nan_}; + clamp_yaw_limit(control_direction_yaw_link); + control_direction_ = fast_tf::cast(yaw_link_to_pitch_link(control_direction_yaw_link, pitch), *tf_); return calculate_control_errors(control_direction_yaw_link, pitch); @@ -144,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) { @@ -188,6 +174,29 @@ class TwoAxisGimbalSolver { *control_direction << lower_limit_.x() * projection, lower_limit_.y(); } + void clamp_yaw_limit(YawLink::DirectionVector& control_direction) { + if (!gimbal_yaw_angle_.ready()) + return; + + constexpr double two_pi = 2 * std::numbers::pi; + double cw = std::fmod(two_pi - *gimbal_yaw_angle_, two_pi); + if (cw < 0) + cw += two_pi; + + const auto& [x, y, z] = *control_direction; + const double err = std::atan2(y, x); + + const double target_cw = cw - err; + const double clamped_cw = std::clamp(target_cw, yaw_cw_min_, yaw_cw_max_); + if (clamped_cw == target_cw) + return; + + // delta = err_new - err = (cw - clamped_cw) - err + const double delta = (cw - clamped_cw) - err; + const double c = std::cos(delta), s = std::sin(delta); + *control_direction << c * x - s * y, s * x + c * y, z; + } + static AngleError calculate_control_errors( const YawLink::DirectionVector& control_direction, const Eigen::Vector2d& pitch) { const auto& [x, y, z] = *control_direction; @@ -204,11 +213,12 @@ 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.; + double yaw_cw_max_ = 0.; + rmcs_executor::Component::InputInterface gimbal_yaw_angle_; + OdomImu::DirectionVector yaw_axis_filtered_{Eigen::Vector3d::UnitZ()}; bool control_enabled_ = false; diff --git a/rmcs_ws/src/rmcs_core/src/controller/pid/friction_wheel_pid_recorder.cpp b/rmcs_ws/src/rmcs_core/src/controller/pid/friction_wheel_pid_recorder.cpp new file mode 100644 index 000000000..366f66b62 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/pid/friction_wheel_pid_recorder.cpp @@ -0,0 +1,166 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace rmcs_core::controller::pid { + +class FrictionWheelPidRecorder + : public rmcs_executor::Component + , public rclcpp::Node { +public: + FrictionWheelPidRecorder() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + const auto wheels = + declare_parameter>("wheels", std::vector{}); + if (wheels.empty()) + throw std::runtime_error("Parameter \"wheels\" must not be empty"); + + min_setpoint_abs_ = declare_parameter("min_setpoint_abs", 10.0); + + const auto flush_every_n_samples = declare_parameter("flush_every_n_samples", 100); + if (flush_every_n_samples <= 0) + throw std::runtime_error("Parameter \"flush_every_n_samples\" must be positive"); + flush_every_n_samples_ = static_cast(flush_every_n_samples); + + const auto output_dir = std::filesystem::path{ + declare_parameter("output_dir", "/tmp/friction_pid_logs")}; + const auto output_name = declare_parameter("output_name", ""); + + wheel_count_ = wheels.size(); + wheel_ids_ = std::make_unique(wheel_count_); + setpoints_ = std::make_unique[]>(wheel_count_); + measurements_ = std::make_unique[]>(wheel_count_); + control_torques_ = std::make_unique[]>(wheel_count_); + run_ids_ = std::make_unique(wheel_count_); + sample_indices_ = std::make_unique(wheel_count_); + previously_enabled_ = std::make_unique(wheel_count_); + + for (size_t i = 0; i < wheel_count_; ++i) { + wheel_ids_[i] = wheel_id_from_topic(wheels[i]); + register_input(wheels[i] + "/control_velocity", setpoints_[i]); + register_input(wheels[i] + "/velocity", measurements_[i]); + register_input(wheels[i] + "/control_torque", control_torques_[i]); + + run_ids_[i] = 0; + sample_indices_[i] = 0; + previously_enabled_[i] = false; + } + + std::filesystem::create_directories(output_dir); + log_file_path_ = output_dir / resolve_output_name(output_name); + log_stream_.open(log_file_path_); + if (!log_stream_.is_open()) + throw std::runtime_error( + "Failed to open recorder output file: " + log_file_path_.string()); + + log_stream_.setf(std::ios::fixed); + log_stream_.precision(9); + log_stream_ + << "timestamp_us,wheel_id,run_id,sample_idx,setpoint_velocity,measured_velocity," + "control_torque,enabled\n"; + + RCLCPP_INFO(get_logger(), "FrictionWheelPidRecorder writing to %s", log_file_path_.c_str()); + } + + ~FrictionWheelPidRecorder() override { + if (log_stream_.is_open()) { + log_stream_.flush(); + log_stream_.close(); + } + } + + void update() override { + const auto timestamp_us = now_timestamp_us(); + + for (size_t i = 0; i < wheel_count_; ++i) { + const bool enabled = should_record(i); + if (enabled && !previously_enabled_[i]) { + ++run_ids_[i]; + sample_indices_[i] = 0; + } + + if (enabled) { + log_stream_ << timestamp_us << ',' << wheel_ids_[i] << ',' << run_ids_[i] << ',' + << sample_indices_[i]++ << ',' << *setpoints_[i] << ',' + << *measurements_[i] << ',' << *control_torques_[i] << ",1\n"; + if (++unflushed_samples_ >= flush_every_n_samples_) { + log_stream_.flush(); + unflushed_samples_ = 0; + } + } + + previously_enabled_[i] = enabled; + } + } + +private: + static std::string wheel_id_from_topic(std::string_view topic) { + auto pos = topic.find_last_of('/'); + if (pos == std::string_view::npos || pos + 1 >= topic.size()) + return std::string(topic); + return std::string(topic.substr(pos + 1)); + } + + static uint64_t now_timestamp_us() { + const auto now = std::chrono::steady_clock::now().time_since_epoch(); + return static_cast( + std::chrono::duration_cast(now).count()); + } + + static std::string resolve_output_name(const std::string& configured_name) { + if (!configured_name.empty()) + return configured_name; + + const auto now = std::chrono::system_clock::now().time_since_epoch(); + const auto ms = std::chrono::duration_cast(now).count(); + return "friction_wheel_pid_" + std::to_string(ms) + ".csv"; + } + + bool should_record(size_t index) const { + if (!setpoints_[index].ready() || !measurements_[index].ready() + || !control_torques_[index].ready()) + return false; + + const auto setpoint = *setpoints_[index]; + const auto measurement = *measurements_[index]; + const auto control_torque = *control_torques_[index]; + + return std::isfinite(setpoint) && std::isfinite(measurement) + && std::isfinite(control_torque) && std::abs(setpoint) >= min_setpoint_abs_; + } + + size_t wheel_count_ = 0; + double min_setpoint_abs_ = 10.0; + size_t flush_every_n_samples_ = 100; + size_t unflushed_samples_ = 0; + + std::filesystem::path log_file_path_; + std::ofstream log_stream_; + + std::unique_ptr wheel_ids_; + std::unique_ptr[]> setpoints_; + std::unique_ptr[]> measurements_; + std::unique_ptr[]> control_torques_; + std::unique_ptr run_ids_; + std::unique_ptr sample_indices_; + std::unique_ptr previously_enabled_; +}; + +} // namespace rmcs_core::controller::pid + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::pid::FrictionWheelPidRecorder, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/pid/matrix_pid_calculator.hpp b/rmcs_ws/src/rmcs_core/src/controller/pid/matrix_pid_calculator.hpp index f17c35a12..688147993 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/pid/matrix_pid_calculator.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/pid/matrix_pid_calculator.hpp @@ -12,7 +12,7 @@ namespace rmcs_core::controller::pid { template class MatrixPidCalculator { using Vector = Eigen::Vector; - using Gain = std::conditional, double>::type; + using Gain = std::conditional, double>::type; public: MatrixPidCalculator(Gain kp, Gain ki, Gain kd) @@ -35,7 +35,7 @@ class MatrixPidCalculator { Vector update(Vector err) { Vector control = kp * err + ki * err_integral_; - err_integral_ = exclude_nan(clamp(err_integral_ + err, integral_min, integral_max)); + err_integral_ = exclude_nan(clamp(err_integral_ + err, integral_min, integral_max)); control += exclude_nan(kd * (err - last_err_)); last_err_ = err; 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 0c97c98a0..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 @@ -1,7 +1,3 @@ -#include - -#include - #include #include #include @@ -18,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; @@ -53,7 +48,8 @@ class BulletFeederController17mm register_input("/remote/mouse", mouse_); register_input("/remote/keyboard", keyboard_); - register_input("/gimbal/auto_aim/fire_control", fire_control_, false); + 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( @@ -63,8 +59,10 @@ class BulletFeederController17mm } void before_updating() override { - if (!fire_control_.ready()) - fire_control_.bind_directly(false); + if (!should_shoot_.ready()) + should_shoot_.bind_directly(false); + if (!single_shoot_.ready()) + single_shoot_.bind_directly(false); } void update() override { @@ -76,38 +74,49 @@ 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(); } else { - int64_t bullet_allowance = 0; + 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; if (*friction_ready_) { if (shoot_mode == ShootMode::AUTOMATIC) { - bool triggered = mouse.left || switch_left == Switch::DOWN - || (switch_right == Switch::UP && *fire_control_); + auto aiming_enable = mouse_->right || (switch_right == Switch::UP); + auto attack_intent = mouse_->left || (switch_left == Switch::DOWN); + auto triggered = aiming_enable ? *should_shoot_ : attack_intent; bullet_allowance = triggered ? *control_bullet_allowance_limited_by_heat_ : 0; } else { - bool triggered = single_shot_stop_counter_ > 0; + auto triggered = single_shot_stop_counter_ > 0; bullet_allowance = triggered && (*control_bullet_allowance_limited_by_heat_ > 0); } @@ -121,6 +130,7 @@ class BulletFeederController17mm last_switch_left_ = switch_left; last_mouse_ = mouse; last_keyboard_ = keyboard; + last_should_shoot_ = current_should_shoot; } private: @@ -166,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_); } } } @@ -192,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_; @@ -209,7 +218,9 @@ class BulletFeederController17mm InputInterface mouse_; InputInterface keyboard_; - InputInterface fire_control_; + 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; @@ -232,4 +243,4 @@ class BulletFeederController17mm #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::shooting::BulletFeederController17mm, rmcs_executor::Component) \ No newline at end of file + rmcs_core::controller::shooting::BulletFeederController17mm, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_42mm.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_42mm.cpp index bf3076e30..d8bd95cb6 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_42mm.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_42mm.cpp @@ -31,14 +31,15 @@ class BulletFeederController42mm register_input( "/gimbal/control_bullet_allowance/limited_by_heat", control_bullet_allowance_limited_by_heat_); + register_input("/gimbal/bullet_fired", bullet_fired_); - bullet_feeder_velocity_pid_.kp = 50.0; - bullet_feeder_velocity_pid_.ki = 10.0; - bullet_feeder_velocity_pid_.kd = 0.0; + bullet_feeder_velocity_pid_.kp = 50.0; + bullet_feeder_velocity_pid_.ki = 10.0; + bullet_feeder_velocity_pid_.kd = 0.0; bullet_feeder_velocity_pid_.integral_max = 60.0; bullet_feeder_velocity_pid_.integral_min = 0.0; - bullet_feeder_angle_pid_.kp = 50.0; + bullet_feeder_angle_pid_.kp = 60.0; bullet_feeder_angle_pid_.ki = 0.0; bullet_feeder_angle_pid_.kd = 2.0; @@ -51,9 +52,9 @@ class BulletFeederController42mm void update() override { const auto switch_right = *switch_right_; - const auto switch_left = *switch_left_; - const auto mouse = *mouse_; - const auto keyboard = *keyboard_; + const auto switch_left = *switch_left_; + const auto mouse = *mouse_; + const auto keyboard = *keyboard_; using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) @@ -61,7 +62,6 @@ class BulletFeederController42mm reset_all_controls(); return; } - overdrive_mode_ = keyboard.f; if (keyboard.ctrl && !last_keyboard_.r && keyboard.r) low_latency_mode_ = !low_latency_mode_; @@ -84,8 +84,8 @@ class BulletFeederController42mm } else { if (!*friction_ready_ || std::isnan(bullet_feeder_control_angle_)) { bullet_feeder_control_angle_ = *bullet_feeder_angle_; - shoot_stage_ = ShootStage::PRELOADED; - bullet_fed_count_ = static_cast( + shoot_stage_ = ShootStage::PRELOADED; + bullet_fed_count_ = static_cast( (*bullet_feeder_angle_ - bullet_feeder_compressed_zero_point_ - 0.1) / bullet_feeder_angle_per_bullet_); } @@ -100,12 +100,10 @@ class BulletFeederController42mm } } - double err_abs = std::abs(bullet_feeder_control_angle_ - *bullet_feeder_angle_); - // RCLCPP_INFO( - // get_logger(), "%.2f %.2f %.2f", bullet_feeder_control_angle_, - // *bullet_feeder_angle_, err_abs); + double angle_error_abs = + std::abs(bullet_feeder_control_angle_ - *bullet_feeder_angle_); if (shoot_stage_ == ShootStage::PRELOADING) { - if (err_abs < 0.1) + if (angle_error_abs < 0.1) set_preloaded(); } if (shoot_stage_ == ShootStage::PRELOADED) { @@ -113,46 +111,42 @@ class BulletFeederController42mm set_compressing(); } if (shoot_stage_ == ShootStage::COMPRESSING) { - if (err_abs < 0.1) + if (angle_error_abs < 0.1) set_compressed(); } if (shoot_stage_ == ShootStage::SHOOTING) { - if (err_abs < 0.1) + if (angle_error_abs < 0.1) set_preloading(); } } - double velocity_err = bullet_feeder_angle_pid_.update( - bullet_feeder_control_angle_ - *bullet_feeder_angle_) - - *bullet_feeder_velocity_; - // bullet_feeder_velocity_pid_.integral_max = std::clamp(1000.0 * velocity_err, - // 0.0, 60.0); - *bullet_feeder_control_torque_ = bullet_feeder_velocity_pid_.update(velocity_err); - // RCLCPP_INFO( - // get_logger(), "%.2f %.2f", velocity_err, *bullet_feeder_control_torque_); + double velocity_error = bullet_feeder_angle_pid_.update( + bullet_feeder_control_angle_ - *bullet_feeder_angle_) + - *bullet_feeder_velocity_; + *bullet_feeder_control_torque_ = bullet_feeder_velocity_pid_.update(velocity_error); update_jam_detection(); } last_switch_right_ = switch_right; - last_switch_left_ = switch_left; - last_mouse_ = mouse; - last_keyboard_ = keyboard; + last_switch_left_ = switch_left; + last_mouse_ = mouse; + last_keyboard_ = keyboard; } private: void reset_all_controls() { last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; - last_mouse_ = rmcs_msgs::Mouse::zero(); - last_keyboard_ = rmcs_msgs::Keyboard::zero(); + last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + last_mouse_ = rmcs_msgs::Mouse::zero(); + last_keyboard_ = rmcs_msgs::Keyboard::zero(); overdrive_mode_ = low_latency_mode_ = false; - shoot_stage_ = ShootStage::PRELOADED; + shoot_stage_ = ShootStage::PRELOADED; bullet_fed_count_ = std::numeric_limits::min(); - bullet_feeder_control_angle_ = nan_; + bullet_feeder_control_angle_ = nan_; bullet_feeder_angle_pid_.output_max = inf_; bullet_feeder_velocity_pid_.reset(); @@ -160,13 +154,13 @@ class BulletFeederController42mm *bullet_feeder_control_torque_ = nan_; bullet_feeder_faulty_count_ = 0; - bullet_feeder_cool_down_ = 0; + bullet_feeder_cool_down_ = 0; } void set_preloading() { RCLCPP_INFO(get_logger(), "PRELOADING"); bullet_fed_count_++; - shoot_stage_ = ShootStage::PRELOADING; + shoot_stage_ = ShootStage::PRELOADING; bullet_feeder_control_angle_ = bullet_feeder_compressed_zero_point_ + (bullet_fed_count_ + 0.5) * bullet_feeder_angle_per_bullet_; bullet_feeder_angle_pid_.output_max = 1.0; @@ -179,7 +173,7 @@ class BulletFeederController42mm void set_compressing() { RCLCPP_INFO(get_logger(), "COMPRESSING"); - shoot_stage_ = ShootStage::COMPRESSING; + shoot_stage_ = ShootStage::COMPRESSING; bullet_feeder_control_angle_ = bullet_feeder_compressed_zero_point_ + (bullet_fed_count_ + 1) * bullet_feeder_angle_per_bullet_; bullet_feeder_angle_pid_.output_max = 0.8; @@ -192,14 +186,13 @@ class BulletFeederController42mm void set_shooting() { RCLCPP_INFO(get_logger(), "SHOOTING"); - shoot_stage_ = ShootStage::SHOOTING; + shoot_stage_ = ShootStage::SHOOTING; bullet_feeder_control_angle_ = bullet_feeder_compressed_zero_point_ + (bullet_fed_count_ + 1.2) * bullet_feeder_angle_per_bullet_; bullet_feeder_angle_pid_.output_max = 1.0; } void update_jam_detection() { - // RCLCPP_INFO(get_logger(), "%.2f --", *bullet_feeder_control_torque_); if (*bullet_feeder_control_torque_ < 300.0) { bullet_feeder_faulty_count_ = 0; return; @@ -215,7 +208,7 @@ class BulletFeederController42mm void enter_jam_protection() { bullet_feeder_control_angle_ = nan_; - bullet_feeder_cool_down_ = 1000; + bullet_feeder_cool_down_ = 1000; bullet_feeder_angle_pid_.reset(); bullet_feeder_velocity_pid_.reset(); RCLCPP_INFO(get_logger(), "Jammed!"); @@ -225,9 +218,10 @@ class BulletFeederController42mm static constexpr double inf_ = std::numeric_limits::infinity(); static constexpr double bullet_feeder_compressed_zero_point_ = 0.58; - static constexpr double bullet_feeder_angle_per_bullet_ = 2 * std::numbers::pi / 6; + static constexpr double bullet_feeder_angle_per_bullet_ = 2 * std::numbers::pi / 6; InputInterface friction_ready_; + InputInterface bullet_fired_; InputInterface switch_right_; InputInterface switch_left_; @@ -235,9 +229,9 @@ class BulletFeederController42mm InputInterface keyboard_; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Mouse last_mouse_ = rmcs_msgs::Mouse::zero(); - rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Mouse last_mouse_ = rmcs_msgs::Mouse::zero(); + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); bool overdrive_mode_ = false, low_latency_mode_ = false; @@ -247,8 +241,8 @@ class BulletFeederController42mm InputInterface control_bullet_allowance_limited_by_heat_; enum class ShootStage { PRELOADING, PRELOADED, COMPRESSING, COMPRESSED, SHOOTING }; - ShootStage shoot_stage_ = ShootStage::PRELOADED; - int bullet_fed_count_ = std::numeric_limits::min(); + ShootStage shoot_stage_ = ShootStage::PRELOADED; + int bullet_fed_count_ = std::numeric_limits::min(); double bullet_feeder_control_angle_ = nan_; pid::PidCalculator bullet_feeder_velocity_pid_; @@ -256,7 +250,7 @@ class BulletFeederController42mm OutputInterface bullet_feeder_control_torque_; int bullet_feeder_faulty_count_ = 0; - int bullet_feeder_cool_down_ = 0; + int bullet_feeder_cool_down_ = 0; OutputInterface shoot_mode_; }; @@ -266,4 +260,4 @@ class BulletFeederController42mm #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::shooting::BulletFeederController42mm, rmcs_executor::Component) \ No newline at end of file + rmcs_core::controller::shooting::BulletFeederController42mm, 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 a83dc5f4d..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(); @@ -45,10 +46,25 @@ 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_); + 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]); + 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_); } @@ -67,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(); @@ -74,6 +97,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_; @@ -95,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; @@ -107,12 +133,34 @@ 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] = target_friction_velocity(i); + } + void update_friction_velocities() { if (std::isnan(friction_soft_start_stop_percentage_)) { 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_ += @@ -122,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() { @@ -169,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; @@ -181,22 +229,29 @@ 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_; bool friction_enabled_ = false; @@ -219,4 +274,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/heat_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp index 3de0960ae..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 @@ -53,4 +53,4 @@ class HeatController #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::HeatController, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::HeatController, 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 new file mode 100644 index 000000000..1656d2276 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_friction_wheel_controller.cpp @@ -0,0 +1,258 @@ +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::controller::shooting { + +class HeroFrictionWheelController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + HeroFrictionWheelController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , logger_(get_logger()) { + + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/keyboard", keyboard_); + + auto friction_wheels = get_parameter("friction_wheels").as_string_array(); + auto friction_profile_0 = get_parameter("friction_velocities_profile_0").as_double_array(); + auto friction_profile_1 = get_parameter("friction_velocities_profile_1").as_double_array(); + if (friction_wheels.size() != friction_profile_0.size() + || friction_wheels.size() != friction_profile_1.size()) { + throw std::runtime_error( + "'friction_wheels' and both friction velocity profiles must have the same length!"); + } else if (friction_wheels.size() == 0) + throw std::runtime_error( + "Empty array error: 'friction_wheels' and 'friction_velocities' cannot be empty!"); + + friction_profile_0_.assign(friction_profile_0.begin(), friction_profile_0.end()); + friction_profile_1_.assign(friction_profile_1.begin(), friction_profile_1.end()); + friction_count_ = friction_wheels.size(); + friction_velocities_ = std::make_unique[]>(friction_count_); + friction_control_velocities_ = std::make_unique[]>(friction_count_); + for (size_t i = 0; i < friction_count_; i++) { + register_input(friction_wheels[i] + "/velocity", friction_velocities_[i]); + register_output( + friction_wheels[i] + "/control_velocity", friction_control_velocities_[i], nan_); + } + + friction_soft_start_stop_step_ = + (1 / 1000.0) / get_parameter("friction_soft_start_stop_time").as_double(); + + register_output("/gimbal/friction_ready", friction_ready_, false); + register_output("/gimbal/friction_jammed", friction_jammed_, false); + register_output("/gimbal/bullet_fired", bullet_fired_, false); + register_output("/gimbal/friction_profile_1_active", friction_profile_1_active_, false); + } + + void update() override { + const auto switch_right = *switch_right_; + const auto switch_left = *switch_left_; + const auto keyboard = *keyboard_; + if (!last_keyboard_.f && keyboard.f) { + active_profile_ ^= 1; + + 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); + } + } + const bool profile_switch_now = keyboard.ctrl && keyboard.f; + const bool profile_switch_last = last_keyboard_.ctrl && last_keyboard_.f; + + if (!profile_switch_last && profile_switch_now) { + active_profile_ ^= 1; + + 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); + } + } + + *friction_profile_1_active_ = active_profile_ == 1; + using namespace rmcs_msgs; + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_all_controls(); + return; + } + + if (switch_right != Switch::DOWN) { + if ((!last_keyboard_.v && keyboard.v) + || (last_switch_left_ == Switch::MIDDLE && switch_left == Switch::UP)) { + friction_enabled_ = !friction_enabled_; + } + + update_friction_velocities(); + update_friction_status(); + if (*friction_jammed_) + RCLCPP_INFO(logger_, "Friction Jammed!"); + if (*bullet_fired_) + RCLCPP_INFO(logger_, "Bullet Fired!"); + + last_switch_right_ = switch_right; + last_switch_left_ = switch_left; + last_keyboard_ = keyboard; + } + if (!friction_enabled_) { + reset_all_controls(); + } + } + +private: + void reset_all_controls() { + friction_enabled_ = false; + + last_primary_friction_velocity_ = nan_; + primary_friction_velocity_decrease_integral_ = 0; + + friction_soft_start_stop_percentage_ = nan_; + + for (size_t i = 0; i < friction_count_; i++) + *friction_control_velocities_[i] = nan_; + + *friction_ready_ = *friction_jammed_ = *bullet_fired_ = false; + } + + void update_friction_velocities() { + if (std::isnan(friction_soft_start_stop_percentage_)) { + friction_soft_start_stop_percentage_ = 0.0; + for (size_t i = 0; i < friction_count_; i++) + friction_soft_start_stop_percentage_ += + *friction_velocities_[i] / target_friction_velocity(i); + friction_soft_start_stop_percentage_ /= static_cast(friction_count_); + } + friction_soft_start_stop_percentage_ += + friction_enabled_ ? friction_soft_start_stop_step_ : -friction_soft_start_stop_step_; + friction_soft_start_stop_percentage_ = + std::clamp(friction_soft_start_stop_percentage_, 0.0, 1.0); + + for (size_t i = 0; i < friction_count_; i++) + *friction_control_velocities_[i] = + friction_soft_start_stop_percentage_ * target_friction_velocity(i); + } + + void update_friction_status() { + *friction_ready_ = *friction_jammed_ = *bullet_fired_ = false; + + if (!friction_enabled_) + return; + if (friction_soft_start_stop_percentage_ < 1.0) + return; + + if (detect_friction_faulty()) { + if (friction_faulty_count_ == 200) { + friction_enabled_ = false; + *friction_jammed_ = true; + } else { + friction_faulty_count_++; + *friction_ready_ = true; + } + return; + } + + *friction_ready_ = true; + *bullet_fired_ = detect_bullet_fire(); + } + + 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 false; + } + return false; + } + + rmcs_utility::FpsCounter fps_counter_; + + bool detect_bullet_fire() { + bool fired = false; + if (!std::isnan(last_primary_friction_velocity_)) { + double differential = *friction_velocities_[0] - last_primary_friction_velocity_; + if (differential < 0.1) + primary_friction_velocity_decrease_integral_ += differential; + else { + if (primary_friction_velocity_decrease_integral_ < -14.0 + && last_primary_friction_velocity_ < target_friction_velocity(2) - 25.0) + fired = true; + + primary_friction_velocity_decrease_integral_ = 0; + } + } + last_primary_friction_velocity_ = *friction_velocities_[0]; + + return fired; + } + + double target_friction_velocity(size_t i) const { + return active_profile_ == 0 ? friction_profile_0_[i] : friction_profile_1_[i]; + } + + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + + rclcpp::Logger logger_; + + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface keyboard_; + + 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(); + + size_t friction_count_; + + std::vector friction_profile_0_; + std::vector friction_profile_1_; + size_t active_profile_ = 0; + + std::unique_ptr[]> friction_velocities_; + + bool friction_enabled_ = false; + + double friction_soft_start_stop_step_; + double friction_soft_start_stop_percentage_ = nan_; + std::unique_ptr[]> friction_control_velocities_; + + OutputInterface friction_ready_; + + int friction_faulty_count_ = 0; + OutputInterface friction_jammed_; + + double last_primary_friction_velocity_ = nan_; + double primary_friction_velocity_decrease_integral_ = 0; + OutputInterface bullet_fired_; + OutputInterface friction_profile_1_active_; +}; + +} // namespace rmcs_core::controller::shooting + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::shooting::HeroFrictionWheelController, rmcs_executor::Component) 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 new file mode 100644 index 000000000..86a136c63 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_heat_controller.cpp @@ -0,0 +1,66 @@ +#include + +#include +#include +#include +#include + +namespace rmcs_core::controller::shooting { + +class HeroHeatController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + HeroHeatController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , heat_per_shot(get_parameter("heat_per_shot").as_int()) + , reserved_heat(get_parameter("reserved_heat").as_int()) { + + register_input("/referee/shooter/cooling", shooter_cooling_); + register_input("/referee/shooter/heat_limit", shooter_heat_limit_); + + register_input("/gimbal/bullet_fired", bullet_fired_); + + register_output( + "/gimbal/control_bullet_allowance/limited_by_heat", control_bullet_allowance_, 0); + register_output("/shoot/heat", shooting_heat_, 0.0); + } + + void update() override { + shooter_heat_ = std::max(0, shooter_heat_ - *shooter_cooling_); + + 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); + + *shooting_heat_ = static_cast(shooter_heat_); + } + +private: + InputInterface shooter_cooling_; + InputInterface shooter_heat_limit_; + + InputInterface bullet_fired_; + + const int64_t heat_per_shot; + const int64_t reserved_heat; + + bool last_bullet_fired_ = false; + int64_t shooter_heat_ = 0; + OutputInterface shooting_heat_; + + OutputInterface control_bullet_allowance_; +}; + +} // namespace rmcs_core::controller::shooting + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::shooting::HeroHeatController, 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 new file mode 100644 index 000000000..eadb30281 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp @@ -0,0 +1,406 @@ +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "controller/pid/pid_calculator.hpp" +#include + +namespace rmcs_core::controller::shooting { + +/** + * @class PutterController + * @brief Putter mechanism controller + * + * Firing mechanism notes: + * Since the photoelectric sensor is placed at the chamber opening, testing showed that + * the double-middle action first triggers the putter to retract, and then stall detection + * confirms that the reset is complete. + * By default, a small holding force is applied so the putter does not slide down. The entire + * process uses angle-loop advancement, and grayscale detection determines whether to feed. + * Bullet firing is detected from two sources: the friction wheels and the putter stroke. + * The full scheme completed stress testing before the summer break. + */ +class PutterController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + PutterController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + auto set_pid_parameter = [this](pid::PidCalculator& pid, const std::string& name) { + pid.kp = get_parameter(name + "_kp").as_double(); + pid.ki = get_parameter(name + "_ki").as_double(); + pid.kd = get_parameter(name + "_kd").as_double(); + get_parameter(name + "_integral_min", pid.integral_min); + get_parameter(name + "_integral_max", pid.integral_max); + get_parameter(name + "_output_min", pid.output_min); + get_parameter(name + "_output_max", pid.output_max); + }; + + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/mouse", mouse_); + register_input("/remote/keyboard", keyboard_); + + register_input("/gimbal/friction_ready", friction_ready_); + + register_input("/gimbal/bullet_feeder/angle", bullet_feeder_angle_); + register_input("/gimbal/bullet_feeder/velocity", bullet_feeder_velocity_); + + register_input( + "/gimbal/control_bullet_allowance/limited_by_heat", + control_bullet_allowance_limited_by_heat_); + + register_input("/gimbal/photoelectric_sensor", photoelectric_sensor_status_); + register_input("/gimbal/grayscale_sensor", grayscale_sensor_status_); + register_input("/gimbal/bullet_fired", bullet_fired_); + + register_input("/gimbal/putter/angle", putter_angle_); + register_input("/gimbal/putter/velocity", putter_velocity_); + + set_pid_parameter(bullet_feeder_velocity_pid_, "bullet_feeder_velocity"); + set_pid_parameter(putter_return_velocity_pid_, "putter_return_velocity"); + + register_output( + "/gimbal/bullet_feeder/control_torque", bullet_feeder_control_torque_, nan_); + register_output("/gimbal/putter/control_torque", putter_control_torque_, nan_); + + register_output("/gimbal/shoot/delay_ms", shoot_delay_ms_, nan_); + + // auto_aim + 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); + } + + void before_updating() override { + if (!should_shoot_.ready()) + should_shoot_.bind_directly(false); + } + + void update() override { + const auto switch_right = *switch_right_; + const auto switch_left = *switch_left_; + const auto mouse = *mouse_; + const auto keyboard = *keyboard_; + + using namespace rmcs_msgs; + + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_all_controls(); + return; + } + + // Normal control flow after the putter has been 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_ > 300) + *bullet_feeder_control_torque_ = bullet_feeder_velocity_pid_.update( + -low_latency_velocity_ / 2 - *bullet_feeder_velocity_); + else { + // Late cooldown stage: stop control. + bullet_feeder_velocity_pid_.reset(); + *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_) { + // Detect fire triggers. + if (switch_right != Switch::DOWN) { + + const auto now = std::chrono::steady_clock::now(); + const bool left_click_edge = (!last_mouse_.left && mouse.left); + if (left_click_edge) { + if (now - last_click_time_ < std::chrono::milliseconds(500)) { + click_count_++; + } else { + click_count_ = 1; + } + last_click_time_ = now; + } + + const bool manual_trigger = + (!last_mouse_.left && mouse.left && !mouse.right) + || (last_switch_left_ == rmcs_msgs::Switch::MIDDLE + && switch_left == rmcs_msgs::Switch::DOWN); + + const bool auto_fire_now = + (switch_right == Switch::UP || mouse.right) && *should_shoot_; + + const bool auto_trigger_emergence = mouse.right && (click_count_ >= 2); + + const bool auto_trigger = + auto_fire_now + && (now - last_fire_time_ > std::chrono::milliseconds(1000)); + + if (manual_trigger || auto_trigger || auto_trigger_emergence) { + if (*control_bullet_allowance_limited_by_heat_ > 0 + && (shoot_stage_ == ShootStage::PRELOADED || shoot_first)) { + set_shooting(); + last_fire_time_ = now; + shoot_first = false; + } + } + if (auto_trigger_emergence) { + click_count_ = 0; + } + } + + if (shoot_stage_ == ShootStage::PRELOADING) { + + *bullet_feeder_control_torque_ = bullet_feeder_velocity_pid_.update( + 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_ && !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 (shooted) { + // Bullet fired: return the putter. + *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(120. - *putter_velocity_); + update_putter_jam_detection(); + } + } + } else { + // Friction wheels not ready: stop the bullet feeder. + *bullet_feeder_control_torque_ = 0.; + } + + // Non-firing state: apply a small holding force to the putter. + if (shoot_stage_ != ShootStage::SHOOTING) + *putter_control_torque_ = -0.02; + } + } else { + // Putter not initialized: perform the reset procedure. + *putter_control_torque_ = putter_return_velocity_pid_.update(-80. - *putter_velocity_); + update_putter_jam_detection(); + } + + // Save the current state for the next comparison. + last_switch_right_ = switch_right; + last_switch_left_ = switch_left; + last_mouse_ = mouse; + last_keyboard_ = keyboard; + } + +private: + void reset_all_controls() { + last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; + last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + last_mouse_ = rmcs_msgs::Mouse::zero(); + last_keyboard_ = rmcs_msgs::Keyboard::zero(); + + bullet_feeder_velocity_pid_.reset(); + *bullet_feeder_control_torque_ = nan_; + + shoot_stage_ = ShootStage::PRELOADED; + + putter_initialized = false; + putter_startpoint = nan_; + putter_return_velocity_pid_.reset(); + *putter_control_torque_ = nan_; + + bullet_feeder_faulty_count_ = 0; + + *shoot_delay_ms_ = nan_; + } + + void set_preloading() { + RCLCPP_INFO(get_logger(), "PRELOADING"); + shoot_stage_ = ShootStage::PRELOADING; + } + + void set_preloaded() { + RCLCPP_INFO(get_logger(), "PRELOADED"); + shoot_stage_ = ShootStage::PRELOADED; + } + + 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_ > 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_reverse_protection(); + } + } + + void update_putter_jam_detection() { + if (std::abs(*putter_velocity_) > 0.1 || std::isnan(*putter_control_torque_)) { + putter_faulty_count_ = 0; + } else { + putter_faulty_count_++; + } + + // Accumulate a fault count when the torque is abnormal. + 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_initialized = true; + putter_startpoint = *putter_angle_; + } else { + // Stall detected during firing: treat the bullet as fired. + RCLCPP_INFO(get_logger(), "DETECT: Putter freezed"); + shooted = true; + } + } + } + + void putter_timeout_detection() { + // 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 (shooted) { + if (putter_timeout_count_ < 400) + ++putter_timeout_count_; + else { + putter_timeout_count_ = 0; + RCLCPP_INFO(get_logger(), "PUTTER TIMEOUT"); + set_preloading(); + shooted = false; + } + } + } + } + + void enter_reverse_protection() { + locked_detect_count_ = 0; + bullet_feeder_faulty_count_ = 0; + bullet_feeder_reverse_end_ = 400; + bullet_feeder_velocity_pid_.reset(); + } + + static constexpr double nan_ = + std::numeric_limits::quiet_NaN(); ///< Not-a-number constant. + static constexpr double inf_ = std::numeric_limits::infinity(); ///< Infinity constant. + + 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; + static constexpr double low_latency_velocity_ = 5.0; + + InputInterface photoelectric_sensor_status_; + InputInterface grayscale_sensor_status_; + InputInterface bullet_fired_; + bool shooted{false}; + bool shoot_first{true}; + + InputInterface friction_ready_; + + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface mouse_; + InputInterface keyboard_; + + rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Mouse last_mouse_ = rmcs_msgs::Mouse::zero(); + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + + InputInterface bullet_feeder_angle_; + InputInterface bullet_feeder_velocity_; + + InputInterface control_bullet_allowance_limited_by_heat_; + + bool putter_initialized = false; + int putter_faulty_count_ = 0; + int putter_timeout_count_ = 0; + double putter_startpoint = nan_; + pid::PidCalculator putter_return_velocity_pid_; + InputInterface putter_velocity_; + + enum class ShootStage { PRELOADING, PRELOADED, SHOOTING }; + ShootStage shoot_stage_ = ShootStage::PRELOADING; + + pid::PidCalculator bullet_feeder_velocity_pid_; + + OutputInterface bullet_feeder_control_torque_; + + InputInterface putter_angle_; + OutputInterface putter_control_torque_; + + int bullet_feeder_faulty_count_ = 0; + + OutputInterface shoot_delay_ms_; + + 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; + + int locked_detect_count_ = 0; + int bullet_feeder_reverse_end_ = 0; + + InputInterface bullet_feeder_torque; + InputInterface putter_torque; + + OutputInterface shoot_mode_; + OutputInterface shoot_condiction_; + OutputInterface preloaded_ready_; +}; + +} // namespace rmcs_core::controller::shooting + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::PutterController, rmcs_executor::Component) 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 5500b15c1..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 @@ -1,7 +1,10 @@ +#include +#include #include #include #include #include +#include namespace rmcs_core::controller::shooting { @@ -18,40 +21,26 @@ class ShootingRecorder log_mode_ = static_cast(get_parameter("log_mode").as_int()); + aim_velocity = get_parameter("aim_velocity").as_double(); + register_input("/referee/shooter/initial_speed", initial_speed_); register_input("/referee/shooter/shoot_timestamp", shoot_timestamp_); - register_input("/friction_wheels/temperature", fractional_temperature_); - - if (friction_wheel_count_ == 2) { - const auto topic = std::array{ - "/gimbal/left_friction/control_velocity", - "/gimbal/right_friction/control_velocity", - }; - for (int i = 0; i < 2; i++) - register_input(topic[i], friction_wheels_velocity_[i]); - } else if (friction_wheel_count_ == 4) { - const auto topic = std::array{ - "/gimbal/first_left_friction/control_velocity", - "/gimbal/first_right_friction/control_velocity", - "/gimbal/second_left_friction/control_velocity", - "/gimbal/second_right_friction/control_velocity", - }; - for (int i = 0; i < 4; i++) - register_input(topic[i], friction_wheels_velocity_[i]); - } using namespace std::chrono; auto now = high_resolution_clock::now(); - auto ms = duration_cast(now.time_since_epoch()).count(); + auto ms = duration_cast(now.time_since_epoch()).count(); auto file = fmt::format("/robot_shoot/{}.log", ms); log_stream_.open(file); + + std::ofstream out_file("shoot_recorder"); + RCLCPP_INFO(get_logger(), "ShootingRecorder initialized, log file: %s", file.c_str()); } ~ShootingRecorder() { log_stream_.close(); } void update() override { - + // Evaluate friction-wheel quality. switch (log_mode_) { case LogMode::TRIGGER: // It will be triggered by shooting action @@ -64,38 +53,52 @@ class ShootingRecorder return; break; } + v = *shoot_timestamp_; + + static constexpr size_t max_velocities_size = 1000; + + velocities.push_back(*initial_speed_); + if (velocities.size() > max_velocities_size) { + velocities.erase(velocities.begin()); + } - auto log_text = std::string{}; + analysis1(); + + auto log_text = std::string{}; auto timestamp = timestamp_to_string(*shoot_timestamp_); - if (friction_wheel_count_ == 2) { - log_text = fmt::format( - "{},{},{},{},{}", timestamp, *initial_speed_, // - *friction_wheels_velocity_[0], *friction_wheels_velocity_[1], - *fractional_temperature_); - } else if (friction_wheel_count_ == 4) { + if (friction_wheel_count_ == 6) { log_text = fmt::format( - "{},{},{},{},{},{},{}", timestamp, *initial_speed_, // - *friction_wheels_velocity_[0], *friction_wheels_velocity_[1], - *friction_wheels_velocity_[2], *friction_wheels_velocity_[3], - *fractional_temperature_); + "{},{},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f}", *initial_speed_, + (int)velocities.size(), // + velocity_, excellence_rate_, pass_rate_, range_, range2_, velocity_max, + velocity_min); } log_stream_ << log_text << std::endl; - RCLCPP_INFO(get_logger(), "%s", log_text.c_str()); + RCLCPP_INFO( + get_logger(), "%s", + log_text.c_str()); // Record each shot's initial speed and statistics. + + log_velocity = fmt::format("{:.3f}", *initial_speed_); + std::ofstream out_file("shoot_recorder", std::ios::app); + if (out_file.is_open()) { + out_file << log_velocity << std::endl; + out_file.close(); + } last_shoot_timestamp_ = *shoot_timestamp_; } private: /// @brief Component interface + std::array, 2> friction_velocities_; + InputInterface initial_speed_; InputInterface shoot_timestamp_; - InputInterface fractional_temperature_; - - std::size_t friction_wheel_count_ = 2; - std::array, 4> friction_wheels_velocity_; + std::size_t friction_wheel_count_ = 6; + std::array, 2> friction_wheels_velocity_; /// @brief For log enum class LogMode { TRIGGER = 1, TIMING = 2 }; @@ -103,29 +106,153 @@ class ShootingRecorder double last_shoot_timestamp_ = 0; std::ofstream log_stream_; + std::string log_velocity; std::size_t log_count_ = 0; + std::vector velocities; + + double velocity_; + + double excellence_rate_; + double pass_rate_; + + double range_; + double range2_; + + double velocity_min; + double velocity_max; + + double v; + double aim_velocity; + private: static std::string timestamp_to_string(double timestamp) { - auto time = static_cast(timestamp); + auto time = static_cast(timestamp); auto local_time = std::localtime(&time); char buffer[100]; std::strftime(buffer, sizeof(buffer), "%Y-%m-%d %H:%M:%S", local_time); double fractional_seconds = timestamp - std::floor(timestamp); - int milliseconds = static_cast(fractional_seconds * 1000); + int milliseconds = static_cast(fractional_seconds * 1000); char result[150]; std::snprintf(result, sizeof(result), "%s.%03d", buffer, milliseconds); return result; } + void analysis1() { + double sum = 0.0; + for (const auto& v : velocities) { + sum += v; + } + velocity_ = sum / double(velocities.size()); + + sort(velocities.begin(), velocities.end()); + + range_ = velocities.back() - velocities.front(); + + if (velocities.size() >= 3) { + range2_ = velocities[int(velocities.size() - 2)] - velocities[1]; + } else { + range2_ = range_; + } + + velocity_max = velocities.back(); + velocity_min = velocities.front(); + + int excellence_count = 0; + int pass_count = 0; + for (int i = 0; i < int(velocities.size()); i++) { + if (velocities[i] >= velocity_ - 0.05 && velocities[i] <= velocity_ + 0.05) { + pass_count += 1; + } + if (velocities[i] >= velocity_ - 0.025 && velocities[i] <= velocity_ + 0.025) { + excellence_count += 1; + } + } + excellence_rate_ = double(excellence_count) / double(velocities.size()); + pass_rate_ = double(pass_count) / double(velocities.size()); + } + + void analysis2() { + double sum = 0.0; + for (const auto& v : velocities) { + sum += v; + } + + sort(velocities.begin(), velocities.end()); + + velocity_max = velocities.back(); + velocity_min = velocities.front(); + + int n_adjust = std::max(1, int((int)velocities.size() * 0.05)); + + for (int i = 0; i < n_adjust; i++) { + sum -= velocities[i]; + sum -= velocities[velocities.size() - 1 - i]; + } + + velocity_ = sum / double(velocities.size() - 2 * n_adjust); + + range_ = velocities.back() - velocities.front(); + range2_ = velocities[int(velocities.size() - 2)] - velocities[1]; + + 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) { + pass_count += 1; + } + if (velocities[i] >= velocity_ - 0.05 && velocities[i] <= velocity_ + 0.05) { + excellence_count += 1; + } + } + excellence_rate_ = double(excellence_count) / double(velocities.size()); + pass_rate_ = double(pass_count) / double(velocities.size()); + } + + void analysis3() { + double sum = 0.0; + for (const auto& v : velocities) { + sum += v; + } + velocity_ = sum / double(velocities.size()); + + int excellence_count = 0; + int pass_count = 0; + + for (const auto& v : velocities) { + if (v >= aim_velocity - 0.025 && v <= aim_velocity + 0.025) { + excellence_count += 1; + } + if (v >= aim_velocity - 0.05 && v <= aim_velocity + 0.05) { + pass_count += 1; + } + } + excellence_rate_ = double(excellence_count) / double(velocities.size()); + pass_rate_ = double(pass_count) / double(velocities.size()); + + sort(velocities.begin(), velocities.end()); + velocity_max = velocities.back(); + velocity_min = velocities.front(); + + range_ = velocities.back() - velocities.front(); + range2_ = velocities[int(velocities.size() - 2)] - velocities[1]; + } + + static double GetTime() { + using namespace std::chrono; + static auto start = high_resolution_clock::now(); + auto now = high_resolution_clock::now(); + duration elapsed = now - start; + return elapsed.count(); + } }; } // namespace rmcs_core::controller::shooting #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::ShootingRecorder, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::ShootingRecorder, rmcs_executor::Component) 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/filter/alpha_beta_angle_filter.hpp b/rmcs_ws/src/rmcs_core/src/filter/alpha_beta_angle_filter.hpp index fcb9a32ad..0f6d9dd20 100644 --- a/rmcs_ws/src/rmcs_core/src/filter/alpha_beta_angle_filter.hpp +++ b/rmcs_ws/src/rmcs_core/src/filter/alpha_beta_angle_filter.hpp @@ -11,7 +11,9 @@ namespace rmcs_core::filter { class AlphaBetaAngleFilter { public: AlphaBetaAngleFilter(double dt, double alpha, double beta) - : dt_(dt), alpha_(alpha), beta_(beta) {} + : dt_(dt) + , alpha_(alpha) + , beta_(beta) {} void reset() { initialized_ = false; @@ -51,9 +53,7 @@ class AlphaBetaAngleFilter { bool initialized() const { return initialized_; } private: - static double wrap_angle_(double x) { - return std::remainder(x, 2.0 * std::numbers::pi); - } + static double wrap_angle_(double x) { return std::remainder(x, 2.0 * std::numbers::pi); } double dt_; double alpha_; diff --git a/rmcs_ws/src/rmcs_core/src/filter/imu_ekf.hpp b/rmcs_ws/src/rmcs_core/src/filter/imu_ekf.hpp new file mode 100644 index 000000000..e7e98cf04 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/filter/imu_ekf.hpp @@ -0,0 +1,428 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::filter { + +class ImuEkf { +public: + using Vec3 = Eigen::Vector3d; + using Vec4 = Eigen::Vector4d; + using Mat3 = Eigen::Matrix3d; + using Mat4 = Eigen::Matrix4d; + using Mat43 = Eigen::Matrix; + using Mat34 = Eigen::Matrix; + + struct Config { + Mat3 process_noise = 1.22e-3 * Mat3::Identity(); + Mat3 measurement_noise = 50.0 * Mat3::Identity(); + Mat3 gate_noise = 0.02 * Mat3::Identity(); + + Mat4 initial_covariance = 0.15 * Mat4::Identity(); + }; + + class AccelCorrection { + public: + [[nodiscard]] double chi_square() const noexcept { return chi_square_; } + + private: + friend class ImuEkf; + + AccelCorrection( + const std::uint64_t revision, Vec3 innovation, Mat34 jacobian_h_x, + Eigen::LDLT innovation_covariance_ldlt, const double chi_square) + : revision_(revision) + , innovation_(std::move(innovation)) + , jacobian_h_x_(std::move(jacobian_h_x)) + , innovation_covariance_ldlt_(std::move(innovation_covariance_ldlt)) + , chi_square_(chi_square) {} + + std::uint64_t revision_ = 0; + Vec3 innovation_ = Vec3::Zero(); + Mat34 jacobian_h_x_ = Mat34::Zero(); + Eigen::LDLT innovation_covariance_ldlt_; + double chi_square_ = std::numeric_limits::quiet_NaN(); + }; + + ImuEkf() + : ImuEkf(Config{}) {} + + explicit ImuEkf(Config config) + : config_(std::move(config)) { + reset(); + } + + [[nodiscard]] const Config& config() const noexcept { return config_; } + + void set_config(const Config& config) { + config_ = config; + reset(); + } + + void reset() { + state_ = identity_quaternion(); + covariance_ = projected_covariance(state_, config_.initial_covariance); + ++revision_; + } + + [[nodiscard]] bool reset_from_accel(const Vec3& accel_g, const double initial_yaw = 0.0) { + if (!accel_g.allFinite() || !std::isfinite(initial_yaw)) { + return false; + } + + const std::optional normalized_accel = normalize_vector(accel_g); + if (!normalized_accel.has_value()) { + return false; + } + + const double gx = (*normalized_accel)(0); + const double gy = (*normalized_accel)(1); + const double gz = std::clamp((*normalized_accel)(2), -1.0, 1.0); + + double q0_val, q1_val, q2_val, q3_val; + if (gz >= 0.0) { + const double s = std::sqrt(0.5 * (1.0 + gz)); + const double inv = 0.5 / s; + q0_val = s; + q1_val = gy * inv; + q2_val = -gx * inv; + q3_val = 0.0; + } else { + const double s = std::sqrt(0.5 * (1.0 - gz)); + const double inv = 0.5 / s; + q0_val = gy * inv; + q1_val = s; + q2_val = 0.0; + q3_val = gx * inv; + } + + const double cy = std::cos(initial_yaw * 0.5); + const double sy = std::sin(initial_yaw * 0.5); + + state_(0) = cy * q0_val - sy * q3_val; + state_(1) = cy * q1_val - sy * q2_val; + state_(2) = cy * q2_val + sy * q1_val; + state_(3) = cy * q3_val + sy * q0_val; + + state_ = normalize_quaternion(state_); + covariance_ = projected_covariance(state_, config_.initial_covariance); + ++revision_; + return true; + } + + void inflate_attitude_uncertainty_to_initial() { + covariance_ = covariance_with_psd_floor( + covariance_, projected_covariance(state_, config_.initial_covariance)); + ++revision_; + } + + [[nodiscard]] bool predict(const Vec3& gyro_rad_per_sec, const double dt_seconds) { + if (!gyro_rad_per_sec.allFinite()) { + return false; + } + + if (!std::isfinite(dt_seconds) || dt_seconds < 0.0) { + return false; + } + + const PropagationResult propagation = + propagate_quaternion(state_, gyro_rad_per_sec, dt_seconds); + if (!propagation.quaternion.allFinite() || !propagation.orthogonalization.allFinite()) { + return false; + } + const Vec4 prior_state = propagation.quaternion; + const Mat4& orthogonalization = propagation.orthogonalization; + + const Mat4 jacobian_f_x = state_jacobian(gyro_rad_per_sec, dt_seconds, orthogonalization); + const Mat43 jacobian_f_w = process_noise_jacobian(state_, orthogonalization); + Mat4 prior_cov = symmetrized_mat4( + jacobian_f_x * covariance_ * jacobian_f_x.transpose() + + dt_seconds * jacobian_f_w * config_.process_noise * jacobian_f_w.transpose()); + if (!is_positive_semidefinite(prior_cov)) { + return false; + } + + state_ = prior_state; + covariance_ = prior_cov; + ++revision_; + return true; + } + + [[nodiscard]] std::optional prepare_correction(const Vec3& accel_g) const { + if (!accel_g.allFinite()) + return std::nullopt; + + const std::optional normalized_accel = normalize_vector(accel_g); + if (!normalized_accel.has_value()) + return std::nullopt; + + const Vec3 predicted_accel = measurement_model(state_); + const Vec3 gate_innovation = accel_g - predicted_accel; + const Vec3 update_innovation = *normalized_accel - predicted_accel; + const Mat34 jacobian_h_x = measurement_jacobian(state_); + const Mat3 projected_covariance = jacobian_h_x * covariance_ * jacobian_h_x.transpose(); + const Mat3 gate_covariance = symmetrized_mat3(projected_covariance + config_.gate_noise); + + if (!is_positive_definite(gate_covariance)) { + return std::nullopt; + } + + const auto gate_ldt = gate_covariance.ldlt(); + if (gate_ldt.info() != Eigen::Success || !gate_ldt.isPositive()) { + return std::nullopt; + } + const Vec3 innovation_weighted = gate_ldt.solve(gate_innovation); + if (!innovation_weighted.allFinite()) { + return std::nullopt; + } + const double chi_square = gate_innovation.dot(innovation_weighted); + if (!std::isfinite(chi_square)) { + return std::nullopt; + } + + const Mat3 update_covariance = + symmetrized_mat3(projected_covariance + config_.measurement_noise); + if (!is_positive_definite(update_covariance)) { + return std::nullopt; + } + + const auto update_ldt = update_covariance.ldlt(); + if (update_ldt.info() != Eigen::Success || !update_ldt.isPositive()) { + return std::nullopt; + } + + return AccelCorrection(revision_, update_innovation, jacobian_h_x, update_ldt, chi_square); + } + + bool correct(const AccelCorrection& update) { + if (update.revision_ != revision_) { + return false; + } + const Vec4 state_prior = state_; + const Mat34 k_transpose = + update.innovation_covariance_ldlt_.solve(update.jacobian_h_x_ * covariance_); + if (!k_transpose.allFinite()) { + return false; + } + const Eigen::Matrix kalman_gain = k_transpose.transpose(); + Mat4 state_update_mask = Mat4::Identity(); + state_update_mask(3, 3) = 0.0; + const Eigen::Matrix effective_gain = state_update_mask * kalman_gain; + + const Vec4 quaternion_raw = state_prior + effective_gain * update.innovation_; + if (!quaternion_raw.allFinite() || quaternion_raw.squaredNorm() <= kEpsilon) { + return false; + } + + const Mat4 matrix_tmp = Mat4::Identity() - effective_gain * update.jacobian_h_x_; + Mat4 corrected_covariance = + matrix_tmp * covariance_ * matrix_tmp.transpose() + + effective_gain * config_.measurement_noise * effective_gain.transpose(); + + const Mat4 j_norm = orthogonalization_jacobian(quaternion_raw); + corrected_covariance = symmetrized_mat4(j_norm * corrected_covariance * j_norm.transpose()); + + if (!is_positive_semidefinite(corrected_covariance)) { + return false; + } + state_ = normalize_quaternion(quaternion_raw); + covariance_ = corrected_covariance; + ++revision_; + return true; + } + + bool correct(const Vec3& accel_mps2) { + const std::optional update = prepare_correction(accel_mps2); + return update.has_value() && correct(*update); + } + + [[nodiscard]] Eigen::Quaterniond quaternion() const noexcept { + return {state_[0], state_[1], state_[2], state_[3]}; + } + +private: + struct PropagationResult { + Vec4 quaternion = identity_quaternion(); + Mat4 orthogonalization = Mat4::Identity(); + }; + + [[nodiscard]] static Vec4 identity_quaternion() { + Vec4 q; + q << 1.0, 0.0, 0.0, 0.0; + return q; + } + + [[nodiscard]] static std::optional normalize_vector(const Vec3& vector) { + const double squared_norm = vector.squaredNorm(); + if (!std::isfinite(squared_norm) || squared_norm <= kEpsilon) { + return std::nullopt; + } + + return vector / std::sqrt(squared_norm); + } + + [[nodiscard]] static Vec4 normalize_quaternion(const Vec4& quaternion) { + const double squared_norm = quaternion.squaredNorm(); + if (!std::isfinite(squared_norm) || squared_norm <= kEpsilon) { + return identity_quaternion(); + } + + return quaternion / std::sqrt(squared_norm); + } + + [[nodiscard]] static Mat3 symmetrized_mat3(const Mat3& matrix) { + return 0.5 * (matrix + matrix.transpose()); + } + + [[nodiscard]] static Mat4 symmetrized_mat4(const Mat4& matrix) { + return 0.5 * (matrix + matrix.transpose()); + } + + [[nodiscard]] static bool is_positive_definite(const Mat3& matrix) { + if (!matrix.allFinite()) { + return false; + } + + const Eigen::SelfAdjointEigenSolver solver(symmetrized_mat3(matrix)); + if (solver.info() != Eigen::Success || !solver.eigenvalues().allFinite()) { + return false; + } + + const double max_abs_eigenvalue = solver.eigenvalues().cwiseAbs().maxCoeff(); + const double tolerance = kEpsilon * std::max(1.0, max_abs_eigenvalue); + return solver.eigenvalues().minCoeff() > tolerance; + } + + [[nodiscard]] static bool is_positive_semidefinite(const Mat4& matrix) { + if (!matrix.allFinite()) { + return false; + } + + const Eigen::SelfAdjointEigenSolver solver(symmetrized_mat4(matrix)); + if (solver.info() != Eigen::Success || !solver.eigenvalues().allFinite()) { + return false; + } + + const double max_abs_eigenvalue = solver.eigenvalues().cwiseAbs().maxCoeff(); + const double tolerance = kEpsilon * std::max(1.0, max_abs_eigenvalue); + return solver.eigenvalues().minCoeff() >= -tolerance; + } + + [[nodiscard]] static Mat4 positive_semidefinite_part(const Mat4& matrix) { + const Eigen::SelfAdjointEigenSolver solver(symmetrized_mat4(matrix)); + if (solver.info() != Eigen::Success || !solver.eigenvalues().allFinite()) { + return Mat4::Zero(); + } + + return symmetrized_mat4( + solver.eigenvectors() * solver.eigenvalues().cwiseMax(0.0).asDiagonal() + * solver.eigenvectors().transpose()); + } + + [[nodiscard]] static Mat4 + covariance_with_psd_floor(const Mat4& covariance, const Mat4& covariance_floor) { + Mat4 floor = symmetrized_mat4(covariance_floor); + Mat4 result = floor + positive_semidefinite_part(symmetrized_mat4(covariance) - floor); + result = symmetrized_mat4(result); + if (is_positive_semidefinite(result)) { + return result; + } + + return floor; + } + + [[nodiscard]] static Mat4 + projected_covariance(const Vec4& quaternion, const Mat4& raw_covariance) { + const Mat4 j_norm = orthogonalization_jacobian(quaternion); + Mat4 covariance = symmetrized_mat4(j_norm * raw_covariance * j_norm.transpose()); + if (is_positive_semidefinite(covariance)) { + return covariance; + } + + return symmetrized_mat4(j_norm * Mat4::Identity() * j_norm.transpose()); + } + + [[nodiscard]] static Mat4 omega_matrix(const Vec3& gyro_rad_per_sec) { + const double gx = gyro_rad_per_sec(0); + const double gy = gyro_rad_per_sec(1); + const double gz = gyro_rad_per_sec(2); + Mat4 matrix; + // clang-format off + matrix << 0, -gx, -gy, -gz, + gx, 0, gz, -gy, + gy, -gz, 0, gx, + gz, gy, -gx, 0; + // clang-format on + return matrix; + } + + [[nodiscard]] static Mat4 orthogonalization_jacobian(const Vec4& quaternion) { + const double norm = quaternion.norm(); + const double inv_norm = (std::isfinite(norm) && norm > kEpsilon) ? 1.0 / norm : 1.0; + return inv_norm + * (Mat4::Identity() - inv_norm * inv_norm * quaternion * quaternion.transpose()); + } + + [[nodiscard]] static PropagationResult + propagate_quaternion(const Vec4& state, const Vec3& gyro_rad_per_sec, const double dt) { + const Mat4 omega = omega_matrix(gyro_rad_per_sec); + const Vec4 quaternion_tmp = state + 0.5 * dt * omega * state; + + PropagationResult result; + result.orthogonalization = orthogonalization_jacobian(quaternion_tmp); + result.quaternion = normalize_quaternion(quaternion_tmp); + return result; + } + + [[nodiscard]] static Mat4 state_jacobian( + const Vec3& gyro_rad_per_sec, const double dt, const Mat4& orthogonalization) { + return orthogonalization * (Mat4::Identity() + 0.5 * dt * omega_matrix(gyro_rad_per_sec)); + } + + [[nodiscard]] static Mat43 + process_noise_jacobian(const Vec4& state, const Mat4& orthogonalization) { + Mat43 matrix_q; + // clang-format off + matrix_q << -state(1), -state(2), -state(3), + state(0), -state(3), state(2), + state(3), state(0), -state(1), + -state(2), state(1), state(0); + // clang-format on + return orthogonalization * (0.5 * matrix_q); + } + + [[nodiscard]] static Vec3 measurement_model(const Vec4& state) { + Vec3 result; + result(0) = 2.0 * (state(1) * state(3) - state(0) * state(2)); + result(1) = 2.0 * (state(2) * state(3) + state(0) * state(1)); + result(2) = + state(0) * state(0) - state(1) * state(1) - state(2) * state(2) + state(3) * state(3); + return result; + } + + [[nodiscard]] static Mat34 measurement_jacobian(const Vec4& state) { + Mat34 result; + // clang-format off + result << -2.0 * state(2), 2.0 * state(3), -2.0 * state(0), 2.0 * state(1), + 2.0 * state(1), 2.0 * state(0), 2.0 * state(3), 2.0 * state(2), + 2.0 * state(0), -2.0 * state(1), -2.0 * state(2), 2.0 * state(3); + // clang-format on + return result; + } + + static constexpr double kEpsilon = 1e-12; + + Config config_; + Vec4 state_ = identity_quaternion(); + Mat4 covariance_ = Mat4::Identity(); + std::uint64_t revision_ = 0; +}; + +} // namespace rmcs_core::filter diff --git a/rmcs_ws/src/rmcs_core/src/filter/low_pass_filter.hpp b/rmcs_ws/src/rmcs_core/src/filter/low_pass_filter.hpp index 126eb4f4f..2d579f0d6 100644 --- a/rmcs_ws/src/rmcs_core/src/filter/low_pass_filter.hpp +++ b/rmcs_ws/src/rmcs_core/src/filter/low_pass_filter.hpp @@ -55,7 +55,7 @@ requires(variable_number > 0) class LowPassFilter { void set_cutoff(double cutoff_freq, double sampling_freq) { double dt = 1.0 / sampling_freq; double rc = 1.0 / (2 * std::numbers::pi * cutoff_freq); - alpha_ = dt / (dt + rc); + alpha_ = dt / (dt + rc); } 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 new file mode 100644 index 000000000..884ebf695 --- /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_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(); } + + 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) \ 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 3b656dd6c..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 @@ -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 "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; @@ -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.16, 0.0, 0.15}); + 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_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,9 +99,19 @@ 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 { + class Command : public Component { public: explicit Command(DeformableInfantryOmni& deformableInfantry) : deformableInfantry(deformableInfantry) {} @@ -92,18 +121,207 @@ class DeformableInfantryOmni DeformableInfantryOmni& deformableInfantry; }; - class BottomBoard final : private librmcs::agent::RmcsBoardLite { - friend class DeformableInfantryOmni; + 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 = {}) - : 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 +329,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 +340,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 +357,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 +421,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 +442,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 +618,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 +647,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 +657,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 +692,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,243 +764,64 @@ 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 { + 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) { + 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 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()); + 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); + 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 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_; - - device::Bmi088 bmi088_; - }; - - class TopBoard final : private librmcs::agent::CBoard { - friend class DeformableInfantryOmni; - - 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) - , 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_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.)); - - scope_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); - - status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); - - bmi088_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(y, -x, z); }); - } - - ~TopBoard() override = default; - - 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(); - - 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(), - }); - } - - private: - void uart1_receive_callback(const librmcs::data::UartDataView&) override {} - - 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 == 0x141) - 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) [[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); - } - - OutputInterface& tf_; - OutputInterface gimbal_yaw_velocity_imu_; - - device::Bmi088 bmi088_; - device::LkMotor gimbal_pitch_motor_; - device::DjiMotor gimbal_left_friction_; - device::DjiMotor gimbal_right_friction_; - device::DjiMotor scope_motor_; + StatusMonitor monitor_{}; }; auto status_service_callback(const std::shared_ptr& response) @@ -732,30 +833,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; @@ -766,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 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/hardware/device/benewake.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/benewake.hpp index 613c6f613..234224455 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/benewake.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/benewake.hpp @@ -45,7 +45,7 @@ class Benewake { void update_status() { const auto package = package_.load(std::memory_order::relaxed); - *distance_ = package.calculate_distance(); + *distance_ = package.calculate_distance(); signal_strength_ = package.calculate_signal_strength(); } diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088.hpp index 96d84a2d8..31a44cbe8 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088.hpp @@ -150,12 +150,8 @@ class Bmi088 { struct alignas(8) ImuData { int16_t x, y, z; }; - std::atomic accelerometer_data_{ - {.x = 0, .y = 0, .z = 0} - }; - std::atomic gyroscope_data_{ - {.x = 0, .y = 0, .z = 0} - }; + std::atomic accelerometer_data_{{.x = 0, .y = 0, .z = 0}}; + std::atomic gyroscope_data_{{.x = 0, .y = 0, .z = 0}}; static_assert(std::atomic::is_always_lock_free); double ax_ = 0, ay_ = 0, az_ = 0, gx_ = 0, gy_ = 0, gz_ = 0; 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 new file mode 100644 index 000000000..83c1b2e32 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp @@ -0,0 +1,172 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include "filter/imu_ekf.hpp" + +namespace rmcs_core::hardware::device { + +class Bmi088Ekf { +public: + struct Config { + Eigen::Matrix3d body_to_sensor = Eigen::Matrix3d::Identity(); + }; + + using TimePoint = rmcs_msgs::BoardClock::time_point; + using Snapshot = rmcs_msgs::ImuSnapshot; + + explicit Bmi088Ekf(Config config) + : config_(std::move(config)) {} + + Bmi088Ekf() + : Bmi088Ekf(Config{}) {} + + void push_accelerometer_sample( + std::int16_t x, std::int16_t y, std::int16_t z, TimePoint sample_time) { + const Eigen::Vector3d accel_g = + config_.body_to_sensor.transpose() * convert_accelerometer(x, y, z); + + if (!initialized_) { + if (ekf_.reset_from_accel(accel_g)) { + ekf_state_time_ = sample_time; + latest_snapshot_ = { + ekf_.quaternion(), + Eigen::Vector3d::Zero(), + ekf_state_time_, + }; + const auto guard = std::scoped_lock{mutex_}; + initialized_ = true; + } + return; + } + + if (sample_time < ekf_state_time_) + return; + + if (pending_accel_sample_ && sample_time < pending_accel_sample_->sample_time) + return; + + pending_accel_sample_ = {accel_g, sample_time}; + } + + std::optional try_update_with_gyroscope_sample( + std::int16_t x, std::int16_t y, std::int16_t z, TimePoint sample_time) { + const Eigen::Vector3d gyro_rad_per_sec = + config_.body_to_sensor.transpose() * convert_gyroscope(x, y, z); + + if (!initialized_) + return std::nullopt; + + if (sample_time < ekf_state_time_) + return std::nullopt; + + if (is_gyro_saturated(gyro_rad_per_sec)) + ekf_.inflate_attitude_uncertainty_to_initial(); + + while (pending_accel_sample_) { + const auto& accel_sample_time = pending_accel_sample_->sample_time; + if (accel_sample_time < ekf_state_time_ || accel_sample_time > sample_time) + break; + + if (!ekf_.predict( + gyro_rad_per_sec, + std::chrono::duration{accel_sample_time - ekf_state_time_}.count())) + break; + ekf_state_time_ = accel_sample_time; + + const auto correction = ekf_.prepare_correction(pending_accel_sample_->accel_g); + if (!correction || correction->chi_square() >= 3.0) + break; + if (!ekf_.correct(*correction)) + break; + + pending_accel_sample_ = std::nullopt; + break; + } + + // Guard against stale IMU frames that librmcs may deliver right after reconnect before the + // device-side buffer is drained. Integrating a frame with a large timestamp jump can inject + // a huge bogus gyro delta, so drop it instead of advancing the filter. + // TODO: Remove this once librmcs guarantees buffered historical IMU frames are flushed on + // connection. + if (std::chrono::duration{sample_time - ekf_state_time_}.count() > 1 / 1000.0) { + ekf_state_time_ = sample_time; + return std::nullopt; + } + + if (!ekf_.predict( + gyro_rad_per_sec, + std::chrono::duration{sample_time - ekf_state_time_}.count())) + return std::nullopt; + ekf_state_time_ = sample_time; + + auto snapshot = Snapshot{ + ekf_.quaternion(), + gyro_rad_per_sec, + ekf_state_time_, + }; + { + const auto guard = std::scoped_lock{mutex_}; + latest_snapshot_ = snapshot; + } + return snapshot; + } + + [[nodiscard]] bool initialized() const noexcept { + const auto guard = std::scoped_lock{mutex_}; + return initialized_; + } + + [[nodiscard]] std::optional snapshot() const noexcept { + const auto guard = std::scoped_lock{mutex_}; + if (!initialized_) + return std::nullopt; + return latest_snapshot_; + } + +private: + [[nodiscard]] static Eigen::Vector3d + convert_accelerometer(std::int16_t x, std::int16_t y, std::int16_t z) noexcept { + return Eigen::Vector3d{ + static_cast(x), static_cast(y), static_cast(z)} + / 32767.0 * 6.0; + } + + [[nodiscard]] static Eigen::Vector3d + convert_gyroscope(std::int16_t x, std::int16_t y, std::int16_t z) noexcept { + return Eigen::Vector3d{ + static_cast(x), static_cast(y), static_cast(z)} + / 32767.0 * 2000.0 / 180.0 * std::numbers::pi; + } + + [[nodiscard]] static bool is_gyro_saturated(const Eigen::Vector3d& gyro_rad_per_sec) noexcept { + return gyro_rad_per_sec.cwiseAbs().maxCoeff() >= 0.98 * 2000.0 / 180.0 * std::numbers::pi; + } + + const Config config_; + + mutable std::mutex mutex_; + bool initialized_ = false; + + filter::ImuEkf ekf_; + TimePoint ekf_state_time_; + + struct AccelSample { + Eigen::Vector3d accel_g; + TimePoint sample_time; + }; + std::optional pending_accel_sample_; + + Snapshot latest_snapshot_; +}; + +} // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/board_clock_lifter.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/board_clock_lifter.hpp new file mode 100644 index 000000000..1706ea2a9 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/board_clock_lifter.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include +#include + +#include + +namespace rmcs_core::hardware::device { + +class BoardClockLifter { +public: + using time_point = rmcs_msgs::BoardClock::time_point; + + time_point advance_timebase(std::uint32_t raw_timestamp_quarter_us) { + if (!has_latest_timebase_) { + has_latest_timebase_ = true; + last_timebase_raw_ = raw_timestamp_quarter_us; + latest_timebase_timestamp_ = raw_timestamp_quarter_us; + } + + latest_timebase_timestamp_ += + static_cast(raw_timestamp_quarter_us - last_timebase_raw_); + last_timebase_raw_ = raw_timestamp_quarter_us; + + return time_point{rmcs_msgs::BoardClock::duration{latest_timebase_timestamp_}}; + } + + [[nodiscard]] auto timebase() const -> std::optional { + if (!has_latest_timebase_) + return std::nullopt; + return time_point{rmcs_msgs::BoardClock::duration{latest_timebase_timestamp_}}; + } + + [[nodiscard]] auto lift_timestamp(std::uint32_t timestamp_quarter_us) const + -> std::optional { + if (!has_latest_timebase_) + return std::nullopt; + + const auto latest_timestamp_low32 = static_cast(latest_timebase_timestamp_); + const auto signed_offset = + static_cast(timestamp_quarter_us - latest_timestamp_low32); + const auto lifted_timestamp = + latest_timebase_timestamp_ + static_cast(signed_offset); + return time_point{rmcs_msgs::BoardClock::duration{lifted_timestamp}}; + } + +private: + bool has_latest_timebase_ = false; + std::uint32_t last_timebase_raw_ = 0; + std::int64_t latest_timebase_timestamp_ = 0; +}; + +} // namespace rmcs_core::hardware::device 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/gy614.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/gy614.hpp index df5271504..e5b8d7b36 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/gy614.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/gy614.hpp @@ -34,9 +34,9 @@ class Gy614 { void update_status() { const auto package = package_.load(std::memory_order::relaxed); - emissivity_ = package.calculate_emissivity(); - *target_temperature_ = package.calculate_target_temperature(); - environment_temperature_ = package.calculate_environment_temperature(); + emissivity_ = package.calculate_emissivity(); + *target_temperature_ = package.calculate_target_temperature(); + environment_temperature_ = package.calculate_environment_temperature(); body_temperature_from_forhead_ = package.calculate_body_temperature_from_forhead(); } @@ -74,7 +74,7 @@ class Gy614 { double emissivity_ = 0; Component::OutputInterface target_temperature_; - double environment_temperature_ = 0; + double environment_temperature_ = 0; double body_temperature_from_forhead_ = 0; }; 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 7ab8a38f5..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 @@ -23,7 +23,14 @@ namespace rmcs_core::hardware::device { class LkMotor { public: - enum class Type : uint8_t { kMG5010Ei10, kMG4010Ei10, kMG6012Ei8, kMG4005Ei10, kMG5010Ei36 }; + enum class Type : uint8_t { + kMG5010Ei10, + kMG4010Ei10, + kMG6012Ei8, + kMG4005Ei10, + kMG5010Ei36, + kMHF7015, + }; struct Config { explicit Config(Type type) @@ -43,6 +50,7 @@ class LkMotor { rmcs_executor::Component& status_component, rmcs_executor::Component& command_component, const std::string& name_prefix) { status_component.register_output(name_prefix + "/angle", angle_output_, 0.0); + status_component.register_output(name_prefix + "/raw_angle", raw_angle_output_, 0); status_component.register_output(name_prefix + "/velocity", velocity_output_, 0.0); status_component.register_output(name_prefix + "/torque", torque_output_, 0.0); status_component.register_output(name_prefix + "/temperature", temperature_output_, 0.0); @@ -75,7 +83,7 @@ class LkMotor { switch (config.motor_type) { case Type::kMG5010Ei10: raw_angle_modulus_ = 1 << 16; - torque_constant = 0.90909; + torque_constant = 0.1; reduction_ratio = 10.0; // Note: max_torque_ should represent the ACTUAL maximum torque of the motor. @@ -93,7 +101,7 @@ class LkMotor { break; case Type::kMG6012Ei8: raw_angle_modulus_ = 1 << 16; - torque_constant = 1.09; + torque_constant = 1.09 / 8.0; reduction_ratio = 8.0; max_torque_ = 16.0; break; @@ -109,6 +117,12 @@ class LkMotor { reduction_ratio = 36.0; max_torque_ = 25.0; break; + case Type::kMHF7015: + raw_angle_modulus_ = 1 << 16; + torque_constant = 0.51; + reduction_ratio = 1.0; + max_torque_ = 2.42; + break; default: std::unreachable(); } @@ -193,6 +207,7 @@ class LkMotor { torque_ = status_current_to_torque_coefficient_ * static_cast(feedback.current); *angle_output_ = angle(); + *raw_angle_output_ = last_raw_angle(); *velocity_output_ = velocity(); *torque_output_ = torque(); *temperature_output_ = temperature(); @@ -472,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())); @@ -521,6 +548,7 @@ class LkMotor { double temperature_; rmcs_executor::Component::OutputInterface angle_output_; + rmcs_executor::Component::OutputInterface raw_angle_output_; rmcs_executor::Component::OutputInterface velocity_output_; rmcs_executor::Component::OutputInterface torque_output_; rmcs_executor::Component::OutputInterface temperature_output_; 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/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp new file mode 100644 index 000000000..754807075 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -0,0 +1,292 @@ +#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 "librmcs/board/rmcs_board_lite.hpp" + +namespace rmcs_core::hardware { + +class Flight + : public rmcs_executor::Component + , public rclcpp::Node + , public librmcs::board::RmcsBoardLite::Callback { +public: + Flight() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} { + + gimbal_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMHF7015} + .set_reversed() + .set_encoder_zero_point( + static_cast(get_parameter("yaw_motor_zero_point").as_int()))); + gimbal_pitch_motor_.configure( + 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} + .set_reversed() + .set_reduction_ratio(1.0)); + gimbal_right_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 4}.set_reduction_ratio(1.0)); + gimbal_bullet_feeder_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM2006, 1}.enable_multi_turn_angle()); + + using namespace rmcs_description; + + constexpr auto kCameraPostionX = 0.10238; + constexpr auto kCameraPostionZ = 0.05286; + 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("/tf", tf_); + + 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) { + 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; + }; + + 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]( + const std_srvs::srv::Trigger::Request::SharedPtr&, + 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(); + remote_control_->update(); + } + + 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(), + }) + .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()}) + .can_transmit( + Spec::kCans.kCan3, // + {.can_id = 0x142, .can_data = gimbal_pitch_motor_.generate_command().as_bytes()}); + } + +private: + void update_motors() { + gimbal_bullet_feeder_.update_status(); + gimbal_left_friction_.update_status(); + gimbal_right_friction_.update_status(); + + using namespace rmcs_description; + + gimbal_yaw_motor_.update_status(); + tf_->set_state(gimbal_yaw_motor_.angle()); + + gimbal_pitch_motor_.update_status(); + tf_->set_state(gimbal_pitch_motor_.angle()); + } + + void update_imu() { + using namespace rmcs_description; + + if (const auto snapshot = bmi088_.snapshot()) { + tf_->set_transform(snapshot->orientation.conjugate()); + + *gimbal_yaw_velocity_imu_ = snapshot->gyro_body.z(); + *gimbal_pitch_velocity_imu_ = snapshot->gyro_body.y(); + } + } + + 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(" yaw_motor_zero_point: {}", gimbal_yaw_motor_.last_raw_angle()); + text(" pitch_motor_zero_point: {}", gimbal_pitch_motor_.last_raw_angle()); + + response->message = feedback_message.str(); + } + +protected: + 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 (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 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 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; + + 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: + class FlightCommand : public rmcs_executor::Component { + public: + explicit FlightCommand(Flight& flight) + : flight_(flight) {} + + void update() override { flight_.command_update(); } + + private: + Flight& flight_; + }; + std::shared_ptr command_component_{ + create_partner_component(get_component_name() + "_command", *this), + }; + 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_; + 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_; + + EventOutputInterface imu_snapshot_output_; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; +}; + +} // namespace rmcs_core::hardware + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Flight, rmcs_executor::Component) 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 234d5aa70..b8c5c4af9 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp @@ -2,11 +2,10 @@ #include #include #include -#include #include #include -#include +#include #include #include #include @@ -16,15 +15,18 @@ #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" namespace rmcs_core::hardware { @@ -32,13 +34,12 @@ namespace rmcs_core::hardware { class OmniInfantry : public rmcs_executor::Component , public rclcpp::Node - , private librmcs::agent::CBoard { + , public librmcs::board::RmcsBoardLite::Callback { public: OmniInfantry() : Node{ get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , librmcs::agent::CBoard{get_parameter("board_serial").as_string()} , logger_(get_logger()) , infantry_command_( create_partner_component(get_component_name() + "_command", *this)) @@ -53,12 +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} - , bmi088_(1000, 0.2, 0.0) { + , 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()); @@ -75,29 +75,32 @@ 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_); + register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); + register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); register_output("/tf", tf_); - bmi088_.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_ = std::make_unique( + *this, get_parameter("board_serial").as_string()); - // Eigen::AngleAxisd pitch_link_to_imu_link{ - // std::numbers::pi / 2, Eigen::Vector3d::UnitZ()}; - // Eigen::Vector3d mapping = pitch_link_to_imu_link * Eigen::Vector3d{1, 2, 3}; - // std::cout << mapping << std::endl; - - return std::make_tuple(y, -x, z); - }); + 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}); @@ -126,11 +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; @@ -144,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: @@ -218,14 +229,15 @@ class OmniInfantry } void update_imu() { - bmi088_.update_status(); - Eigen::Quaterniond const gimbal_imu_pose{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + const auto snapshot = bmi088_.snapshot(); + if (!snapshot) + return; + tf_->set_transform( - gimbal_imu_pose.conjugate()); + 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 gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { @@ -237,68 +249,87 @@ class OmniInfantry gimbal_pitch_motor_.calibrate_zero_point()); } - 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; - 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); + 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 can2_receive_callback(const librmcs::data::CanDataView& data) 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 (gpio != Spec::kGpios.kUart0Tx) + return; + if (!data.timestamp_quarter_us) 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); - } - } + const auto timestamp = board_clock_lifter_.lift_timestamp(*data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; - 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()); + camera_signal_output_.emit(*timestamp); } - 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 { - 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); } - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_.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; + + auto snapshot = + bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); + if (!snapshot) + return; + + imu_snapshot_output_.emit(*snapshot); } private: rclcpp::Logger logger_; + std::unique_ptr board_; + class InfantryCommand : public rmcs_executor::Component { public: explicit InfantryCommand(OmniInfantry& infantry) @@ -324,10 +355,14 @@ class OmniInfantry device::DjiMotor gimbal_bullet_feeder_; device::Dr16 dr16_; - device::Bmi088 bmi088_; + std::unique_ptr remote_control_; + device::Bmi088Ekf bmi088_; + device::BoardClockLifter board_clock_lifter_; OutputInterface gimbal_yaw_velocity_imu_; OutputInterface gimbal_pitch_velocity_imu_; + EventOutputInterface camera_signal_output_; + EventOutputInterface imu_snapshot_output_; OutputInterface tf_; 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/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..b216fa6eb --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp @@ -0,0 +1,950 @@ +#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 +#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 { + +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_); + + 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()); + + 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(); + 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() { + 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_; + + struct TopBoard final : librmcs::board::RmcsBoardLite::Callback { + explicit TopBoard( + SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, + std::string_view board_serial = {}) + : logger_(steering_hero.get_logger()) + , tf_(steering_hero.tf_) + , 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_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, 1} + .set_reversed() + .set_reduction_ratio(1.)); + gimbal_friction_wheels_[1].configure( + 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, 3}.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} + .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, 3} + .set_reduction_ratio(1.) + .enable_multi_turn_angle()); + 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( + 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/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); + + 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() { + // 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(); + + if (auto snapshot = bmi088_.snapshot()) { + tf_->set_transform( + snapshot->orientation.conjugate()); + + *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(); + 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(); + + *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, // + { + .can_id = 0x142, + .can_data = gimbal_bullet_feeder_.generate_torque_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.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( + 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 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); + } + } 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 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) { + 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); + } + } + + 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; + + 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_; + // CanReceiveRateCounter can2_receive_rate_counter_; + // CanReceiveRateCounter can3_receive_rate_counter_; + OutputInterface& tf_; + + int count_ = 0; + int friciton_detect[6]; + int can0_detect[3]; + + 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]; + 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_; + 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_; + }; + + struct BottomBoard final : librmcs::board::RmcsBoardLite::Callback { + explicit BottomBoard( + 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") + , imu_(1000, 0.2, 0.0) + , dr16_{} + , 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, 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, 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, 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, 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, 4} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_wheel_motors_[1].configure( + 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, 3} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_wheel_motors_[3].configure( + 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, 1} + .set_reversed() + .set_reduction_ratio(19.)); + chassis_front_climber_motor_[1].configure( + 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, 2} + .enable_multi_turn_angle() + .set_reduction_ratio(19.)); + chassis_back_climber_motor_[1].configure( + 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() + .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) { + board_->start_transmit().uart_transmit( + Spec::kUarts.kUart0, {.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); + + board_ = std::make_unique(*this, board_serial); + + steering_hero.remote_control_->register_dr16(&dr16_); + } + + 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 = 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 = 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 = 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 = 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(), + }); + } + + 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) { + 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 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()); + } + } + + [[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); + } + + void gyroscope_receive_callback(const View::ImuGyroscope& 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_; + + 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 d30c1ce06..000000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp +++ /dev/null @@ -1,591 +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/benewake.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/supercap.hpp" - -namespace rmcs_core::hardware { - -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_); - tf_->set_transform( - Eigen::Translation3d{0.16, 0.0, 0.15}); - - 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("board_serial_bottom_board").as_string()); - - temperature_logging_timer_.reset(1000); - } - - 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_->update(); - - if (temperature_logging_timer_.tick()) { - temperature_logging_timer_.reset(1000); - RCLCPP_INFO( - get_logger(), - "Temperature: pitch: %.1f, top_yaw: %.1f, bottom_yaw: %.1f, feeder: %.1f", - top_board_->gimbal_pitch_motor_.temperature(), - top_board_->gimbal_top_yaw_motor_.temperature(), - bottom_board_->gimbal_bottom_yaw_motor_.temperature(), - top_board_->gimbal_bullet_feeder_.temperature()); - } - } - - 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 top yaw offset: %ld", - top_board_->gimbal_top_yaw_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New viewer offset: %ld", - top_board_->gimbal_player_viewer_motor_.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] left back steering offset: %d", - bottom_board_->chassis_steering_motors_[1].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] right back steering offset: %d", - bottom_board_->chassis_steering_motors_[2].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] right front steering offset: %d", - bottom_board_->chassis_steering_motors_[3].calibrate_zero_point()); - } - - class SteeringHeroCommand : public rmcs_executor::Component { - public: - explicit SteeringHeroCommand(SteeringHero& hero) - : hero_(hero) {} - - void update() override { hero_.command_update(); } - - private: - SteeringHero& hero_; - }; - std::shared_ptr command_component_; - - class TopBoard final : private librmcs::agent::CBoard { - public: - friend class SteeringHero; - explicit TopBoard( - SteeringHero& hero, SteeringHeroCommand& hero_command, - std::string_view board_serial = {}) - : librmcs::agent::CBoard(board_serial) - , tf_(hero.tf_) - , imu_(1000, 0.2, 0.0) - , benewake_(hero, "/gimbal/auto_aim/laser_distance") - , gimbal_top_yaw_motor_( - hero, hero_command, "/gimbal/top_yaw", - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} - .set_encoder_zero_point( - static_cast( - hero.get_parameter("top_yaw_motor_zero_point").as_int()))) - , gimbal_pitch_motor_( - hero, hero_command, "/gimbal/pitch", - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} - .set_encoder_zero_point( - static_cast(hero.get_parameter("pitch_motor_zero_point").as_int()))) - , gimbal_friction_wheels_( - {hero, hero_command, "/gimbal/second_left_friction", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio( - 1.)}, - {hero, hero_command, "/gimbal/first_left_friction", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio( - 1.)}, - {hero, hero_command, "/gimbal/first_right_friction", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(1.) - .set_reversed()}, - {hero, hero_command, "/gimbal/second_right_friction", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(1.) - .set_reversed()}) - , gimbal_bullet_feeder_( - hero, hero_command, "/gimbal/bullet_feeder", - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} - .set_reversed() - .enable_multi_turn_angle()) - , gimbal_scope_motor_( - hero, hero_command, "/gimbal/scope", - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}) - , gimbal_player_viewer_motor_( - hero, hero_command, "/gimbal/player_viewer", - device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} - .set_encoder_zero_point( - static_cast(hero.get_parameter("viewer_motor_zero_point").as_int())) - .set_reversed()) { - - 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. - - // Eigen::AngleAxisd pitch_link_to_imu_link{ - // std::numbers::pi, Eigen::Vector3d::UnitZ()}; - // Eigen::Vector3d mapping = pitch_link_to_imu_link * Eigen::Vector3d{1, 2, 3}; - // std::cout << mapping << std::endl; - - return std::make_tuple(-x, -y, z); - }); - - hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); - hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); - } - - 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()); - - benewake_.update_status(); - - *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()); - - gimbal_player_viewer_motor_.update_status(); - tf_->set_state( - gimbal_player_viewer_motor_.angle()); - - gimbal_scope_motor_.update_status(); - - for (auto& motor : gimbal_friction_wheels_) - motor.update_status(); - - gimbal_bullet_feeder_.update_status(); - } - - void command_update() { - auto builder = start_transmit(); - - 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 = 0x141, - .can_data = gimbal_bullet_feeder_ - .generate_torque_command(gimbal_bullet_feeder_.control_torque()) - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_scope_motor_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - 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.can2_transmit({ - .can_id = 0x141, - .can_data = gimbal_top_yaw_motor_.generate_command().as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x142, - .can_data = gimbal_pitch_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; - 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 == 0x141) { - gimbal_bullet_feeder_.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 == 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); - } else if (can_id == 0x201) { - gimbal_scope_motor_.store_status(data.can_data); - } - } - - void uart2_receive_callback(const librmcs::data::UartDataView& data) override { - benewake_.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::Benewake benewake_; - - OutputInterface gimbal_yaw_velocity_imu_; - OutputInterface gimbal_pitch_velocity_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 gimbal_scope_motor_; - device::LkMotor gimbal_player_viewer_motor_; - }; - - class BottomBoard final : private librmcs::agent::CBoard { - public: - friend class SteeringHero; - explicit BottomBoard( - SteeringHero& hero, SteeringHeroCommand& hero_command, - std::string_view board_serial = {}) - : librmcs::agent::CBoard(board_serial) - , imu_(1000, 0.2, 0.0) - , tf_(hero.tf_) - , dr16_(hero) - , chassis_steering_motors_( - {hero, hero_command, "/chassis/left_front_steering", - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast(hero.get_parameter("left_front_zero_point").as_int())) - .set_reversed()}, - {hero, hero_command, "/chassis/left_back_steering", - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast(hero.get_parameter("left_back_zero_point").as_int())) - .set_reversed()}, - {hero, hero_command, "/chassis/right_back_steering", - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast(hero.get_parameter("right_back_zero_point").as_int())) - .set_reversed()}, - {hero, hero_command, "/chassis/right_front_steering", - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast(hero.get_parameter("right_front_zero_point").as_int())) - .set_reversed()}) - , chassis_wheel_motors_( - {hero, hero_command, "/chassis/left_front_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)}, - {hero, hero_command, "/chassis/left_back_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)}, - {hero, hero_command, "/chassis/right_back_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)}, - {hero, hero_command, "/chassis/right_front_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)}) - , supercap_(hero, hero_command) - , gimbal_bottom_yaw_motor_( - hero, hero_command, "/gimbal/bottom_yaw", - device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} - .set_reversed() - .set_encoder_zero_point( - static_cast( - hero.get_parameter("bottom_yaw_motor_zero_point").as_int()))) { - - 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; - }; - - hero.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - - hero.register_output( - "/chassis/powermeter/control_enable", powermeter_control_enabled_, false); - hero.register_output( - "/chassis/powermeter/charge_power_limit", powermeter_charge_power_limit_, 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(); - supercap_.update_status(); - - *chassis_yaw_velocity_imu_ = imu_.gz(); - - for (auto& motor : chassis_wheel_motors_) - motor.update_status(); - for (auto& motor : chassis_steering_motors_) - motor.update_status(); - gimbal_bottom_yaw_motor_.update_status(); - tf_->set_state( - gimbal_bottom_yaw_motor_.angle()); - } - - void command_update() { - auto builder = start_transmit(); - - 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.can1_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - } - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steering_motors_[0].generate_command(), - chassis_steering_motors_[1].generate_command(), - chassis_steering_motors_[2].generate_command(), - chassis_steering_motors_[3].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; - 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 == 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; - if (can_id == 0x205) { - chassis_steering_motors_[0].store_status(data.can_data); - } else if (can_id == 0x206) { - chassis_steering_motors_[1].store_status(data.can_data); - } else if (can_id == 0x207) { - chassis_steering_motors_[2].store_status(data.can_data); - } else if (can_id == 0x208) { - chassis_steering_motors_[3].store_status(data.can_data); - } else if (can_id == 0x141) { - gimbal_bottom_yaw_motor_.store_status(data.can_data); - } - } - - 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); - } - - device::Bmi088 imu_; - OutputInterface& tf_; - - OutputInterface chassis_yaw_velocity_imu_; - - OutputInterface powermeter_control_enabled_; - OutputInterface powermeter_charge_power_limit_; - - device::Dr16 dr16_; - - device::DjiMotor chassis_steering_motors_[4]; - device::DjiMotor chassis_wheel_motors_[4]; - device::Supercap supercap_; - - 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::unique_ptr top_board_; - std::unique_ptr bottom_board_; - - rmcs_utility::TickTimer temperature_logging_timer_; -}; - -} // namespace rmcs_core::hardware - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringHero, rmcs_executor::Component) 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 2b30039e1..000000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-infantry.cpp +++ /dev/null @@ -1,537 +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 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_steer_motors_[0].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[steer calibration] New left back offset: %d", - bottom_board_->chassis_steer_motors_[1].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[steer calibration] New right back offset: %d", - bottom_board_->chassis_steer_motors_[2].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[steer calibration] New right front offset: %d", - bottom_board_->chassis_steer_motors_[3].calibrate_zero_point()); - } - class SteeringInfantryCommand : public rmcs_executor::Component { - public: - explicit SteeringInfantryCommand(SteeringInfantry& steering_infantry) - : steering_infantry(steering_infantry) {} - - void update() override { steering_infantry.command_update(); } - - SteeringInfantry& steering_infantry; - }; - - class TopBoard final : private librmcs::agent::CBoard { - public: - friend class SteeringInfantry; - explicit TopBoard( - SteeringInfantry& steering_infantry, SteeringInfantryCommand& steering_infantry_command, - std::string_view board_serial = {}) - : librmcs::agent::CBoard(board_serial) - , tf_(steering_infantry.tf_) - , bmi088_(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()))); - - 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_bmi088_); - steering_infantry.register_output( - "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); - - bmi088_.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. - - // Eigen::AngleAxisd pitch_link_to_bmi088_link{ - // std::numbers::pi / 2, Eigen::Vector3d::UnitZ()}; - // Eigen::Vector3d mapping = pitch_link_to_bmi088_link * Eigen::Vector3d{1, 2, 3}; - // std::cout << mapping << std::endl; - - return std::make_tuple(x, y, z); - }); - } - - TopBoard(const TopBoard&) = delete; - TopBoard& operator=(const TopBoard&) = delete; - TopBoard(TopBoard&&) = delete; - TopBoard& operator=(TopBoard&&) = delete; - - ~TopBoard() override = default; - - void update() { - 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_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{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - gimbal_left_friction_.generate_command(), - gimbal_right_friction_.generate_command(), - } - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x142, - .can_data = gimbal_pitch_motor_ - .generate_velocity_command(gimbal_pitch_motor_.control_velocity()) - .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 == 0x203) { - gimbal_left_friction_.store_status(data.can_data); - } else if (can_id == 0x204) { - gimbal_right_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 == 0x142) - gimbal_pitch_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); - } - OutputInterface& tf_; - - OutputInterface gimbal_yaw_velocity_bmi088_; - OutputInterface gimbal_pitch_velocity_bmi088_; - - device::Bmi088 bmi088_; - device::LkMotor gimbal_pitch_motor_; - device::DjiMotor gimbal_left_friction_; - device::DjiMotor gimbal_right_friction_; - }; - - class BottomBoard final : private librmcs::agent::CBoard { - public: - friend class SteeringInfantry; - - explicit BottomBoard( - SteeringInfantry& steering_infantry, SteeringInfantryCommand& steering_infantry_command, - std::string_view board_serial = {}) - : librmcs::agent::CBoard(board_serial) - , imu_(1000, 0.2, 0.0) - , tf_(steering_infantry.tf_) - , dr16_(steering_infantry) - , gimbal_yaw_motor_(steering_infantry, steering_infantry_command, "/gimbal/yaw") - , gimbal_bullet_feeder_( - steering_infantry, steering_infantry_command, "/gimbal/bullet_feeder") - , chassis_wheel_motors_( - {steering_infantry, steering_infantry_command, "/chassis/left_front_wheel"}, - {steering_infantry, steering_infantry_command, "/chassis/left_back_wheel"}, - {steering_infantry, steering_infantry_command, "/chassis/right_back_wheel"}, - {steering_infantry, steering_infantry_command, "/chassis/right_front_wheel"}) - , chassis_steer_motors_( - {steering_infantry, steering_infantry_command, "/chassis/left_front_steering"}, - {steering_infantry, steering_infantry_command, "/chassis/left_back_steering"}, - {steering_infantry, steering_infantry_command, "/chassis/right_back_steering"}, - {steering_infantry, steering_infantry_command, "/chassis/right_front_steering"}) - , supercap_(steering_infantry, steering_infantry_command) { - - 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().uart1_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( - steering_infantry.get_parameter("yaw_motor_zero_point").as_int()))); - - gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006} - .enable_multi_turn_angle() - .set_reversed() - .set_reduction_ratio(19 * 2)); - - 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()); - chassis_steer_motors_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - steering_infantry.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( - steering_infantry.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( - steering_infantry.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( - steering_infantry.get_parameter("right_front_zero_point").as_int())) - .enable_multi_turn_angle()); - 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() override = default; - - void update() { - imu_.update_status(); - *chassis_yaw_velocity_imu_ = imu_.gy(); - 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_yaw_motor_.update_status(); - tf_->set_state( - gimbal_yaw_motor_.angle()); - - gimbal_bullet_feeder_.update_status(); - } - - void command_update() { - if (can_transmission_mode_) { - auto builder = start_transmit(); - - 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.can1_transmit({ - .can_id = 0x1FF, - .can_data = - device::CanPacket8{ - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .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(), - }); - - auto steer_builder = start_transmit(); - steer_builder.can2_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steer_motors_[0].generate_command(), - chassis_steer_motors_[1].generate_command(), - chassis_steer_motors_[2].generate_command(), - chassis_steer_motors_[3].generate_command(), - } - .as_bytes(), - }); - } else { - auto builder = start_transmit(); - - 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.can1_transmit({ - .can_id = 0x1FF, - .can_data = - device::CanPacket8{ - gimbal_bullet_feeder_.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_velocity_command( - gimbal_yaw_motor_.control_velocity() - imu_.gy()) - .as_bytes(), - }); - } - can_transmission_mode_ = !can_transmission_mode_; - } - - private: - void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); - } - - 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_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 == 0x205) - gimbal_bullet_feeder_.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; - if (can_id == 0x142) - gimbal_yaw_motor_.store_status(data.can_data); - else if (can_id == 0x205) - chassis_steer_motors_[0].store_status(data.can_data); - else if (can_id == 0x206) - chassis_steer_motors_[1].store_status(data.can_data); - else if (can_id == 0x207) - chassis_steer_motors_[2].store_status(data.can_data); - else if (can_id == 0x208) - chassis_steer_motors_[3].store_status(data.can_data); - } - - 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 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); - } - - bool can_transmission_mode_ = true; - device::Bmi088 imu_; - OutputInterface& tf_; - OutputInterface powermeter_control_enabled_; - OutputInterface powermeter_charge_power_limit_; - - device::Dr16 dr16_; - device::LkMotor gimbal_yaw_motor_; - device::DjiMotor gimbal_bullet_feeder_; - 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_; - }; - - OutputInterface tf_; - - std::shared_ptr command_component_; - std::shared_ptr top_board_; - std::shared_ptr bottom_board_; - - rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; - rclcpp::Subscription::SharedPtr steers_calibrate_subscription_; -}; - -} // namespace rmcs_core::hardware - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringInfantry, rmcs_executor::Component) 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 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) 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 new file mode 100644 index 000000000..8e01acd86 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp @@ -0,0 +1,224 @@ +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "referee/app/ui/shape/shape.hpp" + +namespace rmcs_core::referee::app::ui { +using namespace rmcs_description; + +class AutoAimUi + : public rmcs_executor::Component + , public rclcpp::Node { +public: + AutoAimUi() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} { + + offset_ = Eigen::Vector3d{ + get_parameter_or("offset_x", 0.), + get_parameter_or("offset_y", 0.), + get_parameter_or("offset_z", 0.), + }; + + // What's the point of an auto-aim UI if it doesn't have auto-aim? + 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; + } + + const auto point = reproject(*robot_center_); + if (!point.allFinite() // + || point.x() >= kScreenW || point.x() < 0 // + || point.y() >= kScreenH || point.y() < 0 // + ) { + set_distance_text(type, std::nullopt); + hide_all(); + return; + } + + const auto x = std::clamp(std::lround(point.x()), 0, kScreenW - 1); + const auto y = std::clamp(std::lround(point.y()), 0, kScreenH - 1); + + 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); + center_ring_.set_r(radius); + center_ring_.set_visible(true); + + if (*should_shoot_) { + constexpr int kCrossLen = 16; + constexpr int kCrossGap = 5; + + constexpr auto clamp_x = [](int x) { + return std::clamp(x, 0, kScreenW - 1); + }; + constexpr auto clamp_y = [](int y) { + return std::clamp(y, 0, kScreenH - 1); + }; + + cross_top_.set_color(color); + cross_top_.set_x(x); + cross_top_.set_y(clamp_y(y - kCrossLen)); + cross_top_.set_x2(x); + cross_top_.set_y2(clamp_y(y - kCrossGap)); + cross_top_.set_visible(true); + + cross_bottom_.set_color(color); + cross_bottom_.set_x(x); + cross_bottom_.set_y(clamp_y(y + kCrossGap)); + cross_bottom_.set_x2(x); + cross_bottom_.set_y2(clamp_y(y + kCrossLen)); + cross_bottom_.set_visible(true); + + cross_left_.set_color(color); + cross_left_.set_x(clamp_x(x - kCrossLen)); + cross_left_.set_y(y); + cross_left_.set_x2(clamp_x(x - kCrossGap)); + cross_left_.set_y2(y); + cross_left_.set_visible(true); + + cross_right_.set_color(color); + cross_right_.set_x(clamp_x(x + kCrossGap)); + cross_right_.set_y(y); + cross_right_.set_x2(clamp_x(x + kCrossLen)); + cross_right_.set_y2(y); + cross_right_.set_visible(true); + } else { + cross_top_.set_visible(false); + cross_bottom_.set_visible(false); + cross_left_.set_visible(false); + cross_right_.set_visible(false); + } + } + +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; + static constexpr double kCx = 961.8345772991; + static constexpr double kCy = 549.3357457590; + + static constexpr double kK1 = -0.1764932263; + static constexpr double kK2 = +0.1582678597; + static constexpr double kK3 = -0.1557993057; + static constexpr double kP1 = -0.0000433367; + static constexpr double kP2 = +0.0003937540; + + Eigen::Vector3d offset_ = Eigen::Vector3d::Zero(); + + 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, + }; + + void hide_all() { + center_ring_.set_visible(false); + cross_top_.set_visible(false); + 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); + } + + Eigen::Vector2d reproject(const Eigen::Vector3d& center) const { + const auto camera_pose = fast_tf::lookup_transform(*tf_); + + const auto q_aa = Eigen::Quaterniond{camera_pose.rotation()}; + const auto t_aa = Eigen::Vector3d{camera_pose.translation()}; + + const auto t_ui = Eigen::Vector3d{t_aa - q_aa * offset_}; + + const auto point_ros = Eigen::Vector3d{q_aa.inverse() * (center - t_ui)}; + + const double x_cv = -point_ros.y(); + const double y_cv = +point_ros.z(); + const double z_cv = +point_ros.x(); + + if (z_cv <= 1e-6) + return Eigen::Vector2d{ + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + }; + + const double x_norm = x_cv / z_cv; + const double y_norm = y_cv / z_cv; + + const double r2 = x_norm * x_norm + y_norm * y_norm; + const double r4 = r2 * r2; + const double r6 = r4 * r2; + + const double radial = 1.0 + kK1 * r2 + kK2 * r4 + kK3 * r6; + const double x_dist = + x_norm * radial + 2.0 * kP1 * x_norm * y_norm + kP2 * (r2 + 2.0 * x_norm * x_norm); + const double y_dist = + y_norm * radial + kP1 * (r2 + 2.0 * y_norm * y_norm) + 2.0 * kP2 * x_norm * y_norm; + + return Eigen::Vector2d{kFx * x_dist + kCx, kFy * y_dist + kCy}; + } +}; + +} // namespace rmcs_core::referee::app::ui + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::AutoAimUi, 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 269a5682b..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 @@ -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" @@ -22,15 +27,20 @@ class DeformableInfantry , public rclcpp::Node { public: DeformableInfantry() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + : Node{ + 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) { @@ -45,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; @@ -102,44 +136,63 @@ 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; + case rmcs_msgs::ChassisMode::WIRELESS_CHARGING: return Shape::Color::BLACK; + 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_; @@ -147,18 +200,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_; @@ -167,10 +227,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/flight.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp new file mode 100644 index 000000000..88367c269 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp @@ -0,0 +1,96 @@ +#include +#include + +#include +#include +#include + +#include "referee/app/ui/shape/shape.hpp" +#include "referee/app/ui/widget/crosshair_circle.hpp" +#include "referee/app/ui/widget/status_ring.hpp" + +namespace rmcs_core::referee::app::ui { +class Flight + : public rmcs_executor::Component + , public rclcpp::Node { +public: + static constexpr uint8_t kUiModeCombat = 0; + static constexpr uint8_t kUiModeOutpostOnly = 1; + static constexpr uint8_t kUiModeEngineer = 2; + + Flight() + : Node{ + 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, false, false) + , 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}) + , mode_indicator_( + Shape::Color::YELLOW, 30, x_center + 360, y_center + 220, x_center + 400, + y_center + 220) { + + register_input("/referee/shooter/bullet_allowance", robot_bullet_allowance_); + + 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("/auto_aim/ui_mode", auto_aim_mode_, false); + + register_input("/remote/mouse", mouse_); + } + + void update() override { + 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_auto_aim_enable(mouse_->right == 1); + + update_mode_indicator(); + } + +private: + static constexpr uint16_t screen_width = 1920, screen_height = 1080; + static constexpr uint16_t x_center = screen_width / 2, y_center = screen_height / 2; + + auto update_mode_indicator() -> void { + auto mode_value = auto_aim_mode_.ready() ? *auto_aim_mode_ : kUiModeCombat; + + switch (mode_value) { + case kUiModeOutpostOnly: mode_indicator_.set_color(Shape::Color::GREEN); break; + case kUiModeEngineer: mode_indicator_.set_color(Shape::Color::PINK); break; + case kUiModeCombat: + default: mode_indicator_.set_color(Shape::Color::YELLOW); break; + } + } + + InputInterface robot_bullet_allowance_; + + InputInterface left_friction_control_velocity_; + InputInterface left_friction_velocity_; + InputInterface right_friction_velocity_; + + InputInterface auto_aim_mode_; + + InputInterface mouse_; + + CrossHairCircle crosshair_circle_; + StatusRing status_ring_; + + Line horizontal_center_guidelines_[2]; + Line vertical_center_guidelines_[2]; + + Line mode_indicator_; +}; + +} // namespace rmcs_core::referee::app::ui + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::Flight, 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 0f41b8136..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 @@ -1,12 +1,16 @@ #include #include #include +#include #include #include #include #include +#include +#include #include +#include #include #include "referee/app/ui/shape/shape.hpp" @@ -21,20 +25,51 @@ class Hero , public rclcpp::Node { public: Hero() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , status_ring_(26.5, 26.5, 600, 40) - , rangefinder_() - , 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) { + // , chassis_direction_indicator_(Shape::Color::PINK, 8, x_center, y_center, 0, 0, 84, 84) + , chassis_left_wheel_indicator_( + Shape::Color::WHITE, wheel_indicator_width, x_center, y_center, 0, 0, + wheel_indicator_radius, wheel_indicator_radius) + , chassis_right_wheel_indicator_( + Shape::Color::WHITE, wheel_indicator_width, x_center, y_center, 0, 0, + wheel_indicator_radius, wheel_indicator_radius) + , yaw_angle_number_(Shape::Color::YELLOW, 20, 5, x_center + 270, y_center + 95, 0.0, false) + , pitch_angle_number_( + Shape::Color::YELLOW, 20, 5, x_center + 270, y_center - 35, 0.0, false) + , 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_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) + { chassis_control_direction_indicator_.set_x(x_center); chassis_control_direction_indicator_.set_y(y_center); + register_input("/gimbal/mode", gimbal_mode_); + register_input("/remote/keyboard", keyboard_); register_input("/chassis/control_mode", chassis_mode_); register_input("/chassis/angle", chassis_angle_); register_input("/chassis/control_angle", chassis_control_angle_); + register_input("/chassis/climber/left_front_motor/velocity", left_track_velocity_); + register_input("/chassis/climber/right_front_motor/velocity", right_track_velocity_); + register_input("/chassis/supercap/voltage", supercap_voltage_); // register_input("/chassis/supercap/control_enable", supercap_control_enabled_); @@ -42,19 +77,25 @@ class Hero register_input("/chassis/power", chassis_power_); register_input("/chassis/control_power_limit", chassis_control_power_limit_); register_input("/chassis/supercap/charge_power_limit", supercap_charge_power_limit_); - - register_input("/referee/shooter/42mm_bullet_allowance", robot_bullet_allowance_); + 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_); + register_input("/gimbal/player_viewer/raw_angle", gimbal_player_viewer_raw_angle_); register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); - register_input("/gimbal/auto_aim/laser_distance", laser_distance_); + register_input("/gimbal/pitch/raw_angle", gimbal_pitch_raw_angle_); + register_input("/gimbal/bottom_yaw/angle", bottom_yaw_angle_); + register_input("/gimbal/bottom_yaw/raw_angle", bottom_yaw_raw_angle_); + // register_input("/gimbal/auto_aim/laser_distance", laser_distance_); + + register_input("/gimbal/shooter/preloaded_ready", shooter_preloaded_ready_, false); - register_input("/gimbal/shooter/mode", shoot_mode_); - register_input("/gimbal/scope/active", is_scope_active_); + // register_input("/gimbal/scope/active", is_scope_active_); register_input("/remote/mouse", mouse_); @@ -63,35 +104,155 @@ class Hero void update() override { update_normal_ui(); - update_sniper_ui(); - - if (*is_scope_active_) { - set_normal_ui_visible(false); - rangefinder_.set_visible(true); - } else { - set_normal_ui_visible(true); - rangefinder_.set_visible(false); - } + // update_sniper_ui(); + + // if (*is_scope_active_) { + // set_normal_ui_visible(false); + // rangefinder_.set_visible(true); + // } else { + set_normal_ui_visible(true); + // rangefinder_.set_visible(false); + // } } private: + static uint16_t count_digits(int32_t value) { + uint16_t digits = value <= 0 ? 1 : 0; + while (value != 0) { + value /= 10; + ++digits; + } + return digits; + } void set_normal_ui_visible(bool value) { status_ring_.set_visible(value); - chassis_direction_indicator_.set_visible(value); - chassis_control_direction_indicator_.set_visible(value); + // chassis_direction_indicator_.set_visible(value); + chassis_left_wheel_indicator_.set_visible(value); + chassis_right_wheel_indicator_.set_visible(value); + // chassis_control_direction_indicator_.set_visible(value); + yaw_angle_number_.set_visible(value); + pitch_angle_number_.set_visible(value); + bottom_yaw_angle_number_.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); + + const bool show_friction_profile_box = + value && friction_profile_1_active_.ready() && *friction_profile_1_active_; + for (auto& line : friction_profile_indicator_) + line.set_visible(show_friction_profile_box); + if (!value) + chassis_control_direction_indicator_.set_visible(false); } void update_normal_ui() { update_chassis_direction_indicator(); - - status_ring_.update_bullet_allowance(*robot_bullet_allowance_); + yaw_angle_number_.set_value(static_cast(*gimbal_player_viewer_raw_angle_)); + 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(); + 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 + + count_digits(static_cast(*gimbal_player_viewer_raw_angle_)) + * raw_angle_font_size; + const uint16_t pitch_right = + pitch_raw_angle_x + + count_digits(static_cast(*gimbal_pitch_raw_angle_)) * raw_angle_font_size; + const uint16_t box_left = std::max(yaw_right, pitch_right) + friction_profile_box_gap; + const uint16_t box_right = box_left + friction_profile_box_visual_width; + + const uint16_t box_top = yaw_raw_angle_y + raw_angle_font_size; + const uint16_t box_bottom = pitch_raw_angle_y; + + const bool friction_profile_1_active = + friction_profile_1_active_.ready() && *friction_profile_1_active_; + const uint16_t box_center_x = (box_left + box_right) / 2; + const uint16_t profile_number_y = box_top + friction_profile_number_gap; + + for (auto& line : friction_profile_indicator_) + line.set_color(Shape::Color::GREEN); + + friction_profile_number_.set_value(friction_profile_1_active ? 16 : 12); + friction_profile_number_.set_color( + friction_profile_1_active ? Shape::Color::PINK : Shape::Color::GREEN); + friction_profile_number_.set_font_size(friction_profile_number_font_size); + friction_profile_number_.set_xy(box_left, profile_number_y); + friction_profile_number_.set_center_x(box_center_x); + + friction_profile_indicator_[0].set_x(box_left); + friction_profile_indicator_[0].set_y(box_top); + friction_profile_indicator_[0].set_x2(box_right); + friction_profile_indicator_[0].set_y2(box_top); + + friction_profile_indicator_[1].set_x(box_right); + friction_profile_indicator_[1].set_y(box_top); + friction_profile_indicator_[1].set_x2(box_right); + friction_profile_indicator_[1].set_y2(box_bottom); + + friction_profile_indicator_[2].set_x(box_left); + friction_profile_indicator_[2].set_y(box_bottom); + friction_profile_indicator_[2].set_x2(box_right); + friction_profile_indicator_[2].set_y2(box_bottom); + + friction_profile_indicator_[3].set_x(box_left); + friction_profile_indicator_[3].set_y(box_top); + 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_); - update_static_status_ring(); + last_keyboard_ = *keyboard_; + } + + void update_pitch_raw_angle_color() { + if (keyboard_.ready()) { + if (!last_keyboard_.e && keyboard_->e) { + last_e_triggered_with_ctrl_ = keyboard_->ctrl; + } + last_keyboard_ = *keyboard_; + } + + if (!gimbal_mode_.ready()) { + pitch_angle_number_.set_color(Shape::Color::YELLOW); + return; + } + + const bool is_encoder = *gimbal_mode_ == rmcs_msgs::GimbalMode::ENCODER; + const bool entering_encoder = + last_gimbal_mode_ != rmcs_msgs::GimbalMode::ENCODER && is_encoder; + const bool leaving_encoder = + last_gimbal_mode_ == rmcs_msgs::GimbalMode::ENCODER && !is_encoder; + + if (entering_encoder) { + pitch_encoder_by_ctrl_e_ = last_e_triggered_with_ctrl_; + } + + if (leaving_encoder) { + pitch_encoder_by_ctrl_e_ = false; + } + + if (!is_encoder) { + pitch_angle_number_.set_color(Shape::Color::YELLOW); + } else if (pitch_encoder_by_ctrl_e_) { + pitch_angle_number_.set_color(Shape::Color::PINK); + } else { + pitch_angle_number_.set_color(Shape::Color::GREEN); + } + + last_gimbal_mode_ = *gimbal_mode_; } void update_sniper_ui() { @@ -99,7 +260,7 @@ class Hero ? *gimbal_pitch_angle_ - 2 * std::numbers::pi : *gimbal_pitch_angle_; - rangefinder_.update_pitch_angle(-display_angle); + rangefinder_.update_pitch_angle(static_cast(*gimbal_pitch_raw_angle_)); double raw_height = -display_angle / 0.7 * static_cast(height_max); raw_height = std::clamp(raw_height, 0.0, static_cast(height_max)); @@ -107,6 +268,35 @@ class Hero lift_height = std::clamp(lift_height, height_min, height_max); rangefinder_.update_vertical_rangefinder(lift_height); + pitch_angle_number_.set_value(static_cast(*gimbal_pitch_raw_angle_)); + } + + void update_bottom_yaw_tracking_lines() { + const bool ctrl_e_pressed = keyboard_->ctrl && keyboard_->e; + if (ctrl_e_pressed) { + bottom_yaw_tracking_enabled_ = !bottom_yaw_tracking_enabled_; + bottom_yaw_anchor_angle_rad_ = *bottom_yaw_angle_; + } + + const double delta_bottom_yaw_rad = *bottom_yaw_angle_ - bottom_yaw_anchor_angle_rad_; + + const int min_center_x = static_cast(pink_line_half_length); + const int max_center_x = + static_cast(screen_width) - static_cast(pink_line_half_length); + + const int pink_center_x = std::clamp( + static_cast(std::lround( + static_cast(x_center) + + delta_bottom_yaw_rad * pink_line_pixels_per_radian)), + min_center_x, max_center_x); + + tracking_pink_line_.set_x( + static_cast(pink_center_x - static_cast(pink_line_half_length))); + tracking_pink_line_.set_y(y_center + pink_line_offset_y); + tracking_pink_line_.set_x2( + static_cast(pink_center_x + static_cast(pink_line_half_length))); + tracking_pink_line_.set_y2(y_center + pink_line_offset_y); + tracking_pink_line_.set_visible(true); } void update_time_reminder() { @@ -114,24 +304,41 @@ 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_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)); + // return static_cast( + // std::round((2 * std::numbers::pi - angle) / std::numbers::pi * 180)); + int degrees = static_cast( + std::lround((2.0 * std::numbers::pi - angle) / std::numbers::pi * 180.0)); + degrees %= 360; + if (degrees < 0) + degrees += 360; + return static_cast(degrees); }; - chassis_direction_indicator_.set_color( - chassis_mode == rmcs_msgs::ChassisMode::SPIN ? Shape::Color::GREEN - : Shape::Color::PINK); - chassis_direction_indicator_.set_angle(to_referee_angle(*chassis_angle_), 30); + // chassis_direction_indicator_.set_color( + // 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 = + std::abs(*left_track_velocity_) > track_velocity_active_threshold; + const bool right_track_active = + std::abs(*right_track_velocity_) > track_velocity_active_threshold; + + chassis_left_wheel_indicator_.set_color( + left_track_active ? Shape::Color::GREEN : Shape::Color::WHITE); + chassis_right_wheel_indicator_.set_color( + right_track_active ? Shape::Color::GREEN : Shape::Color::WHITE); + + const double wheel_offset = wheel_indicator_offset_deg * std::numbers::pi / 180.0; + const double left_wheel_angle = *chassis_angle_ + wheel_offset; + const double right_wheel_angle = *chassis_angle_ - wheel_offset; + + chassis_left_wheel_indicator_.set_angle( + to_referee_angle(left_wheel_angle), wheel_indicator_half_angle); + chassis_right_wheel_indicator_.set_angle( + to_referee_angle(right_wheel_angle), wheel_indicator_half_angle); bool chassis_control_direction_indicator_visible = false; if (!std::isnan(*chassis_control_angle_)) { @@ -158,10 +365,48 @@ class Hero 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 uint16_t raw_angle_font_size = 20; + + static constexpr uint16_t yaw_raw_angle_x = x_center + 270; + static constexpr uint16_t yaw_raw_angle_y = y_center + 65; + + static constexpr uint16_t pitch_raw_angle_x = x_center + 270; + static constexpr uint16_t pitch_raw_angle_y = y_center - 65; + + static constexpr uint16_t friction_profile_box_gap = 18; + static constexpr uint16_t friction_profile_box_visual_width = 105; + static constexpr uint16_t friction_profile_box_line_width = 6; + static constexpr uint16_t friction_profile_number_font_size = 24; + static constexpr uint16_t friction_profile_number_gap = 12; + static constexpr uint16_t height_min = 0, height_max = 500; + static constexpr uint16_t wheel_indicator_radius = 110; + static constexpr uint16_t wheel_indicator_width = 12; + static constexpr uint16_t wheel_indicator_half_angle = 10; + static constexpr double wheel_indicator_offset_deg = 28.0; + static constexpr double track_velocity_active_threshold = 1.0; + static constexpr uint16_t green_line_half_length = 8; + static constexpr uint16_t green_line_width = 40; + static constexpr uint16_t green_line_offset_y = 0; + + static constexpr uint16_t pink_line_half_length = 40; + static constexpr uint16_t pink_line_width = 8; + static constexpr uint16_t pink_line_offset_y = 0; + + static constexpr double pink_line_pixels_per_radian = 400.0; + + InputInterface gimbal_mode_; + InputInterface keyboard_; + + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + rmcs_msgs::GimbalMode last_gimbal_mode_ = rmcs_msgs::GimbalMode::IMU; + + bool last_e_triggered_with_ctrl_ = false; + bool pitch_encoder_by_ctrl_e_ = false; InputInterface chassis_mode_; InputInterface chassis_angle_, chassis_control_angle_; + InputInterface left_track_velocity_, right_track_velocity_; InputInterface supercap_voltage_; InputInterface supercap_control_enabled_; @@ -171,33 +416,55 @@ class Hero InputInterface chassis_control_power_limit_; InputInterface supercap_charge_power_limit_; - InputInterface robot_bullet_allowance_; + 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_; InputInterface game_stage_; + InputInterface gimbal_yaw_angle_; InputInterface gimbal_pitch_angle_; InputInterface gimbal_player_viewer_angle_; - InputInterface laser_distance_; + InputInterface gimbal_player_viewer_raw_angle_; + InputInterface gimbal_pitch_raw_angle_; + InputInterface bottom_yaw_raw_angle_; + InputInterface bottom_yaw_angle_; + // InputInterface laser_distance_; - InputInterface shoot_mode_; - InputInterface is_scope_active_; + InputInterface shooter_preloaded_ready_; + // InputInterface is_scope_active_; StatusRing status_ring_; Rangefinder rangefinder_; - Arc chassis_direction_indicator_, chassis_control_direction_indicator_; + Arc chassis_left_wheel_indicator_; + Arc chassis_right_wheel_indicator_; + Arc chassis_control_direction_indicator_; + + Float yaw_angle_number_; + Float pitch_angle_number_; + Float bottom_yaw_angle_number_; + Text state_word_; Integer time_reminder_; + + Integer bullet_allowance_number_; + Integer friction_profile_number_; + Line friction_profile_indicator_[4]; + Line center_green_line_; + Line tracking_pink_line_; + + bool bottom_yaw_tracking_enabled_ = false; + double bottom_yaw_anchor_angle_rad_ = 0.0; }; } // namespace rmcs_core::referee::app::ui #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 a12688554..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 @@ -20,7 +20,9 @@ class Infantry , public rclcpp::Node { public: Infantry() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , crosshair_(Shape::Color::WHITE, x_center - 12, y_center - 37) , status_ring_(26.5, 26.5, 600, 300) , horizontal_center_guidelines_( @@ -104,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; @@ -179,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) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/cfs_scheduler.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/cfs_scheduler.hpp index b387b972f..e73e297dc 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/cfs_scheduler.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/cfs_scheduler.hpp @@ -48,7 +48,7 @@ class CfsScheduler { private: bool operator<(const Entity& obj) const { return vruntime_ < obj.vruntime_; } uint64_t vruntime_ : 48 = 65536; - uint16_t priority_ = 0; + uint16_t priority_ = 0; }; // class T : public Entity {}; @@ -58,10 +58,10 @@ class CfsScheduler { UpdateIterator() : current_(run_queue_.first()) , ignored_(nullptr) {} - UpdateIterator(const UpdateIterator&) = delete; + UpdateIterator(const UpdateIterator&) = delete; UpdateIterator& operator=(const UpdateIterator&) = delete; - UpdateIterator(UpdateIterator&&) = default; - UpdateIterator& operator=(UpdateIterator&&) = default; + UpdateIterator(UpdateIterator&&) = default; + UpdateIterator& operator=(UpdateIterator&&) = default; T* get() const { // NOLINTNEXTLINE(cppcoreguidelines-pro-type-static-cast-downcast) @@ -74,12 +74,12 @@ class CfsScheduler { auto update() { min_vruntime_ = current_->vruntime_; - int shift = 65536 - current_->priority_; + int shift = 65536 - current_->priority_; current_->vruntime_ += shift; run_queue_.erase(*current_); auto result = get()->update(); - current_ = ignored_ ? ignored_->next() : run_queue_.first(); + current_ = ignored_ ? ignored_->next() : run_queue_.first(); return result; } diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/red_black_tree.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/red_black_tree.hpp index b95766a93..ff7bed627 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/red_black_tree.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/red_black_tree.hpp @@ -181,7 +181,7 @@ class BasicRedBlackTree { */ tmp->set_parent_and_color(gparent, Color::BLACK); parent->set_parent_and_color(gparent, Color::BLACK); - node = gparent; + node = gparent; parent = node->parent(); node->set_parent_and_color(parent, Color::RED); continue; @@ -209,7 +209,7 @@ class BasicRedBlackTree { tmp->set_parent_and_color(parent, Color::BLACK); parent->set_parent_and_color(node, Color::RED); parent = node; - tmp = node->right; + tmp = node->right; } /* @@ -234,7 +234,7 @@ class BasicRedBlackTree { /* Case 1 - color flips */ tmp->set_parent_and_color(gparent, Color::BLACK); parent->set_parent_and_color(gparent, Color::BLACK); - node = gparent; + node = gparent; parent = node->parent(); node->set_parent_and_color(parent, Color::RED); continue; @@ -250,7 +250,7 @@ class BasicRedBlackTree { tmp->set_parent_and_color(parent, Color::BLACK); parent->set_parent_and_color(node, Color::RED); parent = node; - tmp = node->left; + tmp = node->left; } /* Case 3 - left rotate at gparent */ @@ -315,7 +315,7 @@ class BasicRedBlackTree { * - old gets assigned new as a parent and 'color' as a color. */ void rotate_set_parents(Node* old_node, Node* new_node, Color color) { - Node* parent = old_node->parent(); + Node* parent = old_node->parent(); new_node->parent_and_color = old_node->parent_and_color; old_node->set_parent_and_color(new_node, color); change_child(old_node, new_node, parent); @@ -323,7 +323,7 @@ class BasicRedBlackTree { Node* __erase(Node* node) { Node* child = node->right; - Node* tmp = node->left; + Node* tmp = node->left; Node *parent, *rebalance; uintptr_t pc; @@ -335,22 +335,22 @@ class BasicRedBlackTree { * and node must be black due to 4). We adjust colors locally * so as to bypass __rb_erase_color() later on. */ - pc = node->parent_and_color; + pc = node->parent_and_color; parent = ((Node*)(pc & ~3)); change_child(node, child, parent); if (child) { child->parent_and_color = pc; - rebalance = nullptr; + rebalance = nullptr; } else rebalance = ((pc) & 1) ? parent : nullptr; tmp = parent; } else if (!child) { /* Still case 1, but this time the child is node->rb_left */ tmp->parent_and_color = pc = node->parent_and_color; - parent = ((Node*)(pc & ~3)); + parent = ((Node*)(pc & ~3)); change_child(node, tmp, parent); rebalance = nullptr; - tmp = parent; + tmp = parent; } else { Node *successor = child, *child2; @@ -384,9 +384,9 @@ class BasicRedBlackTree { * (c) */ do { - parent = successor; + parent = successor; successor = tmp; - tmp = tmp->left; + tmp = tmp->left; } while (tmp); child2 = successor->right; WRITE_ONCE(parent->left, child2); @@ -398,7 +398,7 @@ class BasicRedBlackTree { WRITE_ONCE(successor->left, tmp); tmp->set_parent(successor); - pc = node->parent_and_color; + pc = node->parent_and_color; tmp = ((Node*)(pc & ~3)); change_child(node, successor, tmp); @@ -409,7 +409,7 @@ class BasicRedBlackTree { rebalance = successor->is_black() ? parent : nullptr; } successor->parent_and_color = pc; - tmp = successor; + tmp = successor; } return rebalance; @@ -468,7 +468,7 @@ class BasicRedBlackTree { if (parent->is_red()) parent->set_black(); else { - node = parent; + node = parent; parent = node->parent(); if (parent) continue; @@ -508,7 +508,7 @@ class BasicRedBlackTree { WRITE_ONCE(parent->right, tmp2); if (tmp1) tmp1->set_parent_and_color(sibling, Color::BLACK); - tmp1 = sibling; + tmp1 = sibling; sibling = tmp2; } /* @@ -551,7 +551,7 @@ class BasicRedBlackTree { if (parent->is_red()) parent->set_black(); else { - node = parent; + node = parent; parent = node->parent(); if (parent) continue; @@ -565,7 +565,7 @@ class BasicRedBlackTree { WRITE_ONCE(parent->left, tmp2); if (tmp1) tmp1->set_parent_and_color(sibling, Color::BLACK); - tmp1 = sibling; + tmp1 = sibling; sibling = tmp2; } /* Case 4 - right rotate at parent + color flips */ diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/remote_shape.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/remote_shape.hpp index 7fd0d8512..f23053145 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/remote_shape.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/remote_shape.hpp @@ -13,11 +13,11 @@ class RemoteShape { friend class RemoteShape; friend class RedBlackTree; - Descriptor() = default; - Descriptor(const Descriptor&) = delete; + Descriptor() = default; + Descriptor(const Descriptor&) = delete; Descriptor& operator=(const Descriptor&) = delete; - Descriptor(Descriptor&&) = delete; - Descriptor& operator=(Descriptor&& obj) = delete; + Descriptor(Descriptor&&) = delete; + Descriptor& operator=(Descriptor&& obj) = delete; [[nodiscard]] bool has_id() const { return id_; } [[nodiscard]] bool try_assign_id() @@ -79,9 +79,9 @@ class RemoteShape { private: /* Swap requirement: !this->id_ && victim.id_ */ void swap_id(Descriptor& victim) { - id_ = victim.id_; + id_ = victim.id_; assigned_list_[id_ - 1] = this; - existence_confidence_ = victim.existence_confidence_; + existence_confidence_ = victim.existence_confidence_; victim.revoke_id(); } @@ -94,7 +94,7 @@ class RemoteShape { } void revoke_id() { - id_ = 0; + id_ = 0; existence_confidence_ = 0; static_cast(this)->id_revoked(); @@ -104,7 +104,7 @@ class RemoteShape { return existence_confidence_ < obj.existence_confidence_; } - uint8_t id_ = 0; + uint8_t id_ = 0; uint8_t existence_confidence_ = 0; }; 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 e0ac73659..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 @@ -87,18 +87,26 @@ class Shape set_modified(); } + void set_xy(uint16_t x, uint16_t y) { + if (part2_.x == x && part2_.y == y) + return; + part2_.x = x; + part2_.y = y; + set_modified(); + } + bool is_text_shape() const { return is_text_shape_; } enum class Operation : uint8_t { NO_OPERATION = 0, - ADD = 1, - MODIFY = 2, - DELETE = 3, + ADD = 1, + MODIFY = 2, + DELETE = 3, }; Operation predict_update() const { uint8_t predict_existence = existence_confidence(); - uint8_t predict_sync = sync_confidence_; + uint8_t predict_sync = sync_confidence_; if (!has_id() && !predict_try_assign_id(predict_existence)) { return Operation::NO_OPERATION; @@ -120,34 +128,37 @@ class Shape constexpr static inline command::Field no_operation_description() { return command::Field{[](std::byte* buffer) { - auto& description = *new (buffer) DescriptionField{}; + auto& description = *new (buffer) DescriptionField{}; description.part1.operation_type = Operation::NO_OPERATION; return sizeof(DescriptionField); }}; } enum class Color : uint8_t { - SELF = 0, + SELF = 0, YELLOW = 1, - GREEN = 2, + GREEN = 2, ORANGE = 3, PURPLE = 4, - PINK = 5, - CYAN = 6, - BLACK = 7, - WHITE = 8, + PINK = 5, + CYAN = 6, + BLACK = 7, + WHITE = 8, }; protected: + explicit Shape(bool is_text_shape = false) + : is_text_shape_(is_text_shape) {} + enum class ShapeType : uint8_t { - LINE = 0, + LINE = 0, RECTANGLE = 1, - CIRCLE = 2, - ELLIPSE = 3, - ARC = 4, - FLOAT = 5, - INTEGER = 6, - TEXT = 7, + CIRCLE = 2, + ELLIPSE = 3, + ARC = 4, + FLOAT = 5, + INTEGER = 6, + TEXT = 7, }; struct DescriptionField { @@ -173,7 +184,7 @@ class Shape }; void set_modified() { - // Optimization: Assume the modification not exist when invisible. + // Optimization: Assume the modification does not exist when invisible. if (!visible_) return; @@ -187,7 +198,7 @@ class Shape private: void enter_run_queue() { - uint8_t min_confidence = std::min(existence_confidence(), sync_confidence_); + uint8_t min_confidence = std::min(existence_confidence(), sync_confidence_); uint16_t weighted_priority = (priority_ - 256) << (4 * min_confidence); CfsScheduler::Entity::enter_run_queue(weighted_priority); } @@ -200,7 +211,7 @@ class Shape // Re-enter the update queue to try to get a new id. set_modified(); } else { - // Leave run_queue when shape was hidden. + // Leave run_queue when the shape is hidden. leave_run_queue(); } } @@ -210,10 +221,9 @@ class Shape // Called by CfsScheduler. if (!has_id() && !try_assign_id()) { - // TODO: Print error message. sync_confidence_ = max_update_times; - visible_ = false; - // Do nothing when failed + visible_ = false; + // Do nothing when the update fails. return no_operation_description(); } @@ -224,14 +234,14 @@ class Shape command::Field field; - // Optimization1: Stop adding when shape is invisible. - // Optimization2: Prevent continuous modification. + // Optimization 1: Stop adding when the shape is invisible. + // Optimization 2: Prevent continuous modification. if (visible_ && (existence_confidence() <= sync_confidence_ || (last_time_modified_ && existence_confidence() < max_update_times))) { // Send add packet last_time_modified_ = false; - field = command::Field{[this](std::byte* buffer) { + field = command::Field{[this](std::byte* buffer) { return write_full_description_field(buffer, Operation::ADD); }}; if (increase_existence_confidence() < max_update_times @@ -240,7 +250,7 @@ class Shape } else { // Send modify packet last_time_modified_ = true; - field = command::Field{[this](std::byte* buffer) { + field = command::Field{[this](std::byte* buffer) { return write_full_description_field(buffer, Operation::MODIFY); }}; // No need to compare existence_confidence here. @@ -275,11 +285,11 @@ class Shape auto& description = *new (buffer) DescriptionField{}; description.part1.shape_type = ShapeType::LINE; - description.part1.color = Color::WHITE; + description.part1.color = Color::WHITE; description.part2.width = 0; - description.part2.x = 0; - description.part2.y = 0; + description.part2.x = 0; + description.part2.y = 0; description.part3.details_c = 0; description.part3.details_d = 0; @@ -290,7 +300,7 @@ class Shape static constexpr uint8_t max_update_times = 4; - uint8_t priority_ = 15; + uint8_t priority_ = 15; uint8_t sync_confidence_ : 5 = max_update_times; bool is_text_shape_ : 1 = false; bool last_time_modified_ : 1 = false; @@ -305,10 +315,10 @@ class Line : public Shape { bool visible = true) { part3_.color = color; part2_.width = width; - part2_.x = x; - part2_.y = y; - part3_.x2 = x2; - part3_.y2 = y2; + part2_.x = x; + part2_.y = y; + part3_.x2 = x2; + part3_.y2 = y2; set_visible(visible); } @@ -342,7 +352,7 @@ class Line : public Shape { auto& description = *new (buffer) DescriptionField{}; description.part1.shape_type = ShapeType::LINE; - description.part1.color = part3_.color; + description.part1.color = part3_.color; description.part2 = part2_; @@ -370,10 +380,10 @@ class Circle : public Shape { bool visible = true) { part3_.color = color; part2_.width = width; - part2_.x = x; - part2_.y = y; - part3_.rx = rx; - part3_.ry = ry; + part2_.x = x; + part2_.y = y; + part3_.rx = rx; + part3_.ry = ry; set_visible(visible); } @@ -412,7 +422,7 @@ class Circle : public Shape { auto& description = *new (buffer) DescriptionField{}; description.part1.shape_type = ShapeType::ELLIPSE; - description.part1.color = part3_.color; + description.part1.color = part3_.color; description.part2 = part2_; @@ -438,10 +448,10 @@ class Rectangle : public Shape { bool visible = true) { part3_.color = color; part2_.width = width; - part2_.x = x; - part2_.y = y; - part3_.x2 = x2; - part3_.y2 = y2; + part2_.x = x; + part2_.y = y; + part3_.x2 = x2; + part3_.y2 = y2; set_visible(visible); } @@ -475,7 +485,7 @@ class Rectangle : public Shape { auto& description = *new (buffer) DescriptionField{}; description.part1.shape_type = ShapeType::RECTANGLE; - description.part1.color = part3_.color; + description.part1.color = part3_.color; description.part2 = part2_; @@ -497,18 +507,17 @@ class Arc : public Shape { public: Arc() = default; Arc(Color color, uint16_t width, uint16_t x, uint16_t y, uint16_t angle_start, - uint16_t angle_end, uint16_t rx, uint16_t ry, bool visible = true) - : Arc() { + uint16_t angle_end, uint16_t rx, uint16_t ry, bool visible = true) { angle_start_ = angle_start; - angle_end_ = angle_end; + angle_end_ = angle_end; part2_.width = width; - part2_.x = x; - part2_.y = y; + part2_.x = x; + part2_.y = y; part3_.color = color; - part3_.rx = rx; - part3_.ry = ry; + part3_.rx = rx; + part3_.ry = ry; set_visible(visible); } @@ -577,9 +586,9 @@ class Arc : public Shape { auto& description = *new (buffer) DescriptionField{}; description.part1.shape_type = ShapeType::ARC; - description.part1.color = part3_.color; - description.part1.details_a = angle_start_; - description.part1.details_b = angle_end_; + description.part1.color = part3_.color; + description.part1.details_a = angle_start_; + description.part1.details_b = angle_end_; description.part2 = part2_; @@ -604,14 +613,13 @@ class Integer : public Shape { Integer() = default; Integer( Color color, uint16_t font_size, uint16_t width, uint16_t x, uint16_t y, int32_t value, - bool visible = true) - : Integer() { - color_ = color; + bool visible = true) { + color_ = color; font_size_ = font_size; part2_.width = width; - part2_.x = x; - part2_.y = y; + part2_.x = x; + part2_.y = y; value_ = value; @@ -627,7 +635,7 @@ class Integer : public Shape { } void set_center_x(uint16_t x) { - int value = value_; + int value = value_; int number_of_digits = value <= 0 ? 1 : 0; for (; value != 0; number_of_digits++) value /= 10; @@ -656,8 +664,8 @@ class Integer : public Shape { auto& description = *new (buffer) DescriptionField{}; description.part1.shape_type = ShapeType::INTEGER; - description.part1.color = color_; - description.part1.details_a = font_size_; + description.part1.color = color_; + description.part1.details_a = font_size_; description.part2 = part2_; @@ -676,7 +684,7 @@ class Float : public Integer { using Integer::Integer; void set_center_x(uint16_t x) { - int value = value_; + int value = value_; int number_of_digits = value < 0 ? 1 : 0; int integer_part = value / 1000; @@ -701,8 +709,8 @@ class Float : public Integer { auto& description = *new (buffer) DescriptionField{}; description.part1.shape_type = ShapeType::FLOAT; - description.part1.color = color_; - description.part1.details_a = font_size_; + description.part1.color = color_; + description.part1.details_a = font_size_; description.part2 = part2_; @@ -714,17 +722,18 @@ class Float : public Integer { class Text : public Shape { public: - Text() { value_ = nullptr; }; + Text() + : Shape(true) {} Text( Color color, uint16_t font_size, uint16_t width, uint16_t x, uint16_t y, const char* value, bool visible = true) : Text() { - color_ = color; + color_ = color; font_size_ = font_size; part2_.width = width; - part2_.x = x; - part2_.y = y; + part2_.x = x; + part2_.y = y; value_ = value; @@ -741,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(); } @@ -760,14 +767,14 @@ class Text : public Shape { auto& description = *new (buffer) DescriptionField{}; description.part1.shape_type = ShapeType::TEXT; - description.part1.color = color_; - description.part1.details_a = font_size_; + description.part1.color = color_; + description.part1.details_a = font_size_; description.part2 = part2_; constexpr size_t data_part_size = 30; - auto str_length = std::min(strlen(value_), data_part_size); - description.part1.details_b = str_length; + auto str_length = std::min(strlen(value_), data_part_size); + description.part1.details_b = str_length; std::memcpy(buffer + sizeof(DescriptionField), value_, str_length); return sizeof(DescriptionField) + data_part_size; @@ -775,8 +782,8 @@ class Text : public Shape { uint16_t font_size_; Color color_; - const char* value_; + const char* value_ = nullptr; }; } // namespace app::ui -} // namespace rmcs_core::referee \ No newline at end of file +} // namespace rmcs_core::referee 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 21526b9b5..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 @@ -10,13 +10,17 @@ class CrossHairCircle { : CrossHairCircle(color, x, y, 12, 2, visible) {} CrossHairCircle( - Shape::Color color, uint16_t x, uint16_t y, uint16_t r, uint16_t width, - bool visible = true) + Shape::Color color, uint16_t x, uint16_t y, uint16_t r, uint16_t width, bool visible = true) : 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/deformable_chassis_top_view.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp index e5ea754be..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; @@ -67,15 +68,13 @@ class DeformableChassisLegArcs { static constexpr double default_min_angle_deg_ = 8.0; static constexpr double default_max_angle_deg_ = 58.0; - static constexpr double deg_to_rad_(double degrees) { - return degrees * degrees_to_radians_; - } + static constexpr double deg_to_rad_(double degrees) { return degrees * degrees_to_radians_; } static constexpr std::array leg_base_mid_angles_ = { front_pair_offset_deg_ * degrees_to_radians_, - std::numbers::pi_v - rear_pair_offset_deg_ * degrees_to_radians_, - std::numbers::pi_v + rear_pair_offset_deg_ * degrees_to_radians_, - -front_pair_offset_deg_ * degrees_to_radians_, + std::numbers::pi_v - rear_pair_offset_deg_* degrees_to_radians_, + std::numbers::pi_v + rear_pair_offset_deg_* degrees_to_radians_, + -front_pair_offset_deg_* degrees_to_radians_, }; static constexpr std::array leg_radii_near_ = { @@ -93,9 +92,8 @@ class DeformableChassisLegArcs { }; static uint16_t to_referee_angle_(double angle) { - const auto degrees = - static_cast(std::lround((2.0 * std::numbers::pi_v - angle) - / std::numbers::pi_v * 180.0)); + const auto degrees = static_cast(std::lround( + (2.0 * std::numbers::pi_v - angle) / std::numbers::pi_v * 180.0)); int wrapped = degrees % 360; if (wrapped < 0) wrapped += 360; @@ -109,33 +107,28 @@ 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(std::lround( - far_radius - normalized_extension * (far_radius - near_radius))); + const uint16_t radius = static_cast( + std::lround(far_radius - normalized_extension * (far_radius - near_radius))); const uint16_t half_angle = static_cast(std::lround( prone_leg_half_angle_ - normalized_extension * (prone_leg_half_angle_ - upright_leg_half_angle_))); const uint16_t width = static_cast(std::lround( - prone_leg_width_ - + normalized_extension * (upright_leg_width_ - prone_leg_width_))); + prone_leg_width_ + normalized_extension * (upright_leg_width_ - prone_leg_width_))); leg.set_x(center_x_); 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); } diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/rangefinder.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/rangefinder.hpp index f3361b587..efb9f766a 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/rangefinder.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/rangefinder.hpp @@ -151,16 +151,16 @@ class Rangefinder { } private: - constexpr static uint16_t x_center_ = 960; - constexpr static uint16_t y_center_ = 540; + constexpr static uint16_t x_center_ = 960; + constexpr static uint16_t y_center_ = 540; constexpr static uint16_t x_side_center_ = x_center_ - 110; - constexpr static uint16_t horizontal_unit_scale = 15; + constexpr static uint16_t horizontal_unit_scale = 15; constexpr static uint16_t horizontal_scale_accuracy_ = 40; - constexpr static uint16_t vertical_unit_scale_ = 5; + constexpr static uint16_t vertical_unit_scale_ = 5; constexpr static uint16_t vertical_side_unit_scale_ = 15; - constexpr static uint16_t vertical_scale_accuracy_ = 26; + constexpr static uint16_t vertical_scale_accuracy_ = 26; Float pitch_angle_{Shape::Color::BLACK, 17, 2, x_center_ - 100, y_center_ + 170, 0, false}; 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 c00e5f59e..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 @@ -1,11 +1,10 @@ #pragma once #include +#include #include #include #include - -#include #include #include @@ -17,7 +16,11 @@ namespace rmcs_core::referee::app::ui { class StatusRing { public: StatusRing( - double supercap_limit, double battery_limit, double friction_limit, int16_t bullet_limit) { + double supercap_limit, double battery_limit, double friction_limit, int16_t bullet_limit, + bool supercap = true, bool battery = true) + : supercap_ui_{supercap} + , battery_ui_{battery} { + supercap_status_.set_x(x_center); supercap_status_.set_y(y_center); supercap_status_.set_r(visible_radius - width_ring); @@ -25,7 +28,7 @@ class StatusRing { supercap_status_.set_angle_end(275 + visible_angle); supercap_status_.set_width(width_ring); supercap_status_.set_color(Shape::Color::PINK); - supercap_status_.set_visible(true); + supercap_status_.set_visible(supercap); battery_status_.set_x(x_center); battery_status_.set_y(y_center); @@ -34,7 +37,7 @@ class StatusRing { battery_status_.set_angle_end(265); battery_status_.set_width(width_ring); battery_status_.set_color(Shape::Color::PINK); - battery_status_.set_visible(true); + battery_status_.set_visible(battery); friction_wheel_speed_.set_x(x_center); friction_wheel_speed_.set_y(y_center); @@ -112,8 +115,8 @@ class StatusRing { void set_visible(bool value) { // Dynamic - supercap_status_.set_visible(value); - battery_status_.set_visible(value); + supercap_status_.set_visible(value && supercap_ui_); + battery_status_.set_visible(value && battery_ui_); friction_wheel_speed_.set_visible(value); bullet_status_.set_visible(value); @@ -133,7 +136,7 @@ class StatusRing { void update_static_parts(std::tuple enable) { auto& [auto_aim_enable, precise_enable] = enable; - auto static_enable = auto_aim_enable || precise_enable; + auto static_enable = auto_aim_enable || precise_enable; static auto color{Shape::Color::WHITE}; @@ -246,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 > 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 > 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); @@ -305,12 +321,20 @@ 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; - battery_limit_ = battery_limit; + battery_limit_ = battery_limit; friction_limit_ = friction_limit; - bullet_limit_ = bullet_limit; + bullet_limit_ = bullet_limit; int scale_angle = 5; for (auto& bullet_scale : bullet_scales_) { @@ -328,13 +352,13 @@ class StatusRing { } double value = 0; - scale_angle = 5; + scale_angle = 5; for (auto& number : bullet_scales_number_) { scale_angle += (visible_angle) / 4; value += static_cast(bullet_limit_) / 4; - const auto r = visible_radius - width_ring + 30; + const auto r = visible_radius - width_ring + 30; const auto angle = static_cast(-scale_angle) * std::numbers::pi / 180; number.set_x(x_center + static_cast(r * std::cos(angle))); @@ -347,17 +371,20 @@ class StatusRing { } } - constexpr static uint16_t x_center = 960; - constexpr static uint16_t y_center = 540; - constexpr static uint16_t width_ring = 15; + constexpr static uint16_t x_center = 960; + constexpr static uint16_t y_center = 540; + constexpr static uint16_t width_ring = 15; constexpr static uint16_t visible_radius = 400; - constexpr static uint16_t visible_angle = 40; + constexpr static uint16_t visible_angle = 40; double supercap_limit_; double battery_limit_; double friction_limit_; int16_t bullet_limit_; + bool supercap_ui_ = true; + bool battery_ui_ = true; + // Dynamic part Arc supercap_status_; @@ -378,4 +405,4 @@ class StatusRing { Arc bullet_scales_[4]; }; -} // namespace rmcs_core::referee::app::ui \ No newline at end of file +} // namespace rmcs_core::referee::app::ui diff --git a/rmcs_ws/src/rmcs_core/src/referee/command.cpp b/rmcs_ws/src/rmcs_core/src/referee/command.cpp index 95326d349..34c55849d 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/command.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/command.cpp @@ -16,7 +16,9 @@ class Command , public rclcpp::Node { public: Command() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , next_sent_(std::chrono::steady_clock::time_point::min()) , interaction_next_sent_(std::chrono::steady_clock::time_point::min()) , map_marker_next_sent_(std::chrono::steady_clock::time_point::min()) 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/command/interaction/ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp index dc1e32717..57167144b 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp @@ -45,7 +45,7 @@ class Ui resetting_ = 4; } last_game_stage_ = *game_stage_; - last_keyboard_ = *keyboard_; + last_keyboard_ = *keyboard_; if (resetting_) { *ui_field_ = Field{[this](std::byte* buffer) { @@ -67,10 +67,10 @@ class Ui size_t write_resetting_field(std::byte* buffer) const { size_t written = 0; - auto& header = *new (buffer + written) Header{}; - header.command_id = 0x0100; // Clear shapes + auto& header = *new (buffer + written) Header{}; + header.command_id = 0x0100; // Clear shapes auto full_robot_id = rmcs_msgs::FullRobotId{*robot_id_}; - header.sender_id = full_robot_id; + header.sender_id = full_robot_id; header.receiver_id = full_robot_id.client(); written += sizeof(Header); @@ -79,7 +79,7 @@ class Ui uint8_t layer; }; auto& command = *new (buffer + written) Command{}; - command.type = 2; // Clear all layers + command.type = 2; // Clear all layers command.layer = 0; written += sizeof(Command); @@ -89,9 +89,9 @@ class Ui size_t write_updating_field(std::byte* buffer) const { size_t written = 0; - auto& header = *new (buffer + written) Header{}; + auto& header = *new (buffer + written) Header{}; auto full_robot_id = rmcs_msgs::FullRobotId{*robot_id_}; - header.sender_id = full_robot_id; + header.sender_id = full_robot_id; header.receiver_id = full_robot_id.client(); written += sizeof(Header); @@ -165,4 +165,4 @@ class Ui #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::command::interaction::Ui, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::command::interaction::Ui, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/frame.hpp b/rmcs_ws/src/rmcs_core/src/referee/frame.hpp index d8a406e47..4d719adda 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/frame.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/frame.hpp @@ -5,7 +5,7 @@ namespace rmcs_core::referee { -constexpr uint8_t sof_value = 0xa5; +constexpr uint8_t sof_value = 0xa5; constexpr size_t frame_data_max_length = 1024; struct __attribute__((packed)) FrameHeader { diff --git a/rmcs_ws/src/rmcs_core/src/referee/status.cpp b/rmcs_ws/src/rmcs_core/src/referee/status.cpp index c080a355c..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( @@ -284,7 +310,8 @@ class Status *map_command_received_timestamp_ = std::chrono::duration(now.time_since_epoch()).count(); - if (has_last_map_command_ && std::memcmp(&last_map_command_, &data, sizeof(data)) == 0) { + if (has_last_map_command_ + && std::memcmp(&last_map_command_, &data, sizeof(data)) == 0) { // NOLINT return; } @@ -328,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_; @@ -339,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 de3911afc..ad5e21619 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp @@ -5,7 +5,7 @@ namespace rmcs_core::referee::status { struct __attribute__((packed)) GameStatus { - uint8_t game_type : 4; + uint8_t game_type : 4; uint8_t game_progress : 4; uint16_t stage_remain_time; uint64_t sync_timestamp; @@ -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 diff --git a/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp b/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp index 2fc9d17be..1ac5d4eef 100644 --- a/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp +++ b/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp @@ -66,7 +66,7 @@ struct RightFrontWheelLink : fast_tf::Link { template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; @@ -114,37 +114,37 @@ struct fast_tf::Joint : fast_tf::ModificationTracka template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BottomYawLink; + using Parent = rmcs_description::BottomYawLink; Eigen::Quaterniond transform = Eigen::Quaterniond::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Quaterniond transform = Eigen::Quaterniond::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4, Eigen::Vector3d::UnitZ()} @@ -155,7 +155,7 @@ struct fast_tf::Joint : fast_tf::Modificat template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} @@ -166,7 +166,7 @@ struct fast_tf::Joint : fast_tf::Modificati template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} @@ -177,7 +177,7 @@ struct fast_tf::Joint : fast_tf::Modificat template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4, Eigen::Vector3d::UnitZ()} diff --git a/rmcs_ws/src/rmcs_description/include/rmcs_description/tf_description.hpp b/rmcs_ws/src/rmcs_description/include/rmcs_description/tf_description.hpp index 650987f58..4a46051ca 100644 --- a/rmcs_ws/src/rmcs_description/include/rmcs_description/tf_description.hpp +++ b/rmcs_ws/src/rmcs_description/include/rmcs_description/tf_description.hpp @@ -73,7 +73,7 @@ struct OmniLinkRight : fast_tf::Link { template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; @@ -101,25 +101,25 @@ struct fast_tf::Joint : fast_tf::ModificationTracka template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Quaterniond transform = Eigen::Quaterniond::Identity(); }; @@ -135,7 +135,7 @@ struct fast_tf::Joint : fast_tf::ModificationTrack }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4, Eigen::Vector3d::UnitZ()} @@ -146,7 +146,7 @@ struct fast_tf::Joint : fast_tf::Modificat template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} @@ -157,7 +157,7 @@ struct fast_tf::Joint : fast_tf::Modificati template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} @@ -168,7 +168,7 @@ struct fast_tf::Joint : fast_tf::Modificat template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4, Eigen::Vector3d::UnitZ()} diff --git a/rmcs_ws/src/rmcs_executor/CMakeLists.txt b/rmcs_ws/src/rmcs_executor/CMakeLists.txt index ec21edbfb..8f255f45c 100644 --- a/rmcs_ws/src/rmcs_executor/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_executor/CMakeLists.txt @@ -3,7 +3,8 @@ project(rmcs_executor) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c11") -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++20") +set(CMAKE_CXX_STANDARD 23) +set(CMAKE_CXX_STANDARD_REQUIRED ON) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-g1 -rdynamic -O2 -Wall -Wextra -Wpedantic) diff --git a/rmcs_ws/src/rmcs_executor/include/rmcs_executor/component.hpp b/rmcs_ws/src/rmcs_executor/include/rmcs_executor/component.hpp index bfb102888..bd9ed067d 100644 --- a/rmcs_ws/src/rmcs_executor/include/rmcs_executor/component.hpp +++ b/rmcs_ws/src/rmcs_executor/include/rmcs_executor/component.hpp @@ -1,22 +1,56 @@ #pragma once +#include #include +#include +#include +#include #include #include +#include #include #include #include +#include #include #include #include +#include #include +#include +#include + namespace rmcs_executor { +enum class InterfaceKind { + Normal, + Event, +}; + +inline const char* interface_kind_name(InterfaceKind kind) { + switch (kind) { + case InterfaceKind::Normal: return "Normal"; + case InterfaceKind::Event: return "Event"; + } + + return "Unknown"; +} + +using EventState = std::uint32_t; +constexpr EventState EVENT_DISABLED_BIT = EventState{1} << 31; +constexpr EventState EVENT_ACTIVE_MASK = ~EVENT_DISABLED_BIT; + class Component { public: friend class Executor; + struct OutputInfo { + std::reference_wrapper type; + InterfaceKind kind; + }; + using OutputInfoMap = std::map; + Component(const Component&) = delete; Component& operator=(const Component&) = delete; Component(Component&&) = delete; @@ -24,9 +58,7 @@ class Component { virtual ~Component() = default; - virtual void before_pairing(const std::map& output_map) { - (void)output_map; - } + virtual void before_pairing(const OutputInfoMap& output_map) { (void)output_map; } virtual void before_updating() {} virtual void update() = 0; @@ -79,9 +111,9 @@ class Component { const T& operator*() const { return *data_pointer_; } private: - void** activate() { + void* activate() { activated = true; - return reinterpret_cast(&data_pointer_); + return reinterpret_cast(&data_pointer_); } T* data_pointer_ = nullptr; @@ -90,6 +122,139 @@ class Component { bool delete_data_when_deconstruct = false; }; + template + requires(!std::is_reference_v && !std::is_unbounded_array_v) class EventInputInterface { + public: + friend class Component; + + template + requires std::invocable + explicit EventInputInterface(Callback&& callback) + : callback_(std::forward(callback)) {} + + EventInputInterface(const EventInputInterface&) = delete; + EventInputInterface& operator=(const EventInputInterface&) = delete; + EventInputInterface(EventInputInterface&&) = delete; + EventInputInterface& operator=(EventInputInterface&&) = delete; + + [[nodiscard]] bool active() const { return activated; } + [[nodiscard]] bool ready() const { return static_cast(callback_); } + + private: + void* activate() { + if (!ready()) + throw std::runtime_error( + "The event input interface requires a callback before registration"); + + activated = true; + return reinterpret_cast(&callback_); + } + + std::function callback_; + bool activated = false; + }; + + template + requires( + !std::is_reference_v && !std::is_unbounded_array_v + && std::is_nothrow_copy_constructible_v && std::is_nothrow_destructible_v) + class QueuedEventInputInterface final : public EventInputInterface { + public: + template + requires std::invocable + QueuedEventInputInterface(size_t queue_depth, Callback&& callback) + : EventInputInterface([this](const T& event) { enqueue(event); }) + , user_callback_(std::forward(callback)) + , queue_(queue_depth) + , worker_(&QueuedEventInputInterface::worker_main, this) {} + + ~QueuedEventInputInterface() { + stop_requested_.store(true, std::memory_order::relaxed); + notify_event(); + if (worker_.joinable()) + worker_.join(); + } + + QueuedEventInputInterface(const QueuedEventInputInterface&) = delete; + QueuedEventInputInterface& operator=(const QueuedEventInputInterface&) = delete; + QueuedEventInputInterface(QueuedEventInputInterface&&) = delete; + QueuedEventInputInterface& operator=(QueuedEventInputInterface&&) = delete; + + private: + void enqueue(const T& event) { + if (stop_requested_.load(std::memory_order::relaxed)) + return; + + auto guard = std::scoped_lock{enqueue_mutex_}; + if (stop_requested_.load(std::memory_order::relaxed)) + return; + + if (!queue_.push_back(event)) { + const auto dropped_count = dropped_event_count_++; + if (dropped_count == 0) { + RCLCPP_WARN( + rclcpp::get_logger("rmcs_executor"), + "QueuedEventInputInterface started dropping events because the queue is " + "full"); + } + return; + } + + const auto dropped_count = dropped_event_count_; + dropped_event_count_ = 0; + if (dropped_count != 0) { + RCLCPP_WARN( + rclcpp::get_logger("rmcs_executor"), + "QueuedEventInputInterface resumed enqueueing after dropping %u events", + dropped_count); + } + + notify_event(); + } + + void notify_event() { + event_count_.fetch_add(1, std::memory_order::release); + event_count_.notify_one(); + } + + void worker_main() { + while (!stop_requested_.load(std::memory_order::relaxed)) { + if (auto* event = queue_.peek_front()) { + try { + user_callback_(std::move(*event)); + } catch (const std::exception& exception) { + RCLCPP_ERROR( + rclcpp::get_logger("rmcs_executor"), + "QueuedEventInputInterface worker terminated by exception: %s", + exception.what()); + return; + } catch (...) { + RCLCPP_ERROR( + rclcpp::get_logger("rmcs_executor"), + "QueuedEventInputInterface worker terminated by unknown exception"); + return; + } + + if (!queue_.pop_front([](T&&) noexcept {})) + std::terminate(); + continue; + } + + const auto old = event_count_.load(std::memory_order::relaxed); + if (!queue_.readable() && !stop_requested_.load(std::memory_order::relaxed)) + event_count_.wait(old, std::memory_order::acquire); + } + } + + std::function user_callback_; + rmcs_utility::RingBuffer queue_; + std::atomic stop_requested_{false}; + std::uint32_t dropped_event_count_ = 0; + std::atomic event_count_{0}; + std::mutex enqueue_mutex_; + std::thread worker_; + }; + template requires(!std::is_reference_v && !std::is_unbounded_array_v) class OutputInterface { public: @@ -104,25 +269,111 @@ class Component { ~OutputInterface() { if (active()) - std::destroy_at(std::launder(reinterpret_cast(&data_))); + std::destroy_at(storage_pointer()); }; [[nodiscard]] bool active() const { return activated; } - T* operator->() { return reinterpret_cast(&data_); } - const T* operator->() const { return reinterpret_cast(&data_); } - T& operator*() { return *reinterpret_cast(&data_); } - const T& operator*() const { return *reinterpret_cast(&data_); } + T* operator->() { return storage_pointer(); } + const T* operator->() const { return storage_pointer(); } + T& operator*() { return *storage_pointer(); } + const T& operator*() const { return *storage_pointer(); } private: template void* activate(Args&&... args) { - ::new (&data_) T(std::forward(args)...); + std::construct_at(raw_storage_pointer(), std::forward(args)...); + activated = true; + return data_; + } + + [[nodiscard]] T* raw_storage_pointer() { return reinterpret_cast(data_); } + + [[nodiscard]] T* storage_pointer() { return std::launder(raw_storage_pointer()); } + + [[nodiscard]] const T* storage_pointer() const { + return std::launder(reinterpret_cast(data_)); + } + + alignas(T) std::byte data_[sizeof(T)]; + bool activated = false; + }; + + template + requires(!std::is_reference_v && !std::is_unbounded_array_v) class EventOutputInterface { + public: + friend class Component; + + EventOutputInterface() = default; + + EventOutputInterface(const EventOutputInterface&) = delete; + EventOutputInterface& operator=(const EventOutputInterface&) = delete; + EventOutputInterface(EventOutputInterface&&) = delete; + EventOutputInterface& operator=(EventOutputInterface&&) = delete; + + [[nodiscard]] bool active() const { return activated; } + + void emit(const T& event) { + if (!active()) + throw std::runtime_error("The event output interface has not been activated"); + if (!try_enter_emit()) + return; + + struct LeaveEmitGuard { + EventOutputInterface& interface; + + ~LeaveEmitGuard() { interface.leave_emit(); } + } leave_emit_guard{*this}; + + for (const auto* callback : callback_list_) + (*callback)(event); + } + + private: + using EventCallback = std::function; + + void* activate() { activated = true; - return reinterpret_cast(&data_); + return this; } - std::aligned_storage_t data_; + void add_listener(EventCallback* callback) { callback_list_.emplace_back(callback); } + + bool try_enter_emit() { + auto state = state_.load(std::memory_order_relaxed); + while (true) { + if (state & EVENT_DISABLED_BIT) + return false; + if ((state & EVENT_ACTIVE_MASK) == EVENT_ACTIVE_MASK) + throw std::runtime_error("Too many active event emissions"); + + if (state_.compare_exchange_weak( + state, state + 1, std::memory_order_relaxed, std::memory_order_relaxed)) + return true; + } + } + + void leave_emit() { + const auto previous_state = state_.fetch_sub(1, std::memory_order_release); + const auto new_state = previous_state - 1; + if ((new_state & EVENT_DISABLED_BIT) != 0 && (new_state & EVENT_ACTIVE_MASK) == 0) + state_.notify_one(); + } + + void disable() { state_.fetch_or(EVENT_DISABLED_BIT, std::memory_order_relaxed); } + + void enable() { state_.fetch_and(EVENT_ACTIVE_MASK, std::memory_order_relaxed); } + + void wait_idle() { + auto state = state_.load(std::memory_order_acquire); + while ((state & EVENT_ACTIVE_MASK) != 0) { + state_.wait(state, std::memory_order_acquire); + state = state_.load(std::memory_order_acquire); + } + } + + std::vector callback_list_; + std::atomic state_{EVENT_DISABLED_BIT}; bool activated = false; }; @@ -133,7 +384,25 @@ class Component { const std::string& name, InputInterface& interface, bool required = true) { if (interface.active()) throw std::runtime_error("The interface has been activated"); - input_list_.emplace_back(typeid(T), name, required, interface.activate()); + + ensure_registration_name_is_available( + name, InterfaceKind::Normal, RegistrationDirection::Input); + input_list_.emplace_back( + typeid(T), name, InterfaceKind::Normal, required, interface.activate(), + &bind_input_interface); + } + + template + void register_input( + const std::string& name, EventInputInterface& interface, bool required = true) { + if (interface.active()) + throw std::runtime_error("The interface has been activated"); + + ensure_registration_name_is_available( + name, InterfaceKind::Event, RegistrationDirection::Input); + input_list_.emplace_back( + typeid(T), name, InterfaceKind::Event, required, interface.activate(), + &bind_event_input_interface); } template @@ -141,14 +410,31 @@ class Component { void register_output(const std::string& name, OutputInterface& interface, Args&&... args) { if (interface.active()) throw std::runtime_error("The interface has been activated"); + + ensure_registration_name_is_available( + name, InterfaceKind::Normal, RegistrationDirection::Output); output_list_.emplace_back( - typeid(T), name, interface.activate(std::forward(args)...), this); + typeid(T), name, InterfaceKind::Normal, interface.activate(std::forward(args)...), + nullptr, nullptr, nullptr, this); + } + + template + void register_output(const std::string& name, EventOutputInterface& interface) { + if (interface.active()) + throw std::runtime_error("The interface has been activated"); + + ensure_registration_name_is_available( + name, InterfaceKind::Event, RegistrationDirection::Output); + output_list_.emplace_back( + typeid(T), name, InterfaceKind::Event, interface.activate(), + &enable_event_output_interface, &disable_event_output_interface, + &wait_event_output_interface_idle, this); } template requires std::constructible_from std::shared_ptr create_partner_component(const std::string& name, Args&&... args) { - initializing_component_name = name.c_str(); + initializing_component_name = name; auto component = std::make_shared(std::forward(args)...); partner_component_list_.emplace_back(component); @@ -156,26 +442,104 @@ class Component { return component; } - static const char* initializing_component_name; + static std::string initializing_component_name; protected: Component() : component_name_(initializing_component_name) {} private: + enum class RegistrationDirection { + Input, + Output, + }; + + using BindFunction = void (*)(void* input_binding, void* output_binding); + using OutputLifecycleHook = void (*)(void* output_binding); + + static const char* registration_direction_name(RegistrationDirection direction) { + switch (direction) { + case RegistrationDirection::Input: return "input"; + case RegistrationDirection::Output: return "output"; + } + + return "interface"; + } + + void ensure_registration_name_is_available( + const std::string& name, InterfaceKind kind, RegistrationDirection direction) const { + auto check_declarations = [&](const auto& declarations, + RegistrationDirection existing_direction) { + for (const auto& declaration : declarations) { + if (declaration.name != name) + continue; + + if (declaration.kind != kind) { + throw std::runtime_error( + "Component [" + component_name_ + "] cannot register " + + interface_kind_name(kind) + " " + registration_direction_name(direction) + + " \"" + name + "\" because \"" + name + + "\" has already been registered as " + + interface_kind_name(declaration.kind) + " " + + registration_direction_name(existing_direction) + " in this component."); + } + + throw std::runtime_error( + "Component [" + component_name_ + "] registered " + interface_kind_name(kind) + + " " + registration_direction_name(direction) + " \"" + name + + "\" more than once."); + } + }; + + check_declarations(input_list_, RegistrationDirection::Input); + check_declarations(output_list_, RegistrationDirection::Output); + } + + template + static void bind_input_interface(void* input_binding, void* output_binding) { + *reinterpret_cast(input_binding) = reinterpret_cast(output_binding); + } + + template + static void bind_event_input_interface(void* input_binding, void* output_binding) { + auto& callback = *reinterpret_cast*>(input_binding); + reinterpret_cast*>(output_binding)->add_listener(&callback); + } + + template + static void enable_event_output_interface(void* output_binding) { + reinterpret_cast*>(output_binding)->enable(); + } + + template + static void disable_event_output_interface(void* output_binding) { + reinterpret_cast*>(output_binding)->disable(); + } + + template + static void wait_event_output_interface_idle(void* output_binding) { + reinterpret_cast*>(output_binding)->wait_idle(); + } + std::string component_name_; struct InputDeclaration { const std::type_info& type; std::string name; + InterfaceKind kind; bool required; - void** pointer_to_data_pointer; + void* binding; + BindFunction bind; }; struct OutputDeclaration { const std::type_info& type; std::string name; - void* data_pointer; + InterfaceKind kind; + void* binding; + OutputLifecycleHook enable; + OutputLifecycleHook disable; + OutputLifecycleHook wait_idle; Component* component; }; diff --git a/rmcs_ws/src/rmcs_executor/package.xml b/rmcs_ws/src/rmcs_executor/package.xml index a287fb498..5ed7bcd65 100644 --- a/rmcs_ws/src/rmcs_executor/package.xml +++ b/rmcs_ws/src/rmcs_executor/package.xml @@ -11,6 +11,7 @@ rclcpp pluginlib + rmcs_utility ament_cmake diff --git a/rmcs_ws/src/rmcs_executor/src/component.cpp b/rmcs_ws/src/rmcs_executor/src/component.cpp index 561ac7a28..181ae975f 100644 --- a/rmcs_ws/src/rmcs_executor/src/component.cpp +++ b/rmcs_ws/src/rmcs_executor/src/component.cpp @@ -2,6 +2,6 @@ namespace rmcs_executor { -const char* Component::initializing_component_name; +std::string Component::initializing_component_name; -} // namespace rmcs_executor \ No newline at end of file +} // namespace rmcs_executor diff --git a/rmcs_ws/src/rmcs_executor/src/executor.hpp b/rmcs_ws/src/rmcs_executor/src/executor.hpp index a818d50a8..751d4dfdb 100644 --- a/rmcs_ws/src/rmcs_executor/src/executor.hpp +++ b/rmcs_ws/src/rmcs_executor/src/executor.hpp @@ -1,9 +1,19 @@ #pragma once +#include +#include +#include +#include +#include +#include #include #include +#include +#include #include #include +#include +#include #include #include @@ -14,6 +24,8 @@ #include "predefined_msg_provider.hpp" #include "rmcs_executor/component.hpp" +#include "rmcs_utility/tdigest.hpp" +#include "rmcs_utility/thread_config.hpp" namespace rmcs_executor { @@ -21,18 +33,22 @@ class Executor final : public rclcpp::Node { public: explicit Executor( const std::string& node_name, rclcpp::executors::SingleThreadedExecutor& rcl_executor) - : Node{node_name, rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)} + : Node{ + node_name, + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)} , rcl_executor_(rcl_executor) { Component::initializing_component_name = "predefined_msg_provider"; - predefined_msg_provider_ = std::make_shared(); + predefined_msg_provider_ = std::make_shared(); add_component(predefined_msg_provider_); } - ~Executor() { + ~Executor() override { + disable_event_outputs(); + wait_event_outputs_idle(); if (thread_.joinable()) thread_.join(); }; - void add_component(std::shared_ptr component) { + void add_component(const std::shared_ptr& component) { component_list_.emplace_back(component); if (auto node = std::dynamic_pointer_cast(component)) rcl_executor_.add_node(node); @@ -49,36 +65,458 @@ class Executor final : public rclcpp::Node { double update_rate; if (!get_parameter("update_rate", update_rate)) throw std::runtime_error{"Unable to get parameter update_rate"}; + if (!std::isfinite(update_rate) || update_rate <= 0.0) + throw std::runtime_error{"Parameter update_rate must be finite and positive"}; + const auto period = + std::chrono::nanoseconds(static_cast(std::round(1'000'000'000.0 / update_rate))); + if (period.count() <= 0) + throw std::runtime_error{ + "Parameter update_rate is too large to produce a valid period"}; predefined_msg_provider_->set_update_rate(update_rate); - thread_ = std::thread{[update_rate, this]() { - const auto period = std::chrono::nanoseconds( - static_cast(std::round(1'000'000'000.0 / update_rate))); - auto next_iteration_time = std::chrono::steady_clock::now(); + std::string thread_config_spec; + get_parameter_or("thread_config", thread_config_spec, ""); + auto thread_config = rmcs_utility::ThreadConfig{thread_config_spec, "update"}; + + enable_event_outputs(); + + thread_ = std::thread{[this, period, thread_config = std::move(thread_config)]() { + thread_main(period, thread_config); + }}; + } + +private: + using SteadyClock = std::chrono::steady_clock; + + static constexpr size_t digest_size_ = 256; + + struct CumulativeStats { + rmcs_utility::TDigest start_lateness_ms{digest_size_}; + rmcs_utility::TDigest update_duration_ms{digest_size_}; + uint64_t update_count = 0; + uint64_t skipped_count = 0; + uint64_t start_lateness_sample_count = 0; + uint64_t update_duration_sample_count = 0; + double start_lateness_max_ms = 0.0; + double update_duration_max_ms = 0.0; + }; + + struct WindowStats { + explicit WindowStats(size_t component_count) + : component_update_duration_sums(component_count) {} + + uint64_t executed_count = 0; + SteadyClock::duration total_update_duration{}; + std::vector component_update_duration_sums; + }; + + struct ThreadStats { + explicit ThreadStats(size_t component_count, SteadyClock::time_point start_time) + : window(component_count) + , next_report_time(start_time + std::chrono::seconds(5)) {} + + CumulativeStats cumulative; + WindowStats window; + SteadyClock::time_point next_report_time; + }; + + struct ComponentWindowStat { + size_t index; + SteadyClock::duration duration_sum; + }; + + void thread_main( + std::chrono::nanoseconds period, const rmcs_utility::ThreadConfig& thread_config) { + if (const auto result = thread_config.apply_to_current_thread(); !result) + RCLCPP_WARN(get_logger(), "%s", result.error().c_str()); + + auto next_iteration_time = SteadyClock::now(); + auto stats = ThreadStats{updating_order_.size(), next_iteration_time}; + + try { while (rclcpp::ok()) { - predefined_msg_provider_->set_timestamp(next_iteration_time); - next_iteration_time += period; - for (const auto& component : updating_order_) { - component->update(); - } + const auto scheduled_start = next_iteration_time; + const auto actual_end = execute_update_iteration(scheduled_start, stats); + next_iteration_time = calculate_next_iteration_time( + scheduled_start, actual_end, period, stats.cumulative); + log_due_reports(actual_end, stats); std::this_thread::sleep_until(next_iteration_time); } - }}; + } catch (const std::exception& exception) { + RCLCPP_FATAL( + get_logger(), "Executor update thread terminated by exception: %s", + exception.what()); + rclcpp::shutdown(); + } catch (...) { + RCLCPP_FATAL(get_logger(), "Executor update thread terminated by unknown exception"); + rclcpp::shutdown(); + } + } + + SteadyClock::time_point + execute_update_iteration(SteadyClock::time_point scheduled_start, ThreadStats& stats) { + auto current_time = SteadyClock::now(); + const auto actual_start = current_time; + + record_start_lateness(scheduled_start, actual_start, stats.cumulative); + + stats.cumulative.update_count++; + stats.window.executed_count++; + + predefined_msg_provider_->set_timestamp(scheduled_start); + + for (size_t component_index = 0; component_index < updating_order_.size(); + ++component_index) { + const auto component_start_time = current_time; + updating_order_[component_index]->update(); + current_time = SteadyClock::now(); + stats.window.component_update_duration_sums[component_index] += + current_time - component_start_time; + } + + const auto update_duration = current_time - actual_start; + record_update_duration(update_duration, stats); + return current_time; + } + + void record_start_lateness( + SteadyClock::time_point scheduled_start, SteadyClock::time_point actual_start, + CumulativeStats& stats) { + if (actual_start < scheduled_start) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 1000, "Executor update thread woke early by %.3f ms", + duration_to_ms(scheduled_start - actual_start)); + return; + } + + const auto start_lateness_ms = duration_to_ms(actual_start - scheduled_start); + stats.start_lateness_ms.insert(start_lateness_ms); + stats.start_lateness_max_ms = std::max(stats.start_lateness_max_ms, start_lateness_ms); + stats.start_lateness_sample_count++; + } + + static void record_update_duration(SteadyClock::duration update_duration, ThreadStats& stats) { + const auto update_duration_ms = duration_to_ms(update_duration); + stats.cumulative.update_duration_ms.insert(update_duration_ms); + stats.cumulative.update_duration_max_ms = + std::max(stats.cumulative.update_duration_max_ms, update_duration_ms); + stats.cumulative.update_duration_sample_count++; + stats.window.total_update_duration += update_duration; + } + + static SteadyClock::time_point calculate_next_iteration_time( + SteadyClock::time_point scheduled_start, SteadyClock::time_point actual_end, + std::chrono::nanoseconds period, CumulativeStats& stats) { + auto next_iteration_time = scheduled_start + period; + if (actual_end <= next_iteration_time) + return next_iteration_time; + + const auto overdue_duration = actual_end - next_iteration_time; + const auto skipped_cycles = + static_cast( + (overdue_duration - std::chrono::nanoseconds{1}).count() / period.count()) + + 1; + stats.skipped_count += skipped_cycles; + stats.update_count += skipped_cycles; + return next_iteration_time + period * skipped_cycles; + } + + void log_due_reports(SteadyClock::time_point current_time, ThreadStats& stats) { + while (current_time >= stats.next_report_time) { + log_cumulative_stats(stats.cumulative); + log_window_stats(stats.window); + reset_window_stats(stats.window); + stats.next_report_time += std::chrono::seconds(5); + } + } + + void log_cumulative_stats(CumulativeStats& stats) { + stats.start_lateness_ms.merge(); + stats.update_duration_ms.merge(); + + const double skipped_ratio = stats.update_count == 0 + ? 0.0 + : static_cast(stats.skipped_count) + / static_cast(stats.update_count) * 100.0; + + RCLCPP_INFO( + get_logger(), + "Update/Skipped: %llu/%llu (%s%%), " + "Stat ms p50/p99/max: start_late %s/%s/%s, update %s/%s/%s", + static_cast(stats.update_count), + static_cast(stats.skipped_count), + format_percent(skipped_ratio).c_str(), + format_stat( + maybe_quantile(stats.start_lateness_ms, stats.start_lateness_sample_count, 50.0)) + .c_str(), + format_stat( + maybe_quantile(stats.start_lateness_ms, stats.start_lateness_sample_count, 99.0)) + .c_str(), + format_stat( + stats.start_lateness_sample_count == 0 + ? std::nullopt + : std::optional{stats.start_lateness_max_ms}) + .c_str(), + format_stat( + maybe_quantile(stats.update_duration_ms, stats.update_duration_sample_count, 50.0)) + .c_str(), + format_stat( + maybe_quantile(stats.update_duration_ms, stats.update_duration_sample_count, 99.0)) + .c_str(), + format_stat( + stats.update_duration_sample_count == 0 + ? std::nullopt + : std::optional{stats.update_duration_max_ms}) + .c_str()); + } + + void log_window_stats(const WindowStats& stats) { + if (stats.executed_count == 0 + || stats.total_update_duration == SteadyClock::duration::zero()) + return; + + auto top_component_stats = collect_top_component_stats(stats); + if (top_component_stats.empty()) + return; + + RCLCPP_INFO( + get_logger(), "Component 5s: %s", + format_top_component_stats(top_component_stats, stats).c_str()); + } + + static std::vector collect_top_component_stats(const WindowStats& stats) { + auto top_component_stats = std::vector{}; + top_component_stats.reserve(stats.component_update_duration_sums.size()); + for (size_t component_index = 0; + component_index < stats.component_update_duration_sums.size(); ++component_index) { + const auto duration_sum = stats.component_update_duration_sums[component_index]; + if (duration_sum == SteadyClock::duration::zero()) + continue; + top_component_stats.emplace_back(component_index, duration_sum); + } + + const size_t top_count = std::min(3, top_component_stats.size()); + std::partial_sort( + top_component_stats.begin(), + top_component_stats.begin() + static_cast(top_count), + top_component_stats.end(), [](const auto& lhs, const auto& rhs) { + if (lhs.duration_sum != rhs.duration_sum) + return lhs.duration_sum > rhs.duration_sum; + return lhs.index < rhs.index; + }); + top_component_stats.resize(top_count); + return top_component_stats; + } + + std::string format_top_component_stats( + const std::vector& top_component_stats, + const WindowStats& stats) const { + auto top_components = std::string{}; + for (size_t rank = 0; rank < top_component_stats.size(); ++rank) { + if (rank != 0) + top_components += ", "; + + const auto& stat = top_component_stats[rank]; + top_components += std::format( + "{} {}ms/{}%", updating_order_[stat.index]->get_component_name(), + format_stat( + std::optional{ + duration_to_ms(stat.duration_sum) + / static_cast(stats.executed_count)}), + format_percent( + duration_to_ms(stat.duration_sum) / duration_to_ms(stats.total_update_duration) + * 100.0)); + } + return top_components; + } + + static void reset_window_stats(WindowStats& stats) { + stats.executed_count = 0; + stats.total_update_duration = SteadyClock::duration::zero(); + std::fill( + stats.component_update_duration_sums.begin(), + stats.component_update_duration_sums.end(), SteadyClock::duration::zero()); + } + + static double duration_to_ms(SteadyClock::duration duration) { + return std::chrono::duration(duration).count(); + } + + static std::string format_stat(const std::optional& value) { + if (!value.has_value()) + return "n/a"; + return std::format("{:.3f}", *value); + } + + static std::string format_percent(double value) { return std::format("{:.1f}", value); } + + static std::optional maybe_quantile( + const rmcs_utility::TDigest& digest, uint64_t sample_count, double quantile) { + if (sample_count == 0) + return std::nullopt; + return digest.quantile(quantile); + } + + void enable_event_outputs() { + for (const auto& component : component_list_) { + for (const auto& output : component->output_list_) { + if (output.enable == nullptr) + continue; + output.enable(output.binding); + } + } + } + + void disable_event_outputs() { + for (const auto& component : component_list_) { + for (const auto& output : component->output_list_) { + if (output.disable == nullptr) + continue; + output.disable(output.binding); + } + } + } + + void wait_event_outputs_idle() { + for (const auto& component : component_list_) { + for (const auto& output : component->output_list_) { + if (output.wait_idle == nullptr) + continue; + output.wait_idle(output.binding); + } + } + } + + struct InterfaceKey { + std::string name; + InterfaceKind kind; + + bool operator==(const InterfaceKey& other) const { + return name == other.name && kind == other.kind; + } + }; + + struct InterfaceKeyHash { + std::size_t operator()(const InterfaceKey& key) const { + return std::hash{}(key.name) + ^ (std::hash{}(static_cast(key.kind)) << 1U); + } + }; + + using OutputRecord = Component::OutputDeclaration*; + + struct NameKindRecord { + InterfaceKind kind; + std::string component_name; + const char* direction; + }; + + static std::string describe_output( + const std::string& name, InterfaceKind kind, const std::type_info& type, + const std::string& component_name) { + std::ostringstream stream; + stream << "Component [" << component_name << "] registered " << interface_kind_name(kind) + << " output \"" << name << "\" with type \"" << type.name() << "\""; + return stream.str(); + } + + static std::string describe_output(const Component::OutputDeclaration& output) { + return describe_output( + output.name, output.kind, output.type, output.component->get_component_name()); + } + + static std::string describe_declaration( + const std::string& name, InterfaceKind kind, const std::string& component_name, + const char* direction) { + std::ostringstream stream; + stream << "Component [" << component_name << "] registered " << interface_kind_name(kind) + << ' ' << direction << " \"" << name << "\""; + return stream.str(); } -private: void init() { updating_order_.clear(); - auto output_map = std::unordered_map{}; - auto user_output_map = std::map{}; + auto output_map = std::unordered_map{}; + auto output_by_name = std::unordered_map{}; + auto declaration_kind_map = std::unordered_map{}; + auto user_output_map = Component::OutputInfoMap{}; for (const auto& component : component_list_) { component->dependency_count_ = 0; component->wanted_by_.clear(); for (auto& output : component->output_list_) { - if (!output_map.emplace(output.name, &output).second) - throw std::runtime_error{"Duplicate names of output"}; - user_output_map.emplace(output.name, output.type); + auto declaration_iter = declaration_kind_map.find(output.name); + if (declaration_iter != declaration_kind_map.end() + && declaration_iter->second.kind != output.kind) { + const auto message = + std::string{"Conflicting interface kinds for name \""} + output.name + + "\": " + + describe_declaration( + output.name, declaration_iter->second.kind, + declaration_iter->second.component_name, + declaration_iter->second.direction) + + "; " + + describe_declaration( + output.name, output.kind, component->get_component_name(), "output") + + ". A name can only belong to one interface kind."; + RCLCPP_FATAL(get_logger(), "%s", message.c_str()); + throw std::runtime_error{message}; + } + declaration_kind_map.emplace( + output.name, + NameKindRecord{output.kind, component->get_component_name(), "output"}); + + auto existing_output_iter = output_by_name.find(output.name); + if (existing_output_iter != output_by_name.end() + && existing_output_iter->second->kind != output.kind) { + const auto& existing_output = *existing_output_iter->second; + const auto message = std::string{"Conflicting output kinds for name \""} + + output.name + "\": " + describe_output(existing_output) + + "; " + describe_output(output) + + ". A name can only belong to one interface kind."; + RCLCPP_FATAL(get_logger(), "%s", message.c_str()); + throw std::runtime_error{message}; + } + + output_by_name.emplace(output.name, &output); + + const auto [output_iter, inserted] = + output_map.emplace(InterfaceKey{output.name, output.kind}, &output); + if (!inserted) { + const auto& existing_output = *output_iter->second; + const auto message = + std::string{"Duplicate "} + interface_kind_name(output.kind) + + " output name \"" + output.name + + "\": " + describe_output(existing_output) + "; " + describe_output(output) + + ". Only one output may be registered for each (name, kind)."; + RCLCPP_FATAL(get_logger(), "%s", message.c_str()); + throw std::runtime_error{message}; + } + + user_output_map.emplace( + output.name, Component::OutputInfo{std::cref(output.type), output.kind}); + } + + for (const auto& input : component->input_list_) { + auto declaration_iter = declaration_kind_map.find(input.name); + if (declaration_iter != declaration_kind_map.end() + && declaration_iter->second.kind != input.kind) { + const auto message = + std::string{"Conflicting interface kinds for name \""} + input.name + "\": " + + describe_declaration( + input.name, declaration_iter->second.kind, + declaration_iter->second.component_name, + declaration_iter->second.direction) + + "; " + + describe_declaration( + input.name, input.kind, component->get_component_name(), "input") + + ". A name can only belong to one interface kind."; + RCLCPP_FATAL(get_logger(), "%s", message.c_str()); + throw std::runtime_error{message}; + } + declaration_kind_map.emplace( + input.name, + NameKindRecord{input.kind, component->get_component_name(), "input"}); } } @@ -88,36 +526,54 @@ class Executor final : public rclcpp::Node { for (const auto& component : component_list_) { for (const auto& input : component->input_list_) { - auto output_iter = output_map.find(input.name); + auto output_iter = output_map.find(InterfaceKey{input.name, input.kind}); if (output_iter == output_map.end()) { + auto output_by_name_iter = output_by_name.find(input.name); + if (output_by_name_iter != output_by_name.end()) { + const auto& available_output = *output_by_name_iter->second; + const auto message = + std::string{"Component ["} + component->get_component_name() + + "] requested " + interface_kind_name(input.kind) + " input \"" + + input.name + "\", but " + describe_output(available_output) + "."; + + RCLCPP_FATAL(get_logger(), "%s", message.c_str()); + throw std::runtime_error{message}; + } + if (!input.required) continue; - RCLCPP_FATAL( - get_logger(), - "Cannot find the corresponding output of input \"%s\" declared by " - "component [%s]", - input.name.c_str(), component->get_component_name().c_str()); - throw std::runtime_error{"Cannot find the corresponding output"}; + const auto message = std::string{"Cannot find "} + + interface_kind_name(input.kind) + " output \"" + input.name + + "\" required by component [" + + component->get_component_name() + "]."; + RCLCPP_FATAL(get_logger(), "%s", message.c_str()); + throw std::runtime_error{message}; } const auto& output = *output_iter->second; if (input.type != output.type) { - RCLCPP_FATAL(get_logger(), "With message \"%s\":", input.name.c_str()); + const auto message = std::string{"Type mismatch for "} + + interface_kind_name(input.kind) + " interface \"" + + input.name + "\": component [" + + output.component->get_component_name() + + "] declared output type \"" + output.type.name() + + "\", but component [" + component->get_component_name() + + "] requested input type \"" + input.type.name() + "\"."; + RCLCPP_FATAL(get_logger(), "%s", message.c_str()); RCLCPP_FATAL( get_logger(), " Component [%s] declared the output with type \"%s\"", output.component->get_component_name().c_str(), output.type.name()); RCLCPP_FATAL( get_logger(), " Component [%s] requested the input with type \"%s\"", component->get_component_name().c_str(), input.type.name()); - RCLCPP_FATAL(get_logger(), "Type not match."); - throw std::runtime_error{"Type not match"}; + throw std::runtime_error{message}; } if (output.component->wanted_by_.emplace(component.get()).second) component->dependency_count_++; - *input.pointer_to_data_pointer = output.data_pointer; + input.bind(input.binding, output.binding); } } @@ -140,12 +596,17 @@ class Executor final : public rclcpp::Node { RCLCPP_FATAL( get_logger(), "Component [%s]:", component->get_component_name().c_str()); for (const auto& input : component->input_list_) { - const auto& output = output_map[input.name]; + const auto output_iter = output_map.find(InterfaceKey{input.name, input.kind}); + if (output_iter == output_map.end()) + continue; + + const auto* output = output_iter->second; if (output->component->dependency_count_ == 0) continue; RCLCPP_FATAL( - get_logger(), " Depends on [%s] because requesting \"%s\"", - output->component->component_name_.c_str(), output->name.c_str()); + get_logger(), " Depends on [%s] because requesting %s interface \"%s\"", + output->component->component_name_.c_str(), interface_kind_name(input.kind), + output->name.c_str()); } } throw std::runtime_error{"Circular dependency found"}; @@ -182,4 +643,4 @@ class Executor final : public rclcpp::Node { size_t dependency_recursive_level_ = 0; }; -}; // namespace rmcs_executor \ No newline at end of file +} // namespace rmcs_executor diff --git a/rmcs_ws/src/rmcs_executor/src/main.cpp b/rmcs_ws/src/rmcs_executor/src/main.cpp index 99311f24d..c76a0a3ac 100644 --- a/rmcs_ws/src/rmcs_executor/src/main.cpp +++ b/rmcs_ws/src/rmcs_executor/src/main.cpp @@ -43,8 +43,7 @@ int main(int argc, char** argv) { rcl_executor.add_node(executor); std::vector component_descriptions; - if (!executor->get_parameter("components", component_descriptions)) - throw std::runtime_error("Missing parameter 'components' or config is not found"); + executor->get_parameter("components", component_descriptions); std::regex regex(R"(\s*(\S+)\s*->\s*(\S+)\s*)"); for (const auto& component_description : component_descriptions) { @@ -61,7 +60,7 @@ int main(int argc, char** argv) { plugin_name = component_name = component_description; } - rmcs_executor::Component::initializing_component_name = component_name.c_str(); + rmcs_executor::Component::initializing_component_name = component_name; auto component = component_loader.createSharedInstance(plugin_name); executor->add_component(component); } @@ -70,4 +69,4 @@ int main(int argc, char** argv) { rcl_executor.spin(); rclcpp::shutdown(); -} \ No newline at end of file +} diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/board_clock.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/board_clock.hpp new file mode 100644 index 000000000..e96fa76c5 --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/board_clock.hpp @@ -0,0 +1,17 @@ +#pragma once + +#include +#include + +namespace rmcs_msgs { + +struct BoardClock { + using rep = std::int64_t; + using period = std::ratio<1, 4'000'000>; + using duration = std::chrono::duration; + using time_point = std::chrono::time_point; + + static constexpr bool is_steady = true; +}; + +} // namespace rmcs_msgs 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 new file mode 100644 index 000000000..f728f138e --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/camera_frame.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include +#include +#include +#include + +#include + +namespace rmcs_msgs { + +struct CameraFrame { + static constexpr std::uint32_t kWidth = 1440; + static constexpr std::uint32_t kHeight = 1080; + static constexpr std::size_t kFrameSize = + static_cast(kWidth) * static_cast(kHeight); + + std::array data_raw; + int opencv_cvt_color_code; + + 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; + std::chrono::steady_clock::time_point sync_publish_timestamp; +}; + +} // namespace rmcs_msgs diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/camera_frame_raw.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/camera_frame_raw.hpp new file mode 100644 index 000000000..526b79f24 --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/camera_frame_raw.hpp @@ -0,0 +1,28 @@ +#pragma once + +#include +#include +#include +#include + +#include + +namespace rmcs_msgs { + +struct CameraFrameRaw { + static constexpr std::uint32_t kWidth = 1440; + static constexpr std::uint32_t kHeight = 1080; + static constexpr std::size_t kFrameSize = + static_cast(kWidth) * static_cast(kHeight); + + std::array data; + int opencv_cvt_color_code; + + Eigen::Quaterniond imu_snapshot; + + std::chrono::steady_clock::time_point exposure_timestamp; + std::chrono::steady_clock::time_point image_reception_timestamp; + std::chrono::steady_clock::time_point sync_publish_timestamp; +}; + +} // namespace rmcs_msgs 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 e534ce0bf..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 @@ -5,10 +5,23 @@ 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, + WIRELESS_CHARGING, + 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/full_robot_id.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/full_robot_id.hpp index b0ec852c2..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 @@ -12,43 +12,43 @@ class FullRobotId { enum Value : uint16_t { UNKNOWN = 0, - RED_HERO = 1, - RED_ENGINEER = 2, + RED_HERO = 1, + RED_ENGINEER = 2, RED_INFANTRY_III = 3, - RED_INFANTRY_IV = 4, - RED_INFANTRY_V = 5, - RED_AERIAL = 6, - RED_SENTRY = 7, - RED_DART = 8, - RED_RADAR = 9, - RED_OUTPOST = 10, - RED_BASE = 11, - - BLUE_HERO = 101, - BLUE_ENGINEER = 102, + RED_INFANTRY_IV = 4, + RED_INFANTRY_V = 5, + RED_AERIAL = 6, + RED_SENTRY = 7, + RED_DART = 8, + RED_RADAR = 9, + RED_OUTPOST = 10, + RED_BASE = 11, + + BLUE_HERO = 101, + BLUE_ENGINEER = 102, BLUE_INFANTRY_III = 103, - BLUE_INFANTRY_IV = 104, - BLUE_INFANTRY_V = 105, - BLUE_AERIAL = 106, - BLUE_SENTRY = 107, - BLUE_DART = 108, - BLUE_RADAR = 109, - BLUE_OUTPOST = 110, - BLUE_BASE = 111, - - RED_HERO_CLIENT = 0x0101, - RED_ENGINEER_CLIENT = 0x0102, + BLUE_INFANTRY_IV = 104, + BLUE_INFANTRY_V = 105, + BLUE_AERIAL = 106, + BLUE_SENTRY = 107, + BLUE_DART = 108, + BLUE_RADAR = 109, + BLUE_OUTPOST = 110, + BLUE_BASE = 111, + + RED_HERO_CLIENT = 0x0101, + RED_ENGINEER_CLIENT = 0x0102, RED_INFANTRY_III_CLIENT = 0x0103, - RED_INFANTRY_IV_CLIENT = 0x0104, - RED_INFANTRY_V_CLIENT = 0x0105, - RED_AERIAL_CLIENT = 0x0106, + RED_INFANTRY_IV_CLIENT = 0x0104, + RED_INFANTRY_V_CLIENT = 0x0105, + RED_AERIAL_CLIENT = 0x0106, - BLUE_HERO_CLIENT = 0x0165, - BLUE_ENGINEER_CLIENT = 0x0166, + BLUE_HERO_CLIENT = 0x0165, + BLUE_ENGINEER_CLIENT = 0x0166, BLUE_INFANTRY_III_CLIENT = 0x0167, - BLUE_INFANTRY_IV_CLIENT = 0x0168, - BLUE_INFANTRY_V_CLIENT = 0x0169, - BLUE_AERIAL_CLIENT = 0x016A, + BLUE_INFANTRY_IV_CLIENT = 0x0168, + BLUE_INFANTRY_V_CLIENT = 0x0169, + BLUE_AERIAL_CLIENT = 0x016A, REFEREE_SERVER = 0x8080, }; diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/game_stage.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/game_stage.hpp index 622a171df..d94e04dc0 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/game_stage.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/game_stage.hpp @@ -5,13 +5,13 @@ namespace rmcs_msgs { enum class GameStage : uint8_t { - NOT_START = 0, - PREPARATION = 1, + NOT_START = 0, + PREPARATION = 1, REFEREE_CHECK = 2, - COUNTDOWN = 3, - STARTED = 4, - SETTLING = 5, - UNKNOWN = UINT8_MAX, + COUNTDOWN = 3, + STARTED = 4, + SETTLING = 5, + UNKNOWN = UINT8_MAX, }; } // namespace rmcs_msgs \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/imu_snapshot.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/imu_snapshot.hpp new file mode 100644 index 000000000..1c0747b3f --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/imu_snapshot.hpp @@ -0,0 +1,14 @@ +#pragma once + +#include +#include + +namespace rmcs_msgs { + +struct ImuSnapshot { + Eigen::Quaterniond orientation; + Eigen::Vector3d gyro_body; + rmcs_msgs::BoardClock::time_point timestamp; +}; + +} // 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 09925e7b1..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 @@ -9,18 +9,22 @@ # endif #endif -#include "chassis_mode.hpp" -#include "full_robot_id.hpp" -#include "game_stage.hpp" -#include "gimbal_mode.hpp" -#include "keyboard.hpp" -#include "mouse.hpp" -#include "robot_color.hpp" -#include "robot_id.hpp" -#include "serial_interface.hpp" -#include "shoot_mode.hpp" -#include "shoot_status.hpp" -#include "switch.hpp" +#include "board_clock.hpp" // IWYU pragma: export +#include "camera_frame_raw.hpp" // IWYU pragma: export +#include "chassis_mode.hpp" // IWYU pragma: export +#include "full_robot_id.hpp" // IWYU pragma: export +#include "game_stage.hpp" // IWYU pragma: export +#include "gimbal_mode.hpp" // IWYU pragma: export +#include "imu_snapshot.hpp" // IWYU pragma: export +#include "keyboard.hpp" // IWYU pragma: export +#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 +#include "switch.hpp" // IWYU pragma: export namespace rmcs_msgs { @@ -40,9 +44,14 @@ 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::WIRELESS_CHARGING: return "WIRELESS_CHARGING"; + case ChassisMode::CLIMB: return "CLIMB"; } return "INVALID"; } diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_color.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_color.hpp index dc7ee6948..c5517f2de 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_color.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_color.hpp @@ -6,8 +6,8 @@ namespace rmcs_msgs { enum class RobotColor : uint8_t { UNKNOWN = 0, - RED = 1, - BLUE = 2, + RED = 1, + BLUE = 2, }; } // namespace rmcs_msgs \ No newline at end of file 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 622d5fdca..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 @@ -6,18 +6,18 @@ namespace rmcs_msgs { enum class ArmorID : uint16_t { - Unknown = 0, - Hero = 1, - Engineer = 2, + Unknown = 0, + Hero = 1, + Engineer = 2, InfantryIII = 3, - InfantryIV = 4, - InfantryV = 5, - Aerial = 6, - Sentry = 7, - Dart = 8, - Radar = 9, - Outpost = 10, - Base = 11, + InfantryIV = 4, + InfantryV = 5, + Aerial = 6, + Sentry = 7, + Dart = 8, + Radar = 9, + Outpost = 10, + Base = 11, }; class RobotId { @@ -25,29 +25,29 @@ class RobotId { enum Value : uint8_t { UNKNOWN = 0, - RED_HERO = 1, - RED_ENGINEER = 2, + RED_HERO = 1, + RED_ENGINEER = 2, RED_INFANTRY_III = 3, - RED_INFANTRY_IV = 4, - RED_INFANTRY_V = 5, - RED_AERIAL = 6, - RED_SENTRY = 7, - RED_DART = 8, - RED_RADAR = 9, - RED_OUTPOST = 10, - RED_BASE = 11, + RED_INFANTRY_IV = 4, + RED_INFANTRY_V = 5, + RED_AERIAL = 6, + RED_SENTRY = 7, + RED_DART = 8, + RED_RADAR = 9, + RED_OUTPOST = 10, + RED_BASE = 11, - BLUE_HERO = 101, - BLUE_ENGINEER = 102, + BLUE_HERO = 101, + BLUE_ENGINEER = 102, BLUE_INFANTRY_III = 103, - BLUE_INFANTRY_IV = 104, - BLUE_INFANTRY_V = 105, - BLUE_AERIAL = 106, - BLUE_SENTRY = 107, - BLUE_DART = 108, - BLUE_RADAR = 109, - BLUE_OUTPOST = 110, - BLUE_BASE = 111, + BLUE_INFANTRY_IV = 104, + BLUE_INFANTRY_V = 105, + BLUE_AERIAL = 106, + BLUE_SENTRY = 107, + BLUE_DART = 108, + BLUE_RADAR = 109, + BLUE_OUTPOST = 110, + BLUE_BASE = 111, }; constexpr RobotId() @@ -67,9 +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 { return value_ & 0x40 ? RobotColor::BLUE : RobotColor::RED; } + 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_); } @@ -77,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_msgs/include/rmcs_msgs/serial_interface.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/serial_interface.hpp index a198e5a87..52d736a41 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/serial_interface.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/serial_interface.hpp @@ -7,7 +7,7 @@ namespace rmcs_msgs { struct SerialInterface { std::function read; - std::function write; + std::function write; }; -} \ No newline at end of file +} // namespace rmcs_msgs \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/shoot_condiction.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/shoot_condiction.hpp new file mode 100644 index 000000000..95dd40907 --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/shoot_condiction.hpp @@ -0,0 +1,14 @@ +#pragma once + +#include + +namespace rmcs_msgs { + +enum class ShootCondiction : uint8_t { + FRICTION_WAITING = 0, + PRELOADING = 1, + SHOOT = 2, + JAM = 3, + FIRED = 4, +}; +} diff --git a/rmcs_ws/src/rmcs_utility/CMakeLists.txt b/rmcs_ws/src/rmcs_utility/CMakeLists.txt index e9c09fbe5..b6c08a8d0 100644 --- a/rmcs_ws/src/rmcs_utility/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_utility/CMakeLists.txt @@ -3,7 +3,8 @@ project(rmcs_utility) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c11") -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++20") +set(CMAKE_CXX_STANDARD 23) +set(CMAKE_CXX_STANDARD_REQUIRED ON) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-O2 -Wall -Wextra -Wpedantic) @@ -14,4 +15,4 @@ ament_auto_find_build_dependencies () include_directories(${PROJECT_SOURCE_DIR}/include) -ament_auto_package() \ No newline at end of file +ament_auto_package() diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/atomic_futex.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/atomic_futex.hpp new file mode 100644 index 000000000..78ed40620 --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/atomic_futex.hpp @@ -0,0 +1,160 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace rmcs_utility { +namespace detail { + +inline auto atomic_futex_address(const std::atomic& atomic) noexcept -> uint32_t* { + return const_cast(reinterpret_cast(&atomic)); +} + +inline bool atomic_futex_wait_until_steady( + const std::atomic& atomic, uint32_t old_val, + std::chrono::steady_clock::time_point deadline, std::memory_order order) noexcept { + + if (atomic.load(order) != old_val) + return true; + + timespec abs_time; + clock_gettime(CLOCK_MONOTONIC, &abs_time); + + const auto remaining = + std::chrono::ceil(deadline - std::chrono::steady_clock::now()); + + if (remaining <= std::chrono::nanoseconds::zero()) + return atomic.load(order) != old_val; + + const auto secs = std::chrono::duration_cast(remaining); + const auto nsecs = std::chrono::duration_cast(remaining - secs); + + abs_time.tv_sec += static_cast(secs.count()); + abs_time.tv_nsec += static_cast(nsecs.count()); + + if (abs_time.tv_nsec >= 1'000'000'000) { + abs_time.tv_sec += 1; + abs_time.tv_nsec -= 1'000'000'000; + } + + auto* const ptr = atomic_futex_address(atomic); + + while (atomic.load(order) == old_val) { + if (syscall( + SYS_futex, ptr, FUTEX_WAIT_BITSET_PRIVATE, old_val, &abs_time, nullptr, + FUTEX_BITSET_MATCH_ANY) + == 0) + continue; + + if (errno == EAGAIN || errno == EINTR) + continue; + + if (errno == ETIMEDOUT) + return atomic.load(order) != old_val; + + return atomic.load(order) != old_val; + } + + return true; +} + +} // namespace detail + +/** + * @brief Waits until a 32-bit atomic value changes or the timeout expires. + * + * This helper uses Linux futex syscalls directly and must be paired with + * rmcs_utility::atomic_futex_notify_one() or rmcs_utility::atomic_futex_notify_all() + * on the same atomic object. + * + * @warning This implementation is not compatible with `std::atomic::notify_one()` or + * `std::atomic::notify_all()`. The standard library may track waiters in an internal pool, + * so mixing these APIs on the same atomic object can miss wakeups. + * + * @param atomic Atomic word used as the futex key. + * @param old_val Expected value observed before waiting. + * @param timeout Maximum time to wait. A non-positive timeout performs a non-blocking check. + * @param order Memory order used for polling loads around the futex wait. + * @return `true` if `atomic` no longer equals `old_val`, otherwise `false` when the timeout + * expires. + */ +inline bool atomic_futex_wait_for( + const std::atomic& atomic, uint32_t old_val, + std::chrono::steady_clock::duration timeout, + std::memory_order order = std::memory_order_seq_cst) noexcept { + + if (atomic.load(order) != old_val) + return true; + + if (timeout <= std::chrono::steady_clock::duration::zero()) + return false; + + return detail::atomic_futex_wait_until_steady( + atomic, old_val, std::chrono::steady_clock::now() + timeout, order); +} + +/** + * @brief Waits until a 32-bit atomic value changes or the steady-clock deadline is reached. + * + * This helper uses Linux futex syscalls directly and must be paired with + * rmcs_utility::atomic_futex_notify_one() or rmcs_utility::atomic_futex_notify_all() + * on the same atomic object. + * + * @warning This implementation is not compatible with `std::atomic::notify_one()` or + * `std::atomic::notify_all()`. The standard library may track waiters in an internal pool, + * so mixing these APIs on the same atomic object can miss wakeups. + * + * @param atomic Atomic word used as the futex key. + * @param old_val Expected value observed before waiting. + * @param deadline Steady-clock deadline after which the wait times out. + * @param order Memory order used for polling loads around the futex wait. + * @return `true` if `atomic` no longer equals `old_val`, otherwise `false` when the deadline + * is reached. + */ +inline bool atomic_futex_wait_until( + const std::atomic& atomic, uint32_t old_val, + std::chrono::steady_clock::time_point deadline, + std::memory_order order = std::memory_order_seq_cst) noexcept { + + return detail::atomic_futex_wait_until_steady(atomic, old_val, deadline, order); +} + +/** + * @brief Wakes one thread blocked in rmcs_utility::atomic_futex_wait_for() or + * rmcs_utility::atomic_futex_wait_until(). + * + * @warning This must not be mixed with `std::atomic::notify_one()` or + * `std::atomic::notify_all()` on the same atomic object. + * + * @param atomic Atomic word used as the futex key. + */ +inline void atomic_futex_notify_one(const std::atomic& atomic) noexcept { + syscall( + SYS_futex, detail::atomic_futex_address(atomic), FUTEX_WAKE_BITSET_PRIVATE, 1, nullptr, + nullptr, FUTEX_BITSET_MATCH_ANY); +} + +/** + * @brief Wakes all threads blocked in rmcs_utility::atomic_futex_wait_for() or + * rmcs_utility::atomic_futex_wait_until(). + * + * @warning This must not be mixed with `std::atomic::notify_one()` or + * `std::atomic::notify_all()` on the same atomic object. + * + * @param atomic Atomic word used as the futex key. + */ +inline void atomic_futex_notify_all(const std::atomic& atomic) noexcept { + syscall( + SYS_futex, detail::atomic_futex_address(atomic), FUTEX_WAKE_BITSET_PRIVATE, INT_MAX, + nullptr, nullptr, FUTEX_BITSET_MATCH_ANY); +} + +} // namespace rmcs_utility diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/crc/dji_crc.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/crc/dji_crc.hpp index 620c60230..be552b7e3 100644 --- a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/crc/dji_crc.hpp +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/crc/dji_crc.hpp @@ -11,7 +11,7 @@ inline auto& get_tail(T& data) { return *reinterpret_cast(reinterpret_cast(&data) + sizeof(T) - sizeof(uint8_t)); } -constexpr uint8_t crc8_init = 0xff; +constexpr uint8_t crc8_init = 0xff; constexpr uint8_t crc8_table[256] = { 0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20, 0xa3, 0xfd, 0x1f, 0x41, 0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e, 0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc, @@ -31,7 +31,7 @@ constexpr uint8_t crc8_table[256] = { 0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7, 0xb6, 0xe8, 0x0a, 0x54, 0xd7, 0x89, 0x6b, 0x35, }; -constexpr uint16_t crc16_init = 0xffff; +constexpr uint16_t crc16_init = 0xffff; constexpr uint16_t crc16_table[256] = { 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, @@ -59,7 +59,7 @@ constexpr uint16_t crc16_table[256] = { } // namespace internal inline uint8_t calculate_crc8(const void* data, size_t length) { - auto* p = reinterpret_cast(data); + auto* p = reinterpret_cast(data); auto result = internal::crc8_init; while (length--) result = internal::crc8_table[result ^ (*p++)]; @@ -87,7 +87,7 @@ inline void append_crc8(T& package) { } inline uint16_t calculate_crc16(const void* data, size_t length) { - auto* p = reinterpret_cast(data); + auto* p = reinterpret_cast(data); uint16_t result = internal::crc16_init; while (length--) { result = (result >> 8) ^ internal::crc16_table[(result ^ (*p++)) & 0x00ff]; 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/double_buffer.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/double_buffer.hpp index 67edfce9e..9e473d509 100644 --- a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/double_buffer.hpp +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/double_buffer.hpp @@ -21,7 +21,7 @@ requires(std::is_trivially_copyable_v && std::is_trivially_destructible_v) /// @param data The data to be written to the buffer. void write(const T& data) noexcept { const uint64_t current = current_.load(std::memory_order_relaxed); - const uint64_t next = current + 1; + const uint64_t next = current + 1; std::memcpy(&buffers_[next & 1], &data, sizeof(T)); current_.store(next, std::memory_order_release); } diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/fps_counter.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/fps_counter.hpp index 2c5ff84fe..05af4814a 100644 --- a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/fps_counter.hpp +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/fps_counter.hpp @@ -14,11 +14,11 @@ class FpsCounter { bool count() { ++count_; - auto now = std::chrono::steady_clock::now(); + auto now = std::chrono::steady_clock::now(); auto elapsed_time = now - start_; if (elapsed_time >= measurement_window_) { start_ = now; - fps_ = double(count_) / std::chrono::duration(elapsed_time).count(); + fps_ = double(count_) / std::chrono::duration(elapsed_time).count(); count_ = 0; return true; } @@ -33,7 +33,7 @@ class FpsCounter { std::chrono::steady_clock::time_point start_; int64_t count_ = 0; - double fps_ = 0; + double fps_ = 0; }; } // namespace rmcs_utility \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/memory_pool.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/memory_pool.hpp new file mode 100644 index 000000000..2044f4a13 --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/memory_pool.hpp @@ -0,0 +1,196 @@ +#pragma once + +#include +#include +#include +#include + +namespace rmcs_utility { + +template +class MemoryPool { + static_assert(Size > 0, "MemoryPool slot size must be greater than zero"); + static_assert(Align > 0, "MemoryPool slot alignment must be greater than zero"); + static_assert((Align & (Align - 1)) == 0, "MemoryPool slot alignment must be a power of two"); + +public: + /*! + * @brief Construct a fixed-capacity raw storage pool + * @param capacity Number of storage slots to allocate + * @note This container is not thread-safe. + */ + explicit MemoryPool(size_t capacity) + : capacity_(capacity) + , storage_(new Storage[capacity]) + , free_stack_(new size_t[capacity]) + , occupied_(new bool[capacity]{}) { + reset_free_stack(); + } + + MemoryPool(const MemoryPool&) = delete; + MemoryPool& operator=(const MemoryPool&) = delete; + MemoryPool(MemoryPool&&) = delete; + MemoryPool& operator=(MemoryPool&&) = delete; + + /*! + * @brief Destructor + * @note All allocated slots must be returned to the pool before destruction. + * Violating this lifetime contract terminates the program. + */ + ~MemoryPool() noexcept { + if (!empty()) { + std::fprintf( + stderr, + "rmcs_utility::MemoryPool %p destroyed with outstanding slots: size=%zu, " + "capacity=%zu\n", + static_cast(this), size_, capacity_); + std::fflush(stderr); + std::terminate(); + } + } + + /*! + * @brief Payload size of each storage slot in bytes + */ + [[nodiscard]] static constexpr size_t slot_size() noexcept { return Size; } + + /*! + * @brief Alignment guarantee of each storage slot in bytes + */ + [[nodiscard]] static constexpr size_t slot_align() noexcept { return Align; } + + /*! + * @brief Capacity of the memory pool + * @return Total number of storage slots + */ + [[nodiscard]] size_t max_size() const noexcept { return capacity_; } + + /*! + * @brief Number of currently allocated storage slots + */ + [[nodiscard]] size_t size() const noexcept { return size_; } + + /*! + * @brief Number of free storage slots remaining + */ + [[nodiscard]] size_t available() const noexcept { return capacity_ - size_; } + + /*! + * @brief Check whether the pool contains no allocated slot + */ + [[nodiscard]] bool empty() const noexcept { return size_ == 0; } + + /*! + * @brief Check whether the pool has no free slot + */ + [[nodiscard]] bool full() const noexcept { return size_ == capacity_; } + + /*! + * @brief Allocate one raw storage slot from the pool + * @return Pointer to the slot's raw storage, or nullptr if the pool is full + */ + [[nodiscard]] void* allocate() noexcept { + if (full()) + return nullptr; + + const auto index = free_stack_[top_ - 1]; + occupied_[index] = true; + --top_; + ++size_; + + return static_cast(slot_pointer(index)); + } + + /*! + * @brief Release a raw storage slot back to the pool + * @param pointer Pointer previously returned by this pool + * @return true if the slot was released, false if the pointer was invalid + * @note The pointer must refer to the start address of a live slot. + */ + bool free(void* pointer) noexcept { + const auto index = pointer_to_index(pointer); + + if (index == capacity_ || !occupied_[index]) + return false; + + occupied_[index] = false; + free_stack_[top_] = index; + ++top_; + --size_; + + return true; + } + + /*! + * @brief Check whether a pointer refers to a live slot owned by this pool + */ + [[nodiscard]] bool contains(const void* pointer) const noexcept { + const auto index = pointer_to_index(pointer); + return index != capacity_ && occupied_[index]; + } + + /*! + * @brief Release every live slot and reset the pool + * @return Number of slots that were released + */ + size_t clear() noexcept { + size_t count = 0; + + for (size_t i = 0; i < capacity_; i++) { + if (occupied_[i]) { + occupied_[i] = false; + ++count; + } + } + + size_ = 0; + reset_free_stack(); + + return count; + } + +private: + struct Storage { + alignas(Align) std::byte data[Size]; + }; + + [[nodiscard]] std::byte* slot_pointer(size_t index) noexcept { return storage_[index].data; } + + [[nodiscard]] const std::byte* slot_pointer(size_t index) const noexcept { + return storage_[index].data; + } + + [[nodiscard]] size_t pointer_to_index(const void* pointer) const noexcept { + if (pointer == nullptr) + return capacity_; + + const auto begin = reinterpret_cast(storage_.get()); + const auto end = begin + sizeof(Storage) * capacity_; + const auto target = reinterpret_cast(pointer); + + if (target < begin || target >= end) + return capacity_; + + const auto offset = target - begin; + if (offset % sizeof(Storage) != 0) + return capacity_; + + return static_cast(offset / sizeof(Storage)); + } + + void reset_free_stack() noexcept { + for (size_t i = 0; i < capacity_; i++) + free_stack_[capacity_ - 1 - i] = i; + top_ = capacity_; + } + + size_t capacity_; + std::unique_ptr storage_; + std::unique_ptr free_stack_; + std::unique_ptr occupied_; + + size_t top_{0}; + size_t size_{0}; +}; + +} // namespace rmcs_utility diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/package_receive.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/package_receive.hpp index 60571f8e5..f9678c601 100644 --- a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/package_receive.hpp +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/package_receive.hpp @@ -10,8 +10,8 @@ namespace rmcs_utility { enum class ReceiveResult : uint8_t { - SUCCESS = 0, - TIMEOUT = 1, + SUCCESS = 0, + TIMEOUT = 1, HEADER_INVALID = 2, VERIFY_INVALID = 3 }; diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/pooled_shared_factory.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/pooled_shared_factory.hpp new file mode 100644 index 000000000..c2e65a199 --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/pooled_shared_factory.hpp @@ -0,0 +1,76 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "rmcs_utility/memory_pool.hpp" + +namespace rmcs_utility { + +template +requires std::is_nothrow_destructible_v class PooledSharedFactory { +public: + explicit PooledSharedFactory(size_t capacity) + : pool_(std::make_shared(capacity)) {} + + template + [[nodiscard]] std::shared_ptr make(Args&&... args) { + auto ptr = try_make(std::forward(args)...); + if (!ptr) + throw std::bad_alloc{}; + return ptr; + } + + template + [[nodiscard]] std::shared_ptr try_make(Args&&... args) { + void* storage = nullptr; + { + auto guard = std::scoped_lock{pool_->mutex}; + storage = pool_->allocate(); + } + + if (!storage) + return nullptr; + + try { + return std::shared_ptr( + std::construct_at(static_cast(storage), std::forward(args)...), + [pool = pool_](T* ptr) { + std::destroy_at(ptr); + auto guard = std::scoped_lock{pool->mutex}; + if (!pool->free(ptr)) { + std::fprintf( + stderr, + "rmcs_utility::PooledSharedFactory failed to return slot %p to pool " + "%p\n", + static_cast(ptr), static_cast(pool.get())); + std::fflush(stderr); + std::terminate(); + } + }); + } catch (...) { + auto guard = std::scoped_lock{pool_->mutex}; + pool_->free(storage); + throw; + } + } + + [[nodiscard]] size_t max_size() const noexcept { return pool_->max_size(); } + +private: + class PoolImpl final : public MemoryPool { + public: + using MemoryPool::MemoryPool; + + mutable std::mutex mutex; + }; + + std::shared_ptr pool_; +}; + +} // namespace rmcs_utility 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 2c829c64a..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 @@ -2,10 +2,13 @@ #include #include +#include #include +#include #include #include #include +#include #include namespace rmcs_utility { @@ -15,6 +18,179 @@ namespace rmcs_utility { template class RingBuffer { public: + template + class ReadableView; + + template + class BasicIterator { + using Buffer = std::conditional_t; + + public: + using iterator_category = std::random_access_iterator_tag; + using iterator_concept = std::random_access_iterator_tag; + using value_type = T; + using difference_type = std::ptrdiff_t; + using reference = std::conditional_t; + using pointer = std::conditional_t; + + BasicIterator() = default; + BasicIterator(const BasicIterator&) = default; + BasicIterator(BasicIterator&&) = default; + BasicIterator& operator=(const BasicIterator&) = default; + BasicIterator& operator=(BasicIterator&&) = default; + + // NOLINTNEXTLINE(google-explicit-constructor) + BasicIterator(const BasicIterator& other) requires is_const + : buffer_(other.buffer_) + , origin_(other.origin_) + , offset_(other.offset_) {} + + reference operator*() const { return *ptr(); } + pointer operator->() const { return ptr(); } + reference operator[](difference_type n) const { return *(*this + n); } + + BasicIterator& operator++() { + offset_++; + return *this; + } + + BasicIterator operator++(int) { + auto old = *this; + ++*this; + return old; + } + + BasicIterator& operator--() { + offset_--; + return *this; + } + + BasicIterator operator--(int) { + auto old = *this; + --*this; + return old; + } + + BasicIterator& operator+=(difference_type n) { + if (n >= 0) + offset_ += static_cast(n); + else + offset_ -= static_cast(-(n + 1)) + 1; + return *this; + } + + BasicIterator& operator-=(difference_type n) { + if (n >= 0) + offset_ -= static_cast(n); + else + offset_ += static_cast(-(n + 1)) + 1; + return *this; + } + + friend BasicIterator operator+(BasicIterator it, difference_type n) { + it += n; + return it; + } + + friend BasicIterator operator+(difference_type n, BasicIterator it) { + it += n; + return it; + } + + friend BasicIterator operator-(BasicIterator it, difference_type n) { + it -= n; + return it; + } + + friend difference_type operator-(BasicIterator lhs, BasicIterator rhs) { + if (lhs.offset_ >= rhs.offset_) + return static_cast(lhs.offset_ - rhs.offset_); + return -static_cast(rhs.offset_ - lhs.offset_); + } + + friend bool operator==(BasicIterator lhs, BasicIterator rhs) { + return lhs.buffer_ == rhs.buffer_ && lhs.origin_ == rhs.origin_ + && lhs.offset_ == rhs.offset_; + } + + friend std::strong_ordering operator<=>(BasicIterator lhs, BasicIterator rhs) { + return lhs.offset_ <=> rhs.offset_; + } + + private: + friend class RingBuffer; + template + friend class BasicIterator; + template + friend class ReadableView; + + BasicIterator(Buffer* buffer, size_t origin, size_t offset) + : buffer_(buffer) + , origin_(origin) + , offset_(offset) {} + + pointer ptr() const { + return std::launder( + reinterpret_cast( + buffer_->storage_[(origin_ + offset_) & buffer_->mask_].data)); + } + + Buffer* buffer_ = nullptr; + size_t origin_ = 0; + size_t offset_ = 0; + }; + + using iterator = BasicIterator; + using const_iterator = BasicIterator; + + template + class ReadableView { + using Buffer = std::conditional_t; + + public: + using iterator = BasicIterator; + using value_type = T; + using difference_type = std::ptrdiff_t; + using size_type = size_t; + using reference = std::conditional_t; + + ReadableView() = default; + ReadableView(const ReadableView&) = default; + ReadableView(ReadableView&&) = default; + ReadableView& operator=(const ReadableView&) = default; + ReadableView& operator=(ReadableView&&) = default; + + // NOLINTNEXTLINE(google-explicit-constructor) + ReadableView(const ReadableView& other) requires is_const + : buffer_(other.buffer_) + , origin_(other.origin_) + , size_(other.size_) {} + + iterator begin() const { return iterator{buffer_, origin_, 0}; } + iterator end() const { return iterator{buffer_, origin_, size_}; } + + [[nodiscard]] bool empty() const { return size_ == 0; } + [[nodiscard]] size_type size() const { return size_; } + + reference front() const { return *begin(); } + reference back() const { return *(end() - 1); } + reference operator[](size_type index) const { return begin()[index]; } + + private: + friend class RingBuffer; + template + friend class ReadableView; + + ReadableView(Buffer* buffer, size_t origin, size_t size) + : buffer_(buffer) + , origin_(origin) + , size_(size) {} + + Buffer* buffer_ = nullptr; + size_t origin_ = 0; + size_t size_ = 0; + }; + /*! * @brief Construct an SPSC ring buffer * @param size Minimum capacity requested. Actual capacity is rounded up @@ -75,6 +251,31 @@ class RingBuffer { return max_size() - (in - out); } + /*! + * @brief Snapshot view of elements currently readable by the consumer + * @note Captures [out, in) once. Producer pushes do not extend the returned view. + * Consumer pops invalidate iterators to erased elements. + */ + ReadableView readable_view() noexcept { + const auto in = in_.load(std::memory_order::acquire); + const auto out = out_.load(std::memory_order::relaxed); + return {this, out, in - out}; + } + + /*! + * @brief Const snapshot view of elements currently readable by the consumer + */ + ReadableView readable_view() const noexcept { + const auto in = in_.load(std::memory_order::acquire); + const auto out = out_.load(std::memory_order::relaxed); + return {this, out, in - out}; + } + + /*! + * @brief Explicit const snapshot view for non-const buffers + */ + ReadableView const_readable_view() const noexcept { return readable_view(); } + /*! * @brief Peek the first element (consumer side) * @return Pointer to the first element, or nullptr if empty @@ -90,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 @@ -240,6 +478,24 @@ class RingBuffer { return pop_front_n(std::forward(callback_functor), 1); } + /*! + * @brief Pop readable elements before a snapshot iterator + * @return Number of elements erased from the front + * @note Consumer-only. `pos` must come from this buffer's current readable range. + */ + size_t pop_front_until(const_iterator pos) { + if (pos.buffer_ != this) + return 0; + + const auto in = in_.load(std::memory_order::acquire); + const auto out = out_.load(std::memory_order::relaxed); + const auto count = pos.origin_ + pos.offset_ - out; + if (count > in - out) + return 0; + + return pop_front_n([](const T&) noexcept {}, count); + } + /*! * @brief Clear the buffer by consuming all elements * @return Number of elements that were erased @@ -274,4 +530,4 @@ class RingBuffer { std::atomic in_{0}, out_{0}; }; -} // namespace rmcs_utility +} // namespace rmcs_utility \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/tdigest.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/tdigest.hpp new file mode 100644 index 000000000..0f03c8599 --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/tdigest.hpp @@ -0,0 +1,660 @@ +#pragma once + +/* + * This file is derived from the digestible project: + * https://github.com/SpirentOrion/digestible/blob/master/include/digestible/digestible.h + * + * The digestible project provides a modern C++ implementation of a merging + * t-digest data structure under the Apache License, Version 2.0: + * https://github.com/SpirentOrion/digestible + * + * The underlying t-digest algorithm and reference implementation were originally + * authored by Ted Dunning: + * https://github.com/tdunning/t-digest + * + * As noted in the original t-digest NOTICES file: + * "The code for the t-digest was originally authored by Ted Dunning." + * "Adrien Grand contributed the heart of the AVLTreeDigest (https://github.com/jpountz)." + * + * Modifications for RMCS by Zihan Qin, 2026: + * - Changed namespace + * - Adjusted defaults + * - Formatting changes + * - Miscellaneous fixes and tweaks + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_utility { + +// XXX: yes, this could be a std::pair. But being able to refer to values by name +// instead of .first and .second makes merge(), quantile(), and +// cumulative_distribution() way more readable. +template +struct Centroid { + Values mean; + Weight weight; + + Centroid(Values new_mean, Weight new_weight) + : mean(new_mean) + , weight(new_weight) {} +}; + +template +inline bool operator<(const Centroid& lhs, const Centroid& rhs) { + return (lhs.mean < rhs.mean); +} + +template +inline bool operator>(const Centroid& lhs, const Centroid& rhs) { + return (lhs.mean > rhs.mean); +} + +template +class TDigest { + using centroid_t = Centroid; + + struct TDigestImpl { + std::vector values; + Weight total_weight; + + explicit TDigestImpl(size_t size) + : total_weight(0) { + values.reserve(size); + } + + TDigestImpl() = delete; + + enum class insert_result { OK, NEED_COMPRESS }; + + insert_result insert(Values value, Weight weight) { + assert(weight); + values.emplace_back(value, weight); + total_weight += weight; + return ( + values.size() != values.capacity() ? insert_result::OK + : insert_result::NEED_COMPRESS); + } + + insert_result insert(const centroid_t& val) { return (insert(val.mean, val.weight)); } + + void reset() { + values.clear(); + total_weight = 0; + } + + size_t capacity() const { return (values.capacity()); } + }; + + TDigestImpl one; + TDigestImpl two; + + // XXX: buffer multiplier must be > 0. BUT how much greater + // will affect size vs speed balance. The effects of which + // have not been studied. Set to 2 to favor size. Informal + // benchmarks for this value yielded acceptable results. + static constexpr size_t buffer_multiplier = 2; + TDigestImpl buffer; + + TDigestImpl* active; + + Values min_val, max_val; + bool run_forward; + + void swap(TDigest&); + +public: + explicit TDigest(size_t size); + + TDigest() = delete; + + TDigest(const TDigest&); + TDigest(TDigest&&) noexcept; + TDigest& operator=(TDigest); + + /** + * Inserts the given value into the t-digest input buffer. + * Assumes a weight of 1 for the sample. + * If this operation fills the input buffer an automatic merge() + * operation is triggered. + * + * @param value + * value to insert + */ + void insert(Values value) { insert(value, 1); } + + /** + * Inserts the given value with the given weight into the t-digest input buffer. + * If this operation fills the input buffer an automatic merge() + * operation is triggered. + * + * @param value + * value to insert + * + * @param weight + * weight associated with the provided value + */ + void insert(Values value, Weight weight) { + if (!weight) + throw std::invalid_argument("TDigest weight must be positive."); + + const auto insert_result = buffer.insert(value, weight); + min_val = std::min(min_val, value); + max_val = std::max(max_val, value); + + if (insert_result == TDigestImpl::insert_result::NEED_COMPRESS) { + merge(); + } + } + + /** + * Insert and merge an existing t-digest. + * + * Assumes `src` has already been flushed/merged and contains no pending + * buffered samples. This function only imports merged centroids from `src`. + * + * @param src + * source t-digest to copy values from + */ + void insert(const TDigest& src); + + /** + * Reset internal state of the t-digest. + * + * Clears t-digest, incoming data buffer, and resets min/max values. + */ + void reset(); + + /** + * Retrieve contents of the t-digest. + * + * @return + * list of pairs sorted by value in ascending order. + */ + std::vector> get() const; + + /** + * Merge incoming data into the t-digest. + */ + void merge(); + + /** + * Retrieve the probability that a random data point in the digest is + * less than or equal to x. + * + * @param x + * value of interest + * + * @return + * a value between [0, 1] + */ + double cumulative_distribution(Values x) const; + + /** + * Retrieve the value such that p percent of all data points or less than or + * equal to that value. + * + * @param p + * percentile of interest; valid input between [0.0, 100.0] + * + * @return + * a value from the dataset that satisfies the above condition + */ + double quantile(double p) const; + + /** + * Return number of centroids in the t-digest + */ + size_t centroid_count() const { return ((*active).values.size()); } + + /** + * Retrieve the number of merged data points in the t-digest + * + * @return + * the total weight of all data + */ + size_t size() const { return ((*active).total_weight); } + + /** + * Retrieve maximum value seen by this t-digest. + * Value is cleared by reset(). + * + * @return + * maximum value seen by this t-digest. + */ + Values max() const { return (max_val); } + + /** + * Retrieve minimum value seen by this t-digest. + * Value is cleared by reset(). + * + * @return + * minimum value seen by this t-digest. + */ + Values min() const { return (min_val); } +}; + +template +TDigest::TDigest(size_t size) + : one(size) + , two(size) + , buffer(size * buffer_multiplier) + , active(&one) + , min_val(std::numeric_limits::max()) + , max_val(std::numeric_limits::lowest()) + , run_forward(true) { + if (size == 0) + throw std::invalid_argument("TDigest size must be positive."); +} + +template +TDigest::TDigest(const TDigest& other) + : one(other.one) + , two(other.two) + , buffer(other.buffer) + , active(other.active == &other.one ? &one : &two) + , min_val(other.min_val) + , max_val(other.max_val) + , run_forward(other.run_forward) {} + +template +void TDigest::swap(TDigest& other) { + const bool this_active_one = (active == &one); + const bool other_active_one = (other.active == &other.one); + + std::swap(one, other.one); + std::swap(two, other.two); + std::swap(buffer, other.buffer); + std::swap(min_val, other.min_val); + std::swap(max_val, other.max_val); + std::swap(run_forward, other.run_forward); + + active = other_active_one ? &one : &two; + other.active = this_active_one ? &other.one : &other.two; +} + +template +TDigest::TDigest(TDigest&& other) noexcept + : TDigest(other.one.capacity()) { + swap(other); +} + +template +TDigest& TDigest::operator=(TDigest other) { + swap(other); + return (*this); +} + +template +void TDigest::insert(const TDigest& src) { + max_val = std::max(max_val, src.max_val); + min_val = std::min(min_val, src.min_val); + + auto insert_fn = [this](const auto& val) { + if (buffer.insert(val) == TDigestImpl::insert_result::NEED_COMPRESS) { + merge(); + } + }; + + std::for_each(src.active->values.begin(), src.active->values.end(), insert_fn); + // Explicitly merge any unmerged data for a consistent end state. + merge(); +} + +template +void TDigest::reset() { + one.reset(); + two.reset(); + buffer.reset(); + active = &one; + min_val = std::numeric_limits::max(); + max_val = std::numeric_limits::lowest(); +} + +template +std::vector> TDigest::get() const { + std::vector> to_return; + + std::transform( + active->values.begin(), active->values.end(), std::back_inserter(to_return), + [](const centroid_t& val) { return (std::make_pair(val.mean, val.weight)); }); + + return (to_return); +} + +/** + * When taken together the following 4 functions (Z, normalizer_fn, k, q) + * comprise the scaling function for t-digests. + */ + +/** + * C++ translation of the "k_2" version found in the reference implementation + * available here: https://github.com/tdunning/t-digest + */ +inline double Z(double compression, double n) { return (4 * log(n / compression) + 24); } + +/** + * C++ translation of the "k_2" version found in the reference implementation + * available here: https://github.com/tdunning/t-digest + */ +inline double normalizer_fn(double compression, double n) { + return (compression / Z(compression, n)); +} + +/** + * C++ translation of the "k_2" version found in the reference implementation + * available here: https://github.com/tdunning/t-digest + */ +inline double k(double q, double normalizer) { + const double q_min = 1e-15; + const double q_max = 1 - q_min; + if (q < q_min) { + return (2 * k(q_min, normalizer)); + } else if (q > q_max) { + return (2 * k(q_max, normalizer)); + } + + return (log(q / (1 - q)) * normalizer); +} + +/** + * C++ translation of the "k_2" version found in the reference implementation + * available here: https://github.com/tdunning/t-digest + */ +inline double q(double k, double normalizer) { + double w = exp(k / normalizer); + return (w / (1 + w)); +} + +/** + * Based on the equivalent function in the reference implementation available here: + * https://github.com/tdunning/t-digest + */ +template +void TDigest::merge() { + auto& inactive = (&one == active) ? two : one; + + // buffer.values is pre-seeded with the current digest after each merge, so the + // presence of centroids here does not mean there are new samples to merge. + if (buffer.total_weight == 0) { + return; + } + + auto& inputs = buffer.values; + + if (run_forward) { + std::sort(inputs.begin(), inputs.end(), std::less()); + } else { + std::sort(inputs.begin(), inputs.end(), std::greater()); + } + + const Weight new_total_weight = buffer.total_weight + active->total_weight; + const double normalizer = normalizer_fn(inactive.values.capacity(), new_total_weight); + double k1 = k(0, normalizer); + double next_q_limit_weight = new_total_weight * q(k1 + 1, normalizer); + + double weight_so_far = 0; + double weight_to_add = inputs.front().weight; + double mean_to_add = inputs.front().mean; + + auto compress_fn = [&inactive, new_total_weight, &k1, normalizer, &next_q_limit_weight, + &weight_so_far, &weight_to_add, &mean_to_add](const centroid_t& current) { + if ((weight_so_far + weight_to_add + current.weight) <= next_q_limit_weight) { + weight_to_add += current.weight; + assert(weight_to_add); + mean_to_add = + mean_to_add + (current.mean - mean_to_add) * current.weight / weight_to_add; + + } else { + weight_so_far += weight_to_add; + + double new_q = + static_cast(weight_so_far) / static_cast(new_total_weight); + k1 = k(new_q, normalizer); + next_q_limit_weight = new_total_weight * q(k1 + 1, normalizer); + + if constexpr (std::is_integral::value) { + mean_to_add = std::round(mean_to_add); + } + inactive.insert(mean_to_add, weight_to_add); + mean_to_add = current.mean; + weight_to_add = current.weight; + } + }; + + std::for_each(inputs.begin() + 1, inputs.end(), compress_fn); + + if (weight_to_add != 0) { + inactive.insert(mean_to_add, weight_to_add); + } + + if (!run_forward) { + std::sort(inactive.values.begin(), inactive.values.end()); + } + run_forward = !run_forward; + + buffer.reset(); + + // Seed buffer with the current t-digest in preparation for the next + // merge. + inputs.assign(inactive.values.begin(), inactive.values.end()); + + auto new_inactive = active; + active = &inactive; + new_inactive->reset(); +} + +/** + * XXX: replace with std::lerp when C++20 support becomes available. + * This version adapted from libcxx, which is part of the LLVM project + * under an "Apache 2.0 license with LLVM Exceptions." The project can be + * found at https://github.com/llvm-mirror/libcxx + */ +inline double lerp(double a, double b, double t) noexcept { + if ((a <= 0 && b >= 0) || (a >= 0 && b <= 0)) + return t * b + (1 - t) * a; + + if (t == 1) + return b; + const double x = a + t * (b - a); + if ((t > 1) == (b > a)) + return b < x ? x : b; + else + return x < b ? x : b; +} + +/** + * Based on the equivalent function in the reference implementation available here: + * https://github.com/tdunning/t-digest + */ +template +double TDigest::quantile(double p) const { + if (p < 0 || p > 100) { + throw std::out_of_range("Requested quantile must be between 0 and 100."); + } + + if (active->values.empty()) { + return (0); + } else if (active->values.size() == 1) { + return (active->values.front().mean); + } + + const double total_weight = static_cast(active->total_weight); + const double index = (p / 100.0) * total_weight; + + if (index < 1.0) { + return (min_val); + } + + // For smaller quantiles, interpolate between minimum value and the first + // centroid. + const auto& first = active->values.front(); + const double first_half_weight = static_cast(first.weight) / 2.0; + if (first.weight > 1 && first_half_weight > 1.0 && index < first_half_weight) { + return (lerp(min_val, first.mean, (index - 1.0) / (first_half_weight - 1.0))); + } + + if (index > total_weight - 1.0) { + return (max_val); + } + + // For larger quantiles, interpolate between maximum value and the last + // centroid. + const auto& last = active->values.back(); + const double last_half_weight = static_cast(last.weight) / 2.0; + if (last.weight > 1 && last_half_weight > 1.0 && total_weight - index <= last_half_weight) { + return ( + max_val + - (total_weight - index - 1.0) / (last_half_weight - 1.0) * (max_val - last.mean)); + } + + double weight_so_far = first_half_weight; + double quantile = 0.0; + auto quantile_fn = [index, &weight_so_far, + &quantile](const centroid_t& left, const centroid_t& right) { + const double delta_weight = + (static_cast(left.weight) + static_cast(right.weight)) / 2.0; + if (weight_so_far + delta_weight > index) { + const double lower = index - weight_so_far; + const double upper = weight_so_far + delta_weight - index; + + quantile = (left.mean * upper + right.mean * lower) / (lower + upper); + + return (true); + } + + weight_so_far += delta_weight; + return (false); + }; + + // Even though we're using adjacent_find here, we don't actually intend to find + // anything. We just want to iterate over pairs of centroids until we calculate + // the quantile. + auto it = std::adjacent_find(active->values.begin(), active->values.end(), quantile_fn); + + // Did we fail to find a pair of bracketing centroids? + if (it == active->values.end()) { + // Must be between max_val and the last centroid. + return active->values.back().mean; + } + + return (quantile); +} + +/** + * Based on the equivalent function in the reference implementation available here: + * https://github.com/tdunning/t-digest + */ +template +double TDigest::cumulative_distribution(Values x) const { + if (active->values.empty()) { + return (1.0); + } + + if (active->values.size() == 1) { + if (x < min_val) { + return (0); + } + if (x > max_val) { + return (1.0); + } + if (max_val == min_val) { + return (0.5); + } + return ((x - min_val) / (max_val - min_val)); + } + + // From here on out we divide by active->total_weight in multiple places + // along several code paths. + // Let's make sure we're not going to divide by zero. + assert(active->total_weight); + const double total_weight = static_cast(active->total_weight); + + // Is x at one of the extremes? + if (x < active->values.front().mean) { + const auto& first = active->values.front(); + if (first.mean - min_val > 0) { + if (x == min_val) + return (0.5 / total_weight); + + const double first_half_weight = static_cast(first.weight) / 2.0; + return ( + (1.0 + (x - min_val) / (first.mean - min_val) * (first_half_weight - 1.0)) + / total_weight); + } + return (0); + } + if (x > active->values.back().mean) { + const auto& last = active->values.back(); + if (max_val - last.mean > 0) { + if (x == max_val) + return (1.0 - 0.5 / total_weight); + + const double last_half_weight = static_cast(last.weight) / 2.0; + const double dq = + (1.0 + (max_val - x) / (max_val - last.mean) * (last_half_weight - 1.0)) + / total_weight; + return (1.0 - dq); + } + return (1.0); + } + + double weight_so_far = 0.0; + double cdf = 0.0; + auto cdf_fn = [x, &weight_so_far, &cdf, + total_weight](const centroid_t& left, const centroid_t& right) { + assert(total_weight); + if (left.mean <= x && x < right.mean) { + // x is bracketed between left and right. + + const double delta_weight = + (static_cast(right.weight) + static_cast(left.weight)) / 2.0; + const double base = weight_so_far + static_cast(left.weight) / 2.0; + + cdf = lerp( + base, base + delta_weight, + static_cast((x - left.mean)) / (right.mean - left.mean)) + / total_weight; + return (true); + } + + weight_so_far += static_cast(left.weight); + return (false); + }; + + auto it = std::adjacent_find(active->values.begin(), active->values.end(), cdf_fn); + + // Did we fail to find a pair of bracketing centroids? + if (it == active->values.end()) { + // Might be between max_val and the last centroid. + if (x == active->values.back().mean) { + return ((1.0 - 0.5 / total_weight)); + } + } + + return (cdf); +} + +} // namespace rmcs_utility diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/thread_config.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/thread_config.hpp new file mode 100644 index 000000000..2192db5a6 --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/thread_config.hpp @@ -0,0 +1,318 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_utility { + +/** + * Parsed thread configuration. + * + * Supported fields in `spec` are `name`, `cpus`, `policy`, `priority`, and `nice`. + * Format: `key=value;key=value;...` + * Empty specs are allowed and represent a no-op configuration. + * Examples: + * - `name=rmcs-ctrl;cpus=3;policy=fifo;priority=80` + * - `cpus=4-7;policy=other;nice=-5` + */ +class ThreadConfig { +public: + /** + * Parse a thread configuration spec. + * + * @param spec Thread configuration spec string. + * @throws std::invalid_argument If the spec is malformed or contains invalid values. + */ + explicit ThreadConfig(std::string_view spec) { + spec = trim(spec); + parse_fields(spec); + validate(spec); + } + + /** + * Parse a thread configuration spec and fill in a default thread name when `name` is unset. + * + * @param spec Thread configuration spec string. + * @param default_name Fallback name used only when `spec` does not configure `name`. + * @throws std::invalid_argument If the spec is malformed, contains invalid values, or + * `default_name` is invalid when used. + */ + ThreadConfig(std::string_view spec, std::string_view default_name) + : ThreadConfig(spec) { + if (name_) + return; + if (const auto invalid_reason = check_name(default_name)) + throw std::invalid_argument(std::string(*invalid_reason)); + name_ = to_string(default_name); + } + + [[nodiscard]] auto name() const -> const std::optional& { return name_; } + [[nodiscard]] auto cpus() const -> const std::optional& { return cpus_; } + [[nodiscard]] auto policy() const -> const std::optional& { return policy_; } + [[nodiscard]] auto priority() const -> const std::optional& { return priority_; } + [[nodiscard]] auto nice() const -> const std::optional& { return nice_; } + + /** + * Apply the stored configuration to the calling thread. + * + * This operation is not atomic. If a later step fails, earlier changes may already have been + * applied to the current thread. + * + * @return `std::expected{}` on success, or an error string describing the + * failing runtime step. + */ + auto apply_to_current_thread() const -> std::expected { + const auto current_thread = pthread_self(); + + if (policy_) { + sched_param param{}; + param.sched_priority = priority_.value_or(0); + const int error_code = pthread_setschedparam(current_thread, *policy_, ¶m); + if (error_code != 0) { + return std::unexpected( + std::format( + "Failed to set thread scheduling policy: {}", std::strerror(error_code))); + } + } + + if (nice_) { + errno = 0; + const auto tid = static_cast(syscall(SYS_gettid)); + if (setpriority(PRIO_PROCESS, tid, *nice_) != 0) + return std::unexpected( + std::format("Failed to set thread nice value: {}", std::strerror(errno))); + } + + if (cpus_) { + const int error_code = + pthread_setaffinity_np(current_thread, sizeof(cpu_set_t), &*cpus_); + if (error_code != 0) + return std::unexpected( + std::format("Failed to set thread affinity: {}", std::strerror(error_code))); + } + + if (name_) { + const int error_code = pthread_setname_np(current_thread, name_->c_str()); + if (error_code != 0) + return std::unexpected( + std::format("Failed to set thread name: {}", std::strerror(error_code))); + } + + return {}; + } + +private: + static auto trim(std::string_view text) -> std::string_view { + while (!text.empty() && (text.front() == ' ' || text.front() == '\t')) + text.remove_prefix(1); + while (!text.empty() && (text.back() == ' ' || text.back() == '\t')) + text.remove_suffix(1); + return text; + } + + static auto to_string(std::string_view text) -> std::string { return std::string{text}; } + + [[noreturn]] static void throw_invalid_spec(std::string_view reason, std::string_view spec) { + throw std::invalid_argument( + std::format("Invalid thread config spec ({}): \"{}\"", reason, spec)); + } + + static auto parse_int(std::string_view value, std::string_view key, std::string_view spec) + -> int { + int result = 0; + const auto* begin = value.data(); + const auto* end = value.data() + value.size(); + const auto [ptr, error_code] = std::from_chars(begin, end, result); + if (error_code != std::errc{} || ptr != end) { + throw_invalid_spec(std::format("invalid integer for key '{}'", key), spec); + } + return result; + } + + static auto check_name(std::string_view name) -> std::optional { + if (name.empty()) + return "Thread name must not be empty"; + if (name.size() > 15) + return std::format("Thread name exceeds 15 characters: \"{}\"", name); + return std::nullopt; + } + + void parse_fields(std::string_view spec) { + bool seen_name = false; + bool seen_cpus = false; + bool seen_policy = false; + bool seen_priority = false; + bool seen_nice = false; + + auto remaining_spec = spec; + while (true) { + const auto separator = remaining_spec.find(';'); + const auto field = trim(remaining_spec.substr(0, separator)); + + if (!field.empty()) { + const auto equals = field.find('='); + if (equals == std::string_view::npos) + throw_invalid_spec("missing '='", spec); + + const auto key = trim(field.substr(0, equals)); + const auto value = trim(field.substr(equals + 1)); + if (key.empty() || value.empty()) + throw_invalid_spec("empty key or value", spec); + + assign_field( + key, value, spec, seen_name, seen_cpus, seen_policy, seen_priority, seen_nice); + } + + if (separator == std::string_view::npos) + break; + remaining_spec.remove_prefix(separator + 1); + } + } + + void assign_field( + std::string_view key, std::string_view value, std::string_view spec, bool& seen_name, + bool& seen_cpus, bool& seen_policy, bool& seen_priority, bool& seen_nice) { + if (key == "name") { + if (seen_name) + throw_invalid_spec("duplicate key 'name'", spec); + seen_name = true; + if (const auto invalid_reason = check_name(value)) + throw_invalid_spec(*invalid_reason, spec); + name_ = to_string(value); + return; + } + if (key == "cpus") { + if (seen_cpus) + throw_invalid_spec("duplicate key 'cpus'", spec); + seen_cpus = true; + cpus_ = parse_cpu_list(value, spec); + return; + } + if (key == "policy") { + if (seen_policy) + throw_invalid_spec("duplicate key 'policy'", spec); + seen_policy = true; + policy_ = parse_policy(value, spec); + return; + } + if (key == "priority") { + if (seen_priority) + throw_invalid_spec("duplicate key 'priority'", spec); + seen_priority = true; + priority_ = parse_int(value, key, spec); + return; + } + if (key == "nice") { + if (seen_nice) + throw_invalid_spec("duplicate key 'nice'", spec); + seen_nice = true; + nice_ = parse_int(value, key, spec); + return; + } + throw_invalid_spec("unknown key '" + to_string(key) + "'", spec); + } + + static auto parse_policy(std::string_view value, std::string_view spec) -> int { + if (value == "other") + return SCHED_OTHER; + if (value == "batch") + return SCHED_BATCH; + if (value == "idle") + return SCHED_IDLE; + if (value == "fifo") + return SCHED_FIFO; + if (value == "rr") + return SCHED_RR; + throw_invalid_spec("unknown policy", spec); + } + + static auto parse_cpu_list(std::string_view value, std::string_view spec) -> cpu_set_t { + cpu_set_t cpus; + CPU_ZERO(&cpus); + + bool has_any_cpu = false; + while (true) { + const auto separator = value.find(','); + const auto token = trim(value.substr(0, separator)); + if (token.empty()) + throw_invalid_spec("empty cpu list token", spec); + + const auto dash = token.find('-'); + if (dash == std::string_view::npos) { + const int cpu = parse_int(token, "cpus", spec); + set_cpu_range(cpus, cpu, cpu, spec); + } else { + if (token.find('-', dash + 1) != std::string_view::npos) + throw_invalid_spec("invalid cpu range token", spec); + const int first_cpu = parse_int(trim(token.substr(0, dash)), "cpus", spec); + const int last_cpu = parse_int(trim(token.substr(dash + 1)), "cpus", spec); + set_cpu_range(cpus, first_cpu, last_cpu, spec); + } + + has_any_cpu = true; + if (separator == std::string_view::npos) + break; + value.remove_prefix(separator + 1); + } + + if (!has_any_cpu) + throw_invalid_spec("empty cpu list", spec); + return cpus; + } + + static void set_cpu_range(cpu_set_t& cpus, int first_cpu, int last_cpu, std::string_view spec) { + if (first_cpu < 0 || last_cpu < 0 || first_cpu > last_cpu) + throw_invalid_spec("invalid cpu range", spec); + if (last_cpu >= CPU_SETSIZE) + throw_invalid_spec("cpu index exceeds CPU_SETSIZE", spec); + + for (int cpu = first_cpu; cpu <= last_cpu; ++cpu) + CPU_SET(cpu, &cpus); + } + + void validate(std::string_view spec) const { + if (priority_ && !policy_) + throw_invalid_spec("priority requires policy", spec); + + if (policy_) { + const bool is_realtime = *policy_ == SCHED_FIFO || *policy_ == SCHED_RR; + if (is_realtime) { + if (!priority_) + throw_invalid_spec("realtime policy requires priority", spec); + if (nice_) + throw_invalid_spec("realtime policy cannot use nice", spec); + + const int min_priority = sched_get_priority_min(*policy_); + const int max_priority = sched_get_priority_max(*policy_); + if (*priority_ < min_priority || *priority_ > max_priority) + throw_invalid_spec("priority out of range for policy", spec); + } else if (priority_) { + throw_invalid_spec("non-realtime policy cannot use priority", spec); + } + } + + if (nice_ && (*nice_ < -20 || *nice_ > 19)) + throw_invalid_spec("nice out of range", spec); + } + + std::optional name_; + std::optional cpus_; + std::optional policy_; + std::optional priority_; + std::optional nice_; +}; + +} // namespace rmcs_utility