< >
Home » Isaac ENGINE入门教程 » Isaac ENGINE入门教程-Codelets介绍

Isaac ENGINE入门教程-Codelets介绍

Isaac ENGINE入门教程-Codelets介绍

说明：

  • 本教程使用现有组件来解释组件周围的各种重要概念。

Codelets

  • 组件是机器人应用程序的基本构建块。

  • Isaac SDK包含可在您的应用程序中使用的各种组件。

  • //packages/planner包的目录中包含的组件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 it's ego motion in an odometry frame.
  ISAAC_PROTO_RX(DifferentialBaseStateProto, 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 and tick

  • 代码段：
 class DifferentialBaseOdometry : public alice::Codelet {

  • Codelet是非常常见的组件，使您可以编写重复执行的代码。

  • DifferentialBaseOdometry 派生自alice::Codelet。

  • Codelet以下列三种方式之一运行：

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

  • 接受消息执行：只要收到新消息，就会执行功能。 典型的例子是图像处理算法，其计算捕获的每个新相机图像上的某些信息。

  • 触发执行：功能在完成后立即再次执行。 一个典型的例子是硬件驱动程序，它以阻塞模式读取套接字。

  • 如果可能，始终使用硬件阻塞通信而不是线程。

  • Isaac SDK自动完成多线程部分。

  • DifferentialBaseOdometry使用定期执行。

  • 这在启动功能中实现，如下所示：

void DifferentialBaseOdometry::start() {
  ...
  tickPeriodically();
  ...
}
  • 周期本身在配置中设置，如下所述。

接收消息

  • 许多组件接收或传输消息到其他组件。
  • 消息传递是一种封装组件和确保代码库模块化的强大方法。
// Incoming current dynamic state of the differential base which is used to estimate its
// ego motion in an odometry frame.
ISAAC_PROTO_RX(DifferentialBaseStateProto, state)
  • ISAAC_PROTO_RX宏用于定义接收（RX）信道。
  • 宏有两个参数：消息的类型和通道的名称。
  • Isaac SDK并不特别依赖于特定的消息格式，但目前capnproto已广泛使用。
  • 有关更多信息，请参阅capnproto网站。
  • 例如，可以在接收信道上读取消息，如下所示：
const auto& rmp_reader = rx_state().getProto();
...
state_.speed() = rmp_reader.getLinearSpeed();
  • 函数rx_state由ISAAC_PROTO_RX宏自动生成，并且需要一个DifferentialBaseStateProto消息。
  • 函数getLinearSpeed是从消息模式自动生成的。
  • 可以在//message文件夹或本文档的相应部分中找到Isaac SDK的所有消息模式。

传输消息

  • 在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在此通道上启动新消息。
  • capnproto模式自动生成的函数（如initOdomTRobot和setSpeed）可用于将数据写入消息proto。
  • 消息完成后，可以通过publish（）函数发送。
  • 一次只能生成一条消息。

配置参数

  • 复杂的算法通常可以以各种不同的方式进行参数化。
  • 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：该名称定义参数存储在配置文件中的键和用于在代码中访问它的函数名称。

  • default value：如果在配置文件中未指定任何值，则使用此值。 也可以省略默认值，强制用户在配置文件中指定值。

  • 在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部分来更改配置参数：

{
  "config": {
    ...
    "segway_odometry": {
      "isaac.navigation.DifferentialBaseOdometry": {
        "max_acceleration": 2.0
      }
    }
    ...
  }
}
  • 在此示例中，segway_odometry是包含名称为isaac.navigation.DifferentialBaseOdometry的类型的组件的节点的名称。

位姿

  • Isaac SDK自动具有全局姿势树，可用于计算3D或2D坐标系的相对姿势。
  • Isaac SDK姿势树还缓存姿势的时间历史，以允许相对于不同时间点的查询。
  • 如果组件需要读取姿势，则应使用ISAAC_POSE2或ISAAC_POSE3宏：
// This is the pose under which the ego motion estimation will be written to the pose tree.
ISAAC_POSE2(odom, robot)
  • ISAAC_POSE2/3宏有两个参数，表示有问题的两个坐标框架。
  • 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());
  • 请注意，使用宏时会自动生成函数set_odom_T_robot（以及类似的get_odom_T_robot）。
  • 根据特定时间点读取姿势。
  • 在此示例中使用了getTickTime。
  • 在各个时间点查询姿势对于数据信道的时间同步是至关重要的，以避免滞后和数据不匹配。
  • 如果要从姿势树中读取姿势，可以使用类似的机制：
const Pose2d foo_T_bar = get_foo_T_bar(getTickTime());

纠错,疑问,交流: 请进入讨论区或 点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: isaac engine入门教程