diff --git a/src/ros_viz_adapter/CMakeLists.txt b/src/ros_viz_adapter/CMakeLists.txt index 57421b4..5e355b6 100644 --- a/src/ros_viz_adapter/CMakeLists.txt +++ b/src/ros_viz_adapter/CMakeLists.txt @@ -8,9 +8,10 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) find_package(image_transport REQUIRED) +find_package(pluginlib REQUIRED) add_executable(adapter src/adapter.cpp) -ament_target_dependencies(adapter rclcpp sensor_msgs image_transport) +ament_target_dependencies(adapter rclcpp sensor_msgs image_transport pluginlib) install(TARGETS adapter DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) install(FILES README.md DESTINATION share/${PROJECT_NAME}) diff --git a/src/ros_viz_adapter/README.md b/src/ros_viz_adapter/README.md index 9ded79e..c24f369 100644 --- a/src/ros_viz_adapter/README.md +++ b/src/ros_viz_adapter/README.md @@ -6,10 +6,10 @@ YAML 配置文件。节点自动发现 `sensor_msgs/msg/Image` 和 ## 输出 -- RGB：通过 `image_transport` 发布 base topic `/viz//rgb`。 - 安装 `compressed_image_transport` 后，平台订阅其 `/compressed` 传输。 -- 深度：通过 `image_transport` 发布 base topic `/viz//depth`。 - 安装 `compressed_depth_image_transport` 后，平台订阅其 `/compressedDepth`。 +- RGB：直接加载 `image_transport/compressed_pub`，只发布其 + `/viz//rgb/compressed` 传输 topic，不创建 raw base topic。 +- 深度：直接加载 `image_transport/compressedDepth_pub`，只发布其 + `/viz//depth/compressedDepth` 传输 topic，不创建 raw base topic。 - 点云：`/viz//points`，消息仍为 `sensor_msgs/msg/PointCloud2`，但只保留 `x/y/z`，去除 NaN，执行体素降采样、最大点数限制和限频。 @@ -20,6 +20,7 @@ YAML 配置文件。节点自动发现 `sensor_msgs/msg/Image` 和 ## 运行 ```bash +colcon build --packages-select ros_viz_adapter --symlink-install ros2 launch ros_viz_adapter adapter.launch.py ``` @@ -29,6 +30,7 @@ ros2 launch ros_viz_adapter adapter.launch.py output_namespace 默认 /viz input_fps_limit 默认 30，回调入口丢弃超频帧 output_fps 默认 10，处理线程输出上限 +jpeg_quality 默认 80，范围 1-100 voxel_size_m 默认 0.05 max_points 默认 100000 ``` @@ -77,7 +79,9 @@ ros2 run ros_viz_adapter adapter ### 独立 ROS 容器或 sidecar 适配器容器与用户容器必须使用相同的 `ROS_DOMAIN_ID`、`RMW_IMPLEMENTATION` 和可互相发现的 -ROS 2 网络。适配器容器只需安装本包及 image_transport 压缩插件，不需要复制用户项目代码。 +ROS 2 网络。适配器容器只需安装本包、`compressed_image_transport` 和 +`compressed_depth_image_transport`，不需要复制用户项目代码。代码只加载上述两个 +指定插件，不遍历或加载 Theora 插件。 ### 多容器隔离 diff --git a/src/ros_viz_adapter/package.xml b/src/ros_viz_adapter/package.xml index de2964a..d489790 100644 --- a/src/ros_viz_adapter/package.xml +++ b/src/ros_viz_adapter/package.xml @@ -9,6 +9,7 @@ rclcpp sensor_msgs image_transport + pluginlib compressed_image_transport compressed_depth_image_transport ament_cmake diff --git a/src/ros_viz_adapter/src/adapter.cpp b/src/ros_viz_adapter/src/adapter.cpp index 7d749c8..bfa979b 100644 --- a/src/ros_viz_adapter/src/adapter.cpp +++ b/src/ros_viz_adapter/src/adapter.cpp @@ -12,7 +12,8 @@ #include #include -#include "image_transport/image_transport.hpp" +#include "image_transport/publisher_plugin.hpp" +#include "pluginlib/class_loader.hpp" #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/image.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" @@ -103,8 +104,8 @@ private: wake_.notify_all(); } - std::string base(const std::string & topic, bool depth) const { - return (output_ns_.empty() ? "/viz" : output_ns_) + "/" + safe_name(topic) + (depth ? "/depth" : "/rgb"); + std::string image_topic(const std::string & topic, bool depth) const { + return (output_ns_.empty() ? "/viz" : output_ns_) + "/" + safe_name(topic) + (depth ? "/depth/compressedDepth" : "/rgb/compressed"); } void image_loop(std::unordered_map & slots, bool depth) { @@ -121,10 +122,17 @@ private: std::lock_guard lock(pub_mutex_); auto it = image_publishers_.find(key); if (it == image_publishers_.end()) { - auto pub = image_transport::create_publisher(this, base(item.first, depth), qos_.get_rmw_qos_profile()); - it = image_publishers_.emplace(key, std::move(pub)).first; + const auto lookup = depth ? "image_transport/compressedDepth_pub" : "image_transport/compressed_pub"; + try { + auto plugin = image_loader_->createSharedInstance(lookup); + plugin->advertise(this, image_topic(item.first, depth), qos_.get_rmw_qos_profile()); + it = image_publishers_.emplace(key, std::move(plugin)).first; + } catch (const pluginlib::PluginlibException & ex) { + RCLCPP_ERROR(get_logger(), "Cannot load image_transport plugin %s: %s", lookup, ex.what()); + continue; + } } - it->second.publish(item.second); + it->second->publishPtr(item.second); } } } @@ -175,13 +183,15 @@ private: } std::string output_ns_; double input_fps_{30.0}, output_fps_{10.0}, voxel_{0.05}; int max_points_{100000}; + std::shared_ptr> image_loader_{ + std::make_shared>("image_transport", "image_transport::PublisherPlugin")}; rclcpp::QoS qos_{rclcpp::SensorDataQoS()}; rclcpp::TimerBase::SharedPtr timer_; std::atomic_bool stop_{false}; std::mutex data_mutex_, rate_mutex_, pub_mutex_; std::condition_variable wake_; std::thread rgb_thread_, depth_thread_, cloud_thread_; std::unordered_map::SharedPtr> subscriptions_; std::unordered_map::SharedPtr> cloud_subscriptions_; std::unordered_map rgb_latest_, depth_latest_; std::unordered_map cloud_latest_; std::unordered_map input_times_, output_times_; - std::unordered_map image_publishers_; std::unordered_map::SharedPtr> cloud_publishers_; + std::unordered_map> image_publishers_; std::unordered_map::SharedPtr> cloud_publishers_; }; int main(int argc, char ** argv) { rclcpp::init(argc, argv); auto node = std::make_shared(); rclcpp::spin(node); rclcpp::shutdown(); return 0; }