Apollo学习——键盘控制速度

# keyboard_control.py
import time
import keyboard # 键盘输入模块 pip install keyboard
from getkey import getkey, keys
from cyber.python.cyber_py3 import cyber_time
from cyber.python.cyber_py3 import cyber
from modules.common_msgs.control_msgs import control_cmd_pb2
from modules.common_msgs.chassis_msgs import chassis_pb2
import sys, select, os
if os.name == 'nt':
  import msvcrt, time
else:
  import tty, termios
derta_speed = 2.0  # 单位：m/s
derta_heading = 2.0  # 单位：m/s
def getKey():
    if os.name == 'nt':
        timeout = 0.1
        startTime = time.time()
        while(1):
            if msvcrt.kbhit():
                if sys.version_info[0] >= 3:
                    return msvcrt.getch().decode()
                else:
                    return msvcrt.getch()
            elif time.time() - startTime > timeout:
                return ''

    tty.setraw(sys.stdin.fileno())
    rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
    if rlist:
        key = sys.stdin.read(1)
    else:
        key = ''

    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key
class CarController:
    def __init__(self):
        # 控制参数初始化
        self.throttle = 0.0    # 油门 (0.0~1.0)
        self.brake = 0.0       # 刹车 (0.0~1.0)
        self.steering = 0.0    # 转向 (-1.0左 ~ 1.0右)
        self.max_throttle = 100.0  # 最大油门限制（安全阈值）
        self.steering_rate = 0.1  # 转向灵敏度

        # 初始化 Cyber RT
        cyber.init()
        self.node = cyber.Node("keyboard_control")
        self.control_writer = self.node.create_writer("/apollo/control", control_cmd_pb2.ControlCommand)
    
    def send_control_command(self):
        """发布控制指令至 Apollo 控制模块"""
        cmd = control_cmd_pb2.ControlCommand()
        cmd.header.timestamp_sec = cyber_time.Time.now().to_sec()
        cmd.header.module_name = "keyboard_control"
        
        # 指令映射
        cmd.throttle = self.throttle
        cmd.brake = self.brake
        cmd.steering_target = self.steering
        cmd.gear_location = 1  # 前进挡
        
        self.control_writer.write(cmd)
        print(f"指令: 油门={self.throttle:.2f}, 刹车={self.brake:.2f}, 转向={self.steering:.2f}")

    def keyboard_listener(self):
        """监听键盘输入并更新控制参数"""
        print("使用 WASD 控制车辆，Q 退出...")

        while not cyber.is_shutdown():
            key = getKey()
            # 油门控制（W/S）
            if key == 'w':
                self.throttle = min(self.throttle + derta_speed, self.max_throttle)
                self.brake = 0.0
            elif key == 's':
                self.brake = min(self.brake + derta_speed, self.max_throttle)
                self.throttle = 0.0
            # else:
            #     self.throttle = max(self.throttle - derta_speed, 0.0)
            #     self.brake = max(self.brake - derta_speed, 0.0)
            
            # 转向控制（A/D）
            if key == 'a':
                self.steering = max(self.steering - self.steering_rate, -1.0)
            elif key == 'd':
                self.steering = min(self.steering + self.steering_rate, 1.0)
            else:
                self.steering *= 0.9  # 自动回正
            
            # 退出条件
            if key == 'q':
                break
            
            self.send_control_command()
            time.sleep(0.1)  # 控制频率10Hz

if __name__ == '__main__':
    controller = CarController()
    controller.keyboard_listener()
    cyber.shutdown()