From 5fcbc3fa035656fce9f3749bbecd51013b6e87bf Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Tue, 10 Dec 2024 18:22:54 +0900 Subject: [PATCH 01/19] Update UniversalRenderPipelineAsset.asset --- .../RenderPipelines/UniversalRenderPipelineAsset.asset | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Assets/Resources/RenderPipelines/UniversalRenderPipelineAsset.asset b/Assets/Resources/RenderPipelines/UniversalRenderPipelineAsset.asset index 6f3329e0..c31e175b 100644 --- a/Assets/Resources/RenderPipelines/UniversalRenderPipelineAsset.asset +++ b/Assets/Resources/RenderPipelines/UniversalRenderPipelineAsset.asset @@ -84,7 +84,7 @@ MonoBehaviour: blueNoise64LTex: {fileID: 2800000, guid: e3d24661c1e055f45a7560c033dbb837, type: 3} bayerMatrixTex: {fileID: 2800000, guid: f9ee4ed84c1d10c49aabb9b210b0fc44, type: 3} m_PrefilteringModeMainLightShadows: 3 - m_PrefilteringModeAdditionalLight: 0 + m_PrefilteringModeAdditionalLight: 4 m_PrefilteringModeAdditionalLightShadows: 2 m_PrefilterXRKeywords: 1 m_PrefilteringModeForwardPlus: 1 From 3052e3eed17cf4da901c1bc63d3ca3e64146448c Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Thu, 12 Dec 2024 17:02:29 +0900 Subject: [PATCH 02/19] Make it possible to play video file into plane geometry Add new custom shader for playing video on texture Update Packages Version - com.unity.modules.video: 1.0.0 => Enable --- Assets/Resources/Meshes.meta | 8 - Assets/Resources/Meshes/.gitkeep | 0 .../Resources/Shader/UnlitVideoTexture.shader | 147 ++++++++++++++++++ .../Shader/UnlitVideoTexture.shader.meta | 9 ++ Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs | 47 +++++- Packages/manifest.json | 1 + Packages/packages-lock.json | 10 ++ 7 files changed, 213 insertions(+), 9 deletions(-) delete mode 100644 Assets/Resources/Meshes.meta delete mode 100644 Assets/Resources/Meshes/.gitkeep create mode 100644 Assets/Resources/Shader/UnlitVideoTexture.shader create mode 100644 Assets/Resources/Shader/UnlitVideoTexture.shader.meta diff --git a/Assets/Resources/Meshes.meta b/Assets/Resources/Meshes.meta deleted file mode 100644 index 4c9af935..00000000 --- a/Assets/Resources/Meshes.meta +++ /dev/null @@ -1,8 +0,0 @@ -fileFormatVersion: 2 -guid: 41ca95647bc227f1ea34d0346b3c5655 -folderAsset: yes -DefaultImporter: - externalObjects: {} - userData: - assetBundleName: - assetBundleVariant: diff --git a/Assets/Resources/Meshes/.gitkeep b/Assets/Resources/Meshes/.gitkeep deleted file mode 100644 index e69de29b..00000000 diff --git a/Assets/Resources/Shader/UnlitVideoTexture.shader b/Assets/Resources/Shader/UnlitVideoTexture.shader new file mode 100644 index 00000000..7b4f50f0 --- /dev/null +++ b/Assets/Resources/Shader/UnlitVideoTexture.shader @@ -0,0 +1,147 @@ +// Upgrade NOTE: replaced 'mul(UNITY_MATRIX_MVP,*)' with 'UnityObjectToClipPos(*)' + +Shader "Custom/Unlit/VideoTexture" +{ + Properties + { + _MainTex ("Texture", 2D) = "white" {} + } + SubShader + { + Tags { "RenderType"="Opaque" } + LOD 100 + + Pass + { + CGPROGRAM + #pragma vertex vert + #pragma fragment frag + // make fog work + #pragma multi_compile_fog + + #include "UnityCG.cginc" + + struct appdata + { + float4 vertex : POSITION; + float2 uv : TEXCOORD0; + }; + + struct v2f + { + float2 uv : TEXCOORD0; + UNITY_FOG_COORDS(1) + float4 vertex : SV_POSITION; + }; + + sampler2D _MainTex; + float4 _MainTex_ST; + + v2f vert (appdata v) + { + v2f o; + + o.vertex = UnityObjectToClipPos(v.vertex); + + // rotating UV + const float Deg2Rad = (UNITY_PI * 2.0) / 360.0; + const float Rotation = 90.0; + + float rotationRadians = Rotation * Deg2Rad; // convert degrees to radians + float s = sin(rotationRadians); // sin and cos take radians, not degrees + float c = cos(rotationRadians); + + float2x2 rotationMatrix = float2x2( c, -s, s, c); // construct simple rotation matrix + v.uv -= 0.5; // offset UV so we rotate around 0.5 and not 0.0 + v.uv = mul(rotationMatrix, v.uv); // apply rotation matrix + v.uv += 0.5; // offset UV again so UVs are in the correct location + + o.uv = TRANSFORM_TEX(v.uv, _MainTex); + UNITY_TRANSFER_FOG(o,o.vertex); + return o; + } + + fixed4 frag (v2f i) : SV_Target + { + // sample the texture + fixed4 col = tex2D(_MainTex, i.uv); + // apply fog + UNITY_APPLY_FOG(i.fogCoord, col); + return col; + } + ENDCG + } + } +} + + +// Shader "Example/URPUnlitShaderTexture" +// { +// // The _BaseMap variable is visible in the Material's Inspector, as a field +// // called Base Map. +// Properties +// { +// _BaseMap("Base Map", 2D) = "white" +// } + +// SubShader +// { +// Tags { "RenderType" = "Opaque" "RenderPipeline" = "UniversalRenderPipeline" } + +// Pass +// { +// HLSLPROGRAM +// #pragma vertex vert +// #pragma fragment frag + +// #include "Packages/com.unity.render-pipelines.universal/ShaderLibrary/Core.hlsl" + +// struct Attributes +// { +// float4 positionOS : POSITION; +// // The uv variable contains the UV coordinate on the texture for the +// // given vertex. +// float2 uv : TEXCOORD0; +// }; + +// struct Varyings +// { +// float4 positionHCS : SV_POSITION; +// // The uv variable contains the UV coordinate on the texture for the +// // given vertex. +// float2 uv : TEXCOORD0; +// }; + +// // This macro declares _BaseMap as a Texture2D object. +// TEXTURE2D(_BaseMap); +// // This macro declares the sampler for the _BaseMap texture. +// SAMPLER(sampler_BaseMap); + +// CBUFFER_START(UnityPerMaterial) +// // The following line declares the _BaseMap_ST variable, so that you +// // can use the _BaseMap variable in the fragment shader. The _ST +// // suffix is necessary for the tiling and offset function to work. +// float4 _BaseMap_ST; +// CBUFFER_END + +// Varyings vert(Attributes IN) +// { +// Varyings OUT; +// OUT.positionHCS = TransformObjectToHClip(IN.positionOS.xyz); +// // The TRANSFORM_TEX macro performs the tiling and offset +// // transformation. +// OUT.uv = TRANSFORM_TEX(IN.uv, _BaseMap); +// return OUT; +// } + +// half4 frag(Varyings IN) : SV_Target +// { +// // The SAMPLE_TEXTURE2D marco samples the texture with the given +// // sampler. +// half4 color = SAMPLE_TEXTURE2D(_BaseMap, sampler_BaseMap, IN.uv); +// return color; +// } +// ENDHLSL +// } +// } +// } \ No newline at end of file diff --git a/Assets/Resources/Shader/UnlitVideoTexture.shader.meta b/Assets/Resources/Shader/UnlitVideoTexture.shader.meta new file mode 100644 index 00000000..c16c020f --- /dev/null +++ b/Assets/Resources/Shader/UnlitVideoTexture.shader.meta @@ -0,0 +1,9 @@ +fileFormatVersion: 2 +guid: deae5ca476ee55d65b15a5f91a73287d +ShaderImporter: + externalObjects: {} + defaultTextures: [] + nonModifiableTextures: [] + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs index 06bd8788..ba1b6fb2 100644 --- a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs @@ -10,6 +10,7 @@ using System; using Any = cloisim.msgs.Any; using UnityEngine; +using UnityEngine.Video; public class MicomPlugin : CLOiSimPlugin { @@ -65,7 +66,6 @@ protected override void OnStart() Debug.Log(_log.ToString()); } - protected override void OnReset() { _motorControl?.Reset(); @@ -131,6 +131,51 @@ private void SetupMicom() { _micomSensor.SetIMU(targetImuSensorName); } + + if (GetPluginParameters().IsValidNode("display")) + { + SetDisplay(); + } + } + + private void SetDisplay() + { + const float meshScalingFactor = 1000f; + var targetVisual = GetPluginParameters().GetValue("display/target/visual", string.Empty); + + var visualHelpers = GetComponentsInChildren(); + foreach (var visualHelper in visualHelpers) + { + if (visualHelper.name.Equals(targetVisual)) + { + var meshFilter = visualHelper.GetComponentInChildren(); + var mesh = meshFilter.sharedMesh; + var displaySize = mesh.bounds.size; + var videoWidth = (int)(displaySize.x * meshScalingFactor); + var videoHeight = (int)(displaySize.z * meshScalingFactor); + + var renderTexture = new RenderTexture(videoWidth, videoHeight, 0); + renderTexture.name = "VideoTexture"; + + var meshRenderer = visualHelper.GetComponentInChildren(); + var shader = Shader.Find("Custom/Unlit/VideoTexture"); + meshRenderer.material = new Material(shader); + meshRenderer.sharedMaterial.SetTexture("_MainTex", renderTexture); + + var sourceUri = GetPluginParameters().GetValue("display/source/uri", string.Empty); + var videoPlayer = visualHelper.gameObject.AddComponent(); + videoPlayer.audioOutputMode = VideoAudioOutputMode.None; + videoPlayer.isLooping = true; + videoPlayer.source = VideoSource.Url; + videoPlayer.playOnAwake = true; + videoPlayer.waitForFirstFrame = true; + videoPlayer.url = sourceUri; + videoPlayer.renderMode = VideoRenderMode.RenderTexture; + videoPlayer.targetTexture = renderTexture; + videoPlayer.aspectRatio = VideoAspectRatio.Stretch; + break; + } + } } private void SetSelfBalancedWheel(in string parameterPrefix) diff --git a/Packages/manifest.json b/Packages/manifest.json index c4591b58..b5dd18ad 100644 --- a/Packages/manifest.json +++ b/Packages/manifest.json @@ -28,6 +28,7 @@ "com.unity.modules.umbra": "1.0.0", "com.unity.modules.unityanalytics": "1.0.0", "com.unity.modules.unitywebrequestwww": "1.0.0", + "com.unity.modules.video": "1.0.0", "com.unity.modules.wind": "1.0.0" } } diff --git a/Packages/packages-lock.json b/Packages/packages-lock.json index fd6c6524..185b5d0d 100644 --- a/Packages/packages-lock.json +++ b/Packages/packages-lock.json @@ -310,6 +310,16 @@ "com.unity.modules.imageconversion": "1.0.0" } }, + "com.unity.modules.video": { + "version": "1.0.0", + "depth": 0, + "source": "builtin", + "dependencies": { + "com.unity.modules.audio": "1.0.0", + "com.unity.modules.ui": "1.0.0", + "com.unity.modules.unitywebrequest": "1.0.0" + } + }, "com.unity.modules.wind": { "version": "1.0.0", "depth": 0, From b87abad8195522cd1638fbdaeb67261313acde33 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Thu, 12 Dec 2024 17:08:02 +0900 Subject: [PATCH 03/19] Modify SelfBalancedDrive to change adjust body rotaion value via parameters in SDF --- Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs | 6 ++++++ .../Motor/SelfBalanceControl/SelfBalancedDrive.cs | 11 ++++++++--- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs index ba1b6fb2..62e6f316 100644 --- a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs @@ -239,6 +239,12 @@ private void SetSelfBalancedWheel(in string parameterPrefix) (_motorControl as SelfBalancedDrive).SetBodyJoint(bodyJoint); } + if (GetPluginParameters().IsValidNode($"{parameterPrefix}/body/rotation/hip_adjust")) + { + var adjust = GetPluginParameters().GetValue($"{parameterPrefix}/body/rotation/hip_adjust", 1.88); + (_motorControl as SelfBalancedDrive).AdjustBodyRotation = adjust; + } + if (GetPluginParameters().IsValidNode($"{parameterPrefix}/smc")) { if (GetPluginParameters().IsValidNode($"{parameterPrefix}/smc/param")) diff --git a/Assets/Scripts/Devices/Modules/Motor/SelfBalanceControl/SelfBalancedDrive.cs b/Assets/Scripts/Devices/Modules/Motor/SelfBalanceControl/SelfBalancedDrive.cs index c577179e..ea79275a 100644 --- a/Assets/Scripts/Devices/Modules/Motor/SelfBalanceControl/SelfBalancedDrive.cs +++ b/Assets/Scripts/Devices/Modules/Motor/SelfBalanceControl/SelfBalancedDrive.cs @@ -23,6 +23,7 @@ public class SelfBalancedDrive : MotorControl #region Body Control private double _commandTargetBody = 0; // in deg + private double _adjustBodyRotation = 1.88; #endregion #region Roll/Height Control @@ -107,6 +108,12 @@ public bool Balancing set => _onBalancing = value; } + public double AdjustBodyRotation + { + get => _adjustBodyRotation; + set => _adjustBodyRotation = value; + } + public void DoResetPose() { _resetPose = true; @@ -291,8 +298,6 @@ private void AdjustHeadsetByPitch(in double currentPitch, in float duration) // Debug.LogWarning("Adjusting head by pitch"); } - private double adjustBody = 1.84; - private VectorXd ControlHipAndLeg(in double currentPitch) { var hipTarget = _commandTargetHeight * 0.5; @@ -301,7 +306,7 @@ private VectorXd ControlHipAndLeg(in double currentPitch) _commandHipTarget.x = hipTarget; _commandHipTarget.y = hipTarget; - _commandTargetBody = hipTarget * adjustBody; + _commandTargetBody = hipTarget * _adjustBodyRotation; // Debug.Log($"{hipTarget} {_commandTargetBody} "); _commandLegTarget.x = _commandTargetHeight; From 45da8409bd0d790bb1f6fbbd9cdbafa7b1b21611 Mon Sep 17 00:00:00 2001 From: Hyunseok Yang Date: Fri, 13 Dec 2024 18:13:24 +0900 Subject: [PATCH 04/19] Disable MSAA(Anti-aliasing) by default in RenderPipelines --- .../RenderPipelines/UniversalRenderPipelineAsset.asset | 2 +- ProjectSettings/QualitySettings.asset | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Assets/Resources/RenderPipelines/UniversalRenderPipelineAsset.asset b/Assets/Resources/RenderPipelines/UniversalRenderPipelineAsset.asset index c31e175b..1e57b244 100644 --- a/Assets/Resources/RenderPipelines/UniversalRenderPipelineAsset.asset +++ b/Assets/Resources/RenderPipelines/UniversalRenderPipelineAsset.asset @@ -26,7 +26,7 @@ MonoBehaviour: m_SupportsTerrainHoles: 0 m_SupportsHDR: 1 m_HDRColorBufferPrecision: 0 - m_MSAA: 4 + m_MSAA: 1 m_RenderScale: 1 m_UpscalingFilter: 0 m_FsrOverrideSharpness: 0 diff --git a/ProjectSettings/QualitySettings.asset b/ProjectSettings/QualitySettings.asset index 69643542..36272802 100644 --- a/ProjectSettings/QualitySettings.asset +++ b/ProjectSettings/QualitySettings.asset @@ -172,7 +172,7 @@ QualitySettings: globalTextureMipmapLimit: 0 textureMipmapLimitSettings: [] anisotropicTextures: 2 - antiAliasing: 4 + antiAliasing: 0 softParticles: 1 softVegetation: 1 realtimeReflectionProbes: 1 From 8a3339df7ce20b8087c001803677933dd029e77c Mon Sep 17 00:00:00 2001 From: Hyunseok Yang Date: Fri, 13 Dec 2024 18:33:10 +0900 Subject: [PATCH 05/19] Add included shader in GraphicsSettings --- ProjectSettings/GraphicsSettings.asset | 1 + 1 file changed, 1 insertion(+) diff --git a/ProjectSettings/GraphicsSettings.asset b/ProjectSettings/GraphicsSettings.asset index 40740aee..019b5420 100644 --- a/ProjectSettings/GraphicsSettings.asset +++ b/ProjectSettings/GraphicsSettings.asset @@ -32,6 +32,7 @@ GraphicsSettings: - {fileID: 4800000, guid: 419a697b1455619dea4cc9d729b3d0c8, type: 3} - {fileID: 4800000, guid: 53ff4d2ede887e2d0becbbf9a26eb2c9, type: 3} - {fileID: 4800000, guid: c4375bff1dff4da53a01e7b4cebbf573, type: 3} + - {fileID: 4800000, guid: deae5ca476ee55d65b15a5f91a73287d, type: 3} m_PreloadedShaders: - {fileID: 20000000, guid: 444c3b2060d68fa04a081d66c72c2066, type: 2} m_PreloadShadersBatchTimeLimit: -1 From 9402d2a930a4eeb0433375ee84b9703e95172a15 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Mon, 16 Dec 2024 12:21:31 +0900 Subject: [PATCH 06/19] Support display list in MicomPlugin --- Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs index 62e6f316..9959c1bc 100644 --- a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs @@ -21,6 +21,8 @@ public class MicomPlugin : CLOiSimPlugin private SDF.Helper.Link[] _linkHelperInChildren = null; private StringBuilder _log = new StringBuilder(); + private List _displaySourceUris = new List(); + protected override void OnAwake() { type = ICLOiSimPlugin.Type.MICOM; @@ -143,6 +145,20 @@ private void SetDisplay() const float meshScalingFactor = 1000f; var targetVisual = GetPluginParameters().GetValue("display/target/visual", string.Empty); + if (string.IsNullOrEmpty(targetVisual)) + { + _log.AppendLine("Failed to set display - Empty target visual for display"); + return; + } + + if (GetPluginParameters().GetValues("display/source/uri", out _displaySourceUris) == false) + { + _log.AppendLine("Failed to set display - Empty display source uri"); + return; + } + + var defaultSourceUri = GetPluginParameters().GetValue("display/source/uri[@default='true']", _displaySourceUris[0]); + var visualHelpers = GetComponentsInChildren(); foreach (var visualHelper in visualHelpers) { @@ -162,14 +178,13 @@ private void SetDisplay() meshRenderer.material = new Material(shader); meshRenderer.sharedMaterial.SetTexture("_MainTex", renderTexture); - var sourceUri = GetPluginParameters().GetValue("display/source/uri", string.Empty); var videoPlayer = visualHelper.gameObject.AddComponent(); videoPlayer.audioOutputMode = VideoAudioOutputMode.None; videoPlayer.isLooping = true; videoPlayer.source = VideoSource.Url; videoPlayer.playOnAwake = true; videoPlayer.waitForFirstFrame = true; - videoPlayer.url = sourceUri; + videoPlayer.url = defaultSourceUri; videoPlayer.renderMode = VideoRenderMode.RenderTexture; videoPlayer.targetTexture = renderTexture; videoPlayer.aspectRatio = VideoAspectRatio.Stretch; From e05fc5519164001f58a2fba72ed1041247a01c37 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Mon, 16 Dec 2024 14:58:25 +0900 Subject: [PATCH 07/19] Fix wrong value calculation for _commandTargetRollByDrive in SelfBalancedDrive.Drive() --- .../Motor/SelfBalanceControl/SelfBalancedDrive.cs | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/Assets/Scripts/Devices/Modules/Motor/SelfBalanceControl/SelfBalancedDrive.cs b/Assets/Scripts/Devices/Modules/Motor/SelfBalanceControl/SelfBalancedDrive.cs index ea79275a..f36fc172 100644 --- a/Assets/Scripts/Devices/Modules/Motor/SelfBalanceControl/SelfBalancedDrive.cs +++ b/Assets/Scripts/Devices/Modules/Motor/SelfBalanceControl/SelfBalancedDrive.cs @@ -232,8 +232,9 @@ public void ChangeWheelDriveType() public override void Drive(in float linearVelocity, in float angularVelocity) { const float BoostAngularSpeed = 3.0f; + _commandTwistLinear = linearVelocity; - _commandTwistAngular = SDF2Unity.CurveOrientationAngle(angularVelocity) * BoostAngularSpeed; + _commandTwistAngular = SDF2Unity.CurveOrientationAngle(angularVelocity); if (Math.Abs(_commandTwistLinear) < float.Epsilon || Math.Abs(_commandTwistAngular) < float.Epsilon) { @@ -243,11 +244,13 @@ public override void Drive(in float linearVelocity, in float angularVelocity) if (Math.Abs(_commandTwistLinear) > float.Epsilon && Math.Abs(_commandTwistAngular) > float.Epsilon) { - var ratio = _commandTwistAngular / _commandTwistLinear; - _commandTargetRollByDrive = ((ratio > 0) ? RollLimit.min : RollLimit.max) * -Math.Abs(ratio); + var ratio = Mathf.Clamp01(Mathf.Abs((float)(_commandTwistAngular/ _commandTwistLinear))); + _commandTargetRollByDrive = ((_commandTwistAngular > 0) ? RollLimit.max : RollLimit.min) * Math.Abs(ratio); // Debug.Log($"Command - linear: {_commandTwistLinear} angular: {_commandTwistAngular} ratio: {ratio} _commandTargetRollByDrive: {_commandTargetRollByDrive}"); } // Debug.Log($"Command - linear: {_commandTwistLinear} angular: {_commandTwistAngular} pitch: {PitchTarget}"); + + _commandTwistAngular *= BoostAngularSpeed; } private void UpdatePitchProfiler() From 60851b1a5a8621bfc008a7bf99caa86891fe1c88 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Mon, 16 Dec 2024 15:26:51 +0900 Subject: [PATCH 08/19] Modify to save world file on the location where the original world file is loaded. --- Assets/Scripts/Main.cs | 6 ++++-- Assets/Scripts/Tools/SDF/Parser/Root.cs | 21 ++++++++------------- Assets/Scripts/UI/UIController.cs | 4 ---- 3 files changed, 12 insertions(+), 19 deletions(-) diff --git a/Assets/Scripts/Main.cs b/Assets/Scripts/Main.cs index 20177b39..be69d8a3 100644 --- a/Assets/Scripts/Main.cs +++ b/Assets/Scripts/Main.cs @@ -23,6 +23,8 @@ public class Main : MonoBehaviour [SerializeField] private string _worldFilename; + private string _loadedWorldFilePath = string.Empty; + [SerializeField] private List _modelRootDirectories = new List(); @@ -421,7 +423,7 @@ private IEnumerator LoadWorld() // Debug.Log("Hello CLOiSim World!!!!!"); Debug.Log("Target World: " + _worldFilename); - if (_sdfRoot.DoParse(out var world, _worldFilename)) + if (_sdfRoot.DoParse(out var world, out _loadedWorldFilePath, _worldFilename)) { SDF.Import.Util.RootModels = _worldRoot; @@ -455,7 +457,7 @@ public void SaveWorld() var worldSaver = new WorldSaver(saveDoc); worldSaver.Update(); - _sdfRoot.Save(); + _sdfRoot.Save(_loadedWorldFilePath); } void LateUpdate() diff --git a/Assets/Scripts/Tools/SDF/Parser/Root.cs b/Assets/Scripts/Tools/SDF/Parser/Root.cs index 5567b155..cf53adce 100644 --- a/Assets/Scripts/Tools/SDF/Parser/Root.cs +++ b/Assets/Scripts/Tools/SDF/Parser/Root.cs @@ -50,16 +50,16 @@ public Root() Console.SetError(_loggerErr); } - public bool DoParse(out World world, in string worldFileName) + public bool DoParse(out World world, out string worldFilePath, in string worldFileName) { // Console.Write("Loading World File from SDF!!!!!"); world = null; + worldFilePath = string.Empty; if (worldFileName.Trim().Length <= 0) { return false; } - var worldFound = false; // Console.Write("World file, PATH: " + worldFileName); foreach (var worldPath in worldDefaultPaths) { @@ -80,12 +80,12 @@ public bool DoParse(out World world, in string worldFileName) // Console.Write("Load World"); var worldNode = _doc.SelectSingleNode("/sdf/world"); world = new World(worldNode); - worldFound = true; + worldFilePath = worldPath; var infoMessage = "World(" + worldFileName + ") is loaded."; _logger.Write(infoMessage); // _logger.SetShowOnDisplayOnce(); - break; + return true; } catch (XmlException ex) { @@ -96,12 +96,8 @@ public bool DoParse(out World world, in string worldFileName) } } - if (!worldFound) - { - _loggerErr.Write("World file not exist: " + worldFileName); - } - - return worldFound; + _loggerErr.Write("World file not exist: " + worldFileName); + return false; } public bool DoParse(out Model model, in string modelFullPath, in string modelFileName) @@ -508,13 +504,12 @@ private XmlNode GetIncludedModel(XmlNode included_node) return sdfNode; } - public void Save() + public void Save(in string filePath = "") { var fileName = Path.GetFileNameWithoutExtension(_worldFileName); var datetime = DateTime.Now.ToString("yyMMddHHmmss"); // DateTime.Now.ToString("yyyyMMddHHmmss"); - var saveName = fileName + datetime + ".world"; - + var saveName = $"{filePath}/{fileName}{datetime}.world"; _originalDoc.Save(saveName); Console.Write($"Worldfile Saved: {saveName}"); diff --git a/Assets/Scripts/UI/UIController.cs b/Assets/Scripts/UI/UIController.cs index 18ebb684..cf597a6d 100644 --- a/Assets/Scripts/UI/UIController.cs +++ b/Assets/Scripts/UI/UIController.cs @@ -4,13 +4,9 @@ * SPDX-License-Identifier: MIT */ -using System; -using System.Collections; -using System.Collections.Generic; using UnityEngine; using UnityEngine.UIElements; - public class UIController : MonoBehaviour { private UIDocument _uiDocument = null; From a19f2caef5303fa2c8fb4007be624cff30acf9b0 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Mon, 16 Dec 2024 17:18:46 +0900 Subject: [PATCH 09/19] Parse sensor in Joint element --- Assets/Scripts/Tools/SDF/Parser/Joint.cs | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/Assets/Scripts/Tools/SDF/Parser/Joint.cs b/Assets/Scripts/Tools/SDF/Parser/Joint.cs index 63d61352..8b9b68bc 100644 --- a/Assets/Scripts/Tools/SDF/Parser/Joint.cs +++ b/Assets/Scripts/Tools/SDF/Parser/Joint.cs @@ -121,7 +121,7 @@ public class ODE private Physics physics = new Physics(); - // private Sensors sensors = null; + private Sensors _sensors = null; public string ParentLinkName => parent; @@ -139,6 +139,8 @@ public Joint(XmlNode _node) protected override void ParseElements() { + _sensors = new Sensors(root); + parent = GetValue("parent"); child = GetValue("child"); From 790f9668e08e3174530a68cba26aa7f6ac44f5f6 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Mon, 16 Dec 2024 21:58:20 +0900 Subject: [PATCH 10/19] Modify CLOiSimPlugins Base - modify setting partsName field Modify DeviceHelper - add GetPartsName() remove GetPartName() --- .../CLOiSimPlugins/ActorControlPlugin.cs | 3 +- Assets/Scripts/CLOiSimPlugins/ActorPlugin.cs | 3 +- Assets/Scripts/CLOiSimPlugins/CameraPlugin.cs | 4 +-- .../Scripts/CLOiSimPlugins/ElevatorSystem.cs | 3 +- Assets/Scripts/CLOiSimPlugins/GpsPlugin.cs | 2 +- .../CLOiSimPlugins/GroundTruthPlugin.cs | 5 ++- Assets/Scripts/CLOiSimPlugins/ImuPlugin.cs | 2 +- .../CLOiSimPlugins/JointControlPlugin.cs | 1 + Assets/Scripts/CLOiSimPlugins/LaserPlugin.cs | 1 - .../Modules/Base/CLOiSimPlugin.Transport.cs | 36 +++++++++---------- .../Modules/Base/CLOiSimPlugin.cs | 9 ++--- Assets/Scripts/CLOiSimPlugins/MowingPlugin.cs | 5 ++- .../CLOiSimPlugins/MultiCameraPlugin.cs | 2 -- .../CLOiSimPlugins/ParticleSystemPlugin.cs | 9 ++--- .../Scripts/CLOiSimPlugins/RealSensePlugin.cs | 7 ++-- .../SegmentationCameraPlugin.cs | 2 -- Assets/Scripts/CLOiSimPlugins/SonarPlugin.cs | 2 +- Assets/Scripts/Core/Modules/BridgeManager.cs | 18 +++++----- Assets/Scripts/Core/SimulationWorld.cs | 5 ++- .../Devices/Modules/Base/DeviceHelper.cs | 28 ++++++++++++--- 20 files changed, 79 insertions(+), 68 deletions(-) diff --git a/Assets/Scripts/CLOiSimPlugins/ActorControlPlugin.cs b/Assets/Scripts/CLOiSimPlugins/ActorControlPlugin.cs index 85cc1711..ffc7cf27 100644 --- a/Assets/Scripts/CLOiSimPlugins/ActorControlPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/ActorControlPlugin.cs @@ -21,7 +21,8 @@ public class ActorControlPlugin : CLOiSimPlugin protected override void OnAwake() { type = ICLOiSimPlugin.Type.ACTOR; - partsName = "ActorControlPlugin"; + modelName = "World"; + partsName = this.GetType().Name; UpdateActorList(); } diff --git a/Assets/Scripts/CLOiSimPlugins/ActorPlugin.cs b/Assets/Scripts/CLOiSimPlugins/ActorPlugin.cs index ad5ba54b..05096be1 100644 --- a/Assets/Scripts/CLOiSimPlugins/ActorPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/ActorPlugin.cs @@ -12,7 +12,8 @@ public class ActorPlugin : CLOiSimPlugin protected override void OnAwake() { type = ICLOiSimPlugin.Type.ACTOR; - partsName = "actorplugin"; + modelName = "World"; + partsName = this.GetType().Name; } protected override void OnStart() diff --git a/Assets/Scripts/CLOiSimPlugins/CameraPlugin.cs b/Assets/Scripts/CLOiSimPlugins/CameraPlugin.cs index b7f72dfe..adc63681 100644 --- a/Assets/Scripts/CLOiSimPlugins/CameraPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/CameraPlugin.cs @@ -29,9 +29,9 @@ protected override void OnAwake() } if (!string.IsNullOrEmpty(deviceName)) + { attachedDevices.Add(deviceName, _cam); - - partsName = DeviceHelper.GetPartName(gameObject); + } } protected override void OnStart() diff --git a/Assets/Scripts/CLOiSimPlugins/ElevatorSystem.cs b/Assets/Scripts/CLOiSimPlugins/ElevatorSystem.cs index f1938f81..e951f5e2 100644 --- a/Assets/Scripts/CLOiSimPlugins/ElevatorSystem.cs +++ b/Assets/Scripts/CLOiSimPlugins/ElevatorSystem.cs @@ -53,7 +53,8 @@ public ElevatorTask(in string index = "") protected override void OnAwake() { type = ICLOiSimPlugin.Type.ELEVATOR; - partsName = "elevator_system"; + modelName = "World"; + partsName = this.GetType().Name; } protected override void OnStart() diff --git a/Assets/Scripts/CLOiSimPlugins/GpsPlugin.cs b/Assets/Scripts/CLOiSimPlugins/GpsPlugin.cs index 7f6cd728..d26f97c5 100644 --- a/Assets/Scripts/CLOiSimPlugins/GpsPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/GpsPlugin.cs @@ -13,9 +13,9 @@ public class GpsPlugin : CLOiSimPlugin protected override void OnAwake() { type = ICLOiSimPlugin.Type.GPS; + gps = gameObject.GetComponent(); attachedDevices.Add("GPS", gps); - partsName = DeviceHelper.GetPartName(gameObject); } protected override void OnStart() diff --git a/Assets/Scripts/CLOiSimPlugins/GroundTruthPlugin.cs b/Assets/Scripts/CLOiSimPlugins/GroundTruthPlugin.cs index f242ab46..a78bffe1 100644 --- a/Assets/Scripts/CLOiSimPlugins/GroundTruthPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/GroundTruthPlugin.cs @@ -105,9 +105,8 @@ private UE.GameObject GetTrackingObject(in string modelName) protected override void OnAwake() { type = ICLOiSimPlugin.Type.GROUNDTRUTH; - - modelName = "GroundTruth"; - partsName = "tracking"; + modelName = "World"; + partsName = this.GetType().Name; var worldRoot = Main.WorldRoot; foreach (var model in worldRoot.GetComponentsInChildren()) diff --git a/Assets/Scripts/CLOiSimPlugins/ImuPlugin.cs b/Assets/Scripts/CLOiSimPlugins/ImuPlugin.cs index e9c0b14b..84ad8bfc 100644 --- a/Assets/Scripts/CLOiSimPlugins/ImuPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/ImuPlugin.cs @@ -13,9 +13,9 @@ public class ImuPlugin : CLOiSimPlugin protected override void OnAwake() { type = ICLOiSimPlugin.Type.IMU; + imu = gameObject.GetComponent(); attachedDevices.Add("IMU", imu); - partsName = DeviceHelper.GetPartName(gameObject); } protected override void OnStart() diff --git a/Assets/Scripts/CLOiSimPlugins/JointControlPlugin.cs b/Assets/Scripts/CLOiSimPlugins/JointControlPlugin.cs index e42c24f0..fea6b4a2 100644 --- a/Assets/Scripts/CLOiSimPlugins/JointControlPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/JointControlPlugin.cs @@ -18,6 +18,7 @@ public class JointControlPlugin : CLOiSimPlugin protected override void OnAwake() { type = ICLOiSimPlugin.Type.JOINTCONTROL; + jointState = gameObject.AddComponent(); jointCommand = gameObject.AddComponent(); jointCommand.SetJointState(jointState); diff --git a/Assets/Scripts/CLOiSimPlugins/LaserPlugin.cs b/Assets/Scripts/CLOiSimPlugins/LaserPlugin.cs index 87557cb6..bb4c85fc 100644 --- a/Assets/Scripts/CLOiSimPlugins/LaserPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/LaserPlugin.cs @@ -14,7 +14,6 @@ public class LaserPlugin : CLOiSimPlugin protected override void OnAwake() { type = ICLOiSimPlugin.Type.LASER; - partsName = DeviceHelper.GetPartName(gameObject); lidar = GetComponent(); attachedDevices.Add("LIDAR", lidar); diff --git a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.Transport.cs b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.Transport.cs index ca8689cc..49e33960 100644 --- a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.Transport.cs +++ b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.Transport.cs @@ -9,19 +9,19 @@ public abstract partial class CLOiSimPlugin : MonoBehaviour, ICLOiSimPlugin { - private Transporter transport = new Transporter(); + private Transporter _transport = new Transporter(); - private string subPartsName = string.Empty; + private string _subPartsName = string.Empty; public string SubPartsName { - get => this.subPartsName; - set => this.subPartsName = value; + get => this._subPartsName; + set => this._subPartsName = value; } public Transporter GetTransport() { - return transport; + return _transport; } private bool PrepareDevice(in string subPartsAndKey, out ushort port, out ulong hash) @@ -36,7 +36,7 @@ private bool PrepareDevice(in string subPartsAndKey, out ushort port, out ulong return true; } - Debug.LogError("Port for device is not allocated!!!!!!!! - " + hashKey); + Debug.LogError($"Port for device is not allocated !!! {hashKey}"); hash = 0; return false; } @@ -49,52 +49,52 @@ protected static bool DeregisterDevice(in List allocatedPorts, in List allocatedDevicePorts = new List(); private List allocatedDeviceHashKeys = new List(); @@ -50,7 +49,7 @@ protected virtual void OnPluginLoad() { } protected void OnDestroy() { thread.Dispose(); - transport.Dispose(); + _transport.Dispose(); DeregisterDevice(allocatedDevicePorts, allocatedDeviceHashKeys); // Debug.Log($"({type.ToString()}){name}, CLOiSimPlugin destroyed."); @@ -102,9 +101,11 @@ void Start() if (string.IsNullOrEmpty(partsName)) { - partsName = _pluginParameters.Name; + partsName = DeviceHelper.GetPartsName(gameObject); } + // Debug.LogWarning($"modelName={modelName} partsName={partsName} pluginName={pluginName}"); + OnStart(); thread.Start(); diff --git a/Assets/Scripts/CLOiSimPlugins/MowingPlugin.cs b/Assets/Scripts/CLOiSimPlugins/MowingPlugin.cs index 3ef7a05b..8652c58e 100644 --- a/Assets/Scripts/CLOiSimPlugins/MowingPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/MowingPlugin.cs @@ -175,9 +175,8 @@ public Vector3 GetGrassOffset() protected override void OnAwake() { type = ICLOiSimPlugin.Type.NONE; - - modelName = "Mowing"; - partsName = "_"; + modelName = "World"; + partsName = this.GetType().Name; var geomGrassShader = Shader.Find("Custom/GeometryGrass"); _grass = new Grass(geomGrassShader); diff --git a/Assets/Scripts/CLOiSimPlugins/MultiCameraPlugin.cs b/Assets/Scripts/CLOiSimPlugins/MultiCameraPlugin.cs index 5753988a..f8b5d93b 100644 --- a/Assets/Scripts/CLOiSimPlugins/MultiCameraPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/MultiCameraPlugin.cs @@ -13,10 +13,8 @@ public class MultiCameraPlugin : CLOiSimPlugin protected override void OnAwake() { type = ICLOiSimPlugin.Type.MULTICAMERA; - partsName = DeviceHelper.GetPartName(gameObject); multiCam = gameObject.GetComponent(); - attachedDevices.Add("MultiCamera", multiCam); } diff --git a/Assets/Scripts/CLOiSimPlugins/ParticleSystemPlugin.cs b/Assets/Scripts/CLOiSimPlugins/ParticleSystemPlugin.cs index 00117c7d..8faa5594 100644 --- a/Assets/Scripts/CLOiSimPlugins/ParticleSystemPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/ParticleSystemPlugin.cs @@ -13,19 +13,14 @@ [RequireComponent(typeof(ParticleSystem))] public class ParticleSystemPlugin : CLOiSimPlugin { - // private GameObject _mowingList = null; - // private Transform _targetPlaneTranform = null; private ParticleSystem _particleSystem = null; private ParticleSystemRenderer _particleSystemRenderer = null; - // private Rect _planeSize = new Rect(); - protected override void OnAwake() { type = ICLOiSimPlugin.Type.NONE; - - modelName = "ParticleSystem"; - partsName = pluginName; + modelName = "World"; + partsName = this.GetType().Name; _particleSystem = this.gameObject.GetComponent(); _particleSystemRenderer = this.gameObject.GetComponent(); diff --git a/Assets/Scripts/CLOiSimPlugins/RealSensePlugin.cs b/Assets/Scripts/CLOiSimPlugins/RealSensePlugin.cs index 190d625b..230a0e6d 100644 --- a/Assets/Scripts/CLOiSimPlugins/RealSensePlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/RealSensePlugin.cs @@ -8,7 +8,6 @@ using System.Collections.Generic; using messages = cloisim.msgs; using Any = cloisim.msgs.Any; -using UnityEngine; public class RealSensePlugin : CLOiSimMultiPlugin { @@ -19,9 +18,10 @@ public class RealSensePlugin : CLOiSimMultiPlugin protected override void OnAwake() { type = ICLOiSimPlugin.Type.REALSENSE; + _partsName = name; + cameras = GetComponentsInChildren(); imu = GetComponentInChildren(); - partsName = name; } protected override void OnStart() @@ -90,12 +90,11 @@ private CameraPlugin FindAndAddCameraPlugin(in string name) { if (camera.name.Equals(name)) { + camera.SetSubParts(true); var plugin = camera.gameObject.AddComponent(); plugin.ChangePluginType(ICLOiSimPlugin.Type.REALSENSE); plugin.SubPartsName = name; - camera.SetSubParts(true); - AddCLOiSimPlugin(name, plugin); activatedModules.Add(new Tuple("camera", name)); return plugin; diff --git a/Assets/Scripts/CLOiSimPlugins/SegmentationCameraPlugin.cs b/Assets/Scripts/CLOiSimPlugins/SegmentationCameraPlugin.cs index db3d4e11..787bd2a5 100644 --- a/Assets/Scripts/CLOiSimPlugins/SegmentationCameraPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/SegmentationCameraPlugin.cs @@ -24,8 +24,6 @@ protected override void OnAwake() { Debug.LogError("SensorDevices.SegmentationCamera is missing."); } - - partsName = DeviceHelper.GetPartName(gameObject); } protected override void OnPluginLoad() diff --git a/Assets/Scripts/CLOiSimPlugins/SonarPlugin.cs b/Assets/Scripts/CLOiSimPlugins/SonarPlugin.cs index 20673226..b2f17ee1 100644 --- a/Assets/Scripts/CLOiSimPlugins/SonarPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/SonarPlugin.cs @@ -14,9 +14,9 @@ public class SonarPlugin : CLOiSimPlugin protected override void OnAwake() { type = ICLOiSimPlugin.Type.SONAR; + sonar = gameObject.GetComponent(); attachedDevices.Add("SONAR", sonar); - partsName = DeviceHelper.GetPartName(gameObject); } protected override void OnStart() diff --git a/Assets/Scripts/Core/Modules/BridgeManager.cs b/Assets/Scripts/Core/Modules/BridgeManager.cs index 75c031bc..b708722f 100644 --- a/Assets/Scripts/Core/Modules/BridgeManager.cs +++ b/Assets/Scripts/Core/Modules/BridgeManager.cs @@ -183,14 +183,16 @@ public static bool IsAvailablePort(in ushort port) return true; } - private static string MakeHashKey(in string modelName, in string partsName, in string subPartsName) + private static string MakeHashKey(params string[] data) { - return modelName + partsName + subPartsName; + return string.Join("", data); } - public static bool AllocateDevice(in string deviceType, in string modelName, in string partsName, in string subPartsName, out string hashKey, out ushort port) + public static bool AllocateDevice( + in string deviceType, in string modelName, in string partsName, in string subPartsNameAndKey, + out string hashKey, out ushort port) { - hashKey = MakeHashKey(modelName, partsName, subPartsName); + hashKey = MakeHashKey(modelName, partsName, subPartsNameAndKey); if (string.IsNullOrEmpty(hashKey)) { @@ -209,19 +211,19 @@ public static bool AllocateDevice(in string deviceType, in string modelName, in { if (partsMapTable.TryGetValue(partsName, out var portsMapTable)) { - portsMapTable.Add(subPartsName, port); + portsMapTable.Add(subPartsNameAndKey, port); } else { var newPortsMapTable = new Dictionary(); - newPortsMapTable.Add(subPartsName, port); + newPortsMapTable.Add(subPartsNameAndKey, port); partsMapTable.Add(partsName, newPortsMapTable); } } else { var portsMapTable = new Dictionary(); - portsMapTable.Add(subPartsName, port); + portsMapTable.Add(subPartsNameAndKey, port); var newPartsMapTable = new Dictionary>(); newPartsMapTable.Add(partsName, portsMapTable); @@ -231,7 +233,7 @@ public static bool AllocateDevice(in string deviceType, in string modelName, in else { var portsMapTable = new Dictionary(); - portsMapTable.Add(subPartsName, port); + portsMapTable.Add(subPartsNameAndKey, port); var partsMapTable = new Dictionary>(); partsMapTable.Add(partsName, portsMapTable); diff --git a/Assets/Scripts/Core/SimulationWorld.cs b/Assets/Scripts/Core/SimulationWorld.cs index c8242c05..1bc0e31f 100644 --- a/Assets/Scripts/Core/SimulationWorld.cs +++ b/Assets/Scripts/Core/SimulationWorld.cs @@ -13,12 +13,11 @@ public class SimulationWorld : CLOiSimPlugin protected override void OnAwake() { type = ICLOiSimPlugin.Type.WORLD; + modelName = "World"; + partsName = this.GetType().Name; clock = gameObject.GetComponent(); attachedDevices.Add("Clock", clock); - - modelName = "World"; - partsName = "cloisim"; } protected override void OnStart() diff --git a/Assets/Scripts/Devices/Modules/Base/DeviceHelper.cs b/Assets/Scripts/Devices/Modules/Base/DeviceHelper.cs index 985141ae..8c4f1a3c 100644 --- a/Assets/Scripts/Devices/Modules/Base/DeviceHelper.cs +++ b/Assets/Scripts/Devices/Modules/Base/DeviceHelper.cs @@ -38,7 +38,7 @@ public static SphericalCoordinates GetGlobalSphericalCoordinates() return globalSphericalCoordinates; } - public static string GetModelName(in GameObject targetObject, in bool searchOnlyOneDepth = false) + public static string GetModelName(in GameObject targetObject) { try { @@ -49,8 +49,7 @@ public static string GetModelName(in GameObject targetObject, in bool searchOnly nextObject = targetObject.GetComponentInParent(); } - if (searchOnlyOneDepth == false && nextObject != null && - !nextObject.CompareTag("Actor")) + if (nextObject != null && !nextObject.CompareTag("Actor")) { while (!SDF2Unity.IsRootModel(nextObject.transform)) { @@ -77,9 +76,28 @@ public static string GetModelName(in GameObject targetObject, in bool searchOnly } } - public static string GetPartName(in GameObject targetObject) + public static string GetPartsName(in GameObject targetObject) { - return GetModelName(targetObject, true); + try + { + if (targetObject.CompareTag("Model")) + { + return "MODEL"; + } + else if (targetObject.CompareTag("Sensor")) + { + return "SENSOR_" + targetObject.name; + } + else + { + return targetObject.name; + } + } + catch + { + Debug.LogError("Thee is no parent object model"); + return string.Empty; + } } [DllImport("StdHash")] From 53acda94dc5b9c27f64b1e92c5fe6d52cae2846a Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Tue, 17 Dec 2024 11:57:23 +0900 Subject: [PATCH 11/19] Modify GetIncludedModel() in SDF.Parser - parse element in element --- Assets/Scripts/Tools/SDF/Parser/Root.cs | 55 +++++++++++++------------ 1 file changed, 29 insertions(+), 26 deletions(-) diff --git a/Assets/Scripts/Tools/SDF/Parser/Root.cs b/Assets/Scripts/Tools/SDF/Parser/Root.cs index cf53adce..f157df61 100644 --- a/Assets/Scripts/Tools/SDF/Parser/Root.cs +++ b/Assets/Scripts/Tools/SDF/Parser/Root.cs @@ -82,16 +82,14 @@ public bool DoParse(out World world, out string worldFilePath, in string worldFi world = new World(worldNode); worldFilePath = worldPath; - var infoMessage = "World(" + worldFileName + ") is loaded."; - _logger.Write(infoMessage); + _logger.Write($"World({worldFileName}) is loaded."); // _logger.SetShowOnDisplayOnce(); return true; } catch (XmlException ex) { - var errorMessage = "Failed to Load World(" + fullFilePath + ") - " + ex.Message; _loggerErr.SetShowOnDisplayOnce(); - _loggerErr.Write(errorMessage); + _loggerErr.Write($"Failed to Load World({fullFilePath}) - {ex.Message}"); } } } @@ -187,7 +185,7 @@ public void UpdateResourceModelTable() } catch (XmlException e) { - Console.Write("Failed to Load model file(" + modelConfig + ") - " + e.Message); + Console.Write($"Failed to Load model file({modelConfig}) - {e.Message}"); continue; } @@ -206,7 +204,7 @@ public void UpdateResourceModelTable() { // Console.Write(version); // Console.Write(modelNode); - var sdfNode = modelNode.SelectSingleNode("sdf[@version=" + version + " or not(@version)]"); + var sdfNode = modelNode.SelectSingleNode($"sdf[@version={version} or not(@version)]"); if (sdfNode != null) { sdfFileName = sdfNode.InnerText; @@ -218,7 +216,7 @@ public void UpdateResourceModelTable() if (string.IsNullOrEmpty(sdfFileName)) { - Console.Write(modelName + ": SDF FileName is empty!!"); + Console.Write($"{modelName}: SDF FileName is empty!!"); continue; } @@ -399,31 +397,31 @@ private void StoreOriginalModelName(in XmlDocument targetDoc, in string modelNam } #endregion - private XmlNode GetIncludedModel(XmlNode included_node) + private XmlNode GetIncludedModel(XmlNode includedNode) { - var uri_node = included_node.SelectSingleNode("uri"); - if (uri_node == null) + var uriNode = includedNode.SelectSingleNode("uri"); + if (uriNode == null) { Console.Write("uri is empty."); return null; } - var nameNode = included_node.SelectSingleNode("name"); + var nameNode = includedNode.SelectSingleNode("name"); var name = (nameNode == null) ? null : nameNode.InnerText; - var staticNode = included_node.SelectSingleNode("static"); + var staticNode = includedNode.SelectSingleNode("static"); var isStatic = (staticNode == null) ? null : staticNode.InnerText; - var placementFrameNode = included_node.SelectSingleNode("placement_frame"); + var placementFrameNode = includedNode.SelectSingleNode("placement_frame"); var placementFrame = (placementFrameNode == null) ? null : placementFrameNode.InnerText; - var poseNode = included_node.SelectSingleNode("pose"); + var poseNode = includedNode.SelectSingleNode("pose"); var pose = (poseNode == null) ? null : poseNode.InnerText; - // var pluginNode = included_node.SelectSingleNode("plugin"); + var pluginNode = includedNode.SelectSingleNode("plugin"); // var plugin = (pluginNode == null) ? null : pluginNode.InnerText; - var uri = uri_node.InnerText; + var uri = uriNode.InnerText; var modelName = uri.Replace(ProtocolModel, string.Empty); if (resourceModelTable.TryGetValue(modelName, out var value)) @@ -444,9 +442,8 @@ private XmlNode GetIncludedModel(XmlNode included_node) } catch (XmlException e) { - var errorMessage = "Failed to Load included model(" + modelName + ") file - " + e.Message; _loggerErr.SetShowOnDisplayOnce(); - _loggerErr.Write(errorMessage); + _loggerErr.Write($"Failed to Load included model({modelName}) file - {e.Message}"); return null; } @@ -469,16 +466,16 @@ private XmlNode GetIncludedModel(XmlNode included_node) // Edit custom parameter if (nameNode != null) { - sdfNode.Attributes["name"].Value = name; + attributes.GetNamedItem("name").Value = name; } if (poseNode != null) { - if (sdfNode.SelectSingleNode("pose") != null) + var poseElem = sdfNode.SelectSingleNode("pose"); + if (poseElem != null) { - sdfNode.SelectSingleNode("pose").InnerText = pose; + poseElem.InnerText = pose; } - else { var elem = sdfNode.OwnerDocument.CreateElement("pose"); @@ -489,9 +486,10 @@ private XmlNode GetIncludedModel(XmlNode included_node) if (staticNode != null) { - if (sdfNode.SelectSingleNode("static") != null) + var staticElem = sdfNode.SelectSingleNode("static"); + if (staticElem != null) { - sdfNode.SelectSingleNode("static").InnerText = isStatic; + staticElem.InnerText = isStatic; } else { @@ -501,6 +499,11 @@ private XmlNode GetIncludedModel(XmlNode included_node) } } + if (pluginNode != null) + { + sdfNode.InsertBefore(pluginNode, sdfNode.LastChild); + } + return sdfNode; } @@ -518,8 +521,8 @@ public void Save(in string filePath = "") public void Print() { // Print all SDF contents - StringWriter sw = new StringWriter(); - XmlTextWriter xw = new XmlTextWriter(sw); + var sw = new StringWriter(); + var xw = new XmlTextWriter(sw); _doc.WriteTo(xw); Console.Write(sw.ToString()); } From 9eb748837055c17884e1bcca6829d94ee5bb8dfd Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Tue, 17 Dec 2024 12:09:00 +0900 Subject: [PATCH 12/19] Modify field in CLOiSimPlugin - modelName -> _modelName - partsName -> _partsName --- .../CLOiSimPlugins/ActorControlPlugin.cs | 4 +-- Assets/Scripts/CLOiSimPlugins/ActorPlugin.cs | 4 +-- .../Scripts/CLOiSimPlugins/ElevatorSystem.cs | 4 +-- .../CLOiSimPlugins/GroundTruthPlugin.cs | 4 +-- Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs | 4 +-- .../Base/CLOiSimPlugin.CommonMethod.cs | 12 +++---- .../Modules/Base/CLOiSimPlugin.Thread.cs | 12 +++---- .../Modules/Base/CLOiSimPlugin.Transport.cs | 11 ++++--- .../Modules/Base/CLOiSimPlugin.cs | 32 +++++++++++++------ .../Modules/Base/CLOiSimPluginThread.cs | 18 +++++------ Assets/Scripts/CLOiSimPlugins/MowingPlugin.cs | 4 +-- .../CLOiSimPlugins/ParticleSystemPlugin.cs | 4 +-- Assets/Scripts/Core/SimulationWorld.cs | 4 +-- 13 files changed, 66 insertions(+), 51 deletions(-) diff --git a/Assets/Scripts/CLOiSimPlugins/ActorControlPlugin.cs b/Assets/Scripts/CLOiSimPlugins/ActorControlPlugin.cs index ffc7cf27..bbc079a8 100644 --- a/Assets/Scripts/CLOiSimPlugins/ActorControlPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/ActorControlPlugin.cs @@ -21,8 +21,8 @@ public class ActorControlPlugin : CLOiSimPlugin protected override void OnAwake() { type = ICLOiSimPlugin.Type.ACTOR; - modelName = "World"; - partsName = this.GetType().Name; + _modelName = "World"; + _partsName = this.GetType().Name; UpdateActorList(); } diff --git a/Assets/Scripts/CLOiSimPlugins/ActorPlugin.cs b/Assets/Scripts/CLOiSimPlugins/ActorPlugin.cs index 05096be1..020e61e3 100644 --- a/Assets/Scripts/CLOiSimPlugins/ActorPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/ActorPlugin.cs @@ -12,8 +12,8 @@ public class ActorPlugin : CLOiSimPlugin protected override void OnAwake() { type = ICLOiSimPlugin.Type.ACTOR; - modelName = "World"; - partsName = this.GetType().Name; + _modelName = "World"; + _partsName = this.GetType().Name; } protected override void OnStart() diff --git a/Assets/Scripts/CLOiSimPlugins/ElevatorSystem.cs b/Assets/Scripts/CLOiSimPlugins/ElevatorSystem.cs index e951f5e2..3cd676f2 100644 --- a/Assets/Scripts/CLOiSimPlugins/ElevatorSystem.cs +++ b/Assets/Scripts/CLOiSimPlugins/ElevatorSystem.cs @@ -53,8 +53,8 @@ public ElevatorTask(in string index = "") protected override void OnAwake() { type = ICLOiSimPlugin.Type.ELEVATOR; - modelName = "World"; - partsName = this.GetType().Name; + _modelName = "World"; + _partsName = this.GetType().Name; } protected override void OnStart() diff --git a/Assets/Scripts/CLOiSimPlugins/GroundTruthPlugin.cs b/Assets/Scripts/CLOiSimPlugins/GroundTruthPlugin.cs index a78bffe1..e60064be 100644 --- a/Assets/Scripts/CLOiSimPlugins/GroundTruthPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/GroundTruthPlugin.cs @@ -105,8 +105,8 @@ private UE.GameObject GetTrackingObject(in string modelName) protected override void OnAwake() { type = ICLOiSimPlugin.Type.GROUNDTRUTH; - modelName = "World"; - partsName = this.GetType().Name; + _modelName = "World"; + _partsName = this.GetType().Name; var worldRoot = Main.WorldRoot; foreach (var model in worldRoot.GetComponentsInChildren()) diff --git a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs index 9959c1bc..002ef8c1 100644 --- a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs @@ -436,7 +436,7 @@ private void SetBattery() private void LoadStaticTF() { - _log.AppendLine("Loaded Static TF Info : " + modelName); + _log.AppendLine("Loaded Static TF Info : " + _modelName); if (GetPluginParameters().GetValues("ros2/static_transforms/link", out var staticLinks)) { @@ -464,7 +464,7 @@ private void LoadStaticTF() private void LoadTF() { - _log.AppendLine("Loaded TF Info : " + modelName); + _log.AppendLine("Loaded TF Info : " + _modelName); if (GetPluginParameters().GetValues("ros2/transforms/link", out var links)) { diff --git a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs index b7594a45..ee945083 100644 --- a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs +++ b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs @@ -158,7 +158,7 @@ protected static void SetEmptyResponse(ref DeviceMessage msRos2Info) private void SetCustomHandleRequestMessage() { - thread.HandleRequestTypeValue = delegate (in string requestType, in Any requestValue, ref DeviceMessage response) + _thread.HandleRequestTypeValue = delegate (in string requestType, in Any requestValue, ref DeviceMessage response) { switch (requestType) { @@ -168,19 +168,19 @@ private void SetCustomHandleRequestMessage() var topic_name = GetPluginParameters().GetValue("ros2/topic_name[@add_parts_name_prefix='true']"); if (string.IsNullOrEmpty(topic_name)) { - topic_name = GetPluginParameters().GetValue("ros2/topic_name", partsName); - topic_name = topic_name.Replace("{parts_name}", partsName); + topic_name = GetPluginParameters().GetValue("ros2/topic_name", _partsName); + topic_name = topic_name.Replace("{parts_name}", _partsName); } else { - topic_name = partsName + "/" + topic_name; + topic_name = _partsName + "/" + topic_name; } GetPluginParameters().GetValues("ros2/frame_id", out var frameIdList); for (var i = 0; i < frameIdList.Count; i++) { - frameIdList[i] = frameIdList[i].Replace("{parts_name}", partsName); + frameIdList[i] = frameIdList[i].Replace("{parts_name}", _partsName); } SetROS2CommonInfoResponse(ref response, topic_name, frameIdList); @@ -197,7 +197,7 @@ private void SetCustomHandleRequestMessage() } }; - thread.HandleRequestTypeChildren = delegate (in string requestType, in List requestChildren, ref DeviceMessage response) + _thread.HandleRequestTypeChildren = delegate (in string requestType, in List requestChildren, ref DeviceMessage response) { HandleCustomRequestMessage(requestType, requestChildren, ref response); }; diff --git a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.Thread.cs b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.Thread.cs index 36729854..20001cc9 100644 --- a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.Thread.cs +++ b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.Thread.cs @@ -10,12 +10,12 @@ public abstract partial class CLOiSimPlugin : MonoBehaviour, ICLOiSimPlugin { - private CLOiSimPluginThread thread = new CLOiSimPluginThread(); - protected CLOiSimPluginThread PluginThread => thread; + private CLOiSimPluginThread _thread = new CLOiSimPluginThread(); + protected CLOiSimPluginThread PluginThread => _thread; protected bool AddThread(in ushort targetPortForThread, in ParameterizedThreadStart function, in System.Object paramObject = null) { - return thread.Add(targetPortForThread, function, paramObject); + return _thread.Add(targetPortForThread, function, paramObject); } protected void SenderThread(System.Object threadObject) @@ -24,7 +24,7 @@ protected void SenderThread(System.Object threadObject) var publisher = GetTransport().Get(paramObject.targetPort); var deviceParam = paramObject.param as Device; - thread.Sender(publisher, deviceParam); + _thread.Sender(publisher, deviceParam); } protected void ReceiverThread(System.Object threadObject) @@ -33,7 +33,7 @@ protected void ReceiverThread(System.Object threadObject) var subscriber = GetTransport().Get(paramObject.targetPort); var deviceParam = paramObject.param as Device; - thread.Receiver(subscriber, deviceParam); + _thread.Receiver(subscriber, deviceParam); } protected void ServiceThread(System.Object threadObject) @@ -41,6 +41,6 @@ protected void ServiceThread(System.Object threadObject) var paramObject = threadObject as CLOiSimPluginThread.ParamObject; var responsor = GetTransport().Get(paramObject.targetPort); - thread.Service(responsor); + _thread.Service(responsor); } } \ No newline at end of file diff --git a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.Transport.cs b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.Transport.cs index 49e33960..f9d9e59a 100644 --- a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.Transport.cs +++ b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.Transport.cs @@ -11,6 +11,7 @@ public abstract partial class CLOiSimPlugin : MonoBehaviour, ICLOiSimPlugin { private Transporter _transport = new Transporter(); + [SerializeField] private string _subPartsName = string.Empty; public string SubPartsName @@ -26,7 +27,7 @@ public Transporter GetTransport() private bool PrepareDevice(in string subPartsAndKey, out ushort port, out ulong hash) { - if (BridgeManager.AllocateDevice(type.ToString(), modelName, partsName, subPartsAndKey, out var hashKey, out port)) + if (BridgeManager.AllocateDevice(type.ToString(), _modelName, _partsName, subPartsAndKey, out var hashKey, out port)) { allocatedDeviceHashKeys.Add(hashKey); allocatedDevicePorts.Add(port); @@ -55,7 +56,7 @@ protected bool RegisterTxDevice(out ushort port, in string key = "") return true; } - Debug.LogErrorFormat($"Failed to register Tx device {modelName}, {partsName}, {_subPartsName}"); + Debug.LogErrorFormat($"Failed to register Tx device {_modelName}, {_partsName}, {_subPartsName}"); return false; } @@ -68,7 +69,7 @@ protected bool RegisterRxDevice(out ushort port, in string key = "") return true; } - Debug.LogErrorFormat($"Failed to register Rx device {modelName}, {partsName}, {_subPartsName}"); + Debug.LogErrorFormat($"Failed to register Rx device {_modelName}, {_partsName}, {_subPartsName}"); return false; } @@ -81,7 +82,7 @@ protected bool RegisterServiceDevice(out ushort port, in string key = "") return true; } - Debug.LogErrorFormat($"Failed to register service device {modelName}, {partsName}, {_subPartsName}"); + Debug.LogErrorFormat($"Failed to register service device {_modelName}, {_partsName}, {_subPartsName}"); return false; } @@ -94,7 +95,7 @@ protected bool RegisterClientDevice(out ushort port, in string key = "") return true; } - Debug.LogErrorFormat($"Failed to register client device {modelName}, {partsName}, {_subPartsName}"); + Debug.LogErrorFormat($"Failed to register client device {_modelName}, {_partsName}, {_subPartsName}"); return false; } diff --git a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.cs b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.cs index 7741cfab..2524771d 100644 --- a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.cs @@ -11,7 +11,8 @@ public interface ICLOiSimPlugin { enum Type { NONE, - WORLD, GROUNDTRUTH, ELEVATOR, ACTOR, MICOM, JOINTCONTROL, + WORLD, GROUNDTRUTH, ELEVATOR, ACTOR, + MICOM, JOINTCONTROL, SENSOR, GPS, IMU, SONAR, LASER, CAMERA, DEPTHCAMERA, MULTICAMERA, REALSENSE, SEGMENTCAMERA }; void SetPluginParameters(in SDF.Plugin node); @@ -23,8 +24,21 @@ public abstract partial class CLOiSimPlugin : MonoBehaviour, ICLOiSimPlugin { public ICLOiSimPlugin.Type type { get; protected set; } - public string modelName { get; protected set; } = string.Empty; - public string partsName { get; protected set; } = string.Empty; + [SerializeField] + protected string _modelName = string.Empty; + + [SerializeField] + protected string _partsName = string.Empty; + + public string ModelName + { + get => _modelName; + } + + public string PartsName + { + get => _partsName; + } protected List staticTfList = new List(); @@ -48,7 +62,7 @@ protected virtual void OnPluginLoad() { } protected void OnDestroy() { - thread.Dispose(); + _thread.Dispose(); _transport.Dispose(); DeregisterDevice(allocatedDevicePorts, allocatedDeviceHashKeys); @@ -94,21 +108,21 @@ void Start() { StorePose(); - if (string.IsNullOrEmpty(modelName)) + if (string.IsNullOrEmpty(_modelName)) { - modelName = DeviceHelper.GetModelName(gameObject); + _modelName = DeviceHelper.GetModelName(gameObject); } - if (string.IsNullOrEmpty(partsName)) + if (string.IsNullOrEmpty(_partsName)) { - partsName = DeviceHelper.GetPartsName(gameObject); + _partsName = DeviceHelper.GetPartsName(gameObject); } // Debug.LogWarning($"modelName={modelName} partsName={partsName} pluginName={pluginName}"); OnStart(); - thread.Start(); + _thread.Start(); } public void Reset() diff --git a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPluginThread.cs b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPluginThread.cs index a921241a..d8685e9e 100644 --- a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPluginThread.cs +++ b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPluginThread.cs @@ -26,10 +26,10 @@ public ParamObject(in ushort targetPort, in System.Object parameter) } } - private List<(Thread, ParamObject)> threadList = new List<(Thread, ParamObject)>(); + private List<(Thread, ParamObject)> _threadList = new List<(Thread, ParamObject)>(); - private bool runningThread = true; - public bool IsRunning => runningThread; + private bool _runningThread = true; + public bool IsRunning => _runningThread; public delegate void RefAction(in T1 arg1, in T2 arg2, ref T3 arg3); public delegate void RefAction(in T1 arg1, ref T2 arg3); @@ -56,7 +56,7 @@ public bool Add(in ushort targetPortForThread, in ParameterizedThreadStart funct var thread = new Thread(function); var threadObject = new ParamObject(targetPortForThread, paramObject); // thread.Priority = System.Threading.ThreadPriority.AboveNormal; - threadList.Add((thread, threadObject)); + _threadList.Add((thread, threadObject)); return true; } @@ -65,9 +65,9 @@ public bool Add(in ushort targetPortForThread, in ParameterizedThreadStart funct public void Start() { - runningThread = true; + _runningThread = true; - foreach (var threadTuple in threadList) + foreach (var threadTuple in _threadList) { var thread = threadTuple.Item1; if (thread != null && !thread.IsAlive) @@ -80,9 +80,9 @@ public void Start() public void Stop() { - runningThread = false; + _runningThread = false; - foreach (var threadTuple in threadList) + foreach (var threadTuple in _threadList) { var thread = threadTuple.Item1; if (thread != null) @@ -94,7 +94,7 @@ public void Stop() } } } - threadList.Clear(); + _threadList.Clear(); } public void Sender(Publisher publisher, Device device) diff --git a/Assets/Scripts/CLOiSimPlugins/MowingPlugin.cs b/Assets/Scripts/CLOiSimPlugins/MowingPlugin.cs index 8652c58e..6d3c0fe0 100644 --- a/Assets/Scripts/CLOiSimPlugins/MowingPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/MowingPlugin.cs @@ -175,8 +175,8 @@ public Vector3 GetGrassOffset() protected override void OnAwake() { type = ICLOiSimPlugin.Type.NONE; - modelName = "World"; - partsName = this.GetType().Name; + _modelName = "World"; + _partsName = this.GetType().Name; var geomGrassShader = Shader.Find("Custom/GeometryGrass"); _grass = new Grass(geomGrassShader); diff --git a/Assets/Scripts/CLOiSimPlugins/ParticleSystemPlugin.cs b/Assets/Scripts/CLOiSimPlugins/ParticleSystemPlugin.cs index 8faa5594..79760d0f 100644 --- a/Assets/Scripts/CLOiSimPlugins/ParticleSystemPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/ParticleSystemPlugin.cs @@ -19,8 +19,8 @@ public class ParticleSystemPlugin : CLOiSimPlugin protected override void OnAwake() { type = ICLOiSimPlugin.Type.NONE; - modelName = "World"; - partsName = this.GetType().Name; + _modelName = "World"; + _partsName = this.GetType().Name; _particleSystem = this.gameObject.GetComponent(); _particleSystemRenderer = this.gameObject.GetComponent(); diff --git a/Assets/Scripts/Core/SimulationWorld.cs b/Assets/Scripts/Core/SimulationWorld.cs index 1bc0e31f..51433986 100644 --- a/Assets/Scripts/Core/SimulationWorld.cs +++ b/Assets/Scripts/Core/SimulationWorld.cs @@ -13,8 +13,8 @@ public class SimulationWorld : CLOiSimPlugin protected override void OnAwake() { type = ICLOiSimPlugin.Type.WORLD; - modelName = "World"; - partsName = this.GetType().Name; + _modelName = "World"; + _partsName = this.GetType().Name; clock = gameObject.GetComponent(); attachedDevices.Add("Clock", clock); From 5c5d5bd532a14289fe4b441619c56fdee53cf6b8 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Tue, 17 Dec 2024 17:07:25 +0900 Subject: [PATCH 13/19] Add Sec2NSec and NSec2Sec in DeviceHelper --- Assets/Scripts/Devices/Modules/Base/DeviceHelper.cs | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/Assets/Scripts/Devices/Modules/Base/DeviceHelper.cs b/Assets/Scripts/Devices/Modules/Base/DeviceHelper.cs index 8c4f1a3c..c441306b 100644 --- a/Assets/Scripts/Devices/Modules/Base/DeviceHelper.cs +++ b/Assets/Scripts/Devices/Modules/Base/DeviceHelper.cs @@ -13,6 +13,8 @@ public static partial class DeviceHelper { private static Clock globalClock = null; + private static double Sec2NSec = 1e+9; + private static double NSec2Sec = 1 / Sec2NSec; private static SphericalCoordinates globalSphericalCoordinates = null; @@ -84,10 +86,6 @@ public static string GetPartsName(in GameObject targetObject) { return "MODEL"; } - else if (targetObject.CompareTag("Sensor")) - { - return "SENSOR_" + targetObject.name; - } else { return targetObject.name; @@ -116,12 +114,12 @@ public static void Set(this messages.Time msg, in float time) } msg.Sec = (int)time; - msg.Nsec = (int)((time - (double)msg.Sec) * 1e+9); + msg.Nsec = (int)((time - (double)msg.Sec) * Sec2NSec); } public static float Get(this messages.Time msg) { - return (float)msg.Sec + ((float)msg.Nsec / (float)1e-9); + return (float)((double)msg.Sec + ((double)msg.Nsec * NSec2Sec)); } public static void SetCurrentTime(this messages.Time msg, in bool useRealTime = false) @@ -133,7 +131,7 @@ public static void SetCurrentTime(this messages.Time msg, in bool useRealTime = var timeNow = (useRealTime) ? GetGlobalClock().RealTime : GetGlobalClock().SimTime; msg.Sec = (int)timeNow; - msg.Nsec = (int)((timeNow - (double)msg.Sec) * 1e+9); + msg.Nsec = (int)((timeNow - (double)msg.Sec) * Sec2NSec); } public static void Set(this messages.Vector3d vector3d, in Vector3 position) From b30690e17134e4b79d40597ba23e2c74f5434fab Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Tue, 17 Dec 2024 17:16:55 +0900 Subject: [PATCH 14/19] modify parts name in MicomPlugin --- Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs | 1 + 1 file changed, 1 insertion(+) diff --git a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs index 002ef8c1..d695e3bc 100644 --- a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs @@ -26,6 +26,7 @@ public class MicomPlugin : CLOiSimPlugin protected override void OnAwake() { type = ICLOiSimPlugin.Type.MICOM; + _partsName = (GetPluginParameters() == null || string.IsNullOrEmpty(GetPluginParameters().Name)) ? "Micom" : GetPluginParameters().Name; _micomSensor = gameObject.AddComponent(); _micomCommand = gameObject.AddComponent(); From da0a1603c3720be252c67663b43816b5628ab7a2 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Tue, 17 Dec 2024 17:25:38 +0900 Subject: [PATCH 15/19] Fix RealSensePlugin - fix wrong parts name in each Modules --- .../Modules/Base/CLOiSimMultiPlugin.cs | 6 ++-- .../Modules/Base/CLOiSimPlugin.cs | 2 ++ .../Scripts/CLOiSimPlugins/RealSensePlugin.cs | 31 ++++++++++--------- 3 files changed, 21 insertions(+), 18 deletions(-) diff --git a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimMultiPlugin.cs b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimMultiPlugin.cs index dff609b6..23dbbc72 100644 --- a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimMultiPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimMultiPlugin.cs @@ -8,10 +8,10 @@ public abstract class CLOiSimMultiPlugin : CLOiSimPlugin { - private Dictionary CLOiSimPlugins = new Dictionary(); + private Dictionary _CLOiSimPlugins = new Dictionary(); - public void AddCLOiSimPlugin(in string deviceName, in CLOiSimPlugin CLOiSimPlugin) + public void AddPlugin(in string deviceName, in CLOiSimPlugin CLOiSimPlugin) { - CLOiSimPlugins.Add(deviceName, CLOiSimPlugin); + _CLOiSimPlugins.Add(deviceName, CLOiSimPlugin); } } \ No newline at end of file diff --git a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.cs b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.cs index 2524771d..93c396b3 100644 --- a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.cs @@ -33,11 +33,13 @@ public abstract partial class CLOiSimPlugin : MonoBehaviour, ICLOiSimPlugin public string ModelName { get => _modelName; + set => _modelName = value; } public string PartsName { get => _partsName; + set => _partsName = value; } protected List staticTfList = new List(); diff --git a/Assets/Scripts/CLOiSimPlugins/RealSensePlugin.cs b/Assets/Scripts/CLOiSimPlugins/RealSensePlugin.cs index 230a0e6d..0fece0ee 100644 --- a/Assets/Scripts/CLOiSimPlugins/RealSensePlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/RealSensePlugin.cs @@ -11,17 +11,17 @@ public class RealSensePlugin : CLOiSimMultiPlugin { - private SensorDevices.Camera[] cameras = null; - private SensorDevices.IMU imu = null; - private List> activatedModules = new List>(); + private SensorDevices.Camera[] _cameras = null; + private SensorDevices.IMU _imu = null; + private List> _activatedModules = new List>(); protected override void OnAwake() { type = ICLOiSimPlugin.Type.REALSENSE; _partsName = name; - cameras = GetComponentsInChildren(); - imu = GetComponentInChildren(); + _cameras = GetComponentsInChildren(); + _imu = GetComponentInChildren(); } protected override void OnStart() @@ -71,32 +71,33 @@ protected override void OnStart() private void AddImuPlugin(in string name) { - if (imu.name.Equals(name)) + if (_imu.name.Equals(name)) { - var plugin = imu.gameObject.AddComponent(); + _imu.SetSubParts(true); + var plugin = _imu.gameObject.AddComponent(); plugin.ChangePluginType(ICLOiSimPlugin.Type.REALSENSE); + plugin.PartsName = _partsName; plugin.SubPartsName = name; - imu.SetSubParts(true); - - AddCLOiSimPlugin(name, plugin); - activatedModules.Add(new Tuple("imu", name)); + AddPlugin(name, plugin); + _activatedModules.Add(new Tuple("imu", name)); } } private CameraPlugin FindAndAddCameraPlugin(in string name) { - foreach (var camera in cameras) + foreach (var camera in _cameras) { if (camera.name.Equals(name)) { camera.SetSubParts(true); var plugin = camera.gameObject.AddComponent(); plugin.ChangePluginType(ICLOiSimPlugin.Type.REALSENSE); + plugin.PartsName = _partsName; plugin.SubPartsName = name; - AddCLOiSimPlugin(name, plugin); - activatedModules.Add(new Tuple("camera", name)); + AddPlugin(name, plugin); + _activatedModules.Add(new Tuple("camera", name)); return plugin; } } @@ -134,7 +135,7 @@ private void SetModuleListInfoResponse(ref DeviceMessage msModuleInfo) modulesInfo.Name = "activated_modules"; modulesInfo.Value = new Any { Type = Any.ValueType.None }; - foreach (var module in activatedModules) + foreach (var module in _activatedModules) { var moduleInfo = new messages.Param(); moduleInfo.Name = "module"; From 59fc0a5bb02c4910068453e0487f0dc08fc3f7a4 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Tue, 17 Dec 2024 17:26:22 +0900 Subject: [PATCH 16/19] Modify SetCustomHandleRequestMessage() CLOiSimPlugin.CommonMethod - Separate the functionality of handling "request_ros2" request message as a new method -> SetRequestRos2Response() --- .../Base/CLOiSimPlugin.CommonMethod.cs | 49 ++++++++++--------- 1 file changed, 27 insertions(+), 22 deletions(-) diff --git a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs index ee945083..cfa1b987 100644 --- a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs +++ b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs @@ -163,28 +163,7 @@ private void SetCustomHandleRequestMessage() switch (requestType) { case "request_ros2": - if (GetPluginParameters().IsValidNode("ros2")) - { - var topic_name = GetPluginParameters().GetValue("ros2/topic_name[@add_parts_name_prefix='true']"); - if (string.IsNullOrEmpty(topic_name)) - { - topic_name = GetPluginParameters().GetValue("ros2/topic_name", _partsName); - topic_name = topic_name.Replace("{parts_name}", _partsName); - } - else - { - topic_name = _partsName + "/" + topic_name; - } - - GetPluginParameters().GetValues("ros2/frame_id", out var frameIdList); - - for (var i = 0; i < frameIdList.Count; i++) - { - frameIdList[i] = frameIdList[i].Replace("{parts_name}", _partsName); - } - - SetROS2CommonInfoResponse(ref response, topic_name, frameIdList); - } + SetRequestRos2Response(ref response); break; case "request_static_transforms": @@ -203,6 +182,32 @@ private void SetCustomHandleRequestMessage() }; } + protected virtual void SetRequestRos2Response(ref DeviceMessage msRos2Info) + { + if (GetPluginParameters().IsValidNode("ros2")) + { + var topic_name = GetPluginParameters().GetValue("ros2/topic_name[@add_parts_name_prefix='true']"); + if (string.IsNullOrEmpty(topic_name)) + { + topic_name = GetPluginParameters().GetValue("ros2/topic_name", _partsName); + topic_name = topic_name.Replace("{parts_name}", _partsName); + } + else + { + topic_name = _partsName + "/" + topic_name; + } + + GetPluginParameters().GetValues("ros2/frame_id", out var frameIdList); + + for (var i = 0; i < frameIdList.Count; i++) + { + frameIdList[i] = frameIdList[i].Replace("{parts_name}", _partsName); + } + + SetROS2CommonInfoResponse(ref msRos2Info, topic_name, frameIdList); + } + } + private void SetStaticTransformsResponse(ref DeviceMessage msRos2Info) { if (msRos2Info == null) From 2e9dcc95db504eb38ddcedd77e6f7dc09ed34e4d Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Tue, 17 Dec 2024 18:14:50 +0900 Subject: [PATCH 17/19] Modify GetPartsName() in DeviceHelper --- Assets/Scripts/Devices/Modules/Base/DeviceHelper.cs | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/Assets/Scripts/Devices/Modules/Base/DeviceHelper.cs b/Assets/Scripts/Devices/Modules/Base/DeviceHelper.cs index c441306b..a43ea601 100644 --- a/Assets/Scripts/Devices/Modules/Base/DeviceHelper.cs +++ b/Assets/Scripts/Devices/Modules/Base/DeviceHelper.cs @@ -84,11 +84,19 @@ public static string GetPartsName(in GameObject targetObject) { if (targetObject.CompareTag("Model")) { - return "MODEL"; + return targetObject.name; } else { - return targetObject.name; + var linkHelper = targetObject.GetComponentInParent(); + if (linkHelper.transform.parent.CompareTag("Link")) + { + return linkHelper.transform.parent.name; // link name + } + else + { + return linkHelper.Model.name; // model name + } } } catch From 101066a0f3d3022fe463cf38dae1001ffa545355 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Tue, 17 Dec 2024 18:26:04 +0900 Subject: [PATCH 18/19] Update Package Version - com.unity.performance.profile-analyzer: 1.2.2 -> 1.2.3 --- Packages/manifest.json | 2 +- Packages/packages-lock.json | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Packages/manifest.json b/Packages/manifest.json index b5dd18ad..d927059d 100644 --- a/Packages/manifest.json +++ b/Packages/manifest.json @@ -3,7 +3,7 @@ "com.unity.ai.navigation": "1.1.5", "com.unity.ide.vscode": "1.2.5", "com.unity.mathematics": "1.2.6", - "com.unity.performance.profile-analyzer": "1.2.2", + "com.unity.performance.profile-analyzer": "1.2.3", "com.unity.render-pipelines.universal": "14.0.11", "com.unity.robotics.vhacd": "https://github.com/Unity-Technologies/VHACD.git?path=/com.unity.robotics.vhacd", "com.unity.searcher": "4.9.2", diff --git a/Packages/packages-lock.json b/Packages/packages-lock.json index 185b5d0d..12a3f9e4 100644 --- a/Packages/packages-lock.json +++ b/Packages/packages-lock.json @@ -34,7 +34,7 @@ "url": "https://packages.unity.com" }, "com.unity.performance.profile-analyzer": { - "version": "1.2.2", + "version": "1.2.3", "depth": 0, "source": "registry", "dependencies": {}, From 06f928b1fb16f0efc91db065693ac1384de9c61e Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Tue, 17 Dec 2024 18:26:35 +0900 Subject: [PATCH 19/19] Update app version info in Project settings : 4.9.4 -> 4.9.5 --- ProjectSettings/ProjectSettings.asset | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ProjectSettings/ProjectSettings.asset b/ProjectSettings/ProjectSettings.asset index 07b50378..d5d8e763 100644 --- a/ProjectSettings/ProjectSettings.asset +++ b/ProjectSettings/ProjectSettings.asset @@ -140,7 +140,7 @@ PlayerSettings: loadStoreDebugModeEnabled: 0 visionOSBundleVersion: 1.0 tvOSBundleVersion: 1.0 - bundleVersion: 4.9.4 + bundleVersion: 4.9.5 preloadedAssets: [] metroInputSource: 0 wsaTransparentSwapchain: 0