Apollo代码学习—Windows下编译Apollo并与Carsim和Simulink联调
郑重声明
此篇文章可分享，但严禁转载。原创不易，谢谢配合。
感谢开发组的杨乐及张新会前辈对此仿真的支持与帮助。
前言
Apollo的仿真主要在Linux下进行，但由于Apollo代码主要是C++代码，按理说在配置合理的情况下，Linux和Windows下均可运行，因此在同事的协助下，在Windows平台下基于Carsim和Simulink搭建了仿真平台。此仿真是为了验证Apollo的控制模块，由于横向控制所需的部分参数未在Carsim中找到，因此未验证横向控制以及MPC，仅对纵向控制做了验证。然后基于自己的理解，写了横向PID控制，最后进行了泊车的仿真。
如果有人根据我的文章搭建仿真， 并验证了横向控制及MPC，欢迎讨论交流。
准备
所需工具
- Windows平台（本人使用的是Windws 7， 32位）
- Carsim软件（本人使用的版本为8.02， 32位）
- 支持Simulink的Matlab软件（本人使用的版本为2014a， 32位）
- Apollo源代码
- Python 2.x 或 3.x
- C++编译器（本人使用的Visual Studio 2013）
安装说明
- 不建议Matlab的版本过高，因为低版本的Carsim不支持与高版本的Simulink联调，Carsim8.02最高支持到2014版本的Matlab；
- Apollo代码中的python代码是2.x版本的，如果电脑中已安装python3.x，只需在命令行窗口中运行python3.x安装路径下Tools\scripts\中（如：D:\Program Files\Python3.5\Tools\scripts）的
2to3.py文件即可将2.x转换为3.x标准的代码。python 2to3.py xxxx(需要转换的python文件所在路径)
源码编译
如果要直接使用Apollo源代码，首先需要在Windows下对其进行编译，但需要根据编译提示准备很多依赖库，并预定义一些变量，此外还要对部分文件进行修改，因为Linux平台下的C++语法和Windows平台下稍有差异。这个工作量挺大，放在最后讲。
Carsim和Simulink的设置
Carsim和Simulink的设置可参考龚建伟, 姜岩, 徐威. 无人驾驶车辆模型预测控制[M]. 北京理工大学出版社, 2014.书中的4.3章节和附录A的内容，里面有较为详细的教程，部分细节设置部分请参考下面的教程。
Carsim模型
Carsim设置界面如下：
Carsim内置了多种车辆模型，可根据所需进行选择。但在使用时有两点建议：
-
不建议在默认车模型上进行修改，因为车模型的搭建是经过严谨且详尽的计算和测量才实现的，在不清楚改动哪些参数是合理的情况下，尽量不要修改模型里的默认参数，乱改可能车模型就出问题了。因此，在搭建车模型过程中，最可靠的方法是，基于原有模型新建一个模型，以便参数设置出错时可以找回原始参数。
-
请不要随意点击上面工具栏中的，即Delete按钮，会导致删除配置文件！！！！！
请不要随意点击上面工具栏中的，即Delete按钮，会导致删除配置文件！！！！！
请不要随意点击上面工具栏中的，即Delete按钮，会导致删除配置文件！！！！！
这也是为什么建议变更设置前新建模型的原因。
本文以搭建倒车场景为例讲解如何搭建Carsim模型，具体操作如下：
车模型设置
STEP 1：
按如下所示选择车辆模型，其他参数自行理解。所有默认模型初始都是锁定状态的，无法更改，在调整前请先解锁，点击右上角的锁型按钮即可解锁。调整完可再次点击锁定，避免误操作。
STEP 2：
点击“B-Class， Hatchback”进入设置界面，点击“New”按钮，新建车模型，返回车辆选择界面，会发现列表中多了B-Class， Hatchback #1、B-Class， Hatchback #2、B-Class， Hatchback #3等模型，选择其中一个，开始搭建所需场景。
STEP 3：
配置车辆模型，主要需要修改的就是驱动模式，我选择的为前驱模式：Front-wheel drive，还有参考点的设置。其余的像轮胎、车身基本参数等保持默认参数即可，可以自行研究参数都是干嘛用的。
参考点的设置是为了后续和Simulink联调时获取车辆位置，一共设置了5个点，分别为左前轮、右前轮、左后轮、右后轮，以及后轴中心。或者根据自己需要自行设置参考点，参考点的设置可以参考文档，通过点击右上方的“Help”（每个设置界面都有对应的“Help”，里面有对该设置界面的详细介绍）或者顶部工具栏中的“Help”（所有的官方参考文档都可以在这里找到）获取参考文档。
场景定义
场景定义包括速度控制、刹车控制、档位控制、方向盘控制等，这些是车辆控制所必需的，也包括路面、障碍物、传感器等的设置。此处所有的设置，和最终与Simulink联调时的输入、输出有关，可根据需要自行研究说明文档进行设置。
控制量定义
STEP 4：
速度控制、刹车控制、档位控制、方向盘控制等可按照我的设置进行设置，如图6所示，速度控制中有油门控制，但我没有控制油门。
路面定义
STEP 5：
按照图8，选择3D road→Flat and Long，进入设置界面，主要关注红框区域，点击“1200m Road + 200m Light Grass”进行路面参数设置，Update Shapes用于设置完毕后更新路面参数。参数设置见图9。“View with Animator”用于显示设置完的路面状况，如图9所示。
路面参数设置主要关注图9中红框部分，用于设置道路宽度，以道路中心线（白色虚线）为参考线，左右各偏移5m为单侧道路宽度，5m到6m间为白实线到马路边沿，100m为道路边草坪的宽度，可以随意设置。其他几个参数请自行查阅说明文档（点击界面右上角“？”）进行研究。参数设置完，点击Update Shapes更新道路，点击View with Animator查看道路演示画面。
障碍物设置
因为我要验证的是控制模块，因此，感知模块由Carsim提供，同时，我要模拟停车场景，因此在场景中添加几辆不动的车辆来模拟车位，为的是便于感知车位大小和车位位置。如图10红框所示为添加障碍物车。
STEP 6：
三辆障碍物车的设置如图11所示，重要的是红框中障碍物坐标的定义。此处展示平行停车参数，垂直或者斜列停车参数可自行研究。
其中，
Lateral position为障碍物车中心线距离被控车中心线距离，单位为m；
Yaw angle offset为障碍物车起始角度，单位为rad，例如，希望障碍物车起始角度为30度，则设置为30/DR，DR为系统提供的用于角度-弧度转换的量；
S inital为车辆前轴中心横坐标，单位为m；
Length or diam为车辆轴距，不含前悬后悬，前悬后悬长度需要自己测定;
测定方法:
- 添加两辆车，设置一辆车初始角度为0度，另一辆为180度，调整两辆车间距离（通过调节S inital实现），使两辆车相接，即可测得后悬长度；
- 将两辆车初始角度都设置为0度，调整两辆车间距离（通过调节S inital实现），使两辆车首尾相接，即可测得前悬长度；
- 测定过程中，可以以0.1m为间隔调整车辆位置。
Width为车宽，但仅仅左侧轮胎外侧到右侧轮胎外侧距离，真实车宽需要自行测定，可按照上述测定前悬后悬长度的方法进行测定，只不过需要调节Lateral position而不是S inital，使两车外侧相接，注意后视镜的位置，整车宽度应包含后视镜宽度。
但请保持此处的Length or diam和Width不变，因为Carsim计算车辆坐标时基于此处参数计算的，如果此处改变了可能会影响后续使用。建议计算完前悬后悬、真实车宽后，以偏移量的形式写入matlab程序中，在Simulink仿真过程中将Carsim传输过来的数据进行加减操作，这点会在后文提到。
红框中参数设置如下：
// 图11上方红框两障碍物参数
DEFINE_OUTPUT XTL1 = S_OBJ_1-LENGTH_OBJ(1)
DEFINE_OUTPUT XTR1 = S_OBJ_1
DEFINE_OUTPUT XBL1 = S_OBJ_1-LENGTH_OBJ(1)
DEFINE_OUTPUT XBR1 = S_OBJ_1
DEFINE_OUTPUT YTL1 = LatO_1+WIDTH_OBJ(1)/2
DEFINE_OUTPUT YTR1 = LatO_1+WIDTH_OBJ(1)/2
DEFINE_OUTPUT YBL1 = LatO_1-WIDTH_OBJ(1)/2
DEFINE_OUTPUT YBR1 = LatO_1-WIDTH_OBJ(1)/2
DEFINE_OUTPUT XTL2 = S_OBJ_2-LENGTH_OBJ(2)
DEFINE_OUTPUT XTR2 = S_OBJ_2
DEFINE_OUTPUT XBL2 = S_OBJ_2-LENGTH_OBJ(2)
DEFINE_OUTPUT XBR2 = S_OBJ_2
DEFINE_OUTPUT YTL2 = LatO_2+WIDTH_OBJ(2)/2
DEFINE_OUTPUT YTR2 = LatO_2+WIDTH_OBJ(2)/2
DEFINE_OUTPUT YBL2 = LatO_2-WIDTH_OBJ(2)/2
DEFINE_OUTPUT YBR2 = LatO_2-WIDTH_OBJ(2)/2
EXP_XTL1
EXP_YTL1
EXP_XTR1
EXP_YTR1
EXP_XBL1
EXP_YBL1
EXP_XBR1
EXP_YBR1
EXP_XTL2
EXP_YTL2
EXP_XTR2
EXP_YTR2
EXP_XBL2
EXP_YBL2
EXP_XBR2
EXP_YBR2
// 图11下方红框一障碍物参数
DEFINE_OUTPUT XTL3 = S_OBJ_3-LENGTH_OBJ(3)
DEFINE_OUTPUT XTR3 = S_OBJ_3
DEFINE_OUTPUT XBL3 = S_OBJ_3-LENGTH_OBJ(3)
DEFINE_OUTPUT XBR3 = S_OBJ_3
DEFINE_OUTPUT YTL3 = LatO_3+WIDTH_OBJ(3)/2
DEFINE_OUTPUT YTR3 = LatO_3+WIDTH_OBJ(3)/2
DEFINE_OUTPUT YBL3 = LatO_3-WIDTH_OBJ(3)/2
DEFINE_OUTPUT YBR3 = LatO_3-WIDTH_OBJ(3)/2
EXP_XTL3
EXP_YTL3
EXP_XTR3
EXP_YTR3
EXP_XBL3
EXP_YBL3
EXP_XBR3
EXP_YBR3
设置完后，被控车（1号车）以及障碍物（2、3、4号车）的相对坐标位置如图12所示，u1~u34为各车坐标，由Carsim传输至Simulink进行应用。
自定义变量请参考如下文档：
至此，基本场景定义完成。可返回主界面查看整体模型效果。
STEP 7：
点击Home返回主界面，Run Control选择No linked library，然后点击Run Math Model，加载完后点击Animate，观看刚才所定义的场景，如图15所示。
Carsim输入输出设置
Carsim与Simulink联调，肯定有输入输出，Carsim输出参数给Simulink，并接收从Simulink返回的参数输入到Carsim中，用于连续仿真。
STEP 7：
将Run Control由No linked library变更为Models: Simulink，如图1所示。解决方案可选Parking或者新建解决方案，新建解决方案如图3所示。
Parking设置界面如图16所示：
set time step可设置，可不设置；
Input Channels为Carsim的输入，由Simulink传输过来；
Output Channels为Carsim的输出，输出给Simulink；
Simulnk Model为Carsim仿真所需的Simulink模型，一般为.mdl文件，当电脑系统为64为系统时，For 64-bit Windows OS会有两个选项，请选择“Use 64-bit Matlab”，32位系统只有一个选项“Use 32-bit Matlab”，无需更改。
点击Refresh，刷新界面，选择输入变量。所有的输入变量的定义可通过点击框2的 View Spreadsheet查看输入变量的定义文档。框3设置为按模块查找变量，双击框4中的变量即会添加到框5中，右键点击框4中的变量，会显示该变量的详细定义，双击框5中的序号即可删除某一变量。
和输入变量的选择类似，在Output参数设置界面中，点击View Spreadsheet查看输出变量的定义文档，然后按需选择。
设置完后，返回上一届面，选择与之匹配的Simulink文件，然后返回主界面，点击图1中的Send to Simulink按钮，即可将Carsim模型发送至Simulink中。若出现过未找到matlab的提示，请先启动matlab，然后再点击Send to Simulink按钮。
Simulink模型
STEP 8：
Simulink模型如图19、20所示，除CarSim S-Function外，其余所有模块均可在Library中找到，CarSim S-Function需要在Carsim搭建完模型，并点击Send to Simulink按钮后才会出现，具体操作见上述书本。
模型中用到的模块有
- Demux
- mux
- Constant
- Selector
- Memory
- Subsystem
- S-Function
直接在Simulink Libray Browser中搜索即可找到对应模块，具体使用可以研究说明文档。
其中，图20中的S-function模块需要有配套的m文件，需要自己编写。可在matlab的workspace里输入edit sfuntmpl，即可调取matlab自己提供的s函数模板，以此为模板编写自己的s函数。此处不对S-function的编写进行讲解，自行查找资源，如果需求比较大的话，我再新开博文讲解。
至此，CarSim模型和Simulink模型搭建完成，将CarSim模型xxx.cpar和Simulink模型xxx.mdl均放至CarSim安装路径下的D:\Program Files\CarSim802_Prog\CarSim_Data\路径下。
- parking_in7_out56_for_control.cpar // CarSim模型
- parking_in56_out3_for_control.mdl // Simulink模型
接下来就是编写Simulink所需S-function，及Apollo源码的编译与调用。
如果不需要使用Apollo的代码，完全可以仿照Apollo代码自写S-Function函数，实现路径规划及控制逻辑，实现自动泊车、循迹行驶等功能。本人由于未找到横向控制所需的部分参数，自写了横向控制，也同样实现了泊车入库的仿真操作。
如图22所示即为仿真效果图，坐标系基于初始坐标系进行了一定的转换，黑色矩形为障碍物车，绿色框为被空车，紫色虚为被控车实际运动轨迹，紫色虚线掩盖下的黑色虚线为规划轨迹。车身最终航向偏移角度在2度以内，横纵向误差在10cm以内，控制效果基本符合需求。
Apollo编译
源码中有一个文件：WORKSPACE.in，里面声明了Apollo编译所需要依赖的库：
workspace(name = "apollo")
# googletest (GTest and GMock)
...
# gflags
...
# glog
...
# Google Benchmark
...
# cpplint from google style guide
...
# eigen
...
# CivetWeb (web server)
...
# curlpp
...
#ros
...
# OpenCV 2.4.13.2
...
# PCL 1.7
# =======
# This requires libpcl-dev to be installed in your Ubuntu/Debian.
...
# Caffe
...
# YAML-CPP
...
# IpOpt
...
# Cuda
...
# Local-integ
...
# Proj.4
...
# tinyxml2
...
# protobuf 3.3
...
在Windows上编译的时候也需要依赖库，但是并非上面的全部库，具体需要什么库可根据自己要测试的模块，在编译时提示的错误来安装依赖库。
下面仅以控制模块为例讲解在Windows上编译Apollo源码。
依赖库
编译控制模块需要的依赖库大致如下所示：boost、eigen、flann、gflags、glog、googletest、modify、opencv、PCL、protobuf、Ros、zlib等。库版本号可以参考WORKSPACE.in中的说明。
安装时可以先安装PCL，因为它会自动安装依赖库，且下载界面也提供了boost、flann、eigen等的下载链接。请将上述依赖库，下载安装编译到一个自己找得到的路径下，以备后续。
其中，googletest非必须，但可以用来做一些测试，推荐安装，但编译安装时不一定要下载最新版，请找合适自己操作系统和编译器的版本，如编译器为vs 2015以下的，请下载1.8.0及以下的googletest版本，同理，上述每个依赖库不一定需要最新的，要找合适自己操作系统，合适自己编译器的库。
Ros可以直接下载（由乐哥提供）。
modify为自定义文件夹，里面放的是apollo源码中的几个为了编译需要而修改过的源文件，可直接下载，自己比较差异。
若上述依赖已经安装过的，可以找一下所需文件或文件夹是否存在：
# Eigen
|--Eigen
|--Eigen
|--unsupported
|--signature_of_eigen3_matrix_library
# flann
|--flann
|--bin
|--include
|--lib
|--share
# gflags
|--gflags
|--bin
|--include
|--lib
# glog
|--glog
|--bin
|--include
|--lib
# googletest-distribution
# 有些版本安装完后可能bin和lib下的文件都在lib文件夹下
|--googletest-distribution
|--bin
|--gmock_maind.dll
|--gmockd.dll
|--gtest_maind.dll
|--gtestd.dll
|--include
|--gmock
|--gtest
|--lib
|--gmock_maind.lib
|--gmockd.lib
|--gtest_maind.lib
|--gtestd.lib
# opencv
|--opencv
|--etc
|--include
|--java
|--x86
|--LICENSE
|--OpenCVConfig.cmake
|--OpenCVConfig-version.cmake
# PCL
|--PCL
|--bin
|--cmake
|--include
|--lib
# protobuf
|--protobuf
|--bin
|--libprotobufd.dll
|--libprotobuf-lited.dll
|--libprotocd.dll
|--protoc.exe
|--zlibd.dll
|--cmake
|--include
|--lib
# ros
|--ros
|--bin
|--et
|--include
|--share
|--_setup_util.py
|--env.sh
|--meta.txt
|--setup.bash
|--setup.sh
|--setup.zsh
|--
# zlib
|--zlib
|--bin
|--include
|--lib
|--share
配置
1.打开VS2013，新建一个空的控制台项目，添加源文件：main.cc，可以为其他尾缀的C++文件，但为了和Apollo保持一致，可以采用.cc尾缀。
2.项目属性 配置属性 调试 工作目录：D:\Apollo2.5（自己Apollo源码所在路径）；
3.为了让VS和Matlab联调，需要在VS中添加matlab所需的文件
项目属性 配置属性 VC++目录 可执行文件目录：D:\Program Files\MATLAB\R2014a\bin\win32）；
项目属性 配置属性 VC++目录 包含目录：D:\Program Files\MATLAB\R2014a\extern\include；
项目属性 配置属性 VC++目录 库目录：D:\Program Files\MATLAB\R2014a\extern\lib\win32\microsoft；
此步骤可以自行百度VS调用matlab查看更详细的教程；
4. 项目属性 配置属性 C/C++ 常规 附加包含目录：
D:\Program Files\MATLAB\R2014a\extern\include
E:\win_apollo\glog\include
E:\win_apollo\googletest-distribution\include
E:\win_apollo\gflags\include
E:\win_apollo\Eigen
E:\win_apollo\protobuf\include
E:\win_apollo\PCL\include\pcl-1.8
E:\win_apollo\flann\include
D:\boost
E:\Apollo2.5\apollo
E:\win_apollo\gbuddle\gbuddle
E:\win_apollo\opencv\include
E:\win_apollo\ros\include
E:\win_apollo
路径可按自己实际解压或安装路径进行添加设置。其中。win-apollo是我单独创建的文件夹，用于存放依赖库和apollo调试项目。
5.项目属性 配置属性 C/C++ 预处理器 预处理器定义：
_CRT_SECURE_NO_WARNINGS
_SCL_SECURE_NO_WARNINGS
_USE_MATH_DEFINES
PROTOBUF_USE_DLLS
EIGEN_DONT_ALIGN_STATICALLY
BOOST_NO_CXX11_VARIADIC_TEMPLATES
GLOG_NO_ABBREVIATED_SEVERITIES
_ALLOW_KEYWORD_MACROS
constexpr=const
noexcept=
rand_r(x)=rand()
attribute(x)=
这是为了编译需要，将一些Linux中的定义转换为Windows，不仅限于这些参数，在后期调试过程中，遇到一些新的无法编译或识别的符号可通过这种方法使编译通过。
6.项目属性 配置属性 链接器 常规 附加库目录：
D:\Program Files\MATLAB\R2014a\extern\lib
E:\win_apollo\glog\lib
E:\win_apollo\gflags\lib
E:\win_apollo\googletest-distribution\lib
D:\boost\stage\lib
E:\win_apollo\protobuf\lib
E:\win_apollo\opencv\x86\vc12\lib
7.项目属性 配置属性 链接器 输入 附加依赖项：
glogd.lib
gflags.lib
gtestd.lib
libprotobufd.lib
E:\Apollo2.5\apollo\build_vs2013\Debug\ApolloProtos.lib
libmx.lib
libmat.lib
libmex.lib
libeng.lib
opencv_core310d.lib
其中，E:\Apollo2.5\apollo\build_vs2013\Debug\ApolloProtos.lib
libmx.lib是编译apollo中所有的protbuf文件后生成的库，可以自己创建makefile文件和编译脚本编译apollo中的proto文件。下面提供乐哥写的脚本供参考：
CMakeLists.txt：基于protobuf\cmake\protobuf-module.cmake改编
windows-build2013.bat：运行脚本
下载链接：CSDN资源
将两个文件放至apollo源码根目录下，运行.bat文件，即可编译apollo中所有的protob文件，并生成一个工程。根据自己实际情况，需要变更.bat中的文件路径。
上述所有路径请根据自己的实际情况配置。
至此，配置告一段落。然后写main函数，抽调Apollo代码并编译。
测试
1.编写main函数
首先利用googletest写一个测试用main函数，利用googletest框架测试apollo中的test示例：
在源文件中新建main.cc文件
#include <iostream>
#include "glog/logging.h"
#include "gtest/gtest.h"
#include "gflags/gflags.h"
int main(int argc, char **argv)
{
google::ParseCommandLineFlags(&argc, &argv, true);
google::InitGoogleLogging(argv[0]);
testing::InitGoogleTest(&argc, argv);
RUN_ALL_TESTS();
google::ShutdownGoogleLogging();
google::ShutDownCommandLineFlags();
return 0;
}
2.从Apollo总选择需要进行测试的xxxtest.cc文件加入原文件中，如下所示：
除了测试项目还需添加相关的文件，否则无法编译。
如图25所示，编译出错，只需找到出错原因，添加所需的文件即可编译通过。运行结果如图26所示。
当然也会出现测试不通过的情况，如图27所示，有可能文件缺失或者路径不对，或者文件格式不匹配等导致，测试窗口会有提示，具体问题具体分析，测试不通过不代表你编译有问题，找到原因即可。
VS + Matlab联调
基本测试通过之后可以着手编写c++程序了，主要是为了实现C++与Simulink的联动。
在上面说过，Simulink中的S-function模块需要编写专门的m文件用于与CarSim联动。Simulink主要为CarSim提供控制模块、规划模块等，而我搭建这个模型就是为了验证Apollo的控制模块，因此，Simulink中的控制模块由Apollo提供，因此需要进行C++与Simulink的互相调用。
VS
使用控制模块主要需要两部分：初始化和控制量的计算，也就是需要LonController中的
// 初始化
Status LonController::Init(const ControlConf *control_conf)
// 控制量计算
Status LonController::ComputeControlCommand(
const localization::LocalizationEstimate *localization,
const canbus::Chassis *chassis,
const planning::ADCTrajectory *planning_published_trajectory,
control::ControlCommand *cmd)
1.定义自己的功能函数mexLonControlFunc.h，如下：
// 启动初始化
void InitLonControl(int nlhs, mxArray *plhs[], int nrhs, mxArray *prhs[]);
// 调用控制命令计算
void lonControlFunc(int nlhs, mxArray* plhs[], int nrhs, mxArray* prhs[]);
2.实现自己的功能函数mexLonControlFunc.cc，matlab与c++的数据传输请自行研究，此处不展开讨论，只讲解调用Apollo的部分。
// 需要注意时钟的问题，默认时钟为系统时钟，
// 但为了测试方便，需要将系统时钟设置为自定义时钟
// 关于Apollo的时钟，在apollo项目的issue中我曾回答过：
// https://github.com/ApolloAuto/apollo/issues/4793#issuecomment-402073944
// 修改time.h文件
/**
* @brief gets the current timestamp.
* @return a Timestamp object representing the current time.
*/
static Timestamp Now() {
switch (mode()) {
case ClockMode::SYSTEM:
return SystemNow();
case ClockMode::MOCK:
return instance()->mock_now_;
case ClockMode::ROS:
return instance()->mock_now_;
default:
AFATAL << "Unsupported clock mode: " << mode();
}
return SystemNow();
}
...
private:
/**
* @brief constructs the \class Clock instance
* @param mode the desired clock mode
*/
explicit Clock(ClockMode mode) : mode_(mode), mock_now_(Timestamp()) {
ros::Time::init();
}
...
// 编写InitLonControl
// 该函数主要接收Simulink传过来的规划路径，并完成控制模块初始化
void InitLonControl(int nlhs, mxArray *plhs[], int nrhs, mxArray *prhs[])
{
// 获取规划路径并存入全局变量planning_trajectory_中
// 此部分代码自行研究
...
// 设置时钟
Clock::SetMode((Clock::ClockMode)1);
std::string control_conf_file =
"E:/Apollo2.5/apollo/modules/control/conf/lincoln.pb.txt";
// 读取conf文件
apollo::common::util::GetProtoFromFile(control_conf_file, &control_conf);
// 纵向控制器初始化
lon_controller.Init(&control_conf);
// 将初始化状态返回给Simulink
...
}
// 编写lonControlFunc
// 该函数主要接收Simulink传过来的车辆状态信息，包含底盘信息、定位信息等，并完成纵向控制量的计算
void lonControlFunc(int nlhs, mxArray* plhs[], int nrhs, mxArray* prhs[])
{
// 获取Simulink传输过来的数据
...
// 更新车辆状态
// 设置时钟
auto duration = apollo::common::time::Duration(long long((parms[0][44])*std::nano::den));
Clock::SetNow(duration);
// chassis
auto chassis_header_ = chassis_.mutable_header();
chassis_header_->set_timestamp_sec(parms[0][44]);
chassis_header_->set_module_name("chassis");
chassis_header_->set_sequence_num(parms[0][44]*100+1);
chassis_.set_gear_location((ChassisPb::GearPosition)2);
chassis_.set_speed_mps(parms[0][34]);
chassis_.set_brake_percentage(parms[0][42]);
chassis_.set_throttle_percentage(parms[0][43]);
// localization
...
apollo::common::VehicleStateProvider::instance()->Update(localization_, chassis_);
apollo::control::ControlCommand cmd;
// 计算控制命令
lon_controller.ComputeControlCommand(&localization_, &chassis_, &planning_trajectory_, &cmd);
// 将控制命令返回给Simulink
...
3.将项目编译为dll文件，并将项目运行所以来的dll文件全部拷贝至Carsim安装目录下的D:\Program Files\CarSim802_Prog\CarSim_Data\路径下。
- guddle.dll
- gflags.dll
- glogd.dll
- gtestd.dll
- libprotobufd.dll
- opencv_core310d.dll
- zlibd.dll
S-Function
在matlab命令行键入edit sfuntmpl启动内置的s-function模板，以此为基础讲一下s-function的构成，只讲我用的到的模块。
%=============================================================================
% sfuntmpl可以理解为头函数吧，它定义了s-function工作的模式，名字可自定义，需和m文件名一致。
% 其中，flag是系统定义的参数，无法人为修改，在工作循环中系统自动判断flag的值，并跳转至对应的处理函数；
% t为时间，x不重要，我没用到，u为输入。
%=============================================================================
function [sys,x0,str,ts,simStateCompliance] = sfuntmpl(t,x,u,flag)
% The general form of an MATLAB S-function syntax is:
% [SYS,X0,STR,TS,SIMSTATECOMPLIANCE] = SFUNC(T,X,U,FLAG,P1,...,Pn)
%
% FLAG RESULT DESCRIPTION
% ----- ------ --------------------------------------------
% 0 [SIZES,X0,STR,TS] Initialization, return system sizes in SYS,
% initial state in X0, state ordering strings
% in STR, and sample times in TS.
% 1 DX Return continuous state derivatives in SYS.
% 2 DS Update discrete states SYS = X(n+1)
% 3 Y Return outputs in SYS.
% 4 TNEXT Return next time hit for variable step sample
% time in SYS.
% 5 Reserved for future (root finding).
% 9 [] Termination, perform any cleanup SYS=[].
%
% The following outlines the general structure of an S-function.
%
switch flag,
%%%%%%%%%%%%%%%%%%
% Initialization %
%%%%%%%%%%%%%%%%%%
case 0,
[sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes;
%%%%%%%%%%%
% Outputs %
%%%%%%%%%%%
case 3,
sys=mdlOutputs(t,x,u);
// 对于无用的flag可以作如下处理，也可以不做处理
case {1,2,4,5} % Unused flags
sys = [];
%%%%%%%%%%%%%
% Terminate %
%%%%%%%%%%%%%
case 9,
sys=mdlTerminate(t,x,u);
%%%%%%%%%%%%%%%%%%%%
% Unexpected flags %
%%%%%%%%%%%%%%%%%%%%
otherwise
DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag));
end
% end sfuntmpl
% 初始化函数
%=============================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=============================================================================
%
function [sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes
%
% call simsizes for a sizes structure, fill it in and convert it to a
% sizes array.
%
% Note that in this example, the values are hard coded. This is not a
% recommended practice as the characteristics of the block are typically
% defined by the S-function parameters.
%
sizes = simsizes;
sizes.NumContStates = 0;
sizes.NumDiscStates = 0;
sizes.NumOutputs = 0; // 输出参数个数
sizes.NumInputs = 0; // 输入参数个数
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1; % at least one sample time is needed
sys = simsizes(sizes);
%
% initialize the initial conditions
%
x0 = [];
%
% str is always an empty matrix
%
str = [];
%
% initialize the array of sample times
%
ts = [0 0]; % sample time: [period, offset]，如果采样间隔为0.01s，则设置ts=[0.01 0];
% end mdlInitializeSizes
% s-function中的主要部分，用于计算输出，函数内部自定义
%=============================================================================
% mdlOutputs
% Return the block outputs.
%=============================================================================
%
function sys=mdlOutputs(t,x,u)
sys = []; %sys是计算输出的内容，可将[]替换为你在该步的计算结果
% end mdlOutputs
%
%=============================================================================
%终止函数，仿真结束会自动调用该函数
%无论是仿真时间到了还是中途退出，均会调用该函数），可以做一些数据处理、生成图表之类的操作，视需求而定
%=============================================================================
% mdlTerminate
% Perform any end of simulation tasks.
%=============================================================================
%
function sys=mdlTerminate(t,x,u)
sys = []; %程序已终结，返回[]即可，此处不用给sys赋值
% end mdlTerminate
Simulink和VS联调有多种方法，我通过加载dll文件的方式实现。
1.S-function初始化时要加载c++模块，在仿真结束时要卸载c++模块。
% 加载c++模块
% gbuddle为项目编译生成的dll文件
% 若gbuddle已加载，判断m中接口是否正确
if libisloaded('gbuddle')
m = libfunctions('gbuddle','-full');
if size(m,1)<2
[notfound, warnings]=loadlibrary('gbuddle','mexLonControlFunc.h');
end
else
[notfound, warnings]=loadlibrary('gbuddle','mexLonControlFunc.h');
end
% 卸载c++模块
if libisloaded('gbuddle')
unloadlibrary('gbuddle')
end
由于S-function在程序运行过程中无法查看vs里面的运行情况，所以请提前确认vs代码没有问题，或者输出一些必要的信息，以确认自己计算的结果是否正确。
2.编写功能函数
我的S-function函数的工作流程大致如图28所示，主要包含了初始化函数、output函数、终止函数。
提供一种将matlab数据存储为csv文件的方法
filePath = 'D:\Program Files\CarSim802_Prog\CarSim_Data\control_data.csv';
fileTile={'x_station_error','y_station_error','station_pid_input','station_pid_output','speed_error','speed_error_act','speed_pid_input','speed_pid_output','preview_acceleration_reference','calibTable_input_speed','calibTable_input_acc','calibTable_output_break'};
% 存文件，实际值与规划值之差
controlCell=num2cell(control_table);
controlData=cell2table(controlCell,'VariableNames',fileTile);
writetable(controlData,filePath)
若要在同一文件下继续写数据，则用以下方法：
filePath = 'D:\Program Files\CarSim802_Prog\CarSim_Data\control_data.csv';
dlmwrite(filePath,control_table,'-append')
存储单个图片
figure(1)
plot(t,x,'*k')
xlabel('T/ s')
ylabel('X/ m')
saveas(gcf,'t-station_error.png')
最终将写好的S-function文件另存为MY_Parking_apollo.m，并放至D:\Program Files\CarSim802_Prog\CarSim_Data\下，便于查找。
至此，仿真所需的前期准备全部完成。D:\Program Files\CarSim802_Prog\CarSim_Data\路径下的文件有：
- guddle.dll
- gflags.dll
- glogd.dll
- gtestd.dll
- libprotobufd.dll
- opencv_core310d.dll
- zlibd.dll
- parking_in7_out56_for_control.cpar // CarSim模型
- parking_in56_out3_for_control.mdl // Simulink模型
- MY_Parking_apollo.m // S-function
- mexLonControlFunc.h //C++头文件
仿真
万事俱备，开始仿真。
STEP 1
启动CarSim，加载根据自己需求搭建的CarSim模型，设置好输入输出，点击Send to Simulink；
STEP 2
Simulink启动，设置Simulink解释器及仿真时间。
选择S-function文件
然后，开始你的仿真吧。可以将仿真过程中的数据存下来便于后期分析。
总结
总算写完了。因为代码不能完全公开，所以部分地方写的不算详尽，详细写可能这篇博文得拆分成四五篇写，但上面写的差不多涵盖了所有应该注意的点，有问题可以交流。如果内容中有不对或不妥的地方欢迎指正。