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

简介：实现键盘控制虚拟仿真小车移动，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 rospy
from duckietown_msgs.msg import Twist2DStamped, BoolStamped
  
class KeyControlNode():
    def __init__(self):
        rospy.init_node('key_control_node')
        self.v = 0.0
        self.omega = 0.0
        self.estop = False
        self.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 = False
        rate = 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.2
                self.omega = 0.3
            elif ch == 's':  
                self.v = -0.2
                self.omega = 0.3
            elif ch == 'a':  
                self.v = 0.2
                self.omega = 1
            elif ch == 'd':  
                self.v = 0.2
                self.omega = -1
            elif ch == 'q': 
                self.v = 0.0
                self.omega = 0.0
                thread_stop = True
            else:
                if self.estop:
                    e_stop_msg = BoolStamped()
                    e_stop_msg.data = False
                    self.pub_e_stop.publish(e_stop_msg)
                    self.estop = False
                else:
                    e_stop_msg = BoolStamped()
                    e_stop_msg.data = True
                    self.pub_e_stop.publish(e_stop_msg)
                    self.estop = True
              
            msg_car_cmd = Twist2DStamped()
            msg_car_cmd.v = self.v
            msg_car_cmd.omega = self.omega
            self.pub_car_cmd.publish(msg_car_cmd)
            rate.sleep()
   
if __name__ == '__main__':  
    keyControlNode = KeyControlNode()
    keyControlNode.keyDetect()

---

3、编译

$ cd ~/myros/catkin_ws

$ catkin_make

---

4、运行

运行需要3个终端，一个运行roscore与duckiebot节点，一个开ros视频流查看软件，一个开key_control节点：

注：每新开一个终端，都要执行环境变量设置命令

$ source ~/myros/catkin_ws/devel/setup.bash

终端1：$ roslaunch duckiebot duckiebot.launch

终端2：$ rqt_image_view

终端3：$ rosrun key_control key_control_node.py

通过键盘上的w/s/a/d键控制小车移动方向，空格键急停切换，q键退出