Blame for cpp_lowcmd_bridge/src/h2_lowcmd_shm_bridge.cpp

#include <algorithm>
#include <array>
#include <atomic>
#include <cctype>
#include <cerrno>
#include <chrono>
#include <cmath>
#include <condition_variable>
#include <cstddef>
#include <cstdint>
#include <cstdlib>
#include <cstring>
#include <fcntl.h>
#include <fstream>
#include <iostream>
#include <limits>
#include <memory>
#include <mutex>
#include <net/if.h>
#include <sstream>
#include <stdexcept>
#include <string>
#include <thread>
#include <sys/file.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <unistd.h>

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/float64_multi_array.hpp>
#include <unitree_hg/msg/low_cmd.hpp>
#include <unitree_hg/msg/low_state.hpp>

#include "cpp_lowcmd_bridge/gamepad.hpp"
#include "cpp_lowcmd_bridge/motor_crc_hg.hpp"
#include "cpp_lowcmd_bridge/timing_trace.hpp"

namespace {

using namespace std::chrono_literals;

constexpr char kShmName[] = "/h2_pingpong_bridge";
constexpr uint32_t kMagic = 0x48324252;  // H2BR
// cmd_q carries the policy's finite raw learned desired target.  The bridge
// retains that target and projects it only into the latest PD-effort interval
// at 1000 Hz, matching G1/MuJoCo actuator-effort saturation semantics.
constexpr uint32_t kVersion = 7;
constexpr char kJointMapRevision[] = "unitree_sdk2@e02dc701-h2-cpp";
constexpr int kHwDof = 31;
constexpr int kLowCmdDof = 35;
constexpr int kTtDataLen = 32;
constexpr int kTtProtocolLenV3 = 27;
constexpr int kWirelessRemoteLen = 40;
constexpr uint8_t kModePr = 0;
constexpr uint64_t kPublisherDiscoveryNs = 1000000000ULL;
constexpr uint64_t kPublisherRecheckNs = 1000000000ULL;
constexpr float kMinQuaternionNorm = 0.5F;
constexpr float kMaxQuaternionNorm = 1.5F;
constexpr float kMaxKp = 1000.0F;
constexpr float kMaxKd = 100.0F;
constexpr uint8_t kPythonFaultRequest = 0x01U;
constexpr uint8_t kMotionSwitcherAttested = 0x02U;
constexpr float kJointMargin = 0.01F;
constexpr float kStateLimitTolerance = 0.05F;
constexpr float kMaxJointVelocity = 40.0F;
constexpr float kHoldTorqueFraction = 0.90F;
constexpr std::array<float, kHwDof> kJointLo = {
    -2.4526F, -0.467441F, -2.827F, -0.08725F, -0.349066F, -1.13446F,
    -2.4526F, -2.16886F, -2.827F, -0.08725F, -0.296706F, -1.13446F,
    -1.7453F, -0.5236F, -0.43633F, -2.618F, -0.517F, -2.618F, -0.986F,
    -2.618F, -0.576F, -1.22F, -2.618F, -2.494F, -2.618F, -0.986F,
    -2.618F, -0.576F, -1.22F, -0.5236F, -1.7453F};
constexpr std::array<float, kHwDof> kJointHi = {
    2.77542F, 2.16886F, 2.827F, 2.53025F, 0.296706F, 0.610865F,
    2.77542F, 0.467441F, 2.827F, 2.53025F, 0.349066F, 0.610865F,
    1.7453F, 0.5236F, 0.5236F, 1.833F, 2.494F, 2.618F, 3.071F,
    2.618F, 0.576F, 1.22F, 1.833F, 0.517F, 2.618F, 3.071F,
    2.618F, 0.576F, 1.22F, 0.83775F, 1.7453F};
constexpr std::array<float, kHwDof> kTorqueLimit = {
    360.0F, 360.0F, 360.0F, 360.0F, 19.0F, 66.88F,
    360.0F, 360.0F, 360.0F, 360.0F, 19.0F, 66.88F,
    120.0F, 180.0F, 180.0F, 130.0F, 60.0F, 60.0F, 60.0F,
    60.0F, 10.0F, 10.0F, 130.0F, 60.0F, 60.0F, 60.0F,
    60.0F, 10.0F, 10.0F, 50.0F, 50.0F};

#pragma pack(push, 8)
struct BridgeMemory {
  uint32_t magic;
  uint32_t version;
  uint64_t state_seq;
  uint64_t command_seq;
  uint64_t state_stamp_ns;
  uint64_t command_stamp_ns;
  uint64_t tt_stamp_ns;
  uint8_t robot_connected;
  uint8_t state_ready;
  uint8_t tt_ready;
  uint8_t start_requested;
  uint8_t stop_requested;
  uint8_t emergency_stop;
  uint8_t command_enabled;
  uint8_t reserved_flags;
  uint8_t state_mode_pr;
  uint8_t mode_machine;
  uint8_t cmd_mode_pr;
  uint8_t pad0;
  uint32_t bridge_interface_hash;
  float joint_pos[kHwDof];
  float joint_vel[kHwDof];
  float imu_quat_wxyz[4];
  float gyro[3];
  float root_current_xy[2];
  float mocap_pelvis_quat_xyzw[4];
  double tt_data[kTtDataLen];
  uint8_t wireless_remote[kWirelessRemoteLen];
  float cmd_q[kLowCmdDof];
  float cmd_dq[kLowCmdDof];
  float cmd_tau[kLowCmdDof];
  float cmd_kp[kLowCmdDof];
  float cmd_kd[kLowCmdDof];
  uint8_t cmd_mode[kLowCmdDof];
  uint32_t cmd_reserve[kLowCmdDof];
};
#pragma pack(pop)

static_assert(sizeof(BridgeMemory) == 1544, "Python/C++ H2 shared-memory ABI mismatch");
static_assert(offsetof(BridgeMemory, command_seq) == 16, "H2 ABI command_seq offset mismatch");
static_assert(
    offsetof(BridgeMemory, bridge_interface_hash) == 60,
    "H2 ABI bridge_interface_hash offset mismatch");
static_assert(offsetof(BridgeMemory, joint_pos) == 64, "H2 ABI joint_pos offset mismatch");
static_assert(offsetof(BridgeMemory, tt_data) == 368, "H2 ABI tt_data offset mismatch");
static_assert(offsetof(BridgeMemory, cmd_q) == 664, "H2 ABI cmd_q offset mismatch");
static_assert(offsetof(BridgeMemory, cmd_mode) == 1364, "H2 ABI cmd_mode offset mismatch");
static_assert(offsetof(BridgeMemory, cmd_reserve) == 1400, "H2 ABI cmd_reserve offset mismatch");

uint64_t NowNs() {
  return h2_timing::NowNs();
}

uint32_t InterfaceHash(const std::string& interface) {
  uint32_t value = 2166136261U;
  for (const unsigned char byte : interface) {
    value ^= static_cast<uint32_t>(byte);
    value *= 16777619U;
  }
  if (value == 0U) throw std::runtime_error("network interface hash unexpectedly equals zero");
  return value;
}

struct Args {
  // ROS 2 adds its own DDS "rt/" prefix.  The H2 wire topics are
  // rt/lowcmd and rt/lowstate, so the ROS names must be /lowcmd and
  // /lowstate rather than /rt/lowcmd and /rt/lowstate.
  std::string lowcmd_topic = "/lowcmd";
  std::string lowstate_topic = "/lowstate";
  std::string tt_topic = "/wbc_racket_command";
  std::string network_interface;
  double publish_hz = 1000.0;
  double command_timeout_s = 0.10;
  double state_timeout_s = 0.10;
  bool replace_stale_shm = false;
  bool print_contract = false;
  bool validate_network_only = false;
};

Args ParseArgs(int argc, char** argv) {
  Args args;
  for (int index = 1; index < argc; ++index) {
    const std::string key(argv[index]);
    auto value = [&]() -> std::string {
      if (++index >= argc) throw std::runtime_error("missing value for " + key);
      return argv[index];
    };
    if (key == "--lowcmd-topic") args.lowcmd_topic = value();
    else if (key == "--lowstate-topic") args.lowstate_topic = value();
    else if (key == "--tt-topic") args.tt_topic = value();
    else if (key == "--network-interface") args.network_interface = value();
    else if (key == "--publish-hz") args.publish_hz = std::stod(value());
    else if (key == "--command-timeout-s") args.command_timeout_s = std::stod(value());
    else if (key == "--state-timeout-s") args.state_timeout_s = std::stod(value());
    else if (key == "--replace-stale-shm") args.replace_stale_shm = true;
    else if (key == "--print-contract") args.print_contract = true;
    else if (key == "--validate-network-only") args.validate_network_only = true;
    else if (key == "--help" || key == "-h") {
      std::cout
          << "h2_lowcmd_shm_bridge [--lowcmd-topic /lowcmd] [--lowstate-topic /lowstate]\n"
          << "  --network-interface <H2-interface> [--tt-topic /wbc_racket_command]\n"
          << "  [--publish-hz 1000]\n"
          << "  [--command-timeout-s 0.10] [--state-timeout-s 0.10] [--replace-stale-shm]\n"
          << "  [--print-contract] [--validate-network-only]\n";
      std::exit(0);
    } else {
      throw std::runtime_error("unknown argument: " + key);
    }
  }
  if (!(args.publish_hz > 0.0 && args.publish_hz <= 1000.0)) {
    throw std::runtime_error("publish rate must be in (0,1000] Hz");
  }
  if (!(args.command_timeout_s >= 0.02 && args.command_timeout_s <= 0.10)) {
    throw std::runtime_error("command timeout must be in [0.02,0.10] seconds");
  }
  if (!(args.state_timeout_s >= 0.02 && args.state_timeout_s <= 0.10)) {
    throw std::runtime_error("state timeout must be in [0.02,0.10] seconds");
  }
  if (!args.print_contract && args.network_interface.empty()) {
    throw std::runtime_error("--network-interface is required for live bridge startup");
  }
  return args;
}

void ValidateNetworkEnvironment(const std::string& interface) {
  if (interface.empty() || interface.size() >= IFNAMSIZ ||
      !std::all_of(interface.begin(), interface.end(), [](unsigned char character) {
        return std::isalnum(character) != 0 || character == '_' || character == '-' ||
               character == '.';
      })) {
    throw std::runtime_error("invalid --network-interface value");
  }
  if (if_nametoindex(interface.c_str()) == 0U) {
    throw std::runtime_error("network interface does not exist: " + interface);
  }
  std::ifstream state_file("/sys/class/net/" + interface + "/operstate");
  std::string state;
  state_file >> state;
  if (!state_file || state != "up") {
    throw std::runtime_error("network interface is not UP: " + interface);
  }
  const char* rmw = std::getenv("RMW_IMPLEMENTATION");
  if (rmw == nullptr || std::string(rmw) != "rmw_cyclonedds_cpp") {
    throw std::runtime_error("RMW_IMPLEMENTATION must be exactly rmw_cyclonedds_cpp");
  }
  const std::string expected_uri =
      "<CycloneDDS><Domain><General><Interfaces><NetworkInterface name='" + interface +
      "' priority='default' multicast='default'/></Interfaces></General></Domain></CycloneDDS>";
  const char* uri = std::getenv("CYCLONEDDS_URI");
  if (uri == nullptr || std::string(uri) != expected_uri) {
    throw std::runtime_error(
        "CYCLONEDDS_URI does not exactly bind the declared interface; source this project's "
        "scripts/source_unitree_env.sh first");
  }
}

class FileLock {
 public:
  FileLock(int fd, int operation) : fd_(fd) {
    int result;
    do {
      result = flock(fd_, operation | LOCK_NB);
    } while (result != 0 && errno == EINTR);
    locked_ = result == 0;
  }

  ~FileLock() {
    if (!locked_) return;
    int result;
    do {
      result = flock(fd_, LOCK_UN);
    } while (result != 0 && errno == EINTR);
  }

  FileLock(const FileLock&) = delete;
  FileLock& operator=(const FileLock&) = delete;

  bool locked() const { return locked_; }

 private:
  int fd_;
  bool locked_{false};
};

class SharedMemoryOwner {
 public:
  SharedMemoryOwner(bool replace_stale, uint32_t interface_hash) {
    if (replace_stale) shm_unlink(kShmName);
    fd_ = shm_open(kShmName, O_CREAT | O_EXCL | O_RDWR, 0660);
    if (fd_ < 0) {
      throw std::runtime_error(
          "cannot create /dev/shm/h2_pingpong_bridge (another bridge or stale segment exists; "
          "inspect it, then use --replace-stale-shm only when no bridge is running)");
    }
    if (ftruncate(fd_, sizeof(BridgeMemory)) != 0) {
      throw std::runtime_error("cannot size H2 shared memory");
    }
    void* ptr = mmap(nullptr, sizeof(BridgeMemory), PROT_READ | PROT_WRITE, MAP_SHARED, fd_, 0);
    if (ptr == MAP_FAILED) throw std::runtime_error("mmap failed for H2 shared memory");
    mem_ = static_cast<BridgeMemory*>(ptr);
    std::memset(mem_, 0, sizeof(BridgeMemory));
    mem_->magic = kMagic;
    mem_->version = kVersion;
    mem_->bridge_interface_hash = interface_hash;
    mem_->state_mode_pr = std::numeric_limits<uint8_t>::max();
    mem_->cmd_mode_pr = kModePr;
    mem_->imu_quat_wxyz[0] = 1.0F;
    mem_->mocap_pelvis_quat_xyzw[3] = 1.0F;
  }

  ~SharedMemoryOwner() {
    if (mem_ != nullptr) munmap(mem_, sizeof(BridgeMemory));
    if (fd_ >= 0) close(fd_);
    shm_unlink(kShmName);
  }

  BridgeMemory* get() { return mem_; }
  int fd() const { return fd_; }

 private:
  int fd_{-1};
  BridgeMemory* mem_{nullptr};
};

struct Command {
  uint64_t sequence{0};
  uint64_t stamp_ns{0};
  uint8_t mode_pr{kModePr};
  std::array<float, kLowCmdDof> q{};
  std::array<float, kLowCmdDof> dq{};
  std::array<float, kLowCmdDof> tau{};
  std::array<float, kLowCmdDof> kp{};
  std::array<float, kLowCmdDof> kd{};
  std::array<uint8_t, kLowCmdDof> mode{};
  std::array<uint32_t, kLowCmdDof> reserve{};
};

enum class PublisherTopology : uint8_t {
  kDiscovering,
  kReady,
  kBlocked,
};

struct StateSnapshot {
  std::array<float, kHwDof> q{};
  std::array<float, kHwDof> dq{};
  std::array<float, kHwDof> watchdog_q{};
  uint8_t mode_pr{std::numeric_limits<uint8_t>::max()};
  uint8_t mode_machine{0};
  uint64_t stamp_ns{0};
  bool got_state{false};
  bool have_valid_state{false};
  bool usable{false};
  bool output_inhibited{false};
  bool hold_available{false};
};

class H2BridgeNode final : public rclcpp::Node {
 public:
  H2BridgeNode(const Args& args, BridgeMemory* mem, int shm_fd)
      : Node("h2_lowcmd_shm_bridge"),
        args_(args),
        mem_(mem),
        shm_fd_(shm_fd) {
    lowstate_sub_ = create_subscription<unitree_hg::msg::LowState>(
        args_.lowstate_topic, rclcpp::QoS(10),
        [this](const unitree_hg::msg::LowState::SharedPtr msg) { OnLowState(msg); });
    planner_sub_ = create_subscription<std_msgs::msg::Float64MultiArray>(
        args_.tt_topic, rclcpp::QoS(1),
        [this](const std_msgs::msg::Float64MultiArray::SharedPtr msg) { OnPlanner(msg); });
    lowcmd_pub_ = create_publisher<unitree_hg::msg::LowCmd>(args_.lowcmd_topic, rclcpp::QoS(10));
    RCLCPP_INFO(
        get_logger(),
        "waiting for H2 LowState; bridge publishes nothing until a valid Python command arrives");
  }

  ~H2BridgeNode() override { StopWorkers(); }

  void StartWorkers() {
    bool expected = false;
    if (!workers_running_.compare_exchange_strong(expected, true)) return;
    try {
      command_thread_ = std::thread([this]() { CommandLoop(); });
      topology_thread_ = std::thread([this]() { TopologyLoop(); });
    } catch (...) {
      StopWorkers();
      throw;
    }
  }

  void StopWorkers() noexcept {
    workers_running_.store(false);
    worker_cv_.notify_all();
    if (command_thread_.joinable()) command_thread_.join();
    if (topology_thread_.joinable()) topology_thread_.join();
  }

 private:
  void OnPlanner(const std_msgs::msg::Float64MultiArray::SharedPtr msg) {
    const size_t count = std::min(msg->data.size(), static_cast<size_t>(kTtDataLen));
    std::lock_guard<std::mutex> lock(planner_mutex_);
    planner_data_.fill(0.0);
    const bool finite = std::all_of(
        msg->data.begin(), msg->data.begin() + count,
        [](double value) { return std::isfinite(value); });
    if (finite) std::copy_n(msg->data.begin(), count, planner_data_.begin());
    planner_ready_ =
        finite && msg->data.size() == kTtProtocolLenV3 && msg->data[20] == 3.0 && msg->data[21] > 0.0 &&
        msg->data[22] > 0.0 && msg->data[26] == 1.0;
    planner_stamp_ns_ = NowNs();
  }

  static bool LowStateFinite(
      const unitree_hg::msg::LowState& msg, std::array<float, kHwDof>* q,
      std::array<float, kHwDof>* dq, std::array<float, 4>* quaternion,
      std::array<float, 3>* gyro) {
    if (!low_state_crc_valid(msg)) return false;
    for (int index = 0; index < kLowCmdDof; ++index) {
      const auto& motor = msg.motor_state.at(index);
      if (!std::isfinite(motor.q) || !std::isfinite(motor.dq) || !std::isfinite(motor.ddq) ||
          !std::isfinite(motor.tau_est) || !std::isfinite(motor.vol)) {
        return false;
      }
      if (index < kHwDof) {
        if (motor.q < kJointLo[index] - kStateLimitTolerance ||
            motor.q > kJointHi[index] + kStateLimitTolerance ||
            std::abs(motor.dq) > kMaxJointVelocity) {
          return false;
        }
        q->at(index) = motor.q;
        dq->at(index) = motor.dq;
      }
    }

    float norm_squared = 0.0F;
    for (int index = 0; index < 4; ++index) {
      const float value = msg.imu_state.quaternion.at(index);
      if (!std::isfinite(value)) return false;
      quaternion->at(index) = value;
      norm_squared += value * value;
    }
    const float norm = std::sqrt(norm_squared);
    if (!std::isfinite(norm) || norm < kMinQuaternionNorm || norm > kMaxQuaternionNorm) {
      return false;
    }
    for (int index = 0; index < 3; ++index) {
      const float gyro_value = msg.imu_state.gyroscope.at(index);
      if (!std::isfinite(gyro_value) || !std::isfinite(msg.imu_state.accelerometer.at(index)) ||
          !std::isfinite(msg.imu_state.rpy.at(index))) {
        return false;
      }
      gyro->at(index) = gyro_value;
    }
    return true;
  }

  void SyncLatchedFlagsLocked() {
    if (start_request_latched_.load()) mem_->start_requested = 1;
    if (stop_request_latched_.load()) mem_->stop_requested = 1;
    if (fault_latched_.load()) mem_->emergency_stop = 1;
  }

  void SyncLatchedFlags() {
    if (!latched_flags_dirty_.exchange(false)) return;
    // flock() locks belong to an open-file description.  Every use of the
    // shared fd is therefore serialized inside this process before taking the
    // cross-process lock; otherwise two C++ threads could convert or release
    // each other's flock.
    std::lock_guard<std::mutex> local_lock(shm_mutex_);
    FileLock lock(shm_fd_, LOCK_EX);
    if (lock.locked()) {
      SyncLatchedFlagsLocked();
    } else {
      latched_flags_dirty_.store(true);
    }
  }

  void LatchFault(
      const std::string& reason, bool stop_requested = false,
      bool invalidate_state = false, bool inhibit_output = false,
      int observed_mode_pr = -1) {
    bool first_fault = false;
    {
      // This mutex is the publication linearization point.  Once a fault owns
      // it, no later learned LowCmd can pass the final gate.
      std::lock_guard<std::mutex> output_lock(output_mutex_);
      if (stop_requested && !stop_request_latched_.exchange(true)) {
        latched_flags_dirty_.store(true);
      }
      bool expected = false;
      if (fault_latched_.compare_exchange_strong(expected, true)) {
        first_fault = true;
        latched_flags_dirty_.store(true);
      }
      std::lock_guard<std::mutex> state_lock(state_mutex_);
      if (invalidate_state) latest_state_usable_ = false;
      if (inhibit_output) output_inhibited_ = true;
      if (observed_mode_pr >= 0) {
        latest_mode_pr_ = static_cast<uint8_t>(observed_mode_pr);
      }
      if (first_fault && have_valid_state_) {
        watchdog_q_ = latest_q_;
        hold_available_ = true;
      }
    }
    if (first_fault) {
      RCLCPP_ERROR(
          get_logger(), "%s; output is permanently latched until bridge restart", reason.c_str());
    }
    SyncLatchedFlags();
  }

  void OnLowState(const unitree_hg::msg::LowState::SharedPtr msg) {
    if (msg->mode_pr != kModePr) {
      // Ankle values in AB mode do not have PR roll/pitch semantics.  Never
      // capture them as a future PR hold, even if the robot later returns to
      // PR mode during this bridge process.
      LatchFault(
          "LowState left PR(0) mode (mode_pr=" + std::to_string(msg->mode_pr) + ")",
          false, true, true, msg->mode_pr);
      return;
    }
    std::array<float, kHwDof> q{};
    std::array<float, kHwDof> dq{};
    std::array<float, 4> quaternion{};
    std::array<float, 3> gyro{};
    if (!LowStateFinite(*msg, &q, &dq, &quaternion, &gyro)) {
      LatchFault(
          "CRC-invalid, non-finite, or invalid-quaternion H2 LowState",
          false, true);
      return;
    }

    for (int index = 0; index < kHwDof; ++index) {
      if (msg->motor_state.at(index).motorstate != 0U) {
        LatchFault(
            "motor " + std::to_string(index) + " fault code " +
                std::to_string(msg->motor_state.at(index).motorstate),
            false, true);
        return;
      }
    }

    unitree::common::REMOTE_DATA_RX remote{};
    std::memcpy(remote.buff, msg->wireless_remote.data(), kWirelessRemoteLen);
    gamepad_.update(remote.RF_RX);
    if (gamepad_.R1.on_press) {
      LatchFault("R1 stop request", true);
      return;
    }

    // Only a fully validated LowState may replace the last-known-valid state.
    const uint64_t state_stamp_ns = NowNs();
    {
      std::lock_guard<std::mutex> state_lock(state_mutex_);
      latest_q_ = q;
      latest_dq_ = dq;
      latest_mode_pr_ = msg->mode_pr;
      latest_mode_machine_ = msg->mode_machine;
      latest_state_stamp_ns_ = state_stamp_ns;
      got_state_ = true;
      have_valid_state_ = true;
      latest_state_usable_ = true;
    }

    if (gamepad_.L1.on_press) {
      if (!start_request_latched_.exchange(true)) {
        latched_flags_dirty_.store(true);
        RCLCPP_INFO(get_logger(), "L1 start request latched");
      }
    }
    std::array<double, kTtDataLen> planner{};
    bool planner_ready = false;
    uint64_t planner_stamp = 0;
    {
      std::lock_guard<std::mutex> lock(planner_mutex_);
      planner = planner_data_;
      planner_ready = planner_ready_;
      planner_stamp = planner_stamp_ns_;
    }

    // Never let command-side SHM access back-pressure the ROS LowState
    // callback.  The validated internal state above remains current for the
    // watchdog; a missed mirror write is retried by the next ~1 kHz sample.
    std::unique_lock<std::mutex> local_lock(shm_mutex_, std::try_to_lock);
    if (!local_lock.owns_lock()) return;
    FileLock lock(shm_fd_, LOCK_EX);
    if (!lock.locked()) return;
    uint64_t seq = mem_->state_seq;
    if (seq & 1U) ++seq;
    mem_->state_stamp_ns = state_stamp_ns;
    mem_->robot_connected = 1;
    mem_->state_ready = 1;
    mem_->state_mode_pr = msg->mode_pr;
    mem_->mode_machine = msg->mode_machine;
    for (int index = 0; index < kHwDof; ++index) {
      mem_->joint_pos[index] = q[index];
      mem_->joint_vel[index] = dq[index];
    }
    for (int index = 0; index < 4; ++index) {
      mem_->imu_quat_wxyz[index] = quaternion[index];
    }
    for (int index = 0; index < 3; ++index) {
      mem_->gyro[index] = gyro[index];
    }
    std::copy(msg->wireless_remote.begin(), msg->wireless_remote.end(), mem_->wireless_remote);
    std::copy(planner.begin(), planner.end(), mem_->tt_data);
    mem_->tt_ready = planner_ready ? 1 : 0;
    mem_->tt_stamp_ns = planner_stamp;
    if (planner_ready) {
      mem_->root_current_xy[0] = static_cast<float>(planner[0]);
      mem_->root_current_xy[1] = static_cast<float>(planner[1]);
      for (int index = 0; index < 4; ++index) {
        mem_->mocap_pelvis_quat_xyzw[index] = static_cast<float>(planner[3 + index]);
      }
    }
    SyncLatchedFlagsLocked();
    mem_->state_seq = seq + 2;
  }

  bool ReadCommand(Command* result) {
    bool enabled = false;
    bool motion_switcher_attested = false;
    Command next;
    {
      std::lock_guard<std::mutex> local_lock(shm_mutex_);
      FileLock lock(shm_fd_, LOCK_SH);
      if (!lock.locked()) {
        ++command_lock_miss_streak_;
        max_command_lock_miss_streak_ =
            std::max(max_command_lock_miss_streak_, command_lock_miss_streak_);
        return false;
      }
      command_lock_miss_streak_ = 0;
      enabled = mem_->command_enabled != 0;
      if (!enabled) return false;
      motion_switcher_attested =
          (mem_->reserved_flags & kMotionSwitcherAttested) != 0U;
      next.sequence = mem_->command_seq;
      next.stamp_ns = mem_->command_stamp_ns;
      next.mode_pr = mem_->cmd_mode_pr;
      std::copy(std::begin(mem_->cmd_q), std::end(mem_->cmd_q), next.q.begin());
      std::copy(std::begin(mem_->cmd_dq), std::end(mem_->cmd_dq), next.dq.begin());
      std::copy(std::begin(mem_->cmd_tau), std::end(mem_->cmd_tau), next.tau.begin());
      std::copy(std::begin(mem_->cmd_kp), std::end(mem_->cmd_kp), next.kp.begin());
      std::copy(std::begin(mem_->cmd_kd), std::end(mem_->cmd_kd), next.kd.begin());
      std::copy(std::begin(mem_->cmd_mode), std::end(mem_->cmd_mode), next.mode.begin());
      std::copy(std::begin(mem_->cmd_reserve), std::end(mem_->cmd_reserve), next.reserve.begin());
    }
    if (!motion_switcher_attested) {
      LatchFault("Python command lacks a validated MotionSwitcher release attestation");
      return false;
    }
    if (!PythonCommandValid(next)) {
      LatchFault("invalid/non-PR Python command or non-zero raw head target");
      return false;
    }
    Command projected;
    bool adjusted = false;
    if (!ConstrainCommandForState(next, SnapshotState(), &projected, &adjusted)) {
      LatchFault("Python command has no effort-feasible projection");
      return false;
    }
    // Retain the raw learned desired target rather than the projection from
    // this one state sample. OnCommandTimer projects it again from the newest
    // validated q/dq immediately before publication.
    *result = next;
    return true;
  }

  bool PythonFaultRequested() {
    std::lock_guard<std::mutex> local_lock(shm_mutex_);
    FileLock lock(shm_fd_, LOCK_SH);
    return lock.locked() && (mem_->reserved_flags & kPythonFaultRequest) != 0U;
  }

  StateSnapshot SnapshotState() const {
    std::lock_guard<std::mutex> state_lock(state_mutex_);
    return SnapshotStateLocked();
  }

  StateSnapshot SnapshotStateLocked() const {
    StateSnapshot state;
    state.q = latest_q_;
    state.dq = latest_dq_;
    state.watchdog_q = watchdog_q_;
    state.mode_pr = latest_mode_pr_;
    state.mode_machine = latest_mode_machine_;
    state.stamp_ns = latest_state_stamp_ns_;
    state.got_state = got_state_;
    state.have_valid_state = have_valid_state_;
    state.usable = latest_state_usable_;
    state.output_inhibited = output_inhibited_;
    state.hold_available = hold_available_;
    return state;
  }

  static bool CommandStructureValid(const Command& command) {
    if (command.mode_pr != kModePr) return false;
    for (int index = 0; index < kLowCmdDof; ++index) {
      if (!std::isfinite(command.q[index]) || !std::isfinite(command.dq[index]) ||
          !std::isfinite(command.tau[index]) || !std::isfinite(command.kp[index]) ||
          !std::isfinite(command.kd[index]) || command.kp[index] < 0.0F ||
          command.kd[index] < 0.0F || command.kp[index] > kMaxKp ||
          command.kd[index] > kMaxKd) {
        return false;
      }
      const uint8_t expected_mode = index < kHwDof ? 1U : 0U;
      if (command.mode[index] != expected_mode || command.reserve[index] != 0U) return false;
      if (index >= kHwDof &&
          (command.q[index] != 0.0F || command.dq[index] != 0.0F ||
           command.tau[index] != 0.0F || command.kp[index] != 0.0F ||
           command.kd[index] != 0.0F)) {
        return false;
      }
    }
    return true;
  }

  static bool PythonCommandValid(const Command& command) {
    if (!CommandStructureValid(command)) return false;
    // The policy still observes/outputs all 31 actions, but Python locks both
    // raw learned head targets to zero. The 1000 Hz effort projection may move
    // the published q_des away from zero when doing so is required to saturate
    // damping effort, exactly as it may for every other active joint.
    return command.q[29] == 0.0F && command.q[30] == 0.0F;
  }

  static bool CommandSafeForState(
      const Command& command, const StateSnapshot& state) {
    if (!state.have_valid_state) return false;
    for (int index = 0; index < kHwDof; ++index) {
      const float estimated_effort =
          command.tau[index] + command.kp[index] * (command.q[index] - state.q[index]) +
          command.kd[index] * (command.dq[index] - state.dq[index]);
      if (!std::isfinite(estimated_effort) ||
          std::abs(estimated_effort) > kTorqueLimit[index] + 1.0e-3F) {
        return false;
      }
    }
    return true;
  }

  static bool ConstrainCommandForState(
      const Command& input, const StateSnapshot& state, Command* result,
      bool* adjusted) {
    if (!state.have_valid_state || result == nullptr || adjusted == nullptr) return false;
    Command projected = input;
    *adjusted = false;
    for (int index = 0; index < kHwDof; ++index) {
      const float kp = input.kp[index];
      // Unitree PR effort is
      //   tau_ff + kp * (q_des-q) + kd * (dq_des-dq).
      // Hold every term except q_des fixed and solve -limit <= effort <= limit.
      const float offset =
          input.tau[index] + input.kd[index] * (input.dq[index] - state.dq[index]);
      float target = input.q[index];
      if (kp <= 1.0e-8F) {
        if (!std::isfinite(offset) || std::abs(offset) > kTorqueLimit[index] + 1.0e-3F) {
          return false;
        }
      } else {
        const float a = state.q[index] + (-kTorqueLimit[index] - offset) / kp;
        const float b = state.q[index] + (kTorqueLimit[index] - offset) / kp;
        const float feasible_lo = std::min(a, b);
        const float feasible_hi = std::max(a, b);
        if (!std::isfinite(feasible_lo) || !std::isfinite(feasible_hi) ||
            feasible_lo > feasible_hi) {
          return false;
        }
        target = std::clamp(input.q[index], feasible_lo, feasible_hi);
      }
      if (std::abs(target - input.q[index]) > 1.0e-7F) *adjusted = true;
      projected.q[index] = target;
    }
    if (!CommandStructureValid(projected) || !CommandSafeForState(projected, state)) return false;
    *result = projected;
    return true;
  }

  bool StateFresh(const StateSnapshot& state, uint64_t now_ns) const {
    return !state.output_inhibited && state.got_state && state.usable &&
           state.stamp_ns != 0U && state.stamp_ns <= now_ns &&
           now_ns - state.stamp_ns <=
               static_cast<uint64_t>(args_.state_timeout_s * 1.0e9);
  }

  static Command LatchedHold(
      const Command& basis, const StateSnapshot& state) {
    Command hold = basis;
    hold.mode_pr = kModePr;
    for (int index = 0; index < kLowCmdDof; ++index) {
      hold.mode[index] = index < kHwDof ? 1U : 0U;
      hold.q[index] = index < kHwDof
                          ? std::clamp(
                                state.watchdog_q[index], kJointLo[index] + kJointMargin,
                                kJointHi[index] - kJointMargin)
                          : 0.0F;
      hold.dq[index] = 0.0F;
      hold.tau[index] = 0.0F;
      if (index < kHwDof) {
        // Reduce, never increase, the last validated gains so the captured
        // position hold is inside the MJCF effort envelope at this state.
        const float budget = kHoldTorqueFraction * kTorqueLimit[index];
        const float position_error = hold.q[index] - state.q[index];
        const float abs_error = std::abs(position_error);
        if (abs_error > 1.0e-6F && hold.kp[index] * abs_error > budget) {
          hold.kp[index] = budget / abs_error;
        }
        const float position_effort = std::abs(hold.kp[index] * position_error);
        const float damping_budget = std::max(0.0F, budget - position_effort);
        const float abs_velocity = std::abs(state.dq[index]);
        if (abs_velocity > 1.0e-6F && hold.kd[index] * abs_velocity > damping_budget) {
          hold.kd[index] = damping_budget / abs_velocity;
        }
      } else {
        hold.kp[index] = 0.0F;
        hold.kd[index] = 0.0F;
      }
      hold.reserve[index] = 0U;
    }
    return hold;
  }

  bool PublishValidated(
      const Command& command, const StateSnapshot& state, const Command* raw = nullptr) {
    if (!CommandStructureValid(command) || !CommandSafeForState(command, state)) return false;
    unitree_hg::msg::LowCmd message{};
    message.mode_pr = kModePr;
    message.mode_machine = state.mode_machine;
    for (int index = 0; index < kLowCmdDof; ++index) {
      auto& motor = message.motor_cmd.at(index);
      motor.mode = command.mode[index];
      motor.q = command.q[index];
      motor.dq = command.dq[index];
      motor.tau = command.tau[index];
      motor.kp = command.kp[index];
      motor.kd = command.kd[index];
      motor.reserve = command.reserve[index];
    }
    get_crc(message);
    if (timing_.enabled()) {
      timing_row_.command_seq = command.sequence;
      timing_row_.command_stamp_ns = command.stamp_ns;
      timing_row_.state_stamp_ns = state.stamp_ns;
      timing_row_.held = raw == nullptr ? 1 : 0;
      if (raw != nullptr) {
        for (int index = 0; index < kHwDof; ++index) {
          const double delta = std::abs(double(command.q[index]) - raw->q[index]);
          if (delta > 1e-6) ++timing_row_.projected_count;
          if (delta > timing_row_.max_projection_rad) {
            timing_row_.max_projection_rad = delta;
            timing_row_.projection_joint = index;
          }
        }
      }
      timing_row_.publish_start_ns = NowNs();
    }
    lowcmd_pub_->publish(message);
    if (timing_.enabled()) timing_row_.publish_end_ns = NowNs();
    return true;
  }

  bool ProjectAndPublish(const Command& raw_command, bool* adjusted) {
    std::string failure;
    bool local_adjusted = false;
    {
      // Fault transition and final publication share this linearization point.
      // The state mutex is held only long enough to copy a coherent sample;
      // a potentially blocking DDS publish can never delay LowState receipt.
      std::lock_guard<std::mutex> output_lock(output_mutex_);
      if (fault_latched_.load() || publishing_blocked_.load() ||
          publisher_topology_.load() != PublisherTopology::kReady) {
        return false;
      }
      const StateSnapshot state = SnapshotState();
      const uint64_t now_ns = NowNs();
      if (!StateFresh(state, now_ns)) {
        failure = "LowState became stale before final LowCmd publication";
      } else if (state.mode_pr != kModePr) {
        failure = "LowState is not in PR(0) mode at final LowCmd publication";
      } else {
        Command projected;
        if (!ConstrainCommandForState(
                raw_command, state, &projected, &local_adjusted)) {
          failure = "live command has no effort-feasible projection";
        } else if (!PublishValidated(projected, state, &raw_command)) {
          failure = "final LowCmd finite/gain/mode validation failed";
        }
      }
    }
    if (!failure.empty()) {
      LatchFault(failure);
      return false;
    }
    if (adjusted != nullptr) *adjusted = local_adjusted;
    return true;
  }

  void PublishLatchedHoldIfPossible() {
    if (!have_command_) return;
    std::string failure;
    {
      std::lock_guard<std::mutex> output_lock(output_mutex_);
      if (!fault_latched_.load() || publishing_blocked_.load() ||
          publisher_topology_.load() != PublisherTopology::kReady) {
        return;
      }
      const StateSnapshot state = SnapshotState();
      const uint64_t now_ns = NowNs();
      if (!state.hold_available || !StateFresh(state, now_ns) ||
          state.mode_pr != kModePr) {
        return;
      }
      const Command hold = LatchedHold(command_, state);
      if (!PublishValidated(hold, state)) {
        failure = "captured H2 hold failed final effort validation";
      }
    }
    if (!failure.empty()) LatchFault(failure);
  }

  void OnCommandTimer() {
    SyncLatchedFlags();
    if (PythonFaultRequested()) LatchFault("Python controller requested a permanent fault latch");
    const StateSnapshot state = SnapshotState();
    // Snapshot first, then sample the clock.  With concurrent LowState receipt,
    // the opposite order can make a newly committed state appear to come from
    // the future and falsely trip the watchdog.
    const uint64_t state_now_ns = NowNs();
    if (state.output_inhibited) return;
    if (!state.got_state) return;
    if (!StateFresh(state, state_now_ns)) {
      LatchFault("H2 LowState watchdog timeout or invalid timestamp");
      return;
    }
    if (state.mode_pr != kModePr) {
      LatchFault(
          "LowState left PR(0) mode (mode_pr=" + std::to_string(state.mode_pr) + ")");
      return;
    }
    if (publisher_topology_.load() != PublisherTopology::kReady ||
        publishing_blocked_.load()) {
      return;
    }
    if (fault_latched_.load()) {
      PublishLatchedHoldIfPossible();
      return;
    }

    Command next;
    if (timing_.enabled()) timing_row_.read_start_ns = NowNs();
    if (ReadCommand(&next)) {
      command_ = next;
      have_command_ = true;
    }
    if (timing_.enabled()) timing_row_.read_end_ns = NowNs();
    if (fault_latched_.load()) {
      PublishLatchedHoldIfPossible();
      return;
    }
    if (!have_command_) return;

    // ReadCommand and the Python writer run concurrently.  Sample this clock
    // after the read so a command committed between the state clock sample
    // and ReadCommand cannot appear a few microseconds "from the future".
    const uint64_t command_now_ns = NowNs();
    if (command_.stamp_ns == 0U || command_.stamp_ns > command_now_ns ||
        command_now_ns - command_.stamp_ns >
            static_cast<uint64_t>(args_.command_timeout_s * 1.0e9)) {
      std::ostringstream reason;
      reason << "Python command watchdog timeout or invalid timestamp"
             << " (stamp_ns=" << command_.stamp_ns;
      if (command_.stamp_ns > 0U && command_.stamp_ns <= command_now_ns) {
        reason << ", age_s="
               << static_cast<double>(command_now_ns - command_.stamp_ns) * 1.0e-9;
      }
      reason << ", lock_miss_streak=" << command_lock_miss_streak_
             << ", max_lock_miss_streak=" << max_command_lock_miss_streak_ << ")";
      LatchFault(reason.str());
      PublishLatchedHoldIfPossible();
      return;
    }
    bool adjusted = false;
    if (!ProjectAndPublish(command_, &adjusted)) {
      PublishLatchedHoldIfPossible();
      return;
    }
    if (adjusted) {
      const uint64_t warning_now_ns = NowNs();
      if (warning_now_ns - projection_warning_ns_ >= 2000000000ULL) {
        projection_warning_ns_ = warning_now_ns;
        RCLCPP_WARN(
            get_logger(),
            "projected a raw learned target into the latest H2 PD-effort envelope");
      }
    }
  }

  void CommandLoop() noexcept {
    const auto period = std::chrono::duration_cast<std::chrono::steady_clock::duration>(
        std::chrono::duration<double>(1.0 / args_.publish_hz));
    auto next = std::chrono::steady_clock::now() + period;
    while (workers_running_.load()) {
      {
        std::unique_lock<std::mutex> wait_lock(worker_wait_mutex_);
        worker_cv_.wait_until(
            wait_lock, next, [this]() { return !workers_running_.load(); });
      }
      if (!workers_running_.load()) break;
      if (timing_.enabled()) {
        timing_row_ = {};
        timing_row_.cycle_start_ns = NowNs();
      }
      try {
        OnCommandTimer();
      } catch (const std::exception& error) {
        LatchFault(std::string("LowCmd worker exception: ") + error.what());
      } catch (...) {
        LatchFault("LowCmd worker unknown exception");
      }
      if (timing_.enabled()) {
        timing_row_.cycle_end_ns = NowNs();
        timing_row_.lock_miss_streak = command_lock_miss_streak_;
        timing_.Record(timing_row_);
      }
      const auto now = std::chrono::steady_clock::now();
      next += period;
      if (next <= now) next = now + period;
    }
  }

  void TopologyLoop() noexcept {
    auto next = std::chrono::steady_clock::now() +
                std::chrono::nanoseconds(kPublisherDiscoveryNs);
    while (workers_running_.load()) {
      {
        std::unique_lock<std::mutex> wait_lock(worker_wait_mutex_);
        worker_cv_.wait_until(
            wait_lock, next, [this]() { return !workers_running_.load(); });
      }
      if (!workers_running_.load()) break;
      try {
        const size_t publishers = count_publishers(args_.lowcmd_topic);
        if (publishers > 1U) {
          {
            std::lock_guard<std::mutex> output_lock(output_mutex_);
            publisher_topology_.store(PublisherTopology::kBlocked);
            publishing_blocked_.store(true);
          }
          LatchFault("competing LowCmd publisher detected");
          RCLCPP_FATAL(
              get_logger(), "%zu publishers detected on %s; refusing all LowCmd output",
              publishers, args_.lowcmd_topic.c_str());
        } else {
          std::lock_guard<std::mutex> output_lock(output_mutex_);
          if (!publishing_blocked_.load()) {
            publisher_topology_.store(
                publishers == 1U ? PublisherTopology::kReady
                                 : PublisherTopology::kDiscovering);
          }
          if (publishers == 0U) {
            RCLCPP_WARN(
                get_logger(),
                "LowCmd publisher discovery has not observed this bridge; publishing nothing");
          }
        }
      } catch (const std::exception& error) {
        {
          std::lock_guard<std::mutex> output_lock(output_mutex_);
          if (!publishing_blocked_.load()) {
            publisher_topology_.store(PublisherTopology::kDiscovering);
          }
        }
        RCLCPP_WARN(
            get_logger(), "LowCmd topology query failed; publishing nothing: %s", error.what());
      } catch (...) {
        {
          std::lock_guard<std::mutex> output_lock(output_mutex_);
          if (!publishing_blocked_.load()) {
            publisher_topology_.store(PublisherTopology::kDiscovering);
          }
        }
        RCLCPP_WARN(
            get_logger(), "LowCmd topology query failed; publishing nothing: unknown exception");
      }
      const auto now = std::chrono::steady_clock::now();
      next += std::chrono::nanoseconds(kPublisherRecheckNs);
      if (next <= now) next = now + std::chrono::nanoseconds(kPublisherRecheckNs);
    }
  }

  Args args_;
  h2_timing::Trace timing_;
  h2_timing::Row timing_row_;
  BridgeMemory* mem_;
  int shm_fd_;
  rclcpp::Subscription<unitree_hg::msg::LowState>::SharedPtr lowstate_sub_;
  rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr planner_sub_;
  rclcpp::Publisher<unitree_hg::msg::LowCmd>::SharedPtr lowcmd_pub_;
  std::mutex planner_mutex_;
  std::array<double, kTtDataLen> planner_data_{};
  bool planner_ready_{false};
  uint64_t planner_stamp_ns_{0};
  unitree::common::Gamepad gamepad_;
  std::array<float, kHwDof> latest_q_{};
  std::array<float, kHwDof> latest_dq_{};
  std::array<float, kHwDof> watchdog_q_{};
  uint8_t latest_mode_pr_{std::numeric_limits<uint8_t>::max()};
  uint8_t latest_mode_machine_{0};
  uint64_t latest_state_stamp_ns_{0};
  bool got_state_{false};
  bool have_valid_state_{false};
  bool latest_state_usable_{false};
  bool output_inhibited_{false};
  bool have_command_{false};
  bool hold_available_{false};
  mutable std::mutex state_mutex_;
  std::mutex output_mutex_;
  std::mutex shm_mutex_;
  std::mutex worker_wait_mutex_;
  std::condition_variable worker_cv_;
  std::thread command_thread_;
  std::thread topology_thread_;
  std::atomic<bool> workers_running_{false};
  std::atomic<PublisherTopology> publisher_topology_{PublisherTopology::kDiscovering};
  std::atomic<bool> publishing_blocked_{false};
  std::atomic<bool> fault_latched_{false};
  std::atomic<bool> start_request_latched_{false};
  std::atomic<bool> stop_request_latched_{false};
  std::atomic<bool> latched_flags_dirty_{false};
  uint64_t command_lock_miss_streak_{0};
  uint64_t max_command_lock_miss_streak_{0};
  uint64_t projection_warning_ns_{0};
  Command command_{};
};

}  // namespace

int main(int argc, char** argv) {
  try {
    const Args args = ParseArgs(argc, argv);
    if (args.print_contract) {
      std::cout << "H2BR magic=0x" << std::hex << kMagic << std::dec << " version=" << kVersion
                << " bytes=" << sizeof(BridgeMemory) << " state_dof=" << kHwDof
                << " lowcmd_slots=" << kLowCmdDof << " mode_pr=" << static_cast<int>(kModePr)
                << " joint_map=" << kJointMapRevision
                << " cmd_q=raw_learned_desired"
                << " ros_lowstate=" << args.lowstate_topic
                << " ros_lowcmd=" << args.lowcmd_topic
                << " publish_hz=" << args.publish_hz
                << std::endl;
      return 0;
    }
    ValidateNetworkEnvironment(args.network_interface);
    if (args.validate_network_only) {
      std::cout << "H2 network contract PASS interface=" << args.network_interface
                << " hash=0x" << std::hex << InterfaceHash(args.network_interface) << std::dec
                << std::endl;
      return 0;
    }
    rclcpp::init(argc, argv);
    SharedMemoryOwner memory(args.replace_stale_shm, InterfaceHash(args.network_interface));
    auto node = std::make_shared<H2BridgeNode>(args, memory.get(), memory.fd());
    node->StartWorkers();
    try {
      rclcpp::spin(node);
    } catch (...) {
      node->StopWorkers();
      throw;
    }
    node->StopWorkers();
    rclcpp::shutdown();
  } catch (const std::exception& error) {
    std::cerr << "[h2 bridge] fatal: " << error.what() << std::endl;
    if (rclcpp::ok()) rclcpp::shutdown();
    return 1;
  }
  return 0;
}