lidarlib_ros: ROS2 bridge for liblidarlib (GS1-5 LaserScan)

Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>

.gitignore

@@ -0,0 +1,5 @@
+__pycache__/
+*.pyc
+build/
+install/
+log/

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()

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_<ten>)
+      frame_id: "front"    # frame TF (mac dinh: <ten>)
+      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

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',
+        ),
+    ])

package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>lidarlib_ros</name>
+  <version>0.1.0</version>
+  <description>
+    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.
+  </description>
+  <maintainer email="quyvu787899@gmail.com">QUYVN</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+
+  <exec_depend>rviz2</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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: <Fixed 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: <Fixed Frame>
+      Yaw: 3.14
+  Window Geometry:
+    Displays:
+      collapsed: false
+    Height: 846
+    Width: 1200

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 (<ten>.ip, <ten>.port, ...). Moi lidar chay tren 1 thread rieng
+// (giong examples/test_dual.cpp), publish ra topic + frame rieng.
+
+#include <algorithm>
+#include <atomic>
+#include <chrono>
+#include <cstdint>
+#include <memory>
+#include <sstream>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+
+#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<lidarlib::Lidar> lidar;
+  rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub;
+  std::thread       worker;
+  std::atomic<bool> 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<int>(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<int>(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<std::string>("qos_reliability", "reliable");
+
+    // Danh sach ten lidar can chay. Vd: ["front", "rear"]
+    auto names = declare_parameter<std::vector<std::string>>(
+      "lidars", std::vector<std::string>{"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<std::string>(p + "ip", "0.0.0.0");
+    cfg.port     = static_cast<uint16_t>(declare_parameter<int>(p + "port", 2368));
+    cfg.brand    = declare_parameter<std::string>(p + "brand", "OLEI");  // "OLEI" (UDP) | "SICK" (TCP)
+    cfg.model    = declare_parameter<std::string>(p + "model", "AUTO");
+    cfg.inverted = declare_parameter<bool>(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<float>(declare_parameter<double>(p + "angle_min_deg", -360.0));
+    cfg.angle_max_deg = static_cast<float>(declare_parameter<double>(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<LidarWorker>();
+    w->name       = name;
+    w->frame_id   = declare_parameter<std::string>(p + "frame_id", name);
+    w->timeout_ms = declare_parameter<int>(p + "timeout_ms", 1000);
+    w->range_min_override = static_cast<float>(declare_parameter<double>(p + "range_min", 0.0));
+    w->range_max_override = static_cast<float>(declare_parameter<double>(p + "range_max", 0.0));
+    w->invert_in_node = (cfg.inverted && !is_olei);
+
+    const std::string topic = declare_parameter<std::string>(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<sensor_msgs::msg::LaserScan>(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<float>(M_PI),
+      s.angle_max * 180.0f / static_cast<float>(M_PI),
+      format_extra_info(r.info).c_str());
+  }
+
+  std::string qos_reliability_ = "reliable";
+  std::vector<std::unique_ptr<LidarWorker>> workers_;
+};
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  try {
+    rclcpp::spin(std::make_shared<LidarlibNode>());
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR(rclcpp::get_logger("lidarlib_node"), "Thoat: %s", e.what());
+    rclcpp::shutdown();
+    return 1;
+  }
+  rclcpp::shutdown();
+  return 0;
+}