diff --git a/Assets/RosSharp/Plugins/RosBridgeClient.dll b/Assets/RosSharp/Plugins/RosBridgeClient.dll
index 4356524..1a240c2 100644
--- a/Assets/RosSharp/Plugins/RosBridgeClient.dll
+++ b/Assets/RosSharp/Plugins/RosBridgeClient.dll
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
-oid sha256:182571819f6ba629abae3786e9d972dc699ea06ed20f97145ddf78cae0681dc0
-size 91136
+oid sha256:c47bc979dc9ff299f1968365afadb69b71105e944077141195912f7f430457c7
+size 91648
diff --git a/Assets/RosSharpMessages.meta b/Assets/RosSharpMessages.meta
new file mode 100644
index 0000000..943d084
--- /dev/null
+++ b/Assets/RosSharpMessages.meta
@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 18dcb830630cd5643ab12684fd6d5ecf
+folderAsset: yes
+DefaultImporter:
+ externalObjects: {}
+ userData:
+ assetBundleName:
+ assetBundleVariant:
diff --git a/Assets/RosSharpMessages/Velodyne.meta b/Assets/RosSharpMessages/Velodyne.meta
new file mode 100644
index 0000000..8e10112
--- /dev/null
+++ b/Assets/RosSharpMessages/Velodyne.meta
@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 979320030887c5142bb0459cdab2992e
+folderAsset: yes
+DefaultImporter:
+ externalObjects: {}
+ userData:
+ assetBundleName:
+ assetBundleVariant:
diff --git a/Assets/RosSharpMessages/Velodyne/msg.meta b/Assets/RosSharpMessages/Velodyne/msg.meta
new file mode 100644
index 0000000..66830bd
--- /dev/null
+++ b/Assets/RosSharpMessages/Velodyne/msg.meta
@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: b49431a0ace1e904e929453828d12e8d
+folderAsset: yes
+DefaultImporter:
+ externalObjects: {}
+ userData:
+ assetBundleName:
+ assetBundleVariant:
diff --git a/Assets/RosSharpMessages/Velodyne/msg/VelodynePacket.cs b/Assets/RosSharpMessages/Velodyne/msg/VelodynePacket.cs
new file mode 100644
index 0000000..d71e9ec
--- /dev/null
+++ b/Assets/RosSharpMessages/Velodyne/msg/VelodynePacket.cs
@@ -0,0 +1,39 @@
+/*
+ * This message is auto generated by ROS#. Please DO NOT modify.
+ * Note:
+ * - Comments from the original code will be written in their own line
+ * - Variable sized arrays will be initialized to array of size 0
+ * Please report any issues at
+ * <https://github.com/siemens/ros-sharp>
+ */
+
+using Newtonsoft.Json;
+
+using RosSharp.RosBridgeClient.MessageTypes.Std;
+
+namespace RosSharp.RosBridgeClient.MessageTypes.Velodyne
+{
+ public class VelodynePacket : Message
+ {
+ [JsonIgnore]
+ public const string RosMessageName = "velodyne_msgs/VelodynePacket";
+
+ // Raw Velodyne LIDAR packet.
+ public Time stamp;
+ // packet timestamp
+ public byte[] data;
+ // packet contents
+
+ public VelodynePacket()
+ {
+ this.stamp = new Time();
+ this.data = new byte[1206];
+ }
+
+ public VelodynePacket(Time stamp, byte[] data)
+ {
+ this.stamp = stamp;
+ this.data = data;
+ }
+ }
+}
diff --git a/Assets/RosSharpMessages/Velodyne/msg/VelodynePacket.cs.meta b/Assets/RosSharpMessages/Velodyne/msg/VelodynePacket.cs.meta
new file mode 100644
index 0000000..b4e3872
--- /dev/null
+++ b/Assets/RosSharpMessages/Velodyne/msg/VelodynePacket.cs.meta
@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: fd2bbe8188ec8304ea747495ae25fa8f
+MonoImporter:
+ externalObjects: {}
+ serializedVersion: 2
+ defaultReferences: []
+ executionOrder: 0
+ icon: {instanceID: 0}
+ userData:
+ assetBundleName:
+ assetBundleVariant:
diff --git a/Assets/RosSharpMessages/Velodyne/msg/VelodyneScan.cs b/Assets/RosSharpMessages/Velodyne/msg/VelodyneScan.cs
new file mode 100644
index 0000000..762c5b5
--- /dev/null
+++ b/Assets/RosSharpMessages/Velodyne/msg/VelodyneScan.cs
@@ -0,0 +1,39 @@
+/*
+ * This message is auto generated by ROS#. Please DO NOT modify.
+ * Note:
+ * - Comments from the original code will be written in their own line
+ * - Variable sized arrays will be initialized to array of size 0
+ * Please report any issues at
+ * <https://github.com/siemens/ros-sharp>
+ */
+
+using Newtonsoft.Json;
+
+using RosSharp.RosBridgeClient.MessageTypes.Std;
+
+namespace RosSharp.RosBridgeClient.MessageTypes.Velodyne
+{
+ public class VelodyneScan : Message
+ {
+ [JsonIgnore]
+ public const string RosMessageName = "velodyne_msgs/VelodyneScan";
+
+ // Velodyne LIDAR scan packets.
+ public Header header;
+ // standard ROS message header
+ public VelodynePacket[] packets;
+ // vector of raw packets
+
+ public VelodyneScan()
+ {
+ this.header = new Header();
+ this.packets = new VelodynePacket[0];
+ }
+
+ public VelodyneScan(Header header, VelodynePacket[] packets)
+ {
+ this.header = header;
+ this.packets = packets;
+ }
+ }
+}
diff --git a/Assets/RosSharpMessages/Velodyne/msg/VelodyneScan.cs.meta b/Assets/RosSharpMessages/Velodyne/msg/VelodyneScan.cs.meta
new file mode 100644
index 0000000..13146f1
--- /dev/null
+++ b/Assets/RosSharpMessages/Velodyne/msg/VelodyneScan.cs.meta
@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 36a4b28d33b883f42b7cf52bff299e3c
+MonoImporter:
+ externalObjects: {}
+ serializedVersion: 2
+ defaultReferences: []
+ executionOrder: 0
+ icon: {instanceID: 0}
+ userData:
+ assetBundleName:
+ assetBundleVariant:
diff --git a/Assets/Scenes/running.unity b/Assets/Scenes/running.unity
index 0f17f54..422bb26 100644
--- a/Assets/Scenes/running.unity
+++ b/Assets/Scenes/running.unity
@@ -390,6 +390,276 @@ Transform:
m_Father: {fileID: 0}
m_RootOrder: 2
m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!1 &1671675747
+GameObject:
+ m_ObjectHideFlags: 0
+ m_CorrespondingSourceObject: {fileID: 0}
+ m_PrefabInstance: {fileID: 0}
+ m_PrefabAsset: {fileID: 0}
+ serializedVersion: 6
+ m_Component:
+ - component: {fileID: 1671675752}
+ - component: {fileID: 1671675751}
+ - component: {fileID: 1671675750}
+ - component: {fileID: 1671675749}
+ - component: {fileID: 1671675748}
+ m_Layer: 0
+ m_Name: Cube
+ m_TagString: Untagged
+ m_Icon: {fileID: 0}
+ m_NavMeshLayer: 0
+ m_StaticEditorFlags: 0
+ m_IsActive: 1
+--- !u!54 &1671675748
+Rigidbody:
+ m_ObjectHideFlags: 0
+ m_CorrespondingSourceObject: {fileID: 0}
+ m_PrefabInstance: {fileID: 0}
+ m_PrefabAsset: {fileID: 0}
+ m_GameObject: {fileID: 1671675747}
+ serializedVersion: 2
+ m_Mass: 1
+ m_Drag: 0
+ m_AngularDrag: 0.05
+ m_UseGravity: 1
+ m_IsKinematic: 0
+ m_Interpolate: 0
+ m_Constraints: 0
+ m_CollisionDetection: 0
+--- !u!65 &1671675749
+BoxCollider:
+ m_ObjectHideFlags: 0
+ m_CorrespondingSourceObject: {fileID: 0}
+ m_PrefabInstance: {fileID: 0}
+ m_PrefabAsset: {fileID: 0}
+ m_GameObject: {fileID: 1671675747}
+ m_Material: {fileID: 0}
+ m_IsTrigger: 0
+ m_Enabled: 1
+ serializedVersion: 2
+ m_Size: {x: 1, y: 1, z: 1}
+ m_Center: {x: 0, y: 0, z: 0}
+--- !u!23 &1671675750
+MeshRenderer:
+ m_ObjectHideFlags: 0
+ m_CorrespondingSourceObject: {fileID: 0}
+ m_PrefabInstance: {fileID: 0}
+ m_PrefabAsset: {fileID: 0}
+ m_GameObject: {fileID: 1671675747}
+ m_Enabled: 1
+ m_CastShadows: 1
+ m_ReceiveShadows: 1
+ m_DynamicOccludee: 1
+ m_MotionVectors: 1
+ m_LightProbeUsage: 1
+ m_ReflectionProbeUsage: 1
+ m_RayTracingMode: 2
+ m_RenderingLayerMask: 1
+ m_RendererPriority: 0
+ m_Materials:
+ - {fileID: 10303, guid: 0000000000000000f000000000000000, type: 0}
+ m_StaticBatchInfo:
+ firstSubMesh: 0
+ subMeshCount: 0
+ m_StaticBatchRoot: {fileID: 0}
+ m_ProbeAnchor: {fileID: 0}
+ m_LightProbeVolumeOverride: {fileID: 0}
+ m_ScaleInLightmap: 1
+ m_ReceiveGI: 1
+ m_PreserveUVs: 0
+ m_IgnoreNormalsForChartDetection: 0
+ m_ImportantGI: 0
+ m_StitchLightmapSeams: 1
+ m_SelectedEditorRenderState: 3
+ m_MinimumChartSize: 4
+ m_AutoUVMaxDistance: 0.5
+ m_AutoUVMaxAngle: 89
+ m_LightmapParameters: {fileID: 0}
+ m_SortingLayerID: 0
+ m_SortingLayer: 0
+ m_SortingOrder: 0
+--- !u!33 &1671675751
+MeshFilter:
+ m_ObjectHideFlags: 0
+ m_CorrespondingSourceObject: {fileID: 0}
+ m_PrefabInstance: {fileID: 0}
+ m_PrefabAsset: {fileID: 0}
+ m_GameObject: {fileID: 1671675747}
+ m_Mesh: {fileID: 10202, guid: 0000000000000000e000000000000000, type: 0}
+--- !u!4 &1671675752
+Transform:
+ m_ObjectHideFlags: 0
+ m_CorrespondingSourceObject: {fileID: 0}
+ m_PrefabInstance: {fileID: 0}
+ m_PrefabAsset: {fileID: 0}
+ m_GameObject: {fileID: 1671675747}
+ m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
+ m_LocalPosition: {x: -1.31, y: 0.62, z: 0.23}
+ m_LocalScale: {x: 1, y: 1, z: 1}
+ m_Children: []
+ m_Father: {fileID: 0}
+ m_RootOrder: 4
+ m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!1 &1779710246
+GameObject:
+ m_ObjectHideFlags: 0
+ m_CorrespondingSourceObject: {fileID: 0}
+ m_PrefabInstance: {fileID: 0}
+ m_PrefabAsset: {fileID: 0}
+ serializedVersion: 6
+ m_Component:
+ - component: {fileID: 1779710252}
+ - component: {fileID: 1779710251}
+ - component: {fileID: 1779710250}
+ - component: {fileID: 1779710249}
+ - component: {fileID: 1779710248}
+ - component: {fileID: 1779710247}
+ - component: {fileID: 1779710254}
+ - component: {fileID: 1779710253}
+ m_Layer: 0
+ m_Name: Cylinder
+ m_TagString: Untagged
+ m_Icon: {fileID: 0}
+ m_NavMeshLayer: 0
+ m_StaticEditorFlags: 0
+ m_IsActive: 1
+--- !u!114 &1779710247
+MonoBehaviour:
+ m_ObjectHideFlags: 0
+ m_CorrespondingSourceObject: {fileID: 0}
+ m_PrefabInstance: {fileID: 0}
+ m_PrefabAsset: {fileID: 0}
+ m_GameObject: {fileID: 1779710246}
+ m_Enabled: 0
+ m_EditorHideFlags: 0
+ m_Script: {fileID: 11500000, guid: 3f13aa18ca716d841978f7821bcb3921, type: 3}
+ m_Name:
+ m_EditorClassIdentifier:
+--- !u!114 &1779710248
+MonoBehaviour:
+ m_ObjectHideFlags: 0
+ m_CorrespondingSourceObject: {fileID: 0}
+ m_PrefabInstance: {fileID: 0}
+ m_PrefabAsset: {fileID: 0}
+ m_GameObject: {fileID: 1779710246}
+ m_Enabled: 1
+ m_EditorHideFlags: 0
+ m_Script: {fileID: 11500000, guid: 479d6f4dc5ab1e446ab2d632598c7ad5, type: 3}
+ m_Name:
+ m_EditorClassIdentifier:
+ maxAngle: 10
+ minAngle: -10
+ numberOfLayers: 16
+ numberOfIncrements: 360
+ maxRange: 100
+ distances: []
+ azimuts: []
+--- !u!136 &1779710249
+CapsuleCollider:
+ m_ObjectHideFlags: 0
+ m_CorrespondingSourceObject: {fileID: 0}
+ m_PrefabInstance: {fileID: 0}
+ m_PrefabAsset: {fileID: 0}
+ m_GameObject: {fileID: 1779710246}
+ m_Material: {fileID: 0}
+ m_IsTrigger: 0
+ m_Enabled: 1
+ m_Radius: 0.5000001
+ m_Height: 2
+ m_Direction: 1
+ m_Center: {x: 0.000000059604645, y: 0, z: -0.00000008940697}
+--- !u!23 &1779710250
+MeshRenderer:
+ m_ObjectHideFlags: 0
+ m_CorrespondingSourceObject: {fileID: 0}
+ m_PrefabInstance: {fileID: 0}
+ m_PrefabAsset: {fileID: 0}
+ m_GameObject: {fileID: 1779710246}
+ m_Enabled: 1
+ m_CastShadows: 1
+ m_ReceiveShadows: 1
+ m_DynamicOccludee: 1
+ m_MotionVectors: 1
+ m_LightProbeUsage: 1
+ m_ReflectionProbeUsage: 1
+ m_RayTracingMode: 2
+ m_RenderingLayerMask: 1
+ m_RendererPriority: 0
+ m_Materials:
+ - {fileID: 10303, guid: 0000000000000000f000000000000000, type: 0}
+ m_StaticBatchInfo:
+ firstSubMesh: 0
+ subMeshCount: 0
+ m_StaticBatchRoot: {fileID: 0}
+ m_ProbeAnchor: {fileID: 0}
+ m_LightProbeVolumeOverride: {fileID: 0}
+ m_ScaleInLightmap: 1
+ m_ReceiveGI: 1
+ m_PreserveUVs: 0
+ m_IgnoreNormalsForChartDetection: 0
+ m_ImportantGI: 0
+ m_StitchLightmapSeams: 1
+ m_SelectedEditorRenderState: 3
+ m_MinimumChartSize: 4
+ m_AutoUVMaxDistance: 0.5
+ m_AutoUVMaxAngle: 89
+ m_LightmapParameters: {fileID: 0}
+ m_SortingLayerID: 0
+ m_SortingLayer: 0
+ m_SortingOrder: 0
+--- !u!33 &1779710251
+MeshFilter:
+ m_ObjectHideFlags: 0
+ m_CorrespondingSourceObject: {fileID: 0}
+ m_PrefabInstance: {fileID: 0}
+ m_PrefabAsset: {fileID: 0}
+ m_GameObject: {fileID: 1779710246}
+ m_Mesh: {fileID: 10206, guid: 0000000000000000e000000000000000, type: 0}
+--- !u!4 &1779710252
+Transform:
+ m_ObjectHideFlags: 0
+ m_CorrespondingSourceObject: {fileID: 0}
+ m_PrefabInstance: {fileID: 0}
+ m_PrefabAsset: {fileID: 0}
+ m_GameObject: {fileID: 1779710246}
+ m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
+ m_LocalPosition: {x: 0.5, y: 0.2, z: 1}
+ m_LocalScale: {x: 0.3, y: 0.2, z: 0.3}
+ m_Children: []
+ m_Father: {fileID: 0}
+ m_RootOrder: 5
+ m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!114 &1779710253
+MonoBehaviour:
+ m_ObjectHideFlags: 0
+ m_CorrespondingSourceObject: {fileID: 0}
+ m_PrefabInstance: {fileID: 0}
+ m_PrefabAsset: {fileID: 0}
+ m_GameObject: {fileID: 1779710246}
+ m_Enabled: 1
+ m_EditorHideFlags: 0
+ m_Script: {fileID: 11500000, guid: ff98778ae051cfd40a6e4effdf851ebd, type: 3}
+ m_Name:
+ m_EditorClassIdentifier:
+ SecondsTimeout: 10
+ Serializer: 0
+ protocol: 0
+ RosBridgeServerUrl: ws://localhost:9090
+--- !u!114 &1779710254
+MonoBehaviour:
+ m_ObjectHideFlags: 0
+ m_CorrespondingSourceObject: {fileID: 0}
+ m_PrefabInstance: {fileID: 0}
+ m_PrefabAsset: {fileID: 0}
+ m_GameObject: {fileID: 1779710246}
+ m_Enabled: 1
+ m_EditorHideFlags: 0
+ m_Script: {fileID: 11500000, guid: 2cf09ecb17efba9459a691ba8a4bda9f, type: 3}
+ m_Name:
+ m_EditorClassIdentifier:
+ Topic: velodyne_packets
+ FrameId: velodyne
+ numDataBlocks: 12
--- !u!1001 &1834194366728725521
PrefabInstance:
m_ObjectHideFlags: 0
diff --git a/Assets/Scripts/VelodyneLidarUnity.meta b/Assets/Scripts/VelodyneLidarUnity.meta
new file mode 100644
index 0000000..5e11cc3
--- /dev/null
+++ b/Assets/Scripts/VelodyneLidarUnity.meta
@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 695bbc7637ead0442bcbefa56be0a673
+folderAsset: yes
+DefaultImporter:
+ externalObjects: {}
+ userData:
+ assetBundleName:
+ assetBundleVariant:
diff --git a/Assets/Scripts/VelodyneLidarUnity/Lidar.cs b/Assets/Scripts/VelodyneLidarUnity/Lidar.cs
new file mode 100644
index 0000000..9c4b6a1
--- /dev/null
+++ b/Assets/Scripts/VelodyneLidarUnity/Lidar.cs
@@ -0,0 +1,62 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+public class Lidar : MonoBehaviour {
+
+ public float maxAngle = 10;
+ public float minAngle = -10;
+ public int numberOfLayers = 16;
+ public int numberOfIncrements = 360;
+ public float maxRange = 100f;
+
+ float vertIncrement;
+ float azimutIncrAngle;
+
+ [HideInInspector]
+ public float[] distances;
+ public float[] azimuts;
+
+
+ // Use this for initialization
+ void Start () {
+ distances = new float[numberOfLayers* numberOfIncrements];
+ azimuts = new float[numberOfIncrements];
+ vertIncrement = (float)(maxAngle - minAngle) / (float)(numberOfLayers - 1);
+ azimutIncrAngle = (float)(360.0f / numberOfIncrements);
+ }
+
+// Update is called once per frame
+void FixedUpdate () {
+ Vector3 fwd = new Vector3(0, 0, 1);
+ Vector3 dir;
+ RaycastHit hit;
+ int indx = 0;
+ float angle;
+
+ //azimut angles
+ for (int incr = 0; incr < numberOfIncrements; incr++)
+ {
+ for (int layer = 0; layer < numberOfLayers; layer++)
+ {
+ //print("incr "+ incr +" layer "+layer+"\n");
+ indx = layer + incr * numberOfLayers;
+ angle = minAngle + (float)layer * vertIncrement;
+ azimuts[incr] = incr * azimutIncrAngle;
+ dir = transform.rotation * Quaternion.Euler(-angle, azimuts[incr], 0)*fwd;
+ // print("idx "+ indx +" angle " + angle + " azimut " + azimut + " quats " + Quaternion.Euler(-angle, azimut, 0) + " dir " + dir+ " fwd " + fwd+"\n");
+
+ //Debug.DrawRay(transform.position, dir * 100.0f, Color.green);
+ if (Physics.Raycast(transform.position, dir, out hit, maxRange))
+ {
+ distances[indx] = (float)hit.distance;
+ }
+ else
+ {
+ distances[indx] = maxRange;
+ }
+ }
+ }
+
+ }
+}
diff --git a/Assets/Scripts/VelodyneLidarUnity/Lidar.cs.meta b/Assets/Scripts/VelodyneLidarUnity/Lidar.cs.meta
new file mode 100644
index 0000000..9fb8c5e
--- /dev/null
+++ b/Assets/Scripts/VelodyneLidarUnity/Lidar.cs.meta
@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 479d6f4dc5ab1e446ab2d632598c7ad5
+MonoImporter:
+ externalObjects: {}
+ serializedVersion: 2
+ defaultReferences: []
+ executionOrder: 0
+ icon: {instanceID: 0}
+ userData:
+ assetBundleName:
+ assetBundleVariant:
diff --git a/Assets/Scripts/VelodyneLidarUnity/VelodynePublisher.cs b/Assets/Scripts/VelodyneLidarUnity/VelodynePublisher.cs
new file mode 100644
index 0000000..cce95f9
--- /dev/null
+++ b/Assets/Scripts/VelodyneLidarUnity/VelodynePublisher.cs
@@ -0,0 +1,147 @@
+using System;
+using System.Collections.Generic;
+
+namespace RosSharp.RosBridgeClient
+{
+ public class VelodynePublisher : UnityPublisher<MessageTypes.Velodyne.VelodyneScan>
+ {
+ private Lidar lidar;
+ public string FrameId = "velodyne";
+ private MessageTypes.Velodyne.VelodyneScan message;
+ private List<MessageTypes.Velodyne.VelodynePacket> packets;
+ private int[] laserIdxs1 = { 0,8 ,1,9, 2,10, 3,11, 4,12, 5,13, 6,14, 7, 15};
+ public int numDataBlocks = 12;
+ public static DateTime UNIX_EPOCH = new DateTime(1970, 1, 1, 0, 0, 0, 0, DateTimeKind.Utc);
+
+ public virtual MessageTypes.Std.Time Now()
+ {
+ TimeSpan timeSpan = DateTime.Now.ToUniversalTime() - UNIX_EPOCH;
+
+ double msecs = timeSpan.TotalMilliseconds;
+ uint sec = (uint)(msecs / 1000);
+
+ return new MessageTypes.Std.Time
+ {
+ secs = sec,
+ nsecs = (uint)((msecs / 1000 - sec) * 1e+9)
+ };
+ }
+
+ protected override void Start()
+ {
+ base.Start();
+ lidar = gameObject.GetComponent<Lidar>();
+ InitializeMessage();
+ }
+
+ private void InitializeMessage()
+ {
+ message = new MessageTypes.Velodyne.VelodyneScan();
+ message.header.frame_id = FrameId;
+ packets = new List<MessageTypes.Velodyne.VelodynePacket>();
+ }
+
+ public byte[] makeAzimuthBytes(float az)
+ {
+ ushort azimuth = (ushort)(az * 100.0f);
+ //Console.Write("azimuth : {0} -->", azimuth);
+ byte[] azimuthArr = System.BitConverter.GetBytes(azimuth);
+ //Console.WriteLine("Hex: {0:X}", ByteArrayToString(azimuthArr));
+ return azimuthArr;
+ }
+
+ public byte[] makeDistanceBytes(float dist)
+ {
+ ushort distance = (ushort)(dist / 0.002f);
+ //Console.Write("distance : {0} ", distance);
+ byte[] distArr = System.BitConverter.GetBytes(distance);
+ //Console.WriteLine("Hex: {0:X}", ByteArrayToString(distArr));
+ return distArr;
+ }
+
+ public byte[] Serialize(float[] distanceData, float[] azimuth, int azimutStart, int numLayers, int numIncrements)
+ {
+ byte[] result = new byte[1206];
+ byte[] azimuthArr;
+ byte[] distanceArr;
+
+ int dbIdx = 0;
+ int azIdx = azimutStart;
+ int distIdx;
+
+ for (int db = 0; db < 12; db++)
+ {
+ if (azIdx >= numIncrements)
+ {
+ azIdx = 0;
+ }
+ //Debug.Log("azIdx " + azIdx + " dbIdx " + db + "\n");
+
+ distIdx = azIdx * numLayers;
+
+ // write a data block
+ dbIdx = db * 100;
+ result[dbIdx + 0] = 0xff;
+ result[dbIdx + 1] = 0xee;
+ azimuthArr = makeAzimuthBytes(azimuth[azIdx]);
+ Buffer.BlockCopy(azimuthArr, 0, result, dbIdx + 2, 2);
+ //Debug.Log("db "+db +" azimut " + azimuth[azIdx]);
+
+ // write channel data, first firing
+ for (int c1 = 0; c1 < 16; c1++)
+ {
+ distanceArr = makeDistanceBytes(distanceData[distIdx + laserIdxs1[c1]]);
+ //Debug.Log("dist1[ " + (distIdx + c1) + "] " + distanceData[distIdx + c1]+ " for idx "+ laserIdxs1[c1] + " mapped "+ distanceData[distIdx + laserIdxs1[c1]]);
+
+ Buffer.BlockCopy(distanceArr, 0, result, dbIdx + 4 + c1 * 3, 2);
+ result[dbIdx + 4 + c1 * 3 + 2] = 0xff;
+ }
+ // write channel data, 2nd firing
+ for (int c2 = 16; c2 < 32; c2++)
+ {
+ distanceArr = makeDistanceBytes(distanceData[distIdx + laserIdxs1[c2 -16]]);
+ //Debug.Log("dist2[ " + (distIdx + c2)+"] " + distanceData[distIdx + c2]);
+
+ Buffer.BlockCopy(distanceArr, 0, result, dbIdx + 4 + c2 * 3, 2);
+ result[dbIdx + 4 + c2 * 3 + 2] = 0x12;
+ }
+
+ //update idxs
+ azIdx = azIdx + 2;
+ }
+ result[1200] = 0x00;
+ result[1201] = 0x00;
+ result[1202] = 0x00;
+ result[1203] = 0x00;
+ result[1204] = 0x37;
+ result[1205] = 0x22;
+ return result;
+ }
+
+ private void Update()
+ {
+ Boolean cont = true;
+ int idx = 0;
+ int azIncrPerMsg = 2 * numDataBlocks;
+ message.packets.Initialize();
+ Array.Resize(ref message.packets, 0);
+ packets.Clear();
+ while (cont)
+ {
+ MessageTypes.Velodyne.VelodynePacket packet = new MessageTypes.Velodyne.VelodynePacket();
+ packet.data = Serialize(lidar.distances, lidar.azimuts, idx, lidar.numberOfLayers, lidar.numberOfIncrements);
+ packet.stamp = Now();
+ packets.Add(packet);
+ idx = idx + azIncrPerMsg;
+ if (idx > (lidar.numberOfIncrements - 1))
+ {
+ idx = idx - lidar.numberOfIncrements;
+ cont = false;
+ }
+ }
+ message.header.stamp = Now();
+ message.packets = packets.ToArray();
+ Publish(message);
+ }
+ }
+}
diff --git a/Assets/Scripts/VelodyneLidarUnity/VelodynePublisher.cs.meta b/Assets/Scripts/VelodyneLidarUnity/VelodynePublisher.cs.meta
new file mode 100644
index 0000000..fba5941
--- /dev/null
+++ b/Assets/Scripts/VelodyneLidarUnity/VelodynePublisher.cs.meta
@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 2cf09ecb17efba9459a691ba8a4bda9f
+MonoImporter:
+ externalObjects: {}
+ serializedVersion: 2
+ defaultReferences: []
+ executionOrder: 0
+ icon: {instanceID: 0}
+ userData:
+ assetBundleName:
+ assetBundleVariant:
diff --git a/Assets/Scripts/VelodyneLidarUnity/Velodyne_UDP_Send.cs b/Assets/Scripts/VelodyneLidarUnity/Velodyne_UDP_Send.cs
new file mode 100644
index 0000000..0941494
--- /dev/null
+++ b/Assets/Scripts/VelodyneLidarUnity/Velodyne_UDP_Send.cs
@@ -0,0 +1,218 @@
+using UnityEngine;
+using System.Collections;
+using System;
+using System.Text;
+using System.Net;
+using System.Net.Sockets;
+using System.Threading;
+using System.IO;
+
+public class Velodyne_UDP_Send : MonoBehaviour
+{
+ //
+ public static string IP = "192.168.2.101";
+ public static int port = 2368;
+ public static int numDataBLocks = 12;
+
+ //
+ private static int[] laserIdxs = { 0, 2, 4, 6, 8, 10, 12, 14, 1, 3, 5, 7, 9, 11, 13, 15 };
+ private static int[] laserIdxs1 = { 0,8 ,1,9, 2,10, 3,11, 4,12, 5,13, 6,14, 7, 15};
+
+ // connection
+ private static IPAddress broadcast;
+ private static IPEndPoint ep;
+ private static Socket s;
+
+ //gos
+ private static Lidar lidarGO;
+
+ //functions
+ public static string ByteArrayToString(byte[] ba)
+ {
+ StringBuilder hex = new StringBuilder(ba.Length * 2);
+ foreach (byte b in ba)
+ hex.AppendFormat("{0:x2}", b);
+ return hex.ToString();
+ }
+
+ public static float getAzimuth(byte[] az)
+ {
+ //Console.WriteLine("Hex: {0:X}", ByteArrayToString(az));
+ ushort azInt = System.BitConverter.ToUInt16(az, 0);
+ float azimuth = (float)azInt / 100.0f;
+ //Console.Write("azimuth : {0} -->", azimuth);
+ return azimuth;
+ }
+
+ public static byte[] makeAzimuthBytes(float az)
+ {
+ ushort azimuth = (ushort)(az * 100.0f);
+ //Console.Write("azimuth : {0} -->", azimuth);
+ byte[] azimuthArr = System.BitConverter.GetBytes(azimuth);
+ //Console.WriteLine("Hex: {0:X}", ByteArrayToString(azimuthArr));
+ return azimuthArr;
+ }
+
+ public static byte[] makeDistanceBytes(float dist)
+ {
+ ushort distance = (ushort)(dist / 0.002f);
+ //Console.Write("distance : {0} ", distance);
+ byte[] distArr = System.BitConverter.GetBytes(distance);
+ //Console.WriteLine("Hex: {0:X}", ByteArrayToString(distArr));
+ return distArr;
+ }
+
+ public static float getDistance(byte[] distance)
+ {
+ //Console.WriteLine("Hex: {0:X}", ByteArrayToString(distance));
+ ushort distInt = System.BitConverter.ToUInt16(distance, 0);
+ float dist = (float)distInt * 0.002f;
+ //Console.Write(" {0} |", dist);
+ return dist;
+ }
+
+ public static byte[] Serialize(float[] distanceData, float[] azimuth, int azimutStart, int numLayers, int numIncrements)
+ {
+ byte[] result = new byte[1206];
+ byte[] azimuthArr;
+ byte[] distanceArr;
+
+ int dbIdx = 0;
+ int azIdx = azimutStart;
+ int distIdx;
+
+ for (int db = 0; db < 12; db++)
+ {
+ if (azIdx >= numIncrements)
+ {
+ azIdx = 0;
+ }
+ //Debug.Log("azIdx " + azIdx + " dbIdx " + db + "\n");
+
+ distIdx = azIdx * numLayers;
+
+ // write a data block
+ dbIdx = db * 100;
+ result[dbIdx + 0] = 0xff;
+ result[dbIdx + 1] = 0xee;
+ azimuthArr = makeAzimuthBytes(azimuth[azIdx]);
+ Buffer.BlockCopy(azimuthArr, 0, result, dbIdx + 2, 2);
+ //Debug.Log("db "+db +" azimut " + azimuth[azIdx]);
+
+ // write channel data, first firing
+ for (int c1 = 0; c1 < 16; c1++)
+ {
+ distanceArr = makeDistanceBytes(distanceData[distIdx + laserIdxs1[c1]]);
+ //Debug.Log("dist1[ " + (distIdx + c1) + "] " + distanceData[distIdx + c1]+ " for idx "+ laserIdxs1[c1] + " mapped "+ distanceData[distIdx + laserIdxs1[c1]]);
+
+ Buffer.BlockCopy(distanceArr, 0, result, dbIdx + 4 + c1 * 3, 2);
+ result[dbIdx + 4 + c1 * 3 + 2] = 0xff;
+ }
+ // write channel data, 2nd firing
+ for (int c2 = 16; c2 < 32; c2++)
+ {
+ distanceArr = makeDistanceBytes(distanceData[distIdx + laserIdxs1[c2 -16]]);
+ //Debug.Log("dist2[ " + (distIdx + c2)+"] " + distanceData[distIdx + c2]);
+
+ Buffer.BlockCopy(distanceArr, 0, result, dbIdx + 4 + c2 * 3, 2);
+ result[dbIdx + 4 + c2 * 3 + 2] = 0x12;
+ }
+
+ //update idxs
+ azIdx = azIdx + 2;
+ }
+ result[1200] = 0x00;
+ result[1201] = 0x00;
+ result[1202] = 0x00;
+ result[1203] = 0x00;
+ result[1204] = 0x37;
+ result[1205] = 0x22;
+ return result;
+ }
+
+ public static int Deserialize(byte[] data)
+ {
+ using (MemoryStream inputStream = new MemoryStream(data))
+ using (BinaryReader reader = new BinaryReader(inputStream))
+ {
+ byte[] flagArr, azimuthArr, distanceArr;
+ byte reflectivity;
+ int idx;
+
+ float distance = 0;
+ float azimuth = 0;
+ float[] distanceValues = new float[16];
+
+
+ //----channel blocks---
+ for (int db = 0; db < 12; db++)
+ {
+ flagArr = reader.ReadBytes(2);
+ azimuthArr = reader.ReadBytes(2);
+ azimuth = getAzimuth(azimuthArr);
+
+ for (int c = 0; c < 16; c++)
+ {
+ distanceArr = reader.ReadBytes(2);
+ idx = laserIdxs[c];
+ distance = getDistance(distanceArr);
+ distanceValues[idx] = distance;
+ reflectivity = reader.ReadByte();
+ }
+ for (int c = 16; c < 32; c++)
+ {
+ distanceArr = reader.ReadBytes(2);
+ idx = laserIdxs[c-16];
+ distance = getDistance(distanceArr);
+ distanceValues[idx] = distance;
+ reflectivity = reader.ReadByte();
+ }
+ Console.Write("res azimuth : {0} -->", azimuth);
+ for (int i = 0; i < 16; i++)
+ {
+ Console.Write(" {0} | ", distanceValues[i]);
+ }
+ Console.WriteLine("");
+ }
+
+ //----get time---
+ byte[] timestamp = reader.ReadBytes(4);
+ Array.Reverse(timestamp);
+ float time = System.BitConverter.ToSingle(timestamp, 0);
+ //Console.WriteLine("time : {0}", time);
+
+ //----get factory---
+ byte[] factory = reader.ReadBytes(2);
+ }
+ return 0;
+ }
+
+ public void Start()
+ {
+ broadcast = IPAddress.Parse(IP);
+ ep = new IPEndPoint(broadcast, 2368);
+ s = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp);
+ lidarGO = gameObject.GetComponent<Lidar>();
+ }
+
+ public void FixedUpdate()
+ {
+ Boolean cont = true;
+ int idx = 0;
+ int azIncrPerMsg = 2 * numDataBLocks;
+ while (cont)
+ {
+ //Debug.Log("start with IDx "+idx+" at "+Time.time);
+ byte[] dummy1 = Serialize(lidarGO.distances, lidarGO.azimuts, idx, lidarGO.numberOfLayers, lidarGO.numberOfIncrements);
+ s.SendTo(dummy1, ep);
+ idx = idx + azIncrPerMsg;
+ if (idx > (lidarGO.numberOfIncrements-1))
+ {
+ idx = idx - lidarGO.numberOfIncrements;
+ cont = false;
+ }
+
+ }
+ }
+}
+
diff --git a/Assets/Scripts/VelodyneLidarUnity/Velodyne_UDP_Send.cs.meta b/Assets/Scripts/VelodyneLidarUnity/Velodyne_UDP_Send.cs.meta
new file mode 100644
index 0000000..01a24f2
--- /dev/null
+++ b/Assets/Scripts/VelodyneLidarUnity/Velodyne_UDP_Send.cs.meta
@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 3f13aa18ca716d841978f7821bcb3921
+MonoImporter:
+ externalObjects: {}
+ serializedVersion: 2
+ defaultReferences: []
+ executionOrder: 0
+ icon: {instanceID: 0}
+ userData:
+ assetBundleName:
+ assetBundleVariant: