1.简介
此项目实现四个传感器的数据融合，包括顶部激光雷达（如：图中1），两个单线激光雷达(如：图中2，3)，和一个交叉线激光雷达(如：图中4)。这个代码主要包括：时间对齐和空间对齐。其中时间对齐和空间对齐的方法值得看官借鉴。
2.时间对齐
将交叉线激光雷达数据和两个线激光雷达数据放入成员变量中，如下面代码中的声明，然后裁剪出与当前时间最近的数据进行融合。
// Data queues
std::deque<sensor_msgs::msg::PointCloud2> cross_lidar_queue_;
std::deque<sensor_msgs::msg::PointCloud2> left_lidar_queue_;
std::deque<sensor_msgs::msg::PointCloud2> right_lidar_queue_;
void LidarFusion::TopLidarCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg)
{
const rclcpp::Time current_stamp = msg->header.stamp;
is_used_cross_lidar_ = PruneCrossLidarDeque(current_stamp);
is_used_left_edge_ = PruneLeftEdgeDeque(current_stamp);
is_used_right_edge_ = PruneRightEdgeDeque(current_stamp);
FuseAndPublish(*msg, current_cross_msg_, current_left_msg_,current_right_msg_);
}
---------------------------------
bool LidarFusion::PruneCrossLidarDeque(const rclcpp::Time& stamp)
{
std::lock_guard<std::mutex> lock(cross_lock_);
if (cross_lidar_queue_.empty())
return false;
auto best_match = cross_lidar_queue_.end();
double min_diff = std::numeric_limits<double>::max();
for (auto it = cross_lidar_queue_.begin(); it != cross_lidar_queue_.end(); ++it)
{
const auto msg_time = rclcpp::Time(it->header.stamp);
const double diff = fabs((msg_time - stamp).seconds());
if (diff < min_diff) {
min_diff = diff;
best_match = it;
}
// Early exit if we pass the target time 过了目标时间就提前退出
if (msg_time > stamp)
{
// Check previous element if it exists
if (it != cross_lidar_queue_.begin())
{
auto prev_it = std::prev(it);
const double prev_diff = fabs((rclcpp::Time(prev_it->header.stamp) - stamp).seconds());
if (prev_diff < diff)
{
min_diff = prev_diff;
best_match = prev_it;
}
}
break;
}
}
// RCLCPP_INFO(get_logger(), "PruneCrossLidarDeque ,min_diff : %f", min_diff);
if (best_match != cross_lidar_queue_.end() && min_diff <= time_tolerance_)
{
current_cross_msg_ = *best_match;
cross_lidar_queue_.erase(cross_lidar_queue_.begin(), best_match + 1);
return true;
}
return false;
}
3.空间对齐
void LidarFusion::FuseAndPublish(const sensor_msgs::msg::PointCloud2& scan_lidar,
const sensor_msgs::msg::PointCloud2& cross_cloud,
const sensor_msgs::msg::PointCloud2& edge_lidar,
const sensor_msgs::msg::PointCloud2& right_cloud)
{
// 处理数据（示例：打印激光扫描范围和点云尺寸）
// RCLCPP_INFO(get_logger(), "edge_lidar width: %d, right_cloud width: %d, scan_lidar width: %d, cross_cloud width: %d",
// edge_lidar.width,
// right_cloud.width,
// scan_lidar.width,
// cross_cloud.width);
//记录顶部激光雷达数目
laser_point_num_ = scan_lidar.width;
// 转换顶部激光雷达点云
pcl::PointCloud<pcl::PointXYZ> single_cloud;
pcl::fromROSMsg(scan_lidar, single_cloud);
pcl::PointCloud<pcl::PointXYZ> transformed_cloud_single;
transformed_cloud_single.header = single_cloud.header;
Eigen::Affine3f transform = Eigen::Affine3f::Identity();
transform.translation() << 0.0095f, 0.0f, 0.084089f;
Eigen::Matrix3f rotation_matrix;
rotation_matrix = Eigen::AngleAxisf(0.0f,Eigen::Vector3f::UnitX()) *
Eigen::AngleAxisf(0.0f,Eigen::Vector3f::UnitY()) *
Eigen::AngleAxisf(1.5708f,Eigen::Vector3f::UnitZ());
transform.rotate(rotation_matrix);
pcl::transformPointCloud(single_cloud, transformed_cloud_single, transform);
// 融合的点云
pcl::PointCloud<pcl::PointXYZ> merged_cloud;
merged_cloud = transformed_cloud_single;
// 低矮障碍物点云
pcl::PointCloud<pcl::PointXYZ> obstacle_cloud;
// 左线激光雷达
if(is_used_left_edge_)
{
pcl::PointCloud<pcl::PointXYZ> edge_cloud;
pcl::fromROSMsg(edge_lidar, edge_cloud);
pcl::PointCloud<pcl::PointXYZ> transformed_cloud_edge;
transformed_cloud_edge.header = edge_cloud.header;
transform = Eigen::Affine3f::Identity();
transform.translation() << -0.06f, 0.19f, 0.024f;
rotation_matrix = Eigen::AngleAxisf(1.6019082f,Eigen::Vector3f::UnitZ()) *
Eigen::AngleAxisf(0.87f,Eigen::Vector3f::UnitY()) *
Eigen::AngleAxisf(0.0f,Eigen::Vector3f::UnitX());
transform.rotate(rotation_matrix);
pcl::transformPointCloud(edge_cloud, transformed_cloud_edge, transform);
// 创建一个新的点云对象用于存储过滤后的点
pcl::PointCloud<pcl::PointXYZ> filtered_transformed_cloud;
// 遍历变换后的点云，并过滤掉y值大于0.25的点
for (const auto& point : transformed_cloud_edge.points)
{
if (point.z > -0.05)
{ // 只保留y值小于等于0.25的点
filtered_transformed_cloud.push_back(point);
// // 打印满足条件的点的信息
// RCLCPP_INFO(get_logger(), "left edge lidar, Point : X: %f, Y: %f, Z: %f",
// point.x, point.y, point.z);
}
}
// 将过滤后的点云合并到最终结果中
merged_cloud += filtered_transformed_cloud;
obstacle_cloud += filtered_transformed_cloud;
}
if(is_used_right_edge_)
{
// 沿边传感器变换
pcl::PointCloud<pcl::PointXYZ> right_edge_cloud;
pcl::fromROSMsg(right_cloud, right_edge_cloud);
pcl::PointCloud<pcl::PointXYZ> transformed_cloud_right_edge;
transformed_cloud_right_edge.header = right_edge_cloud.header;
transform = Eigen::Affine3f::Identity();
transform.translation() << -0.06f , -0.19f, 0.024f;
rotation_matrix = Eigen::AngleAxisf(-1.6019082f,Eigen::Vector3f::UnitZ()) *
Eigen::AngleAxisf(0.87f,Eigen::Vector3f::UnitY()) *
Eigen::AngleAxisf(0.0f,Eigen::Vector3f::UnitX());
transform.rotate(rotation_matrix);
pcl::transformPointCloud(right_edge_cloud, transformed_cloud_right_edge, transform);
// 遍历变换后的点云，并过滤掉y值大于0.25的点
pcl::PointCloud<pcl::PointXYZ> filtered_transformed_cloud_right; // 创建一个新的点云对象用于存储过滤后的点
for (const auto& point : transformed_cloud_right_edge.points)
{
if (point.z > -0.05)
{
filtered_transformed_cloud_right.push_back(point);
// 打印满足条件的点的信息
// RCLCPP_INFO(get_logger(), "right edge lidar, Point : X: %f, Y: %f, Z: %f",
// point.x, point.y, point.z);
}
}
merged_cloud += filtered_transformed_cloud_right;
obstacle_cloud += filtered_transformed_cloud_right;
}
if(is_used_cross_lidar_)
{
// 交叉激光雷达变换
pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
pcl::fromROSMsg(cross_cloud, pcl_cloud);
pcl::PointCloud<pcl::PointXYZ> transformed_cloud_cross;
transformed_cloud_cross.header = pcl_cloud.header;
transform = Eigen::Affine3f::Identity();
transform.translation() << 0.125f, 0.0f, 0.084089f;
rotation_matrix = Eigen::AngleAxisf(0.0f,Eigen::Vector3f::UnitX()) *
Eigen::AngleAxisf(1.5708f,Eigen::Vector3f::UnitY()) *
Eigen::AngleAxisf(1.5708f,Eigen::Vector3f::UnitZ());
transform.rotate(rotation_matrix);
pcl::transformPointCloud(pcl_cloud, transformed_cloud_cross, transform);
// 对转换后的点云的x值乘以-1.0
for (auto& point : transformed_cloud_cross.points)
{
point.y *= -1.0f;
}
// 计算交叉激光左点
std::vector<float> focus_point = CalculateFocusPoint(transformed_cloud_cross);
if(focus_point.size() > 2)
{
cross_left_point_.x = focus_point[0];
cross_left_point_.y = focus_point[1];
cross_right_point_.x = focus_point[2];
cross_right_point_.y = focus_point[3];
cross_left_point_.is_valid = true;
cross_right_point_.is_valid = true;
}
merged_cloud += transformed_cloud_cross;
obstacle_cloud += transformed_cloud_cross;
}
//发布低矮障碍物点云
sensor_msgs::msg::PointCloud2 output_msg1;
pcl::toROSMsg(obstacle_cloud, output_msg1);
// 添加到队列中
if(is_used_lower_obstacle_)
UpdateLowerObstacle(output_msg1);
// 将融合的点云发布出去
sensor_msgs::msg::PointCloud2 output_msg;
pcl::toROSMsg(merged_cloud, output_msg);
output_msg.header.stamp = this->now();
output_msg.header.frame_id = "base_link";
fusion_pointcloud_publisher_->publish(output_msg);
//发布融合后的二维激光雷达数据
sensor_msgs::msg::LaserScan point2scan_ = PointCloudToLaserscan(merged_cloud);
point2scan_.header.frame_id = "base_link";
pub_fusion_scan_->publish(point2scan_);
//清空数据
cross_left_point_.clear();
cross_right_point_.clear();
}
} // namespace beefast_filter_mask
扫一扫
2595
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
