ROS仿真小车python键盘控制测试
操作
$ 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()
效果: