Carla 轨迹点生成

Wwwwinya

已于 2023-11-01 09:16:05 修改

阅读量481

点赞数

CC 4.0 BY-SA版权

文章标签： python 开发语言

于 2023-10-31 23:45:05 首次发布

本文链接：https://blog.csdn.net/Wwwwinya/article/details/134151850

个人备份用

import carla
import xmltodict

def get_vehicle_transforms(vehicle, distance=1.0, num_points=10):
    trajectory_points = []

    for i in range(num_points):
        location = vehicle.get_location()
        transform = vehicle.get_transform()
        forward_vector = transform.rotation.get_forward_vector()

        # 计算下一个点的位置
        next_point = location + distance * (i + 1) / num_points * forward_vector

        # 获取yaw角度
        yaw = transform.rotation.yaw

        trajectory_points.append((next_point.x, next_point.y, next_point.z, yaw))

    return trajectory_points

def save_trajectory_to_xml(trajectory_points, xml_filename):
    data = {'trajectory': {'point': []}}

    for point in trajectory_points:
        x, y, z, yaw = point
        data['trajectory']['point'].append({'x': x, 'y': y, 'z': z, 'yaw': yaw})

    with open(xml_filename, 'w') as xml_file:
        xml_file.write(xmltodict.unparse(data, pretty=True))

def draw_trajectory_points(world, trajectory_points):
    for point in trajectory_points:
        x, y, z, _ = point
        location = carla.Location(x, y, z)
        world.debug.draw_point(location, size=0.1, color=carla.Color(255, 0, 0), life_time=1.0)

if __name__ == "__main__":
    # 连接到Carla服务器
    client = carla.Client('localhost', 2000)
    client.set_timeout(10.0)

    # 获取世界
    world = client.get_world()

    # 获取车辆
    vehicle = world.get_actors().find(123)  # 替换为你车辆的ID

    try:
        while True:
            # 获取车辆轨迹点
            trajectory_points = get_vehicle_transforms(vehicle)

            # 保存轨迹点到XML文件
            xml_filename = 'carla_trajectory.xml'
            save_trajectory_to_xml(trajectory_points, xml_filename)

            # 在Carla中显示红色的轨迹点
            draw_trajectory_points(world, trajectory_points)

            print(f'Trajectory points saved to {xml_filename}')

            # 可以根据需要调整时间间隔
            carla.World.wait_for_tick(seconds=0.1)

    except KeyboardInterrupt:
        print("User interrupted the script.")
    finally:
        print("Script terminated.")

import carla
import xmltodict

def get_vehicle_transforms(vehicle, distance=1.0, num_points=10):
    trajectory_points = []

    for i in range(num_points):
        location = vehicle.get_location()
        transform = vehicle.get_transform()
        forward_vector = transform.rotation.get_forward_vector()

        # 计算下一个点的位置
        next_point = location + distance * (i + 1) / num_points * forward_vector

        # 获取yaw角度
        yaw = transform.rotation.yaw

        trajectory_points.append((next_point.x, next_point.y, next_point.z, yaw))

    return trajectory_points

def save_trajectory_to_xml(trajectory_points, xml_filename):
    data = {'trajectory': {'point': []}}

    for point in trajectory_points:
        x, y, z, yaw = point
        data['trajectory']['point'].append({'x': x, 'y': y, 'z': z, 'yaw': yaw})

    with open(xml_filename, 'w') as xml_file:
        xml_file.write(xmltodict.unparse(data, pretty=True))

if __name__ == "__main__":
    # 连接到Carla服务器
    client = carla.Client('localhost', 2000)
    client.set_timeout(10.0)

    # 获取世界
    world = client.get_world()

    # 获取车辆
    vehicle = world.get_actors().find(123)  # 替换为你车辆的ID

    try:
        while True:
            # 获取车辆轨迹点
            trajectory_points = get_vehicle_transforms(vehicle)

            # 保存轨迹点到XML文件
            xml_filename = 'carla_trajectory.xml'
            save_trajectory_to_xml(trajectory_points, xml_filename)

            print(f'Trajectory points saved to {xml_filename}')

            # 可以根据需要调整时间间隔
            carla.World.wait_for_tick(seconds=0.1)

    except KeyboardInterrupt:
        print("User interrupted the script.")
    finally:
        print("Script terminated.")

https://www.starflow.tech/