Merge pull request #119 from Autumn60/feature/livoxMsgPublisher

Feature/livox msg publisher

Assets/UnitySensorsROS/Runtime/Scripts/Data/Livox/CustomPoint.cs

@@ -0,0 +1,29 @@
+using RosMessageTypes.Livox;
+
+namespace UnitySensors.ROS.Data.Livox
+{
+    public struct CustomPoint
+    {
+        public uint offset_time;
+        public float x;
+        public float y;
+        public float z;
+        public byte reflectivity;
+        public byte tag;
+        public byte line;
+
+        public CustomPointMsg ConvertToMsg()
+        {
+            return new CustomPointMsg()
+            {
+                offset_time = offset_time,
+                x = x,
+                y = y,
+                z = z,
+                reflectivity = reflectivity,
+                tag = tag,
+                line = line
+            };
+        }
+    }
+}

Assets/UnitySensorsROS/Runtime/Scripts/Messages/Livox/msg/CustomMsgMsg.cs

@@ -0,0 +1,95 @@
+//Do not edit! This file was generated by Unity-ROS MessageGeneration.
+using System;
+using System.Linq;
+using System.Collections.Generic;
+using System.Text;
+using Unity.Robotics.ROSTCPConnector.MessageGeneration;
+using RosMessageTypes.Std;
+
+namespace RosMessageTypes.Livox
+{
+    [Serializable]
+    public class CustomMsgMsg : Message
+    {
+        public const string k_RosMessageName = "Livox/CustomMsg";
+        public override string RosMessageName => k_RosMessageName;
+
+        //  Livox publish pointcloud msg format.
+        public HeaderMsg header;
+        //  ROS standard message header
+        public ulong timebase;
+        //  The time of first point
+        public uint point_num;
+        //  Total number of pointclouds
+        public byte lidar_id;
+        //  Lidar device id number
+        public byte[] rsvd;
+        //  Reserved use
+        public CustomPointMsg[] points;
+        //  Pointcloud data
+
+        public CustomMsgMsg()
+        {
+            this.header = new HeaderMsg();
+            this.timebase = 0;
+            this.point_num = 0;
+            this.lidar_id = 0;
+            this.rsvd = new byte[3];
+            this.points = new CustomPointMsg[0];
+        }
+
+        public CustomMsgMsg(HeaderMsg header, ulong timebase, uint point_num, byte lidar_id, byte[] rsvd, CustomPointMsg[] points)
+        {
+            this.header = header;
+            this.timebase = timebase;
+            this.point_num = point_num;
+            this.lidar_id = lidar_id;
+            this.rsvd = rsvd;
+            this.points = points;
+        }
+
+        public static CustomMsgMsg Deserialize(MessageDeserializer deserializer) => new CustomMsgMsg(deserializer);
+
+        private CustomMsgMsg(MessageDeserializer deserializer)
+        {
+            this.header = HeaderMsg.Deserialize(deserializer);
+            deserializer.Read(out this.timebase);
+            deserializer.Read(out this.point_num);
+            deserializer.Read(out this.lidar_id);
+            deserializer.Read(out this.rsvd, sizeof(byte), 3);
+            deserializer.Read(out this.points, CustomPointMsg.Deserialize, deserializer.ReadLength());
+        }
+
+        public override void SerializeTo(MessageSerializer serializer)
+        {
+            serializer.Write(this.header);
+            serializer.Write(this.timebase);
+            serializer.Write(this.point_num);
+            serializer.Write(this.lidar_id);
+            serializer.Write(this.rsvd);
+            serializer.WriteLength(this.points);
+            serializer.Write(this.points);
+        }
+
+        public override string ToString()
+        {
+            return "CustomMsgMsg: " +
+            "\nheader: " + header.ToString() +
+            "\ntimebase: " + timebase.ToString() +
+            "\npoint_num: " + point_num.ToString() +
+            "\nlidar_id: " + lidar_id.ToString() +
+            "\nrsvd: " + System.String.Join(", ", rsvd.ToList()) +
+            "\npoints: " + System.String.Join(", ", points.ToList());
+        }
+
+#if UNITY_EDITOR
+        [UnityEditor.InitializeOnLoadMethod]
+#else
+        [UnityEngine.RuntimeInitializeOnLoadMethod]
+#endif
+        public static void Register()
+        {
+            MessageRegistry.Register(k_RosMessageName, Deserialize);
+        }
+    }
+}

Assets/UnitySensorsROS/Runtime/Scripts/Messages/Livox/msg/CustomPointMsg.cs

@@ -0,0 +1,100 @@
+//Do not edit! This file was generated by Unity-ROS MessageGeneration.
+using System;
+using System.Linq;
+using System.Collections.Generic;
+using System.Text;
+using Unity.Robotics.ROSTCPConnector.MessageGeneration;
+
+namespace RosMessageTypes.Livox
+{
+    [Serializable]
+    public class CustomPointMsg : Message
+    {
+        public const string k_RosMessageName = "Livox/CustomPoint";
+        public override string RosMessageName => k_RosMessageName;
+
+        //  Livox costom pointcloud format.
+        public uint offset_time;
+        //  offset time relative to the base time
+        public float x;
+        //  X axis, unit:m
+        public float y;
+        //  Y axis, unit:m
+        public float z;
+        //  Z axis, unit:m
+        public byte reflectivity;
+        //  reflectivity, 0~255
+        public byte tag;
+        //  livox tag
+        public byte line;
+        //  laser number in lidar
+
+        public CustomPointMsg()
+        {
+            this.offset_time = 0;
+            this.x = 0.0f;
+            this.y = 0.0f;
+            this.z = 0.0f;
+            this.reflectivity = 0;
+            this.tag = 0;
+            this.line = 0;
+        }
+
+        public CustomPointMsg(uint offset_time, float x, float y, float z, byte reflectivity, byte tag, byte line)
+        {
+            this.offset_time = offset_time;
+            this.x = x;
+            this.y = y;
+            this.z = z;
+            this.reflectivity = reflectivity;
+            this.tag = tag;
+            this.line = line;
+        }
+
+        public static CustomPointMsg Deserialize(MessageDeserializer deserializer) => new CustomPointMsg(deserializer);
+
+        private CustomPointMsg(MessageDeserializer deserializer)
+        {
+            deserializer.Read(out this.offset_time);
+            deserializer.Read(out this.x);
+            deserializer.Read(out this.y);
+            deserializer.Read(out this.z);
+            deserializer.Read(out this.reflectivity);
+            deserializer.Read(out this.tag);
+            deserializer.Read(out this.line);
+        }
+
+        public override void SerializeTo(MessageSerializer serializer)
+        {
+            serializer.Write(this.offset_time);
+            serializer.Write(this.x);
+            serializer.Write(this.y);
+            serializer.Write(this.z);
+            serializer.Write(this.reflectivity);
+            serializer.Write(this.tag);
+            serializer.Write(this.line);
+        }
+
+        public override string ToString()
+        {
+            return "CustomPointMsg: " +
+            "\noffset_time: " + offset_time.ToString() +
+            "\nx: " + x.ToString() +
+            "\ny: " + y.ToString() +
+            "\nz: " + z.ToString() +
+            "\nreflectivity: " + reflectivity.ToString() +
+            "\ntag: " + tag.ToString() +
+            "\nline: " + line.ToString();
+        }
+
+#if UNITY_EDITOR
+        [UnityEditor.InitializeOnLoadMethod]
+#else
+        [UnityEngine.RuntimeInitializeOnLoadMethod]
+#endif
+        public static void Register()
+        {
+            MessageRegistry.Register(k_RosMessageName, Deserialize);
+        }
+    }
+}

Assets/UnitySensorsROS/Runtime/Scripts/Publishers/LiDAR/DepthBufferLiDARLivoxCustomMsgPublisher.cs

@@ -0,0 +1,12 @@
+using UnityEngine;
+using UnitySensors.Data.PointCloud;
+using UnitySensors.Sensor.LiDAR;
+using UnitySensors.ROS.Publisher.PointCloud;
+
+namespace UnitySensors.ROS.Publisher.LiDAR
+{
+    [RequireComponent(typeof(DepthBufferLiDARSensor))]
+    public class DepthBufferLiDARLivoxCustomMsgPublisher : LivoxCustomMsgPublisher<DepthBufferLiDARSensor, PointXYZI>
+    {
+    }
+}

Assets/UnitySensorsROS/Runtime/Scripts/Publishers/LiDAR/RaycastLiDARLivoxCustomMsgPublisher.cs

@@ -0,0 +1,12 @@
+using UnityEngine;
+using UnitySensors.Data.PointCloud;
+using UnitySensors.Sensor.LiDAR;
+using UnitySensors.ROS.Publisher.PointCloud;
+
+namespace UnitySensors.ROS.Publisher.LiDAR
+{
+    [RequireComponent(typeof(RaycastLiDARSensor))]
+    public class RaycastLiDARLivoxCustomMsgPublisher : LivoxCustomMsgPublisher<RaycastLiDARSensor, PointXYZI>
+    {
+    }
+}

Assets/UnitySensorsROS/Runtime/Scripts/Publishers/PointCloud/LivoxCustomMsgPublisher.cs

@@ -0,0 +1,19 @@
+using UnityEngine;
+using Unity.Robotics.ROSTCPConnector.MessageGeneration;
+using RosMessageTypes.Livox;
+using UnitySensors.Data.PointCloud;
+using UnitySensors.Sensor;
+using UnitySensors.ROS.Serializer.PointCloud;
+
+namespace UnitySensors.ROS.Publisher.PointCloud
+{
+    public class LivoxCustomMsgPublisher<T, TT> : RosMsgPublisher<T,LivoxCustomMsgSerializer<T, TT>, CustomMsgMsg>
+        where T : UnitySensor, IPointCloudInterface<TT>
+        where TT : struct, IPointXYZInterface
+    {
+        private void OnDestroy()
+        {
+            _serializer.Dispose();
+        }
+    }
+}