main
parent
ad8b5c8def
commit
25039b044d
@ -1,45 +1,119 @@
|
||||
#!/usr/bin/env python
|
||||
# coding:utf-8
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
激光雷达数据滤波节点
|
||||
Laser scan data filtering node
|
||||
|
||||
功能说明 (Functionality):
|
||||
- 激光雷达数据降采样处理
|
||||
- 减少激光雷达数据点数量
|
||||
- 提高数据处理效率
|
||||
- 保持激光雷达数据的基本特征
|
||||
|
||||
作者 (Author): UVPTCCS Team
|
||||
日期 (Date): 2024
|
||||
"""
|
||||
|
||||
# ROS2相关库 (ROS2 related libraries)
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import LaserScan
|
||||
import numpy as np
|
||||
from rclpy.clock import Clock
|
||||
|
||||
|
||||
class scan_compression(Node):
|
||||
def __init__(self,name):
|
||||
"""
|
||||
激光雷达数据压缩节点类
|
||||
Laser scan data compression node class
|
||||
|
||||
功能 (Functions):
|
||||
- 对激光雷达数据进行降采样
|
||||
- 减少数据传输量
|
||||
- 提高系统处理效率
|
||||
- 保持激光雷达数据的空间分辨率
|
||||
"""
|
||||
|
||||
def __init__(self, name):
|
||||
"""
|
||||
初始化激光雷达数据压缩节点
|
||||
Initialize laser scan data compression node
|
||||
|
||||
Args:
|
||||
name (str): 节点名称 (Node name)
|
||||
"""
|
||||
super().__init__(name)
|
||||
self.multiple = 2
|
||||
self.pub = self.create_publisher(LaserScan, "/downsampled_scan", 1000)
|
||||
self.laserSub = self.create_subscription(LaserScan,"/scan", self.laserCallback, 1000)
|
||||
|
||||
# 降采样倍数 (Downsampling multiple)
|
||||
self.multiple = 2 # 每隔2个点采样一次 (Sample every 2 points)
|
||||
|
||||
# 创建发布者 (Create publisher)
|
||||
self.pub = self.create_publisher(LaserScan, "/downsampled_scan", 1000) # 发布降采样后的激光雷达数据 (Publish downsampled laser scan data)
|
||||
|
||||
# 创建订阅者 (Create subscriber)
|
||||
self.laserSub = self.create_subscription(LaserScan, "/scan", self.laserCallback, 1000) # 订阅原始激光雷达数据 (Subscribe to raw laser scan data)
|
||||
|
||||
print("Laser scan compression node initialized")
|
||||
|
||||
def laserCallback(self, data):
|
||||
# self.get_logger().info("laserCallback")
|
||||
"""
|
||||
激光雷达数据回调函数
|
||||
Laser scan data callback function
|
||||
|
||||
Args:
|
||||
data (LaserScan): 原始激光雷达数据 (Raw laser scan data)
|
||||
|
||||
功能 (Functions):
|
||||
- 接收原始激光雷达数据
|
||||
- 进行降采样处理
|
||||
- 发布处理后的数据
|
||||
- 保持数据的时间戳和坐标系信息
|
||||
"""
|
||||
# 数据类型检查 (Data type check)
|
||||
if not isinstance(data, LaserScan): return
|
||||
|
||||
# 创建新的激光雷达消息 (Create new laser scan message)
|
||||
laser_scan = LaserScan()
|
||||
laser_scan.header.stamp = Clock().now().to_msg()
|
||||
laser_scan.header.frame_id = data.header.frame_id
|
||||
laser_scan.angle_increment = data.angle_increment * self.multiple
|
||||
laser_scan.time_increment = data.time_increment
|
||||
laser_scan.intensities = data.intensities
|
||||
laser_scan.scan_time = data.scan_time
|
||||
laser_scan.angle_min = data.angle_min
|
||||
laser_scan.angle_max = data.angle_max
|
||||
laser_scan.range_min = data.range_min
|
||||
laser_scan.range_max = data.range_max
|
||||
|
||||
# 设置消息头 (Set message header)
|
||||
laser_scan.header.stamp = Clock().now().to_msg() # 当前时间戳 (Current timestamp)
|
||||
laser_scan.header.frame_id = data.header.frame_id # 保持原始坐标系 (Keep original frame)
|
||||
|
||||
# 设置激光雷达参数 (Set laser scan parameters)
|
||||
laser_scan.angle_increment = data.angle_increment * self.multiple # 角度增量乘以降采样倍数 (Angle increment multiplied by downsampling factor)
|
||||
laser_scan.time_increment = data.time_increment # 时间增量保持不变 (Time increment remains unchanged)
|
||||
laser_scan.intensities = data.intensities # 强度数据保持不变 (Intensity data remains unchanged)
|
||||
laser_scan.scan_time = data.scan_time # 扫描时间保持不变 (Scan time remains unchanged)
|
||||
laser_scan.angle_min = data.angle_min # 最小角度保持不变 (Minimum angle remains unchanged)
|
||||
laser_scan.angle_max = data.angle_max # 最大角度保持不变 (Maximum angle remains unchanged)
|
||||
laser_scan.range_min = data.range_min # 最小距离保持不变 (Minimum range remains unchanged)
|
||||
laser_scan.range_max = data.range_max # 最大距离保持不变 (Maximum range remains unchanged)
|
||||
|
||||
# 调试输出 (Debug output)
|
||||
# self.get_logger().info("len(np.array(data.ranges)) = {}".format(len(np.array(data.ranges))))
|
||||
|
||||
# 降采样处理 (Downsampling processing)
|
||||
for i in range(len(np.array(data.ranges))):
|
||||
if i % self.multiple == 0: laser_scan.ranges.append(data.ranges[i])
|
||||
# 每隔multiple个点采样一次 (Sample every 'multiple' points)
|
||||
if i % self.multiple == 0:
|
||||
laser_scan.ranges.append(data.ranges[i])
|
||||
|
||||
# 发布降采样后的数据 (Publish downsampled data)
|
||||
self.pub.publish(laser_scan)
|
||||
|
||||
def main():
|
||||
"""
|
||||
主函数
|
||||
Main function
|
||||
|
||||
初始化ROS节点并启动激光雷达数据压缩功能
|
||||
Initialize ROS node and start laser scan data compression functionality
|
||||
"""
|
||||
rclpy.init()
|
||||
scan_cp = scan_compression("scan_dilute")
|
||||
print("Laser scan compression node started")
|
||||
rclpy.spin(scan_cp)
|
||||
scan_cp.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
Loading…
Reference in new issue