ROS2与Gazebo11入门教程-逻辑相机传感器（Logical Camera Sensor）

说明：

• 介绍逻辑相机传感器

逻辑相机

• 相机输出的是图像，而逻辑相机输出的则是模型名称和位姿。 逻辑相机可以显示在同一位置上的相机可能看见哪些模型

逻辑相机示例

• 下载并保存这个仿真世界，其世界文件内容如下所示：

<?xml version="1.0" ?>

<sdf version="1.6">

<world name="default">

<include>

<uri>model://sun</uri>

</include>



<model name="post">

<pose>0 0 1 0 0 3.14159</pose>

<static>true</static>

<link name="link">

<sensor name="logical_camera" type="logical_camera">

<logical_camera>

<near>0.55</near>

<far>2.5</far>

<horizontal_fov>1.05</horizontal_fov>

<aspect_ratio>1.8</aspect_ratio>

</logical_camera>



<visualize>true</visualize>

<always_on>true</always_on>

<update_rate>10</update_rate>

</sensor>

</link>

</model>



<model name="target_box">

<static>true</static>

<pose>-2 0 1 0 0 0</pose>

<link name="link">

<collision name="collision">

<geometry>

<box>

<size>1 1 1</size>

</box>

</geometry>

</collision>

<visual name="visual">

<geometry>

<box>

<size>1 1 1</size>

</box>

</geometry>

</visual>

</link>

</model>

</world>

</sdf>

• 启动该仿真世界，命令为：

gazebo --verbose ./tutorial_logical_camera.world

• 从Gazebo顶部菜单栏中点击“窗口（Window）”菜单，然后点击“话题可视化（Topic Visualization）”菜单项。

• 选择/gazebo/default/post/link/logical_camera/models话题。该话题会在消息类型gazebo.msgs.LogicalCameraImage下列出。

• 这样就应该可以看见如下图所示的结果。

数据说明

• 逻辑摄像机会报告其看到的模型的名称和位姿。

• 普通相机看到的是几何体，而逻辑相机看到的则是几何体。逻辑相机通过测试其视锥（frustum）与模型的轴对齐边界框（AABB）是否相交来工作。模型的AABB大小正好足以容纳其整个几何体。

模型名称

• 逻辑相机报告的模型名称是作用域名称。

• 仿真世界中的模型其作用域名称为robot。

• 如果该模型具有嵌套模型<model name =“ hand”>，则嵌套模型的作用域名称是robot :: hand。

• 作用域名称可用于获取指向该模型的指针。

// Get the world (named "default")

gazebo::physics::WorldPtr world = physics::get_world("default");



// Get a model by name

gazebo::physics::ModelPtr handModel = world->GetModel("robot::hand");

模型位姿

• 逻辑相机输出其看到的相对于自身位姿的模型位姿。逻辑相机还输出其自身在仿真世界坐标中的位姿。
• 使用其自身位姿可以获得模型在仿真世界坐标系中的位姿。

// msg is of type gazebo::msgs::LogicalCameraImage



// Pose of camera in world coordinates

ignition::math::Pose3d cameraPose = gazebo::msgs::ConvertIgn(msg.pose());



// Pose of first model relative to camera

ignition::math::Pose3d modelRelativePose = gazebo::msgs::ConvertIgn(msg.model(0).pose());



// Pose of first model in world coordinates

ignition::math::Pose3d modelWorldPose = modelRelativePose - cameraPose;

假正例（False Positives）

• 逻辑相机的输出可能会包含普通相机无法看到的模型，如果存在以下情况的话：

• 模型没有。

• 模型的是透明的。

• 与模型的相比，模型的更大或位于不同的位置上。

• 模型被另一个模型遮挡住了。

• 模型在仿真世界中被旋转以至于其AABB在视锥中，但几何体不在视锥中。由于模型AABB的体积大于或等于该模型上碰撞几何体的体积，因此这个额外体积的某些部分可能会与视锥相交，但碰撞几何体没有与视锥相交的部分。下图展示了这一点：

负例（False Negatives）

• 逻辑相机的输出可能不包含普通相机可以看到的模型，如果存在以下情况的话：

• 模型几何体比其几何体更大或者在不同的位置上。

• 逻辑相机位于该模型上。逻辑相机永远不会在其输出中包含其所属的模型。但是，逻辑相机会包含其所属模型的任何嵌套模型或者父级模型。

• 模型唯一位于视锥中的部分是其嵌套模型。一个模型的AABB不包含其嵌套模型。每个嵌套模型都有其自己的AABB。如果只有嵌套模型是可见的，则在逻辑相机的输出中只会出现该嵌套模型。

SDF参数

• update_rate,每秒仿真时间内传感器生成数据的次数。

• topic,将消息发布到的Gazebo传输话题。如果此参数未指定，则传感器会在SDF中生成一个话题名称，该名称包含传感器父类的名称。

• pose,如果模型、链接和传感器的位姿均为0，则仿真世界的X轴正方向会指向逻辑相机的中心。

• visualize,如果设置为true，则相机的视锥会在Gazebo客户端中可视化。

• logical_camera

• near,传感器位姿到附近剪切平面上的最近点的距离（单位为米）。

• far,传感器位姿到远处剪切平面上的最近点的距离（单位为米）。

• horizontal_fov,相机的水平视场角（单位为弧度）。

• aspect_ratio,相机的宽高比。宽高比与水平视场角相结合就可以定义相机的垂直视场角。

获取数据

• 有两种获取数据的方法：直接从传感器获取数据，或者使用Gazebo的传输节点。
• 两种方法都会返回一个LogicalCameraImage。

直接从传感器获取数据

• 数据可以直接从传感器获取。

• 这需要编写一个Gazebo插件才能访问传感器数据。只能获取最新生成的消息。

• 1. 使用传感器名称获得一个通用传感器指针。

gazebo::sensors::SensorPtr genericSensor = sensors::get_sensor("model_name::link_name::my_logical_camera")

• 2. 将该指针投射到一个LogicalCameraSensor上。

sensors::LogicalCameraSensorPtr logicalCamera = std::dynamic_pointer_cast<sensors::LogicalCameraSensor>(genericSensor);

• 3. 调用LogicalCamera::Image()函数来获取最新的传感器数据。

gazebo::msgs::LogicalCameraImage sensorOutput = logicalCamera->Image();

使用Gazebo传输节点获取数据

• 逻辑相机传感器数据是使用Gazebo传输节点进行发布的。数据在生成后会立即发布给订阅者。

• 1. 创建一个Gazebo传输节点并对其进行初始化。

gazebo::transport::NodePtr node(new gazebo::transport::Node());

node->Init();

• 2. 为订阅创建一个回调函数。

/////////////////////////////////////////////////

// Function is called everytime a message is received.

void callback(gazebo::msgs::ConstLogicalCameraImagePtr &_msg)

{

// your code here

}

• 3. 对逻辑相机发布的话题进行侦听。

gazebo::transport::SubscriberPtr sub = node->Subscribe("~/post/link/logical_camera/models", callback);

参考:

• 资料来源