diff --git a/Assets/Resources/Shader/DepthBufferScaling.compute b/Assets/Resources/Shader/DepthBufferScaling.compute index 87feb318..ab704910 100644 --- a/Assets/Resources/Shader/DepthBufferScaling.compute +++ b/Assets/Resources/Shader/DepthBufferScaling.compute @@ -2,8 +2,8 @@ #pragma kernel CSDepthBufferScaling #define MaxValue16Bits 65535.0f; -#define THREADS 16 -#define GROUPS 16 +#define THREADS 8 +#define GROUPS 4 RWByteAddressBuffer _Buffer; diff --git a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs index 924ab458..dbb3712a 100644 --- a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs @@ -6,7 +6,6 @@ using System.Collections.Generic; using System.Text; -using System; using Any = cloisim.msgs.Any; using messages = cloisim.msgs; using UnityEngine; @@ -81,12 +80,12 @@ private void SetupMicom() var motorFriction = GetPluginParameters().GetValue("wheel/friction/motor", 0.1f); var brakeFriction = GetPluginParameters().GetValue("wheel/friction/brake", 0.1f); - var wheelLeftName = GetPluginParameters().GetValue("wheel/location[@type='left']", String.Empty); - var wheelRightName = GetPluginParameters().GetValue("wheel/location[@type='right']", String.Empty); - var rearWheelLeftName = GetPluginParameters().GetValue("wheel/location[@type='rear_left']", String.Empty); - var rearWheelRightName = GetPluginParameters().GetValue("wheel/location[@type='rear_right']", String.Empty); + var wheelLeftName = GetPluginParameters().GetValue("wheel/location[@type='left']", string.Empty); + var wheelRightName = GetPluginParameters().GetValue("wheel/location[@type='right']", string.Empty); + var rearWheelLeftName = GetPluginParameters().GetValue("wheel/location[@type='rear_left']", string.Empty); + var rearWheelRightName = GetPluginParameters().GetValue("wheel/location[@type='rear_right']", string.Empty); - if (!rearWheelLeftName.Equals(String.Empty) && !rearWheelRightName.Equals(String.Empty)) + if (!rearWheelLeftName.Equals(string.Empty) && !rearWheelRightName.Equals(string.Empty)) { micomSensor.SetWheel(wheelLeftName, wheelRightName, rearWheelLeftName, rearWheelRightName); } diff --git a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs index 54a5d499..838e869a 100644 --- a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs +++ b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs @@ -149,7 +149,8 @@ private void SetCustomHandleRequestMessage() switch (requestType) { case "request_ros2": - var topic_name = GetPluginParameters().GetValue("ros2/topic_name"); + + var topic_name = GetPluginParameters().GetValue("ros2/topic_name", partsName); GetPluginParameters().GetValues("ros2/frame_id", out var frameIdList); SetROS2CommonInfoResponse(ref response, topic_name, frameIdList); break; diff --git a/Assets/Scripts/Devices/DepthCamera.cs b/Assets/Scripts/Devices/DepthCamera.cs index 2f1e6618..5c2b4580 100644 --- a/Assets/Scripts/Devices/DepthCamera.cs +++ b/Assets/Scripts/Devices/DepthCamera.cs @@ -119,6 +119,9 @@ protected override void SetupTexture() cb.Release(); } + private int threadGroupsX = 8; + private int threadGroupsY = 4; + protected override void PostProcessing(ref byte[] buffer) { if (readbackDstFormat.Equals(TextureFormat.R16) && computeShader != null) @@ -127,8 +130,8 @@ protected override void PostProcessing(ref byte[] buffer) computeShader.SetBuffer(kernelIndex, "_Buffer", computeBuffer); computeBuffer.SetData(buffer); - var threadGroupX = camParameter.image_width / 16; - var threadGroupY = camParameter.image_height / 8; + var threadGroupX = camParameter.image_width / threadGroupsX; + var threadGroupY = camParameter.image_height / threadGroupsY; computeShader.Dispatch(kernelIndex, threadGroupX, threadGroupY, 1); computeBuffer.GetData(buffer); computeBuffer.Release(); diff --git a/Assets/Scripts/Devices/Lidar.cs b/Assets/Scripts/Devices/Lidar.cs index ef794d09..9ab43d5e 100644 --- a/Assets/Scripts/Devices/Lidar.cs +++ b/Assets/Scripts/Devices/Lidar.cs @@ -485,7 +485,7 @@ protected override void GenerateMessage() } catch (Exception ex) { - Debug.LogWarning("Error occured with Buffer.BlockCopy : " + ex.Message + ", " + srcBufferOffset + ", " + dstBufferOffset + ", " + copyLength); + Debug.LogWarning("Error occured with Buffer.BlockCopy : " + ex.Message + ", " + srcBufferOffset + ", " + dstBufferOffset + ", " + copyLength + ", Len: src "+ srcBuffer.Length + ", dst " + laserScan.Ranges.Length); } } } diff --git a/Assets/Scripts/Main.cs b/Assets/Scripts/Main.cs index 3a9f4dda..0b6d5b79 100644 --- a/Assets/Scripts/Main.cs +++ b/Assets/Scripts/Main.cs @@ -304,6 +304,7 @@ private string GetClonedModelName(in string modelName) if (childTransform.name.CompareTo(tmpModelName) == 0) { tmpModelName = modelName + "_clone_" + numbering++; + i = 0; } } return tmpModelName; @@ -323,7 +324,7 @@ private IEnumerator LoadModel(string modelPath, string modelFileName) var addingModel = uiMainCanvasRoot.GetComponentInChildren(); addingModel.SetAddingModelForDeploy(targetObject); - yield return new WaitForSeconds(0.1f); + yield return new WaitForSeconds(0.01f); yield return new WaitForEndOfFrame(); // for GUI diff --git a/Assets/Scripts/Tools/SDF/Helper/PoseControl.cs b/Assets/Scripts/Tools/SDF/Helper/PoseControl.cs index 7e080f75..68752bf7 100644 --- a/Assets/Scripts/Tools/SDF/Helper/PoseControl.cs +++ b/Assets/Scripts/Tools/SDF/Helper/PoseControl.cs @@ -132,10 +132,15 @@ private void ResetArticulationBody(in int targetFrame = 0) articulationBody.jointVelocity = zeroSpace; articulationBody.jointAcceleration = zeroSpace; articulationBody.jointForce = zeroSpace; + var isPrismatic = articulationBody.jointType.Equals(UE.ArticulationJointType.PrismaticJoint); GetJointTarget(out var targetJoint1, out var targetJoint2, targetFrame); var xDrive = articulationBody.xDrive; + if (!isPrismatic) + { + xDrive.targetVelocity = 0; + } if (!articulationBody.linearLockX.Equals(UE.ArticulationDofLock.LockedMotion) || !articulationBody.twistLock.Equals(UE.ArticulationDofLock.LockedMotion)) { @@ -153,10 +158,13 @@ private void ResetArticulationBody(in int targetFrame = 0) { xDrive.target = 0; } - xDrive.targetVelocity = 0; articulationBody.xDrive = xDrive; var yDrive = articulationBody.yDrive; + if (!isPrismatic) + { + yDrive.targetVelocity = 0; + } if (!articulationBody.linearLockY.Equals(UE.ArticulationDofLock.LockedMotion) || !articulationBody.swingYLock.Equals(UE.ArticulationDofLock.LockedMotion)) { @@ -174,10 +182,13 @@ private void ResetArticulationBody(in int targetFrame = 0) { yDrive.target = 0; } - yDrive.targetVelocity = 0; articulationBody.yDrive = yDrive; var zDrive = articulationBody.zDrive; + if (!isPrismatic) + { + zDrive.targetVelocity = 0; + } if (!articulationBody.linearLockY.Equals(UE.ArticulationDofLock.LockedMotion) || !articulationBody.swingZLock.Equals(UE.ArticulationDofLock.LockedMotion)) { @@ -193,9 +204,8 @@ private void ResetArticulationBody(in int targetFrame = 0) } else { - yDrive.target = 0; + zDrive.target = 0; } - zDrive.targetVelocity = 0; articulationBody.zDrive = zDrive; } } diff --git a/Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs b/Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs index b70d5b0a..8744a403 100644 --- a/Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs +++ b/Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs @@ -18,8 +18,13 @@ public static UE.Pose SetArticulationBodyRelationship(in SDF.Joint joint, UE.Tra var modelTransformChild = linkChild.parent; var modelHelperChild = modelTransformChild.GetComponent(); + var linkHelperParent = linkParent.GetComponent(); + var linkHelperChild = linkChild.GetComponent(); + var anchorPose = new UE.Pose(); - if (modelTransformChild.Equals(modelTransformParent) || modelHelperChild.IsFirstChild) + if (linkHelperChild.Model.Equals(linkHelperParent.Model) || + modelTransformChild.Equals(modelTransformParent) || + modelHelperChild.IsFirstChild) { linkChild.SetParent(linkParent); @@ -205,6 +210,13 @@ public static void MakePrismatic(in UE.ArticulationBody body, in SDF.Axis axis, drive.stiffness = (float)axis.dynamics.spring_stiffness; drive.target = (float)axis.dynamics.spring_reference; drive.damping = (float)axis.dynamics.damping; + + // Check if spring + if (axis.limit.Use()) + { + drive.targetVelocity = 1; + } + body.jointFriction = (float)axis.dynamics.friction; } else diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs b/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs index eaddc55d..c84658ad 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs @@ -24,7 +24,8 @@ private static UE.Transform FindTransformByName(in string name, UE.Transform tar rootTransform = rootTransform.parent; } - (var modelName, var linkName) = SDF2Unity.GetModelLinkName(name); + (var modelName, var linkName) = SDF2Unity.GetModelLinkName(name, targetTransform.name); + // UE.Debug.Log("GetModelLinkName => " + modelName + ", " + linkName); if (string.IsNullOrEmpty(modelName)) { @@ -53,10 +54,14 @@ private static UE.Transform FindTransformByName(in string name, UE.Transform tar if (modelTransform != null) { - var linkTransform = modelTransform.Find(linkName); - if (linkTransform != null) + foreach (var linkObject in modelTransform.GetComponentsInChildren()) { - foundLinkObject = linkTransform; + var linkTransform = linkObject.transform; + if (linkTransform.name.Equals(linkName)) + { + foundLinkObject = linkTransform; + break; + } } } } diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Link.cs b/Assets/Scripts/Tools/SDF/Import/Import.Link.cs index 144c624b..3b6f24ad 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Link.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Link.cs @@ -95,9 +95,9 @@ private static UE.ArticulationBody CreateArticulationBody(in UE.GameObject linkO articulationBody.angularVelocity = UE.Vector3.zero; articulationBody.useGravity = (linkHelper == null) ? false : linkHelper.useGravity; articulationBody.jointType = UE.ArticulationJointType.FixedJoint; - articulationBody.mass = (inertial == null) ? 1e-07f : (float)inertial.mass; - articulationBody.linearDamping = 0.05f; - articulationBody.angularDamping = 0.05f; + articulationBody.mass = (inertial == null) ? 1e-06f : (float)inertial.mass; + articulationBody.linearDamping = 0.01f; + articulationBody.angularDamping = 0.01f; articulationBody.jointFriction = 0f; articulationBody.velocity = UE.Vector3.zero; articulationBody.angularVelocity = UE.Vector3.zero; diff --git a/Assets/Scripts/Tools/SDF/Parser/Pose.cs b/Assets/Scripts/Tools/SDF/Parser/Pose.cs index f57e2183..c478bc2a 100644 --- a/Assets/Scripts/Tools/SDF/Parser/Pose.cs +++ b/Assets/Scripts/Tools/SDF/Parser/Pose.cs @@ -51,8 +51,8 @@ public void Set(string x, string y) var code = Type.GetTypeCode(typeof(T)); if (code != TypeCode.Empty) { - x = regex_num.Replace(x, String.Empty); - y = regex_num.Replace(y, String.Empty); + x = regex_num.Replace(x, string.Empty); + y = regex_num.Replace(y, string.Empty); Set((T)Convert.ChangeType(x, code), (T)Convert.ChangeType(y, code)); } } @@ -116,9 +116,9 @@ public void Set(string x, string y, string z) var code = Type.GetTypeCode(typeof(T)); if (code != TypeCode.Empty) { - x = Vector2.regex_num.Replace(x, String.Empty); - y = Vector2.regex_num.Replace(y, String.Empty); - z = Vector2.regex_num.Replace(z, String.Empty); + x = Vector2.regex_num.Replace(x, string.Empty); + y = Vector2.regex_num.Replace(y, string.Empty); + z = Vector2.regex_num.Replace(z, string.Empty); Set((T)Convert.ChangeType(x, code), (T)Convert.ChangeType(y, code), (T)Convert.ChangeType(z, code)); } } diff --git a/Assets/Scripts/Tools/SDF/Util/SDF2Unity.Model.cs b/Assets/Scripts/Tools/SDF/Util/SDF2Unity.Model.cs index 88030df0..71b59476 100644 --- a/Assets/Scripts/Tools/SDF/Util/SDF2Unity.Model.cs +++ b/Assets/Scripts/Tools/SDF/Util/SDF2Unity.Model.cs @@ -8,9 +8,9 @@ public partial class SDF2Unity { - public static (string, string) GetModelLinkName(in string value) + public static (string, string) GetModelLinkName(in string value, in string defaultModelName = "") { - var modelName = string.Empty; + var modelName = defaultModelName; var linkName = value; if (value.Contains("::")) diff --git a/ProjectSettings/ProjectSettings.asset b/ProjectSettings/ProjectSettings.asset index d0254718..54b88b4f 100644 --- a/ProjectSettings/ProjectSettings.asset +++ b/ProjectSettings/ProjectSettings.asset @@ -134,7 +134,7 @@ PlayerSettings: 16:10: 1 16:9: 1 Others: 1 - bundleVersion: 2.7.3 + bundleVersion: 2.7.4 preloadedAssets: [] metroInputSource: 0 wsaTransparentSwapchain: 0