目录
写在前面的话
上一篇博客：键盘控制车子四轮转向，原代码把键盘控制和车轮速度发布绑定到一起了，不适合后续的分布式独立开发，所以打算将键盘控制进行独立出来。
核心代码
键盘输入
符合 linux 系统的键盘监听
python
import sys, select, termios, tty
self.old_settings = termios.tcgetattr(sys.stdin)
def getKey(self):
tty.setraw(sys.stdin.fileno())
select.select([sys.stdin], [], [], 0)
key = sys.stdin.read(1)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.old_settings)
return key发布车子速度和车子转向
发布/keyboard_vel_msg话题
采用 Twist 数据类型，需要设置 linear 线速度和 angular 角速度
python
from geometry_msgs.msg import Quaternion, TransformStamped, Twist, Vector3
self.vel_pub = self.create_publisher(Twist,'/keyboard_vel_msg', 10)
self.vel_msg.linear.x = self.x * self.speed
self.vel_msg.linear.y = self.y * self.speed
self.vel_msg.angular.x = 0.0
self.vel_msg.angular.y = 0.0
self.vel_msg.angular.z = self.th * self.turn
print(f"currently: mode_selection({self.mode_selection}) \tspeed({self.vel_msg.linear.x}) \tturn({self.vel_msg.angular.z})")
self.vel_pub.publish(self.vel_msg)发布控制模式
发布/keyboard_control_mode话题
控制模式有3种，1（反相转弯）、2（同相变向）、3（原地旋转）
采用 Int32 数据类型，赋值要给data属性
python
from std_msgs.msg import Int32
self.mode_pub = self.create_publisher(Int32, '/keyboard_control_mode', 10)
int32_msg = Int32()
int32_msg.data = self.mode_selection
self.mode_pub.publish(int32_msg)函数调用
通过 rclpy.spin 进行 keyboard_subscriber.listener_keyboard() 循环调用
python
def main(args=None):
rclpy.init(args=sys.argv)
keyboard_subscriber = Keyboard_subscriber()
keyboard_subscriber.listener_keyboard()
rclpy.spin(keyboard_subscriber)
rclpy.shutdown()完整代码
按键功能提示
Moving around:
q w e (opposite-phase)
a s d f g h j (in-phase)
z c
anything else : stop
f/g : run to left_front/right_front (in-phase)
h/j : run to left_back/right_back (in-phase)
,/. : increase/decrease max speed by 10%
v/b : increase/decrease only linear speed by 10%
n/m : increase/decrease only angular speed by 10%
mode_selection
0 or other_number: 静止(stop 没速度控制)
1: 低速小角度转弯(oppostise-phase)
2: 高速变道(in-phase)需要y轴速度
3: 原地转弯(pviot-turn)
CTRL-C to quit
python
#!/usr/bin/python3
import math
import threading
import rclpy
import numpy as np
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Joy
import sys, select, termios, tty
from nav_msgs.msg import Odometry
import pty
from geometry_msgs.msg import Quaternion, TransformStamped, Twist, Vector3
from nav_msgs.msg import Odometry
from tf2_ros import TransformBroadcaster
from tf2_ros import TransformStamped
import math
from std_msgs.msg import Int32
class Keyboard_subscriber(Node):
def __init__(self):
super().__init__('keyboard_subscriber')
self.msg = """
Reading from the keyboard and Publishing to Twist!
---------------------------
注意：r是停止，角速度不能设置太大会导致转弯不稳车身摆动,
要想实现反相效果需要线速度大于角速度，否则车轮转角会不正确。
Moving around:
q w e (opposite-phase)
a s d f g h j (in-phase)
z c
anything else : stop
f/g : run to left_front/right_front (in-phase)
h/j : run to left_back/right_back (in-phase)
,/. : increase/decrease max speed by 10%
v/b : increase/decrease only linear speed by 10%
n/m : increase/decrease only angular speed by 10%
mode_selection
0 or other_number: 静止(stop 没速度控制)
1: 低速小角度转弯(oppostise-phase)
2: 高速变道(in-phase)需要y轴速度
3: 原地转弯(pviot-turn)
CTRL-C to quit
"""
self.moveBindings = {
'w':(1,0,0,0),
's':(-1,0,0,0),
'q':(1,0,0,1),
'e':(1,0,0,-1),
'a':(0,0,0,1),
'd':(0,0,0,-1),
'f':(1,1,0,0),
'g':(1,-1,0,0),
'h':(-1,1,0,0),
'j':(-1,-1,0,0),
'z':(-1,0,0,1),
'c':(-1,0,0,-1),
'r':(0,0,0,0),
}
self.speedBindings={
',':(1.1,1.1),
'.':(0.9,0.9),
'v':(1.1,1),
'b':(0.9,1),
'n':(1,1.1),
'm':(1,.9),
}
self.modeBindings={
'1':'opposite-phase',# 低速转弯
'2':'in-phase',
'3':'pivot-turn',#原地旋转
}
self.old_settings = termios.tcgetattr(sys.stdin)
print(self.old_settings)
self.vel_msg = Twist() # robot velocity
self.mode_selection = 1 # 1:opposite phase, 2:in-phase, 3:pivot turn 4: none
self.speed = 0.5
self.turn = 0.5 #30度左右
self.x = 0.0
self.y = 0.0
self.th = 0.0
self.status = 0.0
self.vel_pub = self.create_publisher(Twist,'/keyboard_vel_msg', 10)
self.mode_pub = self.create_publisher(Int32, '/keyboard_control_mode', 10)
def getKey(self):
tty.setraw(sys.stdin.fileno())
select.select([sys.stdin], [], [], 0)
key = sys.stdin.read(1)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.old_settings)
return key
def listener_keyboard(self):
try:
print(self.msg)
print(f"currently: mode_selection({self.mode_selection}) \tspeed({self.vel_msg.linear.x}) \tturn({self.vel_msg.angular.z})")
while True:
key = self.getKey()#获取按键
# print(f'Press {key}')
if key in self.moveBindings.keys():
self.x = float(self.moveBindings[key][0])
self.y = float(self.moveBindings[key][1])
self.th = float(self.moveBindings[key][3])
print(f"currently: mode_selection({self.mode_selection}) \tspeed({self.x * self.speed}) \tturn({self.th * self.turn})")
elif key in self.speedBindings.keys():
self.speed = self.speed * self.speedBindings[key][0]
self.turn = self.turn * self.speedBindings[key][1]
print(f"currently: mode_selection({self.mode_selection}) \tspeed({self.x * self.speed}) \tturn({self.th * self.turn})")
elif key in self.modeBindings.keys():
if self.modeBindings[key] == 'opposite-phase':
self.mode_selection = 1
elif self.modeBindings[key] == 'in-phase':
self.mode_selection = 2
elif self.modeBindings[key] == 'pivot-turn':
self.mode_selection = 3
else:
self.mode_selection = 0
print(f"currently: mode_selection({self.mode_selection}) \tspeed({self.vel_msg.linear.x}) \tturn({self.vel_msg.angular.z})")
print(self.msg)
else:
self.x = 0.0
self.y = 0.0
self.th = 0.0
if (key == '\x03'):
break
if (self.status == 8):
self.status = 0
print(self.msg)
# print(self.status)
self.status = self.status + 1
self.vel_msg.linear.x = self.x * self.speed
self.vel_msg.linear.y = self.y * self.speed
self.vel_msg.angular.x = 0.0
self.vel_msg.angular.y = 0.0
self.vel_msg.angular.z = self.th * self.turn
print(f"currently: mode_selection({self.mode_selection}) \tspeed({self.vel_msg.linear.x}) \tturn({self.vel_msg.angular.z})")
self.vel_pub.publish(self.vel_msg)
int32_msg = Int32()
int32_msg.data = self.mode_selection
self.mode_pub.publish(int32_msg)
except Exception as e:
print('sss',e)
finally:
self.vel_msg.linear.x = 0.0;
self.vel_msg.linear.y = 0.0;
self.vel_msg.linear.z = 0.0
self.vel_msg.angular.x = 0.0;
self.vel_msg.angular.y = 0.0;
self.vel_msg.angular.z = 0.0
self.vel_pub.publish(self.vel_msg)
print(f'{self.mode_selection}')
int32_msg = Int32()
int32_msg.data = self.mode_selection
self.mode_pub.publish(int32_msg)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.old_settings)
def main(args=None):
rclpy.init(args=sys.argv)
keyboard_subscriber = Keyboard_subscriber()
keyboard_subscriber.listener_keyboard()
rclpy.spin(keyboard_subscriber)
rclpy.shutdown()
if __name__ == '__main__':
main()运行演示
