Merge pull request #28 from lge-ros2/develop

Merge 'develop' branch into 'master'

Assets/Scenes/MainScene.unity

@@ -3802,7 +3802,7 @@ MonoBehaviour:
   m_Name: 
   m_EditorClassIdentifier: 
   blockControl: 0
-  mainSpeed: 7
+  mainSpeed: 12
   shiftAdd: 10
   maxShift: 100
   camSens: 0.25

Assets/Scripts/Connection/BridgePortManager.cs

@@ -10,7 +10,7 @@
 using System.Net;
 using System;
 using UnityEngine;
-using Newtonsoft.Json;
+using Stopwatch = System.Diagnostics.Stopwatch;
 
 public class BridgePortManager : DeviceTransporter
 {
@@ -55,10 +55,9 @@ public ushort SearchSensorPort(string hashKey)
 
 		lock (portMapTable)
 		{
-			foreach (KeyValuePair<string, ushort> each in portMapTable)
+			foreach (var each in portMapTable)
 			{
-				string _Key = each.Key;
-				if (_Key == hashKey)
+				if (each.Key == hashKey)
 				{
 					port = each.Value;
 					break;
@@ -104,7 +103,7 @@ public bool IsAvailablePort(in ushort port)
 	public ushort AllocateSensorPort(string hashKey)
 	{
 		// check if already occupied
-		ushort newPort = SearchSensorPort(hashKey);
+		var newPort = SearchSensorPort(hashKey);
 
 		if (newPort != 0)
 		{
@@ -114,7 +113,7 @@ public ushort AllocateSensorPort(string hashKey)
 
 		// find available port number
 		// start with minimum port range
-		for (ushort index = 0; index < (maxPortRange - minPortRange); index++)
+		for (var index = 0; index < (maxPortRange - minPortRange); index++)
 		{
 			var port = (ushort)(minPortRange + index);
 			var isContained = false;
@@ -149,18 +148,26 @@ public ushort AllocateSensorPort(string hashKey)
 
 	private void PortManageWorker()
 	{
+		var sw = new Stopwatch();
+
 		// Debug.LogFormat("Start SensorPortManager - {0}::{1}", GetType().Name, MethodBase.GetCurrentMethod().Name);
 		while (true)
 		{
-			// Debug.Log("Waiting for Request Data");
-			var hashKey = ReceiveRequest();
+			sw.Restart();
+
+			var hashKey = TryReceiveRequest();
+			if (hashKey != null)
+			{
+				var hashKeyInString = System.Text.Encoding.Default.GetString(hashKey);
+				var port = SearchSensorPort(hashKeyInString);
 
-			var hashKeyInString = (hashKey == null) ? string.Empty : System.Text.Encoding.Default.GetString(hashKey);
-			var port = SearchSensorPort(hashKeyInString);
-			var portBuf = System.Convert.ToString(port);
+				var portBuf = Convert.ToString(port);
+				SendResponse(portBuf);
 
-			SendResponse(portBuf);
-			Debug.LogFormat("-> Reply for {0} = {1}", hashKeyInString, port);
+				sw.Stop();
+				var timeElapsed = sw.ElapsedMilliseconds;
+				Debug.LogFormat("-> Reply for {0} = {1}, {2} ms", hashKeyInString, port, timeElapsed);
+			}
 		}
 	}
 

Assets/Scripts/Connection/DeviceTransporter.cs

@@ -14,7 +14,7 @@ public partial class DeviceTransporter : MonoBehaviour
 	private ushort tagSize = 8;
 	public string defaultPipeAddress = "127.0.0.1";
 
-	private int highwatermark = 0;
+	private int highwatermark = 1000;
 
 	public DeviceTransporter()
 	{
@@ -26,8 +26,6 @@ public DeviceTransporter()
 		}
 
 		SetPipeAddress(defaultPipeAddress);
-
-		highwatermark = 1000;
 	}
 
 	~DeviceTransporter()
@@ -112,7 +110,11 @@ private bool StoreData(ref byte[] targetBuffer, in byte[] dataToStore, in int da
 		if (dataToStoreLength > 0 && dataToStore != null && targetBuffer != null)
 		{
 			var dataLength = tagSize + dataToStoreLength;
-			Array.Resize(ref targetBuffer, dataLength);
+
+			if (dataLength > targetBuffer.Length)
+			{
+				Array.Resize(ref targetBuffer, dataLength);
+			}
 
 			try
 			{

Assets/Scripts/Connection/DeviceTransporter.publisher.cs

@@ -29,9 +29,12 @@ protected bool InitializePublisher(in ushort targetPort)
 
 		if (publisherSocket != null)
 		{
+			publisherSocket.Options.Linger = TimeSpan.FromTicks(0);
+			publisherSocket.Options.IPv4Only = true;
 			publisherSocket.Options.TcpKeepalive = true;
+			publisherSocket.Options.DisableTimeWait = true;
 			publisherSocket.Options.SendHighWatermark = highwatermark;
-			publisherSocket.Options.Linger = new TimeSpan(0);
+
 			publisherSocket.Bind(GetAddress(targetPort));
 			// Debug.Log("Publisher socket binding for - " + targetPort);
 			initialized = StoreTag(ref dataToPublish, hashValueForPublish);
@@ -62,26 +65,25 @@ protected bool Publish(in MemoryStream streamToSend)
 	protected bool Publish(in string stringToSend)
 	{
 		var buffer = System.Text.Encoding.UTF8.GetBytes(stringToSend);
-		return Publish(buffer,stringToSend.Length);
+		return Publish(buffer, stringToSend.Length);
 	}
 
-	protected bool Publish(in byte[] bufferToSend, in int bufferLength)
+	protected bool Publish(in byte[] buffer, in int bufferLength)
 	{
-		bool wasSucessful = false;
-
-		if (StoreData(ref dataToPublish, bufferToSend, bufferLength) == false)
-		{
-			return wasSucessful;
-		}
+		var wasSucessful = false;
 
-		if (publisherSocket != null)
-		{
-			wasSucessful = publisherSocket.TrySendFrame(dataToPublish);
-			// Debug.LogFormat("Publish data({0}) length({1})", bufferToSend, bufferLength);
-		}
-		else
+		if (StoreData(ref dataToPublish, buffer, bufferLength))
 		{
-			Debug.LogWarning("Socket for publisher or response-request is not initilized yet.");
+			if (publisherSocket != null)
+			{
+				var dataLength = tagSize + bufferLength;
+				wasSucessful = publisherSocket.TrySendFrame(dataToPublish, dataLength);
+				// Debug.LogFormat("Publish data({0}) length({1})", buffer, bufferLength);
+			}
+			else
+			{
+				Debug.LogWarning("Socket for publisher or response-request is not initilized yet.");
+			}
 		}
 
 		return wasSucessful;

Assets/Scripts/Connection/DeviceTransporter.request.cs

@@ -29,7 +29,12 @@ protected bool InitializeRequester(in ushort targetPort)
 
 		if (requestSocket != null)
 		{
+			requestSocket.Options.Linger = TimeSpan.FromTicks(0);
+			requestSocket.Options.IPv4Only = true;
 			requestSocket.Options.TcpKeepalive = true;
+			requestSocket.Options.DisableTimeWait = true;
+			requestSocket.Options.SendHighWatermark = highwatermark;
+
 			requestSocket.Bind(GetAddress(targetPort));
 			// Debug.Log("Requester socket connecting... " + targetPort);
 			initialized = StoreTag(ref dataToSendRequest, hashValueForSendRequest);
@@ -68,21 +73,22 @@ protected bool SendRequest(in string stringToSend)
 	/// required to initialize `requestSocket`
 	/// must send request message first before receive response
 	/// </summary>
-	/// <param name="bytesToSend">message data buffer to send in bytes array</param>
-	/// <param name="bytesLength">the length of data buffer</param>
+	/// <param name="buffer">message data buffer to send in bytes array</param>
+	/// <param name="bufferLength">the length of data buffer</param>
 	/// <returns>It returns false if failed to send, otherwise returns true</returns>
-	protected bool SendRequest(in byte[] bytesToSend, in int bytesLength)
+	protected bool SendRequest(in byte[] buffer, in int bufferLength)
 	{
 		bool wasSucessful = false;
 
-		if (StoreData(ref dataToSendRequest, bytesToSend, bytesLength) == false)
+		if (StoreData(ref dataToSendRequest, buffer, bufferLength) == false)
 		{
 			return wasSucessful;
 		}
 
 		if (requestSocket != null)
 		{
-			wasSucessful = requestSocket.TrySendFrame(dataToSendRequest);
+			var dataLength = tagSize + bufferLength;
+			wasSucessful = requestSocket.TrySendFrame(dataToSendRequest, dataLength);
 		}
 		else
 		{

Assets/Scripts/Connection/DeviceTransporter.response.cs

@@ -14,6 +14,8 @@ public partial class DeviceTransporter
 {
 	private ResponseSocket responseSocket = null;
 
+	TimeSpan timeoutForResponse = TimeSpan.FromMilliseconds(100);
+
 	private byte[] hashValueForReceiveRequest = null;
 	private byte[] dataToSendResponse = null;
 
@@ -29,7 +31,11 @@ protected bool InitializeResponsor(in ushort targetPort)
 
 		if (responseSocket != null)
 		{
+			responseSocket.Options.Linger = TimeSpan.FromTicks(0);
+			responseSocket.Options.IPv4Only = true;
 			responseSocket.Options.TcpKeepalive = true;
+			responseSocket.Options.DisableTimeWait = true;
+
 			responseSocket.Bind(GetAddress(targetPort));
 			// Debug.Log("Responsor socket connecting... " + targetPort);
 			initialized = StoreTag(ref dataToSendResponse, hashValueForReceiveRequest);
@@ -38,19 +44,32 @@ protected bool InitializeResponsor(in ushort targetPort)
 		return initialized;
 	}
 
-	protected byte[] ReceiveRequest(in bool checkTag = false)
+	protected byte[] TryReceiveRequest(in bool checkTag = false)
 	{
-		byte[] frameReceived = null;
+		if (responseSocket == null)
+		{
+			Debug.LogWarning("Socket for response is not initilized yet.");
+			return null;
+		}
 
-		if (responseSocket != null)
+		if (responseSocket.TryReceiveFrameBytes(timeoutForResponse, out var frameReceived))
 		{
-			frameReceived = responseSocket.ReceiveFrameBytes();
+			var receivedData = RetrieveData(frameReceived, (checkTag)? hashValueForReceiveRequest : null);
+			return receivedData;
 		}
-		else
+
+		return null;
+	}
+
+	protected byte[] ReceiveRequest(in bool checkTag = false)
+	{
+		if (responseSocket == null)
 		{
 			Debug.LogWarning("Socket for response is not initilized yet.");
+			return null;
 		}
 
+		var frameReceived = responseSocket.ReceiveFrameBytes();
 		var receivedData = RetrieveData(frameReceived, (checkTag)? hashValueForReceiveRequest : null);
 		return receivedData;
 	}
@@ -80,18 +99,19 @@ protected bool SendResponse(in string stringToSend)
 		return SendResponse(buffer, stringToSend.Length);
 	}
 
-	private bool SendResponse(in byte[] bytesToSend, in int bytesLength)
+	private bool SendResponse(in byte[] buffer, in int bufferLength)
 	{
 		var wasSucessful = false;
 
-		if (StoreData(ref dataToSendResponse, bytesToSend, bytesLength) == false)
+		if (StoreData(ref dataToSendResponse, buffer, bufferLength) == false)
 		{
 			return wasSucessful;
 		}
 
 		if (responseSocket != null)
 		{
-			wasSucessful = responseSocket.TrySendFrame(dataToSendResponse);
+			var dataLength = tagSize + bufferLength;
+			wasSucessful = responseSocket.TrySendFrame(dataToSendResponse, dataLength);
 		}
 		else
 		{

Assets/Scripts/Connection/DeviceTransporter.subscriber.cs

@@ -22,9 +22,11 @@ protected bool InitializeSubscriber(in ushort targetPort)
 
 		if (subscriberSocket != null)
 		{
+			subscriberSocket.Options.Linger = TimeSpan.FromTicks(0);
+			subscriberSocket.Options.IPv4Only = true;
 			subscriberSocket.Options.TcpKeepalive = true;
+			subscriberSocket.Options.DisableTimeWait = true;
 			subscriberSocket.Options.ReceiveHighWatermark = highwatermark;
-			subscriberSocket.Options.Linger = new TimeSpan(0);
 
 		 	if (hashValueForSubscription != null)
 			{