16.Isaac教程--Codelets详解

Codelets详解

ISAAC教程合集地址: https://blog.csdn.net/kunhe0512/category_12163211.html

组件是机器人应用程序的基本构建块。 Isaac SDK 包含可在您的应用程序中使用的各种组件。 本教程使用现有组件来解释围绕组件的各种重要概念。

包含在 //packages/navigation 目录中的组件 DifferentialBaseOdometry 的公共接口如下所示。 该组件监听来自差分基座的车轮里程计，并尝试估计机器人的姿态。

namespace isaac {
namespace navigation {

// Integrates (2D) odometry for a differential base to estimate it's
// ego motion.
class DifferentialBaseOdometry : public alice::Codelet {
 public:
  void start() override;
  void tick() override;

  // Incoming current dynamic state of the differential base which is used to estimate its
  // ego motion in an odometry frame (type: DifferentialBaseDynamics)
  ISAAC_PROTO_RX(StateProto, state);
  // Outgoing ego motion estimate for the differential base.
  ISAAC_PROTO_TX(Odometry2Proto, odometry);

  // Maximum acceleration to use (helps with noisy data or wrong data
  // from simulation)
  ISAAC_PARAM(double, max_acceleration, 5.0);
  // The name of the source coordinate frame under which to publish
  // the pose estimate.
  ISAAC_PARAM(std::string, odometry_frame, "odom");
  // The name of the target coordinate frame under which to publish
  // the pose estimate.
  ISAAC_PARAM(std::string, robot_frame, "robot");
  // 1 sigma of noise used for prediction model in the following order:
  //   pos_x, pos_y, heading, speed, angular_speed, acceleration
  ISAAC_PARAM(Vector6d, prediction_noise_stddev, \
      (MakeVector<double, 6>({0.05, 0.05, 0.35, 0.05, 1.00, 3.00})));
  // 1 sigma of noise used for observation model in the following order:
  //   speed, angular_speed, acceleration
  ISAAC_PARAM(Vector3d, observation_noise_stddev, \
      (Vector3d{0.25, 0.45, 2.0}));

  // This is the pose under which the ego motion estimation will be
  // written to the pose tree.
  ISAAC_POSE2(odom, robot);

 private:
  ...
};

}  // namespace navigation
}  // namespace isaac

ISAAC_ALICE_REGISTER_CODELET(isaac::navigation::DifferentialBaseOdometry);

以下部分逐步介绍 DifferentialBaseOdometry，解释每个部分。

Codelets 和 tick

class DifferentialBaseOdometry : public alice::Codelet {

Codelets 是非常常见的组件，它使您能够编写重复执行的代码。 DifferentialBaseOdometry 派生自 alice::Codelet。

小码以下列三种方式之一运行：

• 定期Tick：滴答函数在固定时间段后定期执行。 一个典型的例子是控制器每秒滴答 100 次以向硬件发送控制命令。

• Tick on message：只要收到新消息，就会执行 tick 函数。 一个典型的例子是图像处理算法，它计算关于捕获的每个新相机图像的某些信息。

• Tick blocking：tick 函数完成后立即再次执行。 一个典型的例子是在阻塞模式下读取套接字的硬件驱动程序。

注意

如果可能，始终使用与硬件而不是线程的阻塞通信。 Isaac SDK 自动创建和管理必要的线程。

当codelet ticks时，它会阻止同一节点中的其他小码同时滴答。 要并行运行小码，请将它们放在单独的节点中。

DifferentialBaseOdometry 使用周期性刻度。 这是在启动函数中实现的，如下所示：

void DifferentialBaseOdometry::start() {
  ...
  tickPeriodically();
  ...
}

tick 周期本身在配置中设置，稍后解释。

接收消息

许多组件接收消息或将消息传输到其他组件。 消息传递是封装组件和确保代码库模块化的一种强大方式。

// Incoming current dynamic state of the differential base which is used to estimate its
// ego motion in an odometry frame (type: DifferentialBaseDynamics)
ISAAC_PROTO_RX(StateProto, state);

ISAAC_PROTO_RX 宏用于定义接收 (RX) 通道。 宏有两个参数：消息的类型和通道的名称。 Isaac SDK 并不特别依赖于特定的消息格式，但目前 cap'n'proto 被广泛使用。 有关详细信息，请参阅 cap’n’proto 网站。

例如，可以在接收通道上读取消息，如下所示：

const auto& rmp_reader = rx_state().getProto();
...
state_.speed() = rmp_reader.getLinearSpeed();

函数 rx_state 由 ISAAC_PROTO_RX 宏自动生成，并且预期包含 DifferentialBaseDynamics 的 StateProto 消息。 Isaac SDK 的所有消息模式都可以在 //message 文件夹或本文档的相应部分中找到。

传输消息

在 tick 结束时，在完成所有计算之后，组件通常希望向正在收听的任何人发送一条新消息。

// Outgoing ego motion estimate for the differential base.
ISAAC_PROTO_TX(Odometry2Proto, odometry)

ISAAC_PROTO_TX 宏用于定义传输 (TX) 通道。 这与 ISAAC_PROTO_RX 宏的工作方式非常相似。

可以创建并发送一条消息，如下所示：

auto odom_builder = tx_odometry().initProto();
ToProto(odom_T_robot, odom_builder.initOdomTRobot());
odom_builder.setSpeed(state_.speed());
...
tx_odometry().publish();

同样，tx_odometry 函数由 ISAAC_PROTO_TX 宏自动创建。 使用 initProto 在此频道上开始一条新消息。 由 cap'n'proto 模式自动生成的函数（如 initOdomTRobot 和 setSpeed）可用于将数据写入消息原型。 消息完成后，可以通过 publish() 函数发送。 一次只能生成一条消息。

方便的 ToProto/FromProto 函数

cap'n'proto 直接支持整数等基本数据类型，但处理复杂数据类型可能会更加困难。 为了处理这种情况，Isaac SDK 提供了方便的 ToProto/FromProto 函数，它们具有如下常见的模式：

// Writes a UUID to a proto
void ToProto(const Uuid& uuid, ::UuidProto::Builder builder)
// Reads a UUID from a proto
Uuid FromProto(::UuidProto::Reader reader)

或者

// Parses a tensor from a message. This version parses tensors of any element type and rank.
bool FromProto(::TensorProto::Reader reader, const std::vector<isaac::SharedBuffer>& buffers,
               isaac::UniversalTensorConstView<Storage>& universal_view);
// Creates a tensor from a proto. Will print errors and return false if the tensor type is not
// compatible with the proto.
bool FromProto(::TensorProto::Reader reader, const std::vector<isaac::SharedBuffer>& buffers,
               isaac::TensorBase<K, Dimensions, BufferType>& tensor_view)

请参阅包含 ToProto/FromProto 函数的头文件的 messages/ 文件夹。

配置参数

复杂的算法通常可以通过各种不同的方式进行参数化。 ISAAC_PARAM 允许您定义一个配置参数，该参数可以通过配置设置、在代码中读取并在前端更改。

// Maximum acceleration to use (helps with noisy data or wrong data
// from simulation)
ISAAC_PARAM(double, max_acceleration, 5.0)

ISAAC_PARAM 有三个参数：

• type：这是配置参数的类型。 基本类型是 int、double、bool 和 std::string。 Isaac SDK 还支持各种数学类型，例如 Pose2/3、* 以及特征向量和矩阵。 还支持任何这些类型的 STD 矢量。

• name：名称定义了参数在配置文件中存储的键和可以在代码中访问它的函数名称。

• 默认值：如果配置文件中没有指定值，则使用该值。 也可以省略默认值，这会强制用户在配置文件中指定一个值。

在 DifferentialBaseOdometry 的示例中，tick 函数从检索卡尔曼滤波器中使用的所需预测噪声开始：

void DifferentialBaseOdometry::tick() {
  navigation::DifferentialBaseState prediction_noise_stddev;
  prediction_noise_stddev.elements = get_prediction_noise_stddev();

可以通过多种方式更改配置：

• 可以更改默认配置参数。 这应该谨慎使用，因为它会更改所有尚未覆盖配置文件中值的应用程序的值。

• 该值可以在 JSON 配置文件中设置。 大多数示例应用程序都包含一个 JSON 文件，其中设置了各种参数。 例如在 //app/samples/simple_robot 中，可以通过将以下 JSON 添加到配置部分来更改配置参数：

{
  "config": {
    ...
    "segway_odometry": {
      "isaac.navigation.DifferentialBaseOdometry": {
        "max_acceleration": 2.0
      }
    }
    ...
  }
}

在此示例中，segway_odometry 是节点的名称，该节点包含我们类型的组件，名称为 isaac.navigation.DifferentialBaseOdometry。

应用程序 JSON

每个 Isaac 应用程序都基于一个 JSON 文件。 JSON 文件描述了应用程序的依赖关系、节点图和消息流，并包含自定义配置。 一个JSON应用文件的基本结构：

{
  "name": "my_application",
  "modules": [
    ...
  ],
  "graph": {
    "nodes": [
      ...
    ],
    "edges": [
      ...
    ]
  },
  "config": {
    ...
  }
}

为应用程序指定的“name”必须与相应 BUILD 文件中为 isaac_app 指定的名称相匹配，并且还必须与 JSON 文件的文件名相匹配（在本例中为“my_application.app.json”）。

“modules”列表列举了包含此应用程序中使用的组件的所有包。 BUILD 文件中为 isaac_app 指定的“模块”列表必须包含 JSON 中的列表。

应用程序“graph”定义了与正在使用的组件相对应的“nodes”，以及连接节点的“edges”。 边决定了不同节点之间的消息传递顺序。 应用图的“nodes”部分的示例：

"nodes": [
  {
    "name": "node_1",
    "components": [
      {
        "name": "message_ledger",
        "type": "isaac::alice::MessageLedger"
      },
      {
        "name": "component_a",
        "type": "isaac::alice::ComponentX"
      }
    ]
  },
  {
    "name": "node_2",
    "components": [
      {
        "name": "message_ledger",
        "type": "isaac::alice::MessageLedger"
      },
      {
        "name": "component_b",
        "type": "isaac::alice:CodeletY"
      }
    ]
  }
]

请注意，“type”参数必须与宏 ISAAC_REGISTER_COMPONENT 或 ISAAC_REGISTER_CODELET 给组件的名称相匹配。 此外，每个节点都必须包含一个 message_ledger 组件，以便处理传入/传出该节点的消息。

edges 决定了不同组件之间的消息传递顺序。 每条边都需要一个源和一个目标。 使用上面的示例，node_1 的 component_a 的“输出”消息和 node_2 的 component_b 的“input”消息之间的边看起来像这样：

"edges": [
  {
    "source": "node_1/component_a/output",
    "target": "node_2/component_b/input"
  },
]

此示例假设 component_a 有一个 ISAAC_PROTO_TX 消息，定义为名称“output”，component_b 有一个 ISAAC_PROTO_RX 消息，定义为相同类型的名称“input”。

应用程序 JSON 文件还包含用于自定义行为的各种参数的配置数据或“config”。 每个配置参数都由三个元素引用：节点名称、组件名称和参数名称。

node_1 的 component_a 的名为“param”的浮点值参数的配置如下所示：

"config": {
  "node_1": {
    "component_a": {
      "param": 0.1
     }
   }
 }

这假设 component_a 定义了一个名为“param”的 ISAAC_PARAM 并将其设置为 0.1。

请参阅 Developing Codelets in C++ 以获取包含使用上述概念构建应用程序图的教程。

子图

随着越来越多的组件被添加到应用程序中，应用程序图可能会变得冗长且重复。 子图可以证明在简化应用程序图方面很有用。 当使用和重用多个组件连接在一起的节点时，JSON 子图可以包含所需的组件、边和配置，这样您就可以添加一个相对较高级别的组，而无需关心较低级别的细节。

在下图中，App 1 和 App 2 中的节点 A、B 和 C 是相同的。 可以创建子图 X，而不是在每个应用程序的 JSON 中复制它们。

这样，App 1 和 App 2 的图形就大大简化了，如下所示。 这种抽象简化了应用程序，减少了维护，隐藏了专业知识，并提供了更好的用户体验。

子图的一个示例是 apps/carter/carter_hardware.subgraph.json，它包含几乎所有与 Carter 硬件相关的应用程序所需的边和组件。 子图可以包含在整个应用程序的更大 JSON 配置中，而不是在每个应用程序的 JSON 文件中重复该信息。 下面是 carter_hardware 子图的示例，其中包含 Segway 底座、Velodyne 激光雷达和 BMI160 惯性测量单元 (IMU) 的节点。 组件类型 isaac::alice::Subgraph 使子图的输入/输出更清晰； 本节后面提供了一个示例。 除了“graph”部分之外，子图还包括“modules”, “edges”, 和 “config”，如下所示。

{
  "modules": [
    "imu",
    "segway",
    "velodyne_lidar"
  ],
  "graph": {
    "nodes": [
      {
        "name": "subgraph",
        "components": [
          {
            "name": "message_ledger",
            "type": "isaac::alice::MessageLedger"
          },
          {
            "name": "interface",
            "type": "isaac::alice::Subgraph"
          }
        ]
      },
      {
        "name": "segway_rmp",
        "components": [
          {
            "name": "message_ledger",
            "type": "isaac::alice::MessageLedger"
          },
          {
            "name": "isaac.SegwayRmpDriver",
            "type": "isaac::SegwayRmpDriver"
          },
          {
            "name": "isaac.alice.Failsafe",
            "type": "isaac::alice::Failsafe"
          }
        ]
      },
      {
        "name": "vlp16_initializer",
        "components": [
          {
            "name": "lidar_initializer",
            "type": "isaac::alice::PoseInitializer"
          }
        ]
      },
        "name": "vlp16",
        "components": [
          {
            "name": "message_ledger",
            "type": "isaac::alice::MessageLedger"
          },
          {
            "name": "VelodyneLidar",
            "type": "isaac::velodyne_lidar::VelodyneLidar"
          }
        ]
      },
      {
        "name": "imu",
        "components": [
          {
            "name": "message_ledger",
            "type": "isaac::alice::MessageLedger"
          },
          {
            "name": "IioBmi160",
            "type": "isaac::imu::IioBmi160"
          }
        ]
      }
    ],
    "edges": [
      {
        "source": "subgraph/interface/diff_base_command",
        "target": "segway_rmp/isaac.SegwayRmpDriver/segway_cmd"
      },
      {
        "source": "segway_rmp/isaac.SegwayRmpDriver/segway_state",
        "target": "subgraph/interface/diff_base_state"
      },
      {
        "source": "vlp16/VelodyneLidar/scan",
        "target": "subgraph/interface/scan"
      },
      {
        "source": "imu/IioBmi160/imu_raw",
        "target": "subgraph/interface/imu_raw"
      }
    ]
  },
  "config": {
    "segway_rmp": {
      "isaac.SegwayRmpDriver": {
        "ip": "192.168.0.40",
        "tick_period": "20ms"
      },
      "isaac.alice.Failsafe": {
        "name": "robot_failsafe"
      }
    },
    "vlp16": {
      "VelodyneLidar": {
        "ip": "192.168.0.5"
      }
    },
    "imu": {
      "IioBmi160": {
        "i2c_device_id": 1,
        "tick_period": "100Hz"
      }
    }
  }
}

使用如下所示的语法使用子图。 重要的是要注意子图中的每个节点都以子图名称为前缀，例如 carter1.segway_rmp。 这允许使用子图的特定实例配置或创建边。

{
  ....
  "graph": {
    "nodes": [
      {
        "name": "carter1",
        "subgraph": "apps/carter/carter_hardware.subgraph.json"
      },
      {
        "name": "carter2",
        "subgraph": "apps/carter/carter_hardware.subgraph.json"
      },
      {
        "name": "imu_corrector",
        "components": [
          {
            "name": "message_ledger",
            "type": "isaac::alice::MessageLedger"
          },
          {
            "name": "ImuCorrector",
            "type": "isaac::imu::ImuCorrector"
          }
        ]
      },
      ...
    ],
    "edges": [
      {
        "source": "carter1.subgraph/interface/imu_raw",
        "target": "imu_corrector/ImuCorrector/raw"
      },
      ...
    ]
  },
  "config": {
    "imu_corrector": {
      "ImuCorrector": {
        "calibration_variance_stationary": 0.1,
      }
    },
    "carter1.vlp16_initializer": {
      "lidar_initializer": {
        "pose": [1.0, 0.0, 0.0, 0.0, -0.04, 0.0, 0.59]
      }
    },
    "carter2.vlp16_initializer": {
      "lidar_initializer": {
        "pose": [1.0, 0.0, 0.0, 0.0, -0.04, 0.0, 0.77]
      }
    },
    ...
  }
}

请注意，当引用 carter_hardware 子图中的节点时，子图的名称用作前缀，例如 carter1.subgraph、carter1.vlp16 或 carter2.vlp16。 否则，格式如应用程序 JSON 中所述。

子图可以嵌套。 例如，carter_hardware 和 scan_flattener 子图在 2d_carter.subgraph.json 中使用，如下所示：

{
  "graph": {
    "nodes": [
      {
        "name": "subgraph",
        "components": [
          {
            "name": "message_ledger",
            "type": "isaac::alice::MessageLedger"
          },
          {
            "name": "interface",
            "type": "isaac::alice::Subgraph"
          }
        ]
      },
      {
        "name": "carter_hardware",
        "subgraph": "apps/carter/carter_hardware.subgraph.json"
      },
      {
        "name": "scan_flattener",
        "subgraph": "packages/navigation/apps/scan_flattener.subgraph.json"
      }
    ],
    "edges": [
      {
        "source": "carter_hardware.subgraph/interface/imu_raw",
        "target": "subgraph/interface/imu_raw"
      },
      ...
    ]
  },
  "config": {
      "carter_hardware.vlp16_initializer": {
       "lidar_initializer": {
         "pose": [1.0, 0.0, 0.0, 0.0, -0.04, 0.0, 0.59]
        }
      },
      "scan_flattener.range_scan_flattening": {
       "isaac.perception.RangeScanFlattening": {
         "param": $(fullname carter_hardware.vlp16/lidar_initializer)
        }
      },
      ....
  }
}

2d_carter 本身是一个子图，包含在 gmapping.app.json 和 Isaac SDK 中包含的各种其他示例应用程序中。

请注意，在为 IMU 创建边和为激光雷达设置位姿参数时，会添加所有前缀，直到达到节点定义（在这种情况下仅 carter_hardware）。 应用程序中的完整节点名称还取决于使用 2d_carter 子图时使用的名称，该子图本身不知道该名称。 要引用全名，可以使用 $(fullname <>) 语法，如上所示。

为防止在“edges”部分中添加节点名称前缀，请在边名称中添加“/”字符。 在下面的示例中，如果由于子图名称的前缀是 commander，则目标扩展为 commander.virtual_gamepad_bridge/VirtualGamepadBridge/request，而源读取 websight/WebsightServer/virtual_gamepad，这要归功于“/”特殊字符。

"edges": [
  {
    "source": "/websight/WebsightServer/virtual_gamepad",
    "target": "virtual_gamepad_bridge/VirtualGamepadBridge/request"
  },
  ....

要在应用程序中使用子图，它必须列在 BUILD 文件中 isaac_app Bazel 函数的“data”参数下。 使用 isaac_subgraph 函数在 BUILD 文件中声明子图。

使用 isaac_subgraph 函数声明子图，如下所示：

load("//engine/build:isaac.bzl", "isaac_subgraph")

isaac_subgraph(
    name = "carter_hardware_subgraph",
    subgraph = "carter_hardware.subgraph.json",
    modules = [
        "imu",
        "segway",
        "velodyne_lidar"
    ],
    visibility = ["//visibility:public"],
)

使用 isaac_subgraph 时，可以枚举子图使用的包列表，不必在使用子图的 isaac_app 的模块列表中重复。

要在应用程序中使用子图，请将其列为 isaac_app 的数据参数，如下所示：

isaac_app(
    name = "carter",
    data = [
        ...
        "//apps/carter:carter_hardware_subgraph",
    ],
    modules = [
        ...
    ]
)

要在通过命令行加载配置文件时指定前缀，请使用以下语法：

bob@desktop:~/isaac/sdk$ bazel run packages/freespace_dnn/apps:freespace_dnn_inference_image -- --config inference:packages/freespace_dnn/apps/freespace_dnn_inference_sidewalk_tensorrt.config.json

inference: 文件名前的规范将导致在加载 packages/freespace_dnn/apps/freespace_dnn_inference_sidewalk_tensorrt.config.json 时将“inference”前缀应用于所有节点。

姿态

Isaac SDK 自动拥有全局姿态树，可用于计算 3D 或 2D 坐标系的相对姿态。 Isaac SDK 姿势树还缓存姿势的时间历史，以允许相对于不同时间点的查询。

如果组件需要读取姿势，它应该使用 ISAAC_POSE2 宏：

   // This is the pose under which the ego motion estimation will be written to the pose tree.
   ISAAC_POSE2(odom, robot)

or :code:`ISAAC_POSE3` macro:

// This provides access to the very same pose as above, in Pose3 format instead of Pose2.
ISAAC_POSE3(odom, robot);

ISAAC_POSE2 或 ISAAC_POSE3 宏有两个参数，用于指示所讨论的两个坐标系。 ISAAC_POSE2(lhs, rhs) 将给出转换 lhs_T_rhs。 此变换可用于将 rhs 坐标系中的点变换为 lhs 坐标系中的点：p_lhs = lhs_T_rhs * p_rhs;。

在 DifferentialBaseOdometry 的情况下，机器人相对于它开始的位置的估计姿势被计算并写入姿势树。

const Pose2d odom_T_robot{SO2d::FromAngle(state_.heading()),
                          Vector2d{state_.pos_x(), state_.pos_y()}};
set_odom_T_robot(odom_T_robot, getTickTime());

// In case of using Pose3d, use the following line instead
const Pose3d odom_T_robot{SO3d::FromAxisAngle(Vector3{0.0, 0.0, 1.0}, state_.heading()),
                          Vector3d{state_.pos_x(), state_.pos_y(), state_.pos_z()}};
set_odom_T_robot(odom_T_robot, getTickTime());

请注意，函数 set_odom_T_robot（以及类似的 get_odom_T_robot）是在使用宏时自动生成的。

相对于特定时间点读取姿势。 在此示例中，使用了 getTickTime。 在不同时间点查询姿势对于数据通道的时间同步以避免滞后和数据不匹配至关重要。

如果你想从姿势树中读取一个姿势，可以使用类似的机制：

const Pose2d foo_T_bar = get_foo_T_bar(getTickTime());

// This is for Pose3d case
const Pose3d foo_T_bar = get_foo_T_bar(getTickTime());

Pose3d 提供 6-DOF 信息，包括 3-DOF 方向和 3-DOF 平移，因此适用于一般情况。 Pose2d 提供 1-DOF 方向和 2-DOF 平移，假设运动被限制在 X-Y 平面内，具有微不足道的俯仰和滚动，就像大多数轮式机器人所做的那样。 在这种情况下，使用起来更容易，效率也更高。 请根据运动假设选择其中之一。