示例代码

本节包含一些使用 MetaCar 库的示例和教程，帮助您更快地理解如何实现自己的应用。

基础控制示例

以下是一个基础的车辆控制示例，展示了如何使用键盘输入来控制车辆：

基础控制示例 (examples/main.py)
  1import logging
  2import keyboard
  3import math
  4from metacar import SceneAPI, GearMode, VehicleControl, Vector3, SimCarMsg
  5
  6# 是否使用 GUI 界面
  7USE_GUI = True
  8
  9if USE_GUI:
 10    from gui import Dashboard
 11
 12logging.basicConfig(
 13    filename="autodrive.log",
 14    format="%(asctime)s - %(name)s - %(levelname)s - %(message)s",
 15    level=logging.DEBUG,
 16    encoding="utf-8",
 17)
 18logger = logging.getLogger(__name__)
 19
 20current_gear = GearMode.DRIVE
 21use_keyboard = True
 22
 23
 24def set_gear(gear: GearMode):
 25    global current_gear
 26    current_gear = gear
 27
 28
 29def toggle_keyboard():
 30    global use_keyboard
 31    use_keyboard = not use_keyboard
 32
 33
 34def get_vehicle_control_from_keyboard() -> VehicleControl:
 35    vc = VehicleControl()
 36    vc.gear = current_gear
 37    if keyboard.is_pressed("up") or keyboard.is_pressed("w"):
 38        value = 0.5 if keyboard.is_pressed("shift") else 1
 39        if current_gear == GearMode.DRIVE:
 40            vc.throttle = value
 41        elif current_gear == GearMode.REVERSE:
 42            vc.brake = value
 43    elif keyboard.is_pressed("down") or keyboard.is_pressed("s"):
 44        value = 0.5 if keyboard.is_pressed("shift") else 1
 45        if current_gear == GearMode.DRIVE:
 46            vc.brake = value
 47        elif current_gear == GearMode.REVERSE:
 48            vc.throttle = value
 49    if keyboard.is_pressed("left") or keyboard.is_pressed("a"):
 50        vc.steering = -1
 51    elif keyboard.is_pressed("right") or keyboard.is_pressed("d"):
 52        vc.steering = 1
 53    return vc
 54
 55
 56def calc_throttle_brake(
 57    current_speed: float, target_speed: float
 58) -> tuple[float, float]:
 59    K = 0.2
 60    B = 0.2
 61    acceleration = (target_speed - current_speed) * K + B
 62    if acceleration > 0:
 63        return min(acceleration, 1), 0
 64    return 0, min(-acceleration * 0.5, 1)
 65
 66
 67# Stanley 算法
 68def calc_steering(
 69    pos: Vector3, yaw: float, speed: float, trajectory: list[Vector3]
 70) -> float:
 71    K = 0.5
 72    for traj_pos in trajectory:
 73        if math.dist(traj_pos, pos) > K * speed:
 74            target_pos = traj_pos
 75            break
 76    else:
 77        target_pos = trajectory[-1]
 78    theta = (target_pos - pos).yaw_rad()
 79    steering_angle = (theta - yaw) % (2 * math.pi)
 80    if steering_angle > math.pi:
 81        steering_angle -= 2 * math.pi
 82    steering = math.degrees(-steering_angle) / 45
 83    return max(min(steering, 1), -1)
 84
 85
 86def get_vehicle_control_from_algorithm(msg: SimCarMsg) -> VehicleControl:
 87    vc = VehicleControl()
 88    vc.gear = GearMode.DRIVE
 89    # 目前只支持固定速度
 90    vc.throttle, vc.brake = calc_throttle_brake(msg.data_main_vehicle.speed, 15)
 91    vc.steering = calc_steering(
 92        Vector3(
 93            msg.pose_gnss.pos_x,
 94            msg.pose_gnss.pos_y,
 95            msg.pose_gnss.pos_z,
 96        ),
 97        -math.radians(msg.pose_gnss.ori_z),
 98        msg.data_main_vehicle.speed,
 99        msg.trajectory,
100    )
101    return vc
102
103
104def main():
105    api = SceneAPI()
106
107    keyboard.add_hotkey("space", api.retry_level)
108    keyboard.add_hotkey("n", api.skip_level)
109
110    keyboard.add_hotkey("r", lambda: set_gear(GearMode.REVERSE))
111    keyboard.add_hotkey("f", lambda: set_gear(GearMode.DRIVE))
112    keyboard.add_hotkey("t", lambda: set_gear(GearMode.NEUTRAL))
113    keyboard.add_hotkey("g", lambda: set_gear(GearMode.PARKING))
114
115    keyboard.add_hotkey("c", toggle_keyboard)
116
117    api.connect()
118    static_data = api.get_scene_static_data()
119    if USE_GUI:
120        dashboard = Dashboard(static_data)
121        logger.info("启动 GUI 界面")
122    logger.info("开始场景")
123
124    for sim_car_msg, frame in api.main_loop():
125        if use_keyboard:
126            vehicle_control = get_vehicle_control_from_keyboard()
127        else:
128            vehicle_control = get_vehicle_control_from_algorithm(sim_car_msg)
129        api.set_vehicle_control(vehicle_control)
130        if USE_GUI:
131            dashboard.update(sim_car_msg)
132    logger.info("结束场景")
133    if USE_GUI:
134        dashboard.quit()
135        logger.info("关闭 GUI 界面")
136
137
138if __name__ == "__main__":
139    main()

这个例子展示了：

  1. 如何创建和连接 SceneAPI

  2. 如何使用键盘输入控制车辆

  3. 如何使用 Stanley 横向控制算法控制车辆

  4. 如何在主循环中不断获取车辆状态并发送控制命令

GUI 界面示例

MetaCar 库还包含了一个使用 GUI 界面展示车辆状态的示例。您可以在以下位置找到完整代码：

  • examples/gui.py - GUI 界面实现代码

这个示例展示了：

  1. 如何创建图形界面展示车辆状态

  2. 如何解析和显示车辆传感器数据

  3. 如何实时更新界面中显示的内容

高级开发技巧

在开发自己的应用程序时，以下是一些有用的高级技巧：

  1. 图像处理 - 使用 frame 图像数据实现计算机视觉算法，如车道线检测、交通标志识别等

  2. 路径规划 - 基于 static_data.route 和 sim_car_msg.trajectory 实现更复杂的路径规划

  3. 传感器融合 - 结合摄像头和其他传感器数据实现更准确的环境感知

  4. 状态机设计 - 为车辆控制创建状态机，处理不同场景下的行为转换

备注

关于基本用法和入门指南，请参阅 快速入门 文档。 本节示例侧重于展示更复杂的应用场景和实现技巧。