[real] 整理 ROS 2 v2 Odin 与站姿调参
This commit is contained in:
@@ -30,6 +30,7 @@ log/
|
||||
!05_software/real/sim2real/vendored/odin1_imu/build/libodin1_imu_bridge.so
|
||||
!05_software/real/sim2real/vendored/odin1_imu/lib/*.a
|
||||
!05_software/real/sim2real_v2/vendored/odin1_imu/lib/*.a
|
||||
!05_software/real/sim2real_ros2_v2/src/odin_ros_driver/lib/*.a
|
||||
|
||||
# Training outputs
|
||||
logs/
|
||||
|
||||
@@ -17,6 +17,7 @@
|
||||
| `v0.9.0` | Python Sim2Real v2 | 反馈新鲜度、Odin odom 诊断、Web 调试和安全监控增强 |
|
||||
| `v0.10.0` | ROS 2/C++ 初版 | 50 Hz C++ 推理、200 Hz CAN 热路径和 ROS 2 系统集成 |
|
||||
| `v0.11.0` | ROS 2 导航原型 | 简单导航、PCD 交互定位、任务点和 Web 导航调试 |
|
||||
| `v0.11.1` | Odin 与站姿调参 | 完整 Odin 驱动、TensorRT、多策略切换和调参站姿 |
|
||||
|
||||
> 原先临时归档为 `v0.9.0` 的最终 ROS 2/C++ 比赛部署已保存在 `backup/final-ros2-v0.9.0` 分支和 `backup-v0.9.0-ros2-final` 标签中，重排完成后将正式归入 `v1.0.0`。
|
||||
|
||||
@@ -43,6 +44,15 @@
|
||||
- 原始 `map1.pcd`、`map6.pcd` 分别约 49.05 MiB、44.24 MiB，归档时确定性抽样到 10 MB 以下并记录哈希。
|
||||
- 原始快照中的 Odin 驱动仍为空目录；排除设备运行日志和开发草稿。
|
||||
|
||||
## `v0.11.1` 的 Odin、TensorRT 与站姿调参
|
||||
|
||||
- 归档 `real/sim2real_ros2_v2(z=0.380 hip=0.670 knee=-1.390)`，在同一 `sim2real_ros2_v2` 目录中记录真实差异。
|
||||
- 首次随部署工程保留完整 Odin ROS 驱动、Apache-2.0 许可证、设备标定参数和预编译 SDK 静态库。
|
||||
- 策略运行时增加 TensorRT、ONNX 回退、Rough/Crawl 模式切换、事件日志和更完整的电机失效诊断。
|
||||
- 默认 Rough 策略为 `NEWmodel_1900`，默认站姿为髋俯仰 `0.670`、膝关节 `-1.390`；Crawl 配置使用 IK 后端。
|
||||
- `map_b.pcd` 从 1,080,047 点确定性抽样为 270,012 点，并保留原始和抽样哈希。
|
||||
- 排除嵌套 Git、Odin 运行日志、缓存、开发草稿和未被配置引用的候选策略。
|
||||
|
||||
## `v0.4.0` 的模型变化
|
||||
|
||||
- 机械 CAD 不变。
|
||||
|
||||
@@ -33,7 +33,7 @@ MJCF + mjlab task
|
||||
IK real --------------------------------> 电机
|
||||
```
|
||||
|
||||
`rc_mjlab` 是自包含工程。训练、MJCF、MuJoCo、Sim2Sim、导航工具和策略权重通过相对路径绑定，因此保留其内部布局，没有为了目录外观拆散。第一代完整闭环见 `v0.3.0`，第一份新版 MJCF 与训练框架见 `v0.4.0`，随机化增强版见 `v0.5.0`，比赛最终训练架构见 `v0.6.0`，后期 MuJoCo 工具集见 `v0.7.0`，后期 Sim2Sim 与比赛 Rough 策略见 `v0.8.0`，完整导航打点工具见 `v0.8.1`，Python Sim2Real v2 对应 `v0.9.0`，ROS 2/C++ 初版对应 `v0.10.0`，简单导航与 PCD 导航原型对应 `v0.11.0`。
|
||||
`rc_mjlab` 是自包含工程。训练、MJCF、MuJoCo、Sim2Sim、导航工具和策略权重通过相对路径绑定，因此保留其内部布局，没有为了目录外观拆散。第一代完整闭环见 `v0.3.0`，第一份新版 MJCF 与训练框架见 `v0.4.0`，随机化增强版见 `v0.5.0`，比赛最终训练架构见 `v0.6.0`，后期 MuJoCo 工具集见 `v0.7.0`，后期 Sim2Sim 与比赛 Rough 策略见 `v0.8.0`，完整导航打点工具见 `v0.8.1`，Python Sim2Real v2 对应 `v0.9.0`，ROS 2/C++ 初版对应 `v0.10.0`，简单导航原型对应 `v0.11.0`，完整 Odin/TensorRT 与站姿调参对应 `v0.11.1`。
|
||||
|
||||
详细说明见：
|
||||
|
||||
|
||||
@@ -36,7 +36,7 @@ ROS 2/C++ Sim2Real 初版，将策略热路径迁移为 50 Hz C++ 推理和 200
|
||||
|
||||
ROS 2 Sim2Real v2 导航原型，在初版基础上增加简单导航节点、PCD 交互定位、任务点/任务序列和 Web 导航调试。该版本对应重排主线的 `v0.11.0`。
|
||||
|
||||
本阶段的两份大体积 PCD 已确定性抽样，Odin 驱动仍为外部依赖；详细边界见 [`sim2real_ros2_v2/README.md`](sim2real_ros2_v2/README.md)。
|
||||
`v0.11.1` 在同一目录继续演进，首次随工程归档完整 Odin 驱动、TensorRT、多策略切换和硬件诊断，并使用 `hip=0.670`、`knee=-1.390` 的调参站姿。各 Tag 可恢复对应阶段，当前目录说明见 [`sim2real_ros2_v2/README.md`](sim2real_ros2_v2/README.md)。
|
||||
|
||||
## 实机记录
|
||||
|
||||
|
||||
@@ -62,6 +62,8 @@ COPY src/sim2real_nav2 sim2real_nav2
|
||||
# 拷贝策略文件与运行脚本
|
||||
WORKDIR /sim2real_ws
|
||||
COPY policies policies
|
||||
COPY map map
|
||||
COPY tools tools
|
||||
COPY start_sim2real.sh start_sim2real.sh
|
||||
RUN chmod +x start_sim2real.sh
|
||||
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
# ROS 2 Sim2Real v2
|
||||
# ROS 2 Sim2Real v2：Odin、TensorRT 与站姿调参
|
||||
|
||||
本目录归档 `real/sim2real_ros2_v2`，对应重排主线的 `v0.11.0`。该阶段在 ROS 2/C++ 初版基础上增加简单导航、PCD 交互定位、任务点/任务序列和 Web 导航调试。
|
||||
本目录在 `v0.11.0` 导航原型基础上归档 `real/sim2real_ros2_v2(z=0.380 hip=0.670 knee=-1.390)`，对应重排主线的 `v0.11.1`。虽然源目录以站姿参数命名，但该阶段同时首次完整加入 Odin 驱动、TensorRT、多策略切换、事件日志和更完整的硬件诊断。
|
||||
|
||||
本工程保留当前 `sim2real` 已验证的部署契约，同时将运行时热路径迁移到 C++：
|
||||
|
||||
@@ -70,5 +70,8 @@ Nav2 / cmd_vel ------------------------------> sim2real_runtime
|
||||
- Windows 仅作为编辑环境使用。
|
||||
- 观测顺序、动作缩放、默认站姿、电机映射不得独立修改，
|
||||
除非训练与部署同步更新。
|
||||
- 原始快照中的 `src/odin_ros_driver` 仍为空目录，本版本需要另行提供兼容驱动；完整 Odin 驱动从后续站姿调参版本开始随工程归档。
|
||||
- 原始 `map1.pcd` 和 `map6.pcd` 已确定性抽样到 10 MB 以下，点数和哈希见 [`map/README.md`](map/README.md)。
|
||||
- Rough 默认策略为 `NEWmodel_1900`，优先使用比赛机生成的 TensorRT engine；Crawl 保留 ONNX，但运行配置使用 IK 后端。
|
||||
- 默认 Rough 站姿为髋俯仰 `0.670`、膝关节 `-1.390`，源目录名中的 `z=0.380` 记录该阶段的机身高度配置意图。
|
||||
- `map_b.pcd` 已确定性抽样到 10 MB 以下，点数和哈希见 [`map/README.md`](map/README.md)。
|
||||
- TensorRT engine 与 JetPack、TensorRT 版本及 GPU 架构有关，其他机器应从 ONNX 重新生成。
|
||||
- 源配置引用的 Odin `map_a.bin` 不在该备份中；使用 Relocalization 前必须从设备或建图备份补回真实文件并填写绝对路径，缺失时只能验证软件链路或使用后续 odom 配置。
|
||||
|
||||
@@ -1,10 +1,10 @@
|
||||
# `v0.11.0` 导航点云
|
||||
# `v0.11.1` 导航点云
|
||||
|
||||
本目录保存 `sim2real_ros2_v2` 简单导航原型引用的两份预览点云。为避免大体积 ASCII PCD 进入 Git 历史，使用 `tools/nav_tools/downsample_ascii_pcd.py` 做确定性等步长抽样，坐标值和原始顺序不做修改。
|
||||
原始 `map_b.pcd` 为 ASCII PCD，共 1,080,047 点、34,720,771 字节。为避免大文件进入 Git 历史，使用 `tools/nav_tools/downsample_ascii_pcd.py` 以步长 4 做确定性抽样。
|
||||
|
||||
| 文件 | 原始点数 | 抽样步长 | 抽样点数 | 抽样大小 | 原始 SHA-256 | 抽样 SHA-256 |
|
||||
| --- | ---: | ---: | ---: | ---: | --- | --- |
|
||||
| `map1.pcd` | 1,604,103 | 6 | 267,350 | 8,572,407 B | `FBAD5DBA9AD15042FBC1702BDAAD5F7A3819B678E00558CFEC8BC54479A578AE` | `5F4AF5178E7F0D1AEAD7A2F454F6CB1524EF0F53AF0544FD415A8FB6A384000A` |
|
||||
| `map6.pcd` | 1,446,503 | 5 | 289,301 | 9,278,742 B | `77DEB28D739760C58E9C08B589CFD52C2A6CB2981CB3B51156BBCC26EFC4FA9F` | `792C11023A2F91415378580276F87BE103AD7B22A214D24D9FC1B6928EB875C4` |
|
||||
- 抽样点数：270,012
|
||||
- 抽样大小：8,680,283 字节
|
||||
- 原始 SHA-256：`F17FC7E188C772D48E8D3FE20E3A934AFCDF59357C27BCCDCAC3C7E9AD1D1CB7`
|
||||
- 抽样 SHA-256：`F3052431F217DE8FFFE87726F8E547A59C65048B0285F02AF4803C5F4E4C1B42`
|
||||
|
||||
抽样点云适合路线编辑、可视化和代码演示，不等价于原始高密度建图数据。需要复现实机定位精度时，应从原始备份恢复对应 PCD 并核对原始哈希。
|
||||
抽样文件适合路线编辑和代码演示，不等价于原始高密度点云；需要复现实机导航精度时应恢复原始 PCD 并核对哈希。
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
+270014
-289303
File diff suppressed because it is too large
Load Diff
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,12 @@
|
||||
# `v0.11.1` 策略文件
|
||||
|
||||
本阶段只归档运行配置引用或作为部署契约基线的策略，未纳入其余候选 checkpoint 和 engine。
|
||||
|
||||
| 文件 | 用途 | SHA-256 |
|
||||
| --- | --- | --- |
|
||||
| `NEWmodel_1900.onnx` | Rough 默认 ONNX | `A9E2F0D117225874C8B4F986787C3E6C8E7EA45D1CDA3AED58AC38F581442B00` |
|
||||
| `NEWmodel_1900_fp16.engine` | Rough 比赛机 TensorRT | `DBC73AC9B3780029EC08BE19B9972B9840A067EBD146DEB860698F4F7066CC03` |
|
||||
| `model_crawl.onnx` | Crawl 策略候选；当前配置使用 IK 后端 | `633AB2F3C3737FDDFD657F4A006A414BFA3950DD60D708B101E6744DF9E6DF67` |
|
||||
| `model_rough.onnx` | 原部署契约参考基线 | `4149BE180A197E70495A7A6D186C62BE43271A4CE9035A8EEE56CD293D869A27` |
|
||||
|
||||
TensorRT engine 不保证跨 JetPack、TensorRT 版本或 GPU 架构兼容。
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,5 @@
|
||||
recorddata/
|
||||
/config/calib.yaml
|
||||
!/config/calib.yaml
|
||||
/log
|
||||
/map
|
||||
@@ -0,0 +1,8 @@
|
||||
v0.10.5 2026_0525
|
||||
1. 修改 recorddata 数据格式，新增 device_id、algorithm_version 字段
|
||||
2. 修复 SLAM 模式下下载地图失败的问题(USB2.0)
|
||||
3. 修复重定位模式下上传地图失败的问题(USB2.0)
|
||||
|
||||
v0.10.4 2026_0522
|
||||
1. 修复 USB2.0 心跳超时导致软断开的问题
|
||||
2. control_command.yaml 新增 custom_init_pose_search_radius 和 custom_init_pose_max_rot_deg 参数
|
||||
@@ -0,0 +1,447 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(odin_ros_driver)
|
||||
|
||||
|
||||
if(DEFINED BUILD_SYSTEM)
|
||||
set(ROS_VERSION ${BUILD_SYSTEM})
|
||||
message(STATUS "ROS_VERSION: ${ROS_VERSION}")
|
||||
elseif(DEFINED ENV{ROS_DISTRO})
|
||||
if("$ENV{ROS_DISTRO}" MATCHES "foxy|galactic|humble|iron|rolling")
|
||||
set(ROS_VERSION "ROS2")
|
||||
else()
|
||||
set(ROS_VERSION "ROS1")
|
||||
endif()
|
||||
elseif(DEFINED ENV{ROS_VERSION})
|
||||
if("$ENV{ROS_VERSION}" EQUAL "2")
|
||||
set(ROS_VERSION "ROS2")
|
||||
else()
|
||||
set(ROS_VERSION "ROS1")
|
||||
endif()
|
||||
else()
|
||||
# Attempt automatic detection
|
||||
if(COMMAND catkin_package)
|
||||
set(ROS_VERSION "ROS1")
|
||||
elseif(COMMAND ament_package)
|
||||
set(ROS_VERSION "ROS2")
|
||||
else()
|
||||
# Default to ROS2
|
||||
set(ROS_VERSION "ROS2")
|
||||
message(WARNING "Unable to determine ROS version, defaulting to ROS2")
|
||||
endif()
|
||||
endif()
|
||||
|
||||
# Add compile definitions after detecting ROS version
|
||||
if(ROS_VERSION STREQUAL "ROS2")
|
||||
add_definitions(-DROS2)
|
||||
message(STATUS "Defining ROS2")
|
||||
else()
|
||||
add_definitions(-DROS1)
|
||||
message(STATUS "Defining ROS1")
|
||||
endif()
|
||||
|
||||
message(STATUS "Build system: ${ROS_VERSION}")
|
||||
|
||||
# Platform detection
|
||||
execute_process(
|
||||
COMMAND uname -m
|
||||
OUTPUT_VARIABLE ARCH
|
||||
OUTPUT_STRIP_TRAILING_WHITESPACE
|
||||
)
|
||||
|
||||
if(ARCH STREQUAL "x86_64")
|
||||
set(TARGET_PLATFORM "x86")
|
||||
message(STATUS "Detected x86_64 architecture")
|
||||
elseif(ARCH MATCHES "arm|aarch64")
|
||||
set(TARGET_PLATFORM "arm")
|
||||
message(STATUS "Detected ARM architecture: ${ARCH}")
|
||||
else()
|
||||
message(WARNING "Unsupported architecture: ${ARCH}. Using default settings")
|
||||
set(TARGET_PLATFORM "unknown")
|
||||
endif()
|
||||
|
||||
# Set library path
|
||||
set(LIB_DIR "${CMAKE_CURRENT_SOURCE_DIR}/lib")
|
||||
message(STATUS "Library directory: ${LIB_DIR}")
|
||||
|
||||
# Set library name based on platform
|
||||
if(TARGET_PLATFORM STREQUAL "arm")
|
||||
set(LYD_HOST_API_LIB_NAME "lydHostApi_arm")
|
||||
else()
|
||||
set(LYD_HOST_API_LIB_NAME "lydHostApi_amd")
|
||||
endif()
|
||||
|
||||
# Find precompiled lydHostApi library
|
||||
find_library(LYD_HOST_API_LIB
|
||||
NAMES
|
||||
${LYD_HOST_API_LIB_NAME}
|
||||
lib${LYD_HOST_API_LIB_NAME}.a
|
||||
lib${LYD_HOST_API_LIB_NAME}.so
|
||||
PATHS ${LIB_DIR}
|
||||
NO_DEFAULT_PATH
|
||||
)
|
||||
|
||||
if(LYD_HOST_API_LIB)
|
||||
message(STATUS "Found lydHostApi library: ${LYD_HOST_API_LIB}")
|
||||
else()
|
||||
file(GLOB LIB_FILES "${LIB_DIR}/lib${LYD_HOST_API_LIB_NAME}.*")
|
||||
if(LIB_FILES)
|
||||
message(STATUS "Found library files: ${LIB_FILES}")
|
||||
set(LYD_HOST_API_LIB ${LIB_FILES})
|
||||
else()
|
||||
message(FATAL_ERROR "Could not find precompiled lydHostApi library in ${LIB_DIR}")
|
||||
endif()
|
||||
endif()
|
||||
|
||||
# Set common compile options
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
|
||||
# Set optimization flags
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O2")
|
||||
|
||||
# Find common dependencies
|
||||
find_package(PkgConfig REQUIRED)
|
||||
find_package(OpenCV REQUIRED)
|
||||
find_package(Eigen3 REQUIRED)
|
||||
find_package(PCL REQUIRED)
|
||||
find_package(yaml-cpp REQUIRED)
|
||||
find_package(OpenSSL REQUIRED)
|
||||
pkg_check_modules(LIBUSB REQUIRED libusb-1.0)
|
||||
|
||||
# Shared include directories
|
||||
include_directories(
|
||||
include
|
||||
${EIGEN3_INCLUDE_DIRS}
|
||||
${OpenCV_INCLUDE_DIRS}
|
||||
${PCL_INCLUDE_DIRS}
|
||||
${yaml-cpp_INCLUDE_DIR}
|
||||
${LIBUSB_INCLUDE_DIRS}
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/include
|
||||
)
|
||||
|
||||
# Shared library list
|
||||
set(COMMON_LIBS
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
${yaml-cpp_LIBRARIES}
|
||||
${OPENSSL_LIBRARIES}
|
||||
${LIBUSB_LIBRARIES}
|
||||
pthread
|
||||
rt
|
||||
${CMAKE_DL_LIBS}
|
||||
${LYD_HOST_API_LIB}
|
||||
)
|
||||
|
||||
# ===== ROS1 Configuration =====
|
||||
if(ROS_VERSION STREQUAL "ROS1")
|
||||
message(STATUS "Configuring for ROS1 build")
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
std_msgs
|
||||
sensor_msgs
|
||||
nav_msgs
|
||||
cv_bridge
|
||||
tf
|
||||
image_transport
|
||||
)
|
||||
|
||||
include_directories(${catkin_INCLUDE_DIRS})
|
||||
|
||||
catkin_package(
|
||||
CATKIN_DEPENDS roscpp std_msgs sensor_msgs nav_msgs cv_bridge image_transport
|
||||
INCLUDE_DIRS include
|
||||
)
|
||||
|
||||
# Set output directories
|
||||
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/lib/${PROJECT_NAME})
|
||||
set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/lib)
|
||||
set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/lib)
|
||||
|
||||
add_executable(host_sdk_sample
|
||||
src/host_sdk_sample.cpp
|
||||
src/yaml_parser.cpp
|
||||
src/rawCloudRender.cpp
|
||||
src/camera_pose_visualization.cpp
|
||||
)
|
||||
target_link_libraries(host_sdk_sample
|
||||
${catkin_LIBRARIES}
|
||||
${COMMON_LIBS}
|
||||
${LYD_HOST_API_LIB}
|
||||
${LIBUSB_LIBRARIES}
|
||||
yaml-cpp
|
||||
${OpenCV_LIBS}
|
||||
pthread
|
||||
usb-1.0
|
||||
)
|
||||
|
||||
add_library(pointcloud_depth_converter src/pointcloud_depth_converter.cpp)
|
||||
target_link_libraries(pointcloud_depth_converter
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
add_library(depth_image_ros_node src/depth_image_ros_node.cpp)
|
||||
target_link_libraries(depth_image_ros_node
|
||||
pointcloud_depth_converter
|
||||
${catkin_LIBRARIES}
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
add_executable(pcd2depth_node src/pcd2depth_ros.cpp)
|
||||
target_link_libraries(pcd2depth_node
|
||||
depth_image_ros_node
|
||||
${catkin_LIBRARIES}
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
|
||||
add_library(cloud_reprojector src/cloud_reprojector.cpp)
|
||||
target_link_libraries(cloud_reprojector
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
|
||||
add_executable(cloud_reprojection_node src/cloud_reprojection_ros.cpp)
|
||||
target_link_libraries(cloud_reprojection_node
|
||||
cloud_reprojector
|
||||
${catkin_LIBRARIES}
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
|
||||
add_executable(image_overlay_node src/image_overlay_node.cpp)
|
||||
target_link_libraries(image_overlay_node
|
||||
${catkin_LIBRARIES}
|
||||
${OpenCV_LIBS}
|
||||
)
|
||||
|
||||
# Installation rules
|
||||
install(TARGETS host_sdk_sample
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
||||
install(DIRECTORY include/
|
||||
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
FILES_MATCHING PATTERN "*.h"
|
||||
)
|
||||
if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/config")
|
||||
install(DIRECTORY config/
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config
|
||||
)
|
||||
endif()
|
||||
|
||||
# ===== ROS2 Configuration =====
|
||||
elseif(ROS_VERSION STREQUAL "ROS2")
|
||||
message(STATUS "Configuring for ROS2 build")
|
||||
|
||||
# Find all necessary ROS2 packages
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(visualization_msgs REQUIRED)
|
||||
find_package(cv_bridge REQUIRED)
|
||||
find_package(image_transport REQUIRED)
|
||||
find_package(pcl_conversions REQUIRED)
|
||||
find_package(message_filters REQUIRED)
|
||||
find_package(tf2 REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(tf2_geometry_msgs REQUIRED)
|
||||
|
||||
# Create executable
|
||||
add_executable(host_sdk_sample
|
||||
src/host_sdk_sample.cpp
|
||||
src/yaml_parser.cpp
|
||||
src/rawCloudRender.cpp
|
||||
src/camera_pose_visualization.cpp
|
||||
)
|
||||
|
||||
# Link libraries
|
||||
target_link_libraries(host_sdk_sample
|
||||
${COMMON_LIBS}
|
||||
yaml-cpp
|
||||
usb-1.0
|
||||
)
|
||||
|
||||
# Add ROS2 dependencies
|
||||
ament_target_dependencies(host_sdk_sample
|
||||
rclcpp
|
||||
std_msgs
|
||||
sensor_msgs
|
||||
nav_msgs
|
||||
visualization_msgs
|
||||
cv_bridge
|
||||
image_transport
|
||||
tf2
|
||||
tf2_ros
|
||||
geometry_msgs
|
||||
tf2_geometry_msgs
|
||||
)
|
||||
|
||||
add_library(pointcloud_depth_converter_ros2 src/pointcloud_depth_converter.cpp)
|
||||
target_link_libraries(pointcloud_depth_converter_ros2
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
|
||||
add_library(depth_image_ros2_node_lib src/depth_image_ros2_node.cpp)
|
||||
target_link_libraries(depth_image_ros2_node_lib
|
||||
pointcloud_depth_converter_ros2
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
|
||||
ament_target_dependencies(depth_image_ros2_node_lib
|
||||
rclcpp
|
||||
sensor_msgs
|
||||
std_msgs
|
||||
cv_bridge
|
||||
image_transport
|
||||
pcl_conversions
|
||||
message_filters
|
||||
)
|
||||
|
||||
add_executable(pcd2depth_ros2_node src/pcd2depth_ros2.cpp)
|
||||
target_link_libraries(pcd2depth_ros2_node
|
||||
depth_image_ros2_node_lib
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
yaml-cpp
|
||||
)
|
||||
|
||||
ament_target_dependencies(pcd2depth_ros2_node
|
||||
rclcpp
|
||||
sensor_msgs
|
||||
std_msgs
|
||||
cv_bridge
|
||||
image_transport
|
||||
pcl_conversions
|
||||
message_filters
|
||||
)
|
||||
|
||||
add_library(cloud_reprojector_ros2 src/cloud_reprojector.cpp)
|
||||
target_compile_definitions(cloud_reprojector_ros2 PRIVATE ROS2)
|
||||
target_link_libraries(cloud_reprojector_ros2
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
|
||||
add_executable(cloud_reprojection_ros2_node src/cloud_reprojection_ros.cpp)
|
||||
target_compile_definitions(cloud_reprojection_ros2_node PRIVATE ROS2)
|
||||
target_link_libraries(cloud_reprojection_ros2_node
|
||||
cloud_reprojector_ros2
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
yaml-cpp
|
||||
)
|
||||
ament_target_dependencies(cloud_reprojection_ros2_node
|
||||
rclcpp
|
||||
sensor_msgs
|
||||
nav_msgs
|
||||
cv_bridge
|
||||
image_transport
|
||||
pcl_conversions
|
||||
message_filters
|
||||
)
|
||||
|
||||
add_executable(image_overlay_node src/image_overlay_node.cpp)
|
||||
target_compile_definitions(image_overlay_node PRIVATE ROS2)
|
||||
target_link_libraries(image_overlay_node
|
||||
${OpenCV_LIBS}
|
||||
)
|
||||
ament_target_dependencies(image_overlay_node
|
||||
rclcpp
|
||||
sensor_msgs
|
||||
cv_bridge
|
||||
image_transport
|
||||
message_filters
|
||||
)
|
||||
|
||||
# Installation rules - ensure all install targets are defined before ament_package()
|
||||
# Install executable
|
||||
install(TARGETS
|
||||
host_sdk_sample
|
||||
pcd2depth_ros2_node
|
||||
cloud_reprojection_ros2_node
|
||||
image_overlay_node
|
||||
EXPORT export_${PROJECT_NAME}
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
# Install package.xml
|
||||
install(FILES package.xml
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
# Install headers
|
||||
install(DIRECTORY include/
|
||||
DESTINATION include
|
||||
)
|
||||
# Install launch_ROS2 directory
|
||||
if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/launch_ROS2")
|
||||
install(DIRECTORY launch_ROS2/
|
||||
DESTINATION share/${PROJECT_NAME}/launch
|
||||
)
|
||||
message(STATUS "Installing launch_ROS2 directory to share/${PROJECT_NAME}/launch")
|
||||
else()
|
||||
message(WARNING "launch_ROS2 directory not found")
|
||||
endif()
|
||||
# Install config files
|
||||
if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/config")
|
||||
install(DIRECTORY config/
|
||||
DESTINATION share/${PROJECT_NAME}/config
|
||||
)
|
||||
endif()
|
||||
|
||||
# Install launch files
|
||||
if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/launch")
|
||||
install(DIRECTORY launch/
|
||||
DESTINATION share/${PROJECT_NAME}/launch
|
||||
)
|
||||
endif()
|
||||
ament_export_targets(export_${PROJECT_NAME})
|
||||
# Declare dependencies
|
||||
ament_export_dependencies(
|
||||
rclcpp
|
||||
std_msgs
|
||||
sensor_msgs
|
||||
nav_msgs
|
||||
visualization_msgs
|
||||
cv_bridge
|
||||
image_transport
|
||||
pcl_conversions
|
||||
message_filters
|
||||
tf2
|
||||
tf2_ros
|
||||
geometry_msgs
|
||||
tf2_geometry_msgs
|
||||
)
|
||||
|
||||
ament_package()
|
||||
message(STATUS "Install targets added")
|
||||
|
||||
else()
|
||||
message(FATAL_ERROR "Invalid ROS_VERSION: ${ROS_VERSION}")
|
||||
endif()
|
||||
|
||||
# ARM platform specific link options
|
||||
if(TARGET_PLATFORM STREQUAL "arm")
|
||||
set_target_properties(host_sdk_sample PROPERTIES
|
||||
LINK_FLAGS "-Wl,--no-as-needed -Wl,--rpath=${LIB_DIR}"
|
||||
)
|
||||
message(STATUS "Adding ARM-specific link options and RPATH")
|
||||
endif()
|
||||
|
||||
# Add debug information
|
||||
message(STATUS "=======================================")
|
||||
message(STATUS "Project: ${PROJECT_NAME}")
|
||||
message(STATUS "ROS_VERSION: ${ROS_VERSION}")
|
||||
message(STATUS "Target platform: ${TARGET_PLATFORM}")
|
||||
message(STATUS "lydHostApi library: ${LYD_HOST_API_LIB}")
|
||||
message(STATUS "libusb library: ${LIBUSB_LIBRARIES}")
|
||||
message(STATUS "=======================================")
|
||||
|
||||
@@ -0,0 +1,201 @@
|
||||
Apache License
|
||||
Version 2.0, January 2004
|
||||
http://www.apache.org/licenses/
|
||||
|
||||
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
|
||||
|
||||
1. Definitions.
|
||||
|
||||
"License" shall mean the terms and conditions for use, reproduction,
|
||||
and distribution as defined by Sections 1 through 9 of this document.
|
||||
|
||||
"Licensor" shall mean the copyright owner or entity authorized by
|
||||
the copyright owner that is granting the License.
|
||||
|
||||
"Legal Entity" shall mean the union of the acting entity and all
|
||||
other entities that control, are controlled by, or are under common
|
||||
control with that entity. For the purposes of this definition,
|
||||
"control" means (i) the power, direct or indirect, to cause the
|
||||
direction or management of such entity, whether by contract or
|
||||
otherwise, or (ii) ownership of fifty percent (50%) or more of the
|
||||
outstanding shares, or (iii) beneficial ownership of such entity.
|
||||
|
||||
"You" (or "Your") shall mean an individual or Legal Entity
|
||||
exercising permissions granted by this License.
|
||||
|
||||
"Source" form shall mean the preferred form for making modifications,
|
||||
including but not limited to software source code, documentation
|
||||
source, and configuration files.
|
||||
|
||||
"Object" form shall mean any form resulting from mechanical
|
||||
transformation or translation of a Source form, including but
|
||||
not limited to compiled object code, generated documentation,
|
||||
and conversions to other media types.
|
||||
|
||||
"Work" shall mean the work of authorship, whether in Source or
|
||||
Object form, made available under the License, as indicated by a
|
||||
copyright notice that is included in or attached to the work
|
||||
(an example is provided in the Appendix below).
|
||||
|
||||
"Derivative Works" shall mean any work, whether in Source or Object
|
||||
form, that is based on (or derived from) the Work and for which the
|
||||
editorial revisions, annotations, elaborations, or other modifications
|
||||
represent, as a whole, an original work of authorship. For the purposes
|
||||
of this License, Derivative Works shall not include works that remain
|
||||
separable from, or merely link (or bind by name) to the interfaces of,
|
||||
the Work and Derivative Works thereof.
|
||||
|
||||
"Contribution" shall mean any work of authorship, including
|
||||
the original version of the Work and any modifications or additions
|
||||
to that Work or Derivative Works thereof, that is intentionally
|
||||
submitted to Licensor for inclusion in the Work by the copyright owner
|
||||
or by an individual or Legal Entity authorized to submit on behalf of
|
||||
the copyright owner. For the purposes of this definition, "submitted"
|
||||
means any form of electronic, verbal, or written communication sent
|
||||
to the Licensor or its representatives, including but not limited to
|
||||
communication on electronic mailing lists, source code control systems,
|
||||
and issue tracking systems that are managed by, or on behalf of, the
|
||||
Licensor for the purpose of discussing and improving the Work, but
|
||||
excluding communication that is conspicuously marked or otherwise
|
||||
designated in writing by the copyright owner as "Not a Contribution."
|
||||
|
||||
"Contributor" shall mean Licensor and any individual or Legal Entity
|
||||
on behalf of whom a Contribution has been received by Licensor and
|
||||
subsequently incorporated within the Work.
|
||||
|
||||
2. Grant of Copyright License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
copyright license to reproduce, prepare Derivative Works of,
|
||||
publicly display, publicly perform, sublicense, and distribute the
|
||||
Work and such Derivative Works in Source or Object form.
|
||||
|
||||
3. Grant of Patent License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
(except as stated in this section) patent license to make, have made,
|
||||
use, offer to sell, sell, import, and otherwise transfer the Work,
|
||||
where such license applies only to those patent claims licensable
|
||||
by such Contributor that are necessarily infringed by their
|
||||
Contribution(s) alone or by combination of their Contribution(s)
|
||||
with the Work to which such Contribution(s) was submitted. If You
|
||||
institute patent litigation against any entity (including a
|
||||
cross-claim or counterclaim in a lawsuit) alleging that the Work
|
||||
or a Contribution incorporated within the Work constitutes direct
|
||||
or contributory patent infringement, then any patent licenses
|
||||
granted to You under this License for that Work shall terminate
|
||||
as of the date such litigation is filed.
|
||||
|
||||
4. Redistribution. You may reproduce and distribute copies of the
|
||||
Work or Derivative Works thereof in any medium, with or without
|
||||
modifications, and in Source or Object form, provided that You
|
||||
meet the following conditions:
|
||||
|
||||
(a) You must give any other recipients of the Work or
|
||||
Derivative Works a copy of this License; and
|
||||
|
||||
(b) You must cause any modified files to carry prominent notices
|
||||
stating that You changed the files; and
|
||||
|
||||
(c) You must retain, in the Source form of any Derivative Works
|
||||
that You distribute, all copyright, patent, trademark, and
|
||||
attribution notices from the Source form of the Work,
|
||||
excluding those notices that do not pertain to any part of
|
||||
the Derivative Works; and
|
||||
|
||||
(d) If the Work includes a "NOTICE" text file as part of its
|
||||
distribution, then any Derivative Works that You distribute must
|
||||
include a readable copy of the attribution notices contained
|
||||
within such NOTICE file, excluding those notices that do not
|
||||
pertain to any part of the Derivative Works, in at least one
|
||||
of the following places: within a NOTICE text file distributed
|
||||
as part of the Derivative Works; within the Source form or
|
||||
documentation, if provided along with the Derivative Works; or,
|
||||
within a display generated by the Derivative Works, if and
|
||||
wherever such third-party notices normally appear. The contents
|
||||
of the NOTICE file are for informational purposes only and
|
||||
do not modify the License. You may add Your own attribution
|
||||
notices within Derivative Works that You distribute, alongside
|
||||
or as an addendum to the NOTICE text from the Work, provided
|
||||
that such additional attribution notices cannot be construed
|
||||
as modifying the License.
|
||||
|
||||
You may add Your own copyright statement to Your modifications and
|
||||
may provide additional or different license terms and conditions
|
||||
for use, reproduction, or distribution of Your modifications, or
|
||||
for any such Derivative Works as a whole, provided Your use,
|
||||
reproduction, and distribution of the Work otherwise complies with
|
||||
the conditions stated in this License.
|
||||
|
||||
5. Submission of Contributions. Unless You explicitly state otherwise,
|
||||
any Contribution intentionally submitted for inclusion in the Work
|
||||
by You to the Licensor shall be under the terms and conditions of
|
||||
this License, without any additional terms or conditions.
|
||||
Notwithstanding the above, nothing herein shall supersede or modify
|
||||
the terms of any separate license agreement you may have executed
|
||||
with Licensor regarding such Contributions.
|
||||
|
||||
6. Trademarks. This License does not grant permission to use the trade
|
||||
names, trademarks, service marks, or product names of the Licensor,
|
||||
except as required for reasonable and customary use in describing the
|
||||
origin of the Work and reproducing the content of the NOTICE file.
|
||||
|
||||
7. Disclaimer of Warranty. Unless required by applicable law or
|
||||
agreed to in writing, Licensor provides the Work (and each
|
||||
Contributor provides its Contributions) on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
|
||||
implied, including, without limitation, any warranties or conditions
|
||||
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
|
||||
PARTICULAR PURPOSE. You are solely responsible for determining the
|
||||
appropriateness of using or redistributing the Work and assume any
|
||||
risks associated with Your exercise of permissions under this License.
|
||||
|
||||
8. Limitation of Liability. In no event and under no legal theory,
|
||||
whether in tort (including negligence), contract, or otherwise,
|
||||
unless required by applicable law (such as deliberate and grossly
|
||||
negligent acts) or agreed to in writing, shall any Contributor be
|
||||
liable to You for damages, including any direct, indirect, special,
|
||||
incidental, or consequential damages of any character arising as a
|
||||
result of this License or out of the use or inability to use the
|
||||
Work (including but not limited to damages for loss of goodwill,
|
||||
work stoppage, computer failure or malfunction, or any and all
|
||||
other commercial damages or losses), even if such Contributor
|
||||
has been advised of the possibility of such damages.
|
||||
|
||||
9. Accepting Warranty or Additional Liability. While redistributing
|
||||
the Work or Derivative Works thereof, You may choose to offer,
|
||||
and charge a fee for, acceptance of support, warranty, indemnity,
|
||||
or other liability obligations and/or rights consistent with this
|
||||
License. However, in accepting such obligations, You may act only
|
||||
on Your own behalf and on Your sole responsibility, not on behalf
|
||||
of any other Contributor, and only if You agree to indemnify,
|
||||
defend, and hold each Contributor harmless for any liability
|
||||
incurred by, or claims asserted against, such Contributor by reason
|
||||
of your accepting any such warranty or additional liability.
|
||||
|
||||
END OF TERMS AND CONDITIONS
|
||||
|
||||
APPENDIX: How to apply the Apache License to your work.
|
||||
|
||||
To apply the Apache License to your work, attach the following
|
||||
boilerplate notice, with the fields enclosed by brackets "[]"
|
||||
replaced with your own identifying information. (Don't include
|
||||
the brackets!) The text should be enclosed in the appropriate
|
||||
comment syntax for the file format. We also recommend that a
|
||||
file or class name and description of purpose be included on the
|
||||
same "printed page" as the copyright notice for easier
|
||||
identification within third-party archives.
|
||||
|
||||
Copyright 2025 Manifold Tech Ltd.
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
@@ -1,5 +1,567 @@
|
||||
# Odin 驱动依赖占位
|
||||
# Odin_ROS_Driver 说明文档
|
||||
|
||||
`real/sim2real_ros2_v2` 原始快照中的 `src/odin_ros_driver` 为空目录，但启动文件、Dockerfile 和 `sim2real_bringup` 已引用该包。
|
||||
Odin 传感器模块 ROS 驱动套件（Manifold Tech Ltd.）
|
||||
|
||||
因此 `v0.11.0` 主要记录简单导航与 PCD 导航原型，仍需另行提供兼容的 Odin ROS 2 驱动。完整驱动源码从后续站姿调参版本开始随工程归档。
|
||||
Odin1 Wiki：https://manifoldtechltd.github.io/wiki/Odin1/Cover.html
|
||||
|
||||
## Odin_ROS_Driver
|
||||
|
||||
兼容性：
|
||||
|
||||
● ROS 1（推荐 LTS 版本：Noetic）
|
||||
|
||||
● ROS 2（推荐 LTS 版本：Humble）
|
||||
|
||||
## 重要提示：
|
||||
|
||||
本驱动包提供点云 SLAM 应用的核心功能，面向特定使用场景。仅供专业技术人员进行二次开发使用。最终用户需根据实际部署环境进行场景优化和定制开发，以满足运行需求。
|
||||
|
||||
## 1. 版本
|
||||
|
||||
当前版本：v0.10.2
|
||||
|
||||
所需设备固件版本：v0.10.0
|
||||
|
||||
## 2. 准备工作
|
||||
|
||||
### 2.1 操作系统要求
|
||||
|
||||
● ROS Noetic 和 ROS2 Foxy 需 Ubuntu 20.04；
|
||||
|
||||
● ROS2 Humble 需 Ubuntu 22.04；
|
||||
|
||||
● 当前不支持 Ubuntu 18.04；
|
||||
|
||||
● Ubuntu 24.04 尚未官方支持，但可能经过一定修改后运行。
|
||||
|
||||
### 2.2 依赖项
|
||||
|
||||
● OpenCV >= 4.2.0（推荐 4.5.5/4.8.0，请确保仅安装一个 OpenCV 版本）
|
||||
|
||||
● yaml-cpp
|
||||
|
||||
● thread
|
||||
|
||||
● OpenSSL
|
||||
|
||||
● Eigen3
|
||||
|
||||
### 2.3 依赖安装
|
||||
|
||||
#### 2.3.1 系统基础
|
||||
```shell
|
||||
sudo apt update
|
||||
sudo apt-get install build-essential cmake git libgtk2.0-dev pkg-config libavcodec-dev libavformat-dev libswscale-dev
|
||||
```
|
||||
|
||||
#### 2.3.2 yaml-cpp
|
||||
```shell
|
||||
sudo apt update
|
||||
sudo apt install -y libyaml-cpp-dev
|
||||
```
|
||||
|
||||
#### 2.3.3 libusb
|
||||
```shell
|
||||
sudo apt update
|
||||
sudo apt install -y libusb-1.0-0-dev
|
||||
```
|
||||
|
||||
#### 2.3.4 OpenCV
|
||||
```shell
|
||||
sudo apt update
|
||||
sudo apt-get install libopencv-dev
|
||||
```
|
||||
|
||||
#### 2.3.5 ROS 安装
|
||||
|
||||
ROS Noetic 安装请参考：
|
||||
[ROS Noetic 安装指南](https://wiki.ros.org/noetic/Installation)
|
||||
|
||||
ROS2 Foxy 安装请参考：
|
||||
[ROS Foxy 安装指南](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html)
|
||||
|
||||
ROS2 Humble 安装请参考：
|
||||
[ROS Humble 安装指南](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html)
|
||||
|
||||
## 3. 准备
|
||||
|
||||
### 3.1 创建 Udev 规则
|
||||
```shell
|
||||
sudo vim /etc/udev/rules.d/99-odin-usb.rules
|
||||
```
|
||||
在 99-odin-usb.rules 文件中添加以下内容：
|
||||
```shell
|
||||
SUBSYSTEM=="usb", ATTR{idVendor}=="2207", ATTR{idProduct}=="0019", MODE="0666", GROUP="plugdev"
|
||||
```
|
||||
重新加载规则并重新插拔设备：
|
||||
```shell
|
||||
sudo udevadm control --reload
|
||||
sudo udevadm trigger
|
||||
```
|
||||
|
||||
### 3.2 克隆源码
|
||||
```shell
|
||||
git clone https://github.com/manifoldsdk/odin_ros_driver.git catkin_ws/src/odin_ros_driver
|
||||
```
|
||||
注意：
|
||||
请将源码克隆到 "[ros_workspace]/src/" 目录下，否则会导致编译错误。
|
||||
|
||||
### 3.3 编译
|
||||
|
||||
#### 3.3.1 ROS1（以 Noetic 为例）：
|
||||
|
||||
```shell
|
||||
source /opt/ros/noetic/setup.bash
|
||||
./script/build_ros.sh
|
||||
```
|
||||
|
||||
#### 3.3.2 ROS2（以 Foxy 为例）：
|
||||
|
||||
```shell
|
||||
source /opt/ros/foxy/setup.bash
|
||||
./script/build_ros2.sh
|
||||
```
|
||||
|
||||
### 3.4 运行：
|
||||
|
||||
#### 3.4.1 ROS1（以 Noetic 为例）：
|
||||
|
||||
```shell
|
||||
source [ros_workspace]/devel/setup.bash
|
||||
roslaunch odin_ros_driver [launch file]
|
||||
```
|
||||
● odin_ros_driver：包名；
|
||||
|
||||
● launch file：启动文件名；
|
||||
|
||||
● ros_workspace：用户的 ROS 环境工作区；
|
||||
```shell
|
||||
roslaunch odin_ros_driver odin1_ros1.launch
|
||||
```
|
||||
|
||||
#### 3.4.2 ROS2（以 Foxy 为例）：
|
||||
|
||||
```shell
|
||||
source [ros2_workspace]/install/setup.bash
|
||||
ros2 launch odin_ros_driver [launch file]
|
||||
```
|
||||
● odin_ros_driver：包名；
|
||||
|
||||
● launch file：启动文件名；
|
||||
|
||||
● ros2_workspace：用户的 ROS2 环境工作区；
|
||||
|
||||
ROS2 Demo 启动命令：
|
||||
```shell
|
||||
ros2 launch odin_ros_driver odin1_ros2.launch.py
|
||||
```
|
||||
|
||||
### 3.5 运行模式：
|
||||
|
||||
可通过 `config/control_command.yaml` 中的 `custom_map_mode` 参数配置运行模式。
|
||||
|
||||
#### 里程计模式
|
||||
|
||||
设置 `custom_map_mode = 0` 启用里程计模式。此模式下，map 坐标系与 odom 坐标系共享同一位姿。
|
||||
|
||||
若发现里程计数据漂移，可使用脚本命令 `./set_param.sh algo_reset 1` 动态复位算法。
|
||||
|
||||
#### SLAM 建图模式
|
||||
|
||||
设置 `custom_map_mode = 1` 启用 SLAM 模式。此模式在里程计模式基础上，提供**回环检测**和**地图保存**功能。
|
||||
|
||||
启动驱动后，odin1 将自动进行建图并缓存地图数据。场景采集完成后，需在驱动源码目录下执行 `./set_param.sh save_map 1` 以保存自程序启动以来采集的所有地图数据。地图将保存至 `config/control_command.yaml` 中 `mapping_result_dest_dir` 和 `mapping_result_file_name` 参数指定的路径。若未指定这些参数，将使用默认值。
|
||||
|
||||
首次保存后，可再次执行该命令保存新地图。每次保存操作都会生成一个新的地图文件。（连续保存操作之间请间隔至少 5 秒）
|
||||
|
||||
地图原点对应程序启动时 odom 坐标系的起点。
|
||||
|
||||
##### 重定位模式
|
||||
|
||||
要启用重定位，设置 `custom_map_mode = 2`，并通过 `config/control_command.yaml` 中的 `relocalization_map_abs_path` 参数指定预建地图的绝对路径。
|
||||
|
||||
启动后，odin1 将基于当前视点和指定地图启动重定位过程。为保障高成功率，建议在距 SLAM 轨迹原始位置 1 米、±10° 范围内启动。
|
||||
|
||||
注意，重定位性能高度依赖环境条件。在特征丰富的场景中，成功匹配可能发生在 1m/10° 范围之外，而其他环境可能需要更严格的条件。建议在实际部署环境中测试，以确定实际容忍范围。
|
||||
|
||||
若初始重定位失败，系统将临时以降级 SLAM 模式运行（此状态下地图保存功能禁用）。在此过程中可自由移动 odin1，它将在后台持续尝试重定位。一旦成功，将发布 map 与 odom 坐标系之间的 TF。（提示：初始化后轻轻晃动或移动设备有助于提高重定位准确率。）
|
||||
|
||||
以下话题在 odom 坐标系下发布：`/odin1/cloud_slam`、`/odin1/odom`、`/odin1/highodom` 和 `/odin1/path`。若需在 map 坐标系下获取这些数据，请应用从 odom 坐标系到 map 坐标系的 TF 变换。
|
||||
|
||||
## 4. 文件结构与数据格式
|
||||
### 4.1 文件结构
|
||||
```shell
|
||||
Odin_ROS_Driver/ // ROS1/ROS2 驱动包
|
||||
3rdparty/ // 第三方库
|
||||
src/
|
||||
host_sdk_sample.cpp // 示例源码
|
||||
yaml_parser.cpp // YAML 参数读取源码
|
||||
rawCloudRender.cpp // RenderCloud 渲染源码
|
||||
depth_image_ros_node.cpp // depth_image_ros_node 节点
|
||||
depth_image_ros2_node.cpp // depth_image_ros2_node 节点
|
||||
pcd2depth_ros.cpp // pcd2depth_ros 源码
|
||||
pcd2depth_ros2.cpp // pcd2depth_ros2 源码
|
||||
pointcloud_depth_converter.cpp // pointcloud_depth_converter 源码
|
||||
cloud_reprojection_ros.cpp // 云重投影节点源码 (ROS1/ROS2)
|
||||
cloud_reprojector.cpp // 云重投影核心逻辑
|
||||
lib/
|
||||
liblydHostApi_amd.a // AMD 平台静态库
|
||||
liblydHostApi_arm.a // ARM 平台静态库
|
||||
include/
|
||||
host_sdk_sample.h // 示例头文件
|
||||
lidar_api_type.h // API 数据结构头文件
|
||||
lidar_api.h // API 函数声明
|
||||
yaml_parser.h // 参数文件读取头文件
|
||||
rawCloudRender.h // RenderCloud 相关 API
|
||||
data_logger.h // 数据保存日志
|
||||
depth_image_ros_node.hpp // depth_image_ros_node 头文件
|
||||
depth_image_ros2_node.hpp // depth_image_ros2_node 头文件
|
||||
pointcloud_depth_converter.hpp // pointcloud_depth_convert 头文件
|
||||
cloud_reprojection_ros_node.hpp // cloud_reprojection_ros_node 头文件 (ROS1/ROS2)
|
||||
cloud_reprojector.hpp // 云重投影核心类
|
||||
config/
|
||||
control_command.yaml // 驱动控制参数文件
|
||||
calib.yaml // 设备标定参数 yaml，每个设备独一无二。每次连接 ROS 驱动时从设备读取
|
||||
launch_ROS1/
|
||||
odin1_ros1.launch // ROS1 启动文件
|
||||
launch_ROS2/
|
||||
odin1_ros2.launch.py // ROS2 启动文件
|
||||
script/
|
||||
build_ros1.sh // ROS1 安装脚本
|
||||
build_ros2.sh // ROS2 安装脚本
|
||||
recorddata/ // 存放可导入 MindCloud 的录制数据
|
||||
log/ // 存放日志文件
|
||||
Driver_{timestamp}/ // 每次启动驱动时生成的日志文件夹
|
||||
Conn_{timestamp}/ // 每次 odin1 设备连接时生成的日志文件
|
||||
dev_status.csv // 设备状态日志
|
||||
README.md // 使用说明
|
||||
CMakeLists.txt // CMake 构建文件
|
||||
License // 许可证文件
|
||||
```
|
||||
### 4.2 启动文件
|
||||
| 启动文件名 | 说明 |
|
||||
|--------------------------|-------------|
|
||||
| odin1_ros1.launch | ROS1 启动文件 - Odin1 基础操作演示 |
|
||||
| odin1_ros2.launch.py | ROS2 启动文件 - Odin1 基础操作演示 |
|
||||
|
||||
|
||||
### 4.3 ROS 话题
|
||||
Odin ROS 驱动的内部参数定义在 config/control_command.yaml 中。以下是常用参数说明：
|
||||
|
||||
| 话题 | control_command.yaml | 详细说明 |
|
||||
|---------------------------|----------------------|----------------------|
|
||||
| odin1/imu | sendimu | IMU 话题 |
|
||||
| odin1/image | sendrgb | RGB 相机话题，由设备原始 JPEG 数据解码，bgr8 格式 |
|
||||
| odin1/image_undistort | sendrgbundistort | 去畸变 RGB 相机话题，经设备 calib.yaml 标定参数处理 |
|
||||
| odin1/image/compressed | sendrgbcompressed | RGB 相机压缩话题，设备原始 JPEG 数据 |
|
||||
| odin1/cloud_raw | senddtof | 原始点云话题 |
|
||||
| odin1/cloud_render | sendcloudrender | 渲染点云话题，经原始点云、RGB 图像及设备 calib.yaml 处理 |
|
||||
| odin1/cloud_slam | sendcloudslam | SLAM 点云话题 |
|
||||
| odin1/odometry | sendodom | 里程计话题 |
|
||||
| odin1/odometry_high | sendodom | 高频里程计话题 |
|
||||
| odin1/path | showpath | 里程计路径话题 |
|
||||
| tf | sendodom | TF 树话题 |
|
||||
| odin1/depth_img_competetion | senddepth | 稠密深度图话题。需较高算力，仅作演示。与 odin1/image_undistort 一一对应。使用时请直接订阅本话题而非 echo。原始值即为深度数据，无需额外转换。 |
|
||||
| odin1/depth_img_competetion_cloud | senddepth | 稠密深度点云话题。需较高算力，仅作演示 |
|
||||
| odin1/reprojected_image | sendreprojection | 重投影像素话题。利用里程计将 cloud_slam 投影至相机图像。在主机端处理。 |
|
||||
|
||||
### 4.4 数据格式
|
||||
|
||||
1. 原始点云（cloud_raw）包含以下字段：
|
||||
```
|
||||
float32 x // X 轴，单位：米
|
||||
float32 y // Y 轴，单位：米
|
||||
float32 z // Z 轴，单位：米
|
||||
uint8 intensity // 反射率，范围 0–255
|
||||
uint16 confidence // 点置信度，典型场景下取值范围约 0–1300，数值越高可靠性越强。推荐过滤阈值 30-35，应结合实际环境调整。
|
||||
float32 offset_time // 相对基准时间戳的时间偏移量，单位：秒
|
||||
```
|
||||
|
||||
要在 PCL 中使用此自定义格式，首先定义点类型：
|
||||
```cpp
|
||||
/*** LS ***/
|
||||
namespace ls_ros {
|
||||
struct EIGEN_ALIGN16 Point {
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
uint8_t intensity;
|
||||
uint16_t confidence;
|
||||
float offset_time;
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
} // namespace ls_ros
|
||||
|
||||
POINT_CLOUD_REGISTER_POINT_STRUCT(ls_ros::Point,
|
||||
(float, x, x)
|
||||
(float, y, y)
|
||||
(float, z, z)
|
||||
(uint8_t, intensity, intensity)
|
||||
(uint16_t, confidence, confidence)
|
||||
(float offset_time , offset_time)
|
||||
)
|
||||
```
|
||||
然后即可轻松将 ROS sensor_msgs::PointCloud2 消息转换为 PCL 点云：
|
||||
```
|
||||
pcl::PointCloud<ls_ros::Point> ls_cloud;
|
||||
pcl::fromROSMsg(*msg, ls_cloud);
|
||||
```
|
||||
|
||||
2. SLAM 点云（cloud_slam）与直接渲染点云（cloud_render）包含以下字段：
|
||||
```
|
||||
float32 x // X 轴，单位：米
|
||||
float32 y // Y 轴，单位：米
|
||||
float32 z // Z 轴，单位：米
|
||||
float32 rgb // RGB 颜色值
|
||||
```
|
||||
|
||||
### 4.5 其他功能
|
||||
|
||||
| control_command.yaml 参数 | 详细说明 |
|
||||
|----------------------------|----------------------|
|
||||
| use_host_ros_time | 时间同步模式：0 - 使用 odin 内部系统时间作为数据时间戳（典型用法，推荐）；1 - 接收时使用主机 ROS 时间（不推荐大多数用户使用）；2 - 通过类 NTP 同步将 odin1 时间对齐至主机时间，时间戳为传感器数据在主机时间轴上的接收时间。 |
|
||||
| strict_usb3.0_check | 严格 USB3.0 检查，关闭后即使 USB 连接低于 3.0 标准也允许连接 |
|
||||
| recorddata | 以特定格式记录数据，可导入 MindCloud(TM) 进行后处理。请注意这将消耗大量存储空间，测试显示 10 分钟数据约占 9.5GB。 |
|
||||
| devstatuslog | 设备状态日志记录，当前将设备状态（SoC 温度、CPU 占用率、RAM 占用率、dToF 传感器温度等）及数据发送/接收速率保存至 log 目录下的 devstatus.csv。每次启动驱动时创建新文件。 |
|
||||
| showcamerapose | 显示相机位姿及视野范围。 |
|
||||
| custom_map_mode | 运行模式：模式 0 - 里程计模式：map 坐标系与 odom 坐标系共享同一位姿。模式 1 - 建图模式（带回环检测）：该模式支持地图保存。模式 2 - 重定位模式：需指定地图文件绝对路径，重定位成功后将输出 map 与 odom 坐标系之间的 TF 关系。|
|
||||
| custom_init_pos | 初始化位置（当前未启用）。 |
|
||||
| relocalization_map_abs_path | 地图文件绝对路径：用于重定位模式。 |
|
||||
| mapping_result_dest_dir 和 mapping_result_file_name | 建图模式下地图保存路径与文件名：若未指定，将使用默认值。 |
|
||||
|
||||
## 5. 常见问题
|
||||
### 5.1 重新启动宿主 SDK 时出现段错误
|
||||
**错误信息**
|
||||
60 秒内未连接任何设备
|
||||
|
||||
**解决方案**
|
||||
1. 请重新为 Odin 模块上电 # 断开并重新连接 odin 电源
|
||||
|
||||
2. 重新初始化 Odin SDK # 设备重启后执行 SDK
|
||||
|
||||
|
||||
### 5.2 编译时出现库链接失败
|
||||
|
||||
**错误信息**
|
||||
ld: cannot find -llydHostApi 或符号查找错误
|
||||
|
||||
**解决方案**
|
||||
|
||||
1. 清理之前的构建产物
|
||||
|
||||
ROS1
|
||||
```shell
|
||||
rm -rf devel/ build/
|
||||
```
|
||||
ROS2
|
||||
```shell
|
||||
rm -rf devel/ install/ log/
|
||||
```
|
||||
2. 重新运行脚本安装
|
||||
|
||||
### 5.3 Docker GUI 透传失败
|
||||
|
||||
**错误信息**
|
||||
Unable to open X display 或 No protocol specified
|
||||
|
||||
**解决方案**
|
||||
```shell
|
||||
xhost + # 此命令启用 Docker 容器的图形透传
|
||||
```
|
||||
|
||||
### 5.4 ROS 驱动以"获取版本失败"错误退出
|
||||
|
||||
**错误信息**
|
||||
```shell
|
||||
<ERROR><api.cpp:lidar_get_version:672>: get device version fail.
|
||||
get version failed.
|
||||
```
|
||||
|
||||
**解决方案**
|
||||
|
||||
设备固件版本过低，请升级至最新版本。
|
||||
|
||||
|
||||
### 5.5 RVIZ 长时间无响应
|
||||
|
||||
**错误信息**
|
||||
Rviz 无响应，稍后终端打印"Device disconnected, waiting for reconnection..."
|
||||
|
||||
**解决方案**
|
||||
|
||||
请重新为 Odin 模块上电
|
||||
|
||||
### 5.6 设备无响应
|
||||
|
||||
**错误信息**
|
||||
Missed ok response from device, probably wrong interaction procedure.
|
||||
|
||||
**解决方案**
|
||||
|
||||
请采用 5.1 所述的解决方案
|
||||
|
||||
### 5.7 设备无外部标定文件
|
||||
|
||||
**错误信息**
|
||||
ERROR：Missing camera node 'cam_0'
|
||||
|
||||
**解决方案**
|
||||
|
||||
请重新插拔 USB
|
||||
|
||||
### 5.8 ROS 驱动在数据流启动后立即报设备断开
|
||||
|
||||
**错误信息**
|
||||
|
||||
```shell
|
||||
Device ready and streams activated
|
||||
Device detaching...
|
||||
Wating for device reconnection...
|
||||
Device disconnected, waiting for reconnection...
|
||||
```
|
||||
|
||||
**原因**
|
||||
|
||||
多见于 ROS2 环境且连接到复杂网络环境（如办公 WiFi 和以太网）的情况。ROS2 默认为广播模式，复杂网络环境可能导致 ROS2 发布阻塞，从而引发设备断开。
|
||||
|
||||
**解决方案**
|
||||
|
||||
若不需要跨设备通信，请将 ROS2 限制为仅本地通信：
|
||||
```shell
|
||||
export ROS_LOCALHOST_ONLY=1
|
||||
```
|
||||
|
||||
若需要跨设备通信，请尽量简化网络环境。建议使用仅包含必要设备的小型局域网。
|
||||
|
||||
### 5.9 ROS 驱动在数据流启动后立即崩溃
|
||||
|
||||
**错误信息**
|
||||
|
||||
```shell
|
||||
Device ready and streams activated
|
||||
[host_sdk_sample-2] process has died ......
|
||||
```
|
||||
|
||||
**测试**
|
||||
|
||||
在 control_command.yaml 中设置 sendrgb = 0 禁用 odin1/image，然后重试。若驱动此时正常工作，则问题很可能与系统安装了多个 OpenCV 版本有关。
|
||||
|
||||
**解决方案**
|
||||
|
||||
卸载多余的 OpenCV 版本，仅保留单一完整版本，然后重新编译驱动并重试。
|
||||
|
||||
### 5.10 ROS 驱动打印"TF_OLD_DATA ignoring data"警告
|
||||
|
||||
**错误信息**
|
||||
|
||||
```shell
|
||||
[rviz2-3] Warning: TF_OLD_DATA ignoring data from the past for frame odin1_base_link at time 20.547632 according to authority Authority undetectable
|
||||
[rviz2-3] Possible reasons are listed at http://wiki.ros.org/tf/Errors%20explained
|
||||
[rviz2-3] at line 294 in ./src/buffer_core.cpp
|
||||
```
|
||||
|
||||
**原因**
|
||||
|
||||
这是 ROS 和 rviz 的一项功能，用于警告用户某些 TF 数据因时间戳冲突而被忽略。常见于用户保持 ROS 驱动运行的同时对 odin 设备断电重启，导致 odin 内部系统时间被重置，新数据时间戳与 rviz 上次运行期间接收的旧数据产生冲突。
|
||||
|
||||
**解决方案**
|
||||
|
||||
rviz GUI 底部有一个重置按钮。点击此按钮将重置 rviz 内部状态并停止警告。
|
||||
|
||||
### 5.11 ROS 驱动打印"unknown cmd code: xx"错误
|
||||
|
||||
**错误信息**
|
||||
|
||||
```shell
|
||||
<ERROR><api.cpp:cmd_data_deal:418>: unknow command code 21.
|
||||
```
|
||||
|
||||
**原因**
|
||||
|
||||
这是由于 ROS 驱动版本与设备固件版本不匹配，导致 ROS 驱动无法解码新版固件新增的数据。
|
||||
|
||||
**解决方案**
|
||||
|
||||
请确保使用最新版本的 ROS 驱动和设备固件。
|
||||
|
||||
### 5.12 USB 设备访问错误（LIBUSB_ERROR_BUSY 或 LIBUSB_ERROR_ACCESS）
|
||||
|
||||
**错误信息**
|
||||
|
||||
```shell
|
||||
libusb: error [udev_hotplug_event] ignoring udev action bind
|
||||
LIBUSB_ERROR_BUSY
|
||||
```
|
||||
|
||||
或
|
||||
|
||||
```shell
|
||||
libusb: error [_get_usbfs_fd] libusb couldn't open USB device /dev/bus/usb/xxx/xxx, errno=13
|
||||
LIBUSB_ERROR_ACCESS
|
||||
```
|
||||
|
||||
**原因**
|
||||
|
||||
- **LIBUSB_ERROR_BUSY**：另一个进程正在使用该 USB 设备。常见于多个 ROS 驱动实例正在运行，或其他应用程序（如之前崩溃的实例）仍持有设备句柄。
|
||||
|
||||
- **LIBUSB_ERROR_ACCESS**：当前用户无权访问 USB 设备。通常因缺少 udev 规则或用户权限不足导致。
|
||||
|
||||
**解决方案**
|
||||
|
||||
针对 **LIBUSB_ERROR_BUSY**：
|
||||
|
||||
1. 检查是否有其他驱动实例正在运行：
|
||||
```shell
|
||||
ps aux | grep host_sdk_sample
|
||||
```
|
||||
|
||||
2. 终止所有现存实例：
|
||||
```shell
|
||||
killall host_sdk_sample
|
||||
```
|
||||
|
||||
3. 若问题仍然存在，请拔插 USB 设备以重置设备状态。
|
||||
|
||||
针对 **LIBUSB_ERROR_ACCESS**：
|
||||
|
||||
1. 添加设备 udev 规则。创建文件 `/etc/udev/rules.d/99-odin.rules`，内容如下：
|
||||
```shell
|
||||
SUBSYSTEM=="usb", ATTR{idVendor}=="2207", ATTR{idProduct}=="0019", MODE="0666", GROUP="plugdev"
|
||||
```
|
||||
|
||||
2. 重新加载 udev 规则：
|
||||
```shell
|
||||
sudo udevadm control --reload-rules
|
||||
sudo udevadm trigger
|
||||
```
|
||||
|
||||
3. 或者，使用 sudo 运行驱动（不推荐用于生产环境）：
|
||||
```shell
|
||||
sudo -E ros2 launch odin_ros_driver odin_ros_driver.launch.py
|
||||
```
|
||||
|
||||
4. 确保当前用户属于 `plugdev` 用户组：
|
||||
```shell
|
||||
sudo usermod -aG plugdev $USER
|
||||
```
|
||||
然后注销并重新登录，使组变更生效。
|
||||
|
||||
## 6. 联系方式
|
||||
|
||||
您可通过 support@manifoldtech.cn 联系我们的技术支持。
|
||||
|
||||
为帮助诊断问题，请向我们的 FAE 工程师提供以下信息：
|
||||
|
||||
1. 当前固件版本
|
||||
```shell
|
||||
[device_version_capture]: ros_driver_version: [版本号]
|
||||
```
|
||||
2. 正在使用的电源适配器和转换线缆照片。
|
||||
|
||||
3. 问题是偶发性还是持续性的？
|
||||
|
||||
4. 提供问题场景的图像。
|
||||
|
||||
5. **第 V 节**中的故障排除方法是否解决了问题？
|
||||
|
||||
6. 问题解决的预期时间线。
|
||||
|
||||
@@ -0,0 +1,642 @@
|
||||
# Relocalization Guide / 重定位使用指南
|
||||
|
||||
This guide explains how to use the relocalization feature in Odin ROS Driver, including automatic relocalization and init position relocalization modes.
|
||||
|
||||
本指南介绍如何使用 Odin ROS Driver 的重定位功能，包括自动重定位和指定初始位置重定位两种模式。
|
||||
|
||||
---
|
||||
|
||||
## Table of Contents / 目录
|
||||
|
||||
1. [Overview / 概述](#overview--概述)
|
||||
2. [Prerequisites / 前提条件](#prerequisites--前提条件)
|
||||
3. [Mode 1: Auto Relocalization / 自动重定位](#mode-1-auto-relocalization--自动重定位)
|
||||
4. [Mode 2: Init Position Relocalization / 指定初始位置重定位](#mode-2-init-position-relocalization--指定初始位置重定位)
|
||||
5. [init_pos Format / init_pos 格式说明](#init_pos-format--init_pos-格式说明)
|
||||
6. [Configuration Examples / 配置示例](#configuration-examples--配置示例)
|
||||
7. [Programmatic API / 编程接口](#programmatic-api--编程接口)
|
||||
8. [Troubleshooting / 故障排除](#troubleshooting--故障排除)
|
||||
|
||||
---
|
||||
|
||||
## Overview / 概述
|
||||
|
||||
### English
|
||||
|
||||
Relocalization mode (`custom_map_mode: 2`) allows Odin to localize itself within a pre-built map. There are two approaches:
|
||||
|
||||
| Mode | Description | Use Case |
|
||||
|------|-------------|----------|
|
||||
| **Auto Relocalization** | Algorithm automatically searches for position in the map | Starting position is unknown or within recommended range |
|
||||
| **Init Position Relocalization** | User provides an initial pose estimate via `init_pos` | Starting position is known, faster convergence needed |
|
||||
|
||||
### 中文
|
||||
|
||||
重定位模式（`custom_map_mode: 2`）允许 Odin 在预先构建的地图中进行自我定位。有两种方式：
|
||||
|
||||
| 模式 | 描述 | 适用场景 |
|
||||
|------|------|----------|
|
||||
| **自动重定位** | 算法自动在地图中搜索位置 | 起始位置未知，或在推荐范围内 |
|
||||
| **指定初始位置重定位** | 用户通过 `init_pos` 提供初始位姿估计 | 起始位置已知，需要更快收敛 |
|
||||
|
||||
---
|
||||
|
||||
## Prerequisites / 前提条件
|
||||
|
||||
### English
|
||||
|
||||
1. **Pre-built map file**: A `.bin` map file created in SLAM mode (`custom_map_mode: 1`)
|
||||
2. **Map file path**: Know the absolute path to your map file
|
||||
3. **Starting position**: For init position mode, know the approximate starting pose in map coordinates
|
||||
|
||||
### 中文
|
||||
|
||||
1. **预构建的地图文件**：在 SLAM 模式（`custom_map_mode: 1`）下创建的 `.bin` 地图文件
|
||||
2. **地图文件路径**：知道地图文件的绝对路径
|
||||
3. **起始位置**：对于指定初始位置模式，需要知道在地图坐标系中的大致起始位姿
|
||||
|
||||
---
|
||||
|
||||
## Mode 1: Auto Relocalization / 自动重定位
|
||||
|
||||
### English
|
||||
|
||||
In auto relocalization mode, the algorithm automatically searches for the device's position within the map based on current sensor observations.
|
||||
|
||||
**Configuration** (`config/control_command.yaml`):
|
||||
|
||||
```yaml
|
||||
register_keys:
|
||||
custom_map_mode: 2
|
||||
relocalization_map_abs_path: "/path/to/your/map.bin"
|
||||
# custom_init_pos is NOT set or uses default [0,0,0,0,0,0,1]
|
||||
```
|
||||
|
||||
**Recommended Starting Conditions**:
|
||||
- Within **1 meter** of a position on the original SLAM trajectory
|
||||
- Within **±10 degrees** of the original orientation
|
||||
- In a visually distinctive area of the map
|
||||
|
||||
**Behavior**:
|
||||
1. On startup, Odin attempts to match current observations with the map
|
||||
2. If successful, TF between `map` and `odom` frames is published
|
||||
3. If unsuccessful, system operates in fallback SLAM mode (map saving disabled)
|
||||
4. Relocalization attempts continue in background until successful
|
||||
|
||||
**Tips**:
|
||||
- Gently shaking or moving the device after startup can improve relocalization accuracy
|
||||
- Highly distinctive scenes may allow successful matching beyond the 1m/10° range
|
||||
|
||||
### 中文
|
||||
|
||||
在自动重定位模式下，算法根据当前传感器观测自动在地图中搜索设备位置。
|
||||
|
||||
**配置** (`config/control_command.yaml`):
|
||||
|
||||
```yaml
|
||||
register_keys:
|
||||
custom_map_mode: 2
|
||||
relocalization_map_abs_path: "/path/to/your/map.bin"
|
||||
# custom_init_pos 不设置或使用默认值 [0,0,0,0,0,0,1]
|
||||
```
|
||||
|
||||
**推荐起始条件**：
|
||||
- 距离原始 SLAM 轨迹上某点 **1 米**以内
|
||||
- 朝向与原始方向偏差在 **±10 度**以内
|
||||
- 位于地图中视觉特征明显的区域
|
||||
|
||||
**行为**：
|
||||
1. 启动时，Odin 尝试将当前观测与地图匹配
|
||||
2. 如果成功，发布 `map` 和 `odom` 坐标系之间的 TF
|
||||
3. 如果失败，系统进入后备 SLAM 模式（地图保存功能禁用）
|
||||
4. 后台持续尝试重定位直到成功
|
||||
|
||||
**提示**：
|
||||
- 启动后轻轻晃动或移动设备可以提高重定位精度
|
||||
- 在特征明显的场景中，可能在超出 1m/10° 范围时也能成功匹配
|
||||
|
||||
---
|
||||
|
||||
## Mode 2: Init Position Relocalization / 指定初始位置重定位
|
||||
|
||||
### English
|
||||
|
||||
In init position relocalization mode, you provide an initial pose estimate to help the algorithm converge faster.
|
||||
|
||||
**Configuration** (`config/control_command.yaml`):
|
||||
|
||||
```yaml
|
||||
register_keys:
|
||||
custom_map_mode: 2
|
||||
relocalization_map_abs_path: "/path/to/your/map.bin"
|
||||
custom_init_pos: [x, y, z, qx, qy, qz, qw]
|
||||
```
|
||||
|
||||
**When to Use**:
|
||||
- You know the approximate starting position (e.g., from external localization system)
|
||||
- Starting position is far from the recommended 1m/10° range
|
||||
- You need faster relocalization convergence
|
||||
- Deploying in a fixed docking station with known pose
|
||||
|
||||
**Behavior**:
|
||||
1. Algorithm uses provided `init_pos` as initial pose estimate
|
||||
2. Searches for matches in the vicinity of the provided position
|
||||
3. Faster convergence compared to auto mode when estimate is accurate
|
||||
|
||||
### 中文
|
||||
|
||||
在指定初始位置重定位模式下，您提供初始位姿估计以帮助算法更快收敛。
|
||||
|
||||
**配置** (`config/control_command.yaml`):
|
||||
|
||||
```yaml
|
||||
register_keys:
|
||||
custom_map_mode: 2
|
||||
relocalization_map_abs_path: "/path/to/your/map.bin"
|
||||
custom_init_pos: [x, y, z, qx, qy, qz, qw]
|
||||
```
|
||||
|
||||
**适用场景**：
|
||||
- 您知道大致的起始位置（例如，来自外部定位系统）
|
||||
- 起始位置远离推荐的 1m/10° 范围
|
||||
- 需要更快的重定位收敛速度
|
||||
- 部署在已知位姿的固定充电桩
|
||||
|
||||
**行为**：
|
||||
1. 算法使用提供的 `init_pos` 作为初始位姿估计
|
||||
2. 在提供位置的附近搜索匹配
|
||||
3. 当估计准确时，比自动模式收敛更快
|
||||
|
||||
---
|
||||
|
||||
## init_pos Format / init_pos 格式说明
|
||||
|
||||
### English
|
||||
|
||||
`init_pos` is an array of **7 float values** representing position and orientation:
|
||||
|
||||
```yaml
|
||||
custom_init_pos: [x, y, z, qx, qy, qz, qw]
|
||||
```
|
||||
|
||||
| Index | Parameter | Description | Unit |
|
||||
|-------|-----------|-------------|------|
|
||||
| 0 | x | X position in map frame | meters |
|
||||
| 1 | y | Y position in map frame | meters |
|
||||
| 2 | z | Z position in map frame | meters |
|
||||
| 3 | qx | Quaternion X component | - |
|
||||
| 4 | qy | Quaternion Y component | - |
|
||||
| 5 | qz | Quaternion Z component | - |
|
||||
| 6 | qw | Quaternion W component | - |
|
||||
|
||||
**Important Notes**:
|
||||
- The quaternion must be normalized: `sqrt(qx² + qy² + qz² + qw²) ≈ 1.0`
|
||||
- Coordinates are relative to the **map frame** (world frame at SLAM start)
|
||||
- Default value `[0, 0, 0, 0, 0, 0, 1]` represents origin with no rotation
|
||||
|
||||
**Common Quaternion Values**:
|
||||
|
||||
| Orientation | qx | qy | qz | qw |
|
||||
|-------------|----|----|----|----|
|
||||
| No rotation (identity) | 0 | 0 | 0 | 1 |
|
||||
| 90° around Z-axis | 0 | 0 | 0.707 | 0.707 |
|
||||
| 180° around Z-axis | 0 | 0 | 1 | 0 |
|
||||
| -90° around Z-axis | 0 | 0 | -0.707 | 0.707 |
|
||||
|
||||
### 中文
|
||||
|
||||
`init_pos` 是一个包含 **7 个 float 值**的数组，表示位置和朝向：
|
||||
|
||||
```yaml
|
||||
custom_init_pos: [x, y, z, qx, qy, qz, qw]
|
||||
```
|
||||
|
||||
| 索引 | 参数 | 描述 | 单位 |
|
||||
|------|------|------|------|
|
||||
| 0 | x | 地图坐标系中的 X 位置 | 米 |
|
||||
| 1 | y | 地图坐标系中的 Y 位置 | 米 |
|
||||
| 2 | z | 地图坐标系中的 Z 位置 | 米 |
|
||||
| 3 | qx | 四元数 X 分量 | - |
|
||||
| 4 | qy | 四元数 Y 分量 | - |
|
||||
| 5 | qz | 四元数 Z 分量 | - |
|
||||
| 6 | qw | 四元数 W 分量 | - |
|
||||
|
||||
**重要说明**：
|
||||
- 四元数必须归一化：`sqrt(qx² + qy² + qz² + qw²) ≈ 1.0`
|
||||
- 坐标相对于**地图坐标系**（SLAM 启动时的世界坐标系）
|
||||
- 默认值 `[0, 0, 0, 0, 0, 0, 1]` 表示原点且无旋转
|
||||
|
||||
**常用四元数值**：
|
||||
|
||||
| 朝向 | qx | qy | qz | qw |
|
||||
|------|----|----|----|----|
|
||||
| 无旋转（单位四元数） | 0 | 0 | 0 | 1 |
|
||||
| 绕 Z 轴旋转 90° | 0 | 0 | 0.707 | 0.707 |
|
||||
| 绕 Z 轴旋转 180° | 0 | 0 | 1 | 0 |
|
||||
| 绕 Z 轴旋转 -90° | 0 | 0 | -0.707 | 0.707 |
|
||||
|
||||
---
|
||||
|
||||
## Configuration Examples / 配置示例
|
||||
|
||||
### Example 1: Auto Relocalization / 自动重定位示例
|
||||
|
||||
```yaml
|
||||
register_keys:
|
||||
custom_map_mode: 2
|
||||
relocalization_map_abs_path: "/home/user/maps/office_map.bin"
|
||||
```
|
||||
|
||||
### Example 2: Init Position at Origin / 在原点指定初始位置
|
||||
|
||||
```yaml
|
||||
register_keys:
|
||||
custom_map_mode: 2
|
||||
relocalization_map_abs_path: "/home/user/maps/office_map.bin"
|
||||
custom_init_pos: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
|
||||
```
|
||||
|
||||
### Example 3: Init Position with Offset / 带偏移的初始位置
|
||||
|
||||
Position at (5.2, -3.1, 0) with 90° rotation around Z-axis:
|
||||
|
||||
位置在 (5.2, -3.1, 0)，绕 Z 轴旋转 90°：
|
||||
|
||||
```yaml
|
||||
register_keys:
|
||||
custom_map_mode: 2
|
||||
relocalization_map_abs_path: "/home/user/maps/warehouse_map.bin"
|
||||
custom_init_pos: [5.2, -3.1, 0.0, 0.0, 0.0, 0.707, 0.707]
|
||||
```
|
||||
|
||||
### Example 4: Docking Station Pose / 充电桩位置
|
||||
|
||||
Known docking station at (10.5, 2.3, 0) facing -X direction (180° rotation):
|
||||
|
||||
已知充电桩位置在 (10.5, 2.3, 0)，朝向 -X 方向（旋转 180°）：
|
||||
|
||||
```yaml
|
||||
register_keys:
|
||||
custom_map_mode: 2
|
||||
relocalization_map_abs_path: "/home/user/maps/factory_map.bin"
|
||||
custom_init_pos: [10.5, 2.3, 0.0, 0.0, 0.0, 1.0, 0.0]
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Programmatic API / 编程接口
|
||||
|
||||
### English
|
||||
|
||||
You can also set `init_pos` programmatically using the `lidar_set_custom_parameter` API. This is useful for:
|
||||
- Dynamic relocalization during runtime
|
||||
- Integration with external localization systems
|
||||
- Setting initial pose from robot's last known position
|
||||
|
||||
#### API Function
|
||||
|
||||
```cpp
|
||||
#include "lidar_api.h"
|
||||
|
||||
/**
|
||||
* @brief Set a custom parameter on the device
|
||||
* @param device Device handle obtained from lidar_open_device()
|
||||
* @param param_name Parameter name (e.g., "init_pos")
|
||||
* @param value_data Pointer to the parameter data
|
||||
* @param value_length Size of the data in bytes
|
||||
* @return 0 on success, -1 on error, -2 if file transfer in progress
|
||||
*/
|
||||
int lidar_set_custom_parameter(device_handle device,
|
||||
const char* param_name,
|
||||
const void* value_data,
|
||||
size_t value_length);
|
||||
```
|
||||
|
||||
#### Complete Example
|
||||
|
||||
```cpp
|
||||
#include "lidar_api.h"
|
||||
#include <cstdio>
|
||||
#include <cmath>
|
||||
|
||||
// Helper function to create quaternion from yaw angle (rotation around Z-axis)
|
||||
void yaw_to_quaternion(float yaw_rad, float* qx, float* qy, float* qz, float* qw) {
|
||||
*qx = 0.0f;
|
||||
*qy = 0.0f;
|
||||
*qz = sinf(yaw_rad / 2.0f);
|
||||
*qw = cosf(yaw_rad / 2.0f);
|
||||
}
|
||||
|
||||
int set_init_position(device_handle device,
|
||||
float x, float y, float z,
|
||||
float qx, float qy, float qz, float qw) {
|
||||
// init_pos format: [x, y, z, qx, qy, qz, qw] - 7 floats
|
||||
float init_pos[7] = {x, y, z, qx, qy, qz, qw};
|
||||
|
||||
int result = lidar_set_custom_parameter(
|
||||
device,
|
||||
"init_pos", // Parameter name
|
||||
init_pos, // Data pointer
|
||||
sizeof(init_pos) // 7 * sizeof(float) = 28 bytes
|
||||
);
|
||||
|
||||
if (result == 0) {
|
||||
printf("Successfully set init_pos: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f]\n",
|
||||
x, y, z, qx, qy, qz, qw);
|
||||
} else {
|
||||
printf("Failed to set init_pos, error code: %d\n", result);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
// Usage examples:
|
||||
|
||||
// Example 1: Set position at origin with no rotation
|
||||
void example_origin(device_handle device) {
|
||||
set_init_position(device,
|
||||
0.0f, 0.0f, 0.0f, // x, y, z
|
||||
0.0f, 0.0f, 0.0f, 1.0f // qx, qy, qz, qw (identity)
|
||||
);
|
||||
}
|
||||
|
||||
// Example 2: Set position with 90° yaw rotation
|
||||
void example_with_rotation(device_handle device) {
|
||||
float qx, qy, qz, qw;
|
||||
float yaw_degrees = 90.0f;
|
||||
float yaw_rad = yaw_degrees * M_PI / 180.0f;
|
||||
|
||||
yaw_to_quaternion(yaw_rad, &qx, &qy, &qz, &qw);
|
||||
|
||||
set_init_position(device,
|
||||
5.2f, -3.1f, 0.0f, // x, y, z
|
||||
qx, qy, qz, qw // quaternion from yaw
|
||||
);
|
||||
}
|
||||
|
||||
// Example 3: Set position from external localization system
|
||||
void example_from_external_localization(device_handle device,
|
||||
double ext_x, double ext_y, double ext_yaw) {
|
||||
float qx, qy, qz, qw;
|
||||
yaw_to_quaternion((float)ext_yaw, &qx, &qy, &qz, &qw);
|
||||
|
||||
set_init_position(device,
|
||||
(float)ext_x, (float)ext_y, 0.0f,
|
||||
qx, qy, qz, qw
|
||||
);
|
||||
}
|
||||
```
|
||||
|
||||
#### ROS Integration Example
|
||||
|
||||
```cpp
|
||||
#include "lidar_api.h"
|
||||
#include <geometry_msgs/PoseWithCovarianceStamped.h> // ROS1
|
||||
// or
|
||||
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp> // ROS2
|
||||
|
||||
// Callback for /initialpose topic (from RViz "2D Pose Estimate" tool)
|
||||
void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg,
|
||||
device_handle device) {
|
||||
float init_pos[7] = {
|
||||
(float)msg->pose.pose.position.x,
|
||||
(float)msg->pose.pose.position.y,
|
||||
(float)msg->pose.pose.position.z,
|
||||
(float)msg->pose.pose.orientation.x,
|
||||
(float)msg->pose.pose.orientation.y,
|
||||
(float)msg->pose.pose.orientation.z,
|
||||
(float)msg->pose.pose.orientation.w
|
||||
};
|
||||
|
||||
int result = lidar_set_custom_parameter(device, "init_pos", init_pos, sizeof(init_pos));
|
||||
|
||||
if (result == 0) {
|
||||
ROS_INFO("Set init_pos from RViz: [%.2f, %.2f, %.2f]",
|
||||
init_pos[0], init_pos[1], init_pos[2]);
|
||||
} else {
|
||||
ROS_ERROR("Failed to set init_pos: %d", result);
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
#### Important Notes
|
||||
|
||||
1. **Call timing**: Set `init_pos` **before** starting the stream with `lidar_start_stream()`
|
||||
2. **Map mode**: Ensure `custom_map_mode` is set to `2` (relocalization mode)
|
||||
3. **Map file**: The relocalization map must be set via `lidar_set_relocalization_map()` or YAML config
|
||||
4. **Thread safety**: `lidar_set_custom_parameter` is thread-safe but blocks until response received
|
||||
|
||||
### 中文
|
||||
|
||||
您也可以使用 `lidar_set_custom_parameter` API 以编程方式设置 `init_pos`。适用于：
|
||||
- 运行时动态重定位
|
||||
- 与外部定位系统集成
|
||||
- 从机器人上次已知位置设置初始位姿
|
||||
|
||||
#### API 函数
|
||||
|
||||
```cpp
|
||||
#include "lidar_api.h"
|
||||
|
||||
/**
|
||||
* @brief 在设备上设置自定义参数
|
||||
* @param device 从 lidar_open_device() 获取的设备句柄
|
||||
* @param param_name 参数名称（如 "init_pos"）
|
||||
* @param value_data 指向参数数据的指针
|
||||
* @param value_length 数据大小（字节）
|
||||
* @return 成功返回 0，错误返回 -1，文件传输中返回 -2
|
||||
*/
|
||||
int lidar_set_custom_parameter(device_handle device,
|
||||
const char* param_name,
|
||||
const void* value_data,
|
||||
size_t value_length);
|
||||
```
|
||||
|
||||
#### 完整示例
|
||||
|
||||
```cpp
|
||||
#include "lidar_api.h"
|
||||
#include <cstdio>
|
||||
#include <cmath>
|
||||
|
||||
// 辅助函数：从偏航角（绕 Z 轴旋转）创建四元数
|
||||
void yaw_to_quaternion(float yaw_rad, float* qx, float* qy, float* qz, float* qw) {
|
||||
*qx = 0.0f;
|
||||
*qy = 0.0f;
|
||||
*qz = sinf(yaw_rad / 2.0f);
|
||||
*qw = cosf(yaw_rad / 2.0f);
|
||||
}
|
||||
|
||||
int set_init_position(device_handle device,
|
||||
float x, float y, float z,
|
||||
float qx, float qy, float qz, float qw) {
|
||||
// init_pos 格式: [x, y, z, qx, qy, qz, qw] - 7 个 float
|
||||
float init_pos[7] = {x, y, z, qx, qy, qz, qw};
|
||||
|
||||
int result = lidar_set_custom_parameter(
|
||||
device,
|
||||
"init_pos", // 参数名
|
||||
init_pos, // 数据指针
|
||||
sizeof(init_pos) // 7 * sizeof(float) = 28 字节
|
||||
);
|
||||
|
||||
if (result == 0) {
|
||||
printf("成功设置 init_pos: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f]\n",
|
||||
x, y, z, qx, qy, qz, qw);
|
||||
} else {
|
||||
printf("设置 init_pos 失败，错误码: %d\n", result);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
// 使用示例：
|
||||
|
||||
// 示例 1：在原点设置位置，无旋转
|
||||
void example_origin(device_handle device) {
|
||||
set_init_position(device,
|
||||
0.0f, 0.0f, 0.0f, // x, y, z
|
||||
0.0f, 0.0f, 0.0f, 1.0f // qx, qy, qz, qw（单位四元数）
|
||||
);
|
||||
}
|
||||
|
||||
// 示例 2：设置带 90° 偏航旋转的位置
|
||||
void example_with_rotation(device_handle device) {
|
||||
float qx, qy, qz, qw;
|
||||
float yaw_degrees = 90.0f;
|
||||
float yaw_rad = yaw_degrees * M_PI / 180.0f;
|
||||
|
||||
yaw_to_quaternion(yaw_rad, &qx, &qy, &qz, &qw);
|
||||
|
||||
set_init_position(device,
|
||||
5.2f, -3.1f, 0.0f, // x, y, z
|
||||
qx, qy, qz, qw // 从偏航角计算的四元数
|
||||
);
|
||||
}
|
||||
|
||||
// 示例 3：从外部定位系统设置位置
|
||||
void example_from_external_localization(device_handle device,
|
||||
double ext_x, double ext_y, double ext_yaw) {
|
||||
float qx, qy, qz, qw;
|
||||
yaw_to_quaternion((float)ext_yaw, &qx, &qy, &qz, &qw);
|
||||
|
||||
set_init_position(device,
|
||||
(float)ext_x, (float)ext_y, 0.0f,
|
||||
qx, qy, qz, qw
|
||||
);
|
||||
}
|
||||
```
|
||||
|
||||
#### ROS 集成示例
|
||||
|
||||
```cpp
|
||||
#include "lidar_api.h"
|
||||
#include <geometry_msgs/PoseWithCovarianceStamped.h> // ROS1
|
||||
// 或
|
||||
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp> // ROS2
|
||||
|
||||
// /initialpose 话题的回调函数（来自 RViz 的 "2D Pose Estimate" 工具）
|
||||
void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg,
|
||||
device_handle device) {
|
||||
float init_pos[7] = {
|
||||
(float)msg->pose.pose.position.x,
|
||||
(float)msg->pose.pose.position.y,
|
||||
(float)msg->pose.pose.position.z,
|
||||
(float)msg->pose.pose.orientation.x,
|
||||
(float)msg->pose.pose.orientation.y,
|
||||
(float)msg->pose.pose.orientation.z,
|
||||
(float)msg->pose.pose.orientation.w
|
||||
};
|
||||
|
||||
int result = lidar_set_custom_parameter(device, "init_pos", init_pos, sizeof(init_pos));
|
||||
|
||||
if (result == 0) {
|
||||
ROS_INFO("从 RViz 设置 init_pos: [%.2f, %.2f, %.2f]",
|
||||
init_pos[0], init_pos[1], init_pos[2]);
|
||||
} else {
|
||||
ROS_ERROR("设置 init_pos 失败: %d", result);
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
#### 重要说明
|
||||
|
||||
1. **调用时机**：在调用 `lidar_start_stream()` 启动数据流**之前**设置 `init_pos`
|
||||
2. **地图模式**：确保 `custom_map_mode` 设置为 `2`（重定位模式）
|
||||
3. **地图文件**：必须通过 `lidar_set_relocalization_map()` 或 YAML 配置设置重定位地图
|
||||
4. **线程安全**：`lidar_set_custom_parameter` 是线程安全的，但会阻塞直到收到响应
|
||||
|
||||
---
|
||||
|
||||
## Troubleshooting / 故障排除
|
||||
|
||||
### Relocalization Fails / 重定位失败
|
||||
|
||||
**English**:
|
||||
- Ensure starting position is within recommended range (1m/10°)
|
||||
- Check that the map file path is correct and file exists
|
||||
- Verify the environment hasn't changed significantly since mapping
|
||||
- Try gently moving the device to provide more observations
|
||||
|
||||
**中文**：
|
||||
- 确保起始位置在推荐范围内（1m/10°）
|
||||
- 检查地图文件路径是否正确且文件存在
|
||||
- 验证环境自建图以来没有显著变化
|
||||
- 尝试轻轻移动设备以提供更多观测
|
||||
|
||||
### init_pos Not Taking Effect / init_pos 未生效
|
||||
|
||||
**English**:
|
||||
- Verify `custom_map_mode` is set to `2`
|
||||
- Check that `custom_init_pos` has exactly 7 values
|
||||
- Ensure quaternion is normalized (sum of squares ≈ 1)
|
||||
- Restart the driver after modifying configuration
|
||||
|
||||
**中文**：
|
||||
- 验证 `custom_map_mode` 设置为 `2`
|
||||
- 检查 `custom_init_pos` 是否恰好有 7 个值
|
||||
- 确保四元数已归一化（平方和 ≈ 1）
|
||||
- 修改配置后重启驱动程序
|
||||
|
||||
### TF Not Published / TF 未发布
|
||||
|
||||
**English**:
|
||||
- Relocalization may still be in progress
|
||||
- Check ROS logs for relocalization status messages
|
||||
- System operates in fallback mode until relocalization succeeds
|
||||
|
||||
**中文**：
|
||||
- 重定位可能仍在进行中
|
||||
- 检查 ROS 日志中的重定位状态消息
|
||||
- 系统在重定位成功前以后备模式运行
|
||||
|
||||
### Map File Not Found / 地图文件未找到
|
||||
|
||||
**English**:
|
||||
- Use absolute path (starting with `/`)
|
||||
- Check file permissions
|
||||
- Verify file extension is `.bin`
|
||||
|
||||
**中文**：
|
||||
- 使用绝对路径（以 `/` 开头）
|
||||
- 检查文件权限
|
||||
- 验证文件扩展名为 `.bin`
|
||||
|
||||
---
|
||||
|
||||
## Related Topics / 相关话题
|
||||
|
||||
| Topic | Description |
|
||||
|-------|-------------|
|
||||
| `/odin1/odometry` | Odometry in odom frame |
|
||||
| `/odin1/odometry_highfreq` | High-frequency odometry |
|
||||
| `/odin1/cloud_slam` | SLAM point cloud in odom frame |
|
||||
| `/tf` | Transform tree (includes map→odom after successful relocalization) |
|
||||
|
||||
---
|
||||
|
||||
## See Also / 参见
|
||||
|
||||
- [README.md](README.md) - Main documentation
|
||||
- [config/control_command.yaml](config/control_command.yaml) - Configuration file
|
||||
|
||||
@@ -0,0 +1,30 @@
|
||||
# O1-P040100042
|
||||
cam_num: 1
|
||||
img_topic_0: /camera/rgb
|
||||
Tcl_0: [
|
||||
-0.00916, -0.99996, 0.00032, 0.03215,
|
||||
0.00239, -0.00034, -1.00000, -0.01185,
|
||||
0.99996, -0.00916, 0.00239, 0.00536,
|
||||
0.00000, 0.00000, 0.00000, 1.00000
|
||||
]
|
||||
|
||||
cam_0:
|
||||
cam_model: FishPoly
|
||||
image_width: 1600
|
||||
image_height: 1296
|
||||
k2: 6.5597407531742908e-04
|
||||
k3: -2.8960653254664001e-02
|
||||
k4: 4.5390330144604515e-02
|
||||
k5: -6.4513398102889072e-02
|
||||
k6: 3.8735200610848508e-02
|
||||
k7: -9.9027101865577991e-03
|
||||
p1: 0.
|
||||
p2: 0.
|
||||
A11: 7.3689426240691989e+02
|
||||
A12: -1.6114977429994648e-01
|
||||
A22: 7.3661135398452438e+02
|
||||
u0: 8.0612553451014685e+02
|
||||
v0: 6.3965071026991302e+02
|
||||
isFast: 0
|
||||
numDiff: 3000
|
||||
maxIncidentAngle: 120
|
||||
@@ -0,0 +1,106 @@
|
||||
register_keys:
|
||||
|
||||
# if off, allow connection even if usb connection is below usb 3.0
|
||||
# ATTENTION: usb 3.0 is always recommended, as advance functionality like SLAM mode requires usb 3.0 for reliable map file transfer
|
||||
strict_usb3.0_check: 0 # 0: off: 1: on;
|
||||
|
||||
# 0: use odin internal system time as data time stamp, typical and recommended;
|
||||
# 1: use host ros time (upon receive) as data time stamp, only use if you specifically require this setup, not recommended for most users
|
||||
# 2: align odin1 time to host time, timestamp is the sensor data reception time on host time axis
|
||||
use_host_ros_time: 1
|
||||
|
||||
streamctrl: 1 # 0: off; 1: on
|
||||
|
||||
# original rgb data in jpeg format from device
|
||||
sendrgbcompressed: 1 # 0: off; 1: on
|
||||
|
||||
# RGB data, decoded from original jpeg data from device, bgr8 format
|
||||
# Processed on host device
|
||||
sendrgb: 1 # 0: off; 1: on
|
||||
|
||||
# undistort rgb image processed from decoded rgb data.
|
||||
# depends on sendrgb. related camera parameters can be found in ws/src/odin_ros_driver/config/calib.yaml
|
||||
# Processed on host device
|
||||
sendrgbundistort: 0 # 0: off; 1: on.
|
||||
|
||||
# IMU data
|
||||
sendimu: 1 # 0: off; 1: on
|
||||
|
||||
# SDK IMU smooth sending feature
|
||||
# When enabled, SDK will send IMU data at precise intervals (default 400Hz)
|
||||
# using a dedicated high-priority thread to reduce jitter and timing variance
|
||||
enable_imu_smooth: 1 # 0: disable SDK IMU smooth sending; 1: enable (default)
|
||||
|
||||
# SDK IMU smooth sending frequency in Hz (only effective when enable_imu_smooth = 1)
|
||||
imu_smooth_frequency: 400 # 1-1000 Hz, recommended 400 Hz
|
||||
|
||||
# Odometry data
|
||||
sendodom: 1 # 0: off; 1: on
|
||||
|
||||
# TF from odom to base_link. Leave it on unless you specifically need it off.
|
||||
# ATTENTION: critical for rviz to show cloud_raw.
|
||||
send_odom_baselink_tf: 0 # 0: off; 1: on.
|
||||
|
||||
# raw dtof data
|
||||
senddtof: 1 # 0: off; 1: on
|
||||
cloud_raw_confidence_threshold: 35 # please refer to readme for more details
|
||||
|
||||
# dtof sensor frame rate. Supported values: 100 (10fps) or 145 (14.5fps)
|
||||
# Higher frame rate provides smoother point cloud data but may increase data bandwidth
|
||||
# Note: value is multiplied by 10 (e.g., 145 means 14.5fps)
|
||||
dtof_fps: 100 # 100: 10fps; 145: 14.5fps
|
||||
|
||||
# slam cloud data
|
||||
sendcloudslam: 1 # 0: off; 1: on
|
||||
|
||||
# processed with raw point cloud, rgb image, and calib.yaml from device
|
||||
# Processed on host device
|
||||
sendcloudrender: 1 # 0: off; 1: on
|
||||
|
||||
# depth completion demo, high computing resource usage.
|
||||
# for more information please refer to the readme file.
|
||||
# Processed on host device
|
||||
senddepth: 0 # 0: off; 1: on
|
||||
|
||||
# cloud reprojection demo, projects cloud_slam to camera image using odometry
|
||||
# Processed on host device
|
||||
sendreprojection: 0 # 0: off; 1: on
|
||||
|
||||
# image overlay settings - overlays reprojected points on camera image
|
||||
# Processed on host device
|
||||
sendoverlay: 0 # 0: off; 1: on
|
||||
overlay_reprojected_topic: "/odin1/reprojected_image" # reprojected image topic
|
||||
overlay_camera_topic: "/odin1/image/undistorted" # camera image topic (undistorted)
|
||||
overlay_output_topic: "/odin1/overlay_image" # output overlay image topic
|
||||
overlay_alpha: 0.6 # blend alpha (0.0-1.0, higher = more reproj color)
|
||||
|
||||
# record rgb, odometry, and slam cloud data as proprietary olx format for further processing in MindCloud(TM) software.
|
||||
# save path: ws/src/odin_ros_driver/recorddata/{record_start_time}/
|
||||
# ATTENTION: please copy the full folder for post-processing.
|
||||
recorddata: 0 # 0: off; 1: on
|
||||
|
||||
# Save device runtime status info to ws/src/odin_ros_driver/log/Driver_{drvier_start_time}/Conn_{device_connection_time}/dev_status.csv
|
||||
devstatuslog: 1 # 0: off; 1: on.
|
||||
|
||||
save_log: 0 # 0: off; 1: on;
|
||||
|
||||
# raw dtof sensor intensity data in gray format, mostly for debug purpose.
|
||||
pubintensitygray: 0 # 0: off; 1: on
|
||||
|
||||
showpath: 0 # 0: off; 1: on
|
||||
showcamerapose: 0 # 0: off; 1: on
|
||||
|
||||
custom_map_mode: 2 # 0: Odometry mode 1: SLAM mode 2: Relocalization mode
|
||||
custom_init_pos: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
|
||||
relocalization_map_abs_path: "/absolute/path/to/map_a.bin" # supply the external Odin map before using Relocalization mode
|
||||
|
||||
# To get the mapping result file, please use the set_param.sh script provided: "./set_param.sh save_map 1"
|
||||
mapping_result_dest_dir: "" # "": if not specified, save to default location of {ws}/src/odin_ros_driver/map/{driver_start_time}/
|
||||
mapping_result_file_name: "" # "": if not specified, save to location above with default file name of map_{map_save_time}.bin
|
||||
|
||||
# Image mask transfer settings
|
||||
sendimagemask: 0 # 0: off; 1: on - transfer image mask to device on startup
|
||||
image_mask_abs_path: "" # absolute path to the image mask file (e.g., /path/to/mask.png(1600x1296 resolution)
|
||||
|
||||
# Algorithm reset settings
|
||||
resetalgo: 0 # 0: off; 1: on - send algo_reset command to device on startup
|
||||
@@ -0,0 +1,438 @@
|
||||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 0
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Odometry1
|
||||
- /slam1
|
||||
- /TF1/Frames1
|
||||
Splitter Ratio: 0.4993045926094055
|
||||
Tree Height: 495
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
Expanded:
|
||||
- /2D Pose Estimate1
|
||||
- /2D Nav Goal1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz/Time
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: Image
|
||||
Preferences:
|
||||
PromptSaveOnExit: true
|
||||
Toolbars:
|
||||
toolButtonStyle: 2
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Class: rviz/Image
|
||||
Enabled: true
|
||||
Image Topic: /odin1/image
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Image
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: raw
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Class: rviz/Image
|
||||
Enabled: false
|
||||
Image Topic: /odin1/image/undistorted
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Image_undistort
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: raw
|
||||
Unreliable: false
|
||||
Value: false
|
||||
- Class: rviz/Image
|
||||
Enabled: false
|
||||
Image Topic: /odin1/reprojected_image
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: cloudslam_reprojected
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: raw
|
||||
Unreliable: false
|
||||
Value: false
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: Intensity
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Min Color: 0; 0; 0
|
||||
Name: raw
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic: /odin1/cloud_raw
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 1.8493094444274902
|
||||
Min Value: -0.13891705870628357
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Min Color: 0; 0; 0
|
||||
Name: render
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic: /odin1/cloud_render
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Angle Tolerance: 0.10000000149011612
|
||||
Class: rviz/Odometry
|
||||
Covariance:
|
||||
Orientation:
|
||||
Alpha: 0.5
|
||||
Color: 255; 255; 127
|
||||
Color Style: Unique
|
||||
Frame: Local
|
||||
Offset: 1
|
||||
Scale: 1
|
||||
Value: true
|
||||
Position:
|
||||
Alpha: 0.30000001192092896
|
||||
Color: 204; 51; 204
|
||||
Scale: 1
|
||||
Value: true
|
||||
Value: false
|
||||
Enabled: true
|
||||
Keep: 1
|
||||
Name: Odometry
|
||||
Position Tolerance: 0.10000000149011612
|
||||
Queue Size: 1
|
||||
Shape:
|
||||
Alpha: 1
|
||||
Axes Length: 1
|
||||
Axes Radius: 0.10000000149011612
|
||||
Color: 255; 25; 0
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Value: Axes
|
||||
Topic: /odin1/odometry
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Angle Tolerance: 0.10000000149011612
|
||||
Class: rviz/Odometry
|
||||
Covariance:
|
||||
Orientation:
|
||||
Alpha: 0.5
|
||||
Color: 255; 255; 127
|
||||
Color Style: Unique
|
||||
Frame: Local
|
||||
Offset: 1
|
||||
Scale: 1
|
||||
Value: true
|
||||
Position:
|
||||
Alpha: 0.30000001192092896
|
||||
Color: 204; 51; 204
|
||||
Scale: 1
|
||||
Value: true
|
||||
Value: true
|
||||
Enabled: false
|
||||
Keep: 100
|
||||
Name: Odometry_high
|
||||
Position Tolerance: 0.10000000149011612
|
||||
Queue Size: 10
|
||||
Shape:
|
||||
Alpha: 1
|
||||
Axes Length: 1
|
||||
Axes Radius: 0.10000000149011612
|
||||
Color: 52; 101; 164
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Value: Arrow
|
||||
Topic: /odin1/odometry_highfreq
|
||||
Unreliable: false
|
||||
Value: false
|
||||
- Class: rviz/MarkerArray
|
||||
Enabled: false
|
||||
Marker Topic: /odin1/camera_pose_visual
|
||||
Name: camera_view
|
||||
Namespaces:
|
||||
{}
|
||||
Queue Size: 100
|
||||
Value: false
|
||||
Enabled: true
|
||||
Name: Odometry
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: rgb
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Min Color: 0; 0; 0
|
||||
Name: slam_current
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic: /odin1/cloud_slam
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 0.10000000149011612
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 5
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Min Color: 0; 0; 0
|
||||
Name: slam_decay
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 1
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic: /odin1/cloud_slam
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
- Class: rviz/MarkerArray
|
||||
Enabled: false
|
||||
Marker Topic: /odin1/path
|
||||
Name: path
|
||||
Namespaces:
|
||||
{}
|
||||
Queue Size: 100
|
||||
Value: false
|
||||
Enabled: true
|
||||
Name: slam
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Class: rviz/Image
|
||||
Enabled: true
|
||||
Image Topic: /odin1/depth_img_competetion
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: dense_depth_image
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: raw
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Min Color: 0; 0; 0
|
||||
Name: dense_depth_cloud
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic: /odin1/depth_img_competetion_cloud
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
Enabled: false
|
||||
Name: dense_depth_demo
|
||||
- Class: rviz/TF
|
||||
Enabled: true
|
||||
Filter (blacklist): ""
|
||||
Filter (whitelist): ""
|
||||
Frame Timeout: 15
|
||||
Frames:
|
||||
All Enabled: true
|
||||
odin1_base_link:
|
||||
Value: true
|
||||
odom:
|
||||
Value: true
|
||||
Marker Alpha: 1
|
||||
Marker Scale: 1
|
||||
Name: TF
|
||||
Show Arrows: true
|
||||
Show Axes: true
|
||||
Show Names: true
|
||||
Tree:
|
||||
odom:
|
||||
odin1_base_link:
|
||||
{}
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Default Light: true
|
||||
Fixed Frame: odom
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz/MoveCamera
|
||||
- Class: rviz/Select
|
||||
- Class: rviz/FocusCamera
|
||||
- Class: rviz/Measure
|
||||
- Class: rviz/SetInitialPose
|
||||
Theta std deviation: 0.2617993950843811
|
||||
Topic: /initialpose
|
||||
X std deviation: 0.5
|
||||
Y std deviation: 0.5
|
||||
- Class: rviz/SetGoal
|
||||
Topic: /move_base_simple/goal
|
||||
- Class: rviz/PublishPoint
|
||||
Single click: true
|
||||
Topic: /clicked_point
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz/Orbit
|
||||
Distance: 16.25591278076172
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Field of View: 0.7853981852531433
|
||||
Focal Point:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Focal Shape Fixed Size: false
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 1.010398030281067
|
||||
Target Frame: odom
|
||||
Yaw: 0.8753980994224548
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 1016
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
Image:
|
||||
collapsed: false
|
||||
Image_undistort:
|
||||
collapsed: false
|
||||
QMainWindow State: 000000ff00000000fd0000000400000000000001e70000033afc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000022c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000026f000001080000001600fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d006100670065000000047d000001730000001600fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f007200740000000568000000df0000001600fffffffb0000002a0063006c006f007500640073006c0061006d005f0072006500700072006f006a0065006300740065006400000002b2000000c50000001600ffffff000000010000015f0000033afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000033a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000005efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000003e60000033a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1848
|
||||
X: 72
|
||||
Y: 27
|
||||
cloudslam_reprojected:
|
||||
collapsed: false
|
||||
dense_depth_image:
|
||||
collapsed: false
|
||||
@@ -0,0 +1,499 @@
|
||||
Panels:
|
||||
- Class: rviz_common/Displays
|
||||
Help Height: 0
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
- /Image1/Topic1
|
||||
- /cloudslam_reprojected1
|
||||
- /Odometry1
|
||||
- /slam1
|
||||
- /dense_depth_demo1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 593
|
||||
- Class: rviz_common/Selection
|
||||
Name: Selection
|
||||
- Class: rviz_common/Tool Properties
|
||||
Expanded:
|
||||
- /2D Goal Pose1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz_common/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz_default_plugins/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Class: rviz_default_plugins/Image
|
||||
Enabled: true
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Image
|
||||
Normalize Range: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/image
|
||||
Value: true
|
||||
- Class: rviz_default_plugins/Image
|
||||
Enabled: false
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Image_undistort
|
||||
Normalize Range: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/image/undistorted
|
||||
Value: false
|
||||
- Class: rviz_default_plugins/Image
|
||||
Enabled: false
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: cloudslam_reprojected
|
||||
Normalize Range: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/reprojected_image
|
||||
Value: false
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz_default_plugins/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: Intensity
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 239
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: raw
|
||||
Position Transformer: XYZ
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/cloud_raw
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 1.8730175495147705
|
||||
Min Value: -0.12252448499202728
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: rgb
|
||||
Class: rviz_default_plugins/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 2.3509885615147286e-38
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: render
|
||||
Position Transformer: XYZ
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/cloud_render
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
- Class: rviz_common/Group
|
||||
Displays:
|
||||
- Angle Tolerance: 0.10000000149011612
|
||||
Class: rviz_default_plugins/Odometry
|
||||
Covariance:
|
||||
Orientation:
|
||||
Alpha: 0.5
|
||||
Color: 255; 255; 127
|
||||
Color Style: Unique
|
||||
Frame: Local
|
||||
Offset: 1
|
||||
Scale: 1
|
||||
Value: true
|
||||
Position:
|
||||
Alpha: 0.30000001192092896
|
||||
Color: 204; 51; 204
|
||||
Scale: 1
|
||||
Value: true
|
||||
Value: true
|
||||
Enabled: true
|
||||
Keep: 1
|
||||
Name: Odometry
|
||||
Position Tolerance: 0.10000000149011612
|
||||
Shape:
|
||||
Alpha: 1
|
||||
Axes Length: 1
|
||||
Axes Radius: 0.10000000149011612
|
||||
Color: 255; 25; 0
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Value: Axes
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/odometry
|
||||
Value: true
|
||||
- Angle Tolerance: 0.10000000149011612
|
||||
Class: rviz_default_plugins/Odometry
|
||||
Covariance:
|
||||
Orientation:
|
||||
Alpha: 0.5
|
||||
Color: 255; 255; 127
|
||||
Color Style: Unique
|
||||
Frame: Local
|
||||
Offset: 1
|
||||
Scale: 1
|
||||
Value: true
|
||||
Position:
|
||||
Alpha: 0.30000001192092896
|
||||
Color: 204; 51; 204
|
||||
Scale: 1
|
||||
Value: true
|
||||
Value: true
|
||||
Enabled: false
|
||||
Keep: 10
|
||||
Name: Odometry_high
|
||||
Position Tolerance: 0.10000000149011612
|
||||
Shape:
|
||||
Alpha: 1
|
||||
Axes Length: 1
|
||||
Axes Radius: 0.10000000149011612
|
||||
Color: 98; 160; 234
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Value: Arrow
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/odometry_highfreq
|
||||
Value: false
|
||||
- Class: rviz_default_plugins/MarkerArray
|
||||
Enabled: false
|
||||
Name: camera_view
|
||||
Namespaces:
|
||||
{}
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/camera_pose_visual
|
||||
Value: false
|
||||
Enabled: true
|
||||
Name: Odometry
|
||||
- Class: rviz_common/Group
|
||||
Displays:
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz_default_plugins/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: slam_current
|
||||
Position Transformer: XYZ
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/cloud_slam
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 0.10000000149011612
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz_default_plugins/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 10
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: slam_deacy
|
||||
Position Transformer: XYZ
|
||||
Selectable: true
|
||||
Size (Pixels): 1
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/cloud_slam
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
- Class: rviz_default_plugins/MarkerArray
|
||||
Enabled: false
|
||||
Name: path
|
||||
Namespaces:
|
||||
{}
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/path
|
||||
Value: false
|
||||
Enabled: true
|
||||
Name: slam
|
||||
- Class: rviz_common/Group
|
||||
Displays:
|
||||
- Class: rviz_default_plugins/Image
|
||||
Enabled: true
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: dense_depth_image
|
||||
Normalize Range: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/depth_img_competetion
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz_default_plugins/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: dense_depth_cloud
|
||||
Position Transformer: XYZ
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Flat Squares
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/depth_img_competetion_cloud
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
Enabled: false
|
||||
Name: dense_depth_demo
|
||||
- Class: rviz_default_plugins/TF
|
||||
Enabled: true
|
||||
Frame Timeout: 15
|
||||
Frames:
|
||||
All Enabled: true
|
||||
odin1_base_link:
|
||||
Value: true
|
||||
odom:
|
||||
Value: true
|
||||
Marker Scale: 1
|
||||
Name: TF
|
||||
Show Arrows: true
|
||||
Show Axes: true
|
||||
Show Names: false
|
||||
Tree:
|
||||
odom:
|
||||
odin1_base_link:
|
||||
{}
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Fixed Frame: odom
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz_default_plugins/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz_default_plugins/MoveCamera
|
||||
- Class: rviz_default_plugins/Select
|
||||
- Class: rviz_default_plugins/FocusCamera
|
||||
- Class: rviz_default_plugins/Measure
|
||||
Line color: 128; 128; 0
|
||||
- Class: rviz_default_plugins/SetInitialPose
|
||||
Covariance x: 0.25
|
||||
Covariance y: 0.25
|
||||
Covariance yaw: 0.06853891909122467
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /initialpose
|
||||
- Class: rviz_default_plugins/SetGoal
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /goal_pose
|
||||
- Class: rviz_default_plugins/PublishPoint
|
||||
Single click: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /clicked_point
|
||||
Transformation:
|
||||
Current:
|
||||
Class: rviz_default_plugins/TF
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz_default_plugins/Orbit
|
||||
Distance: 10.9336576461792
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 0.2568470537662506
|
||||
Y: 2.1451337337493896
|
||||
Z: 0.3774382472038269
|
||||
Focal Shape Fixed Size: false
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.3953983187675476
|
||||
Target Frame: odom
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 3.230407953262329
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 1016
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
Image:
|
||||
collapsed: false
|
||||
Image_undistort:
|
||||
collapsed: false
|
||||
QMainWindow State: 000000ff00000000fd0000000400000000000001d40000039efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000028e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002d10000010a0000002800fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d006100670065000000023e0000007a0000002800fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f00720074000000028f0000014c0000002800fffffffb0000002a0063006c006f007500640073006c0061006d005f0072006500700072006f006a006500630074006500640000000310000000cb0000002800ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004910000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1920
|
||||
X: 540
|
||||
Y: 124
|
||||
cloudslam_reprojected:
|
||||
collapsed: false
|
||||
dense_depth_image:
|
||||
collapsed: false
|
||||
+78
@@ -0,0 +1,78 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#ifdef ROS2
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <std_msgs/msg/color_rgba.hpp>
|
||||
#include <std_msgs/msg/header.hpp>
|
||||
#include <visualization_msgs/msg/marker.hpp>
|
||||
#include <visualization_msgs/msg/marker_array.hpp>
|
||||
#else
|
||||
#include <ros/ros.h>
|
||||
#include <std_msgs/ColorRGBA.h>
|
||||
#include <std_msgs/Header.h>
|
||||
#include <visualization_msgs/Marker.h>
|
||||
#include <visualization_msgs/MarkerArray.h>
|
||||
#endif
|
||||
#include <Eigen/Dense>
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
class camera_pose_visualization {
|
||||
public:
|
||||
std::string m_marker_ns;
|
||||
|
||||
camera_pose_visualization(float r, float g, float b, float a);
|
||||
|
||||
void setImageBoundaryColor(float r, float g, float b, float a = 1.0);
|
||||
void setOpticalCenterConnectorColor(float r, float g, float b, float a = 1.0);
|
||||
void setScale(double s);
|
||||
void setLineWidth(double width);
|
||||
|
||||
void add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q);
|
||||
void reset();
|
||||
|
||||
#ifdef ROS2
|
||||
using ColorRGBA = std_msgs::msg::ColorRGBA;
|
||||
using Marker = visualization_msgs::msg::Marker;
|
||||
using MarkerArray = visualization_msgs::msg::MarkerArray;
|
||||
using Header = std_msgs::msg::Header;
|
||||
using Publisher = rclcpp::Publisher<MarkerArray>;
|
||||
#else
|
||||
using ColorRGBA = std_msgs::ColorRGBA;
|
||||
using Marker = visualization_msgs::Marker;
|
||||
using MarkerArray = visualization_msgs::MarkerArray;
|
||||
using Header = std_msgs::Header;
|
||||
using Publisher = ros::Publisher;
|
||||
#endif
|
||||
|
||||
void publish_by(Publisher& pub, const Header& header);
|
||||
void add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1);
|
||||
void add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1);
|
||||
private:
|
||||
std::vector<Marker> m_markers;
|
||||
ColorRGBA m_image_boundary_color;
|
||||
ColorRGBA m_optical_center_connector_color;
|
||||
double m_scale;
|
||||
double m_line_width;
|
||||
|
||||
static const Eigen::Vector3d imlt;
|
||||
static const Eigen::Vector3d imlb;
|
||||
static const Eigen::Vector3d imrt;
|
||||
static const Eigen::Vector3d imrb;
|
||||
static const Eigen::Vector3d oc ;
|
||||
static const Eigen::Vector3d lt0 ;
|
||||
static const Eigen::Vector3d lt1 ;
|
||||
static const Eigen::Vector3d lt2 ;
|
||||
};
|
||||
+112
@@ -0,0 +1,112 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#ifdef ROS2
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <nav_msgs/msg/odometry.hpp>
|
||||
#include <image_transport/image_transport.hpp>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/time_synchronizer.h>
|
||||
#include <message_filters/sync_policies/exact_time.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#else
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/Image.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/time_synchronizer.h>
|
||||
#include <message_filters/sync_policies/exact_time.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#endif
|
||||
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
|
||||
#include "cloud_reprojector.hpp"
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#ifdef ROS2
|
||||
class CloudReprojectionRosNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
CloudReprojectionRosNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
|
||||
|
||||
private:
|
||||
using PointCloud2 = sensor_msgs::msg::PointCloud2;
|
||||
using Odometry = nav_msgs::msg::Odometry;
|
||||
using Image = sensor_msgs::msg::Image;
|
||||
|
||||
std::string cloud_slam_topic_;
|
||||
std::string odometry_topic_;
|
||||
std::string wiwc_topic_;
|
||||
std::string reprojected_image_topic_;
|
||||
|
||||
message_filters::Subscriber<PointCloud2> cloud_sub_;
|
||||
message_filters::Subscriber<Odometry> odom_sub_;
|
||||
message_filters::Subscriber<Odometry> wiwc_sub_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<PointCloud2, Odometry, Odometry> MySyncPolicy;
|
||||
typedef message_filters::Synchronizer<MySyncPolicy> Sync;
|
||||
std::shared_ptr<Sync> sync_;
|
||||
|
||||
image_transport::Publisher reprojected_image_pub_;
|
||||
|
||||
std::unique_ptr<CloudReprojector> reprojector_;
|
||||
|
||||
void loadParameters();
|
||||
void syncCallback(const PointCloud2::ConstSharedPtr& cloud_msg,
|
||||
const Odometry::ConstSharedPtr& odom_msg,
|
||||
const Odometry::ConstSharedPtr& wiwc_msg);
|
||||
};
|
||||
#else
|
||||
class CloudReprojectionRosNode
|
||||
{
|
||||
public:
|
||||
CloudReprojectionRosNode(ros::NodeHandle& nh, ros::NodeHandle& pnh);
|
||||
|
||||
private:
|
||||
ros::NodeHandle nh_, pnh_;
|
||||
|
||||
std::string cloud_slam_topic_;
|
||||
std::string odometry_topic_;
|
||||
std::string wiwc_topic_;
|
||||
std::string reprojected_image_topic_;
|
||||
|
||||
message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub_;
|
||||
message_filters::Subscriber<nav_msgs::Odometry> odom_sub_;
|
||||
message_filters::Subscriber<nav_msgs::Odometry> wiwc_sub_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, nav_msgs::Odometry, nav_msgs::Odometry> MySyncPolicy;
|
||||
typedef message_filters::Synchronizer<MySyncPolicy> Sync;
|
||||
std::shared_ptr<Sync> sync_;
|
||||
|
||||
ros::Publisher reprojected_image_pub_;
|
||||
|
||||
std::unique_ptr<CloudReprojector> reprojector_;
|
||||
|
||||
void loadParameters();
|
||||
void syncCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg,
|
||||
const nav_msgs::OdometryConstPtr& odom_msg,
|
||||
const nav_msgs::OdometryConstPtr& wiwc_msg);
|
||||
};
|
||||
#endif
|
||||
@@ -0,0 +1,86 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <Eigen/Dense>
|
||||
#include <Eigen/Geometry>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/common/transforms.h>
|
||||
|
||||
#include "polynomial_camera.hpp"
|
||||
|
||||
#include <memory>
|
||||
|
||||
class CloudReprojector
|
||||
{
|
||||
public:
|
||||
struct CameraParams
|
||||
{
|
||||
int image_width = 1600;
|
||||
int image_height = 1296;
|
||||
double A11 = 0.0, A12 = 0.0, A22 = 0.0;
|
||||
double u0 = 0.0, v0 = 0.0;
|
||||
double k2 = 0.0, k3 = 0.0, k4 = 0.0, k5 = 0.0, k6 = 0.0, k7 = 0.0;
|
||||
};
|
||||
|
||||
struct ExtrinsicParams
|
||||
{
|
||||
Eigen::Matrix4d Tcl = Eigen::Matrix4d::Identity(); // camera to lidar
|
||||
Eigen::Matrix4d Til = Eigen::Matrix4d::Identity(); // lidar to imu (fixed)
|
||||
Eigen::Matrix4d Tic = Eigen::Matrix4d::Identity(); // camera to imu (calculated)
|
||||
};
|
||||
|
||||
struct OdomPose
|
||||
{
|
||||
Eigen::Quaterniond orientation = Eigen::Quaterniond::Identity();
|
||||
Eigen::Vector3d position = Eigen::Vector3d::Zero();
|
||||
};
|
||||
|
||||
CloudReprojector();
|
||||
~CloudReprojector() = default;
|
||||
|
||||
bool initialize(const CameraParams& cam_params, const ExtrinsicParams& ext_params);
|
||||
|
||||
cv::Mat reprojectCloud(const pcl::PointCloud<pcl::PointXYZRGB>& cloud_odom,
|
||||
const OdomPose& odom_pose);
|
||||
|
||||
void setPointRadius(int radius) { point_radius_ = radius; }
|
||||
int getPointRadius() const { return point_radius_; }
|
||||
|
||||
const CameraParams& getCameraParams() const { return camera_params_; }
|
||||
const ExtrinsicParams& getExtrinsicParams() const { return extrinsic_params_; }
|
||||
|
||||
// Update extrinsic parameters at runtime with real-time values from module
|
||||
void updateExtrinsics(const Eigen::Matrix4d& Tcl, const Eigen::Matrix4d& Til) {
|
||||
extrinsic_params_.Tcl = Tcl;
|
||||
extrinsic_params_.Til = Til;
|
||||
extrinsic_params_.Tic = calculateTic(Tcl, Til);
|
||||
}
|
||||
|
||||
static Eigen::Matrix4d calculateTic(const Eigen::Matrix4d& Tcl, const Eigen::Matrix4d& Til);
|
||||
|
||||
private:
|
||||
Eigen::Matrix4d odomPoseToMatrix(const OdomPose& odom) const;
|
||||
|
||||
cv::Mat projectCloudToImage(const pcl::PointCloud<pcl::PointXYZRGB>& cloud_in_cam) const;
|
||||
|
||||
CameraParams camera_params_;
|
||||
ExtrinsicParams extrinsic_params_;
|
||||
std::unique_ptr<mini_vikit::PolynomialCamera> camera_model_;
|
||||
|
||||
int point_radius_ = 4;
|
||||
bool initialized_ = false;
|
||||
};
|
||||
@@ -0,0 +1,240 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <thread>
|
||||
#include <mutex>
|
||||
#include <condition_variable>
|
||||
#include <atomic>
|
||||
#include <deque>
|
||||
#include <filesystem>
|
||||
#include <fstream>
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
#include <cstdlib>
|
||||
#include <ctime>
|
||||
#include <sstream>
|
||||
|
||||
class BinaryDataLogger {
|
||||
public:
|
||||
struct Options {
|
||||
public:
|
||||
// Number of frames per batch before flushing to disk
|
||||
size_t batch_size = 20;
|
||||
// Base directory to place time-stamped run directory under
|
||||
// If empty, will use $ODIN_DATA_DIR or default to $HOME/OdinData
|
||||
std::filesystem::path base_dir;
|
||||
};
|
||||
|
||||
explicit BinaryDataLogger(const Options& opts) {
|
||||
namespace fs = std::filesystem;
|
||||
// Determine base dir
|
||||
fs::path base = opts.base_dir;
|
||||
if (base.empty()) {
|
||||
const char* env_dir = std::getenv("ODIN_DATA_DIR");
|
||||
if (env_dir && *env_dir) {
|
||||
base = fs::path(env_dir);
|
||||
} else {
|
||||
const char* home = std::getenv("HOME");
|
||||
base = home && *home ? fs::path(home) / "OdinData" : fs::current_path() / "OdinData";
|
||||
}
|
||||
}
|
||||
// Make timestamp folder name: YYYYMMDD_HHMMSS
|
||||
auto now = std::chrono::system_clock::now();
|
||||
std::time_t t = std::chrono::system_clock::to_time_t(now);
|
||||
std::tm tm{};
|
||||
#ifdef _WIN32
|
||||
localtime_s(&tm, &t);
|
||||
#else
|
||||
localtime_r(&t, &tm);
|
||||
#endif
|
||||
char buf[32];
|
||||
std::strftime(buf, sizeof(buf), "%Y%m%d_%H%M%S", &tm);
|
||||
root_dir_ = base / buf;
|
||||
fs::create_directories(root_dir_);
|
||||
fs::create_directories(root_dir_ / "image");
|
||||
// Convert to std::string
|
||||
std::string timestamp(buf);
|
||||
created_at_ = timestamp;
|
||||
// Compose file name
|
||||
std::filesystem::path pcFile = root_dir_ / ("MT" + timestamp + ".olx");
|
||||
// Create placeholder files (device_id / firmware / algorithm filled later via update_info_file)
|
||||
write_info_file_unlocked();
|
||||
write_text_file(root_dir_ / "image" / "cam_in_ex.txt", "# camera intrinsics/extrinsics TBD\n");
|
||||
|
||||
// Init writers
|
||||
pose_writer_ = std::make_unique<Writer>(root_dir_ / "OdinPose.bin", opts.batch_size);
|
||||
cloud_writer_ = std::make_unique<Writer>(pcFile, opts.batch_size);
|
||||
//cloud_writer_ = std::make_unique<Writer>(root_dir_ / "OdinPointCloud.olx", opts.batch_size);
|
||||
image_writer_ = std::make_unique<Writer>(root_dir_ / "OdinImage.bin", opts.batch_size);
|
||||
roatation_writer_ = std::make_unique<Writer>(root_dir_ / "OdinRotate.bin", opts.batch_size);
|
||||
imu_writer_ = std::make_unique<Writer>(root_dir_ / "OdinIMU.bin", opts.batch_size);
|
||||
}
|
||||
|
||||
~BinaryDataLogger() {
|
||||
// Ensure all writers flush on destruction
|
||||
if (pose_writer_) pose_writer_->shutdown();
|
||||
if (cloud_writer_) cloud_writer_->shutdown();
|
||||
if (image_writer_) image_writer_->shutdown();
|
||||
if (roatation_writer_) roatation_writer_->shutdown();
|
||||
if (imu_writer_) imu_writer_->shutdown();
|
||||
}
|
||||
|
||||
const std::filesystem::path& root_dir() const { return root_dir_; }
|
||||
|
||||
// Update info.txt with device_id (SN) / firmware (SoC) / algorithm version.
|
||||
// Safe to call multiple times; latest values win.
|
||||
void update_info_file(const std::string& device_id,
|
||||
const std::string& firmware_version,
|
||||
const std::string& algorithm_version) {
|
||||
std::lock_guard<std::mutex> lk(info_mtx_);
|
||||
device_id_ = device_id;
|
||||
firmware_version_ = firmware_version;
|
||||
algorithm_version_ = algorithm_version;
|
||||
write_info_file_unlocked();
|
||||
}
|
||||
|
||||
// Enqueue ready-to-write frame blobs (already formatted as per spec)
|
||||
void enqueuePoseFrame(std::vector<uint8_t>&& blob) {
|
||||
if (pose_writer_) pose_writer_->enqueue(std::move(blob));
|
||||
}
|
||||
void enqueuePointCloudFrame(std::vector<uint8_t>&& blob) {
|
||||
if (cloud_writer_) cloud_writer_->enqueue(std::move(blob));
|
||||
}
|
||||
void enqueueImageFrame(std::vector<uint8_t>&& blob) {
|
||||
if (image_writer_) image_writer_->enqueue(std::move(blob));
|
||||
}
|
||||
void enqueueRotateFrame(std::vector<uint8_t>&& blob) {
|
||||
if (roatation_writer_) roatation_writer_->enqueue(std::move(blob));
|
||||
}
|
||||
void enqueueIMUFrame(std::vector<uint8_t>&& blob) {
|
||||
if (imu_writer_) imu_writer_->enqueue(std::move(blob));
|
||||
}
|
||||
|
||||
private:
|
||||
struct Writer {
|
||||
explicit Writer(const std::filesystem::path& filepath, size_t batch)
|
||||
: file_path(filepath), batch_size(batch), stop(false) {
|
||||
worker = std::thread([this]() { this->run(); });
|
||||
}
|
||||
~Writer() {
|
||||
shutdown();
|
||||
}
|
||||
void enqueue(std::vector<uint8_t>&& frame) {
|
||||
{
|
||||
std::lock_guard<std::mutex> lk(mtx);
|
||||
pending.emplace_back(std::move(frame));
|
||||
if (pending.size() >= batch_size) {
|
||||
swap_pending_unlocked();
|
||||
}
|
||||
}
|
||||
cv.notify_one();
|
||||
}
|
||||
void shutdown() {
|
||||
{
|
||||
std::lock_guard<std::mutex> lk(mtx);
|
||||
if (stop) return;
|
||||
// Move leftovers to write buffer
|
||||
if (!pending.empty()) {
|
||||
swap_pending_unlocked();
|
||||
}
|
||||
stop = true;
|
||||
}
|
||||
cv.notify_one();
|
||||
if (worker.joinable()) worker.join();
|
||||
}
|
||||
private:
|
||||
void swap_pending_unlocked() {
|
||||
if (!pending.empty()) {
|
||||
write_queue.emplace_back(std::move(pending));
|
||||
pending.clear();
|
||||
}
|
||||
}
|
||||
void run() {
|
||||
std::ofstream out(file_path, std::ios::binary | std::ios::app);
|
||||
if (!out.is_open()) {
|
||||
// If file can't be opened, silently drop (or could add logging hook)
|
||||
return;
|
||||
}
|
||||
for (;;) {
|
||||
std::vector<std::vector<uint8_t>> batch;
|
||||
{
|
||||
std::unique_lock<std::mutex> lk(mtx);
|
||||
cv.wait(lk, [&]{ return stop || !write_queue.empty(); });
|
||||
if (!write_queue.empty()) {
|
||||
batch = std::move(write_queue.front());
|
||||
write_queue.pop_front();
|
||||
} else if (stop) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!batch.empty()) {
|
||||
for (auto& frame : batch) {
|
||||
if (!frame.empty()) {
|
||||
out.write(reinterpret_cast<const char*>(frame.data()), static_cast<std::streamsize>(frame.size()));
|
||||
}
|
||||
}
|
||||
out.flush();
|
||||
}
|
||||
}
|
||||
}
|
||||
std::filesystem::path file_path;
|
||||
size_t batch_size;
|
||||
std::mutex mtx;
|
||||
std::condition_variable cv;
|
||||
std::vector<std::vector<uint8_t>> pending; // accumulate frames
|
||||
std::deque<std::vector<std::vector<uint8_t>>> write_queue; // queued batches
|
||||
std::thread worker;
|
||||
bool stop;
|
||||
};
|
||||
|
||||
static void write_text_file(const std::filesystem::path& p, const std::string& content) {
|
||||
std::ofstream f(p, std::ios::out | std::ios::trunc);
|
||||
if (f.is_open()) {
|
||||
f << content;
|
||||
}
|
||||
}
|
||||
|
||||
// Render info.txt from current member fields. Caller must hold info_mtx_.
|
||||
void write_info_file_unlocked() {
|
||||
std::ostringstream oss;
|
||||
oss << "device=OdinOne\n"
|
||||
<< "pointcloud=xyzrgbi\n";
|
||||
if (!device_id_.empty()) {
|
||||
oss << "device_id=" << device_id_ << "\n";
|
||||
}
|
||||
if (!firmware_version_.empty()) {
|
||||
oss << "firmware_version=" << firmware_version_ << "\n";
|
||||
}
|
||||
if (!algorithm_version_.empty()) {
|
||||
oss << "algorithm_version=" << algorithm_version_ << "\n";
|
||||
}
|
||||
oss << "created_at=" << created_at_ << "\n";
|
||||
write_text_file(root_dir_ / "image" / "info.txt", oss.str());
|
||||
}
|
||||
|
||||
std::filesystem::path root_dir_;
|
||||
std::string created_at_;
|
||||
std::string device_id_;
|
||||
std::string firmware_version_;
|
||||
std::string algorithm_version_;
|
||||
std::mutex info_mtx_;
|
||||
std::unique_ptr<Writer> pose_writer_;
|
||||
std::unique_ptr<Writer> cloud_writer_;
|
||||
std::unique_ptr<Writer> image_writer_;
|
||||
std::unique_ptr<Writer> roatation_writer_;
|
||||
std::unique_ptr<Writer> imu_writer_;
|
||||
};
|
||||
+85
@@ -0,0 +1,85 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <sensor_msgs/msg/compressed_image.hpp>
|
||||
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <image_transport/image_transport.hpp>
|
||||
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/time_synchronizer.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
|
||||
#include <Eigen/Dense>
|
||||
|
||||
#include "pointcloud_depth_converter.hpp"
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
class DepthImageRos2Node : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
explicit DepthImageRos2Node(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
|
||||
|
||||
void initialize();
|
||||
|
||||
private:
|
||||
std::string cloud_raw_topic_;
|
||||
std::string color_compressed_topic_;
|
||||
std::string color_raw_topic_;
|
||||
std::string depth_image_topic_;
|
||||
std::string depth_cloud_topic_;
|
||||
|
||||
message_filters::Subscriber<sensor_msgs::msg::PointCloud2> cloud_sub_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::CompressedImage> color_compressed_sub_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::Image> color_sub_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::msg::PointCloud2,
|
||||
// sensor_msgs::msg::CompressedImage,
|
||||
sensor_msgs::msg::Image> MySyncPolicy;
|
||||
typedef message_filters::Synchronizer<MySyncPolicy> Sync;
|
||||
std::shared_ptr<Sync> sync_;
|
||||
|
||||
std::shared_ptr<image_transport::ImageTransport> it_;
|
||||
image_transport::Publisher depth_image_pub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr depth_cloud_pub_;
|
||||
|
||||
std::unique_ptr<PointCloudToDepthConverter> depth_converter_;
|
||||
|
||||
|
||||
PointCloudToDepthConverter::CameraParams loadCameraParams();
|
||||
|
||||
void syncCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud_msg,
|
||||
// const sensor_msgs::msg::CompressedImage::ConstSharedPtr image_msg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr color_msg);
|
||||
|
||||
|
||||
void publishDepthImage(const cv::Mat &img,
|
||||
const std_msgs::msg::Header &header,
|
||||
const std::string &encoding = "32FC1");
|
||||
|
||||
void publishDepthCloud(const pcl::PointCloud<pcl::PointXYZRGB> &colored_cloud,
|
||||
const std_msgs::msg::Header &header);
|
||||
};
|
||||
+83
@@ -0,0 +1,83 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/Image.h>
|
||||
#include <sensor_msgs/CompressedImage.h>
|
||||
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <image_transport/image_transport.h>
|
||||
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/time_synchronizer.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
|
||||
#include <Eigen/Dense>
|
||||
|
||||
#include "pointcloud_depth_converter.hpp"
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
|
||||
class DepthImageRosNode
|
||||
{
|
||||
public:
|
||||
DepthImageRosNode(ros::NodeHandle &nh, ros::NodeHandle &pnh);
|
||||
|
||||
private:
|
||||
ros::NodeHandle nh_, pnh_;
|
||||
image_transport::ImageTransport it_;
|
||||
|
||||
|
||||
std::string cloud_raw_topic_;
|
||||
std::string color_raw_topic_;
|
||||
std::string color_compressed_topic_;
|
||||
std::string depth_image_topic_;
|
||||
std::string depth_cloud_topic_;
|
||||
|
||||
message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub_;
|
||||
message_filters::Subscriber<sensor_msgs::Image> color_sub_;
|
||||
message_filters::Subscriber<sensor_msgs::CompressedImage> color_compressed_sub_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::Image> MySyncPolicy;
|
||||
typedef message_filters::Synchronizer<MySyncPolicy> Sync;
|
||||
std::shared_ptr<Sync> sync_;
|
||||
|
||||
image_transport::Publisher depth_image_pub_;
|
||||
ros::Publisher depth_cloud_pub_;
|
||||
|
||||
std::unique_ptr<PointCloudToDepthConverter> depth_converter_;
|
||||
|
||||
PointCloudToDepthConverter::CameraParams loadCameraParams();
|
||||
|
||||
|
||||
void syncCallback(const sensor_msgs::PointCloud2ConstPtr &cloud_msg,
|
||||
const sensor_msgs::ImageConstPtr &image_msg);
|
||||
|
||||
|
||||
void publishDepthImage(const cv::Mat &img,
|
||||
const std_msgs::Header &header,
|
||||
const std::string &encoding = "32FC1");
|
||||
|
||||
|
||||
void publishDepthCloud(const pcl::PointCloud<pcl::PointXYZRGB> &colored_cloud,
|
||||
const std_msgs::Header &header);
|
||||
};
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,91 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#ifdef ROS2
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <mutex>
|
||||
#else
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/Image.h>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#include <message_filters/synchronizer.h>
|
||||
#endif
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#ifdef ROS2
|
||||
class ImageOverlayNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
ImageOverlayNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
|
||||
|
||||
private:
|
||||
using Image = sensor_msgs::msg::Image;
|
||||
|
||||
std::string reprojected_topic_;
|
||||
std::string camera_topic_;
|
||||
std::string overlay_topic_;
|
||||
double alpha_; // blend alpha for overlay
|
||||
|
||||
rclcpp::Subscription<Image>::SharedPtr reproj_sub_;
|
||||
rclcpp::Subscription<Image>::SharedPtr camera_sub_;
|
||||
rclcpp::Publisher<Image>::SharedPtr overlay_pub_;
|
||||
|
||||
// Cache latest images
|
||||
cv::Mat latest_reproj_img_;
|
||||
cv::Mat latest_camera_img_;
|
||||
std_msgs::msg::Header latest_header_;
|
||||
std::mutex mutex_;
|
||||
|
||||
void reprojCallback(Image::ConstSharedPtr msg);
|
||||
void cameraCallback(Image::ConstSharedPtr msg);
|
||||
void publishOverlay();
|
||||
};
|
||||
#else
|
||||
#include <mutex>
|
||||
class ImageOverlayNode
|
||||
{
|
||||
public:
|
||||
ImageOverlayNode(ros::NodeHandle& nh, ros::NodeHandle& pnh);
|
||||
|
||||
private:
|
||||
ros::NodeHandle nh_, pnh_;
|
||||
|
||||
std::string reprojected_topic_;
|
||||
std::string camera_topic_;
|
||||
std::string overlay_topic_;
|
||||
double alpha_; // blend alpha for overlay
|
||||
|
||||
ros::Subscriber reproj_sub_;
|
||||
ros::Subscriber camera_sub_;
|
||||
ros::Publisher overlay_pub_;
|
||||
|
||||
// Cache latest images
|
||||
cv::Mat latest_reproj_img_;
|
||||
cv::Mat latest_camera_img_;
|
||||
std_msgs::Header latest_header_;
|
||||
std::mutex mutex_;
|
||||
|
||||
void reprojCallback(const sensor_msgs::ImageConstPtr& msg);
|
||||
void cameraCallback(const sensor_msgs::ImageConstPtr& msg);
|
||||
void publishOverlay();
|
||||
};
|
||||
#endif
|
||||
@@ -0,0 +1,532 @@
|
||||
#ifndef LIDAR_API_H
|
||||
#define LIDAR_API_H
|
||||
|
||||
/**
|
||||
* @file lidar_api.h
|
||||
* @brief LiDAR device API for controlling and accessing LiDAR sensor data
|
||||
*
|
||||
* This header provides the public interface for interacting with LiDAR devices.
|
||||
* It includes functions for device management, data streaming control, and
|
||||
* device configuration.
|
||||
*
|
||||
* @copyright Copyright (c) 2025, Manifold Tech Limited, All Rights Reserved
|
||||
* @version 1.0
|
||||
*
|
||||
* ┌──────────────────────────────────────────────────────────────────────────────┐
|
||||
* │ QUICK START │
|
||||
* ├──────────────────────────────────────────────────────────────────────────────┤
|
||||
* │ 1. API Call Sequence (typical usage): │
|
||||
* │ │
|
||||
* │ lidar_system_init(device_cb) // Initialize system, register device cb│
|
||||
* │ ↓ │
|
||||
* │ [Wait for device_cb with attach=true to get device info] │
|
||||
* │ ↓ │
|
||||
* │ lidar_create_device(&dev_info, &handle) // Create device handle │
|
||||
* │ ↓ │
|
||||
* │ lidar_register_stream_callback(handle, cb_info) // Register data callback│
|
||||
* │ ↓ │
|
||||
* │ lidar_open_device(handle) // Connect to device │
|
||||
* │ ↓ │
|
||||
* │ lidar_set_mode(handle, mode) // Set RAW or SLAM mode │
|
||||
* │ ↓ │
|
||||
* │ lidar_start_stream(handle, type, odr) // Start specific data stream │
|
||||
* │ ↓ │
|
||||
* │ [Data arrives via registered callback] │
|
||||
* │ ↓ │
|
||||
* │ lidar_stop_stream(handle, type) // Stop data stream │
|
||||
* │ ↓ │
|
||||
* │ lidar_close_device(handle) // Disconnect │
|
||||
* │ ↓ │
|
||||
* │ lidar_destory_device(handle) // Release device handle │
|
||||
* │ ↓ │
|
||||
* │ lidar_system_deinit() // Cleanup system resources │
|
||||
* │ │
|
||||
* ├──────────────────────────────────────────────────────────────────────────────┤
|
||||
* │ 2. Operating Modes and Available Data Types: │
|
||||
* │ │
|
||||
* │ LIDAR_MODE_RAW: │
|
||||
* │ - LIDAR_DT_RAW_RGB (RGB camera image, NV12 format) │
|
||||
* │ - LIDAR_DT_RAW_IMU (IMU data at 400Hz) │
|
||||
* │ - LIDAR_DT_RAW_DTOF (DTOF depth + point cloud + confidence) │
|
||||
* │ - LIDAR_DT_DEV_STATUS (Device status info) │
|
||||
* │ - LIDAR_DT_NTP (PTP/NTP sync data) │
|
||||
* │ │
|
||||
* │ LIDAR_MODE_SLAM: │
|
||||
* │ - All RAW mode types, plus: │
|
||||
* │ - LIDAR_DT_SLAM_CLOUD (SLAM point cloud, XYZRGBA) │
|
||||
* │ - LIDAR_DT_SLAM_ODOMETRY (SLAM odometry at ~10Hz) │
|
||||
* │ - LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ (Odometry at IMU rate ~400Hz) │
|
||||
* │ - LIDAR_DT_SLAM_ODOMETRY_TF (Map-Odom transform) │
|
||||
* │ │
|
||||
* ├──────────────────────────────────────────────────────────────────────────────┤
|
||||
* │ 3. activate_stream_type vs start_stream: │
|
||||
* │ │
|
||||
* │ lidar_activate_stream_type(): │
|
||||
* │ - Configure which stream types are enabled in device │
|
||||
* │ - Can enable multiple types before starting │
|
||||
* │ - Does NOT start data transmission │
|
||||
* │ │
|
||||
* │ lidar_start_stream(): │
|
||||
* │ - Actually starts data transmission for the specified type │
|
||||
* │ - Callback will begin receiving data after this call │
|
||||
* │ │
|
||||
* │ Typical flow: │
|
||||
* │ activate_stream_type(LIDAR_DT_RAW_IMU); // Enable IMU │
|
||||
* │ activate_stream_type(LIDAR_DT_RAW_DTOF); // Enable DTOF │
|
||||
* │ start_stream(LIDAR_DT_RAW_IMU, ...); // Start IMU stream │
|
||||
* │ start_stream(LIDAR_DT_RAW_DTOF, ...); // Start DTOF stream │
|
||||
* │ │
|
||||
* ├──────────────────────────────────────────────────────────────────────────────┤
|
||||
* │ 4. Callback Notes (IMPORTANT): │
|
||||
* │ │
|
||||
* │ Thread Safety: │
|
||||
* │ - Callbacks are invoked from internal SDK threads │
|
||||
* │ - Different data types may use different threads │
|
||||
* │ - User callback code must be thread-safe │
|
||||
* │ │
|
||||
* │ Data Lifetime: │
|
||||
* │ - Data pointers (pAddr) are ONLY valid during callback execution │
|
||||
* │ - If you need to keep data, COPY it before callback returns │
|
||||
* │ - Do NOT store or dereference pAddr after callback returns │
|
||||
* │ │
|
||||
* │ Performance: │
|
||||
* │ - Avoid blocking or time-consuming operations in callback │
|
||||
* │ - Long callback execution may cause data loss or jitter │
|
||||
* │ - For heavy processing, copy data and process in separate thread │
|
||||
* │ │
|
||||
* ├──────────────────────────────────────────────────────────────────────────────┤
|
||||
* │ 5. Error Codes: │
|
||||
* │ 0 : Success │
|
||||
* │ -1 : General failure │
|
||||
* │ -2 : Invalid parameter │
|
||||
* │ -3 : Device not found / not connected │
|
||||
* │ -4 : Operation timeout │
|
||||
* │ -5 : Resource allocation failed │
|
||||
* │ │
|
||||
* └──────────────────────────────────────────────────────────────────────────────┘
|
||||
*/
|
||||
|
||||
#include "lidar_api_type.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief Initialize the LiDAR system
|
||||
*
|
||||
* Must be called before any other lidar function to set up the system resources.
|
||||
* This function starts device discovery and will invoke the callback when devices
|
||||
* are found or disconnected.
|
||||
*
|
||||
* @param cb Callback function for device events:
|
||||
* - Called with attach=true when a new device is discovered
|
||||
* - Called with attach=false when a device is disconnected
|
||||
* - The lidar_device_info_t contains serial number to identify the device
|
||||
*
|
||||
* Example callback:
|
||||
* void device_callback(const lidar_device_info_t* info, bool attach) {
|
||||
* if (attach) {
|
||||
* printf("Device connected: %s\n", info->serial);
|
||||
* // Save info for lidar_create_device()
|
||||
* } else {
|
||||
* printf("Device disconnected: %s\n", info->serial);
|
||||
* }
|
||||
* }
|
||||
*
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_system_init(lidar_device_callback_t cb);
|
||||
|
||||
/**
|
||||
* @brief Deinitialize the LiDAR system
|
||||
*
|
||||
* Releases all resources allocated by the system. Should be called when
|
||||
* application is shutting down.
|
||||
*
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_system_deinit(void);
|
||||
|
||||
/**
|
||||
* @brief Create a handle for a LiDAR device
|
||||
*
|
||||
* Creates a device handle using the device info received from the device callback.
|
||||
* The handle is used for all subsequent operations on the device.
|
||||
*
|
||||
* @param dev_info Information about the LiDAR device (from lidar_system_init callback)
|
||||
* - serial: Device serial number (required, used to identify device)
|
||||
* - model: Device model string
|
||||
* - online: Device connection status
|
||||
* @param device [OUTPUT] Pointer to receive the device handle upon success
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*
|
||||
* @note The dev_info should be the same structure received from the device callback
|
||||
*/
|
||||
int lidar_create_device(lidar_device_info_t *dev_info, device_handle *device);
|
||||
|
||||
/**
|
||||
* @brief Destroy a LiDAR device handle
|
||||
*
|
||||
* Releases resources associated with the device handle. Must be called
|
||||
* when the device is no longer needed.
|
||||
*
|
||||
* @param device Handle to the device to destroy
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_destory_device(device_handle device);
|
||||
|
||||
/**
|
||||
* @brief Register callback function for receiving LiDAR data streams
|
||||
*
|
||||
* Sets up a callback function that will be called when new data is available.
|
||||
* All data types use the same callback; use lidar_data_t.type to distinguish.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param cb Callback info structure:
|
||||
* - data_callback: Function pointer, signature: void(const lidar_data_t*, void*)
|
||||
* - user_data: User context pointer passed to callback (can be NULL)
|
||||
*
|
||||
* Example:
|
||||
* void data_callback(const lidar_data_t* data, void* user_data) {
|
||||
* switch (data->type) {
|
||||
* case LIDAR_DT_RAW_IMU:
|
||||
* imu_convert_data_t* imu = (imu_convert_data_t*)data->stream.imageList[0].pAddr;
|
||||
* // Process IMU data (COPY if needed, pointer invalid after return)
|
||||
* break;
|
||||
* case LIDAR_DT_RAW_DTOF:
|
||||
* // data->stream.imageList[0]: depth
|
||||
* // data->stream.imageList[1]: point cloud XYZ
|
||||
* // data->stream.imageList[2]: confidence
|
||||
* // data->stream.imageList[3]: intensity
|
||||
* break;
|
||||
* }
|
||||
* }
|
||||
*
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*
|
||||
* @warning Callback is invoked from SDK internal threads. Avoid blocking operations.
|
||||
* @warning Data pointers are only valid during callback execution. Copy if needed.
|
||||
*/
|
||||
int lidar_register_stream_callback(device_handle device, lidar_data_callback_info_t cb);
|
||||
|
||||
/**
|
||||
* @brief Unregister stream callback for a device
|
||||
*
|
||||
* Stops the device from calling back when new data is available.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_unregister_stream_callback(device_handle device);
|
||||
|
||||
/**
|
||||
* @brief Open a LiDAR device for communication
|
||||
*
|
||||
* Establishes a connection to the physical device.
|
||||
*
|
||||
* @param device Handle to the device to open
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_open_device(device_handle device);
|
||||
|
||||
/**
|
||||
* @brief Close a LiDAR device
|
||||
*
|
||||
* Closes the connection to the physical device.
|
||||
*
|
||||
* @param device Handle to the device to close
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_close_device(device_handle device);
|
||||
|
||||
/**
|
||||
* @brief Set the operating mode of the LiDAR device
|
||||
*
|
||||
* Must be called after lidar_open_device() and before lidar_start_stream().
|
||||
* Mode determines which data types are available for streaming.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param mode Operating mode to set:
|
||||
* - LIDAR_MODE_RAW: Raw sensor data (RGB, IMU, DTOF)
|
||||
* - LIDAR_MODE_SLAM: SLAM processing enabled (adds odometry, point cloud)
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_set_mode(device_handle device, int mode);
|
||||
|
||||
/**
|
||||
* @brief Start data streaming from the device
|
||||
*
|
||||
* Begins the flow of data from the device for the specified type.
|
||||
* After calling this function, registered callbacks will start receiving data.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param type Type of data stream to start (lidar_data_type_e):
|
||||
* - LIDAR_DT_RAW_RGB: RGB camera frames
|
||||
* - LIDAR_DT_RAW_IMU: IMU data at 400Hz
|
||||
* - LIDAR_DT_RAW_DTOF: Depth sensor data
|
||||
* - LIDAR_DT_SLAM_CLOUD: SLAM point cloud (requires SLAM mode)
|
||||
* - LIDAR_DT_SLAM_ODOMETRY: SLAM odometry (requires SLAM mode)
|
||||
* - etc. (see lidar_data_type_e in lidar_api_type.h)
|
||||
* @param dtof_subframe_odr [OUTPUT] Returns DTOF subframe interval in microseconds.
|
||||
* Only meaningful when type=LIDAR_DT_RAW_DTOF.
|
||||
* For other types, this value can be ignored.
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*
|
||||
* @note You can start multiple stream types simultaneously by calling this
|
||||
* function multiple times with different types.
|
||||
*/
|
||||
int lidar_start_stream(device_handle device, int type, uint32_t &dtof_subframe_odr);
|
||||
|
||||
/**
|
||||
* @brief Stop data streaming from the device
|
||||
*
|
||||
* Stops the flow of data from the device for the specified type.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param type Type of data stream to stop
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_stop_stream(device_handle device, int type);
|
||||
|
||||
/**
|
||||
* @brief Activate a specific stream type on the device
|
||||
*
|
||||
* Enables a specific data stream type in the device configuration.
|
||||
* This configures the device to be ready for the stream type, but does NOT
|
||||
* start actual data transmission. Call lidar_start_stream() to begin streaming.
|
||||
*
|
||||
* Use this to pre-configure multiple stream types before starting them:
|
||||
* activate_stream_type(handle, LIDAR_DT_RAW_IMU);
|
||||
* activate_stream_type(handle, LIDAR_DT_RAW_DTOF);
|
||||
* start_stream(handle, LIDAR_DT_RAW_IMU, odr);
|
||||
* start_stream(handle, LIDAR_DT_RAW_DTOF, odr);
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param type Type of data stream to activate (lidar_data_type_e)
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*
|
||||
* @see lidar_start_stream() to actually begin data transmission
|
||||
* @see lidar_deactivate_stream_type() to disable a stream type
|
||||
*/
|
||||
int lidar_activate_stream_type(device_handle device, int type);
|
||||
|
||||
/**
|
||||
* @brief Deactivate a specific stream type on the device
|
||||
*
|
||||
* Disables a specific data stream type in the device configuration.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param type Type of data stream to deactivate
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_deactivate_stream_type(device_handle device, int type);
|
||||
|
||||
/**
|
||||
* @brief Get the calibration file from the device
|
||||
* @param device Handle to the target device
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_get_calib_file(device_handle device, const char* path);
|
||||
|
||||
/**
|
||||
* @brief Get device calibration parameters
|
||||
*
|
||||
* Retrieves the current calibration parameters from the device.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param param Pointer to receive the calibration parameters
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_get_calibration(device_handle device, lidar_calibration_t* param);
|
||||
|
||||
/**
|
||||
* @brief Set device calibration parameters
|
||||
*
|
||||
* Applies new calibration parameters to the device.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param param Pointer to the calibration parameters to set
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_set_calibration(device_handle device, const lidar_calibration_t *param);
|
||||
|
||||
/**
|
||||
* @brief Set log verbosity level
|
||||
*
|
||||
* Controls the amount of log information generated by the LiDAR API.
|
||||
*
|
||||
* @param level Log level to set (see level definitions in lidar_api_type.h)
|
||||
*/
|
||||
void lidar_log_set_level(lidar_log_level_e level);
|
||||
|
||||
/**
|
||||
* @brief Get the version information of the LiDAR device
|
||||
*
|
||||
* Retrieves version information including firmware, system, and application versions.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param version struct Pointer to receive the version information
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_get_version(device_handle device,lidar_fireware_version_t *version);
|
||||
|
||||
/**
|
||||
* @brief Set custom algorithm parameters for the device
|
||||
*
|
||||
* Sends custom parameter settings to the device.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param param_name String name of the parameter to set
|
||||
* @param value_data Pointer to the value data to set for the parameter
|
||||
* @param value_length Length of the value data in bytes
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_set_custom_parameter(device_handle device, const char* param_name, const void* value_data, size_t value_length);
|
||||
|
||||
/**
|
||||
* @brief Get custom algorithm parameters for the device
|
||||
*
|
||||
* Get custom parameter settings from the device.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param param_name String name of the parameter to get
|
||||
* @param value Integer value to get for the parameter
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_get_custom_parameter(device_handle device, const char* param_name, int* value);
|
||||
|
||||
/**
|
||||
* @brief Get custom algorithm parameters for the device
|
||||
*
|
||||
* Get custom parameter settings from the device.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param param_name String name of the parameter to get
|
||||
* @param value Integer value to get for the parameter
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_get_custom_parameter(device_handle device, const char* param_name, int* value);
|
||||
|
||||
/**
|
||||
* @brief Set the map file used for relocalization
|
||||
*
|
||||
* Read & send specified map file to device for relocalization
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param abs_path Absolute path to the map file
|
||||
* @return int 0 on success, otherwise on failure
|
||||
*/
|
||||
int lidar_set_relocalization_map(device_handle device, const char* abs_path);
|
||||
|
||||
/**
|
||||
* @brief Get the mapping result file from device
|
||||
*
|
||||
* Read & send specified map file from device to host
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param dest_dir Destination directory to save the map file
|
||||
* @param file_name File name to save the map file
|
||||
* @return int 0 on success, -1 on failure without error code, error code (> 0) otherwise
|
||||
*/
|
||||
int lidar_get_mapping_result(device_handle device, const char* dest_dir, const char* file_name);
|
||||
|
||||
/**
|
||||
* @brief Save the current map to a file on the host. Synchronous, all-in-one API.
|
||||
*
|
||||
* Internally drives the complete save-map state machine, so callers do not need
|
||||
* to coordinate the multi-step protocol themselves:
|
||||
* 1) Set custom parameter save_map = 1 on the device (kicks off generation).
|
||||
* 2) Poll save_map until the device resets it to 0 (map generation finished).
|
||||
* 3) Pull the resulting file via the standard mapping-result transfer (same as
|
||||
* lidar_get_mapping_result).
|
||||
* 4) Return the final status to the caller.
|
||||
*
|
||||
* This is the recommended entry point for saving a map. The lower-level pair
|
||||
* (lidar_set_custom_parameter("save_map") + lidar_get_mapping_result) remains
|
||||
* available for advanced use cases.
|
||||
*
|
||||
* 同步保存地图到主机上的文件。一站式 API，内部完成「触发生成 → 等待设备完成 →
|
||||
* 拉取文件」的完整流程，调用方无需自行轮询和处理异步状态。
|
||||
*
|
||||
* @param device Device handle.
|
||||
* @param dest_dir Host directory to save the file into (must already exist).
|
||||
* @param file_name File name (e.g. "map.bin").
|
||||
* @param gen_timeout_ms Maximum time (ms) to wait for the device to finish
|
||||
* generating the map. Pass 0 to use the default (120000 ms).
|
||||
* @return int
|
||||
* 0 success, file saved at dest_dir/file_name
|
||||
* -1 invalid arguments (null device/dir, SDK not initialized, etc.)
|
||||
* -2 device is busy with another file transfer
|
||||
* -3 timed out waiting for the device to finish map generation
|
||||
* -4 file transfer stalled or failed (see logs for details)
|
||||
* other negative values are propagated from the underlying transfer.
|
||||
*/
|
||||
int lidar_save_map(device_handle device,
|
||||
const char *dest_dir,
|
||||
const char *file_name,
|
||||
uint32_t gen_timeout_ms);
|
||||
|
||||
/**
|
||||
* @brief Set the image mask file for the device
|
||||
*
|
||||
* Read & send specified image mask file to device
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param abs_path Absolute path to the image mask file (e.g., mask.png)
|
||||
* @return int 0 on success, -1 on failure, -2 if file transfer in progress
|
||||
*/
|
||||
int lidar_set_image_mask(device_handle device, const char* abs_path);
|
||||
|
||||
/**
|
||||
* @brief enable encrypted device log
|
||||
*
|
||||
* enable encrypted device log, save to specified directory
|
||||
* Please send to our support when needed
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param dest_dir Destination directory to save the encrypted logs
|
||||
* @return int 0 on success, -1 on failure
|
||||
*/
|
||||
int lidar_enable_encrypted_device_log(device_handle device, const char* dest_dir);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Set the depth parameters for the device
|
||||
*
|
||||
* This function must be called before starting data stream.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param params Pointer to the depth parameters to set
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_set_depth_parameter(device_handle device, const lidar_depth_para_t *params);
|
||||
|
||||
/**
|
||||
* @brief Enable or disable IMU smooth sending feature
|
||||
*
|
||||
* When enabled, IMU data will be sent at precise intervals (default 400Hz)
|
||||
* using a dedicated high-priority thread to reduce jitter and timing variance.
|
||||
* When disabled, IMU data will be sent immediately upon reception.
|
||||
*
|
||||
* @param enable 1 to enable smooth sending, 0 to disable
|
||||
* @return int 0 on success, -1 on failure
|
||||
*/
|
||||
int lidar_enable_imu_smooth_sending(int enable);
|
||||
|
||||
/**
|
||||
* @brief Set IMU smooth sending frequency
|
||||
*
|
||||
* Set the target frequency for IMU smooth sending. Only effective when
|
||||
* smooth sending is enabled via lidar_enable_imu_smooth_sending().
|
||||
*
|
||||
* @param frequency_hz Target frequency in Hz (1-1000 Hz, recommended 400 Hz)
|
||||
* @return int 0 on success, -1 on failure
|
||||
*/
|
||||
int lidar_set_imu_smooth_frequency(uint32_t frequency_hz);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // LIDAR_API_H
|
||||
@@ -0,0 +1,242 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
#ifndef LIDAR_TYPES_H
|
||||
#define LIDAR_TYPES_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#define LIDAR_SERIAL_MAX 64
|
||||
#define LIDAR_MODEL_MAX 64
|
||||
#define LIDAR_IP_MAX 64
|
||||
|
||||
typedef void * device_handle;
|
||||
|
||||
typedef enum {
|
||||
LIDAR_LOG_ERROR = 0,
|
||||
LIDAR_LOG_WARN,
|
||||
LIDAR_LOG_INFO,
|
||||
LIDAR_LOG_DEBUG,
|
||||
} lidar_log_level_e;
|
||||
|
||||
typedef enum {
|
||||
LIDAR_OTA_ALGORITHM,
|
||||
LIDAR_OTA_FIRMWARE,
|
||||
LIDAR_OTA_SCRIPT,
|
||||
LIDAR_OTA_CALIBRATION
|
||||
} lidar_ota_type_e;
|
||||
|
||||
typedef enum {
|
||||
LIDAR_MODE_RAW,
|
||||
LIDAR_MODE_SLAM,
|
||||
} lidar_mode_e;
|
||||
|
||||
typedef enum {
|
||||
LIDAR_DT_NONE = 0,
|
||||
LIDAR_DT_RAW_RGB,
|
||||
LIDAR_DT_RAW_IMU,
|
||||
LIDAR_DT_RAW_DTOF,
|
||||
LIDAR_DT_SLAM_CLOUD,
|
||||
LIDAR_DT_SLAM_ODOMETRY,
|
||||
LIDAR_DT_DEV_STATUS,
|
||||
LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ,
|
||||
LIDAR_DT_SLAM_ODOMETRY_TF,
|
||||
LIDAR_DT_SLAM_WIWC,
|
||||
LIDAR_DT_NTP
|
||||
} lidar_data_type_e;
|
||||
|
||||
typedef struct {
|
||||
int8_t serial[LIDAR_SERIAL_MAX];
|
||||
int8_t model[LIDAR_MODEL_MAX];
|
||||
bool online;
|
||||
uint32_t initial_state;
|
||||
} lidar_device_info_t;
|
||||
|
||||
typedef struct {
|
||||
float x, y, z;
|
||||
float intensity;
|
||||
} lidar_point_t;
|
||||
|
||||
|
||||
typedef struct {
|
||||
float intrinsics[9];
|
||||
float extrinsics[16];
|
||||
} lidar_calibration_t;
|
||||
|
||||
|
||||
#define DEVICE_MAX_CH_NUMBER 4
|
||||
|
||||
typedef struct {
|
||||
uint64_t timestamp_ns;
|
||||
int64_t pos[3];
|
||||
int64_t orient[4];
|
||||
} ros2_odom_convert_t;
|
||||
|
||||
typedef struct {
|
||||
uint64_t timestamp_ns;
|
||||
int64_t pos[3];
|
||||
int64_t orient[4];
|
||||
int64_t linear_velocity[3];
|
||||
int64_t angular_velocity[3];
|
||||
double pose_cov[36];
|
||||
double twist_cov[36];
|
||||
} ros_odom_convert_complete_t;
|
||||
|
||||
typedef struct {
|
||||
float accel_x;
|
||||
float accel_y;
|
||||
float accel_z;
|
||||
float gyro_x;
|
||||
float gyro_y;
|
||||
float gyro_z;
|
||||
uint64_t stamp;
|
||||
uint64_t sequence;
|
||||
} imu_convert_data_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t length;
|
||||
uint64_t sequence;
|
||||
uint64_t timestamp;
|
||||
uint64_t interval;
|
||||
void* pAddr;
|
||||
uint32_t width;
|
||||
uint32_t height;
|
||||
} buffer_List_t;
|
||||
|
||||
typedef struct {
|
||||
double delay;
|
||||
double offset;
|
||||
} ptp_sync_data_t;
|
||||
|
||||
typedef struct capture_Image_List_t {
|
||||
uint32_t imageCount;
|
||||
buffer_List_t imageList[DEVICE_MAX_CH_NUMBER];
|
||||
} capture_Image_List_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t type;
|
||||
capture_Image_List_t stream;
|
||||
} lidar_data_t;
|
||||
|
||||
typedef void (*lidar_device_callback_t)(const lidar_device_info_t* device, bool attach);
|
||||
typedef void (*lidar_data_callback_t)(const lidar_data_t *data, void *user_data);
|
||||
|
||||
typedef struct {
|
||||
lidar_data_callback_t data_callback;
|
||||
void *user_data;
|
||||
} lidar_data_callback_info_t;
|
||||
|
||||
typedef struct {
|
||||
int major;
|
||||
int minor;
|
||||
int patch;
|
||||
}lidar_version_t;
|
||||
|
||||
typedef struct {
|
||||
lidar_version_t kernel_version;
|
||||
lidar_version_t mcu_version;
|
||||
lidar_version_t soc_version;
|
||||
lidar_version_t Daemon_proc_version;
|
||||
lidar_version_t slam_version;
|
||||
} lidar_fireware_version_t;
|
||||
|
||||
/**
|
||||
* @brief RGB image sensor frame rate
|
||||
*
|
||||
*/
|
||||
typedef struct{
|
||||
|
||||
int configured_odr; /* rgb image sensor configured output data rate */
|
||||
int tx_odr; /* rgb image sensor tx output data rate */
|
||||
|
||||
} lidar_rgb_sensor_status_t;
|
||||
|
||||
/**
|
||||
* @brief DTOF Lidar frame rate
|
||||
*
|
||||
*/
|
||||
typedef struct{
|
||||
|
||||
int configured_odr; /* dtof lidar sensor configured output data rate */
|
||||
int tx_odr; /* dtof lidar sensor tx output data rate */
|
||||
int subframe_odr; /* dtof lidar sensor subframe output data rate */
|
||||
short tx_temp; /* dtof lidar tx module temp */
|
||||
short rx_temp; /* dtof lidar rx module temp */
|
||||
|
||||
} lidar_dtof_sensor_status_t;
|
||||
|
||||
/**
|
||||
* @brief IMU Sensor
|
||||
*
|
||||
*/
|
||||
typedef struct{
|
||||
|
||||
int configured_odr; /* imu sensor configured output data rate */
|
||||
int tx_odr; /* imu sensor tx output data rate */
|
||||
|
||||
} lidar_imu_sensor_status_t;
|
||||
|
||||
typedef struct{
|
||||
|
||||
int package_temp; /* soc package temp */
|
||||
int cpu_temp; /* cpu temp */
|
||||
int center_temp; /* center temp */
|
||||
int gpu_temp; /* gpu temp */
|
||||
int npu_temp; /* npu temp */
|
||||
|
||||
} lidar_soc_thermal_t;
|
||||
typedef struct
|
||||
{
|
||||
double uptime_seconds;
|
||||
lidar_soc_thermal_t soc_thermal;
|
||||
|
||||
int cpu_use_rate[8]; /* cpu usage rate */
|
||||
int ram_use_rate; /* ram usage rate */
|
||||
|
||||
lidar_rgb_sensor_status_t rgb_sensor;
|
||||
lidar_dtof_sensor_status_t dtof_sensor;
|
||||
lidar_imu_sensor_status_t imu_sensor;
|
||||
|
||||
int slam_cloud_tx_odr; /* slam cloud tx output data rate */
|
||||
int slam_odom_tx_odr; /* slam odom tx output data rate */
|
||||
int slam_odom_highfreq_tx_odr; /* slam odom high freq tx output data rate */
|
||||
|
||||
} lidar_device_status_t;
|
||||
|
||||
typedef enum {
|
||||
LIDAR_DEVICE_NONE = 0,
|
||||
LIDAR_DEVICE_NOT_INITIALIZED,
|
||||
LIDAR_DEVICE_INITIALIZED,
|
||||
LIDAR_DEVICE_STREAMING,
|
||||
LIDAR_DEVICE_STREAM_STOPPED,
|
||||
} lidar_device_initial_state_e;
|
||||
|
||||
typedef enum {
|
||||
LIDAR_DEPTH_ODR_10HZ = 0,
|
||||
LIDAR_DEPTH_ODR_14_5HZ,
|
||||
} lidar_depth_odr_e;
|
||||
|
||||
typedef struct {
|
||||
lidar_depth_odr_e odr;
|
||||
} lidar_depth_para_t;
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
+93
@@ -0,0 +1,93 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/common/transforms.h>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <Eigen/Dense>
|
||||
#include <vector>
|
||||
|
||||
|
||||
class PointCloudToDepthConverter
|
||||
{
|
||||
public:
|
||||
|
||||
struct CameraParams
|
||||
{
|
||||
int image_width;
|
||||
int image_height;
|
||||
double A11, A12, A22;
|
||||
double u0, v0;
|
||||
double k2, k3, k4, k5, k6, k7;
|
||||
double scale;
|
||||
int point_sampling_rate;
|
||||
Eigen::Matrix4d Tcl;
|
||||
};
|
||||
|
||||
|
||||
struct ProcessResult
|
||||
{
|
||||
cv::Mat depth_image;
|
||||
pcl::PointCloud<pcl::PointXYZRGB> colored_cloud;
|
||||
bool success;
|
||||
std::string error_message;
|
||||
};
|
||||
|
||||
|
||||
explicit PointCloudToDepthConverter(const CameraParams ¶ms);
|
||||
|
||||
|
||||
ProcessResult processCloudAndImage(const pcl::PointCloud<pcl::PointXYZ> &cloud,
|
||||
const cv::Mat &image);
|
||||
|
||||
cv::Mat customResize(const cv::Mat& src, const cv::Size& size);
|
||||
const CameraParams &getCameraParams() const { return params_; }
|
||||
|
||||
|
||||
void updateCameraParams(const CameraParams ¶ms);
|
||||
|
||||
private:
|
||||
CameraParams params_;
|
||||
|
||||
Eigen::Matrix3d K_;
|
||||
Eigen::Matrix3d Kl_;
|
||||
Eigen::Matrix4d K_4x4_;
|
||||
Eigen::Matrix4d Kcl_;
|
||||
|
||||
cv::Mat map_x_, map_y_;
|
||||
cv::Mat inv_map_x_, inv_map_y_;
|
||||
|
||||
int scaled_width_, scaled_height_;
|
||||
|
||||
|
||||
void initializeInternalParams();
|
||||
|
||||
|
||||
void createDistortionMaps();
|
||||
|
||||
cv::Mat projectCloudToDepth(const pcl::PointCloud<pcl::PointXYZ> &cloud_in_cam);
|
||||
|
||||
|
||||
cv::Mat postProcessDepthImage(const cv::Mat &depth_img);
|
||||
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZRGB> generateColoredCloud(const cv::Mat &depth_img,
|
||||
const cv::Mat &color_img);
|
||||
|
||||
|
||||
std::pair<bool, std::string> validateInputs(const pcl::PointCloud<pcl::PointXYZ> &cloud,
|
||||
const cv::Mat &image);
|
||||
};
|
||||
@@ -0,0 +1,144 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <Eigen/Dense>
|
||||
#include <cmath>
|
||||
|
||||
namespace mini_vikit {
|
||||
|
||||
using namespace Eigen;
|
||||
|
||||
class PolynomialCamera {
|
||||
private:
|
||||
const double fx_, fy_;
|
||||
const double cx_, cy_;
|
||||
const double skew_;
|
||||
bool distortion_;
|
||||
double k2_, k3_, k4_, k5_, k6_, k7_;
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
|
||||
PolynomialCamera(double width, double height,
|
||||
double fx, double fy, double cx, double cy, double skew,
|
||||
double k2=0.0, double k3=0.0, double k4=0.0,
|
||||
double k5=0.0, double k6=0.0, double k7=0.0)
|
||||
: fx_(fx), fy_(fy), cx_(cx), cy_(cy), skew_(skew),
|
||||
distortion_(std::abs(k2) > 1e-7) {
|
||||
k2_ = k2; k3_ = k3; k4_ = k4; k5_ = k5; k6_ = k6; k7_ = k7;
|
||||
}
|
||||
|
||||
Vector3d cam2world(const double& u, const double& v) const {
|
||||
Vector3d xyz;
|
||||
if (!distortion_) {
|
||||
double y = (v - cy_) / fy_;
|
||||
double x = (u - cx_ - y * skew_) / fx_;
|
||||
xyz << x, y, 1.0;
|
||||
} else {
|
||||
double y = (v - cy_) / fy_;
|
||||
double x = (u - cx_ - y * skew_) / fx_;
|
||||
|
||||
const double thetad = std::sqrt(x * x + y * y);
|
||||
double theta = thetad;
|
||||
|
||||
for (int i = 0; i < 7; ++i) {
|
||||
const double theta2 = theta * theta;
|
||||
const double theta3 = theta2 * theta;
|
||||
const double theta4 = theta3 * theta;
|
||||
const double theta5 = theta4 * theta;
|
||||
const double theta6 = theta5 * theta;
|
||||
theta = thetad / (1.0 + k2_ * theta + k3_ * theta2 + k4_ * theta3 +
|
||||
k5_ * theta4 + k6_ * theta5 + k7_ * theta6);
|
||||
}
|
||||
|
||||
const double scaling = std::tan(theta) / thetad;
|
||||
x *= scaling;
|
||||
y *= scaling;
|
||||
xyz << x, y, 1.0;
|
||||
}
|
||||
return xyz.normalized();
|
||||
}
|
||||
|
||||
Vector3d cam2world(const Vector2d& px) const {
|
||||
return cam2world(px[0], px[1]);
|
||||
}
|
||||
|
||||
Vector2d world2cam(const Vector3d& xyz) const {
|
||||
Vector2d px;
|
||||
if (!distortion_) {
|
||||
px[0] = fx_ * xyz[0] + cx_;
|
||||
px[1] = fy_ * xyz[1] + cy_;
|
||||
} else {
|
||||
double xd, yd;
|
||||
const double r = std::sqrt(xyz(1) * xyz(1) + xyz(0) * xyz(0));
|
||||
const double theta = std::acos(xyz(2) / xyz.norm());
|
||||
const double thetad = thetad_from_theta(theta);
|
||||
const double scaling = thetad / r;
|
||||
xd = xyz[0] * scaling;
|
||||
yd = xyz[1] * scaling;
|
||||
px[0] = xd * fx_ + yd * skew_ + cx_;
|
||||
px[1] = yd * fy_ + cy_;
|
||||
}
|
||||
return px;
|
||||
}
|
||||
|
||||
Vector2d world2cam(const Vector2d& uv) const {
|
||||
Vector2d px;
|
||||
if (!distortion_) {
|
||||
px[0] = fx_ * uv[0] + cx_;
|
||||
px[1] = fy_ * uv[1] + cy_;
|
||||
} else {
|
||||
double xd, yd;
|
||||
const double r = uv.norm();
|
||||
if (r < 1e-8) {
|
||||
return uv;
|
||||
}
|
||||
const double theta = std::atan(r);
|
||||
const double thetad = thetad_from_theta(theta);
|
||||
const double scaling = thetad / r;
|
||||
xd = uv[0] * scaling;
|
||||
yd = uv[1] * scaling;
|
||||
px[0] = xd * fx_ + yd * skew_ + cx_;
|
||||
px[1] = yd * fy_ + cy_;
|
||||
}
|
||||
return px;
|
||||
}
|
||||
|
||||
inline double thetad_from_theta(const double theta) const {
|
||||
const double theta2 = theta * theta;
|
||||
const double theta3 = theta2 * theta;
|
||||
const double theta4 = theta3 * theta;
|
||||
const double theta5 = theta4 * theta;
|
||||
const double theta6 = theta5 * theta;
|
||||
const double theta7 = theta6 * theta;
|
||||
const double thetad = theta + k2_ * theta2 + k3_ * theta3 +
|
||||
k4_ * theta4 + k5_ * theta5 + k6_ * theta6 + k7_ * theta7;
|
||||
return thetad;
|
||||
}
|
||||
|
||||
double fx() const { return fx_; }
|
||||
double fy() const { return fy_; }
|
||||
double cx() const { return cx_; }
|
||||
double cy() const { return cy_; }
|
||||
double skew() const { return skew_; }
|
||||
bool has_distortion() const { return distortion_; }
|
||||
|
||||
double k2() const { return k2_; }
|
||||
double k3() const { return k3_; }
|
||||
double k4() const { return k4_; }
|
||||
double k5() const { return k5_; }
|
||||
double k6() const { return k6_; }
|
||||
double k7() const { return k7_; }
|
||||
};
|
||||
} // namespace mini_vikit
|
||||
@@ -0,0 +1,83 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
#ifndef RGBCLOUD_H
|
||||
#define RGBCLOUD_H
|
||||
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <Eigen/Dense>
|
||||
#include "lidar_api.h"
|
||||
|
||||
namespace GlobalCameraParams {
|
||||
extern float g_fx;
|
||||
extern float g_fy;
|
||||
extern float g_cx;
|
||||
extern float g_cy;
|
||||
extern float g_skew;
|
||||
extern float g_k2;
|
||||
extern float g_k3;
|
||||
extern float g_k4;
|
||||
extern float g_k5;
|
||||
extern float g_k6;
|
||||
extern float g_k7;
|
||||
extern Eigen::Matrix4f g_T_camera_lidar;
|
||||
}
|
||||
|
||||
class rawCloudRender {
|
||||
public:
|
||||
|
||||
|
||||
bool init(const std::string& yamlFilePath);
|
||||
|
||||
void nv12buffer_2_rgb(buffer_List_t &image, std::vector<std::vector<float>>& rgb_image);
|
||||
void render(std::vector<std::vector<float>>& rgb_image, capture_Image_List_t* pcdStream, int pcdIdx, std::vector<float>& rgbCloud_flat);
|
||||
|
||||
void print_camera_calib();
|
||||
|
||||
int getImageWidth() const { return image_width_; }
|
||||
int getImageHeight() const { return image_height_; }
|
||||
|
||||
|
||||
|
||||
private:
|
||||
std::string model_type_;
|
||||
std::string camera_name_;
|
||||
int image_width_;
|
||||
int image_height_;
|
||||
int frame_size_;
|
||||
bool opencv_available_;
|
||||
|
||||
// 4x4 transformation matrix (T_camera_lidar)
|
||||
Eigen::Matrix4f T_camera_lidar_;
|
||||
|
||||
float k2_;
|
||||
float k3_;
|
||||
float k4_;
|
||||
float k5_;
|
||||
float k6_;
|
||||
float k7_;
|
||||
float p1_;
|
||||
float p2_;
|
||||
float A11_fx_;
|
||||
float A12_skew_;
|
||||
float A22_fy_;
|
||||
float u0_cx_;
|
||||
float v0_cy_;
|
||||
bool isFast_;
|
||||
int numDiff_;
|
||||
float maxIncidentAngle_;
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif // RGBCLOUD_H
|
||||
@@ -0,0 +1,110 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
#ifndef YAML_PARSER_H
|
||||
#define YAML_PARSER_H
|
||||
|
||||
#include <cstdio>
|
||||
#include <string>
|
||||
#include <map>
|
||||
#include <unordered_set>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
#include "lidar_api.h"
|
||||
|
||||
namespace odin_ros_driver {
|
||||
|
||||
// Data type enum for supporting different value types
|
||||
enum class DataType {
|
||||
INT_TYPE,
|
||||
FLOAT_ARRAY_TYPE,
|
||||
INT_ARRAY_TYPE,
|
||||
};
|
||||
|
||||
// Generic parameter value holder
|
||||
struct ParameterValue {
|
||||
DataType type;
|
||||
std::vector<uint8_t> data;
|
||||
|
||||
ParameterValue() : type(DataType::INT_TYPE) {}
|
||||
|
||||
template<typename T>
|
||||
void setData(const T& value) {
|
||||
const uint8_t* ptr = reinterpret_cast<const uint8_t*>(&value);
|
||||
data.assign(ptr, ptr + sizeof(T));
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
void setArray(const std::vector<T>& arr) {
|
||||
const uint8_t* ptr = reinterpret_cast<const uint8_t*>(arr.data());
|
||||
data.assign(ptr, ptr + arr.size() * sizeof(T));
|
||||
}
|
||||
|
||||
size_t getSize() const {
|
||||
return data.size();
|
||||
}
|
||||
|
||||
const void* getData() const {
|
||||
return data.empty() ? nullptr : data.data();
|
||||
}
|
||||
};
|
||||
|
||||
class YamlParser {
|
||||
public:
|
||||
YamlParser(const std::string& config_file);
|
||||
|
||||
bool loadConfig();
|
||||
const std::map<std::string, int>& getRegisterKeys() const;
|
||||
const std::map<std::string, std::string>& getRegisterKeysStrVal() const;
|
||||
const std::map<std::string, ParameterValue>& getCustomParameters() const;
|
||||
void printConfig() const;
|
||||
bool applyCustomParameters(device_handle device);
|
||||
int getCustomParameterInt(const std::string& param_name, int default_value) const;
|
||||
|
||||
int getCustomMapMode(int default_value) const {
|
||||
auto it = custom_parameters_.find("map_mode");
|
||||
if (it != custom_parameters_.end() && it->second.type == DataType::INT_TYPE) {
|
||||
printf("custom_map_mode = %d\n", *(int*)it->second.getData());
|
||||
return *(int*)it->second.getData();
|
||||
} else {
|
||||
return default_value;
|
||||
}
|
||||
};
|
||||
|
||||
private:
|
||||
std::string config_file_;
|
||||
std::map<std::string, int> register_keys_;
|
||||
std::map<std::string, std::string> register_keys_str_val_;
|
||||
std::map<std::string, double> register_keys_float_val_;
|
||||
std::map<std::string, ParameterValue> custom_parameters_;
|
||||
|
||||
// Keys whose YAML value is a string (not int).
|
||||
std::unordered_set<std::string> allowed_key_w_str_val = {
|
||||
"relocalization_map_abs_path",
|
||||
"mapping_result_dest_dir",
|
||||
"mapping_result_file_name",
|
||||
"image_mask_abs_path",
|
||||
"overlay_reprojected_topic",
|
||||
"overlay_camera_topic",
|
||||
"overlay_output_topic"
|
||||
};
|
||||
|
||||
// Keys whose YAML value is a floating-point number (not int).
|
||||
std::unordered_set<std::string> allowed_key_w_float_val = {
|
||||
"overlay_alpha"
|
||||
};
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,37 @@
|
||||
<launch>
|
||||
<!--
|
||||
Usage: roslaunch odin_ros_driver odin1_ros1.launch
|
||||
-->
|
||||
|
||||
<!-- Set node name -->
|
||||
<arg name="node_name" default="host_sdk_sample"/>
|
||||
|
||||
<!-- Set parameter file path -->
|
||||
<arg name="config_file" default="$(find odin_ros_driver)/config/control_command.yaml"/>
|
||||
|
||||
<!-- Set RViz configuration file path -->
|
||||
<arg name="rviz_config" default="$(find odin_ros_driver)/config/odin_ros.rviz"/>
|
||||
|
||||
<!-- Launch main node -->
|
||||
<node name="$(arg node_name)" pkg="odin_ros_driver" type="host_sdk_sample" output="screen">
|
||||
<param name="config_file" value="$(arg config_file)"/>
|
||||
</node>
|
||||
|
||||
<node pkg="odin_ros_driver" type="pcd2depth_node" name="pcd2depth_node" output="screen" >
|
||||
<rosparam command="load" file="$(find odin_ros_driver)/config/control_command.yaml"/>
|
||||
<param name="calib_file_path" value="$(find odin_ros_driver)/config/calib.yaml"/>
|
||||
</node>
|
||||
|
||||
<node pkg="odin_ros_driver" type="cloud_reprojection_node" name="cloud_reprojection_node" output="screen" >
|
||||
<rosparam command="load" file="$(find odin_ros_driver)/config/control_command.yaml"/>
|
||||
<param name="calib_file_path" value="$(find odin_ros_driver)/config/calib.yaml"/>
|
||||
</node>
|
||||
|
||||
<!-- Image overlay node - overlays reprojected points on camera image -->
|
||||
<node pkg="odin_ros_driver" type="image_overlay_node" name="image_overlay_node" output="screen" >
|
||||
<rosparam command="load" file="$(find odin_ros_driver)/config/control_command.yaml"/>
|
||||
</node>
|
||||
|
||||
<!-- Launch RViz with configuration -->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rviz_config)" output="screen"/>
|
||||
</launch>
|
||||
+111
@@ -0,0 +1,111 @@
|
||||
|
||||
# USAGE: ros2 launch odin_ros_driver odin1_ros2.launch.py
|
||||
import os
|
||||
import yaml
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.conditions import IfCondition
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
# Get package directory
|
||||
package_dir = get_package_share_directory('odin_ros_driver')
|
||||
|
||||
# Declare configuration parameter
|
||||
config_file_arg = DeclareLaunchArgument(
|
||||
'config_file',
|
||||
default_value=os.path.join(package_dir, 'config', 'control_command.yaml'),
|
||||
description='Path to the control config YAML file'
|
||||
)
|
||||
|
||||
# Add RViz2 configuration file parameter
|
||||
rviz_config_arg = DeclareLaunchArgument(
|
||||
'rviz_config',
|
||||
default_value=os.path.join(package_dir, 'config', 'odin_ros2.rviz'),
|
||||
description='Path to RViz2 config file'
|
||||
)
|
||||
|
||||
# Declare launch rviz parameter
|
||||
launch_rviz_arg = DeclareLaunchArgument(
|
||||
'launch_rviz',
|
||||
default_value='false',
|
||||
description='Whether to launch RViz2'
|
||||
)
|
||||
|
||||
|
||||
# Create main node
|
||||
host_sdk_node = Node(
|
||||
package='odin_ros_driver',
|
||||
executable='host_sdk_sample',
|
||||
name='host_sdk_sample',
|
||||
output='screen',
|
||||
# arguments=['--ros-args', '--log-level', 'debug'],
|
||||
parameters=[{
|
||||
'config_file': LaunchConfiguration('config_file')
|
||||
}]
|
||||
)
|
||||
|
||||
pcd2depth_config_path = os.path.join(package_dir, 'config', 'control_command.yaml')
|
||||
with open(pcd2depth_config_path, 'r') as f:
|
||||
pcd2depth_params = yaml.safe_load(f)
|
||||
pcd2depth_calib_path = os.path.join(package_dir, 'config', 'calib.yaml')
|
||||
pcd2depth_params['calib_file_path'] = pcd2depth_calib_path
|
||||
pcd2depth_node = Node(
|
||||
package='odin_ros_driver',
|
||||
executable='pcd2depth_ros2_node',
|
||||
name='pcd2depth_ros2_node',
|
||||
output='screen',
|
||||
parameters=[pcd2depth_params]
|
||||
)
|
||||
|
||||
# Cloud reprojection node
|
||||
reprojection_config_path = os.path.join(package_dir, 'config', 'control_command.yaml')
|
||||
with open(reprojection_config_path, 'r') as f:
|
||||
reprojection_params = yaml.safe_load(f)
|
||||
reprojection_calib_path = os.path.join(package_dir, 'config', 'calib.yaml')
|
||||
reprojection_params['calib_file_path'] = reprojection_calib_path
|
||||
cloud_reprojection_node = Node(
|
||||
package='odin_ros_driver',
|
||||
executable='cloud_reprojection_ros2_node',
|
||||
name='cloud_reprojection_ros2_node',
|
||||
output='screen',
|
||||
parameters=[reprojection_params]
|
||||
)
|
||||
|
||||
# Image overlay node - overlays reprojected points on camera image
|
||||
overlay_config_path = os.path.join(package_dir, 'config', 'control_command.yaml')
|
||||
with open(overlay_config_path, 'r') as f:
|
||||
overlay_params = yaml.safe_load(f)
|
||||
image_overlay_node = Node(
|
||||
package='odin_ros_driver',
|
||||
executable='image_overlay_node',
|
||||
name='image_overlay_node',
|
||||
output='screen',
|
||||
parameters=[overlay_params]
|
||||
)
|
||||
|
||||
# Create RViz2 node - loads specified configuration file
|
||||
rviz_node = Node(
|
||||
package='rviz2',
|
||||
executable='rviz2',
|
||||
name='rviz2',
|
||||
output='screen',
|
||||
arguments=['-d', LaunchConfiguration('rviz_config')],
|
||||
condition=IfCondition(LaunchConfiguration('launch_rviz'))
|
||||
)
|
||||
|
||||
# Create launch description
|
||||
ld = LaunchDescription()
|
||||
ld.add_action(config_file_arg)
|
||||
ld.add_action(rviz_config_arg) # Add RViz configuration argument
|
||||
ld.add_action(launch_rviz_arg) # Add launch_rviz argument
|
||||
ld.add_action(host_sdk_node)
|
||||
ld.add_action(pcd2depth_node)
|
||||
ld.add_action(cloud_reprojection_node)
|
||||
ld.add_action(image_overlay_node)
|
||||
ld.add_action(rviz_node) # Add RViz node
|
||||
|
||||
return ld
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,28 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>odin_ros_driver</name>
|
||||
<version>0.0.1</version>
|
||||
<description>ROS2 driver for Odin sensor</description>
|
||||
<maintainer email="oliveiratang@manifoldtech.cn">rlk</maintainer>
|
||||
<license>Apache 2.0</license>
|
||||
<!-- ROS2 uses colcon as the build tool -->
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<!-- ROS2 dependencies -->
|
||||
<depend>rclcpp</depend>
|
||||
<!-- System dependencies -->
|
||||
<depend>std_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>cv_bridge</depend>
|
||||
<depend>image_transport</depend>
|
||||
<depend>pcl_conversions</depend>
|
||||
<depend>message_filters</depend>
|
||||
<depend>tf2</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>tf2_geometry_msgs</depend>
|
||||
<!-- Specify build type as ament -->
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,29 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>odin_ros_driver</name>
|
||||
<version>0.0.1</version>
|
||||
<description>ROS driver for Odin sensor</description>
|
||||
<maintainer email="oliveiratang@manifoldtech.cn">rlk</maintainer>
|
||||
<license>Apache 2.0</license>
|
||||
|
||||
<!-- ROS1 uses catkin as the build tool -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<!-- ROS1 dependencies -->
|
||||
<depend>roscpp</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>cv_bridge</depend>
|
||||
<depend>image_transport</depend>
|
||||
|
||||
<!-- System dependencies -->
|
||||
<depend>eigen</depend>
|
||||
<depend>opencv</depend>
|
||||
<depend>yaml-cpp</depend>
|
||||
|
||||
<!-- Specify build type as catkin -->
|
||||
<export>
|
||||
<build_type>catkin</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,28 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>odin_ros_driver</name>
|
||||
<version>0.0.1</version>
|
||||
<description>ROS2 driver for Odin sensor</description>
|
||||
<maintainer email="oliveiratang@manifoldtech.cn">rlk</maintainer>
|
||||
<license>Apache 2.0</license>
|
||||
<!-- ROS2 uses colcon as the build tool -->
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<!-- ROS2 dependencies -->
|
||||
<depend>rclcpp</depend>
|
||||
<!-- System dependencies -->
|
||||
<depend>std_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>cv_bridge</depend>
|
||||
<depend>image_transport</depend>
|
||||
<depend>pcl_conversions</depend>
|
||||
<depend>message_filters</depend>
|
||||
<depend>tf2</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>tf2_geometry_msgs</depend>
|
||||
<!-- Specify build type as ament -->
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,128 @@
|
||||
#!/bin/bash
|
||||
|
||||
# Get the directory where the script is located (Odin_ROS_Driver directory)
|
||||
PKG_DIR="$(cd "$(dirname "$0")/.."; pwd)"
|
||||
# Calculate the workspace root directory (contains devel, build, src)
|
||||
WORKSPACE_ROOT="$(dirname "$(dirname "$PKG_DIR")")"
|
||||
# Workspace source directory (contains all packages)
|
||||
WORKSPACE_SRC="${WORKSPACE_ROOT}/src"
|
||||
PROJECT_NAME="odin_ros_driver"
|
||||
|
||||
# Define color codes
|
||||
RED='\033[0;31m'
|
||||
GREEN='\033[0;32m'
|
||||
YELLOW='\033[1;33m'
|
||||
NC='\033[0m'
|
||||
|
||||
# Clean workspace function
|
||||
clean_workspace() {
|
||||
echo -e "${YELLOW}Cleaning build directories${NC}"
|
||||
|
||||
# Clean build artifacts in workspace
|
||||
rm -rf "${WORKSPACE_ROOT}/build"
|
||||
rm -rf "${WORKSPACE_ROOT}/install"
|
||||
rm -rf "${WORKSPACE_ROOT}/log"
|
||||
rm -rf "${WORKSPACE_ROOT}/devel"
|
||||
|
||||
echo -e "${GREEN}Cleanup complete${NC}"
|
||||
}
|
||||
|
||||
# Run node function
|
||||
run_node() {
|
||||
echo -e "${YELLOW}Running ROS1 node${NC}"
|
||||
|
||||
# Check if environment file exists
|
||||
if [ ! -f "${WORKSPACE_ROOT}/devel/setup.bash" ]; then
|
||||
echo -e "${RED}Could not find devel/setup.bash, please build the project with ./build_ros1.sh first${NC}"
|
||||
return 1
|
||||
fi
|
||||
|
||||
# Source environment and run node
|
||||
source "${WORKSPACE_ROOT}/devel/setup.bash"
|
||||
|
||||
}
|
||||
|
||||
# Build workspace function
|
||||
build_workspace() {
|
||||
echo -e "${YELLOW}Workspace structure:${NC}"
|
||||
echo " Workspace root: ${WORKSPACE_ROOT}"
|
||||
echo " Source directory: ${WORKSPACE_SRC}"
|
||||
echo " Package directory: ${PKG_DIR}"
|
||||
echo " ROS version: ROS1"
|
||||
|
||||
echo -e "${YELLOW}Starting ROS1 project build...${NC}"
|
||||
|
||||
# Clean
|
||||
cd $WS_DIR
|
||||
rm -rf build devel install
|
||||
|
||||
# Ensure ROS1 environment is loaded
|
||||
if [ -f "/opt/ros/noetic/setup.bash" ]; then
|
||||
source "/opt/ros/noetic/setup.bash"
|
||||
elif [ -f "/opt/ros/melodic/setup.bash" ]; then
|
||||
source "/opt/ros/melodic/setup.bash"
|
||||
else
|
||||
echo -e "${RED}Could not find ROS1 setup.bash file. Please ensure ROS1 is installed.${NC}"
|
||||
return 1
|
||||
fi
|
||||
|
||||
# Create temporary package.xml
|
||||
if [ -f "${PKG_DIR}/package_ros1.xml" ]; then
|
||||
echo "Creating temporary package.xml (using package_ros1.xml)"
|
||||
cp "${PKG_DIR}/package_ros1.xml" "${PKG_DIR}/package.xml"
|
||||
TEMP_PACKAGE=true
|
||||
elif [ -f "${PKG_DIR}/package.xml" ]; then
|
||||
echo "Using existing package.xml"
|
||||
else
|
||||
echo -e "${RED}Could not find package.xml in package directory${NC}"
|
||||
return 1
|
||||
fi
|
||||
|
||||
# Set build system variable
|
||||
export BUILD_SYSTEM=ROS1
|
||||
|
||||
# Switch to workspace root and build
|
||||
cd "${WORKSPACE_ROOT}" || return 1
|
||||
catkin_make -DBUILD_SYSTEM=ROS1 -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -j$(nproc)
|
||||
BUILD_RESULT=$?
|
||||
|
||||
# If build successful, source environment
|
||||
if [[ $BUILD_RESULT -eq 0 ]]; then
|
||||
echo -e "${GREEN}ROS1 build successful, loading environment: source devel/setup.bash${NC}"
|
||||
source "${WORKSPACE_ROOT}/devel/setup.bash"
|
||||
else
|
||||
echo -e "${RED}ROS1 build failed, please check error logs${NC}"
|
||||
fi
|
||||
|
||||
|
||||
}
|
||||
|
||||
# Help function
|
||||
show_help() {
|
||||
echo -e "${YELLOW}Usage:${NC}"
|
||||
echo " ./build_ros.sh # Build project"
|
||||
echo " ./build_ros.sh -c # Clean build artifacts"
|
||||
echo " ./build_ros.sh -h # Show help information"
|
||||
echo ""
|
||||
echo -e "${YELLOW}Current configuration:${NC}"
|
||||
echo " Project name: ${PROJECT_NAME}"
|
||||
echo " Package directory: ${PKG_DIR}"
|
||||
echo " Workspace root: ${WORKSPACE_ROOT}"
|
||||
echo " Source directory: ${WORKSPACE_SRC}"
|
||||
}
|
||||
|
||||
# Main
|
||||
case "$1" in
|
||||
-c|--clean)
|
||||
clean_workspace
|
||||
;;
|
||||
-r|--run)
|
||||
run_node
|
||||
;;
|
||||
-h|--help)
|
||||
show_help
|
||||
;;
|
||||
*)
|
||||
build_workspace
|
||||
;;
|
||||
esac
|
||||
@@ -0,0 +1,158 @@
|
||||
#!/bin/bash
|
||||
|
||||
# Get the directory where the script is located (Odin_ROS_Driver directory)
|
||||
PKG_DIR="$(cd "$(dirname "$0")/.."; pwd)"
|
||||
# Calculate the workspace root directory (contains devel, build, src)
|
||||
WORKSPACE_ROOT="$(dirname "$(dirname "$PKG_DIR")")"
|
||||
# Workspace source directory (contains all packages)
|
||||
WORKSPACE_SRC="${WORKSPACE_ROOT}/src"
|
||||
PROJECT_NAME="odin_ros_driver"
|
||||
PACKAGE_DIR_NAME=$(basename "$PKG_DIR")
|
||||
|
||||
# Define color codes
|
||||
RED='\033[0;31m'
|
||||
GREEN='\033[0;32m'
|
||||
YELLOW='\033[1;33m'
|
||||
NC='\033[0m'
|
||||
|
||||
# Extract package name from package.xml
|
||||
get_package_name() {
|
||||
local package_xml="$1"
|
||||
if [ -f "$package_xml" ]; then
|
||||
# Extract content of <name> tag
|
||||
grep -oP '<name>\K[^<]+' "$package_xml" | head -1
|
||||
else
|
||||
echo ""
|
||||
fi
|
||||
}
|
||||
|
||||
# Clean workspace function
|
||||
clean_workspace() {
|
||||
echo -e "${YELLOW}Cleaning build directories${NC}"
|
||||
|
||||
# Clean build artifacts in workspace
|
||||
rm -rf "${WORKSPACE_ROOT}/build"
|
||||
rm -rf "${WORKSPACE_ROOT}/install"
|
||||
rm -rf "${WORKSPACE_ROOT}/log"
|
||||
rm -rf "${WORKSPACE_ROOT}/devel"
|
||||
|
||||
echo -e "${GREEN}Cleanup complete${NC}"
|
||||
}
|
||||
|
||||
# Run node function
|
||||
run_node() {
|
||||
echo -e "${YELLOW}Running ROS2 node${NC}"
|
||||
|
||||
# Check if environment file exists
|
||||
if [ ! -f "${WORKSPACE_ROOT}/install/setup.bash" ]; then
|
||||
echo -e "${RED}Could not find install/setup.bash, please build the project with ./build_ros2.sh first${NC}"
|
||||
return 1
|
||||
fi
|
||||
|
||||
# Source environment and run node
|
||||
source "${WORKSPACE_ROOT}/install/setup.bash"
|
||||
|
||||
}
|
||||
|
||||
# Build workspace function
|
||||
build_workspace() {
|
||||
echo -e "${YELLOW}Workspace structure:${NC}"
|
||||
echo " Workspace root: ${WORKSPACE_ROOT}"
|
||||
echo " Source directory: ${WORKSPACE_SRC}"
|
||||
echo " Package directory: ${PKG_DIR}"
|
||||
echo " Directory name: ${PACKAGE_DIR_NAME}"
|
||||
echo " ROS version: ROS2"
|
||||
|
||||
echo -e "${YELLOW}Starting ROS2 project build...${NC}"
|
||||
|
||||
cd $WS_DIR
|
||||
rm -rf build install log
|
||||
# Ensure ROS2 environment is loaded
|
||||
if [ -f "/opt/ros/foxy/setup.bash" ]; then
|
||||
source "/opt/ros/foxy/setup.bash"
|
||||
elif [ -f "/opt/ros/galactic/setup.bash" ]; then
|
||||
source "/opt/ros/galactic/setup.bash"
|
||||
elif [ -f "/opt/ros/humble/setup.bash" ]; then
|
||||
source "/opt/ros/humble/setup.bash"
|
||||
else
|
||||
echo -e "${RED}Could not find ROS2 setup.bash file. Please ensure ROS2 is installed.${NC}"
|
||||
return 1
|
||||
fi
|
||||
|
||||
# Create temporary package.xml
|
||||
if [ -f "${PKG_DIR}/package_ros2.xml" ]; then
|
||||
echo "Creating temporary package.xml (using package_ros2.xml)"
|
||||
cp "${PKG_DIR}/package_ros2.xml" "${PKG_DIR}/package.xml"
|
||||
TEMP_PACKAGE=true
|
||||
elif [ -f "${PKG_DIR}/package.xml" ]; then
|
||||
echo "Using existing package.xml"
|
||||
TEMP_PACKAGE=false
|
||||
else
|
||||
echo -e "${RED}Could not find package.xml in package directory${NC}"
|
||||
return 1
|
||||
fi
|
||||
|
||||
# Extract package name from package.xml
|
||||
PACKAGE_NAME=$(get_package_name "${PKG_DIR}/package.xml")
|
||||
if [ -z "$PACKAGE_NAME" ]; then
|
||||
echo -e "${RED}Failed to extract package name from package.xml${NC}"
|
||||
return 1
|
||||
fi
|
||||
echo " Package name: ${PACKAGE_NAME}"
|
||||
|
||||
# Set build system variable
|
||||
export BUILD_SYSTEM=ROS2
|
||||
|
||||
# Switch to workspace root and build
|
||||
cd "${WORKSPACE_ROOT}" || return 1
|
||||
|
||||
# Build with correct package name
|
||||
colcon build \
|
||||
--packages-select "${PACKAGE_NAME}" \
|
||||
--parallel-workers $(nproc) \
|
||||
--cmake-args \
|
||||
-DBUILD_SYSTEM=ROS2 \
|
||||
-DCMAKE_EXPORT_COMPILE_COMMANDS=ON
|
||||
|
||||
BUILD_RESULT=$?
|
||||
|
||||
# If build successful, source environment
|
||||
if [[ $BUILD_RESULT -eq 0 ]]; then
|
||||
echo -e "${GREEN}ROS2 build successful, loading environment: source install/setup.bash${NC}"
|
||||
source "${WORKSPACE_ROOT}/install/setup.bash"
|
||||
|
||||
else
|
||||
echo -e "${RED}ROS2 build failed, please check error logs${NC}"
|
||||
fi
|
||||
|
||||
}
|
||||
|
||||
# Help function
|
||||
show_help() {
|
||||
echo -e "${YELLOW}Usage:${NC}"
|
||||
echo " ./build_ros2.sh # Build project"
|
||||
echo " ./build_ros2.sh -c # Clean build artifacts"
|
||||
echo " ./build_ros2.sh -h # Show help information"
|
||||
echo ""
|
||||
echo -e "${YELLOW}Current configuration:${NC}"
|
||||
echo " Project name: ${PROJECT_NAME}"
|
||||
echo " Package directory: ${PKG_DIR}"
|
||||
echo " Workspace root: ${WORKSPACE_ROOT}"
|
||||
echo " Source directory: ${WORKSPACE_SRC}"
|
||||
}
|
||||
|
||||
# Main program
|
||||
case "$1" in
|
||||
-c|--clean)
|
||||
clean_workspace
|
||||
;;
|
||||
-r|--run)
|
||||
run_node
|
||||
;;
|
||||
-h|--help)
|
||||
show_help
|
||||
;;
|
||||
*)
|
||||
build_workspace
|
||||
;;
|
||||
esac
|
||||
@@ -0,0 +1,21 @@
|
||||
#!/bin/bash
|
||||
|
||||
# Usage: ./set_param.sh <parameter_name> <value>
|
||||
# Example: ./set_param.sh save_map 1
|
||||
|
||||
if [ $# -ne 2 ]; then
|
||||
echo "Usage: $0 <parameter_name> <value>"
|
||||
echo "Example: $0 save_map 1"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
PARAM_NAME=$1
|
||||
VALUE=$2
|
||||
|
||||
COMMAND_FILE="/tmp/odin_command.txt"
|
||||
|
||||
# Create the command file with the parameter
|
||||
echo "set $PARAM_NAME $VALUE" > "$COMMAND_FILE"
|
||||
|
||||
echo "Command sent: set $PARAM_NAME $VALUE"
|
||||
echo "Command file: $COMMAND_FILE"
|
||||
+232
@@ -0,0 +1,232 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
#include "camera_pose_visualization.h"
|
||||
|
||||
const Eigen::Vector3d camera_pose_visualization::imlt = Eigen::Vector3d(-1.0, -0.5, 1.0);
|
||||
const Eigen::Vector3d camera_pose_visualization::imrt = Eigen::Vector3d( 1.0, -0.5, 1.0);
|
||||
const Eigen::Vector3d camera_pose_visualization::imlb = Eigen::Vector3d(-1.0, 0.5, 1.0);
|
||||
const Eigen::Vector3d camera_pose_visualization::imrb = Eigen::Vector3d( 1.0, 0.5, 1.0);
|
||||
const Eigen::Vector3d camera_pose_visualization::lt0 = Eigen::Vector3d(-0.7, -0.5, 1.0);
|
||||
const Eigen::Vector3d camera_pose_visualization::lt1 = Eigen::Vector3d(-0.7, -0.2, 1.0);
|
||||
const Eigen::Vector3d camera_pose_visualization::lt2 = Eigen::Vector3d(-1.0, -0.2, 1.0);
|
||||
const Eigen::Vector3d camera_pose_visualization::oc = Eigen::Vector3d(0.0, 0.0, 0.0);
|
||||
|
||||
#ifdef ROS2
|
||||
using GeometryPoint = geometry_msgs::msg::Point;
|
||||
#else
|
||||
using GeometryPoint = geometry_msgs::Point;
|
||||
#endif
|
||||
|
||||
void Eigen2Point(const Eigen::Vector3d& v, GeometryPoint& p) {
|
||||
p.x = v.x();
|
||||
p.y = v.y();
|
||||
p.z = v.z();
|
||||
}
|
||||
|
||||
camera_pose_visualization::camera_pose_visualization(float r, float g, float b, float a)
|
||||
: m_marker_ns("camera_pose_visualization"), m_scale(0.3), m_line_width(0.03) {
|
||||
m_image_boundary_color.r = r;
|
||||
m_image_boundary_color.g = g;
|
||||
m_image_boundary_color.b = b;
|
||||
m_image_boundary_color.a = a;
|
||||
m_optical_center_connector_color.r = r;
|
||||
m_optical_center_connector_color.g = g;
|
||||
m_optical_center_connector_color.b = b;
|
||||
m_optical_center_connector_color.a = a;
|
||||
}
|
||||
|
||||
void camera_pose_visualization::setImageBoundaryColor(float r, float g, float b, float a) {
|
||||
m_image_boundary_color.r = r;
|
||||
m_image_boundary_color.g = g;
|
||||
m_image_boundary_color.b = b;
|
||||
m_image_boundary_color.a = a;
|
||||
}
|
||||
|
||||
void camera_pose_visualization::setOpticalCenterConnectorColor(float r, float g, float b, float a) {
|
||||
m_optical_center_connector_color.r = r;
|
||||
m_optical_center_connector_color.g = g;
|
||||
m_optical_center_connector_color.b = b;
|
||||
m_optical_center_connector_color.a = a;
|
||||
}
|
||||
|
||||
void camera_pose_visualization::setScale(double s) {
|
||||
m_scale = s;
|
||||
}
|
||||
void camera_pose_visualization::setLineWidth(double width) {
|
||||
m_line_width = width;
|
||||
}
|
||||
void camera_pose_visualization::add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1) {
|
||||
Marker marker;
|
||||
|
||||
marker.ns = m_marker_ns;
|
||||
marker.id = m_markers.size() + 1;
|
||||
#ifdef ROS2
|
||||
marker.type = Marker::LINE_LIST;
|
||||
marker.action = Marker::ADD;
|
||||
#else
|
||||
marker.type = visualization_msgs::Marker::LINE_LIST;
|
||||
marker.action = visualization_msgs::Marker::ADD;
|
||||
#endif
|
||||
marker.scale.x = 0.005;
|
||||
|
||||
marker.color.g = 1.0f;
|
||||
marker.color.a = 1.0;
|
||||
|
||||
GeometryPoint point0, point1;
|
||||
|
||||
Eigen2Point(p0, point0);
|
||||
Eigen2Point(p1, point1);
|
||||
|
||||
marker.points.push_back(point0);
|
||||
marker.points.push_back(point1);
|
||||
|
||||
m_markers.push_back(marker);
|
||||
}
|
||||
|
||||
void camera_pose_visualization::add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1) {
|
||||
Marker marker;
|
||||
|
||||
marker.ns = m_marker_ns;
|
||||
marker.id = m_markers.size() + 1;
|
||||
#ifdef ROS2
|
||||
marker.type = Marker::LINE_LIST;
|
||||
marker.action = Marker::ADD;
|
||||
#else
|
||||
marker.type = visualization_msgs::Marker::LINE_LIST;
|
||||
marker.action = visualization_msgs::Marker::ADD;
|
||||
#endif
|
||||
marker.scale.x = 0.04;
|
||||
//marker.scale.x = 0.3;
|
||||
|
||||
marker.color.r = 1.0f;
|
||||
marker.color.b = 1.0f;
|
||||
marker.color.a = 1.0;
|
||||
|
||||
GeometryPoint point0, point1;
|
||||
|
||||
Eigen2Point(p0, point0);
|
||||
Eigen2Point(p1, point1);
|
||||
|
||||
marker.points.push_back(point0);
|
||||
marker.points.push_back(point1);
|
||||
|
||||
m_markers.push_back(marker);
|
||||
}
|
||||
|
||||
|
||||
void camera_pose_visualization::add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q) {
|
||||
Marker marker;
|
||||
|
||||
marker.ns = m_marker_ns;
|
||||
marker.id = m_markers.size() + 1;
|
||||
#ifdef ROS2
|
||||
marker.type = Marker::LINE_STRIP;
|
||||
marker.action = Marker::ADD;
|
||||
#else
|
||||
marker.type = visualization_msgs::Marker::LINE_STRIP;
|
||||
marker.action = visualization_msgs::Marker::ADD;
|
||||
#endif
|
||||
marker.scale.x = m_line_width;
|
||||
|
||||
marker.pose.position.x = 0.0;
|
||||
marker.pose.position.y = 0.0;
|
||||
marker.pose.position.z = 0.0;
|
||||
marker.pose.orientation.w = 1.0;
|
||||
marker.pose.orientation.x = 0.0;
|
||||
marker.pose.orientation.y = 0.0;
|
||||
marker.pose.orientation.z = 0.0;
|
||||
|
||||
|
||||
GeometryPoint pt_lt, pt_lb, pt_rt, pt_rb, pt_oc, pt_lt0, pt_lt1, pt_lt2;
|
||||
|
||||
Eigen2Point(q * (m_scale * imlt) + p, pt_lt);
|
||||
Eigen2Point(q * (m_scale * imlb) + p, pt_lb);
|
||||
Eigen2Point(q * (m_scale * imrt) + p, pt_rt);
|
||||
Eigen2Point(q * (m_scale * imrb) + p, pt_rb);
|
||||
Eigen2Point(q * (m_scale * lt0 ) + p, pt_lt0);
|
||||
Eigen2Point(q * (m_scale * lt1 ) + p, pt_lt1);
|
||||
Eigen2Point(q * (m_scale * lt2 ) + p, pt_lt2);
|
||||
Eigen2Point(q * (m_scale * oc ) + p, pt_oc);
|
||||
|
||||
// image boundaries
|
||||
marker.points.push_back(pt_lt);
|
||||
marker.points.push_back(pt_lb);
|
||||
marker.colors.push_back(m_image_boundary_color);
|
||||
marker.colors.push_back(m_image_boundary_color);
|
||||
|
||||
marker.points.push_back(pt_lb);
|
||||
marker.points.push_back(pt_rb);
|
||||
marker.colors.push_back(m_image_boundary_color);
|
||||
marker.colors.push_back(m_image_boundary_color);
|
||||
|
||||
marker.points.push_back(pt_rb);
|
||||
marker.points.push_back(pt_rt);
|
||||
marker.colors.push_back(m_image_boundary_color);
|
||||
marker.colors.push_back(m_image_boundary_color);
|
||||
|
||||
marker.points.push_back(pt_rt);
|
||||
marker.points.push_back(pt_lt);
|
||||
marker.colors.push_back(m_image_boundary_color);
|
||||
marker.colors.push_back(m_image_boundary_color);
|
||||
|
||||
// top-left indicator
|
||||
marker.points.push_back(pt_lt0);
|
||||
marker.points.push_back(pt_lt1);
|
||||
marker.colors.push_back(m_image_boundary_color);
|
||||
marker.colors.push_back(m_image_boundary_color);
|
||||
|
||||
marker.points.push_back(pt_lt1);
|
||||
marker.points.push_back(pt_lt2);
|
||||
marker.colors.push_back(m_image_boundary_color);
|
||||
marker.colors.push_back(m_image_boundary_color);
|
||||
|
||||
// optical center connector
|
||||
marker.points.push_back(pt_lt);
|
||||
marker.points.push_back(pt_oc);
|
||||
marker.colors.push_back(m_optical_center_connector_color);
|
||||
marker.colors.push_back(m_optical_center_connector_color);
|
||||
|
||||
|
||||
marker.points.push_back(pt_lb);
|
||||
marker.points.push_back(pt_oc);
|
||||
marker.colors.push_back(m_optical_center_connector_color);
|
||||
marker.colors.push_back(m_optical_center_connector_color);
|
||||
|
||||
marker.points.push_back(pt_rt);
|
||||
marker.points.push_back(pt_oc);
|
||||
marker.colors.push_back(m_optical_center_connector_color);
|
||||
marker.colors.push_back(m_optical_center_connector_color);
|
||||
|
||||
marker.points.push_back(pt_rb);
|
||||
marker.points.push_back(pt_oc);
|
||||
marker.colors.push_back(m_optical_center_connector_color);
|
||||
marker.colors.push_back(m_optical_center_connector_color);
|
||||
|
||||
m_markers.push_back(marker);
|
||||
}
|
||||
|
||||
void camera_pose_visualization::reset() {
|
||||
m_markers.clear();
|
||||
}
|
||||
|
||||
void camera_pose_visualization::publish_by( Publisher& pub, const Header& header ) {
|
||||
MarkerArray markerArray_msg;
|
||||
|
||||
for (auto& marker : m_markers) {
|
||||
marker.header = header;
|
||||
markerArray_msg.markers.push_back(marker);
|
||||
}
|
||||
|
||||
pub.publish(markerArray_msg);
|
||||
}
|
||||
@@ -0,0 +1,503 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cloud_reprojection_ros_node.hpp"
|
||||
|
||||
#include <fstream>
|
||||
#include <sys/stat.h>
|
||||
#include <thread>
|
||||
#include <chrono>
|
||||
|
||||
#ifdef ROS2
|
||||
#include <functional>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
#include <rcpputils/filesystem_helper.hpp>
|
||||
#else
|
||||
#include <boost/bind.hpp>
|
||||
#endif
|
||||
|
||||
// Fixed Til (T_imu_lidar): lidar position in imu frame, transforms from lidar to imu
|
||||
// TODO: Fill in the actual Til values for your sensor setup
|
||||
static Eigen::Matrix4d getFixedTil()
|
||||
{
|
||||
Eigen::Matrix4d Til = Eigen::Matrix4d::Identity();
|
||||
Til(0, 3) = -0.02663;
|
||||
Til(1, 3) = 0.03447;
|
||||
Til(2, 3) = 0.02174;
|
||||
return Til;
|
||||
}
|
||||
|
||||
static bool fileExists(const std::string& filename) {
|
||||
struct stat buffer;
|
||||
return (stat(filename.c_str(), &buffer) == 0);
|
||||
}
|
||||
|
||||
#ifdef ROS2
|
||||
// Helper function to get package source directory for ROS2
|
||||
static std::string get_package_source_directory() {
|
||||
std::string current_file = __FILE__;
|
||||
size_t pos = current_file.find("/src/cloud_reprojection_ros.cpp");
|
||||
if (pos != std::string::npos) {
|
||||
return current_file.substr(0, pos);
|
||||
}
|
||||
return "";
|
||||
}
|
||||
|
||||
// ==================== ROS2 Implementation ====================
|
||||
|
||||
CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& options)
|
||||
: Node("cloud_reprojection_node", options)
|
||||
{
|
||||
loadParameters();
|
||||
|
||||
RCLCPP_INFO_STREAM(this->get_logger(),
|
||||
"\n cloud_slam_topic: " << cloud_slam_topic_
|
||||
<< "\n odometry_topic: " << odometry_topic_
|
||||
<< "\n wiwc_topic: " << wiwc_topic_
|
||||
<< "\n reprojected_image_topic: " << reprojected_image_topic_);
|
||||
|
||||
cloud_sub_.subscribe(this, cloud_slam_topic_);
|
||||
odom_sub_.subscribe(this, odometry_topic_);
|
||||
wiwc_sub_.subscribe(this, wiwc_topic_);
|
||||
|
||||
sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_);
|
||||
sync_->registerCallback(std::bind(&CloudReprojectionRosNode::syncCallback, this,
|
||||
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
|
||||
reprojected_image_pub_ = image_transport::create_publisher(this, reprojected_image_topic_);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "CloudReprojectionRosNode initialized successfully");
|
||||
}
|
||||
|
||||
void CloudReprojectionRosNode::loadParameters()
|
||||
{
|
||||
// Declare and get parameters
|
||||
this->declare_parameter<std::string>("cloud_slam_topic", "/odin1/cloud_slam");
|
||||
this->declare_parameter<std::string>("odometry_topic", "/odin1/odometry");
|
||||
this->declare_parameter<std::string>("wiwc_topic", "/odin1/wiwc");
|
||||
this->declare_parameter<std::string>("reprojected_image_topic", "/odin1/reprojected_image");
|
||||
|
||||
cloud_slam_topic_ = this->get_parameter("cloud_slam_topic").as_string();
|
||||
odometry_topic_ = this->get_parameter("odometry_topic").as_string();
|
||||
wiwc_topic_ = this->get_parameter("wiwc_topic").as_string();
|
||||
reprojected_image_topic_ = this->get_parameter("reprojected_image_topic").as_string();
|
||||
|
||||
// Load camera parameters from calib.yaml file directly
|
||||
std::string package_path = get_package_source_directory();
|
||||
std::string calib_file = package_path + "/config/calib.yaml";
|
||||
|
||||
YAML::Node calib_config;
|
||||
try {
|
||||
calib_config = YAML::LoadFile(calib_file);
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to load calib.yaml: %s", e.what());
|
||||
rclcpp::shutdown();
|
||||
return;
|
||||
}
|
||||
|
||||
CloudReprojector::CameraParams cam_params;
|
||||
try {
|
||||
cam_params.image_width = calib_config["cam_0"]["image_width"].as<int>();
|
||||
cam_params.image_height = calib_config["cam_0"]["image_height"].as<int>();
|
||||
cam_params.A11 = calib_config["cam_0"]["A11"].as<double>();
|
||||
cam_params.A12 = calib_config["cam_0"]["A12"].as<double>();
|
||||
cam_params.A22 = calib_config["cam_0"]["A22"].as<double>();
|
||||
cam_params.u0 = calib_config["cam_0"]["u0"].as<double>();
|
||||
cam_params.v0 = calib_config["cam_0"]["v0"].as<double>();
|
||||
cam_params.k2 = calib_config["cam_0"]["k2"].as<double>();
|
||||
cam_params.k3 = calib_config["cam_0"]["k3"].as<double>();
|
||||
cam_params.k4 = calib_config["cam_0"]["k4"].as<double>();
|
||||
cam_params.k5 = calib_config["cam_0"]["k5"].as<double>();
|
||||
cam_params.k6 = calib_config["cam_0"]["k6"].as<double>();
|
||||
cam_params.k7 = calib_config["cam_0"]["k7"].as<double>();
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to parse camera parameters: %s", e.what());
|
||||
rclcpp::shutdown();
|
||||
return;
|
||||
}
|
||||
|
||||
// Load extrinsic parameters
|
||||
CloudReprojector::ExtrinsicParams ext_params;
|
||||
try {
|
||||
auto Tcl_vec = calib_config["Tcl_0"].as<std::vector<double>>();
|
||||
|
||||
if (Tcl_vec.size() == 16)
|
||||
{
|
||||
for (int i = 0; i < 4; ++i)
|
||||
for (int j = 0; j < 4; ++j)
|
||||
ext_params.Tcl(i, j) = Tcl_vec[i * 4 + j];
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Tcl_0 has invalid size: %zu (expected 16)", Tcl_vec.size());
|
||||
rclcpp::shutdown();
|
||||
return;
|
||||
}
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to parse Tcl_0: %s", e.what());
|
||||
rclcpp::shutdown();
|
||||
return;
|
||||
}
|
||||
|
||||
ext_params.Til = getFixedTil();
|
||||
ext_params.Tic = CloudReprojector::calculateTic(ext_params.Tcl, ext_params.Til);
|
||||
|
||||
RCLCPP_INFO_STREAM(this->get_logger(), "Loaded Tcl (camera to lidar):\n" << ext_params.Tcl);
|
||||
RCLCPP_INFO_STREAM(this->get_logger(), "Fixed Til (lidar to imu):\n" << ext_params.Til);
|
||||
RCLCPP_INFO_STREAM(this->get_logger(), "Calculated Tic:\n" << ext_params.Tic);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Camera intrinsics:");
|
||||
RCLCPP_INFO(this->get_logger(), "Image size: %dx%d", cam_params.image_width, cam_params.image_height);
|
||||
RCLCPP_INFO(this->get_logger(), "Intrinsics: A11=%f A12=%f A22=%f u0=%f v0=%f",
|
||||
cam_params.A11, cam_params.A12, cam_params.A22, cam_params.u0, cam_params.v0);
|
||||
|
||||
reprojector_ = std::make_unique<CloudReprojector>();
|
||||
if (!reprojector_->initialize(cam_params, ext_params))
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to initialize CloudReprojector");
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
}
|
||||
|
||||
void CloudReprojectionRosNode::syncCallback(
|
||||
const PointCloud2::ConstSharedPtr& cloud_msg,
|
||||
const Odometry::ConstSharedPtr& odom_msg,
|
||||
const Odometry::ConstSharedPtr& wiwc_msg)
|
||||
{
|
||||
// Debug: print that syncCallback is called
|
||||
static int sync_count = 0;
|
||||
// RCLCPP_INFO(this->get_logger(), "=== syncCallback called, count: %d ===", ++sync_count);
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZRGB> cloud_odom;
|
||||
pcl::fromROSMsg(*cloud_msg, cloud_odom);
|
||||
|
||||
if (cloud_odom.empty())
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "Empty cloud_slam received");
|
||||
return;
|
||||
}
|
||||
|
||||
// Extract real-time extrinsics from WIWC message covariance fields
|
||||
// pose.covariance contains T_CL (first 16 values), twist.covariance contains T_IL (first 16 values)
|
||||
Eigen::Matrix4d T_CL = Eigen::Matrix4d::Identity();
|
||||
Eigen::Matrix4d T_IL = Eigen::Matrix4d::Identity();
|
||||
for (int i = 0; i < 16; ++i) {
|
||||
T_CL(i / 4, i % 4) = wiwc_msg->pose.covariance[i];
|
||||
T_IL(i / 4, i % 4) = wiwc_msg->twist.covariance[i];
|
||||
}
|
||||
|
||||
// Update extrinsics if valid (not identity matrix)
|
||||
bool T_CL_valid = (T_CL - Eigen::Matrix4d::Identity()).norm() > 1e-6;
|
||||
bool T_IL_valid = (T_IL - Eigen::Matrix4d::Identity()).norm() > 1e-6;
|
||||
|
||||
// // Debug print to compare with host_sdk_sample values
|
||||
// static int print_count = 0;
|
||||
// if (print_count++) {
|
||||
// // Extract rotation (3x3) and translation (3x1) from T_CL
|
||||
// Eigen::Matrix3d RCL = T_CL.block<3,3>(0,0);
|
||||
// Eigen::Vector3d TCL = T_CL.block<3,1>(0,3);
|
||||
// RCLCPP_INFO_STREAM(this->get_logger(), "=== cloud_reprojection RCL (3x3 rotation from T_CL) ===\n" << RCL);
|
||||
// RCLCPP_INFO_STREAM(this->get_logger(), "=== cloud_reprojection TCL (3x1 translation from T_CL) ===\n" << TCL.transpose());
|
||||
|
||||
// // Extract rotation (3x3) and translation (3x1) from T_IL
|
||||
// Eigen::Matrix3d RIL = T_IL.block<3,3>(0,0);
|
||||
// Eigen::Vector3d TIL = T_IL.block<3,1>(0,3);
|
||||
// RCLCPP_INFO_STREAM(this->get_logger(), "=== cloud_reprojection RIL (3x3 rotation from T_IL) ===\n" << RIL);
|
||||
// RCLCPP_INFO_STREAM(this->get_logger(), "=== cloud_reprojection TIL (3x1 translation from T_IL) ===\n" << TIL.transpose());
|
||||
|
||||
// RCLCPP_INFO(this->get_logger(), "T_CL_valid: %d, T_IL_valid: %d", T_CL_valid, T_IL_valid);
|
||||
// if (T_CL_valid && T_IL_valid) {
|
||||
// Eigen::Matrix4d Tic = CloudReprojector::calculateTic(T_CL, T_IL);
|
||||
// RCLCPP_INFO_STREAM(this->get_logger(), "=== cloud_reprojection calculated Tic ===\n" << Tic);
|
||||
// }
|
||||
// }
|
||||
|
||||
if (T_CL_valid && T_IL_valid) {
|
||||
reprojector_->updateExtrinsics(T_CL, T_IL);
|
||||
}
|
||||
|
||||
CloudReprojector::OdomPose odom_pose;
|
||||
odom_pose.orientation = Eigen::Quaterniond(
|
||||
odom_msg->pose.pose.orientation.w,
|
||||
odom_msg->pose.pose.orientation.x,
|
||||
odom_msg->pose.pose.orientation.y,
|
||||
odom_msg->pose.pose.orientation.z
|
||||
);
|
||||
odom_pose.position = Eigen::Vector3d(
|
||||
odom_msg->pose.pose.position.x,
|
||||
odom_msg->pose.pose.position.y,
|
||||
odom_msg->pose.pose.position.z
|
||||
);
|
||||
|
||||
cv::Mat reprojected_img = reprojector_->reprojectCloud(cloud_odom, odom_pose);
|
||||
|
||||
auto img_msg = cv_bridge::CvImage(cloud_msg->header, "bgr8", reprojected_img).toImageMsg();
|
||||
reprojected_image_pub_.publish(*img_msg);
|
||||
}
|
||||
|
||||
// ==================== ROS2 Main ====================
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
auto temp_node = std::make_shared<rclcpp::Node>("cloud_reprojection_check");
|
||||
|
||||
// Check if reprojection is enabled from control_command.yaml
|
||||
std::string package_path = get_package_source_directory();
|
||||
std::string config_file = package_path + "/config/control_command.yaml";
|
||||
|
||||
try {
|
||||
YAML::Node config = YAML::LoadFile(config_file);
|
||||
std::cout << "config: " << config_file << std::endl;
|
||||
if (!config["register_keys"] || !config["register_keys"]["sendreprojection"]) {
|
||||
RCLCPP_INFO(temp_node->get_logger(), "sendreprojection parameter not found, cloud reprojection disabled.");
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
|
||||
int sendreprojection = config["register_keys"]["sendreprojection"].as<int>();
|
||||
if (sendreprojection == 0) {
|
||||
RCLCPP_INFO(temp_node->get_logger(), "Cloud reprojection will not be published.");
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(temp_node->get_logger(), "Failed to read config: %s", e.what());
|
||||
rclcpp::shutdown();
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Wait for calib.yaml file to be generated by host_sdk_sample
|
||||
std::string calib_file = package_path + "/config/calib.yaml";
|
||||
RCLCPP_INFO(temp_node->get_logger(), "Waiting for calib.yaml file at: %s", calib_file.c_str());
|
||||
|
||||
int wait_count = 0;
|
||||
while (rclcpp::ok() && !fileExists(calib_file)) {
|
||||
if (wait_count % 10 == 0) {
|
||||
RCLCPP_INFO(temp_node->get_logger(), "Still waiting for calib.yaml file...");
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(5000));
|
||||
wait_count++;
|
||||
|
||||
// Timeout after 5 seconds
|
||||
if (wait_count > 10) {
|
||||
RCLCPP_ERROR(temp_node->get_logger(), "Timeout waiting for calib.yaml file");
|
||||
rclcpp::shutdown();
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
if (!rclcpp::ok()) {
|
||||
RCLCPP_INFO(temp_node->get_logger(), "Node shutdown before calib.yaml file was found.");
|
||||
return 0;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(temp_node->get_logger(), "Found calib.yaml file! Starting cloud reprojection node...");
|
||||
|
||||
auto node = std::make_shared<CloudReprojectionRosNode>();
|
||||
|
||||
RCLCPP_INFO(node->get_logger(), "CloudReprojectionRosNode started");
|
||||
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
#else
|
||||
// ==================== ROS1 Implementation ====================
|
||||
|
||||
CloudReprojectionRosNode::CloudReprojectionRosNode(ros::NodeHandle& nh, ros::NodeHandle& pnh)
|
||||
: nh_(nh), pnh_(pnh)
|
||||
{
|
||||
loadParameters();
|
||||
|
||||
ROS_INFO_STREAM("\n cloud_slam_topic: " << cloud_slam_topic_
|
||||
<< "\n odometry_topic: " << odometry_topic_
|
||||
<< "\n wiwc_topic: " << wiwc_topic_
|
||||
<< "\n reprojected_image_topic: " << reprojected_image_topic_);
|
||||
|
||||
cloud_sub_.subscribe(nh_, cloud_slam_topic_, 1);
|
||||
odom_sub_.subscribe(nh_, odometry_topic_, 1);
|
||||
wiwc_sub_.subscribe(nh_, wiwc_topic_, 1);
|
||||
|
||||
sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_);
|
||||
sync_->registerCallback(boost::bind(&CloudReprojectionRosNode::syncCallback, this, _1, _2, _3));
|
||||
|
||||
reprojected_image_pub_ = nh_.advertise<sensor_msgs::Image>(reprojected_image_topic_, 1);
|
||||
|
||||
ROS_INFO("CloudReprojectionRosNode initialized successfully");
|
||||
}
|
||||
|
||||
void CloudReprojectionRosNode::loadParameters()
|
||||
{
|
||||
pnh_.param<std::string>("cloud_slam_topic", cloud_slam_topic_, std::string("/odin1/cloud_slam"));
|
||||
pnh_.param<std::string>("odometry_topic", odometry_topic_, std::string("/odin1/odometry"));
|
||||
pnh_.param<std::string>("wiwc_topic", wiwc_topic_, std::string("/odin1/wiwc"));
|
||||
pnh_.param<std::string>("reprojected_image_topic", reprojected_image_topic_, std::string("/odin1/reprojected_image"));
|
||||
|
||||
// Load camera parameters
|
||||
CloudReprojector::CameraParams cam_params;
|
||||
pnh_.param<int>("cam_0/image_width", cam_params.image_width, 1600);
|
||||
pnh_.param<int>("cam_0/image_height", cam_params.image_height, 1296);
|
||||
pnh_.param<double>("cam_0/A11", cam_params.A11, 0.0);
|
||||
pnh_.param<double>("cam_0/A12", cam_params.A12, 0.0);
|
||||
pnh_.param<double>("cam_0/A22", cam_params.A22, 0.0);
|
||||
pnh_.param<double>("cam_0/u0", cam_params.u0, 0.0);
|
||||
pnh_.param<double>("cam_0/v0", cam_params.v0, 0.0);
|
||||
pnh_.param<double>("cam_0/k2", cam_params.k2, 0.0);
|
||||
pnh_.param<double>("cam_0/k3", cam_params.k3, 0.0);
|
||||
pnh_.param<double>("cam_0/k4", cam_params.k4, 0.0);
|
||||
pnh_.param<double>("cam_0/k5", cam_params.k5, 0.0);
|
||||
pnh_.param<double>("cam_0/k6", cam_params.k6, 0.0);
|
||||
pnh_.param<double>("cam_0/k7", cam_params.k7, 0.0);
|
||||
|
||||
// Load extrinsic parameters
|
||||
CloudReprojector::ExtrinsicParams ext_params;
|
||||
std::vector<double> Tcl_vec_param;
|
||||
if (pnh_.getParam("Tcl_0", Tcl_vec_param) && Tcl_vec_param.size() == 16)
|
||||
{
|
||||
for (int i = 0; i < 4; ++i)
|
||||
for (int j = 0; j < 4; ++j)
|
||||
ext_params.Tcl(i, j) = Tcl_vec_param[i * 4 + j];
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Tcl_0 param missing or invalid.");
|
||||
ros::shutdown();
|
||||
return;
|
||||
}
|
||||
|
||||
ext_params.Til = getFixedTil();
|
||||
ext_params.Tic = CloudReprojector::calculateTic(ext_params.Tcl, ext_params.Til);
|
||||
|
||||
ROS_INFO_STREAM("Loaded Tcl (camera to lidar):\n" << ext_params.Tcl);
|
||||
ROS_INFO_STREAM("Fixed Til (lidar to imu):\n" << ext_params.Til);
|
||||
ROS_INFO_STREAM("Calculated Tic:\n" << ext_params.Tic);
|
||||
|
||||
ROS_INFO("Camera intrinsics:");
|
||||
ROS_INFO("Image size: %dx%d", cam_params.image_width, cam_params.image_height);
|
||||
ROS_INFO("Intrinsics: A11=%f A12=%f A22=%f u0=%f v0=%f",
|
||||
cam_params.A11, cam_params.A12, cam_params.A22, cam_params.u0, cam_params.v0);
|
||||
|
||||
reprojector_ = std::make_unique<CloudReprojector>();
|
||||
if (!reprojector_->initialize(cam_params, ext_params))
|
||||
{
|
||||
ROS_ERROR("Failed to initialize CloudReprojector");
|
||||
ros::shutdown();
|
||||
}
|
||||
}
|
||||
|
||||
void CloudReprojectionRosNode::syncCallback(
|
||||
const sensor_msgs::PointCloud2ConstPtr& cloud_msg,
|
||||
const nav_msgs::OdometryConstPtr& odom_msg,
|
||||
const nav_msgs::OdometryConstPtr& wiwc_msg)
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZRGB> cloud_odom;
|
||||
pcl::fromROSMsg(*cloud_msg, cloud_odom);
|
||||
|
||||
if (cloud_odom.empty())
|
||||
{
|
||||
ROS_WARN("Empty cloud_slam received");
|
||||
return;
|
||||
}
|
||||
|
||||
// Extract real-time extrinsics from WIWC message covariance fields
|
||||
// pose.covariance contains T_CL (first 16 values), twist.covariance contains T_IL (first 16 values)
|
||||
Eigen::Matrix4d T_CL = Eigen::Matrix4d::Identity();
|
||||
Eigen::Matrix4d T_IL = Eigen::Matrix4d::Identity();
|
||||
for (int i = 0; i < 16; ++i) {
|
||||
T_CL(i / 4, i % 4) = wiwc_msg->pose.covariance[i];
|
||||
T_IL(i / 4, i % 4) = wiwc_msg->twist.covariance[i];
|
||||
}
|
||||
|
||||
// Update extrinsics if valid (not identity matrix)
|
||||
bool T_CL_valid = (T_CL - Eigen::Matrix4d::Identity()).norm() > 1e-6;
|
||||
bool T_IL_valid = (T_IL - Eigen::Matrix4d::Identity()).norm() > 1e-6;
|
||||
if (T_CL_valid && T_IL_valid) {
|
||||
reprojector_->updateExtrinsics(T_CL, T_IL);
|
||||
}
|
||||
|
||||
CloudReprojector::OdomPose odom_pose;
|
||||
odom_pose.orientation = Eigen::Quaterniond(
|
||||
odom_msg->pose.pose.orientation.w,
|
||||
odom_msg->pose.pose.orientation.x,
|
||||
odom_msg->pose.pose.orientation.y,
|
||||
odom_msg->pose.pose.orientation.z
|
||||
);
|
||||
odom_pose.position = Eigen::Vector3d(
|
||||
odom_msg->pose.pose.position.x,
|
||||
odom_msg->pose.pose.position.y,
|
||||
odom_msg->pose.pose.position.z
|
||||
);
|
||||
|
||||
cv::Mat reprojected_img = reprojector_->reprojectCloud(cloud_odom, odom_pose);
|
||||
|
||||
sensor_msgs::ImagePtr img_msg = cv_bridge::CvImage(cloud_msg->header, "bgr8", reprojected_img).toImageMsg();
|
||||
reprojected_image_pub_.publish(img_msg);
|
||||
}
|
||||
|
||||
// ==================== ROS1 Main ====================
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "cloud_reprojection");
|
||||
ros::NodeHandle nh;
|
||||
ros::NodeHandle pnh("~");
|
||||
|
||||
// Check if reprojection is enabled
|
||||
int sendreprojection = 0;
|
||||
pnh.param("register_keys/sendreprojection", sendreprojection, 0);
|
||||
if(sendreprojection == 0)
|
||||
{
|
||||
ROS_INFO("Cloud reprojection will not be published.");
|
||||
return 0;
|
||||
}
|
||||
|
||||
std::string calib_file_path;
|
||||
pnh.param<std::string>("calib_file_path", calib_file_path, "");
|
||||
|
||||
ROS_INFO("Waiting for calib.yaml file at: %s", calib_file_path.c_str());
|
||||
while(ros::ok() && !fileExists(calib_file_path))
|
||||
{
|
||||
ROS_INFO_THROTTLE(5, "Still waiting for calib.yaml file...");
|
||||
ros::Duration(0.5).sleep();
|
||||
ros::spinOnce();
|
||||
}
|
||||
|
||||
if(!ros::ok())
|
||||
{
|
||||
ROS_INFO("Node shutdown before calib.yaml file was found.");
|
||||
return 0;
|
||||
}
|
||||
|
||||
ROS_INFO("Found calib.yaml file! Loading parameters...");
|
||||
|
||||
std::string node_name = ros::this_node::getName();
|
||||
std::string rosparam_command = "rosparam load " + calib_file_path + " " + node_name;
|
||||
int result = system(rosparam_command.c_str());
|
||||
|
||||
if(result == 0)
|
||||
{
|
||||
ROS_INFO("Successfully loaded parameters from calib.yaml to namespace: %s", node_name.c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Failed to load parameters from calib.yaml");
|
||||
return 1;
|
||||
}
|
||||
|
||||
CloudReprojectionRosNode reprojection_node(nh, pnh);
|
||||
ros::spin();
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
@@ -0,0 +1,128 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cloud_reprojector.hpp"
|
||||
#include <cmath>
|
||||
|
||||
CloudReprojector::CloudReprojector()
|
||||
{
|
||||
}
|
||||
|
||||
bool CloudReprojector::initialize(const CameraParams& cam_params, const ExtrinsicParams& ext_params)
|
||||
{
|
||||
camera_params_ = cam_params;
|
||||
extrinsic_params_ = ext_params;
|
||||
|
||||
if (camera_params_.A11 < 1e-6 || camera_params_.A22 < 1e-6 ||
|
||||
camera_params_.u0 < 1e-6 || camera_params_.v0 < 1e-6)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
camera_model_ = std::make_unique<mini_vikit::PolynomialCamera>(
|
||||
camera_params_.image_width, camera_params_.image_height,
|
||||
camera_params_.A11, camera_params_.A22,
|
||||
camera_params_.u0, camera_params_.v0,
|
||||
camera_params_.A12,
|
||||
camera_params_.k2, camera_params_.k3, camera_params_.k4,
|
||||
camera_params_.k5, camera_params_.k6, camera_params_.k7
|
||||
);
|
||||
|
||||
initialized_ = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
Eigen::Matrix4d CloudReprojector::calculateTic(const Eigen::Matrix4d& Tcl, const Eigen::Matrix4d& Til)
|
||||
{
|
||||
// Tic = Til * Tlc = Til * Tcl.inverse()
|
||||
Eigen::Matrix4d Tlc = Tcl.inverse();
|
||||
return Til * Tlc;
|
||||
}
|
||||
|
||||
Eigen::Matrix4d CloudReprojector::odomPoseToMatrix(const OdomPose& odom) const
|
||||
{
|
||||
Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
|
||||
T.block<3, 3>(0, 0) = odom.orientation.toRotationMatrix();
|
||||
T(0, 3) = odom.position.x();
|
||||
T(1, 3) = odom.position.y();
|
||||
T(2, 3) = odom.position.z();
|
||||
return T;
|
||||
}
|
||||
|
||||
cv::Mat CloudReprojector::reprojectCloud(const pcl::PointCloud<pcl::PointXYZRGB>& cloud_odom,
|
||||
const OdomPose& odom_pose)
|
||||
{
|
||||
if (!initialized_)
|
||||
{
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
// T_odom_imu: imu pose in odom frame
|
||||
Eigen::Matrix4d T_odom_imu = odomPoseToMatrix(odom_pose);
|
||||
|
||||
// T_imu_odom: transforms points from odom frame to imu frame
|
||||
Eigen::Matrix4d T_imu_odom = T_odom_imu.inverse();
|
||||
|
||||
// T_cam_imu = Tic.inverse(): transforms from imu to camera
|
||||
Eigen::Matrix4d T_cam_imu = extrinsic_params_.Tic.inverse();
|
||||
|
||||
// T_cam_odom: transforms points from odom frame to camera frame
|
||||
Eigen::Matrix4d T_cam_odom = T_cam_imu * T_imu_odom;
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZRGB> cloud_in_cam;
|
||||
pcl::transformPointCloud(cloud_odom, cloud_in_cam, T_cam_odom);
|
||||
|
||||
return projectCloudToImage(cloud_in_cam);
|
||||
}
|
||||
|
||||
cv::Mat CloudReprojector::projectCloudToImage(const pcl::PointCloud<pcl::PointXYZRGB>& cloud_in_cam) const
|
||||
{
|
||||
cv::Mat img = cv::Mat::zeros(camera_params_.image_height, camera_params_.image_width, CV_8UC3);
|
||||
img.setTo(cv::Scalar(255, 255, 255));
|
||||
|
||||
const double fx = camera_model_->fx();
|
||||
const double fy = camera_model_->fy();
|
||||
const double cx = camera_model_->cx();
|
||||
const double cy = camera_model_->cy();
|
||||
|
||||
for (const auto& pt : cloud_in_cam)
|
||||
{
|
||||
if (pt.z <= 0.01)
|
||||
continue;
|
||||
|
||||
int u_int, v_int;
|
||||
if (1)
|
||||
{
|
||||
// Pinhole projection (undistorted image)
|
||||
u_int = static_cast<int>(std::round(fx * pt.x / pt.z + cx));
|
||||
v_int = static_cast<int>(std::round(fy * pt.y / pt.z + cy));
|
||||
}
|
||||
else
|
||||
{
|
||||
// Distorted projection (original image)
|
||||
Eigen::Vector3d pt_cam(pt.x, pt.y, pt.z);
|
||||
Eigen::Vector2d uv = camera_model_->world2cam(pt_cam);
|
||||
u_int = static_cast<int>(std::round(uv[0]));
|
||||
v_int = static_cast<int>(std::round(uv[1]));
|
||||
}
|
||||
|
||||
if (u_int >= 0 && u_int < camera_params_.image_width &&
|
||||
v_int >= 0 && v_int < camera_params_.image_height)
|
||||
{
|
||||
cv::circle(img, cv::Point(u_int, v_int), point_radius_,
|
||||
cv::Scalar(pt.b, pt.g, pt.r), -1);
|
||||
}
|
||||
}
|
||||
|
||||
return img;
|
||||
}
|
||||
@@ -0,0 +1,172 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
#include "depth_image_ros2_node.hpp"
|
||||
#include <functional>
|
||||
|
||||
DepthImageRos2Node::DepthImageRos2Node(const rclcpp::NodeOptions & options)
|
||||
: Node("depth_image_ros2_node", options)
|
||||
{
|
||||
PointCloudToDepthConverter::CameraParams camera_params = loadCameraParams();
|
||||
|
||||
depth_converter_ = std::make_unique<PointCloudToDepthConverter>(camera_params);
|
||||
|
||||
cloud_raw_topic_ = this->declare_parameter<std::string>("cloud_raw_topic", "/odin1/cloud_raw");
|
||||
color_compressed_topic_ = this->declare_parameter<std::string>("color_compressed_topic", "/odin1/image/compressed");
|
||||
color_raw_topic_ = this->declare_parameter<std::string>("color_raw_topic", "/odin1/image");
|
||||
depth_image_topic_ = this->declare_parameter<std::string>("depth_image_topic", "/odin1/depth_img_competetion");
|
||||
depth_cloud_topic_ = this->declare_parameter<std::string>("depth_cloud_topic", "/odin1/depth_img_competetion_cloud");
|
||||
|
||||
RCLCPP_INFO_STREAM(this->get_logger(),
|
||||
"\n cloud_raw_topic: " << cloud_raw_topic_
|
||||
<< "\n color_compressed_topic: " << color_compressed_topic_
|
||||
<< "\n color_raw_topic: " << color_raw_topic_
|
||||
<< "\n depth_image_topic: " << depth_image_topic_
|
||||
<< "\n depth_cloud_topic: " << depth_cloud_topic_);
|
||||
}
|
||||
|
||||
void DepthImageRos2Node::initialize()
|
||||
{
|
||||
cloud_sub_.subscribe(this, cloud_raw_topic_);
|
||||
color_compressed_sub_.subscribe(this, color_compressed_topic_);
|
||||
color_sub_.subscribe(this, color_raw_topic_);
|
||||
|
||||
sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, color_sub_);
|
||||
sync_->registerCallback(std::bind(&DepthImageRos2Node::syncCallback, this,
|
||||
std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
it_ = std::make_shared<image_transport::ImageTransport>(shared_from_this());
|
||||
depth_image_pub_ = it_->advertise(depth_image_topic_, 1);
|
||||
depth_cloud_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(depth_cloud_topic_, 1);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "DepthImageRos2Node initialized successfully");
|
||||
}
|
||||
|
||||
PointCloudToDepthConverter::CameraParams DepthImageRos2Node::loadCameraParams()
|
||||
{
|
||||
PointCloudToDepthConverter::CameraParams params;
|
||||
|
||||
params.image_width = this->declare_parameter<int>("cam_0.image_width", 1600);
|
||||
params.image_height = this->declare_parameter<int>("cam_0.image_height", 1296);
|
||||
params.A11 = this->declare_parameter<double>("cam_0.A11", 0.0);
|
||||
params.A12 = this->declare_parameter<double>("cam_0.A12", 0.0);
|
||||
params.A22 = this->declare_parameter<double>("cam_0.A22", 0.0);
|
||||
params.u0 = this->declare_parameter<double>("cam_0.u0", 0.0);
|
||||
params.v0 = this->declare_parameter<double>("cam_0.v0", 0.0);
|
||||
|
||||
params.k2 = this->declare_parameter<double>("cam_0.k2", 0.0);
|
||||
params.k3 = this->declare_parameter<double>("cam_0.k3", 0.0);
|
||||
params.k4 = this->declare_parameter<double>("cam_0.k4", 0.0);
|
||||
params.k5 = this->declare_parameter<double>("cam_0.k5", 0.0);
|
||||
params.k6 = this->declare_parameter<double>("cam_0.k6", 0.0);
|
||||
params.k7 = this->declare_parameter<double>("cam_0.k7", 0.0);
|
||||
|
||||
params.scale = this->declare_parameter<double>("scale", 7.0);
|
||||
params.point_sampling_rate = this->declare_parameter<int>("point_sampling_rate", 5);
|
||||
|
||||
std::vector<double> Tcl_vec_param = this->declare_parameter<std::vector<double>>("Tcl_0", std::vector<double>(16, 0.0));
|
||||
if (Tcl_vec_param.size() == 16)
|
||||
{
|
||||
for (int i = 0; i < 4; ++i)
|
||||
{
|
||||
for (int j = 0; j < 4; ++j)
|
||||
{
|
||||
params.Tcl(i, j) = Tcl_vec_param[i * 4 + j];
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Tcl_0 param missing or invalid, colored reproject cloud disabled.");
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
|
||||
if (params.A11 < 1e-6 || params.A22 < 1e-6 || params.u0 < 1e-6 || params.v0 < 1e-6)
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Invalid camera intrinsics A11 or A22");
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Camera intrinsics:");
|
||||
RCLCPP_INFO(this->get_logger(), "Image size: %dx%d", params.image_width, params.image_height);
|
||||
RCLCPP_INFO(this->get_logger(), "Intrinsics: A11=%f A12=%f A22=%f u0=%f v0=%f",
|
||||
params.A11, params.A12, params.A22, params.u0, params.v0);
|
||||
RCLCPP_INFO(this->get_logger(), "Distortions: k2=%f k3=%f k4=%f k5=%f k6=%f k7=%f",
|
||||
params.k2, params.k3, params.k4, params.k5, params.k6, params.k7);
|
||||
RCLCPP_INFO(this->get_logger(), "Scale: %f, Point sampling rate: %d", params.scale, params.point_sampling_rate);
|
||||
RCLCPP_INFO_STREAM(this->get_logger(), "Extrinsics (Tcl):\n" << params.Tcl);
|
||||
|
||||
return params;
|
||||
}
|
||||
|
||||
void DepthImageRos2Node::syncCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud_msg,
|
||||
// const sensor_msgs::msg::CompressedImage::ConstSharedPtr image_msg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr color_msg)
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZ> cloud;
|
||||
pcl::fromROSMsg(*cloud_msg, cloud);
|
||||
if (cloud.empty())
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "Empty point cloud received");
|
||||
return;
|
||||
}
|
||||
|
||||
cv::Mat img_raw;
|
||||
try
|
||||
{
|
||||
// img_raw = cv::imdecode(cv::Mat(image_msg->data), cv::IMREAD_COLOR);
|
||||
cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(color_msg, "bgr8");
|
||||
img_raw = cv_ptr->image;
|
||||
if (img_raw.empty())
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "Failed to decode compressed image");
|
||||
return;
|
||||
}
|
||||
}
|
||||
catch (const cv_bridge::Exception &e)
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "cv_bridge: %s", e.what());
|
||||
return;
|
||||
}
|
||||
|
||||
auto result = depth_converter_->processCloudAndImage(cloud, img_raw);
|
||||
|
||||
if (!result.success)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "Data processing failed: %s", result.error_message.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
publishDepthImage(result.depth_image, cloud_msg->header);
|
||||
publishDepthCloud(result.colored_cloud, cloud_msg->header);
|
||||
}
|
||||
|
||||
void DepthImageRos2Node::publishDepthImage(const cv::Mat &img,
|
||||
const std_msgs::msg::Header &header,
|
||||
const std::string &encoding)
|
||||
{
|
||||
sensor_msgs::msg::Image::SharedPtr depth_msg = cv_bridge::CvImage(header, encoding, img).toImageMsg();
|
||||
depth_image_pub_.publish(*depth_msg);
|
||||
}
|
||||
|
||||
void DepthImageRos2Node::publishDepthCloud(const pcl::PointCloud<pcl::PointXYZRGB> &colored_cloud,
|
||||
const std_msgs::msg::Header &header)
|
||||
{
|
||||
if (!colored_cloud.points.empty())
|
||||
{
|
||||
sensor_msgs::msg::PointCloud2 cloud_msg;
|
||||
pcl::toROSMsg(colored_cloud, cloud_msg);
|
||||
cloud_msg.header = header;
|
||||
depth_cloud_pub_->publish(cloud_msg);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,165 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
#include "depth_image_ros_node.hpp"
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
DepthImageRosNode::DepthImageRosNode(ros::NodeHandle &nh, ros::NodeHandle &pnh)
|
||||
: nh_(nh), pnh_(pnh), it_(nh_)
|
||||
{
|
||||
PointCloudToDepthConverter::CameraParams camera_params = loadCameraParams();
|
||||
|
||||
depth_converter_ = std::make_unique<PointCloudToDepthConverter>(camera_params);
|
||||
pnh_.param<std::string>("cloud_raw_topic", cloud_raw_topic_, std::string("/odin1/cloud_raw"));
|
||||
pnh_.param<std::string>("color_raw_topic", color_raw_topic_, std::string("/odin1/image"));
|
||||
pnh_.param<std::string>("color_compressed_topic_", color_compressed_topic_, std::string("/odin1/image/compressed"));
|
||||
pnh_.param<std::string>("depth_image_topic", depth_image_topic_, std::string("/odin1/depth_img_competetion"));
|
||||
pnh_.param<std::string>("depth_cloud_topic", depth_cloud_topic_, std::string("/odin1/depth_img_competetion_cloud"));
|
||||
|
||||
ROS_INFO_STREAM("\n cloud_raw_topic: " << cloud_raw_topic_
|
||||
<< "\n color_raw_topic: " << color_raw_topic_
|
||||
<< "\n color_compressed_topic: " << color_compressed_topic_
|
||||
<< "\n depth_image_topic: " << depth_image_topic_
|
||||
<< "\n depth_cloud_topic: " << depth_cloud_topic_);
|
||||
|
||||
cloud_sub_.subscribe(nh_, cloud_raw_topic_, 1);
|
||||
color_sub_.subscribe(nh_, color_raw_topic_, 1);
|
||||
color_compressed_sub_.subscribe(nh_, color_compressed_topic_, 1);
|
||||
|
||||
sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, color_sub_);
|
||||
sync_->registerCallback(boost::bind(&DepthImageRosNode::syncCallback, this, _1, _2));
|
||||
|
||||
depth_image_pub_ = it_.advertise(depth_image_topic_, 1);
|
||||
depth_cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(depth_cloud_topic_, 1);
|
||||
|
||||
ROS_INFO("DepthImageRosNode initialized successfully");
|
||||
}
|
||||
|
||||
PointCloudToDepthConverter::CameraParams DepthImageRosNode::loadCameraParams()
|
||||
{
|
||||
PointCloudToDepthConverter::CameraParams params;
|
||||
|
||||
pnh_.param<int>("cam_0/image_width", params.image_width, 1600);
|
||||
pnh_.param<int>("cam_0/image_height", params.image_height, 1296);
|
||||
pnh_.param<double>("cam_0/A11", params.A11, 0.0);
|
||||
pnh_.param<double>("cam_0/A12", params.A12, 0.0);
|
||||
pnh_.param<double>("cam_0/A22", params.A22, 0.0);
|
||||
pnh_.param<double>("cam_0/u0", params.u0, 0.0);
|
||||
pnh_.param<double>("cam_0/v0", params.v0, 0.0);
|
||||
|
||||
pnh_.param<double>("cam_0/k2", params.k2, 0.0);
|
||||
pnh_.param<double>("cam_0/k3", params.k3, 0.0);
|
||||
pnh_.param<double>("cam_0/k4", params.k4, 0.0);
|
||||
pnh_.param<double>("cam_0/k5", params.k5, 0.0);
|
||||
pnh_.param<double>("cam_0/k6", params.k6, 0.0);
|
||||
pnh_.param<double>("cam_0/k7", params.k7, 0.0);
|
||||
|
||||
pnh_.param<double>("scale", params.scale, 7.0);
|
||||
pnh_.param<int>("point_sampling_rate", params.point_sampling_rate, 5);
|
||||
|
||||
std::vector<double> Tcl_vec_param;
|
||||
if (pnh_.getParam("Tcl_0", Tcl_vec_param) && Tcl_vec_param.size() == 16)
|
||||
{
|
||||
for (int i = 0; i < 4; ++i)
|
||||
{
|
||||
for (int j = 0; j < 4; ++j)
|
||||
{
|
||||
params.Tcl(i, j) = Tcl_vec_param[i * 4 + j];
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Tcl_0 param missing or invalid, colored reproject cloud disabled.");
|
||||
ros::shutdown();
|
||||
}
|
||||
|
||||
if (params.A11 < 1e-6 || params.A22 < 1e-6 || params.u0 < 1e-6 || params.v0 < 1e-6)
|
||||
{
|
||||
ROS_ERROR("Invalid camera intrinsics A11 or A22");
|
||||
ros::shutdown();
|
||||
}
|
||||
|
||||
ROS_INFO("Camera intrinsics:");
|
||||
ROS_INFO("Image size: %dx%d", params.image_width, params.image_height);
|
||||
ROS_INFO("Intrinsics: A11=%f A12=%f A22=%f u0=%f v0=%f",
|
||||
params.A11, params.A12, params.A22, params.u0, params.v0);
|
||||
ROS_INFO("Distortions: k2=%f k3=%f k4=%f k5=%f k6=%f k7=%f",
|
||||
params.k2, params.k3, params.k4, params.k5, params.k6, params.k7);
|
||||
ROS_INFO("Scale: %f, Point sampling rate: %d", params.scale, params.point_sampling_rate);
|
||||
ROS_INFO_STREAM("Extrinsics (Tcl):\n"
|
||||
<< params.Tcl);
|
||||
|
||||
return params;
|
||||
}
|
||||
|
||||
void DepthImageRosNode::syncCallback(const sensor_msgs::PointCloud2ConstPtr &cloud_msg,
|
||||
const sensor_msgs::ImageConstPtr &image_msg)
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZ> cloud;
|
||||
pcl::fromROSMsg(*cloud_msg, cloud);
|
||||
if (cloud.empty())
|
||||
{
|
||||
ROS_WARN("Empty point cloud received");
|
||||
return;
|
||||
}
|
||||
|
||||
cv::Mat img_raw;
|
||||
try
|
||||
{
|
||||
// img_raw = cv::imdecode(cv::Mat(image_msg->data), cv::IMREAD_COLOR);
|
||||
cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(image_msg, "bgr8");
|
||||
img_raw = cv_ptr->image;
|
||||
if (img_raw.empty())
|
||||
{
|
||||
ROS_WARN("Failed to decode compressed image");
|
||||
return;
|
||||
}
|
||||
}
|
||||
catch (const cv_bridge::Exception &e)
|
||||
{
|
||||
ROS_ERROR("cv_bridge: %s", e.what());
|
||||
return;
|
||||
}
|
||||
|
||||
auto result = depth_converter_->processCloudAndImage(cloud, img_raw);
|
||||
|
||||
if (!result.success)
|
||||
{
|
||||
ROS_WARN("Data processing failed: %s", result.error_message.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
publishDepthImage(result.depth_image, cloud_msg->header);
|
||||
publishDepthCloud(result.colored_cloud, cloud_msg->header);
|
||||
}
|
||||
|
||||
void DepthImageRosNode::publishDepthImage(const cv::Mat &img,
|
||||
const std_msgs::Header &header,
|
||||
const std::string &encoding)
|
||||
{
|
||||
sensor_msgs::ImagePtr depth_msg = cv_bridge::CvImage(header, encoding, img).toImageMsg();
|
||||
depth_image_pub_.publish(depth_msg);
|
||||
}
|
||||
|
||||
void DepthImageRosNode::publishDepthCloud(const pcl::PointCloud<pcl::PointXYZRGB> &colored_cloud,
|
||||
const std_msgs::Header &header)
|
||||
{
|
||||
if (!colored_cloud.points.empty())
|
||||
{
|
||||
sensor_msgs::PointCloud2 cloud_msg;
|
||||
pcl::toROSMsg(colored_cloud, cloud_msg);
|
||||
cloud_msg.header = header;
|
||||
depth_cloud_pub_.publish(cloud_msg);
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,254 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
#include "image_overlay_node.hpp"
|
||||
|
||||
#ifdef ROS2
|
||||
// ==================== ROS2 Implementation ====================
|
||||
|
||||
ImageOverlayNode::ImageOverlayNode(const rclcpp::NodeOptions& options)
|
||||
: Node("image_overlay_node", options)
|
||||
{
|
||||
// Read from register_keys (same structure as control_command.yaml)
|
||||
this->declare_parameter<std::string>("register_keys.overlay_reprojected_topic", "/odin1/reprojected_image");
|
||||
this->declare_parameter<std::string>("register_keys.overlay_camera_topic", "/odin1/image/undistorted");
|
||||
this->declare_parameter<std::string>("register_keys.overlay_output_topic", "/odin1/overlay_image");
|
||||
this->declare_parameter<double>("register_keys.overlay_alpha", 0.6);
|
||||
|
||||
reprojected_topic_ = this->get_parameter("register_keys.overlay_reprojected_topic").as_string();
|
||||
camera_topic_ = this->get_parameter("register_keys.overlay_camera_topic").as_string();
|
||||
overlay_topic_ = this->get_parameter("register_keys.overlay_output_topic").as_string();
|
||||
alpha_ = this->get_parameter("register_keys.overlay_alpha").as_double();
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Subscribing to: %s and %s",
|
||||
reprojected_topic_.c_str(), camera_topic_.c_str());
|
||||
RCLCPP_INFO(this->get_logger(), "Publishing to: %s (alpha=%.2f)", overlay_topic_.c_str(), alpha_);
|
||||
|
||||
// Independent subscriptions - no synchronization needed
|
||||
reproj_sub_ = this->create_subscription<Image>(
|
||||
reprojected_topic_, 10,
|
||||
std::bind(&ImageOverlayNode::reprojCallback, this, std::placeholders::_1));
|
||||
|
||||
camera_sub_ = this->create_subscription<Image>(
|
||||
camera_topic_, 10,
|
||||
std::bind(&ImageOverlayNode::cameraCallback, this, std::placeholders::_1));
|
||||
|
||||
overlay_pub_ = this->create_publisher<sensor_msgs::msg::Image>(overlay_topic_, 10);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "ImageOverlayNode initialized (no-sync mode)");
|
||||
}
|
||||
|
||||
void ImageOverlayNode::reprojCallback(Image::ConstSharedPtr msg)
|
||||
{
|
||||
try {
|
||||
cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
latest_reproj_img_ = cv_ptr->image.clone();
|
||||
latest_header_ = msg->header;
|
||||
}
|
||||
} catch (cv_bridge::Exception& e) {
|
||||
RCLCPP_ERROR(this->get_logger(), "cv_bridge exception (reproj): %s", e.what());
|
||||
return;
|
||||
}
|
||||
publishOverlay();
|
||||
}
|
||||
|
||||
void ImageOverlayNode::cameraCallback(Image::ConstSharedPtr msg)
|
||||
{
|
||||
try {
|
||||
cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
latest_camera_img_ = cv_ptr->image.clone();
|
||||
}
|
||||
} catch (cv_bridge::Exception& e) {
|
||||
RCLCPP_ERROR(this->get_logger(), "cv_bridge exception (camera): %s", e.what());
|
||||
return;
|
||||
}
|
||||
publishOverlay();
|
||||
}
|
||||
|
||||
void ImageOverlayNode::publishOverlay()
|
||||
{
|
||||
cv::Mat reproj_copy, camera_copy;
|
||||
std_msgs::msg::Header header_copy;
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
if (latest_reproj_img_.empty() || latest_camera_img_.empty()) {
|
||||
return;
|
||||
}
|
||||
reproj_copy = latest_reproj_img_.clone();
|
||||
camera_copy = latest_camera_img_.clone();
|
||||
header_copy = latest_header_;
|
||||
}
|
||||
|
||||
if (reproj_copy.size() != camera_copy.size()) {
|
||||
RCLCPP_WARN(this->get_logger(),
|
||||
"Image sizes don't match: reproj(%dx%d) vs camera(%dx%d)",
|
||||
reproj_copy.cols, reproj_copy.rows,
|
||||
camera_copy.cols, camera_copy.rows);
|
||||
return;
|
||||
}
|
||||
|
||||
// Create overlay using alpha blending
|
||||
// Replace white background in reproj with camera image, keep colored points
|
||||
cv::Mat overlay = camera_copy.clone();
|
||||
|
||||
// Blend: where reproj has color (non-white), show reproj color semi-transparently
|
||||
// where reproj is white (background), show camera image
|
||||
|
||||
for (int y = 0; y < reproj_copy.rows; ++y) {
|
||||
for (int x = 0; x < reproj_copy.cols; ++x) {
|
||||
cv::Vec3b reproj_pixel = reproj_copy.at<cv::Vec3b>(y, x);
|
||||
// Check if pixel is not white (has point cloud color)
|
||||
if (reproj_pixel[0] < 250 || reproj_pixel[1] < 250 || reproj_pixel[2] < 250) {
|
||||
// Blend reproj color with camera color
|
||||
cv::Vec3b cam_pixel = camera_copy.at<cv::Vec3b>(y, x);
|
||||
overlay.at<cv::Vec3b>(y, x) = cv::Vec3b(
|
||||
static_cast<uchar>(alpha_ * reproj_pixel[0] + (1 - alpha_) * cam_pixel[0]),
|
||||
static_cast<uchar>(alpha_ * reproj_pixel[1] + (1 - alpha_) * cam_pixel[1]),
|
||||
static_cast<uchar>(alpha_ * reproj_pixel[2] + (1 - alpha_) * cam_pixel[2])
|
||||
);
|
||||
}
|
||||
// else: keep camera image (already in overlay)
|
||||
}
|
||||
}
|
||||
|
||||
auto overlay_msg = cv_bridge::CvImage(header_copy, "bgr8", overlay).toImageMsg();
|
||||
overlay_pub_->publish(*overlay_msg);
|
||||
}
|
||||
|
||||
// ==================== ROS2 Main ====================
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
auto node = std::make_shared<ImageOverlayNode>();
|
||||
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
|
||||
#else
|
||||
// ==================== ROS1 Implementation ====================
|
||||
|
||||
ImageOverlayNode::ImageOverlayNode(ros::NodeHandle& nh, ros::NodeHandle& pnh)
|
||||
: nh_(nh), pnh_(pnh)
|
||||
{
|
||||
// Read from register_keys (same structure as control_command.yaml)
|
||||
pnh_.param<std::string>("register_keys/overlay_reprojected_topic", reprojected_topic_, "/odin1/reprojected_image");
|
||||
pnh_.param<std::string>("register_keys/overlay_camera_topic", camera_topic_, "/odin1/image/undistorted");
|
||||
pnh_.param<std::string>("register_keys/overlay_output_topic", overlay_topic_, "/odin1/overlay_image");
|
||||
pnh_.param<double>("register_keys/overlay_alpha", alpha_, 0.6);
|
||||
|
||||
ROS_INFO("Subscribing to: %s and %s", reprojected_topic_.c_str(), camera_topic_.c_str());
|
||||
ROS_INFO("Publishing to: %s (alpha=%.2f)", overlay_topic_.c_str(), alpha_);
|
||||
|
||||
// Independent subscriptions - no synchronization needed
|
||||
reproj_sub_ = nh_.subscribe(reprojected_topic_, 10, &ImageOverlayNode::reprojCallback, this);
|
||||
camera_sub_ = nh_.subscribe(camera_topic_, 10, &ImageOverlayNode::cameraCallback, this);
|
||||
|
||||
overlay_pub_ = nh_.advertise<sensor_msgs::Image>(overlay_topic_, 10);
|
||||
|
||||
ROS_INFO("ImageOverlayNode initialized (no-sync mode)");
|
||||
}
|
||||
|
||||
void ImageOverlayNode::reprojCallback(const sensor_msgs::ImageConstPtr& msg)
|
||||
{
|
||||
try {
|
||||
cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
latest_reproj_img_ = cv_ptr->image.clone();
|
||||
latest_header_ = msg->header;
|
||||
}
|
||||
} catch (cv_bridge::Exception& e) {
|
||||
ROS_ERROR("cv_bridge exception (reproj): %s", e.what());
|
||||
return;
|
||||
}
|
||||
publishOverlay();
|
||||
}
|
||||
|
||||
void ImageOverlayNode::cameraCallback(const sensor_msgs::ImageConstPtr& msg)
|
||||
{
|
||||
try {
|
||||
cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
latest_camera_img_ = cv_ptr->image.clone();
|
||||
}
|
||||
} catch (cv_bridge::Exception& e) {
|
||||
ROS_ERROR("cv_bridge exception (camera): %s", e.what());
|
||||
return;
|
||||
}
|
||||
publishOverlay();
|
||||
}
|
||||
|
||||
void ImageOverlayNode::publishOverlay()
|
||||
{
|
||||
cv::Mat reproj_copy, camera_copy;
|
||||
std_msgs::Header header_copy;
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
if (latest_reproj_img_.empty() || latest_camera_img_.empty()) {
|
||||
return;
|
||||
}
|
||||
reproj_copy = latest_reproj_img_.clone();
|
||||
camera_copy = latest_camera_img_.clone();
|
||||
header_copy = latest_header_;
|
||||
}
|
||||
|
||||
if (reproj_copy.size() != camera_copy.size()) {
|
||||
ROS_WARN_THROTTLE(2, "Image sizes don't match: reproj(%dx%d) vs camera(%dx%d)",
|
||||
reproj_copy.cols, reproj_copy.rows, camera_copy.cols, camera_copy.rows);
|
||||
return;
|
||||
}
|
||||
|
||||
// Create overlay using alpha blending
|
||||
cv::Mat overlay = camera_copy.clone();
|
||||
|
||||
for (int y = 0; y < reproj_copy.rows; ++y) {
|
||||
for (int x = 0; x < reproj_copy.cols; ++x) {
|
||||
cv::Vec3b reproj_pixel = reproj_copy.at<cv::Vec3b>(y, x);
|
||||
if (reproj_pixel[0] < 250 || reproj_pixel[1] < 250 || reproj_pixel[2] < 250) {
|
||||
cv::Vec3b cam_pixel = camera_copy.at<cv::Vec3b>(y, x);
|
||||
overlay.at<cv::Vec3b>(y, x) = cv::Vec3b(
|
||||
static_cast<uchar>(alpha_ * reproj_pixel[0] + (1 - alpha_) * cam_pixel[0]),
|
||||
static_cast<uchar>(alpha_ * reproj_pixel[1] + (1 - alpha_) * cam_pixel[1]),
|
||||
static_cast<uchar>(alpha_ * reproj_pixel[2] + (1 - alpha_) * cam_pixel[2])
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
sensor_msgs::ImagePtr overlay_msg = cv_bridge::CvImage(header_copy, "bgr8", overlay).toImageMsg();
|
||||
overlay_pub_.publish(overlay_msg);
|
||||
}
|
||||
|
||||
// ==================== ROS1 Main ====================
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "image_overlay_node");
|
||||
ros::NodeHandle nh;
|
||||
ros::NodeHandle pnh("~");
|
||||
|
||||
ImageOverlayNode node(nh, pnh);
|
||||
|
||||
ros::spin();
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
@@ -0,0 +1,74 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <fstream>
|
||||
#include <sys/stat.h>
|
||||
|
||||
#include "depth_image_ros_node.hpp"
|
||||
|
||||
bool fileExists(const std::string& filename) {
|
||||
struct stat buffer;
|
||||
return (stat(filename.c_str(), &buffer) == 0);
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "depth_projector");
|
||||
ros::NodeHandle nh;
|
||||
ros::NodeHandle pnh("~");
|
||||
int senddepth=0;
|
||||
pnh.param("register_keys/senddepth", senddepth, 0);
|
||||
if(senddepth==0)
|
||||
{
|
||||
ROS_INFO("Depth image will not be published.");
|
||||
return 0;
|
||||
}
|
||||
|
||||
std::string calib_file_path;
|
||||
pnh.param<std::string>("calib_file_path", calib_file_path, "");
|
||||
|
||||
ROS_INFO("Waiting for calib.yaml file at: %s", calib_file_path.c_str());
|
||||
while(ros::ok() && !fileExists(calib_file_path))
|
||||
{
|
||||
ROS_INFO_THROTTLE(5, "Still waiting for calib.yaml file...");
|
||||
ros::Duration(0.5).sleep();
|
||||
ros::spinOnce();
|
||||
}
|
||||
|
||||
if(!ros::ok())
|
||||
{
|
||||
ROS_INFO("Node shutdown before calib.yaml file was found.");
|
||||
return 0;
|
||||
}
|
||||
|
||||
ROS_INFO("Found calib.yaml file! Loading parameters...");
|
||||
|
||||
std::string node_name = ros::this_node::getName();
|
||||
std::string rosparam_command = "rosparam load " + calib_file_path + " " + node_name;
|
||||
int result = system(rosparam_command.c_str());
|
||||
|
||||
if(result == 0)
|
||||
{
|
||||
ROS_INFO("Successfully loaded parameters from calib.yaml to namespace: %s", node_name.c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Failed to load parameters from calib.yaml");
|
||||
return 1;
|
||||
}
|
||||
|
||||
DepthImageRosNode projector(nh, pnh);
|
||||
ros::spin();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,238 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <fstream>
|
||||
#include <sys/stat.h>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <string>
|
||||
#include "depth_image_ros2_node.hpp"
|
||||
#include <rcpputils/filesystem_helper.hpp>
|
||||
bool fileExists(const std::string& filename) {
|
||||
struct stat buffer;
|
||||
return (stat(filename.c_str(), &buffer) == 0);
|
||||
}
|
||||
|
||||
bool loadCalibParameters(std::shared_ptr<rclcpp::Node> node, const std::string& calib_file_path) {
|
||||
try {
|
||||
RCLCPP_INFO(node->get_logger(), "Loading parameters from calib.yaml file: %s", calib_file_path.c_str());
|
||||
|
||||
YAML::Node config = YAML::LoadFile(calib_file_path);
|
||||
|
||||
if (config["cam_num"]) {
|
||||
int cam_num = config["cam_num"].as<int>();
|
||||
node->declare_parameter("cam_num", cam_num);
|
||||
RCLCPP_INFO(node->get_logger(), "Loaded parameter: cam_num = %d", cam_num);
|
||||
}
|
||||
|
||||
if (config["img_topic_0"]) {
|
||||
std::string img_topic = config["img_topic_0"].as<std::string>();
|
||||
node->declare_parameter("img_topic_0", img_topic);
|
||||
RCLCPP_INFO(node->get_logger(), "Loaded parameter: img_topic_0 = %s", img_topic.c_str());
|
||||
}
|
||||
|
||||
if (config["Tcl_0"]) {
|
||||
std::vector<double> tcl_matrix = config["Tcl_0"].as<std::vector<double>>();
|
||||
node->declare_parameter("Tcl_0", tcl_matrix);
|
||||
RCLCPP_INFO(node->get_logger(), "Loaded parameter: Tcl_0 (transform matrix with %zu elements)", tcl_matrix.size());
|
||||
}
|
||||
|
||||
if (config["cam_0"]) {
|
||||
YAML::Node cam_0 = config["cam_0"];
|
||||
|
||||
if (cam_0["cam_model"]) {
|
||||
std::string cam_model = cam_0["cam_model"].as<std::string>();
|
||||
node->declare_parameter("cam_0.cam_model", cam_model);
|
||||
RCLCPP_INFO(node->get_logger(), "Loaded parameter: cam_0.cam_model = %s", cam_model.c_str());
|
||||
}
|
||||
|
||||
if (cam_0["image_width"]) {
|
||||
int image_width = cam_0["image_width"].as<int>();
|
||||
node->declare_parameter("cam_0.image_width", image_width);
|
||||
RCLCPP_INFO(node->get_logger(), "Loaded parameter: cam_0.image_width = %d", image_width);
|
||||
}
|
||||
|
||||
if (cam_0["image_height"]) {
|
||||
int image_height = cam_0["image_height"].as<int>();
|
||||
node->declare_parameter("cam_0.image_height", image_height);
|
||||
RCLCPP_INFO(node->get_logger(), "Loaded parameter: cam_0.image_height = %d", image_height);
|
||||
}
|
||||
|
||||
std::vector<std::string> distortion_params = {"k2", "k3", "k4", "k5", "k6", "k7", "p1", "p2"};
|
||||
for (const auto& param : distortion_params) {
|
||||
if (cam_0[param]) {
|
||||
double value = cam_0[param].as<double>();
|
||||
node->declare_parameter("cam_0." + param, value);
|
||||
RCLCPP_INFO(node->get_logger(), "Loaded parameter: cam_0.%s = %f", param.c_str(), value);
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<std::string> intrinsic_params = {"A11", "A12", "A22", "u0", "v0"};
|
||||
for (const auto& param : intrinsic_params) {
|
||||
if (cam_0[param]) {
|
||||
double value = cam_0[param].as<double>();
|
||||
node->declare_parameter("cam_0." + param, value);
|
||||
RCLCPP_INFO(node->get_logger(), "Loaded parameter: cam_0.%s = %f", param.c_str(), value);
|
||||
}
|
||||
}
|
||||
|
||||
if (cam_0["isFast"]) {
|
||||
int is_fast = cam_0["isFast"].as<int>();
|
||||
node->declare_parameter("cam_0.isFast", is_fast);
|
||||
RCLCPP_INFO(node->get_logger(), "Loaded parameter: cam_0.isFast = %d", is_fast);
|
||||
}
|
||||
|
||||
if (cam_0["numDiff"]) {
|
||||
int num_diff = cam_0["numDiff"].as<int>();
|
||||
node->declare_parameter("cam_0.numDiff", num_diff);
|
||||
RCLCPP_INFO(node->get_logger(), "Loaded parameter: cam_0.numDiff = %d", num_diff);
|
||||
}
|
||||
|
||||
if (cam_0["maxIncidentAngle"]) {
|
||||
int max_incident_angle = cam_0["maxIncidentAngle"].as<int>();
|
||||
node->declare_parameter("cam_0.maxIncidentAngle", max_incident_angle);
|
||||
RCLCPP_INFO(node->get_logger(), "Loaded parameter: cam_0.maxIncidentAngle = %d", max_incident_angle);
|
||||
}
|
||||
}
|
||||
|
||||
RCLCPP_INFO(node->get_logger(), "Successfully loaded all parameters from calib.yaml");
|
||||
return true;
|
||||
|
||||
} catch (const YAML::Exception& e) {
|
||||
RCLCPP_ERROR(node->get_logger(), "YAML parsing error: %s", e.what());
|
||||
return false;
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(node->get_logger(), "Error loading calib parameters: %s", e.what());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
std::string get_package_source_directory() {
|
||||
// 使用 rcpputils::fs::path 替代 std::filesystem::path
|
||||
rcpputils::fs::path current_file(__FILE__);
|
||||
|
||||
// 回溯到包根目录
|
||||
auto path = current_file.parent_path();
|
||||
|
||||
// 使用 rcpputils::fs::exists 替代 std::filesystem::exists
|
||||
while (!path.empty() && !rcpputils::fs::exists(path / "package.xml")) {
|
||||
path = path.parent_path();
|
||||
}
|
||||
|
||||
if (path.empty()) {
|
||||
throw std::runtime_error("Failed to locate package root directory");
|
||||
}
|
||||
|
||||
return path.string();
|
||||
}
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
|
||||
auto node = std::make_shared<rclcpp::Node>("pcd2depth_node");
|
||||
RCLCPP_INFO(node->get_logger(), "Node created");
|
||||
std::string package_path = get_package_source_directory();
|
||||
RCLCPP_INFO(node->get_logger(), "Package path: %s", package_path.c_str());
|
||||
|
||||
std::string config_file = package_path + "/config/control_command.yaml";
|
||||
RCLCPP_INFO(node->get_logger(), "Loading config from: %s", config_file.c_str());
|
||||
|
||||
YAML::Node config = YAML::LoadFile(config_file);
|
||||
|
||||
if (!config["register_keys"]) {
|
||||
throw std::runtime_error("Missing 'register_keys' section");
|
||||
}
|
||||
|
||||
if (!config["register_keys"]["senddepth"]) {
|
||||
throw std::runtime_error("Missing 'senddepth' parameter");
|
||||
}
|
||||
int senddepth = config["register_keys"]["senddepth"].as<int>();
|
||||
std::cout << "senddepth: " << senddepth << std::endl;
|
||||
if(senddepth == 0)
|
||||
{
|
||||
RCLCPP_INFO(node->get_logger(), "Depth image will not be published.");
|
||||
return 0;
|
||||
}
|
||||
|
||||
std::string calib_file_path = node->declare_parameter<std::string>("calib_file_path", "");
|
||||
|
||||
RCLCPP_INFO(node->get_logger(), "Waiting for calib.yaml file at: %s", calib_file_path.c_str());
|
||||
while(rclcpp::ok() && !fileExists(calib_file_path))
|
||||
{
|
||||
RCLCPP_INFO_THROTTLE(node->get_logger(), *node->get_clock(), 5000, "Still waiting for calib.yaml file...");
|
||||
rclcpp::sleep_for(std::chrono::milliseconds(500)); // 等待0.5秒后再检查
|
||||
rclcpp::spin_some(node);
|
||||
}
|
||||
|
||||
if(!rclcpp::ok())
|
||||
{
|
||||
RCLCPP_INFO(node->get_logger(), "Node shutdown before calib.yaml file was found.");
|
||||
return 0;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(node->get_logger(), "Found calib.yaml file! Loading parameters...");
|
||||
|
||||
if (!loadCalibParameters(node, calib_file_path)) {
|
||||
RCLCPP_ERROR(node->get_logger(), "Failed to load parameters from calib.yaml file");
|
||||
return 1;
|
||||
}
|
||||
|
||||
rclcpp::NodeOptions depth_node_options;
|
||||
|
||||
std::vector<rclcpp::Parameter> params_override;
|
||||
|
||||
try {
|
||||
params_override.push_back(rclcpp::Parameter("cam_0.image_width", node->get_parameter("cam_0.image_width").as_int()));
|
||||
params_override.push_back(rclcpp::Parameter("cam_0.image_height", node->get_parameter("cam_0.image_height").as_int()));
|
||||
params_override.push_back(rclcpp::Parameter("cam_0.A11", node->get_parameter("cam_0.A11").as_double()));
|
||||
params_override.push_back(rclcpp::Parameter("cam_0.A12", node->get_parameter("cam_0.A12").as_double()));
|
||||
params_override.push_back(rclcpp::Parameter("cam_0.A22", node->get_parameter("cam_0.A22").as_double()));
|
||||
params_override.push_back(rclcpp::Parameter("cam_0.u0", node->get_parameter("cam_0.u0").as_double()));
|
||||
params_override.push_back(rclcpp::Parameter("cam_0.v0", node->get_parameter("cam_0.v0").as_double()));
|
||||
params_override.push_back(rclcpp::Parameter("cam_0.k2", node->get_parameter("cam_0.k2").as_double()));
|
||||
params_override.push_back(rclcpp::Parameter("cam_0.k3", node->get_parameter("cam_0.k3").as_double()));
|
||||
params_override.push_back(rclcpp::Parameter("cam_0.k4", node->get_parameter("cam_0.k4").as_double()));
|
||||
params_override.push_back(rclcpp::Parameter("cam_0.k5", node->get_parameter("cam_0.k5").as_double()));
|
||||
params_override.push_back(rclcpp::Parameter("cam_0.k6", node->get_parameter("cam_0.k6").as_double()));
|
||||
params_override.push_back(rclcpp::Parameter("cam_0.k7", node->get_parameter("cam_0.k7").as_double()));
|
||||
params_override.push_back(rclcpp::Parameter("Tcl_0", node->get_parameter("Tcl_0").as_double_array()));
|
||||
|
||||
depth_node_options.parameter_overrides(params_override);
|
||||
|
||||
RCLCPP_INFO(node->get_logger(), "Parameters successfully prepared for DepthImageRos2Node");
|
||||
}
|
||||
catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(node->get_logger(), "Error preparing parameters for DepthImageRos2Node: %s", e.what());
|
||||
return 1;
|
||||
}
|
||||
try
|
||||
{
|
||||
auto depth_node = std::make_shared<DepthImageRos2Node>(depth_node_options);
|
||||
|
||||
depth_node->initialize();
|
||||
|
||||
rclcpp::executors::MultiThreadedExecutor executor;
|
||||
executor.add_node(depth_node);
|
||||
executor.spin();
|
||||
}
|
||||
catch (const std::exception& e)
|
||||
{
|
||||
RCLCPP_ERROR(node->get_logger(), "Error creating depth node: %s", e.what());
|
||||
return 1;
|
||||
}
|
||||
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
+376
@@ -0,0 +1,376 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
#include "pointcloud_depth_converter.hpp"
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
|
||||
PointCloudToDepthConverter::PointCloudToDepthConverter(const CameraParams ¶ms)
|
||||
: params_(params)
|
||||
{
|
||||
initializeInternalParams();
|
||||
createDistortionMaps();
|
||||
}
|
||||
|
||||
void PointCloudToDepthConverter::initializeInternalParams()
|
||||
{
|
||||
scaled_width_ = static_cast<int>(params_.image_width / params_.scale);
|
||||
scaled_height_ = static_cast<int>(params_.image_height / params_.scale);
|
||||
|
||||
K_ = Eigen::Matrix3d::Identity();
|
||||
K_(0, 0) = params_.A11;
|
||||
K_(0, 1) = params_.A12;
|
||||
K_(0, 2) = params_.u0;
|
||||
K_(1, 1) = params_.A22;
|
||||
K_(1, 2) = params_.v0;
|
||||
|
||||
Kl_ = Eigen::Matrix3d::Identity();
|
||||
Kl_(0, 0) = params_.A11 / params_.scale;
|
||||
Kl_(0, 1) = 0.0;
|
||||
Kl_(0, 2) = params_.u0 / params_.scale;
|
||||
Kl_(1, 1) = params_.A22 / params_.scale;
|
||||
Kl_(1, 2) = params_.v0 / params_.scale;
|
||||
|
||||
K_4x4_ = Eigen::Matrix4d::Identity();
|
||||
K_4x4_.block<3, 3>(0, 0) = Kl_;
|
||||
|
||||
Kcl_ = K_4x4_ * params_.Tcl;
|
||||
}
|
||||
|
||||
void PointCloudToDepthConverter::createDistortionMaps()
|
||||
{
|
||||
map_x_ = cv::Mat::zeros(params_.image_height, params_.image_width, CV_32FC1);
|
||||
map_y_ = cv::Mat::zeros(params_.image_height, params_.image_width, CV_32FC1);
|
||||
|
||||
|
||||
for (int u = 0; u < params_.image_width; ++u)
|
||||
{
|
||||
for (int v = 0; v < params_.image_height; ++v)
|
||||
{
|
||||
double y = (v - params_.v0) / params_.A22;
|
||||
double x = (u - params_.u0 - params_.A12 * y) / params_.A11;
|
||||
|
||||
double r = sqrt(x * x + y * y);
|
||||
double theta = atan(r);
|
||||
|
||||
double theta_d = theta + params_.k2 * pow(theta, 2) + params_.k3 * pow(theta, 3) +
|
||||
params_.k4 * pow(theta, 4) + params_.k5 * pow(theta, 5) +
|
||||
params_.k6 * pow(theta, 6) + params_.k7 * pow(theta, 7);
|
||||
|
||||
double x_distorted = x * (r / theta_d);
|
||||
double y_distorted = y * (r / theta_d);
|
||||
|
||||
map_x_.at<float>(v, u) = static_cast<float>(x_distorted * params_.A11 + params_.A12 * y_distorted + params_.u0);
|
||||
map_y_.at<float>(v, u) = static_cast<float>(y_distorted * params_.A22 + params_.v0);
|
||||
}
|
||||
}
|
||||
|
||||
inv_map_x_ = cv::Mat::zeros(params_.image_height, params_.image_width, CV_32FC1);
|
||||
inv_map_y_ = cv::Mat::zeros(params_.image_height, params_.image_width, CV_32FC1);
|
||||
for (int u = 0; u < params_.image_width; ++u)
|
||||
{
|
||||
for (int v = 0; v < params_.image_height; ++v)
|
||||
{
|
||||
double y = (v - params_.v0) / params_.A22;
|
||||
double x = (u - params_.u0 - params_.A12 * y) / params_.A11;
|
||||
|
||||
double r = sqrt(x * x + y * y);
|
||||
double theta = atan(r);
|
||||
|
||||
double theta_d = theta + params_.k2 * pow(theta, 2) + params_.k3 * pow(theta, 3) +
|
||||
params_.k4 * pow(theta, 4) + params_.k5 * pow(theta, 5) +
|
||||
params_.k6 * pow(theta, 6) + params_.k7 * pow(theta, 7);
|
||||
|
||||
double x_distorted = x * (theta_d / r);
|
||||
double y_distorted = y * (theta_d / r);
|
||||
|
||||
inv_map_x_.at<float>(v, u) = static_cast<float>(x_distorted * params_.A11 + params_.A12 * y_distorted + params_.u0);
|
||||
inv_map_y_.at<float>(v, u) = static_cast<float>(y_distorted * params_.A22 + params_.v0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
PointCloudToDepthConverter::ProcessResult PointCloudToDepthConverter::processCloudAndImage(
|
||||
const pcl::PointCloud<pcl::PointXYZ> &cloud,
|
||||
const cv::Mat &image)
|
||||
{
|
||||
ProcessResult result;
|
||||
result.success = false;
|
||||
|
||||
auto validation_result = validateInputs(cloud, image);
|
||||
if (!validation_result.first)
|
||||
{
|
||||
result.error_message = validation_result.second;
|
||||
return result;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZ> cloud_in_cam;
|
||||
pcl::transformPointCloud(cloud, cloud_in_cam, Kcl_);
|
||||
|
||||
cv::Mat depth_img = projectCloudToDepth(cloud_in_cam);
|
||||
|
||||
cv::Mat processed_depth = postProcessDepthImage(depth_img);
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZRGB> colored_cloud = generateColoredCloud(processed_depth, image);
|
||||
|
||||
result.depth_image = processed_depth;
|
||||
result.colored_cloud = colored_cloud;
|
||||
result.success = true;
|
||||
}
|
||||
catch (const std::exception &e)
|
||||
{
|
||||
result.error_message = std::string("Processing error: ") + e.what();
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
cv::Mat PointCloudToDepthConverter::projectCloudToDepth(const pcl::PointCloud<pcl::PointXYZ> &cloud_in_cam)
|
||||
{
|
||||
cv::Mat depth_img = cv::Mat::zeros(scaled_height_, scaled_width_, CV_32FC1);
|
||||
|
||||
for (const auto &camera_point : cloud_in_cam)
|
||||
{
|
||||
if (camera_point.z <= 0)
|
||||
continue;
|
||||
|
||||
int u = static_cast<int>(std::round(camera_point.x / camera_point.z));
|
||||
int v = static_cast<int>(std::round(camera_point.y / camera_point.z));
|
||||
|
||||
if (u >= 0 && u < scaled_width_ && v >= 0 && v < scaled_height_)
|
||||
{
|
||||
depth_img.at<float>(v, u) = static_cast<float>(camera_point.z);
|
||||
|
||||
for (int du = -1; du <= 1; ++du)
|
||||
{
|
||||
for (int dv = -1; dv <= 1; ++dv)
|
||||
{
|
||||
int nu = u + du;
|
||||
int nv = v + dv;
|
||||
if (nu >= 0 && nu < scaled_width_ && nv >= 0 && nv < scaled_height_)
|
||||
{
|
||||
if (depth_img.at<float>(nv, nu) == 0.0f)
|
||||
{
|
||||
depth_img.at<float>(nv, nu) = static_cast<float>(camera_point.z);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return depth_img;
|
||||
}
|
||||
|
||||
cv::Mat PointCloudToDepthConverter::postProcessDepthImage(const cv::Mat &depth_img) {
|
||||
if (depth_img.empty()) {
|
||||
std::cerr << "ERROR: Input depth image is empty!" << std::endl;
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
if (depth_img.data == nullptr) {
|
||||
std::cerr << "ERROR: Input depth image has null data pointer!" << std::endl;
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
if (depth_img.rows <= 0 || depth_img.cols <= 0) {
|
||||
std::cerr << "ERROR: Invalid input dimensions: "
|
||||
<< depth_img.rows << "x" << depth_img.cols << std::endl;
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
if (params_.image_width <= 0 || params_.image_height <= 0) {
|
||||
std::cerr << "ERROR: Invalid target size: "
|
||||
<< params_.image_width << "x" << params_.image_height << std::endl;
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
cv::Mat safe_input = depth_img.clone();
|
||||
if (safe_input.empty()) {
|
||||
std::cerr << "ERROR: Failed to create safe copy of input image!" << std::endl;
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
|
||||
cv::Mat depth_img_upsampled;
|
||||
try {
|
||||
depth_img_upsampled = customResize(safe_input, cv::Size(1600, 1296));
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "ERROR: Custom resize failed: " << e.what() << std::endl;
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
if (depth_img_upsampled.empty()) {
|
||||
std::cerr << "ERROR: Resized image is empty!" << std::endl;
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
if (depth_img_upsampled.rows != 1296 || depth_img_upsampled.cols != 1600) {
|
||||
std::cerr << "ERROR: Resized image has wrong dimensions: "
|
||||
<< depth_img_upsampled.cols << "x" << depth_img_upsampled.rows
|
||||
<< " (expected " << 1600 << "x" << 1296 << ")" << std::endl;
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
|
||||
cv::Mat grad_x, grad_y, grad_magnitude;
|
||||
try {
|
||||
cv::Sobel(depth_img_upsampled, grad_x, CV_32F, 1, 0, 3);
|
||||
cv::Sobel(depth_img_upsampled, grad_y, CV_32F, 0, 1, 3);
|
||||
cv::magnitude(grad_x, grad_y, grad_magnitude);
|
||||
} catch (const cv::Exception& e) {
|
||||
std::cerr << "ERROR: Sobel/magnitude failed: " << e.what() << std::endl;
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
if (grad_magnitude.type() != CV_32F) {
|
||||
std::cerr << "ERROR: grad_magnitude has wrong type: "
|
||||
<< grad_magnitude.type() << " (expected CV_32F)" << std::endl;
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
|
||||
cv::Mat threshold_mask;
|
||||
try {
|
||||
cv::threshold(grad_magnitude, threshold_mask, 0.75, 1, cv::THRESH_BINARY);
|
||||
threshold_mask.convertTo(threshold_mask, CV_8U);
|
||||
|
||||
|
||||
depth_img_upsampled.setTo(0, threshold_mask);
|
||||
} catch (const cv::Exception& e) {
|
||||
std::cerr << "ERROR: Threshold mask failed: " << e.what() << std::endl;
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
return depth_img_upsampled;
|
||||
}
|
||||
|
||||
cv::Mat PointCloudToDepthConverter::customResize(const cv::Mat& src, const cv::Size& size) {
|
||||
if (src.empty()) {
|
||||
throw std::runtime_error("Source image is empty");
|
||||
}
|
||||
|
||||
if (size.width <= 0 || size.height <= 0) {
|
||||
throw std::runtime_error("Invalid target size");
|
||||
}
|
||||
|
||||
|
||||
cv::Mat dst(size.height, size.width, src.type());
|
||||
|
||||
float scale_x = src.cols / static_cast<float>(size.width);
|
||||
float scale_y = src.rows / static_cast<float>(size.height);
|
||||
|
||||
if (src.channels() != 1 || src.type() != CV_32F) {
|
||||
throw std::runtime_error("Unsupported image type - expected single channel float");
|
||||
}
|
||||
|
||||
|
||||
for (int y = 0; y < dst.rows; y++) {
|
||||
|
||||
int src_y = static_cast<int>(y * scale_y);
|
||||
src_y = std::min(src_y, src.rows - 1);
|
||||
|
||||
for (int x = 0; x < dst.cols; x++) {
|
||||
int src_x = static_cast<int>(x * scale_x);
|
||||
src_x = std::min(src_x, src.cols - 1);
|
||||
dst.at<float>(y, x) = src.at<float>(src_y, src_x);
|
||||
}
|
||||
}
|
||||
|
||||
return dst;
|
||||
}
|
||||
pcl::PointCloud<pcl::PointXYZRGB> PointCloudToDepthConverter::generateColoredCloud(
|
||||
const cv::Mat &depth_img, const cv::Mat &color_img)
|
||||
{
|
||||
cv::Mat depth_undistorted, color_undistorted;
|
||||
depth_undistorted = depth_img.clone();
|
||||
cv::remap(color_img, color_undistorted, inv_map_x_, inv_map_y_, cv::INTER_LINEAR);
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZRGB> cloud_colored;
|
||||
|
||||
Eigen::Matrix4d Tlc = params_.Tcl.inverse();
|
||||
|
||||
for (int v = 0; v < depth_undistorted.rows; v += params_.point_sampling_rate)
|
||||
{
|
||||
for (int u = 0; u < depth_undistorted.cols; u += params_.point_sampling_rate)
|
||||
{
|
||||
float depth = depth_undistorted.at<float>(v, u);
|
||||
if (depth > 0.1f && depth < 100.0f)
|
||||
{
|
||||
double y_cam = (v - params_.v0) * depth / params_.A22;
|
||||
double x_cam = ((u - params_.u0) * depth - params_.A12 * y_cam)/ params_.A11;
|
||||
|
||||
double z_cam = depth;
|
||||
|
||||
Eigen::Vector4d point_cam(x_cam, y_cam, z_cam, 1.0);
|
||||
|
||||
Eigen::Vector4d point_lidar = Tlc * point_cam;
|
||||
|
||||
pcl::PointXYZRGB point;
|
||||
point.x = static_cast<float>(point_lidar[0]);
|
||||
point.y = static_cast<float>(point_lidar[1]);
|
||||
point.z = static_cast<float>(point_lidar[2]);
|
||||
|
||||
if (u < color_undistorted.cols && v < color_undistorted.rows)
|
||||
{
|
||||
cv::Vec3b color = color_undistorted.at<cv::Vec3b>(v, u);
|
||||
point.b = color[0];
|
||||
point.g = color[1];
|
||||
point.r = color[2];
|
||||
}
|
||||
else
|
||||
{
|
||||
point.r = point.g = point.b = 255;
|
||||
}
|
||||
|
||||
cloud_colored.points.push_back(point);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
cloud_colored.width = cloud_colored.points.size();
|
||||
cloud_colored.height = 1;
|
||||
cloud_colored.is_dense = false;
|
||||
|
||||
return cloud_colored;
|
||||
}
|
||||
|
||||
std::pair<bool, std::string> PointCloudToDepthConverter::validateInputs(
|
||||
const pcl::PointCloud<pcl::PointXYZ> &cloud, const cv::Mat &image)
|
||||
{
|
||||
if (cloud.empty())
|
||||
{
|
||||
return {false, "Empty point cloud"};
|
||||
}
|
||||
|
||||
if (image.empty())
|
||||
{
|
||||
return {false, "Empty image"};
|
||||
}
|
||||
|
||||
if (params_.A11 < 1e-6 || params_.A22 < 1e-6)
|
||||
{
|
||||
return {false, "Invalid camera intrinsics"};
|
||||
}
|
||||
|
||||
return {true, ""};
|
||||
}
|
||||
|
||||
void PointCloudToDepthConverter::updateCameraParams(const CameraParams ¶ms)
|
||||
{
|
||||
params_ = params;
|
||||
initializeInternalParams();
|
||||
createDistortionMaps();
|
||||
}
|
||||
@@ -0,0 +1,299 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
#include "rawCloudRender.h"
|
||||
#include <yaml-cpp/yaml.h>
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
#include <Eigen/Dense>
|
||||
#include <algorithm>
|
||||
#include <iostream>
|
||||
#include <cmath>
|
||||
#include <array>
|
||||
#include <algorithm>
|
||||
|
||||
struct ValidPointInfo {
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
int u;
|
||||
int v;
|
||||
};
|
||||
|
||||
|
||||
namespace GlobalCameraParams {
|
||||
float g_fx = 0.0f;
|
||||
float g_fy = 0.0f;
|
||||
float g_cx = 0.0f;
|
||||
float g_cy = 0.0f;
|
||||
float g_skew = 0.0f;
|
||||
float g_k2 = 0.0f;
|
||||
float g_k3 = 0.0f;
|
||||
float g_k4 = 0.0f;
|
||||
float g_k5 = 0.0f;
|
||||
float g_k6 = 0.0f;
|
||||
float g_k7 = 0.0f;
|
||||
Eigen::Matrix4f g_T_camera_lidar = Eigen::Matrix4f::Identity();
|
||||
}
|
||||
bool raw_debug=0;
|
||||
bool rawCloudRender::init(const std::string& yamlFilePath) {
|
||||
YAML::Node config;
|
||||
try {
|
||||
config = YAML::LoadFile(yamlFilePath);
|
||||
} catch (const YAML::BadFile& e) {
|
||||
std::cerr << "Error: Could not open file '" << yamlFilePath << "' - " << e.what() << std::endl;
|
||||
return false;
|
||||
} catch (const YAML::ParserException& e) {
|
||||
std::cerr << "Error: YAML parsing failed - " << e.what() << std::endl;
|
||||
return false;
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "Unexpected error: " << e.what() << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
// Intrinsic parameters node (cam_0)
|
||||
const std::string cam_node_name = "cam_0";
|
||||
if (!config[cam_node_name]) {
|
||||
std::cerr << "Error: Missing camera node '" << cam_node_name << "'" << std::endl;
|
||||
return false;
|
||||
}
|
||||
YAML::Node cam_node = config[cam_node_name];
|
||||
|
||||
// Directly access Tcl_0 node
|
||||
const std::string tcl_node_name = "Tcl_0";
|
||||
if (!config[tcl_node_name]) {
|
||||
std::cerr << "Error: Missing transformation matrix node '" << tcl_node_name << "'" << std::endl;
|
||||
return false;
|
||||
}
|
||||
YAML::Node tclNode = config[tcl_node_name];
|
||||
if (tclNode.size() != 16) {
|
||||
std::cerr << "Error: Transformation matrix must be 4x4 (16 elements)" << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
// === Read intrinsic parameters ===
|
||||
GlobalCameraParams::g_k2 = cam_node["k2"].as<float>();
|
||||
GlobalCameraParams::g_k3 = cam_node["k3"].as<float>();
|
||||
GlobalCameraParams::g_k4 = cam_node["k4"].as<float>();
|
||||
GlobalCameraParams::g_k5 = cam_node["k5"].as<float>();
|
||||
GlobalCameraParams::g_k6 = cam_node["k6"].as<float>();
|
||||
GlobalCameraParams::g_k7 = cam_node["k7"].as<float>();
|
||||
|
||||
GlobalCameraParams::g_fx = cam_node["A11"].as<float>();
|
||||
GlobalCameraParams::g_skew = cam_node["A12"].as<float>();
|
||||
GlobalCameraParams::g_fy = cam_node["A22"].as<float>();
|
||||
|
||||
GlobalCameraParams::g_cx = cam_node["u0"].as<float>();
|
||||
GlobalCameraParams::g_cy = cam_node["v0"].as<float>();
|
||||
|
||||
// === Read extrinsic transformation matrix ===
|
||||
GlobalCameraParams::g_T_camera_lidar <<
|
||||
tclNode[0].as<float>(), tclNode[1].as<float>(), tclNode[2].as<float>(), tclNode[3].as<float>(),
|
||||
tclNode[4].as<float>(), tclNode[5].as<float>(), tclNode[6].as<float>(), tclNode[7].as<float>(),
|
||||
tclNode[8].as<float>(), tclNode[9].as<float>(), tclNode[10].as<float>(), tclNode[11].as<float>(),
|
||||
tclNode[12].as<float>(), tclNode[13].as<float>(), tclNode[14].as<float>(), tclNode[15].as<float>();
|
||||
|
||||
// === Display key parameters concisely ===
|
||||
std::cout << "=== Camera Calibration Parameters ===" << std::endl;
|
||||
std::cout << "Intrinsics:" << std::endl;
|
||||
std::cout << " fx: " << GlobalCameraParams::g_fx
|
||||
<< ", fy: " << GlobalCameraParams::g_fy
|
||||
<< ", cx: " << GlobalCameraParams::g_cx
|
||||
<< ", cy: " << GlobalCameraParams::g_cy << std::endl;
|
||||
std::cout << "Distortion: k2=" << GlobalCameraParams::g_k2
|
||||
<< ", k3=" << GlobalCameraParams::g_k3 << std::endl;
|
||||
|
||||
Eigen::Vector3f translation = GlobalCameraParams::g_T_camera_lidar.block<3,1>(0,3);
|
||||
Eigen::Matrix3f rotation = GlobalCameraParams::g_T_camera_lidar.block<3,3>(0,0);
|
||||
std::cout << "Extrinsics:" << std::endl;
|
||||
std::cout << " Translation: [" << translation.x() << ", "
|
||||
<< translation.y() << ", " << translation.z() << "]" << std::endl;
|
||||
std::cout << " Rotation (euler angles): "
|
||||
<< rotation.eulerAngles(0,1,2).transpose() * 180/M_PI << "°" << std::endl;
|
||||
|
||||
return true;
|
||||
}
|
||||
void rawCloudRender::render(std::vector<std::vector<float>>& rgb_image,
|
||||
capture_Image_List_t* pcd_stream,
|
||||
int pcdIdx,
|
||||
std::vector<float>& rgbCloud_flat)
|
||||
{
|
||||
// Initialize constants
|
||||
constexpr float inv_1000 = 0.001f;
|
||||
const float fx = GlobalCameraParams::g_fx;
|
||||
const float fy = GlobalCameraParams::g_fy;
|
||||
const float cx = GlobalCameraParams::g_cx;
|
||||
const float cy = GlobalCameraParams::g_cy;
|
||||
const float skew = GlobalCameraParams::g_skew;
|
||||
const float k2 = GlobalCameraParams::g_k2;
|
||||
const float k3 = GlobalCameraParams::g_k3;
|
||||
const float k4 = GlobalCameraParams::g_k4;
|
||||
const float k5 = GlobalCameraParams::g_k5;
|
||||
const float k6 = GlobalCameraParams::g_k6;
|
||||
const float k7 = GlobalCameraParams::g_k7;
|
||||
const Eigen::Matrix4f& T = GlobalCameraParams::g_T_camera_lidar;
|
||||
|
||||
// Precompute matrix elements
|
||||
const float T00 = T(0,0), T01 = T(0,1), T02 = T(0,2), T03 = T(0,3);
|
||||
const float T10 = T(1,0), T11 = T(1,1), T12 = T(1,2), T13 = T(1,3);
|
||||
const float T20 = T(2,0), T21 = T(2,1), T22 = T(2,2), T23 = T(2,3);
|
||||
|
||||
// Initialize lookup table
|
||||
static std::array<float, 10000> dist_table;
|
||||
static bool table_init = [&](){
|
||||
for (size_t i=0; i<dist_table.size(); ++i) {
|
||||
float theta = i * (M_PI/2) / dist_table.size();
|
||||
dist_table[i] = theta*(1 + theta*(k2 + theta*(k3 + theta*(k4 + theta*(k5 + theta*(k6 + theta*k7))))));
|
||||
}
|
||||
return true;
|
||||
}();
|
||||
|
||||
// Get point cloud data (direct access)
|
||||
if (!pcd_stream || pcdIdx < 0 || pcdIdx >= 10) {
|
||||
std::cerr << "ERROR: Invalid pcd_stream or index in render function" << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
buffer_List_t& pcd_buffer = pcd_stream->imageList[pcdIdx];
|
||||
const int total_points = pcd_buffer.height * pcd_buffer.width;
|
||||
|
||||
if (!pcd_buffer.pAddr) {
|
||||
std::cerr << "ERROR: Null point cloud data pointer in render function" << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
float* data = static_cast<float*>(pcd_buffer.pAddr);
|
||||
|
||||
// Prepare output
|
||||
rgbCloud_flat.clear();
|
||||
rgbCloud_flat.resize(total_points * 4); // Preallocate maximum space
|
||||
float* output_ptr = rgbCloud_flat.data();
|
||||
|
||||
// Get image dimensions
|
||||
const int img_height = 1296;
|
||||
const int img_width = 1600;
|
||||
|
||||
// Process point cloud
|
||||
int valid_count = 0;
|
||||
for (int idx = 0; idx < total_points; ++idx)
|
||||
{
|
||||
float* pf = data + idx*4;
|
||||
|
||||
// Quick check for invalid points
|
||||
if (std::abs(pf[0]) < 1e-5f && std::abs(pf[1]) < 1e-5f && std::abs(pf[2]) < 1e-5f) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Coordinate transformation
|
||||
const float x = pf[2] * inv_1000;
|
||||
const float y = -pf[0] * inv_1000;
|
||||
const float z = pf[1] * inv_1000;
|
||||
|
||||
// Manual matrix transformation
|
||||
const float x1 = T00*x + T01*y + T02*z + T03;
|
||||
const float y1 = T10*x + T11*y + T12*z + T13;
|
||||
const float z1 = T20*x + T21*y + T22*z + T23;
|
||||
|
||||
// Check for points behind camera
|
||||
if (z1 <= 0.0f) continue;
|
||||
|
||||
// Calculate projection
|
||||
const float x1_sq = x1*x1;
|
||||
const float y1_sq = y1*y1;
|
||||
const float z1_sq = z1*z1;
|
||||
|
||||
const float norm = std::sqrt(x1_sq + y1_sq + z1_sq);
|
||||
if (norm < 1e-7f) continue;
|
||||
|
||||
const float r = std::sqrt(x1_sq + y1_sq);
|
||||
if (r < 1e-7f) continue;
|
||||
|
||||
const float cost = z1 / norm;
|
||||
const float theta = std::acos(cost);
|
||||
|
||||
// Safe table lookup
|
||||
const size_t table_idx = static_cast<size_t>(theta * (2.0f/M_PI) * dist_table.size());
|
||||
const size_t safe_idx = std::min(table_idx, dist_table.size()-1);
|
||||
const float thetad = dist_table[safe_idx];
|
||||
|
||||
const float scaling = thetad / r;
|
||||
const float xd = x1 * scaling;
|
||||
const float yd = y1 * scaling;
|
||||
const float pd_2d_x = xd * fx + yd * skew + cx;
|
||||
const float pd_2d_y = yd * fy + cy;
|
||||
|
||||
// Quick boundary check
|
||||
const int u = static_cast<int>(pd_2d_x);
|
||||
const int v = static_cast<int>(pd_2d_y);
|
||||
|
||||
// Strict boundary check
|
||||
if (u < 0 || u >= img_width || v < 0 || v >= img_height) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Safe image data access
|
||||
if (v < static_cast<int>(rgb_image.size()) && u < static_cast<int>(rgb_image[v].size())) {
|
||||
*output_ptr++ = x;
|
||||
*output_ptr++ = y;
|
||||
*output_ptr++ = z;
|
||||
*output_ptr++ = rgb_image[v][u];
|
||||
valid_count++;
|
||||
} else {
|
||||
// Handle invalid coordinates
|
||||
static bool warned = false;
|
||||
if (!warned) {
|
||||
if(raw_debug)
|
||||
{
|
||||
std::cerr << "WARNING: Invalid image coordinates: u=" << u << ", v=" << v
|
||||
<< " (image size: " << rgb_image.size() << "x"
|
||||
<< (rgb_image.empty() ? 0 : rgb_image[0].size()) << ")" << std::endl;
|
||||
}
|
||||
|
||||
warned = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
// Resize output
|
||||
rgbCloud_flat.resize(output_ptr - rgbCloud_flat.data());
|
||||
if(raw_debug)
|
||||
{
|
||||
std::cout << "Render completed: " << total_points << " points processed, "
|
||||
<< valid_count << " valid points ("
|
||||
<< (100.0 * valid_count / total_points) << "%)" << std::endl;
|
||||
}
|
||||
}
|
||||
void rawCloudRender::print_camera_calib() {
|
||||
std::cout << model_type_ << std::endl;
|
||||
std::cout << image_width_ << std::endl;
|
||||
std::cout << image_height_ << std::endl;
|
||||
|
||||
std::cout << "T_camera_lidar" << std::endl;
|
||||
std::cout << T_camera_lidar_(0,0) << " " << T_camera_lidar_(0,1) << " " << T_camera_lidar_(0,2) << " " << T_camera_lidar_(0,3) << std::endl;
|
||||
std::cout << T_camera_lidar_(1,0) << " " << T_camera_lidar_(1,1) << " " << T_camera_lidar_(1,2) << " " << T_camera_lidar_(1,3) << std::endl;
|
||||
std::cout << T_camera_lidar_(2,0) << " " << T_camera_lidar_(2,1) << " " << T_camera_lidar_(2,2) << " " << T_camera_lidar_(2,3) << std::endl;
|
||||
std::cout << T_camera_lidar_(3,0) << " " << T_camera_lidar_(3,1) << " " << T_camera_lidar_(3,2) << " " << T_camera_lidar_(3,3) << std::endl;
|
||||
|
||||
std::cout << "cam" << std::endl;
|
||||
std::cout << k2_ << std::endl;
|
||||
std::cout << k3_ << std::endl;
|
||||
std::cout << k4_ << std::endl;
|
||||
std::cout << k5_ << std::endl;
|
||||
std::cout << k6_ << std::endl;
|
||||
std::cout << k7_ << std::endl;
|
||||
|
||||
std::cout << A11_fx_ << std::endl;
|
||||
std::cout << A12_skew_ << std::endl;
|
||||
std::cout << A22_fy_ << std::endl;
|
||||
std::cout << u0_cx_ << std::endl;
|
||||
std::cout << v0_cy_ << std::endl;
|
||||
}
|
||||
@@ -0,0 +1,246 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
#include "yaml_parser.h"
|
||||
#include <fstream>
|
||||
#include <filesystem>
|
||||
#include <iostream>
|
||||
#include <algorithm>
|
||||
#include <iomanip>
|
||||
|
||||
#include <cstring>
|
||||
namespace odin_ros_driver {
|
||||
|
||||
YamlParser::YamlParser(const std::string& config_file)
|
||||
: config_file_(config_file) {}
|
||||
|
||||
bool YamlParser::loadConfig() {
|
||||
try {
|
||||
std::cerr << "Loading config file: " << config_file_ << std::endl;
|
||||
|
||||
// Check if file exists
|
||||
if (!std::filesystem::exists(config_file_)) {
|
||||
std::cerr << "Config file not found: " << config_file_ << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
// Print file contents
|
||||
std::ifstream file(config_file_);
|
||||
std::string content((std::istreambuf_iterator<char>(file)),
|
||||
std::istreambuf_iterator<char>());
|
||||
std::cerr << "Config file content:\n" << content << "\n--- End of file ---" << std::endl;
|
||||
|
||||
// Load YAML
|
||||
YAML::Node config = YAML::LoadFile(config_file_);
|
||||
|
||||
// Check if 'register_keys' node exists
|
||||
if (!config["register_keys"]) {
|
||||
std::cerr << "Missing 'register_keys' section in config file" << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
YAML::Node register_keys = config["register_keys"];
|
||||
register_keys_.clear();
|
||||
register_keys_str_val_.clear();
|
||||
custom_parameters_.clear();
|
||||
|
||||
// Print number of key-value pairs found
|
||||
std::cerr << "Found " << register_keys.size() << " keys in config" << std::endl;
|
||||
|
||||
for (YAML::const_iterator it = register_keys.begin(); it != register_keys.end(); ++it) {
|
||||
std::string key = it->first.as<std::string>();
|
||||
const YAML::Node& value_node = it->second;
|
||||
|
||||
// Convert key to lowercase
|
||||
std::transform(key.begin(), key.end(), key.begin(),
|
||||
[](unsigned char c){ return std::tolower(c); });
|
||||
|
||||
// Check if this is a custom parameter
|
||||
if (key.substr(0, 7) == "custom_") {
|
||||
std::string param_name = key.substr(7);
|
||||
|
||||
// Handle different value types
|
||||
if (value_node.IsScalar()) {
|
||||
// Single scalar value (int or float)
|
||||
try {
|
||||
int int_value = value_node.as<int>();
|
||||
ParameterValue param_value;
|
||||
param_value.type = DataType::INT_TYPE;
|
||||
param_value.setData(int_value);
|
||||
custom_parameters_[param_name] = param_value;
|
||||
std::cerr << "Loaded custom parameter (int): " << param_name << " = " << int_value << std::endl;
|
||||
} catch (...) {
|
||||
try {
|
||||
double float_value = value_node.as<double>();
|
||||
ParameterValue param_value;
|
||||
param_value.type = DataType::FLOAT_ARRAY_TYPE;
|
||||
std::vector<float> float_array = {static_cast<float>(float_value)};
|
||||
param_value.setArray(float_array);
|
||||
custom_parameters_[param_name] = param_value;
|
||||
std::cerr << "Loaded custom parameter (float): " << param_name << " = " << float_value << std::endl;
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "Failed to parse custom parameter " << param_name << ": " << e.what() << std::endl;
|
||||
}
|
||||
}
|
||||
} else if (value_node.IsSequence()) {
|
||||
// Array of values
|
||||
size_t array_size = value_node.size();
|
||||
if (array_size == 0) {
|
||||
std::cerr << "Empty array for custom parameter: " << param_name << std::endl;
|
||||
continue;
|
||||
}
|
||||
|
||||
// Try to detect if it's a float or int array based on first element
|
||||
try {
|
||||
// Try to parse as float array first
|
||||
std::vector<float> float_array;
|
||||
for (size_t i = 0; i < array_size; ++i) {
|
||||
float_array.push_back(value_node[i].as<float>());
|
||||
}
|
||||
ParameterValue param_value;
|
||||
param_value.type = DataType::FLOAT_ARRAY_TYPE;
|
||||
param_value.setArray(float_array);
|
||||
custom_parameters_[param_name] = param_value;
|
||||
|
||||
std::cerr << "Loaded custom parameter (float array): " << param_name << " = [";
|
||||
for (size_t i = 0; i < float_array.size(); ++i) {
|
||||
if (i > 0) std::cerr << ", ";
|
||||
std::cerr << std::fixed << std::setprecision(4) << float_array[i];
|
||||
}
|
||||
std::cerr << "]" << std::endl;
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "Failed to parse custom parameter array " << param_name << ": " << e.what() << std::endl;
|
||||
}
|
||||
}
|
||||
} else if (allowed_key_w_str_val.find(key) != allowed_key_w_str_val.end()) {
|
||||
try {
|
||||
std::string value = value_node.as<std::string>();
|
||||
register_keys_str_val_[key] = value;
|
||||
std::cerr << "Loaded key: " << key << " = " << value << std::endl;
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "Failed to parse key " << key << ": " << e.what() << std::endl;
|
||||
}
|
||||
} else if (allowed_key_w_float_val.find(key) != allowed_key_w_float_val.end()) {
|
||||
try {
|
||||
double value = value_node.as<double>();
|
||||
register_keys_float_val_[key] = value;
|
||||
std::cerr << "Loaded key: " << key << " = " << value << std::endl;
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "Failed to parse key " << key << ": " << e.what() << std::endl;
|
||||
}
|
||||
} else {
|
||||
// Regular (non-custom) integer parameter
|
||||
try {
|
||||
int value = value_node.as<int>();
|
||||
register_keys_[key] = value;
|
||||
std::cerr << "Loaded key: " << key << " = " << value << std::endl;
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "Failed to parse key " << key << ": " << e.what() << std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
} catch (const YAML::Exception& e) {
|
||||
std::cerr << "YAML exception: " << e.what() << std::endl;
|
||||
return false;
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "Exception: " << e.what() << std::endl;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
const std::map<std::string, int>& YamlParser::getRegisterKeys() const {
|
||||
return register_keys_;
|
||||
}
|
||||
|
||||
const std::map<std::string, ParameterValue>& YamlParser::getCustomParameters() const {
|
||||
return custom_parameters_;
|
||||
}
|
||||
|
||||
const std::map<std::string, std::string>& YamlParser::getRegisterKeysStrVal() const {
|
||||
return register_keys_str_val_;
|
||||
}
|
||||
|
||||
void YamlParser::printConfig() const {
|
||||
std::cerr << "Configuration Keys:" << std::endl;
|
||||
if (register_keys_.empty()) {
|
||||
std::cerr << " (int val empty)" << std::endl;
|
||||
} else {
|
||||
for (const auto& [key, value] : register_keys_) {
|
||||
std::cerr << " " << key << ": " << value << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
if (register_keys_str_val_.empty()) {
|
||||
std::cerr << " (str_val empty)" << std::endl;
|
||||
} else {
|
||||
for (const auto& [key, value] : register_keys_str_val_) {
|
||||
std::cerr << " " << key << ": " << value << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
std::cerr << "Custom Parameters:" << std::endl;
|
||||
if (custom_parameters_.empty()) {
|
||||
std::cerr << " (custom param empty)" << std::endl;
|
||||
} else {
|
||||
for (const auto& [key, param_val] : custom_parameters_) {
|
||||
std::cerr << " " << key << ": (size=" << param_val.getSize() << " bytes)";
|
||||
if (param_val.type == DataType::INT_TYPE && param_val.getSize() == sizeof(int)) {
|
||||
int int_val = *reinterpret_cast<const int*>(param_val.getData());
|
||||
std::cerr << " = " << int_val;
|
||||
} else if (param_val.type == DataType::FLOAT_ARRAY_TYPE && param_val.getSize() % sizeof(float) == 0) {
|
||||
size_t count = param_val.getSize() / sizeof(float);
|
||||
const float* float_arr = reinterpret_cast<const float*>(param_val.getData());
|
||||
std::cerr << " = [";
|
||||
for (size_t i = 0; i < count; ++i) {
|
||||
if (i > 0) std::cerr << ", ";
|
||||
std::cerr << std::fixed << std::setprecision(4) << float_arr[i];
|
||||
}
|
||||
std::cerr << "]";
|
||||
}
|
||||
std::cerr << std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool YamlParser::applyCustomParameters(device_handle device) {
|
||||
bool success = true;
|
||||
|
||||
for (const auto& [param_name, param_value] : custom_parameters_) {
|
||||
std::cerr << "Setting custom parameter: " << param_name << " (size=" << param_value.getSize() << " bytes)" << std::endl;
|
||||
|
||||
int result = lidar_set_custom_parameter(device, param_name.c_str(), param_value.getData(), param_value.getSize());
|
||||
if (result != 0) {
|
||||
std::cerr << "Failed to set custom parameter " << param_name << ": error code " << result << std::endl;
|
||||
success = false;
|
||||
} else {
|
||||
std::cerr << "Successfully set custom parameter " << param_name << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
int YamlParser::getCustomParameterInt(const std::string& param_name, int default_value) const {
|
||||
auto it = custom_parameters_.find(param_name);
|
||||
if (it != custom_parameters_.end()) {
|
||||
const ParameterValue& param_value = it->second;
|
||||
if (param_value.type == DataType::INT_TYPE && param_value.getSize() == sizeof(int)) {
|
||||
return *reinterpret_cast<const int*>(param_value.getData());
|
||||
}
|
||||
}
|
||||
return default_value;
|
||||
}
|
||||
|
||||
}
|
||||
+1
-1
@@ -47,7 +47,7 @@ action:
|
||||
- rr_wheel
|
||||
wheel_indices: [12, 13, 14, 15]
|
||||
scale: [0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 5.0, 5.0, 5.0, 5.0]
|
||||
default_dof_pos: [0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.0, 0.0, 0.0]
|
||||
default_dof_pos: [0.0, 0.670, -1.390, 0.0, 0.670, -1.390, 0.0, 0.670, -1.390, 0.0, 0.670, -1.390, 0.0, 0.0, 0.0, 0.0]
|
||||
|
||||
motor_mapping:
|
||||
can_id_map:
|
||||
|
||||
@@ -4,7 +4,29 @@
|
||||
motor_hz: 200.0
|
||||
status_hz: 10.0
|
||||
target_timeout_ms: 150.0
|
||||
model_path: policies/model_rough.onnx
|
||||
event_log_dir: "logs_v2_web"
|
||||
model_engine_path: policies/NEWmodel_1900_fp16.engine
|
||||
prefer_tensorrt: true
|
||||
model_path: policies/NEWmodel_1900.onnx
|
||||
rough_model_engine_path: policies/NEWmodel_1900_fp16.engine
|
||||
crawl_model_path: policies/model_crawl.onnx
|
||||
crawl_model_engine_path: ""
|
||||
rough_default_dof_pos: [0.0, 0.670, -1.390, 0.0, 0.670, -1.390, 0.0, 0.670, -1.390, 0.0, 0.670, -1.390, 0.0, 0.0, 0.0, 0.0]
|
||||
crawl_backend: "ik"
|
||||
crawl_default_dof_pos: [0.2, 1.697, -2.650, -0.2, 1.697, -2.650, 0.2, 1.697, -2.650, -0.2, 1.697, -2.650, 0.0, 0.0, 0.0, 0.0]
|
||||
crawl_ik_wheel_linear_gain: 6.25
|
||||
crawl_ik_wheel_yaw_gain: 4.0
|
||||
crawl_ik_max_wheel_speed: 6.0
|
||||
crawl_ik_abduction_clip: 0.45
|
||||
model_switch_transition_s: 1.5
|
||||
model_switch_to_stand_transition_scale: 2.0
|
||||
model_switch_to_model_transition_scale: 2.5
|
||||
model_switch_stand_hold_s: 0.8
|
||||
model_switch_stand_max_err: 0.18
|
||||
model_switch_stand_max_vel: 0.8
|
||||
model_switch_release_scale: 2.0
|
||||
debug_trace_enabled: true
|
||||
debug_trace_decimation: 1
|
||||
use_cuda: true # 启用 CUDA Execution Provider（Orin Nano GPU 加速）
|
||||
contract_file: deployment_contract.yaml
|
||||
dry_run: false
|
||||
@@ -30,6 +52,11 @@
|
||||
remote_publish_inactive_zero: true
|
||||
remote_estop_latch: true
|
||||
remote_poll_hz: 50.0
|
||||
remote_model_switch_enabled: true
|
||||
remote_model_switch_channel: 10
|
||||
remote_model_switch_debounce_frames: 3
|
||||
remote_model_switch_rough_level: "low"
|
||||
remote_model_switch_ik_level: "high"
|
||||
|
||||
# Command mux parameters
|
||||
cmd_mux_default_mode: "NAV"
|
||||
@@ -64,18 +91,43 @@
|
||||
# Safety parameters
|
||||
safety_enabled: true
|
||||
max_target_offset: 0.6
|
||||
model_switch_max_target_offset: 1.8
|
||||
hard_target_offset: 2.0
|
||||
max_ang_vel: 10.0
|
||||
max_ang_vel: 24.0
|
||||
max_tilt_z: -0.3
|
||||
clip_to_brake: 0
|
||||
imu_age_warn_ms: 60.0
|
||||
imu_age_stop_ms: 200.0
|
||||
wheel_no_effect_command_threshold: 1.0
|
||||
wheel_no_effect_min_response_ratio: 0.20
|
||||
wheel_no_effect_velocity_epsilon: 0.25
|
||||
wheel_no_effect_max_temperature_c: 90.0
|
||||
wheel_no_effect_min_bus_voltage_v: 18.0
|
||||
wheel_no_effect_command_warmup_cycles: 12
|
||||
wheel_no_effect_trigger_cycles: 30
|
||||
wheel_no_effect_attempt_limit: 2
|
||||
wheel_no_effect_cooldown_ms: 1200
|
||||
wheel_recovery_verify_timeout_ms: 180
|
||||
wheel_no_effect_diag_freshness_ms: 350
|
||||
wheel_no_effect_diag_request_period_ms: 80
|
||||
leg_no_effect_position_error_threshold: 0.18
|
||||
leg_no_effect_velocity_epsilon: 0.12
|
||||
leg_no_effect_max_estimated_current_arms: 4.0
|
||||
leg_no_effect_max_abs_torque_nm: 5.0
|
||||
leg_no_effect_max_temperature_c: 100.0
|
||||
leg_no_effect_min_bus_voltage_v: 18.0
|
||||
leg_no_effect_command_warmup_cycles: 40
|
||||
leg_no_effect_trigger_cycles: 25
|
||||
leg_no_effect_attempt_limit: 2
|
||||
leg_no_effect_cooldown_ms: 1200
|
||||
leg_recovery_verify_timeout_ms: 220
|
||||
|
||||
# Policy alignment with the Python deployment
|
||||
command_release_s: 0.35
|
||||
release_command_hold_s: 0.12
|
||||
release_posture_max_err: 0.35
|
||||
release_target_blend_s: 0.30
|
||||
model_switch_release_scale: 2.0
|
||||
clip_obs: 100.0
|
||||
hold_zero_command_pose: true
|
||||
enable_zero_cmd_suppression: true
|
||||
@@ -92,9 +144,42 @@
|
||||
nav_max_wz: 0.80
|
||||
nav_kp_dist: 0.80
|
||||
nav_kp_yaw: 1.80
|
||||
nav_goal_exit_tolerance_margin: 0.08
|
||||
nav_goal_complete_stable_cycles: 4
|
||||
nav_final_align_kp_yaw_scale: 0.60
|
||||
nav_final_align_max_wz: 0.45
|
||||
nav_final_align_creep_speed: 0.05
|
||||
nav_goal_yaw_tolerance_deg: 12.0
|
||||
nav_astar_enabled: true
|
||||
nav_astar_resolution: 0.10
|
||||
nav_astar_pcd_sample_step: 5
|
||||
nav_astar_allow_diagonal: true
|
||||
nav_astar_smooth_enabled: true
|
||||
nav_astar_corner_blend_dist: 0.20
|
||||
nav_astar_waypoint_reach_dist: 0.18
|
||||
nav_astar_lookahead_dist: 0.35
|
||||
nav_astar_snap_radius: 0.60
|
||||
nav_astar_max_expansions: 120000
|
||||
nav_turn_in_place_enabled: true
|
||||
nav_turn_in_place_enter_yaw_deg: 70.0
|
||||
nav_turn_in_place_exit_yaw_deg: 18.0
|
||||
nav_turn_in_place_max_wz: 0.80
|
||||
nav_pre_dock_enabled: true
|
||||
nav_pre_dock_distance: 0.35
|
||||
nav_pre_dock_tolerance: 0.18
|
||||
nav_pre_dock_skip_within_goal_dist: 0.45
|
||||
nav_goals_file: src/sim2real_nav2/config/maps/factory_a/goals.yaml
|
||||
nav_missions_file: src/sim2real_nav2/config/maps/factory_a/missions.yaml
|
||||
pcd_nav_file: map/map1.pcd
|
||||
# Keep the legacy YAML route for reference; active task is selected by nav_route_task_file below.
|
||||
nav_route_file: src/sim2real_nav2/config/maps/a_min/test_route.yaml
|
||||
# Task switch entry: change this path to another .json/.yaml route file, then relaunch or reload the nav nodes.
|
||||
nav_route_task_file: tools/test_route.json
|
||||
nav_route_auto_align_enabled: false
|
||||
nav_route_rotation_offset_deg: 0.0
|
||||
nav_route_align_max_angle_deg: 6.0
|
||||
nav_route_align_angle_step_deg: 0.5
|
||||
nav_route_align_search_radius: 0.35
|
||||
pcd_nav_file: map/map_b.pcd
|
||||
pcd_floor_z_min: -1.6
|
||||
pcd_floor_z_max: 0.4
|
||||
pcd_sample_step: 25
|
||||
|
||||
+12
-4
@@ -1,7 +1,7 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
|
||||
from launch.conditions import IfCondition
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.parameter_descriptions import ParameterFile
|
||||
@@ -49,6 +49,14 @@ def generate_launch_description():
|
||||
description='Whether to launch the simple waypoint navigation node'
|
||||
)
|
||||
|
||||
event_log_dir_arg = DeclareLaunchArgument(
|
||||
'event_log_dir',
|
||||
default_value=PythonExpression([
|
||||
"'logs_v2_web/run_' + __import__('datetime').datetime.now().strftime('%Y-%m-%d_%H-%M-%S_%f')[:-3]"
|
||||
]),
|
||||
description='Per-run event log directory'
|
||||
)
|
||||
|
||||
# Include odin_ros_driver launch
|
||||
driver_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
@@ -80,19 +88,20 @@ def generate_launch_description():
|
||||
launch_remote_arg,
|
||||
launch_web_bridge_arg,
|
||||
launch_simple_nav_arg,
|
||||
event_log_dir_arg,
|
||||
Node(
|
||||
package="sim2real_hw",
|
||||
executable="sim2real_hw_node",
|
||||
name="sim2real_hw_node",
|
||||
output="screen",
|
||||
parameters=[runtime_params],
|
||||
parameters=[runtime_params, {"event_log_dir": LaunchConfiguration("event_log_dir")}],
|
||||
),
|
||||
Node(
|
||||
package="sim2real_runtime",
|
||||
executable="sim2real_runtime_node",
|
||||
name="sim2real_runtime_node",
|
||||
output="screen",
|
||||
parameters=[runtime_params],
|
||||
parameters=[runtime_params, {"event_log_dir": LaunchConfiguration("event_log_dir")}],
|
||||
),
|
||||
Node(
|
||||
package="sim2real_runtime",
|
||||
@@ -141,4 +150,3 @@ def generate_launch_description():
|
||||
driver_launch,
|
||||
nav2_launch,
|
||||
])
|
||||
|
||||
|
||||
+4
-4
@@ -64,10 +64,10 @@ struct DeploymentContract
|
||||
};
|
||||
|
||||
static constexpr std::array<float, 16> kDefaultDofPos = {
|
||||
0.0f, 0.9f, -1.8f,
|
||||
0.0f, 0.9f, -1.8f,
|
||||
0.0f, 0.9f, -1.8f,
|
||||
0.0f, 0.9f, -1.8f,
|
||||
0.0f, 0.670f, -1.390f,
|
||||
0.0f, 0.670f, -1.390f,
|
||||
0.0f, 0.670f, -1.390f,
|
||||
0.0f, 0.670f, -1.390f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f
|
||||
};
|
||||
};
|
||||
|
||||
+167
@@ -0,0 +1,167 @@
|
||||
#pragma once
|
||||
|
||||
#include <chrono>
|
||||
#include <ctime>
|
||||
#include <filesystem>
|
||||
#include <fstream>
|
||||
#include <iomanip>
|
||||
#include <mutex>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <system_error>
|
||||
|
||||
namespace sim2real_common
|
||||
{
|
||||
|
||||
class EventLogger
|
||||
{
|
||||
public:
|
||||
static std::string makeRunId()
|
||||
{
|
||||
const auto now = std::chrono::system_clock::now();
|
||||
const auto millis = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
now.time_since_epoch()).count() % 1000;
|
||||
const std::time_t now_time = std::chrono::system_clock::to_time_t(now);
|
||||
const std::tm tm_value = toLocalTm(now_time);
|
||||
|
||||
std::ostringstream oss;
|
||||
oss << "run_"
|
||||
<< std::put_time(&tm_value, "%Y-%m-%d_%H-%M-%S")
|
||||
<< "_"
|
||||
<< std::setw(3) << std::setfill('0') << millis;
|
||||
return oss.str();
|
||||
}
|
||||
|
||||
void configure(const std::string & log_dir, const std::string & file_stem)
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
log_dir_ = log_dir;
|
||||
file_stem_ = file_stem;
|
||||
}
|
||||
|
||||
std::string componentLogPath() const
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
return componentLogPathUnlocked();
|
||||
}
|
||||
|
||||
std::string timelineLogPath() const
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
return timelineLogPathUnlocked();
|
||||
}
|
||||
|
||||
std::string summaryLogPath() const
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
return summaryLogPathUnlocked();
|
||||
}
|
||||
|
||||
void log(
|
||||
const std::string & level,
|
||||
const std::string & component,
|
||||
const std::string & event,
|
||||
const std::string & message)
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
if (!isConfiguredUnlocked()) {
|
||||
return;
|
||||
}
|
||||
|
||||
const std::string line = buildLogLine(level, component, event, message);
|
||||
appendLineUnlocked(componentLogPathUnlocked(), line);
|
||||
appendLineUnlocked(timelineLogPathUnlocked(), line);
|
||||
}
|
||||
|
||||
void logSummary(const std::string & component, const std::string & message)
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
if (!isConfiguredUnlocked()) {
|
||||
return;
|
||||
}
|
||||
|
||||
const std::string line = buildLogLine("SUMMARY", component, "session_summary", message);
|
||||
appendLineUnlocked(summaryLogPathUnlocked(), line);
|
||||
appendLineUnlocked(timelineLogPathUnlocked(), line);
|
||||
}
|
||||
|
||||
private:
|
||||
static std::tm toLocalTm(std::time_t now_time)
|
||||
{
|
||||
std::tm tm_value{};
|
||||
#ifdef _WIN32
|
||||
localtime_s(&tm_value, &now_time);
|
||||
#else
|
||||
localtime_r(&now_time, &tm_value);
|
||||
#endif
|
||||
return tm_value;
|
||||
}
|
||||
|
||||
static std::string formatTimestamp()
|
||||
{
|
||||
const auto now = std::chrono::system_clock::now();
|
||||
const auto millis = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
now.time_since_epoch()).count() % 1000;
|
||||
const std::time_t now_time = std::chrono::system_clock::to_time_t(now);
|
||||
const std::tm tm_value = toLocalTm(now_time);
|
||||
|
||||
std::ostringstream oss;
|
||||
oss << std::put_time(&tm_value, "%Y-%m-%d %H:%M:%S")
|
||||
<< '.'
|
||||
<< std::setw(3) << std::setfill('0') << millis;
|
||||
return oss.str();
|
||||
}
|
||||
|
||||
static std::string buildLogLine(
|
||||
const std::string & level,
|
||||
const std::string & component,
|
||||
const std::string & event,
|
||||
const std::string & message)
|
||||
{
|
||||
std::ostringstream oss;
|
||||
oss << "[" << formatTimestamp() << "]"
|
||||
<< "[" << level << "]"
|
||||
<< "[" << component << "]"
|
||||
<< "[" << event << "] "
|
||||
<< message;
|
||||
return oss.str();
|
||||
}
|
||||
|
||||
bool isConfiguredUnlocked() const
|
||||
{
|
||||
return !log_dir_.empty() && !file_stem_.empty();
|
||||
}
|
||||
|
||||
void appendLineUnlocked(const std::string & path, const std::string & line) const
|
||||
{
|
||||
std::error_code ec;
|
||||
std::filesystem::create_directories(log_dir_, ec);
|
||||
|
||||
std::ofstream stream(path, std::ios::app);
|
||||
if (!stream.is_open()) {
|
||||
return;
|
||||
}
|
||||
stream << line << '\n';
|
||||
}
|
||||
|
||||
std::string componentLogPathUnlocked() const
|
||||
{
|
||||
return (std::filesystem::path(log_dir_) / (file_stem_ + ".log")).string();
|
||||
}
|
||||
|
||||
std::string timelineLogPathUnlocked() const
|
||||
{
|
||||
return (std::filesystem::path(log_dir_) / "timeline.log").string();
|
||||
}
|
||||
|
||||
std::string summaryLogPathUnlocked() const
|
||||
{
|
||||
return (std::filesystem::path(log_dir_) / "summary.log").string();
|
||||
}
|
||||
|
||||
mutable std::mutex mutex_;
|
||||
std::string log_dir_;
|
||||
std::string file_stem_;
|
||||
};
|
||||
|
||||
} // namespace sim2real_common
|
||||
+12
-32
@@ -1,10 +1,11 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
|
||||
#include "sim2real_common/deployment_contract.hpp"
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.14159265358979323846
|
||||
#endif
|
||||
@@ -18,12 +19,15 @@ public:
|
||||
StandBalanceController(double control_dt = 0.02)
|
||||
: control_dt_(control_dt)
|
||||
{
|
||||
profile_h_ = {0.157f, 0.248f, 0.311f, 0.366f, 0.411f, 0.448f};
|
||||
profile_hip_ = {1.5f, 1.2f, 1.0f, 0.8f, 0.6f, 0.4f};
|
||||
profile_knee_ = {-2.5f, -2.1f, -1.8f, -1.5f, -1.2f, -0.9f};
|
||||
reset();
|
||||
}
|
||||
|
||||
void setNominalLegPose(float hip_pitch, float knee)
|
||||
{
|
||||
nominal_hip_pitch_ = hip_pitch;
|
||||
nominal_knee_ = knee;
|
||||
}
|
||||
|
||||
void reset()
|
||||
{
|
||||
stable_time_ = 0.0f;
|
||||
@@ -34,9 +38,8 @@ public:
|
||||
const std::array<float, 3>& imu_gyro,
|
||||
const std::array<float, 3>& cmd)
|
||||
{
|
||||
float hip_base = 0.9f;
|
||||
float knee_base = -1.8f;
|
||||
estimateBaseLegPose(hip_base, knee_base);
|
||||
const float hip_base = nominal_hip_pitch_;
|
||||
const float knee_base = nominal_knee_;
|
||||
|
||||
float roll = 0.0f;
|
||||
float pitch = 0.0f;
|
||||
@@ -84,28 +87,9 @@ private:
|
||||
pitch = std::atan2(gx, std::sqrt(std::max(1e-6f, gy * gy + gz * gz)));
|
||||
}
|
||||
|
||||
void estimateBaseLegPose(float& hip, float& knee)
|
||||
{
|
||||
float h_clamp = std::clamp(height_, profile_h_.front(), profile_h_.back());
|
||||
hip = interpolate(h_clamp, profile_h_, profile_hip_);
|
||||
knee = interpolate(h_clamp, profile_h_, profile_knee_);
|
||||
}
|
||||
|
||||
float interpolate(float x, const std::vector<float>& xp, const std::vector<float>& fp)
|
||||
{
|
||||
if (x <= xp.front()) return fp.front();
|
||||
if (x >= xp.back()) return fp.back();
|
||||
for (std::size_t i = 0; i < xp.size() - 1; ++i) {
|
||||
if (x >= xp[i] && x <= xp[i+1]) {
|
||||
float f = (x - xp[i]) / (xp[i+1] - xp[i]);
|
||||
return fp[i] + f * (fp[i+1] - fp[i]);
|
||||
}
|
||||
}
|
||||
return fp.back();
|
||||
}
|
||||
|
||||
double control_dt_;
|
||||
float height_{0.33f};
|
||||
float nominal_hip_pitch_{DeploymentContract::kDefaultDofPos[1]};
|
||||
float nominal_knee_{DeploymentContract::kDefaultDofPos[2]};
|
||||
float kp_roll_{0.85f};
|
||||
float kd_roll_rate_{0.03f};
|
||||
float lateral_lean_gain_{0.0f};
|
||||
@@ -117,10 +101,6 @@ private:
|
||||
float stable_gyro_deg_s_{45.0f};
|
||||
float enter_hold_s_{1.0f};
|
||||
|
||||
std::vector<float> profile_h_;
|
||||
std::vector<float> profile_hip_;
|
||||
std::vector<float> profile_knee_;
|
||||
|
||||
float stable_time_{0.0f};
|
||||
};
|
||||
|
||||
|
||||
+161
@@ -12,16 +12,40 @@
|
||||
#include "sensor_msgs/msg/imu.hpp"
|
||||
#include "nav_msgs/msg/odometry.hpp"
|
||||
#include "std_msgs/msg/bool.hpp"
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
#include "sim2real_interfaces/msg/runtime_state.hpp"
|
||||
#include "sim2real_interfaces/msg/runtime_target.hpp"
|
||||
#include "sim2real_common/event_logger.hpp"
|
||||
#include "sim2real_common/low_pass_filter.hpp"
|
||||
#include "sim2real_common/mahony_filter.hpp"
|
||||
#include "sim2real_common/safety_monitor.hpp"
|
||||
#include "sim2real_common/runtime_guard.hpp"
|
||||
|
||||
struct can_frame;
|
||||
|
||||
namespace sim2real_hw
|
||||
{
|
||||
|
||||
enum class RecoveryKind
|
||||
{
|
||||
None,
|
||||
Stale,
|
||||
NoEffect
|
||||
};
|
||||
|
||||
enum class RecoveryStage
|
||||
{
|
||||
Idle,
|
||||
AwaitInitFeedback,
|
||||
AwaitEffectVerification
|
||||
};
|
||||
|
||||
enum class ActiveModelMode
|
||||
{
|
||||
Rough,
|
||||
Crawl
|
||||
};
|
||||
|
||||
struct MotorConfig
|
||||
{
|
||||
int bus; // 1 or 2
|
||||
@@ -36,14 +60,49 @@ struct MotorStateInternal
|
||||
float velocity{0.0f};
|
||||
float torque{0.0f};
|
||||
float temperature{0.0f};
|
||||
float bus_voltage{0.0f};
|
||||
float estimated_current_arms{0.0f};
|
||||
float last_command_sim{0.0f};
|
||||
std::uint32_t update_count{0};
|
||||
std::uint32_t stale_count{0};
|
||||
std::uint32_t command_active_count{0};
|
||||
std::uint32_t no_effect_count{0};
|
||||
std::uint16_t fault_code{0};
|
||||
std::uint16_t fault_detail_1{0};
|
||||
std::uint16_t fault_detail_2{0};
|
||||
// Hold-over state
|
||||
float last_valid_pos{0.0f};
|
||||
float last_valid_vel{0.0f};
|
||||
float last_valid_torque{0.0f};
|
||||
std::uint32_t prev_update_count{0};
|
||||
bool has_valid_data{false};
|
||||
bool has_bus_voltage{false};
|
||||
bool has_fault_snapshot{false};
|
||||
bool stale_reported{false};
|
||||
bool recovered_reported{false};
|
||||
bool disable_reported{false};
|
||||
bool command_effect_monitoring_active{false};
|
||||
bool no_effect_reported{false};
|
||||
bool high_temp_reported{false};
|
||||
bool high_current_reported{false};
|
||||
bool high_voltage_reported{false};
|
||||
bool low_voltage_reported{false};
|
||||
bool fault_code_reported{false};
|
||||
bool init_confirmed{false};
|
||||
std::uint32_t init_attempt_count{0};
|
||||
std::uint32_t recovery_attempt_count{0};
|
||||
std::uint32_t no_effect_recovery_attempt_count{0};
|
||||
std::chrono::steady_clock::time_point last_recovery_attempt_time_{};
|
||||
std::chrono::steady_clock::time_point last_no_effect_recovery_attempt_time_{};
|
||||
std::chrono::steady_clock::time_point last_diag_snapshot_time_{};
|
||||
std::chrono::steady_clock::time_point last_diag_request_time_{};
|
||||
std::chrono::steady_clock::time_point recovery_stage_deadline_{};
|
||||
std::uint32_t recovery_start_update_count{0};
|
||||
std::uint32_t recovery_active_attempt_number{0};
|
||||
RecoveryKind recovery_kind{RecoveryKind::None};
|
||||
RecoveryStage recovery_stage{RecoveryStage::Idle};
|
||||
std::string recovery_trigger;
|
||||
std::string last_power_event_reason;
|
||||
};
|
||||
|
||||
class HardwareBridgeNode : public rclcpp::Node
|
||||
@@ -54,6 +113,7 @@ public:
|
||||
|
||||
private:
|
||||
void onTarget(const sim2real_interfaces::msg::RuntimeTarget::SharedPtr msg);
|
||||
void onModelStatus(const std_msgs::msg::String::SharedPtr msg);
|
||||
void onReadLoop();
|
||||
void onWriteLoop();
|
||||
void onImu(const sensor_msgs::msg::Imu::SharedPtr msg);
|
||||
@@ -66,11 +126,59 @@ private:
|
||||
bool enableMotor(int fd, int motor_id);
|
||||
bool disableMotor(int fd, int motor_id);
|
||||
bool setModeRaw(int fd, int motor_id, std::int8_t mode);
|
||||
bool readParameter(int fd, int motor_id, std::uint16_t param_id);
|
||||
bool writeLimit(int fd, int motor_id, std::uint16_t param_id, float limit);
|
||||
bool writeOperationFrame(int fd, int motor_id, double pos, double vel, double kp, double kd, double torque);
|
||||
bool initializeMotor(std::size_t index, const std::string & reason, int max_attempts = 3);
|
||||
bool initializeMotorsOnBus(int bus_id, const std::string & reason);
|
||||
bool waitForMotorFeedback(std::size_t index, std::chrono::milliseconds timeout);
|
||||
void processCanFrame(const struct can_frame & frame, int bus_id);
|
||||
void drainCanFrames(int fd, int bus_id, int timeout_us);
|
||||
bool isLegMotor(std::size_t index) const;
|
||||
bool isWheelMotor(std::size_t index) const;
|
||||
bool motorHasBlockingFault(std::size_t index) const;
|
||||
bool isNoEffectConditionPresent(std::size_t index) const;
|
||||
std::uint32_t noEffectCommandWarmupCycles(std::size_t index) const;
|
||||
std::uint32_t noEffectTriggerCycles(std::size_t index) const;
|
||||
std::uint32_t noEffectAttemptLimit(std::size_t index) const;
|
||||
std::uint32_t noEffectCooldownMs(std::size_t index) const;
|
||||
std::uint32_t noEffectVerifyTimeoutMs(std::size_t index) const;
|
||||
bool hasFreshNoEffectDiagnostics(std::size_t index) const;
|
||||
void requestMotorDiagnostics(std::size_t index);
|
||||
std::string classifyNoEffectSuspect(std::size_t index) const;
|
||||
std::string buildNoEffectSummary(std::size_t index) const;
|
||||
void updateMotorCommandTracking(std::size_t index, float sim_command, const std::string & target_source);
|
||||
void updateNoEffectDetection(std::size_t index);
|
||||
bool startMotorRecoverySequence(
|
||||
std::size_t index,
|
||||
const std::string & trigger,
|
||||
RecoveryKind kind,
|
||||
std::uint32_t attempt_number);
|
||||
void processMotorRecoverySequence(std::size_t index);
|
||||
void clearMotorRecoverySequence(std::size_t index);
|
||||
bool shouldAttemptMotorRecovery(std::size_t index) const;
|
||||
bool attemptMotorRecovery(std::size_t index, const std::string & trigger);
|
||||
bool shouldAttemptNoEffectRecovery(std::size_t index) const;
|
||||
bool attemptNoEffectRecovery(std::size_t index, const std::string & trigger);
|
||||
const char * jointName(std::size_t index) const;
|
||||
std::string motorTag(std::size_t index) const;
|
||||
float estimateCurrentArms(float torque_nm) const;
|
||||
std::string decodeFaultCode(std::uint16_t fault_code) const;
|
||||
std::string decodeFaultDetailRegister(std::uint16_t register_value, int register_index) const;
|
||||
std::string buildMotorFaultSummary(std::size_t index) const;
|
||||
std::string formatProtectionReason(const std::string & trigger, const std::string & reason) const;
|
||||
void logProtectionEvent(const std::string & trigger, const std::string & reason, const std::string & action);
|
||||
void logMotorPowerEvent(std::size_t index, const std::string & state, const std::string & reason);
|
||||
void logMotorDiagnosticEvent(std::size_t index, const std::string & event, const std::string & reason, const char * level = "WARN");
|
||||
void updateMotorTelemetry(std::size_t index, float pos_sim, float vel_sim, float torque_sim, float temperature_c);
|
||||
void handleParameterResponse(const struct can_frame & frame, int bus_id);
|
||||
void updateMotorDiagnostics(std::size_t index);
|
||||
void pollMotorDiagnostics();
|
||||
void finalizeRunSummary();
|
||||
|
||||
rclcpp::Publisher<sim2real_interfaces::msg::RuntimeState>::SharedPtr state_pub_;
|
||||
rclcpp::Subscription<sim2real_interfaces::msg::RuntimeTarget>::SharedPtr target_sub_;
|
||||
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr model_status_sub_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
|
||||
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr estop_sub_;
|
||||
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
|
||||
@@ -83,6 +191,11 @@ private:
|
||||
std::array<float, 16> latest_raw_action_{};
|
||||
std::string latest_target_source_{"boot_hold"};
|
||||
rclcpp::Time latest_target_stamp_{0, 0, RCL_ROS_TIME};
|
||||
std::array<float, 16> rough_default_dof_pos_{};
|
||||
std::array<float, 16> crawl_default_dof_pos_{};
|
||||
std::array<float, 16> active_default_dof_pos_{};
|
||||
ActiveModelMode active_model_mode_{ActiveModelMode::Rough};
|
||||
bool model_switch_active_{false};
|
||||
std::uint32_t target_sequence_{0};
|
||||
std::uint32_t state_sequence_{0};
|
||||
double target_timeout_ms_{150.0};
|
||||
@@ -101,6 +214,7 @@ private:
|
||||
|
||||
// Hold-over constants
|
||||
static constexpr std::uint32_t kHoldoverThreshold = 2;
|
||||
static constexpr std::uint32_t kMotorDropReportThreshold = 40;
|
||||
|
||||
// Motor configurations and states
|
||||
std::array<MotorConfig, 16> motors_;
|
||||
@@ -133,24 +247,71 @@ private:
|
||||
std::unique_ptr<sim2real_common::LowPassFilter> lpf_wheels_;
|
||||
std::unique_ptr<sim2real_common::MahonyFilter> mahony_filter_;
|
||||
std::unique_ptr<sim2real_common::SafetyMonitor> safety_monitor_;
|
||||
std::unique_ptr<sim2real_common::SafetyMonitor> model_switch_safety_monitor_;
|
||||
std::unique_ptr<sim2real_common::RuntimeGuard> runtime_guard_;
|
||||
bool mahony_initialized_{false};
|
||||
rclcpp::Time last_read_time_{0, 0, RCL_ROS_TIME};
|
||||
rclcpp::Time startup_soft_hold_start_time_{0, 0, RCL_ROS_TIME};
|
||||
rclcpp::Time last_diag_poll_time_{0, 0, RCL_ROS_TIME};
|
||||
std::size_t diag_poll_motor_index_{0};
|
||||
|
||||
// Telemetry
|
||||
std::uint32_t fresh_count_{0};
|
||||
std::uint32_t holdover_count_{0};
|
||||
std::uint32_t stale_max_{0};
|
||||
std::uint32_t holdover_events_total_{0};
|
||||
std::uint32_t protection_trigger_count_{0};
|
||||
std::uint32_t motor_drop_event_count_{0};
|
||||
std::uint32_t motor_recover_event_count_{0};
|
||||
std::uint32_t motor_fault_event_count_{0};
|
||||
bool timeout_hold_logged_{false};
|
||||
bool clip_active_logged_{false};
|
||||
|
||||
bool dry_run_{false};
|
||||
std::atomic<bool> estop_triggered_{false};
|
||||
std::atomic<bool> safety_enabled_{true};
|
||||
std::atomic<bool> safety_triggered_{false};
|
||||
std::string safety_reason_{""};
|
||||
sim2real_common::EventLogger event_logger_;
|
||||
std::string run_log_dir_;
|
||||
float motor_temp_warn_c_{100.0f};
|
||||
float motor_temp_fault_c_{135.0f};
|
||||
float motor_bus_overvoltage_v_{60.0f};
|
||||
float motor_bus_undervoltage_v_{12.0f};
|
||||
float motor_current_warn_arms_{10.5f};
|
||||
float motor_current_peak_arms_{14.0f};
|
||||
float motor_torque_warn_nm_{13.0f};
|
||||
float motor_torque_peak_nm_{17.0f};
|
||||
double diag_poll_period_s_{0.10};
|
||||
float wheel_no_effect_command_threshold_{1.0f};
|
||||
float wheel_no_effect_min_response_ratio_{0.20f};
|
||||
float wheel_no_effect_velocity_epsilon_{0.25f};
|
||||
float wheel_no_effect_max_temperature_c_{90.0f};
|
||||
float wheel_no_effect_min_bus_voltage_v_{18.0f};
|
||||
std::uint32_t wheel_no_effect_command_warmup_cycles_{12};
|
||||
std::uint32_t wheel_no_effect_trigger_cycles_{30};
|
||||
std::uint32_t wheel_no_effect_attempt_limit_{2};
|
||||
std::uint32_t wheel_no_effect_cooldown_ms_{1200};
|
||||
std::uint32_t wheel_recovery_verify_timeout_ms_{180};
|
||||
std::uint32_t wheel_no_effect_diag_freshness_ms_{350};
|
||||
std::uint32_t wheel_no_effect_diag_request_period_ms_{80};
|
||||
float leg_no_effect_position_error_threshold_{0.18f};
|
||||
float leg_no_effect_velocity_epsilon_{0.12f};
|
||||
float leg_no_effect_max_estimated_current_arms_{4.0f};
|
||||
float leg_no_effect_max_abs_torque_nm_{5.0f};
|
||||
float leg_no_effect_max_temperature_c_{100.0f};
|
||||
float leg_no_effect_min_bus_voltage_v_{18.0f};
|
||||
std::uint32_t leg_no_effect_command_warmup_cycles_{40};
|
||||
std::uint32_t leg_no_effect_trigger_cycles_{25};
|
||||
std::uint32_t leg_no_effect_attempt_limit_{2};
|
||||
std::uint32_t leg_no_effect_cooldown_ms_{1200};
|
||||
std::uint32_t leg_recovery_verify_timeout_ms_{220};
|
||||
|
||||
void onEstop(const std_msgs::msg::Bool::SharedPtr msg);
|
||||
void logEvent(
|
||||
const std::string & level,
|
||||
const std::string & event,
|
||||
const std::string & message);
|
||||
};
|
||||
|
||||
} // namespace sim2real_hw
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
+155
@@ -0,0 +1,155 @@
|
||||
name: test_route
|
||||
map: map_b
|
||||
frame_id: map
|
||||
yaw_tolerance_deg_default: 30.0
|
||||
segments:
|
||||
- name: segment_1
|
||||
obstacle: slalom
|
||||
waypoints:
|
||||
- id: 1
|
||||
x: 5.3590
|
||||
y: 2.4420
|
||||
yaw_deg: 0.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.15
|
||||
- id: 2
|
||||
x: 6.9240
|
||||
y: 2.3880
|
||||
yaw_deg: -90.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.15
|
||||
- id: 3
|
||||
x: 6.8340
|
||||
y: 0.9200
|
||||
yaw_deg: -5.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.15
|
||||
- id: 4
|
||||
x: 9.3680
|
||||
y: 0.7060
|
||||
yaw_deg: -3.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.15
|
||||
- id: 5
|
||||
x: 11.3730
|
||||
y: 0.4300
|
||||
yaw_deg: 0.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.15
|
||||
- id: 6
|
||||
x: 12.8770
|
||||
y: 0.4320
|
||||
yaw_deg: 90.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.10
|
||||
- id: 7
|
||||
x: 12.9350
|
||||
y: 2.2410
|
||||
yaw_deg: 88.2
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.10
|
||||
- id: 8
|
||||
x: 12.9910
|
||||
y: 4.2770
|
||||
yaw_deg: 180.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.10
|
||||
- id: 9
|
||||
x: 12.0040
|
||||
y: 4.3390
|
||||
yaw_deg: -90.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.15
|
||||
- id: 10
|
||||
x: 11.8720
|
||||
y: 2.5830
|
||||
yaw_deg: 180.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.15
|
||||
- id: 11
|
||||
x: 9.5719
|
||||
y: 2.7972
|
||||
yaw_deg: 174.7
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.15
|
||||
- id: 12
|
||||
x: 9.5080
|
||||
y: 3.5620
|
||||
yaw_deg: 180.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.15
|
||||
- id: 13
|
||||
x: 8.4140
|
||||
y: 3.5730
|
||||
yaw_deg: 125.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.15
|
||||
- id: 14
|
||||
x: 7.8670
|
||||
y: 4.5280
|
||||
yaw_deg: 180.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.15
|
||||
- id: 15
|
||||
x: 5.8060
|
||||
y: 4.7430
|
||||
yaw_deg: 180.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.15
|
||||
- id: 16
|
||||
x: 4.9260
|
||||
y: 4.7850
|
||||
yaw_deg: 180.0
|
||||
speed: 0.35
|
||||
policy: ik
|
||||
tolerance: 0.10
|
||||
- id: 17
|
||||
x: 3.2951
|
||||
y: 4.7963
|
||||
yaw_deg: 179.6
|
||||
speed: 0.35
|
||||
policy: ik
|
||||
tolerance: 0.10
|
||||
- id: 18
|
||||
x: 3.0920
|
||||
y: 4.7770
|
||||
yaw_deg: -180.0
|
||||
speed: 0.35
|
||||
policy: ik
|
||||
tolerance: 0.10
|
||||
- id: 19
|
||||
x: 1.6940
|
||||
y: 4.8860
|
||||
yaw_deg: 210.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.10
|
||||
- id: 20
|
||||
x: 1.0550
|
||||
y: 4.5480
|
||||
yaw_deg: -90.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.10
|
||||
- id: 21
|
||||
x: 1.0420
|
||||
y: 2.9200
|
||||
yaw_deg: -90.4
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.10
|
||||
+6
-6
@@ -3,14 +3,14 @@ goals:
|
||||
dock:
|
||||
position: [0.0, 0.0, 0.0]
|
||||
yaw_deg: 0.0
|
||||
description: "起始点/回充点"
|
||||
description: "dock"
|
||||
|
||||
test_p1:
|
||||
position: [1.5, 0.0, 0.0]
|
||||
position: [4.73, 1.75, 0.0]
|
||||
yaw_deg: 0.0
|
||||
description: "测试点1"
|
||||
description: "test point 1"
|
||||
|
||||
test_p2:
|
||||
position: [1.5, -1.0, 0.0]
|
||||
yaw_deg: -90.0
|
||||
description: "测试点2"
|
||||
position: [4.94, 0.40, 0.0]
|
||||
yaw_deg: 0.0
|
||||
description: "test point 2"
|
||||
|
||||
+2
-3
@@ -1,9 +1,8 @@
|
||||
missions:
|
||||
test_roundtrip:
|
||||
description: "测试点1到点2再回起点"
|
||||
description: "test_p1 -> test_p2 -> test_p1"
|
||||
loop: false
|
||||
goals:
|
||||
- dock
|
||||
- test_p1
|
||||
- test_p2
|
||||
- dock
|
||||
- test_p1
|
||||
|
||||
@@ -133,7 +133,7 @@ planner_server:
|
||||
GridTransition:
|
||||
plugin: "nav2_navfn_planner/NavfnPlanner"
|
||||
tolerance: 0.5
|
||||
use_astar: false
|
||||
use_astar: true
|
||||
allow_unknown: true
|
||||
|
||||
behavior_server:
|
||||
|
||||
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.8)
|
||||
project(sim2real_runtime)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(CUDAToolkit QUIET)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
@@ -31,10 +32,33 @@ find_library(ONNXRUNTIME_LIBRARY NAMES onnxruntime
|
||||
|
||||
get_filename_component(ONNXRUNTIME_LIBRARY_DIR ${ONNXRUNTIME_LIBRARY} DIRECTORY)
|
||||
|
||||
find_path(TENSORRT_INCLUDE_DIR NvInfer.h
|
||||
PATHS
|
||||
/usr/include
|
||||
/usr/include/aarch64-linux-gnu
|
||||
/usr/local/include
|
||||
)
|
||||
|
||||
find_library(TENSORRT_LIBRARY NAMES nvinfer
|
||||
PATHS
|
||||
/usr/lib
|
||||
/usr/lib/aarch64-linux-gnu
|
||||
/usr/lib/x86_64-linux-gnu
|
||||
/usr/local/lib
|
||||
)
|
||||
|
||||
if(NOT ONNXRUNTIME_INCLUDE_DIR OR NOT ONNXRUNTIME_LIBRARY)
|
||||
message(FATAL_ERROR "ONNX Runtime not found! Please install it or specify include/library paths.")
|
||||
endif()
|
||||
|
||||
set(SIM2REAL_RUNTIME_HAS_TENSORRT FALSE)
|
||||
if(TENSORRT_INCLUDE_DIR AND TENSORRT_LIBRARY AND CUDAToolkit_FOUND)
|
||||
set(SIM2REAL_RUNTIME_HAS_TENSORRT TRUE)
|
||||
message(STATUS "TensorRT support enabled for sim2real_runtime")
|
||||
else()
|
||||
message(STATUS "TensorRT support disabled for sim2real_runtime (missing TensorRT or CUDA toolkit)")
|
||||
endif()
|
||||
|
||||
add_executable(sim2real_runtime_node
|
||||
src/policy_runtime_node.cpp
|
||||
)
|
||||
@@ -43,23 +67,60 @@ add_executable(odom_relay_node
|
||||
src/odom_relay_node.cpp
|
||||
)
|
||||
|
||||
if(SIM2REAL_RUNTIME_HAS_TENSORRT)
|
||||
add_executable(compare_onnx_trt
|
||||
src/compare_onnx_trt.cpp
|
||||
)
|
||||
endif()
|
||||
|
||||
target_include_directories(sim2real_runtime_node PRIVATE
|
||||
include
|
||||
${ONNXRUNTIME_INCLUDE_DIR}
|
||||
)
|
||||
|
||||
if(SIM2REAL_RUNTIME_HAS_TENSORRT)
|
||||
target_include_directories(sim2real_runtime_node PRIVATE
|
||||
${TENSORRT_INCLUDE_DIR}
|
||||
)
|
||||
target_include_directories(compare_onnx_trt PRIVATE
|
||||
${TENSORRT_INCLUDE_DIR}
|
||||
${ONNXRUNTIME_INCLUDE_DIR}
|
||||
)
|
||||
endif()
|
||||
|
||||
target_include_directories(odom_relay_node PRIVATE include)
|
||||
|
||||
target_link_libraries(sim2real_runtime_node
|
||||
${ONNXRUNTIME_LIBRARY}
|
||||
)
|
||||
|
||||
if(SIM2REAL_RUNTIME_HAS_TENSORRT)
|
||||
target_link_libraries(sim2real_runtime_node
|
||||
${TENSORRT_LIBRARY}
|
||||
CUDA::cudart
|
||||
)
|
||||
target_link_libraries(compare_onnx_trt
|
||||
${ONNXRUNTIME_LIBRARY}
|
||||
${TENSORRT_LIBRARY}
|
||||
CUDA::cudart
|
||||
)
|
||||
target_compile_definitions(sim2real_runtime_node PRIVATE
|
||||
SIM2REAL_RUNTIME_HAS_TENSORRT=1
|
||||
)
|
||||
target_compile_definitions(compare_onnx_trt PRIVATE
|
||||
SIM2REAL_RUNTIME_HAS_TENSORRT=1
|
||||
)
|
||||
endif()
|
||||
|
||||
set_target_properties(sim2real_runtime_node PROPERTIES
|
||||
BUILD_RPATH "${ONNXRUNTIME_LIBRARY_DIR}"
|
||||
INSTALL_RPATH "${ONNXRUNTIME_LIBRARY_DIR}"
|
||||
)
|
||||
|
||||
target_compile_features(sim2real_runtime_node PRIVATE cxx_std_17)
|
||||
if(SIM2REAL_RUNTIME_HAS_TENSORRT)
|
||||
target_compile_features(compare_onnx_trt PRIVATE cxx_std_17)
|
||||
endif()
|
||||
|
||||
ament_target_dependencies(sim2real_runtime_node
|
||||
geometry_msgs
|
||||
@@ -71,6 +132,12 @@ ament_target_dependencies(sim2real_runtime_node
|
||||
sim2real_interfaces
|
||||
)
|
||||
|
||||
if(SIM2REAL_RUNTIME_HAS_TENSORRT)
|
||||
ament_target_dependencies(compare_onnx_trt
|
||||
sim2real_common
|
||||
)
|
||||
endif()
|
||||
|
||||
ament_target_dependencies(odom_relay_node
|
||||
geometry_msgs
|
||||
nav_msgs
|
||||
@@ -88,6 +155,13 @@ install(
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
if(SIM2REAL_RUNTIME_HAS_TENSORRT)
|
||||
install(
|
||||
TARGETS compare_onnx_trt
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
endif()
|
||||
|
||||
install(
|
||||
PROGRAMS
|
||||
src/remote_uart_node.py
|
||||
|
||||
+133
-7
@@ -5,15 +5,18 @@
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <cstdint>
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
|
||||
#include "geometry_msgs/msg/twist.hpp"
|
||||
#include "geometry_msgs/msg/twist_stamped.hpp"
|
||||
#include "std_msgs/msg/bool.hpp"
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "sim2real_interfaces/msg/runtime_state.hpp"
|
||||
#include "sim2real_interfaces/msg/runtime_target.hpp"
|
||||
#include "sim2real_common/event_logger.hpp"
|
||||
#include "sim2real_common/stand_balance_controller.hpp"
|
||||
#include "sim2real_common/safety_monitor.hpp"
|
||||
#include "sim2real_common/runtime_guard.hpp"
|
||||
@@ -21,6 +24,11 @@
|
||||
// ONNXRuntime C++ API
|
||||
#include <onnxruntime_cxx_api.h>
|
||||
|
||||
#ifdef SIM2REAL_RUNTIME_HAS_TENSORRT
|
||||
#include <NvInfer.h>
|
||||
#include <cuda_runtime_api.h>
|
||||
#endif
|
||||
|
||||
namespace sim2real_runtime
|
||||
{
|
||||
|
||||
@@ -28,13 +36,70 @@ class PolicyRuntimeNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
PolicyRuntimeNode();
|
||||
~PolicyRuntimeNode() override;
|
||||
|
||||
private:
|
||||
enum class InferenceBackend {
|
||||
None,
|
||||
TensorRT,
|
||||
OnnxRuntime,
|
||||
};
|
||||
|
||||
enum class ModelMode {
|
||||
Rough,
|
||||
Crawl,
|
||||
};
|
||||
|
||||
enum class CrawlBackend {
|
||||
Ik,
|
||||
Rl,
|
||||
};
|
||||
|
||||
enum class ModelSwitchState {
|
||||
Idle,
|
||||
ToStand,
|
||||
StandHold,
|
||||
ToModelPose,
|
||||
};
|
||||
|
||||
enum class StartupState {
|
||||
BOOT_HOLD,
|
||||
STARTUP_SOFT_HOLD,
|
||||
STARTUP_TRANSITION,
|
||||
STARTUP_HOLD_AFTER,
|
||||
RUNTIME
|
||||
};
|
||||
|
||||
void onState(const sim2real_interfaces::msg::RuntimeState::SharedPtr msg);
|
||||
void onCmdVel(const geometry_msgs::msg::Twist::SharedPtr msg);
|
||||
void onCmdVelStamped(const geometry_msgs::msg::TwistStamped::SharedPtr msg);
|
||||
void onModelSwitchCmd(const std_msgs::msg::String::SharedPtr msg);
|
||||
void applyCmdVel(float vx, float vy, float vyaw);
|
||||
void onPolicyLoop();
|
||||
bool initInferenceBackend();
|
||||
bool initTensorRt();
|
||||
bool initOnnxRuntime();
|
||||
void shutdownOnnxRuntime();
|
||||
void shutdownTensorRt();
|
||||
std::string deriveTensorRtEnginePath(const std::string & onnx_model_path) const;
|
||||
bool switchInferenceModel(ModelMode target_mode);
|
||||
void publishModelStatus();
|
||||
bool modeUsesInference(ModelMode mode) const;
|
||||
std::array<float, 16> computeHoldTarget(
|
||||
const sim2real_interfaces::msg::RuntimeState & state,
|
||||
const std::array<float, 3> & cmd);
|
||||
std::array<float, 16> computeIkCrawlTarget(
|
||||
const sim2real_interfaces::msg::RuntimeState & state,
|
||||
const std::array<float, 3> & cmd);
|
||||
const char * startupStateName(StartupState state) const;
|
||||
const char * modelModeName(ModelMode mode) const;
|
||||
const char * modelSwitchStateName(ModelSwitchState state) const;
|
||||
const char * inferenceBackendName() const;
|
||||
const char * crawlBackendName() const;
|
||||
void initializeDebugTrace();
|
||||
void appendDebugTrace(
|
||||
const sim2real_interfaces::msg::RuntimeState & state,
|
||||
const sim2real_interfaces::msg::RuntimeTarget & target);
|
||||
|
||||
std::array<float, 53> buildObservation(
|
||||
const sim2real_interfaces::msg::RuntimeState & state,
|
||||
@@ -46,9 +111,11 @@ private:
|
||||
bool isCommandActive(const std::array<float, 3> & cmd) const;
|
||||
|
||||
rclcpp::Publisher<sim2real_interfaces::msg::RuntimeTarget>::SharedPtr target_pub_;
|
||||
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr model_status_pub_;
|
||||
rclcpp::Subscription<sim2real_interfaces::msg::RuntimeState>::SharedPtr state_sub_;
|
||||
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub_;
|
||||
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr cmd_stamped_sub_;
|
||||
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr model_switch_sub_;
|
||||
rclcpp::TimerBase::SharedPtr policy_timer_;
|
||||
|
||||
std::mutex mutex_;
|
||||
@@ -61,13 +128,6 @@ private:
|
||||
std::uint32_t sequence_{0};
|
||||
|
||||
// Startup State Machine
|
||||
enum class StartupState {
|
||||
BOOT_HOLD,
|
||||
STARTUP_SOFT_HOLD,
|
||||
STARTUP_TRANSITION,
|
||||
STARTUP_HOLD_AFTER,
|
||||
RUNTIME
|
||||
};
|
||||
StartupState startup_state_{StartupState::BOOT_HOLD};
|
||||
std::array<float, 16> start_pose_{};
|
||||
std::array<float, 16> startup_delta_{};
|
||||
@@ -76,9 +136,43 @@ private:
|
||||
double hold_time_{1.0};
|
||||
std::unique_ptr<sim2real_common::StandBalanceController> stand_balance_;
|
||||
|
||||
ModelMode current_model_mode_{ModelMode::Rough};
|
||||
ModelMode requested_model_mode_{ModelMode::Rough};
|
||||
ModelMode loaded_model_mode_{ModelMode::Rough};
|
||||
CrawlBackend crawl_backend_{CrawlBackend::Ik};
|
||||
ModelSwitchState model_switch_state_{ModelSwitchState::Idle};
|
||||
bool model_switch_requested_{false};
|
||||
bool hold_active_model_pose_when_unreleased_{false};
|
||||
std::array<float, 16> rough_default_dof_pos_{};
|
||||
std::array<float, 16> crawl_default_dof_pos_{};
|
||||
std::array<float, 16> active_default_dof_pos_{};
|
||||
std::array<float, 16> safety_reference_dof_pos_{};
|
||||
std::array<float, 16> switch_start_pose_{};
|
||||
std::array<float, 16> switch_delta_{};
|
||||
rclcpp::Time model_switch_state_start_time_{0, 0, RCL_ROS_TIME};
|
||||
double model_switch_transition_s_{1.5};
|
||||
double model_switch_to_stand_transition_scale_{2.0};
|
||||
double model_switch_to_model_transition_scale_{2.5};
|
||||
double model_switch_stand_hold_s_{0.8};
|
||||
double model_switch_stand_max_err_{0.18};
|
||||
double model_switch_stand_max_vel_{0.8};
|
||||
double active_switch_transition_s_{1.5};
|
||||
|
||||
std::string rough_model_path_{"policies/model_rough.onnx"};
|
||||
std::string rough_model_engine_path_{""};
|
||||
std::string crawl_model_path_{"policies/model_crawl.onnx"};
|
||||
std::string crawl_model_engine_path_{""};
|
||||
float crawl_ik_wheel_linear_gain_{6.25f};
|
||||
float crawl_ik_wheel_yaw_gain_{4.0f};
|
||||
float crawl_ik_max_wheel_speed_{6.0f};
|
||||
float crawl_ik_abduction_clip_{0.45f};
|
||||
|
||||
// ONNX Runtime members
|
||||
std::string model_path_{"policies/model_rough.onnx"};
|
||||
std::string model_engine_path_{""};
|
||||
bool prefer_tensorrt_{true};
|
||||
bool use_cuda_{false}; // enable CUDA Execution Provider on Orin Nano
|
||||
InferenceBackend inference_backend_{InferenceBackend::None};
|
||||
std::unique_ptr<Ort::Env> env_;
|
||||
std::unique_ptr<Ort::Session> session_;
|
||||
std::unique_ptr<Ort::MemoryInfo> memory_info_;
|
||||
@@ -91,6 +185,17 @@ private:
|
||||
std::vector<std::int64_t> input_shape_;
|
||||
std::vector<std::int64_t> output_shape_;
|
||||
|
||||
#ifdef SIM2REAL_RUNTIME_HAS_TENSORRT
|
||||
nvinfer1::IRuntime * trt_runtime_{nullptr};
|
||||
nvinfer1::ICudaEngine * trt_engine_{nullptr};
|
||||
nvinfer1::IExecutionContext * trt_context_{nullptr};
|
||||
cudaStream_t trt_stream_{nullptr};
|
||||
void * trt_input_buffer_{nullptr};
|
||||
void * trt_output_buffer_{nullptr};
|
||||
std::string trt_input_name_;
|
||||
std::string trt_output_name_;
|
||||
#endif
|
||||
|
||||
// Command filter and release states
|
||||
std::array<float, 3> filtered_cmd_{{0.0f, 0.0f, 0.0f}};
|
||||
float release_alpha_{0.0f};
|
||||
@@ -98,12 +203,14 @@ private:
|
||||
float release_command_hold_s_{0.12f};
|
||||
float release_posture_max_err_{0.35f};
|
||||
float release_target_blend_s_{0.30f};
|
||||
float model_switch_release_scale_{2.0f};
|
||||
float clip_obs_{100.0f};
|
||||
bool hold_zero_command_pose_{true};
|
||||
bool enable_zero_cmd_suppression_{true};
|
||||
bool require_active_command_to_release_{true};
|
||||
bool zero_cmd_use_yaw_rate_{false};
|
||||
bool runtime_released_{false};
|
||||
bool slow_release_after_model_switch_{false};
|
||||
float release_active_time_{0.0f};
|
||||
float zero_cmd_lin_thresh_{0.05f};
|
||||
float zero_cmd_yaw_thresh_{0.05f};
|
||||
@@ -115,10 +222,29 @@ private:
|
||||
std::atomic<bool> safety_enabled_{true};
|
||||
std::atomic<bool> safety_triggered_{false};
|
||||
std::string safety_reason_{""};
|
||||
sim2real_common::EventLogger event_logger_;
|
||||
std::string run_log_dir_;
|
||||
std::string debug_trace_path_;
|
||||
bool debug_trace_enabled_{true};
|
||||
std::uint32_t debug_trace_decimation_{1};
|
||||
std::uint32_t debug_trace_counter_{0};
|
||||
std::uint32_t protection_trigger_count_{0};
|
||||
std::uint32_t target_clip_count_{0};
|
||||
bool clip_active_logged_{false};
|
||||
std::unique_ptr<sim2real_common::SafetyMonitor> safety_monitor_;
|
||||
std::unique_ptr<sim2real_common::SafetyMonitor> model_switch_safety_monitor_;
|
||||
std::unique_ptr<sim2real_common::RuntimeGuard> runtime_guard_;
|
||||
|
||||
void onEstop(const std_msgs::msg::Bool::SharedPtr msg);
|
||||
void logEvent(
|
||||
const std::string & level,
|
||||
const std::string & event,
|
||||
const std::string & message);
|
||||
void logProtectionEvent(
|
||||
const std::string & trigger,
|
||||
const std::string & reason,
|
||||
const std::string & action);
|
||||
void finalizeRunSummary();
|
||||
};
|
||||
|
||||
} // namespace sim2real_runtime
|
||||
|
||||
@@ -0,0 +1,330 @@
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <cstdlib>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
#include <numeric>
|
||||
#include <random>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <onnxruntime_cxx_api.h>
|
||||
|
||||
#include <NvInfer.h>
|
||||
#include <cuda_runtime_api.h>
|
||||
|
||||
#include "sim2real_common/deployment_contract.hpp"
|
||||
|
||||
namespace
|
||||
{
|
||||
|
||||
class TensorRtLogger final : public nvinfer1::ILogger
|
||||
{
|
||||
public:
|
||||
void log(Severity severity, const char * msg) noexcept override
|
||||
{
|
||||
if (msg == nullptr) {
|
||||
return;
|
||||
}
|
||||
if (severity <= Severity::kWARNING) {
|
||||
std::cerr << "[TensorRT] " << msg << std::endl;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
void destroyTensorRtObject(T *& object)
|
||||
{
|
||||
if (object == nullptr) {
|
||||
return;
|
||||
}
|
||||
#if NV_TENSORRT_MAJOR >= 10
|
||||
delete object;
|
||||
#else
|
||||
object->destroy();
|
||||
#endif
|
||||
object = nullptr;
|
||||
}
|
||||
|
||||
class TensorRtRunner
|
||||
{
|
||||
public:
|
||||
explicit TensorRtRunner(const std::string & engine_path)
|
||||
{
|
||||
std::ifstream engine_file(engine_path, std::ios::binary);
|
||||
if (!engine_file) {
|
||||
throw std::runtime_error("Failed to open TensorRT engine: " + engine_path);
|
||||
}
|
||||
engine_file.seekg(0, std::ios::end);
|
||||
const std::streamsize engine_size = engine_file.tellg();
|
||||
engine_file.seekg(0, std::ios::beg);
|
||||
if (engine_size <= 0) {
|
||||
throw std::runtime_error("TensorRT engine is empty: " + engine_path);
|
||||
}
|
||||
|
||||
std::vector<char> engine_data(static_cast<std::size_t>(engine_size));
|
||||
if (!engine_file.read(engine_data.data(), engine_size)) {
|
||||
throw std::runtime_error("Failed to read TensorRT engine: " + engine_path);
|
||||
}
|
||||
|
||||
runtime_ = nvinfer1::createInferRuntime(logger_);
|
||||
if (runtime_ == nullptr) {
|
||||
throw std::runtime_error("Failed to create TensorRT runtime");
|
||||
}
|
||||
|
||||
engine_ = runtime_->deserializeCudaEngine(engine_data.data(), engine_data.size());
|
||||
if (engine_ == nullptr) {
|
||||
throw std::runtime_error("Failed to deserialize TensorRT engine");
|
||||
}
|
||||
|
||||
context_ = engine_->createExecutionContext();
|
||||
if (context_ == nullptr) {
|
||||
throw std::runtime_error("Failed to create TensorRT execution context");
|
||||
}
|
||||
|
||||
for (int i = 0; i < engine_->getNbIOTensors(); ++i) {
|
||||
const char * tensor_name = engine_->getIOTensorName(i);
|
||||
if (engine_->getTensorIOMode(tensor_name) == nvinfer1::TensorIOMode::kINPUT) {
|
||||
input_name_ = tensor_name;
|
||||
} else {
|
||||
output_name_ = tensor_name;
|
||||
}
|
||||
}
|
||||
|
||||
if (input_name_.empty() || output_name_.empty()) {
|
||||
throw std::runtime_error("Failed to resolve TensorRT IO tensor names");
|
||||
}
|
||||
|
||||
if (!context_->setInputShape(
|
||||
input_name_.c_str(),
|
||||
nvinfer1::Dims2{1, static_cast<int>(sim2real_common::DeploymentContract::kObsDim)})) {
|
||||
throw std::runtime_error("Failed to set TensorRT input shape");
|
||||
}
|
||||
|
||||
if (cudaStreamCreate(&stream_) != cudaSuccess) {
|
||||
throw std::runtime_error("Failed to create CUDA stream");
|
||||
}
|
||||
|
||||
const std::size_t input_bytes = sizeof(float) * sim2real_common::DeploymentContract::kObsDim;
|
||||
const std::size_t output_bytes = sizeof(float) * sim2real_common::DeploymentContract::kActionDim;
|
||||
if (cudaMalloc(&input_buffer_, input_bytes) != cudaSuccess ||
|
||||
cudaMalloc(&output_buffer_, output_bytes) != cudaSuccess) {
|
||||
throw std::runtime_error("Failed to allocate TensorRT buffers");
|
||||
}
|
||||
|
||||
if (!context_->setTensorAddress(input_name_.c_str(), input_buffer_) ||
|
||||
!context_->setTensorAddress(output_name_.c_str(), output_buffer_)) {
|
||||
throw std::runtime_error("Failed to bind TensorRT buffers");
|
||||
}
|
||||
}
|
||||
|
||||
~TensorRtRunner()
|
||||
{
|
||||
if (input_buffer_ != nullptr) {
|
||||
cudaFree(input_buffer_);
|
||||
}
|
||||
if (output_buffer_ != nullptr) {
|
||||
cudaFree(output_buffer_);
|
||||
}
|
||||
if (stream_ != nullptr) {
|
||||
cudaStreamDestroy(stream_);
|
||||
}
|
||||
destroyTensorRtObject(context_);
|
||||
destroyTensorRtObject(engine_);
|
||||
destroyTensorRtObject(runtime_);
|
||||
}
|
||||
|
||||
std::array<float, sim2real_common::DeploymentContract::kActionDim> run(
|
||||
const std::array<float, sim2real_common::DeploymentContract::kObsDim> & obs)
|
||||
{
|
||||
std::array<float, sim2real_common::DeploymentContract::kActionDim> out{};
|
||||
const std::size_t input_bytes = sizeof(float) * obs.size();
|
||||
const std::size_t output_bytes = sizeof(float) * out.size();
|
||||
|
||||
if (cudaMemcpyAsync(input_buffer_, obs.data(), input_bytes, cudaMemcpyHostToDevice, stream_) != cudaSuccess) {
|
||||
throw std::runtime_error("TensorRT H2D copy failed");
|
||||
}
|
||||
if (!context_->enqueueV3(stream_)) {
|
||||
throw std::runtime_error("TensorRT enqueue failed");
|
||||
}
|
||||
if (cudaMemcpyAsync(out.data(), output_buffer_, output_bytes, cudaMemcpyDeviceToHost, stream_) != cudaSuccess) {
|
||||
throw std::runtime_error("TensorRT D2H copy failed");
|
||||
}
|
||||
if (cudaStreamSynchronize(stream_) != cudaSuccess) {
|
||||
throw std::runtime_error("TensorRT stream sync failed");
|
||||
}
|
||||
return out;
|
||||
}
|
||||
|
||||
private:
|
||||
TensorRtLogger logger_;
|
||||
nvinfer1::IRuntime * runtime_{nullptr};
|
||||
nvinfer1::ICudaEngine * engine_{nullptr};
|
||||
nvinfer1::IExecutionContext * context_{nullptr};
|
||||
cudaStream_t stream_{nullptr};
|
||||
void * input_buffer_{nullptr};
|
||||
void * output_buffer_{nullptr};
|
||||
std::string input_name_;
|
||||
std::string output_name_;
|
||||
};
|
||||
|
||||
class OnnxRunner
|
||||
{
|
||||
public:
|
||||
explicit OnnxRunner(const std::string & onnx_path)
|
||||
: env_(ORT_LOGGING_LEVEL_WARNING, "compare_onnx_trt")
|
||||
{
|
||||
Ort::SessionOptions session_options;
|
||||
session_options.SetIntraOpNumThreads(1);
|
||||
session_options.SetInterOpNumThreads(1);
|
||||
session_options.SetGraphOptimizationLevel(GraphOptimizationLevel::ORT_ENABLE_ALL);
|
||||
|
||||
session_ = std::make_unique<Ort::Session>(env_, onnx_path.c_str(), session_options);
|
||||
memory_info_ = std::make_unique<Ort::MemoryInfo>(
|
||||
Ort::MemoryInfo::CreateCpu(OrtDeviceAllocator, OrtMemTypeCPU));
|
||||
|
||||
Ort::AllocatorWithDefaultOptions allocator;
|
||||
auto input_name = session_->GetInputNameAllocated(0, allocator);
|
||||
auto output_name = session_->GetOutputNameAllocated(0, allocator);
|
||||
input_name_ = input_name.get();
|
||||
output_name_ = output_name.get();
|
||||
input_name_ptr_ = input_name_.c_str();
|
||||
output_name_ptr_ = output_name_.c_str();
|
||||
|
||||
input_shape_ = {1, static_cast<std::int64_t>(sim2real_common::DeploymentContract::kObsDim)};
|
||||
output_shape_ = {1, static_cast<std::int64_t>(sim2real_common::DeploymentContract::kActionDim)};
|
||||
}
|
||||
|
||||
std::array<float, sim2real_common::DeploymentContract::kActionDim> run(
|
||||
const std::array<float, sim2real_common::DeploymentContract::kObsDim> & obs)
|
||||
{
|
||||
std::array<float, sim2real_common::DeploymentContract::kActionDim> out{};
|
||||
|
||||
auto input_tensor = Ort::Value::CreateTensor<float>(
|
||||
*memory_info_,
|
||||
const_cast<float *>(obs.data()),
|
||||
obs.size(),
|
||||
input_shape_.data(),
|
||||
input_shape_.size());
|
||||
|
||||
auto output_tensor = Ort::Value::CreateTensor<float>(
|
||||
*memory_info_,
|
||||
out.data(),
|
||||
out.size(),
|
||||
output_shape_.data(),
|
||||
output_shape_.size());
|
||||
|
||||
session_->Run(
|
||||
Ort::RunOptions{nullptr},
|
||||
&input_name_ptr_,
|
||||
&input_tensor,
|
||||
1,
|
||||
&output_name_ptr_,
|
||||
&output_tensor,
|
||||
1);
|
||||
|
||||
return out;
|
||||
}
|
||||
|
||||
private:
|
||||
Ort::Env env_;
|
||||
std::unique_ptr<Ort::Session> session_;
|
||||
std::unique_ptr<Ort::MemoryInfo> memory_info_;
|
||||
std::string input_name_;
|
||||
std::string output_name_;
|
||||
const char * input_name_ptr_{nullptr};
|
||||
const char * output_name_ptr_{nullptr};
|
||||
std::array<std::int64_t, 2> input_shape_{};
|
||||
std::array<std::int64_t, 2> output_shape_{};
|
||||
};
|
||||
|
||||
struct DiffStats
|
||||
{
|
||||
double max_abs_diff{0.0};
|
||||
double mean_abs_diff{0.0};
|
||||
};
|
||||
|
||||
DiffStats compareOutputs(
|
||||
const std::array<float, sim2real_common::DeploymentContract::kActionDim> & a,
|
||||
const std::array<float, sim2real_common::DeploymentContract::kActionDim> & b)
|
||||
{
|
||||
DiffStats stats;
|
||||
double sum = 0.0;
|
||||
for (std::size_t i = 0; i < a.size(); ++i) {
|
||||
const double diff = std::abs(static_cast<double>(a[i]) - static_cast<double>(b[i]));
|
||||
stats.max_abs_diff = std::max(stats.max_abs_diff, diff);
|
||||
sum += diff;
|
||||
}
|
||||
stats.mean_abs_diff = sum / static_cast<double>(a.size());
|
||||
return stats;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
if (argc < 3) {
|
||||
std::cerr << "Usage: compare_onnx_trt <model.onnx> <model.engine> [num_samples] [seed] [max_abs_threshold]\n";
|
||||
return 2;
|
||||
}
|
||||
|
||||
const std::string onnx_path = argv[1];
|
||||
const std::string engine_path = argv[2];
|
||||
const int num_samples = argc >= 4 ? std::max(1, std::atoi(argv[3])) : 32;
|
||||
const std::uint32_t seed = argc >= 5 ? static_cast<std::uint32_t>(std::strtoul(argv[4], nullptr, 10)) : 12345U;
|
||||
const double max_abs_threshold = argc >= 6 ? std::atof(argv[5]) : 1.0e-2;
|
||||
|
||||
try {
|
||||
OnnxRunner onnx_runner(onnx_path);
|
||||
TensorRtRunner trt_runner(engine_path);
|
||||
|
||||
std::mt19937 rng(seed);
|
||||
std::uniform_real_distribution<float> dist(-1.0f, 1.0f);
|
||||
|
||||
double worst_max_abs = 0.0;
|
||||
double worst_mean_abs = 0.0;
|
||||
int worst_sample = -1;
|
||||
|
||||
for (int sample_idx = 0; sample_idx < num_samples; ++sample_idx) {
|
||||
std::array<float, sim2real_common::DeploymentContract::kObsDim> obs{};
|
||||
for (float & v : obs) {
|
||||
v = dist(rng);
|
||||
}
|
||||
|
||||
const auto onnx_out = onnx_runner.run(obs);
|
||||
const auto trt_out = trt_runner.run(obs);
|
||||
const auto diff = compareOutputs(onnx_out, trt_out);
|
||||
|
||||
if (diff.max_abs_diff > worst_max_abs) {
|
||||
worst_max_abs = diff.max_abs_diff;
|
||||
worst_mean_abs = diff.mean_abs_diff;
|
||||
worst_sample = sample_idx;
|
||||
}
|
||||
|
||||
std::cout << "sample=" << sample_idx
|
||||
<< " max_abs_diff=" << diff.max_abs_diff
|
||||
<< " mean_abs_diff=" << diff.mean_abs_diff
|
||||
<< '\n';
|
||||
}
|
||||
|
||||
std::cout << "summary worst_sample=" << worst_sample
|
||||
<< " worst_max_abs_diff=" << worst_max_abs
|
||||
<< " worst_mean_abs_diff=" << worst_mean_abs
|
||||
<< " threshold=" << max_abs_threshold
|
||||
<< '\n';
|
||||
|
||||
if (worst_max_abs > max_abs_threshold) {
|
||||
std::cerr << "FAILED: TensorRT output deviates from ONNX Runtime beyond threshold.\n";
|
||||
return 1;
|
||||
}
|
||||
|
||||
std::cout << "PASSED: TensorRT output matches ONNX Runtime within threshold.\n";
|
||||
return 0;
|
||||
} catch (const std::exception & e) {
|
||||
std::cerr << "ERROR: " << e.what() << '\n';
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
+1232
-132
File diff suppressed because it is too large
Load Diff
@@ -10,7 +10,7 @@ import rclpy
|
||||
from geometry_msgs.msg import Twist
|
||||
from rclpy.executors import ExternalShutdownException
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import Bool
|
||||
from std_msgs.msg import Bool, String
|
||||
|
||||
SBUS_FRAME_SIZE = 25
|
||||
SBUS_RC_MID = 1024
|
||||
@@ -23,7 +23,22 @@ SWITCH_HIGH = 1
|
||||
|
||||
@dataclass
|
||||
class RemoteSwitchState:
|
||||
ch5: int = SWITCH_MID
|
||||
ch6: int = SWITCH_MID
|
||||
ch7: int = SWITCH_MID
|
||||
ch8: int = SWITCH_MID
|
||||
ch9: int = SWITCH_MID
|
||||
ch10: int = SWITCH_MID
|
||||
|
||||
def get(self, channel: int) -> Optional[int]:
|
||||
return {
|
||||
5: self.ch5,
|
||||
6: self.ch6,
|
||||
7: self.ch7,
|
||||
8: self.ch8,
|
||||
9: self.ch9,
|
||||
10: self.ch10,
|
||||
}.get(int(channel))
|
||||
|
||||
|
||||
@dataclass
|
||||
@@ -114,7 +129,14 @@ class SbusUartReceiver:
|
||||
ch2=self._normalize_axis(channels[1]),
|
||||
ch3=self._normalize_axis(channels[3]),
|
||||
ch4=self._normalize_axis(channels[2]),
|
||||
switches=RemoteSwitchState(ch7=self._decode_switch(channels[6])),
|
||||
switches=RemoteSwitchState(
|
||||
ch5=self._decode_switch(channels[4]),
|
||||
ch6=self._decode_switch(channels[5]),
|
||||
ch7=self._decode_switch(channels[6]),
|
||||
ch8=self._decode_switch(channels[7]),
|
||||
ch9=self._decode_switch(channels[8]),
|
||||
ch10=self._decode_switch(channels[9]),
|
||||
),
|
||||
frame_ok=True,
|
||||
)
|
||||
if any(abs(value) > 800 for value in (state.ch1, state.ch2, state.ch3, state.ch4)):
|
||||
@@ -154,12 +176,31 @@ class RemoteUartNode(Node):
|
||||
self.publish_inactive_zero = bool(self.declare_parameter("remote_publish_inactive_zero", True).value)
|
||||
self.estop_latch = bool(self.declare_parameter("remote_estop_latch", True).value)
|
||||
self.poll_hz = float(self.declare_parameter("remote_poll_hz", 50.0).value)
|
||||
self.default_mode = str(self.declare_parameter("cmd_mux_default_mode", "REMOTE").value).strip().upper()
|
||||
self.model_switch_enabled = bool(self.declare_parameter("remote_model_switch_enabled", True).value)
|
||||
self.model_switch_channel = int(self.declare_parameter("remote_model_switch_channel", 10).value)
|
||||
self.model_switch_debounce_frames = max(int(self.declare_parameter("remote_model_switch_debounce_frames", 3).value), 1)
|
||||
self.model_switch_rough_level = self.parse_switch_level(
|
||||
str(self.declare_parameter("remote_model_switch_rough_level", "low").value)
|
||||
)
|
||||
legacy_ik_level = str(self.declare_parameter("remote_model_switch_crawl_level", "").value).strip()
|
||||
ik_level_default = legacy_ik_level if legacy_ik_level else "high"
|
||||
self.model_switch_ik_level = self.parse_switch_level(
|
||||
str(self.declare_parameter("remote_model_switch_ik_level", ik_level_default).value)
|
||||
)
|
||||
|
||||
self.cmd_pub = self.create_publisher(Twist, "cmd_vel_remote", 10)
|
||||
self.estop_pub = self.create_publisher(Bool, "/safety/estop", 10)
|
||||
self.model_cmd_pub = self.create_publisher(String, "runtime/model_cmd", 10)
|
||||
self.receiver: Optional[SbusUartReceiver] = None
|
||||
self.estop_published = False
|
||||
self.open_error_logged = False
|
||||
self.remote_mode_active = self.default_mode == "REMOTE"
|
||||
self.model_switch_candidate: Optional[int] = None
|
||||
self.model_switch_candidate_count = 0
|
||||
self.model_switch_stable: Optional[int] = None
|
||||
|
||||
self.create_subscription(String, "control/mode_state", self.on_mode_state, 10)
|
||||
|
||||
if self.enabled:
|
||||
self.receiver = SbusUartReceiver(
|
||||
@@ -213,6 +254,8 @@ class RemoteUartNode(Node):
|
||||
self.estop_pub.publish(Bool(data=False))
|
||||
self.estop_published = False
|
||||
|
||||
self.handle_model_switch(state)
|
||||
|
||||
active = any(abs(value) > self.active_threshold for value in (state.ch1, state.ch2, state.ch4))
|
||||
if active or self.publish_inactive_zero:
|
||||
cmd = Twist()
|
||||
@@ -224,6 +267,15 @@ class RemoteUartNode(Node):
|
||||
def publish_zero_cmd(self) -> None:
|
||||
self.cmd_pub.publish(Twist())
|
||||
|
||||
def on_mode_state(self, msg: String) -> None:
|
||||
mode = str(msg.data).strip().upper()
|
||||
remote_mode_active = mode == "REMOTE"
|
||||
if remote_mode_active == self.remote_mode_active:
|
||||
return
|
||||
|
||||
self.remote_mode_active = remote_mode_active
|
||||
self.reset_model_switch_tracking()
|
||||
|
||||
def axis_to_velocity(self, raw_value: int, limit: float, invert: bool) -> float:
|
||||
if abs(raw_value) <= self.active_threshold:
|
||||
return 0.0
|
||||
@@ -232,6 +284,53 @@ class RemoteUartNode(Node):
|
||||
scaled = -scaled
|
||||
return float(scaled * limit)
|
||||
|
||||
@staticmethod
|
||||
def parse_switch_level(value: str) -> int:
|
||||
normalized = value.strip().lower()
|
||||
if normalized == "low":
|
||||
return SWITCH_LOW
|
||||
if normalized == "high":
|
||||
return SWITCH_HIGH
|
||||
return SWITCH_MID
|
||||
|
||||
def handle_model_switch(self, state: RemoteControlState) -> None:
|
||||
if not self.model_switch_enabled or not self.remote_mode_active:
|
||||
return
|
||||
|
||||
switch_level = state.switches.get(self.model_switch_channel)
|
||||
if switch_level is None:
|
||||
return
|
||||
|
||||
if switch_level == self.model_switch_candidate:
|
||||
self.model_switch_candidate_count += 1
|
||||
else:
|
||||
self.model_switch_candidate = switch_level
|
||||
self.model_switch_candidate_count = 1
|
||||
|
||||
if self.model_switch_candidate_count < self.model_switch_debounce_frames:
|
||||
return
|
||||
|
||||
if switch_level == self.model_switch_stable:
|
||||
return
|
||||
|
||||
self.model_switch_stable = switch_level
|
||||
|
||||
if switch_level == self.model_switch_rough_level:
|
||||
self.model_cmd_pub.publish(String(data="rough"))
|
||||
self.get_logger().info(
|
||||
f"Remote model switch: CH{self.model_switch_channel} -> rough"
|
||||
)
|
||||
elif switch_level == self.model_switch_ik_level:
|
||||
self.model_cmd_pub.publish(String(data="ik"))
|
||||
self.get_logger().info(
|
||||
f"Remote model switch: CH{self.model_switch_channel} -> ik"
|
||||
)
|
||||
|
||||
def reset_model_switch_tracking(self) -> None:
|
||||
self.model_switch_candidate = None
|
||||
self.model_switch_candidate_count = 0
|
||||
self.model_switch_stable = None
|
||||
|
||||
|
||||
def main(args: Optional[list[str]] = None) -> None:
|
||||
rclpy.init(args=args)
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
+599
-24
@@ -9,8 +9,10 @@ from http import HTTPStatus
|
||||
from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer
|
||||
from pathlib import Path
|
||||
from typing import Any, Optional
|
||||
from urllib.parse import urlparse
|
||||
|
||||
import rclpy
|
||||
import yaml
|
||||
from geometry_msgs.msg import Twist
|
||||
from rclpy.executors import ExternalShutdownException
|
||||
from rclpy.node import Node
|
||||
@@ -41,10 +43,31 @@ class WebUdpBridgeNode(Node):
|
||||
self.web_static_dir = str(self.declare_parameter("web_static_dir", "").value)
|
||||
self.nav_map_frame = str(self.declare_parameter("nav_map_frame", "map").value)
|
||||
self.nav_base_frame = str(self.declare_parameter("nav_base_frame", "base_link").value)
|
||||
self.nav_goals_file = str(self.declare_parameter("nav_goals_file", "").value)
|
||||
self.nav_missions_file = str(self.declare_parameter("nav_missions_file", "").value)
|
||||
self.nav_route_file = str(self.declare_parameter("nav_route_file", "").value)
|
||||
self.nav_route_task_file = str(self.declare_parameter("nav_route_task_file", "").value)
|
||||
self.pcd_nav_file = str(self.declare_parameter("pcd_nav_file", "").value)
|
||||
self.pcd_floor_z_min = float(self.declare_parameter("pcd_floor_z_min", -1.6).value)
|
||||
self.pcd_floor_z_max = float(self.declare_parameter("pcd_floor_z_max", 0.4).value)
|
||||
self.pcd_sample_step = max(1, int(self.declare_parameter("pcd_sample_step", 25).value))
|
||||
self.route_align_enabled = bool(
|
||||
self.declare_parameter("nav_route_auto_align_enabled", True).value
|
||||
)
|
||||
self.route_rotation_offset_deg = float(
|
||||
self.declare_parameter("nav_route_rotation_offset_deg", 0.0).value
|
||||
)
|
||||
self.route_align_max_angle_deg = abs(
|
||||
float(self.declare_parameter("nav_route_align_max_angle_deg", 6.0).value)
|
||||
)
|
||||
self.route_align_angle_step_deg = max(
|
||||
0.1,
|
||||
float(self.declare_parameter("nav_route_align_angle_step_deg", 0.5).value),
|
||||
)
|
||||
self.route_align_search_radius = max(
|
||||
0.05,
|
||||
float(self.declare_parameter("nav_route_align_search_radius", 0.35).value),
|
||||
)
|
||||
|
||||
self.tf_buffer = Buffer()
|
||||
self.tf_listener = TransformListener(self.tf_buffer, self)
|
||||
@@ -59,10 +82,23 @@ class WebUdpBridgeNode(Node):
|
||||
|
||||
self.latest_target: Optional[RuntimeTarget] = None
|
||||
self.latest_state: Optional[RuntimeState] = None
|
||||
self.latest_model_status: dict[str, Any] = {
|
||||
"current_model": "rough",
|
||||
"requested_model": "rough",
|
||||
"switch_state": "idle",
|
||||
"backend": "unknown",
|
||||
"switching": False,
|
||||
}
|
||||
self.latest_cmd = Twist()
|
||||
self.latest_mode = "UNKNOWN"
|
||||
self.latest_mux_status = ""
|
||||
self.latest_nav_status = ""
|
||||
self.latest_nav_path: dict[str, Any] = {
|
||||
"goal_name": "",
|
||||
"stage": "idle",
|
||||
"path_index": 0,
|
||||
"points": [],
|
||||
}
|
||||
self.estop = False
|
||||
self.web_enabled = False
|
||||
self.last_cmd_time = self.get_clock().now()
|
||||
@@ -75,23 +111,39 @@ class WebUdpBridgeNode(Node):
|
||||
self.remote_enabled_pub = self.create_publisher(Bool, "remote/enabled", 10)
|
||||
self.nav_enabled_pub = self.create_publisher(Bool, "nav/enabled", 10)
|
||||
self.mode_pub = self.create_publisher(String, "control/mode", 10)
|
||||
self.model_cmd_pub = self.create_publisher(String, "runtime/model_cmd", 10)
|
||||
|
||||
self.create_subscription(RuntimeTarget, "runtime/target", self.on_target, 10)
|
||||
self.create_subscription(RuntimeState, "runtime/state", self.on_state, 10)
|
||||
self.create_subscription(String, "runtime/model_status", self.on_model_status, 10)
|
||||
self.create_subscription(Twist, "cmd_vel", self.on_cmd_vel, 10)
|
||||
self.create_subscription(Bool, "/safety/estop", self.on_estop, 10)
|
||||
self.create_subscription(String, "control/mode_state", self.on_mode_state, 10)
|
||||
self.create_subscription(String, "control/mux_status", self.on_mux_status, 10)
|
||||
self.create_subscription(String, "simple_nav/status", self.on_nav_status, 10)
|
||||
self.create_subscription(String, "simple_nav/path", self.on_nav_path, 10)
|
||||
|
||||
self.map_points = self.load_filtered_pcd(Path(self.pcd_nav_file)) if self.pcd_nav_file else []
|
||||
self.goal_specs: list[dict[str, Any]] = []
|
||||
self.mission_specs: list[dict[str, Any]] = []
|
||||
self.default_mission_name: Optional[str] = None
|
||||
self.route_alignment_info: dict[str, Any] = {}
|
||||
self.reload_nav_task_config()
|
||||
|
||||
self.http_server: Optional[ThreadingHTTPServer] = None
|
||||
self.http_thread: Optional[threading.Thread] = None
|
||||
self.static_dir = Path(self.web_static_dir) if self.web_static_dir else Path(__file__).resolve().parents[3] / "tools" / "win_web_debug" / "static"
|
||||
self.static_dir = (
|
||||
Path(self.web_static_dir)
|
||||
if self.web_static_dir
|
||||
else Path(__file__).resolve().parents[3] / "tools" / "win_web_debug" / "static"
|
||||
)
|
||||
self.start_http_server()
|
||||
|
||||
self.rx_timer = self.create_timer(0.01, self.on_rx_timer)
|
||||
self.state_timer = self.create_timer(1.0 / self.state_hz if self.state_hz > 0.0 else 0.05, self.on_state_timer)
|
||||
self.state_timer = self.create_timer(
|
||||
1.0 / self.state_hz if self.state_hz > 0.0 else 0.05,
|
||||
self.on_state_timer,
|
||||
)
|
||||
self.guard_timer = self.create_timer(0.05, self.on_guard_timer)
|
||||
|
||||
self.get_logger().info(f"Web UDP bridge listening on {self.listen_host}:{self.listen_port}")
|
||||
@@ -125,9 +177,432 @@ class WebUdpBridgeNode(Node):
|
||||
data_started = True
|
||||
return points[:: self.pcd_sample_step]
|
||||
|
||||
def _load_yaml(self, path_value: str) -> dict[str, Any]:
|
||||
if not path_value:
|
||||
return {}
|
||||
path = Path(path_value).expanduser()
|
||||
if not path.exists():
|
||||
self.get_logger().warn(f"Navigation file not found: {path}")
|
||||
return {}
|
||||
try:
|
||||
with path.open("r", encoding="utf-8") as f:
|
||||
data = yaml.safe_load(f) or {}
|
||||
return data if isinstance(data, dict) else {}
|
||||
except Exception as exc:
|
||||
self.get_logger().warn(f"Failed to load navigation file {path}: {exc}")
|
||||
return {}
|
||||
|
||||
@staticmethod
|
||||
def _normalize_policy(value: Any) -> Optional[str]:
|
||||
text = str(value).strip().lower()
|
||||
if not text:
|
||||
return None
|
||||
if text == "ik":
|
||||
return "crawl"
|
||||
if text in {"rough", "crawl"}:
|
||||
return text
|
||||
return None
|
||||
|
||||
@staticmethod
|
||||
def _get_value(data: dict[str, Any], *keys: str) -> Any:
|
||||
for key in keys:
|
||||
if key in data and data.get(key) is not None:
|
||||
return data.get(key)
|
||||
return None
|
||||
|
||||
@classmethod
|
||||
def _get_float(cls, data: dict[str, Any], *keys: str) -> Optional[float]:
|
||||
value = cls._get_value(data, *keys)
|
||||
if value is None:
|
||||
return None
|
||||
try:
|
||||
return float(value)
|
||||
except (TypeError, ValueError):
|
||||
return None
|
||||
|
||||
@classmethod
|
||||
def _get_bool(cls, data: dict[str, Any], *keys: str) -> Optional[bool]:
|
||||
value = cls._get_value(data, *keys)
|
||||
if value is None:
|
||||
return None
|
||||
if isinstance(value, bool):
|
||||
return value
|
||||
text = str(value).strip().lower()
|
||||
if text in {"1", "true", "yes", "on"}:
|
||||
return True
|
||||
if text in {"0", "false", "no", "off"}:
|
||||
return False
|
||||
return None
|
||||
|
||||
def load_nav_task_config(
|
||||
self,
|
||||
) -> tuple[list[dict[str, Any]], list[dict[str, Any]], Optional[str], dict[str, Any]]:
|
||||
goals_by_name = self._load_goals(self.nav_goals_file)
|
||||
missions_by_name = self._load_missions(self.nav_missions_file)
|
||||
route_goals, route_missions, route_default, route_alignment = self._load_route(
|
||||
self.get_route_source_file()
|
||||
)
|
||||
goals_by_name.update(route_goals)
|
||||
missions_by_name.update(route_missions)
|
||||
goal_specs = list(goals_by_name.values())
|
||||
mission_specs = [
|
||||
{"name": mission_name, "goals": list(goal_names)}
|
||||
for mission_name, goal_names in missions_by_name.items()
|
||||
]
|
||||
default_mission_name = route_default or (mission_specs[0]["name"] if mission_specs else None)
|
||||
return goal_specs, mission_specs, default_mission_name, route_alignment
|
||||
|
||||
def reload_nav_task_config(self) -> None:
|
||||
(
|
||||
self.goal_specs,
|
||||
self.mission_specs,
|
||||
self.default_mission_name,
|
||||
self.route_alignment_info,
|
||||
) = self.load_nav_task_config()
|
||||
|
||||
def get_route_source_file(self) -> str:
|
||||
candidate = self.nav_route_task_file.strip() if self.nav_route_task_file else ""
|
||||
if candidate:
|
||||
return candidate
|
||||
return self.nav_route_file
|
||||
|
||||
def _load_goals(self, path_value: str) -> dict[str, dict[str, Any]]:
|
||||
data = self._load_yaml(path_value)
|
||||
raw_goals = data.get("goals", {})
|
||||
parsed: dict[str, dict[str, Any]] = {}
|
||||
if not isinstance(raw_goals, dict):
|
||||
return parsed
|
||||
for name, spec in raw_goals.items():
|
||||
if not isinstance(spec, dict):
|
||||
continue
|
||||
pos = spec.get("position", [0.0, 0.0, 0.0])
|
||||
if not isinstance(pos, list) or len(pos) < 2:
|
||||
continue
|
||||
try:
|
||||
yaw_tolerance_deg = self._get_float(spec, "yaw_tolerance_deg", "yawToleranceDeg")
|
||||
yaw_deg = self._get_float(spec, "yaw_deg", "yawDeg")
|
||||
require_yaw = self._get_bool(spec, "require_yaw", "requireYaw")
|
||||
parsed[str(name)] = {
|
||||
"name": str(name),
|
||||
"x": float(pos[0]),
|
||||
"y": float(pos[1]),
|
||||
"yaw_deg": yaw_deg,
|
||||
"yaw_tolerance_deg": yaw_tolerance_deg,
|
||||
"tolerance": float(spec.get("tolerance", 0.20)),
|
||||
"policy": self._normalize_policy(spec.get("policy")),
|
||||
"speed": float(spec["speed"]) if spec.get("speed") is not None else None,
|
||||
"require_yaw": bool(yaw_deg is not None if require_yaw is None else require_yaw) and yaw_deg is not None,
|
||||
"pre_dock_distance": self._get_float(spec, "pre_dock_distance", "preDockDistance"),
|
||||
"pre_dock_tolerance": self._get_float(spec, "pre_dock_tolerance", "preDockTolerance"),
|
||||
}
|
||||
except (TypeError, ValueError):
|
||||
continue
|
||||
return parsed
|
||||
|
||||
def _load_missions(self, path_value: str) -> dict[str, list[str]]:
|
||||
data = self._load_yaml(path_value)
|
||||
raw_missions = data.get("missions", {})
|
||||
parsed: dict[str, list[str]] = {}
|
||||
if not isinstance(raw_missions, dict):
|
||||
return parsed
|
||||
for name, spec in raw_missions.items():
|
||||
if not isinstance(spec, dict):
|
||||
continue
|
||||
goals = spec.get("goals", [])
|
||||
if isinstance(goals, list) and goals and all(isinstance(item, str) for item in goals):
|
||||
parsed[str(name)] = [str(item) for item in goals]
|
||||
return parsed
|
||||
|
||||
def _load_route(
|
||||
self, path_value: str
|
||||
) -> tuple[dict[str, dict[str, Any]], dict[str, list[str]], Optional[str], dict[str, Any]]:
|
||||
data = self._load_yaml(path_value)
|
||||
if not data:
|
||||
return {}, {}, None, {}
|
||||
|
||||
route_name = str(data.get("name", "")).strip() or Path(path_value).stem
|
||||
route_default_yaw_tolerance_deg = self._get_float(
|
||||
data,
|
||||
"yaw_tolerance_deg_default",
|
||||
"yawToleranceDegDefault",
|
||||
)
|
||||
route_default_require_yaw = self._get_bool(
|
||||
data,
|
||||
"require_yaw_default",
|
||||
"requireYawDefault",
|
||||
)
|
||||
route_default_pre_dock_distance = self._get_float(
|
||||
data,
|
||||
"pre_dock_distance_default",
|
||||
"preDockDistanceDefault",
|
||||
)
|
||||
route_default_pre_dock_tolerance = self._get_float(
|
||||
data,
|
||||
"pre_dock_tolerance_default",
|
||||
"preDockToleranceDefault",
|
||||
)
|
||||
raw_segments = data.get("segments", [])
|
||||
if not isinstance(raw_segments, list) or not raw_segments:
|
||||
top_level_waypoints = data.get("waypoints", [])
|
||||
if isinstance(top_level_waypoints, list) and top_level_waypoints:
|
||||
raw_segments = [{
|
||||
"name": "segment_1",
|
||||
"obstacle": str(data.get("obstacle", "flat") or "flat"),
|
||||
"waypoints": top_level_waypoints,
|
||||
}]
|
||||
else:
|
||||
return {}, {}, None, {}
|
||||
|
||||
raw_waypoints: list[dict[str, Any]] = []
|
||||
for segment_index, segment in enumerate(raw_segments, start=1):
|
||||
if not isinstance(segment, dict):
|
||||
continue
|
||||
segment_name = str(segment.get("name", f"segment_{segment_index}")).strip() or f"segment_{segment_index}"
|
||||
waypoints = segment.get("waypoints", [])
|
||||
if not isinstance(waypoints, list):
|
||||
continue
|
||||
for waypoint_index, waypoint in enumerate(waypoints, start=1):
|
||||
if not isinstance(waypoint, dict):
|
||||
continue
|
||||
try:
|
||||
waypoint_yaw_deg = self._get_float(waypoint, "yaw_deg", "yawDeg")
|
||||
waypoint_yaw_tolerance_deg = self._get_float(
|
||||
waypoint,
|
||||
"yaw_tolerance_deg",
|
||||
"yawToleranceDeg",
|
||||
)
|
||||
waypoint_require_yaw = self._get_bool(
|
||||
waypoint,
|
||||
"require_yaw",
|
||||
"requireYaw",
|
||||
)
|
||||
if waypoint_require_yaw is None:
|
||||
waypoint_require_yaw = bool(route_default_require_yaw) if route_default_require_yaw is not None else False
|
||||
if waypoint_yaw_tolerance_deg is None:
|
||||
waypoint_yaw_tolerance_deg = route_default_yaw_tolerance_deg
|
||||
waypoint_pre_dock_distance = self._get_float(
|
||||
waypoint,
|
||||
"pre_dock_distance",
|
||||
"preDockDistance",
|
||||
)
|
||||
if waypoint_pre_dock_distance is None:
|
||||
waypoint_pre_dock_distance = route_default_pre_dock_distance
|
||||
waypoint_pre_dock_tolerance = self._get_float(
|
||||
waypoint,
|
||||
"pre_dock_tolerance",
|
||||
"preDockTolerance",
|
||||
)
|
||||
if waypoint_pre_dock_tolerance is None:
|
||||
waypoint_pre_dock_tolerance = route_default_pre_dock_tolerance
|
||||
raw_waypoints.append(
|
||||
{
|
||||
"segment": segment_name,
|
||||
"id": int(waypoint.get("id", waypoint_index)),
|
||||
"x": float(waypoint["x"]),
|
||||
"y": float(waypoint["y"]),
|
||||
"yaw_deg": waypoint_yaw_deg,
|
||||
"yaw_tolerance_deg": waypoint_yaw_tolerance_deg,
|
||||
"tolerance": float(waypoint.get("tolerance", 0.20)),
|
||||
"policy": self._normalize_policy(waypoint.get("policy")),
|
||||
"speed": float(waypoint["speed"]) if waypoint.get("speed") is not None else None,
|
||||
"require_yaw": bool(waypoint_require_yaw) and waypoint_yaw_deg is not None,
|
||||
"pre_dock_distance": waypoint_pre_dock_distance,
|
||||
"pre_dock_tolerance": waypoint_pre_dock_tolerance,
|
||||
}
|
||||
)
|
||||
except (KeyError, TypeError, ValueError):
|
||||
continue
|
||||
|
||||
if not raw_waypoints:
|
||||
return {}, {}, None, {}
|
||||
|
||||
aligned_waypoints, alignment_info = self._align_route_waypoints(raw_waypoints)
|
||||
goals: dict[str, dict[str, Any]] = {}
|
||||
mission_goal_names: list[str] = []
|
||||
for index, waypoint in enumerate(aligned_waypoints, start=1):
|
||||
goal_name = f"{route_name}_p{index:02d}"
|
||||
goals[goal_name] = {
|
||||
"name": goal_name,
|
||||
"x": round(float(waypoint["x"]), 3),
|
||||
"y": round(float(waypoint["y"]), 3),
|
||||
"yaw_deg": round(float(waypoint["yaw_deg"]), 3) if waypoint.get("yaw_deg") is not None else None,
|
||||
"yaw_tolerance_deg": round(float(waypoint["yaw_tolerance_deg"]), 3)
|
||||
if waypoint.get("yaw_tolerance_deg") is not None else None,
|
||||
"tolerance": round(float(waypoint.get("tolerance", 0.20)), 3),
|
||||
"policy": self._normalize_policy(waypoint.get("policy")),
|
||||
"speed": float(waypoint["speed"]) if waypoint.get("speed") is not None else None,
|
||||
"require_yaw": bool(waypoint.get("require_yaw", False)),
|
||||
"pre_dock_distance": round(float(waypoint["pre_dock_distance"]), 3)
|
||||
if waypoint.get("pre_dock_distance") is not None else None,
|
||||
"pre_dock_tolerance": round(float(waypoint["pre_dock_tolerance"]), 3)
|
||||
if waypoint.get("pre_dock_tolerance") is not None else None,
|
||||
}
|
||||
mission_goal_names.append(goal_name)
|
||||
|
||||
return goals, {route_name: mission_goal_names}, route_name, alignment_info
|
||||
|
||||
def _align_route_waypoints(
|
||||
self, waypoints: list[dict[str, Any]]
|
||||
) -> tuple[list[dict[str, Any]], dict[str, Any]]:
|
||||
if not waypoints:
|
||||
return [], {}
|
||||
|
||||
if not self.map_points:
|
||||
info = {
|
||||
"enabled": False,
|
||||
"reason": "pcd unavailable",
|
||||
"manual_offset_deg": round(self.route_rotation_offset_deg, 3),
|
||||
"auto_offset_deg": 0.0,
|
||||
"applied_deg": round(self.route_rotation_offset_deg, 3),
|
||||
"hits": 0,
|
||||
"total": len(waypoints),
|
||||
}
|
||||
return self._rotate_waypoints(waypoints, math.radians(self.route_rotation_offset_deg)), info
|
||||
|
||||
manual_offset_deg = self.route_rotation_offset_deg
|
||||
if not self.route_align_enabled or len(waypoints) < 2:
|
||||
info = {
|
||||
"enabled": False,
|
||||
"reason": "auto align disabled",
|
||||
"manual_offset_deg": round(manual_offset_deg, 3),
|
||||
"auto_offset_deg": 0.0,
|
||||
"applied_deg": round(manual_offset_deg, 3),
|
||||
"hits": 0,
|
||||
"total": len(waypoints),
|
||||
}
|
||||
return self._rotate_waypoints(waypoints, math.radians(manual_offset_deg)), info
|
||||
|
||||
anchor_x = float(waypoints[0]["x"])
|
||||
anchor_y = float(waypoints[0]["y"])
|
||||
search_radius_sq = self.route_align_search_radius * self.route_align_search_radius
|
||||
best_hits = -1
|
||||
best_score = float("inf")
|
||||
best_angle_deg = manual_offset_deg
|
||||
manual_hits = -1
|
||||
manual_score = float("inf")
|
||||
steps = max(
|
||||
1,
|
||||
int(round((self.route_align_max_angle_deg * 2.0) / self.route_align_angle_step_deg)),
|
||||
)
|
||||
|
||||
for step_index in range(steps + 1):
|
||||
auto_delta_deg = -self.route_align_max_angle_deg + step_index * self.route_align_angle_step_deg
|
||||
angle_deg = manual_offset_deg + auto_delta_deg
|
||||
angle_rad = math.radians(angle_deg)
|
||||
hits = 0
|
||||
score = 0.0
|
||||
for waypoint in waypoints:
|
||||
tx, ty = self._rotate_xy(
|
||||
float(waypoint["x"]),
|
||||
float(waypoint["y"]),
|
||||
anchor_x,
|
||||
anchor_y,
|
||||
angle_rad,
|
||||
)
|
||||
nearest_sq = search_radius_sq
|
||||
for px, py in self.map_points:
|
||||
dx = px - tx
|
||||
dy = py - ty
|
||||
dist_sq = dx * dx + dy * dy
|
||||
if dist_sq < nearest_sq:
|
||||
nearest_sq = dist_sq
|
||||
if nearest_sq < search_radius_sq:
|
||||
hits += 1
|
||||
score += nearest_sq
|
||||
if abs(angle_deg - manual_offset_deg) <= 1.0e-9:
|
||||
manual_hits = hits
|
||||
manual_score = score
|
||||
better_hits = hits > best_hits
|
||||
better_score = hits == best_hits and (
|
||||
score < best_score - 1.0e-9
|
||||
or (
|
||||
abs(score - best_score) <= 1.0e-9
|
||||
and abs(auto_delta_deg) < abs(best_angle_deg - manual_offset_deg)
|
||||
)
|
||||
)
|
||||
if better_hits or better_score:
|
||||
best_hits = hits
|
||||
best_score = score
|
||||
best_angle_deg = angle_deg
|
||||
|
||||
hits_improved = best_hits > manual_hits
|
||||
score_improvement = (
|
||||
(manual_score - best_score) / max(manual_score, 1.0e-9)
|
||||
if manual_score < float("inf")
|
||||
else 0.0
|
||||
)
|
||||
if not hits_improved and score_improvement < 0.05:
|
||||
applied_angle_deg = manual_offset_deg
|
||||
rotated = self._rotate_waypoints(waypoints, math.radians(applied_angle_deg))
|
||||
info = {
|
||||
"enabled": True,
|
||||
"reason": "ambiguous-auto-align",
|
||||
"manual_offset_deg": round(manual_offset_deg, 3),
|
||||
"auto_offset_deg": 0.0,
|
||||
"applied_deg": round(applied_angle_deg, 3),
|
||||
"score": round(manual_score, 6) if manual_score < float("inf") else 0.0,
|
||||
"hits": int(manual_hits if manual_hits >= 0 else 0),
|
||||
"total": len(waypoints),
|
||||
}
|
||||
return rotated, info
|
||||
|
||||
rotated = self._rotate_waypoints(waypoints, math.radians(best_angle_deg))
|
||||
info = {
|
||||
"enabled": True,
|
||||
"manual_offset_deg": round(manual_offset_deg, 3),
|
||||
"auto_offset_deg": round(best_angle_deg - manual_offset_deg, 3),
|
||||
"applied_deg": round(best_angle_deg, 3),
|
||||
"score": round(best_score, 6),
|
||||
"hits": int(best_hits),
|
||||
"total": len(waypoints),
|
||||
}
|
||||
return rotated, info
|
||||
|
||||
def _rotate_waypoints(self, waypoints: list[dict[str, Any]], angle_rad: float) -> list[dict[str, Any]]:
|
||||
if not waypoints:
|
||||
return []
|
||||
anchor_x = float(waypoints[0]["x"])
|
||||
anchor_y = float(waypoints[0]["y"])
|
||||
rotated: list[dict[str, Any]] = []
|
||||
for waypoint in waypoints:
|
||||
x, y = self._rotate_xy(
|
||||
float(waypoint["x"]),
|
||||
float(waypoint["y"]),
|
||||
anchor_x,
|
||||
anchor_y,
|
||||
angle_rad,
|
||||
)
|
||||
updated = dict(waypoint)
|
||||
updated["x"] = x
|
||||
updated["y"] = y
|
||||
if updated.get("yaw_deg") is not None:
|
||||
updated["yaw_deg"] = round(
|
||||
math.degrees(
|
||||
self.normalize_angle(math.radians(float(updated["yaw_deg"])) + angle_rad)
|
||||
),
|
||||
3,
|
||||
)
|
||||
rotated.append(updated)
|
||||
return rotated
|
||||
|
||||
@staticmethod
|
||||
def _rotate_xy(x: float, y: float, anchor_x: float, anchor_y: float, angle_rad: float) -> tuple[float, float]:
|
||||
dx = x - anchor_x
|
||||
dy = y - anchor_y
|
||||
cos_a = math.cos(angle_rad)
|
||||
sin_a = math.sin(angle_rad)
|
||||
return (
|
||||
anchor_x + dx * cos_a - dy * sin_a,
|
||||
anchor_y + dx * sin_a + dy * cos_a,
|
||||
)
|
||||
|
||||
def lookup_pose(self) -> Optional[dict[str, float]]:
|
||||
try:
|
||||
transform = self.tf_buffer.lookup_transform(self.nav_map_frame, self.nav_base_frame, rclpy.time.Time())
|
||||
transform = self.tf_buffer.lookup_transform(
|
||||
self.nav_map_frame,
|
||||
self.nav_base_frame,
|
||||
rclpy.time.Time(),
|
||||
)
|
||||
except TransformException:
|
||||
return None
|
||||
t = transform.transform.translation
|
||||
@@ -141,21 +616,17 @@ class WebUdpBridgeNode(Node):
|
||||
|
||||
class Handler(BaseHTTPRequestHandler):
|
||||
def do_GET(self):
|
||||
if self.path in ["/", "/index.html"]:
|
||||
request_path = urlparse(self.path).path
|
||||
if request_path in ["/", "/index.html"]:
|
||||
self.serve_file(static_dir / "index.html", "text/html; charset=utf-8")
|
||||
elif self.path == "/app.js":
|
||||
elif request_path == "/app.js":
|
||||
self.serve_file(static_dir / "app.js", "application/javascript; charset=utf-8")
|
||||
elif self.path == "/style.css":
|
||||
elif request_path == "/style.css":
|
||||
self.serve_file(static_dir / "style.css", "text/css; charset=utf-8")
|
||||
elif self.path == "/api/state":
|
||||
elif request_path == "/api/state":
|
||||
self.send_json(node.build_state_packet())
|
||||
elif self.path == "/api/map":
|
||||
self.send_json({
|
||||
"points": node.map_points,
|
||||
"map_frame": node.nav_map_frame,
|
||||
"base_frame": node.nav_base_frame,
|
||||
"pose": node.lookup_pose(),
|
||||
})
|
||||
elif request_path == "/api/map":
|
||||
self.send_json(node.build_map_packet())
|
||||
else:
|
||||
self.send_error(HTTPStatus.NOT_FOUND)
|
||||
|
||||
@@ -170,7 +641,10 @@ class WebUdpBridgeNode(Node):
|
||||
node.handle_http_control(payload)
|
||||
self.send_json({"ok": True})
|
||||
except Exception as exc:
|
||||
self.send_json({"ok": False, "error": str(exc)}, status=HTTPStatus.BAD_REQUEST)
|
||||
self.send_json(
|
||||
{"ok": False, "error": str(exc)},
|
||||
status=HTTPStatus.BAD_REQUEST,
|
||||
)
|
||||
|
||||
def serve_file(self, path: Path, content_type: str):
|
||||
if not path.exists():
|
||||
@@ -198,9 +672,50 @@ class WebUdpBridgeNode(Node):
|
||||
self.http_thread = threading.Thread(target=self.http_server.serve_forever, daemon=True)
|
||||
self.http_thread.start()
|
||||
|
||||
def build_map_packet(self) -> dict[str, Any]:
|
||||
goal_lookup = {goal["name"]: goal for goal in self.goal_specs if goal.get("name")}
|
||||
default_goals: list[dict[str, Any]] = []
|
||||
if self.default_mission_name:
|
||||
for mission in self.mission_specs:
|
||||
if mission["name"] != self.default_mission_name:
|
||||
continue
|
||||
default_goals = [
|
||||
goal_lookup[goal_name]
|
||||
for goal_name in mission.get("goals", [])
|
||||
if goal_name in goal_lookup
|
||||
]
|
||||
break
|
||||
return {
|
||||
"points": self.map_points,
|
||||
"map_frame": self.nav_map_frame,
|
||||
"base_frame": self.nav_base_frame,
|
||||
"pose": self.lookup_pose(),
|
||||
"route_source_file": self.get_route_source_file(),
|
||||
"goal_specs": self.goal_specs,
|
||||
"mission_specs": self.mission_specs,
|
||||
"default_mission_name": self.default_mission_name,
|
||||
"default_mission_goals": default_goals,
|
||||
"route_alignment": self.route_alignment_info,
|
||||
"nav_path": dict(self.latest_nav_path),
|
||||
}
|
||||
|
||||
def handle_http_control(self, payload: dict[str, Any]) -> None:
|
||||
msg_type = str(payload.get("type", "")).lower()
|
||||
if msg_type in {"cmd_vel", "zero", "estop", "mode", "web_enable", "remote_enable", "nav_enable", "ping", "nav_cmd", "go_to", "go_rel"}:
|
||||
if msg_type in {
|
||||
"cmd_vel",
|
||||
"zero",
|
||||
"estop",
|
||||
"mode",
|
||||
"web_enable",
|
||||
"remote_enable",
|
||||
"nav_enable",
|
||||
"ping",
|
||||
"nav_cmd",
|
||||
"go_to",
|
||||
"go_rel",
|
||||
"model_toggle",
|
||||
"model_cmd",
|
||||
}:
|
||||
self.handle_packet(payload)
|
||||
return
|
||||
raise ValueError(f"unknown control type: {msg_type}")
|
||||
@@ -211,6 +726,22 @@ class WebUdpBridgeNode(Node):
|
||||
def on_state(self, msg: RuntimeState) -> None:
|
||||
self.latest_state = msg
|
||||
|
||||
def on_model_status(self, msg: String) -> None:
|
||||
try:
|
||||
data = json.loads(msg.data) if msg.data else {}
|
||||
if isinstance(data, dict):
|
||||
self.latest_model_status = {
|
||||
"current_model": str(data.get("current_model", "rough")),
|
||||
"requested_model": str(
|
||||
data.get("requested_model", data.get("current_model", "rough"))
|
||||
),
|
||||
"switch_state": str(data.get("switch_state", "idle")),
|
||||
"backend": str(data.get("backend", "unknown")),
|
||||
"switching": bool(data.get("switching", False)),
|
||||
}
|
||||
except Exception as exc:
|
||||
self.get_logger().warn(f"Failed to parse model status: {exc}")
|
||||
|
||||
def on_cmd_vel(self, msg: Twist) -> None:
|
||||
self.latest_cmd = msg
|
||||
|
||||
@@ -225,6 +756,38 @@ class WebUdpBridgeNode(Node):
|
||||
|
||||
def on_nav_status(self, msg: String) -> None:
|
||||
self.latest_nav_status = msg.data
|
||||
if msg.data.startswith("reloaded goals="):
|
||||
self.reload_nav_task_config()
|
||||
|
||||
def on_nav_path(self, msg: String) -> None:
|
||||
try:
|
||||
data = json.loads(msg.data) if msg.data else {}
|
||||
except Exception:
|
||||
return
|
||||
if not isinstance(data, dict):
|
||||
return
|
||||
raw_points = data.get("points", [])
|
||||
points: list[list[float]] = []
|
||||
if isinstance(raw_points, list):
|
||||
for item in raw_points:
|
||||
if (
|
||||
isinstance(item, (list, tuple))
|
||||
and len(item) >= 2
|
||||
):
|
||||
try:
|
||||
points.append([round(float(item[0]), 3), round(float(item[1]), 3)])
|
||||
except (TypeError, ValueError):
|
||||
continue
|
||||
try:
|
||||
path_index = max(0, int(data.get("path_index", 0) or 0))
|
||||
except (TypeError, ValueError):
|
||||
path_index = 0
|
||||
self.latest_nav_path = {
|
||||
"goal_name": str(data.get("goal_name", "")),
|
||||
"stage": str(data.get("stage", "idle")),
|
||||
"path_index": path_index,
|
||||
"points": points,
|
||||
}
|
||||
|
||||
def on_rx_timer(self) -> None:
|
||||
if not self.enabled:
|
||||
@@ -290,16 +853,18 @@ class WebUdpBridgeNode(Node):
|
||||
dy = float(payload.get("dy", 0.0))
|
||||
self.set_control_mode("NAV")
|
||||
self.nav_cmd_pub.publish(String(data=f"go_rel {dx:.3f} {dy:.3f}"))
|
||||
elif msg_type == "model_toggle":
|
||||
self.model_cmd_pub.publish(String(data="toggle"))
|
||||
elif msg_type == "model_cmd":
|
||||
command = str(payload.get("command", "")).strip()
|
||||
if command:
|
||||
self.model_cmd_pub.publish(String(data=command))
|
||||
elif msg_type == "ping":
|
||||
self.send_packet({"type": "pong", "stamp": self.now_sec()})
|
||||
elif msg_type == "map_request":
|
||||
self.send_packet({
|
||||
"type": "map",
|
||||
"points": self.map_points,
|
||||
"map_frame": self.nav_map_frame,
|
||||
"base_frame": self.nav_base_frame,
|
||||
"pose": self.lookup_pose(),
|
||||
})
|
||||
packet = self.build_map_packet()
|
||||
packet["type"] = "map"
|
||||
self.send_packet(packet)
|
||||
else:
|
||||
self.send_packet({"type": "error", "message": f"unknown packet type: {msg_type}"})
|
||||
|
||||
@@ -349,7 +914,13 @@ class WebUdpBridgeNode(Node):
|
||||
"cmd_vel": self.twist_to_dict(self.latest_cmd),
|
||||
"runtime": {},
|
||||
"robot": {},
|
||||
"nav": {"pose": self.lookup_pose(), "map_frame": self.nav_map_frame, "base_frame": self.nav_base_frame},
|
||||
"nav": {
|
||||
"pose": self.lookup_pose(),
|
||||
"map_frame": self.nav_map_frame,
|
||||
"base_frame": self.nav_base_frame,
|
||||
"path": dict(self.latest_nav_path),
|
||||
},
|
||||
"model": dict(self.latest_model_status),
|
||||
}
|
||||
if target is not None:
|
||||
packet["runtime"] = {
|
||||
@@ -417,6 +988,10 @@ class WebUdpBridgeNode(Node):
|
||||
cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
|
||||
return math.atan2(siny_cosp, cosy_cosp)
|
||||
|
||||
@staticmethod
|
||||
def normalize_angle(angle: float) -> float:
|
||||
return (angle + math.pi) % (2.0 * math.pi) - math.pi
|
||||
|
||||
@staticmethod
|
||||
def clamp(value: float, low: float, high: float) -> float:
|
||||
return max(low, min(high, value))
|
||||
|
||||
@@ -72,4 +72,3 @@ fi
|
||||
# 4. Run the ROS2 Launch file
|
||||
echo -e "${GREEN}[Launch] Starting sim2real launch file...${NC}"
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py "$@"
|
||||
|
||||
|
||||
@@ -0,0 +1,104 @@
|
||||
# PCD Map Viewer
|
||||
|
||||
Offline Web tool for Odin PCD inspection and route editing.
|
||||
|
||||
This is not the Nano runtime Web UI. Use it before a run to inspect the map and save route YAML files.
|
||||
|
||||
## Start
|
||||
|
||||
From the repository root:
|
||||
|
||||
```powershell
|
||||
python .\tools\pcd_map_viewer\server.py --http-port 8090
|
||||
```
|
||||
|
||||
Open:
|
||||
|
||||
```text
|
||||
http://127.0.0.1:8090
|
||||
```
|
||||
|
||||
You can also run directly from this directory:
|
||||
|
||||
```powershell
|
||||
python server.py --http-port 8090
|
||||
```
|
||||
|
||||
## What It Does
|
||||
|
||||
- Scans `map/*.pcd`.
|
||||
- Shows raw Odin PCD as a rotatable 3D point cloud.
|
||||
- Supports z filtering, default `z=-2.0..1.0m`.
|
||||
- Supports voxel display for structure checks.
|
||||
- Provides a simplified 2D layer view.
|
||||
- Lets you click waypoints in 3D or 2D.
|
||||
- Lets you draw obstacle terrain rectangles and edit their x/y/yaw/size.
|
||||
- Tags new waypoints with the matching obstacle terrain, defaulting to `flat`.
|
||||
- Saves route YAML and JSON under `map/routes/<pcd_name>/`.
|
||||
|
||||
## Route Fields
|
||||
|
||||
Saved waypoint fields:
|
||||
|
||||
- `x`
|
||||
- `y`
|
||||
- `yaw_deg`
|
||||
- `speed`
|
||||
- `policy`
|
||||
- `tolerance`
|
||||
- `obstacle`
|
||||
- `obstacle_name`
|
||||
|
||||
Not saved:
|
||||
|
||||
- `z`
|
||||
- `action`
|
||||
|
||||
The browser may keep local `_viewZ` only for drawing markers in 3D. The runtime route runner is planar.
|
||||
|
||||
## Saved Format
|
||||
|
||||
```yaml
|
||||
name: test_route
|
||||
map: map1
|
||||
frame_id: map
|
||||
obstacles:
|
||||
- id: 1
|
||||
name: wall_1
|
||||
obstacle: wall
|
||||
x: 1.5000
|
||||
y: 2.0000
|
||||
yaw_deg: 0.00
|
||||
length: 1.000
|
||||
width: 0.500
|
||||
policy: rough
|
||||
waypoints:
|
||||
- id: 1
|
||||
x: 1.0000
|
||||
y: 2.0000
|
||||
yaw_deg: 0.00
|
||||
speed: 0.350
|
||||
policy: rough
|
||||
tolerance: 0.150
|
||||
obstacle: wall
|
||||
obstacle_name: wall_1
|
||||
```
|
||||
|
||||
Default runtime route:
|
||||
|
||||
```text
|
||||
map/routes/map1/test_route.yaml
|
||||
```
|
||||
|
||||
## Runtime Test
|
||||
|
||||
```bash
|
||||
ros2 topic pub --once /route_runner/cmd std_msgs/msg/String "{data: reload}"
|
||||
ros2 topic pub --once /route_runner/cmd std_msgs/msg/String "{data: start}"
|
||||
ros2 topic pub --once /route_runner/cmd std_msgs/msg/String "{data: stop}"
|
||||
```
|
||||
|
||||
See also:
|
||||
|
||||
- [Maps And Routes](../../docs/ROUTES_AND_MAPS.md)
|
||||
- [Common Commands](../../docs/COMMANDS.md)
|
||||
@@ -0,0 +1,28 @@
|
||||
# PCD 地图查看和打点工具
|
||||
|
||||
这是离线 Web 工具，用于查看 Odin PCD 地图和保存路线点。它不是 Nano 运行时 Web。
|
||||
|
||||
## 启动
|
||||
|
||||
在仓库根目录：
|
||||
|
||||
```powershell
|
||||
python .\tools\pcd_map_viewer\server.py --http-port 8090
|
||||
```
|
||||
|
||||
打开：
|
||||
|
||||
```text
|
||||
http://127.0.0.1:8090
|
||||
```
|
||||
|
||||
路线点保存：
|
||||
|
||||
- `x`
|
||||
- `y`
|
||||
- `yaw_deg`
|
||||
- `speed`
|
||||
- `policy`
|
||||
- `tolerance`
|
||||
|
||||
不保存 `z` 和 `action`。
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,146 @@
|
||||
<!doctype html>
|
||||
<html lang="zh-CN">
|
||||
<head>
|
||||
<meta charset="utf-8">
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1">
|
||||
<title>PCD Obstacle Terrain Editor</title>
|
||||
<link rel="stylesheet" href="./style.css?v=obstacle-terrain-2">
|
||||
</head>
|
||||
<body>
|
||||
<aside class="sidebar">
|
||||
<div class="title">
|
||||
<h1>Obstacle Terrain Editor</h1>
|
||||
<span id="serverState">obstacle-ui loading</span>
|
||||
</div>
|
||||
|
||||
<label>Map</label>
|
||||
<select id="mapSelect"></select>
|
||||
|
||||
<div id="layerControls" class="hidden">
|
||||
<label>Layer</label>
|
||||
<select id="layerSelect"></select>
|
||||
</div>
|
||||
|
||||
<label>View</label>
|
||||
<select id="viewSelect">
|
||||
<option value="3d" selected>3D map</option>
|
||||
<option value="2d">2D layers</option>
|
||||
</select>
|
||||
|
||||
<label>3D Mode</label>
|
||||
<select id="cloudModeSelect">
|
||||
<option value="points" selected>Point cloud</option>
|
||||
<option value="voxels">Occupied voxels</option>
|
||||
</select>
|
||||
|
||||
<div class="buttonRow">
|
||||
<button id="fitBtn" title="Fit map">Fit</button>
|
||||
<button id="reloadBtn" title="Reload maps">Reload</button>
|
||||
</div>
|
||||
|
||||
<section>
|
||||
<h2>Point Cloud</h2>
|
||||
<div class="formGrid">
|
||||
<label>Z Min</label><input id="cloudZMin" type="number" step="0.1" value="-2.0">
|
||||
<label>Z Max</label><input id="cloudZMax" type="number" step="0.1" value="1.0">
|
||||
<label>Max Pts</label><input id="cloudMaxPoints" type="number" step="1000" value="120000">
|
||||
<label>Voxel</label><input id="voxelSize" type="number" step="0.01" value="0.20">
|
||||
<label>Min Pts</label><input id="voxelMinPoints" type="number" step="1" value="1">
|
||||
<label>Cluster</label><input id="voxelMinCluster" type="number" step="1" value="1">
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<section>
|
||||
<h2>Cursor</h2>
|
||||
<pre id="cursorInfo">-</pre>
|
||||
</section>
|
||||
|
||||
<section>
|
||||
<h2>Waypoint</h2>
|
||||
<input id="routeName" placeholder="route name" value="test_route">
|
||||
<div class="formGrid">
|
||||
<label>X</label><input id="wpX" type="number" step="0.01">
|
||||
<label>Y</label><input id="wpY" type="number" step="0.01">
|
||||
<label>Yaw</label><input id="wpYaw" type="number" step="1" value="0">
|
||||
<label>Terrain</label><input id="wpObstacle" value="flat" readonly>
|
||||
<label>Speed</label><input id="wpSpeed" type="number" step="0.05" value="0.35">
|
||||
<label>Policy</label>
|
||||
<select id="wpPolicy">
|
||||
<option value="rough">rough</option>
|
||||
<option value="crawl">crawl</option>
|
||||
</select>
|
||||
<label>Tol</label><input id="wpTol" type="number" step="0.01" value="0.15">
|
||||
<label>Yaw Tol</label><input id="wpYawTol" type="number" step="1" value="30">
|
||||
<label>Require Yaw</label><input id="wpRequireYaw" type="checkbox">
|
||||
<label>PreDock Dist</label><input id="wpPreDockDist" type="number" step="0.01" value="0.35">
|
||||
<label>PreDock Tol</label><input id="wpPreDockTol" type="number" step="0.01" value="0.18">
|
||||
</div>
|
||||
<div class="buttonRow">
|
||||
<button id="updateWpBtn">Update</button>
|
||||
<button id="deleteWpBtn">Delete</button>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<section>
|
||||
<h2>Obstacle</h2>
|
||||
<label>Region</label>
|
||||
<select id="obstacleSelect"></select>
|
||||
<div class="formGrid">
|
||||
<label>Name</label><input id="obstacleName" value="obstacle_1">
|
||||
<label>Type</label>
|
||||
<select id="obstacleType">
|
||||
<option value="rough_pit">rough_pit</option>
|
||||
<option value="wall">wall</option>
|
||||
<option value="low_bar">low_bar</option>
|
||||
<option value="stairs">stairs</option>
|
||||
<option value="ramp">ramp</option>
|
||||
<option value="bridge_a">bridge_a</option>
|
||||
<option value="bridge_b">bridge_b</option>
|
||||
<option value="slalom">slalom</option>
|
||||
<option value="return_home">return_home</option>
|
||||
</select>
|
||||
<label>X</label><input id="obstacleX" type="number" step="0.01">
|
||||
<label>Y</label><input id="obstacleY" type="number" step="0.01">
|
||||
<label>Yaw</label><input id="obstacleYaw" type="number" step="1" value="0">
|
||||
<label>Length</label><input id="obstacleLength" type="number" step="0.05" value="1.00">
|
||||
<label>Width</label><input id="obstacleWidth" type="number" step="0.05" value="1.00">
|
||||
<label>Policy</label>
|
||||
<select id="obstaclePolicy">
|
||||
<option value="rough">rough</option>
|
||||
<option value="crawl">crawl</option>
|
||||
</select>
|
||||
</div>
|
||||
<div class="buttonRow">
|
||||
<button id="drawObstacleBtn">Draw</button>
|
||||
<button id="updateObstacleBtn">Update</button>
|
||||
<button id="deleteObstacleBtn">Delete</button>
|
||||
</div>
|
||||
<ol id="obstacleList"></ol>
|
||||
</section>
|
||||
|
||||
<section>
|
||||
<h2>Route</h2>
|
||||
<div class="buttonRow">
|
||||
<button id="clearRouteBtn">Clear</button>
|
||||
<button id="exportJsonBtn">Export</button>
|
||||
<button id="saveRouteBtn">Save</button>
|
||||
</div>
|
||||
<ol id="waypointList"></ol>
|
||||
</section>
|
||||
</aside>
|
||||
|
||||
<main class="main view-3d">
|
||||
<canvas id="mapCanvas"></canvas>
|
||||
<canvas id="cloudCanvas"></canvas>
|
||||
<div class="hud">
|
||||
<span>Click cloud: add/select waypoint</span>
|
||||
<span>Draw: drag 2D / click 3D obstacle</span>
|
||||
<span>Wheel: zoom</span>
|
||||
<span>Drag: orbit 3D / pan 2D</span>
|
||||
<span>Shift + drag: set yaw</span>
|
||||
</div>
|
||||
</main>
|
||||
|
||||
<script type="module" src="./app.js?v=obstacle-terrain-2"></script>
|
||||
</body>
|
||||
</html>
|
||||
@@ -0,0 +1,191 @@
|
||||
:root {
|
||||
color-scheme: dark;
|
||||
font-family: "Segoe UI", "Microsoft YaHei", Arial, sans-serif;
|
||||
background: #0d1117;
|
||||
color: #e5e7eb;
|
||||
}
|
||||
|
||||
* {
|
||||
box-sizing: border-box;
|
||||
}
|
||||
|
||||
body {
|
||||
margin: 0;
|
||||
height: 100vh;
|
||||
display: grid;
|
||||
grid-template-columns: 340px 1fr;
|
||||
overflow: hidden;
|
||||
}
|
||||
|
||||
.sidebar {
|
||||
background: #111827;
|
||||
border-right: 1px solid #263244;
|
||||
padding: 16px;
|
||||
overflow: auto;
|
||||
}
|
||||
|
||||
.title {
|
||||
display: flex;
|
||||
align-items: baseline;
|
||||
justify-content: space-between;
|
||||
gap: 12px;
|
||||
margin-bottom: 16px;
|
||||
}
|
||||
|
||||
h1 {
|
||||
font-size: 20px;
|
||||
margin: 0;
|
||||
letter-spacing: 0;
|
||||
}
|
||||
|
||||
h2 {
|
||||
font-size: 14px;
|
||||
margin: 18px 0 8px;
|
||||
color: #cbd5e1;
|
||||
letter-spacing: 0;
|
||||
}
|
||||
|
||||
label {
|
||||
display: block;
|
||||
margin: 10px 0 6px;
|
||||
color: #94a3b8;
|
||||
font-size: 12px;
|
||||
}
|
||||
|
||||
select,
|
||||
input,
|
||||
button {
|
||||
width: 100%;
|
||||
height: 32px;
|
||||
border: 1px solid #344256;
|
||||
background: #0b1220;
|
||||
color: #e5e7eb;
|
||||
border-radius: 6px;
|
||||
padding: 0 10px;
|
||||
font: inherit;
|
||||
}
|
||||
|
||||
button {
|
||||
cursor: pointer;
|
||||
background: #1f2937;
|
||||
}
|
||||
|
||||
button:hover {
|
||||
background: #2f3b4d;
|
||||
}
|
||||
|
||||
.buttonRow {
|
||||
display: grid;
|
||||
grid-template-columns: repeat(3, 1fr);
|
||||
gap: 8px;
|
||||
margin-top: 10px;
|
||||
}
|
||||
|
||||
.buttonRow:has(button:nth-child(2):last-child) {
|
||||
grid-template-columns: repeat(2, 1fr);
|
||||
}
|
||||
|
||||
.formGrid {
|
||||
display: grid;
|
||||
grid-template-columns: 62px 1fr;
|
||||
gap: 8px;
|
||||
align-items: center;
|
||||
}
|
||||
|
||||
.formGrid label {
|
||||
margin: 0;
|
||||
}
|
||||
|
||||
pre {
|
||||
min-height: 112px;
|
||||
margin: 0;
|
||||
padding: 10px;
|
||||
background: #070b12;
|
||||
border: 1px solid #263244;
|
||||
border-radius: 6px;
|
||||
white-space: pre-wrap;
|
||||
font-family: Consolas, monospace;
|
||||
font-size: 12px;
|
||||
color: #a7f3d0;
|
||||
}
|
||||
|
||||
#waypointList,
|
||||
#obstacleList {
|
||||
padding-left: 22px;
|
||||
margin: 10px 0 0;
|
||||
}
|
||||
|
||||
#waypointList li,
|
||||
#obstacleList li {
|
||||
padding: 6px 4px;
|
||||
border-bottom: 1px solid #263244;
|
||||
cursor: pointer;
|
||||
font-size: 12px;
|
||||
}
|
||||
|
||||
#waypointList li.active,
|
||||
#obstacleList li.active {
|
||||
color: #67e8f9;
|
||||
background: #102033;
|
||||
}
|
||||
|
||||
.main {
|
||||
position: relative;
|
||||
background: #0b0f17;
|
||||
overflow: hidden;
|
||||
}
|
||||
|
||||
#mapCanvas,
|
||||
#cloudCanvas {
|
||||
position: absolute;
|
||||
inset: 0;
|
||||
display: block;
|
||||
width: 100%;
|
||||
height: 100%;
|
||||
cursor: crosshair;
|
||||
}
|
||||
|
||||
.main.view-3d #cloudCanvas {
|
||||
z-index: 1;
|
||||
}
|
||||
|
||||
.main.view-3d #mapCanvas {
|
||||
z-index: 2;
|
||||
background: transparent;
|
||||
pointer-events: none;
|
||||
}
|
||||
|
||||
.main.view-2d #mapCanvas {
|
||||
z-index: 1;
|
||||
pointer-events: auto;
|
||||
}
|
||||
|
||||
.main.view-2d #cloudCanvas {
|
||||
z-index: 0;
|
||||
}
|
||||
|
||||
#mapCanvas.hidden,
|
||||
#cloudCanvas.hidden {
|
||||
display: none;
|
||||
}
|
||||
|
||||
.hud {
|
||||
position: absolute;
|
||||
z-index: 3;
|
||||
left: 12px;
|
||||
bottom: 12px;
|
||||
display: flex;
|
||||
gap: 8px;
|
||||
flex-wrap: wrap;
|
||||
pointer-events: none;
|
||||
}
|
||||
|
||||
.hud span,
|
||||
#serverState {
|
||||
border: 1px solid #344256;
|
||||
background: rgba(8, 13, 22, 0.82);
|
||||
color: #cbd5e1;
|
||||
border-radius: 6px;
|
||||
padding: 5px 8px;
|
||||
font-size: 12px;
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,782 @@
|
||||
#!/usr/bin/env python3
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import html
|
||||
import http.server
|
||||
import json
|
||||
import math
|
||||
import socket
|
||||
import socketserver
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
@dataclass
|
||||
class RoutePoint:
|
||||
id: int
|
||||
x: float
|
||||
y: float
|
||||
yaw_deg: float | None
|
||||
segment: str
|
||||
|
||||
|
||||
SVG_SEGMENT_COLORS = [
|
||||
"#d81b60",
|
||||
"#1e88e5",
|
||||
"#43a047",
|
||||
"#fb8c00",
|
||||
"#8e24aa",
|
||||
"#00897b",
|
||||
"#6d4c41",
|
||||
"#546e7a",
|
||||
"#e53935",
|
||||
"#3949ab",
|
||||
]
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Preview an ASCII PCD map with route points and yaw arrows."
|
||||
)
|
||||
parser.add_argument(
|
||||
"--pcd",
|
||||
default="map/map_b.pcd",
|
||||
help="Path to the ASCII PCD file. Default: map/map_b.pcd",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--route",
|
||||
default="tools/test_route.json",
|
||||
help="Path to the route JSON file. Default: tools/test_route.json",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--floor-z-min",
|
||||
type=float,
|
||||
default=-1.6,
|
||||
help="Minimum z value kept from the PCD floor points.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--floor-z-max",
|
||||
type=float,
|
||||
default=0.4,
|
||||
help="Maximum z value kept from the PCD floor points.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--sample-step",
|
||||
type=int,
|
||||
default=25,
|
||||
help="Keep one point every N points after filtering.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--point-size",
|
||||
type=float,
|
||||
default=1.0,
|
||||
help="Scatter point size for the PCD map.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--arrow-len",
|
||||
type=float,
|
||||
default=0.35,
|
||||
help="Arrow length used to visualize waypoint yaw.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--show-id",
|
||||
action="store_true",
|
||||
help="Draw waypoint id labels.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--show-yaw-text",
|
||||
action="store_true",
|
||||
help="Draw yawDeg text next to each waypoint.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--connect",
|
||||
action="store_true",
|
||||
help="Connect route points in order.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--renderer",
|
||||
choices=["auto", "mpl", "html"],
|
||||
default="html",
|
||||
help="Rendering backend. Default: html.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--output",
|
||||
default="tools/pcd_route_preview.html",
|
||||
help="Output html file used by the html renderer.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--width",
|
||||
type=int,
|
||||
default=1400,
|
||||
help="Canvas width for the html/svg renderer.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--height",
|
||||
type=int,
|
||||
default=980,
|
||||
help="Canvas height for the html/svg renderer.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--serve",
|
||||
action="store_true",
|
||||
help="Serve the generated html over HTTP after rendering.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--host",
|
||||
default="0.0.0.0",
|
||||
help="Host used by the built-in HTTP server. Default: 0.0.0.0",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--port",
|
||||
type=int,
|
||||
default=8000,
|
||||
help="Port used by the built-in HTTP server. Default: 8000",
|
||||
)
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def load_ascii_pcd_xy(
|
||||
path: Path,
|
||||
floor_z_min: float,
|
||||
floor_z_max: float,
|
||||
sample_step: int,
|
||||
) -> list[tuple[float, float]]:
|
||||
if not path.exists():
|
||||
raise FileNotFoundError(f"PCD file not found: {path}")
|
||||
|
||||
points: list[tuple[float, float]] = []
|
||||
data_started = False
|
||||
fields: list[str] = []
|
||||
x_index = 0
|
||||
y_index = 1
|
||||
z_index = 2
|
||||
|
||||
with path.open("r", encoding="utf-8") as f:
|
||||
for line in f:
|
||||
stripped = line.strip()
|
||||
if not stripped:
|
||||
continue
|
||||
|
||||
if not data_started:
|
||||
upper = stripped.upper()
|
||||
if upper.startswith("FIELDS "):
|
||||
fields = stripped.split()[1:]
|
||||
if {"x", "y", "z"}.issubset(set(fields)):
|
||||
x_index = fields.index("x")
|
||||
y_index = fields.index("y")
|
||||
z_index = fields.index("z")
|
||||
elif upper.startswith("DATA"):
|
||||
if "ascii" not in stripped.lower():
|
||||
raise RuntimeError("Only ASCII PCD is supported.")
|
||||
data_started = True
|
||||
continue
|
||||
|
||||
parts = stripped.split()
|
||||
needed_index = max(x_index, y_index, z_index)
|
||||
if len(parts) <= needed_index:
|
||||
continue
|
||||
|
||||
try:
|
||||
x = float(parts[x_index])
|
||||
y = float(parts[y_index])
|
||||
z = float(parts[z_index])
|
||||
except ValueError:
|
||||
continue
|
||||
|
||||
if floor_z_min <= z <= floor_z_max:
|
||||
points.append((x, y))
|
||||
|
||||
if not points:
|
||||
raise RuntimeError("No usable floor points found after z filtering.")
|
||||
|
||||
return points[:: max(1, sample_step)]
|
||||
|
||||
|
||||
def load_route_points(path: Path) -> list[RoutePoint]:
|
||||
if not path.exists():
|
||||
raise FileNotFoundError(f"Route file not found: {path}")
|
||||
|
||||
with path.open("r", encoding="utf-8") as f:
|
||||
data = json.load(f)
|
||||
|
||||
raw_segments = data.get("segments", [])
|
||||
if not isinstance(raw_segments, list) or not raw_segments:
|
||||
top_level_waypoints = data.get("waypoints", [])
|
||||
if isinstance(top_level_waypoints, list) and top_level_waypoints:
|
||||
raw_segments = [
|
||||
{
|
||||
"name": "segment_1",
|
||||
"waypoints": top_level_waypoints,
|
||||
}
|
||||
]
|
||||
else:
|
||||
raise RuntimeError("Route JSON has no usable segments or waypoints.")
|
||||
|
||||
points: list[RoutePoint] = []
|
||||
fallback_id = 1
|
||||
for segment_index, segment in enumerate(raw_segments, start=1):
|
||||
if not isinstance(segment, dict):
|
||||
continue
|
||||
segment_name = str(segment.get("name", f"segment_{segment_index}")).strip() or f"segment_{segment_index}"
|
||||
raw_waypoints = segment.get("waypoints", [])
|
||||
if not isinstance(raw_waypoints, list):
|
||||
continue
|
||||
|
||||
for waypoint in raw_waypoints:
|
||||
if not isinstance(waypoint, dict):
|
||||
continue
|
||||
try:
|
||||
x = float(waypoint["x"])
|
||||
y = float(waypoint["y"])
|
||||
except (KeyError, TypeError, ValueError):
|
||||
continue
|
||||
|
||||
yaw_deg = waypoint.get("yawDeg", waypoint.get("yaw_deg"))
|
||||
if yaw_deg is not None:
|
||||
try:
|
||||
yaw_deg = float(yaw_deg)
|
||||
except (TypeError, ValueError):
|
||||
yaw_deg = None
|
||||
|
||||
point_id = waypoint.get("id", fallback_id)
|
||||
try:
|
||||
point_id = int(point_id)
|
||||
except (TypeError, ValueError):
|
||||
point_id = fallback_id
|
||||
|
||||
points.append(
|
||||
RoutePoint(
|
||||
id=point_id,
|
||||
x=x,
|
||||
y=y,
|
||||
yaw_deg=yaw_deg,
|
||||
segment=segment_name,
|
||||
)
|
||||
)
|
||||
fallback_id += 1
|
||||
|
||||
if not points:
|
||||
raise RuntimeError("Route JSON contains no valid waypoint coordinates.")
|
||||
|
||||
return points
|
||||
|
||||
|
||||
def plot_preview(
|
||||
map_points: list[tuple[float, float]],
|
||||
route_points: list[RoutePoint],
|
||||
point_size: float,
|
||||
arrow_len: float,
|
||||
show_id: bool,
|
||||
show_yaw_text: bool,
|
||||
connect: bool,
|
||||
title: str,
|
||||
) -> None:
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
fig, ax = plt.subplots(figsize=(12, 9))
|
||||
|
||||
map_x = [p[0] for p in map_points]
|
||||
map_y = [p[1] for p in map_points]
|
||||
ax.scatter(map_x, map_y, s=point_size, c="black", alpha=0.35, label="PCD floor")
|
||||
|
||||
segment_names: list[str] = []
|
||||
for point in route_points:
|
||||
if point.segment not in segment_names:
|
||||
segment_names.append(point.segment)
|
||||
|
||||
segment_colors = {
|
||||
name: plt.cm.tab10(index % 10) for index, name in enumerate(segment_names)
|
||||
}
|
||||
|
||||
for index, point in enumerate(route_points):
|
||||
color = segment_colors[point.segment]
|
||||
ax.scatter([point.x], [point.y], s=55, c=[color], edgecolors="white", linewidths=0.8)
|
||||
|
||||
if connect and index > 0:
|
||||
prev = route_points[index - 1]
|
||||
ax.plot([prev.x, point.x], [prev.y, point.y], color=color, linewidth=1.4, alpha=0.9)
|
||||
|
||||
if point.yaw_deg is not None:
|
||||
yaw_rad = math.radians(point.yaw_deg)
|
||||
dx = arrow_len * math.cos(yaw_rad)
|
||||
dy = arrow_len * math.sin(yaw_rad)
|
||||
ax.arrow(
|
||||
point.x,
|
||||
point.y,
|
||||
dx,
|
||||
dy,
|
||||
width=0.018,
|
||||
head_width=0.12,
|
||||
head_length=0.12,
|
||||
length_includes_head=True,
|
||||
color=color,
|
||||
alpha=0.95,
|
||||
)
|
||||
|
||||
label_parts: list[str] = []
|
||||
if show_id:
|
||||
label_parts.append(str(point.id))
|
||||
if show_yaw_text and point.yaw_deg is not None:
|
||||
label_parts.append(f"{point.yaw_deg:.1f}deg")
|
||||
if label_parts:
|
||||
ax.text(
|
||||
point.x + 0.05,
|
||||
point.y + 0.05,
|
||||
" | ".join(label_parts),
|
||||
color=color,
|
||||
fontsize=9,
|
||||
weight="bold",
|
||||
)
|
||||
|
||||
ax.set_title(title)
|
||||
ax.set_xlabel("map x")
|
||||
ax.set_ylabel("map y")
|
||||
ax.set_aspect("equal", adjustable="box")
|
||||
ax.grid(True, alpha=0.2)
|
||||
plt.tight_layout()
|
||||
plt.show()
|
||||
|
||||
|
||||
def compute_bounds(
|
||||
map_points: list[tuple[float, float]],
|
||||
route_points: list[RoutePoint],
|
||||
) -> tuple[float, float, float, float]:
|
||||
xs = [p[0] for p in map_points] + [p.x for p in route_points]
|
||||
ys = [p[1] for p in map_points] + [p.y for p in route_points]
|
||||
min_x = min(xs)
|
||||
max_x = max(xs)
|
||||
min_y = min(ys)
|
||||
max_y = max(ys)
|
||||
|
||||
if math.isclose(min_x, max_x):
|
||||
max_x = min_x + 1.0
|
||||
if math.isclose(min_y, max_y):
|
||||
max_y = min_y + 1.0
|
||||
return min_x, max_x, min_y, max_y
|
||||
|
||||
|
||||
def build_segment_color_map(route_points: list[RoutePoint]) -> dict[str, str]:
|
||||
segment_names: list[str] = []
|
||||
for point in route_points:
|
||||
if point.segment not in segment_names:
|
||||
segment_names.append(point.segment)
|
||||
return {
|
||||
name: SVG_SEGMENT_COLORS[index % len(SVG_SEGMENT_COLORS)]
|
||||
for index, name in enumerate(segment_names)
|
||||
}
|
||||
|
||||
|
||||
def compute_canvas_transform(
|
||||
min_x: float,
|
||||
max_x: float,
|
||||
min_y: float,
|
||||
max_y: float,
|
||||
width: int,
|
||||
height: int,
|
||||
padding: int,
|
||||
) -> tuple[float, float, float, float, float]:
|
||||
usable_width = max(1.0, float(width - padding * 2))
|
||||
usable_height = max(1.0, float(height - padding * 2))
|
||||
scale_x = usable_width / max(1e-9, max_x - min_x)
|
||||
scale_y = usable_height / max(1e-9, max_y - min_y)
|
||||
scale = min(scale_x, scale_y)
|
||||
|
||||
draw_width = (max_x - min_x) * scale
|
||||
draw_height = (max_y - min_y) * scale
|
||||
offset_x = padding + (usable_width - draw_width) * 0.5
|
||||
offset_y = padding + (usable_height - draw_height) * 0.5
|
||||
return scale, draw_width, draw_height, offset_x, offset_y
|
||||
|
||||
|
||||
def map_to_canvas(
|
||||
x: float,
|
||||
y: float,
|
||||
min_x: float,
|
||||
max_x: float,
|
||||
min_y: float,
|
||||
max_y: float,
|
||||
width: int,
|
||||
height: int,
|
||||
padding: int,
|
||||
) -> tuple[float, float]:
|
||||
scale, _draw_width, _draw_height, offset_x, offset_y = compute_canvas_transform(
|
||||
min_x=min_x,
|
||||
max_x=max_x,
|
||||
min_y=min_y,
|
||||
max_y=max_y,
|
||||
width=width,
|
||||
height=height,
|
||||
padding=padding,
|
||||
)
|
||||
|
||||
canvas_x = offset_x + (x - min_x) * scale
|
||||
canvas_y = height - (offset_y + (y - min_y) * scale)
|
||||
return canvas_x, canvas_y
|
||||
|
||||
|
||||
def svg_arrow_polygon(
|
||||
start_x: float,
|
||||
start_y: float,
|
||||
end_x: float,
|
||||
end_y: float,
|
||||
color: str,
|
||||
) -> str:
|
||||
dx = end_x - start_x
|
||||
dy = end_y - start_y
|
||||
length = math.hypot(dx, dy)
|
||||
if length < 1e-6:
|
||||
return ""
|
||||
|
||||
ux = dx / length
|
||||
uy = dy / length
|
||||
px = -uy
|
||||
py = ux
|
||||
|
||||
head_len = min(16.0, max(8.0, length * 0.35))
|
||||
shaft_half = 2.5
|
||||
head_half = 7.0
|
||||
|
||||
base_x = end_x - ux * head_len
|
||||
base_y = end_y - uy * head_len
|
||||
|
||||
p1 = (start_x + px * shaft_half, start_y + py * shaft_half)
|
||||
p2 = (base_x + px * shaft_half, base_y + py * shaft_half)
|
||||
p3 = (base_x + px * head_half, base_y + py * head_half)
|
||||
p4 = (end_x, end_y)
|
||||
p5 = (base_x - px * head_half, base_y - py * head_half)
|
||||
p6 = (base_x - px * shaft_half, base_y - py * shaft_half)
|
||||
p7 = (start_x - px * shaft_half, start_y - py * shaft_half)
|
||||
|
||||
points_text = " ".join(f"{x:.2f},{y:.2f}" for x, y in [p1, p2, p3, p4, p5, p6, p7])
|
||||
return f'<polygon points="{points_text}" fill="{color}" fill-opacity="0.95" />'
|
||||
|
||||
|
||||
def write_html_preview(
|
||||
output_path: Path,
|
||||
map_points: list[tuple[float, float]],
|
||||
route_points: list[RoutePoint],
|
||||
point_size: float,
|
||||
arrow_len: float,
|
||||
show_id: bool,
|
||||
show_yaw_text: bool,
|
||||
connect: bool,
|
||||
title: str,
|
||||
width: int,
|
||||
height: int,
|
||||
) -> None:
|
||||
output_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
min_x, max_x, min_y, max_y = compute_bounds(map_points, route_points)
|
||||
padding = 48
|
||||
segment_colors = build_segment_color_map(route_points)
|
||||
scale, draw_width, draw_height, offset_x, offset_y = compute_canvas_transform(
|
||||
min_x=min_x,
|
||||
max_x=max_x,
|
||||
min_y=min_y,
|
||||
max_y=max_y,
|
||||
width=width,
|
||||
height=height,
|
||||
padding=padding,
|
||||
)
|
||||
|
||||
svg_parts: list[str] = []
|
||||
svg_parts.append(
|
||||
f'<svg id="pcd-map-svg" viewBox="0 0 {width} {height}" width="{width}" height="{height}" '
|
||||
f'data-min-x="{min_x:.10f}" data-min-y="{min_y:.10f}" data-scale="{scale:.10f}" '
|
||||
f'data-offset-x="{offset_x:.10f}" data-offset-y="{offset_y:.10f}" '
|
||||
f'data-canvas-height="{float(height):.10f}" data-draw-width="{draw_width:.10f}" '
|
||||
f'data-draw-height="{draw_height:.10f}" xmlns="http://www.w3.org/2000/svg">'
|
||||
)
|
||||
svg_parts.append(f'<rect x="0" y="0" width="{width}" height="{height}" fill="#f7f7f5" />')
|
||||
|
||||
for x, y in map_points:
|
||||
cx, cy = map_to_canvas(x, y, min_x, max_x, min_y, max_y, width, height, padding)
|
||||
radius = max(0.35, point_size * 0.7)
|
||||
svg_parts.append(
|
||||
f'<circle cx="{cx:.2f}" cy="{cy:.2f}" r="{radius:.2f}" fill="#1f1f1f" fill-opacity="0.35" />'
|
||||
)
|
||||
|
||||
if connect:
|
||||
for prev, curr in zip(route_points, route_points[1:]):
|
||||
color = segment_colors[curr.segment]
|
||||
x1, y1 = map_to_canvas(prev.x, prev.y, min_x, max_x, min_y, max_y, width, height, padding)
|
||||
x2, y2 = map_to_canvas(curr.x, curr.y, min_x, max_x, min_y, max_y, width, height, padding)
|
||||
svg_parts.append(
|
||||
f'<line x1="{x1:.2f}" y1="{y1:.2f}" x2="{x2:.2f}" y2="{y2:.2f}" '
|
||||
f'stroke="{color}" stroke-width="2" stroke-opacity="0.85" />'
|
||||
)
|
||||
|
||||
for point in route_points:
|
||||
color = segment_colors[point.segment]
|
||||
cx, cy = map_to_canvas(point.x, point.y, min_x, max_x, min_y, max_y, width, height, padding)
|
||||
svg_parts.append(
|
||||
f'<circle class="route-point" data-id="{point.id}" data-segment="{html.escape(point.segment)}" '
|
||||
f'data-map-x="{point.x:.6f}" data-map-y="{point.y:.6f}" '
|
||||
+ (
|
||||
f'data-yaw-deg="{point.yaw_deg:.3f}" '
|
||||
if point.yaw_deg is not None else
|
||||
""
|
||||
)
|
||||
+ f'cx="{cx:.2f}" cy="{cy:.2f}" r="6.5" fill="{color}" stroke="#ffffff" stroke-width="1.5">'
|
||||
f"<title>ID {point.id} | x={point.x:.3f} y={point.y:.3f}"
|
||||
+ (f" | yaw={point.yaw_deg:.1f}deg" if point.yaw_deg is not None else "")
|
||||
+ "</title></circle>"
|
||||
)
|
||||
|
||||
if point.yaw_deg is not None:
|
||||
yaw_rad = math.radians(point.yaw_deg)
|
||||
end_x, end_y = map_to_canvas(
|
||||
point.x + arrow_len * math.cos(yaw_rad),
|
||||
point.y + arrow_len * math.sin(yaw_rad),
|
||||
min_x,
|
||||
max_x,
|
||||
min_y,
|
||||
max_y,
|
||||
width,
|
||||
height,
|
||||
padding,
|
||||
)
|
||||
svg_parts.append(svg_arrow_polygon(cx, cy, end_x, end_y, color))
|
||||
|
||||
label_parts: list[str] = []
|
||||
if show_id:
|
||||
label_parts.append(str(point.id))
|
||||
if show_yaw_text and point.yaw_deg is not None:
|
||||
label_parts.append(f"{point.yaw_deg:.1f}deg")
|
||||
if label_parts:
|
||||
svg_parts.append(
|
||||
f'<text x="{cx + 8:.2f}" y="{cy - 8:.2f}" font-size="12" font-weight="700" '
|
||||
f'fill="{color}">{html.escape(" | ".join(label_parts))}</text>'
|
||||
)
|
||||
|
||||
svg_parts.append(
|
||||
f'<text x="24" y="30" font-size="20" font-weight="700" fill="#222">{html.escape(title)}</text>'
|
||||
)
|
||||
svg_parts.append(
|
||||
f'<text x="24" y="{height - 24}" font-size="13" fill="#444">'
|
||||
f'{html.escape(f"route points: {len(route_points)} | pcd samples: {len(map_points)} | scale: {scale:.2f}px/m")}'
|
||||
"</text>"
|
||||
)
|
||||
svg_parts.append(
|
||||
'<g id="click-marker" visibility="hidden">'
|
||||
'<circle cx="0" cy="0" r="8" fill="none" stroke="#ff1744" stroke-width="2" />'
|
||||
'<line x1="-12" y1="0" x2="12" y2="0" stroke="#ff1744" stroke-width="2" />'
|
||||
'<line x1="0" y1="-12" x2="0" y2="12" stroke="#ff1744" stroke-width="2" />'
|
||||
"</g>"
|
||||
)
|
||||
svg_parts.append("</svg>")
|
||||
|
||||
html_text = "\n".join(
|
||||
[
|
||||
"<!DOCTYPE html>",
|
||||
'<html lang="zh-CN">',
|
||||
"<head>",
|
||||
'<meta charset="utf-8" />',
|
||||
f"<title>{html.escape(title)}</title>",
|
||||
"<style>",
|
||||
"body { margin: 0; background: #ece9e1; font-family: 'Segoe UI', sans-serif; color: #222; }",
|
||||
".wrap { padding: 20px; }",
|
||||
".panel { background: #ffffff; border-radius: 12px; box-shadow: 0 10px 30px rgba(0,0,0,0.10); overflow: auto; }",
|
||||
".toolbar { display: flex; flex-wrap: wrap; gap: 12px 18px; align-items: center; padding: 14px 20px 0 20px; font-size: 14px; }",
|
||||
".toolbar strong { color: #111; }",
|
||||
".meta { padding: 10px 20px 6px 20px; color: #444; font-size: 14px; }",
|
||||
".chip { display: inline-flex; align-items: center; gap: 8px; padding: 6px 10px; border-radius: 999px; background: #f3f1eb; }",
|
||||
".swatch { width: 12px; height: 12px; border-radius: 50%; display: inline-block; }",
|
||||
"#click-coords { font-family: Consolas, 'Courier New', monospace; }",
|
||||
"#point-detail { font-family: Consolas, 'Courier New', monospace; }",
|
||||
"svg { display: block; margin: 0 auto; cursor: crosshair; user-select: none; }",
|
||||
".hint { padding: 0 20px 12px 20px; color: #666; font-size: 13px; }",
|
||||
"</style>",
|
||||
"</head>",
|
||||
"<body>",
|
||||
'<div class="wrap">',
|
||||
'<div class="panel">',
|
||||
'<div class="toolbar">',
|
||||
'<div class="chip"><strong>点击坐标</strong><span id="click-coords">尚未点击</span></div>',
|
||||
'<div class="chip"><strong>点位信息</strong><span id="point-detail">点击路线点可查看 id / yaw</span></div>',
|
||||
"</div>",
|
||||
'<div class="meta">这是纯浏览器预览页。点击 PCD 或空白位置会显示当前地图坐标，点击路线点还会显示该点的 id、segment 和 yaw。</div>',
|
||||
'<div class="hint">浏览器可直接缩放页面查看细节，图上的红色十字为你最近一次点击的位置。</div>',
|
||||
"\n".join(svg_parts),
|
||||
"<script>",
|
||||
"(() => {",
|
||||
" const svg = document.getElementById('pcd-map-svg');",
|
||||
" const marker = document.getElementById('click-marker');",
|
||||
" const clickCoords = document.getElementById('click-coords');",
|
||||
" const pointDetail = document.getElementById('point-detail');",
|
||||
" const minX = parseFloat(svg.dataset.minX);",
|
||||
" const minY = parseFloat(svg.dataset.minY);",
|
||||
" const scale = parseFloat(svg.dataset.scale);",
|
||||
" const offsetX = parseFloat(svg.dataset.offsetX);",
|
||||
" const offsetY = parseFloat(svg.dataset.offsetY);",
|
||||
" const canvasHeight = parseFloat(svg.dataset.canvasHeight);",
|
||||
" function svgPointFromEvent(evt) {",
|
||||
" const pt = svg.createSVGPoint();",
|
||||
" pt.x = evt.clientX;",
|
||||
" pt.y = evt.clientY;",
|
||||
" return pt.matrixTransform(svg.getScreenCTM().inverse());",
|
||||
" }",
|
||||
" function svgToMap(px, py) {",
|
||||
" const mapX = minX + (px - offsetX) / scale;",
|
||||
" const mapY = minY + ((canvasHeight - py) - offsetY) / scale;",
|
||||
" return { x: mapX, y: mapY };",
|
||||
" }",
|
||||
" function formatNum(v) {",
|
||||
" return Number.isFinite(v) ? v.toFixed(3) : 'NaN';",
|
||||
" }",
|
||||
" function updateFromEvent(evt) {",
|
||||
" const svgPoint = svgPointFromEvent(evt);",
|
||||
" const mapPoint = svgToMap(svgPoint.x, svgPoint.y);",
|
||||
" marker.setAttribute('transform', `translate(${svgPoint.x} ${svgPoint.y})`);",
|
||||
" marker.setAttribute('visibility', 'visible');",
|
||||
" clickCoords.textContent = `x=${formatNum(mapPoint.x)}, y=${formatNum(mapPoint.y)}`;",
|
||||
" const routePoint = evt.target.closest('.route-point');",
|
||||
" if (routePoint) {",
|
||||
" const yaw = routePoint.dataset.yawDeg;",
|
||||
" pointDetail.textContent = `id=${routePoint.dataset.id}, segment=${routePoint.dataset.segment}, x=${routePoint.dataset.mapX}, y=${routePoint.dataset.mapY}` + (yaw ? `, yaw=${Number(yaw).toFixed(1)}deg` : '');",
|
||||
" } else {",
|
||||
" pointDetail.textContent = '未点击路线点';",
|
||||
" }",
|
||||
" }",
|
||||
" svg.addEventListener('click', updateFromEvent);",
|
||||
"})();",
|
||||
"</script>",
|
||||
"</div>",
|
||||
"</div>",
|
||||
"</body>",
|
||||
"</html>",
|
||||
]
|
||||
)
|
||||
|
||||
output_path.write_text(html_text, encoding="utf-8")
|
||||
|
||||
|
||||
def guess_local_ip() -> str:
|
||||
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
try:
|
||||
sock.connect(("8.8.8.8", 80))
|
||||
return str(sock.getsockname()[0])
|
||||
except OSError:
|
||||
return "127.0.0.1"
|
||||
finally:
|
||||
sock.close()
|
||||
|
||||
|
||||
def serve_output_file(output_path: Path, host: str, port: int) -> None:
|
||||
output_path = output_path.resolve()
|
||||
web_root = output_path.parent.resolve()
|
||||
relative_url = output_path.relative_to(web_root).as_posix()
|
||||
|
||||
class PreviewHttpHandler(http.server.SimpleHTTPRequestHandler):
|
||||
def __init__(self, *args, **kwargs):
|
||||
super().__init__(*args, directory=str(web_root), **kwargs)
|
||||
|
||||
server_address = (host, int(port))
|
||||
with socketserver.TCPServer(server_address, PreviewHttpHandler) as httpd:
|
||||
local_ip = guess_local_ip()
|
||||
bound_host = host if host not in {"0.0.0.0", "::"} else local_ip
|
||||
print("Preview server is running.")
|
||||
print(f"Local URL: http://127.0.0.1:{port}/{relative_url}")
|
||||
print(f"LAN URL: http://{bound_host}:{port}/{relative_url}")
|
||||
print("Open the LAN URL from your Windows browser.")
|
||||
print("Press Ctrl+C to stop the server.")
|
||||
try:
|
||||
httpd.serve_forever()
|
||||
except KeyboardInterrupt:
|
||||
print("\nServer stopped.")
|
||||
|
||||
|
||||
def render_preview(
|
||||
renderer: str,
|
||||
output_path: Path,
|
||||
map_points: list[tuple[float, float]],
|
||||
route_points: list[RoutePoint],
|
||||
point_size: float,
|
||||
arrow_len: float,
|
||||
show_id: bool,
|
||||
show_yaw_text: bool,
|
||||
connect: bool,
|
||||
title: str,
|
||||
width: int,
|
||||
height: int,
|
||||
) -> str:
|
||||
if renderer in {"auto", "mpl"}:
|
||||
try:
|
||||
plot_preview(
|
||||
map_points=map_points,
|
||||
route_points=route_points,
|
||||
point_size=point_size,
|
||||
arrow_len=arrow_len,
|
||||
show_id=show_id,
|
||||
show_yaw_text=show_yaw_text,
|
||||
connect=connect,
|
||||
title=title,
|
||||
)
|
||||
return "mpl"
|
||||
except Exception as exc:
|
||||
if renderer == "mpl":
|
||||
raise RuntimeError(
|
||||
"matplotlib 渲染失败。当前环境很可能存在 numpy / matplotlib 二进制不兼容问题。"
|
||||
) from exc
|
||||
print(f"[info] matplotlib 不可用，自动切换到 html 渲染: {exc}")
|
||||
|
||||
write_html_preview(
|
||||
output_path=output_path,
|
||||
map_points=map_points,
|
||||
route_points=route_points,
|
||||
point_size=point_size,
|
||||
arrow_len=arrow_len,
|
||||
show_id=show_id,
|
||||
show_yaw_text=show_yaw_text,
|
||||
connect=connect,
|
||||
title=title,
|
||||
width=width,
|
||||
height=height,
|
||||
)
|
||||
return "html"
|
||||
|
||||
|
||||
def main() -> None:
|
||||
args = parse_args()
|
||||
pcd_path = Path(args.pcd)
|
||||
route_path = Path(args.route)
|
||||
output_path = Path(args.output)
|
||||
|
||||
map_points = load_ascii_pcd_xy(
|
||||
path=pcd_path,
|
||||
floor_z_min=float(args.floor_z_min),
|
||||
floor_z_max=float(args.floor_z_max),
|
||||
sample_step=max(1, int(args.sample_step)),
|
||||
)
|
||||
route_points = load_route_points(route_path)
|
||||
|
||||
title = f"PCD Route Preview: {pcd_path.name} + {route_path.name}"
|
||||
used_renderer = render_preview(
|
||||
renderer=str(args.renderer),
|
||||
output_path=output_path,
|
||||
map_points=map_points,
|
||||
route_points=route_points,
|
||||
point_size=float(args.point_size),
|
||||
arrow_len=float(args.arrow_len),
|
||||
show_id=bool(args.show_id),
|
||||
show_yaw_text=bool(args.show_yaw_text),
|
||||
connect=bool(args.connect),
|
||||
title=title,
|
||||
width=max(600, int(args.width)),
|
||||
height=max(400, int(args.height)),
|
||||
)
|
||||
if used_renderer == "html":
|
||||
print(f"HTML preview written to: {output_path.resolve()}")
|
||||
if bool(args.serve):
|
||||
if used_renderer != "html":
|
||||
raise RuntimeError("Built-in server currently supports html output only.")
|
||||
serve_output_file(
|
||||
output_path=output_path,
|
||||
host=str(args.host),
|
||||
port=int(args.port),
|
||||
)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,204 @@
|
||||
{
|
||||
"name": "test_route",
|
||||
"map": "map_b",
|
||||
"frame_id": "map",
|
||||
"yawToleranceDegDefault": 30.0,
|
||||
"createdAt": "2026-07-02T12:18:22.647Z",
|
||||
"segments": [
|
||||
{
|
||||
"name": "segment_1",
|
||||
"obstacle": "slalom",
|
||||
"waypoints": [
|
||||
{
|
||||
"id": 1,
|
||||
"x": 5.359,
|
||||
"y": 2.442,
|
||||
"yawDeg": 0,
|
||||
"speed": 0.4,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 2,
|
||||
"x": 6.924,
|
||||
"y": 2.388,
|
||||
"yawDeg": -90,
|
||||
"speed": 0.4,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 3,
|
||||
"x": 6.827,
|
||||
"y": 0.712,
|
||||
"yawDeg": -5,
|
||||
"speed": 0.4,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 4,
|
||||
"x": 9.368,
|
||||
"y": 0.706,
|
||||
"yawDeg": -3,
|
||||
"speed": 0.4,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 5,
|
||||
"x": 11.373,
|
||||
"y": 0.43,
|
||||
"yawDeg": 0,
|
||||
"speed": 0.4,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 6,
|
||||
"x": 12.877,
|
||||
"y": 0.432,
|
||||
"yawDeg": 90,
|
||||
"speed": 0.4,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.1
|
||||
},
|
||||
{
|
||||
"id": 7,
|
||||
"x": 12.935,
|
||||
"y": 2.241,
|
||||
"yawDeg": 88.2,
|
||||
"speed": 0.4,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.1
|
||||
},
|
||||
{
|
||||
"id": 8,
|
||||
"x": 12.991,
|
||||
"y": 4.277,
|
||||
"yawDeg": 180,
|
||||
"speed": 0.4,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.1
|
||||
},
|
||||
{
|
||||
"id": 9,
|
||||
"x": 12.004,
|
||||
"y": 4.339,
|
||||
"yawDeg": -90,
|
||||
"speed": 0.4,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 10,
|
||||
"x": 11.872,
|
||||
"y": 2.583,
|
||||
"yawDeg": 180,
|
||||
"speed": 0.4,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 11,
|
||||
"x": 9.5719,
|
||||
"y": 2.7972,
|
||||
"yawDeg": 174.7,
|
||||
"speed": 0.4,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 12,
|
||||
"x": 9.508,
|
||||
"y": 3.562,
|
||||
"yawDeg": 180,
|
||||
"speed": 0.4,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 13,
|
||||
"x": 8.414,
|
||||
"y": 3.573,
|
||||
"yawDeg": 125,
|
||||
"speed": 0.4,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 14,
|
||||
"x": 7.867,
|
||||
"y": 4.528,
|
||||
"yawDeg": 180,
|
||||
"speed": 0.4,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 15,
|
||||
"x": 4.475,
|
||||
"y": 4.743,
|
||||
"yawDeg": 180,
|
||||
"speed": 0.4,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 16,
|
||||
"x": 4.926,
|
||||
"y": 4.785,
|
||||
"yawDeg": 180,
|
||||
"speed": 0.4,
|
||||
"policy": "crawl",
|
||||
"tolerance": 0.1
|
||||
},
|
||||
{
|
||||
"id": 17,
|
||||
"x": 3.2951,
|
||||
"y": 4.7963,
|
||||
"yawDeg": 179.6,
|
||||
"speed": 0.4,
|
||||
"policy": "crawl",
|
||||
"tolerance": 0.1
|
||||
},
|
||||
{
|
||||
"id": 18,
|
||||
"x": 3.092,
|
||||
"y": 4.777,
|
||||
"yawDeg": -180,
|
||||
"speed": 0.4,
|
||||
"policy": "crawl",
|
||||
"tolerance": 0.1
|
||||
},
|
||||
{
|
||||
"id": 19,
|
||||
"x": 1.694,
|
||||
"y": 4.886,
|
||||
"yawDeg": 210,
|
||||
"speed": 0.4,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.1
|
||||
},
|
||||
{
|
||||
"id": 20,
|
||||
"x": 1.055,
|
||||
"y": 4.548,
|
||||
"yawDeg": -90,
|
||||
"speed": 0.4,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.1
|
||||
},
|
||||
{
|
||||
"id": 21,
|
||||
"x": 1.042,
|
||||
"y": 2.92,
|
||||
"yawDeg": -90.4,
|
||||
"speed": 0.4,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.1
|
||||
}
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,64 @@
|
||||
{
|
||||
"name": "test_route",
|
||||
"map": "map_b",
|
||||
"frame_id": "map",
|
||||
"yawToleranceDegDefault": 45.0,
|
||||
"requireYawDefault": false,
|
||||
"preDockDistanceDefault": 0.35,
|
||||
"preDockToleranceDefault": 0.18,
|
||||
"createdAt": "2026-07-02T15:07:00.219Z",
|
||||
"segments": [
|
||||
{
|
||||
"name": "segment_1",
|
||||
"obstacle": "slalom",
|
||||
"waypoints": [
|
||||
{
|
||||
"id": 1,
|
||||
"x": 1.2,
|
||||
"y": 0.5,
|
||||
"yawDeg": 90,
|
||||
"speed": 0.35,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.25
|
||||
},
|
||||
{
|
||||
"id": 2,
|
||||
"x": 1.247,
|
||||
"y": 1.748,
|
||||
"yawDeg": 0,
|
||||
"speed": 0.35,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.25
|
||||
},
|
||||
{
|
||||
"id": 3,
|
||||
"x": 2.312,
|
||||
"y": 1.673,
|
||||
"yawDeg": -90,
|
||||
"speed": 0.35,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.25
|
||||
},
|
||||
{
|
||||
"id": 4,
|
||||
"x": 2.33,
|
||||
"y": 0.496,
|
||||
"yawDeg": -180,
|
||||
"speed": 0.35,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.25
|
||||
},
|
||||
{
|
||||
"id": 5,
|
||||
"x": 0.4626,
|
||||
"y": 0.6271,
|
||||
"yawDeg": 176,
|
||||
"speed": 0.35,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.25,
|
||||
"requireYaw": true
|
||||
}
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,44 @@
|
||||
# Windows Runtime Web Debug
|
||||
|
||||
This folder contains a small Windows-side HTTP server for debugging the Nano runtime Web bridge through UDP.
|
||||
|
||||
Most of the time you can open the Nano runtime Web directly:
|
||||
|
||||
```text
|
||||
http://<nano-ip>:18080
|
||||
```
|
||||
|
||||
Use this tool only when you want the browser and HTTP server to run on Windows while Nano communicates over UDP.
|
||||
|
||||
## Start
|
||||
|
||||
From the repository root:
|
||||
|
||||
```powershell
|
||||
python .\tools\win_web_debug\server.py --nano-host <nano-ip> --http-port 8088
|
||||
```
|
||||
|
||||
Or from this folder:
|
||||
|
||||
```powershell
|
||||
python server.py --nano-host <nano-ip> --http-port 8088
|
||||
```
|
||||
|
||||
Open:
|
||||
|
||||
```text
|
||||
http://127.0.0.1:8088
|
||||
```
|
||||
|
||||
## Related Runtime Node
|
||||
|
||||
Nano side:
|
||||
|
||||
```text
|
||||
src/sim2real_runtime/src/web_udp_bridge_node.py
|
||||
```
|
||||
|
||||
See:
|
||||
|
||||
- [Runtime Web](../../docs/RUNTIME_WEB.md)
|
||||
- [Common Commands](../../docs/COMMANDS.md)
|
||||
@@ -0,0 +1,23 @@
|
||||
# Windows Runtime Web 调试桥
|
||||
|
||||
大多数时候直接打开 Nano Web 即可：
|
||||
|
||||
```text
|
||||
http://<nano-ip>:18080
|
||||
```
|
||||
|
||||
这个工具只在你希望 HTTP 服务跑在 Windows、Nano 通过 UDP 传状态时使用。
|
||||
|
||||
## 启动
|
||||
|
||||
在仓库根目录：
|
||||
|
||||
```powershell
|
||||
python .\tools\win_web_debug\server.py --nano-host <nano-ip> --http-port 8088
|
||||
```
|
||||
|
||||
打开：
|
||||
|
||||
```text
|
||||
http://127.0.0.1:8088
|
||||
```
|
||||
@@ -12,6 +12,7 @@ from typing import Optional
|
||||
|
||||
STATE_LOCK = threading.Lock()
|
||||
LATEST_STATE: dict = {"type": "state", "connected": False}
|
||||
LATEST_MAP: Optional[dict] = None
|
||||
NANO_ADDR: tuple[str, int]
|
||||
UDP_SOCK: socket.socket
|
||||
|
||||
@@ -28,7 +29,7 @@ class Handler(SimpleHTTPRequestHandler):
|
||||
deadline = time.time() + 1.0
|
||||
while time.time() < deadline:
|
||||
with STATE_LOCK:
|
||||
maybe_map = LATEST_STATE.get("map")
|
||||
maybe_map = LATEST_MAP
|
||||
if isinstance(maybe_map, dict):
|
||||
data = json.dumps(maybe_map).encode("utf-8")
|
||||
self._json(200, data)
|
||||
@@ -69,7 +70,7 @@ def send_udp(payload: dict) -> None:
|
||||
|
||||
|
||||
def udp_rx_loop(sock: socket.socket) -> None:
|
||||
global LATEST_STATE
|
||||
global LATEST_STATE, LATEST_MAP
|
||||
while True:
|
||||
try:
|
||||
data, _ = sock.recvfrom(65535)
|
||||
@@ -77,7 +78,10 @@ def udp_rx_loop(sock: socket.socket) -> None:
|
||||
payload["connected"] = True
|
||||
payload["local_receive_time"] = time.time()
|
||||
with STATE_LOCK:
|
||||
LATEST_STATE = payload
|
||||
if payload.get("type") == "map":
|
||||
LATEST_MAP = payload
|
||||
else:
|
||||
LATEST_STATE = payload
|
||||
except Exception:
|
||||
time.sleep(0.01)
|
||||
|
||||
|
||||
@@ -15,20 +15,36 @@ let dragging = false;
|
||||
let mapModel = null;
|
||||
let latestPose = null;
|
||||
let lastGoal = null;
|
||||
let latestConnected = false;
|
||||
let pendingModePromise = null;
|
||||
let currentGoalName = '';
|
||||
let currentGoalIndex = -1;
|
||||
let currentGoalTotal = 0;
|
||||
let latestModelState = { current_model: '--', switch_state: '--', backend: '--', switching: false };
|
||||
let goalPointsByName = {};
|
||||
let defaultMissionName = '';
|
||||
let defaultMissionGoals = [];
|
||||
let routeAlignmentInfo = {};
|
||||
let activeNavPath = { goal_name: '', stage: 'idle', path_index: 0, points: [] };
|
||||
|
||||
const mapCanvas = $('map-canvas');
|
||||
const mapCtx = mapCanvas ? mapCanvas.getContext('2d') : null;
|
||||
|
||||
async function post(payload) {
|
||||
try {
|
||||
await fetch('/api/control', {
|
||||
const res = await fetch('/api/control', {
|
||||
method: 'POST',
|
||||
headers: { 'Content-Type': 'application/json' },
|
||||
body: JSON.stringify(payload),
|
||||
});
|
||||
const data = await res.json().catch(() => ({}));
|
||||
if (!res.ok || data.ok === false) {
|
||||
throw new Error(data.error || `HTTP ${res.status}`);
|
||||
}
|
||||
return { ok: true, data };
|
||||
} catch (e) {
|
||||
appendEvent('API_ERROR', e.message, 'bad');
|
||||
return { ok: false, error: e.message };
|
||||
}
|
||||
}
|
||||
|
||||
@@ -68,32 +84,71 @@ function highlightMode(mode) {
|
||||
el.className = 'stage ' + mode;
|
||||
}
|
||||
currentMode = mode;
|
||||
updateTaskButtonState();
|
||||
}
|
||||
|
||||
async function setMode(mode) {
|
||||
if (mode === 'WEB') {
|
||||
const ok = confirm('Confirm switch to WEB control?\nMake sure the robot is safe and velocity is zero.');
|
||||
if (!ok) return false;
|
||||
function updateTaskButtonState() {
|
||||
const btn = $('btn-run-task');
|
||||
if (!btn) return;
|
||||
const hasMission = Boolean(defaultMissionName);
|
||||
const enabled = latestConnected && currentMode === 'NAV' && hasMission;
|
||||
btn.disabled = !enabled;
|
||||
btn.textContent = hasMission ? `Run ${defaultMissionName}` : 'Run Mission';
|
||||
if (!hasMission) {
|
||||
btn.title = 'No default mission loaded from backend';
|
||||
return;
|
||||
}
|
||||
await post({ type: 'mode', mode });
|
||||
highlightMode(mode);
|
||||
appendEvent('MODE_SET', `-> ${mode}`, 'ok');
|
||||
btn.title = enabled
|
||||
? `Run default mission ${defaultMissionName}`
|
||||
: 'Available only in NAV mode while connected';
|
||||
}
|
||||
|
||||
function setText(id, text, cls) {
|
||||
const el = $(id);
|
||||
if (!el) return;
|
||||
el.textContent = text;
|
||||
if (cls !== undefined) el.className = 'diag-value ' + cls;
|
||||
}
|
||||
|
||||
function formatNumber(value, digits = 2) {
|
||||
return Number.isFinite(value) ? Number(value).toFixed(digits) : '--';
|
||||
}
|
||||
|
||||
function setLastGoalByName(goalName) {
|
||||
const goal = goalPointsByName[goalName];
|
||||
if (!goal) return false;
|
||||
lastGoal = { x: goal.x, y: goal.y };
|
||||
currentGoalName = goalName;
|
||||
currentGoalIndex = defaultMissionGoals.findIndex(goalItem => goalItem.name === goalName);
|
||||
setText('d-active-goal', goalName, 'active');
|
||||
renderTaskGoals();
|
||||
return true;
|
||||
}
|
||||
|
||||
async function ensureMode(mode) {
|
||||
if (currentMode === mode) return true;
|
||||
if (pendingModePromise) {
|
||||
const activeMode = await pendingModePromise;
|
||||
if (activeMode === mode) return true;
|
||||
function syncGoalMarkerFromNavStatus(statusText) {
|
||||
if (!statusText) return;
|
||||
const missionStatusMatch = statusText.match(/waypoint\s+(\d+)\/(\d+)\s*->\s*([A-Za-z0-9_()=.,-]+)\b/i);
|
||||
if (missionStatusMatch) {
|
||||
currentGoalIndex = Math.max(0, Number.parseInt(missionStatusMatch[1], 10) - 1);
|
||||
currentGoalTotal = Math.max(0, Number.parseInt(missionStatusMatch[2], 10));
|
||||
setLastGoalByName(missionStatusMatch[3]);
|
||||
return;
|
||||
}
|
||||
const nextGoalMatch = statusText.match(/->\s*([A-Za-z0-9_()=.,-]+)\b/);
|
||||
if (nextGoalMatch && setLastGoalByName(nextGoalMatch[1])) {
|
||||
return;
|
||||
}
|
||||
const reachedMatch = statusText.match(/reached\s+([A-Za-z0-9_()=.,-]+)\b/i);
|
||||
if (reachedMatch && setLastGoalByName(reachedMatch[1])) {
|
||||
return;
|
||||
}
|
||||
if (/navigation stopped:/i.test(statusText)) {
|
||||
currentGoalName = '';
|
||||
currentGoalIndex = -1;
|
||||
currentGoalTotal = 0;
|
||||
setText('d-active-goal', '--');
|
||||
renderTaskGoals();
|
||||
}
|
||||
pendingModePromise = (async () => {
|
||||
const ok = await setMode(mode);
|
||||
return ok ? mode : currentMode;
|
||||
})();
|
||||
const resolvedMode = await pendingModePromise;
|
||||
pendingModePromise = null;
|
||||
return resolvedMode === mode;
|
||||
}
|
||||
|
||||
function worldToCanvas(x, y) {
|
||||
@@ -115,12 +170,16 @@ function canvasToWorld(px, py) {
|
||||
}
|
||||
|
||||
function buildMapModel(points) {
|
||||
if (!points || !points.length || !mapCanvas) return null;
|
||||
if (!mapCanvas) return null;
|
||||
const boundsPoints = [...(points || [])];
|
||||
for (const goal of defaultMissionGoals) boundsPoints.push([goal.x, goal.y]);
|
||||
if (!boundsPoints.length) return null;
|
||||
|
||||
let minX = Infinity;
|
||||
let minY = Infinity;
|
||||
let maxX = -Infinity;
|
||||
let maxY = -Infinity;
|
||||
for (const [x, y] of points) {
|
||||
for (const [x, y] of boundsPoints) {
|
||||
if (x < minX) minX = x;
|
||||
if (y < minY) minY = y;
|
||||
if (x > maxX) maxX = x;
|
||||
@@ -132,7 +191,102 @@ function buildMapModel(points) {
|
||||
const spanX = Math.max(maxX - minX, 1e-6);
|
||||
const spanY = Math.max(maxY - minY, 1e-6);
|
||||
const scale = Math.min(usableW / spanX, usableH / spanY);
|
||||
return { minX, minY, maxX, maxY, scale, pad, drawH: usableH };
|
||||
return { minX, minY, maxX, maxY, scale, pad, drawH: usableH, points: points || [] };
|
||||
}
|
||||
|
||||
function goalColor(goal) {
|
||||
return goal.policy === 'crawl' ? '#30d158' : '#ffd60a';
|
||||
}
|
||||
|
||||
function drawMissionPath() {
|
||||
if (!mapCtx || defaultMissionGoals.length < 2) return;
|
||||
mapCtx.save();
|
||||
mapCtx.lineWidth = 2;
|
||||
mapCtx.strokeStyle = 'rgba(34, 197, 94, 0.95)';
|
||||
mapCtx.beginPath();
|
||||
for (let i = 0; i < defaultMissionGoals.length; i += 1) {
|
||||
const goal = defaultMissionGoals[i];
|
||||
const p = worldToCanvas(goal.x, goal.y);
|
||||
if (!p) continue;
|
||||
if (i === 0) mapCtx.moveTo(p.x, p.y);
|
||||
else mapCtx.lineTo(p.x, p.y);
|
||||
}
|
||||
mapCtx.stroke();
|
||||
|
||||
if (currentGoalIndex > 0) {
|
||||
mapCtx.strokeStyle = 'rgba(10, 132, 255, 0.95)';
|
||||
mapCtx.lineWidth = 3;
|
||||
mapCtx.beginPath();
|
||||
let started = false;
|
||||
for (let i = 0; i <= Math.min(currentGoalIndex, defaultMissionGoals.length - 1); i += 1) {
|
||||
const goal = defaultMissionGoals[i];
|
||||
const p = worldToCanvas(goal.x, goal.y);
|
||||
if (!p) continue;
|
||||
if (!started) {
|
||||
mapCtx.moveTo(p.x, p.y);
|
||||
started = true;
|
||||
} else {
|
||||
mapCtx.lineTo(p.x, p.y);
|
||||
}
|
||||
}
|
||||
if (started) mapCtx.stroke();
|
||||
}
|
||||
mapCtx.restore();
|
||||
}
|
||||
|
||||
function drawActiveNavPath() {
|
||||
if (!mapCtx || !activeNavPath || !Array.isArray(activeNavPath.points) || activeNavPath.points.length < 2) return;
|
||||
mapCtx.save();
|
||||
mapCtx.lineWidth = 2.5;
|
||||
mapCtx.strokeStyle = 'rgba(168, 85, 247, 0.95)';
|
||||
mapCtx.beginPath();
|
||||
let started = false;
|
||||
for (const point of activeNavPath.points) {
|
||||
if (!Array.isArray(point) || point.length < 2) continue;
|
||||
const p = worldToCanvas(point[0], point[1]);
|
||||
if (!p) continue;
|
||||
if (!started) {
|
||||
mapCtx.moveTo(p.x, p.y);
|
||||
started = true;
|
||||
} else {
|
||||
mapCtx.lineTo(p.x, p.y);
|
||||
}
|
||||
}
|
||||
if (started) mapCtx.stroke();
|
||||
|
||||
const highlightIndex = Math.min(
|
||||
Math.max(0, Number(activeNavPath.path_index || 0)),
|
||||
activeNavPath.points.length - 1,
|
||||
);
|
||||
const highlightPoint = activeNavPath.points[highlightIndex];
|
||||
if (Array.isArray(highlightPoint) && highlightPoint.length >= 2) {
|
||||
const p = worldToCanvas(highlightPoint[0], highlightPoint[1]);
|
||||
if (p) {
|
||||
mapCtx.fillStyle = '#c084fc';
|
||||
mapCtx.beginPath();
|
||||
mapCtx.arc(p.x, p.y, 5, 0, Math.PI * 2);
|
||||
mapCtx.fill();
|
||||
}
|
||||
}
|
||||
mapCtx.restore();
|
||||
}
|
||||
|
||||
function drawGoal(goal, index) {
|
||||
const p = worldToCanvas(goal.x, goal.y);
|
||||
if (!p || !mapCtx) return;
|
||||
const active = goal.name === currentGoalName;
|
||||
const completed = currentGoalIndex > index;
|
||||
mapCtx.fillStyle = active ? '#ff9f0a' : completed ? '#67e8f9' : goalColor(goal);
|
||||
mapCtx.beginPath();
|
||||
mapCtx.arc(p.x, p.y, active ? 7 : 5, 0, Math.PI * 2);
|
||||
mapCtx.fill();
|
||||
mapCtx.strokeStyle = '#111827';
|
||||
mapCtx.lineWidth = 1.5;
|
||||
mapCtx.stroke();
|
||||
|
||||
mapCtx.fillStyle = active ? '#ffcf66' : '#ffe680';
|
||||
mapCtx.font = '12px sans-serif';
|
||||
mapCtx.fillText(`${index + 1}.${goal.name}`, p.x + 8, p.y - 8);
|
||||
}
|
||||
|
||||
function drawMap() {
|
||||
@@ -155,6 +309,13 @@ function drawMap() {
|
||||
mapCtx.fillRect(p.x, p.y, 1.5, 1.5);
|
||||
}
|
||||
|
||||
drawMissionPath();
|
||||
drawActiveNavPath();
|
||||
|
||||
for (const [index, goal] of defaultMissionGoals.entries()) {
|
||||
drawGoal(goal, index);
|
||||
}
|
||||
|
||||
if (latestPose) {
|
||||
const p = worldToCanvas(latestPose.x, latestPose.y);
|
||||
if (p) {
|
||||
@@ -186,16 +347,74 @@ function drawMap() {
|
||||
}
|
||||
}
|
||||
|
||||
function formatRouteAlignment(info) {
|
||||
if (!info || Object.keys(info).length === 0) return '--';
|
||||
const applied = Number.isFinite(info.applied_deg) ? `${Number(info.applied_deg).toFixed(2)}deg` : '--';
|
||||
const hits = Number.isFinite(info.hits) && Number.isFinite(info.total) ? `${info.hits}/${info.total}` : '--';
|
||||
const reason = info.reason || (info.enabled ? 'auto align' : 'fixed');
|
||||
return `${applied} hits=${hits} ${reason}`;
|
||||
}
|
||||
|
||||
function renderTaskGoals() {
|
||||
const list = $('task-goals-list');
|
||||
if (!list) return;
|
||||
if (!defaultMissionGoals.length) {
|
||||
list.innerHTML = '<div class="task-goal-item empty">No mission goals loaded</div>';
|
||||
return;
|
||||
}
|
||||
list.innerHTML = defaultMissionGoals.map((goal, index) => {
|
||||
const yawText = Number.isFinite(goal.yaw_deg) ? `${Number(goal.yaw_deg).toFixed(1)}deg` : '--';
|
||||
const yawTolText = Number.isFinite(goal.yaw_tolerance_deg) ? `${Number(goal.yaw_tolerance_deg).toFixed(1)}deg` : '--';
|
||||
const tolText = Number.isFinite(goal.tolerance) ? Number(goal.tolerance).toFixed(2) : '--';
|
||||
const policyText = goal.policy || 'rough';
|
||||
const requireYawText = goal.require_yaw ? ' strict_yaw' : '';
|
||||
const preDockText = Number.isFinite(goal.pre_dock_distance) ? ` pre_dock=${Number(goal.pre_dock_distance).toFixed(2)}` : '';
|
||||
const activeClass = goal.name === currentGoalName ? ' active' : '';
|
||||
return `
|
||||
<div class="task-goal-item${activeClass}">
|
||||
<div class="task-goal-head">
|
||||
<span class="task-goal-index">${index + 1}</span>
|
||||
<span class="task-goal-name">${goal.name}</span>
|
||||
<span class="task-goal-policy ${policyText}">${policyText}</span>
|
||||
</div>
|
||||
<div class="task-goal-meta">x=${formatNumber(goal.x)} y=${formatNumber(goal.y)} yaw=${yawText} yaw_tol=${yawTolText} tol=${tolText}${requireYawText}${preDockText}</div>
|
||||
</div>`;
|
||||
}).join('');
|
||||
}
|
||||
|
||||
function applyMapMetadata(data) {
|
||||
const goalSpecs = Array.isArray(data.goal_specs) ? data.goal_specs : [];
|
||||
goalPointsByName = {};
|
||||
for (const goal of goalSpecs) {
|
||||
if (!goal || !goal.name) continue;
|
||||
goalPointsByName[goal.name] = goal;
|
||||
}
|
||||
defaultMissionName = data.default_mission_name || '';
|
||||
defaultMissionGoals = Array.isArray(data.default_mission_goals) ? data.default_mission_goals : [];
|
||||
routeAlignmentInfo = data.route_alignment || {};
|
||||
activeNavPath = data.nav_path || activeNavPath;
|
||||
currentGoalIndex = currentGoalName
|
||||
? defaultMissionGoals.findIndex(goal => goal.name === currentGoalName)
|
||||
: -1;
|
||||
currentGoalTotal = defaultMissionGoals.length;
|
||||
|
||||
setText('d-task-mission', defaultMissionName || '--', defaultMissionName ? 'ok' : '');
|
||||
setText('d-task-goals', String(defaultMissionGoals.length || 0), defaultMissionGoals.length ? 'ok' : '');
|
||||
setText('d-route-align', formatRouteAlignment(routeAlignmentInfo), routeAlignmentInfo.enabled ? 'ok' : 'warn');
|
||||
updateTaskButtonState();
|
||||
renderTaskGoals();
|
||||
}
|
||||
|
||||
async function fetchMap() {
|
||||
try {
|
||||
const res = await fetch('/api/map');
|
||||
const data = await res.json();
|
||||
const points = data.points || [];
|
||||
applyMapMetadata(data);
|
||||
const points = Array.isArray(data.points) ? data.points : [];
|
||||
mapModel = buildMapModel(points);
|
||||
if (mapModel) mapModel.points = points;
|
||||
if (data.pose) latestPose = data.pose;
|
||||
drawMap();
|
||||
appendEvent('MAP', `loaded ${points.length} filtered points`, 'ok');
|
||||
appendEvent('MAP', `loaded ${points.length} filtered points, mission=${defaultMissionName || 'none'}`, 'ok');
|
||||
} catch (e) {
|
||||
appendEvent('MAP_ERROR', e.message, 'bad');
|
||||
}
|
||||
@@ -209,27 +428,25 @@ function onMapClick(event) {
|
||||
const world = canvasToWorld(px, py);
|
||||
if (!world) return;
|
||||
lastGoal = world;
|
||||
currentGoalName = '';
|
||||
setText('d-active-goal', 'direct goal', 'active');
|
||||
renderTaskGoals();
|
||||
drawMap();
|
||||
ensureMode('NAV').then(ok => {
|
||||
ensureMode('NAV').then(async ok => {
|
||||
if (!ok) return;
|
||||
post({ type: 'go_to', x: world.x, y: world.y });
|
||||
appendEvent('NAV_GO', `x=${world.x.toFixed(2)} y=${world.y.toFixed(2)}`, 'ok');
|
||||
const result = await post({ type: 'go_to', x: world.x, y: world.y });
|
||||
if (result.ok) {
|
||||
appendEvent('NAV_GO', `x=${world.x.toFixed(2)} y=${world.y.toFixed(2)}`, 'ok');
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
function setText(id, text, cls) {
|
||||
const el = $(id);
|
||||
if (!el) return;
|
||||
el.textContent = text;
|
||||
if (cls !== undefined) el.className = 'diag-value ' + cls;
|
||||
}
|
||||
|
||||
function initJointsGrid() {
|
||||
const grid = $('joints-grid');
|
||||
if (!grid) return;
|
||||
grid.innerHTML = JOINT_NAMES.map((name, i) => `
|
||||
<div class="motor-row" id="mi-${i}">
|
||||
<span class="stale" id="ms-${i}" style="color:#ef4444">●</span>
|
||||
<span class="stale" id="ms-${i}" style="color:#ef4444">*</span>
|
||||
<span class="name">${name}</span>
|
||||
<span class="val pos" id="mp-${i}">0.00</span>
|
||||
<span class="val vel" id="mv-${i}">0.00</span>
|
||||
@@ -261,14 +478,17 @@ function updateJointsGrid(robot) {
|
||||
|
||||
function applyState(data) {
|
||||
const connected = data.connected && (!data.local_receive_time || Date.now() / 1000 - data.local_receive_time < 2.5);
|
||||
latestConnected = connected;
|
||||
if (!connected) {
|
||||
$('stage').textContent = 'DISCONNECTED';
|
||||
$('stage').className = 'stage DISCONNECTED';
|
||||
updateTaskButtonState();
|
||||
return;
|
||||
}
|
||||
|
||||
const mode = data.mode || 'UNKNOWN';
|
||||
if (mode !== currentMode) highlightMode(mode);
|
||||
updateTaskButtonState();
|
||||
|
||||
const rt = data.runtime || {};
|
||||
const src = rt.target_source || '--';
|
||||
@@ -280,6 +500,18 @@ function applyState(data) {
|
||||
setText('d-estop', String(data.estop ?? '--'), data.estop ? 'bad' : 'ok');
|
||||
setText('d-mux', data.mux_status || '--');
|
||||
|
||||
const model = data.model || {};
|
||||
latestModelState = {
|
||||
current_model: model.current_model || '--',
|
||||
switch_state: model.switch_state || '--',
|
||||
backend: model.backend || '--',
|
||||
switching: Boolean(model.switching),
|
||||
};
|
||||
setText('d-model', latestModelState.current_model, latestModelState.current_model === 'crawl' ? 'active' : 'ok');
|
||||
setText('d-model-switch', latestModelState.switch_state, latestModelState.switching ? 'warn' : 'ok');
|
||||
setText('d-model-backend', latestModelState.backend || '--');
|
||||
setText('d-model-switching', String(latestModelState.switching), latestModelState.switching ? 'warn' : 'ok');
|
||||
|
||||
const robot = data.robot || {};
|
||||
const imuAge = robot.imu_age_ms ?? null;
|
||||
setText('d-imu-fresh', String(robot.imu_fresh ?? '--'), robot.imu_fresh ? 'ok' : 'bad');
|
||||
@@ -292,13 +524,15 @@ function applyState(data) {
|
||||
const lp = robot.odom_local_pos;
|
||||
setText('d-odom-pos', lp ? `x=${Number(lp[0]).toFixed(2)} y=${Number(lp[1]).toFixed(2)}` : '--');
|
||||
setText('d-nav-status', data.nav_status || '--');
|
||||
syncGoalMarkerFromNavStatus(data.nav_status || '');
|
||||
|
||||
const nav = data.nav || {};
|
||||
if (nav.pose) {
|
||||
latestPose = nav.pose;
|
||||
setText('d-nav-pose', `x=${nav.pose.x.toFixed(2)} y=${nav.pose.y.toFixed(2)} yaw=${(nav.pose.yaw * 57.2958).toFixed(1)}deg`);
|
||||
drawMap();
|
||||
}
|
||||
if (nav.path) activeNavPath = nav.path;
|
||||
drawMap();
|
||||
|
||||
const cv = data.cmd_vel || {};
|
||||
const lin = cv.linear || {};
|
||||
@@ -328,31 +562,96 @@ function appendEvent(kind, detail, cls) {
|
||||
el.scrollTop = el.scrollHeight;
|
||||
}
|
||||
|
||||
$('btn-disabled').onclick = () => {
|
||||
async function setMode(mode) {
|
||||
if (mode === 'WEB') {
|
||||
const ok = confirm('Confirm switch to WEB control?\nMake sure the robot is safe and velocity is zero.');
|
||||
if (!ok) return false;
|
||||
}
|
||||
const result = await post({ type: 'mode', mode });
|
||||
if (!result.ok) return false;
|
||||
highlightMode(mode);
|
||||
appendEvent('MODE_SET', `-> ${mode}`, 'ok');
|
||||
return true;
|
||||
}
|
||||
|
||||
async function ensureMode(mode) {
|
||||
if (currentMode === mode) return true;
|
||||
if (pendingModePromise) {
|
||||
const activeMode = await pendingModePromise;
|
||||
if (activeMode === mode) return true;
|
||||
}
|
||||
pendingModePromise = (async () => {
|
||||
const ok = await setMode(mode);
|
||||
return ok ? mode : currentMode;
|
||||
})();
|
||||
const resolvedMode = await pendingModePromise;
|
||||
pendingModePromise = null;
|
||||
return resolvedMode === mode;
|
||||
}
|
||||
|
||||
$('btn-disabled').onclick = async () => {
|
||||
zeroAll();
|
||||
setMode('DISABLED');
|
||||
await setMode('DISABLED');
|
||||
};
|
||||
$('btn-remote').onclick = () => setMode('REMOTE');
|
||||
$('btn-web').onclick = () => setMode('WEB');
|
||||
$('btn-nav').onclick = () => setMode('NAV');
|
||||
$('btn-zero').onclick = zeroAll;
|
||||
$('btn-estop').onclick = () => {
|
||||
$('btn-estop').onclick = async () => {
|
||||
if (confirm('Confirm soft e-stop?')) {
|
||||
post({ type: 'estop', data: true });
|
||||
const result = await post({ type: 'estop', data: true });
|
||||
zeroAll();
|
||||
appendEvent('ESTOP', 'soft e-stop triggered', 'bad');
|
||||
if (result.ok) appendEvent('ESTOP', 'soft e-stop triggered', 'bad');
|
||||
}
|
||||
};
|
||||
$('btn-refresh-map').onclick = fetchMap;
|
||||
$('btn-record').onclick = () => ensureMode('NAV').then(ok => {
|
||||
if (ok) post({ type: 'nav_cmd', command: 'record web_p1' });
|
||||
$('btn-run-task').onclick = async () => {
|
||||
if (!latestConnected) {
|
||||
appendEvent('TASK', 'bridge not connected', 'warn');
|
||||
updateTaskButtonState();
|
||||
return;
|
||||
}
|
||||
if (currentMode !== 'NAV') {
|
||||
appendEvent('TASK', 'only available in NAV mode', 'warn');
|
||||
updateTaskButtonState();
|
||||
return;
|
||||
}
|
||||
if (!defaultMissionName) {
|
||||
appendEvent('TASK', 'no default mission loaded', 'warn');
|
||||
return;
|
||||
}
|
||||
if (defaultMissionGoals.length) {
|
||||
setLastGoalByName(defaultMissionGoals[0].name);
|
||||
drawMap();
|
||||
}
|
||||
const result = await post({ type: 'nav_cmd', command: `run ${defaultMissionName}` });
|
||||
if (result.ok) {
|
||||
appendEvent('TASK', `run mission ${defaultMissionName}`, 'ok');
|
||||
}
|
||||
};
|
||||
$('btn-record').onclick = () => ensureMode('NAV').then(async ok => {
|
||||
if (!ok) return;
|
||||
const result = await post({ type: 'nav_cmd', command: 'record web_p1' });
|
||||
if (result.ok) appendEvent('NAV_RECORD', 'record web_p1', 'ok');
|
||||
});
|
||||
$('btn-stop-nav').onclick = () => ensureMode('NAV').then(ok => {
|
||||
if (ok) post({ type: 'nav_cmd', command: 'stop' });
|
||||
$('btn-stop-nav').onclick = () => ensureMode('NAV').then(async ok => {
|
||||
if (!ok) return;
|
||||
const result = await post({ type: 'nav_cmd', command: 'stop' });
|
||||
if (result.ok) appendEvent('NAV_STOP', 'simple_nav stop', 'ok');
|
||||
});
|
||||
$('btn-go-rel').onclick = () => ensureMode('NAV').then(ok => {
|
||||
if (ok) post({ type: 'go_rel', dx: 0.3, dy: 0.0 });
|
||||
$('btn-go-rel').onclick = () => ensureMode('NAV').then(async ok => {
|
||||
if (!ok) return;
|
||||
const result = await post({ type: 'go_rel', dx: 0.3, dy: 0.0 });
|
||||
if (result.ok) appendEvent('NAV_REL', 'forward 0.3m', 'ok');
|
||||
});
|
||||
$('btn-model-switch').onclick = async () => {
|
||||
if (latestModelState.switching) {
|
||||
appendEvent('MODEL', 'switch already in progress', 'warn');
|
||||
return;
|
||||
}
|
||||
const result = await post({ type: 'model_toggle' });
|
||||
if (result.ok) appendEvent('MODEL', `toggle requested from ${latestModelState.current_model}`, 'ok');
|
||||
};
|
||||
|
||||
for (const [id, key] of [['cmd-vx', 'vx'], ['cmd-vy', 'vy'], ['cmd-yaw', 'yaw']]) {
|
||||
$(id).addEventListener('input', e => {
|
||||
|
||||
@@ -1,10 +1,10 @@
|
||||
<!doctype html>
|
||||
<html lang="zh-CN" data-theme="dark">
|
||||
<html lang="en" data-theme="dark">
|
||||
<head>
|
||||
<meta charset="utf-8">
|
||||
<meta name="viewport" content="width=device-width,initial-scale=1">
|
||||
<title>sim2real ROS2 控制台</title>
|
||||
<link rel="stylesheet" href="style.css">
|
||||
<title>sim2real ROS2 Console</title>
|
||||
<link rel="stylesheet" href="style.css?v=mission-sync-2">
|
||||
</head>
|
||||
<body>
|
||||
<header class="glass-panel top-bar">
|
||||
@@ -13,32 +13,37 @@
|
||||
<span class="stage" id="stage">DISCONNECTED</span>
|
||||
</div>
|
||||
<div class="top-bar-center">
|
||||
<span class="label">控制模式</span>
|
||||
<span class="label">Control Mode</span>
|
||||
<button class="btn" id="btn-disabled">DISABLED</button>
|
||||
<button class="btn btn-remote" id="btn-remote">REMOTE</button>
|
||||
<button class="btn btn-web" id="btn-web">WEB</button>
|
||||
<button class="btn btn-nav" id="btn-nav">NAV</button>
|
||||
<div class="divider"></div>
|
||||
<button class="btn" id="btn-zero">速度归零</button>
|
||||
<button class="btn" id="btn-zero">Zero Velocity</button>
|
||||
</div>
|
||||
<div class="top-bar-right">
|
||||
<button class="btn btn-danger" id="btn-estop">软急停</button>
|
||||
<button class="btn btn-danger" id="btn-estop">Soft E-Stop</button>
|
||||
</div>
|
||||
</header>
|
||||
|
||||
<div class="glass-panel side-panel left-panel">
|
||||
<div class="panel-section">
|
||||
<h2 class="panel-title">运行状态</h2>
|
||||
<h2 class="panel-title">Runtime</h2>
|
||||
<div class="diag-row"><span class="diag-label">target_source</span><span class="diag-value" id="d-source">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">runtime_released</span><span class="diag-value" id="d-released">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">release_alpha</span><span class="diag-value" id="d-alpha">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">zero_command</span><span class="diag-value" id="d-zero">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">estop</span><span class="diag-value" id="d-estop">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">mux</span><span class="diag-value" id="d-mux">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">model</span><span class="diag-value" id="d-model">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">switch_state</span><span class="diag-value" id="d-model-switch">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">backend</span><span class="diag-value" id="d-model-backend">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">switching</span><span class="diag-value" id="d-model-switching">--</span></div>
|
||||
<div class="diag-row"><button class="btn" id="btn-model-switch">Toggle Model</button></div>
|
||||
</div>
|
||||
|
||||
<div class="panel-section">
|
||||
<h2 class="panel-title">IMU & 里程计</h2>
|
||||
<h2 class="panel-title">IMU and Odom</h2>
|
||||
<div class="diag-row"><span class="diag-label">IMU fresh</span><span class="diag-value" id="d-imu-fresh">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">IMU age ms</span><span class="diag-value" id="d-imu-age">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">projected_gravity</span><span class="diag-value" id="d-gravity">--</span></div>
|
||||
@@ -47,10 +52,14 @@
|
||||
<div class="diag-row"><span class="diag-label">odom local pos</span><span class="diag-value" id="d-odom-pos">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">nav pose</span><span class="diag-value" id="d-nav-pose">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">nav status</span><span class="diag-value" id="d-nav-status">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">task mission</span><span class="diag-value" id="d-task-mission">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">active goal</span><span class="diag-value" id="d-active-goal">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">goal count</span><span class="diag-value" id="d-task-goals">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">route align</span><span class="diag-value" id="d-route-align">--</span></div>
|
||||
</div>
|
||||
|
||||
<div class="panel-section flex-1">
|
||||
<h2 class="panel-title">关节状态 (16轴)</h2>
|
||||
<h2 class="panel-title">Joint State (16)</h2>
|
||||
<div id="joints-grid" class="motors-grid-list"></div>
|
||||
</div>
|
||||
</div>
|
||||
@@ -58,25 +67,26 @@
|
||||
<main class="glass-panel center-panel">
|
||||
<div class="panel-section map-section">
|
||||
<div class="map-header">
|
||||
<h2 class="panel-title">PCD 2D 导航</h2>
|
||||
<h2 class="panel-title">PCD 2D Nav</h2>
|
||||
<div class="map-actions">
|
||||
<button class="btn" id="btn-refresh-map">刷新地图</button>
|
||||
<button class="btn" id="btn-record">记录当前位置</button>
|
||||
<button class="btn" id="btn-stop-nav">停止导航</button>
|
||||
<button class="btn" id="btn-go-rel">前进 0.3m</button>
|
||||
<button class="btn" id="btn-refresh-map">Refresh Map</button>
|
||||
<button class="btn btn-nav" id="btn-run-task" disabled>Run Mission</button>
|
||||
<button class="btn" id="btn-record">Record Pose</button>
|
||||
<button class="btn" id="btn-stop-nav">Stop Nav</button>
|
||||
<button class="btn" id="btn-go-rel">Forward 0.3m</button>
|
||||
</div>
|
||||
</div>
|
||||
<canvas id="map-canvas" width="900" height="680"></canvas>
|
||||
<div class="map-help">左键点击地图发送绝对目标；蓝点是当前机器人，红叉是最近目标。</div>
|
||||
<div class="map-help">Left click sends an absolute goal. Green line is the mission path, purple is the active A* path, cyan marks completed waypoints, orange is the current target, and blue is the robot pose.</div>
|
||||
</div>
|
||||
</main>
|
||||
|
||||
<div class="glass-panel side-panel right-panel">
|
||||
<div class="panel-section">
|
||||
<h2 class="panel-title">Web 手动控制</h2>
|
||||
<h2 class="panel-title">Web Manual Control</h2>
|
||||
<div class="joystick-area">
|
||||
<div class="joystick" id="joystick"><div id="stick"></div></div>
|
||||
<p class="hint">拖动控制前后(vx)和横移(vy)，松开归零</p>
|
||||
<p class="hint">Drag to control vx and vy. Release to return to zero.</p>
|
||||
</div>
|
||||
<div class="slider-group">
|
||||
<div class="slider-row">
|
||||
@@ -91,7 +101,7 @@
|
||||
</div>
|
||||
<div class="slider-row">
|
||||
<span class="slider-label">yaw</span>
|
||||
<input type="range" id="cmd-yaw" class="glass-slider" min="-0.5" max="0.5" step="0.01" value="0">
|
||||
<input type="range" id="cmd-yaw" class="glass-slider" min="-0.8" max="0.8" step="0.01" value="0">
|
||||
<span class="slider-val" id="cmd-yaw-v">0.00</span>
|
||||
</div>
|
||||
</div>
|
||||
@@ -99,18 +109,18 @@
|
||||
</div>
|
||||
|
||||
<div class="panel-section">
|
||||
<h2 class="panel-title">当前输出 /cmd_vel</h2>
|
||||
<h2 class="panel-title">Current /cmd_vel</h2>
|
||||
<div class="diag-row"><span class="diag-label">linear.x</span><span class="diag-value" id="cv-vx">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">linear.y</span><span class="diag-value" id="cv-vy">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">angular.z</span><span class="diag-value" id="cv-yaw">--</span></div>
|
||||
</div>
|
||||
|
||||
<div class="panel-section log-section flex-1">
|
||||
<h2 class="panel-title">事件流</h2>
|
||||
<h2 class="panel-title">Events</h2>
|
||||
<div id="events-log" class="log"></div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<script src="app.js"></script>
|
||||
<script src="app.js?v=mission-sync-2"></script>
|
||||
</body>
|
||||
</html>
|
||||
|
||||
@@ -107,6 +107,18 @@ body {
|
||||
.btn-nav { background: rgba(255,214,10,0.2); border-color: rgba(255,214,10,0.4); color: #ffe680; }
|
||||
.btn.active-mode { box-shadow: 0 0 0 2px white; }
|
||||
|
||||
.route-select {
|
||||
min-width: 220px;
|
||||
height: 30px;
|
||||
border: 1px solid rgba(255,255,255,0.12);
|
||||
border-radius: 8px;
|
||||
background: rgba(0,0,0,0.32);
|
||||
color: var(--text-primary);
|
||||
font-size: 12px;
|
||||
font-family: inherit;
|
||||
padding: 0 8px;
|
||||
}
|
||||
|
||||
.label { font-size: 11px; color: var(--text-tertiary); }
|
||||
|
||||
.side-panel,
|
||||
@@ -172,6 +184,85 @@ body {
|
||||
flex-wrap: wrap;
|
||||
gap: 8px;
|
||||
}
|
||||
.route-section {
|
||||
flex-shrink: 0;
|
||||
}
|
||||
.task-goals-list {
|
||||
display: grid;
|
||||
grid-template-columns: repeat(2, minmax(0, 1fr));
|
||||
gap: 8px;
|
||||
}
|
||||
.task-goal-item {
|
||||
padding: 10px 12px;
|
||||
border-radius: 8px;
|
||||
background: rgba(0,0,0,0.24);
|
||||
border: 1px solid rgba(255,255,255,0.06);
|
||||
}
|
||||
.task-goal-item.active {
|
||||
border-color: rgba(255,214,10,0.55);
|
||||
box-shadow: inset 0 0 0 1px rgba(255,214,10,0.18);
|
||||
}
|
||||
.task-goal-item.empty {
|
||||
grid-column: 1 / -1;
|
||||
color: var(--text-tertiary);
|
||||
text-align: center;
|
||||
}
|
||||
.task-goal-head {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 8px;
|
||||
margin-bottom: 6px;
|
||||
}
|
||||
.task-goal-index {
|
||||
min-width: 20px;
|
||||
height: 20px;
|
||||
border-radius: 999px;
|
||||
background: rgba(10,132,255,0.22);
|
||||
color: #82c4ff;
|
||||
font-size: 11px;
|
||||
font-weight: 700;
|
||||
display: inline-flex;
|
||||
align-items: center;
|
||||
justify-content: center;
|
||||
}
|
||||
.task-goal-name {
|
||||
font-size: 12px;
|
||||
font-weight: 600;
|
||||
color: var(--text-primary);
|
||||
font-family: monospace;
|
||||
flex: 1;
|
||||
}
|
||||
.task-goal-policy {
|
||||
font-size: 10px;
|
||||
font-weight: 700;
|
||||
text-transform: uppercase;
|
||||
letter-spacing: 0.4px;
|
||||
padding: 2px 6px;
|
||||
border-radius: 999px;
|
||||
background: rgba(255,255,255,0.08);
|
||||
}
|
||||
.task-goal-policy.rough {
|
||||
color: #ffe680;
|
||||
}
|
||||
.task-goal-policy.crawl {
|
||||
color: #8deda7;
|
||||
}
|
||||
.task-goal-meta {
|
||||
font-size: 11px;
|
||||
color: var(--text-tertiary);
|
||||
font-family: monospace;
|
||||
}
|
||||
.obstacle-grid {
|
||||
display: grid;
|
||||
grid-template-columns: repeat(5, minmax(0, 1fr));
|
||||
gap: 8px;
|
||||
}
|
||||
.obstacle-grid .btn {
|
||||
padding-left: 6px;
|
||||
padding-right: 6px;
|
||||
overflow: hidden;
|
||||
text-overflow: ellipsis;
|
||||
}
|
||||
#map-canvas {
|
||||
width: 100%;
|
||||
height: calc(100% - 56px);
|
||||
@@ -296,4 +387,5 @@ body {
|
||||
}
|
||||
.center-panel { min-height: 700px; }
|
||||
#map-canvas { min-height: 420px; }
|
||||
.task-goals-list { grid-template-columns: 1fr; }
|
||||
}
|
||||
|
||||
@@ -39,7 +39,7 @@ RC_WheelLeg/
|
||||
- [x] 整理后期 MuJoCo 姿态、IK、动力学和 MPC 工具
|
||||
- [x] 整理后期 Sim2Sim、路线检查与比赛 Rough ONNX 策略
|
||||
- [x] 整理导航地图、打点工具、路线迭代和抽样 PCD
|
||||
- [x] 整理 Python Sim2Real v2、ROS 2/C++ 初版与导航原型
|
||||
- [x] 整理 Python Sim2Real v2、ROS 2/C++ 初版、导航原型与 Odin/TensorRT 调参版
|
||||
- [ ] 核对比赛机械与仿真模型参数
|
||||
- [ ] 整理 URDF/MJCF 机器人描述
|
||||
- [ ] 整理后续统一训练、ROS 2 和比赛版本
|
||||
|
||||
Reference in New Issue
Block a user