本文通过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;
// 计算前轮转角 Delta
double 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; // 如果索引无效，则返回
}
// 更新全局的 output
output = 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 COMPONENTS
roscpp
std_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计算得到的执行器数值，并发送到场景软件中：
最后点击运行，如果没有问题，车辆应该能实现简单的轨迹跟踪。
扫一扫
698
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
