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

<add> 1.Added point cloud confidence and optimized point cloud processing

Browse Source 
2.Modified USB handling logic and added a set of new error messages
This commit is contained in:
oliveiratang
2025-08-08 21:45:56 +08:00
parent eb750f9fe0
commit 24accb60ec
9 changed files with 300 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/host_sdk_sample.cpp
+180 -50
View File
@@ -17,7 +17,10 @@
#include <sys/wait.h>
#include <signal.h>
#include <chrono>
#include <fstream>
#include <vector>
#include <cstdio>
#include <array>
#ifdef ROS2
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <rclcpp/rclcpp.hpp>
@@ -25,13 +28,14 @@
#include <ros/package.h>
#include <ros/ros.h>
#endif
#define ros_driver_version "0.3.0"
// Global variable declarations
static device_handle odinDevice = nullptr;
static std::atomic<bool> deviceConnected(false);
static std::atomic<bool> deviceDisconnected(false); // Device disconnection flag
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);
#ifdef ROS2
std::shared_ptr<MultiSensorPublisher> g_ros_object = nullptr;
#else
@@ -49,6 +53,9 @@ static capture_Image_List_t g_latest_rgb;
static bool g_renderer_initialized = false;
static std::shared_ptr<rawCloudRender> g_renderer = nullptr;
// usb device
static std::string TARGET_VENDOR = "2207";
static std::string TARGET_PRODUCT = "0019";
// Global configuration variables
int g_sendrgb = 1;
int g_sendimu = 1;
@@ -58,9 +65,89 @@ int g_sendcloudslam = 0;
int g_sendcloudrender = 0;
int g_sendrgb_compressed = 0;
// Function declarations
void clear_all_queues();
// detect USB3.0
bool isUsb3OrHigher(const std::string& vendorId, const std::string& productId) {
std::string command = "lsusb -d " + vendorId + ":" + productId + " -v | grep 'bcdUSB'";
std::array<char, 128> buffer;
std::string result;
std::unique_ptr<FILE, decltype(&pclose)> pipe(popen(command.c_str(), "r"), pclose);
if (!pipe) {
throw std::runtime_error("popen() failed!");
}
while (fgets(buffer.data(), buffer.size(), pipe.get()) != nullptr) {
result += buffer.data();
}
if (result.empty()) {
#ifdef ROS2
RCLCPP_ERROR(rclcpp::get_logger("usb_check"), "Failed to get USB version information");
#else
ROS_ERROR("Failed to get USB version information");
#endif
return false;
}
// find bcdUSB
size_t pos = result.find("bcdUSB");
if (pos == std::string::npos) {
#ifdef ROS2
RCLCPP_ERROR(rclcpp::get_logger("usb_check"), "bcdUSB field not found in lsusb output");
#else
ROS_ERROR("bcdUSB field not found in lsusb output");
#endif
return false;
}
std::string versionStr = result.substr(pos + 7); // "bcdUSB" + space
float version = std::stof(versionStr);
#ifdef ROS2
RCLCPP_INFO(rclcpp::get_logger("usb_check"), "Detected USB version: %.1f", version);
#else
ROS_INFO("Detected USB version: %.1f", version);
#endif
return version >= 3.0;
}
bool isUsbDevicePresent(const std::string& vendorId, const std::string& productId) {
std::ifstream devicesList("/sys/bus/usb/devices");
if (devicesList.is_open()) {
std::string line;
while (std::getline(devicesList, line)) {
if (line.find('.') != std::string::npos) continue;
if (line.empty()) continue;
std::string vendorPath = "/sys/bus/usb/devices/" + line + "/idVendor";
std::ifstream vendorFile(vendorPath);
if (vendorFile.is_open()) {
std::string vendorContent;
if (std::getline(vendorFile, vendorContent)) {
vendorContent.erase(vendorContent.find_last_not_of(" \n\r\t") + 1);
std::string productPath = "/sys/bus/usb/devices/" + line + "/idProduct";
std::ifstream productFile(productPath);
if (productFile.is_open()) {
std::string productContent;
if (std::getline(productFile, productContent)) {
productContent.erase(productContent.find_last_not_of(" \n\r\t") + 1);
if (vendorContent == vendorId && productContent == productId) {
return true;
}
}
productFile.close();
}
}
vendorFile.close();
}
}
devicesList.close();
}
return false;
}
// Get package share path
std::string get_package_share_path(const std::string& package_name) {
#ifdef ROS2
@@ -144,24 +231,39 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
}
}
// Lidar device callback
static void lidar_device_callback(const lidar_device_info_t* device, bool attach)
{
int type = LIDAR_MODE_SLAM;
static std::chrono::steady_clock::time_point software_connect_start;
static bool software_connect_timing = false;
if(attach == true) {
#ifdef ROS2
RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Device attaching...");
RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Hardware connected, starting software connection...");
#else
ROS_INFO("Device attaching...");
ROS_INFO("Hardware connected, starting software connection...");
#endif
if (!isUsb3OrHigher(TARGET_VENDOR, TARGET_PRODUCT)) {
#ifdef ROS2
RCLCPP_FATAL(rclcpp::get_logger("device_cb"),
"Device connected to USB 2.0 port. This device requires USB 3.0 or higher. Exiting program.");
#else
ROS_FATAL("Device connected to USB 2.0 port. This device requires USB 3.0 or higher. Exiting program.");
#endif
g_usb_version_error = true;
system("pkill -f rviz");
exit(1);
return;
}
software_connect_start = std::chrono::steady_clock::now();
software_connect_timing = true;
// Clean up existing device resources
if (odinDevice) {
// Skip stopping data stream, unregistering callbacks, closing device, destroying device
odinDevice = nullptr;
}
// Create new device
if (lidar_create_device(const_cast<lidar_device_info_t*>(device), &odinDevice)) {
#ifdef ROS2
RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Create device failed");
@@ -171,7 +273,6 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
return;
}
// Open device
if (lidar_open_device(odinDevice)) {
#ifdef ROS2
RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Open device failed");
@@ -183,39 +284,58 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
return;
}
// Get package path
const std::string package_name = "odin_ros_driver";
std::string config_dir = "";
#ifdef ROS2
// Get source code directory (not install directory)
char* ros_workspace = std::getenv("COLCON_PREFIX_PATH");
if (ros_workspace) {
// Infer source directory from COLCON_PREFIX_PATH
std::string workspace_path(ros_workspace);
// Remove "/install" part
size_t pos = workspace_path.find("/install");
if (pos != std::string::npos) {
config_dir = workspace_path.substr(0, pos) + "/src/odin_ros_driver/config";
} else {
// Fallback to install directory
config_dir = ament_index_cpp::get_package_share_directory(package_name) + "/config";
}
} else {
// Fallback to install directory
config_dir = ament_index_cpp::get_package_share_directory(package_name) + "/config";
}
#else
config_dir = ros::package::getPath(package_name) + "/config";
#endif
// Print path information
#ifdef ROS2
RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Calibration files will be saved to: %s", config_dir.c_str());
#else
ROS_INFO("Calibration files will be saved to: %s", config_dir.c_str());
#endif
// Get calibration files - using modified function
auto now = std::chrono::steady_clock::now();
auto elapsed = std::chrono::duration_cast<std::chrono::seconds>(now - software_connect_start);
if (elapsed.count() >= 60) {
#ifdef ROS2
RCLCPP_FATAL(rclcpp::get_logger("device_cb"),
"Software connection timed out after 60 seconds. Exiting program.");
#else
ROS_FATAL("Software connection timed out after 60 seconds. Exiting program.");
#endif
if (odinDevice) {
lidar_close_device(odinDevice);
lidar_destory_device(odinDevice);
odinDevice = nullptr;
}
g_connection_timeout = true;
return;
}
if(lidar_get_version(odinDevice)) {
printf("get version failed.\n");
}
else {
printf("get version success.\n");
}
if (lidar_get_calib_file(odinDevice, config_dir.c_str())) {
#ifdef ROS2
RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Failed to get calibration file");
@@ -234,7 +354,6 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
ROS_INFO("Successfully retrieved calibration files");
#endif
// Move point cloud renderer initialization here
std::string calib_config = config_dir + "/calib.yaml";
if (std::filesystem::exists(calib_config)) {
g_renderer = std::make_shared<rawCloudRender>();
@@ -271,7 +390,6 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
return;
}
// Register callback
lidar_data_callback_info_t data_callback_info;
data_callback_info.data_callback = lidar_data_callback;
data_callback_info.user_data = &odinDevice;
@@ -288,7 +406,6 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
return;
}
// Start data stream
if (lidar_start_stream(odinDevice, type)) {
#ifdef ROS2
RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Start stream failed");
@@ -301,7 +418,6 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
return;
}
// Activate stream types based on configuration
if (g_sendrgb) {
lidar_activate_stream_type(odinDevice, LIDAR_DT_RAW_RGB);
}
@@ -318,11 +434,17 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
lidar_activate_stream_type(odinDevice, LIDAR_DT_SLAM_CLOUD);
}
software_connect_timing = false;
deviceConnected = true;
deviceDisconnected = false; // Reset disconnection flag
deviceDisconnected = false;
#ifdef ROS2
RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Software connection successful in %ld seconds",
std::chrono::duration_cast<std::chrono::seconds>(std::chrono::steady_clock::now() - software_connect_start).count());
RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Device ready and streams activated");
#else
ROS_INFO("Software connection successful in %ld seconds",
std::chrono::duration_cast<std::chrono::seconds>(std::chrono::steady_clock::now() - software_connect_start).count());
ROS_INFO("Device ready and streams activated");
#endif
} else {
@@ -332,11 +454,9 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
ROS_INFO("Device detaching...");
#endif
// Set device disconnection flag
deviceConnected = false;
deviceDisconnected = true;
// Clear all message queues
clear_all_queues();
#ifdef ROS2
@@ -346,7 +466,6 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
#endif
}
}
int main(int argc, char *argv[])
{
#ifdef ROS2
@@ -362,9 +481,7 @@ int main(int argc, char *argv[])
try {
std::string package_path = get_package_share_path("odin_ros_driver");
std::string config_file = package_path + "/config/control_command.yaml";
odin_ros_driver::YamlParser parser(config_file);
if (!parser.loadConfig()) {
#ifdef ROS2
@@ -394,30 +511,43 @@ int main(int argc, char *argv[])
lidar_log_set_level(LIDAR_LOG_INFO);
if (lidar_system_init(lidar_device_callback)) {
#ifdef ROS2
RCLCPP_ERROR(node->get_logger(), "Lidar system init failed");
#else
ROS_ERROR("Lidar system init failed");
#endif
return -1;
}
#ifdef ROS2
RCLCPP_INFO(node->get_logger(), "Waiting for device connection...");
#else
ROS_INFO("Waiting for device connection...");
#endif
// Wait indefinitely for device connection
while (!deviceConnected) {
#ifdef ROS2
RCLCPP_INFO(node->get_logger(), "Waiting for device connection...");
RCLCPP_ERROR(node->get_logger(), "Lidar system init failed");
#else
ROS_INFO("Waiting for device connection...");
ROS_ERROR("Lidar system init failed");
#endif
std::this_thread::sleep_for(std::chrono::seconds(1)); // Check every second
return -1;
}
bool usbPresent = false;
bool usbVersionChecked = false;
while (!deviceConnected) {
usbPresent = isUsbDevicePresent(TARGET_VENDOR, TARGET_PRODUCT);
if (usbPresent) {
if (!usbVersionChecked) {
usbVersionChecked = true;
if (!isUsb3OrHigher(TARGET_VENDOR, TARGET_PRODUCT)) {
#ifdef ROS2
RCLCPP_FATAL(node->get_logger(),
"Device connected to USB 2.0 port. This device requires USB 3.0 or higher. Exiting program.Please use USB 3.0 and restart the device.");
#else
ROS_FATAL("Device connected to USB 2.0 port. This device requires USB 3.0 or higher. Exiting program .Please use USB 3.0 and restart the device.");
#endif
lidar_system_deinit();
return 1;
}
}
}
#ifdef ROS2
std::this_thread::sleep_for(std::chrono::seconds(1));
#else
ros::Duration(1.0).sleep();
#endif
}
} catch (const std::exception& e) {
#ifdef ROS2
RCLCPP_ERROR(node->get_logger(), "Exception: %s", e.what());
@@ -493,4 +623,4 @@ int main(int argc, char *argv[])
lidar_system_deinit();
return 0;
}
}