宇树四足机器狗开源代码详解[unitree_ros]中的unitree_controller功能包：代码框架整理2

不摆烂的fy

于 2024-08-30 00:07:00 发布

阅读量932

点赞数 12

文章标签： ubuntu linux 开源机器人 c++

本文链接：https://blog.csdn.net/BIT_TF/article/details/141681635

版权

#这一篇文章会讲讲[unitree_ros](https://github.com/unitreerobotics/unitree_ros)的代码框架，仅代表个人看法，写出来加深自己的印象，欢迎交流。

#附上宇树家的学习资料[《四足机器人控制算法--建模、控制与实践》](https://detail.tmall.com/item.htm？spm=a212k0.12153887.0.0.5487687dBgiovR&id=704510718152)#

下载了GitHub上的压缩包之后，有五个文件（第五个unitree_ros_to_real是空的)，

robots装的是宇树家不同机器狗的描述文件

unittree_controller是一个控制器功能包

unitree_legged_control是底层电机控制功能包

在上一篇文章《宇树机器狗代码详解：代码框架整理1》里面已经写了unittree_controller文件夹里子文件夹include中的body.h头文件，子文件夹src里面的body.cpp move_publisher.cpp文件，接下来将剩下的servo.cpp external_force.cpp文件

1、servo.cpp（伺服，表示对于电机的底层驱动）

/************************************************************************
Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved.
Use of this source code is governed by the MPL-2.0 license, see LICENSE.
************************************************************************/
//该文件定义了一个multiThread类，用于处理多线程ROS订阅和发布。类中包含了处理IMU（惯性测量单元）、脚部力传感器、电机状态的回调函数。
//订阅了多个与机器人电机状态相关的话题，并在回调中更新了lowState结构体，该结构体包含了机器人的电机状态信息。
//在主函数中，初始化了ROS节点，订阅了多个传感器和电机状态话题，并发布了低级状态和电机指令。

#include "ros/ros.h"
#include <stdio.h>//告诉编译器包含输入输出库，类如printf和scanf
#include <stdlib.h>//告诉编译器包含标准库（Standard Library Header），函数如内存分配（malloc、free）、随机数生成（rand）、搜索和排序（qsort）等
#include "unitree_legged_msgs/LowCmd.h"//在这个功能包readme里，能找到unitree_legged_msgs包的下载GitHub地址，这是包中的头文件，定义了消息类型，msgs是ros特有消息文件后缀
#include "unitree_legged_msgs/LowState.h"//LowState是低级状态，包含MotorCmd[12]，以及一系列操作12个电机的函数，例如setQ
#include "unitree_legged_msgs/MotorCmd.h"//电机命令有模式mode、角度q、角速度dq、力矩Tau、位置刚度Kp、阻尼Kd
#include "unitree_legged_msgs/MotorState.h"//MOtorState是一个结构体，定义了角度q，角速度dq，角加速度ddq，力矩TauEst
#include <geometry_msgs/WrenchStamped.h>//WrenchStamped消息用于表示一个受力和力矩的组合，它是在某个时间戳下测量的，包含线性力和角力矩
#include <sensor_msgs/Imu.h>//惯性测量单元IMU的数据，通常有四元数、角速度、加速度、欧拉角这些量
#include <std_msgs/Bool.h>
#include <vector>//C++标准模板库（STL）中的头文件，分别用于包含std::vector容器和std::string类的定义
#include <string>
#include <math.h>
#include <nav_msgs/Odometry.h>//Odometry消息用于表示机器人的里程计信息，包括位置、速度、姿态等
#include "body.h"

using namespace std;
using namespace unitree_model;

bool start_up = true; //全局变量，用于标识机器人是否处于启动状态。

class multiThread //定义multiThread类
{
public: //初始化了多个ROS订阅者，用于订阅IMU数据、脚部力量数据和各个电机的状态数据。
    multiThread(string rname){ //使用的时候就multiThread（机器人名称）
        robot_name = rname;
        imu_sub = nm.subscribe("/trunk_imu", 1, &multiThread::imuCallback, this);//使用nm（ROS节点句柄）订阅名为"/trunk_imu"的IMU主题，设置消息队列大小为1，并指定回调函数为multiThread::imuCallback。this关键字表示回调函数应绑定到当前对象实例。
        footForce_sub[0] = nm.subscribe("/visual/FR_foot_contact/the_force", 1, &multiThread::FRfootCallback, this);//订阅“名称太长force”的主题，把消息存到foorForce_sub[4]里
        footForce_sub[1] = nm.subscribe("/visual/FL_foot_contact/the_force", 1, &multiThread::FLfootCallback, this);
        footForce_sub[2] = nm.subscribe("/visual/RR_foot_contact/the_force", 1, &multiThread::RRfootCallback, this);
        footForce_sub[3] = nm.subscribe("/visual/RL_foot_contact/the_force", 1, &multiThread::RLfootCallback, this);
        servo_sub[0] = nm.subscribe("/" + robot_name + "_gazebo/FR_hip_controller/state", 1, &multiThread::FRhipCallback, this);//订阅12个电机状态
        servo_sub[1] = nm.subscribe("/" + robot_name + "_gazebo/FR_thigh_controller/state", 1, &multiThread::FRthighCallback, this);
        servo_sub[2] = nm.subscribe("/" + robot_name + "_gazebo/FR_calf_controller/state", 1, &multiThread::FRcalfCallback, this);
        servo_sub[3] = nm.subscribe("/" + robot_name + "_gazebo/FL_hip_controller/state", 1, &multiThread::FLhipCallback, this);
        servo_sub[4] = nm.subscribe("/" + robot_name + "_gazebo/FL_thigh_controller/state", 1, &multiThread::FLthighCallback, this);
        servo_sub[5] = nm.subscribe("/" + robot_name + "_gazebo/FL_calf_controller/state", 1, &multiThread::FLcalfCallback, this);
        servo_sub[6] = nm.subscribe("/" + robot_name + "_gazebo/RR_hip_controller/state", 1, &multiThread::RRhipCallback, this);
        servo_sub[7] = nm.subscribe("/" + robot_name + "_gazebo/RR_thigh_controller/state", 1, &multiThread::RRthighCallback, this);
        servo_sub[8] = nm.subscribe("/" + robot_name + "_gazebo/RR_calf_controller/state", 1, &multiThread::RRcalfCallback, this);
        servo_sub[9] = nm.subscribe("/" + robot_name + "_gazebo/RL_hip_controller/state", 1, &multiThread::RLhipCallback, this);
        servo_sub[10] = nm.subscribe("/" + robot_name + "_gazebo/RL_thigh_controller/state", 1, &multiThread::RLthighCallback, this);
        servo_sub[11] = nm.subscribe("/" + robot_name + "_gazebo/RL_calf_controller/state", 1, &multiThread::RLcalfCallback, this);
    }
    //类里面定义了多个回调函数，分别对应不同的订阅者，这些回调函数更新lowState结构体，该结构体保存了机器人的当前状态。
    void imuCallback(const sensor_msgs::Imu & msg) //处理IMU数据
    {//这些数据是从ROS的sensor_msgs/Imu消息类型中提取的，该消息类型是ROS中用于传递IMU数据的标准格式。msg变量是传递给回调函数的参数，包含了最新接收到的IMU数据。 
        lowState.imu.quaternion[0] = msg.orientation.w;//一方面在上面把值赋给了imu_sub，另一方面用回调函数赋给了body.cpp里的lowState变量
        lowState.imu.quaternion[1] = msg.orientation.x;
        lowState.imu.quaternion[2] = msg.orientation.y;
        lowState.imu.quaternion[3] = msg.orientation.z;

        lowState.imu.gyroscope[0] = msg.angular_velocity.x;
        lowState.imu.gyroscope[1] = msg.angular_velocity.y;
        lowState.imu.gyroscope[2] = msg.angular_velocity.z;
        
        lowState.imu.accelerometer[0] = msg.linear_acceleration.x;
        lowState.imu.accelerometer[1] = msg.linear_acceleration.y;
        lowState.imu.accelerometer[2] = msg.linear_acceleration.z;
        
    }
//FRhipCallback、FRthighCallback、FRcalfCallback等：处理前右腿、前左腿、后右腿、后左腿的电机状态数据。
    void FRhipCallback(const unitree_legged_msgs::MotorState& msg)
    {
        start_up = false;//这个变量可能用于标识机器人是否完成了启动过程或是否已经接收到了初始状态信息
        lowState.motorState[0].mode = msg.mode;
        lowState.motorState[0].q = msg.q;
        lowState.motorState[0].dq = msg.dq;
        lowState.motorState[0].tauEst = msg.tauEst;//msg中的电机扭矩估计值（msg.tauEst），扭矩估计是电机施加在轴上的扭矩的估计值
    }

    void FRthighCallback(const unitree_legged_msgs::MotorState& msg)
    {
        lowState.motorState[1].mode = msg.mode;
        lowState.motorState[1].q = msg.q;
        lowState.motorState[1].dq = msg.dq;
        lowState.motorState[1].tauEst = msg.tauEst;
    }

    void FRcalfCallback(const unitree_legged_msgs::MotorState& msg)
    {
        lowState.motorState[2].mode = msg.mode;
        lowState.motorState[2].q = msg.q;
        lowState.motorState[2].dq = msg.dq;
        lowState.motorState[2].tauEst = msg.tauEst;
    }

    void FLhipCallback(const unitree_legged_msgs::MotorState& msg)
    {
        start_up = false;
        lowState.motorState[3].mode = msg.mode;
        lowState.motorState[3].q = msg.q;
        lowState.motorState[3].dq = msg.dq;
        lowState.motorState[3].tauEst = msg.tauEst;
    }

    void FLthighCallback(const unitree_legged_msgs::MotorState& msg)
    {
        lowState.motorState[4].mode = msg.mode;
        lowState.motorState[4].q = msg.q;
        lowState.motorState[4].dq = msg.dq;
        lowState.motorState[4].tauEst = msg.tauEst;
    }

    void FLcalfCallback(const unitree_legged_msgs::MotorState& msg)
    {
        lowState.motorState[5].mode = msg.mode;
        lowState.motorState[5].q = msg.q;
        lowState.motorState[5].dq = msg.dq;
        lowState.motorState[5].tauEst = msg.tauEst;
    }

    void RRhipCallback(const unitree_legged_msgs::MotorState& msg)
    {
        start_up = false;
        lowState.motorState[6].mode = msg.mode;
        lowState.motorState[6].q = msg.q;
        lowState.motorState[6].dq = msg.dq;
        lowState.motorState[6].tauEst = msg.tauEst;
    }

    void RRthighCallback(const unitree_legged_msgs::MotorState& msg)
    {
        lowState.motorState[7].mode = msg.mode;
        lowState.motorState[7].q = msg.q;
        lowState.motorState[7].dq = msg.dq;
        lowState.motorState[7].tauEst = msg.tauEst;
    }

    void RRcalfCallback(const unitree_legged_msgs::MotorState& msg)
    {
        lowState.motorState[8].mode = msg.mode;
        lowState.motorState[8].q = msg.q;
        lowState.motorState[8].dq = msg.dq;
        lowState.motorState[8].tauEst = msg.tauEst;
    }

    void RLhipCallback(const unitree_legged_msgs::MotorState& msg)
    {
        start_up = false;
        lowState.motorState[9].mode = msg.mode;
        lowState.motorState[9].q = msg.q;
        lowState.motorState[9].dq = msg.dq;
        lowState.motorState[9].tauEst = msg.tauEst;
    }

    void RLthighCallback(const unitree_legged_msgs::MotorState& msg)
    {
        lowState.motorState[10].mode = msg.mode;
        lowState.motorState[10].q = msg.q;
        lowState.motorState[10].dq = msg.dq;
        lowState.motorState[10].tauEst = msg.tauEst;
    }

    void RLcalfCallback(const unitree_legged_msgs::MotorState& msg)
    {
        lowState.motorState[11].mode = msg.mode;
        lowState.motorState[11].q = msg.q;
        lowState.motorState[11].dq = msg.dq;
        lowState.motorState[11].tauEst = msg.tauEst;
    }

    void FRfootCallback(const geometry_msgs::WrenchStamped& msg)
    { //这行代码将传感器消息msg中的wrench（扳手，这里指力矩传感器数据）的force（力）
    //分量的x轴分量赋值给lowState中的eeForce（末端执行器力）数组的第一个元素的x坐标。这代表了机器人末端执行器在x轴方向上受到的力。
        lowState.eeForce[0].x = msg.wrench.force.x;
        lowState.eeForce[0].y = msg.wrench.force.y;
        lowState.eeForce[0].z = msg.wrench.force.z;
        lowState.footForce[0] = msg.wrench.force.z;
    //wrench.force的z轴分量赋值给lowState中的footForce数组的第一个元素。这里只记录了垂直于地面的力分量，这通常用于表示机器人脚部与地面接触的强度或压力。
    }

    void FLfootCallback(const geometry_msgs::WrenchStamped& msg)
    {
        lowState.eeForce[1].x = msg.wrench.force.x;
        lowState.eeForce[1].y = msg.wrench.force.y;
        lowState.eeForce[1].z = msg.wrench.force.z;
        lowState.footForce[1] = msg.wrench.force.z;
    }

    void RRfootCallback(const geometry_msgs::WrenchStamped& msg)
    {
        lowState.eeForce[2].x = msg.wrench.force.x;
        lowState.eeForce[2].y = msg.wrench.force.y;
        lowState.eeForce[2].z = msg.wrench.force.z;
        lowState.footForce[2] = msg.wrench.force.z;
    }

    void RLfootCallback(const geometry_msgs::WrenchStamped& msg)
    {
        lowState.eeForce[3].x = msg.wrench.force.x;
        lowState.eeForce[3].y = msg.wrench.force.y;
        lowState.eeForce[3].z = msg.wrench.force.z;
        lowState.footForce[3] = msg.wrench.force.z;
    }

private:
    ros::NodeHandle nm;
    ros::Subscriber servo_sub[12], footForce_sub[4], imu_sub;//这个servo_sub[12]是上面的12，是接收topic消息的，下面main函数的是另外的servo_sub[12]
    string robot_name;
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "unitree_gazebo_servo");//创建了一个名为“unitree_gazebo_servo"的节点node

    string robot_name;
    ros::param::get("/robot_name", robot_name);//整个cpp文件在这里从参数服务器调用API找机器人名称
    cout << "robot_name: " << robot_name << endl;//输出在控制台上

    multiThread listen_publish_obj(robot_name);//实例化了一个类，名称是传入的参数
    ros::AsyncSpinner spinner(1); // one threads  //创建并启动一个异步spinner，允许并发处理回调，这里使用单个线程。
    spinner.start();
//spinner.start(); 这行代码调用了ros::AsyncSpinner对象的start方法，启动了异步回调处理
//当start方法被调用后，ros::AsyncSpinner会创建多个线程（根据构造时指定的数量），每个线程可以独立地处理回调函数。
//ros::spin是ROS中传统的回调处理方式，但它是同步的，并且在单个线程中处理所有回调。相比之下，ros::AsyncSpinner提供了异步处理的能力
    usleep(300000); // must wait 300ms, to get first state

    ros::NodeHandle n;
    ros::Publisher lowState_pub; //for rviz visualization
    // ros::Rate loop_rate(1000);
    // the following nodes have been initialized by "gazebo.launch"
    lowState_pub = n.advertise<unitree_legged_msgs::LowState>("/" + robot_name + "_gazebo/lowState/state", 1);//为低状态创建一个Topic，名称为“/机器人名称_gzebo/lowState/state”，消息长度为1
    servo_pub[0] = n.advertise<unitree_legged_msgs::MotorCmd>("/" + robot_name + "_gazebo/FR_hip_controller/command", 1);//为12个电机控制器各创建一个发布者，和topic
    servo_pub[1] = n.advertise<unitree_legged_msgs::MotorCmd>("/" + robot_name + "_gazebo/FR_thigh_controller/command", 1);//在body的sendServoCmd函数中发布了那边的LowCmd电机实时状态
    servo_pub[2] = n.advertise<unitree_legged_msgs::MotorCmd>("/" + robot_name + "_gazebo/FR_calf_controller/command", 1);//这里的servo_sub[12]定义在body第一行，在这里赋值
    servo_pub[3] = n.advertise<unitree_legged_msgs::MotorCmd>("/" + robot_name + "_gazebo/FL_hip_controller/command", 1);
    servo_pub[4] = n.advertise<unitree_legged_msgs::MotorCmd>("/" + robot_name + "_gazebo/FL_thigh_controller/command", 1);
    servo_pub[5] = n.advertise<unitree_legged_msgs::MotorCmd>("/" + robot_name + "_gazebo/FL_calf_controller/command", 1);
    servo_pub[6] = n.advertise<unitree_legged_msgs::MotorCmd>("/" + robot_name + "_gazebo/RR_hip_controller/command", 1);
    servo_pub[7] = n.advertise<unitree_legged_msgs::MotorCmd>("/" + robot_name + "_gazebo/RR_thigh_controller/command", 1);
    servo_pub[8] = n.advertise<unitree_legged_msgs::MotorCmd>("/" + robot_name + "_gazebo/RR_calf_controller/command", 1);
    servo_pub[9] = n.advertise<unitree_legged_msgs::MotorCmd>("/" + robot_name + "_gazebo/RL_hip_controller/command", 1);
    servo_pub[10] = n.advertise<unitree_legged_msgs::MotorCmd>("/" + robot_name + "_gazebo/RL_thigh_controller/command", 1);
    servo_pub[11] = n.advertise<unitree_legged_msgs::MotorCmd>("/" + robot_name + "_gazebo/RL_calf_controller/command", 1);

    motion_init();//调用motion_init函数，定义在body里

    while (ros::ok()){ //当节点不死，就会一直在while中循环，所以就会一直发布
        /*
        control logic
        */
        lowState_pub.publish(lowState);//发布lowState变量，这个变量在body中定义的，用回调函数赋值的
        sendServoCmd();//定义在body里，实现了servo_pub[m].publish(lowCmd.motorCmd[m]，即发布12个电机状态

    }
    return 0;
}

2、external_force.cpp（用键盘施加额外力到gazebo的机器狗躯干上）

/************************************************************************
Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved.
Use of this source code is governed by the MPL-2.0 license, see LICENSE.
************************************************************************/
//这个文件实现了一个ROS节点，用于通过键盘输入实时应用外部力到机器人的特定部位（在本例中是躯干）。
//使用了信号处理来捕获中断信号，并安全地退出程序。
//teleForceCmd类包含发布力的逻辑和处理键盘输入的方法。
//支持脉冲模式和连续模式，用户可以通过键盘的上下左右键来改变施加的力的大小和方向。

#include <ros/ros.h>
#include <geometry_msgs/Wrench.h>//这个类型用于表示力和力矩
#include <signal.h>//包含用于处理信号的C标准库头文件，可以用来捕捉和处理如SIGINT（通常由Ctrl+C产生）这样的信号。
#include <termios.h>//包含用于终端I/O设置的C标准库头文件，允许对终端的行为进行更细致的控制，如获取和设置终端属性。
#include <stdio.h>

#define KEYCODE_UP    0x41 //代表上箭头键的ASCII码。
#define KEYCODE_DOWN  0x42 //代表下箭头键的ASCII码
#define KEYCODE_LEFT  0x44 //代表左箭头键的ASCII码
#define KEYCODE_RIGHT 0x43 //代表右箭头键的ASCII码
#define KEYCODE_SPACE 0x20 //代表空格的ASCII码

int mode = 1; // pulsed mode or continuous mode

class teleForceCmd
{
public:
    teleForceCmd();//teleForceCmd 类的构造函数，用于初始化类的对象
    void keyLoop();//一个成员函数，用于处理键盘输入，根据用户按下的键来改变力的大小和方向。
    void pubForce(double x, double y, double z);//一个成员函数，接受三个参数（x，y，z），代表要发布的力的三个分量。这个函数负责更新 Force 消息并发布它。
private:
    double Fx, Fy, Fz;//私有变量，分别存储在x、y、z方向上的力的值。
    ros::NodeHandle n;
    ros::Publisher force_pub;
    geometry_msgs::Wrench Force; //私有变量，geometry_msgs::Wrench 类型的对象，用于存储要发布的力和力矩数据。
};

teleForceCmd::teleForceCmd() //teleForceCmd 类的构造函数负责设置力控制节点的初始状态，包括初始化力变量、创建ROS发布者、等待ROS系统稳定。
{
    Fx = 0;
    Fy = 0;
    Fz = 0;
    force_pub = n.advertise<geometry_msgs::Wrench>("/apply_force/trunk", 20);
    sleep(1);//调用 sleep(1) 函数使程序暂停1秒钟。这通常用于等待ROS系统完全启动并稳定，确保消息发布者和订阅者都已准备好。
    pubForce(Fx, Fy, Fz);//赋值（0，0，0）
}
//这段代码是用于处理ROS节点退出时的清理工作，以及设置终端属性，使其能够在接收到特定信号时正确响应
int kfd = 0;//用作存储终端的文件描述符。文件描述符在Linux系统中用于访问文件和其他输入输出资源，这里它代表当前终端的文件描述符
struct termios cooked, raw;//cooked 通常用于保存终端的原始属性（用户可配置的），而 raw 用于保存更改后的属性（通常是以“原始”模式，即不处理特殊字符）。

void quit(int sig)//接收一个整数参数 sig，代表信号的编号。这个函数用于处理退出时的清理工作，参数 sig 表示触发退出的信号。
{
    tcsetattr(kfd, TCSANOW, &cooked);// tcsetattr 函数立即（TCSANOW）将 cooked 结构体中保存的终端属性应用到文件描述符 kfd 所代表的终端上。这通常用于在程序退出前恢复终端的原始状态。
    ros::shutdown();//ros::shutdown() 函数关闭ROS节点，这个函数会停止ROS节点的所有活动，包括取消订阅者、发布者和正在运行的服务。
    exit(0);
}//定义了一个退出处理函数 quit，它在接收到退出信号（如SIGINT，通常由Ctrl+C产生）时被调用。函数中首先恢复终端的原始属性，然后关闭ROS节点，并退出程序。

int main(int argc, char** argv)
{
    ros::init(argc, argv, "external_force");//创建名为"external_force"的节点node
    teleForceCmd remote;//实例化类的对象
    signal(SIGINT,quit);
    remote.keyLoop();
    return(0);
}

void teleForceCmd::pubForce(double x, double y, double z)
{
    Force.force.x = Fx;
    Force.force.y = Fy;
    Force.force.z = Fz;//Fx、Fy 和 Fz 成员变量的值分别赋给 Force 消息对象的 force 字段的 x、y 和 z 分量
    force_pub.publish(Force);//发布了力的消息
    ros::spinOnce();//这是 ROS 中的一个函数，用于处理与订阅主题相关的任何回调。在发布消息后调用 ros::spinOnce() 可以确保节点能够及时处理接收到的消息
}

void teleForceCmd::keyLoop()//keyLoop 函数通过无限循环监听键盘输入，在里面调用了pubForce函数发布力的消息
{
    char c;
    bool dirty=false;
    // get the console in raw mode
    tcgetattr(kfd, &cooked);//tcgetattr 函数用于获取文件描述符 kfd（通常为0，标准输入）的当前终端属性，并将其存储在 cooked 结构体中。
    memcpy(&raw, &cooked, sizeof(struct termios));//使用 memcpy 函数复制原始的终端属性到 raw 结构体中，以便稍后可以恢复原始设置
    raw.c_lflag &=~ (ICANON | ECHO);//修改 raw 结构体中的 c_lflag 成员，清除 ICANON 和 ECHO 标志。ICANON 使得终端以“原始”模式运行，ECHO 标志的清除表示终端不会自动回显输入的字符
    // Setting a new line, then end of file
    raw.c_cc[VEOL] = 1;//这里设置了终端的新行字符为1（通常对应 Ctrl+A）和文件结束符为2（通常对应 Ctrl+D）。这些设置允许程序通过特定的控制字符来检测特定的操作
    raw.c_cc[VEOF] = 2;
    tcsetattr(kfd, TCSANOW, &raw);//使用 tcsetattr 函数立即应用新的终端属性
    puts("Reading from keyboard");// puts 调用向用户显示程序如何使用键盘输入进行操作的说明。
    puts("---------------------------");
    puts("Use 'Space' to change mode, default is Pulsed mode:");
    puts("Use 'Up/Down/Left/Right' to change direction");
    for(;;){//这个循环会无限运行，持续监听键盘输入
        // get the next event from the keyboard
        if(read(kfd, &c, 1) < 0){//读取单个字符，存到字符c中
            perror("read():");
            exit(-1);//然后退出
        }
        ROS_DEBUG("value: 0x%02X\n", c);//使用 ROS_DEBUG 输出读取到的字符的值
        switch(c){
        case KEYCODE_UP://如果c是 up 按键
            if(mode > 0) {
                Fx = 60;
            } else {
                Fx += 16;
                if(Fx > 220) Fx = 220;
                if(Fx < -220) Fx = -220;
            }
            ROS_INFO("Fx:%3d   Fy:%3d   Fz:%3d", (int)Fx, (int)Fy, (int)Fz);
            dirty = true;
            break;
        case KEYCODE_DOWN:
            if(mode > 0) {
                Fx = -60;
            } else {
                Fx -= 16;
                if(Fx > 220) Fx = 220;
                if(Fx < -220) Fx = -220;
            }
            ROS_INFO("Fx:%3d   Fy:%3d   Fz:%3d", (int)Fx, (int)Fy, (int)Fz);
            dirty = true;
            break;
        case KEYCODE_LEFT:
            if(mode > 0) {
                Fy = 30;
            } else {
                Fy += 8;
                if(Fy > 220) Fy = 220;
                if(Fy < -220) Fy = -220;
            }
            ROS_INFO("Fx:%3d   Fy:%3d   Fz:%3d", (int)Fx, (int)Fy, (int)Fz);
            dirty = true;
            break;
        case KEYCODE_RIGHT:
            if(mode > 0) {
                Fy = -30;
            } else {
                Fy -= 8;
                if(Fy > 220) Fy = 220;
                if(Fy < -220) Fy = -220;
            }
            ROS_INFO("Fx:%3d   Fy:%3d   Fz:%3d", (int)Fx, (int)Fy, (int)Fz);
            dirty = true;
            break;
        case KEYCODE_SPACE://用于切换脉冲模式和连续模式
            mode = mode*(-1);//切换模式
            if(mode > 0){
                ROS_INFO("Change to Pulsed mode.");//1是脉冲模式
            } else {
                ROS_INFO("Change to Continuous mode.");//-1是连续模式
            }
            Fx = 0;
            Fy = 0;
            Fz = 0;            
            ROS_INFO("Fx:%3d   Fy:%3d   Fz:%3d", (int)Fx, (int)Fy, (int)Fz);
            dirty = true;
            break;
        }
        if(dirty == true){
            pubForce(Fx, Fy, Fz);
            if(mode > 0){//脉冲模式发布完之后要把力置0
                usleep(100000); // 100 ms
                Fx = 0;
                Fy = 0;
                Fz = 0;
                pubForce(Fx, Fy, Fz);
            }
            dirty=false;
        }
    }
    return;
}