Carsim、Simulink与Ros联合仿真(三)-编写控制节点程序

前言

这部分内容就是直接编写控制节点程序订阅和发布话题给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;
};

类的结构

  1. 公有成员函数:

    • Init(double Kp, double Ki, double Kd): 初始化PID控制器的参数,包括比例系数(Kp)、积分系数(Ki)和微分系数(Kd)。
    • ComputeControl(double error, ros::Duration dt): 根据当前误差(error)和时间间隔(dt)计算控制输出。
  2. 私有数据成员:

    • 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; // 添加记录上次打印时间的变量
};

类的结构和功能

  1. 构造函数:

    • 初始化ROS节点句柄。
    • 创建一个发布者cmd_publisher,用于发布控制命令。
    • 创建一个订阅者feedback_subscriber,用于接收车辆状态反馈,并设置回调函数feedbackCallback
    • 初始化PID控制器。
    • 初始化last_print_time,用于记录上次打印信息的时间。
  2. 反馈回调函数(feedbackCallback):

    • 接收车辆状态消息。
    • 计算误差(目标速度和实际速度之差)。
    • 使用PID控制器计算加速度。
    • 根据加速度计算油门和制动力。
    • 计算扭矩和制动压力。
    • 调用issueCommand发布命令。
    • 定期打印车辆状态和控制参数。
  3. 发布命令函数(issueCommand):

    • 创建一个carsim_msgs::Carsim消息。
    • 设置消息的扭矩、制动和加速度字段。
    • 通过cmd_publisher发布消息。

ros::NodeHandle nh

  • ros::: 这是ROS命名空间的前缀。在ROS的代码中,ros是一个包含所有ROS类和函数的命名空间。使用ros::可以确保我们访问的是ROS库中的NodeHandle类,而不是其他库中可能存在的同名类。

  • NodeHandle: 这是ROS库中的一个类,用于处理节点的所有通信功能。它提供了创建发布者、订阅者、服务等的接口。

  • nh: 这是一个对象实例的名称。在这个例子中,我们创建了一个名为nhNodeHandle对象。这个对象实例可以在代码中用来与ROS通信层交互。

创建发布者(Publisher)

cmd_publisher = nh.advertise<carsim_msgs::Carsim>("Issue_cmd", 1000);

这行代码创建了一个名为cmd_publisherros::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_subscriberros::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

  1. 参数:

    • motor_torque: 电机扭矩,用于控制车辆的加速。
    • brake: 制动压力,用于减速或停车。
    • acceleration: 加速度,用于表示车辆当前的加速状态。
  2. 创建消息:

    • 首先,创建一个carsim_msgs::Carsim类型的消息对象msg
    • 然后,将函数参数分别设置为消息对象的相应字段。
  3. 发布消息:

    • 最后,通过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;
}

函数解析

  1. 初始化ROS节点:

    • ros::init(argc, argv, "control_node"): 这行代码初始化一个ROS节点,指定节点名称和命令行参数。
  2. 打印节点状态:

    • ROS_INFO("control_node is running");: 打印一条消息,表明节点正在运行。
  3. 创建控制节点对象:

    • ControlNode node;: 创建一个ControlNode对象,用于处理节点的逻辑。
  4. 设置循环频率:

    • ros::Rate loop_rate(100);: 创建一个ros::Rate对象,用于控制循环的频率。在这里,频率设置为100Hz,意味着循环每秒执行100次。
  5. 主循环:

    • while (ros::ok()): 进入一个循环,直到ROS节点被关闭。
    • ros::spinOnce();: 处理ROS网络中的所有消息,并调用所有回调函数。
    • loop_rate.sleep();: 根据loop_rate的设置,让当前线程休眠一段时间,以控制循环的频率。
  6. 停止节点和清理:

    • ROS_INFO("control_node is stopped");: 在循环结束时打印一条消息,表明节点已停止。
    • ros::shutdown();: 关闭ROS节点,释放资源。

  • 25
    点赞
  • 26
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值