众擎机器人开源代码解读

SLAM必须dunk2025-09-05 16:42

一，综述

EngineAI ROS 包：

  • 高层开发模式：用户可通过发布身体速度指令，直接调用 EngineAI 机器人的行走控制器。
  • 底层开发模式：用户可通过发布关节指令，自主开发专属的控制器。
  • ROS2 package：全称为 "Robot Operating System 2 Package"（机器人操作系统 2 功能包），是 ROS2 生态中用于组织代码、配置文件等资源的基本单元，便于模块化开发与复用。
  • body velocity command："身体速度指令"，通常包含机器人在三维空间中的线速度（如前后、左右移动速度）和角速度（如转向速度），是高层控制中常用的指令类型，无需用户关注单个关节的运动细节。
  • joint command："关节指令"，针对机器人单个或多个关节的控制指令（如目标角度、角速度等），底层开发模式下用户需通过该指令精确控制关节运动，以实现自定义动作或控制逻辑。

二，接口协议

Topic/Service Name Type Rate Message Description DataStructure
/hardware/gamepad_keys Topic Publish >=500hz GamepadKeys.msg Gamepad Message Feedback # Timestamp field using ROS2 header uint8 LB = 0 uint8 RB = 1 uint8 A = 2 uint8 B = 3 uint8 X = 4 uint8 Y = 5 uint8 BACK = 6 uint8 START = 7 uint8 CROSS_X_UP = 8 uint8 CROSS_X_DOWN = 9 uint8 CROSS_Y_LEFT = 10 uint8 CROSS_Y_RIGHT = 11 uint8 LT = 0 uint8 RT = 1 uint8 LEFT_STICK_X = 2 uint8 LEFT_STICK_Y = 3 uint8 RIGHT_STICK_X = 4 uint8 RIGHT_STICK_Y = 5 std_msgs/Header header # Digital buttons (0 = released, 1 = pressed) int32[12] digital_states # Fixed size array of 12 elements # Analog inputs (range: -1.0 to 1.0) float64[6] analog_states # Fixed size array of 6 elements
/hardware/imu_info Topic Publish >=500hz IMUInfo.msg IMU Sensor Feedback std_msgs/Header header geometry_msgs/Quaternion quaternion geometry_msgs/Vector3 rpy geometry_msgs/Vector3 linear_acceleration geometry_msgs/Vector3 angular_velocity
/hardware/joint_state Topic Publish >=500hz JointState.msg Joint Status Feedback std_msgs/Header header float64[] position float64[] velocity float64[] torque
/hardware/joint_command Topic Subscription 0~500hz JointCommand.msg Joint Command Subscription std_msgs/Header header float64[] position float64[] velocity float64[] feed_forward_torque float64[] torque float64[] stiffness float64[] damping uint8 parallel_parser_type
/motion/motion_state Topic Publish >=5hz MotionState.msg Motion Status Feedback of Robot string current_motion_task
/hardware/motor_debug Topic Publish >=50hz MotorDebug.msg Feedback of Temperature, Tau, etc. float64[] tau_cmd float64[] mos_temperature float64[] motor_temperature
/motion/body_vel_cmd Topic Publish >=5hz BodyVelCmd.msg body velocity command publish, linear_x_vel ranges [-0.5m/s +0.5m/s] linear_y_vel ranges [-0.2m/s, +0.2m/s]， yaw_vel rangs [-0.5rad/s, 0.5rad/s] std_msgs/Header header float64[] linear_velocity float64 yaw_velocity
/heartbeat std_msgs/Header header string node_name int64 startup_timestamp # idle, running, error, sleep string node_status int64 error_code string error_message
/enablemotor serve EnableMotor.srv bool enable --- bool success string message

三，工作区架构及编译环境

复制代码 
└── src
    ├── interface_example # Interface examples, recommended to run on Nezha
    ├── interface_protocol # Interface protocols, common modules
    ├── third_party # Third-party libraries
    ├── simulation # Simulation environment running on host
  • Ubuntu 22.04
  • ROS2 Humble Desktop
  • GCC >= 11
  • CMake >= 3.22
  • Python >= 3.10

四，FSM 三个模式：操纵杆模式，高层开发模式，底层开发模式

操纵杆模式、高层开发模式和底层开发模式的有限状态机（FSM）

操纵杆状态机

注意：LB+CORSS_Y_LEFT 是切入 joint commands即低层开发控制

高层开发模式

Body Velocity Control

底层开发模式

底层开发（Low-level Development）允许用户使用基于 EngineAI 开源强化学习（RL）训练框架（仓库地址：https://github.com/engineai-robotics/engineai_gym）训练的自定义强化学习控制器。用户可按照以下步骤逐步操作，实现基于 Mujoco 模拟器的 "仿真到仿真"（sim2sim）功能及实际部署。

五，部署操作：
在主机上运行 rl_basic_example 程序

步骤 1：进入 PD 站立模式（pd-stand mode）

  1. 确保机器人稳固放置在平整地面上，且周围有足够操作空间；
  2. 使用遥控器进入 PD 站立模式；
  3. 继续操作前，请确认机器人已稳定站立。

步骤 2：进入关节桥接模式（joint bridge mode）

关节桥接模式支持通过关节指令对机器人进行控制，操作步骤如下：

  1. 当机器人处于 PD 站立模式时，按下遥控器的【LB 键 + 左十字键 Y 向（CROSS_Y_LEFT）】组合键；

  2. 机器人将进入关节桥接模式，此时可通过关节指令对其进行控制；

  3. 执行以下命令验证机器人状态： bash

    复制代码 
    # 在主机端执行
ros2 topic echo /motion/motion_state

  4. 继续操作前，请确认运动状态（motion state）显示为 "joint_bridge"。

步骤 3：运行强化学习（RL）示例程序

安全警告（SAFETY WARNING）
  • 确保所有人员与机器人保持安全距离；
  • 若机器人出现异常动作，请随时准备紧急停止（可按下急停按钮或使机器人进入被动模式）。
启动示例程序 执行以下命令启动示例程序：bash
复制代码 
# 在主机端执行
ros2 launch interface_example rl_basic_example.launch.py

六，编译

几个脚本梳理：

1）同步代码到板级

在主机（host） 端执行以下命令：./scripts/sync_src.sh nezha

（说明："nezha" 指 "哪吒" 目标板，是该机器人系统适配的硬件设备；sync_src.sh为代码同步脚本，用于将主机端的开发代码传输到目标板，确保两端代码一致性。）
2）构建工作空间

首先通过 SSH 连接到哪吒板（Nezha board），并进入工作空间目录：

cd ~/source/engineai_workspace

（说明：~/source/engineai_workspace是代码在目标板上的默认工作空间路径，cd命令用于切换到该目录。）

然后执行构建脚本，编译节点代码：

./scripts/build_nodes.sh

（说明：build_nodes.sh是用于编译机器人功能节点的脚本，"nodes" 指 ROS 2 框架中的 "节点"，是实现具体功能的可执行单元，编译后生成可运行的程序文件。）
3)运行 PlotJuggler 进行数据监控

在主机（host） 端编译interface_protocol功能包：

in host

colcon build --packages-select interface_protocol

（说明：colcon是 ROS 2 的官方构建工具，--packages-select interface_protocol表示仅编译 "interface_protocol" 这一个功能包，该包通常包含机器人的数据通信协议相关代码。）

source install/setup.bash

（说明：source命令用于加载脚本文件，install/setup.bash是编译后生成的环境配置脚本，加载后主机才能识别并运行后续的 ROS 2 程序。）

ros2 run plotjuggler plotjuggler -n

（说明：ros2 run用于启动 ROS 2 功能包中的可执行程序，-n是 PlotJuggler 的参数，全称 "--no-config"，表示不自动加载之前保存的配置文件，便于用户重新选择需监控的数据话题。）

src/interface_protocol/pm_data_layout.xml

（说明："layout file"（布局文件）包含数据图表的排列、待监控的话题选择等配置，加载后无需手动设置即可直接查看预设的机器人关键数据，如关节角度、电机电流等。）

check_format.sh

使用场景

这个脚本通常会集成到 CI/CD 流程中，在代码提交或 PR 创建时自动运行，确保提交的代码符合项目的格式规范。也可以在开发过程中手动运行，提前发现并修复格式问题。

依赖工具

脚本需要以下工具在系统中可用：

  • clang-format (用于 C/C++ 格式化)
  • yamllint (用于 YAML 文件检查)
  • autopep8 (用于 Python 格式检查)
  • git (用于检查文件变更)

如果需要修改检查规则，可以相应调整各工具的配置文件（如.clang-format、.yamllint 等）。

七，代码解析

interface_example

--components

message_handler.cc 一个 ROS 2 消息处理器类MessageHandler，负责订阅和发布各种 ROS 消息，是 ROS 2 节点与外部通信的关键组件

功能简述：MessageHandler类封装了 ROS 2 的订阅者 (subscriber) 和发布者 (publisher)，用于处理与硬件和运动控制相关的消息，包括游戏手柄输入、IMU 传感器数据、关节状态和运动状态，并能发布关节控制命令

cpp 复制代码 
#include "message_handler.hpp"
#include <rclcpp/qos.hpp>

namespace example {

/* 
接收一个 ROS 2 节点的智能指针并存储，所有的 ROS 通信操作都依赖这个节点对象
这是 ROS 2 中常见的设计模式，将通信功能封装在独立类中，通过节点指针与 ROS 系统交互
*/
MessageHandler::MessageHandler(rclcpp::Node::SharedPtr node) : node_(node) {}

void MessageHandler::Initialize() {
  rclcpp::QoS qos(3);
  qos.best_effort();
  qos.durability_volatile();
/*
rclcpp::QoS qos(3)：创建 QoS 配置，设置消息队列深度为 3
qos.best_effort()：使用 "尽力而为" 策略，不保证消息可靠传递但开销小
qos.durability_volatile()：非持久化，新订阅者不会收到历史消息
这种配置适合实时性要求高但对消息丢失不敏感的场景（如传感器数据）
*/
  // Initialize subscribers 初始化订阅
  // 游戏手柄消息订阅
  /*
    模板参数指定消息类型：interface_protocol::msg::GamepadKeys
    第一个参数是订阅的话题名：/hardware/gamepad_keys
    第二个参数是 QoS 配置
    第三个参数是回调函数绑定，当收到消息时调用GamepadCallback 会在回调函数再去做内部变量的传递
  */
  gamepad_sub_ = node_->create_subscription<interface_protocol::msg::GamepadKeys>(
      "/hardware/gamepad_keys", qos, 
    std::bind(&MessageHandler::GamepadCallback, this, std::placeholders::_1));

  imu_sub_ = node_->create_subscription<interface_protocol::msg::ImuInfo>(
      "/hardware/imu_info", qos, 
    std::bind(&MessageHandler::ImuCallback, this, std::placeholders::_1));

  joint_state_sub_ = node_->create_subscription<interface_protocol::msg::JointState>(
      "/hardware/joint_state", qos, 
    std::bind(&MessageHandler::JointStateCallback, this, std::placeholders::_1));


  // Initialize publisher 初始化发布
  //发布joint_command
  /*
    创建发布者，用于发布interface_protocol::msg::JointCommand类型消息
    发布到/hardware/joint_command话题，使用前面定义的 QoS 配置
  */

  joint_cmd_pub_ = node_->create_publisher<interface_protocol::msg::JointCommand>("/hardware/joint_command", qos);

  motion_state_sub_ = node_->create_subscription<interface_protocol::msg::MotionState>(
      "/motion/motion_state", 1, std::bind(&MessageHandler::MotionStateCallback, this, std::placeholders::_1));
}



void MessageHandler::MotionStateCallback(const interface_protocol::msg::MotionState::SharedPtr msg) {
  latest_motion_state_ = msg;
}

void MessageHandler::GamepadCallback(const interface_protocol::msg::GamepadKeys::SharedPtr msg) {
  latest_gamepad_ = msg;
}

void MessageHandler::ImuCallback(const interface_protocol::msg::ImuInfo::SharedPtr msg) { latest_imu_ = msg; }

void MessageHandler::JointStateCallback(const interface_protocol::msg::JointState::SharedPtr msg) {
  latest_joint_state_ = msg;
}

//PublishJointCommand 俩种重载形式：1接收智能指针  2接收对象索引
void MessageHandler::PublishJointCommand(const interface_protocol::msg::JointCommand::SharedPtr command) {
  joint_cmd_pub_->publish(*command);
}
void MessageHandler::PublishJointCommand(const interface_protocol::msg::JointCommand& command) {
  joint_cmd_pub_->publish(command);
}
}  // namespace example

--src

hold_joint_status.cc

功能描述：这个节点的核心功能是 "关节保持"：

1，等待并获取当前关节状态

2，以当前关节位置为目标位置，构建控制命令

3，以 500Hz 的频率持续发布控制命令，使关节保持在初始位置

cpp 复制代码 
#include <chrono>
#include <interface_protocol/msg/detail/motor_command__struct.hpp>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include "components/message_handler.hpp"

using namespace std::chrono_literals;

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<rclcpp::Node>("joint_test_example");
/*
rclcpp::init()：初始化 ROS 2 运行时环境，解析命令行参数
rclcpp::Node：创建名为joint_test_example的节点，是 ROS 2 中所有通信的基础
*/
  auto message_handler = std::make_shared<example::MessageHandler>(node);
  message_handler->Initialize();
/*
实例化消息处理器，将节点指针传入，用于创建订阅者和发布者
Initialize()方法会初始化所有 ROS 2 通信接口（见之前的MessageHandler代码）
*/

  interface_protocol::msg::JointCommand hold_joint_command;

  while (!message_handler->GetLatestJointState()) {
    rclcpp::spin_some(node);
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    RCLCPP_INFO_THROTTLE(node->get_logger(), *node->get_clock(), 1000, "Waiting for first joint state...");
  }

/*
rclcpp::spin_some()：处理当前队列中的所有回调，非阻塞式
RCLCPP_INFO_THROTTLE：带节流的日志输出，避免日志刷屏
这段逻辑确保在获取到第一个关节状态之前程序不会继续执行，相当于初始化
*/


  auto latest_joint_state = message_handler->GetLatestJointState();
  //print latest_joint_state 打印最新的关节状态
  std::stringstream ss;
  ss << "Latest joint state size: " << latest_joint_state->position.size() << "\nvalues: " ;
  for (size_t i = 0; i < latest_joint_state->position.size(); i++) {
    ss << " " << latest_joint_state->position[i];
  }
  RCLCPP_INFO(node->get_logger(), "%s", ss.str().c_str());  

  // Example: Copy current positions and add some feed-forward torque

  hold_joint_command.position = latest_joint_state->position;
  hold_joint_command.feed_forward_torque = latest_joint_state->torque;
  hold_joint_command.torque = latest_joint_state->torque;
  hold_joint_command.velocity.resize(latest_joint_state->position.size(), 0.0);
  hold_joint_command.stiffness.resize(latest_joint_state->position.size(), 400.0);  // Example stiffness
  hold_joint_command.damping.resize(latest_joint_state->position.size(), 5.0);      // Example damping

  // Create timer for publishing motor commands at 500Hz
  rclcpp::TimerBase::SharedPtr timer =
      node->create_wall_timer(2ms,  // 500Hz = 2ms period
                              [&]() {
                                hold_joint_command.header.stamp = node->get_clock()->now();
                                message_handler->PublishJointCommand(hold_joint_command);
                              });
/*
create_wall_timer：创建一个墙壁时钟定时器，每 2ms 触发一次回调
回调函数中更新消息时间戳并发布关节控制命令
500Hz 的发布频率对于实时控制来说是比较合理的选择
*/

  rclcpp::spin(node); // 阻塞式运行节点，处理回调和定时器
  rclcpp::shutdown(); // 退出时清理ROS 2资源
  return 0;
}

joint_test_example.cc

功能描述：一个基于 ROS 2 的机器人关节控制节点程序，相比之前的版本增加了轨迹规划功能，能够根据配置文件生成关节点轨迹并控制关节运动到目标位置。

1，程序启动，加载配置文件参数

2，等待并获取初始关节状态

3，根据初始位置和目标位置生成线性插值轨迹

4，以 500Hz 频率循环发布控制命令，使关节按轨迹运动

5，当所有关节到达目标位置时，输出提示信息

cpp 复制代码 
#include <yaml-cpp/yaml.h>
#include <Eigen/Dense>
#include <chrono>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include "components/message_handler.hpp"

using namespace std::chrono_literals;

namespace {
// Constants Default 这么写是个好习惯，当然放到XML更好，可以不用编译
constexpr double kControlFrequency = 500.0;                 // Control frequency in Hz
constexpr double kControlPeriod = 1.0 / kControlFrequency;  // Control period in seconds
}  // namespace



class JointTestExample : public rclcpp::Node {
 public:
  JointTestExample(const std::string& config_dir_path)
      : Node("joint_test_example"), joint_test_config_path_(config_dir_path + "/joint_test.yaml") {
    // Load motor parameters from yaml
    LoadJointTestParams();
  }
/*
类继承自rclcpp::Node，使节点功能更内聚
JointTestExample 接收配置文件路径，拼接出关节测试配置文件的完整路径
调用LoadJointTestParams()从 YAML 文件加载控制参数
*/

 //初始化消息处理器，获取初始关节状态，很好的编程习惯用了try catch
  bool Initialize() {
    try {
      // Initialize message handler
      msg_handler_ = std::make_shared<example::MessageHandler>(shared_from_this());
      msg_handler_->Initialize();

      // Wait for first motor state
      while (!msg_handler_->GetLatestJointState()) {
        rclcpp::spin_some(shared_from_this());
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
        RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, "Waiting for first motor state...");
      }

      // Get initial motor state
      auto initial_state = msg_handler_->GetLatestJointState();
      initial_positions_ = initial_state->position;

      // Print initial positions
      std::stringstream ss;
      ss << "\nInitial positions:\n[";
      for (size_t i = 0; i < initial_positions_.size(); ++i) {
        ss << std::fixed << std::setprecision(3) << initial_positions_[i];
        if (i < initial_positions_.size() - 1) {
          ss << ", ";
          // Add newline every 6 elements for better readability
          if ((i + 1) % 6 == 0) {
            ss << "\n ";
          }
        }
      }
      ss << "]\n";
      RCLCPP_INFO(get_logger(), "%s", ss.str().c_str());

      // Convert initial motor positions to joint positions and generate trajectories
      GenerateTrajectories();

      // Create timer for periodic execution
      timer_ = create_wall_timer(std::chrono::duration<double>(kControlPeriod),
                                 std::bind(&JointTestExample::TimerCallback, this));

      return true;
    } catch (const std::exception& e) {
      RCLCPP_ERROR(get_logger(), "Failed to initialize: %s", e.what());
      return false;
    }
  }



 private:
 //生成从初始位置到目标位置的插值轨迹
  void GenerateTrajectories() {
    // Convert initial motor positions to joint positions
    current_joint_positions_ = Eigen::VectorXd(initial_positions_.size());
    for (size_t i = 0; i < initial_positions_.size(); ++i) {
      current_joint_positions_[i] = initial_positions_[i];
    }

    // Initialize interpolation for each joint
    interpolated_positions_.resize(initial_positions_.size());
    current_steps_.resize(initial_positions_.size(), 0);
    reached_targets_.resize(initial_positions_.size(), false);

    // Generate interpolated trajectories for each joint
    for (size_t i = 0; i < initial_positions_.size(); ++i) {
      interpolated_positions_[i].resize(num_steps_[i]);
      double step = (target_positions_[i] - current_joint_positions_[i]) / (num_steps_[i] - 1);
      for (int j = 0; j < num_steps_[i]; ++j) {
        interpolated_positions_[i][j] = current_joint_positions_[i] + step * j;
      }
    }

    RCLCPP_INFO(get_logger(), "Trajectories generated for %ld joints", initial_positions_.size());
  }

  //从 YAML 配置文件加载关节控制参数（目标位置、步数、PID 参数等）
  void LoadJointTestParams() {
    try {
      YAML::Node config = YAML::LoadFile(joint_test_config_path_);

      // Load parameters
      auto target_pos = config["target_position"];
      auto steps = config["num_steps"];
      auto kp = config["kp"];
      auto kd = config["kd"];

      // Clear vectors
      target_positions_.clear();
      num_steps_.clear();
      kp_.clear();
      kd_.clear();

      // Concatenate all groups into single vectors
      for (const auto& pos_group : target_pos) {
        auto vec = pos_group.as<std::vector<double>>();
        target_positions_.insert(target_positions_.end(), vec.begin(), vec.end());
      }

      for (const auto& steps_group : steps) {
        auto vec = steps_group.as<std::vector<double>>();
        num_steps_.insert(num_steps_.end(), vec.begin(), vec.end());
      }

      for (const auto& kp_group : kp) {
        auto vec = kp_group.as<std::vector<double>>();
        kp_.insert(kp_.end(), vec.begin(), vec.end());
      }

      for (const auto& kd_group : kd) {
        auto vec = kd_group.as<std::vector<double>>();
        kd_.insert(kd_.end(), vec.begin(), vec.end());
      }

      // Print loaded parameters for verification
      RCLCPP_INFO(get_logger(), "Loaded joint test parameters:");
      RCLCPP_INFO(get_logger(), "Number of joints: %ld", target_positions_.size());

    } catch (const std::exception& e) {
      RCLCPP_ERROR(get_logger(), "Failed to load joint test parameters: %s", e.what());
      throw;
    }
  }

  //以固定频率（500Hz）发布关节控制命令，使关节按轨迹运动,监控运动状态，当所有关节到达目标位置时给出提示
  void TimerCallback() {
    auto joint_command = std::make_shared<interface_protocol::msg::JointCommand>();
    joint_command->position.resize(initial_positions_.size());
    joint_command->velocity.resize(initial_positions_.size());
    joint_command->feed_forward_torque.resize(initial_positions_.size(), 0.0);
    joint_command->torque.resize(initial_positions_.size(), 0.0);
    joint_command->stiffness.resize(initial_positions_.size(), 400.0);
    joint_command->damping.resize(initial_positions_.size(), 5.0);

    bool all_reached = true;
    for (size_t i = 0; i < initial_positions_.size(); ++i) {
      if (!reached_targets_[i]) {
        // 如果还没到达目标位置，发送当前插值点
        if (current_steps_[i] < num_steps_[i]) {
          double joint_pos = interpolated_positions_[i][current_steps_[i]];
          joint_command->position[i] = joint_pos;
          joint_command->velocity[i] = 0.0;  // Zero velocity for position control
          current_steps_[i]++;
        } else {
          // Hold at target position // 已到达目标位置某些关节，保持目标位置
          double joint_pos = target_positions_[i];
          joint_command->position[i] = joint_pos;
          joint_command->velocity[i] = 0.0;
          reached_targets_[i] = true;
        }
        all_reached = false;
      } else {
        // Keep sending the target position // 已到达目标，持续发送目标位置
        double joint_pos = target_positions_[i];
        joint_command->position[i] = joint_pos;
        joint_command->velocity[i] = 0.0;
      }
    }
    // Print initial positions
    std::stringstream ss;
    ss << "\nCurrent positions:\n[";
    for (size_t i = 0; i < joint_command->position.size(); ++i) {
      ss << std::fixed << std::setprecision(3) << joint_command->position[i];
      if (i < joint_command->position.size() - 1) {
        ss << ", ";
        // Add newline every 6 elements for better readability
        if ((i + 1) % 6 == 0) {
          ss << "\n ";
        }
      }
    }
    ss << "]\n";
    // RCLCPP_INFO(get_logger(), "%s", ss.str().c_str()); 这也是ROS2自带函数RCLCPP_INFO
    // 检查是否所有关节都到达目标
    if (all_reached) {
      RCLCPP_INFO_ONCE(get_logger(), "All joints have reached their target positions!");
      return;
    }

    msg_handler_->PublishJointCommand(joint_command);
  }

  std::string config_path_;
  std::string joint_test_config_path_;
  std::shared_ptr<example::MessageHandler> msg_handler_;
  std::vector<double> initial_positions_;

  // Joint test parameters
  std::vector<double> target_positions_;
  std::vector<double> num_steps_;
  std::vector<double> kp_;
  std::vector<double> kd_;
  std::vector<std::vector<double>> interpolated_positions_;
  std::vector<int> current_steps_;
  std::vector<bool> reached_targets_;
  Eigen::VectorXd current_joint_positions_;
  rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char** argv) {
  if (argc != 2) {
    std::cerr << "Usage: " << argv[0] << " <path_to_config_directory>" << std::endl;
    return 1;
  }

  rclcpp::init(argc, argv);

  // Remove trailing slash if present
  std::string config_dir = argv[1];
  if (!config_dir.empty() && config_dir.back() == '/') {
    config_dir.pop_back();
  }

  auto node = std::make_shared<JointTestExample>(config_dir);
  if (!node->Initialize()) {
    rclcpp::shutdown();
    return 1;
  }

  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

rl_basic_example.cc

功能描述：一个基于 ROS 2 的强化学习 (RL) 基础运行器节点RlBasicRunner，用于通过强化学习模型生成机器人关节控制命令

目录

一，综述

二，接口协议

三，工作区架构及编译环境

[四，FSM 三个模式：操纵杆模式，高层开发模式，底层开发模式](#四，FSM 三个模式：操纵杆模式，高层开发模式，底层开发模式)

操纵杆状态机

高层开发模式

[Body Velocity Control](#Body Velocity Control)

底层开发模式

[步骤 1：进入 PD 站立模式（pd-stand mode）](#步骤 1：进入 PD 站立模式（pd-stand mode）)

[步骤 2：进入关节桥接模式（joint bridge mode）](#步骤 2：进入关节桥接模式（joint bridge mode）)

[步骤 3：运行强化学习（RL）示例程序](#步骤 3：运行强化学习（RL）示例程序)

[安全警告（SAFETY WARNING）](#安全警告（SAFETY WARNING）)

[启动示例程序 执行以下命令启动示例程序：bash](#启动示例程序 执行以下命令启动示例程序：bash)

六，编译

使用场景

依赖工具

七，代码解析

cpp 复制代码 
#include <chrono>
#include <memory>
#include <rclcpp/logging.hpp>
#include <rclcpp/rclcpp.hpp>
#include <thread>

#include "components/message_handler.hpp"
#include "math/concatenate_vector.h"
#include "math/mnn_model.h"
#include "math/rotation_matrix.h"
#include "parameter/rl_basic_param.h"

using namespace std::chrono_literals;

namespace example {

class RlBasicRunner : public rclcpp::Node {
 public:
  //在 ROS 2 节点类中，explicit这种写法很常见，因为节点构造通常需要明确的参数（如配置路径），不希望被编译器自动转换，避免逻辑错误。
  explicit RlBasicRunner(const std::string& config_file_dir) : Node("rl_basic_runner") {
    std::string config_file = config_file_dir + "/rl_basic_param.yaml";
    param_ = std::make_shared<RlBasicParam>(config_file);
    config_file_dir_ = config_file_dir;
    joint_command_ = std::make_shared<interface_protocol::msg::JointCommand>();
  }

  bool Initialize() {
    try {
      // Initialize message handler 初始化发布和订阅
      message_handler_ = std::make_shared<MessageHandler>(shared_from_this());
      message_handler_->Initialize();

      // Wait for first motion state 等待获取初始关节状态
      while (!message_handler_->GetLatestMotionState() ||
             message_handler_->GetLatestMotionState()->current_motion_task != "joint_bridge") {
        rclcpp::spin_some(shared_from_this());
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
        RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, "Waiting for joint bridge state...");
      }
      RCLCPP_INFO(get_logger(), "Already in joint bridge state");

      // Get initial joint positions  获取初始关节状态
      auto initial_state = message_handler_->GetLatestJointState();
      if (!initial_state) {
        RCLCPP_ERROR(get_logger(), "Failed to get initial joint state");
        return false;
      }

      initial_joint_q_ =
          Eigen::Map<const Eigen::VectorXd>(initial_state->position.data(), initial_state->position.size());

      RCLCPP_INFO_STREAM(get_logger(), "Initial joint positions: " << initial_joint_q_.transpose());

      // Initialize active joint indices 初始化关节参数（从配置文件拼接）
      active_joint_idx_ = param_->active_joint_idx;
      // Concatenate joint parameters from yaml
      default_joint_q_ = math::ConcatenateVectors(param_->default_joint_q);
      joint_kp_ = math::ConcatenateVectors(param_->joint_kp);
      joint_kd_ = math::ConcatenateVectors(param_->joint_kd);
      action_scale_ = math::ConcatenateVectors(param_->action_scale);

      // Initialize MNN model // 初始化MNN神经网络模型
      mlp_net_ = std::make_unique<math::MnnModel>(config_file_dir_ + "/" + param_->policy_file);
      mlp_net_observation_.setZero(param_->num_observations, param_->num_include_obs_steps);
      mlp_net_action_.setZero(param_->num_actions);

      // Initialize control variables
      time_ = 0.0;
      global_phase_ = 0.0;
      is_first_time_ = true;

      RCLCPP_INFO(get_logger(), "Starting control loop");
      // Create control timer
      control_timer_ = create_wall_timer(std::chrono::duration<double>(param_->control_dt),
                                         std::bind(&RlBasicRunner::ControlCallback, this));

      return true;
    } catch (const std::exception& e) {
      RCLCPP_ERROR(get_logger(), "Failed to initialize: %s", e.what());
      return false;
    }
  }

 private:
  void ControlCallback() {
    //检查当前运动状态是否为"joint_bridge"
    if (message_handler_->GetLatestMotionState()->current_motion_task != "joint_bridge") {
      time_ = 0.0;
      is_first_time_ = true;
      return;
    }
    auto joint_state = message_handler_->GetLatestJointState();
    if (!joint_state) return;  // Skip if no joint state received yet // 无关节状态则跳过

    // 控制流程：更新状态 -> 计算观测 -> 计算命令 -> 发送命令
    UpdateState(joint_state);
    CalculateObservation();
    CalculateMotorCommand();
    SendMotorCommand();

    time_ += param_->control_dt; // 更新控制时间
  }

  void UpdateState(const interface_protocol::msg::JointState::SharedPtr& joint_state) {
    // Update joint states // 更新关节状态（位置、速度）
    q_real_ = Eigen::Map<const Eigen::VectorXd>(joint_state->position.data(), joint_state->position.size());
    qd_real_ = Eigen::Map<const Eigen::VectorXd>(joint_state->velocity.data(), joint_state->velocity.size());

    // Update command from gamepad // 处理游戏手柄输入命令 command x,y,z
    auto gamepad = message_handler_->GetLatestGamepad();
    if (gamepad) {
      command_.x() =
          gamepad->analog_states[interface_protocol::msg::GamepadKeys::LEFT_STICK_X] * param_->command_scale.x();
      command_.y() =
          gamepad->analog_states[interface_protocol::msg::GamepadKeys::LEFT_STICK_Y] * param_->command_scale.y();
      command_.z() =
          gamepad->analog_states[interface_protocol::msg::GamepadKeys::RIGHT_STICK_Y] * param_->command_scale.z();
    }
  }

  //构建强化学习的观测向量
  void CalculateObservation() { 
    // Calculate phase info // 计算全局相位（周期信号）
    global_phase_ += param_->control_dt / param_->cycle_time;
    global_phase_ -= static_cast<int>(global_phase_);



    // Get IMU data // 处理IMU数据（旋转矩阵、欧拉角）
    auto imu = message_handler_->GetLatestImu();
    Eigen::Matrix3d R_real =
        Eigen::Quaterniond(imu->quaternion.w, imu->quaternion.x, imu->quaternion.y, imu->quaternion.z)
            .toRotationMatrix();
    Eigen::Vector3d w_real = Eigen::Vector3d(imu->angular_velocity.x, imu->angular_velocity.y, imu->angular_velocity.z);
    // ZYX顺序的欧拉角（等同于RPY）
    Eigen::Vector3d euler_xyz = math::CalcRollPitchYawFromRotationMatrix(R_real);

    // Stack the observation
    Eigen::VectorXd mlp_net_observation_single = Eigen::VectorXd::Zero(param_->num_observations);
    // if (param_->mix) {
    //   mlp_net_observation_single <<                         //  command
    //       (q_real_ - default_joint_q_)(active_joint_idx_),  //  joint position - joint default position: kDoFs
    //       qd_real_(active_joint_idx_),                      //  joint velocity: kDoFs
    //       mlp_net_action_,                                  //  last joint action: kDoFs
    //       w_real,                                           //  base angular velocity w.r.t base frame: 3
    //       euler_xyz;                                        //  base euler angle rpy w.r.t base frame: 3
    // } else {
    //   mlp_net_observation_single << clock_signal,           //  phase signals: 2
    //       command_,                                         //  command
    //       (q_real_ - default_joint_q_)(active_joint_idx_),  //  joint position - joint default position: kDoFs
    //       qd_real_(active_joint_idx_),                      //  joint velocity: kDoFs
    //       mlp_net_action_,                                  //  last joint action: kDoFs
    //       w_real,                                           //  base angular velocity w.r.t base frame: 3
    //       euler_xyz;                                        //  base euler angle rpy w.r.t base frame: 3
    // }
    mlp_net_observation_single << (q_real_ - default_joint_q_)(
      active_joint_idx_),           //  joint position - joint default position: kDoFs // 关节位置偏差
      qd_real_(active_joint_idx_),  //  joint velocity: kDoFs                          // 关节速度
      mlp_net_action_,              //  last joint action: kDoFs                       // 上一时刻动作
      w_real,                       //  base angular velocity w.r.t base frame: 3      // IMU角速度
      euler_xyz;                    //  base euler angle rpy w.r.t base frame: 3       // 欧拉角

  // Scales and clips the observation

    // Scale and clip the observation // 观测值缩放和裁剪
    mlp_net_observation_single.array() *= param_->observation_scale.array();
    mlp_net_observation_single =
        mlp_net_observation_single.cwiseMax(-param_->observation_clip).cwiseMin(param_->observation_clip);

    // Update the observation buffer // 更新观测缓冲区（多步观测），可能用于时序建模
    // mlp_net_observation_ 是一个 Eigen 矩阵，维度为 [param_->num_observations × param_->num_include_obs_steps]
    // mlp_net_observation_single 是一个 Eigen 向量，维度为 [param_->num_observations × 1]
    if (is_first_time_) {
      is_first_time_ = false;
      mlp_net_observation_.setZero(param_->num_observations, param_->num_include_obs_steps);
      mlp_net_action_.setZero(param_->num_actions);
      mlp_net_observation_.rightCols(1) = mlp_net_observation_single;
    } else {
      //将矩阵中右侧的 n-1 列数据整体左移，覆盖最左侧的 n-1 列
      mlp_net_observation_.leftCols(param_->num_include_obs_steps - 1) =
          mlp_net_observation_.rightCols(param_->num_include_obs_steps - 1);
      //将当前最新的观测数据 mlp_net_observation_single 放入矩阵最右侧的新列中   
      mlp_net_observation_.rightCols(1) = mlp_net_observation_single;
    }
    /*
    假设 
          [ t0_obs1, t1_obs1, t2_obs1 ]
          [ t0_obs2, t1_obs2, t2_obs2 ]
          [ ...     ...     ...       ]
    经过第一步
          [ t1_obs1, t2_obs1, t2_obs1 ]  // 注意最后一列临时保留原值，下一步会被覆盖
          [ t1_obs2, t2_obs2, t2_obs2 ]
          [ ...     ...     ...       ]   
    经过第二步
          [ t1_obs1, t2_obs1, t3_obs1 ]  // 完成窗口滑动，包含 t1, t2, t3 三步观测
          [ t1_obs2, t2_obs2, t3_obs2 ]
          [ ...     ...     ...       ]  
    */
  }

  void CalculateMotorCommand() {
    // 构建完整的模型输入（包含时钟信号和命令）
    Eigen::Vector2d clock_signal(std::sin(2 * M_PI * global_phase_), std::cos(2 * M_PI * global_phase_));
    Eigen::VectorXd obs = Eigen::VectorXd::Zero(
      param_->num_observations * param_->num_include_obs_steps + 
      param_->num_clock_signal + 
      param_->num_commands
    ); // 组合观测、时钟信号和命令

    obs = Eigen::VectorXd::Zero(param_->num_observations * param_->num_include_obs_steps + param_->num_clock_signal + param_->num_commands);
    obs.head(param_->num_observations * param_->num_include_obs_steps) =
        Eigen::Map<Eigen::VectorXd>(mlp_net_observation_.transpose().data(), mlp_net_observation_.size());
    command_.array() *= param_->obs_commands_scale.array();
    obs.tail(param_->num_clock_signal + param_->num_commands) << clock_signal, command_;


    // Get MNN  // MNN模型推理得到动作
    mlp_net_action_ = mlp_net_->Inference(obs.cast<float>()).cast<double>();
    mlp_net_action_ = mlp_net_action_.cwiseMax(-param_->action_clip).cwiseMin(param_->action_clip);

    // Calculate desired joint positions  // 计算期望关节位置
    q_des_ = default_joint_q_;                                                 // Use concatenated default_joint_q_
    q_des_(active_joint_idx_) += mlp_net_action_.cwiseProduct(action_scale_);  // Use concatenated action_scale_

    // 过渡阶段处理（从初始位置平滑过渡）
    if (time_ < param_->transition_time) {
      float ratio = time_ / param_->transition_time;
      q_des_ = ratio * q_des_ + (1 - ratio) * initial_joint_q_;
    }
  }

  void SendMotorCommand() {
    // Convert Eigen vectors to std::vector
    joint_command_->position = std::vector<double>(q_des_.data(), q_des_.data() + q_des_.size());
    joint_command_->velocity = std::vector<double>(q_des_.size(), 0.0);
    joint_command_->feed_forward_torque = std::vector<double>(q_des_.size(), 0.0);
    joint_command_->torque = std::vector<double>(q_des_.size(), 0.0);
    joint_command_->stiffness = std::vector<double>(joint_kp_.data(), joint_kp_.data() + joint_kp_.size());
    joint_command_->damping = std::vector<double>(joint_kd_.data(), joint_kd_.data() + joint_kd_.size());
    joint_command_->parallel_parser_type = interface_protocol::msg::ParallelParserType::RL_PARSER;
    // Send command through message handler
    message_handler_->PublishJointCommand(*joint_command_);
  }

  // Parameters
  std::shared_ptr<RlBasicParam> param_;

  // Message handling
  std::shared_ptr<MessageHandler> message_handler_;

  // MNN model
  std::unique_ptr<math::MnnModel> mlp_net_;
  Eigen::MatrixXd mlp_net_observation_;
  Eigen::VectorXd mlp_net_action_;

  // State variables
  double time_;
  double global_phase_;
  bool is_first_time_;
  Eigen::Vector3d command_;
  Eigen::VectorXd q_real_;
  Eigen::VectorXd qd_real_;
  Eigen::VectorXd q_des_;
  Eigen::VectorXi active_joint_idx_;
  Eigen::VectorXd initial_joint_q_;
  Eigen::VectorXd default_joint_q_;
  Eigen::VectorXd joint_kp_;
  Eigen::VectorXd joint_kd_;
  Eigen::VectorXd action_scale_;

  // ROS timer
  rclcpp::TimerBase::SharedPtr control_timer_;
  std::string config_file_dir_;
  interface_protocol::msg::JointCommand::SharedPtr joint_command_;
};

}  // namespace example

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);

  if (argc < 2) {
    RCLCPP_ERROR(rclcpp::get_logger("rl_basic_example"), "Usage: rl_basic_example <config_file_dir>");
    return 1;
  }

  auto node = std::make_shared<example::RlBasicRunner>(argv[1]);
  if (!node->Initialize()) {
    rclcpp::shutdown();
    return 1;
  }

  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

simulation

功能描述：一个基于 ROS（Robot Operating System，机器人操作系统） 与 MuJoCo（一款物理模拟引擎） 结合的项目代码架构，整体遵循典型的 ROS 功能包组织方式，同时融入了 MuJoCo 模拟相关的模块与依赖管理

架构：整体架构的核心逻辑是：通过 launch 启动文件一键启动，SimManager 协调 MuJoCo 模拟与 ROS 通信，ConfigLoader 加载配置，ROSInterface 处理机器人与外部系统的通信，simulate 模块结合 GLFW 实现可视化物理模拟，同时通过跨平台适配层支持多操作系统运行。

simulation\mujoco是根目录层级

1）asserts目录 ：通常用于存放非代码类资源
config：模拟参数、机器人模型等配置文件（如 XML 格式的 MuJoCo 模型描述、ROS 参数服务器配置）。
resource：环境（ground，chair等xml）、模型 mesh 文件（STL文件）、数据文件，urdf等资源（若项目涉及可视化或复杂模型）

2）include目录 ：存放公共头文件， 供项目内多个源文件引用，定义类、函数的接口（声明）：config_loader.h：配置加载模块的接口，负责解析 assets/config 中的配置文件。ros_interface.h：ROS 通信接口，封装话题（Topic）、服务（Service）、动作（Action）等 ROS 通信逻辑。
sim_manager.h：模拟管理模块的接口，协调 MuJoCo 物理模拟的初始化、步进（Step）、终止等流程。

3）launch目录 ：存放 ROS 启动文件 （.launch.py 是 ROS 2 的 Python 格式启动文件）
mujoco_simulator.launch.py：一键启动整个模拟系统的脚本，会按顺序启动 simulate 节点、加载配置、初始化 ROS 通信等。

4）simulate目录 ：核心的模拟功能模块，包含 MuJoCo 模拟的实现与跨平台界面适配：

  • cmake：可能存放与 CMake 构建相关的自定义脚本（如平台特定的编译选项）。
  • array_safety.h：数组安全操作的工具头文件（如边界检查、安全访问封装）。
  • CMakeLists.txt：该子模块的 CMake 构建规则，指定编译源文件、链接库（如 MuJoCo、ROS 库）。
  • glfw_*.cc/.h/.mm：与 GLFW（轻量级窗口库） 相关的适配代码，用于创建模拟可视化窗口（如 glfw_adapter 封装窗口创建，glfw_corevideo 处理 macOS 平台的视频 / 显示接口，glfw_dispatch 做跨平台函数分发）。
  • macos_gui.mm：macOS 平台特有的图形界面实现（.mm 是 Objective-C++ 后缀，用于混编 C++ 与 Objective-C）。
  • platform_ui_adapter.{cc/h}：跨平台 UI 适配层，屏蔽不同操作系统（如 Linux、macOS）的界面差异，向上提供统一的窗口 / 交互接口。
    simulate.{cc/h}：MuJoCo 模拟的核心实现，包含物理世界初始化、模拟步进循环、与 ROS 模块的数据交互等逻辑

5）SRC目录 存放源文件 ，实现 include 中声明的接口，以及程序入口：

  • config_loader.cc：config_loader.h 接口的实现，解析配置文件并将参数传递给模拟模块。
  • main.cc：程序入口点 ，初始化 ROS 节点、创建 SimManager 实例、启动模拟循环。
  • ros_interface.cc：ros_interface.h 接口的实现，处理 ROS 话题发布 / 订阅、服务调用等通信逻辑。
  • sim_manager.cc：sim_manager.h 接口的实现，管理 MuJoCo 模拟的生命周期（初始化、运行、停止）。
  • CMakeLists.txt：整个功能包的 CMake 构建规则，指定编译所有 src 与 simulate 下的源文件、链接依赖（如 MuJoCo 库、ROS 相关库）。
  • package.xml：ROS 功能包的元数据文件 ，声明依赖的 ROS 包（如 rclcpp、mujoco_ros 等）、功能包名称、版本、维护者等信息。

6）third_party 目录（推测，因截图中为展开状态）

通常用于存放第三方依赖库（如 MuJoCo 库的源码 / 预编译文件、GLFW 库等），若项目未使用系统全局安装的依赖，会将第三方库源码放在此处并通过 CMake 引入编译。

相关推荐
广州虚拟动力-动捕&虚拟主播3 小时前
动作捕捉技术如何攻克人形机器人动作稳定性难题？
机器人·具身智能·人形机器人数据采集
凉茶社4 小时前
分布式无人机、机器人多机协同系统设计
机器人·无人机
不辞远_嵌入式4 小时前
ROS （无人机、机器人）与外部系统对接
机器人·无人机
小幽余生不加糖6 小时前
电路方案分析（二十四）汽车高压互锁参考设计
笔记·学习·机器人·汽车·能源
科技圈快讯6 小时前
语音交互接待服务机器人深度推荐
机器人
大橘6 小时前
【qml-10】Quick3D实现机器人渲染（mesh）记录
qt·机器人·qml
不辞远_嵌入式6 小时前
分布式机器人多机协同巡检系统设计
分布式·机器人·无人机
JiaWen技术圈6 小时前
关于【机器人小脑】的快速入门介绍
单片机·嵌入式硬件·机器人·硬件架构
万俟淋曦14 小时前
【ROS2】通讯机制 Topic 常用命令行
人工智能·ai·机器人·ros·topic·ros2·具身智能
起个名字费劲死了20 小时前
Pytorch Yolov11 OBB 旋转框检测+window部署+推理封装 留贴记录
c++·人工智能·pytorch·python·深度学习·yolo·机器人
热门推荐
01Multisim使用教程详尽版--(2025最新版)02UV 工具安装与国内镜像源配置指南03GitHub 镜像站点04Spec-Kit 使用指南0546个Nano-banana 精选提示词，持续更新中06UV安装并设置国内源072025年华为杯研赛数学建模竞赛C题完整参考论文 （含模型、MATLAB和Python代码）08保姆级教程：手把手教你用Dify实现完美多轮对话（附Chatflow和提示词）09KGG转MP3工具|非KGM文件|解密音频10Linux下V2Ray安装配置指南