前言
这部分内容就是直接编写控制节点程序订阅和发布话题给Simulink,来控制Carsim中的车辆,就不需要通过Simulink生成ros节点程序了。
一、在Ubuntu系统创建控制功能包
1、创建控制功能包
打开终端,到工作空间的src目录,运行:
cd ros_simulink_ws/src/
创建功能包,运行:
catkin_create_pkg control_pkg roscpp rospy std_msgs
结果:
2、创建node节点文件
在control_pkg/src/目录下创建control_node.cpp文件,代码如下:
#include <ros/ros.h> // 包含ROS的头文件
#include <carsim_msgs/Carsim.h> // 包含carsim_msgs/Carsim消息类型
#include "pid_control.h" // 包含PIDController类的头文件
// // PID控制器类定义
// class PIDController
// {
// public:
// void Init(double Kp, double Ki, double Kd)
// {
// kp = Kp;
// ki = Ki;
// kd = Kd;
// integral = 0.0;
// last_error = 0.0;
// }
// double ComputeControl(double error, ros::Duration dt)
// {
// integral += error * dt.toSec();
// double derivative = (error - last_error) / dt.toSec();
// last_error = error;
// return kp * error + ki * integral + kd * derivative;
// }
// private:
// double kp, ki, kd;
// double integral;
// double last_error;
// };
// 控制节点类
class ControlNode
{
public:
ControlNode()
{
// 初始化节点
ros::NodeHandle nh;
// 创建发布者
cmd_publisher = nh.advertise<carsim_msgs::Carsim>("Issue_cmd", 1000);
// 创建订阅者
feedback_subscriber = nh.subscribe("cmd_feedback", 1000, &ControlNode::feedbackCallback, this);
// 初始化PID控制器参数
pid.Init(5.0, 0.1, 0.05); // Kp, Ki, Kd
// 初始化上次打印时间
last_print_time = ros::Time::now();
}
void feedbackCallback(const carsim_msgs::Carsim::ConstPtr& msg)
{
// 整车质量(kg)和车轮滚动半径(m)
double m = 1270.0;
double r = 0.325;
// 计算误差
double error = msg->target_speed - msg->speed;
// 使用PID控制器计算加速度
double acceleration = pid.ComputeControl(error, ros::Duration(0.01));
// 限制加速度在-4到4之间
if (acceleration > 4.0) {
acceleration = 4.0;
} else if (acceleration < -4.0) {
acceleration = -4.0;
}
// 如果加速度在-0.05到0.05之间,则设置为0
if (acceleration > -0.05 && acceleration < 0.05) {
acceleration = 0.0;
}
// 根据加速度的正负设置油门和刹车
double a_throttle = 0.0;
double a_brake = 0.0;
if (acceleration >= 0) {
a_throttle = acceleration;
a_brake = 0;
} else {
a_throttle = 0;
a_brake = -acceleration;
}
// 计算扭矩Torque
double Torque = a_throttle * m * r / 4;
// 计算制动压力Pressure
double Pressure = a_brake * 2;
// 发布命令
issueCommand(Torque, Pressure, acceleration);
// 检查是否已经过去了5秒
if (ros::Time::now() - last_print_time > ros::Duration(5.0))
{
// 打印当前速度和目标速度
ROS_INFO("Current Speed: %f, Target Speed: %f", msg->speed, msg->target_speed);
// 打印加速度、扭矩和压力
ROS_INFO("Acceleration: %f, Torque: %f, Pressure: %f", acceleration, Torque, Pressure);
// 更新上次打印时间
last_print_time = ros::Time::now();
}
}
void issueCommand(double motor_torque, double brake, double acceleration)
{
carsim_msgs::Carsim msg;
msg.motor_torque = motor_torque;
msg.brake = brake;
msg.acceleration = acceleration;
cmd_publisher.publish(msg);
}
private:
ros::Publisher cmd_publisher;
ros::Subscriber feedback_subscriber;
PIDController pid;// PID控制器类(需要定义PID控制器)
ros::Time last_print_time; // 添加记录上次打印时间的变量
};
// 主函数
int main(int argc, char **argv)
{
// 初始化ROS节点
ros::init(argc, argv, "control_node");
ROS_INFO("control_node is running");
ControlNode node; // 创建控制节点对象
// 设置循环频率
ros::Rate loop_rate(100); // 100Hz, 每秒100次
while (ros::ok())
{
// 处理回调函数
ros::spinOnce();
// 按照设定的频率休眠
loop_rate.sleep();
}
ROS_INFO("control_node is stopped");
ros::shutdown();
return 0;
}
上面的PID控制器类定义已被我注释,我在control_pkg/src/目录下还创建了两个文件
这两个文件可以要,也可以不要。
不要的话,把“PID控制器类定义”取消注释,删除头文件#include "pid_control.h",就可以了
下面是两个文件的代码:
// pid_control.cpp
#include "pid_control.h"
void PIDController::Init(double Kp, double Ki, double Kd)
{
kp = Kp;
ki = Ki;
kd = Kd;
integral = 0.0;
last_error = 0.0;
}
double PIDController::ComputeControl(double error, ros::Duration dt)
{
integral += error * dt.toSec();
double derivative = (error - last_error) / dt.toSec();
last_error = error;
return kp * error + ki * integral + kd * derivative;
}
// pid_control.h
#ifndef PID_CONTROL_H
#define PID_CONTROL_H
#include <ros/ros.h>
class PIDController
{
public:
void Init(double Kp, double Ki, double Kd);
double ComputeControl(double error, ros::Duration dt);
private:
double kp, ki, kd;
double integral;
double last_error;
};
#endif // PID_CONTROL_H
3、配置CMakeLists.txt文件
在CMakeLists.txt文件的最后添加下面内容:
# 添加可执行文件control_node的构建规则,指定源文件为src/control_node.cpp和src/pid_control.cpp
add_executable(control_node src/control_node.cpp src/pid_control.cpp)
# 为可执行文件control_node添加依赖项
# 这些依赖项包括本项目中导出的目标和catkin包中导出的目标
# 这样做确保在构建control_node时,这些依赖项已经被正确构建并可用
add_dependencies(
control_node
carsim_msgs_generate_messages_cpp
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
# 链接可执行文件control_node所需的库
# 这里链接了catkin包中定义的库,这些库是control_node正常运行所必需的
target_link_libraries(
control_node
${catkin_LIBRARIES}
)
add_executable中的src/pid_control.cpp是为了调用pid_control.cpp文件
add_dependencies中的carsim_msgs_generate_messages_cpp是为了调用我自定义的数据类型,在(Carsim、Simulink与Ros联合仿真(二)-自定义消息数据类型)中自定义的
4、配置package.xml文件
为了调用我自定义的数据类型,添加下面内容
<build_depend>carsim_msgs</build_depend>
<exec_depend>carsim_msgs</exec_depend>
如下所示
5、编译
在工作空间ros_simulink_ws目录下,运行:
catkin_make
设置环境变量
source devel/setup.bash
(如果在Carsim、Simulink与Ros联合仿真(一)5.5 设置环境变量用的是方法二,就不用设置了)
二、运行仿真
复习一下
1、通过carsim打开Simulink
2、建立MATLAB与ros通信
3、运行控制节点
rosrun control_pkg control_node
4、运行Simulink仿真
5、仿真结果
三、control_node.cpp代码解读
我自己复习一下C++,与上面仿真无关
1、C++中的类
类的定义
一个基本的类定义包括类名、私有或公共的数据成员和成员函数。下面是一个简单的类定义示例:
class MyClass {
private:
int privateData; // 私有数据成员
public:
MyClass() { privateData = 0; } // 构造函数
void publicMethod() { // 公共成员函数
// 对数据成员进行操作
}
};
构造函数和析构函数
- 构造函数:在创建对象时自动调用,用于初始化对象的数据成员。
- 析构函数:在对象生命周期结束时自动调用,用于清理资源。
class MyClass {
public:
MyClass() { /* 初始化 */ } // 构造函数
~MyClass() { /* 清理 */ } // 析构函数
};
访问控制
- 私有(private):类内部可以访问,外部不可访问。
- 公有(public):类内部和外部都可以访问。
成员函数
成员函数是类的一部分,可以访问类的所有成员(私有和公有)。
class MyClass {
public:
void doSomething() {
// 可以访问类的所有成员
}
};
2、PID控制器类定义 解读
// PID控制器类定义
class PIDController
{
public:
void Init(double Kp, double Ki, double Kd)
{
kp = Kp;
ki = Ki;
kd = Kd;
integral = 0.0;
last_error = 0.0;
}
double ComputeControl(double error, ros::Duration dt)
{
integral += error * dt.toSec();
double derivative = (error - last_error) / dt.toSec();
last_error = error;
return kp * error + ki * integral + kd * derivative;
}
private:
double kp, ki, kd;
double integral;
double last_error;
};
类的结构
-
公有成员函数:
Init(double Kp, double Ki, double Kd)
: 初始化PID控制器的参数,包括比例系数(Kp)、积分系数(Ki)和微分系数(Kd)。ComputeControl(double error, ros::Duration dt)
: 根据当前误差(error)和时间间隔(dt)计算控制输出。
-
私有数据成员:
kp, ki, kd
: PID控制器的三个主要参数。integral
: 积分误差累加值。last_error
: 上一次的误差值,用于计算微分项。
功能解释
-
初始化函数(Init): 设置PID控制器的三个参数,并将积分误差和上一次误差初始化为0。
-
计算控制输出(ComputeControl): 根据PID算法计算控制输出。这包括更新积分误差、计算微分误差,并应用PID公式。
dt.toSec()
dt.toSec(): dt 是一个ros::Duration对象,表示从上一次计算到当前时刻的时间间隔。toSec()方法将这个时间间隔转换为秒。
ros::Duration
- 用途:
ros::Duration
用于表示和操作时间间隔,可以用于记录两个时间点之间的差异。 - 构造函数: 可以通过提供秒数和微秒数来创建一个
Duration
对象,例如ros::Duration(1.5)
表示1.5秒。 - 常用方法:
toSec()
: 将Duration
对象转换为秒数。这在需要以秒为单位进行计算时非常有用。sleep()
: 使当前线程暂停指定的时长。wallSec()
: 返回自1970年1月1日以来的秒数(不考虑时钟调整)。
3、控制节点类 解读
// 控制节点类
class ControlNode
{
public:
ControlNode()
{
// 初始化节点
ros::NodeHandle nh;
// 创建发布者
cmd_publisher = nh.advertise<carsim_msgs::Carsim>("Issue_cmd", 1000);
// 创建订阅者
feedback_subscriber = nh.subscribe("cmd_feedback", 1000, &ControlNode::feedbackCallback, this);
// 初始化PID控制器参数
pid.Init(5.0, 0.1, 0.05); // Kp, Ki, Kd
// 初始化上次打印时间
last_print_time = ros::Time::now();
}
void feedbackCallback(const carsim_msgs::Carsim::ConstPtr& msg)
{
// 整车质量(kg)和车轮滚动半径(m)
double m = 1270.0;
double r = 0.325;
// 计算误差
double error = msg->target_speed - msg->speed;
// 使用PID控制器计算加速度
double acceleration = pid.ComputeControl(error, ros::Duration(0.01));
// 限制加速度在-4到4之间
if (acceleration > 4.0) {
acceleration = 4.0;
} else if (acceleration < -4.0) {
acceleration = -4.0;
}
// 如果加速度在-0.05到0.05之间,则设置为0
if (acceleration > -0.05 && acceleration < 0.05) {
acceleration = 0.0;
}
// 根据加速度的正负设置油门和刹车
double a_throttle = 0.0;
double a_brake = 0.0;
if (acceleration >= 0) {
a_throttle = acceleration;
a_brake = 0;
} else {
a_throttle = 0;
a_brake = -acceleration;
}
// 计算扭矩Torque
double Torque = a_throttle * m * r / 4;
// 计算制动压力Pressure
double Pressure = a_brake * 2;
// 发布命令
issueCommand(Torque, Pressure, acceleration);
// 检查是否已经过去了5秒
if (ros::Time::now() - last_print_time > ros::Duration(5.0))
{
// 打印当前速度和目标速度
ROS_INFO("Current Speed: %f, Target Speed: %f", msg->speed, msg->target_speed);
// 打印加速度、扭矩和压力
ROS_INFO("Acceleration: %f, Torque: %f, Pressure: %f", acceleration, Torque, Pressure);
// 更新上次打印时间
last_print_time = ros::Time::now();
}
}
void issueCommand(double motor_torque, double brake, double acceleration)
{
carsim_msgs::Carsim msg;
msg.motor_torque = motor_torque;
msg.brake = brake;
msg.acceleration = acceleration;
cmd_publisher.publish(msg);
}
private:
ros::Publisher cmd_publisher;
ros::Subscriber feedback_subscriber;
PIDController pid;// PID控制器类(需要定义PID控制器)
ros::Time last_print_time; // 添加记录上次打印时间的变量
};
类的结构和功能
-
构造函数:
- 初始化ROS节点句柄。
- 创建一个发布者
cmd_publisher
,用于发布控制命令。 - 创建一个订阅者
feedback_subscriber
,用于接收车辆状态反馈,并设置回调函数feedbackCallback
。 - 初始化PID控制器。
- 初始化
last_print_time
,用于记录上次打印信息的时间。
-
反馈回调函数(feedbackCallback):
- 接收车辆状态消息。
- 计算误差(目标速度和实际速度之差)。
- 使用PID控制器计算加速度。
- 根据加速度计算油门和制动力。
- 计算扭矩和制动压力。
- 调用
issueCommand
发布命令。 - 定期打印车辆状态和控制参数。
-
发布命令函数(issueCommand):
- 创建一个
carsim_msgs::Carsim
消息。 - 设置消息的扭矩、制动和加速度字段。
- 通过
cmd_publisher
发布消息。
- 创建一个
ros::NodeHandle nh
-
ros::
: 这是ROS命名空间的前缀。在ROS的代码中,ros
是一个包含所有ROS类和函数的命名空间。使用ros::
可以确保我们访问的是ROS库中的NodeHandle
类,而不是其他库中可能存在的同名类。 -
NodeHandle
: 这是ROS库中的一个类,用于处理节点的所有通信功能。它提供了创建发布者、订阅者、服务等的接口。 -
nh
: 这是一个对象实例的名称。在这个例子中,我们创建了一个名为nh
的NodeHandle
对象。这个对象实例可以在代码中用来与ROS通信层交互。
创建发布者(Publisher)
cmd_publisher = nh.advertise<carsim_msgs::Carsim>("Issue_cmd", 1000);
这行代码创建了一个名为cmd_publisher
的ros::Publisher
对象,用于发布名为carsim_msgs::Carsim
的消息类型到名为Issue_cmd
的话题。关键点如下:
nh.advertise<carsim_msgs::Carsim>("Issue_cmd", 1000)
: 这是一个调用,用于创建发布者。<carsim_msgs::Carsim>
: 这是消息的类型。carsim_msgs::Carsim
应该是一个自定义的ROS消息类型。"Issue_cmd"
: 这是发布者将发布消息到的话题名称。1000
: 这是消息队列的大小,即发布者在内存中为尚未发送的消息保留的空间。
创建订阅者(Subscriber)
feedback_subscriber = nh.subscribe("cmd_feedback", 1000, &ControlNode::feedbackCallback, this);
这行代码创建了一个名为feedback_subscriber
的ros::Subscriber
对象,用于订阅名为cmd_feedback
的话题上的消息,并将接收到的消息传递给ControlNode::feedbackCallback
回调函数。关键点如下:
nh.subscribe("cmd_feedback", 1000, &ControlNode::feedbackCallback, this)
: 这是一个调用,用于创建订阅者。"cmd_feedback"
: 这是订阅者将监听的话题名称。1000
: 这是消息队列的大小,即订阅者在内存中为尚未处理的消息保留的空间。&ControlNode::feedbackCallback
: 这是回调函数的地址。每当订阅者接收到新消息时,都会调用这个函数。this
: 这是指向当前ControlNode
对象的指针,用于在回调函数中访问类的成员变量和方法。
ros::Time::now() 的用途
- 时间戳: 在记录事件、消息或其他数据时,可以使用
ros::Time::now()
来获取一个时间戳,以便知道事件发生的具体时间。 - 定时: 可以使用
ros::Time::now()
来检查是否已经过了特定的时间间隔,例如在周期性任务或定时打印日志时。 - 时间计算:
ros::Time
提供了多种运算符,可以用于计算时间差、比较时间等。
issueCommand
-
参数:
motor_torque
: 电机扭矩,用于控制车辆的加速。brake
: 制动压力,用于减速或停车。acceleration
: 加速度,用于表示车辆当前的加速状态。
-
创建消息:
- 首先,创建一个
carsim_msgs::Carsim
类型的消息对象msg
。 - 然后,将函数参数分别设置为消息对象的相应字段。
- 首先,创建一个
-
发布消息:
- 最后,通过
cmd_publisher
发布消息。cmd_publisher
是一个ros::Publisher
对象,它负责将消息发布到ROS网络中的特定话题。
- 最后,通过
4、主函数 解读
// 主函数
int main(int argc, char **argv)
{
// 初始化ROS节点
ros::init(argc, argv, "control_node");
ROS_INFO("control_node is running");
ControlNode node; // 创建控制节点对象
// 设置循环频率
ros::Rate loop_rate(100); // 100Hz, 每秒100次
while (ros::ok())
{
// 处理回调函数
ros::spinOnce();
// 按照设定的频率休眠
loop_rate.sleep();
}
ROS_INFO("control_node is stopped");
ros::shutdown();
return 0;
}
函数解析
-
初始化ROS节点:
ros::init(argc, argv, "control_node")
: 这行代码初始化一个ROS节点,指定节点名称和命令行参数。
-
打印节点状态:
ROS_INFO("control_node is running");
: 打印一条消息,表明节点正在运行。
-
创建控制节点对象:
ControlNode node;
: 创建一个ControlNode
对象,用于处理节点的逻辑。
-
设置循环频率:
ros::Rate loop_rate(100);
: 创建一个ros::Rate
对象,用于控制循环的频率。在这里,频率设置为100Hz,意味着循环每秒执行100次。
-
主循环:
while (ros::ok())
: 进入一个循环,直到ROS节点被关闭。ros::spinOnce();
: 处理ROS网络中的所有消息,并调用所有回调函数。loop_rate.sleep();
: 根据loop_rate
的设置,让当前线程休眠一段时间,以控制循环的频率。
-
停止节点和清理:
ROS_INFO("control_node is stopped");
: 在循环结束时打印一条消息,表明节点已停止。ros::shutdown();
: 关闭ROS节点,释放资源。