无人驾驶虚拟仿真(四)--通过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

无人驾驶虚拟仿真(四)--通过ROS系统控制小车行走_第1张图片

终端2:$ rqt_image_view

无人驾驶虚拟仿真(四)--通过ROS系统控制小车行走_第2张图片

终端3:$ rosrun key_control key_control_node.py

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

无人驾驶虚拟仿真(四)--通过ROS系统控制小车行走_第3张图片

你可能感兴趣的:(无人驾驶虚拟仿真,python,自动驾驶)