Compare commits
No commits in common. "main" and "main" have entirely different histories.
|
|
@ -193,12 +193,13 @@ def process_dsv_file(
|
|||
):
|
||||
commands = []
|
||||
if _include_comments():
|
||||
commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path}))
|
||||
commands.append(FORMAT_STR_COMMENT_LINE.format_map({
|
||||
'comment': dsv_path}))
|
||||
with open(dsv_path, 'r') as h:
|
||||
content = h.read()
|
||||
lines = content.splitlines()
|
||||
|
||||
basenames = OrderedDict()
|
||||
basename_map = OrderedDict()
|
||||
for i, line in enumerate(lines):
|
||||
# skip over empty or whitespace-only lines
|
||||
if not line.strip():
|
||||
|
|
@ -223,21 +224,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 basenames:
|
||||
basenames[path_without_ext] = set()
|
||||
if path_without_ext not in basename_map:
|
||||
basename_map[path_without_ext] = set()
|
||||
assert ext.startswith('.')
|
||||
ext = ext[1:]
|
||||
if ext in (primary_extension, additional_extension):
|
||||
basenames[path_without_ext].add(ext)
|
||||
basename_map[path_without_ext].add(ext)
|
||||
|
||||
# add the dsv extension to each basename if the file exists
|
||||
for basename, extensions in basenames.items():
|
||||
for basename, extensions in basename_map.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 basenames.items():
|
||||
for basename, extensions in basename_map.items():
|
||||
if not os.path.isabs(basename):
|
||||
basename = os.path.join(prefix, basename)
|
||||
if 'dsv' in extensions:
|
||||
|
|
@ -304,8 +305,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({'comment': comment}))
|
||||
commands.append(FORMAT_STR_COMMENT_LINE.format_map({
|
||||
'comment': comment}))
|
||||
elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE:
|
||||
commands += _append_unique_value(env_name, value)
|
||||
else:
|
||||
|
|
@ -320,15 +321,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 sets the
|
||||
# same variable without the knowledge of this Python script.
|
||||
# later _remove_ending_separators() will cleanup any unintentional leading separator
|
||||
# Append even if the variable has not been set yet, in case a shell script
|
||||
# sets the same variable without the knowledge of this Python script.
|
||||
# Later, _remove_ending_separators() will cleanup any unintentional
|
||||
# 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})
|
||||
|
|
@ -342,15 +343,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 sets the
|
||||
# same variable without the knowledge of this Python script.
|
||||
# later _remove_ending_separators() will cleanup any unintentional trailing separator
|
||||
# Prepend even if the variable has not been set yet, in case a shell script
|
||||
# sets the same variable without the knowledge of this Python script.
|
||||
# Later, _remove_ending_separators() will cleanup any unintentional
|
||||
# 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})
|
||||
|
|
@ -369,10 +370,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 prepending
|
||||
# skip variables that already had values before this script started
|
||||
# appending/prepending
|
||||
if name in os.environ:
|
||||
continue
|
||||
commands += [
|
||||
|
|
@ -382,7 +383,6 @@ 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,7 +390,6 @@ 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)):
|
||||
|
|
|
|||
|
|
@ -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 = 'if [ "$(echo -n ${name} | head -c 1)" = ":" ]; then export {name}=${{{name}#?}} ; fi' # noqa: E501
|
||||
FORMAT_STR_REMOVE_TRAILING_SEPARATOR = 'if [ "$(echo -n ${name} | tail -c 1)" = ":" ]; then export {name}=${{{name}%?}} ; fi' # noqa: E501
|
||||
FORMAT_STR_REMOVE_LEADING_SEPARATOR = 'export {name}=${{{name}#:}}' # noqa: E501
|
||||
FORMAT_STR_REMOVE_TRAILING_SEPARATOR = 'export {name}=${{{name}%:}}' # noqa: E501
|
||||
|
||||
DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate'
|
||||
DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate'
|
||||
|
|
@ -193,12 +193,13 @@ def process_dsv_file(
|
|||
):
|
||||
commands = []
|
||||
if _include_comments():
|
||||
commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path}))
|
||||
commands.append(FORMAT_STR_COMMENT_LINE.format_map({
|
||||
'comment': dsv_path}))
|
||||
with open(dsv_path, 'r') as h:
|
||||
content = h.read()
|
||||
lines = content.splitlines()
|
||||
|
||||
basenames = OrderedDict()
|
||||
basename_map = OrderedDict()
|
||||
for i, line in enumerate(lines):
|
||||
# skip over empty or whitespace-only lines
|
||||
if not line.strip():
|
||||
|
|
@ -223,21 +224,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 basenames:
|
||||
basenames[path_without_ext] = set()
|
||||
if path_without_ext not in basename_map:
|
||||
basename_map[path_without_ext] = set()
|
||||
assert ext.startswith('.')
|
||||
ext = ext[1:]
|
||||
if ext in (primary_extension, additional_extension):
|
||||
basenames[path_without_ext].add(ext)
|
||||
basename_map[path_without_ext].add(ext)
|
||||
|
||||
# add the dsv extension to each basename if the file exists
|
||||
for basename, extensions in basenames.items():
|
||||
for basename, extensions in basename_map.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 basenames.items():
|
||||
for basename, extensions in basename_map.items():
|
||||
if not os.path.isabs(basename):
|
||||
basename = os.path.join(prefix, basename)
|
||||
if 'dsv' in extensions:
|
||||
|
|
@ -304,8 +305,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({'comment': comment}))
|
||||
commands.append(FORMAT_STR_COMMENT_LINE.format_map({
|
||||
'comment': comment}))
|
||||
elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE:
|
||||
commands += _append_unique_value(env_name, value)
|
||||
else:
|
||||
|
|
@ -320,15 +321,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 sets the
|
||||
# same variable without the knowledge of this Python script.
|
||||
# later _remove_ending_separators() will cleanup any unintentional leading separator
|
||||
# Append even if the variable has not been set yet, in case a shell script
|
||||
# sets the same variable without the knowledge of this Python script.
|
||||
# Later, _remove_ending_separators() will cleanup any unintentional
|
||||
# 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})
|
||||
|
|
@ -342,15 +343,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 sets the
|
||||
# same variable without the knowledge of this Python script.
|
||||
# later _remove_ending_separators() will cleanup any unintentional trailing separator
|
||||
# Prepend even if the variable has not been set yet, in case a shell script
|
||||
# sets the same variable without the knowledge of this Python script.
|
||||
# Later, _remove_ending_separators() will cleanup any unintentional
|
||||
# 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})
|
||||
|
|
@ -369,10 +370,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 prepending
|
||||
# skip variables that already had values before this script started
|
||||
# appending/prepending
|
||||
if name in os.environ:
|
||||
continue
|
||||
commands += [
|
||||
|
|
@ -382,7 +383,6 @@ 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,7 +390,6 @@ 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)):
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
/workspace/install/ros_viz_adapter:/workspace/install/bcr_bot:/opt/ros/humble
|
||||
/workspace/install/bcr_bot:/opt/ros/humble
|
||||
|
|
@ -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=True)
|
||||
stereo_camera_enabled = LaunchConfiguration("stereo_camera_enabled", default=False)
|
||||
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')
|
||||
|
|
|
|||
|
|
@ -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=True)
|
||||
stereo_camera_enabled = LaunchConfiguration("stereo_camera_enabled", default=False)
|
||||
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
|
||||
])
|
||||
])
|
||||
|
|
@ -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=True)
|
||||
stereo_camera_enabled = LaunchConfiguration("stereo_camera_enabled", default=False)
|
||||
two_d_lidar_enabled = LaunchConfiguration("two_d_lidar_enabled", default=True)
|
||||
odometry_source = LaunchConfiguration("odometry_source")
|
||||
|
||||
|
|
|
|||
|
|
@ -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="true" />
|
||||
<xacro:arg name="stereo_camera_enabled" default="false" />
|
||||
<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"/>
|
||||
|
|
|
|||
|
|
@ -87,7 +87,7 @@
|
|||
<xacro:if value="$(arg camera_enabled)">
|
||||
<gazebo reference="kinect_camera">
|
||||
<sensor type="depth_camera" name="kinect_camera">
|
||||
<update_rate>10.0</update_rate>
|
||||
<update_rate>30.0</update_rate>
|
||||
<topic>kinect_camera</topic>
|
||||
<gz_frame_id>kinect_camera</gz_frame_id>
|
||||
<camera>
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -1 +0,0 @@
|
|||
rclcpp;sensor_msgs;image_transport;pluginlib;compressed_image_transport;compressed_depth_image_transport
|
||||
|
|
@ -1 +0,0 @@
|
|||
/workspace/install/ros_viz_adapter:/workspace/install/bcr_bot:/opt/ros/humble
|
||||
|
|
@ -1 +0,0 @@
|
|||
compressed_depth_image_transport:compressed_image_transport:image_transport:pluginlib:rclcpp:sensor_msgs
|
||||
|
|
@ -1,110 +0,0 @@
|
|||
# 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
|
||||
```
|
||||
|
|
@ -1,14 +0,0 @@
|
|||
# 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()
|
||||
|
|
@ -1,42 +0,0 @@
|
|||
# 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()
|
||||
|
|
@ -1 +0,0 @@
|
|||
prepend-non-duplicate;AMENT_PREFIX_PATH;
|
||||
|
|
@ -1,4 +0,0 @@
|
|||
# copied from
|
||||
# ament_cmake_core/cmake/environment_hooks/environment/ament_prefix_path.sh
|
||||
|
||||
ament_prepend_unique_value AMENT_PREFIX_PATH "$AMENT_CURRENT_PREFIX"
|
||||
|
|
@ -1 +0,0 @@
|
|||
prepend-non-duplicate-if-exists;PATH;bin
|
||||
|
|
@ -1,5 +0,0 @@
|
|||
# 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
|
||||
|
|
@ -1 +0,0 @@
|
|||
prepend-non-duplicate;CMAKE_PREFIX_PATH;
|
||||
|
|
@ -1,3 +0,0 @@
|
|||
# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
|
||||
|
||||
colcon_prepend_unique_value CMAKE_PREFIX_PATH "$env:COLCON_CURRENT_PREFIX"
|
||||
|
|
@ -1,3 +0,0 @@
|
|||
# generated from colcon_core/shell/template/hook_prepend_value.sh.em
|
||||
|
||||
_colcon_prepend_unique_value CMAKE_PREFIX_PATH "$COLCON_CURRENT_PREFIX"
|
||||
|
|
@ -1,25 +0,0 @@
|
|||
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'),
|
||||
}],
|
||||
),
|
||||
])
|
||||
|
|
@ -1,46 +0,0 @@
|
|||
# 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
|
||||
|
|
@ -1,2 +0,0 @@
|
|||
source;share/ros_viz_adapter/environment/ament_prefix_path.sh
|
||||
source;share/ros_viz_adapter/environment/path.sh
|
||||
|
|
@ -1,184 +0,0 @@
|
|||
# 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
|
||||
|
|
@ -1,59 +0,0 @@
|
|||
# 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
|
||||
|
|
@ -1,39 +0,0 @@
|
|||
# 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
|
||||
|
|
@ -1,8 +0,0 @@
|
|||
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
|
||||
|
|
@ -1,116 +0,0 @@
|
|||
# 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
|
||||
|
|
@ -1,87 +0,0 @@
|
|||
# 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
|
||||
|
|
@ -1,16 +0,0 @@
|
|||
<?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>
|
||||
|
|
@ -1,50 +0,0 @@
|
|||
# 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
|
||||
|
|
@ -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=True)
|
||||
stereo_camera_enabled = LaunchConfiguration("stereo_camera_enabled", default=False)
|
||||
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')
|
||||
|
|
|
|||
|
|
@ -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=True)
|
||||
stereo_camera_enabled = LaunchConfiguration("stereo_camera_enabled", default=False)
|
||||
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
|
||||
])
|
||||
])
|
||||
|
|
@ -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=True)
|
||||
stereo_camera_enabled = LaunchConfiguration("stereo_camera_enabled", default=False)
|
||||
two_d_lidar_enabled = LaunchConfiguration("two_d_lidar_enabled", default=True)
|
||||
odometry_source = LaunchConfiguration("odometry_source")
|
||||
|
||||
|
|
|
|||
|
|
@ -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="true" />
|
||||
<xacro:arg name="stereo_camera_enabled" default="false" />
|
||||
<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"/>
|
||||
|
|
|
|||
|
|
@ -87,7 +87,7 @@
|
|||
<xacro:if value="$(arg camera_enabled)">
|
||||
<gazebo reference="kinect_camera">
|
||||
<sensor type="depth_camera" name="kinect_camera">
|
||||
<update_rate>10.0</update_rate>
|
||||
<update_rate>30.0</update_rate>
|
||||
<topic>kinect_camera</topic>
|
||||
<gz_frame_id>kinect_camera</gz_frame_id>
|
||||
<camera>
|
||||
|
|
|
|||
|
|
@ -1,18 +0,0 @@
|
|||
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()
|
||||
|
|
@ -1,110 +0,0 @@
|
|||
# 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
|
||||
```
|
||||
|
|
@ -1,25 +0,0 @@
|
|||
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'),
|
||||
}],
|
||||
),
|
||||
])
|
||||
|
|
@ -1,16 +0,0 @@
|
|||
<?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>
|
||||
|
|
@ -1,207 +0,0 @@
|
|||
#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; }
|
||||
Loading…
Reference in New Issue