122 lines
3.0 KiB
Python
Executable File
122 lines
3.0 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
"""
|
|
Keyboard control node for robot chassis.
|
|
W/S: forward/backward
|
|
A/D: turn left/right
|
|
I/K: increase/decrease linear speed
|
|
J/L: increase/decrease angular speed
|
|
X: stop
|
|
Q: quit
|
|
"""
|
|
|
|
import rclpy
|
|
from rclpy.node import Node
|
|
from geometry_msgs.msg import Twist
|
|
import sys
|
|
import select
|
|
import termios
|
|
import tty
|
|
|
|
HELP = """
|
|
========================================
|
|
Keyboard Control
|
|
========================================
|
|
W/S : forward / backward
|
|
A/D : turn left / turn right
|
|
I/K : linear speed +/- (step 0.05)
|
|
J/L : angular speed +/- (step 0.1)
|
|
X/Space: stop
|
|
Q : quit
|
|
========================================
|
|
Current: lin=%.2f ang=%.2f
|
|
"""
|
|
|
|
|
|
class KeyboardControl(Node):
|
|
def __init__(self):
|
|
super().__init__("keyboard_control")
|
|
self.pub = self.create_publisher(Twist, "/cmd_vel", 10)
|
|
|
|
self.lin_speed = 0.2
|
|
self.ang_speed = 0.5
|
|
self.lin_step = 0.05
|
|
self.ang_step = 0.1
|
|
|
|
self.print_help()
|
|
self.create_timer(0.05, self.read_key)
|
|
|
|
def print_help(self):
|
|
print(HELP % (self.lin_speed, self.ang_speed))
|
|
|
|
def get_key(self):
|
|
fd = sys.stdin.fileno()
|
|
old = termios.tcgetattr(fd)
|
|
try:
|
|
tty.setraw(fd)
|
|
r, _, _ = select.select([sys.stdin], [], [], 0.05)
|
|
if r:
|
|
return sys.stdin.read(1)
|
|
return None
|
|
finally:
|
|
termios.tcsetattr(fd, termios.TCSADRAIN, old)
|
|
|
|
def read_key(self):
|
|
key = self.get_key()
|
|
if key is None:
|
|
return
|
|
|
|
twist = Twist()
|
|
|
|
if key == 'w':
|
|
twist.linear.x = self.lin_speed
|
|
elif key == 's':
|
|
twist.linear.x = -self.lin_speed
|
|
elif key == 'a':
|
|
twist.angular.z = self.ang_speed
|
|
elif key == 'd':
|
|
twist.angular.z = -self.ang_speed
|
|
elif key == 'i':
|
|
self.lin_speed = round(self.lin_speed + self.lin_step, 2)
|
|
self.print_help()
|
|
return
|
|
elif key == 'k':
|
|
self.lin_speed = round(max(0.0, self.lin_speed - self.lin_step), 2)
|
|
self.print_help()
|
|
return
|
|
elif key == 'j':
|
|
self.ang_speed = round(self.ang_speed + self.ang_step, 2)
|
|
self.print_help()
|
|
return
|
|
elif key == 'l':
|
|
self.ang_speed = round(max(0.0, self.ang_speed - self.ang_step), 2)
|
|
self.print_help()
|
|
return
|
|
elif key == 'x' or key == ' ':
|
|
twist.linear.x = 0.0
|
|
twist.angular.z = 0.0
|
|
print("*** STOP ***")
|
|
elif key == 'q' or ord(key) == 3:
|
|
twist.linear.x = 0.0
|
|
twist.angular.z = 0.0
|
|
self.pub.publish(twist)
|
|
self.get_logger().info("Quitting...")
|
|
raise SystemExit
|
|
else:
|
|
return
|
|
|
|
self.pub.publish(twist)
|
|
|
|
|
|
def main():
|
|
rclpy.init()
|
|
try:
|
|
rclpy.spin(KeyboardControl())
|
|
except SystemExit:
|
|
pass
|
|
finally:
|
|
rclpy.shutdown()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|