update

install/_local_setup_util_ps1.py

@@ -193,13 +193,12 @@ def process_dsv_file(
 ):
     commands = []
     if _include_comments():
-        commands.append(FORMAT_STR_COMMENT_LINE.format_map({
+        commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path}))
-            'comment': dsv_path}))
     with open(dsv_path, 'r') as h:
         content = h.read()
     lines = content.splitlines()
 
-    basename_map = OrderedDict()
+    basenames = OrderedDict()
     for i, line in enumerate(lines):
         # skip over empty or whitespace-only lines
         if not line.strip():
@@ -224,21 +223,21 @@ def process_dsv_file(
         else:
             # group remaining source lines by basename
             path_without_ext, ext = os.path.splitext(remainder)
-            if path_without_ext not in basename_map:
+            if path_without_ext not in basenames:
-                basename_map[path_without_ext] = set()
+                basenames[path_without_ext] = set()
             assert ext.startswith('.')
             ext = ext[1:]
             if ext in (primary_extension, additional_extension):
-                basename_map[path_without_ext].add(ext)
+                basenames[path_without_ext].add(ext)
 
     # add the dsv extension to each basename if the file exists
-    for basename, extensions in basename_map.items():
+    for basename, extensions in basenames.items():
         if not os.path.isabs(basename):
             basename = os.path.join(prefix, basename)
         if os.path.exists(basename + '.dsv'):
             extensions.add('dsv')
 
-    for basename, extensions in basename_map.items():
+    for basename, extensions in basenames.items():
         if not os.path.isabs(basename):
             basename = os.path.join(prefix, basename)
         if 'dsv' in extensions:
@@ -305,8 +304,8 @@ def handle_dsv_types_except_source(type_, remainder, prefix):
                 comment = f'skip extending {env_name} with not existing ' \
                     f'path: {value}'
                 if _include_comments():
-                    commands.append(FORMAT_STR_COMMENT_LINE.format_map({
+                    commands.append(
-                        'comment': comment}))
+                        FORMAT_STR_COMMENT_LINE.format_map({'comment': comment}))
             elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE:
                 commands += _append_unique_value(env_name, value)
             else:
@@ -321,15 +320,15 @@ env_state = {}
 
 
 def _append_unique_value(name, value):
+    global env_state
     if name not in env_state:
         if os.environ.get(name):
             env_state[name] = set(os.environ[name].split(os.pathsep))
         else:
             env_state[name] = set()
-    # Append even if the variable has not been set yet, in case a shell script
+    # append even if the variable has not been set yet, in case a shell script sets the
-    # sets the same variable without the knowledge of this Python script.
+    # same variable without the knowledge of this Python script.
-    # Later, _remove_ending_separators() will cleanup any unintentional
+    # later _remove_ending_separators() will cleanup any unintentional leading separator
-    # leading separator.
     extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep
     line = FORMAT_STR_SET_ENV_VAR.format_map(
         {'name': name, 'value': extend + value})
@@ -343,15 +342,15 @@ def _append_unique_value(name, value):
 
 
 def _prepend_unique_value(name, value):
+    global env_state
     if name not in env_state:
         if os.environ.get(name):
             env_state[name] = set(os.environ[name].split(os.pathsep))
         else:
             env_state[name] = set()
-    # Prepend even if the variable has not been set yet, in case a shell script
+    # prepend even if the variable has not been set yet, in case a shell script sets the
-    # sets the same variable without the knowledge of this Python script.
+    # same variable without the knowledge of this Python script.
-    # Later, _remove_ending_separators() will cleanup any unintentional
+    # later _remove_ending_separators() will cleanup any unintentional trailing separator
-    # trailing separator.
     extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name})
     line = FORMAT_STR_SET_ENV_VAR.format_map(
         {'name': name, 'value': value + extend})
@@ -370,10 +369,10 @@ def _remove_ending_separators():
     if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None:
         return []
 
+    global env_state
     commands = []
     for name in env_state:
-        # skip variables that already had values before this script started
+        # skip variables that already had values before this script started prepending
-        # appending/prepending
         if name in os.environ:
             continue
         commands += [
@@ -383,6 +382,7 @@ def _remove_ending_separators():
 
 
 def _set(name, value):
+    global env_state
     env_state[name] = value
     line = FORMAT_STR_SET_ENV_VAR.format_map(
         {'name': name, 'value': value})
@@ -390,6 +390,7 @@ def _set(name, value):
 
 
 def _set_if_unset(name, value):
+    global env_state
     line = FORMAT_STR_SET_ENV_VAR.format_map(
         {'name': name, 'value': value})
     if env_state.get(name, os.environ.get(name)):

install/_local_setup_util_sh.py

@@ -12,8 +12,8 @@ FORMAT_STR_COMMENT_LINE = '# {comment}'
 FORMAT_STR_SET_ENV_VAR = 'export {name}="{value}"'
 FORMAT_STR_USE_ENV_VAR = '${name}'
 FORMAT_STR_INVOKE_SCRIPT = 'COLCON_CURRENT_PREFIX="{prefix}" _colcon_prefix_sh_source_script "{script_path}"'  # noqa: E501
-FORMAT_STR_REMOVE_LEADING_SEPARATOR = 'export {name}=${{{name}#:}}'  # noqa: E501
+FORMAT_STR_REMOVE_LEADING_SEPARATOR = 'if [ "$(echo -n ${name} | head -c 1)" = ":" ]; then export {name}=${{{name}#?}} ; fi'  # noqa: E501
-FORMAT_STR_REMOVE_TRAILING_SEPARATOR = 'export {name}=${{{name}%:}}'  # noqa: E501
+FORMAT_STR_REMOVE_TRAILING_SEPARATOR = 'if [ "$(echo -n ${name} | tail -c 1)" = ":" ]; then export {name}=${{{name}%?}} ; fi'  # noqa: E501
 
 DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate'
 DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate'
@@ -193,13 +193,12 @@ def process_dsv_file(
 ):
     commands = []
     if _include_comments():
-        commands.append(FORMAT_STR_COMMENT_LINE.format_map({
+        commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path}))
-            'comment': dsv_path}))
     with open(dsv_path, 'r') as h:
         content = h.read()
     lines = content.splitlines()
 
-    basename_map = OrderedDict()
+    basenames = OrderedDict()
     for i, line in enumerate(lines):
         # skip over empty or whitespace-only lines
         if not line.strip():
@@ -224,21 +223,21 @@ def process_dsv_file(
         else:
             # group remaining source lines by basename
             path_without_ext, ext = os.path.splitext(remainder)
-            if path_without_ext not in basename_map:
+            if path_without_ext not in basenames:
-                basename_map[path_without_ext] = set()
+                basenames[path_without_ext] = set()
             assert ext.startswith('.')
             ext = ext[1:]
             if ext in (primary_extension, additional_extension):
-                basename_map[path_without_ext].add(ext)
+                basenames[path_without_ext].add(ext)
 
     # add the dsv extension to each basename if the file exists
-    for basename, extensions in basename_map.items():
+    for basename, extensions in basenames.items():
         if not os.path.isabs(basename):
             basename = os.path.join(prefix, basename)
         if os.path.exists(basename + '.dsv'):
             extensions.add('dsv')
 
-    for basename, extensions in basename_map.items():
+    for basename, extensions in basenames.items():
         if not os.path.isabs(basename):
             basename = os.path.join(prefix, basename)
         if 'dsv' in extensions:
@@ -305,8 +304,8 @@ def handle_dsv_types_except_source(type_, remainder, prefix):
                 comment = f'skip extending {env_name} with not existing ' \
                     f'path: {value}'
                 if _include_comments():
-                    commands.append(FORMAT_STR_COMMENT_LINE.format_map({
+                    commands.append(
-                        'comment': comment}))
+                        FORMAT_STR_COMMENT_LINE.format_map({'comment': comment}))
             elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE:
                 commands += _append_unique_value(env_name, value)
             else:
@@ -321,15 +320,15 @@ env_state = {}
 
 
 def _append_unique_value(name, value):
+    global env_state
     if name not in env_state:
         if os.environ.get(name):
             env_state[name] = set(os.environ[name].split(os.pathsep))
         else:
             env_state[name] = set()
-    # Append even if the variable has not been set yet, in case a shell script
+    # append even if the variable has not been set yet, in case a shell script sets the
-    # sets the same variable without the knowledge of this Python script.
+    # same variable without the knowledge of this Python script.
-    # Later, _remove_ending_separators() will cleanup any unintentional
+    # later _remove_ending_separators() will cleanup any unintentional leading separator
-    # leading separator.
     extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep
     line = FORMAT_STR_SET_ENV_VAR.format_map(
         {'name': name, 'value': extend + value})
@@ -343,15 +342,15 @@ def _append_unique_value(name, value):
 
 
 def _prepend_unique_value(name, value):
+    global env_state
     if name not in env_state:
         if os.environ.get(name):
             env_state[name] = set(os.environ[name].split(os.pathsep))
         else:
             env_state[name] = set()
-    # Prepend even if the variable has not been set yet, in case a shell script
+    # prepend even if the variable has not been set yet, in case a shell script sets the
-    # sets the same variable without the knowledge of this Python script.
+    # same variable without the knowledge of this Python script.
-    # Later, _remove_ending_separators() will cleanup any unintentional
+    # later _remove_ending_separators() will cleanup any unintentional trailing separator
-    # trailing separator.
     extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name})
     line = FORMAT_STR_SET_ENV_VAR.format_map(
         {'name': name, 'value': value + extend})
@@ -370,10 +369,10 @@ def _remove_ending_separators():
     if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None:
         return []
 
+    global env_state
     commands = []
     for name in env_state:
-        # skip variables that already had values before this script started
+        # skip variables that already had values before this script started prepending
-        # appending/prepending
         if name in os.environ:
             continue
         commands += [
@@ -383,6 +382,7 @@ def _remove_ending_separators():
 
 
 def _set(name, value):
+    global env_state
     env_state[name] = value
     line = FORMAT_STR_SET_ENV_VAR.format_map(
         {'name': name, 'value': value})
@@ -390,6 +390,7 @@ def _set(name, value):
 
 
 def _set_if_unset(name, value):
+    global env_state
     line = FORMAT_STR_SET_ENV_VAR.format_map(
         {'name': name, 'value': value})
     if env_state.get(name, os.environ.get(name)):

install/bcr_bot/share/ament_index/resource_index/parent_prefix_path/bcr_bot

@@ -1 +1 @@
-/workspace/install/bcr_bot:/opt/ros/humble
+/workspace/install/ros_viz_adapter:/workspace/install/bcr_bot:/opt/ros/humble

install/bcr_bot/share/bcr_bot/launch/bcr_bot_gazebo_spawn.launch.py

@@ -25,7 +25,7 @@ def generate_launch_description():
     position_y = LaunchConfiguration("position_y")
     orientation_yaw = LaunchConfiguration("orientation_yaw")
     camera_enabled = LaunchConfiguration("camera_enabled", default=True)
-    stereo_camera_enabled = LaunchConfiguration("stereo_camera_enabled", default=False)
+    stereo_camera_enabled = LaunchConfiguration("stereo_camera_enabled", default=True)
     two_d_lidar_enabled = LaunchConfiguration("two_d_lidar_enabled", default=True)
     odometry_source = LaunchConfiguration("odometry_source", default="world")
     robot_namespace = LaunchConfiguration("robot_namespace", default='bcr_bot')

install/bcr_bot/share/bcr_bot/launch/bcr_bot_gz_spawn.launch.py

@@ -23,7 +23,7 @@ def generate_launch_description():
     position_y = LaunchConfiguration("position_y")
     orientation_yaw = LaunchConfiguration("orientation_yaw")
     camera_enabled = LaunchConfiguration("camera_enabled", default=True)
-    stereo_camera_enabled = LaunchConfiguration("stereo_camera_enabled", default=False)
+    stereo_camera_enabled = LaunchConfiguration("stereo_camera_enabled", default=True)
     two_d_lidar_enabled = LaunchConfiguration("two_d_lidar_enabled", default=True)
     odometry_source = LaunchConfiguration("odometry_source")
 
@@ -126,4 +126,4 @@ def generate_launch_description():
         DeclareLaunchArgument("odometry_source", default_value="world"),
         robot_state_publisher,
         gz_spawn_entity, transform_publisher, gz_ros2_bridge
-    ])
+    ])

install/bcr_bot/share/bcr_bot/launch/bcr_bot_ign_spawn.launch.py

@@ -23,7 +23,7 @@ def generate_launch_description():
     position_y = LaunchConfiguration("position_y")
     orientation_yaw = LaunchConfiguration("orientation_yaw")
     camera_enabled = LaunchConfiguration("camera_enabled", default=True)
-    stereo_camera_enabled = LaunchConfiguration("stereo_camera_enabled", default=False)
+    stereo_camera_enabled = LaunchConfiguration("stereo_camera_enabled", default=True)
     two_d_lidar_enabled = LaunchConfiguration("two_d_lidar_enabled", default=True)
     odometry_source = LaunchConfiguration("odometry_source")
 

install/bcr_bot/share/bcr_bot/urdf/bcr_bot.xacro

@@ -38,7 +38,7 @@
     <xacro:arg name="robot_namespace" default=""/>
     <xacro:arg name="wheel_odom_topic" default="odom" />
     <xacro:arg name="camera_enabled" default="false" />
-    <xacro:arg name="stereo_camera_enabled" default="false" />
+    <xacro:arg name="stereo_camera_enabled" default="true" />
     <xacro:arg name="two_d_lidar_enabled" default="false" />
     <xacro:arg name="publish_wheel_odom_tf" default="true" />
     <xacro:arg name="conveyor_enabled" default="false"/>

install/bcr_bot/share/bcr_bot/urdf/gz.xacro

@@ -87,7 +87,7 @@
     <xacro:if value="$(arg camera_enabled)">
         <gazebo reference="kinect_camera">
             <sensor type="depth_camera" name="kinect_camera">
-                <update_rate>30.0</update_rate>
+                <update_rate>10.0</update_rate>
                 <topic>kinect_camera</topic>
                 <gz_frame_id>kinect_camera</gz_frame_id>
                 <camera>

install/ros_viz_adapter/share/ament_index/resource_index/package_run_dependencies/ros_viz_adapter

@@ -0,0 +1 @@
+rclcpp;sensor_msgs;image_transport;pluginlib;compressed_image_transport;compressed_depth_image_transport

install/ros_viz_adapter/share/ament_index/resource_index/parent_prefix_path/ros_viz_adapter

@@ -0,0 +1 @@
+/workspace/install/ros_viz_adapter:/workspace/install/bcr_bot:/opt/ros/humble

install/ros_viz_adapter/share/colcon-core/packages/ros_viz_adapter

@@ -0,0 +1 @@
+compressed_depth_image_transport:compressed_image_transport:image_transport:pluginlib:rclcpp:sensor_msgs

install/ros_viz_adapter/share/ros_viz_adapter/README.md

@@ -0,0 +1,110 @@
+# ros_viz_adapter
+
+通用 ROS 2 可视化发布适配器，不需要用户修改原始话题名称，也不需要
+YAML 配置文件。节点自动发现 `sensor_msgs/msg/Image` 和
+`sensor_msgs/msg/PointCloud2`，并把结果发布到 `/viz` 命名空间。
+
+## 输出
+
+- RGB：直接加载 `image_transport/compressed_pub`，插件发布
+  `/viz/<source>/rgb/compressed`，不创建 raw base topic。
+- 深度：直接加载 `image_transport/compressedDepth_pub`，插件发布
+  `/viz/<source>/depth/compressedDepth`，不创建 raw base topic。
+- 点云：`/viz/<source>/points`，消息仍为 `sensor_msgs/msg/PointCloud2`，但只保留
+  `x/y/z`，去除 NaN，执行体素降采样、最大点数限制和限频。
+
+图像依据 `encoding` 分类：`rgb8/bgr8/rgba8/bgra8/mono8` 为 RGB，
+`16UC1/32FC1/mono16/32SC1` 为深度。RGB、深度、点云各使用一个独立处理线程；
+不存在对应数据类型时，不创建该线程。
+
+## 运行
+
+```bash
+colcon build --packages-select ros_viz_adapter --symlink-install
+ros2 launch ros_viz_adapter adapter.launch.py
+```
+
+点云可视化示例：
+
+```bash
+ros2 launch ros_viz_adapter adapter.launch.py \
+  input_fps_limit:=10 \
+  output_fps:=5 \
+  voxel_size_m:=0.10 \
+  max_points:=50000
+```
+
+参数：
+
+```text
+output_namespace      默认 /viz
+input_fps_limit       默认 30，回调入口丢弃超频帧
+output_fps            默认 10，处理线程输出上限
+jpeg_quality          默认 80，范围 1-100
+voxel_size_m          默认 0.05
+max_points            默认 100000
+```
+
+调参规律：
+input_fps_limit 越低，适配器接收和处理次数越少；
+output_fps 越低，平台网络和渲染负载越低；
+voxel_size_m 越大，点越少、处理越快，但细节损失越明显，=0 可以关闭体素降采样，但通常不建议这么做；
+max_points 越小，输出消息越小，但点云显示更稀疏；
+input_fps_limit:=0 或 output_fps:=0 表示不限制频率。
+
+QoS 为 `BEST_EFFORT + KEEP_LAST + depth=1`。限频降低的是适配器处理、网络和
+平台负载；如果还要降低源容器 CPU，应同时调整相机/上游发布频率。
+
+## 在其他项目中接入
+
+### 同一个 colcon 工作空间
+
+把本包目录放到项目工作空间的 `src/` 下，然后正常构建：
+
+```bash
+cd <workspace>
+colcon build --symlink-install
+source install/setup.bash
+ros2 launch ros_viz_adapter adapter.launch.py
+```
+
+不需要修改原始 RGB、深度或点云节点。适配器会在同一个 ROS domain 中发现传感器话题，
+并发布 `/viz/...`。
+
+### 在项目 launch 中启动
+
+也可以在项目自己的 launch 文件中包含适配器：
+
+```python
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.substitutions import FindPackageShare
+
+viz_adapter = IncludeLaunchDescription(
+    PythonLaunchDescriptionSource([
+        FindPackageShare('ros_viz_adapter'), '/launch/adapter.launch.py'
+    ])
+)
+```
+
+将 `viz_adapter` 放入项目的 `LaunchDescription` 即可。也可以直接运行：
+
+```bash
+ros2 run ros_viz_adapter adapter
+```
+
+### 独立 ROS 容器或 sidecar
+
+适配器容器与用户容器必须使用相同的 `ROS_DOMAIN_ID`、`RMW_IMPLEMENTATION` 和可互相发现的
+ROS 2 网络。适配器容器只需安装本包、`compressed_image_transport` 和
+`compressed_depth_image_transport`，不需要复制用户项目代码。代码只加载上述两个
+指定插件，不遍历或加载 Theora 插件。
+
+### 多容器隔离
+
+为避免不同容器输出重名，可以指定不同 namespace：
+
+```bash
+ros2 launch ros_viz_adapter adapter.launch.py \
+  output_namespace:=/viz/robot_01
+```

install/ros_viz_adapter/share/ros_viz_adapter/cmake/ros_viz_adapterConfig-version.cmake

@@ -0,0 +1,14 @@
+# generated from ament/cmake/core/templates/nameConfig-version.cmake.in
+set(PACKAGE_VERSION "0.2.0")
+
+set(PACKAGE_VERSION_EXACT False)
+set(PACKAGE_VERSION_COMPATIBLE False)
+
+if("${PACKAGE_FIND_VERSION}" VERSION_EQUAL "${PACKAGE_VERSION}")
+  set(PACKAGE_VERSION_EXACT True)
+  set(PACKAGE_VERSION_COMPATIBLE True)
+endif()
+
+if("${PACKAGE_FIND_VERSION}" VERSION_LESS "${PACKAGE_VERSION}")
+  set(PACKAGE_VERSION_COMPATIBLE True)
+endif()

install/ros_viz_adapter/share/ros_viz_adapter/cmake/ros_viz_adapterConfig.cmake

@@ -0,0 +1,42 @@
+# generated from ament/cmake/core/templates/nameConfig.cmake.in
+
+# prevent multiple inclusion
+if(_ros_viz_adapter_CONFIG_INCLUDED)
+  # ensure to keep the found flag the same
+  if(NOT DEFINED ros_viz_adapter_FOUND)
+    # explicitly set it to FALSE, otherwise CMake will set it to TRUE
+    set(ros_viz_adapter_FOUND FALSE)
+  elseif(NOT ros_viz_adapter_FOUND)
+    # use separate condition to avoid uninitialized variable warning
+    set(ros_viz_adapter_FOUND FALSE)
+  endif()
+  return()
+endif()
+set(_ros_viz_adapter_CONFIG_INCLUDED TRUE)
+
+# output package information
+if(NOT ros_viz_adapter_FIND_QUIETLY)
+  message(STATUS "Found ros_viz_adapter: 0.2.0 (${ros_viz_adapter_DIR})")
+endif()
+
+# warn when using a deprecated package
+if(NOT "" STREQUAL "")
+  set(_msg "Package 'ros_viz_adapter' is deprecated")
+  # append custom deprecation text if available
+  if(NOT "" STREQUAL "TRUE")
+    set(_msg "${_msg} ()")
+  endif()
+  # optionally quiet the deprecation message
+  if(NOT ${ros_viz_adapter_DEPRECATED_QUIET})
+    message(DEPRECATION "${_msg}")
+  endif()
+endif()
+
+# flag package as ament-based to distinguish it after being find_package()-ed
+set(ros_viz_adapter_FOUND_AMENT_PACKAGE TRUE)
+
+# include all config extra files
+set(_extras "")
+foreach(_extra ${_extras})
+  include("${ros_viz_adapter_DIR}/${_extra}")
+endforeach()

install/ros_viz_adapter/share/ros_viz_adapter/environment/ament_prefix_path.dsv

@@ -0,0 +1 @@
+prepend-non-duplicate;AMENT_PREFIX_PATH;

install/ros_viz_adapter/share/ros_viz_adapter/environment/ament_prefix_path.sh

@@ -0,0 +1,4 @@
+# copied from
+# ament_cmake_core/cmake/environment_hooks/environment/ament_prefix_path.sh
+
+ament_prepend_unique_value AMENT_PREFIX_PATH "$AMENT_CURRENT_PREFIX"

install/ros_viz_adapter/share/ros_viz_adapter/environment/path.dsv

@@ -0,0 +1 @@
+prepend-non-duplicate-if-exists;PATH;bin

install/ros_viz_adapter/share/ros_viz_adapter/environment/path.sh

@@ -0,0 +1,5 @@
+# copied from ament_cmake_core/cmake/environment_hooks/environment/path.sh
+
+if [ -d "$AMENT_CURRENT_PREFIX/bin" ]; then
+  ament_prepend_unique_value PATH "$AMENT_CURRENT_PREFIX/bin"
+fi

install/ros_viz_adapter/share/ros_viz_adapter/hook/cmake_prefix_path.dsv

@@ -0,0 +1 @@
+prepend-non-duplicate;CMAKE_PREFIX_PATH;

install/ros_viz_adapter/share/ros_viz_adapter/hook/cmake_prefix_path.ps1

@@ -0,0 +1,3 @@
+# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
+
+colcon_prepend_unique_value CMAKE_PREFIX_PATH "$env:COLCON_CURRENT_PREFIX"

install/ros_viz_adapter/share/ros_viz_adapter/hook/cmake_prefix_path.sh

@@ -0,0 +1,3 @@
+# generated from colcon_core/shell/template/hook_prepend_value.sh.em
+
+_colcon_prepend_unique_value CMAKE_PREFIX_PATH "$COLCON_CURRENT_PREFIX"

install/ros_viz_adapter/share/ros_viz_adapter/launch/adapter.launch.py

@@ -0,0 +1,25 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument('output_namespace', default_value='/viz'),
+        DeclareLaunchArgument('input_fps_limit', default_value='30.0'),
+        DeclareLaunchArgument('output_fps', default_value='10.0'),
+        DeclareLaunchArgument('voxel_size_m', default_value='0.05'),
+        DeclareLaunchArgument('max_points', default_value='100000'),
+        Node(
+            package='ros_viz_adapter', executable='adapter',
+            name='ros_viz_adapter', output='screen',
+            parameters=[{
+                'output_namespace': LaunchConfiguration('output_namespace'),
+                'input_fps_limit': LaunchConfiguration('input_fps_limit'),
+                'output_fps': LaunchConfiguration('output_fps'),
+                'voxel_size_m': LaunchConfiguration('voxel_size_m'),
+                'max_points': LaunchConfiguration('max_points'),
+            }],
+        ),
+    ])

install/ros_viz_adapter/share/ros_viz_adapter/local_setup.bash

@@ -0,0 +1,46 @@
+# generated from ament_package/template/package_level/local_setup.bash.in
+
+# source local_setup.sh from same directory as this file
+_this_path=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" && pwd)
+# provide AMENT_CURRENT_PREFIX to shell script
+AMENT_CURRENT_PREFIX=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`/../.." && pwd)
+# store AMENT_CURRENT_PREFIX to restore it before each environment hook
+_package_local_setup_AMENT_CURRENT_PREFIX=$AMENT_CURRENT_PREFIX
+
+# trace output
+if [ -n "$AMENT_TRACE_SETUP_FILES" ]; then
+  echo "# . \"$_this_path/local_setup.sh\""
+fi
+. "$_this_path/local_setup.sh"
+unset _this_path
+
+# unset AMENT_ENVIRONMENT_HOOKS
+# if not appending to them for return
+if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then
+  unset AMENT_ENVIRONMENT_HOOKS
+fi
+
+# restore AMENT_CURRENT_PREFIX before evaluating the environment hooks
+AMENT_CURRENT_PREFIX=$_package_local_setup_AMENT_CURRENT_PREFIX
+# list all environment hooks of this package
+
+# source all shell-specific environment hooks of this package
+# if not returning them
+if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then
+  _package_local_setup_IFS=$IFS
+  IFS=":"
+  for _hook in $AMENT_ENVIRONMENT_HOOKS; do
+    # restore AMENT_CURRENT_PREFIX for each environment hook
+    AMENT_CURRENT_PREFIX=$_package_local_setup_AMENT_CURRENT_PREFIX
+    # restore IFS before sourcing other files
+    IFS=$_package_local_setup_IFS
+    . "$_hook"
+  done
+  unset _hook
+  IFS=$_package_local_setup_IFS
+  unset _package_local_setup_IFS
+  unset AMENT_ENVIRONMENT_HOOKS
+fi
+
+unset _package_local_setup_AMENT_CURRENT_PREFIX
+unset AMENT_CURRENT_PREFIX

install/ros_viz_adapter/share/ros_viz_adapter/local_setup.dsv

@@ -0,0 +1,2 @@
+source;share/ros_viz_adapter/environment/ament_prefix_path.sh
+source;share/ros_viz_adapter/environment/path.sh

install/ros_viz_adapter/share/ros_viz_adapter/local_setup.sh

@@ -0,0 +1,184 @@
+# generated from ament_package/template/package_level/local_setup.sh.in
+
+# since this file is sourced use either the provided AMENT_CURRENT_PREFIX
+# or fall back to the destination set at configure time
+: ${AMENT_CURRENT_PREFIX:="/workspace/install/ros_viz_adapter"}
+if [ ! -d "$AMENT_CURRENT_PREFIX" ]; then
+  if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+    echo "The compile time prefix path '$AMENT_CURRENT_PREFIX' doesn't " \
+      "exist. Consider sourcing a different extension than '.sh'." 1>&2
+  else
+    AMENT_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
+  fi
+fi
+
+# function to append values to environment variables
+# using colons as separators and avoiding leading separators
+ament_append_value() {
+  # arguments
+  _listname="$1"
+  _value="$2"
+  #echo "listname $_listname"
+  #eval echo "list value \$$_listname"
+  #echo "value $_value"
+
+  # avoid leading separator
+  eval _values=\"\$$_listname\"
+  if [ -z "$_values" ]; then
+    eval export $_listname=\"$_value\"
+    #eval echo "set list \$$_listname"
+  else
+    # field separator must not be a colon
+    _ament_append_value_IFS=$IFS
+    unset IFS
+    eval export $_listname=\"\$$_listname:$_value\"
+    #eval echo "append list \$$_listname"
+    IFS=$_ament_append_value_IFS
+    unset _ament_append_value_IFS
+  fi
+  unset _values
+
+  unset _value
+  unset _listname
+}
+
+# function to append non-duplicate values to environment variables
+# using colons as separators and avoiding leading separators
+ament_append_unique_value() {
+  # arguments
+  _listname=$1
+  _value=$2
+  #echo "listname $_listname"
+  #eval echo "list value \$$_listname"
+  #echo "value $_value"
+
+  # check if the list contains the value
+  eval _values=\$$_listname
+  _duplicate=
+  _ament_append_unique_value_IFS=$IFS
+  IFS=":"
+  if [ "$AMENT_SHELL" = "zsh" ]; then
+    ament_zsh_to_array _values
+  fi
+  for _item in $_values; do
+    # ignore empty strings
+    if [ -z "$_item" ]; then
+      continue
+    fi
+    if [ $_item = $_value ]; then
+      _duplicate=1
+    fi
+  done
+  unset _item
+
+  # append only non-duplicates
+  if [ -z "$_duplicate" ]; then
+    # avoid leading separator
+    if [ -z "$_values" ]; then
+      eval $_listname=\"$_value\"
+      #eval echo "set list \$$_listname"
+    else
+      # field separator must not be a colon
+      unset IFS
+      eval $_listname=\"\$$_listname:$_value\"
+      #eval echo "append list \$$_listname"
+    fi
+  fi
+  IFS=$_ament_append_unique_value_IFS
+  unset _ament_append_unique_value_IFS
+  unset _duplicate
+  unset _values
+
+  unset _value
+  unset _listname
+}
+
+# function to prepend non-duplicate values to environment variables
+# using colons as separators and avoiding trailing separators
+ament_prepend_unique_value() {
+  # arguments
+  _listname="$1"
+  _value="$2"
+  #echo "listname $_listname"
+  #eval echo "list value \$$_listname"
+  #echo "value $_value"
+
+  # check if the list contains the value
+  eval _values=\"\$$_listname\"
+  _duplicate=
+  _ament_prepend_unique_value_IFS=$IFS
+  IFS=":"
+  if [ "$AMENT_SHELL" = "zsh" ]; then
+    ament_zsh_to_array _values
+  fi
+  for _item in $_values; do
+    # ignore empty strings
+    if [ -z "$_item" ]; then
+      continue
+    fi
+    if [ "$_item" = "$_value" ]; then
+      _duplicate=1
+    fi
+  done
+  unset _item
+
+  # prepend only non-duplicates
+  if [ -z "$_duplicate" ]; then
+    # avoid trailing separator
+    if [ -z "$_values" ]; then
+      eval export $_listname=\"$_value\"
+      #eval echo "set list \$$_listname"
+    else
+      # field separator must not be a colon
+      unset IFS
+      eval export $_listname=\"$_value:\$$_listname\"
+      #eval echo "prepend list \$$_listname"
+    fi
+  fi
+  IFS=$_ament_prepend_unique_value_IFS
+  unset _ament_prepend_unique_value_IFS
+  unset _duplicate
+  unset _values
+
+  unset _value
+  unset _listname
+}
+
+# unset AMENT_ENVIRONMENT_HOOKS
+# if not appending to them for return
+if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then
+  unset AMENT_ENVIRONMENT_HOOKS
+fi
+
+# list all environment hooks of this package
+ament_append_value AMENT_ENVIRONMENT_HOOKS "$AMENT_CURRENT_PREFIX/share/ros_viz_adapter/environment/ament_prefix_path.sh"
+ament_append_value AMENT_ENVIRONMENT_HOOKS "$AMENT_CURRENT_PREFIX/share/ros_viz_adapter/environment/path.sh"
+
+# source all shell-specific environment hooks of this package
+# if not returning them
+if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then
+  _package_local_setup_IFS=$IFS
+  IFS=":"
+  if [ "$AMENT_SHELL" = "zsh" ]; then
+    ament_zsh_to_array AMENT_ENVIRONMENT_HOOKS
+  fi
+  for _hook in $AMENT_ENVIRONMENT_HOOKS; do
+    if [ -f "$_hook" ]; then
+      # restore IFS before sourcing other files
+      IFS=$_package_local_setup_IFS
+      # trace output
+      if [ -n "$AMENT_TRACE_SETUP_FILES" ]; then
+        echo "# . \"$_hook\""
+      fi
+      . "$_hook"
+    fi
+  done
+  unset _hook
+  IFS=$_package_local_setup_IFS
+  unset _package_local_setup_IFS
+  unset AMENT_ENVIRONMENT_HOOKS
+fi
+
+# reset AMENT_CURRENT_PREFIX after each package
+# allowing to source multiple package-level setup files
+unset AMENT_CURRENT_PREFIX

install/ros_viz_adapter/share/ros_viz_adapter/local_setup.zsh

@@ -0,0 +1,59 @@
+# generated from ament_package/template/package_level/local_setup.zsh.in
+
+AMENT_SHELL=zsh
+
+# source local_setup.sh from same directory as this file
+_this_path=$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)
+# provide AMENT_CURRENT_PREFIX to shell script
+AMENT_CURRENT_PREFIX=$(builtin cd -q "`dirname "${(%):-%N}"`/../.." > /dev/null && pwd)
+# store AMENT_CURRENT_PREFIX to restore it before each environment hook
+_package_local_setup_AMENT_CURRENT_PREFIX=$AMENT_CURRENT_PREFIX
+
+# function to convert array-like strings into arrays
+# to wordaround SH_WORD_SPLIT not being set
+ament_zsh_to_array() {
+  local _listname=$1
+  local _dollar="$"
+  local _split="{="
+  local _to_array="(\"$_dollar$_split$_listname}\")"
+  eval $_listname=$_to_array
+}
+
+# trace output
+if [ -n "$AMENT_TRACE_SETUP_FILES" ]; then
+  echo "# . \"$_this_path/local_setup.sh\""
+fi
+# the package-level local_setup file unsets AMENT_CURRENT_PREFIX
+. "$_this_path/local_setup.sh"
+unset _this_path
+
+# unset AMENT_ENVIRONMENT_HOOKS
+# if not appending to them for return
+if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then
+  unset AMENT_ENVIRONMENT_HOOKS
+fi
+
+# restore AMENT_CURRENT_PREFIX before evaluating the environment hooks
+AMENT_CURRENT_PREFIX=$_package_local_setup_AMENT_CURRENT_PREFIX
+# list all environment hooks of this package
+
+# source all shell-specific environment hooks of this package
+# if not returning them
+if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then
+  _package_local_setup_IFS=$IFS
+  IFS=":"
+  for _hook in $AMENT_ENVIRONMENT_HOOKS; do
+    # restore AMENT_CURRENT_PREFIX for each environment hook
+    AMENT_CURRENT_PREFIX=$_package_local_setup_AMENT_CURRENT_PREFIX
+    # restore IFS before sourcing other files
+    IFS=$_package_local_setup_IFS
+    . "$_hook"
+  done
+  unset _hook
+  IFS=$_package_local_setup_IFS
+  unset _package_local_setup_IFS
+  unset AMENT_ENVIRONMENT_HOOKS
+fi
+
+unset _package_local_setup_AMENT_CURRENT_PREFIX
+unset AMENT_CURRENT_PREFIX

install/ros_viz_adapter/share/ros_viz_adapter/package.bash

@@ -0,0 +1,39 @@
+# generated from colcon_bash/shell/template/package.bash.em
+
+# This script extends the environment for this package.
+
+# a bash script is able to determine its own path if necessary
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+  # the prefix is two levels up from the package specific share directory
+  _colcon_package_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`/../.." > /dev/null && pwd)"
+else
+  _colcon_package_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
+fi
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+_colcon_package_bash_source_script() {
+  if [ -f "$1" ]; then
+    if [ -n "$COLCON_TRACE" ]; then
+      echo "# . \"$1\""
+    fi
+    . "$@"
+  else
+    echo "not found: \"$1\"" 1>&2
+  fi
+}
+
+# source sh script of this package
+_colcon_package_bash_source_script "$_colcon_package_bash_COLCON_CURRENT_PREFIX/share/ros_viz_adapter/package.sh"
+
+# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced scripts
+COLCON_CURRENT_PREFIX="$_colcon_package_bash_COLCON_CURRENT_PREFIX"
+
+# source bash hooks
+_colcon_package_bash_source_script "$COLCON_CURRENT_PREFIX/share/ros_viz_adapter/local_setup.bash"
+
+unset COLCON_CURRENT_PREFIX
+
+unset _colcon_package_bash_source_script
+unset _colcon_package_bash_COLCON_CURRENT_PREFIX

install/ros_viz_adapter/share/ros_viz_adapter/package.dsv

@@ -0,0 +1,8 @@
+source;share/ros_viz_adapter/hook/cmake_prefix_path.ps1
+source;share/ros_viz_adapter/hook/cmake_prefix_path.dsv
+source;share/ros_viz_adapter/hook/cmake_prefix_path.sh
+source;share/ros_viz_adapter/local_setup.bash
+source;share/ros_viz_adapter/local_setup.dsv
+source;share/ros_viz_adapter/local_setup.ps1
+source;share/ros_viz_adapter/local_setup.sh
+source;share/ros_viz_adapter/local_setup.zsh

install/ros_viz_adapter/share/ros_viz_adapter/package.ps1

@@ -0,0 +1,116 @@
+# generated from colcon_powershell/shell/template/package.ps1.em
+
+# function to append a value to a variable
+# which uses colons as separators
+# duplicates as well as leading separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+function colcon_append_unique_value {
+  param (
+    $_listname,
+    $_value
+  )
+
+  # get values from variable
+  if (Test-Path Env:$_listname) {
+    $_values=(Get-Item env:$_listname).Value
+  } else {
+    $_values=""
+  }
+  $_duplicate=""
+  # start with no values
+  $_all_values=""
+  # iterate over existing values in the variable
+  if ($_values) {
+    $_values.Split(":") | ForEach {
+      # not an empty string
+      if ($_) {
+        # not a duplicate of _value
+        if ($_ -eq $_value) {
+          $_duplicate="1"
+        }
+        if ($_all_values) {
+          $_all_values="${_all_values}:$_"
+        } else {
+          $_all_values="$_"
+        }
+      }
+    }
+  }
+  # append only non-duplicates
+  if (!$_duplicate) {
+    # avoid leading separator
+    if ($_all_values) {
+      $_all_values="${_all_values}:${_value}"
+    } else {
+      $_all_values="${_value}"
+    }
+  }
+
+  # export the updated variable
+  Set-Item env:\$_listname -Value "$_all_values"
+}
+
+# function to prepend a value to a variable
+# which uses colons as separators
+# duplicates as well as trailing separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+function colcon_prepend_unique_value {
+  param (
+    $_listname,
+    $_value
+  )
+
+  # get values from variable
+  if (Test-Path Env:$_listname) {
+    $_values=(Get-Item env:$_listname).Value
+  } else {
+    $_values=""
+  }
+  # start with the new value
+  $_all_values="$_value"
+  # iterate over existing values in the variable
+  if ($_values) {
+    $_values.Split(":") | ForEach {
+      # not an empty string
+      if ($_) {
+        # not a duplicate of _value
+        if ($_ -ne $_value) {
+          # keep non-duplicate values
+          $_all_values="${_all_values}:$_"
+        }
+      }
+    }
+  }
+  # export the updated variable
+  Set-Item env:\$_listname -Value "$_all_values"
+}
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+function colcon_package_source_powershell_script {
+  param (
+    $_colcon_package_source_powershell_script
+  )
+  # source script with conditional trace output
+  if (Test-Path $_colcon_package_source_powershell_script) {
+    if ($env:COLCON_TRACE) {
+      echo ". '$_colcon_package_source_powershell_script'"
+    }
+    . "$_colcon_package_source_powershell_script"
+  } else {
+    Write-Error "not found: '$_colcon_package_source_powershell_script'"
+  }
+}
+
+
+# a powershell script is able to determine its own path
+# the prefix is two levels up from the package specific share directory
+$env:COLCON_CURRENT_PREFIX=(Get-Item $PSCommandPath).Directory.Parent.Parent.FullName
+
+colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX/share/ros_viz_adapter/hook/cmake_prefix_path.ps1"
+colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX/share/ros_viz_adapter/local_setup.ps1"
+
+Remove-Item Env:\COLCON_CURRENT_PREFIX

install/ros_viz_adapter/share/ros_viz_adapter/package.sh

@@ -0,0 +1,87 @@
+# generated from colcon_core/shell/template/package.sh.em
+
+# This script extends the environment for this package.
+
+# function to prepend a value to a variable
+# which uses colons as separators
+# duplicates as well as trailing separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+_colcon_prepend_unique_value() {
+  # arguments
+  _listname="$1"
+  _value="$2"
+
+  # get values from variable
+  eval _values=\"\$$_listname\"
+  # backup the field separator
+  _colcon_prepend_unique_value_IFS=$IFS
+  IFS=":"
+  # start with the new value
+  _all_values="$_value"
+  # workaround SH_WORD_SPLIT not being set in zsh
+  if [ "$(command -v colcon_zsh_convert_to_array)" ]; then
+    colcon_zsh_convert_to_array _values
+  fi
+  # iterate over existing values in the variable
+  for _item in $_values; do
+    # ignore empty strings
+    if [ -z "$_item" ]; then
+      continue
+    fi
+    # ignore duplicates of _value
+    if [ "$_item" = "$_value" ]; then
+      continue
+    fi
+    # keep non-duplicate values
+    _all_values="$_all_values:$_item"
+  done
+  unset _item
+  # restore the field separator
+  IFS=$_colcon_prepend_unique_value_IFS
+  unset _colcon_prepend_unique_value_IFS
+  # export the updated variable
+  eval export $_listname=\"$_all_values\"
+  unset _all_values
+  unset _values
+
+  unset _value
+  unset _listname
+}
+
+# since a plain shell script can't determine its own path when being sourced
+# either use the provided COLCON_CURRENT_PREFIX
+# or fall back to the build time prefix (if it exists)
+_colcon_package_sh_COLCON_CURRENT_PREFIX="/workspace/install/ros_viz_adapter"
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+  if [ ! -d "$_colcon_package_sh_COLCON_CURRENT_PREFIX" ]; then
+    echo "The build time path \"$_colcon_package_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2
+    unset _colcon_package_sh_COLCON_CURRENT_PREFIX
+    return 1
+  fi
+  COLCON_CURRENT_PREFIX="$_colcon_package_sh_COLCON_CURRENT_PREFIX"
+fi
+unset _colcon_package_sh_COLCON_CURRENT_PREFIX
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+_colcon_package_sh_source_script() {
+  if [ -f "$1" ]; then
+    if [ -n "$COLCON_TRACE" ]; then
+      echo "# . \"$1\""
+    fi
+    . "$@"
+  else
+    echo "not found: \"$1\"" 1>&2
+  fi
+}
+
+# source sh hooks
+_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/ros_viz_adapter/hook/cmake_prefix_path.sh"
+_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/ros_viz_adapter/local_setup.sh"
+
+unset _colcon_package_sh_source_script
+unset COLCON_CURRENT_PREFIX
+
+# do not unset _colcon_prepend_unique_value since it might be used by non-primary shell hooks

install/ros_viz_adapter/share/ros_viz_adapter/package.xml

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>ros_viz_adapter</name>
+  <version>0.2.0</version>
+  <description>Rate-limited ROS 2 visualization publishers for images and point clouds.</description>
+  <maintainer email="platform@example.com">Platform Team</maintainer>
+  <license>Apache-2.0</license>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>image_transport</depend>
+  <depend>pluginlib</depend>
+  <exec_depend>compressed_image_transport</exec_depend>
+  <exec_depend>compressed_depth_image_transport</exec_depend>
+  <export><build_type>ament_cmake</build_type></export>
+</package>

install/ros_viz_adapter/share/ros_viz_adapter/package.zsh

@@ -0,0 +1,50 @@
+# generated from colcon_zsh/shell/template/package.zsh.em
+
+# This script extends the environment for this package.
+
+# a zsh script is able to determine its own path if necessary
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+  # the prefix is two levels up from the package specific share directory
+  _colcon_package_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`/../.." > /dev/null && pwd)"
+else
+  _colcon_package_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
+fi
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+_colcon_package_zsh_source_script() {
+  if [ -f "$1" ]; then
+    if [ -n "$COLCON_TRACE" ]; then
+      echo "# . \"$1\""
+    fi
+    . "$@"
+  else
+    echo "not found: \"$1\"" 1>&2
+  fi
+}
+
+# function to convert array-like strings into arrays
+# to workaround SH_WORD_SPLIT not being set
+colcon_zsh_convert_to_array() {
+  local _listname=$1
+  local _dollar="$"
+  local _split="{="
+  local _to_array="(\"$_dollar$_split$_listname}\")"
+  eval $_listname=$_to_array
+}
+
+# source sh script of this package
+_colcon_package_zsh_source_script "$_colcon_package_zsh_COLCON_CURRENT_PREFIX/share/ros_viz_adapter/package.sh"
+unset convert_zsh_to_array
+
+# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced scripts
+COLCON_CURRENT_PREFIX="$_colcon_package_zsh_COLCON_CURRENT_PREFIX"
+
+# source zsh hooks
+_colcon_package_zsh_source_script "$COLCON_CURRENT_PREFIX/share/ros_viz_adapter/local_setup.zsh"
+
+unset COLCON_CURRENT_PREFIX
+
+unset _colcon_package_zsh_source_script
+unset _colcon_package_zsh_COLCON_CURRENT_PREFIX

src/bcr_bot/launch/bcr_bot_gazebo_spawn.launch.py

@@ -25,7 +25,7 @@ def generate_launch_description():
     position_y = LaunchConfiguration("position_y")
     orientation_yaw = LaunchConfiguration("orientation_yaw")
     camera_enabled = LaunchConfiguration("camera_enabled", default=True)
-    stereo_camera_enabled = LaunchConfiguration("stereo_camera_enabled", default=False)
+    stereo_camera_enabled = LaunchConfiguration("stereo_camera_enabled", default=True)
     two_d_lidar_enabled = LaunchConfiguration("two_d_lidar_enabled", default=True)
     odometry_source = LaunchConfiguration("odometry_source", default="world")
     robot_namespace = LaunchConfiguration("robot_namespace", default='bcr_bot')

src/bcr_bot/launch/bcr_bot_gz_spawn.launch.py

@@ -23,7 +23,7 @@ def generate_launch_description():
     position_y = LaunchConfiguration("position_y")
     orientation_yaw = LaunchConfiguration("orientation_yaw")
     camera_enabled = LaunchConfiguration("camera_enabled", default=True)
-    stereo_camera_enabled = LaunchConfiguration("stereo_camera_enabled", default=False)
+    stereo_camera_enabled = LaunchConfiguration("stereo_camera_enabled", default=True)
     two_d_lidar_enabled = LaunchConfiguration("two_d_lidar_enabled", default=True)
     odometry_source = LaunchConfiguration("odometry_source")
 
@@ -126,4 +126,4 @@ def generate_launch_description():
         DeclareLaunchArgument("odometry_source", default_value="world"),
         robot_state_publisher,
         gz_spawn_entity, transform_publisher, gz_ros2_bridge
-    ])
+    ])

src/bcr_bot/launch/bcr_bot_ign_spawn.launch.py

@@ -23,7 +23,7 @@ def generate_launch_description():
     position_y = LaunchConfiguration("position_y")
     orientation_yaw = LaunchConfiguration("orientation_yaw")
     camera_enabled = LaunchConfiguration("camera_enabled", default=True)
-    stereo_camera_enabled = LaunchConfiguration("stereo_camera_enabled", default=False)
+    stereo_camera_enabled = LaunchConfiguration("stereo_camera_enabled", default=True)
     two_d_lidar_enabled = LaunchConfiguration("two_d_lidar_enabled", default=True)
     odometry_source = LaunchConfiguration("odometry_source")
 

src/bcr_bot/urdf/bcr_bot.xacro

@@ -38,7 +38,7 @@
     <xacro:arg name="robot_namespace" default=""/>
     <xacro:arg name="wheel_odom_topic" default="odom" />
     <xacro:arg name="camera_enabled" default="false" />
-    <xacro:arg name="stereo_camera_enabled" default="false" />
+    <xacro:arg name="stereo_camera_enabled" default="true" />
     <xacro:arg name="two_d_lidar_enabled" default="false" />
     <xacro:arg name="publish_wheel_odom_tf" default="true" />
     <xacro:arg name="conveyor_enabled" default="false"/>

src/bcr_bot/urdf/gz.xacro

@@ -87,7 +87,7 @@
     <xacro:if value="$(arg camera_enabled)">
         <gazebo reference="kinect_camera">
             <sensor type="depth_camera" name="kinect_camera">
-                <update_rate>30.0</update_rate>
+                <update_rate>10.0</update_rate>
                 <topic>kinect_camera</topic>
                 <gz_frame_id>kinect_camera</gz_frame_id>
                 <camera>

src/ros_viz_adapter/CMakeLists.txt

@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 3.8)
+project(ros_viz_adapter)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+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 pluginlib)
+install(TARGETS adapter DESTINATION lib/${PROJECT_NAME})
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+install(FILES README.md DESTINATION share/${PROJECT_NAME})
+ament_package()

src/ros_viz_adapter/README.md

@@ -0,0 +1,110 @@
+# ros_viz_adapter
+
+通用 ROS 2 可视化发布适配器，不需要用户修改原始话题名称，也不需要
+YAML 配置文件。节点自动发现 `sensor_msgs/msg/Image` 和
+`sensor_msgs/msg/PointCloud2`，并把结果发布到 `/viz` 命名空间。
+
+## 输出
+
+- RGB：直接加载 `image_transport/compressed_pub`，插件发布
+  `/viz/<source>/rgb/compressed`，不创建 raw base topic。
+- 深度：直接加载 `image_transport/compressedDepth_pub`，插件发布
+  `/viz/<source>/depth/compressedDepth`，不创建 raw base topic。
+- 点云：`/viz/<source>/points`，消息仍为 `sensor_msgs/msg/PointCloud2`，但只保留
+  `x/y/z`，去除 NaN，执行体素降采样、最大点数限制和限频。
+
+图像依据 `encoding` 分类：`rgb8/bgr8/rgba8/bgra8/mono8` 为 RGB，
+`16UC1/32FC1/mono16/32SC1` 为深度。RGB、深度、点云各使用一个独立处理线程；
+不存在对应数据类型时，不创建该线程。
+
+## 运行
+
+```bash
+colcon build --packages-select ros_viz_adapter --symlink-install
+ros2 launch ros_viz_adapter adapter.launch.py
+```
+
+点云可视化示例：
+
+```bash
+ros2 launch ros_viz_adapter adapter.launch.py \
+  input_fps_limit:=10 \
+  output_fps:=5 \
+  voxel_size_m:=0.10 \
+  max_points:=50000
+```
+
+参数：
+
+```text
+output_namespace      默认 /viz
+input_fps_limit       默认 30，回调入口丢弃超频帧
+output_fps            默认 10，处理线程输出上限
+jpeg_quality          默认 80，范围 1-100
+voxel_size_m          默认 0.05
+max_points            默认 100000
+```
+
+调参规律：
+input_fps_limit 越低，适配器接收和处理次数越少；
+output_fps 越低，平台网络和渲染负载越低；
+voxel_size_m 越大，点越少、处理越快，但细节损失越明显，=0 可以关闭体素降采样，但通常不建议这么做；
+max_points 越小，输出消息越小，但点云显示更稀疏；
+input_fps_limit:=0 或 output_fps:=0 表示不限制频率。
+
+QoS 为 `BEST_EFFORT + KEEP_LAST + depth=1`。限频降低的是适配器处理、网络和
+平台负载；如果还要降低源容器 CPU，应同时调整相机/上游发布频率。
+
+## 在其他项目中接入
+
+### 同一个 colcon 工作空间
+
+把本包目录放到项目工作空间的 `src/` 下，然后正常构建：
+
+```bash
+cd <workspace>
+colcon build --symlink-install
+source install/setup.bash
+ros2 launch ros_viz_adapter adapter.launch.py
+```
+
+不需要修改原始 RGB、深度或点云节点。适配器会在同一个 ROS domain 中发现传感器话题，
+并发布 `/viz/...`。
+
+### 在项目 launch 中启动
+
+也可以在项目自己的 launch 文件中包含适配器：
+
+```python
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.substitutions import FindPackageShare
+
+viz_adapter = IncludeLaunchDescription(
+    PythonLaunchDescriptionSource([
+        FindPackageShare('ros_viz_adapter'), '/launch/adapter.launch.py'
+    ])
+)
+```
+
+将 `viz_adapter` 放入项目的 `LaunchDescription` 即可。也可以直接运行：
+
+```bash
+ros2 run ros_viz_adapter adapter
+```
+
+### 独立 ROS 容器或 sidecar
+
+适配器容器与用户容器必须使用相同的 `ROS_DOMAIN_ID`、`RMW_IMPLEMENTATION` 和可互相发现的
+ROS 2 网络。适配器容器只需安装本包、`compressed_image_transport` 和
+`compressed_depth_image_transport`，不需要复制用户项目代码。代码只加载上述两个
+指定插件，不遍历或加载 Theora 插件。
+
+### 多容器隔离
+
+为避免不同容器输出重名，可以指定不同 namespace：
+
+```bash
+ros2 launch ros_viz_adapter adapter.launch.py \
+  output_namespace:=/viz/robot_01
+```

src/ros_viz_adapter/launch/adapter.launch.py

@@ -0,0 +1,25 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument('output_namespace', default_value='/viz'),
+        DeclareLaunchArgument('input_fps_limit', default_value='30.0'),
+        DeclareLaunchArgument('output_fps', default_value='10.0'),
+        DeclareLaunchArgument('voxel_size_m', default_value='0.05'),
+        DeclareLaunchArgument('max_points', default_value='100000'),
+        Node(
+            package='ros_viz_adapter', executable='adapter',
+            name='ros_viz_adapter', output='screen',
+            parameters=[{
+                'output_namespace': LaunchConfiguration('output_namespace'),
+                'input_fps_limit': LaunchConfiguration('input_fps_limit'),
+                'output_fps': LaunchConfiguration('output_fps'),
+                'voxel_size_m': LaunchConfiguration('voxel_size_m'),
+                'max_points': LaunchConfiguration('max_points'),
+            }],
+        ),
+    ])

src/ros_viz_adapter/package.xml

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>ros_viz_adapter</name>
+  <version>0.2.0</version>
+  <description>Rate-limited ROS 2 visualization publishers for images and point clouds.</description>
+  <maintainer email="platform@example.com">Platform Team</maintainer>
+  <license>Apache-2.0</license>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>image_transport</depend>
+  <depend>pluginlib</depend>
+  <exec_depend>compressed_image_transport</exec_depend>
+  <exec_depend>compressed_depth_image_transport</exec_depend>
+  <export><build_type>ament_cmake</build_type></export>
+</package>

src/ros_viz_adapter/src/adapter.cpp

@@ -0,0 +1,207 @@
+#include <algorithm>
+#include <atomic>
+#include <chrono>
+#include <cmath>
+#include <condition_variable>
+#include <cstring>
+#include <mutex>
+#include <regex>
+#include <string>
+#include <thread>
+#include <unordered_map>
+#include <unordered_set>
+#include <vector>
+
+#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"
+#include "sensor_msgs/msg/point_field.hpp"
+
+using Image = sensor_msgs::msg::Image;
+using PointCloud2 = sensor_msgs::msg::PointCloud2;
+
+class Adapter final : public rclcpp::Node {
+public:
+  Adapter() : Node("ros_viz_adapter") {
+    output_ns_ = declare_parameter<std::string>("output_namespace", "/viz");
+    input_fps_ = std::max(0.0, declare_parameter<double>("input_fps_limit", 30.0));
+    output_fps_ = std::max(0.0, declare_parameter<double>("output_fps", 10.0));
+    voxel_ = std::max(0.0, declare_parameter<double>("voxel_size_m", 0.05));
+    max_points_ = std::max<int>(1, declare_parameter<int>("max_points", 100000));
+    qos_ = rclcpp::SensorDataQoS().keep_last(1);
+    discover();
+    timer_ = create_wall_timer(std::chrono::seconds(2), [this] { discover(); });
+  }
+
+  ~Adapter() override {
+    stop_ = true; wake_.notify_all();
+    if (rgb_thread_.joinable()) rgb_thread_.join();
+    if (depth_thread_.joinable()) depth_thread_.join();
+    if (cloud_thread_.joinable()) cloud_thread_.join();
+  }
+
+private:
+  enum class ImageKind { RGB, DEPTH, UNKNOWN };
+
+  static std::string safe_name(const std::string & topic) {
+    auto name = std::regex_replace(topic, std::regex("[^A-Za-z0-9_]+"), "_");
+    while (!name.empty() && name.front() == '_') name.erase(name.begin());
+    while (!name.empty() && name.back() == '_') name.pop_back();
+    return name.empty() ? "stream" : name;
+  }
+
+  static ImageKind classify(const std::string & encoding) {
+    std::string e = encoding;
+    std::transform(e.begin(), e.end(), e.begin(), [](unsigned char c) { return std::tolower(c); });
+    if (e == "16uc1" || e == "32fc1" || e == "mono16" || e == "32sc1") return ImageKind::DEPTH;
+    if (e == "rgb8" || e == "bgr8" || e == "rgba8" || e == "bgra8" ||
+        e == "mono8" || e == "8uc3" || e == "8uc4" || e == "r8g8b8" ||
+        e == "b8g8r8" || e == "rgb_int8" || e == "rgba_int8" ||
+        e == "bgr_int8" || e == "bgra_int8") return ImageKind::RGB;
+    return ImageKind::UNKNOWN;
+  }
+
+  bool due(const std::string & key, double fps, bool output) {
+    if (fps <= 0.0) return true;
+    std::lock_guard<std::mutex> lock(rate_mutex_);
+    auto & table = output ? output_times_ : input_times_;
+    const auto now = std::chrono::steady_clock::now();
+    auto it = table.find(key);
+    if (it != table.end() && std::chrono::duration<double>(now - it->second).count() < 1.0 / fps) return false;
+    table[key] = now; return true;
+  }
+
+  void discover() {
+    for (const auto & entry : get_topic_names_and_types()) {
+      const auto & topic = entry.first;
+      if (topic == output_ns_ || topic.rfind(output_ns_ + "/", 0) == 0) continue;
+      if (subscriptions_.count(topic) || cloud_subscriptions_.count(topic)) continue;
+      const auto & types = entry.second;
+      if (std::find(types.begin(), types.end(), "sensor_msgs/msg/Image") != types.end()) {
+        RCLCPP_INFO(get_logger(), "Discovered image topic: %s", topic.c_str());
+        subscriptions_[topic] = create_subscription<Image>(topic, qos_, [this, topic](Image::ConstSharedPtr msg) { on_image(topic, msg); });
+      } else if (std::find(types.begin(), types.end(), "sensor_msgs/msg/PointCloud2") != types.end()) {
+        cloud_subscriptions_[topic] = create_subscription<PointCloud2>(topic, qos_, [this, topic](PointCloud2::ConstSharedPtr msg) { on_cloud(topic, msg); });
+      }
+    }
+  }
+
+  void on_image(const std::string & topic, Image::ConstSharedPtr msg) {
+    if (!due(topic, input_fps_, false)) return;
+    const auto kind = classify(msg->encoding);
+    if (kind == ImageKind::UNKNOWN) {
+      RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "Ignoring image topic %s with encoding '%s'", topic.c_str(), msg->encoding.c_str());
+      return;
+    }
+    RCLCPP_INFO_ONCE(get_logger(), "Classified image topic %s as %s (encoding=%s)", topic.c_str(), kind == ImageKind::RGB ? "RGB" : "DEPTH", msg->encoding.c_str());
+    std::lock_guard<std::mutex> lock(data_mutex_);
+    auto & slot = kind == ImageKind::RGB ? rgb_latest_ : depth_latest_;
+    slot[topic] = msg;
+    if (kind == ImageKind::RGB && !rgb_thread_.joinable()) rgb_thread_ = std::thread([this] { image_loop(rgb_latest_, false); });
+    if (kind == ImageKind::DEPTH && !depth_thread_.joinable()) depth_thread_ = std::thread([this] { image_loop(depth_latest_, true); });
+    wake_.notify_all();
+  }
+
+  void on_cloud(const std::string & topic, PointCloud2::ConstSharedPtr msg) {
+    if (!due(topic, input_fps_, false)) return;
+    std::lock_guard<std::mutex> lock(data_mutex_);
+    cloud_latest_[topic] = msg;
+    if (!cloud_thread_.joinable()) cloud_thread_ = std::thread([this] { cloud_loop(); });
+    wake_.notify_all();
+  }
+
+  std::string image_topic(const std::string & topic, bool depth) const {
+    // Publisher plugins append their own transport suffix.
+    return (output_ns_.empty() ? "/viz" : output_ns_) + "/" + safe_name(topic) + (depth ? "/depth" : "/rgb");
+  }
+
+  void image_loop(std::unordered_map<std::string, Image::ConstSharedPtr> & slots, bool depth) {
+    while (!stop_) {
+      std::unordered_map<std::string, Image::ConstSharedPtr> pending;
+      {
+        std::unique_lock<std::mutex> lock(data_mutex_);
+        wake_.wait_for(lock, std::chrono::milliseconds(100), [this, &slots] { return stop_ || !slots.empty(); });
+        pending.swap(slots);
+      }
+      for (const auto & item : pending) {
+        const auto key = item.first + (depth ? ":depth" : ":rgb");
+        if (!due(key, output_fps_, true)) continue;
+        std::lock_guard<std::mutex> lock(pub_mutex_);
+        auto it = image_publishers_.find(key);
+        if (it == image_publishers_.end()) {
+          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());
+            RCLCPP_INFO(get_logger(), "Loaded %s -> %s", lookup, plugin->getTopic().c_str());
+            it = image_publishers_.emplace(key, std::move(plugin)).first;
+          } catch (const pluginlib::PluginlibException & ex) {
+            RCLCPP_ERROR(get_logger(), "Cannot load image_transport plugin %s for %s: %s", lookup, item.first.c_str(), ex.what());
+            continue;
+          }
+        }
+        it->second->publishPtr(item.second);
+      }
+    }
+  }
+
+  void cloud_loop() {
+    while (!stop_) {
+      std::unordered_map<std::string, PointCloud2::ConstSharedPtr> pending;
+      {
+        std::unique_lock<std::mutex> lock(data_mutex_);
+        wake_.wait_for(lock, std::chrono::milliseconds(100), [this] { return stop_ || !cloud_latest_.empty(); });
+        pending.swap(cloud_latest_);
+      }
+      for (const auto & item : pending) {
+        if (!due(item.first + ":points", output_fps_, true)) continue;
+        auto output = downsample(*item.second);
+        std::lock_guard<std::mutex> lock(pub_mutex_);
+        auto it = cloud_publishers_.find(item.first);
+        if (it == cloud_publishers_.end()) {
+          it = cloud_publishers_.emplace(item.first, create_publisher<PointCloud2>(output_ns_ + "/" + safe_name(item.first) + "/points", qos_)).first;
+        }
+        it->second->publish(output);
+      }
+    }
+  }
+
+  PointCloud2 downsample(const PointCloud2 & input) const {
+    int xoff = -1, yoff = -1, zoff = -1;
+    for (const auto & field : input.fields) {
+      if (field.name == "x") xoff = field.offset;
+      if (field.name == "y") yoff = field.offset;
+      if (field.name == "z") zoff = field.offset;
+    }
+    PointCloud2 output; output.header = input.header; output.height = 1; output.point_step = 12; output.is_dense = true;
+    sensor_msgs::msg::PointField fx; fx.name = "x"; fx.offset = 0; fx.datatype = sensor_msgs::msg::PointField::FLOAT32; fx.count = 1;
+    sensor_msgs::msg::PointField fy; fy.name = "y"; fy.offset = 4; fy.datatype = sensor_msgs::msg::PointField::FLOAT32; fy.count = 1;
+    sensor_msgs::msg::PointField fz; fz.name = "z"; fz.offset = 8; fz.datatype = sensor_msgs::msg::PointField::FLOAT32; fz.count = 1;
+    output.fields = {fx, fy, fz};
+    if (xoff < 0 || yoff < 0 || zoff < 0 || input.point_step == 0) return output;
+    std::unordered_set<std::string> voxels;
+    for (size_t i = 0; i < input.width * input.height && output.width < static_cast<uint32_t>(max_points_); ++i) {
+      const auto * raw = input.data.data() + i * input.point_step; float xyz[3];
+      std::memcpy(&xyz[0], raw + xoff, 4); std::memcpy(&xyz[1], raw + yoff, 4); std::memcpy(&xyz[2], raw + zoff, 4);
+      if (!std::isfinite(xyz[0]) || !std::isfinite(xyz[1]) || !std::isfinite(xyz[2])) continue;
+      if (voxel_ > 0.0) { const auto key = std::to_string(static_cast<long long>(std::floor(xyz[0] / voxel_))) + ":" + std::to_string(static_cast<long long>(std::floor(xyz[1] / voxel_))) + ":" + std::to_string(static_cast<long long>(std::floor(xyz[2] / voxel_))); if (!voxels.insert(key).second) continue; }
+      const auto old = output.data.size(); output.data.resize(old + 12); std::memcpy(output.data.data() + old, xyz, 12); ++output.width;
+    }
+    output.row_step = output.width * output.point_step; return output;
+  }
+
+  std::string output_ns_; double input_fps_{30.0}, output_fps_{10.0}, voxel_{0.05}; int max_points_{100000};
+  std::shared_ptr<pluginlib::ClassLoader<image_transport::PublisherPlugin>> image_loader_{
+    std::make_shared<pluginlib::ClassLoader<image_transport::PublisherPlugin>>("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<std::string, rclcpp::Subscription<Image>::SharedPtr> subscriptions_; std::unordered_map<std::string, rclcpp::Subscription<PointCloud2>::SharedPtr> cloud_subscriptions_;
+  std::unordered_map<std::string, Image::ConstSharedPtr> rgb_latest_, depth_latest_; std::unordered_map<std::string, PointCloud2::ConstSharedPtr> cloud_latest_;
+  std::unordered_map<std::string, std::chrono::steady_clock::time_point> input_times_, output_times_;
+  std::unordered_map<std::string, std::shared_ptr<image_transport::PublisherPlugin>> image_publishers_; std::unordered_map<std::string, rclcpp::Publisher<PointCloud2>::SharedPtr> cloud_publishers_;
+};
+
+int main(int argc, char ** argv) { rclcpp::init(argc, argv); auto node = std::make_shared<Adapter>(); rclcpp::spin(node); rclcpp::shutdown(); return 0; }