Merge from 'develop' into 'main' for CLOiSim-4.9.1 (#325)

Assets/Resources/RenderPipelines/SegmentationRenderer.asset

@@ -62,7 +62,7 @@ MonoBehaviour:
   - {fileID: -5373475574186842855}
   m_RendererFeatureMap: 19e55a676f946db5
   m_UseNativeRenderPass: 0
-  postProcessData: {fileID: 11400000, guid: 41439944d30ece34e96484bdb6645b55, type: 2}
+  postProcessData: {fileID: 0}
   shaders:
     blitPS: {fileID: 4800000, guid: c17132b1f77d20942aa75f8429c0f8bc, type: 3}
     copyDepthPS: {fileID: 4800000, guid: d6dae50ee9e1bfa4db75f19f99355220, type: 3}

Assets/Resources/Shader/Segmentation.shader

@@ -2,9 +2,7 @@ Shader "Sensor/Segmentation"
 {
 	Properties
 	{
-		_SegmentationColor ("Segmentation Color", Color) = (1, 1, 1, 1)
-		_SegmentationClassId ("Segmentation Class ID Value in 16bits", Color) = (0, 0, 0, 1)
-		[Toggle] _DisableColor ("Disable Color method", int) = 0
+		_SegmentationValue  ("Segmentation Value", int) = 0
 		[Toggle] _Hide ("Hide this label", int) = 0
 	}
 
@@ -24,9 +22,7 @@ Shader "Sensor/Segmentation"
 		#include "Packages/com.unity.render-pipelines.universal/ShaderLibrary/Core.hlsl"
 
 		CBUFFER_START(UnityPerMaterial)
-		half4 _SegmentationColor;
-		half4 _SegmentationClassId;
-		int _DisableColor;
+		int _SegmentationValue;
 		int _Hide;
 		CBUFFER_END
 
@@ -38,18 +34,18 @@ Shader "Sensor/Segmentation"
 
 			HLSLPROGRAM
 
-			#pragma target 4.5
+			#pragma target 4.6
 			#pragma vertex vert
 			#pragma fragment frag
 
 			struct Attributes
 			{
-				float4 positionOS : POSITION;
+				half4 positionOS : POSITION;
 			};
 
 			struct Varyings
 			{
-				float4 positionCS : SV_POSITION;
+				half4 positionCS : SV_POSITION;
 			};
 
 			Varyings vert(Attributes i)
@@ -65,7 +61,13 @@ Shader "Sensor/Segmentation"
 				half4 segColor = half4(0, 0, 0, 0);
 				if (_Hide == 0)
 				{
-					segColor = (_DisableColor == 0)? _SegmentationColor : _SegmentationClassId;
+					// Encode 16Bits To RG
+					float R = ((_SegmentationValue >> 8) & 0xFF) / 255.0;
+					float G = (_SegmentationValue & 0xFF) / 255.0;
+
+					// Due to little-endian data, SWAP
+					segColor.r = G;
+					segColor.g = R;
 				}
 				return segColor;
 			}

Assets/Scenes/MainScene.unity

@@ -1848,11 +1848,15 @@ MonoBehaviour:
   - ../world_resources/lawn_ground/models
   - ../world_resources/seocho_tower/models
   - ../world_resources/app.gazebosim.org
+  - ../world_resources/logistics_center/models
+  - ../world_resources/smallhouse/models
   _worldRootDirectories:
   - ../sample_resources/worlds/
   - ../world_resources/bluepearl/worlds
   - ../world_resources/lawn_ground/worlds
   - ../world_resources/seocho_tower/worlds
+  - ../world_resources/logistics_center/worlds
+  - ../world_resources/smallhouse/worlds
   _fileRootDirectories:
   - ../sample_resources/media/
   - ../world_resources/bluepearl/media

Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs

@@ -249,6 +249,7 @@ private void SetDriveForWheel(in string parameterPrefix)
 		var wheelTread = GetPluginParameters().GetValue<float>($"{parameterPrefix}/tread"); // TODO: to be deprecated
 		var wheelSeparation = GetPluginParameters().GetValue<float>($"{parameterPrefix}/separation", wheelTread);
 
+		_log.AppendLine($"wheel separation/radius: {wheelSeparation}/{wheelRadius}");
 		_motorControl.SetWheelInfo(wheelRadius, wheelSeparation);
 
 		var wheelLeftName = GetPluginParameters().GetValue<string>($"{parameterPrefix}/location[@type='left']", string.Empty);
@@ -287,13 +288,26 @@ private void SetMotorPID(
 
 		if (GetPluginParameters().IsValidNode($"{parameterPrefix}/limit"))
 		{
-			var limitIntegral = GetPluginParameters().GetValue<float>($"{parameterPrefix}/limit/integral");
-			var limitOutput = GetPluginParameters().GetValue<float>($"{parameterPrefix}/limit/output");
+			if (!GetPluginParameters().IsValidNode($"{parameterPrefix}/limit/integral/min") &&
+				!GetPluginParameters().IsValidNode($"{parameterPrefix}/limit/integral/max"))
+			{
+				var limitIntegral = GetPluginParameters().GetValue<float>($"{parameterPrefix}/limit/integral");
+				integralMin = -Math.Abs(limitIntegral);
+				integralMax = Math.Abs(limitIntegral);
+			}
+
+			if (!GetPluginParameters().IsValidNode($"{parameterPrefix}/limit/integral/min") &&
+				!GetPluginParameters().IsValidNode($"{parameterPrefix}/limit/integral/max"))
+			{
+				var limitOutput = GetPluginParameters().GetValue<float>($"{parameterPrefix}/limit/output");
+				outputMin = -Math.Abs(limitOutput);
+				outputMax = Math.Abs(limitOutput);
+			}
 
-			integralMin = GetPluginParameters().GetValue<float>($"{parameterPrefix}/limit/integral/min", -Mathf.Abs(limitIntegral));
-			integralMax = GetPluginParameters().GetValue<float>($"{parameterPrefix}/limit/integral/max", Mathf.Abs(limitIntegral));
-			outputMin = GetPluginParameters().GetValue<float>($"{parameterPrefix}/limit/output/min", -Mathf.Abs(limitOutput));
-			outputMax = GetPluginParameters().GetValue<float>($"{parameterPrefix}/limit/output/max", Mathf.Abs(limitOutput));
+			integralMin = GetPluginParameters().GetValue<float>($"{parameterPrefix}/limit/integral/min", integralMin);
+			integralMax = GetPluginParameters().GetValue<float>($"{parameterPrefix}/limit/integral/max", integralMax);
+			outputMin = GetPluginParameters().GetValue<float>($"{parameterPrefix}/limit/output/min", outputMin);
+			outputMax = GetPluginParameters().GetValue<float>($"{parameterPrefix}/limit/output/max", outputMax);
 		}
 
 		SetPID(P, I, D, integralMin, integralMax, outputMin, outputMax);
@@ -318,6 +332,11 @@ private void SetMowing()
 				mowingBlade.HeightMax = GetPluginParameters().GetValue<float>("mowing/blade/height/max", 0.1f);
 				mowingBlade.RevSpeedMax = GetPluginParameters().GetValue<UInt16>("mowing/blade/rev_speed/max", 1000);
 				mowingBlade.Height = 0;
+
+				if (_micomCommand != null)
+				{
+					_micomCommand.SetMowingBlade(mowingBlade);
+				}
 			}
 		}
 	}
@@ -414,8 +433,11 @@ protected override void HandleCustomRequestMessage(in string requestType, in Any
 				SetEmptyResponse(ref response);
 				break;
 
+			case "request_bumper_topic_name":
+				break;
+
 			default:
 				break;
 		}
 	}
-}
+}

Assets/Scripts/CLOiSimPlugins/Modules/Connection/Requestor.cs

@@ -88,7 +88,7 @@ public byte[] ReceiveResponse(in bool checkTag = false)
 		if (!IsDisposed)
 		{
 			var frameReceived = this.ReceiveFrameBytes();
-			return TransportHelper.RetrieveData(frameReceived, (checkTag)? hashValue : null);;
+			return TransportHelper.RetrieveData(frameReceived, (checkTag)? hashValue : null);
 		}
 		else
 		{

Assets/Scripts/CLOiSimPlugins/MowingPlugin.cs

@@ -214,7 +214,7 @@ protected override void OnReset()
 	{
 		if (_initialTexturePixels != null)
 		{
-			Debug.Log("Reset grass texture");
+			Debug.Log($"{this.GetType().Name}: Reset Grass Texture");
 			_grass.texture.Fill(ref _initialTexturePixels);
 			_grass.texture.Apply();
 		}
@@ -278,28 +278,29 @@ private void PlantGrass()
 		}
 	}
 
-	private IEnumerator PunchingGrass()
+	private IEnumerator InitializeTexture()
 	{
+		_initialTexturePixels = new Color[_grass.texture.GetPixels().LongLength];
+		Array.Copy(_grass.texture.GetPixels(), _initialTexturePixels, _initialTexturePixels.LongLength);
 		yield return null;
+	}
 
+	private IEnumerator PunchingGrass()
+	{
 		var tempVisualMeshCollider = new List<MeshCollider>();
 
 		CreateTempColliderInVisuals(ref tempVisualMeshCollider);
 
-		yield return FindMeshFiltersToPunching();
+		FindMeshFiltersToPunching();
 
 		RemoveTempColliderInVisuals(ref tempVisualMeshCollider);
 
-		yield return null;
-
 		foreach (var meshFilter in _punchingMeshFilters)
 		{
 			PunchingTexture(ref _grass.texture, meshFilter);
-			yield return null;
 		}
 
-		_initialTexturePixels = new Color[_grass.texture.GetPixels().LongLength];
-		Array.Copy(_grass.texture.GetPixels(), _initialTexturePixels, _initialTexturePixels.LongLength);
+		yield return InitializeTexture();
 
 		yield return StartMowing();
 	}
@@ -312,35 +313,33 @@ private void CreateTempColliderInVisuals(ref List<MeshCollider> tempMeshCollider
 			var meshColliders = helperLink.GetComponentsInChildren<MeshCollider>();
 			if (meshColliders.Length == 0)
 			{
-				var helperVisuals = GetComponentsInChildren<SDF.Helper.Visual>();
+				var helperVisuals = helperLink.GetComponentsInChildren<SDF.Helper.Visual>();
 				foreach (var helperVisual in helperVisuals)
 				{
 					var meshFilters = helperVisual.GetComponentsInChildren<MeshFilter>();
 					foreach (var meshFilter in meshFilters)
 					{
-						var meshCollider = meshFilter.transform.gameObject.AddComponent<MeshCollider>();
+						var meshCollider = meshFilter.gameObject.AddComponent<MeshCollider>();
 						meshCollider.convex = true;
-						meshCollider.isTrigger = true;
+						meshCollider.isTrigger = false;
 						tempMeshColliders.Add(meshCollider);
 					}
 				}
 			}
 		}
 	}
 
-	private IEnumerator FindMeshFiltersToPunching()
+	private void FindMeshFiltersToPunching()
 	{
 		var layerMask = LayerMask.GetMask("Default");
 
-		yield return null;
-
 		var hitColliders = Physics.OverlapBox(_grass.bounds.center, _grass.bounds.extents, Quaternion.identity, layerMask);
 		var i = 0;
 		while (i < hitColliders.Length)
 		{
 			var hitCollider = hitColliders[i++];
 			var helperModel = hitCollider.GetComponentInParent<SDF.Helper.Model>();
-			// Debug.Log("Hit : " + hitCollider.name + "-" + i);
+			// Debug.Log($"Hit: {helperModel?.name} {hitCollider.name}-{i}");
 
 			if (helperModel != null)
 			{
@@ -356,7 +355,7 @@ private IEnumerator FindMeshFiltersToPunching()
 				}
 				else
 				{
-					var meshFilters = hitCollider.GetComponentsInChildren<MeshFilter>();
+					var meshFilters = helperModel.GetComponentsInChildren<MeshFilter>();
 					_punchingMeshFilters.AddRange(meshFilters);
 				}
 			}
@@ -368,8 +367,6 @@ private IEnumerator FindMeshFiltersToPunching()
 					_punchingMeshFilters.AddRange(meshFilters);
 				}
 			}
-
-			yield return null;
 		}
 	}
 
@@ -397,6 +394,7 @@ private void PunchingTexture(ref Texture2D texture, in MeshFilter meshFilter)
 		var textureCenterY = (int)(texture.height * 0.5f);
 		var textureCenter = new Vector2(textureCenterX, textureCenterY);
 		var offset = _grass.GetGrassOffset();
+		var targetPlaneY = _targetPlane.position.y;
 
 		var mesh = meshFilter.sharedMesh;
 		var vertices = mesh.vertices;
@@ -411,6 +409,15 @@ private void PunchingTexture(ref Texture2D texture, in MeshFilter meshFilter)
 			var tp1 = meshFilter.transform.TransformPoint(p1) - offset;
 			var tp2 = meshFilter.transform.TransformPoint(p2) - offset;
 
+			if ((tp0.y > _grass.blade.heightMax &&
+				 tp1.y > _grass.blade.heightMax &&
+				 tp2.y > _grass.blade.heightMax)
+				|| (tp0.y < targetPlaneY && tp1.y < targetPlaneY && tp2.y < targetPlaneY))
+			{
+				// Debug.Log($"{meshFilter.name} {p0.y} {p1.y} {p2.y} => {tp0.y} {tp1.y} {tp2.y}");
+				continue;
+			}
+
 			var P1 = new Vector2(tp0.z, tp0.x);
 			var P2 = new Vector2(tp1.z, tp1.x);
 			var P3 = new Vector2(tp2.z, tp2.x);
@@ -434,12 +441,12 @@ private IEnumerator StartMowing()
 		var mowingThreshold = _grass.blade.heightMax;
 		var mowingRatioInColor = Color.clear;
 		var planeCenterPosition = _targetPlane.position;
-		var bladeRadiusIntexture = _mowingBlade.Diameter /_grass.mapResolution;
 
 		while (true)
 		{
-			if (_mowingBlade.IsRunning() && _mowingBlade != null)
+			if (_mowingBlade != null && _mowingBlade.IsRunning())
 			{
+				var bladeRadiusIntexture = _mowingBlade.Diameter /_grass.mapResolution;
 				var bladeToPlaneDistance = _targetPlane.TransformPoint(_mowingBlade.Position);
 
 				if (bladeToPlaneDistance.y <= mowingThreshold)

Assets/Scripts/Core/Modules/ColorEncoding.cs

@@ -56,8 +56,8 @@ public static Color EncodeTagAsColor(in string tag)
 		var r = (byte)(hash >> 16);
 		var g = (byte)(hash >> 8);
 		var b = (byte)(hash);
-		// Debug.Log($"EncodeTagAsColor({tag}): {r} {g} {b}");
-		return new Color32(r, g, b, 255);
+		// Debug.Log($"EncodeTagAsColor({tag}): {r} {g} {b} {a}");
+		return new Color32(r, g, b, a);
 	}
 
 	public static Color EncodeLayerAsColor(in int layer)
@@ -84,18 +84,4 @@ public static Color EncodeLayerAsColor(in int layer)
 
 		return color;
 	}
-
-	public static Color Encode16BitsToRG(in UInt16 value)
-	{
-		var classIdR = (byte)((value >> 8) & 0xFF);
-		var classIdG = (byte)(value & 0xFF);
-		return new Color32(classIdR, classIdG, 0, 0);
-	}
-
-	// for little endian order
-	public static Color Encode16BitsToGR(in UInt16 value)
-	{
-		var rgColor = Encode16BitsToRG(value);
-		return new Color(rgColor.g, rgColor.r, 0, 0);
-	}
 }

Assets/Scripts/Core/Modules/SegmentationTag.cs

@@ -85,19 +85,12 @@ public void Refresh()
 					break;
 			}
 
-			var grayscale = color.grayscale;
-
-			_classId = (UInt16)(grayscale * UInt16.MaxValue);
-
-			var classValue = ColorEncoding.Encode16BitsToGR(_classId);
-
-			mpb.SetColor("_SegmentationColor", color);
-			mpb.SetColor("_SegmentationClassId", classValue);
+			_classId = (UInt16)(color.grayscale * UInt16.MaxValue);
+
+			mpb.SetInt("_SegmentationValue", (int)_classId);
 			mpb.SetInt("_Hide", 0);
 
-			// Debug.Log(TagName + ": mode=" + Main.SegmentationManager.Mode +
-			// 			" color=" + color +
-			// 			" calssId=" + classValue.r + " "  + classValue.g);
+			// Debug.Log($"{TagName}: mode={Main.SegmentationManager.Mode} color={color} {_classId}");
 			// Debug.Log($"{TagName} : {grayscale} > {_classId}");
 
 			AllocateMaterialPropertyBlock(mpb);