示例代码

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

基础控制示例

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

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

这个例子展示了：

  1. 如何创建和连接 SceneAPI

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

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

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

GUI 界面示例

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

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

这个示例展示了：

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

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

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

开发您的第一个应用

开发自己的应用程序时，建议按照以下流程进行：

  1. 引入所需的模块和类

  2. 初始化并连接 SceneAPI

  3. 设计车辆控制算法

  4. 在主循环中实时处理数据并发送控制指令

代码框架示例：

from metacar import SceneAPI, VehicleControl, GearMode
import logging

# 配置日志系统
logging.basicConfig(filename="myapp.log", level=logging.INFO)
logger = logging.getLogger(__name__)

def main():
    # 初始化 API
    api = SceneAPI()

    # 建立仿真连接
    api.connect()
    logger.info("成功连接仿真环境")

    # 获取场景静态信息
    static_data = api.get_scene_static_data()

    # 启动主循环
    for sim_car_msg, frame in api.main_loop():
        # 在此处实现控制算法
        vc = VehicleControl()

        # 向仿真环境发送控制指令
        api.set_vehicle_control(vc)

    logger.info("仿真任务完成")