SAR '26

COMMANDS.txt

@@ -0,0 +1,25 @@
+# NAVIGATOR: to the right of bay door
+ros2 run navigator navigator_node --ros-args -p latitude:=35.2101718 -p longitude:=-97.4414963 -p mode:=0
+
+# NAVIGATOR: straight ahead of bay door
+ros2 run navigator navigator_node --ros-args -p latitude:=35.2103225 -p longitude:=-97.4411651 -p mode:=0
+
+# NAVIGATOR: end of repf closest segment
+ros2 run navigator navigator_node --ros-args -p latitude:=35.2105681 -p longitude:=-97.4419394 -p mode:=0
+
+# NAVIGATOR: aruco marker placed in equad (2026-02-25 5:09 PM)
+ros2 run navigator navigator_node --ros-args -p latitude:=35.2103889 -p longitude:=-97.4418893 -p mode:=0
+
+# NAVIGATOR: aruco marker placed in equad (2026-02-25 6:08 PM)
+ros2 run navigator navigator_node --ros-args -p latitude:=35.2106322 -p longitude:=-97.4419303 -p mode:=0
+
+ros2 launch drive_launcher rover.launch.py use_sim_time:=False
+
+ros2 launch drive_launcher ebox.launch.py
+
+
+# For RVIZ:
+# terminal 1
+Xvfb :99 -screen 0 1920x1080x24 & export DISPLAY=:99 & gnome-session --session=ubuntu & rviz2
+# terminal 2
+x11vnc -display :99

lib/feedback/src/lib.rs

@@ -45,7 +45,7 @@ impl Wheels {
             right,
 
             // see electrical ebox teensy code
-            checksum: 255_u8.overflowing_add(left + right).0,
+            checksum: 255_u8.wrapping_add(left.wrapping_add(right)),
         }
     }
 }

pyproject.toml

@@ -31,6 +31,7 @@ exclude = [
     "src/**/build/",
     "**/__pycache__",
     "**/.*",
+    "src/aruco/extra_scripts/aruco_visualizer.py",
 ]
 
 [tool.pixi.workspace]

scripts/get_navsat_yaw_offset.sh

@@ -0,0 +1,179 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+usage() {
+    cat <<'EOF'
+Usage:
+  scripts/get_navsat_yaw_offset.sh --heading-deg <deg> [options]
+
+Computes a recommended /navsat_transform_node yaw_offset from live IMU data.
+
+Heading convention for --heading-deg:
+  0 = North, 90 = East, 180 = South, 270 = West.
+
+Options:
+  --topic <topic>       IMU topic to sample (default: /unilidar/imu)
+  --duration <seconds>  Sampling time (default: 10)
+  --node <name>         Node used by --apply (default: /navsat_transform_node)
+  --apply               Apply computed yaw_offset live with ros2 param set
+  --no-source           Skip sourcing install/setup.bash
+  -h, --help            Show this help
+EOF
+}
+
+topic="/unilidar/imu"
+duration="10"
+node="/navsat_transform_node"
+apply_now=0
+auto_source=1
+heading_deg=""
+
+while [[ $# -gt 0 ]]; do
+    case "$1" in
+    --heading-deg)
+        heading_deg="${2:-}"
+        shift 2
+        ;;
+    --topic)
+        topic="${2:-}"
+        shift 2
+        ;;
+    --duration)
+        duration="${2:-}"
+        shift 2
+        ;;
+    --node)
+        node="${2:-}"
+        shift 2
+        ;;
+    --apply)
+        apply_now=1
+        shift
+        ;;
+    --no-source)
+        auto_source=0
+        shift
+        ;;
+    -h | --help)
+        usage
+        exit 0
+        ;;
+    *)
+        echo "Unknown option: $1" >&2
+        usage >&2
+        exit 2
+        ;;
+    esac
+done
+
+if [[ -z "${heading_deg}" ]]; then
+    echo "Missing required --heading-deg <deg>" >&2
+    usage >&2
+    exit 2
+fi
+
+if ! [[ "${heading_deg}" =~ ^-?[0-9]+([.][0-9]+)?$ ]]; then
+    echo "--heading-deg must be numeric" >&2
+    exit 2
+fi
+
+if ! [[ "${duration}" =~ ^[0-9]+([.][0-9]+)?$ ]]; then
+    echo "--duration must be numeric" >&2
+    exit 2
+fi
+
+if [[ "${auto_source}" -eq 1 ]]; then
+    script_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+    workspace_root="$(cd "${script_dir}/.." && pwd)"
+    setup_file="${workspace_root}/install/setup.bash"
+    if [[ -f "${setup_file}" ]]; then
+        set +u
+        # shellcheck disable=SC1090
+        source "${setup_file}"
+        set -u
+    else
+        echo "warning: ${setup_file} not found, assuming ROS env already sourced" >&2
+    fi
+fi
+
+if ! command -v ros2 >/dev/null 2>&1; then
+    echo "ros2 not found in PATH. Source your ROS environment first." >&2
+    exit 1
+fi
+
+awk_program='
+function wrap_deg(a) { while (a > 180.0) a -= 360.0; while (a <= -180.0) a += 360.0; return a }
+function wrap_rad(a) {
+    pi = atan2(0, -1)
+    while (a > pi) a -= 2.0 * pi
+    while (a <= -pi) a += 2.0 * pi
+    return a
+}
+BEGIN {
+    pi = atan2(0, -1)
+    n = 0
+    sum_s = 0.0
+    sum_c = 0.0
+    sum_wz = 0.0
+}
+NF >= 19 {
+    x = $4 + 0.0
+    y = $5 + 0.0
+    z = $6 + 0.0
+    w = $7 + 0.0
+    yaw = atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))
+    sum_s += sin(yaw)
+    sum_c += cos(yaw)
+    sum_wz += ($19 + 0.0)
+    n++
+}
+END {
+    if (n < 1) {
+        exit 2
+    }
+
+    mean_yaw = atan2(sum_s / n, sum_c / n)
+    mean_yaw_deg = mean_yaw * 180.0 / pi
+    expected_yaw_deg = wrap_deg(90.0 - (heading_deg + 0.0))
+    offset_deg = wrap_deg(expected_yaw_deg - mean_yaw_deg)
+    offset_rad = wrap_rad(offset_deg * pi / 180.0)
+
+    R = sqrt((sum_c / n) * (sum_c / n) + (sum_s / n) * (sum_s / n))
+    if (R <= 0.0) {
+        yaw_spread_deg = 180.0
+    } else {
+        yaw_spread_deg = sqrt(-2.0 * log(R)) * 180.0 / pi
+    }
+
+    printf("samples=%d\n", n)
+    printf("mean_imu_yaw_deg=%.6f\n", mean_yaw_deg)
+    printf("yaw_spread_deg=%.6f\n", yaw_spread_deg)
+    printf("expected_yaw_deg=%.6f\n", expected_yaw_deg)
+    printf("recommended_yaw_offset_deg=%.6f\n", offset_deg)
+    printf("recommended_yaw_offset_rad=%.6f\n", offset_rad)
+    printf("mean_gyro_z_rad_s=%.6f\n", sum_wz / n)
+}
+'
+
+if ! stats="$(
+    {
+        timeout "${duration}" ros2 topic echo "${topic}" --csv 2>/dev/null || true
+    } | awk -F, -v heading_deg="${heading_deg}" "${awk_program}"
+)"; then
+    echo "No IMU samples received from ${topic} in ${duration}s." >&2
+    exit 1
+fi
+
+echo "${stats}"
+offset_rad="$(awk -F= '/^recommended_yaw_offset_rad=/{print $2}' <<<"${stats}")"
+
+if [[ -z "${offset_rad}" ]]; then
+    echo "Failed to compute recommended_yaw_offset_rad." >&2
+    exit 1
+fi
+
+echo "apply_cmd=ros2 param set ${node} yaw_offset ${offset_rad}"
+
+if [[ "${apply_now}" -eq 1 ]]; then
+    ros2 param set "${node}" yaw_offset "${offset_rad}"
+fi