wanngwe/odin_ros_driver1
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

<add> 1. add odin1 SLAM mode && relocalization mode support

Browse Source 
2. optimize rx data fps calculation
3. optimize exit logic on ctrl-c to reduce issue on restart the the driver
This commit is contained in:
mt-lifan
2025-10-29 10:27:41 +08:00
parent 28c17e5611
commit 8d337abf8e
16 changed files with 1079 additions and 240 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/host_sdk_sample.h
+217 -133
View File
@@ -55,6 +55,12 @@ struct CameraParams {
double p1, p2;
};
enum class OdometryType {
STANDARD = LIDAR_DT_SLAM_ODOMETRY,
HIGHFREQ = LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ,
TRANSFORM = LIDAR_DT_SLAM_ODOMETRY_TF
};
#define LOG_LEVEL_NONE 0
#define LOG_LEVEL_ERROR 1
#define LOG_LEVEL_WARN 2
@@ -79,6 +85,8 @@ extern int g_sendcloudrender;
#include <nav_msgs/msg/odometry.hpp>
#include <nav_msgs/msg/path.hpp>
#include <sensor_msgs/msg/point_field.hpp>
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/transform_broadcaster.h"
namespace ros {
using namespace rclcpp;
using namespace std_msgs::msg;
@@ -106,7 +114,9 @@ extern int g_sendcloudrender;
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/Image.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
namespace ros {
using namespace ::ros;
using namespace sensor_msgs;
@@ -454,7 +464,7 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
// Create and publish RGB point cloud
PointCloud2Msg output_msg;
output_msg.header.frame_id = "map";
output_msg.header.frame_id = "odin1_base_link";
output_msg.header.stamp = rgb_msg->header.stamp; // Use original image timestamp
output_msg.height = 1;
output_msg.width = valid_point_num;
@@ -523,7 +533,7 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
#endif
// Set message header
msg->header.frame_id = "map";
msg->header.frame_id = "odin1_base_link";
msg->header.stamp = ns_to_ros_time(cloud.timestamp);
msg->height = cloud.height;
@@ -901,9 +911,9 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
{
#ifdef ROS2
sensor_msgs::msg::PointCloud2 msg;
msg.header.frame_id = "map";
msg.header.frame_id = "odom";
msg.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp);
//RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Point cloudrgba %ld",stream->imageList[0].timestamp);
size_t pt_size = sizeof(int32_t) * 3 + sizeof(int32_t) * 4;
@@ -929,7 +939,7 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
sensor_msgs::PointCloud2Iterator<float> iter_rgb(msg, "rgb");
#else
sensor_msgs::PointCloud2 msg;
msg.header.frame_id = "map";
msg.header.frame_id = "odom";
msg.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp);
size_t pt_size = sizeof(int32_t) * 3 + sizeof(int32_t) * 4;
@@ -1027,7 +1037,7 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
#endif
}
void publishOdometry(capture_Image_List_t* stream, bool is_highfreq, bool show_path, bool show_camerapose) {
void publishOdometry(capture_Image_List_t* stream, OdometryType odom_type, bool show_path, bool show_camerapose) {
#ifdef ROS2
auto msg = nav_msgs::msg::Odometry();
@@ -1035,8 +1045,8 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
ros::Odometry msg;
#endif
msg.header.frame_id = "map";
msg.child_frame_id = "base_link";
msg.header.frame_id = "odom";
msg.child_frame_id = "odin1_base_link";
//RCLCPP_INFO(rclcpp::get_logger("device_cb"), "odom %ld",odom_data->timestamp_ns);
@@ -1056,7 +1066,7 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
msg.pose.pose.orientation.w = static_cast<double>(odom_data->orient[3]) / 1e6;
// Enqueue binary logging for pose
if (!is_highfreq && data_logger_) {
if ((odom_type == OdometryType::STANDARD) && data_logger_) {
const uint32_t idx_now = pose_index_.fetch_add(1, std::memory_order_relaxed);
const double ts_sec = static_cast<double>(odom_data->timestamp_ns) / 1e9;
float pose_arr[7];
@@ -1112,127 +1122,193 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
}
#ifdef ROS2
if (is_highfreq) {
odom_highfreq_publisher_->publish(std::move(msg));
} else {
odom_publisher_->publish(std::move(msg));
switch(odom_type) {
case OdometryType::STANDARD:
{
geometry_msgs::msg::TransformStamped transformStamped;
transformStamped.header.stamp = msg.header.stamp;
transformStamped.header.frame_id = "odom";
transformStamped.child_frame_id = "odin1_base_link";
transformStamped.transform.translation.x = msg.pose.pose.position.x;
transformStamped.transform.translation.y = msg.pose.pose.position.y;
transformStamped.transform.translation.z = msg.pose.pose.position.z;
transformStamped.transform.rotation.x = msg.pose.pose.orientation.x;
transformStamped.transform.rotation.y = msg.pose.pose.orientation.y;
transformStamped.transform.rotation.z = msg.pose.pose.orientation.z;
transformStamped.transform.rotation.w = msg.pose.pose.orientation.w;
tf_broadcaster->sendTransform(transformStamped);
odom_publisher_->publish(msg);
// Publish odom trajectory as visualization markers (green lines connecting adjacent points)
static visualization_msgs::msg::Marker marker;
static std::vector<geometry_msgs::msg::Point> path_points;
if (show_path) {
marker.header = msg.header;
marker.ns = "odom_trajectory";
marker.id = 0;
marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
marker.action = visualization_msgs::msg::Marker::ADD;
marker.pose.orientation.w = 1.0;
marker.scale.x = 0.02; // Line width
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
marker.color.a = 1.0;
geometry_msgs::msg::Point pt;
pt.x = msg.pose.pose.position.x;
pt.y = msg.pose.pose.position.y;
pt.z = msg.pose.pose.position.z;
path_points.push_back(pt);
// Publish odom trajectory as visualization markers (green lines connecting adjacent points)
static visualization_msgs::msg::Marker marker;
static std::vector<geometry_msgs::msg::Point> path_points;
// Keep only recent points to avoid memory issues (e.g., last 1000 points)
if (path_points.size() > 30000) {
path_points.erase(path_points.begin());
if (show_path) {
marker.header = msg.header;
marker.ns = "odom_trajectory";
marker.id = 0;
marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
marker.action = visualization_msgs::msg::Marker::ADD;
marker.pose.orientation.w = 1.0;
marker.scale.x = 0.02; // Line width
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
marker.color.a = 1.0;
geometry_msgs::msg::Point pt;
pt.x = msg.pose.pose.position.x;
pt.y = msg.pose.pose.position.y;
pt.z = msg.pose.pose.position.z;
path_points.push_back(pt);
// Keep only recent points to avoid memory issues (e.g., last 1000 points)
if (path_points.size() > 30000) {
path_points.erase(path_points.begin());
}
marker.points = path_points;
// Publish marker array
static visualization_msgs::msg::MarkerArray marker_array;
marker_array.markers.clear(); // Clear previous markers
marker_array.markers.push_back(marker);
path_publisher_->publish(marker_array);
}
marker.points = path_points;
// Publish marker array
static visualization_msgs::msg::MarkerArray marker_array;
marker_array.markers.clear(); // Clear previous markers
marker_array.markers.push_back(marker);
path_publisher_->publish(marker_array);
}
if (show_camerapose) {
// camera pose visualization (ROS2)
Eigen::Vector3d P(msg.pose.pose.position.x,
msg.pose.pose.position.y,
msg.pose.pose.position.z);
Eigen::Quaterniond R(msg.pose.pose.orientation.w,
msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z);
if (extrinsic_ok_) {
P = P + R * t_ic_;
R = R * R_ic_;
if (show_camerapose) {
// camera pose visualization (ROS2)
Eigen::Vector3d P(msg.pose.pose.position.x,
msg.pose.pose.position.y,
msg.pose.pose.position.z);
Eigen::Quaterniond R(msg.pose.pose.orientation.w,
msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z);
if (extrinsic_ok_) {
P = P + R * t_ic_;
R = R * R_ic_;
}
cameraposevisual_.reset();
cameraposevisual_.add_pose(P, R);
cameraposevisual_.publish_by(*pub_camera_pose_visual_, msg.header);
}
cameraposevisual_.reset();
cameraposevisual_.add_pose(P, R);
cameraposevisual_.publish_by(*pub_camera_pose_visual_, msg.header);
}
}
break;
case OdometryType::HIGHFREQ:
odom_highfreq_publisher_->publish(std::move(msg));
break;
case OdometryType::TRANSFORM:
{
geometry_msgs::msg::TransformStamped transformStamped;
transformStamped.header.stamp = msg.header.stamp;
transformStamped.header.frame_id = "odom";
transformStamped.child_frame_id = "map";
transformStamped.transform.translation.x = msg.pose.pose.position.x;
transformStamped.transform.translation.y = msg.pose.pose.position.y;
transformStamped.transform.translation.z = msg.pose.pose.position.z;
transformStamped.transform.rotation.x = msg.pose.pose.orientation.x;
transformStamped.transform.rotation.y = msg.pose.pose.orientation.y;
transformStamped.transform.rotation.z = msg.pose.pose.orientation.z;
transformStamped.transform.rotation.w = msg.pose.pose.orientation.w;
tf_broadcaster->sendTransform(transformStamped);
}
break;
}
#else
if (is_highfreq) {
odom_highfreq_publisher_.publish(msg);
} else {
odom_publisher_.publish(msg);
switch(odom_type) {
case OdometryType::STANDARD:
{
geometry_msgs::TransformStamped transformStamped;
transformStamped.header.stamp = msg.header.stamp;
transformStamped.header.frame_id = "odom";
transformStamped.child_frame_id = "odin1_base_link";
transformStamped.transform.translation.x = msg.pose.pose.position.x;
transformStamped.transform.translation.y = msg.pose.pose.position.y;
transformStamped.transform.translation.z = msg.pose.pose.position.z;
transformStamped.transform.rotation.x = msg.pose.pose.orientation.x;
transformStamped.transform.rotation.y = msg.pose.pose.orientation.y;
transformStamped.transform.rotation.z = msg.pose.pose.orientation.z;
transformStamped.transform.rotation.w = msg.pose.pose.orientation.w;
tf_broadcaster->sendTransform(transformStamped);
odom_publisher_.publish(msg);
if (show_path) {
// Publish odom trajectory as visualization markers (green lines connecting adjacent points)
static visualization_msgs::Marker marker;
static std::vector<geometry_msgs::Point> path_points;
marker.header = msg.header;
marker.ns = "odom_trajectory";
marker.id = 0;
marker.type = visualization_msgs::Marker::LINE_STRIP;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.orientation.w = 1.0;
marker.scale.x = 0.02; // Line width
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
marker.color.a = 1.0;
geometry_msgs::Point pt;
pt.x = msg.pose.pose.position.x;
pt.y = msg.pose.pose.position.y;
pt.z = msg.pose.pose.position.z;
path_points.push_back(pt);
// Keep only recent points to avoid memory issues (e.g., last 1000 points)
if (path_points.size() > 30000) {
path_points.erase(path_points.begin());
}
marker.points = path_points;
// Publish marker array
static visualization_msgs::MarkerArray marker_array;
marker_array.markers.clear(); // Clear previous markers
marker_array.markers.push_back(marker);
path_publisher_.publish(marker_array);
}
if (show_camerapose) {
// camera pose visualization (ROS1)
Eigen::Vector3d P(msg.pose.pose.position.x,
msg.pose.pose.position.y,
msg.pose.pose.position.z);
Eigen::Quaterniond R(msg.pose.pose.orientation.w,
msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z);
if (show_path) {
// Publish odom trajectory as visualization markers (green lines connecting adjacent points)
static visualization_msgs::Marker marker;
static std::vector<geometry_msgs::Point> path_points;
if (extrinsic_ok_) {
P = P + R * t_ic_;
R = R * R_ic_;
marker.header = msg.header;
marker.ns = "odom_trajectory";
marker.id = 0;
marker.type = visualization_msgs::Marker::LINE_STRIP;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.orientation.w = 1.0;
marker.scale.x = 0.02; // Line width
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
marker.color.a = 1.0;
geometry_msgs::Point pt;
pt.x = msg.pose.pose.position.x;
pt.y = msg.pose.pose.position.y;
pt.z = msg.pose.pose.position.z;
path_points.push_back(pt);
// Keep only recent points to avoid memory issues (e.g., last 1000 points)
if (path_points.size() > 30000) {
path_points.erase(path_points.begin());
}
marker.points = path_points;
// Publish marker array
static visualization_msgs::MarkerArray marker_array;
marker_array.markers.clear(); // Clear previous markers
marker_array.markers.push_back(marker);
path_publisher_.publish(marker_array);
}
cameraposevisual_.reset();
cameraposevisual_.add_pose(P, R);
cameraposevisual_.publish_by(pub_camera_pose_visual_, msg.header);
}
if (show_camerapose) {
// camera pose visualization (ROS1)
Eigen::Vector3d P(msg.pose.pose.position.x,
msg.pose.pose.position.y,
msg.pose.pose.position.z);
Eigen::Quaterniond R(msg.pose.pose.orientation.w,
msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z);
if (extrinsic_ok_) {
P = P + R * t_ic_;
R = R * R_ic_;
}
cameraposevisual_.reset();
cameraposevisual_.add_pose(P, R);
cameraposevisual_.publish_by(pub_camera_pose_visual_, msg.header);
}
}
break;
case OdometryType::HIGHFREQ:
odom_highfreq_publisher_.publish(msg);
break;
case OdometryType::TRANSFORM:
{
geometry_msgs::TransformStamped transformStamped;
transformStamped.header.stamp = msg.header.stamp;
transformStamped.header.frame_id = "odom";
transformStamped.child_frame_id = "map";
transformStamped.transform.translation.x = msg.pose.pose.position.x;
transformStamped.transform.translation.y = msg.pose.pose.position.y;
transformStamped.transform.translation.z = msg.pose.pose.position.z;
transformStamped.transform.rotation.x = msg.pose.pose.orientation.x;
transformStamped.transform.rotation.y = msg.pose.pose.orientation.y;
transformStamped.transform.rotation.z = msg.pose.pose.orientation.z;
transformStamped.transform.rotation.w = msg.pose.pose.orientation.w;
tf_broadcaster->sendTransform(transformStamped);
}
break;
}
#endif
}
@@ -1436,18 +1512,23 @@ private:
void initialize_publishers() {
#ifdef ROS2
imu_pub_ = node_->create_publisher<ros::Imu>("odin1/imu", 10);
rgb_pub_ = node_->create_publisher<ros::Image>("odin1/image", 10);
cloud_pub_ = node_->create_publisher<ros::PointCloud2>("odin1/cloud_raw", 10);
xyzrgbacloud_pub_ = node_->create_publisher<ros::PointCloud2>("odin1/cloud_slam", 10);
odom_publisher_ = node_->create_publisher<ros::Odometry>("odin1/odometry", 10);
odom_highfreq_publisher_ = node_->create_publisher<ros::Odometry>("odin1/odometry_highfreq", 10);
path_publisher_ = node_->create_publisher<visualization_msgs::msg::MarkerArray>("odin1/path", 10);
pub_camera_pose_visual_ = node_->create_publisher<visualization_msgs::msg::MarkerArray>("odin1/camera_pose_visual", 10);
rgbcloud_pub_ = node_->create_publisher<sensor_msgs::msg::PointCloud2>("odin1/cloud_render", 10);
compressed_rgb_pub_ = node_->create_publisher<sensor_msgs::msg::CompressedImage>("odin1/image/compressed", 10);
undistort_rgb_pub_ = node_->create_publisher<sensor_msgs::msg::Image>("odin1/image/undistorted", 10);
intensity_gray_pub_ = node_->create_publisher<sensor_msgs::msg::Image>("odin1/image/intensity_gray", 10);
auto qos_profile = rclcpp::QoS(1)
.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE)
.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE);
imu_pub_ = node_->create_publisher<ros::Imu>("odin1/imu", qos_profile);
rgb_pub_ = node_->create_publisher<ros::Image>("odin1/image", qos_profile);
cloud_pub_ = node_->create_publisher<ros::PointCloud2>("odin1/cloud_raw", qos_profile);
xyzrgbacloud_pub_ = node_->create_publisher<ros::PointCloud2>("odin1/cloud_slam", qos_profile);
odom_publisher_ = node_->create_publisher<ros::Odometry>("odin1/odometry", qos_profile);
odom_highfreq_publisher_ = node_->create_publisher<ros::Odometry>("odin1/odometry_highfreq", qos_profile);
path_publisher_ = node_->create_publisher<visualization_msgs::msg::MarkerArray>("odin1/path", qos_profile);
pub_camera_pose_visual_ = node_->create_publisher<visualization_msgs::msg::MarkerArray>("odin1/camera_pose_visual", qos_profile);
rgbcloud_pub_ = node_->create_publisher<sensor_msgs::msg::PointCloud2>("odin1/cloud_render", qos_profile);
compressed_rgb_pub_ = node_->create_publisher<sensor_msgs::msg::CompressedImage>("odin1/image/compressed", qos_profile);
undistort_rgb_pub_ = node_->create_publisher<sensor_msgs::msg::Image>("odin1/image/undistorted", qos_profile);
intensity_gray_pub_ = node_->create_publisher<sensor_msgs::msg::Image>("odin1/image/intensity_gray", qos_profile);
tf_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(node_);
#endif
}
#ifdef ROS1
@@ -1464,6 +1545,7 @@ private:
compressed_rgb_pub_ = nh.advertise<sensor_msgs::CompressedImage>("odin1/image/compressed", 10);
undistort_rgb_pub_ = nh.advertise<sensor_msgs::Image>("odin1/image/undistorted", 10);
intensity_gray_pub_ = nh.advertise<sensor_msgs::Image>("odin1/image/intensity_gray", 10);
tf_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>();
}
#endif
@@ -1484,6 +1566,7 @@ private:
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr intensity_gray_pub_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_camera_pose_visual_;
camera_pose_visualization cameraposevisual_;
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster;
#else
ros::Publisher imu_pub_;
ros::Publisher rgb_pub_;
@@ -1500,6 +1583,7 @@ private:
ros::Publisher compressed_rgb_pub_; // New compressed image publisher
ros::Publisher undistort_rgb_pub_;
ros::Publisher intensity_gray_pub_;
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster;
#endif
};
include/lidar_api.h
+48
View File
@@ -197,6 +197,54 @@ void lidar_log_set_level(lidar_log_level_e level);
*/
int lidar_get_version(device_handle device);
/**
* @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 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);
#ifdef __cplusplus
}
#endif
include/lidar_api_type.h
+9 -8
View File
@@ -47,14 +47,15 @@ typedef enum {
} 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_DT_DEV_STATUS = 1 << 6,
LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ = 1 << 7,
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_data_type_e;
typedef struct {
include/yaml_parser.h
+62 -4
View File
@@ -13,25 +13,83 @@ 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::string config_file_;
std::map<std::string, int> register_keys_;
std::map<std::string, std::string> register_keys_str_val_;
std::map<std::string, ParameterValue> custom_parameters_;
std::unordered_set<std::string> allowed_key_w_str_val = {"relocalization_map_abs_path", "mapping_result_dest_dir", "mapping_result_file_name"};
};
}
}
#endif
#endif