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
.gitignore
+2 -1
View File
@@ -1,3 +1,4 @@
recorddata/
/config/calib.yaml
/log
/log
/map
CMakeLists.txt
+14 -3
View File
@@ -142,6 +142,7 @@ if(ROS_VERSION STREQUAL "ROS1")
sensor_msgs
nav_msgs
cv_bridge
tf
image_transport
)
@@ -224,6 +225,10 @@ elseif(ROS_VERSION STREQUAL "ROS2")
find_package(image_transport REQUIRED)
find_package(pcl_conversions REQUIRED)
find_package(message_filters REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
# Create executable
add_executable(host_sdk_sample
@@ -235,9 +240,8 @@ elseif(ROS_VERSION STREQUAL "ROS2")
# Link libraries
target_link_libraries(host_sdk_sample
${catkin_LIBRARIES}
${COMMON_LIBS}
yaml-cpp
yaml-cpp
usb-1.0
)
@@ -250,9 +254,12 @@ elseif(ROS_VERSION STREQUAL "ROS2")
visualization_msgs
cv_bridge
image_transport
tf2
tf2_ros
geometry_msgs
tf2_geometry_msgs
)
add_library(pointcloud_depth_converter_ros2 src/pointcloud_depth_converter.cpp)
target_link_libraries(pointcloud_depth_converter_ros2
${OpenCV_LIBS}
@@ -348,6 +355,10 @@ elseif(ROS_VERSION STREQUAL "ROS2")
image_transport
pcl_conversions
message_filters
tf2
tf2_ros
geometry_msgs
tf2_geometry_msgs
)
ament_package()
README.md
+39 -1
View File
@@ -16,7 +16,7 @@ This driver package provides core functionality for point cloud SLAM application
## 1. Version
Current Version: v0.5.2
Current Version: v0.6.0
## 2. Preparation
@@ -149,6 +149,37 @@ ROS2 Demo Launch Instructions:
```shell
ros2 launch odin_ros_driver odin1_ros2.launch.py
```
### 3.5 Operation Mode:
The operation mode can be configured via the `custom_map_mode` parameter in config/control_command.yaml.
#### Odometry mode
Set `custom_map_mode = 0` to enable odometry mode. In this mode, the map frame and odom frame share the same pose.
#### SLAM mode
Set `custom_map_mode = 1` to enable slam mode. This mode provides a complete SLAM system that builds upon the Odometry Mode by adding **loop closure detection** and **map saving** capabilities.
After launching the driver, odin1 will automatically perform mapping and cache map data. When the scene capture is complete, users need to execute `./set_param.sh save_map 1` in the driver's source directory to save all map data collected since the program started. The map will be saved to the location specified by the `mapping_result_dest_dir` and `mapping_result_file_name` parameters in config/control_command.yaml. If these parameters are not specified, default values will be used.
After the initial save, you can execute the command again to save a new map. Each save operation will generate a new map file. (Please allow at least 5 seconds between consecutive save operations)
The map origin corresponds to the odom coordinate system's origin at the program's startup.
##### Relocalization mode
To enable relocalization, set `custom_map_mode = 2` and specify the absolute path to the pre-built map using the `relocalization_map_abs_path` parameter in config/control_command.yaml.
Once launched, odin1 will initiate the relocalization process based on the current viewpoint and the specified map. To ensure a high success rate, it is recommended to starting within 1 meter ±10 degrees of the original position and orientation from the SLAM trajectory.
Note that relocalization performance is highly environment-dependent. In highly distinctive scenes, successful matching may occur even beyond the 1m/10° range, while other environments may require more stringent conditions. We advise testing in your target environment to determine practical tolerances.
If relocalization fails initially, the system will temporarily operate in a fallback SLAM mode (map saving is disabled in this state). During this time, you can freely move odin1. It will continue relocalization attempts in the background. Once successful, the TF between map and odom frames will be published. (Tip: Gently shaking or moving the device after initialization can help improve relocalization accuracy.)
The following topics are published in the odom frame: `/odin1/cloud_slam, /odin1/odom, /odin1/highodom and /odin1/path`. To obtain these in the map frame, apply the TF from odom frame to map frame.
## 4. File structure and data format
### 4.1 File structure
```shell
@@ -216,6 +247,8 @@ Internal parameters of the Odin ROS driver are defined in config/control_command
| odin1/cloud_slam | sendcloudslam | Slam_PointCloud Topic |
| odin1/odometry | sendodom | Odom Topic |
| odin1/odometry_high | sendodom | high frequency Odom Topic |
| odin1/path | showpath | Odom Path Topic |
| tf | sendodom | tf tree Topic |
| odin1/depth_img_competetion | senddepth | Dense depth image Topic. Demo, high computing power required. One-to-one with odin1/image_undistort. To utilize the data please directly subscribe to this topic instead of echoing it. Original value is already depth data, no need for further convert. |
| odin1/depth_img_competetion_cloud | senddepth | Dense Depth_Cloud Topic. Demo, high computing power required |
@@ -275,6 +308,11 @@ float32 rgb // RGB value
|-----------------------|----------------------|
| recorddata | Record data in specific format that can be imported into MindCloud(TM) for post-processing. Please be aware that this will consume a lot of storage space. Testing shows 9.5G for 10mins of data. |
| devstatuslog | Device status logging, currently save device status (soc temperature, cpu usage, ram usage, dtof sensor temp .etc) and data tx & rx rate to devstatus.csv under log folder. A new file will be created every time the driver is started. |
| showcamerapose | Display Camera Pose and Field of View. |
| custom_map_mode | Operation Modes: Mode 0 - Odometry mode: The map frame and odom frame share the same pose. Mode 1 - Mapping (with loop closure) mode: This mode supports map saving. Mode 2 - Relocalization mode: Requires specifying the absolute path to the map file. After successful relocalization, it will output the TF relationship between the map and odom frames.|
| custom_init_pos | Initialization Position (currently unused). |
| relocalization_map_abs_path | Absolute Path to Map File: Used for relocalization mode. |
| mapping_result_dest_dir and mapping_result_file_name| Path and Name for Saving Maps in Mapping Mode: If not specified, default values will be used. |
## 5. FAQ
### 5.1 Segmentation fault upon re-launching host SDK
config/control_command.yaml
+6
View File
@@ -14,3 +14,9 @@ register_keys:
pubintensitygray: 0 # 0: off; 1: on
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 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
config/odin_ros.rviz
+47 -19
View File
@@ -4,11 +4,12 @@ Panels:
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Odometry1
- /slam1
- /dense_depth_demo1
- /TF1/Frames1
Splitter Ratio: 0.4993045926094055
Tree Height: 549
Tree Height: 837
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
@@ -26,7 +27,7 @@ Panels:
- Class: rviz/Time
Name: Time
SyncMode: 0
SyncSource: ""
SyncSource: Image
Preferences:
PromptSaveOnExit: true
Toolbars:
@@ -116,7 +117,7 @@ Visualization Manager:
Color: 255; 255; 255
Color Transformer: RGB8
Decay Time: 0
Enabled: true
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
@@ -131,7 +132,7 @@ Visualization Manager:
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
Value: false
- Class: rviz/Group
Displays:
- Angle Tolerance: 0.10000000149011612
@@ -280,7 +281,7 @@ Visualization Manager:
{}
Queue Size: 100
Value: false
Enabled: false
Enabled: true
Name: slam
- Class: rviz/Group
Displays:
@@ -326,11 +327,38 @@ Visualization Manager:
Value: false
Enabled: false
Name: dense_depth_demo
- Class: rviz/TF
Enabled: true
Filter (blacklist): ""
Filter (whitelist): ""
Frame Timeout: 15
Frames:
All Enabled: true
map:
Value: true
odin1_base_link:
Value: true
odom:
Value: true
Marker Alpha: 1
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
odom:
map:
{}
odin1_base_link:
{}
Update Interval: 0
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: map
Fixed Frame: odom
Frame Rate: 30
Name: root
Tools:
@@ -354,7 +382,7 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
Distance: 8.56683349609375
Distance: 15.013533592224121
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
@@ -362,29 +390,29 @@ Visualization Manager:
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 1.4208989143371582
Y: -0.7709289789199829
Z: 0.5377280712127686
X: 0
Y: 0
Z: 0
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.8353985548019409
Target Frame: <Fixed Frame>
Yaw: 2.530402660369873
Pitch: 0.7853981852531433
Target Frame: odom
Yaw: 0.7853981852531433
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1016
Height: 1672
Hide Left Dock: false
Hide Right Dock: false
Image:
collapsed: false
Image_undistort:
collapsed: false
QMainWindow State: 000000ff00000000fd0000000400000000000001bf0000033afc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000262000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002a5000000d20000001600fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d00610067006500000001d5000000d70000001600fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f0072007400000002690000010e0000001600ffffff000000010000015f0000033afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000033a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000005efc0100000002fb0000000800540069006d00650100000000000007380000033700fffffffb0000000800540069006d006501000000000000045000000000000000000000040e0000033a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd00000004000000000000025f00000582fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000003b30000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000042d000001c30000002600fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d006100670065000000047d000001730000002600fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f007200740000000568000000df0000002600ffffff000000010000015f00000582fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000006e000005820000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000ab00000005efc0100000002fb0000000800540069006d0065010000000000000ab0000006dc00fffffffb0000000800540069006d00650100000000000004500000000000000000000006da0000058200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@@ -393,8 +421,8 @@ Window Geometry:
collapsed: false
Views:
collapsed: false
Width: 1848
X: 57
Y: 27
Width: 2736
X: 144
Y: 54
dense_depth_image:
collapsed: false
config/odin_ros2.rviz
+40 -16
View File
@@ -120,7 +120,7 @@ Visualization Manager:
Color: 255; 255; 255
Color Transformer: RGB8
Decay Time: 0
Enabled: true
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 2.3509885615147286e-38
@@ -141,7 +141,7 @@ Visualization Manager:
Value: /odin1/cloud_render
Use Fixed Frame: true
Use rainbow: true
Value: true
Value: false
- Class: rviz_common/Group
Displays:
- Angle Tolerance: 0.10000000149011612
@@ -318,7 +318,7 @@ Visualization Manager:
Reliability Policy: Reliable
Value: /odin1/path
Value: false
Enabled: false
Enabled: true
Name: slam
- Class: rviz_common/Group
Displays:
@@ -372,10 +372,34 @@ Visualization Manager:
Value: false
Enabled: false
Name: dense_depth_demo
- Class: rviz_default_plugins/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: true
map:
Value: true
odin1_base_link:
Value: true
odom:
Value: true
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: false
Tree:
odom:
map:
{}
odin1_base_link:
{}
Update Interval: 0
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: map
Fixed Frame: odom
Frame Rate: 30
Name: root
Tools:
@@ -418,25 +442,25 @@ Visualization Manager:
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 9.397579193115234
Distance: 10.9336576461792
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 1.1073570251464844
Y: 0.6017969250679016
Z: 0.4314231276512146
X: 0
Y: 0
Z: 0
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 1.035398006439209
Target Frame: <Fixed Frame>
Pitch: 0.3203984200954437
Target Frame: odom
Value: Orbit (rviz)
Yaw: 2.960400104522705
Yaw: 3.130404233932495
Saved: ~
Window Geometry:
Displays:
@@ -448,15 +472,15 @@ Window Geometry:
collapsed: false
Image_undistort:
collapsed: false
QMainWindow State: 000000ff00000000fd0000000400000000000001f50000039efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002d8000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000031b000000c00000002800fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d006100670065000000023e0000007a0000002800fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f00720074000000028f0000014c0000002800ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004280000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd0000000400000000000001f50000039efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002d8000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000031b000000c00000002800fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d006100670065000000023e0000007a0000002800fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f00720074000000028f0000014c0000002800ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004700000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1848
X: 392
Y: 165
Width: 1920
X: 540
Y: 124
dense_depth_image:
collapsed: false
collapsed: false
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
lib/liblydHostApi_amd.a
BIN
View File
Binary file not shown.
lib/liblydHostApi_arm.a
BIN
View File
Binary file not shown.
package_ros2.xml
+6
View File
@@ -13,8 +13,14 @@
<depend>std_msgs</depend>
<depend>sensor_msgs</depend>
<depend>nav_msgs</depend>
<depend>geometry_msgs</depend>
<depend>cv_bridge</depend>
<depend>image_transport</depend>
<depend>pcl_conversions</depend>
<depend>message_filters</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tf2_geometry_msgs</depend>
<!-- Specify build type as ament -->
<export>
<build_type>ament_cmake</build_type>
set_param.sh
Executable
+21
View File
@@ -0,0 +1,21 @@
#!/bin/bash
# Usage: ./set_param.sh <parameter_name> <value>
# Example: ./set_param.sh save_map 1
if [ $# -ne 2 ]; then
echo "Usage: $0 <parameter_name> <value>"
echo "Example: $0 save_map 1"
exit 1
fi
PARAM_NAME=$1
VALUE=$2
COMMAND_FILE="/tmp/odin_command.txt"
# Create the command file with the parameter
echo "set $PARAM_NAME $VALUE" > "$COMMAND_FILE"
echo "Command sent: set $PARAM_NAME $VALUE"
echo "Command file: $COMMAND_FILE"
src/host_sdk_sample.cpp
+401 -31
View File
@@ -37,6 +37,7 @@ limitations under the License.
#include <array>
// #include <yaml-cpp/yaml.h>
#include <iomanip>
#include <sstream>
#ifdef ROS2
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <rclcpp/rclcpp.hpp>
@@ -44,7 +45,7 @@ limitations under the License.
#include <ros/package.h>
#include <ros/ros.h>
#endif
#define ros_driver_version "0.5.2"
#define ros_driver_version "0.6.0"
// Global variable declarations
static device_handle odinDevice = nullptr;
static std::atomic<bool> deviceConnected(false);
@@ -52,15 +53,23 @@ static std::atomic<bool> deviceDisconnected(false); // Device disconnection fla
static std::mutex device_mutex; // Device operation mutex lock
static std::atomic<bool> g_connection_timeout(false);
static std::atomic<bool> g_usb_version_error(false);
static std::atomic<bool> g_shutdown_requested(false); // Signal handler flag
#ifdef ROS2
std::shared_ptr<MultiSensorPublisher> g_ros_object = nullptr;
#else
MultiSensorPublisher* g_ros_object = nullptr;
#endif
int g_log_level = LOG_LEVEL_INFO;
int g_log_level = LOG_LEVEL_INFO;
int g_show_fps = 0; // FPS display toggle control
// Custom parameter monitoring
static std::atomic<bool> g_param_monitor_running(false);
static std::thread g_param_monitor_thread;
// Command file monitoring
static std::string g_command_file_path = "";
static std::mutex g_rgb_mutex;
static std::shared_ptr<cv::Mat> g_latest_bgr;
static uint64_t g_latest_rgb_timestamp = 0;
@@ -69,6 +78,7 @@ static capture_Image_List_t g_latest_rgb;
static bool g_renderer_initialized = false;
static std::shared_ptr<rawCloudRender> g_renderer = nullptr;
std::string calib_file_ = "";
static std::shared_ptr<odin_ros_driver::YamlParser> g_parser = nullptr;
// usb device
static std::string TARGET_VENDOR = "2207";
@@ -89,9 +99,19 @@ int g_show_path = 0;
int g_show_camerapose = 0;
std::filesystem::path log_root_dir_;
int g_custom_map_mode = 0;
std::string g_relocalization_map_abs_path = "";
std::string g_mapping_result_dest_dir = "";
std::string g_mapping_result_file_name = "";
const char* DEV_STATUS_CSV_FILE = "dev_status.csv";
FILE* dev_status_csv_file = nullptr;
std::filesystem::path map_root_dir_;
char driver_start_time[32];
typedef struct {
struct timespec start = {0, 0};
struct timespec last = {0, 0};
@@ -206,6 +226,240 @@ void collect_children(pid_t pid, std::vector<pid_t>& all) {
void clear_all_queues();
static bool convert_calib_to_cam_in_ex(const std::string& calib_path, const std::filesystem::path& out_path);
// Signal handler for Ctrl+C
static void signal_handler(int signum) {
if (signum == SIGINT || signum == SIGTERM) {
#ifdef ROS2
RCLCPP_INFO(rclcpp::get_logger("signal_handler"), "Received signal %d, shutting down...", signum);
#else
ROS_INFO("Received signal %d, shutting down...", signum);
#endif
g_shutdown_requested = true;
// Stop custom parameter monitoring thread
g_param_monitor_running = false;
if (g_param_monitor_thread.joinable()) {
g_param_monitor_thread.join();
}
// Close device
if (odinDevice) {
// Convert calib.yaml to cam_in_ex.txt at program end
if (g_ros_object) {
const std::filesystem::path out_path = g_ros_object->get_root_dir() / "image" / "cam_in_ex.txt";
(void)convert_calib_to_cam_in_ex(calib_file_, out_path);
#ifdef ROS2
RCLCPP_INFO(rclcpp::get_logger("device_cb"), "pose_index: %d", g_ros_object->get_pose_index());
RCLCPP_INFO(rclcpp::get_logger("device_cb"), "cloud_index: %d", g_ros_object->get_cloud_index());
RCLCPP_INFO(rclcpp::get_logger("device_cb"), "image_index: %d", g_ros_object->get_image_index());
#else
ROS_INFO("pose_index: %d", g_ros_object->get_pose_index());
ROS_INFO("cloud_index: %d", g_ros_object->get_cloud_index());
ROS_INFO("image_index: %d", g_ros_object->get_image_index());
#endif
}
#ifdef ROS2
RCLCPP_INFO(rclcpp::get_logger("signal_handler"), "Closing device...");
#else
ROS_INFO("Closing device...");
#endif
if (lidar_stop_stream(odinDevice, LIDAR_MODE_SLAM))
{
#ifdef ROS2
RCLCPP_INFO(rclcpp::get_logger("device_cb"), "lidar_stop_stream failed");
#else
ROS_INFO("lidar_stop_stream failed");
#endif
}
odinDevice = nullptr;
}
// Deinitialize lidar system
#ifdef ROS2
RCLCPP_INFO(rclcpp::get_logger("signal_handler"), "Deinitializing lidar system...");
#else
ROS_INFO("Deinitializing lidar system...");
#endif
lidar_system_deinit();
// Close CSV file
if (dev_status_csv_file) {
std::fflush(dev_status_csv_file);
fclose(dev_status_csv_file);
dev_status_csv_file = nullptr;
}
// Shutdown ROS
#ifdef ROS2
rclcpp::shutdown();
#else
ros::shutdown();
#endif
exit(0);
}
}
// Custom parameter monitoring function
static void custom_parameter_monitor() {
int last_save_map_val = -1;
while (g_param_monitor_running && deviceConnected) {
if (odinDevice) {
if (g_custom_map_mode == 1) {
int value = 0;
int result = lidar_get_custom_parameter(odinDevice, "save_map", &value);
if (result == 0) {
// #ifdef ROS2
// RCLCPP_INFO(rclcpp::get_logger("param_monitor"), "save_map = %d", value);
// #else
// ROS_INFO("save_map = %d", value);
// #endif
if (last_save_map_val == 1 && value == 0) {
auto now = std::chrono::system_clock::now();
std::time_t t = std::chrono::system_clock::to_time_t(now);
std::tm tm{};
#ifdef _WIN32
localtime_s(&tm, &t);
#else
localtime_r(&t, &tm);
#endif
char map_save_time[32];
std::strftime(map_save_time, sizeof(map_save_time), "%Y%m%d_%H%M%S", &tm);
std::string map_dir = g_mapping_result_dest_dir != "" ? g_mapping_result_dest_dir : map_root_dir_.string();
std::string map_name = g_mapping_result_file_name != "" ? g_mapping_result_file_name : "map_" + std::string(map_save_time) + ".bin";
#ifdef ROS2
RCLCPP_INFO(rclcpp::get_logger("param_monitor"), "Map is saved on device, now transfering to [%s/%s]", map_dir.c_str(), map_name.c_str());
#else
ROS_INFO("Map is saved on device, now transfering to [%s/%s]", map_dir.c_str(), map_name.c_str());
#endif
int ret = lidar_get_mapping_result(odinDevice, map_dir.c_str(), map_name.c_str());
if (ret < 0 ) {
#ifdef ROS2
RCLCPP_WARN(rclcpp::get_logger("param_monitor"), "Failed to get mapping result");
#else
ROS_WARN("Failed to get mapping result");
#endif
} else if (ret == 0) {
#ifdef ROS2
RCLCPP_INFO(rclcpp::get_logger("param_monitor"), "map get success");
#else
ROS_INFO("map get success");
#endif
} else {
#ifdef ROS2
RCLCPP_WARN(rclcpp::get_logger("param_monitor"), "Failed to get mapping result, error code: %d", ret);
#else
ROS_WARN("Failed to get mapping result, error code: %d", ret);
#endif
}
}
last_save_map_val = value;
} else {
#ifdef ROS2
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
}
}
}
// Sleep for 1 second (1Hz)
std::this_thread::sleep_for(std::chrono::seconds(1));
}
}
// Process command from file
static void process_command_file() {
if (!std::filesystem::exists(g_command_file_path)) {
return;
}
std::ifstream file(g_command_file_path);
if (!file.is_open()) {
return;
}
std::string line;
if (std::getline(file, line)) {
file.close();
// Delete the command file after reading
std::filesystem::remove(g_command_file_path);
if (line.empty()) return;
std::istringstream iss(line);
std::string command, param_name, value_str;
if (!(iss >> command >> param_name >> value_str)) {
#ifdef ROS2
RCLCPP_WARN(rclcpp::get_logger("command_processor"), "Invalid command format. Usage: set <parameter_name> <value>");
#else
ROS_WARN("Invalid command format. Usage: set <parameter_name> <value>");
#endif
return;
}
if (command == "set") {
if (!deviceConnected || !odinDevice) {
#ifdef ROS2
RCLCPP_WARN(rclcpp::get_logger("command_processor"), "Device not connected!");
#else
ROS_WARN("Device not connected!");
#endif
return;
}
try {
int value = std::stoi(value_str);
int result = lidar_set_custom_parameter(odinDevice, param_name.c_str(), &value, sizeof(int));
if (result == 0) {
#ifdef ROS2
RCLCPP_INFO(rclcpp::get_logger("command_processor"),
"Successfully set %s = %d", param_name.c_str(), value);
#else
ROS_INFO("Successfully set %s = %d", param_name.c_str(), value);
#endif
} else {
#ifdef ROS2
RCLCPP_ERROR(rclcpp::get_logger("command_processor"),
"Failed to set %s = %d, error: %d", param_name.c_str(), value, result);
#else
ROS_ERROR("Failed to set %s = %d, error: %d", param_name.c_str(), value, result);
#endif
}
} catch (const std::exception& e) {
#ifdef ROS2
RCLCPP_ERROR(rclcpp::get_logger("command_processor"), "Invalid value: %s", value_str.c_str());
#else
ROS_ERROR("Invalid value: %s", value_str.c_str());
#endif
}
} else {
#ifdef ROS2
RCLCPP_WARN(rclcpp::get_logger("command_processor"), "Unknown command: %s", command.c_str());
#else
ROS_WARN("Unknown command: %s", command.c_str());
#endif
}
} else {
file.close();
}
}
// detect USB3.0
bool isUsb3OrHigher(const std::string& vendorId, const std::string& productId) {
std::string command = "lsusb -d " + vendorId + ":" + productId + " -v | grep 'bcdUSB'";
@@ -510,7 +764,7 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
break;
case LIDAR_DT_SLAM_ODOMETRY:
if (g_sendodom) {
g_ros_object->publishOdometry((capture_Image_List_t *)&data->stream, false, g_show_path, g_show_camerapose);
g_ros_object->publishOdometry((capture_Image_List_t *)&data->stream, OdometryType::STANDARD, g_show_path, g_show_camerapose);
}
update_count(&slam_odom_rx_fps);
break;
@@ -667,11 +921,16 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
case LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ:
{
if (g_sendodom) {
g_ros_object->publishOdometry((capture_Image_List_t *)&data->stream, true, false, false);
g_ros_object->publishOdometry((capture_Image_List_t *)&data->stream, OdometryType::HIGHFREQ, false, false);
}
update_count(&slam_odom_highfreq_rx_fps);
}
break;
case LIDAR_DT_SLAM_ODOMETRY_TF:
{
g_ros_object->publishOdometry((capture_Image_List_t *)&data->stream, OdometryType::TRANSFORM, false, false);
}
break;
default:
printf("Unknown lidar data type: %x", data->type);
return;
@@ -845,6 +1104,42 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
odinDevice = nullptr;
return;
}
// Apply custom parameters after setting mode
if (g_parser && !g_parser->applyCustomParameters(odinDevice)) {
#ifdef ROS2
RCLCPP_WARN(rclcpp::get_logger("device_cb"), "Some custom parameters failed to apply");
#else
ROS_WARN("Some custom parameters failed to apply");
#endif
}
#ifdef ROS2
RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Custom map mode: %d", g_custom_map_mode);
#else
ROS_INFO("Custom map mode: %d", g_custom_map_mode);
#endif
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
RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Relocalization map set successfully");
#else
ROS_INFO("Relocalization map set successfully");
#endif
} else {
#ifdef ROS2
RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Relocalization map path set fail");
#else
ROS_ERROR("Relocalization map path set fail");
#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;
@@ -937,7 +1232,18 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
software_connect_timing = false;
deviceConnected = true;
deviceDisconnected = false;
deviceDisconnected = false;
// Start custom parameter monitoring thread
g_param_monitor_running = true;
g_param_monitor_thread = std::thread(custom_parameter_monitor);
#ifdef ROS2
RCLCPP_INFO(rclcpp::get_logger("device_cb"),
"Command interface ready. Use: echo 'set save_map 1' > %s", g_command_file_path.c_str());
#else
ROS_INFO("Command interface ready. Use: echo 'set save_map 1' > %s", g_command_file_path.c_str());
#endif
bool load_status = g_ros_object->loadCameraParams(calib_config);
if (g_sendrgb_undistort && load_status == 0) {
@@ -962,6 +1268,13 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
deviceConnected = false;
deviceDisconnected = true;
// Stop custom parameter monitoring thread
g_param_monitor_running = false;
if (g_param_monitor_thread.joinable()) {
g_param_monitor_thread.join();
}
clear_all_queues();
@@ -991,6 +1304,10 @@ int main(int argc, char *argv[])
g_ros_object = new MultiSensorPublisher(nh);
#endif
// Register signal handlers for Ctrl+C
signal(SIGINT, signal_handler);
signal(SIGTERM, signal_handler);
try {
#ifdef ROS2
std::string package_path = get_package_source_directory();
@@ -998,10 +1315,20 @@ int main(int argc, char *argv[])
#else
std::string package_path = get_package_share_path("odin_ros_driver");
#endif
std::string config_file = package_path + "/config/control_command.yaml";
odin_ros_driver::YamlParser parser(config_file);
if (!parser.loadConfig()) {
std::string config_dir = package_path + "/config";
std::string config_file = config_dir + "/control_command.yaml";
// Initialize command file path to /tmp/odin_command.txt
g_command_file_path = "/tmp/odin_command.txt";
#ifdef ROS2
RCLCPP_INFO(rclcpp::get_logger("init"), "Command file path set to: %s", g_command_file_path.c_str());
#else
ROS_INFO("Command file path set to: %s", g_command_file_path.c_str());
#endif
g_parser = std::make_shared<odin_ros_driver::YamlParser>(config_file);
if (!g_parser->loadConfig()) {
#ifdef ROS2
RCLCPP_ERROR(node->get_logger(), "Failed to load config file: %s", config_file.c_str());
#else
@@ -1010,8 +1337,9 @@ int main(int argc, char *argv[])
return -1;
}
auto keys = parser.getRegisterKeys();
parser.printConfig();
auto keys = g_parser->getRegisterKeys();
auto keys_w_str_val = g_parser->getRegisterKeysStrVal();
g_parser->printConfig();
auto get_key_value = [&](const std::string& key, int default_value) -> int {
auto it = keys.find(key);
@@ -1033,52 +1361,74 @@ int main(int argc, char *argv[])
g_show_path = get_key_value("showpath", 0);
g_show_camerapose = get_key_value("showcamerapose", 0);
g_log_level = get_key_value("log_devel", LOG_LEVEL_INFO);
auto get_key_str_value = [&](const std::string& key, const std::string& default_value) -> std::string {
auto it = keys_w_str_val.find(key);
return it != keys_w_str_val.end() ? it->second : default_value;
};
g_relocalization_map_abs_path = get_key_str_value("relocalization_map_abs_path", "");
g_mapping_result_dest_dir = get_key_str_value("mapping_result_dest_dir", "");
g_mapping_result_file_name = get_key_str_value("mapping_result_file_name", "");
g_custom_map_mode = g_parser->getCustomMapMode(2);
lidar_log_set_level(LIDAR_LOG_INFO);
const std::string package_name = "odin_ros_driver";
std::string data_dir = "";
std::string log_dir = "";
std::string map_dir = "";
#ifdef ROS2
char* ros_workspace = std::getenv("COLCON_PREFIX_PATH");
if (ros_workspace) {
std::string workspace_path(ros_workspace);
size_t pos = workspace_path.find("/install");
if (pos != std::string::npos) {
data_dir = workspace_path.substr(0, pos) + "/src/odin_ros_driver/recorddata";
log_dir = workspace_path.substr(0, pos) + "/src/odin_ros_driver/log";
std::string workspace_path(ros_workspace);
size_t pos = workspace_path.find("/install");
if (pos != std::string::npos) {
data_dir = workspace_path.substr(0, pos) + "/src/odin_ros_driver/recorddata";
log_dir = workspace_path.substr(0, pos) + "/src/odin_ros_driver/log";
map_dir = workspace_path.substr(0, pos) + "/src/odin_ros_driver/map";
} else {
data_dir = ament_index_cpp::get_package_share_directory(package_name) + "/recorddata";
log_dir = ament_index_cpp::get_package_share_directory(package_name) + "/log";
map_dir = ament_index_cpp::get_package_share_directory(package_name) + "/map";
}
} else {
data_dir = ament_index_cpp::get_package_share_directory(package_name) + "/recorddata";
log_dir = ament_index_cpp::get_package_share_directory(package_name) + "/log";
}
} else {
data_dir = ament_index_cpp::get_package_share_directory(package_name) + "/recorddata";
log_dir = ament_index_cpp::get_package_share_directory(package_name) + "/log";
map_dir = ament_index_cpp::get_package_share_directory(package_name) + "/map";
}
#else
data_dir = ros::package::getPath(package_name) + "/recorddata";
log_dir = ros::package::getPath(package_name) + "/log";
map_dir = ros::package::getPath(package_name) + "/map";
#endif
if (g_record_data) {
g_ros_object->initialize_data_logger(data_dir);
}
auto now = std::chrono::system_clock::now();
std::time_t t = std::chrono::system_clock::to_time_t(now);
std::tm tm{};
#ifdef _WIN32
localtime_s(&tm, &t);
#else
localtime_r(&t, &tm);
#endif
std::strftime(driver_start_time, sizeof(driver_start_time), "%Y%m%d_%H%M%S", &tm);
if (g_devstatus_log) {
auto now = std::chrono::system_clock::now();
std::time_t t = std::chrono::system_clock::to_time_t(now);
std::tm tm{};
#ifdef _WIN32
localtime_s(&tm, &t);
#else
localtime_r(&t, &tm);
#endif
char buf[32];
std::strftime(buf, sizeof(buf), "%Y%m%d_%H%M%S", &tm);
std::string folder_name = std::string("Driver_") + std::string(buf);
std::string folder_name = std::string("Driver_") + std::string(driver_start_time);
log_root_dir_ = std::filesystem::path(log_dir) / folder_name;
std::filesystem::create_directories(log_root_dir_);
}
if (g_custom_map_mode == 1 && g_mapping_result_dest_dir == "") {
map_root_dir_ = std::filesystem::path(map_dir) / driver_start_time;
std::filesystem::create_directories(map_root_dir_);
}
if (lidar_system_init(lidar_device_callback)) {
#ifdef ROS2
RCLCPP_ERROR(node->get_logger(), "Lidar system init failed");
@@ -1178,6 +1528,12 @@ int main(int argc, char *argv[])
if (g_sendcloudrender) {
g_ros_object->try_process_pair();
}
// Check for command file
if (deviceConnected) {
process_command_file();
}
disconnect_msg_printed = false;
// Wait 0.1 seconds
@@ -1206,6 +1562,12 @@ int main(int argc, char *argv[])
if (g_sendcloudrender) {
g_ros_object->try_process_pair();
}
// Check for command file
if (deviceConnected) {
process_command_file();
}
disconnect_msg_printed = false;
// Wait 0.1 seconds
@@ -1257,6 +1619,14 @@ int main(int argc, char *argv[])
dev_status_csv_file = nullptr;
}
}
// Stop custom parameter monitoring thread on exit
g_param_monitor_running = false;
if (g_param_monitor_thread.joinable()) {
g_param_monitor_thread.join();
}
// lidar_system_deinit();
src/yaml_parser.cpp
+167 -24
View File
@@ -15,57 +15,133 @@ limitations under the License.
#include <fstream>
#include <filesystem>
#include <iostream>
#include <algorithm>
#include <algorithm>
#include <iomanip>
#include <cstring>
namespace odin_ros_driver {
YamlParser::YamlParser(const std::string& config_file)
: config_file_(config_file) {} // // Using config_file_ member variable
YamlParser::YamlParser(const std::string& config_file)
: config_file_(config_file) {}
bool YamlParser::loadConfig() {
try {
// Using member variable config_file_
std::cerr << "Loading config file: " << config_file_ << std::endl;
// Check if file exists
if (!std::filesystem::exists(config_file_)) {
std::cerr << "Config file not found: " << config_file_ << std::endl;
return false;
}
// Print file contents
std::ifstream file(config_file_);
std::string content((std::istreambuf_iterator<char>(file)),
std::string content((std::istreambuf_iterator<char>(file)),
std::istreambuf_iterator<char>());
std::cerr << "Config file content:\n" << content << "\n--- End of file ---" << std::endl;
// Load YAML
YAML::Node config = YAML::LoadFile(config_file_);
// Check if 'register_keys' node exists
if (!config["register_keys"]) {
std::cerr << "Missing 'register_keys' section in config file" << std::endl;
return false;
}
YAML::Node register_keys = config["register_keys"];
register_keys_.clear();
register_keys_str_val_.clear();
custom_parameters_.clear();
// Print number of key-value pairs found
std::cerr << "Found " << register_keys.size() << " keys in config" << std::endl;
for (YAML::const_iterator it = register_keys.begin(); it != register_keys.end(); ++it) {
std::string key = it->first.as<std::string>();
int value = it->second.as<int>();
const YAML::Node& value_node = it->second;
// Convert key to lowercase
std::transform(key.begin(), key.end(), key.begin(),
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;
// Check if this is a custom parameter
if (key.substr(0, 7) == "custom_") {
std::string param_name = key.substr(7);
// Handle different value types
if (value_node.IsScalar()) {
// Single scalar value (int or float)
try {
int int_value = value_node.as<int>();
ParameterValue param_value;
param_value.type = DataType::INT_TYPE;
param_value.setData(int_value);
custom_parameters_[param_name] = param_value;
std::cerr << "Loaded custom parameter (int): " << param_name << " = " << int_value << std::endl;
} catch (...) {
try {
double float_value = value_node.as<double>();
ParameterValue param_value;
param_value.type = DataType::FLOAT_ARRAY_TYPE;
std::vector<float> float_array = {static_cast<float>(float_value)};
param_value.setArray(float_array);
custom_parameters_[param_name] = param_value;
std::cerr << "Loaded custom parameter (float): " << param_name << " = " << float_value << std::endl;
} catch (const std::exception& e) {
std::cerr << "Failed to parse custom parameter " << param_name << ": " << e.what() << std::endl;
}
}
} else if (value_node.IsSequence()) {
// Array of values
size_t array_size = value_node.size();
if (array_size == 0) {
std::cerr << "Empty array for custom parameter: " << param_name << std::endl;
continue;
}
// Try to detect if it's a float or int array based on first element
try {
// Try to parse as float array first
std::vector<float> float_array;
for (size_t i = 0; i < array_size; ++i) {
float_array.push_back(value_node[i].as<float>());
}
ParameterValue param_value;
param_value.type = DataType::FLOAT_ARRAY_TYPE;
param_value.setArray(float_array);
custom_parameters_[param_name] = param_value;
std::cerr << "Loaded custom parameter (float array): " << param_name << " = [";
for (size_t i = 0; i < float_array.size(); ++i) {
if (i > 0) std::cerr << ", ";
std::cerr << std::fixed << std::setprecision(4) << float_array[i];
}
std::cerr << "]" << std::endl;
} catch (const std::exception& e) {
std::cerr << "Failed to parse custom parameter array " << param_name << ": " << e.what() << std::endl;
}
}
} else if (allowed_key_w_str_val.find(key) != allowed_key_w_str_val.end()) {
try {
std::string value = value_node.as<std::string>();
register_keys_str_val_[key] = value;
std::cerr << "Loaded key: " << key << " = " << value << std::endl;
} catch (const std::exception& e) {
std::cerr << "Failed to parse key " << key << ": " << e.what() << std::endl;
}
} else {
// Regular (non-custom) integer parameter
try {
int value = value_node.as<int>();
register_keys_[key] = value;
std::cerr << "Loaded key: " << key << " = " << value << std::endl;
} catch (const std::exception& e) {
std::cerr << "Failed to parse key " << key << ": " << e.what() << std::endl;
}
}
}
return true;
} catch (const YAML::Exception& e) {
std::cerr << "YAML exception: " << e.what() << std::endl;
@@ -80,16 +156,83 @@ const std::map<std::string, int>& YamlParser::getRegisterKeys() const {
return register_keys_;
}
const std::map<std::string, ParameterValue>& YamlParser::getCustomParameters() const {
return custom_parameters_;
}
const std::map<std::string, std::string>& YamlParser::getRegisterKeysStrVal() const {
return register_keys_str_val_;
}
void YamlParser::printConfig() const {
std::cerr << "Configuration Keys:" << std::endl;
if (register_keys_.empty()) {
std::cerr << " (empty)" << std::endl;
return;
std::cerr << " (int val empty)" << std::endl;
} else {
for (const auto& [key, value] : register_keys_) {
std::cerr << " " << key << ": " << value << std::endl;
}
}
for (const auto& [key, value] : register_keys_) {
std::cerr << " " << key << ": " << value << std::endl;
if (register_keys_str_val_.empty()) {
std::cerr << " (str_val empty)" << std::endl;
} else {
for (const auto& [key, value] : register_keys_str_val_) {
std::cerr << " " << key << ": " << value << std::endl;
}
}
std::cerr << "Custom Parameters:" << std::endl;
if (custom_parameters_.empty()) {
std::cerr << " (custom param empty)" << std::endl;
} else {
for (const auto& [key, param_val] : custom_parameters_) {
std::cerr << " " << key << ": (size=" << param_val.getSize() << " bytes)";
if (param_val.type == DataType::INT_TYPE && param_val.getSize() == sizeof(int)) {
int int_val = *reinterpret_cast<const int*>(param_val.getData());
std::cerr << " = " << int_val;
} else if (param_val.type == DataType::FLOAT_ARRAY_TYPE && param_val.getSize() % sizeof(float) == 0) {
size_t count = param_val.getSize() / sizeof(float);
const float* float_arr = reinterpret_cast<const float*>(param_val.getData());
std::cerr << " = [";
for (size_t i = 0; i < count; ++i) {
if (i > 0) std::cerr << ", ";
std::cerr << std::fixed << std::setprecision(4) << float_arr[i];
}
std::cerr << "]";
}
std::cerr << std::endl;
}
}
}
bool YamlParser::applyCustomParameters(device_handle device) {
bool success = true;
for (const auto& [param_name, param_value] : custom_parameters_) {
std::cerr << "Setting custom parameter: " << param_name << " (size=" << param_value.getSize() << " bytes)" << std::endl;
int result = lidar_set_custom_parameter(device, param_name.c_str(), param_value.getData(), param_value.getSize());
if (result != 0) {
std::cerr << "Failed to set custom parameter " << param_name << ": error code " << result << std::endl;
success = false;
} else {
std::cerr << "Successfully set custom parameter " << param_name << std::endl;
}
}
return success;
}
int YamlParser::getCustomParameterInt(const std::string& param_name, int default_value) const {
auto it = custom_parameters_.find(param_name);
if (it != custom_parameters_.end()) {
const ParameterValue& param_value = it->second;
if (param_value.type == DataType::INT_TYPE && param_value.getSize() == sizeof(int)) {
return *reinterpret_cast<const int*>(param_value.getData());
}
}
return default_value;
}
}