diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..e9485e7 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,297 @@ +cmake_minimum_required(VERSION 3.5) +project(odin_ros_driver) + + +if(DEFINED BUILD_SYSTEM) + set(ROS_VERSION ${BUILD_SYSTEM}) + message(STATUS "使用命令行指定的构建系统: ${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() + # 尝试自动检测 + if(COMMAND catkin_package) + set(ROS_VERSION "ROS1") + elseif(COMMAND ament_package) + set(ROS_VERSION "ROS2") + else() + # 默认使用ROS2 + set(ROS_VERSION "ROS2") + message(WARNING "无法确定ROS版本，默认使用ROS2") + endif() +endif() + +# 检测 ROS 版本后添加编译宏 +if(ROS_VERSION STREQUAL "ROS2") + add_definitions(-DROS2) + message(STATUS "定义 ROS2 宏") +else() + add_definitions(-DROS1) + message(STATUS "定义 ROS1 宏") +endif() + +message(STATUS "构建系统: ${ROS_VERSION}") + +# 平台检测 +execute_process( + COMMAND uname -m + OUTPUT_VARIABLE ARCH + OUTPUT_STRIP_TRAILING_WHITESPACE +) + +if(ARCH STREQUAL "x86_64") + set(TARGET_PLATFORM "x86") + message(STATUS "检测到 x86_64 架构") +elseif(ARCH MATCHES "arm|aarch64") + set(TARGET_PLATFORM "arm") + message(STATUS "检测到 ARM 架构: ${ARCH}") +else() + message(WARNING "不支持的架构: ${ARCH}. 使用默认设置") + set(TARGET_PLATFORM "unknown") +endif() + +# 设置库路径 +set(LIB_DIR "${CMAKE_CURRENT_SOURCE_DIR}/lib") +message(STATUS "库目录: ${LIB_DIR}") + +# 根据平台设置库名称 +if(TARGET_PLATFORM STREQUAL "arm") + set(LYD_HOST_API_LIB_NAME "lydHostApi_arm") +else() + set(LYD_HOST_API_LIB_NAME "lydHostApi_amd") +endif() + +# 查找预编译的 lydHostApi 库 +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 "找到 lydHostApi 库: ${LYD_HOST_API_LIB}") +else() + file(GLOB LIB_FILES "${LIB_DIR}/lib${LYD_HOST_API_LIB_NAME}.*") + if(LIB_FILES) + message(STATUS "找到库文件: ${LIB_FILES}") + set(LYD_HOST_API_LIB ${LIB_FILES}) + else() + message(FATAL_ERROR "在 ${LIB_DIR} 中找不到预编译的 lydHostApi 库") + endif() +endif() + +# 设置公共编译选项 +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# 公共依赖查找 +find_package(PkgConfig REQUIRED) +find_package(OpenCV REQUIRED COMPONENTS core imgproc highgui) +find_package(Eigen3 REQUIRED) +find_package(yaml-cpp REQUIRED) +find_package(OpenSSL REQUIRED) +pkg_check_modules(LIBUSB REQUIRED libusb-1.0) + +# 共享包含目录 +include_directories( + include + ${EIGEN3_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${yaml-cpp_INCLUDE_DIR} + ${LIBUSB_INCLUDE_DIRS} + ${CMAKE_CURRENT_SOURCE_DIR}/include +) + +# 共享链接库列表 +set(COMMON_LIBS + ${OpenCV_LIBS} + ${yaml-cpp_LIBRARIES} + ${OPENSSL_LIBRARIES} + ${LIBUSB_LIBRARIES} + pthread + rt + ${CMAKE_DL_LIBS} + ${LYD_HOST_API_LIB} +) + +# ===== ROS1 专属配置 ===== +if(ROS_VERSION STREQUAL "ROS1") + message(STATUS "配置为 ROS1 构建") + + find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + sensor_msgs + nav_msgs + cv_bridge + 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(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 + ) + target_link_libraries(host_sdk_sample + ${catkin_LIBRARIES} + ${COMMON_LIBS} + ${LYD_HOST_API_LIB} + ${LIBUSB_LIBRARIES} + yaml-cpp + ${OpenCV_LIBS} + pthread + usb-1.0 + ) + + # 安装规则 + 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 配置 ===== +elseif(ROS_VERSION STREQUAL "ROS2") + message(STATUS "配置为 ROS2 构建") + + # 查找所有必要的ROS2包 + 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(cv_bridge REQUIRED) + find_package(image_transport REQUIRED) + + # 创建可执行文件 + add_executable(host_sdk_sample + src/host_sdk_sample.cpp + src/yaml_parser.cpp + ) + + # 链接库 + target_link_libraries(host_sdk_sample + ${catkin_LIBRARIES} + ${COMMON_LIBS} + yaml-cpp + usb-1.0 + ) + + # 添加ROS2依赖 + ament_target_dependencies(host_sdk_sample + rclcpp + std_msgs + sensor_msgs + nav_msgs + cv_bridge + image_transport + ) + + # 安装规则 - 确保所有安装目标在 ament_package() 之前定义 + # 安装可执行文件 + install(TARGETS host_sdk_sample + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} + ) + + # 安装 package.xml + install(FILES package.xml + DESTINATION share/${PROJECT_NAME} + ) + + # 安装头文件 + install(DIRECTORY include/ + DESTINATION include + ) + # 安装 launch_ROS2 目录 + if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/launch_ROS2") + install(DIRECTORY launch_ROS2/ + DESTINATION share/${PROJECT_NAME}/launch + ) + message(STATUS "安装 launch_ROS2 目录到 share/${PROJECT_NAME}/launch") + else() + message(WARNING "未找到 launch_ROS2 目录") + endif() + # 安装配置文件 (如果存在) + if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/config") + install(DIRECTORY config/ + DESTINATION share/${PROJECT_NAME}/config + ) + endif() + + # 安装启动文件 (如果存在) + if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/launch") + install(DIRECTORY launch/ + DESTINATION share/${PROJECT_NAME}/launch + ) + endif() + ament_export_targets(export_${PROJECT_NAME}) + # 声明依赖 + ament_export_dependencies( + rclcpp + std_msgs + sensor_msgs + nav_msgs + cv_bridge + image_transport + ) + + # 可选的lint检查 + + # 完成包配置 - 必须在所有安装规则之后调用 + ament_package() + message(STATUS "安装目标已添加") + +else() + message(FATAL_ERROR "无效的 ROS_VERSION: ${ROS_VERSION}") +endif() + +# ARM平台特定的链接选项 +if(TARGET_PLATFORM STREQUAL "arm") + set_target_properties(host_sdk_sample PROPERTIES + LINK_FLAGS "-Wl,--no-as-needed -Wl,--rpath=${LIB_DIR}" + ) + message(STATUS "添加 ARM 特定的链接选项和 RPATH") +endif() + +# 添加调试信息 +message(STATUS "=======================================") +message(STATUS "Project: ${PROJECT_NAME}") +message(STATUS "ROS 版本: ${ROS_VERSION}") +message(STATUS "Target platform: ${TARGET_PLATFORM}") +message(STATUS "lydHostApi library: ${LYD_HOST_API_LIB}") +message(STATUS "libusb library: ${LIBUSB_LIBRARIES}") +message(STATUS "=======================================") diff --git a/LICENSE b/LICENSE index 261eeb9..54665e1 100644 --- a/LICENSE +++ b/LICENSE @@ -186,7 +186,7 @@ same "printed page" as the copyright notice for easier identification within third-party archives. - Copyright [yyyy] [name of copyright owner] + 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. diff --git a/README.md b/README.md new file mode 100644 index 0000000..7d328f5 --- /dev/null +++ b/README.md @@ -0,0 +1,235 @@ +# Odin_ROS_Driver Readme + +ROS driver suite for Odin sensor modules (Manifold Tech Ltd.) + +## Odin_ROS_Driver + +Compatibility: + +● ROS 1(LTS Release: Noetic recommended) + +● ROS 2(LTS Release: Humble recommended) + +## Important Notice: + +This driver package provides core functionality for point cloud SLAM applications and targets specific use cases. It is intended exclusively for technical professionals conducting secondary development. End users must perform scenario-specific optimization and custom development to align with operational requirements in practical deployment environments. + +## 1. Version + +Current Version: v0.1 + +## 2. Preparation + +### 2.1 OS Requirement + +● Ubuntu 18.04 for ROS Melodic; + +● Ubuntu 20.04 for ROS Noetic and ROS2 Foxy; + +● Ubuntu 22.04 for ROS2 Humble; + +### 2.2 Dependencies + +● Opencv + +● yaml-cpp + +● thread + +● OpenSSL + +● Eigen3 + +### 2.3 Dependencies Install + +#### 2.3.1 System +```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.4 ROS install +```shell +#Installation method +For ROS Melodic installation, please refer to: ROS Melodic installation instructions +For ROS Noetic installation, please refer to: ROS Noetic installation instructions +For ROS2 Foxy installation, please refer to: ROS Foxy installation instructions +For ROS2 Humble installation, please refer to: ROS Humble installation instructions +``` + +## 3. Preparation + +### 3.1 Create Udev rules +```shell +sudo vim /etc/udev/rules.d/99-odin-usb.rules +``` +Add the following content to the 99-odin-usb.rules file +```shell +SUBSYSTEM=="usb", ATTR{idVendor}=="2207", ATTR{idProduct}=="0019", MODE="0666", GROUP="plugdev +``` +Reload rules and reinsert devices +```shell +sudo udevadm control --reload +sudo udevadm trigger +``` +### 3.2 OS Requirement +```shell +git clone https://github.com/manifoldsdk/odin_ros_driver.git catkin_ws/src/odin_ros_driver +``` +Note: +Please clone the source code into the "[workspace]/src/" folder, otherwise compilation errors will occur. + +### 3.3 make + +#### 3.3.1 ROS1 (Noetic for example): + +```shell +source /opt/ros/noetic/setup.sh +./scripty/build_ros.sh +``` + +#### 3.3.2 ROS2 (Foxy for example): + +```shell +source /opt/ros/foxy/setup.sh +./scripty/build_ros2.sh +``` + +### 3.4 run: + +#### 3.4.1 ROS1 (Noetic for example): + +```shell +source ../../devel/setup.sh +roslaunch odin_ros_driver [launch file] +``` +● odin_ros_driver: package name; + +● launch file: launch file; + +ROS1 Demo Launch Instructions: +```shell +roslaunch odin_ros_driver odin1_ros1.launch +``` +#### 3.4.2 ROS2 (Foxy for example): + +```shell +source ../../install/setup.sh +ros2 launch odin_ros_driver [launch file] +``` +● odin_ros_driver: package name; + +● launch file: launch file; + +ROS2 Demo Launch Instructions: +```shell +ros2 launch odin_ros_driver odin1_ros2.launch.py +``` +## 4. File structure and data format +### 4.1 File structure +```shell +Odin_ROS_Driver/ // ROS1/ROS2 driver package + 3rdparty/ // Third-party libraries + src/ + host_sdk_sample.cpp // Example source code + yaml_parser.cpp // Source code for reading yaml parameters + lib/ + liblydHostApi_amd.a // Static library for AMD platform + liblydHostApi_arm.a // Static library for ARM platform + include/ + host_sdk_sample.h // Example header file + lidar_api_type.h // API data structure header file + lidar_api.h // API function declarations + yaml_parser.h // Parameter file reading header file + config/ + control_command.yaml // control parameter file for driver + launch_ROS1/ + odin1_ros1.launch // ROS1 launch file + launch_ROS2/ + odin1_ros2.launch.py // ROS2 launch file + script/ + build_ros1.sh // Installation script for ROS1 + build_ros2.sh // Installation script for ROS2 + README.md // Usage instructions + CMakeLists.txt // CMake build file + License // License file +``` +### 4.2 File structure +| Launch File Name | Description | +|--------------------------|-------------| +| odin1_ros1.launch | Launch file for ROS1 - Odin1 Basic Operations Demo | +| odin1_ros2.launch.py | Launch file for ROS2 - Odin1 Basic Operations Demo | + +### 4.3 Config file +Internal parameters of the Odin ROS driver are defined in config/control_command.yaml. Below are descriptions of the commonly used parameters: +| Parameter | Detailed Description | Default | +|---------------|----------------------|---------| +| streamctrl: 1 | Master data stream control (0: OFF, 1: ON) | 1 | +| sendrgb:1 | RGB image stream control (0: OFF, 1: ON) | 1 | +| sendimu:1 | IMU (Inertial Measurement Unit) stream control (0: OFF, 1: ON) | 1 | +| sendodom:1 | Odometry data stream control (0: OFF, 1: ON) | 1 | +| senddtof:0 | Raw PointCloud sensor stream control (0: OFF, 1: ON) | 0 | +| sendcloudslam:1 | Slam PointCloud stream control (0: OFF, 1: ON) | 1 | + +### 4.4 ROS topics +Internal parameters of the Odin ROS driver are defined in config/control_command.yaml. Below are descriptions of the commonly used parameters: + +| Topic | Detailed Description | +|---------------------|----------------------| +| odin1/imu | Imu Topic | +| odin1/image | RGB Camera Topic | +| odin1/cloud_raw | Raw_Cloud Topic | +| odin1/cloud_slam | Slam_PointCloud Topic | +| odin1/odometry_map | Odom Topic | + +## 5. FAQ +### 5.1 Segmentation fault upon re-launching host SDK +**Error Message** +Core dump occurs when restarting host SDK after initial successful run + +**Solution** +```shell +power cycle Odin1 # Disconnect and reconnect LiDAR power +reinitialize host SDK # Execute SDK after device reboot +``` + +### 5.2 Library binding failure during compilation + +**Error Message** +ld: cannot find -llydHostApi or symbol lookup errors + +**Resolution** +```shell +Clean previous build artifacts +ROS1 rm -rf devel/ build/ +ROS2 rm -rf devel/ install/ log/ +Re-run script installation (refer to section 2.3) +``` + +### 5.3 Docker GUI passthrough failure + +**Error Message** +Unable to open X display or No protocol specified + +**Resolution** +```shell +xhost + #This command enables graphical passthrough to Docker containers +``` \ No newline at end of file diff --git a/config/control_command.yaml b/config/control_command.yaml new file mode 100644 index 0000000..c3477ce --- /dev/null +++ b/config/control_command.yaml @@ -0,0 +1,7 @@ +register_keys: + streamctrl: 1 + sendrgb: 1 + sendimu: 1 + sendodom: 1 + senddtof: 0 + sendcloudslam: 1 diff --git a/include/host_sdk_sample.h b/include/host_sdk_sample.h new file mode 100644 index 0000000..7faedbe --- /dev/null +++ b/include/host_sdk_sample.h @@ -0,0 +1,549 @@ +/* +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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "lidar_api.h" +#include "lidar_api_type.h" + + +#ifdef ROS2 + + #include "rclcpp/rclcpp.hpp" + #include "std_msgs/msg/string.hpp" + #include "sensor_msgs/msg/image.hpp" + #include "sensor_msgs/msg/imu.hpp" + #include "sensor_msgs/msg/point_cloud2.hpp" + #include "sensor_msgs/point_cloud2_iterator.hpp" + #include + #include + #include + #include + namespace ros { + using namespace rclcpp; + using namespace std_msgs::msg; + using namespace sensor_msgs::msg; + using namespace nav_msgs::msg; + using Time = builtin_interfaces::msg::Time; + } +#else + + #include + #include + #include + #include + #include + #include + #include + namespace ros { + using namespace ::ros; + using namespace sensor_msgs; + using namespace nav_msgs; + } +#endif + +// 公共定义 +#define GD_ACCL_G 9.7833f +#define ACC_1G_ms2 9.8 +#define ACC_SEN_SCALE 16348 +#define PAI 3.14159265358979323846 +#define GYRO_SEN_SCALE 32.8f + +// 公共函数 +inline float accel_convert(int16_t raw, int sen_scale) { + return (raw * GD_ACCL_G / sen_scale); +} + +inline float gyro_convert(int16_t raw, float sen_scale) { + return (raw * PAI) / (sen_scale * 180); +} + +inline ros::Time ns_to_ros_time(uint64_t timestamp_ns) { + ros::Time t; + #ifdef ROS2 + t.sec = static_cast(timestamp_ns / 1000000000); + t.nanosec = static_cast(timestamp_ns % 1000000000); + #else + t.sec = static_cast(timestamp_ns / 1000000000); + t.nsec = static_cast(timestamp_ns % 1000000000); + #endif + return t; +} + +inline uint64_t ros_time_to_ns(const ros::Time &t) { + #ifdef ROS2 + return static_cast(t.sec) * 1000000000ULL + t.nanosec; + #else + return static_cast(t.sec) * 1000000000ULL + t.nsec; + #endif +} + +// 多传感器发布器类 +class MultiSensorPublisher { +public: + #ifdef ROS2 + MultiSensorPublisher(rclcpp::Node::SharedPtr node) + : node_(node) { + initialize_publishers(); + } + #else + MultiSensorPublisher(ros::NodeHandle& nh) { + initialize_publishers(nh); + } + #endif + + void publishImu(icm_6aixs_data_t *stream) { + #ifdef ROS2 + sensor_msgs::msg::Imu imu_msg; + #else + ros::Imu imu_msg; + #endif + + imu_msg.header.stamp = ns_to_ros_time(stream->stamp); + imu_msg.header.frame_id = "imu_link"; + + imu_msg.linear_acceleration.y = -1 * static_cast(accel_convert(stream->aacx, ACC_SEN_SCALE)); + imu_msg.linear_acceleration.x = static_cast(accel_convert(stream->aacy, ACC_SEN_SCALE)); + imu_msg.linear_acceleration.z = static_cast(accel_convert(stream->aacz, ACC_SEN_SCALE)); + + imu_msg.angular_velocity.y = -1 * static_cast(gyro_convert(stream->gyrox, GYRO_SEN_SCALE)); + imu_msg.angular_velocity.x = static_cast(gyro_convert(stream->gyroy, GYRO_SEN_SCALE)); + imu_msg.angular_velocity.z = static_cast(gyro_convert(stream->gyroz, GYRO_SEN_SCALE)); + + imu_msg.orientation.x = 0.0; + imu_msg.orientation.y = 0.0; + imu_msg.orientation.z = 0.0; + imu_msg.orientation.w = 1.0; + + #ifdef ROS2 + imu_pub_->publish(std::move(imu_msg)); + #else + imu_pub_.publish(imu_msg); + #endif + } + +void publishIntensityCloud(capture_Image_List_t* stream, int idx) +{ + #ifdef ROS2 + sensor_msgs::msg::PointCloud2 msg; + msg.header.frame_id = "map"; + msg.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp); + + msg.height = stream->imageList[idx].height; + msg.width = stream->imageList[idx].width; + msg.is_dense = false; + + sensor_msgs::PointCloud2Modifier modifier(msg); + modifier.setPointCloud2Fields( + 4, + "x", 1, sensor_msgs::msg::PointField::FLOAT32, + "y", 1, sensor_msgs::msg::PointField::FLOAT32, + "z", 1, sensor_msgs::msg::PointField::FLOAT32, + "intensity", 1, sensor_msgs::msg::PointField::UINT8 + ); + modifier.resize(stream->imageList[idx].width * stream->imageList[idx].height); + + sensor_msgs::PointCloud2Iterator iter_x(msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(msg, "z"); + sensor_msgs::PointCloud2Iterator iter_intensity(msg, "intensity"); + #else + sensor_msgs::PointCloud2 msg; + msg.header.frame_id = "map"; + msg.header.stamp = ros::Time::now(); // ROS1使用全局时间 + msg.height = stream->imageList[idx].height; + msg.width = stream->imageList[idx].width; + msg.is_dense = false; + msg.is_bigendian = false; + + sensor_msgs::PointCloud2Modifier modifier(msg); + modifier.setPointCloud2Fields( + 4, + "x", 1, sensor_msgs::PointField::FLOAT32, + "y", 1, sensor_msgs::PointField::FLOAT32, + "z", 1, sensor_msgs::PointField::FLOAT32, + "intensity", 1, sensor_msgs::PointField::UINT8 + ); + modifier.resize(msg.height * msg.width); + + sensor_msgs::PointCloud2Iterator iter_x(msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(msg, "z"); + sensor_msgs::PointCloud2Iterator iter_intensity(msg, "intensity"); + #endif + + float* xyz_data = static_cast(stream->imageList[idx].pAddr); + uint16_t* intensity_data = static_cast(stream->imageList[2].pAddr); + + int total_points = stream->imageList[idx].height * stream->imageList[idx].width; + for (int i = 0; i < total_points; ++i) { + float* pf = xyz_data + i * 4; + + #ifdef ROS2 + *iter_x = pf[2] / 1000.0f; ++iter_x; + *iter_y = pf[0] / 1000.0f; ++iter_y; + *iter_z = -pf[1] / 1000.0f; ++iter_z; + *iter_intensity = static_cast(intensity_data[i] >> 8); ++iter_intensity; + #else + *iter_x = pf[2] / 1000.0f; ++iter_x; + *iter_y = pf[0] / 1000.0f; ++iter_y; + *iter_z = -pf[1] / 1000.0f; ++iter_z; + *iter_intensity = static_cast(intensity_data[i] >> 8); ++iter_intensity; + #endif + } + + // 发布点云 + #ifdef ROS2 + cloud_pub_->publish(std::move(msg)); + #else + cloud_pub_.publish(msg); + #endif +} + + void publishRgb(capture_Image_List_t *stream) { + buffer_List_t &image = stream->imageList[0]; + + // 验证图像参数 + if (!image.pAddr) { + #ifdef ROS2 + RCLCPP_ERROR(node_->get_logger(), "Invalid RGB image: null data pointer"); + #else + ROS_ERROR("Invalid RGB image: null data pointer"); + #endif + return; + } + + if (image.width <= 0 || image.height <= 0) { + #ifdef ROS2 + RCLCPP_ERROR(node_->get_logger(), "Invalid RGB image dimensions: %dx%d", image.width, image.height); + #else + ROS_ERROR("Invalid RGB image dimensions: %dx%d", image.width, image.height); + #endif + return; + } + + // 计算 NV12 图像高度 + const int height_nv12 = image.height * 3 / 2; + + // 验证 NV12 图像尺寸 + const size_t expected_size = static_cast(image.width) * height_nv12; + if (image.length < expected_size) { + #ifdef ROS2 + RCLCPP_ERROR(node_->get_logger(), "RGB buffer too small: expected %zu bytes, got %u bytes", + expected_size, image.length); + #else + ROS_ERROR("RGB buffer too small: expected %zu bytes, got %u bytes", + expected_size, image.length); + #endif + return; + } + + try { + cv::Mat nv12_mat(height_nv12, image.width, CV_8UC1, image.pAddr); + cv::Mat bgr; + cv::cvtColor(nv12_mat, bgr, cv::COLOR_YUV2BGR_NV12); + + if (bgr.empty()) { + #ifdef ROS2 + RCLCPP_ERROR(node_->get_logger(), "Failed to convert NV12 to BGR"); + #else + ROS_ERROR("Failed to convert NV12 to BGR"); + #endif + return; + } + + #ifdef ROS2 + std_msgs::msg::Header header; + sensor_msgs::msg::Image::SharedPtr msg; + #else + std_msgs::Header header; + sensor_msgs::Image msg; + #endif + + header.stamp = ns_to_ros_time(image.timestamp + 719060); + header.frame_id = "camera_rgb_frame"; + + #ifdef ROS2 + auto cv_image = std::make_shared(header, "bgr8", bgr); + msg = cv_image->toImageMsg(); + rgb_pub_->publish(*msg); + #else + cv_bridge::CvImage(header, "bgr8", bgr).toImageMsg(msg); + rgb_pub_.publish(msg); + #endif + + } catch (const cv::Exception& e) { + #ifdef ROS2 + RCLCPP_ERROR(node_->get_logger(), "OpenCV error in publishRgb: %s", e.what()); + #else + ROS_ERROR("OpenCV error in publishRgb: %s", e.what()); + #endif + } catch (const std::exception& e) { + #ifdef ROS2 + RCLCPP_ERROR(node_->get_logger(), "Exception in publishRgb: %s", e.what()); + #else + ROS_ERROR("Exception in publishRgb: %s", e.what()); + #endif + } + } + +void publishPC2XYZRGBA(capture_Image_List_t* stream, int idx) +{ + static int flag = 1; + #ifdef ROS2 + sensor_msgs::msg::PointCloud2 msg; + // msg.header.frame_id = "base_link"; + msg.header.frame_id = "map"; + msg.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp); + // msg.header.stamp = this->now(); + size_t pt_size = sizeof(int32_t) * 3 + sizeof(int32_t) * 4; + uint32_t points = stream->imageList[idx].length / pt_size; + + msg.height = 1; + msg.width = points; + msg.is_dense = false; + // LOG_INFO("msg.height=%ld, msg.width=%ld.\n", msg.height, msg.width); + + sensor_msgs::PointCloud2Modifier modifier(msg); + modifier.setPointCloud2Fields( + 4, + "x", 1, sensor_msgs::msg::PointField::FLOAT32, + "y", 1, sensor_msgs::msg::PointField::FLOAT32, + "z", 1, sensor_msgs::msg::PointField::FLOAT32, + "rgb", 1, sensor_msgs::msg::PointField::FLOAT32 + ); + modifier.resize(msg.width * msg.height); + + sensor_msgs::PointCloud2Iterator iter_x(msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(msg, "z"); + sensor_msgs::PointCloud2Iterator iter_rgb(msg, "rgb"); + #else + sensor_msgs::PointCloud2 msg; + msg.header.frame_id = "map"; + msg.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp); + + size_t pt_size = sizeof(int32_t) * 3 + sizeof(int32_t) * 4; + uint32_t points = stream->imageList[idx].length / pt_size; + + msg.height = 1; + msg.width = points; + msg.is_dense = false; + + sensor_msgs::PointCloud2Modifier modifier(msg); + modifier.setPointCloud2Fields( + 4, + "x", 1, sensor_msgs::PointField::FLOAT32, + "y", 1, sensor_msgs::PointField::FLOAT32, + "z", 1, sensor_msgs::PointField::FLOAT32, + "rgb", 1, sensor_msgs::PointField::FLOAT32 + ); + modifier.resize(msg.width * msg.height); + + sensor_msgs::PointCloud2Iterator iter_x(msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(msg, "z"); + sensor_msgs::PointCloud2Iterator iter_rgb(msg, "rgb"); + #endif + + // 共享的数据处理逻辑 + int32_t* xyz_data = static_cast(stream->imageList[idx].pAddr); + + for(uint32_t i = 0; i < points; i++) { + int32_t* ptr = xyz_data + 7*i; + + #ifdef ROS2 + *iter_x = static_cast(ptr[0]) / 10000.0f; ++iter_x; + *iter_y = static_cast(ptr[1]) / 10000.0f; ++iter_y; + *iter_z = static_cast(ptr[2]) / 10000.0f; ++iter_z; + #else + *iter_x = (1.0 * ptr[0]) / 1e4; ++iter_x; + *iter_y = (1.0 * ptr[1]) / 1e4; ++iter_y; + *iter_z = (1.0 * ptr[2]) / 1e4; ++iter_z; + #endif + + uint8_t r = ptr[3] & 0xff; + uint8_t g = ptr[4] & 0xff; + uint8_t b = ptr[5] & 0xff; + + uint32_t packed_rgb = (static_cast(r) << 16) | + (static_cast(g) << 8) | + static_cast(b); + + float rgb_float; + std::memcpy(&rgb_float, &packed_rgb, sizeof(float)); + + *iter_rgb = rgb_float; ++iter_rgb; + } + + #ifdef ROS2 + flag = 0; // 如果flag在其他地方使用，保留赋值 + xyzrgbacloud_pub_->publish(std::move(msg)); + #else + flag = 0; + xyzrgbacloud_pub_.publish(msg); + #endif +} + + void publishOdometry(capture_Image_List_t* stream) { + ros2_odom_convert_t* odom_data = (ros2_odom_convert_t*)stream->imageList[0].pAddr; + + #ifdef ROS2 + auto msg = nav_msgs::msg::Odometry(); + #else + ros::Odometry msg; + #endif + + msg.header.stamp = ns_to_ros_time(odom_data->timestamp_ns); + msg.header.frame_id = "map"; + msg.child_frame_id = "base_link"; + + msg.pose.pose.position.x = static_cast(odom_data->pos[0]) / 1e6; + msg.pose.pose.position.y = static_cast(odom_data->pos[1]) / 1e6; + msg.pose.pose.position.z = static_cast(odom_data->pos[2]) / 1e6; + + msg.pose.pose.orientation.x = static_cast(odom_data->orient[0]) / 1e6; + msg.pose.pose.orientation.y = static_cast(odom_data->orient[1]) / 1e6; + msg.pose.pose.orientation.z = static_cast(odom_data->orient[2]) / 1e6; + msg.pose.pose.orientation.w = static_cast(odom_data->orient[3]) / 1e6; + + #ifdef ROS2 + odom_publisher_->publish(std::move(msg)); + #else + odom_publisher_.publish(msg); + #endif + } + +private: + void initialize_publishers() { + #ifdef ROS2 + imu_pub_ = node_->create_publisher("odin1/imu", 10); + rgb_pub_ = node_->create_publisher("odin1/image", 10); + cloud_pub_ = node_->create_publisher("odin1/cloud_raw", 10); + xyzrgbacloud_pub_ = node_->create_publisher("odin1/cloud_slam", 10); + odom_publisher_ = node_->create_publisher("odin1/odometry_map", 10); + #endif + } + + #ifdef ROS1 + void initialize_publishers(ros::NodeHandle& nh) { + imu_pub_ = nh.advertise("odin1/imu", 10); + rgb_pub_ = nh.advertise("odin1/image", 10); + cloud_pub_ = nh.advertise("odin1/cloud_raw", 10); + xyzrgbacloud_pub_ = nh.advertise("odin1/cloud_slam", 10); + odom_publisher_ = nh.advertise("odin1/odometry_map", 10); + } + #endif + + #ifdef ROS2 + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr imu_pub_; + rclcpp::Publisher::SharedPtr rgb_pub_; + rclcpp::Publisher::SharedPtr cloud_pub_; + rclcpp::Publisher::SharedPtr xyzrgbacloud_pub_; + rclcpp::Publisher::SharedPtr odom_publisher_; + #else + ros::Publisher imu_pub_; + ros::Publisher rgb_pub_; + ros::Publisher cloud_pub_; + ros::Publisher xyzrgbacloud_pub_; + ros::Publisher odom_publisher_; + #endif +}; + +// 命令行控制类 +class CommandLineControl { +public: + using Callback = std::function; + + + void register_key(const std::string& key, int default_value = 0) { + std::lock_guard lock(mtx); + kv_map[key] = default_value; + } + + void set(const std::string& key, int value) { + Callback cb_to_invoke = nullptr; + { + std::lock_guard lock(mtx); + auto it = kv_map.find(key); + if (it != kv_map.end()) { + if (it->second != value) { + it->second = value; + std::cout << "Set " << key << " = " << value << std::endl; + cb_to_invoke = callback; + } else { + std::cout << "Set ignored: " << key << " is already " << value << std::endl; + } + } else { + std::cout << "Unknown key: " << key << std::endl; + return; + } + } + + if (cb_to_invoke) { + cb_to_invoke(key, value); + } + } + + int get(const std::string& key) const { + std::lock_guard lock(mtx); + auto it = kv_map.find(key); + return (it != kv_map.end()) ? it->second : -1; + } + + void print_all() const { + std::lock_guard lock(mtx); + std::cout << "Available keys and values:" << std::endl; + for (const auto& [key, value] : kv_map) { + std::cout << " " << key << " = " << value << std::endl; + } + } + + void register_callback(Callback cb) { + std::lock_guard lock(mtx); + callback = cb; + } + +private: + std::unordered_map kv_map; + std::thread input_thread; + std::atomic running; + mutable std::mutex mtx; + Callback callback; + +}; + +// 控制命令定义 +#define STREAMCTRL "streamctrl" /* start/stop all streams */ +#define SENDRGB "sendrgb" /* send RGB data */ +#define SENDIMU "sendimu" /* send IMU data */ +#define SENDODOM "sendodom" /* send odometry data */ +#define SENDDTOF "senddtof" /* send raw cloud data */ +#define SENDCLOUDSLAM "sendcloudslam" /* send rgb cloud data */ +#define EXIT "q" /* exit sample */ diff --git a/include/lidar_api.h b/include/lidar_api.h new file mode 100644 index 0000000..c19647c --- /dev/null +++ b/include/lidar_api.h @@ -0,0 +1,216 @@ +/* +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_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 + */ + +#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. + * + * @param cb Callback function for device events (connection, disconnection) + * @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 + * + * @param dev_info Information about the LiDAR device to create + * @param device Pointer to receive the device handle upon success + * @return int 0 on success, negative error code on failure + */ +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. + * + * @param device Handle to the target device + * @param cb Callback information containing function pointers for different data types + * @return int 0 on success, negative error code on failure + */ +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 + * + * @param device Handle to the target device + * @param mode Operating mode to set (see mode definitions in lidar_api_type.h) + * @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. + * + * @param device Handle to the target device + * @param type Type of data stream to start (see stream type definitions in lidar_api_type.h) + * @return int 0 on success, negative error code on failure + */ +int lidar_start_stream(device_handle device, int type); + +/** + * @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. + * + * @param device Handle to the target device + * @param type Type of data stream to activate + * @return int 0 on success, negative error code on failure + */ +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 Perform over-the-air firmware update + * + * Updates the device firmware using the specified file. + * + * @param device Handle to the target device + * @param type Type of OTA update to perform + * @param filepath Path to the firmware file + * @param process_cb Callback function to report update progress + * @return int 0 on success, negative error code on failure + */ +int lidar_ota_update(device_handle device, lidar_ota_type_e type, const char* filepath, void(*process_cb)(float process)); + +/** + * @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); + +#ifdef __cplusplus +} +#endif + +#endif // LIDAR_API_H \ No newline at end of file diff --git a/include/lidar_api_type.h b/include/lidar_api_type.h new file mode 100644 index 0000000..0bb5315 --- /dev/null +++ b/include/lidar_api_type.h @@ -0,0 +1,131 @@ +/* +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 +#include +#include + +#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 = 1 << 1, + LIDAR_DT_RAW_IMU = 1 << 2, + LIDAR_DT_RAW_DTOF = 1 << 3, + LIDAR_DT_SLAM_CLOUD = 1 << 4, + LIDAR_DT_SLAM_ODOMETRY = 1 << 5, +} lidar_data_type_e; + +typedef struct { + int8_t serial[LIDAR_SERIAL_MAX]; + int8_t model[LIDAR_MODEL_MAX]; + bool online; +} 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 icm_6aixs_data_t { + int16_t aacx; + int16_t aacy; + int16_t aacz; + int16_t gyrox; + int16_t gyroy; + int16_t gyroz; + uint8_t valid; + uint32_t nums; + uint8_t fsync_pack; + uint16_t interval; + uint64_t stamp; +} icm_6aixs_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 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; + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/include/yaml_parser.h b/include/yaml_parser.h new file mode 100644 index 0000000..887c980 --- /dev/null +++ b/include/yaml_parser.h @@ -0,0 +1,38 @@ +/* +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 +#include +#include + +namespace odin_ros_driver { + +class YamlParser { +public: + // 使用一致的成员变量名 + YamlParser(const std::string& config_file); + + bool loadConfig(); + const std::map& getRegisterKeys() const; + void printConfig() const; + +private: + std::string config_file_; + std::map register_keys_; +}; + +} // namespace odin_ros_driver + +#endif // YAML_PARSER_H \ No newline at end of file diff --git a/launch_ROS1/odin1_ros1.launch b/launch_ROS1/odin1_ros1.launch new file mode 100644 index 0000000..d978903 --- /dev/null +++ b/launch_ROS1/odin1_ros1.launch @@ -0,0 +1,12 @@ + + + + + + + + + + diff --git a/launch_ROS2/odin1_ros2.launch.py b/launch_ROS2/odin1_ros2.launch.py new file mode 100644 index 0000000..a7e1301 --- /dev/null +++ b/launch_ROS2/odin1_ros2.launch.py @@ -0,0 +1,38 @@ + +#用法: ros2 launch odin_ros_driver odin1_ros2.launch.py +import os +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_ros.actions import Node + +def generate_launch_description(): + # 获取包目录 + package_dir = get_package_share_directory('odin_ros_driver') + + # 声明配置参数 + 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' + ) + + # 创建节点 + 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') + }] + ) + + # 创建启动描述 + ld = LaunchDescription() + ld.add_action(config_file_arg) + ld.add_action(host_sdk_node) + + return ld diff --git a/lib/liblydHostApi_amd.a b/lib/liblydHostApi_amd.a new file mode 100644 index 0000000..f0fa0ff Binary files /dev/null and b/lib/liblydHostApi_amd.a differ diff --git a/lib/liblydHostApi_arm.a b/lib/liblydHostApi_arm.a new file mode 100644 index 0000000..8affc90 Binary files /dev/null and b/lib/liblydHostApi_arm.a differ diff --git a/package_ros1.xml b/package_ros1.xml new file mode 100644 index 0000000..92c28a1 --- /dev/null +++ b/package_ros1.xml @@ -0,0 +1,29 @@ + + + odin_ros_driver + 0.0.1 + ROS driver for Odin sensor + rlk + Apache 2.0 + + + catkin + + + roscpp + std_msgs + sensor_msgs + nav_msgs + cv_bridge + image_transport + + + eigen + opencv + yaml-cpp + + + + catkin + + diff --git a/package_ros2.xml b/package_ros2.xml new file mode 100644 index 0000000..5d5f028 --- /dev/null +++ b/package_ros2.xml @@ -0,0 +1,22 @@ + + + odin_ros_driver + 0.0.1 + ROS2 driver for Odin sensor + rlk + Apache 2.0 + + ament_cmake + + rclcpp + + std_msgs + sensor_msgs + nav_msgs + cv_bridge + image_transport + + + ament_cmake + + \ No newline at end of file diff --git a/script/build_ros.sh b/script/build_ros.sh new file mode 100644 index 0000000..56479ab --- /dev/null +++ b/script/build_ros.sh @@ -0,0 +1,128 @@ +#!/bin/bash + +# 获取脚本所在目录（Odin_ROS_Driver 目录） +PKG_DIR="$(cd "$(dirname "$0")/.."; pwd)" +# 计算工作空间根目录（包含 devel、build、src 的目录） +WORKSPACE_ROOT="$(dirname "$(dirname "$PKG_DIR")")" +# 工作空间源码目录（包含所有包的 src） +WORKSPACE_SRC="${WORKSPACE_ROOT}/src" +PROJECT_NAME="odin_ros_driver" + +# 定义颜色代码 +RED='\033[0;31m' +GREEN='\033[0;32m' +YELLOW='\033[1;33m' +NC='\033[0m' # 无颜色 + +# 清理函数 +clean_workspace() { + echo -e "${YELLOW}🧹 清理构建目录${NC}" + + # 清理工作空间根目录下的构建产物 + rm -rf "${WORKSPACE_ROOT}/build" + rm -rf "${WORKSPACE_ROOT}/install" + rm -rf "${WORKSPACE_ROOT}/log" + rm -rf "${WORKSPACE_ROOT}/devel" + + echo -e "${GREEN}✅ 清理完成${NC}" +} + +# 运行函数 +run_node() { + echo -e "${YELLOW}🏃‍♂️‍➡️ 运行 ROS1 节点${NC}" + + # 检查环境文件是否存在 + if [ ! -f "${WORKSPACE_ROOT}/devel/setup.bash" ]; then + echo -e "${RED}❌ 找不到 devel/setup.bash，请先执行 ./build_ros1.sh 构建项目${NC}" + return 1 + fi + + # Source 环境并运行节点 + source "${WORKSPACE_ROOT}/devel/setup.bash" + +} + +# 构建函数 +build_workspace() { + echo -e "${YELLOW}🔍 工作空间结构:${NC}" + echo " 工作空间根目录: ${WORKSPACE_ROOT}" + echo " 源码目录: ${WORKSPACE_SRC}" + echo " 包目录: ${PKG_DIR}" + echo " ROS版本: ROS1" + + echo -e "${YELLOW}🔧 开始构建 ROS1 工程...${NC}" + + # 清理 + cd $WS_DIR + rm -rf build devel install + + # 确保 ROS1 环境已加载 + 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}❌ 找不到 ROS1 的 setup.bash 文件。请确保 ROS1 已安装。${NC}" + return 1 + fi + + # 创建临时 package.xml + if [ -f "${PKG_DIR}/package_ros1.xml" ]; then + echo "🔄 创建临时 package.xml（使用 package_ros1.xml）" + cp "${PKG_DIR}/package_ros1.xml" "${PKG_DIR}/package.xml" + TEMP_PACKAGE=true + elif [ -f "${PKG_DIR}/package.xml" ]; then + echo "ℹ️ 使用现有的 package.xml" + else + echo -e "${RED}❌ 在包目录中找不到 package.xml${NC}" + return 1 + fi + + # 设置构建系统变量 + export BUILD_SYSTEM=ROS1 + + # 切换到工作空间根目录并构建 + cd "${WORKSPACE_ROOT}" || return 1 + catkin_make -DBUILD_SYSTEM=ROS1 -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -j$(nproc) + BUILD_RESULT=$? + + # 构建成功，source 环境 + if [[ $BUILD_RESULT -eq 0 ]]; then + echo -e "${GREEN}✅ ROS1 构建成功，载入环境变量：source devel/setup.bash${NC}" + source "${WORKSPACE_ROOT}/devel/setup.bash" + else + echo -e "${RED}❌ ROS1 构建失败，请检查错误日志${NC}" + fi + + +} + +# 帮助函数 +show_help() { + echo -e "${YELLOW}使用说明:${NC}" + echo " ./build_ros.sh # 构建项目" + echo " ./build_ros.sh -c # 清理构建产物" + echo " ./build_ros.sh -h # 显示帮助信息" + echo "" + echo -e "${YELLOW}当前配置:${NC}" + echo " 项目名称: ${PROJECT_NAME}" + echo " 包目录: ${PKG_DIR}" + echo " 工作空间根目录: ${WORKSPACE_ROOT}" + echo " 源码目录: ${WORKSPACE_SRC}" +} + +# 主程序 +case "$1" in + -c|--clean) + clean_workspace + ;; + -r|--run) + run_node + ;; + -h|--help) + show_help + ;; + *) + build_workspace + ;; +esac \ No newline at end of file diff --git a/script/build_ros2.sh b/script/build_ros2.sh new file mode 100644 index 0000000..2253102 --- /dev/null +++ b/script/build_ros2.sh @@ -0,0 +1,158 @@ +#!/bin/bash + +# 获取脚本所在目录（Odin_ROS_Driver 目录） +PKG_DIR="$(cd "$(dirname "$0")/.."; pwd)" +# 计算工作空间根目录（包含 devel、build、src 的目录） +WORKSPACE_ROOT="$(dirname "$(dirname "$PKG_DIR")")" +# 工作空间源码目录（包含所有包的 src） +WORKSPACE_SRC="${WORKSPACE_ROOT}/src" +PROJECT_NAME="odin_ros_driver" +PACKAGE_DIR_NAME=$(basename "$PKG_DIR") + +# 定义颜色代码 +RED='\033[0;31m' +GREEN='\033[0;32m' +YELLOW='\033[1;33m' +NC='\033[0m' # 无颜色 + +# 从 package.xml 中提取包名 +get_package_name() { + local package_xml="$1" + if [ -f "$package_xml" ]; then + # 提取 标签内容 + grep -oP '\K[^<]+' "$package_xml" | head -1 + else + echo "" + fi +} + +# 清理函数 +clean_workspace() { + echo -e "${YELLOW}🧹 清理构建目录${NC}" + + # 清理工作空间根目录下的构建产物 + rm -rf "${WORKSPACE_ROOT}/build" + rm -rf "${WORKSPACE_ROOT}/install" + rm -rf "${WORKSPACE_ROOT}/log" + rm -rf "${WORKSPACE_ROOT}/devel" + + echo -e "${GREEN}✅ 清理完成${NC}" +} + +# 运行函数 +run_node() { + echo -e "${YELLOW}🏃‍♂️‍➡️ 运行 ROS2 节点${NC}" + + # 检查环境文件是否存在 + if [ ! -f "${WORKSPACE_ROOT}/install/setup.bash" ]; then + echo -e "${RED}❌ 找不到 install/setup.bash，请先执行 ./build_ros2.sh 构建项目${NC}" + return 1 + fi + + # Source 环境并运行节点 + source "${WORKSPACE_ROOT}/install/setup.bash" + +} + +# 构建函数 +build_workspace() { + echo -e "${YELLOW}🔍 工作空间结构:${NC}" + echo " 工作空间根目录: ${WORKSPACE_ROOT}" + echo " 源码目录: ${WORKSPACE_SRC}" + echo " 包目录: ${PKG_DIR}" + echo " 目录名: ${PACKAGE_DIR_NAME}" + echo " ROS版本: ROS2" + + echo -e "${YELLOW}🔧 开始构建 ROS2 工程...${NC}" + # 清理 + cd $WS_DIR + rm -rf build install log + # 确保 ROS2 环境已加载 + 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}❌ 找不到 ROS2 的 setup.bash 文件。请确保 ROS2 已安装。${NC}" + return 1 + fi + + # 创建临时 package.xml + if [ -f "${PKG_DIR}/package_ros2.xml" ]; then + echo "🔄 创建临时 package.xml（使用 package_ros2.xml）" + cp "${PKG_DIR}/package_ros2.xml" "${PKG_DIR}/package.xml" + TEMP_PACKAGE=true + elif [ -f "${PKG_DIR}/package.xml" ]; then + echo "ℹ️ 使用现有的 package.xml" + TEMP_PACKAGE=false + else + echo -e "${RED}❌ 在包目录中找不到 package.xml${NC}" + return 1 + fi + + # 从 package.xml 中提取包名 + PACKAGE_NAME=$(get_package_name "${PKG_DIR}/package.xml") + if [ -z "$PACKAGE_NAME" ]; then + echo -e "${RED}❌ 无法从 package.xml 中提取包名${NC}" + return 1 + fi + echo " 包名: ${PACKAGE_NAME}" + + # 设置构建系统变量 + export BUILD_SYSTEM=ROS2 + + # 切换到工作空间根目录并构建 + cd "${WORKSPACE_ROOT}" || return 1 + + # 使用正确的包名构建 + colcon build \ + --packages-select "${PACKAGE_NAME}" \ + --parallel-workers $(nproc) \ + --cmake-args \ + -DBUILD_SYSTEM=ROS2 \ + -DCMAKE_EXPORT_COMPILE_COMMANDS=ON + + BUILD_RESULT=$? + + # 构建成功，source 环境 + if [[ $BUILD_RESULT -eq 0 ]]; then + echo -e "${GREEN}✅ ROS2 构建成功，载入环境变量：source install/setup.bash${NC}" + source "${WORKSPACE_ROOT}/install/setup.bash" + + else + echo -e "${RED}❌ ROS2 构建失败，请检查错误日志${NC}" + fi + +} + +# 帮助函数 +show_help() { + echo -e "${YELLOW}使用说明:${NC}" + echo " ./build_ros2.sh # 构建项目" + echo " ./build_ros2.sh -c # 清理构建产物" + echo " ./build_ros2.sh -h # 显示帮助信息" + echo "" + echo -e "${YELLOW}当前配置:${NC}" + echo " 项目名称: ${PROJECT_NAME}" + echo " 包目录: ${PKG_DIR}" + echo " 工作空间根目录: ${WORKSPACE_ROOT}" + echo " 源码目录: ${WORKSPACE_SRC}" +} + +# 主程序 +case "$1" in + -c|--clean) + clean_workspace + ;; + -r|--run) + run_node + ;; + -h|--help) + show_help + ;; + *) + build_workspace + ;; +esac \ No newline at end of file diff --git a/src/host_sdk_sample.cpp b/src/host_sdk_sample.cpp new file mode 100644 index 0000000..cdf93fd --- /dev/null +++ b/src/host_sdk_sample.cpp @@ -0,0 +1,330 @@ +#include "host_sdk_sample.h" +#include "yaml_parser.h" +#include +#include +#include +#include +#include + +#ifdef ROS2 + #include +#else + #include +#endif + +static device_handle odinDevice = nullptr; +static std::shared_ptr g_ros_object; +static std::atomic deviceConnected(false); + +// 定义获取包路径的函数 +std::string get_package_share_path(const std::string& package_name) { +#ifdef ROS2 + try { + return ament_index_cpp::get_package_share_directory(package_name); + } catch (const std::exception& e) { + throw std::runtime_error("Package not found: " + std::string(e.what())); + } +#else + try { + return ros::package::getPath(package_name); + } catch (const ros::InvalidNameException& e) { + throw std::runtime_error("Package not found: " + std::string(e.what())); + } +#endif +} + +// 全局配置变量 +static int g_sendrgb = 1; +static int g_sendimu = 1; +static int g_senddtof = 1; +static int g_sendodom = 1; +static int g_sendcloudslam = 0; + +static void lidar_data_callback(const lidar_data_t *data, void *user_data) +{ + device_handle *dev_handle = static_cast(user_data); + if(!dev_handle || !data) { + printf("Invalid device handle or data.\n"); + return; + } + + switch(data->type) { + case LIDAR_DT_NONE: + printf("empty lidar data type: %x\n", data->type); + break; + case LIDAR_DT_RAW_RGB: + if (g_sendrgb) { + g_ros_object->publishRgb((capture_Image_List_t *)&data->stream); + } + break; + case LIDAR_DT_RAW_IMU: + if (g_sendimu) { + g_ros_object->publishImu((icm_6aixs_data_t *)data->stream.imageList[0].pAddr); + } + break; + case LIDAR_DT_RAW_DTOF: + if (g_senddtof) { + g_ros_object->publishIntensityCloud((capture_Image_List_t *)&data->stream, 1); + } + break; + case LIDAR_DT_SLAM_CLOUD: + if (g_sendcloudslam) { + g_ros_object->publishPC2XYZRGBA((capture_Image_List_t *)&data->stream, 0); + } + break; + case LIDAR_DT_SLAM_ODOMETRY: + if (g_sendodom) { + g_ros_object->publishOdometry((capture_Image_List_t *)&data->stream); + } + break; + default: + printf("Unknown lidar data type: %x", data->type); + return; + } +} + +static void lidar_device_callback(const lidar_device_info_t* device, bool attach) +{ + int type = LIDAR_MODE_SLAM; + if(attach == true) { + #ifdef ROS2 + RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Device attaching..."); + #else + ROS_INFO("Device attaching..."); + #endif + + // 清理任何已存在的设备 + if (odinDevice) { + lidar_stop_stream(odinDevice, type); + lidar_unregister_stream_callback(odinDevice); + lidar_close_device(odinDevice); + lidar_destory_device(odinDevice); + odinDevice = nullptr; + } + + // 修复：使用const_cast移除const限定符 + if (lidar_create_device(const_cast(device), &odinDevice)) { + #ifdef ROS2 + RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Create device failed"); + #else + ROS_ERROR("Create device failed"); + #endif + return; + } + + // 打开设备 + if (lidar_open_device(odinDevice)) { + #ifdef ROS2 + RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Open device failed"); + #else + ROS_ERROR("Open device failed"); + #endif + lidar_destory_device(odinDevice); + odinDevice = nullptr; + return; + } + + // 设置模式 + if (lidar_set_mode(odinDevice, type)) { + #ifdef ROS2 + RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Set mode failed"); + #else + ROS_ERROR("Set mode failed"); + #endif + lidar_close_device(odinDevice); + lidar_destory_device(odinDevice); + odinDevice = nullptr; + return; + } + + // 注册回调 + lidar_data_callback_info_t data_callback_info; + data_callback_info.data_callback = lidar_data_callback; + data_callback_info.user_data = &odinDevice; + + if (lidar_register_stream_callback(odinDevice, data_callback_info)) { + #ifdef ROS2 + RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Register callback failed"); + #else + ROS_ERROR("Register callback failed"); + #endif + lidar_close_device(odinDevice); + lidar_destory_device(odinDevice); + odinDevice = nullptr; + return; + } + + // 启动数据流 + if (lidar_start_stream(odinDevice, type)) { + #ifdef ROS2 + RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Start stream failed"); + #else + ROS_ERROR("Start stream failed"); + #endif + lidar_close_device(odinDevice); + lidar_destory_device(odinDevice); + odinDevice = nullptr; + return; + } + + // 根据配置激活数据流类型 + if (g_sendrgb) { + lidar_activate_stream_type(odinDevice, LIDAR_DT_RAW_RGB); + } + if (g_sendimu) { + lidar_activate_stream_type(odinDevice, LIDAR_DT_RAW_IMU); + } + if (g_sendodom) { + lidar_activate_stream_type(odinDevice, LIDAR_DT_SLAM_ODOMETRY); + } + if (g_senddtof) { + lidar_activate_stream_type(odinDevice, LIDAR_DT_RAW_DTOF); + } + if (g_sendcloudslam) { + lidar_activate_stream_type(odinDevice, LIDAR_DT_SLAM_CLOUD); + } + + deviceConnected = true; + #ifdef ROS2 + RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Device ready and streams activated"); + #else + ROS_INFO("Device ready and streams activated"); + #endif + } else { + #ifdef ROS2 + RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Device detaching..."); + #else + ROS_INFO("Device detaching..."); + #endif + + deviceConnected = false; + + if (odinDevice) { + lidar_stop_stream(odinDevice, LIDAR_MODE_SLAM); + lidar_unregister_stream_callback(odinDevice); + lidar_close_device(odinDevice); + lidar_destory_device(odinDevice); + odinDevice = nullptr; + } + } +} + +int main(int argc, char *argv[]) +{ + // ROS初始化 + #ifdef ROS2 + rclcpp::init(argc, argv); + auto node = std::make_shared("lydros_node"); + g_ros_object = std::make_shared(node); + #else + ros::init(argc, argv, "lydros_node"); + ros::NodeHandle nh; + g_ros_object = std::make_shared(nh); + #endif + + try { + std::string package_path = get_package_share_path("odin_ros_driver"); + std::string config_file = package_path + "/config/control_command.yaml"; + + // 创建 YAML 解析器 + odin_ros_driver::YamlParser parser(config_file); + + // 加载配置 + if (!parser.loadConfig()) { + #ifdef ROS2 + RCLCPP_ERROR(node->get_logger(), "Failed to load config file: %s", config_file.c_str()); + #else + ROS_ERROR("Failed to load config file: %s", config_file.c_str()); + #endif + return -1; + } + + // 获取键值映射 + auto keys = parser.getRegisterKeys(); + + // 打印配置 + parser.printConfig(); + + // 获取键值函数 + auto get_key_value = [&](const std::string& key_name, int default_value) -> int { + auto it = keys.find(key_name); + if (it != keys.end()) { + return it->second; + } + return default_value; + }; + + // 读取配置值到全局变量 + g_sendrgb = get_key_value("sendrgb", 1); + g_sendimu = get_key_value("sendimu", 1); + g_senddtof = get_key_value("senddtof", 1); + g_sendodom = get_key_value("sendodom", 1); + g_sendcloudslam = get_key_value("sendcloudslam", 0); + + // 设置日志级别 + lidar_log_set_level(LIDAR_LOG_INFO); + + // 初始化系统，启动USB监控 + if(lidar_system_init(lidar_device_callback)) { + #ifdef ROS2 + RCLCPP_ERROR(node->get_logger(), "Lidar system init failed"); + #else + ROS_ERROR("Lidar system init failed"); + #endif + return -1; + } + + // 等待设备连接（最长30秒） + #ifdef ROS2 + RCLCPP_INFO(node->get_logger(), "Waiting for device connection..."); + #else + ROS_INFO("Waiting for device connection..."); + #endif + + auto start = std::chrono::steady_clock::now(); + while (!deviceConnected) { + auto now = std::chrono::steady_clock::now(); + auto elapsed = std::chrono::duration_cast(now - start); + + if (elapsed.count() >= 30) { + #ifdef ROS2 + RCLCPP_ERROR(node->get_logger(), "No device connected after 30 seconds"); + #else + ROS_ERROR("No device connected after 30 seconds"); + #endif + lidar_system_deinit(); + return -1; + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + } catch (const std::exception& e) { + #ifdef ROS2 + RCLCPP_ERROR(node->get_logger(), "Exception: %s", e.what()); + #else + ROS_ERROR("Exception: %s", e.what()); + #endif + lidar_system_deinit(); + return -1; + } + + // ROS主循环 + #ifdef ROS2 + rclcpp::spin(node); + rclcpp::shutdown(); + #else + ros::spin(); + ros::shutdown(); + #endif + + // 清理 + if (odinDevice) { + lidar_stop_stream(odinDevice, LIDAR_MODE_SLAM); + lidar_unregister_stream_callback(odinDevice); + lidar_close_device(odinDevice); + lidar_destory_device(odinDevice); + } + lidar_system_deinit(); + + return 0; +} diff --git a/src/yaml_parser.cpp b/src/yaml_parser.cpp new file mode 100644 index 0000000..ca6e9e1 --- /dev/null +++ b/src/yaml_parser.cpp @@ -0,0 +1,83 @@ +#include "yaml_parser.h" +#include +#include +#include +#include // 用于 std::transform + +namespace odin_ros_driver { + +// 使用一致的成员变量名 +YamlParser::YamlParser(const std::string& config_file) + : config_file_(config_file) {} // 这里使用 config_file_ + +bool YamlParser::loadConfig() { + try { + // 使用成员变量 config_file_ + std::cerr << "Loading config file: " << config_file_ << std::endl; + + // 检查文件是否存在 + if (!std::filesystem::exists(config_file_)) { + std::cerr << "Config file not found: " << config_file_ << std::endl; + return false; + } + + // 打印文件内容 + std::ifstream file(config_file_); + std::string content((std::istreambuf_iterator(file)), + std::istreambuf_iterator()); + std::cerr << "Config file content:\n" << content << "\n--- End of file ---" << std::endl; + + // 加载 YAML - 使用成员变量 config_file_ + YAML::Node config = YAML::LoadFile(config_file_); + + // 检查是否存在 register_keys 节点 + 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(); + + // 打印键值对数量 + 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(); + int value = it->second.as(); + + // 转换为小写 + std::transform(key.begin(), key.end(), key.begin(), + [](unsigned char c){ return std::tolower(c); }); + + register_keys_[key] = value; + std::cerr << "Loaded key: " << key << " = " << value << 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& YamlParser::getRegisterKeys() const { + return register_keys_; +} + +void YamlParser::printConfig() const { + std::cerr << "Configuration Keys:" << std::endl; + if (register_keys_.empty()) { + std::cerr << " (empty)" << std::endl; + return; + } + + for (const auto& [key, value] : register_keys_) { + std::cerr << " " << key << ": " << value << std::endl; + } +} + +} // namespace odin_ros_driver \ No newline at end of file