ros::init(argc, argv, damiao_hardware_node); ros::NodeHandle nh; ros::NodeHandle nh_private(~);ros::init你的程序来到 ROS 世界第一件事就是去注册中心登记名字“大家好我是damiao_hardware_node”。nh与nh_private(~)你向系统申请了两部通讯工具。nh公共大喇叭用来向全网广播你的关节状态/joint_states。nh_private私密对讲机带波浪号~专门用来悄悄读取 launch 文件里加载的属于你自己的motor_config.yaml配置文件绝不会和其他节点弄混。damiao::DmHW robot; if (!robot.init(nh, nh_private)) { ROS_ERROR(Failed to initialize Damiao Hardware Interface!); return -1; }init串口初始化使能电机开启读取电机数据线程注册关节位置接口ros上层得知要控制电机只需要王cmd_pos的地址写数据就行controller_manager::ControllerManager cm(robot, nh);为什么必须要传robot解决内存共享问题robot是什么它是你自己写的DmHW类的实例。在这个实例的内存里存着你之前注册的各种接口jointStateInterface_、positionJointInterface_以及电机实际位置 (pos) 和目标位置 (cmd_pos) 的物理内存地址。传递robot的底层逻辑ControllerManager(cm) 本身只是一个纯数学算法库它生来就是没有数据的。当你使用取地址符把robot传给它时cm就直接获得了访问robot内部变量的权限。使用结果从此以后cm内部的控制算法在计算时不需要调用任何函数去向robot索要数据而是直接去读写robot内存里的pos和cmd_pos变量。这在 C 中叫做“零拷贝数据交互”效率极高。2. 为什么必须要传nh解决 ROS 通信问题nh是什么ros::NodeHandle是 ROS 系统提供的通信接口。有了它C 程序才能在 ROS 网络中发布话题Topic或创建服务Service。传递nh的底层逻辑cm需要接收外部的指令比如你想启动哪个控制器、停止哪个控制器。通过接收你传进来的nhcm会在后台自动为你生成并注册一系列标准的 ROS 服务Services。使用结果只要这行代码一执行你在终端里输入rosservice list就会立刻看到多出了一大堆服务比如/controller_manager/load_controller/controller_manager/switch_controller/controller_manager/list_controllers此时外部程序就可以通过调用这些服务来操控你的 C 代码。这行代码仅仅是完成了“初始化”。在真正的机器人运行中它是按以下 3 个步骤被使用的第一步代码层面的初始化就是这行代码本身当 C 的main函数执行到这一行时cm对象被创建。它绑定了底层硬件内存robot并在 ROS 网络中注册了控制服务利用nh。此时算法处于待机状态没有任何运算发生。第二步外部唤醒Launch 文件中的spawner节点在你的.launch文件中有一行专门针对它的指令node namecontroller_spawner pkgcontroller_manager typespawner argsjoint_state_controller arm_controller/这个叫spawner的脚本会去调用第一步里cm创建的 ROS 服务。它对cm下达指令“请立刻加载joint_state_controller和arm_controller这两个控制算法并让它们开始运行。”此时cm会根据指令在内存中把这两个算法实例化。第三步循环调用运算C 代码中的cm.update在 C 的while(ros::ok())死循环中你写了cm.update(current_time, elapsed_time);。每次执行到这句话cm就会触发刚才加载的那两个控制算法。算法会利用第一步拿到的robot权限读取电机的实际角度。算法如arm_controller接收 MoveIt 传来的目标轨迹利用 PID 公式计算出当前的控制量并直接把计算结果覆盖写回到robot的cmd_pos内存地址中。随后你的robot.write()函数就会把这个新的cmd_pos变成 CAN 数据发给达妙电机。ros::AsyncSpinner spinner(1); spinner.start();ros::AsyncSpinner这是 ROS 提供的一个异步多线程处理器Spinner 意为“轮询器”。(1)括号里的1代表你想分配几个独立的子线程。写1就意味着你在主线程跑while循环的那个之外额外开辟了 1 个平行的后台线程专门用来处理 ROS 的消息回调。在后面有while循环需要有独立的子线程去处理movelt发过来的数据spinner.start();函数作用启动这个后台线程。ros::Rate rate(200); ros::Time last_time ros::Time::now();代表频率是 200Hz。意思是告诉程序“接下来你要干的活必须每秒钟执行 200 次记录下程序运行到这一行的当前绝对时间。ROS_INFO(Damiao Hardware Node Started!);打印出一句话“达妙硬件节点已启动”while (ros::ok()) {死循环ros::Time current_time ros::Time::now(); ros::Duration elapsed_time current_time - last_time; last_time current_time;看一眼现在几点了存到current_time当前时间里。用现在的时刻减去刚才记录的时刻算出了一个时间段elapsed_time流逝的时间差。控制管家cm里的 PID 算法比如算速度、算加速度极度依赖物理时间。如果时间算不准算出来的控制指令就是错的机械臂就会疯狂发抖。robot.read(current_time, elapsed_time); // 读电机调用你写的底层硬件代码通过串口去问达妙电机“你现在实际上转到多少度了” 然后把度数存进共享内存里。cm.update(current_time, elapsed_time); // 算控制控制管家ControllerManager出场它把刚才读上来的真实角度和 MoveIt 下发的目标角度做对比。结合时间差elapsed_time它经过一顿复杂的数学运算算出“为了消除误差下一刻电机应该转到某某位置。” 并把这个新位置写进内存。robot.write(current_time, elapsed_time); // 写电机再次调用底层代码把刚才管家算出来的新目标位置打包成 CAN 通信的十六进制指令顺着数据线狠狠地发给物理电机。电机收到后就会嗡嗡地转动一丝丝距离。