Record the CARLA generation path and store the waypoint information as a txt file

CARLA 0.9.7, generate a 40-meter path under town04, get a waypoint (x, y, desire_speed) every one meter and write it into a txt file. And draw the path, visualize it.

#!/usr/bin/python
# -*- coding: utf-8 -*

import glob
import os
import sys

try:
    sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % (
        sys.version_info.major,
        sys.version_info.minor,
        'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
except IndexError:
    pass


import carla
import scenario_runner	
#Import the library Transform used to explicitly spawn an actor
from carla import Transform, Location, Rotation
from carla import Map
from carla import Vector3D
from srunner.challenge.utils.route_manipulation import interpolate_trajectory

import random
import time

actor_list = []

try:


	##General connection to server and get blue_print
	client = carla.Client('localhost',2000)
	client.set_timeout(5.0)

	world = client.get_world()
	world = client.load_world('Town04') #change maps
	mp = world.get_map()#get the map of the current world.
	blueprint_library = world.get_blueprint_library()
	
	transform = carla.Transform(carla.Location(x=-65.4, y=4.0, z=11), carla.Rotation(pitch=0,yaw=180,roll=0))
	map = world.get_map()
	start_waypoint = map.get_waypoint(transform.location)
	end_waypoint = start_waypoint.next(40)[-1]
	waypoints = [start_waypoint.transform.location, end_waypoint.transform.location]
	gps_route, trajectory = interpolate_trajectory(world,waypoints,1.0)
	#print(gps_route)
	#print(len(gps_route))
	#print("**************************************")
	#print(trajectory)
	#print(len(trajectory))

	def _get_waypoints(trajectory):
		waypoints = []
		for index in range(len(trajectory)):
			waypoint = trajectory[index][0]
			# print(waypoint)
			# waypoints.append([waypoint['lat'], waypoint['lon'], waypoint['z']])
			waypoints.append([waypoint.location.x, waypoint.location.y, 10.0])
		# print(waypoints)
		return waypoints
	
	#print(_get_waypoints(trajectory))
	waypoints_list = _get_waypoints(trajectory)

	def text_save(filename, data):#filename为写入CSV文件的路径，data为要写入数据列表.
		file = open(filename,'a')
		for i in range(len(data)):
			s = str(data[i]).replace('[','').replace(']','')#去除[],这两行按数据不同，可以选择
			s = s.replace("'",'').replace(',','') +'\n'   #去除单引号，逗号，每行末尾追加换行符
			file.write(s)
		file.close()
		print("保存文件成功") 
	
	filename = '/home/juling/Desktop/waypoint.txt'
	text_save(filename, waypoints_list)

	def draw_trajectory(trajectory, persistency=0, vertical_shift=0):
		for index in range(len(trajectory)):
			waypoint = trajectory[index][0]
			location = waypoint.location + carla.Location(z=vertical_shift)
			world.debug.draw_point(location, size=0.1, color=carla.Color(255, 255, 0), life_time=persistency)
	
	draw_trajectory(trajectory)

finally:
	for actor in actor_list:
		actor.destroy()
	print("All cleaned up!")

1. Using the interpolate_trajectorymethod, you can generate a path and specify the interval of the path points. This method has two return values, one latitude and longitude, and one world coordinate.

The above figure shows two printed return values, latitude and longitude and world coordinates.

The method can be seen in the route_manipulation.py file
. The start point and end point need to be specified.

from srunner.challenge.utils.route_manipulation import interpolate_trajectory

Scenario_runner needs a git clone, and also needs to configure bashrc, otherwise there will be an error that the scenario_runner module cannot be imported.

# bashrc配置
CARLA_ROOT=/home/juling/CARLA_0.9.7
export CARLA_SERVER=${
    
    CARLA_ROOT}/CarlaUE4.sh
export ROOT_SCENARIO_RUNNER=/home/juling/CARLA_0.9.7/PythonAPI/my/scenario_runner
export PYTHONPATH=$PYTHONPATH:${
    
    CARLA_ROOT}/PythonAPI/carla/dist/carla-0.9.7-py2.7-linux-x86_64.egg:${
    
    CARLA_ROOT}/PythonAPI/carla/agents:${
    
    CARLA_ROOT}/PythonAPI/carla/:${
    
    CARLA_ROOT}/PythonAPI/my/scenario_runner

2. Define the _get_waypoints() method. The input is the second return value of the interpolate_trajectory() method. This method can convert the world coordinates into the form of x, y and desire_spped. The return value of the method is a list of waypoints, each waypoint includes x, y, desire_speed. Speed ​​customization.

3. Write a txt file, and finally generate waypoint.txt on the desktop

4. Visualization of the drawing path.
You can see the debug instance variables of the World class in the official API documentation and the draw_point method in DebugHelper. https://carla.readthedocs.io/en/latest/python_api/#carlaworld
https://carla.readthedocs.io/en/latest/python_api/#carla.DebugHelper

generates 400-meter path results under town04, the starting point is x=233, y=-365.5, z=11
The location of a specific point on the map is the Location (only x and y) on the visual interface using manual control.