[software] 添加16DOF早期训练仿真与Sim2Real闭环
This commit is contained in:
@@ -0,0 +1,36 @@
|
||||
cmake_minimum_required(VERSION 3.16)
|
||||
project(odin1 LANGUAGES C CXX)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
|
||||
|
||||
find_package(PkgConfig REQUIRED)
|
||||
find_package(OpenSSL REQUIRED)
|
||||
pkg_check_modules(LIBUSB REQUIRED libusb-1.0)
|
||||
|
||||
add_library(odin1_imu_bridge SHARED
|
||||
src/odin1_imu_bridge.cpp
|
||||
)
|
||||
|
||||
target_include_directories(odin1_imu_bridge
|
||||
PUBLIC
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/include
|
||||
${LIBUSB_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
target_link_directories(odin1_imu_bridge
|
||||
PRIVATE
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/lib
|
||||
)
|
||||
|
||||
target_link_libraries(odin1_imu_bridge
|
||||
PRIVATE
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/lib/liblydHostApi_arm.a
|
||||
${LIBUSB_LIBRARIES}
|
||||
OpenSSL::SSL
|
||||
OpenSSL::Crypto
|
||||
pthread
|
||||
rt
|
||||
dl
|
||||
)
|
||||
@@ -0,0 +1,8 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
|
||||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
BUILD_DIR="${SCRIPT_DIR}/build"
|
||||
|
||||
cmake -S "${SCRIPT_DIR}" -B "${BUILD_DIR}" -DCMAKE_BUILD_TYPE=Release
|
||||
cmake --build "${BUILD_DIR}" -j"$(nproc)"
|
||||
Binary file not shown.
@@ -0,0 +1,308 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
#ifndef LIDAR_API_H
|
||||
#define LIDAR_API_H
|
||||
|
||||
/**
|
||||
* @file lidar_api.h
|
||||
* @brief LiDAR device API for controlling and accessing LiDAR sensor data
|
||||
*
|
||||
* This header provides the public interface for interacting with LiDAR devices.
|
||||
* It includes functions for device management, data streaming control, and
|
||||
* device configuration.
|
||||
*
|
||||
* @copyright Copyright (c) 2025, Manifold Tech Limited, All Rights Reserved
|
||||
* @version 1.0
|
||||
*/
|
||||
|
||||
#include "lidar_api_type.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief Initialize the LiDAR system
|
||||
*
|
||||
* Must be called before any other lidar function to set up the system resources.
|
||||
*
|
||||
* @param cb Callback function for device events (connection, disconnection)
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_system_init(lidar_device_callback_t cb);
|
||||
|
||||
/**
|
||||
* @brief Deinitialize the LiDAR system
|
||||
*
|
||||
* Releases all resources allocated by the system. Should be called when
|
||||
* application is shutting down.
|
||||
*
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_system_deinit(void);
|
||||
|
||||
/**
|
||||
* @brief Create a handle for a LiDAR device
|
||||
*
|
||||
* @param dev_info Information about the LiDAR device to create
|
||||
* @param device Pointer to receive the device handle upon success
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_create_device(lidar_device_info_t *dev_info, device_handle *device);
|
||||
|
||||
/**
|
||||
* @brief Destroy a LiDAR device handle
|
||||
*
|
||||
* Releases resources associated with the device handle. Must be called
|
||||
* when the device is no longer needed.
|
||||
*
|
||||
* @param device Handle to the device to destroy
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_destory_device(device_handle device);
|
||||
|
||||
/**
|
||||
* @brief Register callback function for receiving LiDAR data streams
|
||||
*
|
||||
* Sets up a callback function that will be called when new data is available.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param cb Callback information containing function pointers for different data types
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_register_stream_callback(device_handle device, lidar_data_callback_info_t cb);
|
||||
|
||||
/**
|
||||
* @brief Unregister stream callback for a device
|
||||
*
|
||||
* Stops the device from calling back when new data is available.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_unregister_stream_callback(device_handle device);
|
||||
|
||||
/**
|
||||
* @brief Open a LiDAR device for communication
|
||||
*
|
||||
* Establishes a connection to the physical device.
|
||||
*
|
||||
* @param device Handle to the device to open
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_open_device(device_handle device);
|
||||
|
||||
/**
|
||||
* @brief Close a LiDAR device
|
||||
*
|
||||
* Closes the connection to the physical device.
|
||||
*
|
||||
* @param device Handle to the device to close
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_close_device(device_handle device);
|
||||
|
||||
/**
|
||||
* @brief Set the operating mode of the LiDAR device
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param mode Operating mode to set (see mode definitions in lidar_api_type.h)
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_set_mode(device_handle device, int mode);
|
||||
|
||||
/**
|
||||
* @brief Start data streaming from the device
|
||||
*
|
||||
* Begins the flow of data from the device for the specified type.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param type Type of data stream to start (see stream type definitions in lidar_api_type.h)
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_start_stream(device_handle device, int type, uint32_t &dtof_subframe_odr);
|
||||
|
||||
/**
|
||||
* @brief Stop data streaming from the device
|
||||
*
|
||||
* Stops the flow of data from the device for the specified type.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param type Type of data stream to stop
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_stop_stream(device_handle device, int type);
|
||||
|
||||
/**
|
||||
* @brief Activate a specific stream type on the device
|
||||
*
|
||||
* Enables a specific data stream type in the device configuration.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param type Type of data stream to activate
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_activate_stream_type(device_handle device, int type);
|
||||
|
||||
/**
|
||||
* @brief Deactivate a specific stream type on the device
|
||||
*
|
||||
* Disables a specific data stream type in the device configuration.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param type Type of data stream to deactivate
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_deactivate_stream_type(device_handle device, int type);
|
||||
|
||||
/**
|
||||
* @brief Get calibration file from the device
|
||||
*
|
||||
* Retrieves the calibration file from the device.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param path Path to save the calibration file
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_get_calib_file(device_handle device, const char* path);
|
||||
|
||||
/**
|
||||
* @brief Set log verbosity level
|
||||
*
|
||||
* Controls the amount of log information generated by the LiDAR API.
|
||||
*
|
||||
* @param level Log level to set (see level definitions in lidar_api_type.h)
|
||||
*/
|
||||
void lidar_log_set_level(lidar_log_level_e level);
|
||||
|
||||
/**
|
||||
* @brief Get the version information of the LiDAR device
|
||||
*
|
||||
* Retrieves version information including firmware, system, and application versions.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param version struct Pointer to receive the version information
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_get_version(device_handle device,lidar_fireware_version_t *version);
|
||||
|
||||
/**
|
||||
* @brief Set custom algorithm parameters for the device
|
||||
*
|
||||
* Sends custom parameter settings to the device.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param param_name String name of the parameter to set
|
||||
* @param value_data Pointer to the value data to set for the parameter
|
||||
* @param value_length Length of the value data in bytes
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_set_custom_parameter(device_handle device, const char* param_name, const void* value_data, size_t value_length);
|
||||
|
||||
/**
|
||||
* @brief Get custom algorithm parameters for the device
|
||||
*
|
||||
* Get custom parameter settings from the device.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param param_name String name of the parameter to get
|
||||
* @param value Integer value to get for the parameter
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_get_custom_parameter(device_handle device, const char* param_name, int* value);
|
||||
|
||||
/**
|
||||
* @brief Set the map file used for relocalization
|
||||
*
|
||||
* Read & send specified map file to device for relocalization
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param abs_path Absolute path to the map file
|
||||
* @return int 0 on success, otherwise on failure
|
||||
*/
|
||||
int lidar_set_relocalization_map(device_handle device, const char* abs_path);
|
||||
|
||||
/**
|
||||
* @brief Get the mapping result file from device
|
||||
*
|
||||
* Read & send specified map file from device to host
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param dest_dir Destination directory to save the map file
|
||||
* @param file_name File name to save the map file
|
||||
* @return int 0 on success, -1 on failure without error code, error code (> 0) otherwise
|
||||
*/
|
||||
int lidar_get_mapping_result(device_handle device, const char* dest_dir, const char* file_name);
|
||||
|
||||
/**
|
||||
* @brief Set the image mask file for the device
|
||||
*
|
||||
* Read & send specified image mask file to device
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param abs_path Absolute path to the image mask file (e.g., mask.png)
|
||||
* @return int 0 on success, -1 on failure, -2 if file transfer in progress
|
||||
*/
|
||||
int lidar_set_image_mask(device_handle device, const char* abs_path);
|
||||
|
||||
|
||||
/**
|
||||
* @brief enable device log
|
||||
*
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param dest_dir Destination directory to save the logs
|
||||
* @return int 0 on success, -1 on failure
|
||||
*/
|
||||
int lidar_enable_encrypted_device_log(device_handle device, const char* dest_dir);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Set the depth parameters for the device
|
||||
*
|
||||
* This function must be called before starting data stream.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param params Pointer to the depth parameters to set
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_set_depth_parameter(device_handle device, const lidar_depth_para_t *params);
|
||||
|
||||
/**
|
||||
* @brief Enable or disable IMU smooth sending feature
|
||||
*
|
||||
* When enabled, IMU data will be sent at precise intervals (default 400Hz)
|
||||
* using a dedicated high-priority thread to reduce jitter and timing variance.
|
||||
* When disabled, IMU data will be sent immediately upon reception.
|
||||
*
|
||||
* @param enable 1 to enable smooth sending, 0 to disable
|
||||
* @return int 0 on success, -1 on failure
|
||||
*/
|
||||
int lidar_enable_imu_smooth_sending(int enable);
|
||||
|
||||
/**
|
||||
* @brief Set IMU smooth sending frequency
|
||||
*
|
||||
* Set the target frequency for IMU smooth sending. Only effective when
|
||||
* smooth sending is enabled via lidar_enable_imu_smooth_sending().
|
||||
*
|
||||
* @param frequency_hz Target frequency in Hz (1-1000 Hz, recommended 400 Hz)
|
||||
* @return int 0 on success, -1 on failure
|
||||
*/
|
||||
int lidar_set_imu_smooth_frequency(uint32_t frequency_hz);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // LIDAR_API_H
|
||||
@@ -0,0 +1,242 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
#ifndef LIDAR_TYPES_H
|
||||
#define LIDAR_TYPES_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#define LIDAR_SERIAL_MAX 64
|
||||
#define LIDAR_MODEL_MAX 64
|
||||
#define LIDAR_IP_MAX 64
|
||||
|
||||
typedef void * device_handle;
|
||||
|
||||
typedef enum {
|
||||
LIDAR_LOG_ERROR = 0,
|
||||
LIDAR_LOG_WARN,
|
||||
LIDAR_LOG_INFO,
|
||||
LIDAR_LOG_DEBUG,
|
||||
} lidar_log_level_e;
|
||||
|
||||
typedef enum {
|
||||
LIDAR_OTA_ALGORITHM,
|
||||
LIDAR_OTA_FIRMWARE,
|
||||
LIDAR_OTA_SCRIPT,
|
||||
LIDAR_OTA_CALIBRATION
|
||||
} lidar_ota_type_e;
|
||||
|
||||
typedef enum {
|
||||
LIDAR_MODE_RAW,
|
||||
LIDAR_MODE_SLAM,
|
||||
} lidar_mode_e;
|
||||
|
||||
typedef enum {
|
||||
LIDAR_DT_NONE = 0,
|
||||
LIDAR_DT_RAW_RGB,
|
||||
LIDAR_DT_RAW_IMU,
|
||||
LIDAR_DT_RAW_DTOF,
|
||||
LIDAR_DT_SLAM_CLOUD,
|
||||
LIDAR_DT_SLAM_ODOMETRY,
|
||||
LIDAR_DT_DEV_STATUS,
|
||||
LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ,
|
||||
LIDAR_DT_SLAM_ODOMETRY_TF,
|
||||
LIDAR_DT_SLAM_WIWC,
|
||||
LIDAR_DT_NTP
|
||||
} lidar_data_type_e;
|
||||
|
||||
typedef struct {
|
||||
int8_t serial[LIDAR_SERIAL_MAX];
|
||||
int8_t model[LIDAR_MODEL_MAX];
|
||||
bool online;
|
||||
uint32_t initial_state;
|
||||
} lidar_device_info_t;
|
||||
|
||||
typedef struct {
|
||||
float x, y, z;
|
||||
float intensity;
|
||||
} lidar_point_t;
|
||||
|
||||
|
||||
typedef struct {
|
||||
float intrinsics[9];
|
||||
float extrinsics[16];
|
||||
} lidar_calibration_t;
|
||||
|
||||
|
||||
#define DEVICE_MAX_CH_NUMBER 4
|
||||
|
||||
typedef struct {
|
||||
uint64_t timestamp_ns;
|
||||
int64_t pos[3];
|
||||
int64_t orient[4];
|
||||
} ros2_odom_convert_t;
|
||||
|
||||
typedef struct {
|
||||
uint64_t timestamp_ns;
|
||||
int64_t pos[3];
|
||||
int64_t orient[4];
|
||||
int64_t linear_velocity[3];
|
||||
int64_t angular_velocity[3];
|
||||
double pose_cov[36];
|
||||
double twist_cov[36];
|
||||
} ros_odom_convert_complete_t;
|
||||
|
||||
typedef struct {
|
||||
float accel_x;
|
||||
float accel_y;
|
||||
float accel_z;
|
||||
float gyro_x;
|
||||
float gyro_y;
|
||||
float gyro_z;
|
||||
uint64_t stamp;
|
||||
uint64_t sequence;
|
||||
} imu_convert_data_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t length;
|
||||
uint64_t sequence;
|
||||
uint64_t timestamp;
|
||||
uint64_t interval;
|
||||
void* pAddr;
|
||||
uint32_t width;
|
||||
uint32_t height;
|
||||
} buffer_List_t;
|
||||
|
||||
typedef struct {
|
||||
double delay;
|
||||
double offset;
|
||||
} ptp_sync_data_t;
|
||||
|
||||
typedef struct capture_Image_List_t {
|
||||
uint32_t imageCount;
|
||||
buffer_List_t imageList[DEVICE_MAX_CH_NUMBER];
|
||||
} capture_Image_List_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t type;
|
||||
capture_Image_List_t stream;
|
||||
} lidar_data_t;
|
||||
|
||||
typedef void (*lidar_device_callback_t)(const lidar_device_info_t* device, bool attach);
|
||||
typedef void (*lidar_data_callback_t)(const lidar_data_t *data, void *user_data);
|
||||
|
||||
typedef struct {
|
||||
lidar_data_callback_t data_callback;
|
||||
void *user_data;
|
||||
} lidar_data_callback_info_t;
|
||||
|
||||
typedef struct {
|
||||
int major;
|
||||
int minor;
|
||||
int patch;
|
||||
}lidar_version_t;
|
||||
|
||||
typedef struct {
|
||||
lidar_version_t kernel_version;
|
||||
lidar_version_t mcu_version;
|
||||
lidar_version_t soc_version;
|
||||
lidar_version_t Daemon_proc_version;
|
||||
lidar_version_t slam_version;
|
||||
} lidar_fireware_version_t;
|
||||
|
||||
/**
|
||||
* @brief RGB image sensor frame rate
|
||||
*
|
||||
*/
|
||||
typedef struct{
|
||||
|
||||
int configured_odr; /* rgb image sensor configured output data rate */
|
||||
int tx_odr; /* rgb image sensor tx output data rate */
|
||||
|
||||
} lidar_rgb_sensor_status_t;
|
||||
|
||||
/**
|
||||
* @brief DTOF Lidar frame rate
|
||||
*
|
||||
*/
|
||||
typedef struct{
|
||||
|
||||
int configured_odr; /* dtof lidar sensor configured output data rate */
|
||||
int tx_odr; /* dtof lidar sensor tx output data rate */
|
||||
int subframe_odr; /* dtof lidar sensor subframe output data rate */
|
||||
short tx_temp; /* dtof lidar tx module temp */
|
||||
short rx_temp; /* dtof lidar rx module temp */
|
||||
|
||||
} lidar_dtof_sensor_status_t;
|
||||
|
||||
/**
|
||||
* @brief IMU Sensor
|
||||
*
|
||||
*/
|
||||
typedef struct{
|
||||
|
||||
int configured_odr; /* imu sensor configured output data rate */
|
||||
int tx_odr; /* imu sensor tx output data rate */
|
||||
|
||||
} lidar_imu_sensor_status_t;
|
||||
|
||||
typedef struct{
|
||||
|
||||
int package_temp; /* soc package temp */
|
||||
int cpu_temp; /* cpu temp */
|
||||
int center_temp; /* center temp */
|
||||
int gpu_temp; /* gpu temp */
|
||||
int npu_temp; /* npu temp */
|
||||
|
||||
} lidar_soc_thermal_t;
|
||||
typedef struct
|
||||
{
|
||||
double uptime_seconds;
|
||||
lidar_soc_thermal_t soc_thermal;
|
||||
|
||||
int cpu_use_rate[8]; /* cpu usage rate */
|
||||
int ram_use_rate; /* ram usage rate */
|
||||
|
||||
lidar_rgb_sensor_status_t rgb_sensor;
|
||||
lidar_dtof_sensor_status_t dtof_sensor;
|
||||
lidar_imu_sensor_status_t imu_sensor;
|
||||
|
||||
int slam_cloud_tx_odr; /* slam cloud tx output data rate */
|
||||
int slam_odom_tx_odr; /* slam odom tx output data rate */
|
||||
int slam_odom_highfreq_tx_odr; /* slam odom high freq tx output data rate */
|
||||
|
||||
} lidar_device_status_t;
|
||||
|
||||
typedef enum {
|
||||
LIDAR_DEVICE_NONE = 0,
|
||||
LIDAR_DEVICE_NOT_INITIALIZED,
|
||||
LIDAR_DEVICE_INITIALIZED,
|
||||
LIDAR_DEVICE_STREAMING,
|
||||
LIDAR_DEVICE_STREAM_STOPPED,
|
||||
} lidar_device_initial_state_e;
|
||||
|
||||
typedef enum {
|
||||
LIDAR_DEPTH_ODR_10HZ = 0,
|
||||
LIDAR_DEPTH_ODR_14_5HZ,
|
||||
} lidar_depth_odr_e;
|
||||
|
||||
typedef struct {
|
||||
lidar_depth_odr_e odr;
|
||||
} lidar_depth_para_t;
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,86 @@
|
||||
#ifndef ODIN1_IMU_BRIDGE_H
|
||||
#define ODIN1_IMU_BRIDGE_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: odin1_imu_sample_t
|
||||
* 作用: 描述一帧 IMU 数据, 供 C/C++/Python 共享使用
|
||||
*/
|
||||
typedef struct odin1_imu_sample_t {
|
||||
float accel_x;
|
||||
float accel_y;
|
||||
float accel_z;
|
||||
float gyro_x;
|
||||
float gyro_y;
|
||||
float gyro_z;
|
||||
uint64_t stamp_ns;
|
||||
uint64_t sequence;
|
||||
} odin1_imu_sample_t;
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: const char*
|
||||
* 作用: 返回当前 bridge 的版本字符串
|
||||
*/
|
||||
const char* odin1_imu_version(void);
|
||||
|
||||
/**
|
||||
* 输入: timeout_ms[int]
|
||||
* 输出: int, 0 表示成功, 非 0 表示失败
|
||||
* 作用: 初始化 SDK, 等待设备连接并开始 IMU 数据流
|
||||
*/
|
||||
int odin1_imu_start(int timeout_ms);
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: 无
|
||||
* 作用: 停止数据流并释放 SDK 资源
|
||||
*/
|
||||
void odin1_imu_stop(void);
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: int, 1 表示运行中, 0 表示未运行
|
||||
* 作用: 返回当前 bridge 是否处于运行状态
|
||||
*/
|
||||
int odin1_imu_is_running(void);
|
||||
|
||||
/**
|
||||
* 输入: timeout_ms[int]
|
||||
* 输出: int, 1 表示有数据可读, 0 表示超时, 负数表示异常
|
||||
* 作用: 阻塞等待 IMU 数据到达
|
||||
*/
|
||||
int odin1_imu_wait_for_data(int timeout_ms);
|
||||
|
||||
/**
|
||||
* 输入: out_sample[odin1_imu_sample_t*]
|
||||
* 输出: int, 1 表示成功取出一帧, 0 表示队列为空, 负数表示异常
|
||||
* 作用: 从内部队列中弹出一帧 IMU 数据
|
||||
*/
|
||||
int odin1_imu_pop_sample(odin1_imu_sample_t* out_sample);
|
||||
|
||||
/**
|
||||
* 输入: out_sample[odin1_imu_sample_t*]
|
||||
* 输出: int, 1 表示成功读取, 0 表示当前还没有数据, 负数表示异常
|
||||
* 作用: 获取最近一帧 IMU 数据, 不会从队列中删除
|
||||
*/
|
||||
int odin1_imu_get_latest(odin1_imu_sample_t* out_sample);
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: const char*
|
||||
* 作用: 返回最近一次错误信息
|
||||
*/
|
||||
const char* odin1_imu_last_error(void);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
Binary file not shown.
@@ -0,0 +1,124 @@
|
||||
#!/usr/bin/python3
|
||||
"""ODIN1 IMU ctypes 封装."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import ctypes
|
||||
from pathlib import Path
|
||||
from typing import Iterator, Optional
|
||||
|
||||
|
||||
class Odin1ImuSample(ctypes.Structure):
|
||||
"""输入: 无; 输出: Odin1ImuSample; 作用: 映射 C++ bridge 的 IMU 结构体."""
|
||||
|
||||
_fields_ = [
|
||||
("accel_x", ctypes.c_float),
|
||||
("accel_y", ctypes.c_float),
|
||||
("accel_z", ctypes.c_float),
|
||||
("gyro_x", ctypes.c_float),
|
||||
("gyro_y", ctypes.c_float),
|
||||
("gyro_z", ctypes.c_float),
|
||||
("stamp_ns", ctypes.c_uint64),
|
||||
("sequence", ctypes.c_uint64),
|
||||
]
|
||||
|
||||
|
||||
class Odin1ImuClient:
|
||||
"""输入: lib_path[Optional[str|Path]]; 输出: Odin1ImuClient; 作用: 提供 Python 对 ODIN1 IMU bridge 的访问接口."""
|
||||
|
||||
def __init__(self, lib_path: Optional[str | Path] = None) -> None:
|
||||
self._project_root = Path(__file__).resolve().parents[1]
|
||||
resolved_path = Path(lib_path) if lib_path else self._project_root / "build" / "libodin1_imu_bridge.so"
|
||||
self._lib = ctypes.CDLL(str(resolved_path))
|
||||
self._configure_signatures()
|
||||
|
||||
def _configure_signatures(self) -> None:
|
||||
"""输入: 无; 输出: 无; 作用: 配置 ctypes 函数签名."""
|
||||
|
||||
self._lib.odin1_imu_version.restype = ctypes.c_char_p
|
||||
|
||||
self._lib.odin1_imu_start.argtypes = [ctypes.c_int]
|
||||
self._lib.odin1_imu_start.restype = ctypes.c_int
|
||||
|
||||
self._lib.odin1_imu_stop.argtypes = []
|
||||
self._lib.odin1_imu_stop.restype = None
|
||||
|
||||
self._lib.odin1_imu_is_running.argtypes = []
|
||||
self._lib.odin1_imu_is_running.restype = ctypes.c_int
|
||||
|
||||
self._lib.odin1_imu_wait_for_data.argtypes = [ctypes.c_int]
|
||||
self._lib.odin1_imu_wait_for_data.restype = ctypes.c_int
|
||||
|
||||
self._lib.odin1_imu_pop_sample.argtypes = [ctypes.POINTER(Odin1ImuSample)]
|
||||
self._lib.odin1_imu_pop_sample.restype = ctypes.c_int
|
||||
|
||||
self._lib.odin1_imu_get_latest.argtypes = [ctypes.POINTER(Odin1ImuSample)]
|
||||
self._lib.odin1_imu_get_latest.restype = ctypes.c_int
|
||||
|
||||
self._lib.odin1_imu_last_error.argtypes = []
|
||||
self._lib.odin1_imu_last_error.restype = ctypes.c_char_p
|
||||
|
||||
def version(self) -> str:
|
||||
"""输入: 无; 输出: str; 作用: 获取 C++ bridge 版本号."""
|
||||
|
||||
return self._lib.odin1_imu_version().decode("utf-8")
|
||||
|
||||
def last_error(self) -> str:
|
||||
"""输入: 无; 输出: str; 作用: 获取最近一次 bridge 错误信息."""
|
||||
|
||||
return self._lib.odin1_imu_last_error().decode("utf-8")
|
||||
|
||||
def start(self, timeout_ms: int = 5000) -> None:
|
||||
"""输入: timeout_ms[int]; 输出: 无; 作用: 启动 IMU 数据接收."""
|
||||
|
||||
result = self._lib.odin1_imu_start(timeout_ms)
|
||||
if result != 0:
|
||||
raise RuntimeError(f"启动 ODIN1 IMU 失败: {self.last_error()} (code={result})")
|
||||
|
||||
def stop(self) -> None:
|
||||
"""输入: 无; 输出: 无; 作用: 停止 IMU 数据接收."""
|
||||
|
||||
self._lib.odin1_imu_stop()
|
||||
|
||||
def is_running(self) -> bool:
|
||||
"""输入: 无; 输出: bool; 作用: 返回 bridge 是否仍在运行."""
|
||||
|
||||
return bool(self._lib.odin1_imu_is_running())
|
||||
|
||||
def wait_for_data(self, timeout_ms: int = 1000) -> bool:
|
||||
"""输入: timeout_ms[int]; 输出: bool; 作用: 等待 IMU 数据到达."""
|
||||
|
||||
result = self._lib.odin1_imu_wait_for_data(timeout_ms)
|
||||
if result < 0:
|
||||
raise RuntimeError(f"等待 IMU 数据失败: {self.last_error()} (code={result})")
|
||||
return bool(result)
|
||||
|
||||
def pop_sample(self) -> Optional[Odin1ImuSample]:
|
||||
"""输入: 无; 输出: Optional[Odin1ImuSample]; 作用: 从队列中取出一帧 IMU 数据."""
|
||||
|
||||
sample = Odin1ImuSample()
|
||||
result = self._lib.odin1_imu_pop_sample(ctypes.byref(sample))
|
||||
if result < 0:
|
||||
raise RuntimeError(f"读取 IMU 队列失败: {self.last_error()} (code={result})")
|
||||
return sample if result == 1 else None
|
||||
|
||||
def get_latest(self) -> Optional[Odin1ImuSample]:
|
||||
"""输入: 无; 输出: Optional[Odin1ImuSample]; 作用: 获取最近一帧 IMU 数据."""
|
||||
|
||||
sample = Odin1ImuSample()
|
||||
result = self._lib.odin1_imu_get_latest(ctypes.byref(sample))
|
||||
if result < 0:
|
||||
raise RuntimeError(f"读取最新 IMU 数据失败: {self.last_error()} (code={result})")
|
||||
return sample if result == 1 else None
|
||||
|
||||
def iter_samples(self, timeout_ms: int = 1000) -> Iterator[Odin1ImuSample]:
|
||||
"""输入: timeout_ms[int]; 输出: Iterator[Odin1ImuSample]; 作用: 连续迭代输出 IMU 数据."""
|
||||
|
||||
while self.is_running():
|
||||
if not self.wait_for_data(timeout_ms):
|
||||
continue
|
||||
while True:
|
||||
sample = self.pop_sample()
|
||||
if sample is None:
|
||||
break
|
||||
yield sample
|
||||
@@ -0,0 +1,376 @@
|
||||
#include "odin1_imu_bridge.h"
|
||||
|
||||
#include "lidar_api.h"
|
||||
#include "lidar_api_type.h"
|
||||
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <condition_variable>
|
||||
#include <cstring>
|
||||
#include <deque>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
|
||||
namespace {
|
||||
|
||||
constexpr const char* kBridgeVersion = "0.1.0";
|
||||
constexpr std::size_t kMaxQueueSize = 1024;
|
||||
constexpr int kDefaultMode = LIDAR_MODE_SLAM;
|
||||
|
||||
std::atomic<bool> g_running{false};
|
||||
std::atomic<bool> g_sdk_initialized{false};
|
||||
std::atomic<bool> g_device_connected{false};
|
||||
std::atomic<bool> g_stream_started{false};
|
||||
|
||||
device_handle g_device = nullptr;
|
||||
|
||||
std::mutex g_state_mutex;
|
||||
std::mutex g_queue_mutex;
|
||||
std::condition_variable g_queue_cv;
|
||||
std::deque<odin1_imu_sample_t> g_queue;
|
||||
odin1_imu_sample_t g_latest_sample{};
|
||||
bool g_has_latest_sample = false;
|
||||
|
||||
std::mutex g_error_mutex;
|
||||
std::string g_last_error = "bridge not started";
|
||||
|
||||
/**
|
||||
* 输入: message[const std::string&]
|
||||
* 输出: 无
|
||||
* 作用: 线程安全地记录最近一次错误信息
|
||||
*/
|
||||
void set_last_error(const std::string& message) {
|
||||
std::lock_guard<std::mutex> lock(g_error_mutex);
|
||||
g_last_error = message;
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: 无
|
||||
* 作用: 清空内部 IMU 队列和最近一帧缓存
|
||||
*/
|
||||
void clear_queue_locked_state() {
|
||||
std::lock_guard<std::mutex> lock(g_queue_mutex);
|
||||
g_queue.clear();
|
||||
g_latest_sample = {};
|
||||
g_has_latest_sample = false;
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: raw_sample[const imu_convert_data_t*]
|
||||
* 输出: odin1_imu_sample_t
|
||||
* 作用: 将 SDK IMU 结构转换为 bridge 对外结构
|
||||
*/
|
||||
odin1_imu_sample_t convert_sample(const imu_convert_data_t* raw_sample) {
|
||||
odin1_imu_sample_t converted{};
|
||||
if (raw_sample == nullptr) {
|
||||
return converted;
|
||||
}
|
||||
|
||||
converted.accel_x = raw_sample->accel_x;
|
||||
converted.accel_y = raw_sample->accel_y;
|
||||
converted.accel_z = raw_sample->accel_z;
|
||||
converted.gyro_x = raw_sample->gyro_x;
|
||||
converted.gyro_y = raw_sample->gyro_y;
|
||||
converted.gyro_z = raw_sample->gyro_z;
|
||||
converted.stamp_ns = raw_sample->stamp;
|
||||
converted.sequence = raw_sample->sequence;
|
||||
return converted;
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: 无
|
||||
* 作用: 安全关闭当前设备与 SDK 资源
|
||||
*/
|
||||
void cleanup_device_and_sdk() {
|
||||
std::lock_guard<std::mutex> lock(g_state_mutex);
|
||||
|
||||
if (g_device != nullptr) {
|
||||
try {
|
||||
if (g_stream_started.load()) {
|
||||
lidar_deactivate_stream_type(g_device, LIDAR_DT_RAW_IMU);
|
||||
lidar_stop_stream(g_device, kDefaultMode);
|
||||
g_stream_started = false;
|
||||
}
|
||||
|
||||
lidar_unregister_stream_callback(g_device);
|
||||
lidar_close_device(g_device);
|
||||
lidar_destory_device(g_device);
|
||||
} catch (...) {
|
||||
}
|
||||
g_device = nullptr;
|
||||
}
|
||||
|
||||
if (g_sdk_initialized.load()) {
|
||||
try {
|
||||
lidar_system_deinit();
|
||||
} catch (...) {
|
||||
}
|
||||
g_sdk_initialized = false;
|
||||
}
|
||||
|
||||
g_device_connected = false;
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: data[const lidar_data_t*], user_data[void*]
|
||||
* 输出: 无
|
||||
* 作用: 接收 SDK 回调中的 IMU 数据并写入内部缓存队列
|
||||
*/
|
||||
void lidar_data_callback(const lidar_data_t* data, void* user_data) {
|
||||
(void)user_data;
|
||||
|
||||
if (!g_running.load() || data == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (data->type != LIDAR_DT_RAW_IMU) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (data->stream.imageList[0].pAddr == nullptr) {
|
||||
set_last_error("sdk imu callback returned null payload");
|
||||
return;
|
||||
}
|
||||
|
||||
const auto* raw_sample =
|
||||
static_cast<const imu_convert_data_t*>(data->stream.imageList[0].pAddr);
|
||||
odin1_imu_sample_t sample = convert_sample(raw_sample);
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(g_queue_mutex);
|
||||
if (g_queue.size() >= kMaxQueueSize) {
|
||||
g_queue.pop_front();
|
||||
}
|
||||
g_queue.push_back(sample);
|
||||
g_latest_sample = sample;
|
||||
g_has_latest_sample = true;
|
||||
}
|
||||
|
||||
g_queue_cv.notify_all();
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: device_info[const lidar_device_info_t*], attach[bool]
|
||||
* 输出: 无
|
||||
* 作用: 响应 SDK 设备插拔事件并启动 IMU 数据流
|
||||
*/
|
||||
void lidar_device_callback(const lidar_device_info_t* device_info, bool attach) {
|
||||
if (!g_running.load()) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (!attach) {
|
||||
g_device_connected = false;
|
||||
g_stream_started = false;
|
||||
return;
|
||||
}
|
||||
|
||||
if (device_info == nullptr) {
|
||||
set_last_error("sdk device callback returned null device info");
|
||||
return;
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(g_state_mutex);
|
||||
|
||||
if (g_device != nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
device_handle device_handle_local = nullptr;
|
||||
if (lidar_create_device(const_cast<lidar_device_info_t*>(device_info), &device_handle_local) != 0) { // SDK接口，来源: include/lidar_api.h
|
||||
set_last_error("lidar_create_device failed");
|
||||
return;
|
||||
}
|
||||
|
||||
if (lidar_open_device(device_handle_local) != 0) { // SDK接口，来源: include/lidar_api.h
|
||||
set_last_error("lidar_open_device failed");
|
||||
lidar_destory_device(device_handle_local); // SDK接口，来源: include/lidar_api.h
|
||||
return;
|
||||
}
|
||||
|
||||
lidar_data_callback_info_t callback_info{};
|
||||
callback_info.data_callback = lidar_data_callback;
|
||||
callback_info.user_data = nullptr;
|
||||
if (lidar_register_stream_callback(device_handle_local, callback_info) != 0) { // SDK接口，来源: include/lidar_api.h
|
||||
set_last_error("lidar_register_stream_callback failed");
|
||||
lidar_close_device(device_handle_local); // SDK接口，来源: include/lidar_api.h
|
||||
lidar_destory_device(device_handle_local); // SDK接口，来源: include/lidar_api.h
|
||||
return;
|
||||
}
|
||||
|
||||
uint32_t dtof_subframe_odr = 0;
|
||||
if (lidar_start_stream(device_handle_local, kDefaultMode, dtof_subframe_odr) != 0) { // SDK接口，来源: include/lidar_api.h
|
||||
(void)dtof_subframe_odr;
|
||||
set_last_error("lidar_start_stream failed");
|
||||
lidar_unregister_stream_callback(device_handle_local); // SDK接口，来源: include/lidar_api.h
|
||||
lidar_close_device(device_handle_local); // SDK接口，来源: include/lidar_api.h
|
||||
lidar_destory_device(device_handle_local); // SDK接口，来源: include/lidar_api.h
|
||||
return;
|
||||
}
|
||||
|
||||
if (lidar_activate_stream_type(device_handle_local, LIDAR_DT_RAW_IMU) != 0) { // SDK接口，来源: include/lidar_api.h
|
||||
set_last_error("lidar_activate_stream_type(raw_imu) failed");
|
||||
lidar_stop_stream(device_handle_local, kDefaultMode); // SDK接口，来源: include/lidar_api.h
|
||||
lidar_unregister_stream_callback(device_handle_local); // SDK接口，来源: include/lidar_api.h
|
||||
lidar_close_device(device_handle_local); // SDK接口，来源: include/lidar_api.h
|
||||
lidar_destory_device(device_handle_local); // SDK接口，来源: include/lidar_api.h
|
||||
return;
|
||||
}
|
||||
|
||||
g_device = device_handle_local;
|
||||
g_stream_started = true;
|
||||
g_device_connected = true;
|
||||
set_last_error("");
|
||||
g_queue_cv.notify_all();
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
extern "C" {
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: const char*
|
||||
* 作用: 返回当前 bridge 的版本字符串
|
||||
*/
|
||||
const char* odin1_imu_version(void) {
|
||||
return kBridgeVersion;
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: timeout_ms[int]
|
||||
* 输出: int, 0 表示成功, 非 0 表示失败
|
||||
* 作用: 初始化 SDK, 等待设备连接并开始 IMU 数据流
|
||||
*/
|
||||
int odin1_imu_start(int timeout_ms) {
|
||||
if (timeout_ms <= 0) {
|
||||
timeout_ms = 5000;
|
||||
}
|
||||
|
||||
if (g_running.load()) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
clear_queue_locked_state();
|
||||
set_last_error("waiting for odin1 device");
|
||||
|
||||
if (lidar_system_init(lidar_device_callback) != 0) { // SDK接口，来源: include/lidar_api.h
|
||||
set_last_error("lidar_system_init failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
g_sdk_initialized = true;
|
||||
g_running = true;
|
||||
|
||||
const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(timeout_ms);
|
||||
while (std::chrono::steady_clock::now() < deadline) {
|
||||
if (g_device_connected.load()) {
|
||||
return 0;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
||||
}
|
||||
|
||||
set_last_error("timeout waiting for odin1 imu stream");
|
||||
odin1_imu_stop();
|
||||
return -2;
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: 无
|
||||
* 作用: 停止数据流并释放 SDK 资源
|
||||
*/
|
||||
void odin1_imu_stop(void) {
|
||||
g_running = false;
|
||||
cleanup_device_and_sdk();
|
||||
clear_queue_locked_state();
|
||||
g_queue_cv.notify_all();
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: int, 1 表示运行中, 0 表示未运行
|
||||
* 作用: 返回当前 bridge 是否处于运行状态
|
||||
*/
|
||||
int odin1_imu_is_running(void) {
|
||||
return g_running.load() ? 1 : 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: timeout_ms[int]
|
||||
* 输出: int, 1 表示有数据可读, 0 表示超时, 负数表示异常
|
||||
* 作用: 阻塞等待 IMU 数据到达
|
||||
*/
|
||||
int odin1_imu_wait_for_data(int timeout_ms) {
|
||||
if (!g_running.load()) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
std::unique_lock<std::mutex> lock(g_queue_mutex);
|
||||
const bool ready = g_queue_cv.wait_for(
|
||||
lock,
|
||||
std::chrono::milliseconds(timeout_ms > 0 ? timeout_ms : 1000),
|
||||
[] { return !g_queue.empty() || !g_running.load(); });
|
||||
|
||||
if (!g_running.load()) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
return ready && !g_queue.empty() ? 1 : 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: out_sample[odin1_imu_sample_t*]
|
||||
* 输出: int, 1 表示成功取出一帧, 0 表示队列为空, 负数表示异常
|
||||
* 作用: 从内部队列中弹出一帧 IMU 数据
|
||||
*/
|
||||
int odin1_imu_pop_sample(odin1_imu_sample_t* out_sample) {
|
||||
if (out_sample == nullptr) {
|
||||
set_last_error("odin1_imu_pop_sample received null output pointer");
|
||||
return -1;
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(g_queue_mutex);
|
||||
if (g_queue.empty()) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
*out_sample = g_queue.front();
|
||||
g_queue.pop_front();
|
||||
return 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: out_sample[odin1_imu_sample_t*]
|
||||
* 输出: int, 1 表示成功读取, 0 表示当前还没有数据, 负数表示异常
|
||||
* 作用: 获取最近一帧 IMU 数据, 不会从队列中删除
|
||||
*/
|
||||
int odin1_imu_get_latest(odin1_imu_sample_t* out_sample) {
|
||||
if (out_sample == nullptr) {
|
||||
set_last_error("odin1_imu_get_latest received null output pointer");
|
||||
return -1;
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(g_queue_mutex);
|
||||
if (!g_has_latest_sample) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
*out_sample = g_latest_sample;
|
||||
return 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: const char*
|
||||
* 作用: 返回最近一次错误信息
|
||||
*/
|
||||
const char* odin1_imu_last_error(void) {
|
||||
std::lock_guard<std::mutex> lock(g_error_mutex);
|
||||
return g_last_error.c_str();
|
||||
}
|
||||
|
||||
} // extern "C"
|
||||
Reference in New Issue
Block a user