diff --git a/README.md b/README.md index 32bce0b..48bdf5e 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,7 @@ ROS driver suite for Odin sensor modules (Manifold Tech Ltd.) -Odin1 wiki: https://manifoldtehltd.github.io/wiki/Odin1/Cover.html +Odin1 wiki: https://manifoldtechltd.github.io/wiki/Odin1/Cover.html ## Odin_ROS_Driver @@ -18,7 +18,9 @@ This driver package provides core functionality for point cloud SLAM application ## 1. Version -Current Version: v0.7.1 +Current version: v0.8.0 + +Required device firmware version: v0.9.0 ## 2. Preparation @@ -262,7 +264,7 @@ float32 x // X axis, in meters float32 y // Y axis, in meters float32 z // Z axis, in meters uint8 intensity // Reflectivity, range 0–255 -uint16 confidence // Point confidence, range 0–65535 +uint16 confidence // Point confidence, actual value range from 0 to around 1300 in typical scene, higher value means more reliable. Recommanded filtering threshold is 30-35, should be adjusted accordingly. float32 offset_time // Time offset relative to the base timestamp unit: s ``` @@ -438,6 +440,40 @@ Disable odin1/image with sendrgb = 0 in control_command.yaml and try again. If t Purge the unused version of opencv and maintain a single complete version, then rebuild the driver and try again. +### 5.10 ROS Driver printing "TF_OLD_DATA ignoring data" warning + +**Error Message** + +```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 +``` + +**Reason** + +This is a ros & rviz feature to warn user that some tf data is being ignored due to timestamp conflicts. It happens when user keeps ros driver running and power-cycles odin device, which cause odin's internal system time being reset and now data timestamps conflicts with old data recieved by rviz during last run. + +**Resolution** + +There's a reset button on bottom of rviz gui. Click on this button will reset rviz's internal state and stop the warning. + +### 5.11 ROS Driver printing "unknown cmd code: xx" error + +**Error Message** + +```shell +: unknow command code 21. +``` + +**Reason** + +This is due to ros driver version mismatch with device firmware version, resulting in ros driver unable to decode new data added in newer firmware. + +**Resolution** + +Please make sure you are using most up-to-date ros driver and device firmware. + ## 6. Contact Information​​ You can contact our support through support@manifoldtech.cn diff --git a/config/control_command.yaml b/config/control_command.yaml index 233486a..63b1325 100644 --- a/config/control_command.yaml +++ b/config/control_command.yaml @@ -62,9 +62,10 @@ register_keys: showpath: 0 # 0: off; 1: on showcamerapose: 0 # 0: off; 1: on - custom_save_map: 0 custom_map_mode: 0 # 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: "" # must be set for Relocalization mode or will fail - mapping_result_dest_dir: "" # "": use default value; other: use custom value - mapping_result_file_name: "" # "": use default value; other: use custom value + + # 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 diff --git a/include/data_logger.h b/include/data_logger.h index 1199411..1366440 100644 --- a/include/data_logger.h +++ b/include/data_logger.h @@ -70,7 +70,7 @@ public: // 拼接文件名 std::filesystem::path pcFile = root_dir_ / ("MT" + timestamp + ".olx"); // Create placeholder files - write_text_file(root_dir_ / "image" / "info.txt", "device=OdinOne\ncreated_at=" + std::string(buf) + "\n"); + write_text_file(root_dir_ / "image" / "info.txt", "device=OdinOne\npointcloud=xyzrgbi\ncreated_at=" + std::string(buf) + "\n"); write_text_file(root_dir_ / "image" / "cam_in_ex.txt", "# camera intrinsics/extrinsics TBD\n"); // Init writers @@ -78,6 +78,7 @@ public: cloud_writer_ = std::make_unique(pcFile, opts.batch_size); //cloud_writer_ = std::make_unique(root_dir_ / "OdinPointCloud.olx", opts.batch_size); image_writer_ = std::make_unique(root_dir_ / "OdinImage.bin", opts.batch_size); + roatation_writer_ = std::make_unique(root_dir_ / "OdinRotate.bin", opts.batch_size); } ~BinaryDataLogger() { @@ -85,6 +86,7 @@ public: if (pose_writer_) pose_writer_->shutdown(); if (cloud_writer_) cloud_writer_->shutdown(); if (image_writer_) image_writer_->shutdown(); + if (roatation_writer_) roatation_writer_->shutdown(); } const std::filesystem::path& root_dir() const { return root_dir_; } @@ -99,6 +101,9 @@ public: void enqueueImageFrame(std::vector&& blob) { if (image_writer_) image_writer_->enqueue(std::move(blob)); } + void enqueueRotateFrame(std::vector&& blob) { + if (roatation_writer_) roatation_writer_->enqueue(std::move(blob)); + } private: struct Writer { @@ -188,4 +193,5 @@ private: std::unique_ptr pose_writer_; std::unique_ptr cloud_writer_; std::unique_ptr image_writer_; + std::unique_ptr roatation_writer_; }; diff --git a/include/host_sdk_sample.h b/include/host_sdk_sample.h index 108335a..c6c92bb 100644 --- a/include/host_sdk_sample.h +++ b/include/host_sdk_sample.h @@ -228,6 +228,10 @@ public: int get_image_index() { return image_index_.load(); } + + int get_wcwi_index() { + return wcwi_index_.load(); + } rawCloudRender render_; void publishImu(imu_convert_data_t *stream) { @@ -913,6 +917,7 @@ void publishRgb(capture_Image_List_t *stream) { uint8_t r = ptr[3] & 0xff; uint8_t g = ptr[4] & 0xff; uint8_t b = ptr[5] & 0xff; + uint8_t a = ptr[6] & 0xff; uint32_t packed_rgb = (static_cast(r) << 16) | (static_cast(g) << 8) | @@ -930,7 +935,7 @@ void publishRgb(capture_Image_List_t *stream) { const uint32_t idx_now = cloud_index_.fetch_add(1, std::memory_order_relaxed); // Compute total blob size: header + per-point payload const size_t header_size = sizeof(uint32_t) + sizeof(double) + sizeof(uint32_t); - const size_t point_size = sizeof(float) * 3 + sizeof(uint8_t) * 3; + const size_t point_size = sizeof(float) * 3 + sizeof(uint8_t) * 4; std::vector blob; blob.reserve(header_size + static_cast(points) * point_size); auto append_pod = [&](const auto& v) { @@ -949,12 +954,14 @@ void publishRgb(capture_Image_List_t *stream) { uint8_t r = static_cast(ptr[3] & 0xff); uint8_t g = static_cast(ptr[4] & 0xff); uint8_t b = static_cast(ptr[5] & 0xff); + uint8_t a = static_cast(ptr[6] & 0xff); append_pod(fx); append_pod(fy); append_pod(fz); blob.push_back(r); blob.push_back(g); blob.push_back(b); + blob.push_back(a); } data_logger_->enqueuePointCloudFrame(std::move(blob)); } @@ -966,6 +973,114 @@ void publishRgb(capture_Image_List_t *stream) { #endif } + void recordrotate(capture_Image_List_t* stream) { + if(data_logger_) { + uint32_t data_len = stream->imageList[0].length; + if (data_len == sizeof(ros_odom_convert_complete_t)) { + ros_odom_convert_complete_t* odom_data = (ros_odom_convert_complete_t*)stream->imageList[0].pAddr; + const uint32_t idx_now = wcwi_index_.fetch_add(1, std::memory_order_relaxed); + const double ts_sec = static_cast(odom_data->timestamp_ns) / 1e9; + float pose_arr[4]; + pose_arr[0] = static_cast((odom_data->orient[0]) / 1e6); + pose_arr[1] = static_cast((odom_data->orient[1]) / 1e6); + pose_arr[2] = static_cast((odom_data->orient[2]) / 1e6); + pose_arr[3] = static_cast((odom_data->orient[3]) / 1e6); + + // Save only rotate (pose_arr) to bin file + std::vector blob; + blob.reserve(sizeof(uint32_t) + sizeof(double) + sizeof(float) * 4); + auto append_pod = [&](const auto& v) { + const uint8_t* p = reinterpret_cast(&v); + blob.insert(blob.end(), p, p + sizeof(v)); + }; + append_pod(idx_now); + append_pod(ts_sec); + for (int i = 0; i < 4; ++i) append_pod(pose_arr[i]); + data_logger_->enqueueRotateFrame(std::move(blob)); + + // Build TCL matrix (4x4) from pose_cov + Eigen::Matrix4d T_CL = Eigen::Matrix4d::Identity(); + for (int idx = 0; idx < 16; ++idx) { + T_CL(idx / 4, idx % 4) = static_cast(odom_data->pose_cov[idx]); + } + + // Build TIL matrix (4x4) from twist_cov + Eigen::Matrix4d T_IL = Eigen::Matrix4d::Identity(); + for (int idx = 0; idx < 16; ++idx) { + T_IL(idx / 4, idx % 4) = static_cast(odom_data->twist_cov[idx]); + } + + // Extract rotation and translation from T_CL + Eigen::Matrix3d RCL = T_CL.block<3, 3>(0, 0); + Eigen::Vector3d TCL = T_CL.block<3, 1>(0, 3); + + // Extract rotation and translation from T_IL + Eigen::Matrix3d RIL = T_IL.block<3, 3>(0, 0); + Eigen::Vector3d TIL = T_IL.block<3, 1>(0, 3); + + // Save RIL, TIL, RCL, TCL to YAML file + static int save_count = 0; + static int index_count = 0; + save_count++; + + bool should_save = true; // Always save, or modify this condition as needed + if (should_save || save_count % 1 == 0 || index_count == 0) { + std::string OUTPUT_PATH = root_dir_.string(); + std::ofstream yaml_file; + if(index_count == 0) + yaml_file.open(OUTPUT_PATH + "/calib_online.yaml"); + else + yaml_file.open(OUTPUT_PATH + "/calib_online.yaml", std::ios::app); + + if (yaml_file.is_open()) { + yaml_file << "frame:\n"; + yaml_file << " index: " << index_count << "\n"; + yaml_file << " timestamp: " << std::fixed << std::setprecision(10) << ts_sec << "\n"; + yaml_file << " cam_num: 1\n"; + + // Save TCL (camera-lidar translation) in the same format as Tcl_0 + yaml_file << " Tcl_0: [\n"; + Eigen::Matrix4d T_CL_output = Eigen::Matrix4d::Identity(); + T_CL_output.block<3, 3>(0, 0) = RCL; + T_CL_output.block<3, 1>(0, 3) = TCL; + for (int i = 0; i < 4; i++) { + yaml_file << " "; + for (int j = 0; j < 4; j++) { + yaml_file << std::fixed << std::setprecision(10) << T_CL_output(i, j); + if (i != 3 || j != 3) yaml_file << ", "; + } + if (i != 3) yaml_file << "\n"; + } + yaml_file << "\n ]\n\n"; + + // Save TIL (IMU-lidar transformation) as body_T_lidar + yaml_file << " body_T_lidar: !!opencv-matrix\n"; + yaml_file << " rows: 4\n"; + yaml_file << " cols: 4\n"; + yaml_file << " dt: d\n"; + yaml_file << " data: [ "; + Eigen::Matrix4d T_IL_output = Eigen::Matrix4d::Identity(); + T_IL_output.block<3, 3>(0, 0) = RIL; + T_IL_output.block<3, 1>(0, 3) = TIL; + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + yaml_file << std::fixed << std::setprecision(10) << T_IL_output(i, j); + if (i != 3 || j != 3) yaml_file << ","; + } + if (i != 3) yaml_file << "\n "; + } + yaml_file << "]\n\n"; + + index_count++; + yaml_file.close(); + } + } + } + } + + } + + void publishOdometry(capture_Image_List_t* stream, OdometryType odom_type, bool show_path, bool show_camerapose) { #ifdef ROS2 @@ -1034,14 +1149,13 @@ void publishRgb(capture_Image_List_t *stream) { msg.twist.twist.angular.y = static_cast(odom_data->angular_velocity[1]) / 1e6; msg.twist.twist.angular.z = static_cast(odom_data->angular_velocity[2]) / 1e6; - msg.pose.covariance = { - static_cast(odom_data->cov[0]) / 1e9, static_cast(odom_data->cov[1]) / 1e9, static_cast(odom_data->cov[2]) / 1e9, 0.0, 0.0, 0.0, - static_cast(odom_data->cov[3]) / 1e9, static_cast(odom_data->cov[4]) / 1e9, static_cast(odom_data->cov[5]) / 1e9, 0.0, 0.0, 0.0, - static_cast(odom_data->cov[6]) / 1e9, static_cast(odom_data->cov[7]) / 1e9, static_cast(odom_data->cov[8]) / 1e9, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, static_cast(odom_data->cov[9]) / 1e9, static_cast(odom_data->cov[10]) / 1e9, static_cast(odom_data->cov[11]) / 1e9, - 0.0, 0.0, 0.0, static_cast(odom_data->cov[12]) / 1e9, static_cast(odom_data->cov[13]) / 1e9, static_cast(odom_data->cov[14]) / 1e9, - 0.0, 0.0, 0.0, static_cast(odom_data->cov[15]) / 1e9, static_cast(odom_data->cov[16]) / 1e9, static_cast(odom_data->cov[17]) / 1e9, - }; + for (int i = 0; i < 36; ++i) { + msg.pose.covariance[i] = odom_data->pose_cov[i]; + } + + for (int i = 0; i < 36; ++i) { + msg.twist.covariance[i] = odom_data->twist_cov[i]; + } } else if (data_len == sizeof(ros2_odom_convert_t)) { @@ -1407,6 +1521,7 @@ private: std::atomic pose_index_{0}; std::atomic cloud_index_{0}; std::atomic image_index_{0}; + std::atomic wcwi_index_{0}; std::filesystem::path root_dir_; diff --git a/include/lidar_api.h b/include/lidar_api.h index 1324179..c7bc9c1 100644 --- a/include/lidar_api.h +++ b/include/lidar_api.h @@ -128,7 +128,7 @@ int lidar_set_mode(device_handle device, int mode); * * @param device Handle to the target device * @param type Type of data stream to start (see stream type definitions in lidar_api_type.h) - * @param dtof_subframe_odr DTOF subframe ODR from device, used for raw point cloud per-point time offset calculation + * @param dtof_subframe_odr DTOF subframe ODR from device, used for raw point cloud per-point time offset estimation * @return int 0 on success, negative error code on failure */ int lidar_start_stream(device_handle device, int type, uint32_t &dtof_subframe_odr); @@ -195,7 +195,7 @@ void lidar_log_set_level(lidar_log_level_e level); * @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); +int lidar_get_version(device_handle device,lidar_fireware_version_t *version); /** * @brief Set custom algorithm parameters for the device diff --git a/include/lidar_api_type.h b/include/lidar_api_type.h index 5e7d56c..7b4cc61 100644 --- a/include/lidar_api_type.h +++ b/include/lidar_api_type.h @@ -92,7 +92,8 @@ typedef struct { int64_t orient[4]; int64_t linear_velocity[3]; int64_t angular_velocity[3]; - int64_t cov[3 * 3 * 2]; + double pose_cov[36]; + double twist_cov[36]; } ros_odom_convert_complete_t; typedef struct { @@ -135,12 +136,18 @@ typedef struct { } lidar_data_callback_info_t; typedef struct { - char mcu_version[64]; - char sys_version[64]; - char slam_version[64]; - char dev_app_version[64]; - char host_app_version[64]; -} lidar_version_t; + 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 diff --git a/lib/liblydHostApi_amd.a b/lib/liblydHostApi_amd.a index 9ca742e..8660301 100644 Binary files a/lib/liblydHostApi_amd.a and b/lib/liblydHostApi_amd.a differ diff --git a/lib/liblydHostApi_arm.a b/lib/liblydHostApi_arm.a index d0e7ece..232990c 100644 Binary files a/lib/liblydHostApi_arm.a and b/lib/liblydHostApi_arm.a differ diff --git a/src/host_sdk_sample.cpp b/src/host_sdk_sample.cpp index b48ab9b..136e0a0 100644 --- a/src/host_sdk_sample.cpp +++ b/src/host_sdk_sample.cpp @@ -46,8 +46,10 @@ limitations under the License. #include #include #endif -#define ros_driver_version "0.7.1" -#define recommended_firmware_version "0.8.0" +#define ros_driver_version "0.8.0" +#define required_firmware_version_major 0 +#define required_firmware_version_minor 9 +#define required_firmware_version_patch 0 // Global variable declarations static device_handle odinDevice = nullptr; @@ -107,6 +109,7 @@ int g_save_log = 0; std::filesystem::path log_root_dir_; int g_custom_map_mode = 0; +bool g_relocalization_success_msg_printed = false; std::string g_relocalization_map_abs_path = ""; std::string g_mapping_result_dest_dir = ""; @@ -375,9 +378,9 @@ static void custom_parameter_monitor() { #endif } else if (ret == 0) { #ifdef ROS2 - RCLCPP_INFO(rclcpp::get_logger("param_monitor"), "map get success"); + RCLCPP_INFO(rclcpp::get_logger("param_monitor"), "map get start success, now transfering..."); #else - ROS_INFO("map get success"); + ROS_INFO("map get start success, now transfering..."); #endif } else { #ifdef ROS2 @@ -389,10 +392,15 @@ static void custom_parameter_monitor() { } last_save_map_val = value; + } else if (result == -2) { + #ifdef ROS2 + RCLCPP_INFO(rclcpp::get_logger("param_monitor"),"file transfering, try again later..."); + #else + ROS_INFO("file transfering, try again later..."); + #endif } else { #ifdef ROS2 - RCLCPP_WARN(rclcpp::get_logger("param_monitor"), - "Failed to get save_map parameter, error: %d", result); + RCLCPP_WARN(rclcpp::get_logger("param_monitor"),"Failed to get save_map parameter, error: %d", result); #else ROS_WARN("Failed to get save_map parameter, error: %d", result); #endif @@ -962,12 +970,22 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data) { if (g_custom_map_mode == 2) { g_ros_object->publishOdometry((capture_Image_List_t *)&data->stream, OdometryType::TRANSFORM, false, false); + if (!g_relocalization_success_msg_printed) { + #ifdef ROS2 + RCLCPP_INFO(rclcpp::get_logger("odom"), "relocalization success!"); + #else + ROS_INFO("relocalization success!"); + #endif + g_relocalization_success_msg_printed = true; + } } } break; case LIDAR_DT_SLAM_WIWC: { - //... + if(g_record_data ) { + g_ros_object->recordrotate((capture_Image_List_t *)&data->stream); + } } break; default: @@ -1097,18 +1115,46 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach return; } - if(lidar_get_version(odinDevice)) { + lidar_fireware_version_t version; + if(lidar_get_version(odinDevice,&version)) { #ifdef ROS2 RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Failed to get device firmware version, potential incompatible, please upgrade device firmware and retry."); #else ROS_ERROR("Failed to get device firmware version, potential incompatible, please upgrade device firmware and retry."); #endif system("pkill -f rviz"); + system("pkill -f host_sdk_sample"); exit(1); } else { - printf("ros_driver_version:%s, recommended_firmware_version:%s\n", ros_driver_version, recommended_firmware_version); - printf("get version success.\n"); + #ifdef ROS2 + RCLCPP_INFO(rclcpp::get_logger(__func__), "ros_driver_version:%s, recommended_firmware_version:%d.%d.%d", ros_driver_version, required_firmware_version_major, required_firmware_version_minor, required_firmware_version_patch); + RCLCPP_INFO(rclcpp::get_logger(__func__), "get version success."); + RCLCPP_INFO(rclcpp::get_logger(__func__), "kernel_version: V%d.%d.%d",version.kernel_version.major,version.kernel_version.minor,version.kernel_version.patch); + RCLCPP_INFO(rclcpp::get_logger(__func__), "mcu_version: V%d.%d.%d",version.mcu_version.major,version.mcu_version.minor,version.mcu_version.patch); + RCLCPP_INFO(rclcpp::get_logger(__func__), "soc_version: V%d.%d.%d",version.soc_version.major,version.soc_version.minor,version.soc_version.patch); + RCLCPP_INFO(rclcpp::get_logger(__func__), "Daemon_proc_version: V%d.%d.%d",version.Daemon_proc_version.major,version.Daemon_proc_version.minor,version.Daemon_proc_version.patch); + RCLCPP_INFO(rclcpp::get_logger(__func__), "slam_version: V%d.%d.%d",version.slam_version.major,version.slam_version.minor,version.slam_version.patch); + #else + ROS_INFO("ros_driver_version:%s, recommended_firmware_version:%d.%d.%d", ros_driver_version, required_firmware_version_major, required_firmware_version_minor, required_firmware_version_patch); + ROS_INFO("get version success."); + ROS_INFO("kernel_version: V%d.%d.%d",version.kernel_version.major,version.kernel_version.minor,version.kernel_version.patch); + ROS_INFO("mcu_version: V%d.%d.%d",version.mcu_version.major,version.mcu_version.minor,version.mcu_version.patch); + ROS_INFO("soc_version: V%d.%d.%d",version.soc_version.major,version.soc_version.minor,version.soc_version.patch); + ROS_INFO("Daemon_proc_version: V%d.%d.%d",version.Daemon_proc_version.major,version.Daemon_proc_version.minor,version.Daemon_proc_version.patch); + ROS_INFO("slam_version: V%d.%d.%d",version.slam_version.major,version.slam_version.minor,version.slam_version.patch); + #endif + + if (version.soc_version.major < required_firmware_version_major || (version.soc_version.minor < required_firmware_version_minor) || (version.soc_version.patch < required_firmware_version_patch)) { + #ifdef ROS2 + RCLCPP_ERROR(rclcpp::get_logger(__func__),"The soc version is too low, please upgrade the device firmware to at least %d.%d.%d\n",required_firmware_version_major,required_firmware_version_minor,required_firmware_version_patch); + #else + ROS_ERROR("The soc version is too low, please upgrade the device firmware to at least %d.%d.%d\n",required_firmware_version_major,required_firmware_version_minor,required_firmware_version_patch); + #endif + system("pkill -f rviz"); + system("pkill -f host_sdk_sample"); + exit(1); + } } if (g_save_log) { @@ -1273,7 +1319,26 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach ROS_INFO("Custom map mode: %d", g_custom_map_mode); #endif - if (g_custom_map_mode == 2) { + if (g_custom_map_mode == 1) { + int save_map_init_value = 0; + int result = lidar_set_custom_parameter(odinDevice, "save_map", &save_map_init_value, sizeof(int)); + + if (result == 0) { + #ifdef ROS2 + RCLCPP_INFO(rclcpp::get_logger("command_processor"), + "Successfully initialized %s = %d", "save_map", save_map_init_value); + #else + ROS_INFO("Successfully initialized %s = %d", "save_map", save_map_init_value); + #endif + } else { + #ifdef ROS2 + RCLCPP_ERROR(rclcpp::get_logger("command_processor"), + "Failed to initialize %s = %d, error: %d", "save_map", save_map_init_value, result); + #else + ROS_ERROR("Failed to initialize %s = %d, error: %d", "save_map", save_map_init_value, result); + #endif + } + } else if (g_custom_map_mode == 2) { if (g_relocalization_map_abs_path != "" && std::filesystem::exists(g_relocalization_map_abs_path) && lidar_set_relocalization_map(odinDevice, g_relocalization_map_abs_path.c_str()) == 0) { #ifdef ROS2