发送固定点云消息
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2
import std_msgs.msg
import time
mport numpy as np
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2
from sensor_msgs.msg import PointField
from sensor_msgs_py import point_cloud2
from std_msgs.msg import Header
import time
class PointCloudPublisher(Node):
rate = 30
moving = True
width = 100
height = 100
header = Header()
header.frame_id = 'map'
dtype = PointField.FLOAT32
point_step = 16
fields = [PointField(name='x', offset=0, datatype=dtype, count=1),
PointField(name='y', offset=4, datatype=dtype, count=1),
PointField(name='z', offset=8, datatype=dtype, count=1),
PointField(name='intensity', offset=12, datatype=dtype, count=1)]
def __init__(self):
super().__init__('pc_publisher')
self.publisher_ = self.create_publisher(PointCloud2, 'test_cloud', 10)
timer_period = 1 / self.rate
self.timer = self.create_timer(timer_period, self.timer_callback)
self.counter = 0
self.c = 0
def timer_callback(self):
self.header.stamp = self.get_clock().now().to_msg()
# x, y = np.meshgrid(np.linspace(-2, 2, self.width), np.linspace(-2, 2, self.height))
# z = 0.5 * np.sin(2*x-self.counter/10.0) * np.sin(2*y)
# print(z)
x = [2, 2, 2,2.01,2.02,2.052,2.0 ,2.0 , 2.0 ]
y = [-0.5,-0.6,-0.7,-0.8, -0.9,-0.95 ,-1.0 ,-1.1, -1.2]
z = [0, 0, 0,0, 0, 0 ,0 ,0 , 0 ]
# if self.c < 12:
# time.sleep(1);
# x = 1
# y = 1
# z = 0
# self.c = self.c+1
# elif self.c < 23:
# # time.sleep(1);
# x = 1
# y = 2.5
# z = 0
# self.c = self.c + 1
# else:
# x = 1
# y = 1
# z = 0
# self.c = 0
points = np.array([x, y, z, z]).reshape(4, -1).T
pc2_msg = point_cloud2.create_cloud(self.header, self.fields, points)
print("I publish message points size is self c is ",self.c)
# time.sleep(0.2)
self.publisher_.publish(pc2_msg)
if self.moving:
self.counter += 1
def main(args=None):
rclpy.init(args=args)
pc_publisher = PointCloudPublisher()
rclpy.spin(pc_publisher)
pc_publisher.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
接收并转发点云数据，赋予当前系统时间戳
class CloudSubscriber(Node):
def __init__(self):
super().__init__('cloud_subscriber')
self.subscription = self.create_subscription(
PointCloud2,
'input_cloud_topic', # 输入的点云消息话题
self.cloud_callback,
10
)
self.publisher = self.create_publisher(
PointCloud2,
'test_cloud', # 输出的点云消息话题
10
)
def cloud_callback(self, msg):
# 将时间戳改为当前接收到的时间戳
msg.header.stamp = self.get_clock().now().to_msg()
# 发布修改后的消息
self.publisher.publish(msg)
def main(args=None):
rclpy.init(args=args)
node = CloudSubscriber()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
将PointCloud2转成LaserScan
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2, LaserScan
from sensor_msgs.msg import PointField
from sensor_msgs_py import point_cloud2
import math
class PointCloudToScanNode(Node):
def __init__(self):
super().__init__('pointcloud_to_scan_node')
self.subscription = self.create_subscription(
PointCloud2,
'mos_point_cloud', # 输入PointCloud2的话题名称
self.pointcloud_callback,
10
)
self.publisher = self.create_publisher(
LaserScan,
'scan', # 输出LaserScan的话题名称
10
)
def pointcloud_callback(self, msg):
# 创建LaserScan消息对象
scan_msg = LaserScan()
# 设置LaserScan的相关参数
scan_msg.header = msg.header
scan_msg.header.frame_id = "laser_link"
scan_msg.header.stamp = self.get_clock().now().to_msg()
scan_msg.angle_min = -1.57079632 # 激光扫描的最小角度
scan_msg.angle_max = 1.57079632 # 激光扫描的最大角度
# // scan_msg_h.angle_increment = (scan_msg_h.angle_max-scan_msg_h.angle_min)/(scan_count-1);
# // scan_msg_h.scan_time = 0.02;//1.0/publishHz;
scan_msg.angle_increment = 0.0174532925199 # 角度的增量，这里设置为1度
scan_msg.time_increment = 0.0007936507936507937 # 时间增量
scan_msg.range_min = 0.1
scan_msg.range_max = 3.0
scan_msg.scan_time = 0.14
# 解析PointCloud2数据
points = list(point_cloud2.read_points(msg))
ranges = []
# 遍历每个角度
for angle in range(-90, 91):
# 将角度转换为弧度
angle_rad = math.radians(angle)
# 检查是否有对应角度的点云数据
found = False
for point in points:
# 计算点的角度
point_angle = math.atan2(point[1], point[0])
if math.isclose(point_angle, angle_rad, abs_tol=0.0174532925199): # 容忍度为1度
# 计算激光扫描的距离值
distance = (point[0] ** 2 + point[1] ** 2 + point[2] ** 2) ** 0.5
ranges.append(distance)
found = True
break
# 如果没有找到对应的数据，则填充为0
if not found:
ranges.append(0.0)
# ranges = []
# # for i in range(180):
# # ranges.append(1.0)
# for point in points:
# # 计算激光扫描的距离值
# distance = (point[0] ** 2 + point[1] ** 2 + point[2] ** 2) ** 0.5
# ranges.append(distance)
# 设置LaserScan的数据
scan_msg.ranges = ranges
print("发布成功!!!",len(scan_msg.ranges))
# 发布转换后的LaserScan消息
self.publisher.publish(scan_msg)
def main(args=None):
rclpy.init(args=args)
node = PointCloudToScanNode()
rclpy.spin(node)
rclpy.shutdown()
if __name__ == '__main__':
main()