From 40eed662b3f2dcd52bc8626ac01ea4f4fc07ef87 Mon Sep 17 00:00:00 2001 From: QUYVN Date: Fri, 3 Jul 2026 09:38:40 +0700 Subject: [PATCH] lidarlib_ros: ROS2 bridge for liblidarlib (GS1-5 LaserScan) Co-Authored-By: Claude Fable 5 --- .gitignore | 5 + CMakeLists.txt | 36 ++++++ config/lidar.yaml | 61 +++++++++ launch/lidar.launch.py | 113 +++++++++++++++++ package.xml | 27 ++++ rviz/lidar.rviz | 137 ++++++++++++++++++++ src/lidarlib_node.cpp | 276 +++++++++++++++++++++++++++++++++++++++++ 7 files changed, 655 insertions(+) create mode 100644 .gitignore create mode 100644 CMakeLists.txt create mode 100644 config/lidar.yaml create mode 100644 launch/lidar.launch.py create mode 100644 package.xml create mode 100644 rviz/lidar.rviz create mode 100644 src/lidarlib_node.cpp diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..9140035 --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +__pycache__/ +*.pyc +build/ +install/ +log/ diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..68e22f0 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.8) +project(lidarlib_ros) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) + +# Thư viện lidar đã cài ở /usr/local (sudo cmake --install build --prefix /usr/local) +# Nếu cài vào $HOME/.local thì configure với: -DCMAKE_PREFIX_PATH=$HOME/.local +find_package(lidarlib REQUIRED) + +add_executable(lidarlib_node src/lidarlib_node.cpp) +target_link_libraries(lidarlib_node lidarlib::lidarlib) +ament_target_dependencies(lidarlib_node rclcpp sensor_msgs) + +install(TARGETS lidarlib_node + DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY launch config rviz + DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/config/lidar.yaml b/config/lidar.yaml new file mode 100644 index 0000000..3ad091f --- /dev/null +++ b/config/lidar.yaml @@ -0,0 +1,61 @@ +lidarlib_node: + ros__parameters: + # Danh sach lidar chay cung luc. Them/bot ten tuy y; moi ten co 1 block rieng ben duoi. + lidars: ["tim7", "nano", "gs15", "1f"] + + + gs15: + brand: "OLEI" # "OLEI" = UDP | "SICK" = TCP/SOPAS + model: "AUTO" # OLEI: AUTO/VB/VF/LR-1F/LR-1BS5/LR-16F/GS1-5 ; SICK: SICK-TIM5XX/SICK-TIM571/SICK-TIM7XX + ip: "0.0.0.0" # OLEI: dia chi bind cua host (thuong 0.0.0.0) ; SICK: IP cua lidar + port: 2368 # OLEI 2368/2369 ; SICK 2111 + inverted: false # true neu lidar lap up nguoc (OLEI: thu vien xu ly; brand khac: node dao chieu) + topic: "scan_front" # topic publish (mac dinh: scan_) + frame_id: "front" # frame TF (mac dinh: ) + timeout_ms: 1000 + range_min: 0.0 + range_max: 0.0 + angle_min_deg: -360.0 + angle_max_deg: 360.0 + + 1f: + brand: "OLEI" + model: "AUTO" + ip: "0.0.0.0" + port: 2371 + inverted: true + topic: "scan_rear" + frame_id: "rear" + timeout_ms: 1000 + range_min: 0.0 # met; 0.0 = tat (dung gia tri thiet bi bao) + range_max: 0.0 + angle_min_deg: -360.0 # do; +/-360 = tat (giu nguyen goc thiet bi) + angle_max_deg: 360.0 + + tim7: + brand: "SICK" + model: "SICK-TIM5XX" + ip: "192.168.100.22" + port: 2111 + inverted: false # true neu lap up nguoc (node dao chieu scan) + topic: "scan_sick1" + frame_id: "sick1" + timeout_ms: 1000 + range_min: 0.0 # met; 0.0 = tat (dung gia tri thiet bi bao) + range_max: 0.0 + angle_min_deg: -360.0 + angle_max_deg: 360.0 + + nano: + brand: "SICK" + model: "SICK-nanoScan3" + ip: "0.0.0.0" + port: 6061 + inverted: false # true neu lap up nguoc (node dao chieu scan) + topic: "scan_front" + frame_id: "front" + timeout_ms: 1000 + range_min: 0.0 # met; 0.0 = tat (dung gia tri thiet bi bao) + range_max: 0.0 + angle_min_deg: -360.0 # do; +/-360 = tat (giu nguyen goc thiet bi) + angle_max_deg: 360.0 \ No newline at end of file diff --git a/launch/lidar.launch.py b/launch/lidar.launch.py new file mode 100644 index 0000000..775006b --- /dev/null +++ b/launch/lidar.launch.py @@ -0,0 +1,113 @@ +import os + +import yaml +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, LogInfo +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +# Vi tri lap dat tung frame lidar so voi base_link: frame -> (x, y, z, yaw) met/rad. +# Frame khong liet ke o day mac dinh dat tai goc base_link (0,0,0,0) -> van co TF +# nen RViz khong con vut message. Sua toa do thuc te tai day. +MOUNT_POSES = { + 'front': (0.0, 0.0, 0.0, 0.0), + 'rear': (0.0, 0.0, 0.0, 0.0), + 'sick1': (0.0, 0.00, 0.0, 0.0), +} + + +def lidars_from_config(params_file): + """Doc config.yaml -> danh sach dict thong tin moi lidar trong 'lidars'.""" + with open(params_file, 'r') as f: + data = yaml.safe_load(f) + params = data['lidarlib_node']['ros__parameters'] + lidars = [] + for name in params.get('lidars', []): + block = params.get(name, {}) or {} + lidars.append({ + 'name': name, + 'brand': block.get('brand', '?'), + 'model': block.get('model', '?'), + 'ip': block.get('ip', '?'), + 'port': block.get('port', '?'), + 'inverted': block.get('inverted', False), + 'topic': block.get('topic', f'scan_{name}'), + 'frame_id': block.get('frame_id', name), + }) + return lidars + + +def lidar_frames_from_config(params_file): + """Doc config.yaml -> danh sach (frame_id) cua moi lidar trong 'lidars'.""" + return [ld['frame_id'] for ld in lidars_from_config(params_file)] + + +def generate_launch_description(): + pkg = get_package_share_directory('lidarlib_ros') + params_file = os.path.join(pkg, 'config', 'lidar.yaml') + rviz_config = os.path.join(pkg, 'rviz', 'lidar.rviz') + + use_rviz = LaunchConfiguration('rviz') + + lidars = lidars_from_config(params_file) + + # In thong tin cau hinh ra man hinh de tien theo doi/test. + log_nodes = [ + LogInfo(msg='========== LIDAR LAUNCH INFO =========='), + LogInfo(msg=f'Params file : {params_file}'), + LogInfo(msg=f'RViz config : {rviz_config}'), + LogInfo(msg=f'So luong lidar: {len(lidars)}'), + LogInfo(msg='----------------------------------------'), + ] + + # Tu sinh 1 static TF cho MOI frame lidar co trong config -> them lidar la co TF. + tf_nodes = [] + for ld in lidars: + frame = ld['frame_id'] + x, y, z, yaw = MOUNT_POSES.get(frame, (0.0, 0.0, 0.0, 0.0)) + default_pose = frame not in MOUNT_POSES + log_nodes.append(LogInfo( + msg=(f"[{ld['name']}] brand={ld['brand']} model={ld['model']} " + f"ip={ld['ip']}:{ld['port']} inverted={ld['inverted']} " + f"topic={ld['topic']} frame={frame} " + f"TF(x={x} y={y} z={z} yaw={yaw})" + f"{' <-- mac dinh (0,0,0), sua trong MOUNT_POSES' if default_pose else ''}"))) + tf_nodes.append(Node( + package='tf2_ros', + executable='static_transform_publisher', + name=f'base_to_{frame}', + arguments=[str(x), str(y), str(z), str(yaw), '0', '0', 'base_link', frame], + )) + + log_nodes.append(LogInfo(msg='========================================')) + + return LaunchDescription([ + DeclareLaunchArgument( + 'rviz', default_value='true', + description='Mo RViz2 de hien thi LaserScan'), + + *log_nodes, + + # Node cau noi lidarlib -> sensor_msgs/LaserScan (nhieu lidar 1 node) + Node( + package='lidarlib_ros', + executable='lidarlib_node', + name='lidarlib_node', + output='screen', + parameters=[params_file], + ), + + *tf_nodes, + + # RViz2 voi config san + Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config], + condition=IfCondition(use_rviz), + output='screen', + ), + ]) diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..34745db --- /dev/null +++ b/package.xml @@ -0,0 +1,27 @@ + + + + lidarlib_ros + 0.1.0 + + ROS 2 node cầu nối thư viện C++ lidarlib (OLEI/SICK, /usr/local/lib/liblidarlib.so): + đọc scan qua lidarlib::make_lidar() và publish ra sensor_msgs/LaserScan để xem trên RViz. + + QUYVN + MIT + + ament_cmake + + rclcpp + sensor_msgs + + rviz2 + tf2_ros + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/rviz/lidar.rviz b/rviz/lidar.rviz new file mode 100644 index 0000000..b3df37f --- /dev/null +++ b/rviz/lidar.rviz @@ -0,0 +1,137 @@ +Panels: + - Class: rviz_common/Displays + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /front1 + - /rear1 + Splitter Ratio: 0.5 + Tree Height: 617 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Plane: XY + Plane Cell Count: 20 + Reference Frame: + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 0; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Max Intensity: 255 + Min Intensity: 0 + Name: front + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.03 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scan_front + Use Fixed Frame: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Max Intensity: 255 + Min Intensity: 0 + Name: rear + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.03 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scan_rear + Use Fixed Frame: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 0; 170; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Max Intensity: 255 + Min Intensity: 0 + Name: sick1 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.03 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scan_sick1 + Use Fixed Frame: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 20 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 1.4 + Target Frame: + Yaw: 3.14 + Window Geometry: + Displays: + collapsed: false + Height: 846 + Width: 1200 diff --git a/src/lidarlib_node.cpp b/src/lidarlib_node.cpp new file mode 100644 index 0000000..6d22be9 --- /dev/null +++ b/src/lidarlib_node.cpp @@ -0,0 +1,276 @@ +// ─── lidarlib_ros ──────────────────────────────────────────────────────────── +// Cau noi thu vien C++ lidarlib (liblidarlib.so, OLEI/UDP + SICK/TCP) sang ROS 2. +// +// lidarlib::make_lidar(cfg) -> ->open() -> ->recv_scan(r, timeout) +// r.scan (lidarlib::LaserScan) da cung field/don vi voi sensor_msgs/LaserScan +// => node chi copy gan nhu 1:1 roi publish de RViz hien thi. +// +// HO TRO NHIEU LIDAR CUNG LUC: param 'lidars' la danh sach ten; moi ten co bo +// tham so rieng (.ip, .port, ...). Moi lidar chay tren 1 thread rieng +// (giong examples/test_dual.cpp), publish ra topic + frame rieng. + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "lidarlib/lidarlib.hpp" + +// Mot lidar + thread doc + publisher rieng. +struct LidarWorker +{ + std::string name; + std::string frame_id; + int timeout_ms = 1000; + float range_min_override = 0.f; + float range_max_override = 0.f; + bool invert_in_node = false; // dao chieu scan tai node (brand != OLEI) + + std::unique_ptr lidar; + rclcpp::Publisher::SharedPtr pub; + std::thread worker; + std::atomic running{false}; +}; + +// Gom moi field cua ExtraInfo (output #2 tu thu vien) thanh 1 chuoi de in log. +// Field optional khong duoc model cap se hien '-'. error_status giai ma theo bit. +static std::string format_extra_info(const lidarlib::ExtraInfo & e) +{ + std::ostringstream os; + os << "detected_model=" << e.detected_model + << " error=0x" << std::hex << std::uppercase + << static_cast(e.error_status) << std::dec; + if (e.error_status) { + os << '['; + if (e.error_status & 0x01) os << "Monitor "; + if (e.error_status & 0x02) os << "Voltage "; + if (e.error_status & 0x04) os << "Temp "; + os << ']'; + } + os << " dist_scale_mm=" << static_cast(e.distance_scale_mm); + + // In gia tri optional (hoac '-' neu nullopt); '+' de ep uint8_t thanh so. + auto opt = [&os](const char * name, const auto & v) { + os << ' ' << name << '='; + if (v) { os << +(*v); } else { os << '-'; } + }; + opt("rotation_raw", e.rotation_raw); + opt("distance_ratio_raw", e.distance_ratio_raw); + opt("scan_freq_raw", e.scan_frequency_raw); + opt("input_status", e.input_status); + opt("output_status", e.output_status); + opt("field_status", e.field_status); + opt("status_flags", e.status_flags); + return os.str(); +} + +class LidarlibNode : public rclcpp::Node +{ +public: + LidarlibNode() + : rclcpp::Node("lidarlib_node") + { + // QoS reliability cho tat ca topic scan: "reliable" (mac dinh, khop RViz & + // 'ros2 topic echo' mac dinh) hoac "best_effort" (nhe hon, hop cam bien toc do cao). + qos_reliability_ = declare_parameter("qos_reliability", "reliable"); + + // Danh sach ten lidar can chay. Vd: ["front", "rear"] + auto names = declare_parameter>( + "lidars", std::vector{"front"}); + + for (const auto & name : names) { + start_lidar(name); + } + + if (workers_.empty()) { + throw std::runtime_error("Khong mo duoc lidar nao (kiem tra tham so 'lidars' va IP/port)."); + } + RCLCPP_INFO(get_logger(), "Dang chay %zu lidar.", workers_.size()); + } + + ~LidarlibNode() override + { + for (auto & w : workers_) { + w->running.store(false); + } + for (auto & w : workers_) { + if (w->worker.joinable()) { + w->worker.join(); + } + if (w->lidar) { + w->lidar->close(); + } + } + } + +private: + void start_lidar(const std::string & name) + { + const std::string p = name + "."; // tien to tham so cho lidar nay + + lidarlib::LidarConfig cfg; + cfg.name = name; + cfg.ip = declare_parameter(p + "ip", "0.0.0.0"); + cfg.port = static_cast(declare_parameter(p + "port", 2368)); + cfg.brand = declare_parameter(p + "brand", "OLEI"); // "OLEI" (UDP) | "SICK" (TCP) + cfg.model = declare_parameter(p + "model", "AUTO"); + cfg.inverted = declare_parameter(p + "inverted", false); + + // Cua so goc output (do): thu vien remap tuyen tinh goc cua moi scan sang + // [angle_min_deg, angle_max_deg] - khong bo diem nao, chi doi nhan goc + // (angle_min/max/increment) trong LaserScan. Vd TiM -45..225 gan lai thanh + // -135..135 = xoay frame. Mac dinh +/-360 = tat, giu nguyen goc tu thiet bi. + cfg.angle_min_deg = static_cast(declare_parameter(p + "angle_min_deg", -360.0)); + cfg.angle_max_deg = static_cast(declare_parameter(p + "angle_max_deg", 360.0)); + + // 'inverted' cho OLEI do thu vien (liblidarlib) xu ly ben trong. Cac brand + // khac (vd SICK) thu vien bo qua, nen node tu dao chieu scan luc publish. + // => tach rieng de tranh dao 2 lan voi OLEI. + const bool is_olei = (cfg.brand == "OLEI"); + + auto w = std::make_unique(); + w->name = name; + w->frame_id = declare_parameter(p + "frame_id", name); + w->timeout_ms = declare_parameter(p + "timeout_ms", 1000); + w->range_min_override = static_cast(declare_parameter(p + "range_min", 0.0)); + w->range_max_override = static_cast(declare_parameter(p + "range_max", 0.0)); + w->invert_in_node = (cfg.inverted && !is_olei); + + const std::string topic = declare_parameter(p + "topic", "scan_" + name); + + rclcpp::QoS qos(rclcpp::KeepLast(10)); + if (qos_reliability_ == "best_effort") { + qos.best_effort(); + } else { + qos.reliable(); + } + w->pub = create_publisher(topic, qos); + + // Chi in cua so goc khi user thu hep tu mac dinh +/-360 (co remap). + std::string angle_note; + if (cfg.angle_min_deg > -360.f || cfg.angle_max_deg < 360.f) { + std::ostringstream os; + os << " goc[" << cfg.angle_min_deg << ".." << cfg.angle_max_deg << "]deg"; + angle_note = os.str(); + } + + RCLCPP_INFO(get_logger(), + "[%s] brand=%s model=%s %s:%u inverted=%d%s -> topic '%s' frame '%s'", + name.c_str(), cfg.brand.c_str(), cfg.model.c_str(), + cfg.ip.c_str(), cfg.port, cfg.inverted, angle_note.c_str(), + topic.c_str(), w->frame_id.c_str()); + + w->lidar = lidarlib::make_lidar(cfg); // khong bao gio tra ve nullptr + if (!w->lidar->open()) { + RCLCPP_ERROR(get_logger(), + "[%s] Khong mo duoc lidar (%s:%u) - bo qua con nay. " + "Kiem tra IP/port, cap mang, hoac port dang bi tien trinh khac giu.", + name.c_str(), cfg.ip.c_str(), cfg.port); + return; // khong lam sap node; cac lidar khac van chay + } + + w->running.store(true); + LidarWorker * wp = w.get(); + wp->worker = std::thread([this, wp]() { spin_recv(wp); }); + workers_.push_back(std::move(w)); + } + + void spin_recv(LidarWorker * w) + { + lidarlib::ScanResult r; + + // So lan recv that bai lien tiep truoc khi coi la mat ket noi va mo lai. + // Can cho SICK/TCP: khi socket TCP dut, recv_scan timeout mai mai neu khong + // close+open lai. OLEI/UDP thi mo lai cung vo hai (chi rebind socket). + const int reconnect_after = 5; + int consecutive_failures = 0; + + while (w->running.load() && rclcpp::ok()) { + if (!w->lidar->recv_scan(r, w->timeout_ms)) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, + "[%s] Chua nhan duoc scan (timeout %d ms). Cho goi tu lidar...", + w->name.c_str(), w->timeout_ms); + + if (++consecutive_failures >= reconnect_after) { + RCLCPP_WARN(get_logger(), + "[%s] Mat ket noi (%d lan lien tiep). Dang mo lai...", + w->name.c_str(), consecutive_failures); + w->lidar->close(); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); // backoff + if (w->lidar->open()) { + RCLCPP_INFO(get_logger(), "[%s] Da mo lai ket noi.", w->name.c_str()); + consecutive_failures = 0; + } else { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 2000, + "[%s] Mo lai that bai, se thu lai...", w->name.c_str()); + } + } + continue; + } + consecutive_failures = 0; + publish(w, r); + } + } + + void publish(LidarWorker * w, const lidarlib::ScanResult & r) + { + const auto & s = r.scan; + + sensor_msgs::msg::LaserScan msg; + msg.header.stamp = now(); + msg.header.frame_id = w->frame_id; + + msg.angle_min = s.angle_min; + msg.angle_max = s.angle_max; + msg.angle_increment = s.angle_increment; + msg.time_increment = s.time_increment; // thu vien luon = 0 + msg.scan_time = s.scan_time; // thu vien luon = 0 + msg.range_min = (w->range_min_override > 0.f) ? w->range_min_override : s.range_min; + msg.range_max = (w->range_max_override > 0.f) ? w->range_max_override : s.range_max; + + msg.ranges = s.ranges; + msg.intensities = s.intensities; + + // Lidar lap up nguoc (brand != OLEI): dao chieu scan bang cach lat thu tu + // cac diem. Goc angle_min/max/increment giu nguyen -> tuong duong mirror + // quanh truc cam bien, khop voi cach thu vien OLEI xu ly 'inverted'. + if (w->invert_in_node) { + std::reverse(msg.ranges.begin(), msg.ranges.end()); + std::reverse(msg.intensities.begin(), msg.intensities.end()); + } + + w->pub->publish(msg); + + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 2000, + "[%s] scan: %zu diem, ts=%u ms, FOV[%.1f..%.1f]deg | %s", + w->name.c_str(), s.ranges.size(), s.timestamp_ms, + s.angle_min * 180.0f / static_cast(M_PI), + s.angle_max * 180.0f / static_cast(M_PI), + format_extra_info(r.info).c_str()); + } + + std::string qos_reliability_ = "reliable"; + std::vector> workers_; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + try { + rclcpp::spin(std::make_shared()); + } catch (const std::exception & e) { + RCLCPP_ERROR(rclcpp::get_logger("lidarlib_node"), "Thoat: %s", e.what()); + rclcpp::shutdown(); + return 1; + } + rclcpp::shutdown(); + return 0; +}