/**
* @brief 机器狗雷达转激光扫描节点
* 完全参考 pointcloud2_scan_car.cpp 的实现方法
*/
#include <rclcpp/rclcpp.hpp> // ROS2 C++接口
#include <sensor_msgs/msg/point_cloud2.hpp> // 点云消息类型
#include <sensor_msgs/msg/laser_scan.hpp> // 激光扫描消息类型
#include <pcl_conversions/pcl_conversions.h> // PCL与ROS消息转换
#include <pcl/point_cloud.h> // PCL点云数据结构
#include <pcl/point_types.h> // PCL点类型定义
#include <pcl/filters/voxel_grid.h> // 体素滤波
#include <pcl/filters/statistical_outlier_removal.h> // 统计离群点滤波
#include <pcl/filters/radius_outlier_removal.h> // 半径离群点滤波
#include <tf2/LinearMath/Transform.h> // TF2变换
#include <tf2/LinearMath/Quaternion.h> // TF2四元数
#include <tf2_ros/buffer.h> // TF2缓冲区
#include <tf2_ros/transform_listener.h> // TF2变换监听器
#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp> // TF2传感器消息变换
#include <cmath> // 数学函数
#include <vector> // 向量容器
#include <chrono> // 时间相关功能
/**
* @brief 机器狗点云转激光扫描类
* 将机器狗3D点云数据转换为2D激光扫描数据,用于导航避障
*/
class DogPointCloudToLaserScan : public rclcpp::Node
{
public:
/**
* @brief 构造函数 - 完全参考pointcloud2_scan_car.cpp
*/
DogPointCloudToLaserScan() : Node("dog_pointcloud_to_laserscan")
{
RCLCPP_INFO(this->get_logger(),"机器狗雷达转换节点启动成功");
RCLCPP_INFO(this->get_logger(),"机器狗雷达:前方180度扫描,专注0-1.5米近距离检测");
// 参数声明 - 完全参考pointcloud2_scan_car.cpp
this->declare_parameters<double>("", {
{"roll", 0.0}, // 绕X轴旋转角度
{"pitch", 0.0}, // 绕Y轴旋转角度
{"yaw", 0.0}, // 使用原始数据,无需旋转
{"min_z", -0.3}, // 专注地面到低矮障碍物高度
{"max_z", 0.8}, // 机器狗高度范围内的障碍物
{"min_range", 0.0}, // 从0米开始检测
{"max_range", 1.5}, // 只检测1.5米范围内
{"angle_min", -1.57}, // 保持前方180度 (-90度)
{"angle_max", 1.57}, // 保持前方180度 (+90度)
{"angle_increment", M_PI/180.0} // 角度增量,1度分辨率
});
// 点云预处理参数 - 参考pointcloud2_scan_car.cpp
this->declare_parameters<double>("", {
{"voxel_size", 0.02}, // 体素滤波网格大小
{"outlier_mean_k", 50}, // 统计滤波K近邻数量
{"outlier_stddev", 1.0}, // 统计滤波标准差阈值
{"radius_search", 0.1}, // 半径滤波搜索半径
{"min_neighbors", 5}, // 半径滤波最小近邻数
{"intensity_threshold", 10.0} // 强度阈值过滤
});
// 其他参数 - 参考pointcloud2_scan_car.cpp
this->declare_parameter<std::string>("frame_id", "utlidar_lidar"); // 使用原始坐标系,避免变换问题
this->declare_parameter<std::string>("output_topic", "/dog_scan"); // 输出话题名
this->declare_parameter<std::vector<double>>("angle_blocks", std::vector<double>{});
this->declare_parameter<bool>("use_voxel_filter", true);
this->declare_parameter<bool>("use_statistical_filter", false);
this->declare_parameter<bool>("use_radius_filter", true);
this->declare_parameter<bool>("use_median_filter", false);
this->declare_parameter<bool>("use_intensity_filter", false);
this->declare_parameter<bool>("use_temporal_filter", false);
this->declare_parameter<int>("queue_size", 30);
// 获取参数 - 逐个获取确保正确
roll_ = this->get_parameter("roll").as_double();
pitch_ = this->get_parameter("pitch").as_double();
yaw_ = this->get_parameter("yaw").as_double();
min_z_ = this->get_parameter("min_z").as_double();
max_z_ = this->get_parameter("max_z").as_double();
min_range_ = this->get_parameter("min_range").as_double();
max_range_ = this->get_parameter("max_range").as_double();
angle_min_ = this->get_parameter("angle_min").as_double();
angle_max_ = this->get_parameter("angle_max").as_double();
angle_increment_ = this->get_parameter("angle_increment").as_double();
frame_id_ = this->get_parameter("frame_id").as_string();
std::string output_topic = this->get_parameter("output_topic").as_string();
// 获取点云预处理参数 - 参考pointcloud2_scan_car.cpp
const auto filter_params = this->get_parameters({"voxel_size", "outlier_mean_k",
"outlier_stddev", "radius_search",
"min_neighbors", "intensity_threshold"});
voxel_size_ = filter_params[0].as_double();
outlier_mean_k_ = static_cast<int>(filter_params[1].as_double());
outlier_stddev_ = filter_params[2].as_double();
radius_search_ = filter_params[3].as_double();
min_neighbors_ = static_cast<int>(filter_params[4].as_double());
intensity_threshold_ = filter_params[5].as_double();
// 获取过滤器启用状态 - 参考pointcloud2_scan_car.cpp
use_voxel_filter_ = this->get_parameter("use_voxel_filter").as_bool();
use_statistical_filter_ = this->get_parameter("use_statistical_filter").as_bool();
use_radius_filter_ = this->get_parameter("use_radius_filter").as_bool();
use_median_filter_ = this->get_parameter("use_median_filter").as_bool();
use_intensity_filter_ = this->get_parameter("use_intensity_filter").as_bool();
use_temporal_filter_ = this->get_parameter("use_temporal_filter").as_bool();
int queue_size = this->get_parameter("queue_size").as_int();
// 处理角度屏蔽参数 - 参考pointcloud2_scan_car.cpp
auto angle_blocks_param = this->get_parameter("angle_blocks").as_double_array();
if (angle_blocks_param.size() % 2 != 0) {
RCLCPP_WARN(this->get_logger(), "angle_blocks参数元素数量应为偶数,已忽略最后一个元素");
angle_blocks_param.pop_back();
}
for (size_t i = 0; i < angle_blocks_param.size(); i += 2) {
angle_blocks_.emplace_back(angle_blocks_param[i], angle_blocks_param[i+1]);
}
// 初始化坐标变换 - 参考pointcloud2_scan_car.cpp
update_transform();
// 计算扫描参数 - 参考pointcloud2_scan_car.cpp
const size_t ranges_size = std::ceil((angle_max_ - angle_min_) / angle_increment_);
scan_msg_.ranges.assign(ranges_size, std::numeric_limits<float>::infinity());
scan_msg_.header.frame_id = frame_id_;
scan_msg_.angle_min = angle_min_;
scan_msg_.angle_max = angle_max_;
scan_msg_.angle_increment = angle_increment_;
scan_msg_.time_increment = 0.0;
scan_msg_.scan_time = 0.033;
scan_msg_.range_min = min_range_;
scan_msg_.range_max = max_range_;
// 初始化TF - 新增
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
// 创建订阅和发布 - 直接订阅L1原始点云
cloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
"/utlidar/cloud", queue_size, // ✅ 直接订阅L1原始点云(串口转网口,有硬件延迟)
std::bind(&DogPointCloudToLaserScan::cloud_callback, this, std::placeholders::_1));
// 🔧 使用BEST_EFFORT QoS以匹配Nav2 costmap的订阅策略
rclcpp::QoS qos_profile(queue_size);
qos_profile.reliability(rclcpp::ReliabilityPolicy::BestEffort);
scan_pub_ = this->create_publisher<sensor_msgs::msg::LaserScan>(output_topic, qos_profile);
// 打印配置信息
print_filter_config();
RCLCPP_INFO(this->get_logger(), "订阅: /utlidar/cloud -> 发布: /dog_scan (串口转网口硬件延迟已校正)");
RCLCPP_INFO(this->get_logger(), "坐标系: %s, 检测范围: %.1f-%.1fm, 高度: %.2f-%.2fm",
frame_id_.c_str(), min_range_, max_range_, min_z_, max_z_);
}
private:
/**
* @brief 打印过滤器配置信息 - 参考pointcloud2_scan_car.cpp
*/
void print_filter_config() {
RCLCPP_INFO(this->get_logger(), "机器狗雷达预处理配置:");
RCLCPP_INFO(this->get_logger(), " 体素滤波: %s (大小: %.3f)",
use_voxel_filter_ ? "启用" : "禁用", voxel_size_);
RCLCPP_INFO(this->get_logger(), " 统计离群点滤波: %s (K: %d, 标准差: %.2f)",
use_statistical_filter_ ? "启用" : "禁用", outlier_mean_k_, outlier_stddev_);
RCLCPP_INFO(this->get_logger(), " 半径离群点滤波: %s (半径: %.3f, 最小近邻: %d)",
use_radius_filter_ ? "启用" : "禁用", radius_search_, min_neighbors_);
}
/**
* @brief 更新坐标变换 - 参考pointcloud2_scan_car.cpp
*/
void update_transform()
{
tf2::Quaternion q;
q.setRPY(-roll_, -pitch_, -yaw_);
transform_.setRotation(q);
}
/**
* @brief 点云预处理函数 - 参考pointcloud2_scan_car.cpp
*/
void preprocess_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
auto start_time = std::chrono::high_resolution_clock::now();
size_t original_size = cloud->size();
// 体素滤波 - 参考pointcloud2_scan_car.cpp
if (use_voxel_filter_ && cloud->size() > 100) {
pcl::VoxelGrid<pcl::PointXYZ> voxel_filter;
voxel_filter.setInputCloud(cloud);
voxel_filter.setLeafSize(voxel_size_, voxel_size_, voxel_size_);
voxel_filter.filter(*cloud);
}
// 统计离群点滤波 - 参考pointcloud2_scan_car.cpp
if (use_statistical_filter_ && cloud->size() > 100) {
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> stat_filter;
stat_filter.setInputCloud(cloud);
stat_filter.setMeanK(outlier_mean_k_);
stat_filter.setStddevMulThresh(outlier_stddev_);
stat_filter.filter(*cloud);
}
// 半径离群点滤波 - 参考pointcloud2_scan_car.cpp
if (use_radius_filter_ && cloud->size() > 100) {
pcl::RadiusOutlierRemoval<pcl::PointXYZ> radius_filter;
radius_filter.setInputCloud(cloud);
radius_filter.setRadiusSearch(radius_search_);
radius_filter.setMinNeighborsInRadius(min_neighbors_);
radius_filter.filter(*cloud);
}
auto end_time = std::chrono::high_resolution_clock::now();
auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time).count();
if (cloud->size() < original_size * 0.9 || duration > 20) {
RCLCPP_DEBUG(this->get_logger(), "机器狗雷达预处理: %zu -> %zu 点 (%.1f%%), 耗时: %ld ms",
original_size, cloud->size(),
original_size > 0 ? (100.0 * cloud->size() / original_size) : 0.0,
duration);
}
}
/**
* @brief 点云回调函数 - 完全参考pointcloud2_scan_car.cpp
*/
void cloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
{
// 简化处理:直接使用原始数据,不做坐标变换
// 让RViz通过TF自动处理坐标显示
sensor_msgs::msg::PointCloud2 transformed_msg = *msg;
// 转换点云数据 - 参考pointcloud2_scan_car.cpp
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::fromROSMsg(transformed_msg, *cloud);
// 点云预处理 - 参考pointcloud2_scan_car.cpp
preprocess_cloud(cloud);
// 重置扫描数据 - 参考pointcloud2_scan_car.cpp
std::fill(scan_msg_.ranges.begin(), scan_msg_.ranges.end(),
std::numeric_limits<float>::infinity());
// ⏱️ L1雷达是串口转网口,硬件传输延迟大,直接用系统当前时间替换时间戳
scan_msg_.header.stamp = this->get_clock()->now();
// 存储每个角度索引的点 - 参考pointcloud2_scan_car.cpp
std::vector<std::vector<float>> angle_ranges(scan_msg_.ranges.size());
// 处理每个点 - 完全参考pointcloud2_scan_car.cpp
int total_points = 0;
int height_filtered = 0;
int range_filtered = 0;
int valid_points = 0;
for (const auto& point : *cloud)
{
total_points++;
// 坐标变换 - 参考pointcloud2_scan_car.cpp
tf2::Vector3 p(point.x, point.y, point.z);
const tf2::Vector3 transformed = transform_ * p;
// 高度过滤 - 参考pointcloud2_scan_car.cpp
if (transformed.z() < min_z_ || transformed.z() > max_z_) {
height_filtered++;
continue;
}
// 计算极坐标 - 参考pointcloud2_scan_car.cpp
const double range = std::hypot(transformed.x(), transformed.y());
const double angle = std::atan2(transformed.y(), transformed.x());
// 角度屏蔽检查 - 参考pointcloud2_scan_car.cpp
bool is_blocked = false;
for (const auto& block : angle_blocks_) {
const double start = block.first;
const double end = block.second;
if (start <= end) {
if (angle >= start && angle <= end) {
is_blocked = true;
break;
}
} else {
if (angle >= start || angle <= end) {
is_blocked = true;
break;
}
}
}
if (is_blocked) continue;
// 过滤有效距离 - 参考pointcloud2_scan_car.cpp
if (range < min_range_ || range > max_range_) {
range_filtered++;
continue;
}
// 计算扫描索引 - 参考pointcloud2_scan_car.cpp
const int index = static_cast<int>((angle - angle_min_) / angle_increment_);
// 调试:记录前几个点的详细信息
if (valid_points < 5) {
RCLCPP_INFO(this->get_logger(),
"🔍 点%d: x=%.2f, y=%.2f, z=%.2f, range=%.2f, angle=%.2f度, index=%d",
valid_points, transformed.x(), transformed.y(), transformed.z(),
range, angle*180.0/M_PI, index);
}
// 收集每个角度索引的所有点 - 参考pointcloud2_scan_car.cpp
if (index >= 0 && static_cast<size_t>(index) < scan_msg_.ranges.size()) {
angle_ranges[index].push_back(range);
valid_points++;
} else if (valid_points < 5) {
RCLCPP_WARN(this->get_logger(),
"⚠️ 索引超范围: index=%d, 范围=[0, %zu)", index, scan_msg_.ranges.size());
}
}
// 对每个角度索引的点进行处理 - 完全参考pointcloud2_scan_car.cpp
for (size_t i = 0; i < angle_ranges.size(); ++i) {
if (angle_ranges[i].empty()) continue;
// 对每个角度的点进行排序 - 参考pointcloud2_scan_car.cpp
std::sort(angle_ranges[i].begin(), angle_ranges[i].end());
// 使用中值或最近点作为该角度的距离 - 参考pointcloud2_scan_car.cpp
if (angle_ranges[i].size() >= 3) {
// 使用中值滤波
scan_msg_.ranges[i] = angle_ranges[i][angle_ranges[i].size() / 2];
} else {
// 使用最近点
scan_msg_.ranges[i] = angle_ranges[i].front();
}
}
// 时间平滑处理 - 参考pointcloud2_scan_car.cpp
if (use_temporal_filter_ && !previous_scan_.ranges.empty()) {
const float alpha = 0.3f;
for (size_t i = 0; i < scan_msg_.ranges.size(); ++i) {
if (std::isfinite(previous_scan_.ranges[i]) && std::isfinite(scan_msg_.ranges[i])) {
scan_msg_.ranges[i] = alpha * scan_msg_.ranges[i] + (1.0f - alpha) * previous_scan_.ranges[i];
}
}
}
// 保存当前扫描用于下次平滑 - 参考pointcloud2_scan_car.cpp
if (use_temporal_filter_) {
previous_scan_ = scan_msg_;
}
// 发布扫描数据
scan_pub_->publish(scan_msg_);
// 统计有数据的角度范围
int filled_ranges = 0;
for (size_t i = 0; i < angle_ranges.size(); ++i) {
if (!angle_ranges[i].empty()) filled_ranges++;
}
// 调试信息 - 每5秒输出一次统计
RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000,
"🔍 机器狗雷达统计: 总点数=%d, 高度过滤=%d, 距离过滤=%d, 有效点数=%d, 有数据角度=%d/%zu",
total_points, height_filtered, range_filtered, valid_points, filled_ranges, angle_ranges.size());
}
// 成员变量 - 完全参考pointcloud2_scan_car.cpp
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_sub_;
rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan_pub_;
sensor_msgs::msg::LaserScan scan_msg_;
sensor_msgs::msg::LaserScan previous_scan_;
tf2::Transform transform_;
std::vector<std::pair<double, double>> angle_blocks_;
// 参数存储 - 参考pointcloud2_scan_car.cpp
double roll_, pitch_, yaw_;
double min_z_, max_z_;
double min_range_, max_range_;
double angle_min_, angle_max_, angle_increment_;
std::string frame_id_;
// 点云预处理参数 - 参考pointcloud2_scan_car.cpp
double voxel_size_;
int outlier_mean_k_;
double outlier_stddev_;
double radius_search_;
int min_neighbors_;
double intensity_threshold_;
// 过滤器启用标志 - 参考pointcloud2_scan_car.cpp
bool use_voxel_filter_;
bool use_statistical_filter_;
bool use_radius_filter_;
bool use_median_filter_;
bool use_intensity_filter_;
bool use_temporal_filter_;
// TF变换相关 - 新增
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
};
/**
* @brief 主函数 - 参考pointcloud2_scan_car.cpp
*/
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<DogPointCloudToLaserScan>());
rclcpp::shutdown();
return
与这个代码的区别是什么