working 3d lidar publisher

Author: micahpearlman

Runtime/Scripts/ROS/MessageTypes/Sensor/ZOROSSensorMessages.cs

@@ -144,6 +144,237 @@ public LaserScanMessage(HeaderMessage header, float angle_min, float angle_max,
         }
     }
 
+
+    /// <summary>
+    /// This message holds the description of one point entry in the
+    /// PointCloud2 message format.
+    /// uint8 INT8    = 1
+    /// uint8 UINT8   = 2
+    /// uint8 INT16   = 3
+    /// uint8 UINT16  = 4
+    /// uint8 INT32   = 5
+    /// uint8 UINT32  = 6
+    /// uint8 FLOAT32 = 7
+    /// uint8 FLOAT64 = 8
+    /// </summary>
+    public class PointFieldMessage : ZOROSMessageInterface {
+
+        [Newtonsoft.Json.JsonIgnore]
+        public string MessageType { get { return PointFieldMessage.Type; } }
+
+        [Newtonsoft.Json.JsonIgnore]
+        public static string Type = "sensor_msgs/PointField";
+
+
+        /// <summary>
+        /// name of field.
+        /// </summary>
+        /// <value></value>
+        public string name { get; set; }
+
+        /// <summary>
+        /// Offset from start of point struct
+        /// </summary>
+        /// <value></value>
+        public uint offset { get; set; }
+
+        /// <summary>
+        /// Datatype enumeration.
+        /// uint8 INT8    = 1
+        /// uint8 UINT8   = 2
+        /// uint8 INT16   = 3
+        /// uint8 UINT16  = 4
+        /// uint8 INT32   = 5
+        /// uint8 UINT32  = 6
+        /// uint8 FLOAT32 = 7
+        /// uint8 FLOAT64 = 8
+        /// </summary>
+        /// <value></value>
+        public byte datatype { get; set; }
+
+        /// <summary>
+        /// How many elements in the field
+        /// </summary>
+        /// <value></value>
+        public uint count { get; set; }
+
+        public PointFieldMessage() {
+            this.name = "";
+            this.offset = 0;
+            this.datatype = 1;
+            this.count = 0;
+        }
+
+        public PointFieldMessage(string name, uint offset, byte datatype, uint count) {
+            this.name = name;
+            this.offset = offset;
+            this.datatype = datatype;
+            this.count = count;
+        }
+
+        public static PointFieldMessage[] CreateXYZPointFieldArray(uint count) {
+            PointFieldMessage[] pointFieldMessages = new PointFieldMessage[3];
+
+            // x
+            pointFieldMessages[0] = new PointFieldMessage();
+            pointFieldMessages[0].name = "x";
+            pointFieldMessages[0].offset = 0;
+            pointFieldMessages[0].datatype = 7; // float32
+            pointFieldMessages[0].count = count;
+
+            // y
+            pointFieldMessages[1] = new PointFieldMessage();
+            pointFieldMessages[1].name = "y";
+            pointFieldMessages[1].offset = 4;
+            pointFieldMessages[1].datatype = 7; // float32
+            pointFieldMessages[1].count = count;
+
+            // z
+            pointFieldMessages[2] = new PointFieldMessage();
+            pointFieldMessages[2].name = "z";
+            pointFieldMessages[2].offset = 8;
+            pointFieldMessages[2].datatype = 7; // float32
+            pointFieldMessages[2].count = count;
+
+            return pointFieldMessages;
+
+        }
+
+    }
+
+    /// <summary>
+    /// This message holds a collection of N-dimensional points, which may
+    /// contain additional information such as normals, intensity, etc. The
+    /// point data is stored as a binary blob, its layout described by the
+    /// contents of the "fields" array.
+    /// 
+    /// The point cloud data may be organized 2d (image-like) or 1d
+    /// (unordered). Point clouds organized as 2d images may be produced by
+    /// camera depth sensors such as stereo or time-of-flight.
+    /// </summary>
+    public class PointCloud2Message : ZOROSMessageInterface {
+
+        [Newtonsoft.Json.JsonIgnore]
+        public string MessageType { get { return PointCloud2Message.Type; } }
+
+        [Newtonsoft.Json.JsonIgnore]
+        public static string Type = "sensor_msgs/PointCloud2";
+
+
+        /// <summary>
+        /// Time of sensor data acquisition, and the coordinate frame ID (for 3d points).
+        /// </summary>
+        /// <value></value>
+        public HeaderMessage header { get; set; }
+
+
+        /// <summary>
+        /// 2D structure of the point cloud. If the cloud is unordered, height is
+        /// 1 and width is the length of the point cloud.
+        /// </summary>
+        /// <value></value>
+        public uint height { get; set; }
+        public uint width { get; set; }
+
+        /// <summary>
+        /// Describes the channels and their layout in the binary data blob.
+        /// </summary>
+        /// <value></value>
+        public PointFieldMessage[] fields { get; set; }
+
+        /// <summary>
+        /// Is this data bigendian?
+        /// </summary>
+        /// <value></value>
+        public bool is_bigendian { get; set; }
+
+        /// <summary>
+        /// Length of a point in bytes
+        /// </summary>
+        /// <value></value>
+        public uint point_step { get; set; }
+
+        /// <summary>
+        /// Length of a row in bytes
+        /// </summary>
+        /// <value></value>
+        public uint row_step { get; set; }
+
+        /// <summary>
+        /// Actual point data, size is (row_step*height)
+        /// </summary>
+        /// <value></value>
+        public byte[] data { get; set; }
+
+        /// <summary>
+        /// True if there are no invalid points
+        /// </summary>
+        /// <value></value>
+        public bool is_dense { get; set; }
+
+        public PointCloud2Message() {
+            this.header = new HeaderMessage();
+            this.height = 0;
+            this.width = 0;
+            this.fields = new PointFieldMessage[0];
+            this.is_bigendian = false;
+            this.point_step = 0;
+            this.row_step = 0;
+            this.data = new byte[0];
+            this.is_dense = true;
+
+        }
+
+        public PointCloud2Message(HeaderMessage header, uint height, uint width, PointFieldMessage[] pointFieldMessages, bool is_bigendian, uint point_step, uint row_step, byte[] data, bool is_dense) {
+            this.header = header;
+            this.height = height;
+            this.width = width;
+            this.fields = pointFieldMessages;
+            this.is_bigendian = is_bigendian;
+            this.point_step = point_step;
+            this.row_step = row_step;
+            this.data = data;
+            this.is_dense = is_dense;
+
+        }
+
+        public void Update() {
+            this.header.Update();
+        }
+
+        public static PointCloud2Message CreateXYZPointCloud(UnityEngine.Vector3[] pointsInUnityCoordinates) {
+            byte[] byteData = new byte[sizeof(float) * 3 * pointsInUnityCoordinates.Length];
+
+            // public void FromUnityVector3(UnityEngine.Vector3 v) {
+            // this.x = (double)v.z;
+            // this.y = -(double)v.x;
+            // this.z = (double)v.y;
+            // }
+
+            int byteDataOffset = 0;
+            foreach (UnityEngine.Vector3 v in pointsInUnityCoordinates) {
+                Buffer.BlockCopy(BitConverter.GetBytes(v.z), 0, byteData, byteDataOffset + (0 * sizeof(float)), sizeof(float));
+                Buffer.BlockCopy(BitConverter.GetBytes(-v.x), 0, byteData, byteDataOffset + (1 * sizeof(float)), sizeof(float));
+                Buffer.BlockCopy(BitConverter.GetBytes(v.y), 0, byteData, byteDataOffset + (2 * sizeof(float)), sizeof(float));
+                byteDataOffset += (3 * sizeof(float));
+            }
+
+            PointCloud2Message pointCloud2Message = new PointCloud2Message(new HeaderMessage(),
+                                                                            height: 1,
+                                                                            width: (uint)pointsInUnityCoordinates.Length,
+                                                                            pointFieldMessages: PointFieldMessage.CreateXYZPointFieldArray((uint)pointsInUnityCoordinates.Length),
+                                                                            is_bigendian: false,
+                                                                            point_step: sizeof(float) * 3,
+                                                                            row_step: sizeof(float) * 12 * (uint)pointsInUnityCoordinates.Length,
+                                                                            byteData,
+                                                                            false);
+
+            return pointCloud2Message;
+        }
+
+
+    }
+
     /// <summary>
     /// This is a message that holds data to describe the state of a set of torque controlled joints. 
     /// 

Runtime/Scripts/ROS/Unity/Publishers/ZOROSPointCloud2Publisher.cs

@@ -0,0 +1,136 @@
+using System.Threading.Tasks;
+using UnityEngine;
+using Unity.Collections;
+using Newtonsoft.Json.Linq;
+using ZO.ROS.MessageTypes.Sensor;
+using ZO.ROS.MessageTypes.Geometry;
+using ZO.ROS.Publisher;
+using ZO.Sensors;
+using ZO.ROS.Unity;
+using ZO.Document;
+
+
+namespace ZO.ROS.Publisher {
+
+    /// <summary>
+    /// Publish ROS /sensor/LaserScan message using the ZOLIDAR2D sensor.
+    /// See: http://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/LaserScan.html
+    /// </summary>
+    [RequireComponent(typeof(ZOROSTransformPublisher))]
+    [RequireComponent(typeof(ZOLIDAR3D))]
+    public class ZOROSPointCloud2Publisher : ZOROSUnityGameObjectBase {
+
+        public ZOLIDAR3D _lidar3DSensor;
+
+        /// <summary>
+        /// The 2D LIDAR sensor to publish it's scan data.
+        /// </summary>
+        /// <value></value>
+        public ZOLIDAR3D LIDAR3DSensor {
+            get => _lidar3DSensor;
+            set => _lidar3DSensor = value;
+        }
+
+        public string _parentTransformId;
+
+
+        private ZOROSTransformPublisher _transformPublisher = null;
+        public ZOROSTransformPublisher TransformPublisher {
+            get {
+                if (_transformPublisher == null) {
+                    _transformPublisher = GetComponent<ZOROSTransformPublisher>();
+                }
+
+                if (_transformPublisher == null) {
+                    Debug.LogError("ERROR: ZOROSLaserScanPublisher is missing corresponding ZOROSTransformPublisher");
+                }
+                return _transformPublisher;
+            }
+        }
+
+        private PointCloud2Message _pointCloud2Message = new PointCloud2Message();
+
+
+        protected override void ZOStart() {
+            base.ZOStart();
+            if (ZOROSBridgeConnection.Instance.IsConnected) {
+                Initialize();
+            }
+        }
+
+        protected override void ZOOnValidate() {
+            base.ZOOnValidate();
+            if (LIDAR3DSensor == null) {
+                LIDAR3DSensor = GetComponent<ZOLIDAR3D>();
+            }
+
+            if (ROSTopic == "") {
+                ROSTopic = "point_cloud";
+            }
+
+            if (UpdateRateHz == 0) {
+                UpdateRateHz = 10;
+            }
+
+            if (_parentTransformId == "" || _parentTransformId == null) {
+                if (transform.parent != null) {
+                    ZOROSTransformPublisher parentTransformPublisher = transform.parent.GetComponent<ZOROSTransformPublisher>();
+                    if (parentTransformPublisher != null) {
+                        _parentTransformId = parentTransformPublisher.ChildFrameID;
+                    } else {
+                        _parentTransformId = ZOROSUnityManager.Instance.WorldFrameId;
+                    }
+
+                } else {
+                    _parentTransformId = ZOROSUnityManager.Instance.WorldFrameId;
+                }
+            }
+        }
+
+        private void Initialize() {
+            // advertise
+            ROSBridgeConnection.Advertise(ROSTopic, _pointCloud2Message.MessageType);
+
+            // hookup to the sensor update delegate
+            _lidar3DSensor.OnPublishDelegate = OnPublishLidarScanDelegate;
+
+        }
+
+
+        protected override void ZOOnDestroy() {
+            ROSBridgeConnection?.UnAdvertise(ROSTopic);
+        }
+
+        public override void OnROSBridgeConnected(ZOROSUnityManager rosUnityManager) {
+            Debug.Log("INFO: ZOROSLaserScanPublisher::OnROSBridgeConnected");
+            Initialize();
+        }
+
+        public override void OnROSBridgeDisconnected(ZOROSUnityManager rosUnityManager) {
+            Debug.Log("INFO: ZOROSLaserScanPublisher::OnROSBridgeDisconnected");
+            ROSBridgeConnection.UnAdvertise(ROSTopic);
+        }
+
+        private Task OnPublishLidarScanDelegate(ZOLIDAR3D lidar, string name, NativeArray<Vector3> positions, NativeArray<Vector3> normals) {
+            _pointCloud2Message = PointCloud2Message.CreateXYZPointCloud(positions.ToArray());
+            _pointCloud2Message.header.Update();
+            _pointCloud2Message.header.frame_id = _parentTransformId;
+
+
+            ROSBridgeConnection.Publish(_pointCloud2Message, ROSTopic, Name);
+
+            return Task.CompletedTask;
+        }
+
+        #region ZOSerializationInterface
+        public override string Type {
+            get { return "ros.publisher.point_cloud2"; }
+        }
+
+
+
+        #endregion // ZOSerializationInterface
+
+    }
+
+}