机器人自动导航ROS开发之-激光雷达驱动----激光雷达-3D视觉-IMU-GPS融合机器人SLAM导航--老毛

基于 ROS 的激光雷达（LiDAR）防区监测节点。它的核心功能是订阅激光雷达的扫描数据，在预设的多个"防区"（扇形区域）内检测障碍物，计算最近距离，并根据滤波规则判断是否触发警报，最后将处理后的距离信息以 sensor_msgs/Range 格式发布。

📂 1. 主程序入口 (Laser_node.cpp)

这个文件非常简洁，负责 ROS 节点的初始化和核心类的实例化。

include <Laser.h> // 包含核心功能的头文件

int main(int argc, char** argv) {

// 1. 初始化 ROS 节点，命名为 "Laser_node"

// 这一步是 ROS 程序的标准起手式，负责与 ROS Master 通信

ros::init(argc, argv, "Laser_node");

// 2. 实例化核心类 Laser_CORE

// 构造函数中会完成参数读取、话题订阅和发布者初始化

LOA_CORE Laser;

// 3. 启动节点主循环

// 进入 ros::spin()，开始处理回调函数

Laser.start();

return 0;

}

⚙️ 2. 核心逻辑实现 (Laser.cpp)

这个文件包含了 Laser_CORE 类的具体实现，涵盖了参数配置、数据回调、防区逻辑处理和数据发布。

构造函数：初始化与参数加载

include "Laser.h" // 包含头文件

// 构造函数：负责节点的初始化配置

LOA_CORE::Laser_CORE() {

ros::NodeHandle nh; // 全局句柄，用于访问全局参数和话题

ros::NodeHandle nh_p("~"); // 私有句柄，用于访问该节点特有的参数（~param_name）

ros::Rate rate(25); // 设置循环频率为 25Hz（虽然在此构造函数中未实际使用循环）

// --- 参数获取 ---

// 从参数服务器获取激光雷达话题名称，默认为 "/scan"

nh_p.paramstd::string("scan_topic", scan_topic_, "/scan");

// 获取防区数量，默认为 3 个

nh_p.param("num_of_area", number_of_area_, 3);
std::cout << "Get " << number_of_area_ << " Areas." << std::endl;
// --- 循环读取每个防区的具体配置 ---
for(int temp = 1; temp <= number_of_area_; temp ++) {
std::ostringstream oss_temp; // 用于拼接参数名称的字符串流
// 1. 获取最小角度
oss_temp << "area_" << temp << "min_ang";
nh_p.param(oss_temp.str(), area_temp .min_ang, 0.0);
std::cout << "get " << oss_temp.str() << ": " << area_temp_.min_ang << std::endl;
oss_temp.str(""); // 清空字符串流以便复用
// 2. 获取最大角度
oss_temp << "area_" << temp << "max_ang";
nh_p.param(oss_temp.str(), area_temp .max_ang, 0.0);
std::cout << "get " << oss_temp.str() << ": " << area_temp_.max_ang << std::endl;
oss_temp.str("");
// 3. 获取最大检测距离
oss_temp << "area_" << temp << "max_range";
nh_p.param(oss_temp.str(), area_temp .max_range, 0.0);
std::cout << "get " << oss_temp.str() << ": " << area_temp_.max_range << std::endl;
oss_temp.str("");
// 4. 获取最小检测距离
oss_temp << "area_" << temp << "min_range";
nh_p.param(oss_temp.str(), area_temp .min_range, 0.0);
std::cout << "get " << oss_temp.str() << ": " << area_temp_.min_range << std::endl;
oss_temp.str("");
// 5. 获取滤波器尺寸（连续多少个点满足条件才报警）
oss_temp << "area_" << temp << "filter_size";
nh_p.param(oss_temp.str(), area_temp .filter_size, 0);
std::cout << "get " << oss_temp.str() << ": " << area_temp_.filter_size << std::endl;
oss_temp.str("");
// 如果最大距离不为0，认为该防区有效，加入防区组
if(area_temp_.max_range != 0) {
std::cout << "Loading #" << area_group_.size() + 1 << " Denfence Area" << std::endl;
area_group_.push_back(area_temp_); // 将临时结构体存入防区向量
}
// --- 初始化发布者 ---
// 为每个防区创建一个独立的话题发布者，话题名为 "/range" + 编号
oss_temp << "range" << temp;
range_pub_temp_ = nh.advertise<sensor_msgs::Range>(oss_temp.str(), 10);
range_pub_group_.push_back(range_pub_temp_); // 存入发布者向量
oss_temp.str("");
}
std::cout << "Now We Have " << range_pub_group_.size() << " Defennce Range Publisher !" << std::endl;
// --- 订阅激光雷达话题 ---
// 订阅 scan_topic_ 话题，队列长度 1，绑定回调函数 scan_callback
lidar_sub_ = nh.subscribe<sensor_msgs::LaserScan>(scan_topic_, 1, boost::bind(&Laser_CORE::scan_callback, this, 1));
}
回调函数：数据处理流水线
当激光雷达数据到达时，scan_callback 被触发，依次调用四个处理函数。
// 雷达话题的回调函数
void Laser_CORE::scan_callback(const sensor_msgs::LaserScanConstPtr &scan) {
scan = *scan; // 1. 保存原始数据到成员变量
fill_scan_to_zone(); // 2. 根据角度将数据分配到不同防区
filter(); // 3. 滤波处理（去噪、防误报）
find_the_nearest_obs(); // 4. 寻找防区内最近障碍物
publish_result(); // 5. 发布处理结果
}
逻辑处理函数详解

1. 数据分配 (fill_scan_to_zone)
   将 360 度（或部分角度）的激光点云，根据配置的角度范围，切割到不同的防区向量中。
   void Laser_CORE::fill_scan_to_zone(void) {
   // 遍历所有防区
   for(int i = 0; i < area_group_.size(); i++) {
   // 清空上一帧该防区的数据
   std::vector().swap(area_group_i.alert_area);
   // 计算该防区在激光数组中的起始和结束索引
   // 公式：(目标角度 - 激光起始角) / 角分辨率
   int start_idx = (int)((area_group_i.min_ang * (PI/180) - scan_.angle_min) / scan_.angle_increment);
   int end_idx = (int)(((area_group_i.max_ang * (PI/180)) - scan_.angle_min) / scan_.angle_increment);
   // 将该范围内的距离数据提取出来存入防区
   for(int j = start_idx; j < end_idx; j++) {
   area_group_i.alert_area.push_back(scan_.rangesj);
   }
   }
   }
2. 滤波逻辑 (filter)
   这是一个简单的连续点滤波算法。只有当连续 filter_size 个点都在有效距离内，才判定为报警。
   void Laser_CORE::filter(void) {
   for(int i = 0; i < area_group_.size(); i++) {
   int k = 0; // 计数器：记录连续满足条件的点数
   for(int j = 0; j < area_group_i.alert_area.size(); j++) {
   // 判断距离是否在设定的 min_range, max_range 之间
   if(area_group_i.alert_areaj <= area_group_i.max_range &&
   area_group_i.alert_areaj >= area_group_i.min_range) {
   k++; // 满足条件，计数加 1
   // 如果连续满足条件的点数达到阈值，标记为警告并跳出
   if(k >= area_group_i.filter_size) {
   area_group_i.warning = true;
   break;
   }
   } else {
   k = 0; // 一旦遇到不满足条件的点（如超出范围或无效点），计数清零
   }
   // 注意：原代码逻辑中，如果循环跑完都没break，这里会不断覆盖warning状态
   // 这里的逻辑略显冗余，主要依赖上面的 break
   area_group_i.warning = false;
   }
   }
   }
3. 寻找最近障碍物 (find_the_nearest_obs)
   遍历防区内的所有点，找到最小值。
   void Laser_CORE::find_the_nearest_obs(void) {
   for(int i = 0; i < area_group_.size(); i++) {
   // 初始化最近距离为防区的最大范围
   area_group_i.nearest_obstacle_distance = area_group_i.max_range;
   // 遍历防区内所有点
   for(int j = 0; j < area_group_i.alert_area.size(); j++) {
   // 更新最小值
   area_group_i.nearest_obstacle_distance =
   area_group_i.nearest_obstacle_distance > area_group_i.alert_areaj ?
   area_group_i.alert_areaj : area_group_i.nearest_obstacle_distance;
   // 边界钳位：确保结果不超过最大范围
   if(area_group_i.nearest_obstacle_distance > area_group_i.max_range)
   area_group_i.nearest_obstacle_distance = area_group_i.max_range;
   // 边界钳位：确保结果不小于最小范围
   if(area_group_i.nearest_obstacle_distance < area_group_i.min_range)
   area_group_i.nearest_obstacle_distance = area_group_i.min_range;
   }
   }
   }
4. 结果发布 (publish_result)
   将计算出的最近距离封装成 ROS 标准消息发布。
   void Laser_CORE::publish_result(void) {
   for(int i = 0; i < area_group_.size(); i++) {
   // 填充 sensor_msgs::Range 消息
   range_temp_.radiation_type = sensor_msgs::Range::INFRARED; // 辐射类型（此处仅为占位）
   // 设置 frame_id 为 "/range" + 编号
   oss_temp << "/range" << i+1;
   range_temp_.header.frame_id = oss_temp.str();
   oss_temp.str("");
   range_temp_.header.stamp = ros::Time::now(); // 时间戳
   // 视场角 = 最大角 - 最小角 (转为弧度)
   range_temp_.field_of_view = (area_group_i.max_ang * (PI/180)) - (area_group_i.min_ang * (PI/180));
   range_temp_.min_range = area_group_i.min_range;
   range_temp_.max_range = area_group_i.max_range;
   range_temp_.range = area_group_i.nearest_obstacle_distance; // 核心数据：最近距离
   // 发布消息
   range_pub_group_i.publish(range_temp_);
   }
   // 注释掉的代码块原本是用于发布自定义的障碍物标志位消息
   }
   启动函数
   void Laser_CORE::start(void) {
   ros::spin(); // 进入死循环，等待并处理回调函数
   }

1)Laser.h

ifndef Laser_H

define Laser_H

include <sensor_msgs/LaserScan.h>

include <sensor_msgs/Range.h>

include <std_msgs/Bool.h>

include "ros/ros.h"

include

include <stdio.h>

include

include <stdlib.h>

include <math.h>

include

include

using namespace std;

define PI 3.141592653

define myabs(x) (x)>0? (x) : (-(x))

//激光转声纳的数据结果

typedef struct loa_data_struct

{

//最小角度

double min_ang;

//最大角度

double max_ang;

//最小距离

double min_range;

//最大距离

double max_range;

//报警区域

std::vector alert_area;

//滤波器尺寸

int filter_size;

//报警标志

bool warning;

//最近障碍物的距离

double nearest_obstacle_distance;

}

Laser_data;

//核心类

class Laser_CORE

{

public:

Laser_CORE();

~Laser_CORE();

//开始监控防区

void start(void);

private:

//激光雷达话题的名称

std::string scan_topic_;

//雷达话题的订阅器

ros::Subscriber lidar_sub_;

//储存接收的激光雷达数据

sensor_msgs::LaserScan scan_;

//临时通用字符

std::stringstream oss_temp;

//防区的数量

int number_of_area_;

//防区的数据结构

loa_data area_temp_;

//防区的数组

std::vector<loa_data> area_group_;

//模拟出来的声纳防区

sensor_msgs::Range range_temp_;

//单一防区的发布话题

ros::Publisher range_pub_temp_;

//防区数组的发布话题

std::vectorros::Publisher range_pub_group_;

//防区标志的发布话题

ros::Publisher obs_flag_pub_;

/// /// 函数 /// ///

//根据设置的防区范围，填充雷达数据

void fill_scan_to_zone(void);

//根据设置的滤波器大小，过滤选区内的雷达数据

void filter(void);

//搜寻最近障碍物

void find_the_nearest_obs(void);

//发布结果

void publish_result(void);

//绑定激光话题的回调函数

void scan_callback(const sensor_msgs::LaserScan::ConstPtr &scan);

};

endif

2.Laser.cpp

include "laser.h"

//构造函数

Laser_CORE::Laser_CORE()

{

ros::NodeHandle nh;

ros::NodeHandle nh_p("~");

ros::Rate rate(25);

//std_msgs::Bool init_flag_;

//init_flag_.data = false;

//获取激光话题的名称

nh_p.paramstd::string("scan_topic", scan_topic_, "/scan");

//获取防区数量

nh_p.param("num_of_area",number_of_area_, 3);
std::cout << "Get " << number_of_area_ << " Areas." << std::endl;

//获取每个防区结构

for(int temp = 1; temp <= number_of_area_; temp ++)

{

//最小角度

oss_temp << "area_" << temp << "min_ang";
nh_p.param (oss_temp.str(), area_temp .min_ang, 0.0);

std::cout << "get " << oss_temp.str() << ": " << area_temp_.min_ang << std::endl;

oss_temp.str("");

//最大角度

oss_temp << "area_" << temp << "max_ang";
nh_p.param(oss_temp.str(),area_temp .max_ang,0.0);

std::cout << "get " << oss_temp.str() << ": " << area_temp_.max_ang << std::endl;

oss_temp.str("");

//最大距离

oss_temp << "area_" << temp << "max_range";
nh_p.param(oss_temp.str(),area_temp .max_range,0.0);

std::cout << "get " << oss_temp.str() << ": " << area_temp_.max_range << std::endl;

oss_temp.str("");

//最小距离

oss_temp << "area_" << temp << "min_range";
nh_p.param(oss_temp.str(),area_temp .min_range,0.0);

std::cout << "get " << oss_temp.str() << ": " << area_temp_.min_range << std::endl;

oss_temp.str("");

//滤波器尺寸

oss_temp << "area_" << temp << "filter_size";
nh_p.param(oss_temp.str(),area_temp .filter_size,0);

std::cout << "get " << oss_temp.str() << ": " << area_temp_.filter_size << std::endl;

oss_temp.str("");

if(area_temp_.max_range != 0)

{

//ROS_INFO("Loading %d Denfence Area",area_group_.size()+1);

std::cout << "Loading #" << area_group_.size() +1 << " Denfence Area" << std::endl;

//压入防区序列

area_group_.push_back(area_temp_);

}

oss_temp << "range" << temp ;

//发布这个单一防区

range_pub_temp_ = nh.advertise<sensor_msgs::Range> (oss_temp.str(),10);

//压入防区发布区序列

range_pub_group_.push_back(range_pub_temp_);

oss_temp.str("");

}

std::cout << "Now We Have " <<range_pub_group_.size() << " Defennce Range Publisher !"<< std::endl;

//激光话题的订阅器

lidar_sub_ = nh.subscribe<sensor_msgs::LaserScan> (scan_topic_,1,boost::bind(&Laser_CORE::scan_callback,this,_1));

//绑定激光话题的回调函数

//obs_flag_pub_ = nh.advertise<LOA_CORE::loa_msgs>("/loa",1);

//obs_flag_.zone.resize(number_of_area_);

}

//解构函数

Laser_CORE::~Laser_CORE()

{

}

//雷达话题的回调函数

void Laser_CORE::scan_callback(const sensor_msgs::LaserScanConstPtr &scan)

{

//储存接收的激光雷达数据

scan_ = *scan;

fill_scan_to_zone();

filter();

find_the_nearest_obs();

publish_result();

}

//根据设置的防区范围，填充雷达数据

void Laser_CORE::fill_scan_to_zone(void)

{

//防区序列

for(int i = 0; i < area_group_.size(); i++)

{

//单一防区

std::vector().swap(area_group_i.alert_area);
for(int j = (int) ((area_group_i.min_ang*(PI/180) - scan_.angle_min) / scan_.angle_increment);
j < (int) (((area_group_i.max_ang* (PI/180)) - scan_.angle_min) / scan_.angle_increment);
j++
)
{
area_group_i.alert_area.push_back(scan_.rangesj);
}
}
}
//根据设置的滤波器大小，过滤选区内的雷达数据
void Laser_CORE::filter(void)
{
for(int i = 0; i < area_group_.size(); i++)
{
int k = 0;
for(int j = 0; j < area_group_i.alert_area.size(); j++)
{
if( area_group_i.alert_areaj <= area_group_i.max_range
&& area_group_i.alert_areaj >= area_group_i.min_range
)
{
k++;
if(k >= area_group_i.filter_size)
{
area_group_i.warning = true;
break;
}
}
else
{
k = 0;
}
area_group_i.warning = false;
}
}
}

//搜寻最近障碍物

void Laser_CORE::find_the_nearest_obs(void)

{

for(int i = 0; i < area_group_.size(); i++)

{

area_group_i.nearest_obstacle_distance = area_group_i.max_range;

for(int j = 0; j < area_group_i.alert_area.size(); j++)

{

area_group_i.nearest_obstacle_distance =

area_group_i.nearest_obstacle_distance > area_group_i.alert_areaj ?

area_group_i.alert_areaj :

area_group_i.nearest_obstacle_distance

;

if(area_group_i.nearest_obstacle_distance > area_group_i.max_range)

area_group_i.nearest_obstacle_distance = area_group_i.max_range;

if(area_group_i.nearest_obstacle_distance < area_group_i.min_range)

area_group_i.nearest_obstacle_distance = area_group_i.min_range;

}

}

}

//发布结果

void Laser_CORE::publish_result(void)

{

for(int i = 0; i < area_group_.size();i++)

{

range_temp_.radiation_type = sensor_msgs::Range::INFRARED;

oss_temp << "/range" << i+1 ;

range_temp_.header.frame_id = oss_temp.str();

oss_temp.str("");

range_temp_.header.stamp = ros::Time::now();

range_temp_.field_of_view = (area_group_i.max_ang*(PI/180)) - (area_group_i.min_ang* (PI/180));

range_temp_.min_range = area_group_i.min_range;

range_temp_.max_range = area_group_i.max_range;

range_temp_.range = area_group_i.nearest_obstacle_distance;

range_pub_group_i.publish(range_temp_);

}

//for(vector::size_type i = 0; i != area_group_.size(); ++i)
/*
for(int i = 0; i < area_group_.size(); i++)
{
if(area_group_i.warning == true)
obs_flag_.zonei.data = true;
else
obs_flag_.zonei.data = false;
}
obs_flag_.header.stamp = ros::Time::now();
obs_flag_.header.frame_id = "laser";
obs_flag_pub_.publish(obs_flag_);
*/
}

void Laser_CORE::start(void)

{

ros::spin();

}

代码逻辑总结:

该驱动验证了以下逻辑：

配置化：通过 ROS 参数服务器灵活配置多个防区，无需修改代码即可改变监测区域。

区域化：将激光雷达数据按角度切分，实现分区监测。

抗干扰：通过 filter_size 参数实现简单的连续点滤波，防止因单个噪点导致的误报。

标准化：输出标准的 sensor_msgs/Range 消息，便于其他导航或避障模块直接调用。

验证建议

在实际运行前，建议检查以下几点：

PI 定义：代码中使用了 PI，需确保 laser.h 中定义了 #define PI 3.1415926... 或包含 并正确使用。
索引越界：fill_scan_to_zone 中计算索引时，未严格检查 start_idx 和 end_idx 是否超出了 scan_.ranges 的实际大小，若配置角度超出雷达物理范围可能导致崩溃。
参数配置：启动节点时，需通过 launch 文件或命令行传入 num_of_area 及各 area_xxx 参数，否则将使用默认值（可能不符合预期）。