import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前 45° 右前 45° 左后 45° 右后 45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
self.motion_map = {
'w': (1, 0, 0, 0), 'e': (1, 0, 0, -1), 'a': (0, 1, 0, 0), 'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1), 'x': (-1, 0, 0, 0), 'c': (-1, 0, 0, 1), 'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1), 't': (0, 0, 0, -1), 'f': (1, 1, 0, 0), 'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0), 'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0), 'i': (0.9, 1.0), 'j': (1.0, 1.1), 'k': (1.0, 0.9),
'm': (1.1, 1.1), ',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i], self.target[i], self.acc_step, self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
