OCS3 入门教程（六）- Double Integrator（三）-阿里云开发者社区

OCS3 入门教程（六）- Double Integrator（二）+https://developer.aliyun.com/article/1585273

三、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，并为它们指定了默认值。

：声明了一个名为 rviz 的布尔型参数，默认值为 true。这意味着如果没有明确指定该参数，则视为开启 RViz 可视化工具。
：声明了 multiplot 参数，默认关闭。
：声明了 task_name 参数，用于指定任务名称，默认值为 "mpc"。
：声明了 debug 参数，默认不启用调试模式。

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

: 如果 rviz 参数为 true，则会执行其中的内容，即包含并启动 ocs2_double_integrator_ros 包中的 visualize.launch 文件。
: 同理，如果 multiplot 参数为 true，则包含并启动 multiplot.launch 文件。
标签用于启动ROS节点：

第一个根据 debug 参数的值启动 double_integrator_mpc 节点。如果 debug 为 true，则使用 GDB 调试器并在新的 gnome-terminal 中启动该节点；否则直接在终端中启动。
第二个无条件启动另一个 double_integrator_dummy_test 节点，同样在新的 gnome-terminal 中启动，传入参数 task_name。

最后一个同样无条件启动 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文件的开始，它是ROS launch文件的基本结构。

是一个注释，解释了该部分代码的目的，即启动一个能够展示多个性能指标的图表。

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

定义了一个参数传递给被包含的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文件配置，主要用于启动与双积分器模型相关的节点和可视化工具。详细解释如下：

标签：

定义了两个参数变量：rvizconfig 和 model。

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

标签：

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

标签：

启动名为 "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工具）的布局和参数设置。具体分析如下：

Panels部分：

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

Visualization Manager部分：

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

Tools部分：

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

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）环境下进行双积分器系统的可视化模拟。这个类中有两个核心方法：

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

OCS4 入门教程（六）- Double Integrator（四）+https://developer.aliyun.com/article/1585275

OCS3 入门教程（六）- Double Integrator（三）