rviz 显示Laserscan 数据 缓冲之前数据显示.

rviz显示Laserscan数据缓冲问题解决

rviz 显示Laserscan 数据 缓冲之前数据显示.

解决方法:重启roscore。

/** * @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 与这个代码的区别是什么
10-08
评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
红包 添加红包
表情包 插入表情
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值