系列文章目录

---

前言

双积分器示例是我们最简单的问题。它模拟了一个沿 x 方向移动的一维点质量。模型是线性的，成本函数是二次函数。目标点通过参考管理器模块设置为二次成本。

---

一、查看文件结构

1.1 ocs2_double_integrator 文件夹

.
├── auto_generated
├── CMakeLists.txt
├── config
│   └── mpc
│       └── task.info
├── include
│   └── ocs2_double_integrator
│       ├── definitions.h
│       ├── DoubleIntegratorInterface.h
│       ├── DoubleIntegratorPyBindings.h
│       └── package_path.h.in
├── package.xml
├── setup.py
├── src
│   ├── DoubleIntegratorInterface.cpp
│   ├── ocs2_double_integrator
│   │   └── __init__.py
│   └── pyBindModule.cpp
└── test
    ├── DoubleIntegratorNoRosIntegrationTest.cpp
    ├── DoubleIntegratorPyBindingTest.cpp
    └── DoubleIntegratorPyBindingTest.py

8 directories, 14 files

1.2 ocs2_double_integrator_ros 文件夹

.
├── CMakeLists.txt
├── include
│   └── ocs2_double_integrator_ros
│       └── DoubleIntegratorDummyVisualization.h
├── launch
│   ├── double_integrator.launch
│   ├── multiplot.launch
│   └── visualize.launch
├── package.xml
├── rviz
│   └── double_integrator.rviz
└── src
    ├── DoubleIntegratorDummyVisualization.cpp
    ├── DoubleIntegratorMpcNode.cpp
    ├── DoubleIntegratorTargetPoseCommand.cpp
    └── DummyDoubleIntegratorNode.cpp

5 directories, 11 files

二、ocs2_double_integrator 文件夹

2.1 config/mpc/task.info

这段代码是一个模型预测控制（MPC）的设置代码。其中包括了模式序列、模板模式序列、DDP设置、 rollout设置、MPC设置、初始状态、状态权重矩阵、控制权重矩阵、最终状态权重矩阵和最终目标。这些设置参数控制了MPC的运行过程和目标函数的优化。

这段代码详细定义了一个动态规划（Dynamic Programming，通过DDP实现）优化的模型预测控制（Model Predictive Control, MPC）系统参数。以下是各项设置的具体含义：

1. subsystemsSequence和templateSubsystemsSequence：这两个数组可能用于定义系统在不同模式下的行为序列或模板，但在这个例子中它们只包含一个元素[0]，没有具体信息。

2. DDP设置：

   • 使用SLQ算法进行动态规划求解。
   • 设置了单线程计算、最大迭代次数、最小相对成本改进阈值以及约束容忍度等参数来控制DDP优化过程。
   • 通过AbsTolODE和RelTolODE设定了解常微分方程的绝对误差和相对误差容忍度，timeStep为时间步长，maxNumStepsPerSecond限制了每秒的最大步数。
   • 启用了反馈策略，并设置了线搜索策略，包括最小步长、最大步长、海森矩阵修正策略及其修正倍数。

3. rollout设置：与DDP类似，这些参数用于仿真系统的滚动输出，如ODE求解器的精度要求和时间步长。

4. MPC设置：

   • timeHorizon指定了MPC的预测时域（2.5秒）。
   • 指定了冷启动（coldStart）状态及调试打印选项。
   • 设定了MPC期望的更新频率和实时调节器（MRT）的期望频率。

5. initialState：定义了系统的初始状态，即x1和x2的初始值均为0。

6. Q, R, Q_final：这三个矩阵分别代表状态权重矩阵、控制权重矩阵和最终状态权重矩阵，它们被用来构建MPC问题的成本函数，以平衡状态误差、控制输入大小以及最终状态到达目标区域的重要性。

7. finalGoal：定义了MPC问题的目标状态，即希望系统在结束时达到的状态（x1=2.0, x2=0.0）。

; Mode sequence
subsystemsSequence
{
  [0]     0
}
; Template mode sequence
templateSubsystemsSequence
{
  [0]     0
}
templateSwitchingTimes
{
}

; DDP settings
ddp
{
  algorithm                    SLQ    算法选择为SLQ

  nThreads                     1        线程数为1

  maxNumIterations             1        最大迭代次数为1
  minRelCost                   0.1        相对成本的最小值为0.1
  constraintTolerance          1e-3        约束条件的容忍度为1e-3

  displayInfo                  false    
  displayShortSummary          false    
  debugPrintRollout            false    
  checkNumericalStability      true

  AbsTolODE                    1e-9    ODE的绝对容忍度为1e-9
  RelTolODE                    1e-6    ODE的相对容忍度为1e-6
  timeStep                     1e-3    时间步长为1e-3
  maxNumStepsPerSecond         100000    每秒最大步数为100000
  backwardPassIntegratorType   ODE45    回溯积分器类型为ODE45

  preComputeRiccatiTerms       true    预计算Riccati项为真

  useFeedbackPolicy            true    使用反馈策略为真
  
  strategy                     LINE_SEARCH    策略为线搜索
  lineSearch
  {
    minStepLength              0.1        最小步长为0.1
    maxStepLength              1.0        最大步长为1.0
    hessianCorrectionStrategy  EIGENVALUE_MODIFICATION    Hessian修正策略为EIGENVALUE_MODIFICATION
    hessianCorrectionMultiple  1e-3        Hessian修正倍数为1e-3 
  }
}

; Rollout settings
rollout
{
  AbsTolODE                    1e-9    ODE的绝对容忍度为1e-9
  RelTolODE                    1e-6    ODE的相对容忍度为1e-6
  timeStep                     1e-2    时间步长为1e-2
  maxNumStepsPerSecond         100000    每秒最大步数为100000
  checkNumericalStability      true    检查数值稳定性为真
}

; MPC settings
mpc
{
  timeHorizon                 2.5  ; [s]    时间窗口为2.5秒
  solutionTimeWindow          -1   ; maximum [s]    最大时间为负值
  coldStart                   false        冷启动为假

  debugPrint                  false

  mpcDesiredFrequency         100  ; [Hz]    MPC期望频率为100Hz
  mrtDesiredFrequency         400  ; [Hz]    MRT期望频率为400Hz
}


; initial state
initialState
{
  (0,0) 0.0    ; x1    x1的初始值为0 (1,0) 0.0
  (1,0) 0.0    ; x2    x2的初始值为0 
}

; state weight matrix
Q
{
  scaling 1e-1
  (0,0)  1.0 ; x1        x1的权重为1.0
  (1,1)  1.0 ; x2        x2的权重为1.0
}


; control weight matrix
R
{
  scaling 1e-2
  (0,0)  1.0            控制的权重为1.0
}

; final state weight matrix
Q_final
{
  scaling 1e+1
  (0,0)  2.7064  ; x11    x11的权重为2.7064
  (0,1)  3.1623  ; x12    x12的权重为3.1623
  (1,0)  3.1623  ; x21    x21的权重为3.1623 
  (1,1)  8.5584  ; x22    x22的权重为8.5584
}

; final goal
finalGoal
{
  (0,0) 2.0    ; x1    x1的目标值为2
  (1,0) 0.0    ; x2    x2的目标值为0
}

2.2  include/ocs2_double_integrator/definitions.h

这段代码定义了一个名为double_integrator的命名空间，其中定义了两个静态常量STATE_DIM和INPUT_DIM，分别表示状态向量的维度为2，输入向量的维度为1。

这段C++代码片段声明了一个名为ocs2::double_integrator的命名空间，该命名空间主要用于与二阶双积分器相关的数学模型或系统。在该命名空间内定义了两个静态常量：

1. static constexpr size_t STATE_DIM = 2; 这行代码定义了一个编译时常量STATE_DIM，其类型为size_t（无符号整型），表示系统的状态维度。在这里，状态维度被设定为2，意味着双积分器系统在描述其动态行为时，需要一个包含两个元素的状态向量。

2. static constexpr size_t INPUT_DIM = 1; 同样地，这行代码也定义了一个编译时常量INPUT_DIM，同样为size_t类型，表示系统的输入维度。这里，输入维度被设定为1，意味着控制这个二阶双积分器系统只需要一个单元素的输入向量。

总结起来，这段代码为一个二阶双积分器系统提供了基本的维度信息，其中状态向量有2个元素，而控制输入向量有1个元素。这些信息通常用于设计控制系统算法、定义系统动力学模型等场景。

// 版权所有 (c) 2017, Farbod Farshidian。保留所有权利。

// 允许通过源代码或二进制形式的重新分发和使用，条件是以下条件得到满足：
// 
// * 在分发源代码时必须保留以上版权声明、条件列表和以下弃权声明。
// * 在分发二进制形式时必须在文档ation和/或其他随软件提供的材料中重现以上版权声明、条件列表和以下弃权声明。
// 
// 未获得版权所有者的特定事先书面许可，版权所有者和贡献者的名称或名称不能用于支持或促进从该软件派生的产品的推广或促销。
// 
// 本软件按“原样”提供，如有明示或暗示的保证，包括但不限于 merchantability 和特定目的适用性方面的保证，但并不限于这些保证。在任何情况下，版权所有者和贡献者都不对任何直接、间接、偶发、特殊、示例或后果性损害（包括但不限于采购替代商品或服务、使用或数据丢失）负责，无论这种损害是由于使用、数据、利润损失还是由于中断业务活动而造成的，即使已事先被告知这种损害的可能性。
// 
//******************************************************************************/
#pragma once

#include <cstddef>

// 命名空间声明
namespace ocs2 {
namespace double_integrator {

    // 定义状态维度
    static constexpr size_t STATE_DIM = 2;
    // 定义输入维度
    static constexpr size_t INPUT_DIM = 1;

}  // 命名空间 double_integrator
}  // 命名空间 ocs2

2.3 include/ocs2_double_integrator/DoubleIntegratorInterface.h

这段代码定义了一个名为DoubleIntegratorInterface的类，继承自RobotInterface类。该类是用于解决双积分问题（Double Integrator）的接口类，提供了一些成员函数和成员变量，包括初始化函数、获取初始状态和目标函数、获取DDP和MPC的设置、获取优化问题、获取参考管理器和 rollout等功能。该类可用于构建双积分问题的OCS2（Optimal Control System 2）模型。

这段C++代码定义了一个名为DoubleIntegratorInterface的类，该类继承自RobotInterface基类，且为final类型（不可被继承）。这个类主要针对一个双积分器系统的建模和控制问题，并通过OCS2框架进行实现。

构造函数：

• DoubleIntegratorInterface(const std::string& taskFile, const std::string& libraryFolder, bool verbose = true); 接收三个参数：
  • taskFile：指向MPC配置文件的绝对路径。
  • libraryFolder：用于生成CppAD库的目录的绝对路径。
  • verbose：一个布尔值，默认为true，表示是否以详细模式加载设置。

成员方法：

• getInitialState()：返回系统的初始状态，存储在initialState_变量中。
• getInitialTarget()：返回最终目标状态，存储在finalGoal_变量中。
• ddpSettings()：返回DDP（Dual Dynamic Programming）求解器的相关设置，存储在ddpSettings_对象中，用户可以修改这些设置。
• mpcSettings()：返回MPC（Model Predictive Control）控制器的相关设置，存储在mpcSettings_对象中，用户也可以修改这些设置。
• getOptimalControlProblem()：返回一个常量引用指向最优控制问题实例problem_，该实例包含了描述双积分系统动力学、成本函数等信息。
• getReferenceManagerPtr()：返回一个指向参考管理器实例的智能指针，用于处理参考轨迹。
• getRollout()：返回一个常量引用指向rolloutPtr_，它是一个实现了时间触发式 rollout策略的对象。
• getInitializer()：返回一个常量引用指向线性系统初始化器实例，用于对控制系统进行初始化。

私有成员变量：

• ddpSettings_ 和 mpcSettings_ 分别保存了DDP和MPC算法的设置参数。
• problem_ 是一个最优控制问题实例，包含双积分系统的具体模型信息。
• referenceManagerPtr_ 是一个指向参考管理器对象的智能指针。
• rolloutPtr_ 指向一个实现Rollout策略的对象，用于模拟系统动态行为。
• linearSystemInitializerPtr_ 指向一个用于初始化系统的对象。
• initialState_ 和 finalGoal_ 分别存储了系统的初始状态和期望达到的目标状态。

总之，这个类封装了与解决双积分器系统相关的各种功能组件，如最优控制问题的描述、求解器设置、参考轨迹管理、系统初始化以及仿真滚动过程。通过此类，可以在OCS2框架下有效地搭建并解决双积分器系统的最优控制问题。

// DoubleIntegratorInterface 类：机器人接口类，用于双积分问题
class DoubleIntegratorInterface final : public RobotInterface {
 public:
  /**
   * 构造函数
   * @param [in] taskFile: MPC的配置文件的绝对路径。
   * @param [in] libraryFolder: 生成CppAD库的绝对路径。
   * @param [in] verbose: 加载设置时是否显示详细信息。
   */
  DoubleIntegratorInterface(const std::string& taskFile, const std::string& libraryFolder, bool verbose = true);

  /** 析构函数 */
  ~DoubleIntegratorInterface() override = default;

  /**
   * 获取初始状态
   * @return 初始状态vector_t的引用
   */
  const vector_t& getInitialState() { return initialState_; }

  /**
   * 获取初始目标
   * @return 初始目标vector_t的引用
   */
  const vector_t& getInitialTarget() { return finalGoal_; }

  /**
   * 获取DDP设置
   * @return ddp::Settings的引用
   */
  ddp::Settings& ddpSettings() { return ddpSettings_; }

  /**
   * 获取MPC设置
   * @return mpc::Settings的引用
   */
  mpc::Settings& mpcSettings() { return mpcSettings_; }

  /**
   * 获取最优控制问题
   * @return OptimalControlProblem的常量引用
   */
  const OptimalControlProblem& getOptimalControlProblem() const override { return problem_; }

  /**
   * 获取引用管理器的指针
   * @return std::shared_ptr<ReferenceManagerInterface>的常量引用
   */
  std::shared_ptr<ReferenceManagerInterface> getReferenceManagerPtr() const override { return referenceManagerPtr_; }

  /**
   * 获取 rollout
   * @return RolloutBase的常量引用
   */
  const RolloutBase& getRollout() const { return *rolloutPtr_; }

  /**
   * 获取初始化器
   * @return Initializer的常量引用
   */
  const Initializer& getInitializer() const override { return *linearSystemInitializerPtr_; }

 private:
  ddp::Settings ddpSettings_;
  mpc::Settings mpcSettings_;

  OptimalControlProblem problem_;
  std::shared_ptr<ReferenceManager> referenceManagerPtr_;

  std::unique_ptr<RolloutBase> rolloutPtr_;
  std::unique_ptr<Initializer> linearSystemInitializerPtr_;

  vector_t initialState_;  // 初始状态
  vector_t finalGoal_;    // 初始目标
};

2.4 include/ocs2_double_integrator/DoubleIntegratorPyBindings.h

该cpp函数是一个类的构造函数。它接受三个参数：taskFile是配置MPC的文件路径，libraryFolder是生成C++库的目录路径，urdfFile是机器人模型的URDF文件路径（对于双积分控制器不需要使用）。该函数初始化系统的维度，创建DoubleIntegratorInterface对象，并使用该对象创建一个GaussNewtonDDP_MPC对象。最后，调用父类PythonInterface的初始化函数。

该cpp代码定义了一个名为DoubleIntegratorPyBindings的C++类，它是ocs2::PythonInterface类的final子类。这个类主要用于为双积分器系统提供与Python接口交互的功能。

在DoubleIntegratorPyBindings类的构造函数中：

1. 接收三个参数：

   • taskFile：一个字符串类型的参数，表示MPC（Model Predictive Control）配置文件的绝对路径。
   • libraryFolder：同样是一个字符串类型的参数，用于指定生成CppAD库的目录的绝对路径。
   • urdfFile：也是一个字符串类型参数，默认为空，通常用于机器人系统的URDF（Unified Robot Description Format）模型文件，但在这个双积分器场景下并不使用。

2. 初始化系统状态维度（stateDim_）和输入维度（inputDim_），分别从预定义的宏STATE_DIM和INPUT_DIM转换而来。

3. 创建一个DoubleIntegratorInterface对象，通过传入taskFile和libraryFolder来初始化双积分器系统的相关设置、滚动优化和最优控制问题等信息。

4. 使用上述创建的DoubleIntegratorInterface对象中的各项属性，构建一个GaussNewtonDDP_MPC智能指针对象。GaussNewtonDDP_MPC是用于解决MPC问题的一种基于高斯-牛顿法的动态规划算法实现。

5. 设置GaussNewtonDDP_MPC对象内部的solver引用管理器，关联到DoubleIntegratorInterface提供的参考管理器。

6. 最后，调用基类ocs2::PythonInterface的初始化方法init，将DoubleIntegratorInterface对象和刚构建的GaussNewtonDDP_MPC智能指针传递给它，完成Python接口的初始化工作，以便后续在Python环境中对双积分器系统进行MPC相关的操作和控制。

#pragma once

#include <ocs2_ddp/GaussNewtonDDP_MPC.h>
#include <ocs2_python_interface/PythonInterface.h>

#include "ocs2_double_integrator/DoubleIntegratorInterface.h"
#include "ocs2_double_integrator/definitions.h"

namespace ocs2 {
namespace double_integrator {

class DoubleIntegratorPyBindings final : public PythonInterface {
 public:
  /**
   * 构造函数
   *
   * @note 创建生成库的目录（如果目录不存在的话）。
   * @throw Invalid argument error 如果输入的MPC配置文件不存在。
   *
   * @param [in] taskFile: MPC的配置文件的绝对路径。
   * @param [in] libraryFolder: 生成库的目录的绝对路径。
   * @param [in] urdfFile: 机器人URDF文件的绝对路径。对于双积分控制器来说这个参数没有用。
   */
  DoubleIntegratorPyBindings(const std::string& taskFile, const std::string& libraryFolder, const std::string urdfFile = "") {
    // 系统维数
    stateDim_ = static_cast<int>(STATE_DIM);
    inputDim_ = static_cast<int>(INPUT_DIM);

    // 机器人接口
    DoubleIntegratorInterface doubleIntegratorInterface(taskFile, libraryFolder);

    // MPC
    auto mpcPtr = std::make_unique<GaussNewtonDDP_MPC>(
        doubleIntegratorInterface.mpcSettings(), doubleIntegratorInterface.ddpSettings(), doubleIntegratorInterface.getRollout(),
        doubleIntegratorInterface.getOptimalControlProblem(), doubleIntegratorInterface.getInitializer());
    mpcPtr->getSolverPtr()->setReferenceManager(doubleIntegratorInterface.getReferenceManagerPtr());

    // Python接口
    PythonInterface::init(doubleIntegratorInterface, std::move(mpcPtr));
  }
};

}  // namespace double_integrator
}  // namespace ocs2

2.5 include/ocs2_double_integrator/package_path.h.in

这是一个C++函数，用于获取包源代码目录的路径。它返回一个字符串，其中包含包的源代码目录的路径。 

该C++函数是定义在ocs2::double_integrator命名空间下的一个内联函数，名为getPath()。其主要功能是从预定义的变量@PROJECT_SOURCE_DIR@获取当前项目的源代码目录路径，并以字符串形式返回。

这个函数可能是用于一个更大的软件项目中，当需要引用项目特定的资源文件或其他与源码相关的路径时，可以通过调用此函数来得到正确的路径信息。

注意：

• inline关键字提示编译器尝试将函数体插入到每个调用处，以减少函数调用开销。
• @PROJECT_SOURCE_DIR@是一个通常在构建系统（如CMake）中定义的变量，表示项目源代码根目录的实际路径。在编译阶段，构建系统会将其替换为实际路径。

总结来说，这是一个方便开发者在运行时获取项目源代码根目录路径的实用函数。

// 导入所需头文件
#include <cstddef>
#include <string>

// 定义命名空间
namespace ocs2 {
    namespace double_integrator {

        // 获取包的源代码目录路径
        inline std::string getPath() {
            return "@PROJECT_SOURCE_DIR@";
        }

    }  // namespace double_integrator
}  // namespace ocs2

2.6 src/ocs2_double_integrator/__init__.py

这段代码从名为"DoubleIntegratorPyBindings"的模块中导入了四个对象：mpc_interface、scalar_array、vector_array和TargetTrajectories。

这段Python代码导入了与"ocs2_double_integrator"库相关的几个关键组件：

1. mpc_interface：这是一个接口类，提供了与OCS2（Optimization-based Control, Simulation, and Estimation）库中双积分器模型的Model Predictive Control (MPC)算法进行交互的方法。通过这个接口，可以设置参数、目标轨迹，并执行MPC计算以获取控制输入。

2. scalar_array、vector_array 和 matrix_array：这三个类型通常代表在OCS2库中处理的不同类型的数组对象，分别对应标量数组、向量数组和矩阵数组。在优化和仿真过程中，它们用于存储和操作数值数据，如状态、控制输入或系统参数。

3. TargetTrajectories：这可能是一个类或者一个数据结构，用于表示期望的目标状态或输入轨迹。它包含了在一段时间内系统的理想状态、控制输入或其他相关变量的时间序列信息，这些信息会被用作MPC算法的优化目标或约束条件。

总结来说，这段代码是为了实现对基于优化的双积分器模型进行预测控制所需的核心工具和数据结构。

from ocs2_double_integrator.DoubleIntegratorPyBindings import mpc_interface
from ocs2_double_integrator.DoubleIntegratorPyBindings import scalar_array, vector_array, matrix_array, TargetTrajectories

2.7 src/DoubleIntegratorInterface.cpp

该函数是DoubleIntegratorInterface类的构造函数，接受三个参数：taskFile（表示任务文件路径的字符串），libraryFolder（表示用于生成库文件的文件夹路径）和verbose（一个布尔值，表示是否打印详细输出）。构造函数通过加载任务文件、如果不存在则创建库文件夹、设置默认初始条件和最终目标、加载优化设置、创建所需类和对象的实例，并为最优控制问题设置问题参数，从而初始化DoubleIntegratorInterface对象。

该构造函数属于名为DoubleIntegratorInterface的C++类，它主要用于初始化一个双积分器系统的接口实例。在构造过程中，首先检查提供的taskFile（任务文件）是否存在，并将其内容加载到程序中，包括初始状态(initialState_)、最终目标(finalGoal_)以及DDP和MPC设置(ddpSettings_和mpcSettings_)。

接下来，根据libraryFolder参数创建或确认库文件夹路径的存在，并将此信息输出至标准错误流cerr。

然后从taskFile中加载用于计算成本函数的成本矩阵Q、R和Qf，并使用这些矩阵构建二次状态输入成本(QuadraticStateInputCost)和二次状态最终成本(QuadraticStateCost)，分别添加到问题实例(problem_)的costPtr和finalCostPtr中。

同时，构造函数为系统定义了线性动态模型，其状态转移矩阵A和控制输入矩阵B通过硬编码设定。

此外，根据taskFile中的rollout部分加载滚动优化设置，并创建一个TimeTriggeredRollout实例(rolloutPtr_)，用于执行系统的滚动优化过程。

最后，创建一个DefaultInitializer实例(linearSystemInitializerPtr_)，用于对系统进行初始化设置，这里考虑的是INPUT_DIM维度的线性系统。

总之，这个构造函数负责读取配置文件、初始化关键对象与参数，并为基于双积分器的动力学系统建立最优控制框架。

/******************************************************************************
版权 (c) 2017, Farbod Farshidian. 所有权利保留。

根据以下条件分配.redist ribution和二进制形式的修改，如果满足以下条件：

* 保留上面的版权通知，这个列表条件和以下放弃声明。

* 二进制形式的重分配必须重现上述的版权通知，这个列表条件和以下放弃声明在文档和/或其他与分配有关的材料中。

* 不能用从本软件派生的产品来加强或推广产品，而没有特定的先前书面许可。

本软件由版权人和/或其贡献者“因为是”分发。

本软件包括附带的文档资料，都是“因为是”，没有任何明确或暗示的保证，包括但不仅限于销售性以及适用于某一特定用途，从法律角度出发，版权人和/或其贡献者对本软件所造成的利润损失，间接性损害，附带损害，特别损害或后果性损害不承担任何责任。

 Generated by NetBeans IDE.
******************************************************************************/
#include <iostream>
#include <string>

#include "ocs2_double_integrator/DoubleIntegratorInterface.h"

#include <ocs2_core/cost/QuadraticStateCost.h>
#include <ocs2_core/cost/QuadraticStateInputCost.h>
#include <ocs2_core/dynamics/LinearSystemDynamics.h>
#include <ocs2_core/initialization/DefaultInitializer.h>
#include <ocs2_core/misc/LoadData.h>

// Boost
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/path.hpp>

namespace ocs2 {
namespace double_integrator {

/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
/**
 * @brief DoubleIntegratorInterface类的构造函数
 *
 * @param taskFile 任务文件的路径
 * @param libraryFolder 库文件夹的路径
 * @param verbose 是否显示详细信息
 */
DoubleIntegratorInterface::DoubleIntegratorInterface(const std::string& taskFile, const std::string& libraryFolder, bool verbose) {
  // 检查任务文件是否存在
  boost::filesystem::path taskFilePath(taskFile);
  if (boost::filesystem::exists(taskFilePath)) {
    std::cerr << "[DoubleIntegratorInterface] 加载任务文件: " << taskFilePath << std::endl;
  } else {
    throw std::invalid_argument("[DoubleIntegratorInterface] 未找到任务文件: " + taskFilePath.string());
  }
  
  // 创建库文件夹，如果不存在的话
  boost::filesystem::path libraryFolderPath(libraryFolder);
  boost::filesystem::create_directories(libraryFolderPath);
  std::cerr << "[DoubleIntegratorInterface] 生成的库路径: " << libraryFolderPath << std::endl;
  
  // 默认的初始状态和最终目标
  loadData::loadEigenMatrix(taskFile, "initialState", initialState_);
  loadData::loadEigenMatrix(taskFile, "finalGoal", finalGoal_);
  
  // DDP-MPC设置
  ddpSettings_ = ddp::loadSettings(taskFile, "ddp", verbose);
  mpcSettings_ = mpc::loadSettings(taskFile, "mpc", verbose);
  
  /*
   * ReferenceManager & SolverSynchronizedModule
   */
  referenceManagerPtr_.reset(new ReferenceManager);
  
  /*
   * 最优化控制问题
   */
  // 代价函数
  matrix_t Q(STATE_DIM, STATE_DIM);
  matrix_t R(INPUT_DIM, INPUT_DIM);
  matrix_t Qf(STATE_DIM, STATE_DIM);
  loadData::loadEigenMatrix(taskFile, "Q", Q);
  loadData::loadEigenMatrix(taskFile, "R", R);
  loadData::loadEigenMatrix(taskFile, "Q_final", Qf);
  std::cerr << "Q:  \n" << Q << "\n";
  std::cerr << "R:  \n" << R << "\n";
  std::cerr << "Q_final:\n" << Qf << "\n";
  
  problem_.costPtr->add("cost", std::make_unique<QuadraticStateInputCost>(Q, R));
  problem_.finalCostPtr->add("finalCost", std::make_unique<QuadraticStateCost>(Qf));
  
  // 动态
  const matrix_t A = (matrix_t(STATE_DIM, STATE_DIM) << 0.0, 1.0, 0.0, 0.0).finished();
  const matrix_t B = (matrix_t(STATE_DIM, INPUT_DIM) << 0.0, 1.0).finished();
  problem_.dynamicsPtr.reset(new LinearSystemDynamics(A, B));
  
  // 延时触发的 rollout
  auto rolloutSettings = rollout::loadSettings(taskFile, "rollout", verbose);
  rolloutPtr_.reset(new TimeTriggeredRollout(*problem_.dynamicsPtr, rolloutSettings));
  
  // 初始化
  linearSystemInitializerPtr_.reset(new DefaultInitializer(INPUT_DIM));
}

}  // namespace double_integrator
}  // namespace ocs2

2.8 src/pyBindModule.cpp

这段C++代码是利用ocs2库中的宏定义CREATE_ROBOT_PYTHON_BINDINGS来为ocs2::double_integrator命名空间下的DoubleIntegratorPyBindings类创建Python接口绑定。通过这种方式，C++编写的ocs2::double_integrator::DoubleIntegratorPyBindings类的功能可以被Python程序直接调用和使用。

具体来说：

1. #include <ocs2_double_integrator/DoubleIntegratorPyBindings.h>：引入了ocs2库中与双积分器相关的C++类的头文件，其中包含了要转换为Python接口的DoubleIntegratorPyBindings类的声明和实现。

2. #include <ocs2_python_interface/PybindMacros.h>：包含了一系列用于生成Python绑定的宏定义和工具函数。

3. CREATE_ROBOT_PYTHON_BINDINGS(ocs2::double_integrator::DoubleIntegratorPyBindings, DoubleIntegratorPyBindings)：这个宏会自动将ocs2::double_integrator::DoubleIntegratorPyBindings类的所有公共成员函数、枚举以及其他公开API封装并暴露给Python环境，使得在编写Python代码时可以直接使用一个名为DoubleIntegratorPyBindings的模块来访问和操作这些功能。

总之，这段代码的目的在于桥接C++与Python，让Python程序员能够方便地在Python脚本中调用和控制C++实现的ocs2::double_integrator::DoubleIntegratorPyBindings类的相关功能。

#include <ocs2_double_integrator/DoubleIntegratorPyBindings.h>
#include <ocs2_python_interface/PybindMacros.h>

CREATE_ROBOT_PYTHON_BINDINGS(ocs2::double_integrator::DoubleIntegratorPyBindings, DoubleIntegratorPyBindings)

// 包含头文件《ocs2_double_integrator/DoubleIntegratorPyBindings.h》
// 包含头文件《ocs2_python_interface/PybindMacros.h》

// 创建机器人Python绑定
CREATE_ROBOT_PYTHON_BINDINGS(ocs2::double_integrator::DoubleIntegratorPyBindings, DoubleIntegratorPyBindings)

2.9 test/DoubleIntegratorNoRosIntegrationTest.cpp

这段代码是一个用于二阶积分器系统的C++测试类。它通过使用高斯-牛顿DDP（直接 collocation）MPC（模型预测控制）算法，测试该系统同步和异步跟踪的能力。该类初始化二阶积分器接口，设置MPC参数，并通过运行MPC迭代以及评估得到的策略来执行跟踪测试。测试的目标是检查系统状态是否能在一定误差范围内收敛到期望的目标状态。

这段C++代码定义了一个名为DoubleIntegratorIntegrationTest的测试类，用于测试针对二阶积分器系统的模型预测控制（MPC）算法。该类在初始化时会加载一个任务配置文件，并创建一个DoubleIntegratorInterface实例，该实例封装了与二阶积分器系统相关的物理模型、最优控制问题和参考管理器等信息。

测试类中有三个主要测试方法：

1. synchronousTracking：同步跟踪测试。该测试首先通过调用getMpc(true)获取一个MPC对象，并设置初始观测状态。然后在指定的时间区间内不断执行MPC循环，每次迭代后更新当前观测状态为优化后的状态，并在循环结束后检查最终状态是否接近预设的目标状态。

2. coldStartMPC：冷启动MPC测试。类似于同步跟踪测试，但这里的MPC对象是通过调用getMpc(false)获取的，表示从非初始解开始求解（即冷启动），并设置了较少的DDP迭代次数。同样地，测试结束后验证最终状态与目标状态之间的差距是否满足给定的容差要求。

3. asynchronousTracking（仅在非调试模式下运行）：异步跟踪测试。这里同样获取一个MPC对象，但在主程序中以更高的频率运行实时（Real-Time, MRT）部分，并在一个单独线程中以较低频率运行MPC计算。测试过程中，实时部分根据接收到的最新策略更新系统状态，并在线程中持续进行MPC计算。最后验证系统是否能在规定时间内达到目标状态。

以上所有测试均使用高斯-牛顿DDP MPC算法，并设定了一定的误差容忍度，以检验算法对二阶积分器系统的控制性能。

#include <cmath>

#include <gtest/gtest.h>

#include <ocs2_double_integrator/DoubleIntegratorInterface.h>
#include <ocs2_double_integrator/package_path.h>

#include <ocs2_core/thread_support/ExecuteAndSleep.h>
#include <ocs2_ddp/GaussNewtonDDP_MPC.h>
#include <ocs2_mpc/MPC_MRT_Interface.h>

using namespace ocs2;
using namespace double_integrator;

// 配置DoubleIntegratorIntegrationTest类为testing::Test的子类
class DoubleIntegratorIntegrationTest : public testing::Test {
 protected:
  // DoubleIntegratorIntegrationTest类的构造函数
  DoubleIntegratorIntegrationTest() {
    const bool verbose = false;
    const std::string taskFile = ocs2::double_integrator::getPath() + "/config/mpc/task.info";
    const std::string libFolder = ocs2::double_integrator::getPath() + "/auto_generated";
    doubleIntegratorInterfacePtr.reset(new DoubleIntegratorInterface(taskFile, libFolder, verbose));

    initState = doubleIntegratorInterfacePtr->getInitialState();
    goalState = doubleIntegratorInterfacePtr->getInitialTarget();

    // 初始化参考轨迹
    TargetTrajectories targetTrajectories({initTime}, {goalState}, {vector_t::Zero(INPUT_DIM)});
    doubleIntegratorInterfacePtr->getReferenceManagerPtr()->setTargetTrajectories(std::move(targetTrajectories));
  }

  // 释放内存的智能指针
  std::unique_ptr<GaussNewtonDDP_MPC> getMpc(bool warmStart) {
    auto& interface = *doubleIntegratorInterfacePtr;
    auto mpcSettings = interface.mpcSettings();
    auto ddpSettings = interface.ddpSettings();
    if (!warmStart) {
      mpcSettings.coldStart_ = true;
      ddpSettings.maxNumIterations_ = 5;
    }

    auto mpcPtr = std::make_unique<GaussNewtonDDP_MPC>(std::move(mpcSettings), std::move(ddpSettings), interface.getRollout(),
                                                       interface.getOptimalControlProblem(), interface.getInitializer());
    mpcPtr->getSolverPtr()->setReferenceManager(interface.getReferenceManagerPtr());

    return mpcPtr;
  }

  const scalar_t tolerance = 2e-2;  // 容差
  const scalar_t f_mpc = 10.0;  // f_mpc值
  const scalar_t initTime = 1234.5;  // 初始化时间为随机时间
  const scalar_t finalTime = initTime + 5.0;  // 结束时间为初始化时间加上5.0

  vector_t initState;  // 初始状态
  vector_t goalState;  // 目标状态
  std::unique_ptr<DoubleIntegratorInterface> doubleIntegratorInterfacePtr;  // DoubleIntegratorInterface的智能指针
};

// 测试DoubleIntegratorIntegrationTest类的synchronousTracking函数
TEST_F(DoubleIntegratorIntegrationTest, synchronousTracking) {
  auto mpcPtr = getMpc(true);
  MPC_MRT_Interface mpcInterface(*mpcPtr);

  SystemObservation observation;
  observation.time = initTime;
  observation.state = initState;
  observation.input.setZero(INPUT_DIM);
  mpcInterface.setCurrentObservation(observation);

  // 运行MPC循环
  auto time = initTime;
  while (time < finalTime) {
    // 运行MPC
    mpcInterface.advanceMpc();
    time += 1.0 / f_mpc;

    if (mpcInterface.initialPolicyReceived()) {
      size_t mode;
      vector_t optimalState, optimalInput;

      mpcInterface.updatePolicy();
      mpcInterface.evaluatePolicy(time, vector_t::Zero(STATE_DIM), optimalState, optimalInput, mode);

      // 使用MPC得到的最优状态作为下一个观察值:
      observation.time = time;
      observation.state = optimalState;
      observation.input.setZero(INPUT_DIM);
      mpcInterface.setCurrentObservation(observation);
    }
  }

  // 断言观察值状态与目标状态的差距在容差范围内
  ASSERT_NEAR(observation.state(0), goalState(0), tolerance);
}

// 测试DoubleIntegratorIntegrationTest类的coldStartMPC函数
TEST_F(DoubleIntegratorIntegrationTest, coldStartMPC) {
  auto mpcPtr = getMpc(false);
  MPC_MRT_Interface mpcInterface(*mpcPtr);

  SystemObservation observation;
  observation.time = initTime;
  observation.state = initState;
  observation.input.setZero(INPUT_DIM);
  mpcInterface.setCurrentObservation(observation);

  // 运行MPC循环
  auto time = initTime;
  while (time < finalTime) {
    // 运行MPC
    mpcInterface.advanceMpc();
    time += 1.0 / f_mpc;

    if (mpcInterface.initialPolicyReceived()) {
      size_t mode;
      vector_t optimalState, optimalInput;

      mpcInterface.updatePolicy();
      mpcPtr->getSolverPtr()->setReferenceManager(interface.getReferenceManagerPtr());
      mpcInterface.evaluatePolicy(time, vector_t::Zero(STATE_DIM), optimalState, optimalInput, mode);

      // 使用MPC得到的最优状态作为下一个观察值:
      observation.time = time;
      observation.state = optimalState;
      observation.input.setZero(INPUT_DIM);
      mpcInterface.setCurrentObservation(observation);
    }
  }

  // 断言观察值状态与目标状态的差距在容差范围内
  ASSERT_NEAR(observation.state(0), goalState(0), tolerance);
}

// 测试DoubleIntegratorIntegrationTest类的asynchronousTracking函数（仅在 NDEBUG 编译时测试）
#ifdef NDEBUG
TEST_F(DoubleIntegratorIntegrationTest, asynchronousTracking) {
  auto mpcPtr = getMpc(true);
  MPC_MRT_Interface mpcInterface(*mpcPtr);

  const scalar_t f_mrt = 100;

  SystemObservation observation;
  observation.time = initTime;
  observation.state = initState;
  observation.input.setZero(INPUT_DIM);

  // 等待第一个策略
  mpcInterface.setCurrentObservation(observation);
  while (!mpcInterface.initialPolicyReceived()) {
    mpcInterface.advanceMpc();
  }

  // 使用线程运行 MPC
  std::atomic_bool mpcRunning{true};
  auto mpcThread = std::thread([&]() {
    while (mpcRunning) {
      try {
        ocs2::executeAndSleep([&]() { mpcInterface.advanceMpc(); }, f_mpc);
      } catch (const std::exception& e) {
        mpcRunning = false;
        std::cerr << "EXCEPTION " << e.what() << std::endl;
        EXPECT_TRUE(false);
      }
    }
  });

  // 运行 MRT
  while (observation.time < finalTime) {
    ocs2::executeAndSleep(
        [&]() {
          observation.time += 1.0 / f_mrt;

          // 评估策略
          mpcInterface.updatePolicy();
          mpcInterface.evaluatePolicy(observation.time, vector_t::Zero(STATE_DIM), observation.state, observation.input, observation.mode);

          // 使用MPC得到的最优状态作为下一个观察值:
          mpcInterface.setCurrentObservation(observation);
        },
        f_mrt);
  }

  mpcRunning = false;
  if (mpcThread.joinable()) {
    mpcThread.join();
  }

  // 断言观察值状态与目标状态的差距在容差范围内
  ASSERT_NEAR(observation.state(0), goalState(0), tolerance);
}
#endif

2.10 test/DoubleIntegratorPyBindingTest.cpp

该函数是一个测试函数，用于测试DoubleIntegratorPyBindings类的功能。该类是用于双积分微分方程的Python绑定。在测试中，首先创建了一个DoubleIntegratorPyBindings的实例，然后设置观测值和目标轨迹，并通过advanceMpc()方法来执行模型预测控制算法。接着，使用getMpcSolution()方法获取模型预测控制的解，并将其打印出来。最后，使用flowMap()方法和flowMapLinearApproximation()方法获取流映射矩阵和线性化近似矩阵，并将其打印出来。

该C++函数是一个Google测试框架（gtest）中的测试用例，名为"DoubleIntegratorTest.pyBindings"。这个测试用例主要目的是验证ocs2_double_integrator库中DoubleIntegratorPyBindings类的正确性和功能完整性。

首先，通过包含相应的头文件并创建一个ocs2::double_integrator::DoubleIntegratorPyBindings类型的变量bindings，初始化该类的实例。初始化时需要提供两个参数：一个是任务配置文件路径（taskFile），它是双积分模型MPC任务的配置信息；另一个是库文件夹路径（libFolder），它包含了自动生成的相关代码和数据。

接着，设置初始状态(state)和零输入(zeroInput)，并通过bindings.setObservation()方法将这些信息传递给类的实例。

然后，通过bindings.setTargetTrajectories()方法设定目标状态和输入轨迹。

调用bindings.advanceMpc()执行模型预测控制（MPC）算法，得到最优控制序列。

之后，使用bindings.getMpcSolution()获取计算出的MPC解，包括时间序列t_arr、状态序列x_arr和控制输入序列u_arr。并对结果进行断言检查，确保时间、状态和控制输入的数量相等。

接下来，打印出MPC解的时间、状态和输入值，并进一步调用bindings.flowMap()计算在指定时间、状态和输入下的系统动态dxdt，以及调用bindings.flowMapLinearApproximation()获取流映射的线性近似矩阵。

另外，还调用bindings.costQuadraticApproximation()获得当前状态和输入下的二次成本函数近似值，并将其打印出来。

最后，测试还包括获取反馈增益矩阵（K）的功能，但注释掉了获取值函数状态导数的部分（getValueFunctionStateDerivative()）。

总的来说，此测试函数对DoubleIntegratorPyBindings类的关键接口进行了全面的功能验证。

2.11 test/DoubleIntegratorPyBindingTest.py

这个Python函数是一个单元测试类，包含了对一个名为mpc_interface的模块的测试。在setUp()方法中，它初始化了mpc_interface对象和一些其他变量。然后，它定义了一个名为test_run_mpc的方法，该方法设置了目标轨迹并调用mpc_interface的reset()，setObservation()和advanceMpc()方法。接下来，它使用mpc_interface的getMpcSolution()方法获取结果，并打印出来。最后，它测试了流映射和成本函数的导数。

这个Python脚本定义了一个名为double_integrator_python_tests的测试类，它继承自unittest.TestCase类，用于对一个双积分器模型的Model Predictive Control（MPC）接口进行单元测试。以下是对关键部分的详细解释：

1. setUp(self)方法：

   • 使用rospkg.RosPack().get_path('ocs2_double_integrator')获取ROS包'ocs2_double_integrator'的路径。
   • 根据路径构造taskFile和libFolder，分别指向任务配置文件和自动生成的库文件夹。
   • 初始化一个mpc_interface对象（可能是一个C++编译后的Python接口），通过传入taskFile和libFolder来设置MPC所需的参数和库。
   • 设置状态维度stateDim为2，输入维度inputDim为1。

2. test_run_mpc(self)方法：

   • 首先设定目标轨迹，包括时间、状态和输入轨迹，分别用desiredTimeTraj、desiredStateTraj和desiredInputTraj表示，并填充初始值。
   • 创建一个TargetTrajectories对象，包含上述三个轨迹，并重置MPC接口的初始目标。
   • 初始化当前时间和系统状态变量time、x及控制输入变量u。
   • 调用self.mpc.setObservation(time, x, u)将当前观测到的状态和输入传递给MPC接口。
   • 调用self.mpc.advanceMpc()执行一次MPC优化过程以得到最优控制序列。
   • 通过调用self.mpc.getMpcSolution(t_result, x_result, u_result)获取MPC计算出的时间、状态和输入的最优序列，并将其存储在对应的C++风格数组中。
   • 打印并验证计算出的MPC解的时间步长数及其具体数值。
   • 对流映射（flowMap）的线性近似以及成本函数的二次近似进行测试，分别调用flowMapLinearApproximation和costQuadraticApproximation方法，并打印其导数矩阵。

如果直接运行该脚本，unittest.main()会自动执行所有定义在类中的测试方法，这里即执行test_run_mpc方法进行MPC算法的功能验证与性能测试。

2.12 CMakeLists.txt

这段代码是使用CMake构建一个名为ocs2_double_integrator的项目。它使用catkin工具链，并包含C++和Python绑定。代码配置了项目包含的依赖项、编译选项、安装目录等。还添加了单元测试和Python测试。同时，使用clang工具进行代码检查和分析。

该CMake脚本用于构建一个名为ocs2_double_integrator的C++项目，该项目属于ROS（机器人操作系统）生态中的catkin工作空间。以下是对各部分功能的具体解释：

1. cmake_minimum_required: 指定此CMakeLists.txt文件需要至少3.0.2版本的CMake才能解析。

2. project(ocs2_double_integrator): 定义当前项目名称为ocs2_double_integrator。

3. CMAKE_EXPORT_COMPILE_COMMANDS: 设置为ON以生成compile_commands.json文件，方便clang工具链进行静态代码分析和格式检查。

4. CATKIN_PACKAGE_DEPENDENCIES: 列出了此项目依赖的一系列catkin包。

5. find_package(catkin REQUIRED COMPONENTS): 寻找并配置必要的catkin组件以及项目依赖的其他catkin包。

6. find_package(Boost REQUIRED COMPONENTS): 寻找并配置Boost库中的system和filesystem组件。

7. find_package(Eigen3 3.3 REQUIRED NO_MODULE): 寻找并配置Eigen3数学库，要求版本不低于3.3，并不使用模块方式查找。

8. catkin_package(): 定义catkin包属性，包括头文件目录、依赖的catkin包、非catkin依赖项（如Boost）、库文件等。

9. 构建部分:

   • configure_file: 生成包含项目路径信息的编译时头文件。
   • include_directories: 添加所有目标的包含目录，包括Eigen3、Boost和catkin库的头文件目录。
   • add_library: 创建名为ocs2_double_integrator的C++库，指定源文件并链接catkin和Boost库及dl库。
   • pybind11_add_module: 使用pybind11创建Python绑定模块，并设置输出目录。
   • 如果找到cmake_clang_tools，将运行clang工具对特定目标进行静态代码分析。

10. 安装部分: 配置如何在系统中安装项目的目标文件、头文件、配置文件以及Python绑定模块。

11. 测试部分: 使用catkin_add_gtest添加针对C++代码的gtest单元测试；使用catkin_add_nosetests添加针对Python绑定的nose测试。

总之，这段CMake脚本主要负责配置项目编译环境，定义项目依赖关系，创建C++库及其Python绑定，并设置单元测试，最后安排项目的安装过程。

cmake_minimum_required(VERSION 3.0.2)
project(ocs2_double_integrator)

# 为clang工具生成compile_commands.json
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)

# 设置CATKIN_PACKAGE_DEPENDENCIES依赖包
set(CATKIN_PACKAGE_DEPENDENCIES
  pybind11_catkin
  ocs2_core
  ocs2_mpc
  ocs2_ddp
  ocs2_python_interface
  ocs2_robotic_tools
)

# 查找catkin包，并设置依赖和链接库
find_package(catkin REQUIRED COMPONENTS
  roslib
  ${CATKIN_PACKAGE_DEPENDENCIES}
)

# 查找Boost包，并设置依赖
find_package(Boost REQUIRED COMPONENTS
  system
  filesystem
)

# 查找Eigen3包，并设置依赖
find_package(Eigen3 3.3 REQUIRED NO_MODULE)

###################################
## catkin特定配置 ##
###################################

# 生成catkin包信息
catkin_package(
  INCLUDE_DIRS
    include
    ${EIGEN3_INCLUDE_DIRS}
  CATKIN_DEPENDS
    ${CATKIN_PACKAGE_DEPENDENCIES}
  DEPENDS
    Boost
  LIBRARIES
    ${PROJECT_NAME}
)

###########
## 编译 ##
###########

# 配置包路径
configure_file (
  "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/package_path.h.in"
  "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" @ONLY
)

# 添加所有目标的目录
include_directories(
  include
  ${EIGEN3_INCLUDE_DIRS}
  ${Boost_INCLUDE_DIRS}
  ${catkin_INCLUDE_DIRS}
)

# 创建double integrator接口库
add_library(${PROJECT_NAME}
  src/DoubleIntegratorInterface.cpp
)
add_dependencies(${PROJECT_NAME}
  ${catkin_EXPORTED_TARGETS}
)
target_link_libraries(${PROJECT_NAME}
  ${catkin_LIBRARIES}
  ${Boost_LIBRARIES}
  dl
)
target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS})

# 创建Python绑定
pybind11_add_module(DoubleIntegratorPyBindings SHARED
  src/pyBindModule.cpp
)
add_dependencies(DoubleIntegratorPyBindings
  ${PROJECT_NAME}
  ${catkin_EXPORTED_TARGETS}
)
target_link_libraries(DoubleIntegratorPyBindings PRIVATE
  ${PROJECT_NAME}
  ${catkin_LIBRARIES}
)
set_target_properties(DoubleIntegratorPyBindings
    PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}
)

# 设置Python包装
catkin_python_setup()

#########################
###   CLANG TOOLING   ###
#########################

# 查找cmake_clang_tools包
find_package(cmake_clang_tools QUIET)
if(cmake_clang_tools_FOUND)
  # 显示clang工具配置信息
  message(STATUS "使用clang工具进行ocs2_double_integrator配置")
  # 配置clang工具
  add_clang_tooling(
    TARGETS
      ${PROJECT_NAME}
    SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test
    CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
    CF_WERROR
  )
endif(cmake_clang_tools_FOUND)

#############
## 安装 ##
#############

# 安装目标文件
install(TARGETS ${PROJECT_NAME}
  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
# 安装头文件
install(DIRECTORY include/${PROJECT_NAME}/
  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
# 安装配置文件
install(DIRECTORY config
  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
# 安装Python绑定
install(TARGETS DoubleIntegratorPyBindings
  ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}
  LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}
)

#############
## 测试 ##
#############

# 添加测试目标1
catkin_add_gtest(ocs2_double_integrator_no_ros_integration_test
  test/DoubleIntegratorNoRosIntegrationTest.cpp
)
add_dependencies(ocs2_double_integrator_no_ros_integration_test
  ${catkin_EXPORTED_TARGETS}
)
target_include_directories(ocs2_double_integrator_no_ros_integration_test
  PRIVATE ${PROJECT_BINARY_DIR}/include
)
target_link_libraries(ocs2_double_integrator_no_ros_integration_test
  ${PROJECT_NAME}
  ${catkin_LIBRARIES}
  gtest_main
)

# 添加测试目标2
catkin_add_gtest(ocs2_double_integrator_pybinding_test
  test/DoubleIntegratorPyBindingTest.cpp
)
add_dependencies(ocs2_double_integrator_pybinding_test
  ${PROJECT_NAME}
  ${catkin_EXPORTED_TARGETS}
)
target_include_directories(ocs2_double_integrator_pybinding_test
  PRIVATE ${PROJECT_BINARY_DIR}/include
)
target_link_libraries(ocs2_double_integrator_pybinding_test
  ${PROJECT_NAME}
  ${catkin_LIBRARIES}
)

# 添加Python测试目标
catkin_add_nosetests(test/DoubleIntegratorPyBindingTest.py)

2.13 package.xml

这是一个XML格式的文件，用于描述一个名为"ocs2_double_integrator"的ROS包。该包依赖于其他一些包，并使用catkin作为构建工具。它还包含有关包的作者、许可证和其他元数据的信息。

此XML文件是一个ROS（Robot Operating System）软件包的manifest文件，用于定义和描述名为"ocs2_double_integrator"的软件包。具体解析如下：

• <?xml version="1.0"?>：声明该文档为XML格式，并指定了版本为1.0。

• <package format="2">：表示这是一个ROS软件包的元数据信息块，其中"format=2"指定这是第二版的ROS包规范。

• <name>ocs2_double_integrator</name>：声明包的名称是"ocs2_double_integrator"。

• <version>0.0.0</version>：表示当前包的版本号为0.0.0。

• <description>The ocs2_double_integrator package</description>：提供了包的简短描述。

• <maintainer>标签：列出了三位维护者及其电子邮件地址，分别是Farbod Farshidian、Jan Carius和Ruben Grandia。

• <license>TODO</license>：说明了该软件包的许可证信息尚未填写，需要后续补充。

• <buildtool_depend>catkin</buildtool_depend>：指出该包使用catkin作为构建工具链。

• <build_depend>和<depend>标签：列出了一系列依赖包，包括cmake_clang_tools、roslib、pybind11_catkin、ocs2_core、ocs2_mpc、ocs2_ddp、ocs2_python_interface以及ocs2_robotic_tools。这些依赖项在编译和运行时都是必需的。

总之，这个XML文件详细描述了"ocs2_double_integrator"这个ROS软件包的基本信息，如名称、版本、描述、维护者、许可证、构建工具及依赖的其他软件包等关键属性。

<?xml version="1.0"?>
<package format="2">
  <name>ocs2_double_integrator</name>
  <version>0.0.0</version>
  <description>The ocs2_double_integrator package</description>

  <maintainer email="farbod.farshidian@gmail.com">Farbod Farshidian</maintainer>
  <maintainer email="jcarius@ethz.ch">Jan Carius</maintainer>
  <maintainer email="rgrandia@ethz.ch">Ruben Grandia</maintainer>

  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>cmake_clang_tools</build_depend>

  <depend>roslib</depend>
  <depend>pybind11_catkin</depend>

  <depend>ocs2_core</depend>
  <depend>ocs2_mpc</depend>
  <depend>ocs2_ddp</depend>
  <depend>ocs2_python_interface</depend>
  <depend>ocs2_robotic_tools</depend>

</package>

XML格式的注释
<?xml version="1.0"?>
<package format="2">
  **包的名称**： ocs2_double_integrator  
  **包的版本**： 0.0.0  
  **包的描述**： The ocs2_double_integrator package  

  **包的维护人员**： Farbod Farshidian (farbod.farshidian@gmail.com)  
                         Jan Carius (jcarius@ethz.ch)  
                         Ruben Grandia (rgrandia@ethz.ch)  

  **包的许可证**： TODO  

  **构建工具依赖**： catkin  

  **构建依赖**： cmake_clang_tools  

  **依赖**： roslib  
                   pybind11_catkin  
                   ocs2_core  
                   ocs2_mpc  
                   ocs2_ddp  
                   ocs2_python_interface  
                   ocs2_robotic_tools  

2.14 setup.py

这段代码是一个Python函数，使用setuptools库进行Python包的安装和管理。它使用catkin_pkg库中的函数generate_distutils_setup生成setuptools的配置参数，并使用该参数进行包的安装。这个函数将包的名称设置为'ocs2_double_integrator'，并将包的源目录设置为'src'。最后，使用生成的配置参数进行setup函数的调用，完成包的安装。

这段Python脚本的主要功能是为名为"ocs2_double_integrator"的Python包设置安装和打包配置。它通过以下几个步骤实现：

1. 首行#!/usr/bin/env python是一个Shebang，用于指定该脚本应使用Python解释器执行。

2. 引入两个Python库：

   • setuptools：这是一个在Python中广泛使用的库，用于构建和分发Python项目，包括处理依赖、生成二进制分发文件等。
   • catkin_pkg.python_setup：这是ROS（Robot Operating System）中的一个模块，提供与setuptools集成的功能，以便在ROS环境中更好地管理和打包相关的Python代码。

3. 调用generate_distutils_setup函数，来自catkin_pkg.python_setup模块，用于生成符合distutils（setuptools的基础模块）规范的配置字典。这里传入了两个参数：

   • packages=['ocs2_double_integrator']：指定了要打包的Python包名称列表，这里是'ocs2_double_integrator'。
   • package_dir={'': 'src'}：定义了源码目录映射规则，空字符串''表示顶级目录，其对应的值'src'表示实际的Python源代码所在路径。

4. 将generate_distutils_setup返回的配置字典赋值给setup_args变量。

5. 最后，调用setuptools的setup函数，并将setup_args作为关键字参数展开传递给它。这样，setuptools将根据这些配置信息来编译、打包并安装指定的Python包及其依赖项。

总结来说，这个脚本的主要目的是为ROS中的"ocs2_double_integrator"Python包创建一套标准的安装和打包流程。

#!/usr/bin/env python
# 设置环境变量

from setuptools import setup
from catkin_pkg.python_setup import generate_distutils_setup

# 生成distutils setup参数
setup_args = generate_distutils_setup(
     packages=['ocs2_double_integrator'],  # 指定Python包
     package_dir={'': 'src'}  # 指定源代码目录
)

# 使用生成的参数进行setup
setup(**setup_args)

三、ocs2_double_integrator_ros 文件夹

3.1 include/ocs2_double_integrator_ros/DoubleIntegratorDummyVisualization.h

这段代码定义了一个名为DoubleIntegratorDummyVisualization的C++类，它继承自DummyObserver类。该类用于可视化双积分系统，并实现了update函数和launchVisualizerNode函数。该类还包含一个用于发布关节状态的ros::Publisher对象。 

这段C++代码定义了一个名为DoubleIntegratorDummyVisualization的类，该类位于命名空间ocs2::double_integrator中，并且继承自ocs2_ros_interfaces::mrt::DummyObserver基类。这个类主要用于双积分系统的可视化观察与数据发布。

• 构造函数：explicit DoubleIntegratorDummyVisualization(ros::NodeHandle& nodeHandle)接收一个ROS节点句柄作为参数，在构造对象时调用launchVisualizerNode(nodeHandle)方法来初始化和启动一个用于可视化的ROS节点。

• 析构函数：~DoubleIntegratorDummyVisualization() override = default;提供默认析构行为，确保在对象销毁时资源得到正确释放。

• void update(const SystemObservation& observation, const PrimalSolution& policy, const CommandData& command) override;是一个虚函数，重写了父类中的update方法。当系统有新的观测值、最优策略或命令数据时，会调用此函数进行数据处理及可能的可视化更新操作。

• 私有成员函数：void launchVisualizerNode(ros::NodeHandle& nodeHandle);负责创建并配置一个用于实现可视化的ROS节点。

• 成员变量：ros::Publisher jointPublisher_;是一个ROS发布器对象，用于发布主题（可能是传感器_msgs::JointState消息类型），将关节状态信息发送到其他ROS节点，从而实现对双积分系统状态的可视化展示。

#pragma once

#include <ros/ros.h>
#include <sensor_msgs/JointState.h>

#include <ocs2_ros_interfaces/mrt/DummyObserver.h>

#include "ocs2_double_integrator/definitions.h"

namespace ocs2 {
namespace double_integrator {

// DoubleIntegratorDummyVisualization 类用于可视化双积分系统中的虚拟观察器
class DoubleIntegratorDummyVisualization final : public DummyObserver {
 public:
  // 构造函数，初始化 DoubleIntegratorDummyVisualization 对象并启动可视化节点
  explicit DoubleIntegratorDummyVisualization(ros::NodeHandle& nodeHandle) { launchVisualizerNode(nodeHandle); }

  // 析构函数
  ~DoubleIntegratorDummyVisualization() override = default;

  // 重写 update 函数，用于更新观察器
  void update(const SystemObservation& observation, const PrimalSolution& policy, const CommandData& command) override;

 private:
  // 启动可视化节点的私有函数
  void launchVisualizerNode(ros::NodeHandle& nodeHandle);

  // 公告发布器
  ros::Publisher jointPublisher_;
};

}  // namespace double_integrator
}  // namespace ocs2

3.2 launch/double_integrator.launch

这段XML代码是一个参数化launch文件，用于启动ocs2_double_integrator_ros包中的节点。根据传递的参数不同，可以选择性地包括其他launch文件或启动特定类型的节点。如果定义了rviz参数为true，则包括visualize.launch文件；如果定义了multiplot参数为true，则包括multiplot.launch文件。如果定义了debug参数为true，则使用GDB调试器启动double_integrator_mpc节点；否则，正常启动该节点。另外，还会启动一个名为double_integrator_dummy_test的节点和一个名为double_integrator_target的节点，并将传递的参数task_name作为参数传递给这些节点。

这段XML代码是一个 ROS（Robot Operating System）的 launch 文件，用于管理和配置一个或多个节点的启动过程。它定义了多个可选参数（arg标签），如 rviz、multiplot、task_name 和 debug，并为它们指定了默认值。

1. <arg name="rviz" default="true" />：声明了一个名为 rviz 的布尔型参数，默认值为 true。这意味着如果没有明确指定该参数，则视为开启 RViz 可视化工具。

2. <arg name="multiplot" default="false"/>：声明了 multiplot 参数，默认关闭。

3. <arg name="task_name" default="mpc"/>：声明了 task_name 参数，用于指定任务名称，默认值为 "mpc"。

4. <arg name="debug" default="false"/>：声明了 debug 参数，默认不启用调试模式。

接下来的 group 和 node 标签根据上述参数的值来决定是否运行特定的节点或包含其他的 launch 文件：

• <group if="$(arg rviz)">: 如果 rviz 参数为 true，则会执行其中的内容，即包含并启动 ocs2_double_integrator_ros 包中的 visualize.launch 文件。

• <group if="$(arg multiplot)">: 同理，如果 multiplot 参数为 true，则包含并启动 multiplot.launch 文件。

• <node> 标签用于启动ROS节点：

  • 第一个 <node> 根据 debug 参数的值启动 double_integrator_mpc 节点。如果 debug 为 true，则使用 GDB 调试器并在新的 gnome-terminal 中启动该节点；否则直接在终端中启动。

  • 第二个 <node> 无条件启动另一个 double_integrator_dummy_test 节点，同样在新的 gnome-terminal 中启动，传入参数 task_name。

• 最后一个 <node> 同样无条件启动 double_integrator_target 节点，其启动方式与第二个节点相同，也会传递参数 task_name。

总结来说，这个launch文件通过解析用户输入的参数，灵活地控制着一系列ROS节点和服务的启动和运行方式，包括可视化、多图绘制、MPC控制器以及测试等组件。

<launch>
    <arg name="rviz"       default="true" />  <!-- 是否启动rviz -->
    <arg name="multiplot"  default="false"/>  <!-- 是否启动多图表模式 -->
    <arg name="task_name"  default="mpc"/>  <!-- 任务名称 -->
    <arg name="debug"      default="false"/>  <!-- 是否启动调试模式 -->

    <group if="$(arg rviz)">  <!-- 如果rviz参数为true则执行以下内容 -->
      <include file="$(find ocs2_double_integrator_ros)/launch/visualize.launch"/>  <!-- 包含可视化启动文件 -->
    </group>

    <group if="$(arg multiplot)">  <!-- 如果multiplot参数为true则执行以下内容 -->
      <include file="$(find ocs2_double_integrator_ros)/launch/multiplot.launch"/>  <!-- 包含多图表启动文件 -->
    </group>

    <node if="$(arg debug)" pkg="ocs2_double_integrator_ros" type="double_integrator_mpc" name="double_integrator_mpc"
          output="screen" args="$(arg task_name)" launch-prefix="gnome-terminal -- gdb --args"/>  <!-- 如果debug参数为true则执行以下内容 -->

    <node unless="$(arg debug)" pkg="ocs2_double_integrator_ros" type="double_integrator_mpc" name="double_integrator_mpc"
          output="screen" args="$(arg task_name)" launch-prefix=""/>  <!-- 如果debug参数为false则执行以下内容 -->

    <node pkg="ocs2_double_integrator_ros" type="double_integrator_dummy_test" name="double_integrator_dummy_test"
          output="screen" args="$(arg task_name)" launch-prefix="gnome-terminal --"/>  <!-- 启动dummy_test节点 -->

    <node pkg="ocs2_double_integrator_ros" type="double_integrator_target" name="double_integrator_target"
          output="screen" args="$(arg task_name)" launch-prefix="gnome-terminal --"/>  <!-- 启动target节点 -->
</launch>

3.3 launch/multiplot.launch

该XML函数是一个参数化launch文件，它包括了另一个launch文件（performance_indices.launch）。它传递了一个参数（mpc_policy_topic_name）给被包括的launch文件，并设置其值为"double_integrator_mpc_policy"。

在ROS（机器人操作系统）中，XML是用于配置和组织系统的常见格式。上述代码片段是在一个launch文件中的内容，这个launch文件主要用于启动一个性能指标多图显示功能。

<launch>标签定义了整个launch文件的开始，它是ROS launch文件的基本结构。

<!-- Launch Performance Indices Multi-plot --> 是一个注释，解释了该部分代码的目的，即启动一个能够展示多个性能指标的图表。

<include file="$(find ocs2_ros_interfaces)/launch/performance_indices.launch"> 表示此launch文件将引入（或合并）名为"performance_indices.launch"的另一个launch文件。$(find ocs2_ros_interfaces)是一个ROS特定的语法，它会查找并替换为名为"ocs2_ros_interfaces"的ROS包下的指定路径。

<arg name="mpc_policy_topic_name" value="double_integrator_mpc_policy"/> 定义了一个参数传递给被包含的launch文件。这里声明了一个名为"mpc_policy_topic_name"的参数，并赋予其值"double_integrator_mpc_policy"。这意味着在"performance_indices.launch"中可以引用并使用这个参数来配置相关节点或者进程的行为，例如订阅或发布与MPC（Model Predictive Control）策略相关的主题。

总结来说，这段XML代码的主要作用是：启动一个性能指标多图显示功能，并且在初始化过程中指定了MPC策略的主题名称为"double_integrator_mpc_policy"。

<launch>
  <!-- 启动性能指数多图 -->
  <include file="$(find ocs2_ros_interfaces)/launch/performance_indices.launch">
    <arg name="mpc_policy_topic_name"      value="double_integrator_mpc_policy"/>
  </include>
</launch>

3.4 launch/visualize.launch

这段XML代码是一个ROS（机器人操作系统）launch文件，用于启动一个包含双积分器机器人的可视化演示。它定义了两个参数（rvizconfig和model），并在节点中使用这些参数来加载机器人描述和运行robot_state_publisher和rviz节点。

这段XML代码是ROS（机器人操作系统）中的一个launch文件配置，主要用于启动与双积分器模型相关的节点和可视化工具。详细解释如下：

1. <arg> 标签：

   • 定义了两个参数变量：rvizconfig 和 model。
     • rvizconfig 参数默认值指向 ocs2_double_integrator_ros 包下的 rviz/double_integrator.rviz 文件，这是一个RVIZ配置文件，用于定义可视化界面的布局和显示内容。
     • model 参数默认值指向 ocs2_robotic_assets 包下的URDF（统一机器人描述格式）文件路径，该文件描述了双积分器机器人的物理模型。

2. <param> 标签：

   • 为名为 "robot_description" 的参数设置值，命令行执行 xacro 工具（ROS中用于处理XML宏扩展的工具），使用 inorder 参数解析并展开指定的URDF模型文件（即上文定义的 model 参数）。

3. <node> 标签：

   • 启动名为 "robot_state_publisher" 的节点，来自 robot_state_publisher 包，类型为 robot_state_publisher。这个节点负责发布机器人的TF（变换）消息，使得系统内各组件可以理解机器人的位姿信息。
   • 启动另一个名为 "double_integrator" 的节点，来自 rviz 包，类型为 rviz。此节点运行RVIZ可视化工具，并通过 -d 参数加载之前定义的 rvizconfig 参数所指的配置文件，以可视化双积分器机器人的状态和相关信息。同时，output="screen" 表示将此节点的标准输出重定向到终端屏幕，便于调试查看。

综上所述，这段launch文件主要是为了启动一个包含双积分器机器人模型的状态发布服务以及相应的RVIZ可视化界面。

3.5 rviz/double_integrator.rviz

这是一个YAML函数，用于配置一个可视化管理器。包括设置显示选项、工具栏、视图和时间等参数。

此YAML文件主要定义了一个三维可视化界面（可能为ROS中的RViz工具）的布局和参数设置。具体分析如下：

1. Panels部分：

   • 定义了多个面板组件，如Displays、Selection、Tool Properties、Views和Time等。
   • 每个面板包含Class属性，指定了面板类型及功能，如显示选项、选择工具、工具属性设置、视图管理和时间控制。
   • 还包括面板的扩展状态(Expanded)、分割器比例(Splitter Ratio)、高度(Tree Height)等布局属性。

2. Visualization Manager部分：

   • 设置了全局选项，例如背景颜色、固定帧、帧率等。
   • 在Displays子项中定义了一系列可视化元素，例如二维网格(Grid)和机器人模型(RobotModel)等，并对每个元素的具体参数进行了详细配置，比如透明度(Alpha)、大小(Cell Size)、颜色、是否启用(Enabled)、更新频率(Update Interval)等。

3. Tools部分：

   • 列出了可用的工具列表，如交互(Interact)、移动相机(MoveCamera)、选择(Select)、聚焦相机(FocusCamera)、测量(Measure)以及用于设置初始位姿和目标位姿的工具，并指定了它们所订阅的主题(Topic)。

4. Window Geometry部分：

   • 配置了窗口的几何属性，如各面板的折叠状态(collapsed)、窗口的高度(Height)、宽度(Width)、位置坐标(X, Y)、左右停靠区的隐藏状态(Hide Left Dock和Hide Right Dock)以及主窗口的状态信息。

总结：这个YAML文件是针对一个三维可视化环境进行详细配置的，包含了各种面板、显示元素、工具及其布局参数等一系列设置。

Panels:
  - Class: rviz/Displays
    Help Height: 78
    Name: Displays
    Property Tree Widget:
      Expanded:
        - /Global Options1
        - /Status1
        - /Grid1
        - /Grid1/Offset1
        - /RobotModel1
        - /RobotModel1/Links1
        - /RobotModel1/Links1/cart1
      Splitter Ratio: 0.5
    Tree Height: 836
  - Class: rviz/Selection
    Name: Selection
  - Class: rviz/Tool Properties
    Expanded:
      - /2D Pose Estimate1
      - /2D Nav Goal1
      - /Publish Point1
    Name: Tool Properties
    Splitter Ratio: 0.588679
  - Class: rviz/Views
    Expanded:
      - /Current View1
    Name: Views
    Splitter Ratio: 0.5
  - Class: rviz/Time
    Experimental: false
    Name: Time
    SyncMode: 0
    SyncSource: ""
Visualization Manager:
  Class: ""
  Displays:
    - Alpha: 0.5
      Cell Size: 1
      Class: rviz/Grid
      Color: 160; 160; 164
      Enabled: true
      Line Style:
        Line Width: 0.03
        Value: Lines
      Name: Grid
      Normal Cell Count: 0
      Offset:
        X: 0
        Y: 0
        Z: -1
      Plane: XY
      Plane Cell Count: 100
      Reference Frame: <Fixed Frame>
      Value: true
    - Alpha: 1
      Class: rviz/RobotModel
      Collision Enabled: false
      Enabled: true
      Links:
        All Links Enabled: true
        Expand Joint Details: false
        Expand Link Details: false
        Expand Tree: false
        Link Tree Style: Links in Alphabetic Order
        cart:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        slideBar:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
      Name: RobotModel
      Robot Description: robot_description
      TF Prefix: ""
      Update Interval: 0
      Value: true
      Visual Enabled: true
  Enabled: true
  Global Options:
    Background Color: 255; 255; 255
    Fixed Frame: slideBar
    Frame Rate: 30
  Name: root
  Tools:
    - Class: rviz/Interact
      Hide Inactive Objects: true
    - Class: rviz/MoveCamera
    - Class: rviz/Select
    - Class: rviz/FocusCamera
    - Class: rviz/Measure
    - Class: rviz/SetInitialPose
      Topic: /initialpose
    - Class: rviz/SetGoal
      Topic: /move_base_simple/goal
    - Class: rviz/PublishPoint
      Single click: true
      Topic: /clicked_point
  Value: true
  Views:
    Current:
      Class: rviz/Orbit
      Distance: 6.14627
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.06
        Stereo Focal Distance: 1
        Swap Stereo Eyes: false
        Value: false
      Focal Point:
        X: 0.994313
        Y: -7.20188
        Z: 2.65788
      Name: Current View
      Near Clip Distance: 0.01
      Pitch: 0.305701
      Target Frame: <Fixed Frame>
      Value: Orbit (rviz)
      Yaw: 4.69755
    Saved: ~
Window Geometry:
  Displays:
    collapsed: true
  Height: 560
  Hide Left Dock: true
  Hide Right Dock: true
  QMainWindow State: 000000ff00000000fd00000004000000000000016a000003d5fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006600fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000043000003d5000000df00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003d5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000043000003d5000000b800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003bf0000003efc0100000002fb0000000800540069006d00650100000000000003bf0000022a00fffffffb0000000800540069006d00650100000000000004500000000000000000000003bf0000018900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  Selection:
    collapsed: false
  Time:
    collapsed: false
  Tool Properties:
    collapsed: false
  Views:
    collapsed: true
  Width: 959
  X: 951
  Y: -8

3.6 src/DoubleIntegratorDummyVisualization.cpp

这段代码定义了一个名为DoubleIntegratorDummyVisualization的类，该类具有两个方法：launchVisualizerNode和update。launchVisualizerNode方法用于创建并广告一个关节状态的ROS发布器。update方法根据给定的观测值、 primal解决方案和指令更新关节状态，并通过发布器发布关节状态信息。

这段C++代码属于一个名为ocs2::double_integrator::DoubleIntegratorDummyVisualization的类，该类主要用于在ROS（Robot Operating System）环境下进行双积分器系统的可视化模拟。这个类中有两个核心方法：

1. launchVisualizerNode(ros::NodeHandle& nodeHandle)： 此函数接收一个ROS节点句柄作为参数，并使用它创建并初始化一个名为“joint_states”的主题发布器，类型为sensor_msgs::JointState。通过调用此函数，可以在ROS系统中发布关节状态信息，以便在可视化工具如RViz中展示。

2. update(const SystemObservation& observation, const PrimalSolution& policy, const CommandData& command)： 此函数用于根据系统的当前观测值、最优控制策略和命令数据更新待发布的关节状态信息。具体实现是：

   • 从输入参数command.mpcTargetTrajectories_中获取目标轨迹信息。
   • 创建一个sensor_msgs::JointState类型的joint_state消息，其中包含了时间戳（设置为当前ROS时间）、关节名称列表以及对应的关节位置值列表。
   • 将观测值中的第一个状态值赋给slider_to_cart关节的位置，将目标轨迹的第一个状态值赋给slider_to_target关节的位置。
   • 最后，通过之前创建并保存在成员变量jointPublisher_中的发布器，发布更新后的joint_state消息。

总结起来，这个类的作用是在ROS环境中，根据双积分器系统的实际状态和规划的目标状态，以关节状态的形式进行实时可视化。

/******************************************************************************
 * 版权声明：本代码由Farbod Farshidian于2017年版权所有。
 *
 * 在遵守以下条件的前提下，允许以源码和二进制形式进行再发布和使用：
 * 1. 源代码分发必须保留上述版权声明、此列表条件及以下免责声明。
 * 2. 二进制形式的再分发必须复制上述版权声明、此列表条件及以下免责声明到相关文档或其他随分发提供的材料中。
 * 3. 不得将版权所有者的名字或贡献者的名字用于直接或间接地推广源自本软件的产品，除非得到明确的书面许可。
 *
 * 本软件按“原样”提供，不提供任何明示或暗示的担保，包括但不限于适销性、特定用途适用性和非侵权性的默示担保。在任何情况下，无论何种原因造成的损害（包括但不限于因使用、数据或利润损失而造成的损害），无论是合同责任、严格责任还是侵权责任，即使已被告知可能出现此类损害，版权持有者或其贡献者均不对因使用本软件而产生的任何索赔、损害或其他责任承担责任。
 ******************************************************************************/

#include "ocs2_double_integrator_ros/DoubleIntegratorDummyVisualization.h"

// 定义ocs2::double_integrator命名空间
namespace ocs2 {
namespace double_integrator {

/**
 * @brief DoubleIntegratorDummyVisualization类，用于双积分器系统的可视化节点管理
 */
class DoubleIntegratorDummyVisualization {
public:
  /**
   * @brief 启动ROS可视化节点
   * @param nodeHandle ROS节点句柄
   */
  void launchVisualizerNode(ros::NodeHandle& nodeHandle) {
    jointPublisher_ = nodeHandle.advertise<sensor_msgs::JointState>("joint_states", 1);  // 创建并初始化关节状态主题发布器
  }

  /**
   * @brief 更新可视化节点中的关节状态信息
   * @param observation 当前系统观测结果
   * @param policy 当前系统的Primal解决方案
   * @param command 当前系统命令数据
   */
  void update(const SystemObservation& observation, const PrimalSolution& policy,
              const CommandData& command) {
    const auto& targetTrajectories = command.mpcTargetTrajectories_;  // 获取目标轨迹数据

    sensor_msgs::JointState joint_state;  // 创建一个关节状态对象
    joint_state.header.stamp = ros::Time::now();  // 设置时间戳为当前时间

    // 设置两个关节名称与位置信息
    joint_state.name.resize(2);
    joint_state.position.resize(2);
    joint_state.name[0] = "slider_to_cart";
    joint_state.position[0] = observation.state(0);
    joint_state.name[1] = "slider_to_target";
    joint_state.position[1] = targetTrajectories.stateTrajectory[0](0);

    jointPublisher_.publish(joint_state);  // 发布关节状态信息至主题"joint_states"
  }

private:
  ros::Publisher jointPublisher_;  // 关节状态主题发布器
};

}  // namespace double_integrator
}  // namespace ocs2

3.7 src/DoubleIntegratorMpcNode.cpp

这段C++代码是一个主函数，用于初始化ROS节点并执行模型预测控制（MPC）。它首先从命令行参数中获取任务文件路径，然后初始化ROS节点。接着，它创建了一个双积分模型的接口对象，并初始化ROS参考管理器。然后，它创建了一个GaussNewtonDDP_MPC对象，并设置MPC的参数和模型。最后，它创建了一个MPC_ROS_Interface对象，并启动节点。整个过程顺利结束后，主函数返回0。

这段C++代码是一个实现基于ROS（Robot Operating System）的模型预测控制（MPC）程序的主函数。该程序主要用于一个双积分器系统的控制，以下是详细步骤解析：

1. 定义机器人名称为 "double_integrator"，并定义相关类型别名interface_t对应ocs2::double_integrator::DoubleIntegratorInterface，mpc_ros_t对应ocs2::MPC_ROS_Interface。

2. 从命令行参数中提取任务文件夹路径，如果未指定，则抛出异常并终止程序运行。

3. 初始化ROS节点，名称为“double_integrator_mpc”。

4. 使用ROS包路径找到并加载双积分器系统的任务配置文件（task.info）和自动生成库文件所在的目录。

5. 创建interface_t类型的对象doubleIntegratorInterface，通过给定的任务文件和库文件夹初始化双积分器系统接口。

6. 创建一个ocs2::RosReferenceManager共享指针，并关联到doubleIntegratorInterface的参考管理器，然后订阅ROS消息以获取参考轨迹或目标。

7. 使用doubleIntegratorInterface提供的MPC设置、DDP设置、滚动优化策略、最优控制问题实例和初始化器来创建一个ocs2::GaussNewtonDDP_MPC对象（mpc）。

8. 将之前创建的ROS参考管理器设置到MPC的求解器中。

9. 使用创建好的MPC对象创建一个mpc_ros_t类型的对象mpcNode，并将ROS节点句柄传入以便启动与ROS相关的发布、订阅和服务。

10. 最后，调用mpcNode.launchNodes(nodeHandle)启动MPC在ROS环境下的节点及所需的相关线程。

11. 主函数执行完毕后返回0，表示程序成功退出。整个程序通过ROS实现了对双积分器系统的实时MPC控制。

// 版权所有 (c) 2017, Farbod Farshidian。保留所有权利。

// 以下条款适用于源代码和二进制形式的再分发： 
// * 须保留以上版权声明，以及以下条件和以下弃权声明。
// * 无论是否经过修改，在二进制形式中重新分发本软件须复制以上版权声明， 
//   以及以下条件和以下弃权声明，并且需要获得明确的事先书面许可。
// 无法用上述条款获得许可的情况下，不得使用本软件的名称或其贡献者的名称 
// 为经过本软件派生的产品或服务进行宣传或促销。

 ROS软件的初始化
#include <ros/init.h>
#include <ros/package.h>

# include <ocs2_ddp/GaussNewtonDDP_MPC.h>
# include <ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h>
# include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h>

# include "ocs2_double_integrator/DoubleIntegratorInterface.h"

 主函数
int main(int argc, char** argv) {
  const std::string robotName = "double_integrator";
  using interface_t = ocs2::double_integrator::DoubleIntegratorInterface;
  using mpc_ros_t = ocs2::MPC_ROS_Interface;

  // 任务文件
  std::vector<std::string> programArgs{};
  ::ros::removeROSArgs(argc, argv, programArgs);
  if (programArgs.size() <= 1) {
    throw std::runtime_error("No task file specified. Aborting.");
  }
  std::string taskFileFolderName = std::string(programArgs[1]);

  // 初始化ROS节点
  ros::init(argc, argv, robotName + "_mpc");
  ros::NodeHandle nodeHandle;

  // 机器人接口
  const std::string taskFile = ros::package::getPath("ocs2_double_integrator") + "/config/" + taskFileFolderName + "/task.info";
  const std::string libFolder = ros::package::getPath("ocs2_double_integrator") + "/auto_generated";
  interface_t doubleIntegratorInterface(taskFile, libFolder);

  // ROS参考管理器
  auto rosReferenceManagerPtr = std::make_shared<ocs2::RosReferenceManager>(robotName, doubleIntegratorInterface.getReferenceManagerPtr());
  rosReferenceManagerPtr->subscribe(nodeHandle);

  // MPC
  ocs2::GaussNewtonDDP_MPC mpc(doubleIntegratorInterface.mpcSettings(), doubleIntegratorInterface.ddpSettings(),
                               doubleIntegratorInterface.getRollout(), doubleIntegratorInterface.getOptimalControlProblem(),
                               doubleIntegratorInterface.getInitializer());
  mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr);

  // 启动MPC ROS节点
  mpc_ros_t mpcNode(mpc, robotName);
  mpcNode.launchNodes(nodeHandle);

  // 成功退出
  return 0;
}

3.8 src/DoubleIntegratorTargetPoseCommand.cpp

这段代码定义了一个函数commandLineToTargetTrajectories，它将命令行参数和当前观察结果转换为目标轨迹。该函数返回一个具有单个位移的目标轨迹。代码还包含一个主函数，它初始化ROS节点，创建一个目标轨迹键盘发布器，并发布一个命令消息。最后，代码返回0表示成功退出。

这段C++代码实现了一个命令行工具，通过ROS（Robot Operating System）接口控制一个名为"double_integrator"的双积分器系统的运动。主要功能包括：

1. 定义了一个名为commandLineToTargetTrajectories的函数，该函数接受两个参数：

   • commadLineTarget：表示从命令行输入的期望位移增量值（仅包含deltaX分量）。
   • observation：当前系统的观测结果，其中包含了时间戳和系统状态。

   函数内部首先计算新的目标位置（displacement），然后使用此目标位置和当前时间以及零输入构建一个TargetTrajectories对象并返回。这个对象代表了系统未来的目标状态轨迹。

2. main函数中初始化了ROS节点，并设置了一些参数：

   • 初始化ROS节点，将节点名称设置为"double_integrator_target"。
   • 设置变量goalLimit为一个标量数组，表示允许的最大位移增量值（此处为10.0）。
   • 创建一个TargetTrajectoriesKeyboardPublisher对象，它负责监听键盘输入并将输入转换为系统目标轨迹，这里的转换函数就是之前定义的commandLineToTargetTrajectories。
   • 发布一条消息提示用户输入双积分器的期望位移值。

3. 最后，主函数以返回0结束，表示程序正常退出。

总之，这段代码提供了一个基于命令行的人机交互界面，允许用户通过键盘输入来指定双积分器系统的位移目标，从而控制系统的运动轨迹。

/******************************************************************************
* @版权信息: Copyright (c) 2017, Farbod Farshidian. All rights reserved.
*
* @许可协议: Redistribution and use in source and binary forms, with or without
*             modification, are permitted provided that the following conditions are met:
*              * Redistributions of source code must retain the above copyright notice,
*                this list of conditions and the following disclaimer.
*              * Redistributions in binary form must reproduce the above copyright notice,
*                this list of conditions and the following disclaimer in the documentation
*                and/or other materials provided with the distribution.
*              * Neither the name of the copyright holder nor the names of its
*                contributors may be used to endorse or promote products derived from
*                this software without specific prior written permission.
*
* @免责声明: THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
*             AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
*             IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
*             DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
*             FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
*             DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
*             SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*             CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
*             OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
*             OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
******************************************************************************/

// 引入头文件
#include <ocs2_double_integrator/definitions.h>
#include <ocs2_ros_interfaces/command/TargetTrajectoriesKeyboardPublisher.h>

using namespace ocs2;
using namespace double_integrator;

/**
 * @brief 将命令行参数转换为TargetTrajectories对象
 * 
 * @param commadLineTarget 命令行目标值 [deltaX]
 * @param observation 当前观测状态
 * @return TargetTrajectories 转换后的目标轨迹
 */
TargetTrajectories commandLineToTargetTrajectories(const vector_t& commadLineTarget, const SystemObservation& observation) {
  const vector_t displacement = (vector_t(STATE_DIM) << observation.state(0) + commadLineTarget(0), 0.0).finished();
  return TargetTrajectories({observation.time}, {displacement}, {vector_t::Zero(INPUT_DIM)});
}

int main(int argc, char* argv[]) {
  // 初始化ROS节点
  const std::string robotName = "double_integrator";
  ::ros::init(argc, argv, robotName + "_target");
  ::ros::NodeHandle nodeHandle;

  // 设置位移目标限制
  const scalar_array_t goalLimit{10.0};  // [deltaX]

  // 创建TargetTrajectories键盘发布器，用于接收并处理用户通过键盘输入的位移指令
  TargetTrajectoriesKeyboardPublisher targetPoseCommand(nodeHandle, robotName, goalLimit, &commandLineToTargetTrajectories);

  // 发布提示消息，告知用户如何输入双积分器所需的位移
  const std::string commadMsg = "输入双积分器所需的位移";
  targetPoseCommand.publishKeyboardCommand(commadMsg);

  // 程序成功执行，退出
  return 0;
}

3.9 src/DummyDoubleIntegratorNode.cpp

这段C++代码是一个ROS节点，用于运行多机器人任务规划（MRT）的仿真。它使用了ocs2_double_integrator包中的接口和可视化工具，以及MRT_ROS_INTERFACE类来实现MRT任务规划和节点部署。最后，它使用一个虚拟循环来运行MRT仿真。

这段C++代码实现了一个基于ROS（Robot Operating System）的主程序，主要用于模拟双积分器系统的运动规划与控制。该程序的核心功能模块包括：

1. 参数初始化：

   • 从命令行参数中提取任务文件所在的文件夹名称，若未提供，则抛出异常。
   • 初始化一个ROS节点，名为"double_integrator_mrt"。

2. 机器人接口创建：

   • 根据ROS包路径构建完整的任务文件路径和库文件路径。
   • 创建ocs2::double_integrator::DoubleIntegratorInterface对象，用于定义双积分机器人的动力学模型，并加载任务配置信息。

3. MRT模块初始化：

   • 创建ocs2::MRT_ROS_Interface对象（mrt），用于处理多机器人任务分配问题。
   • 调用initRollout方法初始化 rollout 对象，并通过launchNodes方法启动相关ROS节点。

4. 可视化模块创建：

   • 创建一个ocs2::double_integrator::DoubleIntegratorDummyVisualization对象，用于在ROS环境中对双积分器系统进行可视化展示。

5. 虚拟循环设置：

   • 创建ocs2::MRT_ROS_Dummy_Loop对象（dummyDoubleIntegrator），它按照设定的频率执行虚拟循环。
   • 将可视化模块订阅到 dummyDoubleIntegrator 中，以便在每次循环时更新可视化状态。

6. 初始状态及目标轨迹设置：

   • 初始化系统观测值（包含时间、状态和输入）和初始的目标轨迹（包含目标状态和输入）。

7. 运行仿真：

   • 使用初始观测值和目标轨迹调用dummyDoubleIntegrator.run()方法开始执行虚拟循环，直至ROS环境关闭。

总之，这段代码是搭建在ROS上的一个闭环控制系统框架，针对的是双积分器系统，使用了OCS2库进行运动规划与控制，并且集成了多机器人任务分配(MRT)算法以及相应的可视化功能。

#include <ros/init.h>  // 包含ROS初始化头文件
#include <ros/package.h>  // 包含ROS包操作头文件

#include <ocs2_ros_interfaces/mrt/MRT_ROS_Dummy_Loop.h>  // 包含自定义MRT_ROS_Dummy_Loop头文件
#include <ocs2_ros_interfaces/mrt/MRT_ROS_Interface.h>  // 包含自定义MRT_ROS_Interface头文件

#include <ocs2_double_integrator/DoubleIntegratorInterface.h>  // 包含自定义DoubleIntegratorInterface头文件
#include <ocs2_double_integrator/definitions.h>  // 包含自定义definitions头文件

#include "ocs2_double_integrator_ros/DoubleIntegratorDummyVisualization.h"  // 包含自定义DoubleIntegratorDummyVisualization头文件

int main(int argc, char** argv) {
  const std::string robotName = "double_integrator";  // 定义机器人名称为"double_integrator"

  // task file
  std::vector<std::string> programArgs{};  // 定义程序参数向量
  ::ros::removeROSArgs(argc, argv, programArgs);  // 移除ROS参数
  if (programArgs.size() <= 1) {  // 如果程序参数大小小于等于1
    throw std::runtime_error("No task file specified. Aborting.");  // 抛出运行时错误，没有指定任务文件，终止程序
  }
  std::string taskFileFolderName = std::string(programArgs[1]);  // 将programArgs[1]转换为std::string类型，并赋值给taskFileFolderName

  // Initialize ros node
  ros::init(argc, argv, robotName + "_mrt");  // 初始化ROS节点
  ros::NodeHandle nodeHandle;  // 创建ROS节点句柄

  // Robot interface
  const std::string taskFile = ros::package::getPath("ocs2_double_integrator") + "/config/" + taskFileFolderName + "/task.info";  // 定义taskFile路径
  const std::string libFolder = ros::package::getPath("ocs2_double_integrator") + "/auto_generated";  // 定义libFolder路径
  ocs2::double_integrator::DoubleIntegratorInterface doubleIntegratorInterface(taskFile, libFolder);  // 创建DoubleIntegratorInterface对象

  // MRT
  ocs2::MRT_ROS_Interface mrt(robotName);  // 创建MRT_ROS_Interface对象
  mrt.initRollout(&doubleIntegratorInterface.getRollout());  // 初始化MRT_ROS_Interface的rollout
  mrt.launchNodes(nodeHandle);  // 启动MRT_ROS_Interface的节点

  // Visualization
  auto doubleIntegratorDummyVisualization = std::make_shared<ocs2::double_integrator::DoubleIntegratorDummyVisualization>(nodeHandle);  // 创建DoubleIntegratorDummyVisualization对象

  // Dummy loop
  ocs2::MRT_ROS_Dummy_Loop dummyDoubleIntegrator(mrt, doubleIntegratorInterface.mpcSettings().mrtDesiredFrequency_,  // 创建MRT_ROS_Dummy_Loop对象
                                                 doubleIntegratorInterface.mpcSettings().mpcDesiredFrequency_);

  dummyDoubleIntegrator.subscribeObservers({doubleIntegratorDummyVisualization});  // 订阅dummyDoubleIntegrator的observers

  // initial state
  ocs2::SystemObservation initObservation;  // 创建SystemObservation对象
  initObservation.time = 0.0;  // 设置initObservation的时间为0.0
  initObservation.state = doubleIntegratorInterface.getInitialState();  // 设置initObservation的状态为doubleIntegratorInterface的初始状态
  initObservation.input = ocs2::vector_t::Zero(ocs2::double_integrator::INPUT_DIM);  // 设置initObservation的输入为长度为double_integrator::INPUT_DIM的全零向量

  // initial command
  const ocs2::TargetTrajectories initTargetTrajectories({0.0}, {doubleIntegratorInterface.getInitialTarget()},
                                                        {ocs2::vector_t::Zero(ocs2::double_integrator::INPUT_DIM)});  // 创建initTargetTrajectories对象

  // Run dummy (loops while ros is ok)
  dummyDoubleIntegrator.run(initObservation, initTargetTrajectories);  // 运行dummyDoubleIntegrator

  return 0;  // 返回0
}

3.10 CMakeLists.txt

这段代码是一个CMake文件，用于构建一个名为ocs2_double_integrator_ros的ROS节点包。它配置了依赖项、包含目录和链接库，并生成可执行文件。还配置了安装目录和clang工具。

该CMakeLists.txt文件用于配置一个基于ROS的项目ocs2_double_integrator_ros的构建过程。以下是逐行详细解释：

1. cmake_minimum_required(VERSION 3.0.2): 设置所需的CMake最低版本为3.0.2。

2. project(ocs2_double_integrator_ros): 定义当前项目名为ocs2_double_integrator_ros。

3. set(CMAKE_EXPORT_COMPILE_COMMANDS ON): 启用生成compile_commands.json文件，以便于Clang工具链使用。

4. set(CATKIN_PACKAGE_DEPENDENCIES...): 设置该项目依赖的一系列catkin_packages。

5-6. find_package(catkin REQUIRED COMPONENTS ...) 和 find_package(Boost REQUIRED COMPONENTS ...): 寻找并配置ROS核心库catkin及其指定组件以及Boost库的系统和文件系统模块。

7.find_package(Eigen3 3.3 REQUIRED NO_MODULE): 寻找并配置Eigen3数学库，要求版本至少为3.3。

8-27. catkin_package(...): 定义catkin包信息，包括头文件目录、依赖的catkin包以及其他非catkin依赖（如Boost）。

29-36. include_directories(...): 添加所有必要的包含目录到编译器搜索路径中。

38-47. add_executable(double_integrator_mpc ...) 和相关链接库命令：定义名为double_integrator_mpc的可执行目标，源代码为src/DoubleIntegratorMpcNode.cpp，并链接所需的catkin库。

49-60. 类似地，定义了另外两个可执行目标double_integrator_dummy_test和double_integrator_target，分别对应不同的源文件，并同样链接所需库。

62-75. find_package(cmake_clang_tools QUIET)及后续内容：如果找到cmake_clang_tools，则为上述三个可执行目标添加Clang工具支持，设置源码目录、头文件目录等。

77-82. install(DIRECTORY ...): 配置安装阶段，将头文件目录安装至指定位置。

83-88. install(TARGETS ...): 安装构建好的可执行文件至指定的bin目录。

89-90. install(DIRECTORY ...): 将launch和rviz目录下的文件安装至share目录下。

总之，这段CMake脚本主要完成了以下任务：

• 设置项目属性和依赖项
• 配置编译选项和包含目录
• 定义并配置可执行目标及其链接库
• 设置Clang工具支持（如果可用）
• 配置项目的安装路径和内容

cmake_minimum_required(VERSION 3.0.2)
project(ocs2_double_integrator_ros)

# 生成 compile_commands.json 用于 clang 工具
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)

set(CATKIN_PACKAGE_DEPENDENCIES
  ocs2_core
  ocs2_ddp
  ocs2_mpc
  ocs2_ros_interfaces
  ocs2_double_integrator
  ocs2_robotic_tools
  ocs2_robotic_assets
)

find_package(catkin REQUIRED COMPONENTS
  roslib
  ${CATKIN_PACKAGE_DEPENDENCIES}
)

find_package(Boost REQUIRED COMPONENTS
  system
  filesystem
)

find_package(Eigen3 3.3 REQUIRED NO_MODULE)

###################################
## catkin 专属配置信息 ##
###################################

catkin_package(
  INCLUDE_DIRS
    include
    ${EIGEN3_INCLUDE_DIRS}
  CATKIN_DEPENDS
    ${CATKIN_PACKAGE_DEPENDENCIES}
  DEPENDS
    Boost
)

###########
## 编译 ##
###########

include_directories(
  include
  ${EIGEN3_INCLUDE_DIRS}
  ${Boost_INCLUDE_DIRS}
  ${catkin_INCLUDE_DIRS}
)

# Mpc节点
add_executable(double_integrator_mpc
  src/DoubleIntegratorMpcNode.cpp
)
add_dependencies(double_integrator_mpc
  ${catkin_EXPORTED_TARGETS}
)
target_link_libraries(double_integrator_mpc
  ${catkin_LIBRARIES}
)

# Dummy节点（用于测试）
add_executable(double_integrator_dummy_test
  src/DummyDoubleIntegratorNode.cpp
  src/DoubleIntegratorDummyVisualization.cpp
)
add_dependencies(double_integrator_dummy_test
  ${catkin_EXPORTED_TARGETS}
)
target_link_libraries(double_integrator_dummy_test
  ${catkin_LIBRARIES}
)

# 目标节点
add_executable(double_integrator_target
  src/DoubleIntegratorTargetPoseCommand.cpp
)
add_dependencies(double_integrator_target
  ${catkin_EXPORTED_TARGETS}
)
target_link_libraries(double_integrator_target
  ${catkin_LIBRARIES}
  ${Boost_LIBRARIES}
)
target_compile_options(double_integrator_target PRIVATE ${OCS2_CXX_FLAGS})

#########################
###   CLANG 工具集  ###
#########################

find_package(cmake_clang_tools QUIET)
if(cmake_clang_tools_FOUND)
  message(STATUS "运行 clang 工具集。")
  add_clang_tooling(
    TARGETS
      double_integrator_mpc
      double_integrator_dummy_test
      double_integrator_target
    SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test
    CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
    CF_WERROR
  )
endif(cmake_clang_tools_FOUND)

#############
## 安装 ##
#############

install(DIRECTORY include/${PROJECT_NAME}/
  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
install(
  TARGETS
    double_integrator_mpc
    double_integrator_dummy_test
    double_integrator_target
  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY launch rviz
  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

3.11 package.xml

该XML函数是一个用于描述软件包信息的文件，包含了软件包的名称、版本、描述、维护人员、许可证、构建工具依赖、构建依赖和依赖关系等信息。

该XML文件是一个遵循ROS（Robot Operating System）的package.xml格式的文件，用于定义名为"ocs2_double_integrator_ros"的软件包详细信息：

1. <?xml version="1.0"?>：声明这是一个XML文档，版本为1.0。
2. <package format="2">：表示package描述符的版本为2。
3. <name>ocs2_double_integrator_ros</name>：定义了软件包的名称。
4. <version>0.0.0</version>：指定当前软件包的版本号。
5. <description>The ocs2_double_integrator_ros package</description>：简要介绍软件包的功能或用途。
6. <maintainer>标签下的内容：列举了三位维护者及其联系方式，分别是Farbod Farshidian、Jan Carius和Ruben Grandia。
7. <license>TODO</license>：本应包含软件包的许可证信息，但此处未填写完整，仅显示“TODO”待定。
8. <buildtool_depend>catkin</buildtool_depend>：表明此软件包使用Catkin作为构建工具。
9. <build_depend>cmake_clang_tools</build_depend>：说明在构建过程中依赖于cmake_clang_tools包。
10. <depend>标签下的多个条目：列举了一系列运行时依赖的其他ROS软件包，如roslib、pybind11_catkin、ocs2_core等。

综上，这个XML文件提供了管理和构建"ocs2_double_integrator_ros"软件包所需的所有基本信息。

<?xml version="1.0"?>
<package format="2">
  <name>ocs2_double_integrator_ros</name>
  <version>0.0.0</version>
  <description>The ocs2_double_integrator_ros package</description>

  <maintainer email="farbod.farshidian@gmail.com">Farbod Farshidian</maintainer>
  <maintainer email="jcarius@ethz.ch">Jan Carius</maintainer>
  <maintainer email="rgrandia@ethz.ch">Ruben Grandia</maintainer>

  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>cmake_clang_tools</build_depend>

  <depend>roslib</depend>
  <depend>pybind11_catkin</depend>

  <depend>ocs2_core</depend>
  <depend>ocs2_ddp</depend>
  <depend>ocs2_mpc</depend>
  <depend>ocs2_ros_interfaces</depend>
  <depend>ocs2_double_integrator</depend>
  <depend>ocs2_robotic_tools</depend>
  <depend>ocs2_robotic_assets</depend>

</package>

该XML文件是一个ROS包描述文件，用于描述一个名为ocs2_double_integrator_ros的ROS包。

属性	说明
name	包的名称为ocs2_double_integrator_ros。
version	包的版本号为0.0.0。
description	包的描述为"The ocs2_double_integrator_ros package"。
maintainer	包的维护人员的邮箱和姓名，共有三个维护人员。
license	包的许可证信息还未填写，需要补充。
buildtool_depend	构建工具依赖为catkin。
build_depend	构建依赖为cmake_clang_tools。
depend	依赖了多个包，用于实现包的功能。

该XML文件标记了包的相关信息和依赖关系，用于在ROS环境中管理和构建该包。