写一份ROS的键盘控制底盘运动的C++代码

以下是一个简单的C++代码示例，可以使用键盘控制ROS底盘运动：

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <iostream>
#include <termios.h>
#include <stdio.h>

// 自定义函数：读取键盘输入
int getch()
{
    static struct termios oldt, newt;
    tcgetattr(STDIN_FILENO, &oldt);
    newt = oldt;
    newt.c_lflag &= ~(ICANON | ECHO);
    tcsetattr(STDIN_FILENO, TCSANOW, &newt);

    int ch = getchar();

    tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
    return ch;
}

int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "keyboard_control");
    ros::NodeHandle nh;

    // 创建一个Publisher，发布Twist消息到/cmd_vel主题
    ros::Publisher cmd_vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);

    // 创建一个Twist消息对象，用于控制底盘运动
    geometry_msgs::Twist twist;

    // 打印提示信息
    std::cout << "Use arrow keys to move the robot. Press Ctrl-C to quit." << std::endl;

    // 循环等待键盘输入
    while (ros::ok())
    {
        int c = getch();

        // 根据键盘输入设置Twist消息的线速度和角速度
        switch (c)
        {
            case 'A':  // Up arrow key
                twist.linear.x = 1.0;
                twist.angular.z = 0.0;
                break;
            case 'B':  // Down arrow key
                twist.linear.x = -1.0;
                twist.angular.z = 0.0;
                break;
            case 'C':  // Right arrow key
                twist.linear.x = 0.0;
                twist.angular.z = -1.0;
                break;
            case 'D':  // Left arrow key
                twist.linear.x = 0.0;
                twist.angular.z = 1.0;
                break;
            default:
                twist.linear.x = 0.0;
                twist.angular.z = 0.0;
                break;
        }

        // 发布Twist消息
        cmd_vel_pub.publish(twist);

        // 打印Twist消息的线速度和角速度
        std::cout << "Linear: " << twist.linear.x << " Angular: " << twist.angular.z << std::endl;
    }

    return 0;
}

此代码将读取键盘输入，并使用Twist消息控制底盘运动。根据不同的键盘输入，设置不同的线速度和角速度。Twist消息发布到/cmd_vel主题，可以通过订阅该主题来控制底盘的运动。