目录
- 写在前面的话
- 核心代码
- 键盘输入
- 发布车子速度和车子转向
- 发布控制模式
- 函数调用
- 完整代码
- 运行演示
写在前面的话
上一篇博客：键盘控制车子四轮转向，原代码把键盘控制和车轮速度发布绑定到一起了，不适合后续的分布式独立开发，所以打算将键盘控制进行独立出来。
核心代码
键盘输入
符合 linux 系统的键盘监听
import sys, select, termios, ttyself.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 角速度
from geometry_msgs.msg import Quaternion, TransformStamped, Twist, Vector3self.vel_pub = self.create_publisher(Twist,'/keyboard_vel_msg', 10)self.vel_msg.linear.x = self.x * self.speedself.vel_msg.linear.y = self.y * self.speedself.vel_msg.angular.x = 0.0 self.vel_msg.angular.y = 0.0 self.vel_msg.angular.z = self.th * self.turnprint(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属性
from std_msgs.msg import Int32self.mode_pub = self.create_publisher(Int32, '/keyboard_control_mode', 10)int32_msg = Int32()int32_msg.data = self.mode_selectionself.mode_pub.publish(int32_msg)
函数调用
通过 rclpy.spin 进行 keyboard_subscriber.listener_keyboard() 循环调用
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 canything else : stopf/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_selection0 or other_number: 静止(stop 没速度控制)1: 低速小角度转弯(oppostise-phase)2: 高速变道(in-phase)需要y轴速度3: 原地转弯(pviot-turn)CTRL-C to quit
#!/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 Int32class 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 canything else : stopf/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_selection0 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 velocityself.mode_selection = 1 # 1:opposite phase, 2:in-phase, 3:pivot turn 4: noneself.speed = 0.5self.turn = 0.5 #30度左右self.x = 0.0self.y = 0.0self.th = 0.0self.status = 0.0self.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 keydef 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 = 1elif self.modeBindings[key] == 'in-phase':self.mode_selection = 2elif self.modeBindings[key] == 'pivot-turn':self.mode_selection = 3else:self.mode_selection = 0print(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.0self.y = 0.0self.th = 0.0if (key == '\x03'):breakif (self.status == 8):self.status = 0 print(self.msg)# print(self.status)self.status = self.status + 1self.vel_msg.linear.x = self.x * self.speedself.vel_msg.linear.y = self.y * self.speedself.vel_msg.angular.x = 0.0 self.vel_msg.angular.y = 0.0 self.vel_msg.angular.z = self.th * self.turnprint(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_selectionself.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.0self.vel_msg.angular.x = 0.0; self.vel_msg.angular.y = 0.0; self.vel_msg.angular.z = 0.0self.vel_pub.publish(self.vel_msg)print(f'{self.mode_selection}')int32_msg = Int32()int32_msg.data = self.mode_selectionself.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()运行演示
