diff --git a/Assets/Resources/Shader/GeometryGrass.shader b/Assets/Resources/Shader/GeometryGrass.shader index 50defb28..622f9257 100644 --- a/Assets/Resources/Shader/GeometryGrass.shader +++ b/Assets/Resources/Shader/GeometryGrass.shader @@ -358,8 +358,8 @@ Shader "Custom/GeometryGrass" #endif float width = lerp(_BladeWidthMin, _BladeWidthMax, rand(pos.xzy) * falloff); - float height = lerp(_BladeHeightMin, _BladeHeightMax, rand(pos.zyx) * falloff); - float forward = rand(pos.yyz) * _BladeBendDistance; + float height = lerp(_BladeHeightMin, _BladeHeightMax * grassVisibility, rand(pos.zyx) * falloff); + float forward = rand(pos.yyz) * _BladeBendDistance * grassVisibility; #ifdef DRY_GRASS_ON float dryRate = tex2Dlod(_DryGrassMap, float4(-input[0].uv, 0, 0)).r; diff --git a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs index 70e07308..4da9ca35 100644 --- a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs @@ -7,6 +7,7 @@ using System.Collections.Generic; using System.Text; using System.Linq; +using System; using Any = cloisim.msgs.Any; using UnityEngine; @@ -88,6 +89,11 @@ private void SetupMicom() SetWheel(); } + if (GetPluginParameters().IsValidNode("mowing")) + { + SetMowing(); + } + if (GetPluginParameters().IsValidNode("battery")) { SetBattery(); @@ -170,6 +176,26 @@ private void SetWheelPID() _motorControl.SetPID(P, I, D, iMin, iMax, outputMin, outputMax); } + private void SetMowing() + { + var targetBladeName = GetPluginParameters().GetAttributeInPath("mowing/blade", "target"); + if (!string.IsNullOrEmpty(targetBladeName)) + { + var linkHelpers = GetComponentsInChildren(); + var targetBlade = linkHelpers.FirstOrDefault(x => x.name == targetBladeName); + + if (targetBlade != null) + { + var mowingBlade = targetBlade.gameObject.AddComponent(); + + mowingBlade.HeightMin = GetPluginParameters().GetValue("mowing/blade/height/min", 0f); + mowingBlade.HeightMax = GetPluginParameters().GetValue("mowing/blade/height/max", 0.1f); + mowingBlade.RevSpeedMax = GetPluginParameters().GetValue("mowing/blade/rev_speed/max", 1000); + mowingBlade.Height = 0; + } + } + } + private void SetBattery() { if (GetPluginParameters().GetValues("battery/voltage", out var batteryList)) diff --git a/Assets/Scripts/CLOiSimPlugins/MowingPlugin.cs b/Assets/Scripts/CLOiSimPlugins/MowingPlugin.cs index a8920052..7e29a091 100644 --- a/Assets/Scripts/CLOiSimPlugins/MowingPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/MowingPlugin.cs @@ -13,36 +13,6 @@ [DefaultExecutionOrder(800)] public class MowingPlugin : CLOiSimPlugin { - public struct Target - { - public string modelName; - public string linkName; - - public Target(in string model, in string link) - { - modelName = model; - linkName = link; - } - } - - private class Blade - { - public float ratio = 0.1f; - - public Target target; - - public float size = 0; - - public float heightThreshold = 0; - - public void SetBladingRatio(in float bladeMin, in float bladeMax, in float bladingHeight) - { - this.heightThreshold = bladingHeight; - this.ratio = Mathf.Lerp(bladeMin, bladeMax, bladingHeight); - // Debug.Log("_bladingRatio: " + this.ratio); - } - } - private class Grass { public struct Blade @@ -55,13 +25,14 @@ public Blade(in float min, in float max) heightMin = min; heightMax = max; } - } + public string modelName; + public string linkName; + public float mapResolution = 0.05f; // m/pixel public Blade blade = new Blade(-1, -1); - public Target targetPlane; public Material material = null; public Bounds bounds = new Bounds(); public Texture2D texture = null; @@ -193,14 +164,13 @@ public Vector3 GetGrassOffset() } private Grass _grass = null; - private Blade _blade = null; + private MowingBlade _mowingBlade = null; private Color[] _initialTexturePixels = null; private Transform _targetPlane = null; - private Transform _targetBlade = null; private List _punchingMeshFilters = new List(); - private bool _startMowing = true; + protected override void OnAwake() { @@ -211,7 +181,6 @@ protected override void OnAwake() var geomGrassShader = Shader.Find("Custom/GeometryGrass"); _grass = new Grass(geomGrassShader); - _blade = new Blade(); } protected override void OnStart() @@ -224,11 +193,8 @@ private IEnumerator Start() yield return new WaitForEndOfFrame(); var grassTarget = GetPluginParameters().GetValue("grass/target"); - var grassTragetSplit = grassTarget.Split("::"); - - _grass.targetPlane = new Target(grassTragetSplit[0], grassTragetSplit[1]); - if (FindTargetPlane(_grass.targetPlane)) + if (FindTargetPlane(grassTarget)) { PlantGrass(); } @@ -238,11 +204,7 @@ private IEnumerator Start() } var bladeTarget = GetPluginParameters().GetValue("mowing/blade/target"); - var bladeTargetSplit = bladeTarget.Split("::"); - - _blade.target = new Target(bladeTargetSplit[0], bladeTargetSplit[1]); - - if (FindTargetBlade(_blade.target) == false) + if (FindTargetBlade(bladeTarget) == false) { Debug.LogWarning("Target blade not found"); } @@ -258,13 +220,15 @@ protected override void OnReset() } } - private bool FindTargetPlane(Target targetPlane) + private bool FindTargetPlane(in string targetPlane) { + (_grass.modelName, _grass.linkName) = SDF2Unity.GetModelLinkName(targetPlane); + var modelHelpers = GetComponentsInChildren(); - var targetModel = modelHelpers.FirstOrDefault(x => x.name == targetPlane.modelName); + var targetModel = modelHelpers.FirstOrDefault(x => x.name == _grass.modelName); _targetPlane = targetModel?.GetComponentsInChildren() - .FirstOrDefault(x => x.name == targetPlane.linkName)?.transform; + .FirstOrDefault(x => x.name == _grass.linkName)?.transform; var targetPlaneCollision = _targetPlane?.GetComponentsInChildren() @@ -273,54 +237,45 @@ var targetPlaneCollision return targetPlaneCollision != null; } - private bool FindTargetBlade(Target targetBlade) + private bool FindTargetBlade(in string targetBlade) { + var (targetBladeModelName, targetBladeLinkName) = SDF2Unity.GetModelLinkName(targetBlade); + var modelHelpers = GetComponentsInChildren(); - var targetModel = modelHelpers.FirstOrDefault(x => x.name == targetBlade.modelName); + var targetModel = modelHelpers.FirstOrDefault(x => x.name == targetBladeModelName); - _targetBlade = targetModel?.GetComponentsInChildren() - .FirstOrDefault(x => x.name == targetBlade.linkName)?.transform; + var targetBladeLinkHelper = targetModel?.GetComponentsInChildren() + .FirstOrDefault(x => x.name == targetBladeLinkName)?.transform; - if (_targetBlade == null) + if (targetBladeLinkHelper == null) { return false; } - var meshFilters = _targetBlade.GetComponentsInChildren(); + _mowingBlade = targetBladeLinkHelper.GetComponent(); - var bladeBounds = new Bounds(); - foreach (var meshFilter in meshFilters) + if (_mowingBlade == null) { - // Debug.Log(meshFilter.name); - // Debug.Log(meshFilter.sharedMesh.bounds); - var bounds = meshFilter.sharedMesh.bounds; - bladeBounds.Encapsulate(bounds); + _mowingBlade = targetBladeLinkHelper.gameObject.AddComponent(); } - // Debug.Log(bladeBounds); - _blade.size = Mathf.Max(bladeBounds.extents.x, bladeBounds.extents.z); - var bladeMin = _targetPlane.TransformPoint(_targetBlade.position); - // Debug.Log(bladeMin.y); - - _blade.SetBladingRatio(_grass.blade.heightMin, _grass.blade.heightMax, bladeMin.y); - return true; } private void PlantGrass() { - if (_targetPlane == null) - return; - - var targetPlaneMesh = _targetPlane?.GetComponentInChildren(); - _grass.SetMaterial(GetPluginParameters()); - _grass.SetGrassOffset(_targetPlane.position); - _grass.SetBound(targetPlaneMesh.sharedMesh); - _grass.Generate(); + if (_targetPlane != null) + { + var targetPlaneMesh = _targetPlane?.GetComponentInChildren(); + _grass.SetMaterial(GetPluginParameters()); + _grass.SetGrassOffset(_targetPlane.position); + _grass.SetBound(targetPlaneMesh.sharedMesh); + _grass.Generate(); - StartCoroutine(PunchingGrass()); + StartCoroutine(PunchingGrass()); - AssignMaterial(); + AssignMaterial(); + } } private IEnumerator PunchingGrass() @@ -339,7 +294,7 @@ private IEnumerator PunchingGrass() foreach (var meshFilter in _punchingMeshFilters) { - PunchingTexture(_grass.texture, meshFilter); + PunchingTexture(ref _grass.texture, meshFilter); yield return null; } @@ -364,7 +319,6 @@ private void CreateTempColliderInVisuals(ref List tempMeshCollider foreach (var meshFilter in meshFilters) { var meshCollider = meshFilter.transform.gameObject.AddComponent(); - // Debug.Log(helperVisual.name + "," + meshFilter.name + "," + meshCollider.name); meshCollider.convex = true; meshCollider.isTrigger = true; tempMeshColliders.Add(meshCollider); @@ -376,10 +330,10 @@ private void CreateTempColliderInVisuals(ref List tempMeshCollider private IEnumerator FindMeshFiltersToPunching() { - yield return null; - 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) @@ -390,11 +344,11 @@ private IEnumerator FindMeshFiltersToPunching() if (helperModel != null) { - // punching other object on same target model - if (helperModel.name.Equals(_grass.targetPlane.modelName)) + // punching other object on same target model + if (helperModel.name.Equals(_grass.modelName)) { var helperLink = hitCollider.GetComponentInParent(); - if (helperLink != null && !helperLink.name.Equals(_grass.targetPlane.linkName)) + if (helperLink != null && !helperLink.name.Equals(_grass.linkName)) { var meshFilters = helperLink.GetComponentsInChildren(); _punchingMeshFilters.AddRange(meshFilters); @@ -414,6 +368,7 @@ private IEnumerator FindMeshFiltersToPunching() _punchingMeshFilters.AddRange(meshFilters); } } + yield return null; } } @@ -436,7 +391,7 @@ private void AssignMaterial() targetPlaneMeshRenderer.materials = newMaterials; } - private void PunchingTexture(Texture2D texture, MeshFilter meshFilter) + private void PunchingTexture(ref Texture2D texture, in MeshFilter meshFilter) { var textureCenterX = (int)(texture.width * 0.5f); var textureCenterY = (int)(texture.height * 0.5f); @@ -447,9 +402,6 @@ private void PunchingTexture(Texture2D texture, MeshFilter meshFilter) var vertices = mesh.vertices; var triangles = mesh.triangles; - // Debug.Log(triangles.Length); - // Debug.Log(vertices.Length); - for (var i = 0; i < triangles.Length; i += 3) { var p0 = vertices[triangles[i + 0]]; @@ -459,61 +411,65 @@ private void PunchingTexture(Texture2D texture, MeshFilter meshFilter) var tp1 = meshFilter.transform.TransformPoint(p1) - offset; var tp2 = meshFilter.transform.TransformPoint(p2) - offset; - // Debug.Log("Punch >> " + tp0.y + ", " + tp1.y + ", " + tp2.y); - { - var P1 = new Vector2(tp0.z, tp0.x); - var P2 = new Vector2(tp1.z, tp1.x); - var P3 = new Vector2(tp2.z, tp2.x); + var P1 = new Vector2(tp0.z, tp0.x); + var P2 = new Vector2(tp1.z, tp1.x); + var P3 = new Vector2(tp2.z, tp2.x); - P1 /= _grass.mapResolution; - P2 /= _grass.mapResolution; - P3 /= _grass.mapResolution; + P1 /= _grass.mapResolution; + P2 /= _grass.mapResolution; + P3 /= _grass.mapResolution; - P1 += textureCenter; - P2 += textureCenter; - P3 += textureCenter; + P1 += textureCenter; + P2 += textureCenter; + P3 += textureCenter; - texture.FillTriangle(P1, P2, P3, Color.clear); - } + texture.FillTriangle(P1, P2, P3, Color.clear); } } private IEnumerator StartMowing() { - const float mowingMargin = 0.01f; - yield return null; - var threshold = _blade.heightThreshold + mowingMargin; - var color = new Color(_blade.ratio, 0, 0, 0); + var mowingThreshold = _grass.blade.heightMax; + var mowingRatioInColor = Color.clear; var planeCenterPosition = _targetPlane.position; - var bladeRadiusIntexture = _blade.size /_grass.mapResolution; + var bladeRadiusIntexture = _mowingBlade.Diameter /_grass.mapResolution; - // Debug.Log("blade threshold= " + threshold + ", bladeRadiusIntexture=" + bladeRadiusIntexture); while (true) { - if (_startMowing && _targetBlade != null) + if (_mowingBlade.IsRunning() && _mowingBlade != null) { - var bladePositionInTexture = _targetBlade.position; + var bladeToPlaneDistance = _targetPlane.TransformPoint(_mowingBlade.Position); - // Debug.Log("blade " + bladePositionInTexture.y.ToString("F4") + " threshold= " + threshold); - if (bladePositionInTexture.y <= threshold) + if (bladeToPlaneDistance.y <= mowingThreshold) { + mowingRatioInColor.r = bladeToPlaneDistance.y / mowingThreshold; + // Debug.Log($"{_grass.blade.heightMin}, {_grass.blade.heightMax}, {bladeToPlaneDistance.y} => {mowingRatioInColor.r }"); + + var bladePositionInTexture = _mowingBlade.Position; bladePositionInTexture -= planeCenterPosition; bladePositionInTexture += _grass.bounds.extents; bladePositionInTexture /= _grass.mapResolution; - // Debug.Log(bladePositionInTexture); _grass.texture.FillCircle( bladePositionInTexture.z, bladePositionInTexture.x, bladeRadiusIntexture, - color); + mowingRatioInColor, + TextureUtil.FillOptions.Lesser); yield return null; } + else + { + yield return new WaitForEndOfFrame(); + } + } + else + { + yield return new WaitForEndOfFrame(); } - yield return new WaitForEndOfFrame(); } } } diff --git a/Assets/Scripts/Devices/MicomCommand.cs b/Assets/Scripts/Devices/MicomCommand.cs index ee7b85ee..22cd7ab0 100644 --- a/Assets/Scripts/Devices/MicomCommand.cs +++ b/Assets/Scripts/Devices/MicomCommand.cs @@ -12,6 +12,7 @@ namespace SensorDevices public class MicomCommand : Device { private MotorControl _motorControl = null; + private MowingBlade _mowingBlade = null; protected override void OnAwake() { @@ -21,6 +22,7 @@ protected override void OnAwake() protected override void OnStart() { + _mowingBlade = GetComponentInChildren(); } protected override void OnReset() @@ -35,22 +37,39 @@ public void SetMotorControl(in MotorControl motorControl) protected override void ProcessDevice() { - if (PopDeviceMessage(out var micomWritingData)) + if (PopDeviceMessage(out var receivedMessage)) { - var linear = micomWritingData.Linear; - var angular = micomWritingData.Angular; + var cmdVelocity = receivedMessage.GetMessage(); - var linearVelocity = SDF2Unity.Position(linear.X, linear.Y, linear.Z); - var angularVelocity = SDF2Unity.Position(angular.X, angular.Y, angular.Z); + if (cmdVelocity != null) + { + var linear = cmdVelocity.Linear; + var angular = cmdVelocity.Angular; - DoWheelDrive(linearVelocity, angularVelocity); - } + var linearVelocity = SDF2Unity.Position(linear.X, linear.Y, linear.Z); + var angularVelocity = SDF2Unity.Position(angular.X, angular.Y, angular.Z); + + DoWheelDrive(linearVelocity, angularVelocity); + } + else + { + var cmdMowing = receivedMessage.GetMessage(); + + if (cmdMowing != null) + { + if (!string.IsNullOrEmpty(cmdMowing.Name)) + { + ControlMowing(cmdMowing.Name, cmdMowing.Value); + } + } #if UNITY_EDITOR - else - { - Debug.LogWarning("ERROR: failed to pop device message"); - } + else + { + Debug.LogWarning("ERROR: failed to pop device message"); + } #endif + } + } } /// m/s @@ -65,7 +84,32 @@ private void DoWheelDrive(in Vector3 linearVelocity, in Vector3 angularVelocity) var targetLinearVelocity = linearVelocity.z; var targetAngularVelocity = angularVelocity.y; + _motorControl.SetTwistDrive(targetLinearVelocity, targetAngularVelocity); } + + private void ControlMowing(in string target, in cloisim.msgs.Any value) + { + if (string.Compare(target, "mowing_blade_height") == 0) + { + if (_mowingBlade != null && value.Type == messages.Any.ValueType.Double) + { + _mowingBlade.Height = (float)value.DoubleValue; + // Debug.Log($"mowing_blade_height {value} -> {_mowingBlade.Height}"); + } + } + else if (string.Compare(target, "mowing_blade_rev_speed") == 0) + { + if (_mowingBlade != null && value.Type == messages.Any.ValueType.Int32) + { + _mowingBlade.RevSpeed = System.Convert.ToUInt16(value.IntValue); + // Debug.Log($"mowing_blade_rev_speed {value} -> {_mowingBlade.RevSpeed}"); + } + } + else + { + Debug.LogWarning($"Invalid Control Mowing message received: {target}"); + } + } } } \ No newline at end of file diff --git a/Assets/Scripts/Devices/Modules/MowingBlade.cs b/Assets/Scripts/Devices/Modules/MowingBlade.cs new file mode 100644 index 00000000..65431530 --- /dev/null +++ b/Assets/Scripts/Devices/Modules/MowingBlade.cs @@ -0,0 +1,138 @@ +/* + * Copyright (c) 2024 LG Electronics Inc. + * + * SPDX-License-Identifier: MIT + */ + +using System.Collections.Generic; +using System.Collections; +using System; +using UnityEngine; + +public class MowingBlade : MonoBehaviour +{ + [SerializeField] + private float _height = 0; + + [SerializeField] + private UInt16 _revSpeed = 0; + + [SerializeField] + private float _heightMin = 0; + + [SerializeField] + private float _heightMax = 0; + + [SerializeField] + private UInt16 _revSpeedMax = 0; + + [SerializeField] + private float _heightBaseInTransform = 0; + + private Bounds _bladeBounds = new Bounds(); + + private bool _doAdjust = false; + + public float Diameter + { + get => Mathf.Max(_bladeBounds.extents.x, _bladeBounds.extents.z); + } + + public float HeightMin + { + get => _heightMin; + set => _heightMin = value; + } + + public float HeightMax + { + get => _heightMax; + set => _heightMax = value; + } + + public UInt16 RevSpeedMax + { + get => _revSpeedMax; + set => _revSpeedMax = value; + } + + public float Height + { + get => _height; + set => _height = (value <= _heightMin) ? _heightMin : (value >= _heightMax ? _heightMax : value); + } + + public UInt16 RevSpeed + { + get => _revSpeed; + set => _revSpeed = (value >= _revSpeedMax ? _revSpeedMax : value); + } + + public Vector3 Position + { + get => this.transform.position; + } + + public bool IsRunning() + { + return _revSpeed > float.Epsilon; + } + + void Start() + { + StartCoroutine(CalculateBladeSize()); + } + + void OnDestroy() + { + _doAdjust = false; + } + + private IEnumerator CalculateBladeSize() + { + var meshFilters = GetComponentsInChildren(); + + _bladeBounds.center = Vector3.zero; + _bladeBounds.size = Vector3.zero; + foreach (var meshFilter in meshFilters) + { + var bounds = meshFilter.sharedMesh.bounds; + _bladeBounds.Encapsulate(bounds); + } + + _doAdjust = true; + yield return DoAdjust(); + } + + private IEnumerator DoAdjust() + { + var targets = new List(); + var indices = new List(); + var articulationBody = GetComponent(); + + while (_doAdjust) + { + var dof = articulationBody.GetDriveTargets(targets); + articulationBody.GetDofStartIndices(indices); + if (dof > 0) + { + var bodyIndex = articulationBody.index; + var targetIndex = indices[bodyIndex]; + var currentTarget = targets[targetIndex]; + var targetHeight = articulationBody.yDrive.lowerLimit + _height; + if (targetHeight >= articulationBody.yDrive.upperLimit) + { + targetHeight = articulationBody.yDrive.upperLimit; + } + + if (!Mathf.Approximately(currentTarget, targetHeight)) + { + articulationBody.SetDriveTarget(ArticulationDriveAxis.Y, targetHeight); + } + } + yield return null; + } + + yield return null; + } +} \ No newline at end of file diff --git a/Assets/Scripts/Devices/Modules/MowingBlade.cs.meta b/Assets/Scripts/Devices/Modules/MowingBlade.cs.meta new file mode 100644 index 00000000..845b2043 --- /dev/null +++ b/Assets/Scripts/Devices/Modules/MowingBlade.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: de13c3d505bb498458879e613e126aef +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Scripts/Tools/Mesh/TextureUtil.cs b/Assets/Scripts/Tools/Mesh/TextureUtil.cs index 04e8c495..87d36dad 100644 --- a/Assets/Scripts/Tools/Mesh/TextureUtil.cs +++ b/Assets/Scripts/Tools/Mesh/TextureUtil.cs @@ -11,6 +11,13 @@ public static class TextureUtil { + public enum FillOptions + { + Overwrite, + Lesser, + Greater + }; + public static void Clear(this Texture2D texture) { Clear(texture, Color.clear); @@ -47,12 +54,16 @@ public static void Fill(this Texture2D texture, ref Color[] colors) texture.SetPixels(colors); } - public static void FillCircle(this Texture2D texture, in float x, in float y, in float radius, in Color color) + public static void FillCircle( + this Texture2D texture, in float x, in float y, in float radius, in Color color, + in FillOptions option = FillOptions.Overwrite) { - FillCircle(texture, (int)x, (int)y, (int)radius, color); + FillCircle(texture, (int)x, (int)y, (int)radius, color, option); } - public static void FillCircle(this Texture2D texture, in int x, in int y, in int radius, in Color color) + public static void FillCircle( + this Texture2D texture, in int x, in int y, in int radius, in Color color, + in FillOptions option = FillOptions.Overwrite) { var rSquared = radius * radius; @@ -67,7 +78,30 @@ public static void FillCircle(this Texture2D texture, in int x, in int y, in int { if ((x - u) * (x - u) + (y - v) * (y - v) < rSquared) { - texture.SetPixel(u, v, color); + var pixelColor = texture.GetPixel(u, v); + switch (option) + { + case FillOptions.Lesser: + pixelColor.r = (pixelColor.r < color.r) ? pixelColor.r : color.r; + pixelColor.g = (pixelColor.g < color.g) ? pixelColor.g : color.g; + pixelColor.b = (pixelColor.b < color.b) ? pixelColor.b : color.b; + pixelColor.a = (pixelColor.a < color.a) ? pixelColor.a : color.a; + break; + + case FillOptions.Greater: + pixelColor.r = (pixelColor.r > color.r) ? pixelColor.r : color.r; + pixelColor.g = (pixelColor.g > color.g) ? pixelColor.g : color.g; + pixelColor.b = (pixelColor.b > color.b) ? pixelColor.b : color.b; + pixelColor.a = (pixelColor.a > color.a) ? pixelColor.a : color.a; + break; + + default: + case FillOptions.Overwrite: + pixelColor = color; + break; + } + + texture.SetPixel(u, v, pixelColor); } } }