79 lines
2.8 KiB
C#
79 lines
2.8 KiB
C#
using System;
|
|
using System.Runtime.InteropServices;
|
|
using ROS2;
|
|
using Unity.Collections;
|
|
using Unity.Collections.LowLevel.Unsafe;
|
|
using UnityEngine;
|
|
|
|
namespace ZC
|
|
{
|
|
[RequireComponent(typeof(ROS2UnityComponent))]
|
|
public class PointsPublisher : MonoBehaviour
|
|
{
|
|
public static PointsPublisher pointPublisher;
|
|
private ROS2UnityComponent ros2Unity;
|
|
private ROS2Node ros2Node;
|
|
private IPublisher<sensor_msgs.msg.PointCloud2> chatter_pub;
|
|
private NativeArray<ZPointInfo> _pointInfos;
|
|
|
|
private void Awake()
|
|
{
|
|
pointPublisher = this;
|
|
}
|
|
|
|
void Start()
|
|
{
|
|
ros2Unity = GetComponent<ROS2UnityComponent>();
|
|
}
|
|
|
|
void Update()
|
|
{
|
|
if (ros2Unity.Ok())
|
|
{
|
|
if (ros2Node == null)
|
|
{
|
|
ros2Node = ros2Unity.CreateNode("ROS2UnityTalkerPointCloudNode");
|
|
chatter_pub = ros2Node.CreatePublisher<sensor_msgs.msg.PointCloud2>("chatter_PointCloud");
|
|
}
|
|
}
|
|
}
|
|
|
|
public void PushData(NativeArray<ZPointInfo> pointInfos, int hitCount, Vector3 position, Quaternion rotation, Bounds bounds)
|
|
{
|
|
this._pointInfos = pointInfos;
|
|
unsafe
|
|
{
|
|
Debug.Log(111);
|
|
sensor_msgs.msg.PointCloud2 msg = new sensor_msgs.msg.PointCloud2();
|
|
msg.Point_step = 16;
|
|
msg.Data = new byte[this._pointInfos.Length * msg.Point_step + sizeof(MessageHeader)];
|
|
msg.Width = 1;
|
|
msg.Row_step = 16;
|
|
msg.Height = (uint)(this._pointInfos.Length + sizeof(MessageHeader)/msg.Point_step);
|
|
var arrayPtr = UnsafeUtility.PinGCArrayAndGetDataAddress(msg.Data, out var handle);
|
|
try
|
|
{
|
|
MessageHeader messageHeader = new MessageHeader() { hitCount = hitCount, position = position, bounds = bounds, rotation = rotation };
|
|
void* ptr = &messageHeader;
|
|
UnsafeUtility.MemCpy(arrayPtr, ptr, sizeof(MessageHeader));
|
|
UnsafeUtility.MemCpy((void*)((nint)arrayPtr + sizeof(MessageHeader)), NativeArrayUnsafeUtility.GetUnsafeBufferPointerWithoutChecks(this._pointInfos), 150000 * 16);
|
|
}
|
|
finally
|
|
{
|
|
UnsafeUtility.ReleaseGCObject(handle);
|
|
}
|
|
|
|
this.chatter_pub.Publish(msg);
|
|
}
|
|
}
|
|
}
|
|
|
|
[StructLayout(LayoutKind.Explicit)]
|
|
public struct MessageHeader
|
|
{
|
|
[FieldOffset(0)] public int hitCount;
|
|
[FieldOffset(4)] public Vector3 position;
|
|
[FieldOffset(16)] public Bounds bounds;
|
|
[FieldOffset(48)] public Quaternion rotation;
|
|
}
|
|
} |