面向智元平台的四足机器人算法—从零开始写轨迹优化与运动控制算法【附Github代码仓库】（一）

机器人规划与控制研究所 ——机器人/自动驾驶规划与控制方向综合、全面、专业的平台。4万人订阅的微信大号。点击标题下蓝字“机器人规划与控制研究所”关注，我们将为您提供有价值、有深度的延伸阅读。

本文将以智元四足机器人D1为研究平台，开发基于ROS2的控制驱动，相关二次开发资料在这里获取：

https://agirobot.feishu.cn/share/base/form/shrcnDUDV3IKWPO8aIw7p3kbBYf

作者序

今天是2025年12月20日，星期六。全国硕士研究生入学初试今天开始。

思绪回到多年前的今天，想到这我也有点紧张了，考完最后一科，走出考场，外面的大雪，一大片的，一大片的，我猛地一跑，被摔个两脚朝天，好在地上的雪厚实。哈哈哈，祝各位考生一战成硕！

今天也起了个大早，因为上周日我收到了智元四足机器人D1的样机，今天也是继续调试底层驱动。截至目前，基于ROS2架构下，编写的ROS驱动可以很好的适配智元四足机器人D1 SDK控制接口，开源代码仓库：

https://github.com/JackJu-HIT/AgiBot_D1_Controller_ROS2

最近，并没有怎么给大家分享好的一些项目，公众号也没有发布什么有深度的文章，净发些推广了，实在是时间和精力有限，创作需要一些灵感和精力，一方面是我在上班时需要做公司的研发项目，这是职责所在，另一方面，我也是一边学习一边写代码，并开源代码，我理解通过这种方式，给大家分享，大家关注我的公众号能得到实质的东西，我同时也得到了提升，两全其美的事情。

其实，我一直对四足机器人有一种魅力感，人一旦对自己不了解的东西，因为神秘感的缘故就会导致不自住的去美化它。其实我需要学会去魅。尤然记得之前我分析了MIT四足机械狗的代码，后面由于时间和精力缘故，没有继续再深入研究，学习一个东西最好的方式就是跟着项目去做，成长非常快。

然后正当我觉得做不了四足机器人的时候，沉寂了几个月忙于公司的工程项目，以及Ego-Planner-2D进一步优化和开源的项目时候，上周日作者拿到了智元公司的四足机器人，于是从这周下班时间开始了研究。

我猜测关注我的各位同行和专家，应该是做算法比较多，包括但不限于感知，融合，导航，规划，控制，比较多。所以，这些算法从四足机器人领域看，属于顶层算法，我具体解释下，由于我个人在轨迹优化与运动控制方向积累了很多算法解决方案和工程经验，我就先拿这个规划控制举例，给大家分析下，站在智能机器人能自主执行任务的角度，四足机器人的控制到底对我们来说是一个什么样的存在？

首先，四足机器人的控制，包括宇树科技那个教材，它研究的是针对于四足机器人本体坐标系，它的姿态应该是什么样的？比如我现在想做站立、趴下的姿态，它的每个关节电机角度和扭矩应该发多少，这都属于四足机器人自己内部的事情，所以你不需要考虑。我举个例子你就明白了，之前做过自动驾驶横向控制，我们规划控制算法，是根据感知和定位提供的数据，去计算目标的一个方向盘角度，至于这个方向盘角度它究竟是如何让EPS执行器响应的，其实并不关注，这个像博世公司已经做好了，他会保证很好的响应你发送的角度；汽车纵向控制也一样。再比如说机器人，差速轮的，你计算好的v和w下发的轮子上每个轮子应该达到的速度，至于这个速度怎么响应的，提供伺服电机的厂家已经做好了，它会内置一个PID、MPC控制区跟随你的控制指令，所以你不需要关心电机控制那里。所以，我想说的是，站在应用层控制这块，其实你属于high_level_control,而EPS以及伺服电机控制以及四足机器人姿态控制这里都属于Low_level_control.

至此，我应该说明白了，我们在开发机器人能执行自主导航任务的时候，我不需要关注他们的步态规划怎么设计的，他们的MPC怎么计算电机的，他们都属于Low level control，你直接发送V和W控制四足机器人在全局坐标系下移动，并控制他们的动作状态，如起身，站立，趴着，下使能等。

本文也是使用的智元机器人High_level控制接口，目的实现四足机器人能实现自主执行任务。当然它也提供了Low_level接口，你可以直接控制关机电机，如果你觉得原厂的四足机器人步态规划，MPC，WBC做的不好，可以自己写一套四足机器人姿态控制。

到此，我应该说清楚了四足机器人的在我们感知导航规划控制架构中扮演的角色。就不画系统架构图了。

本文是本系列第一篇文章，我将重点分析智元机器人官方给的SDK demo如何在自己的电脑跑起来并控制机器人，然后我将分析如何在ROS2中构建ROS2的控制驱动，这将会为后续的顶层轨迹优化与运动控制算法实现奠定基础。

另外，还有最重要一点，关于ROS2控制驱动，我想了一周怎么能保证机器人在算法异常的情况不能失控，最终采用了对速度模块控制采用了“上层指令看门狗”功能，就是一旦上层控制异常，我只需要杀死上层控制进程，它不发控制指令，ROS2控制驱动就会把机器人速度置0。

让智元D1给大家打个招呼：

再给大家看下用的ROS2驱动控制的智元四足机器人D1的视频：

开源代码：https://github.com/JackJu-HIT/AgiBot_D1_Controller_ROS2/tree/master

上述视频控制的参数是最大线速度0.5m/s，最大角速度0.1rad/s。

我还录制了一个Rviz中对四足机器人实时显示底层控制通过IMU计算的定位轨迹的视频。

注意哈这个渲染的红色轨迹和上面那个不是一一对应的。

目前看底层SDK上传的全局位姿有误差，看看后续开发控制的时候，怎么处理。

这个系列我将计划如下的算法开发与工程落地：

1.使用ROS2框架的对智元四足机器人SDK进行二次开发，确保ROS2控制驱动能拿到底层控制权并获取SDK模块四足机器人状态信息。这是本文正在做的事情。

2.控制算法使用teb，实现自主跟线控制功能，能自主完成预定轨迹控制。

3.规划算法使用ego-plnner-2d，模拟加入障碍物（目前没有感知点云），四足机器人能在执行跟线控制任务时遇到障碍能自主绕开，达到自主绕障功能。

本系列将以将轨迹优化与运动控制算法在智元四足机器人实体上工程落地为目标展开叙述。

下面开始分析如何构建ROS节点。

因个人水平有限，错误在所难免，如有错误，欢迎指出。

2025-10-20 13:24:01

Jack Ju 于沈阳

1 官方SDK构建

首先我们需要知道如何访问机载控制器，首先连接机器狗的wifi，wifi名字信息和密码信息请参见机器人底部，有标签。连接好机器人wifi后，我们需要通过用自己的笔记本通过ssh 访问机载电脑：

ssh firefly@192.168.234.1密码:firefly

这里需要注意，你需要把笔记本电脑额ip设置成234网段，如我设置成了：

192.168.234.10

然后我们需要从github上克隆官方的demo

git clone https://github.com/AgibotTech/agibot_D1_Edu-Ultra

重点来了：

我们现在的目标是：使用我自己的笔记本电脑执行驱动，然后通过udp协议去控制SDK，所以这里涉及了2个电脑之间的udp通信，因此我们需要设置下。

首先我们设置机械狗的人主控参数：

ssh登录到四足机器人的主控后，通过vim命令修改：

/opt/export/config/sdk_config.yaml

target_ip: "192.168.234.10"target_port: 43988

只需要把target_ip改成我的笔记本电脑ip就可以了，我上面设置的是192.168.234.10，这里就填它。

机器狗的主控配置好以后，我们需要设置demo里的样例；这里我只举C++例子：

在 {用户目录}/agibot_sdk/demo/zsl-1/cpp/examples/ 中，按需修改

constexpr int CLIENT_PORT = 43988;   // local portstd::string CLIENT_IP = "192.168.234.10"; // local IP addressstd::string DOG_IP = "192.168.234.1";    // dog ip

CLIENT_IP是你笔记本电脑的IP，DOG_IP是固定的。

如此，便配置好了UDP通信协议。

最后一步就行编译c++：

cd demo/zsl-1/cppmkdir buildcd buildcmake ..make -j20

然后运行

cd build./highlevel_demo

注意奥，这里我只使用了high_level_control，后面ROS2驱动也是基于这个写的。

然后根据代码注释去操作机器人了，注意一定要先站立，才能控制它移动。

我建议你尝试的时候，把速度改小，不然速度快，我把速度改的很小，因为我在家里测试，空间受限，担心它把我家拆了

 switch (ch) {      case 'w': // 向前 move(vx, vy, yaw_rate) 以1m/s的速度向前移动，                // 侧向速度vy为零， 旋转速度yaw_rate为零, 使能巡逻模式支持超低速控制，默认为false        ret = highlevel.move(0.1


    
, 0.0, 0.0);        break;      case 's': // 向后 move(vx, vy, yaw_rate) 以-1m/s的速度向后移动，                // 侧向速度vy为零， 旋转速度yaw_rate为零        ret = highlevel.move(-0.1, 0.0, 0.0);        break;      case 'a': // 向左 move(vx, vy, yaw_rate) 以1m/s的速度向左移动，                // 前向速度vx为零， 旋转速度yaw_rate为零        ret = highlevel.move(0.0, 0.1, 0.0);        break;      case 'd': // 向右 move(vx, vy, yaw_rate) 以-1m/s的速度向右移动，                // 前向速度vx为零， 旋转速度yaw_rate为零        ret = highlevel.move(0.0, -0.1, 0.0);        break;      case 'q': // 左转 move(vx, vy, yaw_rate) 以1m/s的速度向左转动，                // 侧向速度vy为零， 前向速度vx为零        ret = highlevel.move(0.0, 0.0, 0.1);        break;      case 'e': // 右转 move(vx, vy, yaw_rate) 以-1m/s的速度向右转动，                // 侧向速度vy为零， 前向速度vx为零        ret = highlevel.move(0.0, 0.0, -0.1);        break;      case 'c': // 停止 move(vx, vy, yaw_rate) 前向速度vx为零， 侧向速度vy为零，                // 前向速度vx为零  停止移动        ret = highlevel.move(0.0, 0.0, 0.0);        break;

2 ROS2控制驱动构建

此部分是根据上述的官方demo将SDK适配到ROS2驱动中，开源代码仓库：

https://github.com/JackJu-HIT/AgiBot_D1_Controller_ROS2/tree/master

在这里，给大家分享一个使用ROS2的开发的SDK驱动所踩到的坑，也算是工程经验：

之前写好代码的时候，我在调试的时候，注意到，我的dog_driver_node节点驱动，运行一会儿，智元SDK就会报连接超时，一开始以为卡线程了，查来查去没问题，后面又怀疑智元SDK有问题，也没查出来啥，最后最后，由于巧合，我能稳定复现udp超时的方法，就是Rviz2一启动，就会导致udp报超时，后面研究其原因是这样的：

ROS 2 默认会扫描所有网卡并发送组播信号。如果你的机器狗 SDK 也在同一个网卡（或回环网卡）上运行，DDS 的流量会直接“淹没”SDK 的小 UDP 包。


方案：限制 ROS 2 的网卡访问（最有效）
ROS 2 默认会扫描所有网卡并发送组播信号。如果你的机器狗 SDK 也在同一个网卡（或回环网卡）上运行，DDS 的流量会直接“淹没”SDK 的小 UDP 包。
操作步骤：创建一个名为 shm_only.xml 的文件（限制 ROS 2 仅使用内存通讯，不往物理网卡发 discovery 流量）：
codeXmldownloadcontent_copyexpand_less"1.0" encoding="UTF-8" ?><profiles xmlns="http://www.eprosima.com/XMLSchemas/fastrtps_profiles">    <library_settings>        <introspection>falseintrospection>    library_settings


    
>    <participant profile_name="shm_only" is_default_profile="true">        <rtps>            <useBuiltinTransports>falseuseBuiltinTransports>            <userTransports>                <transport_id>shm_transporttransport_id>            userTransports>            <builtin>                <metatrafficUnicastLocatorList>                    <locator/>                metatrafficUnicastLocatorList>                <initialPeersList>                    <locator/>                initialPeersList>            builtin>        rtps>    participant>profiles>
运行方式：在启动你的节点和 RViz 之前，执行：
codeBashdownloadcontent_copyexpand_lessexport FASTRTPS_DEFAULT_PROFILES_FILE=path/to/shm_only.xmlros2 run dog_control dog_driver_node# 另一个终端export FASTRTPS_DEFAULT_PROFILES_FILE=path/to/shm_only.xmlrviz2

然后，你按照我这个方案，就能把此问题解决掉。

下面给大家分析下代码：

代码结构分为下图两部分:

（1）dog_driver_node.cpp:ROS2控制启动节点，负责与四足机器人底层SDK交互，包括下发控制指令以及读取机器人状态信息。

（2）dog_keyboard_node.cpp：遥控发送指令节点，模拟上层控制模块下发控制指令。

1.解析dog_driver_node.cpp

DogDriverNode::DogDriverNode() : Node("dog_driver_node") {    initParam();    initSDK();      initROS2Node();   }

初始化参数，初始化SDK，初始化ROS2订阅和发布节点。具体实现可以读代码，这里不赘述。

void DogDriverNode::initROS2Node(){    // 订阅速度指令 (cmd_vel)    sub_vel_ = this->create_subscription<:msg:: class="code-snippet__title">Twist>(        "cmd_vel", 10, std::bind(&DogDriverNode::cmdVelCallback, this, _1));    // 订阅特殊动作指令 (cmd_action)    sub_action_ = this->create_subscription<:msg:: class="code-snippet__title">String>(        "cmd_action", 10, std::bind(&DogDriverNode::actionCallback, this, _1));
    // 发布话题 "odom"    pub_odom_ = this->create_publisher<:msg:: class="code-snippet__title">Odometry>("odom", 10);    pub_mode_ = this->create_publisher<:msg:: class="code-snippet__title">UInt32>("robot_mode", 10);
    //发布当前已经走轨迹    pub_path_ = this->create_publisher<:msg:: class="code-snippet__title">Path>("run_path", 10);    control_thread_ = std::thread(&DogDriverNode::udpControlLoop, this);    timer_writer_robot_state_ = this->create_wall_timer(        std::chrono::milliseconds(20), std::bind(&DogDriverNode::getRobotStateCallback, this));}

这里创建了订阅速度指令sub_vel_，订阅特殊动作sub_action_指令，这两个都是用于接收顶层控制指令。

发布者odom主要包含了当前的机器人状态信息，用于上层控制使用。

当前走过的轨迹（用于Rviz2显示）以及当前工作模式的发布者（用于上层决策）。

// 处理特殊动作回调void DogDriverNode::actionCallback(const std_msgs::msg::String::SharedPtr msg) {    std::string cmd = msg->data;    RCLCPP_INFO(this->get_logger(), "Received Action: %s", cmd.c_str());    if (cmd == "passive")     {        highlevel_->passive();        // current_mode_ = RobotMode::PASSIVE;     }    else if (cmd == "lie_down")     {        highlevel_->lieDown();        // current_mode_ = RobotMode::LIE_DOWN;;     }    else if (cmd == "stand_up")    {        highlevel_->standUp();        // current_mode_ = RobotMode::STAND;    }    else if (cmd == "jump")     {        highlevel_->jump();        // current_mode_ = RobotMode::JUMP;    }    else if (cmd == "front_jump")     {        highlevel_->frontJump();        // current_mode_ = RobotMode::FRONT_JUMP;    }    else if (cmd == "backflip")     {        highlevel_->backflip();        // current_mode_ = RobotMode::BACK_FLIP;    }    else if (cmd == "shake_hand")     {        highlevel_->shakeHand();        // current_mode_ = RobotMode::SHAKE_HAND;    }    else if (cmd == "two_leg_stand")     {        highlevel_->twoLegStand(0.0, 0.0); // 默认参数        // current_mode_ = RobotMode::TWO_LEG_STAND;    }    else if (cmd == "cancel_two_leg")     {


    
        highlevel_->cancelTwoLegStand();        // current_mode_ = RobotMode::CANCEL_TWO_LEG;    }    else if (cmd == "attitude_demo")     {        // 演示一下姿态控制        highlevel_->attitudeControl(0.1, 0.0, 0.0, 0.0);         // current_mode_ = RobotMode::ATTITUDE_DEMO;    }    else     {        // current_mode_ = RobotMode::PASSIVE;         RCLCPP_WARN(this->get_logger(), "Unknown action command: %s", cmd.c_str());    }}

上述代码用于接收顶层控制下发的特殊动作回调。

关于控制机器人前进后退转弯，这是个重点，因为在这里涉及到安全控制功能—“上层指令看门狗”。

先看代码：

void DogDriverNode::udpControlLoop(){    while(true)    {        auto start_time = std::chrono::steady_clock::now();        uint32_t raw_mode = highlevel_->getCurrentCtrlmode();        {            std::lock_guard<:mutex> lock(data_mode_mutex_);            current_robot_mode_ = raw_mode;         }        //只有特定模式下，才启用速度看门狗        if (raw_mode == 18 || raw_mode == 1 || raw_mode == 21)         {            float vx, vy, w;            {                std::lock_guard<:mutex> lock(data_mutex_);                // 看门狗检查                auto now = std::chrono::steady_clock::now();                auto elapsed = std::chrono::duration_cast<:chrono::milliseconds>(now - last_cmd_time_);                // 检查超时                if (elapsed > CMD_TIMEOUT_MS_)                 {                    vx = 0.0f;                    vy = 0.0f;                     w = 0.0f;                }                 else                 {                    vx = current_cmd_.vx; vy = current_cmd_.vy; w = current_cmd_.w;                }            }            RCLCPP_INFO(this->get_logger(), "udpControlLoop: vx=%.2f, vy=%.2f, yaw=%.2f", vx, vy, w);            // 发送速度指令            highlevel_->move(vx, vy, w);        }        RCLCPP_INFO(this->get_logger(), "udpControlLoop: mode=%d", raw_mode);        std::this_thread::sleep_until(start_time + std::chrono::milliseconds(20));    }}

上层指令看门狗 (Command Watchdog & Failsafe)

针对上层控制节点（如导航规划器）可能出现的崩溃、网络断连或冻结情况，设计了基于时间戳的熔断机制。

实现逻辑：

(1)系统记录最近一次收到/cmd_vel 指令的时间戳 (last_cmd_time_)。

(2)在每一帧 UDP 发送周期前，计算当前时间与 last_cmd_time_ 的差值 (elapsed)。

(3)若 elapsed 超过设定的安全阈值（我设置 100ms），系统判定上层控制失效。

(4)安全动作：看门狗将强制接管控制权，将下发给机器人的速度向量 (vx,vy,ω) 覆写为零向量，迫使机器人平稳减速并保持站立，防止“飞车”事故。

这个功能其实很好，这个四足机器人18kg，一旦失控，也挺严重的后果。这个是后续调试的基础。

最后就是获取机器人信息了。

// 周期性获取机器人信息void DogDriverNode::getRobotStateCallback(){    // while(true)    {        uint32_t ret = highlevel_->getBatteryPower();        // static float value = 0.1;        // value += 0.1;


    
        // 2. 从 SDK 获取数据        // 假设 getPosition 返回 {x, y, z}        auto pos = highlevel_->getPosition();         // 假设 getRPY 返回 {roll, pitch, yaw}        auto rpy = highlevel_->getRPY();        // 假设 getBodyVelocity 返回 {vx, vy, vz}        auto vel = highlevel_->getBodyVelocity();        // 假设 getBodyGyro 返回 {wx, wy, wz}        auto gyro = highlevel_->getBodyGyro();        // --- 更新本地调试用的结构体 ---        robot_state_.x = pos[0];// + value;        // std::cout << "模拟" << std::endl;        robot_state_.y = pos[1];        robot_state_.theta = rpy[2];// + 0.01 * value; // Yaw        robot_state_.v = vel[0];// + 0.1 * value;     // Linear X        robot_state_.w = gyro[2];// + 0.05 * value;    // Angular Z        // 打印调试信息 (如果你不想刷屏太快，可以加个计数器限制打印频率)        std::cout  << robot_state_.x                  << robot_state_.theta << std::endl;        std::cout  << robot_state_.v         auto odom_msg = nav_msgs::msg::Odometry();        odom_msg.header.stamp = this->now();        odom_msg.header.frame_id = "odom";        odom_msg.child_frame_id = "base_link";        // 1. 填充位置 (XYZ)        odom_msg.pose.pose.position.x = robot_state_.x;        odom_msg.pose.pose.position.y = robot_state_.y;        odom_msg.pose.pose.position.z = 0.0; // 如果是平面移动，Z设为0        // 2. 【核心】将 Theta (Yaw) 转换为四元数        tf2::Quaternion q;        // setRPY 参数顺序: (Roll, Pitch, Yaw)        // 这里的 theta 就是 Yaw。因为是平面导航，Roll 和 Pitch 通常设为 0        q.setRPY(0.0, 0.0, robot_state_.theta);         // 3. 将 tf2::Quaternion 转换为 msg 格式并赋值        odom_msg.pose.pose.orientation = tf2::toMsg(q);        // 4. 填充速度        odom_msg.twist.twist.linear.x = robot_state_.v;        odom_msg.twist.twist.angular.z = robot_state_.w;        // 发布        pub_odom_->publish(odom_msg);        // std::cout << " pub_odom_->publish(odom_msg) OK" << std::endl;        // ===========================        // === 处理轨迹 Path ===        // ===========================        #if 1        // 1. 获取当前位置        geometry_msgs::msg::PoseStamped pose_stamped;        pose_stamped.header = odom_msg.header; // 使用相同的时间戳和坐标系        pose_stamped.pose = odom_msg.pose.pose; // 直接拷贝里程计的位姿        // 2. (可选) 简单的过滤器：只有当移动距离超过一定阈值才记录        // 防止原地不动时数组无限膨胀        bool should_record = true;        if (!path_msg_.poses.empty())         {            auto last_pose = path_msg_.poses.back().pose.position;            double dist = std::sqrt(std::pow(last_pose.x - robot_state_.x, 2) +                                         std::pow(last_pose.y - robot_state_.y, 2));            if (dist 0.05) { // 如果移动小于 5cm，就不记录                    should_record = false;            }            std::cout          }        std::cout <<"should_record =" << should_record  << std::endl;        // 3. 添加进路径并发布        if (should_record) {            path_msg_.header.stamp = this->now(); // 更新路径的时间戳            path_msg_.header.frame_id = "odom"; //必须显式设置容器的坐标系            path_msg_.poses.push_back(pose_stamped);            pub_path_->publish(path_msg_);        }        #endif        auto mode_msg = std_msgs::msg::UInt32();        {            std::lock_guard<:mutex> lock(data_mode_mutex_);            mode_msg.data = current_robot_mode_;        }        pub_mode_->publish(mode_msg);    }}

简单讲就是获取机器人的全局坐标系下的(x,y,theta)以及车体坐标系的v和w信息。并记录全局定位轨迹信息。

它的全局坐标系原点是相对于上电原点的。

2.解析dog_driver_keyboard.cpp


 void publishVel(float x, float y, float z) {        auto msg = geometry_msgs::msg::Twist();        msg.linear.x = x;        msg.linear.y = y;        msg.angular.z = z;        pub_vel_->publish(msg);    }    void publishAction(const std::string& action) {        auto msg = std_msgs::msg::String();        msg.data = action;        pub_action_->publish(msg);        RCLCPP_INFO(this->get_logger(), "Sent action: %s", action.c_str());        // 重新打印菜单以防被日志刷屏        // printUsage();     }    void keyLoop() {        int ch = kb_reader_.readKey();        if (ch == -1) return;        switch (ch) {            // Movement            case 'w': publishVel(0.50, 0.0, 0.0); break;            case 's': publishVel(-0.50, 0.0, 0.0); break;            case 'a': publishVel(0.0, 0.10, 0.0); break;            case 'd': publishVel(0.0, -0.10, 0.0); break;            case 'q': publishVel(0.0, 0.0, 0.30); break;            case 'e': publishVel(0.0, 0.0, -0.30); break;            case 'c': publishVel(0.0, 0.0, 0.0); break;            // Actions            case '0': publishAction("passive"); break;            case '1': publishAction("lie_down"); break;            case '2': publishAction("stand_up"); break;            case '3': publishAction("jump"); break;            case '4': publishAction("front_jump"); break;            case '6': publishAction("backflip"); break;            case '7': publishAction("attitude_demo"); break;            case '8': publishAction("shake_hand"); break;            case '9': publishAction("two_leg_stand"); break;            case 'z': publishAction("cancel_two_leg"); break;            default: break;        }    }};

核心发送指令的，和官方demo一致。

3 如何执行ROS2控制驱动

首先解决“ROS 2 默认会扫描所有网卡并发送组播信号。如果你的机器狗 SDK 也在同一个网卡（或回环网卡）上运行，DDS 的流量会直接“淹没”SDK 的小 UDP 包。“的问题。

你需要在你打开的每个终端都运行下

export FASTRTPS_DEFAULT_PROFILES_FILE=/home/juchunyu/dog_ws/shm_only.xml

/home/juchunyu/dog_ws/shm_only.xml这个路径是绝对路径换成你自己的，仓库里有这个shm_only.xml文件。

然后在运行：

ros2 run dog_control dog_driver_node

以及

ros2 run dog_control dog_keyboard_node

还有

rviz2

注意：

rviz2需要监听话题odom，才能看到像一开始视频实际红色移动轨迹。

然后就可以用键盘模拟ROS2架构下的上层指令控制它移动了。

文章最后：

感谢智元机器人提供的可以二次开发版本的四足机器人D1 Edu以及技术支持。

如果大家想获取智元四足机器人D1二次开资料，请访问如下链接。

https://agirobot.feishu.cn/share/base/form/shrcnDUDV3IKWPO8aIw7p3kbBYf

我也建了一个轨迹优化与运动控制方向技术交流群，欢迎各位同行专家加入交流讨论！扫描下方二维码，添加作者微信，然后拉进群。

下一次解决IMU定位精度后，会按计划开源TEB控制四足机器人的代码以及技术文章分享，敬请期待！