16.Isaac教程--Codelets详解

63 篇文章 98 订阅

Codelets详解

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

namespace isaac {

// 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 isaac



Codelets 和 tick

class DifferentialBaseOdometry : public alice::Codelet {


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

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

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

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

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();
...


传输消息

// 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();


方便的 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



// Parses a tensor from a message. This version parses tensors of any element type and rank.
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.
isaac::TensorBase<K, Dimensions, BufferType>& tensor_view)



配置参数

// 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、boolstd::string。 Isaac SDK 还支持各种数学类型，例如 Pose2/3*  以及特征向量和矩阵。 还支持任何这些类型的 STD 矢量。

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

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

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

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


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

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

{
"config": {
...
"segway_odometry": {
"max_acceleration": 2.0
}
}
...
}
}


应用程序 JSON

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


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

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


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

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


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

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


子图

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


{
....
"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]
}
},
...
}
}


{
"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",
}
],
"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": [
{
},
....


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_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 姿势树还缓存姿势的时间历史，以允许相对于不同时间点的查询。

   // 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_POSE2ISAAC_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());



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 平面内，具有微不足道的俯仰和滚动，就像大多数轮式机器人所做的那样。 在这种情况下，使用起来更容易，效率也更高。 请根据运动假设选择其中之一。

• 1
点赞
• 2
收藏
觉得还不错? 一键收藏
• 打赏
• 0
评论
06-26
03-31
08-29 622
01-22 813
07-02
01-14 414
01-18 390
01-18 1462
01-11 814

“相关推荐”对你有帮助么？

• 非常没帮助
• 没帮助
• 一般
• 有帮助
• 非常有帮助

¥1 ¥2 ¥4 ¥6 ¥10 ¥20

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。