#!/usr/bin/env pythonimport rospy
from ackermann_msgs.msg import AckermannDriveStamped
import sys, select, termios, tty
banner ="""
1. Reading from the keyboard
2. Publishing to AckermannDriveStamped!
---------------------------
w
a s d
anything else : stop
CTRL-C to quit
"""
keyBindings ={'w':(1,0),'d':(1,-1),'a':(1,1),'s':(-1,0),}defgetKey():
tty.setraw(sys.stdin.fileno())
select.select([sys.stdin],[],[],0)
key = sys.stdin.read(1)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)return key
speed =1
turn =0.6defvels(speed,turn):return"currently:\tspeed %s\tturn %s "%(speed,turn)if __name__=="__main__":
settings = termios.tcgetattr(sys.stdin)
pub = rospy.Publisher("/ackermann_cmd_mux/output", AckermannDriveStamped,queue_size=1)
rospy.init_node('keyop')
x =0
th =0
status =0try:while(1):
key = getKey()if key in keyBindings.keys():
x = keyBindings[key][0]
th = keyBindings[key][1]else:
x =0
th =0if(key =='\x03'):break
msg = AckermannDriveStamped();
msg.header.stamp = rospy.Time.now();
msg.header.frame_id ="base_link";
msg.drive.speed = x*speed;
msg.drive.acceleration =1;
msg.drive.jerk =1;
msg.drive.steering_angle = th*turn
msg.drive.steering_angle_velocity =1
pub.publish(msg)except:print('error')finally:
msg = AckermannDriveStamped();
msg.header.stamp = rospy.Time.now();
msg.header.frame_id ="base_link";
msg.drive.speed =0;
msg.drive.acceleration =1;
msg.drive.jerk =1;
msg.drive.steering_angle =0
msg.drive.steering_angle_velocity =1
pub.publish(msg)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
二.GUI界面模式下
#!/usr/bin/env python3# Copyright (c) 2019, The Personal Robotics Lab, The MuSHR Team, The Contributors of MuSHR# License: BSD 3-Clause. See LICENSE.md file in root directory.import atexit
import os
import signal
from threading import Lock
from tkinter import Frame, Label, Tk
import rospy
from ackermann_msgs.msg import AckermannDriveStamped
UP ="w"
LEFT ="a"
DOWN ="s"
RIGHT ="d"
QUIT ="q"
state =[False,False,False,False]
state_lock = Lock()
state_pub =None
root =None
control =Falsedefkeyeq(e, c):return e.char == c or e.keysym == c
defkeyup(e):global state
global control
with state_lock:if keyeq(e, UP):
state[0]=Falseelif keyeq(e, LEFT):
state[1]=Falseelif keyeq(e, DOWN):
state[2]=Falseelif keyeq(e, RIGHT):
state[3]=False
control =Truedefkeydown(e):global state
global control
with state_lock:if keyeq(e, QUIT):
shutdown()elif keyeq(e, UP):
state[0]=True
state[2]=Falseelif keyeq(e, LEFT):
state[1]=True
state[3]=Falseelif keyeq(e, DOWN):
state[2]=True
state[0]=Falseelif keyeq(e, RIGHT):
state[3]=True
state[1]=False
control =sum(state)>0# Up -> linear.x = 1.0# Down -> linear.x = -1.0# Left -> angular.z = 1.0# Right -> angular.z = -1.0defpublish_cb(_):global control
with state_lock:ifnot control:return
ack = AckermannDriveStamped()if state[0]:
ack.drive.speed = max_velocity
elif state[2]:
ack.drive.speed =-max_velocity
if state[1]:
ack.drive.steering_angle = max_steering_angle
elif state[3]:
ack.drive.steering_angle =-max_steering_angle
if state_pub isnotNone:
state_pub.publish(ack)
control =sum(state)>0defexit_func():
os.system("xset r on")defshutdown():
root.destroy()
rospy.signal_shutdown("shutdown")defmain():global state_pub
global root
global max_velocity
global max_steering_angle
max_velocity = rospy.get_param("~speed",2.0)
max_steering_angle = rospy.get_param("~max_steering_angle",0.6)
state_pub = rospy.Publisher("/ackermann_cmd_mux/output", AckermannDriveStamped, queue_size=1)
rospy.Timer(rospy.Duration(0.1), publish_cb)
atexit.register(exit_func)
os.system("xset r off")
root = Tk()
frame = Frame(root, width=100, height=100)
frame.bind("<KeyPress>", keydown)
frame.bind("<KeyRelease>", keyup)
frame.pack()
frame.focus_set()
lab = Label(
frame,
height=10,
width=30,
text="Focus on this window\nand use the WASD keys\nto drive the car.\n\nPress Q to quit",)
lab.pack()print("Press %c to quit"% QUIT)
root.mainloop()if __name__ =="__main__":
rospy.init_node("keyboard_teleop", disable_signals=True)
signal.signal(signal.SIGINT,lambda s, f: shutdown())
main()