电子商务网站建设服务,百度热搜榜,广州专业做标书公司,wordpress加密文章本文通过ROSPrescanCarsimsimulink搭建简单的硬件在环仿真测试平台。
系统架构如下#xff1a; 在Windows中运行prescan场景仿真软件#xff0c;在jetson Nano中运行ROS#xff0c;硬件上两台电脑通过一根网线相连传输信息#xff1b;
1.prescan与carsim的集成
在C:\car…本文通过ROSPrescanCarsimsimulink搭建简单的硬件在环仿真测试平台。
系统架构如下 在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的工作区间 打红框部分展开如下 X0Y0的数据录制模块如下图所示 然后先运行仿真工程让车辆沿着道路先走一次录制得到的.mat格式的数据如下图所示 在Windows这台PC上点击WinR打开输入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端IPROSIP为主机WINDOWS的IP地址这段代码主要是用于在matlab中初始化一个ROS节点。
编写代码将.mat文件储存为txt文件
Xglobal_x.Data;
Yglobal_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::vectordouble X, const std::vectordouble Y, double currentX, double currentY) {int closestIndex -1;double minDistance std::numeric_limitsdouble::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 closestIndex15;}// 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::vectorstd::vectordouble matrix, double control_input, double current_speed) {// 假设横坐标是控制输入纵坐标是当前车速int row_index std::min(static_castint(current_speed), static_castint(matrix.size() - 1));int col_index std::min(static_castint(control_input), static_castint(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::vectordouble gloabel_X;
std::vectordouble gloabel_Y;
std::vectordouble globel_Angel;
Algorithm algorithm;
std::vectorstd::vectordouble 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::vectordouble readData(const std::string filePath) {std::ifstream file(filePath);std::vectordouble 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::vectorstd::vectordouble readMatrix(const std::string filePath, int rows, int cols) {std::ifstream file(filePath);std::vectorstd::vectordouble matrix(rows, std::vectordouble(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.advertisegeometry_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 C11, supported in ROS Kinetic and newer
# add_compile_options(-stdc11)## 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 CMakes 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 isnt 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 dont 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计算得到的执行器数值并发送到场景软件中 最后点击运行如果没有问题车辆应该能实现简单的轨迹跟踪。
