277 lines
10 KiB
C++
277 lines
10 KiB
C++
// ─── 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;
|
|
}
|