ROS仿真小车python键盘控制测试

作者：chenqy2017-08-24 12:28:19浏览(633)

---

          

操作

$ cd ~/catkin_ws/src/
$ roscreate-pkg smartcar_teleop rospy geometry_msgs std_msgs roscpp
$ vim smartcar_teleop/src/teleop.py
$
$ sudo chmod +x smartcar_teleop/src/teleop.py
$ source ~/catkin_ws/devel/setup.bash
$
$ roslaunch smartcar_description smartcar_display.rviz.launch
$ rosrun smartcar_teleop teleop.py
$ 

code

#!/usr/bin/env python  
# -*- coding: utf-8 -*  
  
import os  
import sys  
import tty, termios  
import roslib 
import rospy  
from geometry_msgs.msg import Twist  
from std_msgs.msg import String  
  

def teleop_key():
   
    thread_stop = False 
    cmd = Twist() 
    roslib.load_manifest('smartcar_teleop') 
    pub = rospy.Publisher('cmd_vel', Twist)      
    rospy.init_node('smartcar_teleop')  
    
    rate = rospy.Rate(rospy.get_param('~hz', 1))   
    walk_vel_ = rospy.get_param('walk_vel', 0.5)   
    yaw_rate_ = rospy.get_param('yaw_rate', 1.0)   
      
    max_tv = walk_vel_  
    max_rv = yaw_rate_  
    turn = 0
    speed = 0 
    
    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)
            
        if ch == 'w':  
            max_tv = walk_vel_  
            speed = 1  
        elif ch == 's':  
            max_tv = walk_vel_  
            speed = -1   
        elif ch == 'a':  
            max_rv = yaw_rate_  
            turn = 1  
        elif ch == 'd':  
            max_rv = yaw_rate_  
            turn = -1  
        elif ch == 'q':  
            turn = 0
            speed = 0 
            thread_stop = True  
        else:  
            turn = 0
            speed = 0 
            max_tv = walk_vel_  
            max_rv = yaw_rate_  
            
        cmd.linear.x = speed * max_tv 
        cmd.angular.z = turn * max_rv
        pub.publish(cmd)  
        rate.sleep() 
    
if __name__ == '__main__':  
    teleop_key() 

效果：