Autoware.universe部署03：与Carla（二进制版）联调

本文介绍了 Autoware.universe 与 Carla 二进制版的联调过程。Autoware.universe 是一个开源的自动驾驶软件平台，而 Carla 则是一个用于自动驾驶仿真的开源工具。将这两者联调起来可以在仿真环境中测试和验证 Autoware.universe 的功能和性能，为自动驾驶系统的开发和测试提供有力支持。
其中Autoware.universe的安装以及高精地图的绘制参考我的其他文章：
Autoware.universe部署01：Ubuntu20.04安装Autoware.universe并与Awsim联调
Autoware.universe部署02：高精Lanelet2地图的绘制

一、Carla的使用

1.1 二进制Carla的安装

二进制包github地址：https://github.com/carla-simulator/carla/releases，下载第一个.tar.gz压缩包：

解压后可以直接运行，出现如下的服务器界面：

tar -zxvf CARLA_0.9.13.tar.gz
cd CARLA_0.9.13
./CarlaUE4.sh

去/home/autoware/CARLA_0.9.13/PythonAPI/examples下运行API：

python manual_control.py

即可开启Carla仿真驾驶，界面如下：

命令行选项：启动Carla时有一些可用的配置选项，它们可以按如下方式使用：

./CarlaUE4.sh -quality-level=Low

启动 CARLA 时，有一些配置选项可用：

• carla-rpc-port=N：侦听端口 N 处的客户端连接。默认情况下，流式端口 Streaming port 设置为 N+1；
• carla-streaming-port=N：指定用于传感器数据流的端口。 使用 0 获取随机未使用的端口。 第二个端口将自动设置为 N + 1；
• quality-level={Low,Epic}：更改图形质量级别；
• -carla-server：让 carla以服务的方式运行；
• -benchmark -fps=15：引擎以1/15秒的固定时间逐步运行；

1.2 Carla API的使用

这里介绍常用的几个API，首先打开carla：./CarlaUE4.sh

1.2.1 增加行人流和车流

执行CARLA_0.9.13/PythonAPI/examples路径下的generate_traffic.py文件：

python generate_traffic.py -w 10 -n 10

可以看到，carla服务器中的街道上多了很多运动的车辆和行人。后面的参数 -n 10 -w 10分别代表车辆和行人的数量，这个脚本还有很多其他的参数：

• --host #主机服务器ip
• -p --port #端口
• -n --number-of-vehicles #车辆数量
• -w --number-of-walkers #行人数量
• --safe #避免碰撞
• --filterv #车辆滤波器
• --filterw #行人滤波器
• --generationv #限制某些车辆生成
• --generationw #限制某些行人生成

1.2.3 改变服务器的时间，光照，天气

执行CARLA_0.9.13/PythonAPI/examples路径下的dynamic_weather.py文件

python dynamic_weather.py -s 5

参数-s 5意思是把天气变化加速了5倍。这个脚本里是按照仿真时间，修改服务器中设置天气的参数carla.WeatherParameters，按照一定的顺序自动的调整系统的光照和天气。光照条件包括：太阳高度和太阳角度；天气条件包括：云，雨，水坑，风，雾，湿度。

更多天气参数的设置可以查看/pythonAPI/python_api.md中的carla.WeatherParameters.

1.2.3 手动控制车辆

执行CARLA_0.9.13/PythonAPI/examples路径下的manual_control.py文件：

python manual_control.py

键盘B按键可以切换自动控制和人工控制，pygame操作手册如下：

1.1.4 车辆的自动驾驶

执行CARLA_0.9.13/PythonAPI/examples路径下的automatic_control.py文件

python automatic_control.py 

会弹出一个pygame窗口，车辆正在进行自动驾驶

到达终点后，自动关闭

二、Universe与Carla联调

参考文章：https://blog.csdn.net/yuteng12138/article/details/130102293

2.1 设置OpenPlanner

（1）下载拓展代码

cd ~/autoware_universe
mkdir op_carla && cd op_carla/
git clone https://github.com/hatem-darweesh/op_bridge.git -b ros2
git clone https://github.com/hatem-darweesh/op_agent.git -b ros2
git clone https://github.com/hatem-darweesh/scenario_runner.git

（2）从https://drive.google.com/drive/folders/1Or0CMS08AW8XvJtzzR8TfhqdY9MMUBpS下载Town01.pcd 和 Town01.osm地图。创建 Town01 文件夹，将 Town01.pcd 和 Town01.osm 复制到Town01_map 文件夹中。

• Town01.pcd重命名为pointcloud_map.pcd
• Town01.osm重命名为lanelet2_map.osm

其他地图可以在这里下载

git clone  https://bitbucket.org/carla-simulator/autoware-contents/src/master.git

（3）修改文件：op_carla/op_bridge/op_scripts/run_exploration_mode_ros2.sh，在第8行，如果不在同一台电脑上运行仿真，需要设置运行仿真的电脑的 IP 地址。如下：

export SIMULATOR_LOCAL_HOST=“192.168.11.5”

如果在本机上运行仿真需要修改为：

export SIMULATOR_LOCAL_HOST=“localhost”

（4）修改文件：op_carla/op_agent/start_ros2.sh，在第15行，按照实际的安装路径修改。

source ${YouPath}/autoware_universe/autoware/install/setup.bash

在第18行，按照实际的安装路径修改启动Autoware的launch文件路径，按照Town01的路径修改map_path。

ros2 launch \
  ${YouPath}/src/launcher/autoware_launch/autoware_launch/launch/autoware.launch.xml \
  map_path:=${YouPath}/autoware_universe/autoware/src/${map_name} \
  vehicle_model:=sample_vehicle \
  sensor_model:=sample_sensor_kit

（5）修改文件: src/launcher/autoware_launch/autoware_launch/launch/autoware.launch.xml
在第11行添加如下，用于将Carla点云转换为Autoware.universe使用的点云：

<arg name="launch_carla_interface" default="true" description="convert carla sensor data to autoware suitable format"/>

在第17行将launch_sensing_driver中的true改成false，关闭自带的传感器驱动，可以直接复制以下替换第17行。

<arg name="launch_sensing_driver" default="false" description="launch sensing driver"/>

在第23行将use_sim_time中的false改成true，使用仿真时间，可以直接复制以下替换第23行。

<arg name="use_sim_time" default="true" description="use_sim_time"/>

注意：这里改了之后，如果再使用非仿真测试时要改回来，否则将不能定位。另外也可以不改参数文件，在launch时添加参数，例如：

ros2 launch \
  ${YouPath}/src/launcher/autoware_launch/autoware_launch/launch/autoware.launch.xml \
  map_path:=${YouPath}/${map_name} \
  vehicle_model:=sample_vehicle \
  sensor_model:=sample_sensor_kit \
  use_sim_time:=true

在第52行添加如下，将Carla点云转化为Universe可用的点云：

  <!-- CARLA -->
  <group if="$(var launch_carla_interface)">
    <node pkg="carla_pointcloud" exec="carla_pointcloud_node" name="carla_pointcloud_interface" output="screen"/>
  </group>

（6）修改文件: src/sensor_kit/sample_sensor_kit_launch/sample_sensor_kit_launch/launch/gnss.launch.xml，在第4行将coordinate_system中的1改成2，坐标系使用MGRS，可以直接复制以下替换第4行。

<arg name="coordinate_system" default="2" description="0:UTM, 1:MGRS, 2:PLANE"/>

在第5行添加如下：

<arg name="plane_zone" default="0"/>

（7）修改文件：src/sensor_kit/sample_sensor_kit_launch/sample_sensor_kit_description/config/sensor_kit_calibration.yaml，由于修改较多，可以直接全部替换：

sensor_kit_base_link:
  camera0/camera_link:
    x: 0.7
    y: 0.0
    z: 0.0
    roll: 0.0
    pitch: 0.0
    yaw: 0.0
  camera1/camera_link:
    x: 0.0
    y: 0.0
    z: 0.0
    roll: 0.0
    pitch: 0.0
    yaw: 0.0
  camera2/camera_link:
    x: 0.0
    y: 0.0
    z: 0.0
    roll: 0.0
    pitch: 0.0
    yaw: 0.0
  camera3/camera_link:
    x: 0.0
    y: 0.0
    z: 0.0
    roll: 0.0
    pitch: 0.0
    yaw: 0.0
  camera4/camera_link:
    x: 0.0
    y: 0.0
    z: 0.0
    roll: 0.0
    pitch: 0.0
    yaw: 0.0
  camera5/camera_link:
    x: 0.0
    y: 0.0
    z: 0.0
    roll: 0.0
    pitch: 0.0
    yaw: 0.0
  traffic_light_right_camera/camera_link:
    x: 0.0
    y: 0.0
    z: 0.0
    roll: 0.0
    pitch: 0.0
    yaw: 0.0
  traffic_light_left_camera/camera_link:
    x: 0.0
    y: 0.0
    z: 0.0
    roll: 0.0
    pitch: 0.0
    yaw: 0.0
  velodyne_top_base_link:
    x: 0.0
    y: 0.0
    z: 0.8
    roll: 0.0
    pitch: 0.0
    yaw: 0.0
  velodyne_left_base_link:
    x: -0.5
    y: 0.0
    z: 0.8
    roll: 0.0
    pitch: 0.0
    yaw: 0.0
  velodyne_right_base_link:
    x: 0.5
    y: 0.0
    z: 0.8
    roll: 0.0
    pitch: 0.0
    yaw: 0.0
  gnss_link:
    x: 0.0
    y: 0.0
    z: 0.8
    roll: 0.0
    pitch: 0.0
    yaw: 0.0
  tamagawa/imu_link:
    x: 0.0
    y: 0.0
    z: 0.8
    roll: 0.0
    pitch: 0.0
    yaw: 0.0

（8）修改文件：src/sensor_kit/sample_sensor_kit_launch/sample_sensor_kit_description/config/sensors_calibration.yaml，由于修改较多，可以直接全部替换：

base_link:
  sensor_kit_base_link:
    x: 0.0
    y: 0.0
    z: 1.6
    roll: 0.0
    pitch: 0.0
    yaw: 0.0
  velodyne_rear_base_link:
    x: 0.0
    y: 0.0
    z: 0.0
    roll: 0.0
    pitch: 0.0
    yaw: 0.0

（9）修改环境变量

gedit ~/.bashrc

根据安装位置修改路径后，再添加到环境变量中。

export CARLA_ROOT=/yourpath/carla
export SCENARIO_RUNNER_ROOT=/home/username/autoware_universe/op_carla/scenario_runner
export LEADERBOARD_ROOT=/home/username/autoware_universe/op_carla/op_bridge
export TEAM_CODE_ROOT=/home/username/autoware_universe/op_carla/op_agent
export PYTHONPATH=$PYTHONPATH:${CARLA_ROOT}/PythonAPI
export PYTHONPATH=$PYTHONPATH:${CARLA_ROOT}/PythonAPI/util
export PYTHONPATH=$PYTHONPATH:${CARLA_ROOT}/PythonAPI/carla
export PYTHONPATH=$PYTHONPATH:${CARLA_ROOT}/PythonAPI/carla/agents
# 注意下面的文件名称要对，可以去Carla文件夹看看(我的是，3.7)
export PYTHONPATH=$PYTHONPATH:${CARLA_ROOT}/PythonAPI/carla/dist/carla-0.9.13-py3.7-linux-x86_64.egg

2.2 重新编译并运行

把上面改动的几个包重新编译（ROS2需要把所有文件复制到install，因此包里任何文件修改均需重新编译）一下：

colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select autoware_launch sample_sensor_kit_description

安装依赖：

pip install py-trees networkx tabulate transforms3d
sudo apt-get install ros-galactic-sensor-msgs-py

运行Carla服务器：

cd $CARLA_ROOT
./CarlaUE4.sh -quality-level=Epic -world-port=2000 -resx=800 -resy=600

再打开一个终端运行Universe，同时启动Carla ros2 bridge：

cd ${YouPath}/autoware_universe/op_carla/op_bridge/op_scripts
source ~/autoware_universe/install/setup.bash
./run_exploration_mode_ros2.sh

成功加载地图

自动驾驶测试：

三、更换地图（Town10HD服务器默认地图）调试

（1）首先创建 Town10HD文件夹，将 Town10HD.pcd 和 Town10HD.osm 复制到文件夹中。

• Town10HD.pcd重命名为pointcloud_map.pcd
• Town10HD.osm重命名为lanelet2_map.osm

（2）修改 run_exploration_mode_ros2.sh 文件

sudo gedit op_carla/op_bridge/op_scripts/run_exploration_mode_ros2.sh

在第25行，更改地图为Town10HD。如下：

export FREE_MAP_NAME="Town10HD" 

（3）修改 start_ros2.sh 文件

sudo gedit op_carla/op_agent/start_ros2.sh

在第18行，按照实际的安装路径修改启动Autoware的launch文件路径，按照Town10HD的路径修改map_path。

ros2 launch ${You_path}/src/launcher/autoware_launch/autoware_launch/launch/autoware.launch.xml map_path:=${map_path_name} vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit

再次运行就可以加载Town10HD地图（服务器默认的地图）:

（4）测试Open-planner功能

四、测试其他功能

4.1 测试倒车入库功能

由于之前下载的高精地图中只含有车道元素，因此我们按照05的方法绘制停车场与停车位，之后测试

但是无法倒车，查看源码之后发现Carla bridge并未写关于倒车的代码，无法倒车入库，因此需要修改op_ros2_agent.py文件的速度控制函数：

 def on_autoware_universe_vehicle_control(self, data):
    # print(' $$$$$$$$$$$$$ >>>> Steering Angle: ', data.lateral.steering_tire_angle)
    cmd = carla.VehicleControl()  
	cmd.steer = (-data.lateral.steering_tire_angle / self.max_steer_angle)*self.steering_factor
	# 源码没有考虑倒车
    # speed_diff = data.longitudinal.speed - self.speed 
    # if speed_diff > 0:            
    #     cmd.throttle = 0.75           
    #     cmd.brake = 0.0  
    # elif speed_diff < 0.0:
    #     cmd.throttle = 0.0
    #     if data.longitudinal.speed <= 0.0 :                
    #         cmd.brake = 0.75
    #     elif  speed_diff > -1:
    #         cmd.brake = 0.0
    #     else :
    #         cmd.brake = 0.01

    if data.longitudinal.speed >= 0:
    	cmd.reverse = 0
        cmd.gear = 1
    else:
        cmd.reverse = 1
        cmd.gear = -1

    if data.longitudinal.acceleration > 0:            
        cmd.throttle = 0.4        
        cmd.brake = 0.0
    elif data.longitudinal.acceleration <= 0.0:
        cmd.throttle = 0.0
        if(- data.longitudinal.acceleration > 1.0):
            cmd.brake = 1.0
        else:
            cmd.brake = - data.longitudinal.acceleration//1.5


    # cmd.steer = -data.lateral.steering_tire_rotation_angle 
    self.current_control = cmd
    self.step_mode_possible = True

之后就可以正常倒车（但是轨迹跟踪不太好，估计是车身参数导致的，后面可以优化）

另外，我们发现由于雷达安装位置的不同，导致车体影响了点云，因此在Carla（src/universe/external/open_planner/op_carla_bridge/carla_pointcloud/src/carla_pointcloud/carla_pointcloud_interface_node.cpp）中添加以下代码，用于剔除雷达一定范围内的点云：

    else
    {
        // std::cout << "CARLA CARLA >>>> Received Cloud : " << scanMsg << ", Converted: " << std::endl;
        // 过滤掉距离传感器较近的点
        pcl::PointCloud<pcl::PointXYZI>::Ptr xyz_cloud(new pcl::PointCloud<pcl::PointXYZI>);
        pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_output(new pcl::PointCloud<pcl::PointXYZI>);
        pcl::fromROSMsg(*scanMsg, *xyz_cloud);
        for (size_t i = 0; i < xyz_cloud->points.size(); ++i)
        {
            if (sqrt(xyz_cloud->points[i].x * xyz_cloud->points[i].x + xyz_cloud->points[i].y * xyz_cloud->points[i].y +
                    xyz_cloud->points[i].z * xyz_cloud->points[i].z) >= 3.0 && !isnan(xyz_cloud->points[i].z))
            {
                pcl_output->points.push_back(xyz_cloud->points.at(i));
            }
        }
        sensor_msgs::msg::PointCloud2 output;
        pcl::toROSMsg(*pcl_output, output);
        output.header = scanMsg->header;
        
        sensor_msgs::msg::PointCloud2 transformed_cloud;
        if (pcl_ros::transformPointCloud(tf_output_frame_, output, transformed_cloud, *tf_buffer_))
        {

            transformed_cloud.header.stamp = scanMsg->header.stamp;
            velodyne_points_localization->publish(transformed_cloud);
            velodyne_points_perception->publish(transformed_cloud);
        }
    }