第一句子网 > 无人驾驶虚拟仿真（四）--通过ROS系统控制小车行走

无人驾驶虚拟仿真（四）--通过ROS系统控制小车行走

时间：2023-09-14 05:00:05

简介：实现键盘控制虚拟仿真小车移动，w/s/a/d/空格，对应向前/向后/向左/向右/急停切换功能，q键退出

1、创建key_control节点

进入工作空间源码目录：

$ cd ~/myros/catkin_ws/src/

创建功能包：

$ catkin_create_pkg key_control rospy std_msgs duckietown_msgs

创建源码文件：

$ touch key_control/src/key_control_node.py

修改编译配置文件：

$ gedit key_control/CMakeLists.txt

修改为：

2、编写节点代码

$ gedit key_control/src/key_control_node.py

代码主要功能包括启动仿真环境，监听键盘输入，判断键盘输入发布控制指令话题，

附源码：

#!/usr/bin/env python3# -*- coding: utf-8 -* import os import sys import tty, termios import roslib import rospyfrom duckietown_msgs.msg import Twist2DStamped, BoolStampedclass KeyControlNode():def __init__(self):rospy.init_node('key_control_node')self.v = 0.0self.omega = 0.0self.estop = Falseself.pub_car_cmd = rospy.Publisher('/duckietown/duckiebot_node/car_cmd', Twist2DStamped, queue_size=10)self.pub_e_stop = rospy.Publisher('/duckietown/duckiebot_node/emergency_stop', BoolStamped, queue_size=10)def keyDetect(self):thread_stop = Falserate = rospy.Rate(10)while not thread_stop: fd = sys.stdin.fileno()old_settings = termios.tcgetattr(fd)try :tty.setraw( fd )ch = sys.stdin.read(1)finally :termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)print(ch)if ch == 'w': self.v = 0.2self.omega = 0.3elif ch == 's': self.v = -0.2self.omega = 0.3elif ch == 'a': self.v = 0.2self.omega = 1elif ch == 'd': self.v = 0.2self.omega = -1elif ch == 'q': self.v = 0.0self.omega = 0.0thread_stop = Trueelse:if self.estop:e_stop_msg = BoolStamped()e_stop_msg.data = Falseself.pub_e_stop.publish(e_stop_msg)self.estop = Falseelse:e_stop_msg = BoolStamped()e_stop_msg.data = Trueself.pub_e_stop.publish(e_stop_msg)self.estop = Truemsg_car_cmd = Twist2DStamped()msg_car_cmd.v = self.vmsg_car_cmd.omega = self.omegaself.pub_car_cmd.publish(msg_car_cmd)rate.sleep()if __name__ == '__main__': keyControlNode = KeyControlNode()keyControlNode.keyDetect()