QUYVN/Test_LibLidarROS2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source 
Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
This commit is contained in:
QUYVN
2026-07-03 09:38:40 +07:00
commit 40eed662b3
7 changed files with 655 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

276
src/lidarlib_node.cpp Normal file
View File

@@ -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;
}