小码详解(Understanding Codelets)

小码详解(Understanding Codelets)

https://docs.nvidia.com/isaac/isaac/doc/engine/components.html

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

公共组件接口DifferentialBaseOdometry位于 //packages/planner目录下。该组件从差分轮的里程计获取数据，并尝试估计机器人的姿势。

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({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 {

小码是非常常见的组件，可用于编写复用代码。 DifferentialBaseOdometry继承alice::Codelet。

小码可以以下三种方式运行：

• 定期执行(Tick periodically)：滴答功能在固定时间段定期执行。一个典型的例子是控制器，它每秒滴答100次将控制命令发送到相应硬件。
• 消息触发(Tick on message)：每当收到新消息时便执行函数。一个典型的示例是图像处理算法，该算法会针对相机捕获的每个新图像进行计算。
• 滴答阻塞(Tick blocking)：滴答函数完成后立即再次执行。一个典型的示例是硬件驱动程序，它以阻塞模式读取套接字。

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

DifferentialBaseOdometry使用定期执行。这在start函数中实现，如下所示：

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

滴答周期本身是可配置中设置的，如下文所述。

接收消息(Receiving Messages)

许多组件都接收消息或发送消息到其他组件。消息传递是封装组件并确保代码库模块化的有效方法。

// 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宏自动生成，并且StateProto消息会包含包含DifferentialBaseDynamics 信息。Isaac SDK的所有消息架构都可以在//message文件夹中或本文档的相应部分中找到。

发送消息(Transmitting Messages)

滴答声结束时，在完成所有计算之后，组件通常希望向正在侦听的接收者发送新消息。

// 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()功能发送。一次只能生成一条消息。

配置参数(Configuration Parameters)

复杂算法通常可以用各种不同的方式进行参数化。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，SO2 / 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中，可以通过将以下config部分添加到JSON来更改配置参数：

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

在此示例中，segway_odometry是节点的名称，其中包含name为isaac.navigation.DifferentialBaseOdometry的组件。

应用程序JSON

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

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

应用程序提供的“name”必须与适当的BUILD文件中给isaac_app的name匹配，并且还必须与JSON文件的文件名（在本例中为“ my_application.app.json”）匹配。

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

应用程序“图形(graph)”定义了与使用中的组件相对应的“节点”，“边缘(edges)”连接了这些节点。边缘确定不同节点之间的消息传递顺序。应用程序图的“节点”部分的示例：

"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"
      }
    ]
  }
]

请注意，“类型”参数必须与宏ISAAC_REGISTER_COMPONENT或ISAAC_REGISTER_CODELET赋予的组件名称匹配。另外，(需要发送和接收消息功能的）每个节点必须包含message_ledger组件，以便处理往返于该节点的消息。

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

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

此示例假定component_a具有定义为名称“输出”的ISAAC_PROTO_TX消息，而component_b具有定义为相同类型的名称“ input”的ISAAC_PROTO_RX消息。

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

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

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

上面假定component_a已通过“ param”的名称定义了一个ISAAC_PARAM，并将其设置为0.1。

有关包含使用上面介绍的概念构建应用程序图的教程，请参见使用C ++开发Codelets。

子图(Subgraphs)

随着向应用程序中添加更多组件，应用程序图可能变得冗长且重复。子图可用于简化应用程序图。当使用和重用具有多个组件连接在一起的节点时，JSON子图可以包括所需的组件，边和配置，因此您可以添加一个相对较高级别的组，而不必担心较低级别的细节。

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

../../_images/subgraphs1.png

这样，可以极大地简化App 1和App 2的图形，如下所示。这种抽象简化了应用程序，降低了维护成本，隐藏了专业知识，并提供了更好的用户体验。

../../_images/subgraphs2.png

子图的示例是apps / carter / carter_hardware.subgraph.json，其中包含几乎所有与Carter硬件有关的应用程序所需的边缘和组件。子图可以包含在整个应用程序的较大JSON配置中，而不是在每个应用程序的JSON文件中重复该信息。以下是carter_hardware子图的示例 ，其中包含Segway基座，Velodyne激光雷达和BMI160惯性测量单元（IMU）的节点。组件类型isaac :: alice :: Subgraph使子图的输入/输出更整洁；本节稍后部分将提供一个示例。子图除了“图形”部分外，还包括“模块”，“边缘”和“配置”，如下所示。

{
  "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",
        "components": [
          {
            "name": "lidar_initializer",
            "type": "isaac::alice::PoseInitializer"
          },
          {
            "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": {
      "lidar_initializer": {
        "pose": [1.0, 0.0, 0.0, 0.0, -0.04, 0.0, 0.59]
      }
    },
    "carter2.vlp16": {
      "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。否则，格式如Application JSONs中所述。

子图可以嵌套。例如，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": {
       "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 <>）语法，如上所示。

为防止节点名称在“边缘”部分中添加前缀，请在边缘名称中添加“ /”字符。在下面的示例中，如果由于子图名称引起的前缀为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$ bazel run packages/freespace_dnn/apps:freespace_dnn_inference_image -- --config inference:packages/freespace_dnn/apps/freespace_dnn_inference_sidewalk_tensorrt.config.json

该inference:会导致“推断”前缀的文件名前规范被应用到所有的节点时，装载 包/ freespace_dnn /应用/ freespace_dnn_inference_sidewalk_tensorrt.config.json。

姿势

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提供由3-DOF方向和3-DOF转换组成的6-DOF信息，因此在一般情况下适用。 Pose2d像大多数轮式机器人一样，假设运动在XY平面上的倾斜和滚动很小，可以提供1-DOF方向和2-DOF平移。在这种情况下，使用起来更容易，效率也更高。请根据移动的假设选择其中之一。