image_transport 源码详细分析

工作区路径：/home/cp/work2/ros2Learn/ros2_humble/src/ros-perception/image_common/image_transport
版本：3.1.12，许可证 BSD。

image_transport 是 ROS 图像话题的 统一发布/订阅抽象层。应用代码只面对 sensor_msgs/Image 的 base topic，底层通过 pluginlib 插件 按需启用 raw、compressed、theora 等传输方式，从而在带宽受限场景下透明切换压缩格式，而不改业务逻辑。

1. 仓库结构

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26

image_transport/
├── include/image_transport/
│   ├── image_transport.hpp          # 主入口：ImageTransport + 自由函数
│   ├── publisher.hpp / subscriber.hpp
│   ├── camera_publisher.hpp / camera_subscriber.hpp
│   ├── publisher_plugin.hpp / subscriber_plugin.hpp   # 插件基类
│   ├── simple_publisher_plugin.hpp / simple_subscriber_plugin.hpp  # 插件模板
│   ├── raw_publisher.hpp / raw_subscriber.hpp         # 内置 raw 插件
│   ├── subscriber_filter.hpp        # message_filters 适配
│   ├── transport_hints.hpp          # 传输方式参数
│   ├── camera_common.hpp            # camera_info 话题推导
│   ├── single_subscriber_publisher.hpp
│   ├── exception.hpp / loader_fwds.hpp
│   └── *.h                          # ROS 1 遗留头（与 .hpp 并存）
├── src/
│   ├── image_transport.cpp          # 全局 plugin loader + ImageTransport
│   ├── publisher.cpp / subscriber.cpp
│   ├── camera_publisher.cpp / camera_subscriber.cpp / camera_common.cpp
│   ├── single_subscriber_publisher.cpp
│   ├── manifest.cpp                   # raw 插件 PLUGINLIB 导出
│   ├── list_transports.cpp            # CLI 工具
│   └── republish.cpp                  # 传输格式转换节点
├── default_plugins.xml                # raw 插件描述
├── test/                              # 6 个 gtest
├── CMakeLists.txt
└── package.xml

构建产物：

2. 架构总览

设计核心：

• Publisher 端：一次 advertise() 加载 多个 PublisherPlugin，每个 transport 各建一个 ROS publisher。
• Subscriber 端：一次 subscribe() 只选 一个 SubscriberPlugin（由 TransportHints 或参数决定）。
• 按需发布：publish() 时只对 有订阅者 的 transport 调用插件，避免无谓压缩/编码开销。

3. 话题命名约定

3.1 图像 base topic

用户指定 base topic，例如 /camera/image。

3.2 各 transport 的实际话题

1
2
3
4

virtual std::string getTopicToAdvertise(const std::string & base_topic) const
{
  return base_topic + "/" + getTransportName();
}

3.3 camera_info 话题推导

1
2
3
4
5
6
7

std::string getCameraInfoTopic(const std::string & base_topic)
{
  // ...
  // 去掉最后一段，追加 /camera_info
  info_topic += "/camera_info";
  return info_topic;
}

示例：

注意：camera_info 始终走 普通 rclcpp Publisher/Subscriber（raw CameraInfo），不经 image_transport 插件。

4. 插件系统

4.1 插件注册

default_plugins.xml 声明 raw 插件：

1
2
3
4

<library path="image_transport_plugins">
  <class name="image_transport/raw_pub" type="image_transport::RawPublisher" .../>
  <class name="image_transport/raw_sub" type="image_transport::RawSubscriber" .../>
</library>

manifest.cpp 导出：

1
2

PLUGINLIB_EXPORT_CLASS(image_transport::RawPublisher, image_transport::PublisherPlugin)
PLUGINLIB_EXPORT_CLASS(image_transport::RawSubscriber, image_transport::SubscriberPlugin)

4.2 插件 lookup 命名

1
2
3
4

static std::string getLookupName(const std::string & transport_name)
{
  return "image_transport/" + transport_name + "_pub";
}

Subscriber 对应 _sub 后缀。对外暴露的 transport 名（如 raw、compressed）由 getTransportName() 返回。

4.3 全局 ClassLoader 单例

1
2
3
4
5
6
7
8
9
10

struct Impl
{
  PubLoaderPtr pub_loader_;
  SubLoaderPtr sub_loader_;
  Impl()
  : pub_loader_(std::make_shared<PubLoader>("image_transport", "image_transport::PublisherPlugin")),
    sub_loader_(std::make_shared<SubLoader>("image_transport", "image_transport::SubscriberPlugin"))
  {}
};
static Impl * kImpl = new Impl();

进程内共享 plugin loader；create_publisher / create_subscription 自由函数直接使用 kImpl。

4.4 外部插件包（本工作区未包含）

典型独立包（通过 package.xml export <image_transport plugin=...> 注册）：

可用 ros2 run image_transport list_transports 查看当前环境已声明/可加载的 transport。

5. 核心 API

5.1 两种使用风格

风格 A：自由函数（ROS 2 推荐）

1
2
3

auto pub = image_transport::create_publisher(node.get(), "camera/image");
auto sub = image_transport::create_subscription(
  node.get(), "camera/image", callback, "raw");

风格 B：ImageTransport 类（ROS 1 兼容风格）

1
2
3

image_transport::ImageTransport it(node);
auto pub = it.advertise("camera/image", 10);
auto sub = it.subscribe("camera/image", 10, callback);

ImageTransport 内部仍调用 create_publisher / create_subscription。

5.2 Publisher：多 transport 并行 advertise

1
2
3
4
5
6
7
8
9
10
11

Publisher::Publisher(...)
{
  // 1. expand_topic_or_service_name 解析 remap
  // 2. 读取 <topic>.enable_pub_plugins 参数（默认全部 declared transports）
  // 3. 对每个 allowlist 中的 transport 创建 PublisherPlugin 并 advertise
  for (const auto & transport_name : allowlist) {
    auto pub = loader->createUniqueInstance(transport_name + "_pub");
    pub->advertise(node, image_topic, custom_qos);
    impl_->publishers_.push_back(std::move(pub));
  }
}

Publisher 白名单参数：

• 参数名：<base_topic 相对路径，/ 换 .>.enable_pub_plugins
• 例：topic /camera/image → 参数 camera.image.enable_pub_plugins
• 默认：所有已声明的 pub 插件（通常含 raw + compressed 等）

按需 publish：

1
2
3
4
5

for (const auto & pub : impl_->publishers_) {
  if (pub->getNumSubscribers() > 0) {
    pub->publish(message);
  }
}

5.3 Subscriber：单 transport 加载

1
2
3
4

impl_->lookup_name_ = SubscriberPlugin::getLookupName(transport);
impl_->subscriber_ = loader->createSharedInstance(impl_->lookup_name_);
// ...
impl_->subscriber_->subscribe(node, base_topic, callback, custom_qos, options);

若用户误订阅 transport 专用话题（如 /camera/image/compressed），会 WARN 提示应订阅 base topic 并设置 image_transport 参数。

5.4 TransportHints：选择订阅 transport

1
2
3
4
5
6

TransportHints(const rclcpp::Node * node,
  const std::string & default_transport = "raw",
  const std::string & parameter_name = "image_transport")
{
  node->get_parameter_or<std::string>(parameter_name, transport_, default_transport);
}

常用 launch 参数：

1
2

# 订阅 compressed 而非 raw
image_transport: compressed

命令行：_image_transport:=compressed

6. Camera 封装

6.1 CameraPublisher

组合 image_transport::Publisher + 原生 CameraInfo publisher：

1
2

impl_->image_pub_ = image_transport::create_publisher(node, image_topic, custom_qos);
impl_->info_pub_ = node->create_publisher<sensor_msgs::msg::CameraInfo>(info_topic, qos);

publish(image, info) 分别发布两路消息（不做时间戳强制同步，调用方应保证 stamp 一致）。

6.2 CameraSubscriber

用 message_filters::TimeSynchronizer 同步 image + camera_info：

1
2
3
4
5

impl_->image_sub_.subscribe(node, image_topic, transport, custom_qos);
impl_->info_sub_.subscribe(node, info_topic, custom_qos);
impl_->sync_.connectInput(impl_->image_sub_, impl_->info_sub_);
impl_->sync_.registerCallback(callback);
// 每 1s 检查 image/info 是否严重不同步，WARN

• image 经 SubscriberFilter（支持 compressed 等 transport）
• camera_info 直接 message_filters::Subscriber<CameraInfo>

7. 插件开发模板

7.1 SimplePublisherPlugin<M>

子类只需实现：

1. getTransportName() — 返回 "compressed" 等
2. publish(const Image&, const PublishFn&) — 编码后调用 publish_fn(transport_msg)

基类负责创建 rclcpp::Publisher<M> 并管理生命周期。

7.2 SimpleSubscriberPlugin<M>

子类只需实现：

1. getTransportName()
2. internalCallback(const M&, const Callback& user_cb) — 解码后调用 user_cb(image)

7.3 Raw 插件（最简单参考）

Publisher 直接透传，话题即 base topic：

1
2
3
4
5
6
7
8

void publish(const sensor_msgs::msg::Image & message, const PublishFn & publish_fn) const
{
  publish_fn(message);
}
std::string getTopicToAdvertise(const std::string & base_topic) const
{
  return base_topic;
}

Subscriber 直接 user_cb(message)。

8. SubscriberFilter

将 Subscriber 包装为 message_filters::SimpleFilter<Image>，供 TimeSynchronizer、Chain 等使用：

1
2
3

sub_ = image_transport::create_subscription(
  node, base_topic,
  std::bind(&SubscriberFilter::cb, this, std::placeholders::_1), transport, custom_qos, options);

下游典型用法：RViz ImageTransportDisplay、DepthCloudDisplay 多话题同步。

9. 工具程序

9.1 list_transports

遍历 pub/sub 插件，打印 transport 名、所属包、加载状态（SUCCESS / LIB_LOAD_FAILURE / CREATE_FAILURE）。

9.2 republish

传输格式转换 relay 节点：

1
2
3
4
5

# 从 compressed 订阅，以所有可用 transport 重新发布
ros2 run image_transport republish compressed in:=/camera/image out:=/relay/image

# 指定输出 transport
ros2 run image_transport republish compressed in:=/in out:=/out compressed

10. 依赖关系

1
2
3
4
5

image_transport
  ├── rclcpp              # Node、Publisher、Subscription、参数
  ├── sensor_msgs         # Image、CameraInfo
  ├── pluginlib           # 插件加载（Publisher 链接 PRIVATE）
  └── message_filters     # CameraSubscriber / SubscriberFilter 同步

11. 测试

CMake 启用 6 个 gtest（ROS 2 已迁移）：

12. ROS 2 迁移遗留

13. 典型数据流

场景：驱动 raw 发布 + RViz compressed 订阅

Publisher 同时 advertise /camera/image 和 /camera/image/compressed；只有 RViz 订阅 compressed 时，驱动侧才执行 JPEG 编码。

14. 与 image_common 栈关系

• 上游：相机驱动应使用 CameraPublisher + camera_info_manager
• 下游：image_pipeline、rviz2 通过 image_transport 订阅
• 平行：camera_info_manager 管理标定；image_transport 管理图像传输格式

15. 设计特点与局限

16. 推荐阅读顺序

1. image_transport.hpp — 公共 API 全貌
2. publisher.cpp + subscriber.cpp — 插件加载与 publish 逻辑
3. raw_publisher.hpp / raw_subscriber.hpp — 最小插件示例
4. simple_*_plugin.hpp — 编写 compressed 等插件的模板
5. camera_subscriber.cpp — message_filters 同步模式
6. republish.cpp — 理解 transport 转换
7. 外部 compressed_image_transport — 真实压缩插件实现

17. 小结

image_transport 是 ROS 图像通信的 传输抽象框架：Publisher 通过 pluginlib 同时 advertise 多种 transport 子话题，Subscriber 按参数选择一种并解码为 sensor_msgs/Image；内置 raw 插件，压缩/视频类 transport 由独立包扩展。CameraPublisher/Subscriber 在其上封装 image + camera_info 双话题约定，是相机驱动、image_pipeline 和 RViz 的共同基础设施。

如需，我可以把本文写入 ros2doc/ros-perception/image_common/image_transport源码详细分析.md，或继续分析 compressed_image_transport 的 JPEG 编解码实现。

polled_camera 源码详细分析

工作区路径：/home/cp/work2/ros2Learn/ros2_humble/src/ros-perception/image_common/polled_camera
版本：1.11.13，许可证 BSD。

polled_camera 提供 按需拉取（polled）相机图像 的协议与服务定义：客户端通过 service 请求拍一张图，驱动端捕获后把 Image + CameraInfo 发布到指定 namespace 的 latched 话题，客户端再像普通相机流一样订阅。与连续发布的 streaming 驱动形成互补，适用于工业相机、PR2 Prosilica 等 触发式采集 场景。

1. 重要前提：仍为 ROS 1 遗留包

package.xml 自述 API 仍在开发中、仅供内部使用（for internal use as the API is still under development）。

2. 仓库结构

1
2
3
4
5
6
7
8
9
10
11
12

polled_camera/
├── include/polled_camera/
│   └── publication_server.h     # 服务端辅助类 + advertise() 工厂
├── src/
│   ├── publication_server.cpp   # 核心实现（~160 行）
│   └── poller.cpp               # 示例 CLI 客户端
├── srv/
│   └── GetPolledImage.srv       # 拉取图像服务定义
├── CMakeLists.txt               # catkin 构建
├── package.xml
├── mainpage.dox                 # Doxygen 协议说明
└── CHANGELOG.rst

构建产物：

3. 在 image_common 栈中的位置

PR2 URDF 中仍有遗留引用：/prosilica/request_image（见 pr2_desc.urdf 的 pollServiceName）。

4. 通信协议（mainpage.dox）

1. 驱动 advertise 服务：<camera>/request_image
2. 客户端调用 service，在 request 中指定 response_namespace
3. 驱动捕获图像，在 response 中返回 stamp
4. 驱动将 Image、CameraInfo latched 发布到：
   • <response_namespace>/image_raw
   • <response_namespace>/camera_info（由 image_transport 约定推导）
5. 客户端订阅上述话题，用 rsp.stamp 与 Image.header.stamp 对齐

目前没有官方 Client 类，客户端需自行：ServiceClient + image_transport::CameraSubscriber。

5. 服务定义：GetPolledImage.srv

Request：

Response：

1
2
3
4
5
6
7
8
9

string response_namespace
duration timeout
uint32 binning_x
uint32 binning_y
sensor_msgs/RegionOfInterest roi
---
bool success
string status_message
time stamp

6. 核心类：PublicationServer

6.1 公共 API

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

class PublicationServer
{
public:
  typedef boost::function<void (GetPolledImage::Request&,
                                GetPolledImage::Response&,
                                sensor_msgs::Image&,
                                sensor_msgs::CameraInfo&)> DriverCallback;

  void shutdown();
  std::string getService() const;
  operator void*() const;
};

PublicationServer advertise(ros::NodeHandle& nh, const std::string& service,
                            const PublicationServer::DriverCallback& cb, ...);

驱动侧只需实现 DriverCallback：填充 image/info，设置 rsp.success；失败时填 rsp.status_message。

6.2 内部结构 Impl

1
2
3
4
5
6
7
8

class PublicationServer::Impl
{
  ros::ServiceServer srv_server_;
  DriverCallback driver_cb_;
  image_transport::ImageTransport it_;
  std::map<std::string, image_transport::CameraPublisher> client_map_;
  // ...
};

6.3 服务回调流程

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

bool requestCallback(GetPolledImage::Request& req, GetPolledImage::Response& rsp)
{
  std::string image_topic = req.response_namespace + "/image_raw";
  image_transport::CameraPublisher& pub = client_map_[image_topic];
  if (!pub) {
    pub = it_.advertiseCamera(image_topic, 1, ..., true /*latch*/);
  }

  req.binning_x = std::max(req.binning_x, (uint32_t)1);
  req.binning_y = std::max(req.binning_y, (uint32_t)1);

  sensor_msgs::Image image;
  sensor_msgs::CameraInfo info;
  driver_cb_(req, rsp, image, info);

  if (rsp.success) {
    assert(image.header.stamp == info.header.stamp);
    rsp.stamp = image.header.stamp;
    pub.publish(image, info);
  }
  return true;  // ROS service 层始终 true，业务成败看 rsp.success
}

要点：

• 懒创建 Publisher：每个不同的 response_namespace/image_raw 对应一个 CameraPublisher
• Latch = true：新订阅者能立即收到最后一帧（适合 polled 单次响应）
• binning 归一化：0 自动改为 1，避免驱动端除零
• 时间戳约束：成功时 image.header.stamp == info.header.stamp，并写回 rsp.stamp

6.4 订阅者断开自动清理

1
2
3
4
5
6

void disconnectCallback(const image_transport::SingleSubscriberPublisher& ssp)
{
  if (ssp.getNumSubscribers() == 0) {
    client_map_.erase(ssp.getTopic());
  }
}

当某 response 话题订阅数降为 0 时，从 client_map_ 移除对应 CameraPublisher，释放 advertise 资源。该机制依赖 ROS 1 image_transport 的 SubscriberStatusCallback（ROS 2 版 image_transport 尚未实现此回调）。

6.5 生命周期保护

析构时若创建后 < 1ms 即销毁，会 WARN：

1
2

if (ros::WallTime::now().toSec() - constructed_ < 0.001)
  ROS_WARN("PublicationServer destroyed immediately after creation. Did you forget to store the handle?");

PublicationServer 必须 保存句柄（类似 ros::Publisher 的 pimpl 引用计数模式）。

7. 示例客户端：poller

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

int main(int argc, char** argv)
{
  ros::init(argc, argv, "poller", ros::init_options::AnonymousName);
  // Usage: poller <Hz> camera:=<namespace> output:=<namespace>
  double hz = boost::lexical_cast<double>(argv[1]);

  std::string service_name = nh.resolveName("camera") + "/request_image";
  ros::ServiceClient client = nh.serviceClient<GetPolledImage>(service_name);

  req.response_namespace = nh.resolveName("output");

  ros::Rate loop_rate(hz);
  while (nh.ok()) {
    if (client.call(req, rsp))
      std::cout << "Timestamp: " << rsp.stamp << std::endl;
    else {
      ROS_ERROR("Service call failed");
      client.waitForExistence();
    }
  }
}

用法示例（ROS 1）：

1
2

rosrun polled_camera poller 1.0 camera:=/my_camera output:=/my_output
# 调用 /my_camera/request_image，响应发布到 /my_output/image_raw

注意：poller 只打印 stamp，并不订阅图像话题；完整客户端需额外 subscribeCamera("output/image_raw", ...)。

8. 典型集成示例

8.1 驱动端（PublicationServer）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

void captureCallback(GetPolledImage::Request& req,
                     GetPolledImage::Response& rsp,
                     sensor_msgs::Image& image,
                     sensor_msgs::CameraInfo& info)
{
  // 按 req.timeout / roi / binning 配置硬件
  if (!hardware.capture(image, info)) {
    rsp.success = false;
    rsp.status_message = "capture timeout";
    return;
  }
  rsp.success = true;
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "my_polled_camera");
  ros::NodeHandle nh;
  polled_camera::PublicationServer server =
    polled_camera::advertise(nh, "request_image", captureCallback);
  ros::spin();
}

8.2 客户端（service + subscribe）

1
2
3
4
5
6
7
8
9
10

// 1. 先订阅
image_transport::CameraSubscriber sub =
  it.subscribeCamera("output_ns/image_raw", 1, imageCallback);

// 2. 再请求拍图
GetPolledImage srv;
srv.request.response_namespace = "output_ns";
if (client.call(srv) && srv.response.success) {
  // 用 srv.response.stamp 匹配 callback 中的 frame
}

推荐顺序：先 subscribe 再 call，避免 latched 消息在订阅建立前丢失（虽 latch 可缓解，但时序更清晰）。

9. 依赖关系

1
2
3
4
5
6

polled_camera (ROS 1 catkin)
  ├── roscpp
  ├── sensor_msgs        # Image, CameraInfo, RegionOfInterest
  ├── std_msgs           # 消息生成依赖
  ├── message_generation / message_runtime
  └── image_transport    # CameraPublisher + latch + disconnect 回调

与 camera_info_manager 无直接依赖；驱动可在 DriverCallback 内自行填充 CameraInfo。

10. 测试与文档

11. 设计特点与局限

12. 与 image_common 其他包对比

现代 ROS 2 相机驱动多采用 连续 publish + trigger service（自定义） 或 action 替代此包；polled_camera 更多是历史兼容（PR2 工业相机生态）。

13. 推荐阅读顺序

1. srv/GetPolledImage.srv — 请求/响应字段语义
2. mainpage.dox — 端到端协议
3. publication_server.cpp 的 requestCallback — 核心状态机
4. publication_server.h 的 DriverCallback 文档 — 驱动实现契约
5. poller.cpp — 最小客户端调用
6. image_transport CameraPublisher — latched 发布与 camera_info 话题规则

14. 小结

polled_camera 是一个 体量很小但语义清晰 的 ROS 1 包：用 GetPolledImage service 触发采集，用 PublicationServer 管理 latched 的 image_transport::CameraPublisher，按 response_namespace 动态创建/销毁话题。它填补了 非连续流相机 的通信模式，但在 ROS 2 Humble 中 尚未端口，属于 image_common 中的 遗留源码；若要在 ROS 2 实现类似能力，需自行用 rclcpp::Service + image_transport::CameraPublisher（或 action）重写，并处理 ROS 2 下 latch/disconnect 回调缺失问题。

如需，我可以把本文写入 ros2doc/ros-perception/image_common/polled_camera源码详细分析.md，或给出一份 ROS 2 版 PublicationServer 移植 sketch。

laser_geometry 源码详细分析

工作区路径：/home/cp/work2/ros2Learn/ros2_humble/src/ros-perception/laser_geometry
版本：2.4.1，许可证 BSD。

laser_geometry 是 ROS 感知栈中的 2D 激光几何转换库：将 sensor_msgs/LaserScan 投影为 sensor_msgs/PointCloud2，并在 C++ 中支持 运动畸变补偿（扫描期间机器人/激光头移动时的 skew 校正）。提供 C++ 与 Python 两套 API，是 RViz LaserScan 显示、costmap 等组件的基础工具库。

1. 仓库结构

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

laser_geometry/
├── include/laser_geometry/
│   ├── laser_geometry.hpp       # C++ 公共 API：LaserProjection
│   └── visibility_control.hpp
├── src/
│   ├── laser_geometry.cpp       # C++ 实现（~467 行）
│   └── laser_geometry/
│       ├── laser_geometry.py    # Python 实现（仅 projectLaser）
│       └── __init__.py
├── test/
│   ├── projection_test.cpp      # C++ gtest
│   └── projection_test.py       # Python pytest
├── CMakeLists.txt
├── package.xml
└── CHANGELOG.rst

构建产物：

2. 在感知栈中的位置

3. 核心类：LaserProjection

单一类，无 ROS 节点；纯几何/TF 工具。

3.1 两个公共方法（C++）

1
2
3
4
5
6
7
8
9
10
11
12
13

void projectLaser(
  const sensor_msgs::msg::LaserScan & scan_in,
  sensor_msgs::msg::PointCloud2 & cloud_out,
  double range_cutoff = -1.0,
  int channel_options = channel_option::Default)

void transformLaserScanToPointCloud(
  const std::string & target_frame,
  const sensor_msgs::msg::LaserScan & scan_in,
  sensor_msgs::msg::PointCloud2 & cloud_out,
  tf2::BufferCore & tf,
  double range_cutoff = -1.0,
  int channel_options = channel_option::Default)

3.2 可选输出通道（ChannelOption）

位标志 OR 组合：

ROS 2 已 移除 PointCloud1 支持（见 header TODO / GitHub #29）。

4. projectLaser 算法

4.1 极坐标投影

对每个 beam i：

[
\theta_i = \text{angle_min} + i \cdot \text{angle_increment}
]
[
x_i = r_i \cos\theta_i,\quad y_i = r_i \sin\theta_i,\quad z_i = 0
]

2D 激光在 激光平面内，Z 恒为 0。

4.2 cos/sin 缓存

1
2
3
4
5
6
7
8
9
10

if (co_sine_map_.rows() != static_cast<int>(n_pts) || angle_min_ != scan_in.angle_min ||
  angle_max_ != scan_in.angle_max)
{
  co_sine_map_ = Eigen::ArrayXXd(n_pts, 2);
  for (size_t i = 0; i < n_pts; ++i) {
    co_sine_map_(i, 0) = cos(angle_min + i * angle_increment);
    co_sine_map_(i, 1) = sin(angle_min + i * angle_increment);
  }
}
output = ranges * co_sine_map_;  // Eigen 逐元素乘法

当 beam 数量或角度范围不变 时复用 co_sine_map_，避免重复三角函数计算。

4.3 有效点过滤

1
2
3
4
5
6
7
8
9
10

if (range_cutoff < 0) range_cutoff = scan_in.range_max;

for (size_t i = 0; i < n_pts; ++i) {
  const float range = scan_in.ranges[i];
  if (range < range_cutoff && range >= scan_in.range_min) {
    // 写入 x,y,z + 可选通道
    ++count;
  }
}
cloud_out.width = count;  // 压缩为有效点

• 无效测距（< range_min 或 >= range_cutoff）直接丢弃，不保留 NaN 占位
• 旧版保留 NaN 点的逻辑已注释掉（源码 TODO 质疑其合理性）
• 若需 index 与原 ranges 一一对应，应预先过滤 scan 或启用 Index 通道

4.4 PointCloud2 布局

固定字段顺序：x, y, z（各 FLOAT32，offset 0/4/8），再按 flags 追加可选字段；is_dense = false（因过滤后 width 可能小于原 ranges 数）。

5. transformLaserScanToPointCloud：运动畸变补偿

5.1 流程

5.2 时间端点

1
2
3
4
5
6

rclcpp::Time start_time(scan_in.header.stamp);
rclcpp::Time end_time = start_time + Duration(
  (ranges.size() - 1) * time_increment);

start_transform = tf.lookupTransform(target_frame, scan_in.header.frame_id, t_start);
end_transform   = tf.lookupTransform(target_frame, scan_in.header.frame_id, t_end);

5.3 逐点插值变换

1
2
3
4
5
6

tfScalar ratio = pt_index * ranges_norm;  // ranges_norm = 1/(N-1)

v.setInterpolate3(origin_start, origin_end, ratio);   // 平移线性插值
cur_transform.setRotation(slerp(quat_start, quat_end, ratio));  // 旋转 slerp

point_out = cur_transform * point_in;

假设：扫描期间 匀速运动（constant velocity），各 beam 按 index 均匀分布在 [t_start, t_end]。

target_frame 应为 固定世界系（如 map/odom），否则补偿无意义。

5.4 Index 通道的临时使用

内部强制 channel_options |= Index，变换完成后若用户未请求 Index，则复制点云并 剥离 index 字段（调整 offset/point_step）。

6. Python 实现

1
2
3
4

from laser_geometry import LaserProjection
projector = LaserProjection()
cloud = projector.projectLaser(scan, range_cutoff=-1.0,
    channel_options=LaserProjection.ChannelOption.DEFAULT)

Python 用 sensor_msgs_py.point_cloud2.create_cloud 构建消息，逻辑与 C++ 对齐。

7. 下游集成：RViz LaserScanDisplay

1
2
3
4
5
6
7

projector_->transformLaserScanToPointCloud(
  fixed_frame_.toStdString(),
  *scan,
  *cloud,
  *tf_wrapper->getBuffer(),
  -1,
  laser_geometry::channel_option::Intensity);

RViz 将 LaserScan 转为 fixed frame 的 PointCloud2 再渲染；并根据 scan 时长动态调整 TF filter tolerance。

8. 依赖关系

1
2
3
4
5
6

laser_geometry
  ├── Eigen3              # C++ 矩阵运算
  ├── rclcpp              # Time / Duration
  ├── sensor_msgs         # LaserScan, PointCloud2
  ├── tf2                 # BufferCore, Transform, slerp
  └── (Python) rclpy, numpy, sensor_msgs_py

9. 测试

10. 设计特点与局限

11. 使用示例

11.1 静态投影（C++）

1
2
3
4
5
6

laser_geometry::LaserProjection projector;
sensor_msgs::msg::PointCloud2 cloud;
projector.projectLaser(scan, cloud, -1.0,
  laser_geometry::channel_option::Intensity |
  laser_geometry::channel_option::Index);
// cloud.header.frame_id == scan.header.frame_id

11.2 变换到 fixed frame（C++）

1
2
3

projector.transformLaserScanToPointCloud(
  "odom", scan, cloud, tf_buffer);
// cloud.header.frame_id == "odom"

11.3 Python

1
2

from laser_geometry import LaserProjection
cloud = LaserProjection().projectLaser(scan)

12. 推荐阅读顺序

1. laser_geometry.hpp — API 与 ChannelOption 文档
2. projectLaser_ — 投影 + 过滤 + 字段布局
3. transformLaserScanToPointCloud_（带 quat 版本） — 运动补偿核心
4. transformLaserScanToPointCloud_（带 tf 版本） — TF 查表入口
5. laser_geometry.py — Python 子集对照
6. rviz laser_scan_display.cpp — 真实集成方式

13. 小结

laser_geometry 解决两个经典问题：LaserScan → PointCloud2 的极坐标投影，以及 扫描期间运动造成的几何畸变（通过 scan 起止 TF + 逐 beam 插值）。C++ 版功能完整；Python 版仅 projectLaser。在 Humble 中它是 已完整 ROS 2 端口 的感知基础库，RViz 激光显示是其最主要消费者。

如需，我可以把本文写入 ros2doc/ros-perception/laser_geometry源码详细分析.md，或继续分析 Nav2 costmap 如何直接使用 LaserScan（不经 laser_geometry） 的对比。

keyboard_handler 源码详细分析

工作区路径：/home/cp/work2/ros2Learn/ros2_humble/src/ros-tooling/keyboard_handler
版本：0.0.5，许可证 Apache 2.0。

keyboard_handler 是 跨平台终端键盘输入库：通过回调订阅按键组合（含 Ctrl/Alt/Shift 修饰键），在后台线程轮询 stdin，供 CLI 工具在运行时响应快捷键。主要下游是 rosbag2 的 play / record 交互控制；本身 不依赖 rclcpp，是纯 C++ 库 + ament 打包。

1. 仓库结构

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

keyboard_handler/
├── keyboard_handler/
│   ├── include/keyboard_handler/
│   │   ├── keyboard_handler.hpp          # 平台别名入口
│   │   ├── keyboard_handler_base.hpp     # 公共 API + KeyCode 枚举
│   │   ├── keyboard_handler_unix_impl.hpp
│   │   ├── keyboard_handler_windows_impl.hpp
│   │   └── visibility_control.hpp
│   ├── src/
│   │   ├── keyboard_handler_base.cpp     # 回调注册/工具函数
│   │   ├── keyboard_handler_unix_impl.cpp
│   │   ├── keyboard_handler_windows_impl.cpp
│   │   ├── default_unix_key_map.cpp      # xterm 转义序列映射
│   │   └── default_windows_key_map.cpp   # _getch 码映射
│   ├── test/
│   │   ├── keyboard_handler_unix_tests.cpp
│   │   ├── keyboard_handler_windows_tests.cpp
│   │   ├── fake_player.hpp / fake_recorder.hpp
│   └── CMakeLists.txt / package.xml
├── docs/design/README.md                 # 设计文档（较完整）
└── README.md

构建产物： 单一共享库 libkeyboard_handler.so（Windows 为 DLL）。

2. 架构总览

3. 公共 API

3.1 平台入口

1
2
3
4
5
6
7

#ifdef _WIN32
#include "keyboard_handler_windows_impl.hpp"
using KeyboardHandler = KeyboardHandlerWindowsImpl;
#else
#include "keyboard_handler_unix_impl.hpp"
using KeyboardHandler = KeyboardHandlerUnixImpl;
#endif

用户只需 #include "keyboard_handler/keyboard_handler.hpp"。

3.2 核心方法

1
2
3
4
5
6
7
8

callback_handle_t add_key_press_callback(...)
{
  if (callback == nullptr || !is_init_succeed_) {
    return invalid_handle;
  }
  callbacks_.emplace(KeyAndModifiers{key_code, key_modifiers}, ...);
  return new_handle;
}

同一按键可注册多个回调（unordered_multimap + equal_range 派发）。

3.3 KeyCode 与 KeyModifiers

• KeyCode：100+ 枚举值（字母、数字、F1–F12、方向键、Home/End 等）
• KeyModifiers：位掩码 SHIFT | ALT | CTRL
• 自定义运算符：operator| 组合修饰键，operator&& 检测位

工具函数：

• enum_key_code_to_str() / enum_str_to_key_code()
• enum_key_modifiers_to_str()

4. Unix 实现（POSIX）

4.1 初始化流程

1
2
3
4
5
6
7
8
9
10
11

if (!isatty_fn(stdin_fd_)) {
  std::cerr << "stdin is not a terminal device. Keyboard handling disabled.";
  return;  // is_init_succeed_ 保持 false
}
tcgetattr → 保存 old_term_settings_
可选安装 SIGINT handler
new_term_settings.c_lflag &= ~(ICANON | ECHO);  // 非规范模式、关闭回显
new_term_settings.c_cc[VMIN] = 0;
new_term_settings.c_cc[VTIME] = 1;  // 100ms 超时 read
tcsetattr → is_init_succeed_ = true
启动 key_handler_thread_

要点：

• 必须真实 TTY：stdin 重定向到文件/管道时禁用键盘（不抛异常，便于 gtest）
• 构造时切非规范模式，析构时恢复 canonical
• Unix 可选 KeyboardHandler(false) 不安装 SIGINT 处理器（rosbag2 使用此模式，避免与进程信号冲突）

4.2 读键线程

1
2
3
4
5
6
7
8

do {
  read_bytes = read_fn(stdin_fd_, buff, BUFF_LEN);
  if (read_bytes > 0) {
    auto [pressed_key_code, key_modifiers] = parse_input(buff, read_bytes);
    lock → equal_range → 调用所有匹配 callback
  }
} while (!exit_);
restore_buffer_mode_for_stdin();

4.3 parse_input 解析逻辑

映射表在 default_unix_key_map.cpp，基于 xterm 控制序列（如 \x1b[A = 上箭头）。注释说明不同终端模拟器序列可能不同。

4.4 SIGINT (Ctrl+C) 处理

1
2
3
4
5

void on_signal(int signal_number) {
  restore_buffer_mode_for_stdin();
  if (old_sigint_handler == SIG_DFL) _exit(...);
  else { exit_ = true; 链式调用旧 handler; }
}

Ctrl+C 不会通过 callback 传给客户端（设计限制）；仅保证终端模式恢复。

5. Windows 实现

5.1 读键线程

1
2
3
4
5
6
7
8
9
10
11
12
13

while (!exit_) {
  if (kbhit_fn()) {
    ch = getch_fn();
    if (GetAsyncKeyState(VK_MENU)) key_modifiers |= ALT;
    if (ch == 0 || ch == 0xE0) {  // 功能键/方向键前缀
      ch = getch_fn();
      win_key_code.second = ch;
    }
    auto [key, mods] = win_key_code_to_enums(win_key_code);
    派发 callbacks
    sleep 100ms;  // 让出 CPU
  }
}

• 使用 _kbhit + _getch（DOS 传统 API）
• 功能键需 两次 getch（第一次 0 或 0xE0）
• ALT 通过 GetAsyncKeyState(VK_MENU) 检测

5.2 win_key_code_to_enums

类似 Unix：处理 CTRL+F1..F12、SHIFT+F1..F12、大写字母→SHIFT、0–26→CTRL 等，再查 default_windows_key_map.cpp 中的 {first, second} 对。

Windows 无 SIGINT/termios 处理；构造函数也无 install_signal_handler 参数。

6. 键位映射表

Unix（xterm 序列示例）

1
2
3

static constexpr char CURSOR_UP[]   = {27, 91, 65, '\0'};  // ESC [ A
static constexpr char F1[]          = {27, 79, 80, '\0'};   // ESC O P
// ...

SHIFT+F1..F12 序列在注释中列出但未启用（修饰键检测局限）。

Windows（_getch 码示例）

1
2
3
4

{KeyCode::CURSOR_UP,   {0xE0, 72}},
{KeyCode::F1,          {0,    59}},
{KeyCode::SPACE,       {32,   NOT_A_KEY}},
// ...

7. 回调生命周期管理

设计文档给出两种模式：

1. 显式删除：析构时 delete_key_press_callback(handle)（rosbag2 Recorder 采用）
2. weak_ptr lambda：客户端先于 handler 销毁时避免悬空指针

测试中的 FakePlayer / FakeRecorder 演示 enable_shared_from_this + weak_ptr 模式。

8. 下游集成：rosbag2

8.1 Player 默认快捷键

8.2 Recorder

• 空格：toggle_paused()
• 析构时删除 callback handle

8.3 Unix 特殊构造

1
2
3
4
5

#ifndef _WIN32
    std::make_shared<KeyboardHandler>(false),  // 不装 SIGINT handler
#else
    std::shared_ptr<KeyboardHandler>(new KeyboardHandler()),
#endif

测试注入 MockKeyboardHandler 模拟按键，无需真实终端。

9. 依赖关系

1
2
3
4
5

keyboard_handler
  └── (无 rclcpp / 无 ROS 消息依赖)
      仅 ament_cmake 打包
      平台：termios/read/signal (Unix)
            conio.h/Windows.h (Windows)

纯工具库，ROS 集成体现在 rosbag2 等消费者侧。

10. 测试

测试覆盖：按键解析、callback 注册/删除、对象先于 handler 销毁、stdin 非 TTY 安全模式等。

11. 已知局限（设计文档 + 头文件注释）

12. 使用示例

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

#include "keyboard_handler/keyboard_handler.hpp"

void on_key(KeyboardHandler::KeyCode code,
            KeyboardHandler::KeyModifiers mods) {
  if (code == KeyboardHandler::KeyCode::SPACE) { /* pause */ }
  if (code == KeyboardHandler::KeyCode::A &&
      (mods && KeyboardHandler::KeyModifiers::CTRL)) { /* Ctrl+A */ }
}

int main() {
  KeyboardHandler handler;  // Unix: 改 termios，启后台线程
  auto h = handler.add_key_press_callback(
    on_key, KeyboardHandler::KeyCode::SPACE);
  // ... 主逻辑 ...
  handler.delete_key_press_callback(h);
  return 0;  // 析构恢复终端
}

13. 设计特点

14. 推荐阅读顺序

1. docs/design/README.md — 设计目标、局限、生命周期模式
2. keyboard_handler_base.hpp — KeyCode/KeyModifiers API
3. keyboard_handler_unix_impl.cpp — termios + parse_input + 线程
4. keyboard_handler_windows_impl.cpp — kbhit/getch 路径
5. default_*_key_map.cpp — 平台码表
6. rosbag2 player.cpp / recorder.cpp — 真实集成
7. keyboard_handler_unix_tests.cpp — mock 测试模式

15. 小结

keyboard_handler 是 ros-tooling 提供的 轻量跨平台终端键盘库：基类管理回调表，Unix 用 termios 非规范 read，Windows 用 _kbhit/_getch 轮询，通过静态映射表统一为 KeyCode + KeyModifiers。它不绑定 ROS 中间件，但被 rosbag2 play/record 用作运行时交互控制的核心依赖。使用时需注意 修饰键检测局限、Ctrl+C 不进入 callback、以及 stdin 必须连接真实终端 等约束。

如需，我可以把本文写入 ros2doc/ros-tooling/keyboard_handler源码详细分析.md，或继续分析 rosbag2 Player 如何绑定全部快捷键。

libstatistics_collector 源码详细分析

工作区路径：/home/cp/work2/ros2Learn/ros2_humble/src/ros-tooling/libstatistics_collector
版本：1.3.4，许可证 Apache 2.0，Quality Level 1。

libstatistics_collector 是 ROS 2 轻量级统计聚合库：提供在线滑动窗口统计（均值/最大/最小/标准差）、通用 Collector 框架、话题级 message_age / message_period 采集器，以及将结果封装为 statistics_msgs/MetricsMessage 的工具。主要消费者是 rclcpp 的 Topic Statistics 功能。

1. 仓库结构

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

libstatistics_collector/
├── include/libstatistics_collector/
│   ├── collector/
│   │   ├── collector.hpp                    # 抽象采集器基类
│   │   ├── metric_details_interface.hpp     # 指标名/单位接口
│   │   └── generate_statistics_message.hpp  # → MetricsMessage
│   ├── moving_average_statistics/
│   │   ├── moving_average.hpp               # Welford 在线统计
│   │   └── types.hpp                        # StatisticData
│   ├── topic_statistics_collector/
│   │   ├── topic_statistics_collector.hpp   # 话题采集器模板接口
│   │   ├── received_message_age.hpp         # 消息年龄
│   │   ├── received_message_period.hpp      # 消息周期
│   │   └── constants.hpp                    # 指标名/参数名常量
│   └── visibility_control.hpp
├── src/libstatistics_collector/
│   ├── collector/collector.cpp
│   ├── collector/generate_statistics_message.cpp
│   └── moving_average_statistics/{moving_average,types}.cpp
├── test/                                    # gtest + benchmark
├── CMakeLists.txt / package.xml
├── README.md / QUALITY_DECLARATION.md
└── Doxyfile

构建产物： 单一共享库 liblibstatistics_collector.so（话题采集器为 header-only 模板）。

2. 在 ROS 2 栈中的位置

3. 核心模块一：MovingAverageStatistics

3.1 算法

使用 Welford 在线算法 计算总体标准差，无需存储全部样本：

1
2
3
4
5
6
7
8
9
10
11

void MovingAverageStatistics::AddMeasurement(const double item)
{
  if (!std::isnan(item)) {
    count_++;
    const double previous_average = average_;
    average_ = previous_average + (item - previous_average) / count_;
    min_ = std::min(min_, item);
    max_ = std::max(max_, item);
    sum_of_square_diff_from_mean_ += (item - previous_average) * (item - average_);
  }
}

标准差：sqrt(sum_of_square_diff_from_mean_ / count_)（总体标准差，非样本标准差）。

3.2 StatisticData

1
2
3
4
5
6
7

struct StatisticData {
  double average = NaN;
  double min = NaN;
  double max = NaN;
  double standard_deviation = NaN;
  uint64_t sample_count = 0;
};

无样本时返回 NaN；NaN 输入会被 丢弃。

3.3 “Moving Average” 含义

名称易误解：并非固定长度的滑动窗口，而是 当前采集窗口 内的在线统计；窗口结束需调用 Reset() / ClearCurrentMeasurements() 清零开始新窗口。

4. 核心模块二：Collector

4.1 类层次

1
2
3
4
5
6
7
8
9

MetricDetailsInterface
  ├── GetMetricName()
  └── GetMetricUnit()
        ↑
Collector (abstract)
  ├── AcceptData(double)
  ├── GetStatisticsResults()
  ├── Start() / Stop()
  └── SetupStart() / SetupStop()  [纯虚]

4.2 生命周期

1
2
3
4
5
6
7
8

bool Collector::Start() {
  lock → if already started return false
  started_ = true → SetupStart()
}
bool Collector::Stop() {
  lock → started_ = false → SetupStop()
  ClearCurrentMeasurements()  // 在锁外调用 Reset
}

4.3 线程安全

• Collector 的 mutex_ 保护 started_ 与 Start/Stop
• MovingAverageStatistics 自带 mutex_ 保护统计数据
• AcceptData 不加 Collector 锁，仅依赖内部聚合器锁

5. 核心模块三：GenerateStatisticMessage

将 StatisticData 转为 ROS 消息：

1
2
3
4
5
6

MetricsMessage GenerateStatisticMessage(node_name, metric_name, unit,
  window_start, window_stop, data)
{
  // 填充 5 个 StatisticDataPoint:
  // AVERAGE, MAXIMUM, MINIMUM, SAMPLE_COUNT, STDDEV
}

6. 核心模块四：Topic Statistics Collectors

6.1 接口

1
2
3
4
5

template<typename T>
class TopicStatisticsCollector : public collector::Collector {
  virtual void OnMessageReceived(const T & msg,
    const rcl_time_point_value_t now_nanoseconds) = 0;
};

6.2 ReceivedMessageAgeCollector

度量： 接收时刻 − 消息 header.stamp（毫秒）

1
2
3
4
5
6
7

void OnMessageReceived(const T & msg, rcl_time_point_value_t now_ns) {
  auto [has_header, stamp_ns] = TimeStamp<T>::value(msg);
  if (has_header && stamp_ns && now_ns) {
    age_ms = (now_ns - stamp_ns) in milliseconds;
    AcceptData(age_ms);
  }
}

编译期检测 header：

• HasHeader<M>：是否存在 M.header.stamp 且类型为 builtin_interfaces/msg/Time
• 无 header 的消息：静默跳过（不记录）

6.3 ReceivedMessagePeriodCollector

度量： 相邻两次 OnMessageReceived 调用的时间间隔（毫秒）

1
2
3
4
5
6
7
8
9
10

void OnMessageReceived(..., now_ns) {
  lock(mutex_);
  if (time_last_ == kUninitializedTime)
    time_last_ = now_ns;  // 首条消息只初始化，不产生样本
  else {
    period_ms = (now_ns - time_last_) in ms;
    time_last_ = now_ns;
    AcceptData(period_ms);
  }
}

6.4 常量

1
2
3
4
5

kMsgAgeStatName = "message_age"
kMsgPeriodStatName = "message_period"
kMillisecondUnitName = "ms"
kCollectStatsTopicNameParam = "collect_topic_name"
kPublishStatsTopicNameParam = "publish_topic_name"

7. rclcpp 集成（下游）

rclcpp::topic_statistics::SubscriptionTopicStatistics 封装完整工作流：

1
2
3
4
5
6
7
8
9
10
11
12
13

bring_up():
  ReceivedMessageAge → Start()
  ReceivedMessagePeriod → Start()

handle_message(msg, now):
  for collector: collector->OnMessageReceived(msg, now.ns())

publish_message_and_reset_measurements():  // 定时器触发，默认 1s
  for collector:
    stats = GetStatisticsResults()
    ClearCurrentMeasurements()
    publish GenerateStatisticMessage(...)
  window_start = window_end

默认发布话题：/statistics
启用方式：创建 subscription 时 SubscriptionOptions.enable_topic_statistics = true（及节点参数配置）。

8. 依赖关系

1
2
3
4
5

libstatistics_collector
  ├── rcl              # rcl_time_point_value_t, RCL_S_TO_NS
  ├── rcpputils        # thread_safety_annotations
  ├── statistics_msgs # MetricsMessage, StatisticDataType
  └── builtin_interfaces  # Time（header 检测）

不依赖 rclcpp——保持库层轻量，由 rclcpp 在上层集成。

9. 测试

测试用 libstatistics_collector_test_msgs（DummyMessage / DummyCustomHeaderMessage）验证 header 检测。

10. 设计特点与局限

11. 扩展自定义 Collector 示例

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

class MyMetricCollector : public libstatistics_collector::collector::Collector {
protected:
  bool SetupStart() override { return true; }
  bool SetupStop() override { return true; }
public:
  std::string GetMetricName() const override { return "my_metric"; }
  std::string GetMetricUnit() const override { return "ms"; }

  void OnEvent(double value_ms) {
    AcceptData(value_ms);
  }
};

// 使用
MyMetricCollector c;
c.Start();
c.OnEvent(12.3);
auto stats = c.GetStatisticsResults();
auto msg = GenerateStatisticMessage("my_node", c.GetMetricName(),
  c.GetMetricUnit(), t0, t1, stats);

话题级可继承 TopicStatisticsCollector<T> 并实现 OnMessageReceived。

12. 推荐阅读顺序

1. moving_average.hpp + moving_average.cpp — 核心算法
2. collector.hpp + collector.cpp — 生命周期模式
3. generate_statistics_message.cpp — ROS 消息映射
4. received_message_age.hpp / received_message_period.hpp — 话题 metric
5. rclcpp/.../subscription_topic_statistics.hpp — 端到端集成
6. test_moving_average_statistics.cpp — 算法边界条件

13. 小结

libstatistics_collector 是 ROS 2 Topic Statistics 的底层数学与抽象层：MovingAverageStatistics 用 Welford 算法 O(1) 聚合样本；Collector 提供 Start/Stop 与指标元数据；两个内置话题采集器计算 message_age 与 message_period；GenerateStatisticMessage 输出标准 MetricsMessage。rclcpp 在其上实现定时发布，使订阅者可向 /statistics 报告通信质量指标。

如需，我可以把本文写入 ros2doc/ros-tooling/libstatistics_collector源码详细分析.md，或继续分析 rclcpp 中 enable_topic_statistics 的完整启用路径与参数。

interactive_markers 源码详细分析

工作区路径：/home/cp/work2/ros2Learn/ros2_humble/src/ros-visualization/interactive_markers
版本：2.3.3，许可证 BSD。

interactive_markers 是 3D 可交互标记 的通信库：Server 端（应用/规划器）发布 InteractiveMarker，Client 端（RViz2）渲染并回传用户操作（拖拽、菜单点击）。提供 C++ 与 Python 双实现。

1. 仓库结构

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

interactive_markers/
├── include/interactive_markers/
│   ├── interactive_marker_server.hpp   # Server API
│   ├── interactive_marker_client.hpp   # Client API
│   ├── menu_handler.hpp                # 右键菜单
│   ├── message_context.hpp             # 序列号/上下文
│   ├── tools.hpp                       # 辅助工具
│   └── exceptions.hpp
├── src/
│   ├── interactive_marker_server.cpp
│   ├── interactive_marker_client.cpp
│   ├── menu_handler.cpp
│   ├── message_context.cpp
│   └── tools.cpp
├── interactive_markers/                # Python 包
│   ├── interactive_marker_server.py
│   └── menu_handler.py
└── test/

2. 通信协议

topic_namespace 由构造参数指定，通常为节点私有 namespace。

3. InteractiveMarkerServer

3.1 核心 API

1
2
3
4
5
6
7
8
9

class InteractiveMarkerServer {
  void insert(const InteractiveMarker & marker);
  void insert(const InteractiveMarker & marker, FeedbackCallback cb);
  bool erase(const std::string & name);
  void clear();
  bool setPose(...);
  bool setCallback(...);
  void applyChanges();  // 批量提交，此前修改不发布
};

关键设计：insert/erase/setPose 仅写入 pending 队列，必须调用 applyChanges() 才会发布 UPDATE 消息。

3.2 构造与 ROS 接口

1
2
3

get_interactive_markers_service_ = create_service(..., topic_namespace + "/get_interactive_markers");
update_pub_ = create_publisher(..., topic_namespace + "/update");
feedback_sub_ = create_subscription(..., topic_namespace + "/feedback", processFeedback);

使用 node interfaces 注入（NodeBaseInterface 等），支持 composable node；也提供 NodePtr 模板便捷构造。

3.3 applyChanges 流程

遍历 pending_updates_，对每条记录执行 INSERT/UPDATE/ERASE/POSE_UPDATE，组装 InteractiveMarkerUpdate 并 publish。

3.4 Feedback 处理

processFeedback 校验 marker 存在、序列号、回调注册，调用用户 FeedbackCallback，必要时更新 marker pose 并再次 applyChanges()。

4. InteractiveMarkerClient

4.1 职责

1
2
3

/// Handles topic subscription, error detection and tf transformations.
/// After connecting, sends GetInteractiveMarkers for initial state.
/// On error (update loss, tf failure), connection is reset.

4.2 状态机

4.3 connect 流程

1
2
3

get_interactive_markers_client_ = create_client(..., topic_namespace + "/get_interactive_markers");
feedback_pub_ = create_publisher(..., topic_namespace + "/feedback");
update_sub_ = create_subscription(..., topic_namespace + "/update", ...);

支持 TF：带时间戳的 marker 变换到 target_frame（依赖 tf2::BufferCoreInterface）。

4.4 回调类型

• UpdateCallback — 收到 update
• InitializeCallback — 初始 service 完成
• ResetCallback — 连接重置
• StatusCallback — 调试/错误信息

5. MenuHandler

MenuHandler 管理 marker 附加的 菜单项（CHECK、RADIO、FEEDBACK 等），与 Server 的 marker 描述合并后一并发布。用户点击菜单项时通过 InteractiveMarkerFeedback 的 MENU_SELECT 事件回传。

6. MessageContext

维护 update 消息的 sequence_number，Client 用于检测丢包并触发 reset。

7. Python 实现

interactive_markers/interactive_marker_server.py 镜像 C++ Server API，供纯 Python 节点（如简单 demo）使用。生产环境 RViz/MoveIt 多使用 C++ Server。

8. 依赖

1
2
3
4
5

interactive_markers
  ├── rclcpp / rclpy
  ├── visualization_msgs  (InteractiveMarker, Update, Feedback, GetInteractiveMarkers)
  ├── tf2 / tf2_geometry_msgs
  └── builtin_interfaces

9. 下游

10. 测试

• test_interactive_marker_server.cpp — Server insert/apply/feedback
• test_interactive_marker_client.cpp — Client 连接与 update 队列（MAX_UPDATE_QUEUE_SIZE = 100）

11. 设计特点与局限

12. 推荐阅读顺序

1. interactive_marker_server.hpp — Server API 与 applyChanges 语义
2. interactive_marker_server.cpp — applyChanges / processFeedback
3. interactive_marker_client.hpp + .cpp — Client 状态机
4. menu_handler.hpp — 菜单扩展
5. RViz2 interactive_marker_display（在 ros2/rviz 仓库）

13. 小结

interactive_markers 定义了 RViz 3D 交互的 标准 Server/Client 协议：service 全量同步 + topic 增量更新 + feedback 回传。应用侧用 Server 管理 marker 生命周期，RViz 用 Client 显示并捕获用户输入。

python_qt_binding 源码详细分析

工作区路径：/home/cp/work2/ros2Learn/ros2_humble/src/ros-visualization/python_qt_binding
版本：1.1.3，许可证 BSD。

python_qt_binding 是 ROS Qt 栈的 Python Qt 绑定抽象层：统一 PyQt5 与 PySide2 的导入路径，使 qt_gui、rqt 等包无需关心底层绑定实现。

1. 仓库结构

1
2
3
4
5
6
7
8
9
10

python_qt_binding/
├── src/python_qt_binding/
│   ├── __init__.py           # 导出 QtCore/QtGui/QtWidgets
│   └── binding_helper.py     # 绑定选择与加载
├── cmake/
│   ├── sip_helper.cmake      # SIP 绑定生成（C++ 扩展）
│   └── shiboken_helper.cmake
├── test/test_imports.py
├── CMakeLists.txt / setup.py
└── package.xml

2. 核心机制

2.1 绑定选择

1
2
3

def _select_qt_binding(binding_name=None, binding_order=None):
    DEFAULT_BINDING_ORDER = ['pyqt', 'pyside']
    # 按顺序尝试 import PyQt5 / PySide2 模块

优先级：PyQt5 > PySide2（可通过环境或参数指定）。

2.2 统一导入

应用代码写法：

1
2

from python_qt_binding.QtCore import QObject, Signal, Slot
from python_qt_binding.QtWidgets import QMainWindow

__init__.py 在 import 时调用 _select_qt_binding()，设置全局 QT_BINDING、QT_BINDING_VERSION。

2.3 CMake 辅助

为需要 SIP/Shiboken 生成 C++ Python 绑定的包（如 qt_gui_cpp）提供 sip_helper.cmake、shiboken_helper.cmake。

3. 在栈中的位置

所有 Python 版 rqt 插件 必须 通过本包导入 Qt，禁止直接 import PyQt5。

4. 依赖

• 运行时：python3-pyqt5 或 pyside2（二选一）
• 构建：ament_cmake + 可选 SIP 工具链

5. 测试

test_imports.py 验证 QtCore、QtGui、QtWidgets 可导入。

6. 小结

python_qt_binding 体量小但 全栈依赖：解决 ROS 生态中 PyQt/PySide 分裂问题，是 qt_gui_core 与所有 Python rqt 插件的基础。

qt_gui_core 源码详细分析

工作区路径：/home/cp/work2/ros2Learn/ros2_humble/src/ros-visualization/qt_gui_core
子包版本：2.2.5（统一发布）。

qt_gui_core 是 通用 Qt GUI 插件框架（源自 ROS 1 qt_gui），与 ROS 无强耦合的核心在 qt_gui 包；rqt 在其上叠加 ROS 插件发现。本目录含 6 个 ament 包。

1. 子包一览

2. 架构总览

3. qt_gui — 核心框架

3.1 入口 main.py

• 创建 QApplication
• 实例化 PluginManager、PerspectiveManager、MainWindow
• 加载 settings（perspective 布局持久化）
• 注册 PluginProvider 链

3.2 PluginManager

1
2
3

class PluginManager(QObject):
    """Manager of plugin life cycle.
    Creates PluginHandler for each instance, maintains perspective-specific running plugins."""

职责：

• 发现插件（PluginProvider.discover）
• 加载/卸载插件实例（PluginHandlerDirect / Container）
• 管理 dock 布局与 perspective 保存
• 信号：plugins_changed_signal、plugin_help_signal

3.3 关键模块

3.4 Plugin 基类

plugin.py 定义插件生命周期：

• startup(context, instance_id) — 初始化
• shutdown() — 清理
• save_settings / restore_settings — 实例配置

4. qt_gui_cpp — C++ 插件

• plugin.xml 导出 CppPluginProvider
• 通过 pluginlib 加载 qt_gui_cpp::Plugin 子类
• 提供 qt_gui_cpp::PluginContext，供 C++ 插件访问 node 等

5. qt_gui_py_common

Python 插件基类与 PluginContext 包装，被 rqt_gui_py 扩展为 ROS 感知版本。

6. qt_dotgraph

• 将 DOT 语言 解析为 Qt Graphics 场景
• rqt_graph 用于绘制 ROS 计算图
• 依赖 pydot / Graphviz

7. qt_gui_app

提供非 ROS 的 qt_gui 独立可执行文件，用于测试纯 Qt 插件。

8. 插件发现流程

1. RecursivePluginProvider 遍历 ament index 中 qt_gui / rqt_gui plugin 声明
2. 读取各包 plugin.xml
3. 解析 <class type="..." base_class_type="qt_gui_py::Plugin">
4. PluginManager 按菜单分组展示，用户选择后 PluginHandler 实例化

9. 与 rqt 的关系

rqt_gui 额外注册 RosPluginProvider、RosPyPluginProvider，扫描 ROS 包 export。

10. 依赖

1
2
3

qt_gui → python_qt_binding, tango_icons_vendor
qt_gui_cpp → qt_gui, pluginlib, python_qt_binding (sip)
qt_dotgraph → python_qt_binding, pydot

11. 推荐阅读顺序

1. qt_gui/main.py — 启动流程
2. plugin_manager.py — 生命周期
3. plugin_provider.py + recursive_plugin_provider.py — 发现机制
4. perspective_manager.py — 布局持久化
5. rqt_gui/main.py — ROS 扩展点
6. qt_dotgraph — 图渲染（配合 rqt_graph）

12. 小结

qt_gui_core 提供 与 ROS 无关的 Qt 插件壳：Perspective、Dock、PluginManager 是 rqt 的基石；ROS 特有逻辑（rclpy、话题图）在 rqt 与各 rqt_* 插件中实现。

ros-visualization 源码总览

工作区路径：/home/cp/work2/ros2Learn/ros2_humble/src/ros-visualization
文档输出：/home/cp/work2/ros2Learn/ros2doc/ros-visualization/

ros-visualization 是 ROS 2 可视化与 GUI 工具栈：提供 RViz 交互标记、Qt/rqt GUI 框架，以及大量 introspection 插件（话题图、plot、bag 回放等）。

1. 顶层目录与文档索引

2. 栈层次架构

3. 功能分组

3.1 3D 交互（非 RViz 本体）

• interactive_markers：Server/Client 协议，供 RViz、Nav2、MoveIt 等实现可拖拽 3D 控件

3.2 GUI 基础设施

• python_qt_binding：统一 PyQt5/PySide2 导入
• tango_icons_vendor：非 Linux 平台 Tango 图标
• qt_gui_core：通用 Qt 插件框架（Perspective、Dock、PluginManager）
• rqt：ROS 专用 qt_gui 入口 + Python/C++ 插件 Provider

3.3 rqt 插件（Introspection / Tools）

4. 典型启动链

1
2
3
4
5
6

# 通用 rqt 壳
ros2 run rqt_gui rqt_gui

# 独立插件
ros2 run rqt_graph rqt_graph
ros2 run rqt_bag rqt_bag

内部均继承 qt_gui.main.Main，通过 plugin.xml + RecursivePluginProvider 发现插件。

5. 推荐阅读顺序

1. python_qt_binding → qt_gui_core → rqt（理解插件如何加载）
2. 任选一个 rqt_ 插件*（如 rqt_graph）看 plugin.xml + Plugin 类
3. interactive_markers（与 RViz 3D 交互，独立于 rqt）
4. rqt_bag（较复杂，含子插件体系）

6. 小结

ros-visualization 不是单一包，而是 GUI 基础设施 + 大量插件 的集合：底层 qt_gui 管插件生命周期与布局，中层 rqt 接 ROS 发现机制，上层各 rqt_* 提供具体工具；interactive_markers 则服务 3D 可视化交互，与 rqt 栈平行。

rqt_action 源码详细分析

工作区路径：/home/cp/work2/ros2Learn/ros2_humble/src/ros-visualization/rqt_action
版本：2.0.1

rqt_action 用于 浏览 ROS 2 action 类型（.action 定义：goal/result/feedback）。

1. 结构

1
2
3
4
5

rqt_action/
├── plugin.xml          # ActionPlugin → rqt_action.action_plugin.ActionPlugin
└── src/rqt_action/
    ├── main.py
    └── action_plugin.py

2. 功能

• 枚举 action 接口
• 展示 goal、result、feedback 各部分字段

3. 插件

1
2

<class name="ActionPlugin" type="rqt_action.action_plugin.ActionPlugin"
       base_class_type="rqt_gui_py::Plugin">

4. 依赖

rosidl_runtime_py, ament_index_python, rqt_gui_py

5. 小结

Nav2 / MoveIt 等 action 接口调试前的 类型 introspection 工具（不含 action client 调用 UI）。