ROS学习——通过键盘控制机器人运动

嵵266

于 2025-03-19 22:26:34 发布

阅读量1.1k

点赞数 15

文章标签：学习计算机外设机器人

本文链接：https://blog.csdn.net/2403_88733025/article/details/146370917

版权

ROS学习

通过键盘实现机器人的运动

C++ 代码

在软件包中创建节点
代码及其解析如下

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

using namespace std;

// 设定键位映射 (线速度, 角速度)
map<char, pair<double, double>> key_mapping = 
{
    {'w', {1.0, 0.0}},  // 前进
    {'s', {-1.0, 0.0}}, // 后退
    {'a', {0.0, 1.0}},  // 左转
    {'d', {0.0, -1.0}}, // 右转
    {'q', {0.0, 0.0}}   // 停止
};

void setTerminalMode()
{
    struct termios new_settings;
    tcgetattr(0, &new_settings);
    new_settings.c_lflag &= ~ICANON; // 规定按键输入后不需要回车立即生效
    new_settings.c_lflag &= ~ECHO;   // 规定输入不显示在终端上
    new_settings.c_cc[VMIN] = 1;     // 规定按键必须输入一个字符 
    new_settings.c_cc[VTIME] = 0;    // 规定输入进行不等待，即立即响应
    tcsetattr(0, TCSANOW, &new_settings); // 进行设置
}

char getKey()
{
    char c;
    if (read(0, &c, 1) < 0) return '\0';
    return c;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "keyboard_control"); // 初始化节点
    ros::NodeHandle nh;
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10); // 规定话题参数
    ros::Rate rate(10); // 控制发布话题频率

    setTerminalMode();
    cout << "控制小车移动：\n---------------------------\n   w    \n a  q  d\n   s    \n\nw/s: 前进/后退\na/d: 左转/右转\nq: 停止\nCTRL+C 退出\n";
    geometry_msgs::Twist twist;
    while (ros::ok()) 
    {
        char key = getKey();
        if (key_mapping.count(key) && key != 'q') //判断
        {
            twist.linear.x += key_mapping[key].first * 0.5;
            twist.angular.z += key_mapping[key].second * 0.5;
            pub.publish(twist);
        }
        if (key == 'q') 
        {
            twist.linear.x = 0;
            twist.angular.z = 0;
            pub.publish(twist);
        }
        rate.sleep(); // 设置频率
    }
    
    geometry_msgs::Twist empty_twist; 
    pub.publish(empty_twist); // 发布一个空话题，使小车停止移动
    return 0;
}

在该节点的软件包中的CMakelist.txt文件添加节点声明如下

add_executable(keyboard_control src/keyboard_control.cpp)
target_link_libraries(keyboard_control
${catkin_LIBRARIES}
)
add_dependencies(keyboard_control ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

1. catkin_make编译工作空间

Python 代码

软件包中新建节点，编写代码

#!/usr/bin/env python3
#coding = utf-8
import rospy
import sys
import termios
import tty
from geometry_msgs.msg import Twist

# 设定键位映射 (线速度, 角速度)
key_mapping = {
    'w': (0.1, 0.0),  # 前进
    's': (-0.1, 0.0),  # 后退
    'a': (0.0, 0.1),   # 左转
    'd': (0.0, -0.1),  # 右转
    'q': (0.0, 0.0)    # 停止
}

""" 获取键盘输入 """
def get_key():
    fd = sys.stdin.fileno() 
    old_settings = termios.tcgetattr(fd)
    try:
        tty.setraw(sys.stdin.fileno())  # 设置终端不回显和缓冲输入
        key = sys.stdin.read(1)  # 读取一个字符
    finally:
        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)  # 还原终端设置
    return key

def main():
    rospy.init_node("keyboard_control")  # 初始化 ROS 节点
    pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)  # 发布 /cmd_vel 话题
    rate = rospy.Rate(10)  # 设置发布话题频率

    twist = Twist()
    print("控制小车移动：\n---------------------------\n   w    \n a  q  d\n   s    \n\nw/s: 前进/后退\na/d: 左转/右转\nq: 停止\nCTRL+C 退出\n")

    while not rospy.is_shutdown():
        key = get_key()
        if key in key_mapping:
            twist.linear.x += key_mapping[key][0] 
            twist.angular.z += key_mapping[key][1]

        if key == 'q':  # 按 'q' 停止
            twist.linear.x = 0
            twist.angular.z = 0
        
        pub.publish(twist)
        rate.sleep()

    empty_twist = Twist()
    pub.publish(empty_twist)

if __name__ == "__main__":
    main()