【ROS2】演示:理解实时编程

十年一梦实验室

于 2024-07-25 13:53:25 发布

阅读量220

点赞数 1

本文链接：https://blog.csdn.net/cxyhjl/article/details/140704594

版权

背景
安装并运行演示
- 运行测试
- 刚刚发生了什么事？
- 调整内存锁定权限
- 输出概述
- 延迟
- 为调度程序设置权限
- 绘图结果

背景

实时计算是许多机器人系统的关键特性，特别是自动驾驶汽车、航天器和工业制造等安全和任务关键型应用。我们在设计和原型开发 ROS 2 时考虑了实时性能约束，因为这是在 ROS 1 的早期阶段没有考虑到的需求，现在要将 ROS 1 重构为实时友好已经变得难以处理。

本文档概述了实时计算的要求和软件工程师的最佳实践。简而言之：

要创建一个实时计算机系统，我们的实时循环必须定期更新以满足截止日期。我们只能容忍这些截止日期的小误差（我们最大允许的抖动）。为此，我们必须避免在执行路径中出现非确定性操作，例如：页面错误事件、动态内存分配/释放以及无限期阻塞的同步原语。

一个经典的控制问题例子，通常通过实时计算解决的是平衡倒立摆。如果控制器意外地长时间阻塞，摆锤会倒下或变得不稳定。但如果控制器以比控制摆锤的电机运行速度更快的速率可靠地更新，摆锤将成功地适应传感器数据以平衡摆锤。

现在你已经了解了实时计算的所有内容，让我们尝试一个演示吧！

安装并运行演示程序

实时演示是针对 Linux 操作系统编写的，因为许多从事实时计算的 ROS 社区成员使用 Xenomai 或 RT_PREEMPT 作为他们的实时解决方案。由于演示中完成的许多操作都是特定于操作系统的，因此该演示仅在 Linux 系统上构建和运行。因此，如果您是 OSX 或 Windows 用户，请不要尝试这一部分！

此外，这必须使用静态 DDS API 从源代码构建。目前唯一支持的实现是 Connext。

首先，按照说明使用 Connext DDS 作为中间件从源代码构建 ROS 2。源码构建目录在

以下为手动构建：

cxy@cxy-Ubuntu2404:~/pendulum$ sudo apt install ros-jazzy-apex-test-tools
[sudo] password for cxy: 
Reading package lists... Done
Building dependency tree... Done
Reading state information... Done
The following additional packages will be installed:
  ros-jazzy-osrf-testing-tools-cpp
The following NEW packages will be installed:
  ros-jazzy-apex-test-tools ros-jazzy-osrf-testing-tools-cpp
0 upgraded, 2 newly installed, 0 to remove and 5 not upgraded.
Need to get 5,074 kB of archives.
After this operation, 5,391 kB of additional disk space will be used.
Do you want to continue? [Y/n] y
0% [Working]
Get:1 http://packages.ros.org/ros2/ubuntu noble/main amd64 ros-jazzy-osrf-testing-tools-cpp amd64 2.0.0-3noble.20240702.042307 [5,066 kB]
1% [1 ros-jazzy-osrf-testing-tools-cpp 54.2 kB/5,066 kB 1%]                  1% [Get:2 http://packages.ros.org/ros2/ubuntu noble/main amd64 ros-jazzy-apex-test-tools amd64 0.0.2-9noble.20240702.042443 [7,926 B]
Fetched 5,074 kB in 13min 15s (6,384 B/s)                                       
Selecting previously unselected package ros-jazzy-osrf-testing-tools-cpp.
(Reading database ... 270244 files and directories currently installed.)
Preparing to unpack .../ros-jazzy-osrf-testing-tools-cpp_2.0.0-3noble.20240702.04
2307_amd64.deb ...
Unpacking ros-jazzy-osrf-testing-tools-cpp (2.0.0-3noble.20240702.042307) ...
Selecting previously unselected package ros-jazzy-apex-test-tools.
Preparing to unpack .../ros-jazzy-apex-test-tools_0.0.2-9noble.20240702.042443_am
d64.deb ...
Unpacking ros-jazzy-apex-test-tools (0.0.2-9noble.20240702.042443) ...
Setting up ros-jazzy-osrf-testing-tools-cpp (2.0.0-3noble.20240702.042307) ...
Setting up ros-jazzy-apex-test-tools (0.0.2-9noble.20240702.042443) ...
cxy@cxy-Ubuntu2404:~/pendulum$ colcon build
Starting >>> pendulum2_msgs
Starting >>> pendulum_utils
Finished <<< pendulum_utils [0.33s]                                    
Finished <<< pendulum2_msgs [1.94s]                     
Starting >>> pendulum_controller
Starting >>> pendulum_driver
Starting >>> pendulum_state_publisher
Finished <<< pendulum_state_publisher [9.47s]                                                 --- stderr: pendulum_driver                                                   
/home/cxy/pendulum/pendulum_driver/src/pendulum_driver.cpp: In member function ‘void pendulum::pendulum_driver::PendulumDriver::set_controller_cart_force(double)’:
/home/cxy/pendulum/pendulum_driver/src/pendulum_driver.cpp:71:38: warning: ‘constexpr const T& rcppmath::clamp(const T&, const T&, const T&) [with T = double]’ is deprecated: use std::clamp instead, introduced in C++17 [-Wdeprecated-declarations]
   71 |   controller_force_ = rcppmath::clamp(force, -cfg_.get_max_cart_force(), cfg_.get_max_cart_force());
      |                       ~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
In file included from /home/cxy/pendulum/pendulum_driver/src/pendulum_driver.cpp:17:
/home/cxy/ros2_jazzy/src/ros2/rcpputils/include/rcppmath/clamp.hpp:41:21: note: declared here
   41 | constexpr const T & clamp(const T & v, const T & lo, const T & hi)
      |                     ^~~~~
---
Finished <<< pendulum_driver [18.1s]
Finished <<< pendulum_controller [19.6s]                       
Starting >>> pendulum_demo
Finished <<< pendulum_demo [7.67s]                      
Starting >>> pendulum_bringup
Finished <<< pendulum_bringup [2.23s]                   
Starting >>> pendulum
Finished <<< pendulum [2.00s]                   


Summary: 8 packages finished [33.6s]
  1 package had stderr output: pendulum_driver
cxy@cxy-Ubuntu2404:~/pendulum$ source install/setup.bash

运行测试

在运行之前，请确保至少有 8GB 的空闲 RAM。由于内存锁定，交换将不再起作用。

请配置您的 ROS 2 setup.bash。

运行演示二进制文件，并重定向输出。如果遇到权限错误，您可能需要使用 sudo 。

pendulum_demo > output.txt

cxy@cxy-Ubuntu2404:~/ros2_jazzy$ ros2 run pendulum_control pendulum_demo
Couldn't set scheduling priority and policy: Operation not permitted
Caught exception: std::bad_alloc
Unlocking memory and continuing.
Couldn't lock all cached virtual memory.
Pagefaults from reading pages not yet mapped into RAM will be recorded.
Initial major pagefaults: 0
Initial minor pagefaults: 963851
No results filename given, not writing results
rttest statistics:
  - Minor pagefaults: 0
  - Major pagefaults: 0
  Latency (time after deadline was missed):
    - Min: 53992 ns
    - Max: 198680 ns
    - Mean: 123688.674000 ns
    - Standard deviation: 14301.050338




PendulumMotor received 498 messages
PendulumController received 499 messages
cxy@cxy-Ubuntu2404:~/ros2_jazzy$ ros2 run pendulum_control pendulum_demo > output.txt
Couldn't set scheduling priority and policy: Operation not permitted
Caught exception: std::bad_alloc
Unlocking memory and continuing.
Couldn't lock all cached virtual memory.
Pagefaults from reading pages not yet mapped into RAM will be recorded.
No results filename given, not writing results

刚刚发生了什么事？

首先，即使你重定向了 stdout，你仍然会看到一些输出到控制台（来自 stderr）：

mlockall failed: Cannot allocate memory
Couldn't lock all cached virtual memory.
Pagefaults from reading pages not yet mapped into RAM will be recorded.

在演示程序的初始化阶段之后，它将尝试将所有缓存的内存锁定到 RAM 中，并使用 mlockall 防止将来动态内存分配。这是为了防止页面错误将大量新内存加载到 RAM 中。（有关更多信息，请参见实时设计文章。）

当这种情况发生时，演示将照常继续。在演示生成的 output.txt 文件的底部，您将看到执行期间遇到的页面错误数量。

rttest statistics:
  - Minor pagefaults: 20
  - Major pagefaults: 0

如果我们想让那些页面错误消失，我们将不得不..

调整内存锁定的权限

添加到 /etc/security/limits.conf （以 sudo 身份）：

<your username>    -   memlock   <limit in kB>

-1 的限制是无限的。如果选择此选项，编辑文件后可能需要使用 ulimit -l unlimited （作为 root）进行操作。

cxy@cxy-Ubuntu2404:~/Desktop$ sudo -i
[sudo] password for cxy: 
root@cxy-Ubuntu2404:~# ulimit -l unlimited
root@cxy-Ubuntu2404:~# exit
logout

保存文件后，注销并重新登录。然后重新运行 pendulum_demo 调用。

您将会在输出文件中看到零页错误，或者看到一个错误，提示捕获了 bad_alloc 异常。如果发生这种情况，说明您没有足够的可用内存将进程分配的内存锁定到 RAM 中。您需要在计算机中安装更多的 RAM 才能看到零页错误！

输出概述

要查看更多输出，我们必须运行 pendulum_logger 节点。

在一个 shell 中使用您的 install/setup.bash 源，调用：

pendulum_logger

cxy@cxy-Ubuntu2404:~/Desktop$ cd ~/ros2_jazzy
cxy@cxy-Ubuntu2404:~/ros2_jazzy$ source ~/.bashrc
cxy@cxy-Ubuntu2404:~/ros2_jazzy$ pendulum_logger
pendulum_logger: command not found
cxy@cxy-Ubuntu2404:~/ros2_jazzy$ ros2 run pendulum_control pendulum_logger
Logger node initialized.

您应该看到输出消息：

Logger node initialized.

在另一个已加载 setup.bash 的 shell 中，再次调用 pendulum_demo 。

一旦这个可执行文件启动，您应该会看到另一个 shell 不断打印输出：

Commanded motor angle: 1.570796
Actual motor angle: 1.570796
Mean latency: 210144.000000 ns
Min latency: 4805 ns
Max latency: 578137 ns
Minor pagefaults during execution: 0
Major pagefaults during execution: 0

演示控制一个非常简单的倒立摆模拟。倒立摆模拟在其自己的线程中计算其位置。一个 ROS 节点模拟倒立摆的电机编码器传感器并发布其位置。另一个 ROS 节点充当简单的 PID 控制器并计算下一个命令消息。

记录器节点在执行阶段定期打印出摆锤的状态和演示的运行时性能统计数据。

在 pendulum_demo 完成后，你需要按 CTRL-C 退出记录器节点以退出。

延迟

在 pendulum_demo 执行时，您将看到为演示收集的最终统计数据：

rttest statistics:
  - Minor pagefaults: 0
  - Major pagefaults: 0
  Latency (time after deadline was missed):
    - Min: 3354 ns
    - Max: 2752187 ns
    - Mean: 19871.8 ns
    - Standard deviation: 1.35819e+08


PendulumMotor received 985 messages
PendulumController received 987 messages

延迟字段显示更新循环的最小、最大和平均延迟（以纳秒为单位）。在这里，延迟是指更新预期发生后的时间量。

实时系统的要求取决于应用程序，但在这个演示中，我们假设有一个 1kHz（1 毫秒）的更新循环，并且我们的目标是最大允许延迟为更新周期的 5%。

所以，我们这次运行的平均延迟非常好，但最大延迟是不可接受的，因为它实际上超过了我们的更新循环！发生了什么事？

我们可能正在遭受非确定性调度程序的困扰。如果您运行的是原生 Linux 系统，并且没有安装 RT_PREEMPT 内核，您可能无法达到我们为自己设定的实时目标，因为 Linux 调度程序不允许您在用户级别任意抢占线程。

请参阅实时设计文章以获取更多信息。

演示尝试将演示的调度程序和线程优先级设置为适合实时性能。如果此操作失败，您将看到一条错误消息：“无法设置调度优先级和策略：操作不允许”。您可以通过遵循下一部分中的说明获得稍好的性能：

为调度程序设置权限

添加到 /etc/security/limits.conf （以 sudo 身份）：

<your username>    -   rtprio   98

rtprio（实时优先级）字段的范围是 0-99。然而，不要将限制设置为 99，因为这样您的进程可能会干扰以最高优先级运行的重要系统进程（例如看门狗）。此演示将尝试以优先级 98 运行控制循环。

绘图结果

您可以在演示运行后绘制在此演示中收集的延迟和页面错误统计信息。

由于代码已使用 rttest 进行了检测，因此有一些有用的命令行参数可用：

命令	描述	默认值
-i	指定运行实时循环的迭代次数	1000
-u	指定更新周期，默认单位为微秒。使用后缀“s”表示秒，“ms”表示毫秒 “us” 表示微秒，“ns” 表示纳秒。	1 毫秒
-f	指定用于写入收集数据的文件名。

再次运行演示并使用文件名保存结果：

pendulum_demo -f pendulum_demo_results

然后在生成的文件上运行 rttest_plot 脚本：

ros2 run rttest rttest_plot pendulum_demo_results

此脚本将生成多个文件：

pendulum_demo_results_plot_latency.svg
pendulum_demo_results_plot_latency_hist.svg
pendulum_demo_results_plot_majflts.svg
pendulum_demo_results_plot_minflts.svg

您可以在您选择的图像查看器中查看这些图表。

附录

pendulum_demo.h

这段代码定义了一个用于倒立摆的简单PID控制器。以下是代码的总结：

包含头文件和命名空间：代码包含了必要的头文件，并定义了M_PI常量以确保在Windows上可用。
PIDProperties结构体：用于存储PID控制器的属性，包括比例常数（p）、积分常数（i）、微分常数（d）和期望状态（command）。
PendulumController类：

构造函数：初始化控制器的更新周期和PID属性，计算时间步长（dt_），并设置初始命令消息。
on_sensor_message函数：根据传感器状态和PID控制器属性计算新的命令。包括比例、积分和微分增益的计算，并确保命令在合理范围内。
on_pendulum_setpoint函数：接收并设置新的摆锤设定点命令。
get_next_command_message函数：返回计算出的命令消息。
next_message_ready函数：检查命令消息是否已准备好。
get_publish_period函数：返回控制器的更新周期。
set_pid_properties函数：设置PID控制器的属性。
get_pid_properties函数：获取PID控制器的属性。
set_command函数：设置控制器的命令位置。
get_command函数：获取控制器的命令位置。

私有成员变量：包括控制器的更新周期、PID属性、命令消息、PID控制器的状态变量（如上次误差、积分增益和时间步长）。

这个控制器通过PID算法计算新的命令位置，并确保命令在合理范围内。

#ifndef PENDULUM_CONTROL__PENDULUM_CONTROLLER_HPP_  // 如果没有定义PENDULUM_CONTROL__PENDULUM_CONTROLLER_HPP_
#define PENDULUM_CONTROL__PENDULUM_CONTROLLER_HPP_  // 则定义PENDULUM_CONTROL__PENDULUM_CONTROLLER_HPP_


// Needed for M_PI on Windows
#ifdef _MSC_VER  // 如果是在微软的编译器环境下
#ifndef _USE_MATH_DEFINES  // 如果没有定义_USE_MATH_DEFINES
#define _USE_MATH_DEFINES  // 则定义_USE_MATH_DEFINES
#endif
#endif


#include <chrono>  // 引入<chrono>头文件，用于处理时间
#include <cmath>  // 引入<cmath>头文件，用于处理数学运算
#include <memory>  // 引入<memory>头文件，用于处理内存


#include "pendulum_msgs/msg/joint_command.hpp"  // 引入"pendulum_msgs/msg/joint_command.hpp"头文件
#include "pendulum_msgs/msg/joint_state.hpp"  // 引入"pendulum_msgs/msg/joint_state.hpp"头文件


namespace pendulum_control  // 定义pendulum_control命名空间
{


/// Struct for storing PID controller properties.
struct PIDProperties  // 定义PIDProperties结构体，用于存储PID控制器的属性
{
  /// Proportional constant.
  double p = 1;  // 比例常数
  /// Integral constant.
  double i = 0;  // 积分常数
  /// Derivative constant.
  double d = 0;  // 微分常数
  /// Desired state of the plant.
  double command = M_PI / 2;  // 设备的期望状态
};


/// Provides a simple PID controller for the inverted pendulum.
class PendulumController  // 定义PendulumController类，提供一个简单的倒立摆PID控制器
{
public:
  /// Default constructor.
  /**
   * \param[in] period The update period of the controller.
   * \param[in] pid The properties of the controller.
   */
  PendulumController(std::chrono::nanoseconds period, PIDProperties pid)  // 默认构造函数
  : publish_period_(period), pid_(pid),  // 初始化发布周期和PID属性
    message_ready_(false)  // 初始化消息准备状态为false
  {
    command_message_.position = pid_.command;  // 将PID命令赋值给命令消息的位置
    // Calculate the controller timestep (for discrete differentiation/integration).
    dt_ = publish_period_.count() / (1000.0 * 1000.0 * 1000.0);  // 计算控制器的时间步长（用于离散差分/积分）
    if (std::isnan(dt_) || dt_ == 0) {  // 如果时间步长是非数字或者等于0
      throw std::runtime_error("Invalid dt_ calculated in PendulumController constructor");  // 抛出运行时错误
    }
  }


  /// Calculate new command based on new sensor state and PID controller properties.
  // \param[in] msg Received sensor message.
  void on_sensor_message(pendulum_msgs::msg::JointState::ConstSharedPtr msg)  // 根据新的传感器状态和PID控制器属性计算新的命令
{
    ++messages_received;  // 接收到的消息数量加1


    if (std::isnan(msg->position)) {  // 如果接收到的消息位置是非数字
      throw std::runtime_error("Sensor value was NaN in on_sensor_message callback");  // 抛出运行时错误
    }
    // PID controller algorithm
    double error = pid_.command - msg->position;  // 计算误差
    // Proportional gain is proportional to error
    double p_gain = pid_.p * error;  // 比例增益与误差成比例
    // Integral gain is proportional to the accumulation of error
    i_gain_ = pid_.i * (i_gain_ + error * dt_);  // 积分增益与误差的积累成比例
    // Differential gain is proportional to the change in error
    double d_gain = pid_.d * (error - last_error_) / dt_;  // 微分增益与误差的变化成比例
    last_error_ = error;  // 更新最后的误差


    // Calculate the message based on PID gains
    command_message_.position = msg->position + p_gain + i_gain_ + d_gain;  // 根据PID增益计算消息
    // Enforce positional limits
    if (command_message_.position > M_PI) {  // 如果消息位置大于π
      command_message_.position = M_PI;  // 则将消息位置设置为π
    } else if (command_message_.position < 0) {  // 如果消息位置小于0
      command_message_.position = 0;  // 则将消息位置设置为0
    }


    if (std::isnan(command_message_.position)) {  // 如果结果命令是非数字
      throw std::runtime_error("Resulting command was NaN in on_sensor_message callback");  // 抛出运行时错误
    }
    message_ready_ = true;  // 设置消息准备状态为true
  }


  /// Callback when a pendulum JointCommand message is received.
  // \param[in] msg The incoming message containing the position.
  void on_pendulum_setpoint(pendulum_msgs::msg::JointCommand::ConstSharedPtr msg)  // 当接收到一个倒立摆JointCommand消息时的回调函数
{
    set_command(msg->position);  // 设置命令
    printf("Pendulum set to: %f\n", msg->position);  // 打印设置的倒立摆位置
    fflush(stdout);  // 刷新输出流
  }


  /// Retrieve the command calculated from the last sensor message.
  // \return Command message.
  const pendulum_msgs::msg::JointCommand & get_next_command_message() const  // 获取从最后一个传感器消息计算出的命令
{
    return command_message_;  // 返回命令消息
  }


  /// True if the command message has been initialized.
  // \return True if the message is ready.
  bool next_message_ready() const  // 如果命令消息已经初始化，则返回true
{
    return message_ready_;  // 返回消息准备状态
  }


  /// Get the update period of the controller.
  // \return Duration struct representing the update period in nanoseconds.
  std::chrono::nanoseconds get_publish_period() const  // 获取控制器的更新周期
{
    return publish_period_;  // 返回发布周期
  }


  /// Set the properties of the PID controller (gains and desired output).
  // \param[in] properties Struct representing the desired properties.
  void set_pid_properties(const PIDProperties & properties)  // 设置PID控制器的属性（增益和期望输出）
{
    pid_ = properties;  // 设置PID属性
  }


  /// Get the properties of the controller.
  // \return Struct representing the properties of the controller.
  const PIDProperties & get_pid_properties() const  // 获取控制器的属性
{
    return pid_;  // 返回PID属性
  }


  /// Set the commanded position of the controller.
  // \param[in] command The new commanded position (in radians).
  void set_command(double command)  // 设置控制器的命令位置
{
    pid_.command = command;  // 设置PID命令
  }


  /// Get the commanded position of the controller.
  // \return The commanded position.
  double get_command() const  // 获取控制器的命令位置
{
    return pid_.command;  // 返回PID命令
  }


  /// Count the number of messages received (number of times the callback fired).
  size_t messages_received = 0;  // 计数接收到的消息数量（回调函数触发的次数）


private:
  // controller should publish less frequently than the motor
  std::chrono::nanoseconds publish_period_;  // 控制器应该比电机发布的频率低
  PIDProperties pid_;  // PID属性
  pendulum_msgs::msg::JointCommand command_message_;  // 命令消息
  bool message_ready_;  // 消息准备状态


  // state for PID controller
  double last_error_ = 0;  // PID控制器的最后误差
  double i_gain_ = 0;  // 积分增益
  double dt_;  // 时间步长
};


}  // namespace pendulum_control


#endif  // PENDULUM_CONTROL__PENDULUM_CONTROLLER_HPP_

pendulum_motor.hpp

这段代码定义了一个 PendulumMotor 类，用于模拟摆的物理行为并根据接收到的命令控制其运动。以下是总结：

摆的属性和状态：

PendulumProperties：定义摆的质量和长度。
PendulumState：跟踪摆的位置、速度、加速度和扭矩。

PendulumMotor 类：

构造函数：初始化摆，设置更新周期和属性，并设置一个高优先级线程来运行物理更新循环。
on_command_message：根据接收到的命令更新摆的位置，并强制执行位置限制。
get_next_sensor_message：返回包含摆状态的最新传感器消息。
next_message_ready：检查下一条消息是否准备好发布。
set_done：信号物理引擎停止。
done：检查物理引擎是否在运行。
get_publish_period：返回发布者的更新速率。
get_position：返回摆的当前位置。
get_state：返回摆的当前状态。
set_state：设置摆的状态。
get_properties：返回摆的物理属性。
set_properties：设置摆的物理属性。
messages_received：统计接收到的消息数量。

物理更新：

在一个单独的高优先级线程中运行。
根据物理定律更新摆的加速度、速度和位置。
模拟传感器噪声并准备下一条传感器消息。

这个类旨在实时模拟摆的运动，响应控制命令并相应地更新其状态。

#ifndef PENDULUM_CONTROL__PENDULUM_MOTOR_HPP_  // 如果没有定义PENDULUM_CONTROL__PENDULUM_MOTOR_HPP_
#define PENDULUM_CONTROL__PENDULUM_MOTOR_HPP_  // 定义PENDULUM_CONTROL__PENDULUM_MOTOR_HPP_


// Needed for M_PI on Windows
#ifdef _MSC_VER  // 如果是在微软的编译器环境下
#ifndef _USE_MATH_DEFINES  // 如果没有定义_USE_MATH_DEFINES
#define _USE_MATH_DEFINES  // 定义_USE_MATH_DEFINES
#endif
#endif


#include <chrono>  // 包含<chrono>头文件，用于处理时间
#include <cmath>  // 包含<cmath>头文件，用于处理数学运算
#include <memory>  // 包含<memory>头文件，用于处理内存


#include "rttest/rttest.h"  // 包含"rttest/rttest.h"头文件
#include "rttest/utils.hpp"  // 包含"rttest/utils.hpp"头文件


#include "pendulum_msgs/msg/joint_command.hpp"  // 包含"pendulum_msgs/msg/joint_command.hpp"头文件
#include "pendulum_msgs/msg/joint_state.hpp"  // 包含"pendulum_msgs/msg/joint_state.hpp"头文件


#ifndef GRAVITY  // 如果没有定义GRAVITY
#define GRAVITY 9.80665  // 定义GRAVITY为9.80665
#endif


namespace pendulum_control  // 定义pendulum_control命名空间
{


/// Struct representing the physical properties of the pendulum.
struct PendulumProperties  // 定义PendulumProperties结构体，表示摆的物理属性
{
  /// Mass of the weight on the end of the pendulum in kilograms
  double mass = 0.01;  // 摆的质量，单位为千克
  /// Length of the pendulum in meters
  double length = 0.5;  // 摆的长度，单位为米
};


/// Struct representing the dynamic/kinematic state of the pendulum.
struct PendulumState  // 定义PendulumState结构体，表示摆的动态/运动状态
{
  /// Angle from the ground in radians
  double position = 0;  // 与地面的角度，单位为弧度
  /// Angular velocity in radians/sec
  double velocity = 0;  // 角速度，单位为弧度/秒
  /// Angular acceleration in radians/sec^2
  double acceleration = 0;  // 角加速度，单位为弧度/秒^2
  /// Torque on the joint (currently unused)
  double torque = 0;  // 关节的扭矩（当前未使用）
};


/// Represents the physical state of the pendulum, the controlling motor, and the position sensor.
class PendulumMotor  // 定义PendulumMotor类，表示摆的物理状态、控制电机和位置传感器
{
public:
  /// Default constructor.
  /**
   * \param[in] period Time between sending messages.
   * \param[in] properties Physical properties of the pendulum.
   */
  PendulumMotor(std::chrono::nanoseconds period, PendulumProperties properties)  // 默认构造函数
  : publish_period_(period), properties_(properties),
    physics_update_period_(std::chrono::nanoseconds(1000000)),
    message_ready_(false), done_(false)
  {
    // Calculate physics engine timestep.
    dt_ = physics_update_period_.count() / (1000.0 * 1000.0 * 1000.0);  // 计算物理引擎的时间步长
    uint64_to_timespec(physics_update_period_.count(), &physics_update_timespec_);


    // Initialize a separate high-priority thread to run the physics update loop.
    pthread_attr_init(&thread_attr_);  // 初始化线程属性
    sched_param thread_param;
    thread_param.sched_priority = 90;  // 设置线程优先级为90
    pthread_attr_setschedparam(&thread_attr_, &thread_param);  // 设置线程的调度参数
    pthread_attr_setschedpolicy(&thread_attr_, SCHED_RR);  // 设置线程的调度策略为SCHED_RR
    pthread_create(
      &physics_update_thread_, &thread_attr_,
      &pendulum_control::PendulumMotor::physics_update_wrapper, this);  // 创建一个新线程来运行物理更新循环
  }


  /// Update the position of motor based on the command.
  // \param[in] msg Received command.
  void on_command_message(pendulum_msgs::msg::JointCommand::ConstSharedPtr msg)  // 根据接收到的命令更新电机的位置
{
    ++messages_received;  // 接收到的消息数量加一
    // Assume direct, instantaneous position control
    // (It would be more realistic to simulate a motor model)
    state_.position = msg->position;  // 假设直接、瞬时的位置控制


    // Enforce position limits
    if (state_.position > M_PI) {  // 如果位置大于π
      state_.position = M_PI;  // 设置位置为π
    } else if (state_.position < 0) {  // 如果位置小于0
      state_.position = 0;  // 设置位置为0
    }


    if (std::isnan(state_.position)) {  // 如果位置是NaN
      throw std::runtime_error("Tried to set state to NaN in on_command_message callback");  // 抛出运行时错误
    }
  }


  /// Return the next sensor message calculated by the physics engine.
  // \return The sensor message
  const pendulum_msgs::msg::JointState & get_next_sensor_message() const  // 返回由物理引擎计算的下一个传感器消息
{
    return sensor_message_;
  }


  /// Get the status of the next message
  // \return True if the message is ready to be published.
  bool next_message_ready() const  // 获取下一个消息的状态
{
    return message_ready_;  // 如果消息准备好被发布，则返回true
  }


  /// Set the boolean to signal that the physics engine should finish.
  // \param[in] done True if the physics engine should stop.
  void set_done(bool done)  // 设置一个布尔值，表示物理引擎应该结束
{
    done_ = done;
  }


  /// Get the status of the physics engine.
  // \return True if the physics engine is running, false otherwise.
  bool done() const  // 获取物理引擎的状态
{
    return done_;  // 如果物理引擎正在运行，则返回true，否则返回false
  }


  /// Get the update rate of the publisher.
  // \return The update rate as a std::chrono::duration.
  std::chrono::nanoseconds get_publish_period() const  // 获取发布器的更新率
{
    return publish_period_;
  }


  /// Get the current position of the pendulum.
  // \return Position of the pendulum.
  double get_position() const  // 获取摆的当前位置
{
    return state_.position;
  }


  /// Get the current state of the pendulum.
  // \return State of the pendulum.
  PendulumState get_state() const  // 获取摆的当前状态
{
    return state_;
  }


  /// Set the state of the pendulum.
  // \param[in] state State to set.
  void set_state(const PendulumState & state)  // 设置摆的状态
{
    state_ = state;
  }


  /// Get the physical properties of the pendulum.
  // \return Properties of the pendulum.
  const PendulumProperties & get_properties() const  // 获取摆的物理属性
{
    return properties_;
  }


  /// Set the properties of the pendulum.
  // \param[in] properties Properties to set.
  void set_properties(const PendulumProperties & properties)  // 设置摆的属性
{
    properties_ = properties;
  }


  /// Count the number of messages received (number of times the callback fired).
  size_t messages_received = 0;  // 接收到的消息数量（回调函数被触发的次数）


private:
  static void * physics_update_wrapper(void * args)  // 静态函数，用于包装物理更新函数
{
    PendulumMotor * motor = static_cast<PendulumMotor *>(args);  // 将参数转换为PendulumMotor类型
    if (!motor) {  // 如果电机为空
      return NULL;  // 返回NULL
    }
    return motor->physics_update();  // 返回物理更新函数的结果
  }
  // Set kinematic and dynamic properties of the pendulum based on state inputs
  void * physics_update()  // 根据状态输入设置摆的运动学和动力学属性 
{
    rttest_lock_and_prefault_dynamic();  // 锁定并预处理动态内存
    while (!done_) {  // 当物理引擎没有结束时
      state_.acceleration = GRAVITY * std::sin(state_.position - M_PI / 2.0) / properties_.length +  // 计算加速度
        state_.torque / (properties_.mass * properties_.length * properties_.length);
      state_.velocity += state_.acceleration * dt_;  // 更新速度
      state_.position += state_.velocity * dt_;  // 更新位置
      if (state_.position > M_PI) {  // 如果位置大于π
        state_.position = M_PI;  // 设置位置为π
      } else if (state_.position < 0) {  // 如果位置小于0
        state_.position = 0;  // 设置位置为0
      }


      if (std::isnan(state_.position)) {  // 如果位置是NaN
        throw std::runtime_error("Tried to set state to NaN in on_command_message callback");  // 抛出运行时错误
      }


      sensor_message_.velocity = state_.velocity;  // 设置传感器消息的速度
      // Simulate a noisy sensor on position
      sensor_message_.position = state_.position;  // 模拟一个有噪声的位置传感器


      message_ready_ = true;  // 设置消息已经准备好
      // high resolution sleep
      clock_nanosleep(CLOCK_MONOTONIC, 0, &physics_update_timespec_, NULL);  // 高精度睡眠
    }
    return 0;
  }


  // motor should publish more frequently than the controller
  std::chrono::nanoseconds publish_period_;  // 发布器的更新频率应该比控制器更高


  // Physics should update most frequently, in separate RT thread
  timespec physics_update_timespec_;  // 物理更新应该最频繁地在单独的RT线程中进行
  double dt_;  // 时间步长


  // Physical qualities of the pendulum
  /*
       M
        \
         \ length
       p  \
     0 ----------- pi
   */


  PendulumProperties properties_;  // 摆的物理属性
  PendulumState state_;  // 摆的状态


  std::chrono::nanoseconds physics_update_period_;  // 物理更新的周期
  pendulum_msgs::msg::JointState sensor_message_;  // 传感器消息
  bool message_ready_;  // 消息是否准备好
  bool done_;  // 物理引擎是否结束


  pthread_t physics_update_thread_;  // 物理更新线程
  pthread_attr_t thread_attr_;  // 线程属性
};


}  // namespace pendulum_control


#endif  // PENDULUM_CONTROL__PENDULUM_MOTOR_HPP_

rtt_executor.hpp

这段代码定义了一个名为 RttExecutor 的类，它继承自 rclcpp::Executor，并将其与 rttest 库集成，用于实时测试。以下是代码的主要功能和结构：

类定义和构造函数：

RttExecutor 类继承自 rclcpp::Executor。
构造函数初始化了一些成员变量，并检查 rttest 是否正在运行。

成员函数：

is_running()：检查执行器是否正在运行。
set_rtt_results_message()：将执行器的实时测试结果填充到 RttestResults 消息中。
spin()：将执行器的 spin 函数包装到 rttest_spin 中，执行所有可用的工作，并在 rttest 完成后清理状态。
loop_callback()：执行器的核心组件，执行一些工作并更新额外的状态。

成员变量：

results：存储累积的性能统计数据。
results_available：指示结果是否可用。
running：指示执行器是否正在运行。
rttest_ready：指示 rttest 是否已初始化且尚未停止。
last_sample：存储最新的样本数据。

保护和私有成员：

start_time_：记录收集第一个数据点的绝对时间戳。
禁用复制构造函数和赋值操作符。

这个类的主要目的是在执行器的 spin 函数中集成实时测试功能，以便在执行过程中收集和报告性能数据。

#ifndef PENDULUM_CONTROL__RTT_EXECUTOR_HPP_  // 确保头文件只被包含一次
#define PENDULUM_CONTROL__RTT_EXECUTOR_HPP_


#include <cassert>   // 引入断言库
#include <cstdlib>   // 引入标准库函数
#include <cstring>   // 引入字符串处理函数
#include <memory>    // 引入智能指针库
#include <vector>    // 引入向量容器库


#include "rttest/rttest.h"  // 引入实时测试库
#include "rttest/utils.hpp" // 引入实时测试工具函数


#include "rmw/rmw.h"        // 引入 RMW 接口库


#include "rclcpp/executor.hpp"  // 引入 ROS 2 C++ 客户端库中的 Executor 类
#include "rclcpp/macros.hpp"    // 引入 ROS 2 C++ 客户端库中的宏定义
#include "rclcpp/memory_strategies.hpp"  // 引入 ROS 2 C++ 客户端库中的内存策略


namespace pendulum_control
{
/// 仪器化的执行器，将 Executor::spin 函数与 rttest_spin 同步。
class RttExecutor : public rclcpp::Executor
{
public:
  /// 构造函数。
  /**
   * 扩展默认的 Executor 构造函数。
   */
  RttExecutor(
    const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions())
  : rclcpp::Executor(options), running(false)
  {
    rttest_ready = rttest_running();  // 初始化时检查 rttest 是否正在运行
    memset(&start_time_, 0, sizeof(timespec));  // 清空 start_time_ 变量
  }


  /// 默认析构函数。
  virtual ~RttExecutor() {}


  /// 如果执行器当前正在运行，则返回 true。
  // \return 如果 rclcpp 正在运行且 "running" 布尔值为 true，则返回 true。
  bool is_running() const
{
    return rclcpp::ok() && running;  // 检查 rclcpp 是否正常运行以及 running 标志
  }


  /// 用执行器的数据填充 RttestResults 消息。
  /**
   * RttestResults 消息包含最新的延迟、平均延迟、最小和最大延迟、主要和次要页面错误的数量，以及当前时间。
   */
  // \param[out] msg 要填充的消息。
  bool set_rtt_results_message(pendulum_msgs::msg::RttestResults & msg) const
{
    if (!results_available) {  // 如果结果不可用，则返回 false
      return false;
    }
    msg.cur_latency = last_sample;  // 当前延迟
    msg.mean_latency = results.mean_latency;  // 平均延迟
    assert(results.min_latency >= 0);  // 确保最小延迟非负
    assert(results.max_latency >= 0);  // 确保最大延迟非负
    msg.min_latency = results.min_latency;  // 最小延迟
    msg.max_latency = results.max_latency;  // 最大延迟
    msg.minor_pagefaults = results.minor_pagefaults;  // 次要页面错误
    msg.major_pagefaults = results.major_pagefaults;  // 主要页面错误
    timespec curtime;
    clock_gettime(CLOCK_MONOTONIC, &curtime);  // 获取当前时间
    msg.stamp.sec = curtime.tv_sec;  // 秒数
    msg.stamp.nanosec = curtime.tv_nsec;  // 纳秒数


    return true;
  }


  /// 将 executor::spin 包装到 rttest_spin 中。
  // 执行所有可用的工作，直到 rttest 指定的迭代次数完成。
  void spin()
{
    // 此调用将阻塞，直到 rttest 完成，按命令行指定的周期调用 loop_callback
    rttest_spin(RttExecutor::loop_callback, static_cast<void *>(this));


    // 在 rttest 结束旋转后清理状态并写入结果
    running = false;
    rttest_write_results();  // 写入结果
    if (rttest_running()) {
      rttest_finish();  // 完成 rttest 运行
    }
    rttest_ready = rttest_running();  // 更新 rttest 的准备状态
  }


  /// 执行器的核心组件。做一点工作并更新额外的状态。
  // \param[in] 匿名参数，将被强制转换为指向 RttExecutor 的指针。
  static void * loop_callback(void * arg)
{
    // 强制转换参数以访问执行器的状态
    RttExecutor * executor = static_cast<RttExecutor *>(arg);
    // 如果输入指针为 NULL 或无效，或者 rclcpp 已停止，通知 rttest 停止
    if (!executor || !rclcpp::ok()) {
      rttest_finish();
      return 0;
    }
    // 单线程执行 spin_some：完成所有可用的工作
    executor->spin_some();


    // 检索到目前为止的 rttest 统计数据并将其存储在执行器中
    if (rttest_get_statistics(&executor->results) >= 0) {
      executor->results_available = true;
    }
    rttest_get_sample_at(executor->results.iteration, &executor->last_sample);  // 获取当前样本
    // 如果布尔值未设置，通知我们最近运行了回调
    executor->running = true;
    return 0;
  }


  /// 存储累计的性能统计数据。
  rttest_results results;
  /// 结果是否当前可用。
  bool results_available{false};
  /// 如果执行器正在旋转，则为 true。
  bool running;
  /// 如果 rttest 已初始化且尚未停止，则为 true。
  bool rttest_ready;


  /// 最新的样本，用于统计数据。
  int64_t last_sample;


protected:
  /// 在 rttest 中收集的第一个数据点的绝对时间戳。
  timespec start_time_;


private:
  RCLCPP_DISABLE_COPY(RttExecutor)  // 禁止拷贝构造和赋值
};


}  // namespace pendulum_control


#endif  // PENDULUM_CONTROL__RTT_EXECUTOR_HPP_

pendulum_demo.cpp

这段代码是一个实时控制系统的例子，它使用了ROS2（Robot Operating System 2）和rttest库。这个系统模拟了一个倒立摆的物理行为，并使用PID控制器来控制摆动。

以下是代码的主要部分的解释：

初始化阶段：在实时程序的初始化阶段，允许进行非实时安全操作，如内存分配。在这个阶段，创建了摆锤的物理属性结构，实例化了一个模拟倒立摆物理行为的PendulumMotor类，并为当前位置提供了一个传感器消息。
创建节点和发布者/订阅者：创建了两个节点，一个是"pendulum_controller"，代表用户代码，实现了一个简单的PID控制器；另一个是"pendulum_motor"，模拟电机和传感器，提供传感器数据，并根据命令改变物理模型。然后，初始化了传感器消息的发布者和命令消息的订阅者。
设置执行器：初始化了一个特殊的单线程执行器RttExecutor，该执行器被设计为计算和记录实时性能统计数据。然后，将电机和控制器节点添加到执行器中。
创建定时器：添加了几个定时器，以定期发布传感器消息、命令消息和结果消息。
设置线程优先级：将此线程的优先级设置为最大安全值，并将其调度策略设置为确定性（实时安全）算法，即循环调度。
锁定虚拟内存：将当前缓存的虚拟内存锁定到RAM中，以及任何未来的内存分配，并尽最大努力预先故障锁定的内存，以防止未来的页面错误。
执行阶段：与默认的SingleThreadedExecutor::spin函数不同，RttExecutor::spin在有界时间内运行（对于rttest参数指定的迭代次数）。一旦执行器退出，通知物理模拟停止运行。
结束执行阶段：打印PendulumMotor和PendulumController接收到的消息数量，然后关闭ROS2节点。

// 引入unistd.h库，它是POSIX操作系统API的一部分，提供了许多UNIX系统服务的访问功能。
#include <unistd.h>


// 引入C++标准库中的chrono（时间库）、cstdio（C标准输入和输出库）和memory（内存库）。
#include <chrono>
#include <cstdio>
#include <memory>


// 引入ROS实时测试库。
#include "rttest/rttest.h"


// 引入ROS C++库，以及消息池内存策略和分配器内存策略。
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/strategies/message_pool_memory_strategy.hpp"
#include "rclcpp/strategies/allocator_memory_strategy.hpp"


// 引入TLSF（两级分离适应性内存分配器）库。
#include "tlsf_cpp/tlsf.hpp"


// 引入pendulum_msgs消息类型，包括关节命令、关节状态和实时测试结果。
#include "pendulum_msgs/msg/joint_command.hpp"
#include "pendulum_msgs/msg/joint_state.hpp"
#include "pendulum_msgs/msg/rttest_results.hpp"


// 引入pendulum_control库，包括倒立摆控制器、倒立摆电机和实时测试执行器。
#include "pendulum_control/pendulum_controller.hpp"
#include "pendulum_control/pendulum_motor.hpp"
#include "pendulum_control/rtt_executor.hpp"


// 使用rclcpp::strategies::message_pool_memory_strategy::MessagePoolMemoryStrategy和
// rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy命名空间。
using rclcpp::strategies::message_pool_memory_strategy::MessagePoolMemoryStrategy;
using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy;


// 定义TLSFAllocator模板，使用tlsf_heap_allocator。
template<typename T = void>
using TLSFAllocator = tlsf_heap_allocator<T>;


// 主函数
int main(int argc, char * argv[])
{
  // 初始化阶段。
  // 在实时程序的初始化阶段，允许进行非实时安全的操作，如内存分配。


  // 创建一个具有默认物理属性（长度和质量）的倒立摆结构。
  pendulum_control::PendulumProperties properties;
  // 实例化一个PendulumMotor类，该类模拟倒立摆的物理特性，并提供当前位置的传感器消息。
  // 使电机的回调运行速度略快于执行器更新循环。
  auto pendulum_motor = std::make_shared<pendulum_control::PendulumMotor>(
    std::chrono::nanoseconds(970000), properties);


  // 创建PID控制器的属性。
  pendulum_control::PIDProperties pid;
  // 实例化一个PendulumController类，该类将计算下一个电机命令。
  // 使控制器的回调运行速度略快于执行器更新循环。
  auto pendulum_controller = std::make_shared<pendulum_control::PendulumController>(
    std::chrono::nanoseconds(960000), pid);


  // 将输入参数传递给rttest。
  // rttest将存储相关参数并分配数据收集缓冲区
  rttest_read_args(argc, argv);


  // 将输入参数传递给rclcpp并初始化信号处理器。
  rclcpp::init(argc, argv);


  // MessagePoolMemoryStrategy预先分配一个消息池，供订阅使用。
  // 通常，每种订阅类型都使用一个MessagePoolMemoryStrategy，消息池的大小由线程数决定
  // （订阅的最大并发访问数）。
  // 由于此示例是单线程的，我们为每种策略选择一个消息池大小为1。
  auto state_msg_strategy =
    std::make_shared<MessagePoolMemoryStrategy<pendulum_msgs::msg::JointState, 1>>();
  auto command_msg_strategy =
    std::make_shared<MessagePoolMemoryStrategy<pendulum_msgs::msg::JointCommand, 1>>();
  auto setpoint_msg_strategy =
    std::make_shared<MessagePoolMemoryStrategy<pendulum_msgs::msg::JointCommand, 1>>();


  // 控制器节点代表用户代码。此示例实现了一个简单的PID控制器。
  auto controller_node = rclcpp::Node::make_shared("pendulum_controller");


  // “电机”节点模拟电机和传感器。
  // 它提供传感器数据，并根据命令改变物理模型。
  auto motor_node = rclcpp::Node::make_shared("pendulum_motor");


  // 服务质量配置文件调整为实时性能。
  // 未来可能会通过rmw接口公开更多的QoS设置，以满足实时要求。
  auto qos = rclcpp::QoS(
    // “KEEP_LAST”历史设置告诉DDS在发送值之前存储固定大小的缓冲区，以便在消息丢失的情况下进行恢复。
    // “depth”指定此缓冲区的大小。
    // 在此示例中，我们优化性能和有限的资源使用（防止页面错误），而不是可靠性。因此，我们将历史缓冲区的大小设置为1。
    rclcpp::KeepLast(1)
  );
  // 从http://www.opendds.org/qosusages.html：“RELIABLE设置可能会在尝试发送时阻塞。”
  // 因此，将策略设置为最大努力，以避免在执行过程中阻塞。
  qos.best_effort();


  // 初始化传感器消息（倒立摆的当前位置）的发布器。
  auto sensor_pub =
    motor_node->create_publisher<pendulum_msgs::msg::JointState>("pendulum_sensor", qos);


  // 创建一个lambda函数，当收到命令时调用电机回调。
  auto motor_subscribe_callback =
    &pendulum_motor -> void
    {
      pendulum_motor->on_command_message(msg);
    };


  // 初始化命令消息的订阅。
  // 注意，我们传递了上面初始化的MessagePoolMemoryStrategy<JointCommand>。
  auto command_sub = motor_node->create_subscription<pendulum_msgs::msg::JointCommand>(
    "pendulum_command", qos, motor_subscribe_callback,
    rclcpp::SubscriptionOptions(), command_msg_strategy);


  // 创建一个lambda函数，当收到命令时调用控制器回调。
  auto controller_subscribe_callback =
    &pendulum_controller -> void
    {
      pendulum_controller->on_sensor_message(msg);
    };


  // 初始化命令消息的发布器。
  auto command_pub = controller_node->create_publisher<pendulum_msgs::msg::JointCommand>(
    "pendulum_command", qos);


  // 初始化传感器消息的订阅。
  // 注意，我们传递了上面初始化的MessageMemoryPoolStrategy<JointState>。
  auto sensor_sub = controller_node->create_subscription<pendulum_msgs::msg::JointState>(
    "pendulum_sensor", qos, controller_subscribe_callback,
    rclcpp::SubscriptionOptions(), state_msg_strategy);


  // 创建一个lambda函数，接受用户输入以命令倒立摆
  auto controller_command_callback =
    &pendulum_controller -> void
    {
      pendulum_controller->on_pendulum_setpoint(msg);
    };


  // 接收teleop节点发布器最近发布的消息。
  auto qos_setpoint_sub = qos;
  qos_setpoint_sub.transient_local();


  auto setpoint_sub = controller_node->create_subscription<pendulum_msgs::msg::JointCommand>(
    "pendulum_setpoint", qos_setpoint_sub, controller_command_callback,
    rclcpp::SubscriptionOptions(), setpoint_msg_strategy);


  // 初始化日志发布器。
  auto logger_pub = controller_node->create_publisher<pendulum_msgs::msg::RttestResults>(
    "pendulum_statistics", qos);
  std::chrono::nanoseconds logger_publisher_period(1000000);


  // 初始化执行器。
  rclcpp::ExecutorOptions options;
  // 传递给执行器的一个参数是内存策略，它将运行时执行分配委托给TLSF分配器。
  rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy =
    std::make_shared<AllocatorMemoryStrategy<TLSFAllocator<void>>>();
  options.memory_strategy = memory_strategy;
  // RttExecutor是一个特殊的单线程执行器，被仪器化以计算和记录实时性能统计数据。
  auto executor = std::make_shared<pendulum_control::RttExecutor>(options);


  // 将电机和控制器节点添加到执行器。
  executor->add_node(motor_node);
  executor->add_node(controller_node);


  // 创建一个lambda函数，将定期触发以发布下一个传感器消息。
  auto motor_publish_callback =
    &sensor_pub, &pendulum_motor
    {
      if (pendulum_motor->next_message_ready()) {
        auto msg = pendulum_motor->get_next_sensor_message();
        sensor_pub->publish(msg);
      }
    };


  // 创建一个lambda函数，将定期触发以发布下一个命令消息。
  auto controller_publish_callback =
    &command_pub, &pendulum_controller
    {
      if (pendulum_controller->next_message_ready()) {
        auto msg = pendulum_controller->get_next_command_message();
        command_pub->publish(msg);
      }
    };


  // 创建一个lambda函数，将定期触发以发布下一个结果消息。
  auto logger_publish_callback =
    &logger_pub, &executor, &pendulum_motor, &pendulum_controller {
      pendulum_msgs::msg::RttestResults results_msg;
      if (!executor->set_rtt_results_message(results_msg)) {
        // 如果没有数据可用，就直接退出，而不是发布错误的数据。
        return;
      }
      results_msg.command = pendulum_controller->get_next_command_message();
      results_msg.state = pendulum_motor->get_next_sensor_message();
      logger_pub->publish(results_msg);
    };


  // 添加一个定时器以启用传感器消息的定期发布。
  auto motor_publisher_timer = motor_node->create_wall_timer(
    pendulum_motor->get_publish_period(), motor_publish_callback);
  // 添加一个定时器以启用命令消息的定期发布。
  auto controller_publisher_timer = controller_node->create_wall_timer(
    pendulum_controller->get_publish_period(), controller_publish_callback);
  // 添加一个定时器以启用结果消息的定期发布。
  auto logger_publisher_timer = controller_node->create_wall_timer(
    logger_publisher_period, logger_publish_callback);


  // 将此线程的优先级设置为最大安全值，并将其调度策略设置为确定性（实时安全）算法，即轮询。
  if (rttest_set_sched_priority(98, SCHED_RR)) {
    perror("无法设置调度优先级和策略");
  }


  // 将当前缓存的虚拟内存锁定到RAM中，以及任何未来的内存分配，并尽最大努力预先故障锁定的内存，以防止未来的页面错误。
  // 如果出现问题（资源或权限不足），将返回非零错误代码。
  // 始终在初始化阶段的最后一步执行此操作。
  // 有关设置权限的说明，请参阅README.md。
  // 有关更多详细信息，请参阅rttest/rttest.cpp。
  if (rttest_lock_and_prefault_dynamic() != 0) {
    fprintf(stderr, "无法锁定所有缓存的虚拟内存。\n");
    fprintf(stderr, "从读取尚未映射到RAM的页面中记录页面错误。\n");
  }


  // 结束初始化阶段


  // 与默认的SingleThreadedExecutor::spin函数不同，RttExecutor::spin在有限的时间内运行
  // （根据rttest参数指定的迭代次数）。
  executor->spin();
  // 一旦执行器退出，通知物理模拟停止运行。
  pendulum_motor->set_done(true);


  // 结束执行阶段


  printf("PendulumMotor接收到%zu条消息\n", pendulum_motor->messages_received);
  printf("PendulumController接收到%zu条消息\n", pendulum_controller->messages_received);


  rclcpp::shutdown();


  return 0;
}

这个程序的目标是展示如何在ROS2中实现实时性能，包括使用实时安全的内存策略、定时器和执行器，以及如何使用rttest库来测试和记录实时性能数据。这个例子是单线程的，但是这些技术也可以扩展到多线程应用中。

#include <unistd.h>  // 包含了Unix标准函数库


#include <chrono>  // 包含了时间库
#include <cstdio>  // 包含了C标准输入输出库
#include <memory>  // 包含了智能指针库


#include "rttest/rttest.h"  // 实时测试库


#include "rclcpp/rclcpp.hpp"  // ROS2的核心库
#include "rclcpp/strategies/message_pool_memory_strategy.hpp"  // 消息池内存策略
#include "rclcpp/strategies/allocator_memory_strategy.hpp"  // 分配器内存策略


#include "tlsf_cpp/tlsf.hpp"  // TLSF内存分配器


#include "pendulum_msgs/msg/joint_command.hpp"  // 机械臂控制消息
#include "pendulum_msgs/msg/joint_state.hpp"  // 机械臂状态消息
#include "pendulum_msgs/msg/rttest_results.hpp"  // 实时测试结果消息


#include "pendulum_control/pendulum_controller.hpp"  // 机械臂控制器
#include "pendulum_control/pendulum_motor.hpp"  // 机械臂电机
#include "pendulum_control/rtt_executor.hpp"  // 实时测试执行器


using rclcpp::strategies::message_pool_memory_strategy::MessagePoolMemoryStrategy;  // 使用消息池内存策略
using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy;  // 使用分配器内存策略


template<typename T = void>
using TLSFAllocator = tlsf_heap_allocator<T>;  // 使用TLSF内存分配器


int main(int argc, char * argv[])
{
  // 初始化阶段
  // 在实时程序的初始化阶段，允许进行非实时安全操作，如内存分配


  // 创建一个具有摆锤默认物理属性（长度和质量）的结构
  pendulum_control::PendulumProperties properties;
  // 实例化一个PendulumMotor类，该类模拟倒立摆的物理行为
  // 并为当前位置提供一个传感器消息
  // 让电机的回调比执行器更新循环稍快一些
  auto pendulum_motor = std::make_shared<pendulum_control::PendulumMotor>(
    std::chrono::nanoseconds(970000), properties);


  // 创建PID控制器的属性
  pendulum_control::PIDProperties pid;
  // 实例化一个PendulumController类，该类将计算下一个电机命令
  // 让控制器的回调比执行器更新循环稍快一些
  auto pendulum_controller = std::make_shared<pendulum_control::PendulumController>(
    std::chrono::nanoseconds(960000), pid);


  // 将输入参数传递给rttest
  // rttest将存储相关参数并分配数据收集缓冲区
  rttest_read_args(argc, argv);


  // 将输入参数传递给rclcpp并初始化信号处理器
  rclcpp::init(argc, argv);


  // MessagePoolMemoryStrategy预先分配一个消息池，供订阅使用
  // 通常，每种订阅类型使用一个MessagePoolMemoryStrategy，消息池的大小由线程数决定（并发访问订阅的最大数量）
  // 由于这个例子是单线程的，我们为每个策略选择一个消息池大小为1
  auto state_msg_strategy =
    std::make_shared<MessagePoolMemoryStrategy<pendulum_msgs::msg::JointState, 1>>();
  auto command_msg_strategy =
    std::make_shared<MessagePoolMemoryStrategy<pendulum_msgs::msg::JointCommand, 1>>();
  auto setpoint_msg_strategy =
    std::make_shared<MessagePoolMemoryStrategy<pendulum_msgs::msg::JointCommand, 1>>();


  // 控制器节点代表用户代码，这个例子实现了一个简单的PID控制器
  auto controller_node = rclcpp::Node::make_shared("pendulum_controller");


  // “电机”节点模拟电机和传感器
  // 它提供传感器数据，并根据命令改变物理模型
  auto motor_node = rclcpp::Node::make_shared("pendulum_motor");


  // 服务质量配置文件被调整为实时性能
  // 未来可能会有更多的QoS设置通过rmw接口暴露出来，以满足实时性要求
  auto qos = rclcpp::QoS(
    // "KEEP_LAST"历史设置告诉DDS在发送值之前存储固定大小的缓冲区，以便在消息丢失的情况下进行恢复
    // "depth"指定了这个缓冲区的大小
    // 在这个例子中，我们优化性能和有限的资源使用（防止页面错误），而不是可靠性。因此，我们将历史缓冲区的大小设置为1
    rclcpp::KeepLast(1)
  );
  // 来自http://www.opendds.org/qosusages.html: "RELIABLE设置可能会在尝试发送时阻塞。"因此，将策略设置为尽力而为，以避免在执行过程中阻塞
  qos.best_effort();


  // 初始化传感器消息的发布者（摆锤的当前位置）
  auto sensor_pub =
    motor_node->create_publisher<pendulum_msgs::msg::JointState>("pendulum_sensor", qos);


  // 创建一个lambda函数，当收到命令时调用电机回调
  auto motor_subscribe_callback =
    &pendulum_motor -> void
    {
      pendulum_motor->on_command_message(msg);
    };


  // 初始化命令消息的订阅者
  // 注意我们传递了上面初始化的MessagePoolMemoryStrategy<JointCommand>
  auto command_sub = motor_node->create_subscription<pendulum_msgs::msg::JointCommand>(
    "pendulum_command", qos, motor_subscribe_callback,
    rclcpp::SubscriptionOptions(), command_msg_strategy);


  // 创建一个lambda函数，当收到命令时调用控制器回调
  auto controller_subscribe_callback =
    &pendulum_controller -> void
    {
      pendulum_controller->on_sensor_message(msg);
    };


  // 初始化命令消息的发布者
  auto command_pub = controller_node->create_publisher<pendulum_msgs::msg::JointCommand>(
    "pendulum_command", qos);


  // 初始化传感器消息的订阅者
  // 注意我们传递了上面初始化的MessageMemoryPoolStrategy<JointState>
  auto sensor_sub = controller_node->create_subscription<pendulum_msgs::msg::JointState>(
    "pendulum_sensor", qos, controller_subscribe_callback,
    rclcpp::SubscriptionOptions(), state_msg_strategy);


  // 创建一个lambda函数，接受用户输入来命令摆锤
  auto controller_command_callback =
    &pendulum_controller -> void
    {
      pendulum_controller->on_pendulum_setpoint(msg);
    };


  // 接收teleop节点发布者最近发布的消息
  auto qos_setpoint_sub = qos;
  qos_setpoint_sub.transient_local();


  auto setpoint_sub = controller_node->create_subscription<pendulum_msgs::msg::JointCommand>(
    "pendulum_setpoint", qos_setpoint_sub, controller_command_callback,
    rclcpp::SubscriptionOptions(), setpoint_msg_strategy);


  // 初始化日志发布者
  auto logger_pub = controller_node->create_publisher<pendulum_msgs::msg::RttestResults>(
    "pendulum_statistics", qos);
  std::chrono::nanoseconds logger_publisher_period(1000000);


  // 初始化执行器
  rclcpp::ExecutorOptions options;
  // 传递给执行器的一个参数是内存策略，它将运行时执行分配委托给TLSF分配器
  rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy =
    std::make_shared<AllocatorMemoryStrategy<TLSFAllocator<void>>>();
  options.memory_strategy = memory_strategy;
  // RttExecutor是一个特殊的单线程执行器，被设计为计算和记录实时性能统计数据
  auto executor = std::make_shared<pendulum_control::RttExecutor>(options);


  // 将电机和控制器节点添加到执行器
  executor->add_node(motor_node);
  executor->add_node(controller_node);


  // 创建一个lambda函数，将定期触发以发布下一个传感器消息
  auto motor_publish_callback =
    &sensor_pub, &pendulum_motor
    {
      if (pendulum_motor->next_message_ready()) {
        auto msg = pendulum_motor->get_next_sensor_message();
        sensor_pub->publish(msg);
      }
    };


  // 创建一个lambda函数，将定期触发以发布下一个命令消息
  auto controller_publish_callback =
    &command_pub, &pendulum_controller
    {
      if (pendulum_controller->next_message_ready()) {
        auto msg = pendulum_controller->get_next_command_message();
        command_pub->publish(msg);
      }
    };


  // 创建一个lambda函数，将定期触发以发布下一个结果消息
  auto logger_publish_callback =
    &logger_pub, &executor, &pendulum_motor, &pendulum_controller {
      pendulum_msgs::msg::RttestResults results_msg;
      if (!executor->set_rtt_results_message(results_msg)) {
        // 没有数据可用，只是退出而不是发布错误数据
        return;
      }
      results_msg.command = pendulum_controller->get_next_command_message();
      results_msg.state = pendulum_motor->get_next_sensor_message();
      logger_pub->publish(results_msg);
    };


  // 添加一个定时器，以启用定期发布传感器消息
  auto motor_publisher_timer = motor_node->create_wall_timer(
    pendulum_motor->get_publish_period(), motor_publish_callback);
  // 添加一个定时器，以启用定期发布命令消息
  auto controller_publisher_timer = controller_node->create_wall_timer(
    pendulum_controller->get_publish_period(), controller_publish_callback);
  // 添加一个定时器，以启用定期发布结果消息
  auto logger_publisher_timer = controller_node->create_wall_timer(
    logger_publisher_period, logger_publish_callback);


  // 将此线程的优先级设置为最大安全值，并将其调度策略设置为确定性（实时安全）算法，即循环调度
  if (rttest_set_sched_priority(98, SCHED_RR)) {
    perror("Couldn't set scheduling priority and policy");
  }


  // 将当前缓存的虚拟内存锁定到RAM中，以及任何未来的内存分配，并尽最大努力预先故障锁定的内存，以防止未来的页面错误
  // 如果出了问题（资源或权限不足），将返回非零错误代码
  // 总是在初始化阶段的最后一步做这个
  // 参见README.md获取设置权限的说明
  // 参见rttest/rttest.cpp获取更多细节
  if (rttest_lock_and_prefault_dynamic() != 0) {
    fprintf(stderr, "Couldn't lock all cached virtual memory.\n");
    fprintf(stderr, "Pagefaults from reading pages not yet mapped into RAM will be recorded.\n");
  }


  // 结束初始化阶段


  // 与默认的SingleThreadedExecutor::spin函数不同，RttExecutor::spin在有界时间内运行（对于rttest参数指定的迭代次数）
  executor->spin();
  // 一旦执行器退出，通知物理模拟停止运行
  pendulum_motor->set_done(true);


  // 结束执行阶段


  printf("PendulumMotor received %zu messages\n", pendulum_motor->messages_received);
  printf("PendulumController received %zu messages\n", pendulum_controller->messages_received);


  rclcpp::shutdown();


  return 0;
}

pendulum_logger.cpp

这段代码实现了一个用于记录摆锤实验结果的非实时安全节点。以下是代码的总结：

引入头文件：

包含必要的标准库和ROS 2库头文件。

主函数：

设置标准输出缓冲区为NULL。
初始化ROS 2。
创建一个名为pendulum_logger的节点。

日志记录回调函数：

定义一个回调函数，用于处理接收到的RttestResults消息，并将相关信息打印到控制台。

质量服务配置：

配置质量服务（QoS）以优化实时性能。
使用KEEP_LAST策略存储固定大小的值缓冲区，并设置为best_effort策略以避免在执行期间阻塞。

创建订阅者：

创建一个订阅者，订阅pendulum_statistics主题，并使用定义的回调函数处理接收到的消息。

启动节点：

打印日志节点初始化消息。
使用rclcpp::spin函数运行节点，以处理传入的消息。
关闭ROS 2。

这段代码的主要功能是接收并记录摆锤实验的实时测试结果，包括电机角度、延迟和页面错误等信息。

#include <cinttypes>  // 包含了一些用于打印整数类型的宏
#include <fstream>  // 包含了文件流类，用于文件操作
#include <string>  // 包含了字符串类


#include "rclcpp/rclcpp.hpp"  // ROS2的核心库


#include "rttest/utils.hpp"  // 实时测试工具库


#include "pendulum_msgs/msg/joint_command.hpp"  // 机械臂控制消息
#include "pendulum_msgs/msg/joint_state.hpp"  // 机械臂状态消息
#include "pendulum_msgs/msg/rttest_results.hpp"  // 实时测试结果消息


// 非实时安全节点，用于日志记录（文件IO，控制台输出）


int main(int argc, char * argv[])
{
  setbuf(stdout, NULL);  // 设置stdout的缓冲区为空，使得printf可以立即输出
  rclcpp::init(argc, argv);  // 初始化ROS2节点


  auto logger_node = rclcpp::Node::make_shared("pendulum_logger");  // 创建一个名为"pendulum_logger"的节点


  auto logging_callback =  // 定义一个回调函数，用于处理接收到的消息
     {
      printf("Commanded motor angle: %f\n", msg->command.position);  // 打印命令的电机角度
      printf("Actual motor angle: %f\n", msg->state.position);  // 打印实际的电机角度


      printf("Current latency: %" PRIu64 " ns\n", msg->cur_latency);  // 打印当前的延迟
      printf("Mean latency: %f ns\n", msg->mean_latency);  // 打印平均延迟
      printf("Min latency: %" PRIu64 " ns\n", msg->min_latency);  // 打印最小延迟
      printf("Max latency: %" PRIu64 " ns\n", msg->max_latency);  // 打印最大延迟


      printf("Minor pagefaults during execution: %" PRIu64 "\n", msg->minor_pagefaults);  // 打印执行过程中的次要页面错误
      printf("Major pagefaults during execution: %" PRIu64 "\n\n", msg->major_pagefaults);  // 打印执行过程中的主要页面错误
    };


  // 服务质量配置文件被调整为实时性能
  // 未来可能会有更多的QoS设置通过rmw接口暴露出来，以满足实时性要求
  auto qos =
    // "KEEP_LAST"历史设置告诉DDS在发送值之前存储固定大小的缓冲区，以便在消息丢失的情况下进行恢复
    rclcpp::QoS(rclcpp::KeepLast(100))
    // 来自http://www.opendds.org/qosusages.html: "RELIABLE设置可能会在尝试发送时阻塞。"因此，将策略设置为尽力而为，以避免在执行过程中阻塞
    .best_effort();


  auto subscription = logger_node->create_subscription<pendulum_msgs::msg::RttestResults>(  // 创建一个订阅者，订阅"pendulum_statistics"主题
    "pendulum_statistics", qos, logging_callback);


  printf("Logger node initialized.\n");  // 打印节点初始化完成的信息
  rclcpp::spin(logger_node);  // 让节点开始处理事件


  rclcpp::shutdown();  // 关闭ROS2节点


  return 0;  // 程序正常退出
}

pendulum_teleop.cpp

这段代码是一个简单的ROS 2节点，用于发布一个用户指定的摆锤设定点。以下是代码的总结：

包含头文件和命名空间：代码包含了必要的头文件，并使用了std::chrono_literals命名空间来处理时间字面量。
主函数：

初始化ROS 2。
设置默认命令值为90度（π/2弧度）。如果提供了命令参数，则使用该参数值。
创建一个名为“pendulum_teleop”的ROS 2节点。
设置一个QoS配置文件，保留最后10条消息，具有瞬态本地性和可靠性。
创建一个消息，并设置其位置为指定的命令值。
等待500毫秒，发布消息，并旋转节点以处理回调。
打印确认消息，再等待一秒钟，然后关闭ROS 2。

这个节点设计用于仅发布一次用户指定的摆锤设定点。如果未指定命令，则默认为90度。

#include <chrono>  // 引入C++标准库中的时间库
#include <cmath>  // 引入C++标准库中的数学库
#include <fstream>  // 引入C++标准库中的文件流库
#include <memory>  // 引入C++标准库中的内存库
#include <utility>  // 引入C++标准库中的实用程序库


#include "rclcpp/rclcpp.hpp"  // 引入ROS 2中的rclcpp库


#include "rttest/utils.hpp"  // 引入实时测试工具库


#include "pendulum_msgs/msg/joint_command.hpp"  // 引入摆锤消息类型


using namespace std::chrono_literals;  // 使用C++标准库中的时间字面量


// 非实时安全节点，用于发布用户指定的摆锤设定点，仅发布一次


int main(int argc, char * argv[])  // 主函数
{
  rclcpp::init(argc, argv);  // 初始化ROS节点


  double command = M_PI / 2;  // 默认命令为π/2
  if (argc < 2) {  // 如果没有提供命令参数
    fprintf(
      stderr,
      "Command argument not specified. Setting command to 90 degrees (PI/2 radians).\n");  // 输出错误信息
  } else {
    command = atof(argv[1]);  // 否则，将命令参数转换为浮点数
  }


  auto teleop_node = rclcpp::Node::make_shared("pendulum_teleop");  // 创建一个名为"pendulum_teleop"的节点


  auto qos = rclcpp::QoS(rclcpp::KeepLast(10)).transient_local().reliable();  // 设置QoS策略


  auto pub =
    teleop_node->create_publisher<pendulum_msgs::msg::JointCommand>("pendulum_setpoint", qos);  // 创建一个发布者，发布到"pendulum_setpoint"主题


  auto msg = std::make_unique<pendulum_msgs::msg::JointCommand>();  // 创建一个新的JointCommand消息
  msg->position = command;  // 设置消息的位置字段为命令值


  rclcpp::sleep_for(500ms);  // 等待500毫秒
  pub->publish(std::move(msg));  // 发布消息
  rclcpp::spin_some(teleop_node);  // 处理一些回调
  printf("Teleop message published.\n");  // 输出消息已发布的信息
  rclcpp::sleep_for(1s);  // 等待1秒
  printf("Teleop node exited.\n");  // 输出节点已退出的信息


  rclcpp::shutdown();  // 关闭ROS


  return 0;  // 返回0表示程序正常结束
}

十年一梦实验室

关注

1
点赞
踩
3

收藏

觉得还不错? 一键收藏
0
评论
【ROS2】演示:理解实时编程

目录背景安装并运行演示运行测试刚刚发生了什么事？调整内存锁定权限输出概述延迟为调度程序设置权限绘图结果背景实时计算是许多机器人系统的关键特性，特别是自动驾驶汽车、航天器和工业制造等安全和任务关键型应用。我们在设计和原型开发 ROS 2 时考虑了实时性能约束，因为这是在 ROS 1 的早期阶段没有考虑到的需求，现在要将 ROS 1 重构为实时友好已经变得难以处理。本文档概述了实时计算的...
复制链接

扫一扫