CARLA-Autoware 联合仿真

测试环境：CARLA0.9.7.4，Autoware1.12.0，ubuntu16.04LTS，ROS kinetic。

参考资料：

Autoware入门教程：https://www.ncnynl.com/archives/201910/3401.html

Autoware和LGSVL联调：https://www.jianshu.com/p/c4fa774892a9

Autoware培训笔记：https://www.cnblogs.com/hgl0417/p/11147109.html

YouTube视频：https://www.youtube.com/watch?v=Z7PLkcY9Mtk

bilibili视频1：https://www.bilibili.com/video/BV1LJ411E7hK?from=search&seid=12197307514393209183

bilibili视频2：https://www.bilibili.com/video/BV1GE411n7wp

CARLA安装

docker安装
编译包安装

Autoware1.12.0源码编译

官方文档：https://gitlab.com/autowarefoundation/autoware.ai/autoware/-/wikis/Source-Build

#依赖
$ sudo apt-get update
$ sudo apt-get install -y python-catkin-pkg python-rosdep ros-$ROS_DISTRO-catkin gksu
$ sudo apt-get install -y python3-pip python3-colcon-common-extensions python3-setuptools python3-vcstool
$ pip3 install -U setuptools
#创建工作空间
$ mkdir -p autoware.ai/src
$ cd autoware.ai
#下载库和依赖
$ wget -O autoware.ai.repos "https://gitlab.com/autowarefoundation/autoware.ai/autoware/raw/1.12.0/autoware.ai.repos?inline=false"
$ vcs import src < autoware.ai.repos
$ rosdep update
$ rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO
#编译（有CUDA支持）
$ AUTOWARE_COMPILE_WITH_CUDA=1 colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release
#编译（无CUDA支持）
$ colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release

安装完成以后跑ROSBAG demo
https://gitlab.com/autowarefoundation/autoware.ai/autoware/-/wikis/ROSBAG-Demo

CARLA-Autoware ROS Bridge

carla-autoware官方文档：https://github.com/carla-simulator/carla-autoware

1.setup

#下载carla-autoware
cd ~
git lfs clone https://github.com/carla-simulator/carla-autoware.git
cd carla-autoware
git submodule update --init
cd catkin_ws
source <path-to-autoware>/install/setup.bash
catkin_init_workspace src/

# install dependencies
rosdep update
rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO

#编译
catkin_make

2.配置bashrc环境变量

#配置bashrc
export ROS_DISTRO=kinetic
source /opt/ros/kinetic/setup.bash
source ~/autoware.ai/install/setup.bash

export CARLA_AUTOWARE_ROOT=~/carla-autoware/autoware_launch
export CARLA_MAPS_PATH=~/carla-autoware/autoware_data/maps
source $CARLA_AUTOWARE_ROOT/../catkin_ws/devel/setup.bash

export CARLA_ROOT=~/CARLA_0.9.7.4
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/ju/scenario_runner

3.启动CARLA

option1：docker启动

$ docker run -p 2000-2001:2000-2001 -it --rm carlasim/carla:<carla-version> ./CarlaUE4.sh

option2：编译包启动

$ cd ~/CARLA_0.9.7.4
$ ./CarlaUE4.sh

4.连接ROS Bridge

option1:先启动roscore，启动rviz，再运行devel.launch,可以不打开runtime manager界面，此时rviz中已经有地图了。

# 打开一个终端
$ roscore
# 再打开一个终端
$ rviz
# 再打开一个终端
$ cd carla-autoware/autoware_launch
$ roslaunch devel.launch

option2:启动runtime manager，再运行devel.launch。

# 打开一个终端,在runtime manager中打开rviz
$ roslaunch runtime_manager runtime_manager.launch
# 再打开一个终端，运行devel.launch
$ cd carla-autoware/autoware_launch
$ roslaunch devel.launch

备注：

1.使用Python2.7环境，运行devel.launch终端会报错，原因是devel.launch中也包括了manual control的启动。经试验，在Python2.7环境不连接ros bridge和连接ros bridge的报错一样，而manual control在Python3.5环境下运行不会报错，由于使用ROS，必须使用2.7的环境,故终端会出现报错的现象，但不影响。

2.devel.launch的结构。
第一大部分：carla_autoware_bridge_with_manual_control.launch
1)carla_manual_control.launch #手动控制车的pygame窗口
2)carla_autoware_bridge.launch
2.1) carla_ros_bridge.launch
2.2) carla_ego_vehicle.launch #生成主车，可以指定spawn point
2.3) carla_waypoint_publisher.launch #发布waypoint信息
2.4) carla_point_map_loader.launch #加载地图，已经tf转换
2.5) carla_autoware_bridge_common.launch
2.5.1)tf.launch
2.5.2)carla_ackermann_control.launch
第二大部分：autoware.launch
1)my_map.launch #空文件
2)my_sensing.launch
3)my_localization.launch
4)my_detection.launch
5)my_mission_planning.launch
6)my_motion_planning.launch

3.默认的devel.launch包括了以上内容，正常运行以后，town03下，会任意位置生成主车，终点固定，通过2.3）carla_waypoint_publisher发布waypoint信息，且waypoint只发布一次，这里发布的waypoint是自动生成计算得到的，因此每次循迹的路线都不一样，但是终点是固定的。

4.devel.launch循迹的最大速度是15km/h，测试在town03下会出现撞车的问题，可能是速度太大跟踪不上，在town04下稍微好点，偶尔也会出现撞车的情况，撞车以后，可以在manual control的界面下，按键盘B，切换为手动模式，手动控制一下，再切为自动模式。

5.launch文件是多节点的同时启动，默认的devel.launch可以实现自动驾驶循迹的例子，也可以将其中的某些节点注释点，只保留需要使用的，然后通过runtime manager中的模块来实现对应的功能，runtime manager中的模块实际上是对应节点的封装。

录制bag包回放跟踪

1.注释launch文件
保留下面的部分

第一大部分：carla_autoware_bridge_with_manual_control.launch
my_localization.launch

a)故原本的devel.launch现在只保留了bridge，manual control和localization部分，连接以后只实现了两边的通信，lidar数据实时传输，没有控制命令，不会进行自动驾驶的循迹。
b)可以在终端echo /point raw话题，也可以在runtime manager中echo。

2.录制bag

沿可视化路径，通过manual control控制车，录制/point raw话题的数据包。
manual control的pygame界面和CARLA服务端有一定的延迟，存在录制的时候，车辆不沿车道跑的现象，录制的时候在rviz中添加image的话题，该话题的图像没有延迟，另外，manual control不太好控制，要尽量控制速度，匀速低速行驶。

3.ndt_mapping建图

Autoware培训笔记1：https://www.cnblogs.com/hgl0417/p/11130747.html

回放bag，加载ndt_mapping来建立pcd图
经过多次试验，ndt_mapping建图的开始定位不太准确，很多次都失败了，可以录制一个稍微长一点的bag。之前试验town04下的一段弯道，只有后面小半截是正常的，前半截有重合错位。最后试验town04下一段直道，前面一段还是有重合错位，不过相对弯道好很多。
使用ndt_mapping.rviz的配置文件，打开rviz可视化建图过程
ndt_mapping.rviz获取：https://github.com/autowarefoundation/autoware/blob/1.11.0/ros/src/.config/rviz/ndt_mapping.rviz
建图过程中可以不打开rviz，直接等待建图结束，导出pcd，rviz会占用大量的系统资源。
建完图关闭rviz可视化建图，否则无法导出。
bag包已经回放完毕，但是ndt_mapping仍在计算，也无法导出地图，可能需要等较长时间完成计算，才能导出pcd图。

4.回放bag，生成csv文件

Autoware培训笔记3：https://www.cnblogs.com/hgl0417/p/11144203.html

加载生成的地图，回放bag，通过ndt_matching完成点云数据的匹配，生成路径点的csv文件
路径点的信息包括x,y,z,yaw,velocity,change_flag这六个信息。
这里tf转换的launch文件，是将devel.launch中的tf.launch文件的后面注释掉了，保留了前两句，但和上面Autoware培训笔记3中的效果是一样的，没区别。

5.回放bag，加载csv，跟踪

Autoware培训笔记4：https://www.cnblogs.com/hgl0417/p/11147109.html

通过waypoint loader模块加载csv文件，终端命令rostopic echo /based/lane_waypoint要有输出，否则错误。
由于ndt_mapping建图，前一段总是有些重合和错位，导致生成的csv前一段有些不正确，在town04的一段直道下测试，稍微好点，使用pure pursuit，前一段有些波动，后面基本能稳定跟踪，没有问题。

CARLA0.9.7.4生成400m路径，并导入txt，绘制路径

#!/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()
	
    #使用interpolate_trajectory方法，可以生成一段路径，并指定路径点的间隔，该方法有两个返回值，一个经纬度，一个transform格式。
    #该方法使用scenario_runner模块，需要制定起点和终点。
	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(400)[-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))

    #transform格式解析为（x,y,z,yaw,velocity,change_flag)
	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, waypoint.location.z, waypoint.rotation.yaw, 5.0, 0])
		# print(waypoints)
		return waypoints
	
	#print(_get_waypoints(trajectory))
	waypoints_list = _get_waypoints(trajectory)

    #导入txt文件
	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)
    
    #使用draw_point方法绘制路径
	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!")

路径跟踪联合仿真

路径跟踪联合仿真和回放bag跟踪本质上是有无反馈的区别，基于bag数据回放的跟踪是没有反馈的开环仿真，而联合仿真是有反馈的闭环仿真。

1.注释launch，只保留carla_autoware_bridge_with_manual_control.launch以及my_localization.launch这两个部分。

2.连接ROS Bridge，参照上述CARLA-Autoware ROS Bridge部分。

3.将CARLA0.9.7.4生成400m路径，并导入txt，绘制路径部分生成的txt文件转为csv文件。

4.输入csv路径，使用pure pursuit跟踪，参照上述回放bag，加载csv，跟踪部分。

问题：
1.代码生成的csv文件（也包括x,y,z,yaw,velocity,change_flag）加载会出现lane data is something wrong的错误，但是通过建图生成的csv文件加载正常。两者格式都是x,y,z,yaw,velocity,change_flag。

2.经过多次试验，单个数据复制粘贴到建图生成的csv文件，20×5个数据，发现加载成功，但是轨迹发生偏移超出车道线外。代码生成的路径点在导出的同时，也被绘制出来，是在正常车道线上的，在ROS Bridge连接以后，manual control的pygame界面显示的x，y位置也符合csv中的坐标，rviz中/image话题显示的路径也是正常车道线上的，但是点云中的这个20个点偏移到车道外了。

3.也考虑是否是yaw数值不对，最终排除，CARLA中yaw的值范围为-180到180。

4.代码生成的csv文件和录制bag建图生成的csv文件是同一段路径下的，但是两者的坐标相差甚远，因此判断存在tf转换的问题。代码生成的csv文件无法正确加载，建图生成的csv文件可以正常加载，但是加载出来的点不与相应路径重合，确实存在tf转换的问题，修改2.5) carla_autoware_bridge_common.launch中的tf.launch里的值，有变化，但是它们之间的转换规律和法则无法知晓，不可行。

5.通过复制粘贴数据来替换建图生成的csv文件，可以加载成功，但是工作量巨大。因为是400×5的数据量，通过单个粘贴复制的数据量太大，另外，通过单个粘贴复制20×5个数据，发现加载成功，但是轨迹发生偏移超出车道线外，这意味着还要对每个数据修改，使轨迹正常在车道上，这样做也不太可行。

6.只要这个路径正确加载在车道上，就可以实现路径跟踪的闭环联合仿真。