ROS学习篇之手动控制（八）

ROS学习篇之手动控制（八）

一.命令行模式下

#!/usr/bin/env python
import rospyfrom ackermann_msgs.msg import AckermannDriveStampedimport sys, select, termios, ttybanner = """
1. Reading from the keyboard  
2. Publishing to AckermannDriveStamped!
---------------------------wa    s    d
anything else : stop
CTRL-C to quit
"""keyBindings = {'w':(1,0),'d':(1,-1),'a':(1,1),'s':(-1,0),
}def getKey():tty.setraw(sys.stdin.fileno())select.select([sys.stdin], [], [], 0)key = ad(setattr(sys.stdin, termios.TCSADRAIN, settings)return keyspeed = 1
turn = 0.6def vels(speed,turn):return "currently:tspeed %stturn %s " % (speed,turn)if __name__=="__main__":settings = attr(sys.stdin)pub = rospy.Publisher("/ackermann_cmd_mux/output", AckermannDriveStamped,queue_size=1)rospy.init_node('keyop')x = 0th = 0status = 0try:while(1):key = getKey()if key in keyBindings.keys():x = keyBindings[key][0]th = keyBindings[key][1]else:x = 0th = 0if (key == 'x03'):breakmsg = AckermannDriveStamped();msg.header.stamp = w();msg.header.frame_id = "base_link";msg.drive.speed = x*speed;msg.drive.acceleration = 1;msg.drive.jerk = 1;msg.drive.steering_angle = th*turnmsg.drive.steering_angle_velocity = 1pub.publish(msg)except:print('error')finally:msg = AckermannDriveStamped();msg.header.stamp = w();msg.header.frame_id = "base_link";msg.drive.speed = 0;msg.drive.acceleration = 1;msg.drive.jerk = 1;msg.drive.steering_angle = 0msg.drive.steering_angle_velocity = 1pub.publish(setattr(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, Tkimport rospy
from ackermann_msgs.msg import AckermannDriveStampedUP = "w"
LEFT = "a"
DOWN = "s"
RIGHT = "d"
QUIT = "q"state = [False, False, False, False]
state_lock = Lock()
state_pub = None
root = None
control = Falsedef keyeq(e, c):return e.char == c or e.keysym == cdef keyup(e):global stateglobal controlwith 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] = Falsecontrol = Truedef keydown(e):global stateglobal controlwith state_lock:if keyeq(e, QUIT):shutdown()elif keyeq(e, UP):state[0] = Truestate[2] = Falseelif keyeq(e, LEFT):state[1] = Truestate[3] = Falseelif keyeq(e, DOWN):state[2] = Truestate[0] = Falseelif keyeq(e, RIGHT):state[3] = Truestate[1] = Falsecontrol = sum(state) > 0# Up -> linear.x = 1.0
# Down -> linear.x = -1.0
# Left ->  angular.z = 1.0
# Right -> angular.z = -1.0def publish_cb(_):global controlwith state_lock:if not control:returnack = AckermannDriveStamped()if state[0]:ack.drive.speed = max_velocityelif state[2]:ack.drive.speed = -max_velocityif state[1]:ack.drive.steering_angle = max_steering_angleelif state[3]:ack.drive.steering_angle = -max_steering_angleif state_pub is not None:state_pub.publish(ack)control = sum(state) > 0def exit_func():os.system("xset r on")def shutdown():root.destroy()rospy.signal_shutdown("shutdown")def main():global state_pubglobal rootglobal max_velocityglobal max_steering_anglemax_velocity = _param("~speed",2.0)max_steering_angle = _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_ister(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 windownand use the WASD keysnto drive the car.nnPress 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()