#include "tide_hw_interface.hpp"
#include <algorithm>
#include <cmath>
#include <limits>
#include <vector>
#include <string>
#include <iomanip>

namespace tide_hw_interface
{

TideHardwareInterface::TideHardwareInterface() : hardware_interface::SystemInterface() {}

namespace
{

double normalizeRemoteChannel(int16_t value)
{
  constexpr double kRcChannelRange = 660.0;
  const double normalized = static_cast<double>(value) / kRcChannelRange;
  return std::max(-1.0, std::min(1.0, normalized));
}

bool isDmMotor(Motor_Type_e motor_type)
{
  return motor_type == DM || motor_type == DM3510;
}

}  // namespace

hardware_interface::CallbackReturn
TideHardwareInterface::on_init(const hardware_interface::HardwareInfo& info)
{
  //自检
  if (hardware_interface::SystemInterface::on_init(info) !=
      hardware_interface::CallbackReturn::SUCCESS)
  {
    return hardware_interface::CallbackReturn::ERROR;
  }
  //查询电机/关节数量，并分配空间，NaN表示未初始化，机器人不清楚当前电机状态
  joint_count = info_.joints.size();
  state_positions_.resize(joint_count, std::numeric_limits<double>::quiet_NaN());
  state_velocities_.resize(joint_count, std::numeric_limits<double>::quiet_NaN());
  state_currents_.resize(joint_count, std::numeric_limits<double>::quiet_NaN());
  state_temperatures_.resize(joint_count, std::numeric_limits<double>::quiet_NaN());

  cmd_positions_.resize(joint_count, std::numeric_limits<double>::quiet_NaN());
  cmd_velocities_.resize(joint_count, std::numeric_limits<double>::quiet_NaN());
  cmd_efforts_.resize(joint_count, std::numeric_limits<double>::quiet_NaN());
  // last_sent_positions_.resize(joint_count, 0.0);
  
  last_sent_positions_.resize(joint_count, std::numeric_limits<double>::quiet_NaN());
  last_sent_position_valid_.resize(joint_count, false);
  //last_sent_position_valid_.resize(joint_count, std::numeric_limits<double>::quiet_NaN());
  //在YAML文件中读取enable_virtual_control参数，决定是否启用虚拟控制模式
  //在YAML文件中读取need_calibration参数，决定是否需要校准
  //在YAML文件中读取enable_serial_bridge参数，决定是否启用串口桥接模式
  const auto virtual_it = info_.hardware_parameters.find("enable_virtual_control");
  //没有取值到enable_virtual_control_，则默认为false
  enable_virtual_control_ =
      (virtual_it != info_.hardware_parameters.end() && virtual_it->second == "true");
  const auto calibration_it = info_.hardware_parameters.find("need_calibration");
  need_calibration_ =
      (calibration_it != info_.hardware_parameters.end() && calibration_it->second == "true");
  const auto transport_it = info_.hardware_parameters.find("transport");
  enable_serial_bridge_ =
      !enable_virtual_control_ &&
      (transport_it == info_.hardware_parameters.end() || transport_it->second == "serial");
  const auto serial_port_it = info_.hardware_parameters.find("serial_port");
  if (serial_port_it != info_.hardware_parameters.end())
  {
    serial_port_ = serial_port_it->second;
  }
  //读取串口波特率（通信速度）。std::stoi 把文字 "115200" 变成真正的数字 115200。std::stod 则是变成带小数点的数字。
  const auto serial_baudrate_it = info_.hardware_parameters.find("serial_baudrate");
  if (serial_baudrate_it != info_.hardware_parameters.end())
  {
    serial_baudrate_ = std::stoi(serial_baudrate_it->second);
  }
  const auto serial_period_it = info_.hardware_parameters.find("serial_command_period");
  if (serial_period_it != info_.hardware_parameters.end())
  {
    serial_command_period_ = std::stod(serial_period_it->second);
  }

  bridge_node_ = std::make_shared<rclcpp::Node>("astra_hw_bridge");
  remote_pub_ = bridge_node_->create_publisher<sensor_msgs::msg::Joy>("/astra/remote/joy", 10);
  motor_enable_sub_ = bridge_node_->create_subscription<std_msgs::msg::Bool>(
      "/astra/motor_enable", 10, [this](const std_msgs::msg::Bool::SharedPtr msg) {
        motor_enable_ = msg->data;
      });

  for (const auto& joint : info_.joints)
  {
    Motor_Config_t config;
    config.motor_name = joint.name;

    for (const auto& [key, value] : joint.parameters)
    {
      if (key == "can_bus")
        config.can_bus = value;
      else if (key == "tx_id")
      {
        config.tx_id = std::stoi(value);
      }
      else if (key == "rx_id")
      {
        config.rx_id = std::stoi(value);
      }
      else if (key == "motor_type")
      {
        if (value == "M2006")
        {
          config.motor_type = M2006;
        }
        else if (value == "M3508")
        {
          config.motor_type = M3508;
        }
        else if (value == "GM6020")
        {
          config.motor_type = GM6020;
        }
        else if (value == "DM")
        {
          config.motor_type = DM;
        }
        else if (value == "DM3510")
        {
          config.motor_type = DM3510;
        }
        else if (value == "VIRTUAL_JOINT")
        {
          config.motor_type = VIRTUAL_JOINT;
        }
        else
        {
          RCLCPP_ERROR(rclcpp::get_logger("TideHardwareInterface"), "Unknown motor type: %s",
                       value.c_str());
        }
      }
      else if (key == "offset")
        config.offset = std::stoi(value);
      else if (key == "kp")
        config.kp = std::stod(value);
      else if (key == "kd")
        config.kd = std::stod(value);
    }

    auto motor = std::make_shared<TideMotor>(config);
    motors_.push_back(motor);
  }

  if (enable_virtual_control_)
  {
    return hardware_interface::CallbackReturn::SUCCESS;
  }

  if (enable_serial_bridge_)
  {
    if (!serial_bridge_.open(serial_port_, serial_baudrate_))
    {
      RCLCPP_ERROR(rclcpp::get_logger("TideHardwareInterface"),
                   "Failed to open STM32 serial bridge: %s @ %d", serial_port_.c_str(),
                   serial_baudrate_);
      return hardware_interface::CallbackReturn::ERROR;
    }
    RCLCPP_INFO(rclcpp::get_logger("TideHardwareInterface"), "Using STM32 serial bridge: %s @ %d",
                serial_port_.c_str(), serial_baudrate_);
    return hardware_interface::CallbackReturn::SUCCESS;
  }

  if (!enable_serial_bridge_)
  {
    RCLCPP_ERROR(
        rclcpp::get_logger("TideHardwareInterface"),
        "Unsupported hardware transport. Use transport:=serial or enable_virtual_control:=true.");
    return hardware_interface::CallbackReturn::ERROR;
  }

  RCLCPP_INFO(rclcpp::get_logger("TideHardwareInterface"),
              "Successful loaded %ld motors with STM32 serial bridge", joint_count);

  return hardware_interface::CallbackReturn::SUCCESS;
}

std::vector<hardware_interface::StateInterface> TideHardwareInterface::export_state_interfaces()
{
  std::vector<hardware_interface::StateInterface> interfaces;

  for (size_t i = 0; i < joint_count; i++)
  {
    for (const auto& state_interface : info_.joints[i].state_interfaces)
    {
      if (state_interface.name == "position")
      {
        interfaces.emplace_back(info_.joints[i].name, state_interface.name, &state_positions_[i]);
      }
      else if (state_interface.name == "velocity")
      {
        interfaces.emplace_back(info_.joints[i].name, state_interface.name, &state_velocities_[i]);
      }
      else if (state_interface.name == "effort")
      {
        interfaces.emplace_back(info_.joints[i].name, state_interface.name, &state_currents_[i]);
      }
      else if (state_interface.name == "current")
      {
        interfaces.emplace_back(info_.joints[i].name, state_interface.name, &state_currents_[i]);
      }
      else if (state_interface.name == "temperature")
      {
        interfaces.emplace_back(info_.joints[i].name, state_interface.name, &state_temperatures_[i]);
      }
    }
  }
  return interfaces;
}

std::vector<hardware_interface::CommandInterface> TideHardwareInterface::export_command_interfaces()
{
  std::vector<hardware_interface::CommandInterface> interfaces;

  for (size_t i = 0; i < joint_count; i++)
  {
    for (const auto& command_interface : info_.joints[i].command_interfaces)
    {
      if (command_interface.name == "position")
      {
        interfaces.emplace_back(info_.joints[i].name, command_interface.name, &cmd_positions_[i]);
      }
      else if (command_interface.name == "velocity")
      {
        interfaces.emplace_back(info_.joints[i].name, command_interface.name, &cmd_velocities_[i]);
      }
      else if (command_interface.name == "effort")
      {
        interfaces.emplace_back(info_.joints[i].name, command_interface.name, &cmd_efforts_[i]);
      }
    }
  }
  return interfaces;
}

hardware_interface::CallbackReturn
TideHardwareInterface::on_configure(const rclcpp_lifecycle::State& /*previous_state*/)
{
  std::fill(state_positions_.begin(), state_positions_.end(), 0.0);
  std::fill(state_velocities_.begin(), state_velocities_.end(), 0.0);
  std::fill(state_currents_.begin(), state_currents_.end(), 0.0);
  std::fill(state_temperatures_.begin(), state_temperatures_.end(), 0.0);

  std::fill(cmd_positions_.begin(), cmd_positions_.end(), 0.0);
  std::fill(cmd_velocities_.begin(), cmd_velocities_.end(), 0.0);
  std::fill(cmd_efforts_.begin(), cmd_efforts_.end(), 0.0);
  std::fill(last_sent_positions_.begin(), last_sent_positions_.end(), 0.0);
  std::fill(last_sent_position_valid_.begin(), last_sent_position_valid_.end(), true);

  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn
TideHardwareInterface::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
{ return hardware_interface::CallbackReturn::SUCCESS; }

hardware_interface::CallbackReturn
TideHardwareInterface::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/)
{
  try
  {
    stopMotors();
    return hardware_interface::CallbackReturn::SUCCESS;
  }
  catch (const std::exception& e)
  {
    RCLCPP_ERROR(rclcpp::get_logger("TideHardwareInterface"), "Error in on_deactivate: %s",
                 e.what());
    return hardware_interface::CallbackReturn::ERROR;
  }
}

hardware_interface::CallbackReturn
TideHardwareInterface::on_cleanup(const rclcpp_lifecycle::State& /*previous_state*/)
{
  try
  {
    serial_bridge_.close();
    motors_.clear();
    return hardware_interface::CallbackReturn::SUCCESS;
  }
  catch (const std::exception& e)
  {
    RCLCPP_ERROR(rclcpp::get_logger("TideHardwareInterface"), "Error in on_cleanup: %s", e.what());
    return hardware_interface::CallbackReturn::ERROR;
  }
}

uint8_t TideHardwareInterface::parseCanBusIndex(const std::string& can_bus) const
{
  if (can_bus.size() >= 4 && can_bus.rfind("can", 0) == 0)
  {
    return static_cast<uint8_t>(std::stoi(can_bus.substr(3)) + 1);
  }
  return 0;
}

AstraMotorType TideHardwareInterface::toAstraMotorType(Motor_Type_e motor_type) const
{
  switch (motor_type)
  {
    case M2006:
    case M3508:
    case GM6020:
      return AstraMotorType::DJI;
    case DM:
      return AstraMotorType::DM;
    case DM3510:
      return AstraMotorType::DM3510;
    case VIRTUAL_JOINT:
      return AstraMotorType::Virtual;
    default:
      return AstraMotorType::None;
  }
}

AstraControlMode TideHardwareInterface::commandModeForJoint(size_t joint_index) const
{
  if (info_.joints[joint_index].command_interfaces.empty())
  {
    return AstraControlMode::None;
  }
  const auto& name = info_.joints[joint_index].command_interfaces[0].name;
  if (name == "position")
  {
    if (isDmMotor(motors_[joint_index]->config_.motor_type))
    {
      return AstraControlMode::MIT;
    }
    return AstraControlMode::Position;
  }
  if (name == "velocity")
  {
    return AstraControlMode::Velocity;
  }
  if (name == "effort")
  {
    return AstraControlMode::Effort;
  }
  return AstraControlMode::None;
}

/**
 * @brief 从串口通信桥接中获取反馈数据并更新电机状态及远程/裁判系统状态。
 *
 * 依次轮询串口桥接，消费电机状态、遥控器状态和裁判系统状态数据。
 * 对于每个有效的电机状态，更新对应关节电机的运行状态、位置、速度、电流及温度，
 * 并刷新时间戳；对于遥控器状态，发布到对应话题；裁判系统状态当前仅消费不做处理。
 * 若电机状态中的关节索引超出有效范围，则跳过该条数据。
 */
void TideHardwareInterface::applySerialFeedback()
{
  serial_bridge_.poll();

  AstraMotorState state;
  while (serial_bridge_.consumeMotorState(state))
  {
    if (state.joint_index >= joint_count)
    {
      continue;
    }
    auto& motor = motors_[state.joint_index];
    motor->status = state.status != 0 ? MOTOR_OK : MOTOR_LOST;
    motor->angle_current = state.position;
    motor->measure.speed_aps = state.velocity;
    motor->measure.real_current = static_cast<int16_t>(state.current);
    motor->measure.temperature = static_cast<uint8_t>(state.temperature);
    motor->update_timestamp(rclcpp::Clock().now());
  }

  AstraRemoteState remote_state;
  if (serial_bridge_.consumeRemoteState(remote_state))
  {
    publishRemoteState(remote_state);
  }

  AstraRefereeState referee_state;
  while (serial_bridge_.consumeRefereeState(referee_state))
  {
    (void)referee_state;
  }
}

void TideHardwareInterface::publishRemoteState(const AstraRemoteState& state)
{
  if (!remote_pub_)
  {
    return;
  }

  sensor_msgs::msg::Joy joy;
  joy.header.stamp = bridge_node_ ? bridge_node_->now() : rclcpp::Clock().now();
  joy.header.frame_id = "vt13_remote";

  joy.axes.resize(8, 0.0f);
  for (std::size_t i = 0; i < 5; ++i)
  {
    joy.axes[i] = static_cast<float>(normalizeRemoteChannel(state.ch[i]));
  }
  joy.axes[5] = static_cast<float>(state.mouse_x) / 32767.0f;
  joy.axes[6] = static_cast<float>(state.mouse_y) / 32767.0f;
  joy.axes[7] = static_cast<float>(state.mouse_z) / 32767.0f;

  joy.buttons.resize(22, 0);
  for (std::size_t i = 0; i < 16; ++i)
  {
    joy.buttons[i] = ((state.key >> i) & 0x01U) ? 1 : 0;
  }
  joy.buttons[16] = state.mouse_l ? 1 : 0;
  joy.buttons[17] = state.mouse_r ? 1 : 0;
  joy.buttons[18] = state.mode_sw == 1 ? 1 : 0;
  joy.buttons[19] = state.mode_sw == 2 ? 1 : 0;
  joy.buttons[20] = state.mode_sw == 3 ? 1 : 0;
  joy.buttons[21] = state.stop ? 1 : 0;

  remote_pub_->publish(joy);
}

std::vector<AstraMotorCommand> TideHardwareInterface::buildSerialCommands()
{
  std::vector<AstraMotorCommand> commands;
  commands.reserve(joint_count);

  for (size_t i = 0; i < joint_count; i++)
  {
    if (info_.joints[i].command_interfaces.empty())
    {
      continue;
    }

    const auto& motor = motors_[i];
    if (motor->config_.motor_type == VIRTUAL_JOINT)
    {
      continue;
    }

    const auto mode = commandModeForJoint(i);
    const bool velocity_command = mode == AstraControlMode::Velocity;
    const bool position_command =
        mode == AstraControlMode::Position || mode == AstraControlMode::MIT;
    if (!velocity_command && !position_command)
    {
      continue;
    }

    if (position_command)
    {
      if (!motor_enable_ || std::isnan(cmd_positions_[i]))
      {
        continue;
      }
      if (last_sent_position_valid_[i] &&
          std::fabs(cmd_positions_[i] - last_sent_positions_[i]) < 1e-4)
      {
        continue;
      }
    }

    AstraMotorCommand cmd;
    cmd.joint_index = static_cast<uint8_t>(i);
    cmd.motor_type = static_cast<uint8_t>(toAstraMotorType(motor->config_.motor_type));
    cmd.control_mode = static_cast<uint8_t>(mode);
    cmd.flags = motor_enable_ ? 0x01 : 0x00;
    cmd.can_bus = parseCanBusIndex(motor->config_.can_bus);
    cmd.can_id = static_cast<uint8_t>(motor->config_.tx_id);
    cmd.kp = static_cast<float>(motor->config_.kp);
    cmd.kd = static_cast<float>(motor->config_.kd);

    if (!std::isnan(cmd_positions_[i]))
    {
      cmd.position = static_cast<float>(cmd_positions_[i]);
    }
    if (!std::isnan(cmd_velocities_[i]))
    {
      cmd.velocity = static_cast<float>(cmd_velocities_[i]);
    }
    if (!std::isnan(cmd_efforts_[i]))
    {
      cmd.effort = static_cast<float>(cmd_efforts_[i]);
    }
    commands.push_back(cmd);

    if (position_command)
    {
      last_sent_positions_[i] = cmd_positions_[i];
      last_sent_position_valid_[i] = true;
    }
  }

  return commands;
}

void TideHardwareInterface::stopMotors()
{
  for (auto& motor : motors_)
  {
    motor->stop();
  }
}

hardware_interface::return_type TideHardwareInterface::read(const rclcpp::Time& time,
                                                            const rclcpp::Duration& period)
{
  auto current_time = time;

  if (enable_serial_bridge_)
  {
    if (bridge_node_)
    {
      rclcpp::spin_some(bridge_node_);
    }
    applySerialFeedback();
  }

  for (size_t i = 0; i < joint_count; i++)
  {
    auto& motor = motors_[i];
    if (enable_virtual_control_ || motor->config_.motor_type == VIRTUAL_JOINT)
    {
      if (!std::isnan(cmd_positions_[i]) &&
          info_.joints[i].command_interfaces[0].name == "position")
      {
        state_positions_[i] = cmd_positions_[i];
      }
      else if (!std::isnan(cmd_velocities_[i]) &&
               info_.joints[i].command_interfaces[0].name == "velocity")
      {
        state_positions_[i] += cmd_velocities_[i] * period.seconds();
        state_velocities_[i] = cmd_velocities_[i];
      }
    }
    else if (!enable_serial_bridge_)
    {
      state_positions_[i] = 0.0;
      state_velocities_[i] = 0.0; 
      state_currents_[i] = 0.0;
      state_temperatures_[i] = 0.0; 
    }
    else
    {
      motor->check_connection(current_time);
      state_positions_[i] = motor->angle_current;
      state_velocities_[i] = motor->measure.speed_aps;
      state_currents_[i] = motor->measure.real_current;
      state_temperatures_[i] = motor->measure.temperature;
    }
  }
  return hardware_interface::return_type::OK;
}

hardware_interface::return_type TideHardwareInterface::write(const rclcpp::Time& time,
                                                             const rclcpp::Duration& /*period*/)
{
  if (need_calibration_)
  {
    for (const auto& motor : motors_)
    {
      std::stringstream ss;
      ss << std::left << std::setw(15) << motor->config_.motor_name << "Encoder: " << std::setw(6)
         << motor->measure.ecd;
      RCLCPP_INFO(rclcpp::get_logger("TideHardwareInterface"), "%s", ss.str().c_str());
    }
    return hardware_interface::return_type::OK;
  }

  if (!enable_serial_bridge_)
  {
    return hardware_interface::return_type::OK;
  }

  if (bridge_node_)
  {
    rclcpp::spin_some(bridge_node_);
  }

  if (last_serial_write_time_.nanoseconds() != 0 &&
      (time - last_serial_write_time_).seconds() < serial_command_period_)
  {
    return hardware_interface::return_type::OK;
  }
  last_serial_write_time_ = time;

  const auto commands = buildSerialCommands();
  serial_bridge_.sendMotorCommands(commands);
  serial_bridge_.sendHeartbeat();
  return hardware_interface::return_type::OK;
}

}  // namespace tide_hw_interface

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(tide_hw_interface::TideHardwareInterface,
                       hardware_interface::SystemInterface)  // 把它注册为ros2_control的硬件接口插件

哪些是官方的，哪些是自己写的

官方 ROS 2 提供：

hardware_interface::SystemInterface
hardware_interface::CallbackReturn
hardware_interface::return_type
hardware_interface::StateInterface
hardware_interface::CommandInterface

自己编写：

TideHardwareInterface
TideMotor
UpperBridgeProtocol
AstraMotorCommand
applySerialFeedback()
buildSerialCommands()

类继承官方接口：

class TideHardwareInterface
  : public hardware_interface::SystemInterface

这相当于向 ROS 保证：

我会按照 ROS 2 规定，实现初始化、读取状态、写入命令等函数。

override 表示这个函数是对父类接口的具体实现：

hardware_interface::return_type read(...) override;
hardware_interface::return_type write(...) override;

on_init()：读取机器人硬件说明

参数：

const hardware_interface::HardwareInfo& info

HardwareInfo 可以理解成 ROS 解析 <ros2_control> 后得到的结构化数据，里面包含：

info.hardware_parameters  硬件参数
info.joints               所有关节
joint.parameters          某个关节的电机参数
joint.command_interfaces  接收哪些命令
joint.state_interfaces    提供哪些反馈

第一步必须调用父类初始化：

if (hardware_interface::SystemInterface::on_init(info) !=
    hardware_interface::CallbackReturn::SUCCESS)
{
  return hardware_interface::CallbackReturn::ERROR;
}

父类会把 info 保存到成员变量 info_。因此后面才能访问：

info_.joints
info_.hardware_parameters

随后按关节数量分配数组：

joint_count = info_.joints.size();

state_positions_.resize(joint_count, NaN);
state_velocities_.resize(joint_count, NaN);

cmd_positions_.resize(joint_count, NaN);
cmd_velocities_.resize(joint_count, NaN);

假设第 0 个关节是左前轮，那么对应关系就是：

info_.joints[0]       左前轮关节说明
cmd_velocities_[0]    左前轮目标速度
state_positions_[0]   左前轮实际位置
state_velocities_[0]  左前轮实际速度

这里使用 NaN 表示“还没有有效数据”，比初始化成 0 更严谨，因为 0 本身可能是一个真实命令。

读取 Xacro 参数

Xacro 中：

<param name="serial_baudrate">115200</param>

C++ 中：

const auto it =
    info_.hardware_parameters.find("serial_baudrate");

if (it != info_.hardware_parameters.end())
{
  serial_baudrate_ = std::stoi(it->second);
}

这里需要注意：

• Xacro 参数进入 C++ 后首先都是字符串
• std::stoi()：字符串转整数
• std::stod()：字符串转浮点数
• find() 失败会返回 end()

每个关节参数也一样：

for (const auto& joint : info_.joints)
{
  Motor_Config_t config;
  config.motor_name = joint.name;

  for (const auto& [key, value] : joint.parameters)
  {
    if (key == "can_bus")
      config.can_bus = value;
    else if (key == "tx_id")
      config.tx_id = std::stoi(value);
  }

  motors_.push_back(std::make_shared<TideMotor>(config));
}

这一段把 Xacro 中每个 <joint> 转换成一个 TideMotor 对象。

导出状态接口

代码位置：

export_state_interfaces()

关键代码：

interfaces.emplace_back(
    info_.joints[i].name,
    state_interface.name,
    &state_positions_[i]);

假设关节名是 arm_j6_joint，接口名是 position，最终导出的接口名称相当于：

arm_j6_joint/position

最后一个参数：

&state_positions_[i]

是变量地址。以后 ROS 读取 arm_j6_joint/position，实际上就是读取 state_positions_[i]。

因此：

state_positions_[i] = motor->angle_current;

不仅是普通赋值，还会同步更新 ROS 能看到的关节位置。

导出命令接口

命令接口方向相反：

interfaces.emplace_back(
    info_.joints[i].name,
    command_interface.name,
    &cmd_velocities_[i]);

控制器写入：

front_left_wheel_joint/velocity

最终修改的是：

cmd_velocities_[0]

所以控制器不需要知道串口、CAN ID 或电机型号，它只操作标准接口。这就是 ros2_control 最重要的解耦：

控制器只关心 position/velocity/effort
硬件插件负责串口、CAN 和电机协议

on_configure()：准备运行数据

当前项目将所有状态和命令清零：

std::fill(state_positions_.begin(), state_positions_.end(), 0.0);
std::fill(cmd_positions_.begin(), cmd_positions_.end(), 0.0);

返回值：

return hardware_interface::CallbackReturn::SUCCESS;

表示配置成功，可以进入下一状态。

on_activate()：开始工作

当前实现非常简单：

hardware_interface::CallbackReturn
TideHardwareInterface::on_activate(...)
{
  return hardware_interface::CallbackReturn::SUCCESS;
}

真实项目中常在这里做：

清除历史命令
将当前位置作为初始目标位置
启动电机
清除通信缓存
设置 active 标志

对于位置控制，不能轻易把目标位置初始化成 0，否则控制器激活时，机械臂可能突然回零。更安全的做法通常是：

cmd_positions_[i] = state_positions_[i];

即“刚激活时先保持当前位置”。

read()：硬件状态进入 ROS

真机模式先读取串口：

applySerialFeedback();

随后更新接口数组：

state_positions_[i] = motor->angle_current;
state_velocities_[i] = motor->measure.speed_aps;
state_currents_[i] = motor->measure.real_current;
state_temperatures_[i] = motor->measure.temperature;

虚拟模式没有真实反馈，所以代码自己模拟：

state_positions_[i] += cmd_velocities_[i] * period.seconds();
state_velocities_[i] = cmd_velocities_[i];

这就是最基础的积分公式：

新位置 = 旧位置 + 速度 × 时间

例如：

旧位置 = 1.0 rad
速度   = 2.0 rad/s
周期   = 0.01 s

新位置 = 1.0 + 2.0 × 0.01
       = 1.02 rad

这不是物理仿真，只是一个用于检查控制链路的最小虚拟硬件模型。

控制器计算发生在哪里

一次控制循环可以理解为：

hardware.read(time, period);
controller_manager.update(time, period);
hardware.write(time, period);

例如底盘控制器：

1. 从 /chassis_controller/reference_unstamped 收到底盘速度。
2. 计算四个麦轮的目标速度。
3. 写入四个 cmd_velocities_。
4. write() 把这些速度发给 STM32。

机械臂控制器则写入 cmd_positions_。

write()：ROS 命令发送给硬件

首先处理校准模式，然后检查是否使用串口。

接着限制串口发送频率：

if (last_serial_write_time_.nanoseconds() != 0 &&
    (time - last_serial_write_time_).seconds() <
        serial_command_period_)
{
  return hardware_interface::return_type::OK;
}

当前配置：

update_rate: 1000

表示 ROS 控制循环理论周期为：

1 / 1000 = 0.001 秒 = 1 毫秒

而 Xacro 中：

<param name="serial_command_period">0.02</param>

表示串口命令每 20 毫秒发送一次，即 50 Hz。

所以实际效果是：

控制器计算：1000 Hz
串口发送：50 Hz

到达发送时刻后：

const auto commands = buildSerialCommands();
serial_bridge_.sendMotorCommands(commands);
serial_bridge_.sendHeartbeat();

停止生命周期

停用时：

on_deactivate()

调用：

stopMotors();

清理时：

on_cleanup()

执行：

serial_bridge_.close();
motors_.clear();

职责区别是：

deactivate：停止输出，但资源可以暂时保留
cleanup：释放资源，关闭串口

先手写这个最小虚拟版本

先不要写串口。只练习一个关节的位置控制核心：

hardware_interface::CallbackReturn
MyHardware::on_init(const hardware_interface::HardwareInfo& info)
{
  if (SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
    return CallbackReturn::ERROR;

  const auto count = info_.joints.size();
  positions_.resize(count, 0.0);
  position_commands_.resize(count, 0.0);

  return CallbackReturn::SUCCESS;
}

std::vector<hardware_interface::StateInterface>
MyHardware::export_state_interfaces()
{
  std::vector<hardware_interface::StateInterface> interfaces;

  for (std::size_t i = 0; i < info_.joints.size(); ++i)
  {
    interfaces.emplace_back(
        info_.joints[i].name,
        "position",
        &positions_[i]);
  }

  return interfaces;
}

std::vector<hardware_interface::CommandInterface>
MyHardware::export_command_interfaces()
{
  std::vector<hardware_interface::CommandInterface> interfaces;

  for (std::size_t i = 0; i < info_.joints.size(); ++i)
  {
    interfaces.emplace_back(
        info_.joints[i].name,
        "position",
        &position_commands_[i]);
  }

  return interfaces;
}

hardware_interface::return_type
MyHardware::read(const rclcpp::Time&, const rclcpp::Duration&)
{
  positions_ = position_commands_;
  return hardware_interface::return_type::OK;
}

hardware_interface::return_type
MyHardware::write(const rclcpp::Time&, const rclcpp::Duration&)
{
  return hardware_interface::return_type::OK;
}

这个虚拟模型表示：

控制器要求到哪里，关节就瞬间到哪里

虽然不真实，但它完整体现了：

command interface → 命令数组
read() → 状态数组
state interface → 控制器和 joint_states