You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
81 lines
1.7 KiB
Python
81 lines
1.7 KiB
Python
#!/usr/bin/env python3
|
|
import sys, select, termios, tty
|
|
|
|
import rospy
|
|
from std_msgs.msg import String
|
|
|
|
|
|
COMMAND_MAP = {
|
|
"i": "forward",
|
|
",": "stop",
|
|
"j": "left",
|
|
"l": "right",
|
|
"k": "back",
|
|
"\x03": "break",
|
|
}
|
|
|
|
command = "stop"
|
|
last_command = "stop"
|
|
|
|
msg = """
|
|
Reading from the keyboard and publishing to /command!
|
|
-----------------------------------------------------
|
|
Moving around:
|
|
i
|
|
j k l
|
|
,
|
|
CTRL-C to quit
|
|
"""
|
|
|
|
|
|
def broadcaster():
|
|
global command
|
|
global last_command
|
|
|
|
pub = rospy.Publisher("/command", String, queue_size=10)
|
|
rospy.init_node("keyb_commander", anonymous=True, log_level=rospy.INFO)
|
|
rate = rospy.Rate(10)
|
|
|
|
print(msg)
|
|
|
|
while not rospy.is_shutdown():
|
|
key_timeout = 0.6 # seconds
|
|
k = get_key(key_timeout)
|
|
|
|
command = COMMAND_MAP.get(k)
|
|
|
|
if command == "break":
|
|
# shut it down
|
|
break
|
|
|
|
|
|
|
|
if command != last_command and command is not None:
|
|
# print("Published command: {}".format(command))
|
|
rospy.loginfo("Published command: {}".format(command))
|
|
last_command = command
|
|
|
|
if command is not None:
|
|
pub.publish(command)
|
|
|
|
rate.sleep()
|
|
|
|
|
|
def get_key(key_timeout):
|
|
tty.setraw(sys.stdin.fileno())
|
|
rlist, _, _ = select.select([sys.stdin], [], [], key_timeout)
|
|
if rlist:
|
|
key = sys.stdin.read(1)
|
|
else:
|
|
key = ""
|
|
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
|
|
return key
|
|
|
|
|
|
if __name__ == "__main__":
|
|
settings = termios.tcgetattr(sys.stdin)
|
|
try:
|
|
broadcaster()
|
|
except rospy.ROSInterruptException:
|
|
pass
|