简单硬件在环搭建（ROS+Prescan+Carsim+simulink)

本文通过ROS+Prescan+Carsim+simulink搭建简单的硬件在环仿真测试平台。

系统架构如下：

在Windows中运行prescan场景仿真软件，在jetson Nano中运行ROS，硬件上两台电脑通过一根网线相连传输信息；

1.prescan与carsim的集成

在C:\carsimworkspace\prescan\Extensions\Simulink的路径下，找到如下模型：

将carsim模型发送到simulink中如下：

打开prescan，建议使用prescan8.5，放置主车与一段道路：

在主车驾驶员模型中选择Path Follower,以便于录制跟踪的路径数据。在prescan中选择车辆动力学模型如下，点击browse，选择之间extension文件夹下的carsim动力学模型：

在道路设置中，用黄线将道路的参考线绘制出来，并将其添加到车辆的运动轨迹中：

添加路径示意图如下：

这样就将道路的参考线数据添加到了车辆的运动路径里面了。

2.ROS与prescan通讯配置

将prescan的模型发送到simulink，打开模型得到：

左边打红框的地方是carsim到prescan的坐标转换模块，右边是to workspace模块，用于将数据录制到matlab的工作区间：

打红框部分展开如下：

X0，Y0的数据录制模块如下图所示：

然后先运行仿真工程，让车辆沿着道路先走一次，录制得到的.mat格式的数据如下图所示：

在Windows这台PC上，点击Win+R打开，输入CMD打开终端：

找到本机的IP地址，如果设置IP地址与Ubuntu不在同一网段的话，点击网络设置中的以太网设置，对IP进行重新配置：

打开jetson nano,输入ifconfig查看IP：

在Ping一下jetson nano的IP地址，以确保通信是没有问题的：

注意，在ROS中，需要修改etc/home文件夹下的设置，将IP与用户名相绑定，这一步必须做，否则可能导致通讯链路是通的，但是ROS收不到simulink发布的ROS消息：

3.matlab/simulink与ROS通讯的接口代码

在matlab中建立与ros的通讯，代码如下：

setenv('ROS_MASTER_URI','http://10.120.175.3:11311') 
%setenv('ROS_MASTER_URI','http://10.120.175.3:13637')
setenv('ROSIP', '10.120.175.22');
rosinit

其中ROS_MASTER_URI为Ubuntu端IP，ROSIP为主机（WINDOWS）的IP地址，这段代码主要是用于在matlab中初始化一个ROS节点。

编写代码，将.mat文件储存为txt文件：

X=global_x.Data;
Y=global_y.Data;
save('globalx.mat',"X");
save('globaly.mat',"Y");
% 1. 加载 .mat 文件
load('globalx.mat');
% 假设 data 是你要保存的变量
% 提取某一列数据（例如第1列）
global_X = X(:, 1); 
% 2. 打开一个新的文本文件来写入数据
fileID = fopen('global_X.txt', 'w');
% 3. 将数据写入文本文件
fprintf(fileID, '%f\n', global_X);
% 4. 关闭文件
fclose(fileID);% 1. 加载 .mat 文件
load('globaly.mat');
% 假设 data 是你要保存的变量
% 提取某一列数据（例如第1列）
global_Y = Y(:, 1); 
% 2. 打开一个新的文本文件来写入数据
fileID = fopen('global_Y.txt', 'w');
% 3. 将数据写入文本文件
fprintf(fileID, '%f\n', global_Y);
% 4. 关闭文件
fclose(fileID);

打开FileZilla，建立连接后将TXT文件传输到nano中： 

在ROS中编写PID控制以及ROS节点的代码，这里建议先不要写PID算法，可以给执行器简单的输入进行开环测试：

#include <ros/ros.h>
#include <geometry_msgs/Pose2D.h>
#include <std_msgs/Float64.h>
#include <fstream>
#include <sstream>
#include <vector>
#include <string>
#include <cmath>
#include <limits>// 创建一个算法类
class Algorithm {
public:double lambda = 0; // 积分权重参数double u = 1.0; // 微分权重参数// 距离计算函数double calculateDistance(double X, double Y, double X1, double Y1) {return sqrt((X - X1) * (X - X1) + (Y - Y1) * (Y - Y1));}// 找出最近的点int findClosestPoint(const std::vector<double>& X, const std::vector<double>& Y, double currentX, double currentY) {int closestIndex = -1;double minDistance = std::numeric_limits<double>::max();for (size_t i = 0; i < X.size(); ++i) {double distance = calculateDistance(X[i], Y[i], currentX, currentY);if (distance < minDistance) {minDistance = distance;closestIndex = i;}}return closestIndex+15;}// PID速度控制代码double PIDControl(double target_speed, double current_speed, double& integral, double& previous_error) {// PID增益double Kp = 14;double Ki = 0.4;double Kd = 0.06;// 计算误差double error = target_speed - current_speed;// 计算积分和微分项integral += error;double derivative = error - previous_error;// PID输出double output = Kp * error + Ki * integral + Kd * derivative;// 更新前一误差previous_error = error;return output;}// 二维查找表插值double lookupControlValue(const std::vector<std::vector<double>>& matrix, double control_input, double current_speed) {// 假设横坐标是控制输入，纵坐标是当前车速int row_index = std::min(static_cast<int>(current_speed), static_cast<int>(matrix.size() - 1));int col_index = std::min(static_cast<int>(control_input), static_cast<int>(matrix[0].size() - 1));// 插值输出return matrix[row_index][col_index];}// PID横向控制double PID(double target_X, double target_Y, double Phik, double X, double Y) {double Kp = 17;  double Ki = 0.05;  double Kd = 0.04;double Phi= Phik *3.1415/180;static double prev_err = 0; // 改为静态变量，保持上次调用的状态static double integral_err = 0; // 改为静态变量，保持上次调用的状态// 计算目标方向与当前位置的角度double alpha =atan2(target_Y - Y,target_X - X)-Phi;// 计算比例误差double proportional_err = alpha;// 计算积分误差integral_err += proportional_err;// 计算微分误差double derivative_err = proportional_err - prev_err;// 计算前轮转角 Deltadouble Delta = Kp * proportional_err + Ki * integral_err+ Kd *derivative_err;// 更新前一误差prev_err = proportional_err;return Delta; // 返回前轮转角}// 横向LQR控制double LQR() {// 实现横向LQR控制}// 横向PP控制double PureP() {// 实现横向PP控制}
};// 全局变量声明
std::vector<double> gloabel_X;
std::vector<double> gloabel_Y;
std::vector<double> globel_Angel;
Algorithm algorithm;
std::vector<std::vector<double>> matrix;
double integral = 0.0;
double previous_error = 0.0;
double target_speed = 30.0; // 目标速度 30 km/h
double current_speed = 0.0;
double output = 0;// 位置回调函数
void chatterCallback(const geometry_msgs::Pose2D& Pose_msg) {// 寻找索引值int index = algorithm.findClosestPoint(gloabel_X, gloabel_Y, Pose_msg.x, Pose_msg.y);ROS_INFO("索引: %f", gloabel_Y[index]);// 添加有效性检查if (index < 0 || index >= gloabel_X.size() || index >= gloabel_Y.size() || index >= globel_Angel.size()) {ROS_ERROR("索引越界: %d", index);return; // 如果索引无效，则返回}// 更新全局的 outputoutput = algorithm.PID(gloabel_X[index], gloabel_Y[index],Pose_msg.theta, Pose_msg.x, Pose_msg.y);
}// 速度回调函数
void velocityCallback(const geometry_msgs::Pose2D& Vel_msg) {current_speed = Vel_msg.x; // 假设当前速度在 x 轴
}// 从文件读取数据
std::vector<double> readData(const std::string& filePath) {std::ifstream file(filePath);std::vector<double> data;if (!file.is_open()) {ROS_ERROR("无法打开文件: %s", filePath.c_str());return data; // 返回空的vector}std::string line;while (std::getline(file, line)) {std::istringstream iss(line);double value;if (iss >> value) {data.push_back(value);} else {ROS_WARN("读取数据失败: %s", line.c_str());}}file.close();return data;
}// 从文件读取矩阵
std::vector<std::vector<double>> readMatrix(const std::string& filePath, int rows, int cols) {std::ifstream file(filePath);std::vector<std::vector<double>> matrix(rows, std::vector<double>(cols));if (!file.is_open()) {ROS_ERROR("无法打开文件: %s", filePath.c_str());return matrix;}std::string line;for (int i = 0; i < rows; ++i) {if (!std::getline(file, line)) {ROS_ERROR("读取文件时出错或文件行数不足");return matrix;}std::istringstream iss(line);for (int j = 0; j < cols; ++j) {if (!(iss >> matrix[i][j])) {ROS_ERROR("读取数据时出错或文件列数不足: 行 %d 列 %d", i, j);return matrix;}}}file.close();return matrix;
}int main(int argc, char **argv) {ros::init(argc, argv, "Communicate");ros::NodeHandle n;ros::Publisher chatter_pub = n.advertise<geometry_msgs::Pose2D>("control", 1000);ros::Subscriber sub = n.subscribe("simulink_pose", 1000, chatterCallback);ros::Subscriber sub1 = n.subscribe("vel_cmd", 1000, velocityCallback);ros::Rate loop_rate(10);// 读取X轴坐标轨迹值std::string filePathX = "/home/jetson/ROS/R2/yahboomcar_ws/src/ROS_Prescan/prescanpp/globel_X.txt"; // 替换为实际文件路径gloabel_X = readData(filePathX);// for (const double& value : gloabel_X) {//     ROS_INFO("读取到的数据: %f", value);// }// 读取Y轴坐标轨迹值std::string filePathY = "/home/jetson/ROS/R2/yahboomcar_ws/src/ROS_Prescan/prescanpp/globel_Y.txt"; // 替换为实际文件路径gloabel_Y = readData(filePathY);// for (const double& value : gloabel_Y) {//     ROS_INFO("读取到的数据: %f", value);// }// 读取航向角值std::string filePathAngel = "/home/jetson/ROS/R2/yahboomcar_ws/src/ROS_Prescan/prescanpp/globel_Angel.txt"; // 替换为实际文件路径globel_Angel = readData(filePathAngel);// for (const double& value : globel_Angel) {//     ROS_INFO("读取到的航向角: %f", value);// }// 读取油门刹车查找表std::string filePathMatrix = "/home/jetson/ROS/R2/yahboomcar_ws/src/ROS_Prescan/prescanpp/src/tablebr.txt";matrix = readMatrix(filePathMatrix, 1001, 261);while (ros::ok()) {// 计算PID控制器输出double control_output = algorithm.PIDControl(target_speed, current_speed, integral, previous_error);// 查找表获取控制值double control_value = algorithm.lookupControlValue(matrix, control_output, current_speed);// 确定油门和刹车double throttle = (control_value > 0) ? control_value : 0;double brake = (control_value < 0) ? -control_value : 0;// 发布控制消息geometry_msgs::Pose2D msg;msg.x = throttle; // 油门开度msg.y = brake; // 刹车压力msg.theta = output-3.14/2; // 方向盘扭矩chatter_pub.publish(msg);// ROS_INFO("Throttle: %f, Brake: %f, Steering Torque (msg.theta): %f", throttle, brake, msg.theta);ros::spinOnce();loop_rate.sleep();}return 0;
}

编写CmakeList.txt文件如下：

cmake_minimum_required(VERSION 3.0.2)
project(prescanpp)## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTSroscppstd_msgs
)## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()################################################
## Declare ROS messages, services and actions ##
################################################## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
##   * add a build_depend tag for "message_generation"
##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
##     but can be declared for certainty nonetheless:
##     * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
##   * add "message_generation" and every package in MSG_DEP_SET to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * add "message_runtime" and every package in MSG_DEP_SET to
##     catkin_package(CATKIN_DEPENDS ...)
##   * uncomment the add_*_files sections below as needed
##     and list every .msg/.srv/.action file to be processed
##   * uncomment the generate_messages entry below
##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)## Generate messages in the 'msg' folder
# add_message_files(
#   FILES
#   Message1.msg
#   Message2.msg
# )## Generate services in the 'srv' folder
# add_service_files(
#   FILES
#   Service1.srv
#   Service2.srv
# )## Generate actions in the 'action' folder
# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )## Generate added messages and services with any dependencies listed here
# generate_messages(
#   DEPENDENCIES
#   std_msgs
# )################################################
## Declare ROS dynamic reconfigure parameters ##
################################################## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
##   * add "dynamic_reconfigure" to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * uncomment the "generate_dynamic_reconfigure_options" section below
##     and list every .cfg file to be processed## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
#   cfg/DynReconf1.cfg
#   cfg/DynReconf2.cfg
# )###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES prescanpp
#  CATKIN_DEPENDS roscpp std_msgs
#  DEPENDS system_lib
)###########
## Build ##
############# Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include${catkin_INCLUDE_DIRS}
)## Declare a C++ library
# add_library(${PROJECT_NAME}
#   src/${PROJECT_NAME}/prescanpp.cpp
# )## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/prescanpp_node.cpp)## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
#   ${catkin_LIBRARIES}
# )#############
## Install ##
############## all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
#   scripts/my_python_script
#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
#   FILES_MATCHING PATTERN "*.h"
#   PATTERN ".svn" EXCLUDE
# )## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
#   # myfile1
#   # myfile2
#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )#############
## Testing ##
############### Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_prescanpp.cpp)
# if(TARGET ${PROJECT_NAME}-test)
#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()## Add folders to be run by python nosetests
# catkin_add_nosetests(test)#add_library(${PROJECT_NAME}
#   src/${PROJECT_NAME}/prescanpp.cpp
# )add_executable(Communicate src/Communicate.cpp)
add_dependencies(Communicate ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})target_link_libraries(Communicate${catkin_LIBRARIES})#----------------------------------------------------------------------#

在simulink中搭建ROS的发送与接收接口如下：

ROS_Prescan位置更新模块如下,如果前面的步骤均没有问题，这个地方应该可以自动读取出识别到的ROS节点，这个模块主要是将Prescan中车辆的数据接收并发送到ROS中:

ROS_Prescan位置信息更新模块如下，此模块主要用于接收ROS计算得到的执行器数值，并发送到场景软件中：

最后点击运行，如果没有问题，车辆应该能实现简单的轨迹跟踪。