diff --git a/Assets/Scripts/DevicePlugins/ElevatorSystem.cs b/Assets/Scripts/DevicePlugins/ElevatorSystem.cs index 5b49c0af..e2b00a41 100644 --- a/Assets/Scripts/DevicePlugins/ElevatorSystem.cs +++ b/Assets/Scripts/DevicePlugins/ElevatorSystem.cs @@ -50,7 +50,7 @@ public ElevatorTask(in int index) protected override void OnAwake() { type = Type.ELEVATOR; - partName = "ElevatorSystem"; + partName = "elevator_system"; } protected override void OnStart() @@ -513,7 +513,7 @@ private void DoServiceInStandby(ref ElevatorTask task) } else { - Debug.LogErrorFormat("Wrong:: elevator is not arrived yet Select floor: {0}, {1}, {2}", entity.Name, task.fromFloor, task.toFloor); + Debug.LogWarningFormat("Wrong:: elevator is not arrived yet Select floor: {0}, {1}, {2}", entity.Name, task.fromFloor, task.toFloor); task.state = ElevatorTaskState.DONE; } } diff --git a/Assets/Scripts/DevicePlugins/UnityRosWorld.cs b/Assets/Scripts/DevicePlugins/UnityRosWorld.cs index ce68e02f..0f906a84 100644 --- a/Assets/Scripts/DevicePlugins/UnityRosWorld.cs +++ b/Assets/Scripts/DevicePlugins/UnityRosWorld.cs @@ -18,7 +18,7 @@ protected override void OnAwake() clock = gameObject.AddComponent(); modelName = "World"; - partName = "UnityRos"; + partName = "unity_ros"; } protected override void OnStart() diff --git a/Assets/Scripts/Devices/Modules/Actuator.cs b/Assets/Scripts/Devices/Modules/Actuator.cs index 2d7d43eb..8f46533e 100644 --- a/Assets/Scripts/Devices/Modules/Actuator.cs +++ b/Assets/Scripts/Devices/Modules/Actuator.cs @@ -13,18 +13,57 @@ public enum MovingType {MoveTowards = 0, Lerp, SmoothDamp}; private const float lerpEpsilon = 0.009f; private Vector3 currentVelocity = Vector3.zero; // only for SmoothDamp + private Vector3 initialPose = Vector3.zero; private Transform targetTransform = null; private MovingType movingType = MovingType.MoveTowards; private Vector3 targetPosition = Vector3.zero; + #region Parameters + private Vector3 rodDirection = Vector3.zero; + private float minOffset = 0f; + private float maxOffset = 0f; + private Vector3 minPosition = Vector3.zero; + private Vector3 maxPosition = Vector3.zero; + #endregion + private float distanceEpsilon = Vector3.kEpsilon; private float maxMovingSpeed = 1; private bool isMoving = false; public bool IsMoving => isMoving; - public Vector3 TargetPosition => targetPosition; + public void SetDirection(in Vector3 direction) + { + rodDirection = direction; + RecalculateInitialPoseByRodDirection(); + } + + public void SetMinOffset(in float offset) + { + if (minOffset > maxOffset) + { + Debug.LogWarningFormat("minOffset({0}) cannot be lower than maxOffset({1})", minOffset, maxOffset); + } + else + { + minOffset = offset; + minPosition = rodDirection * minOffset; + } + } + + public void SetMaxOffset(in float offset) + { + if (maxOffset < minOffset) + { + Debug.LogWarningFormat("maxOffset({0}) cannot be lower than minOffset({1})", maxOffset, minOffset); + } + else + { + maxOffset = offset; + maxPosition = rodDirection * maxOffset; + } + } public void SetTarget(in string name) { @@ -49,6 +88,19 @@ public void SetTarget(in Transform target) targetTransform = target; } + public void SetInitialPose(in Vector3 targetPosition) + { + initialPose = targetPosition; + RecalculateInitialPoseByRodDirection(); + } + + private void RecalculateInitialPoseByRodDirection() + { + initialPose.x -= (initialPose.x * rodDirection.x); + initialPose.y -= (initialPose.y * rodDirection.y); + initialPose.z -= (initialPose.z * rodDirection.z); + } + public void SetMaxSpeed(in float value) { maxMovingSpeed = value; @@ -90,32 +142,20 @@ public Vector3 CurrentPosition(in bool worldPosition = false) // Set target position before drive. // // Parameters: - // direction: Vector3.up/down/forwad/back/left/right // offset: target offset - // relative: if true, target shall move from current position, - // otherwise target shall be set absolute offset along with direction // - public void SetTargetPosition(in Vector3 direction, in float offset, in bool relative = false) + public void SetTargetPosition(float offset) { - var targetValue = (direction * offset); - - if (relative) + if (offset < minOffset) { - targetPosition = CurrentPosition() + targetValue; + offset = minOffset; } - else + else if (offset > maxOffset) { - targetPosition = CurrentPosition(); - SetZeroOnDirection(ref targetPosition, direction); - targetPosition += targetValue; + offset = maxOffset; } - // Debug.Log("SetTargetPosition: " + targetPosition.ToString("F5")); - } - - public void SetTargetPosition(in Vector3 absolutePosition) - { - targetPosition = absolutePosition; + targetPosition = initialPose + (rodDirection * offset); } public void Drive() @@ -149,7 +189,7 @@ public void Drive() targetTransform.localPosition = nextPosition; // check if it arrived - var distance = Vector3.SqrMagnitude(targetPosition - nextPosition); + var distance = Vector3.Distance(targetPosition, nextPosition); if (distance < distanceEpsilon) { // final touch @@ -160,32 +200,18 @@ public void Drive() } } - public bool IsSamePosition(in Vector3 checkPosition) + public bool IsReachedMin() { - return DeviceHelper.IsSamePosition(CurrentPosition(), checkPosition); + return IsSamePosition(minPosition); } - public bool IsSamePosition(in Vector3 checkDirection, in float checkOffset) + public bool IsReachedMax() { - var tempTarget = CurrentPosition(); - SetZeroOnDirection(ref tempTarget, checkDirection); - tempTarget += (checkDirection * checkOffset); - return DeviceHelper.IsSamePosition(CurrentPosition(), tempTarget); + return IsSamePosition(maxPosition); } - public static void SetZeroOnDirection(ref Vector3 target, in Vector3 direction) + public bool IsSamePosition(in Vector3 checkPosition) { - if (direction.Equals(Vector3.left) || direction.Equals(Vector3.right)) - { - target.x = 0; - } - else if (direction.Equals(Vector3.up) || direction.Equals(Vector3.down)) - { - target.y = 0; - } - else if (direction.Equals(Vector3.forward) || direction.Equals(Vector3.back)) - { - target.z = 0; - } + return DeviceHelper.IsSamePosition(CurrentPosition(), checkPosition); } } \ No newline at end of file diff --git a/Assets/Scripts/Devices/Modules/DoorsControl.cs b/Assets/Scripts/Devices/Modules/DoorsControl.cs index 88be4e27..37a84d6b 100644 --- a/Assets/Scripts/Devices/Modules/DoorsControl.cs +++ b/Assets/Scripts/Devices/Modules/DoorsControl.cs @@ -9,13 +9,10 @@ public class DoorsControl : MonoBehaviour { - private Actuator doorLeft = null; - private Actuator doorRight = null; + private Actuator doorLeft = new Actuator(); + private Actuator doorRight = new Actuator(); - private Vector3 closedDoorPositionLeft = Vector3.zero; - private Vector3 closedDoorPositionRight = Vector3.zero; - private Vector3 openedDoorPositionLeft = Vector3.zero; - private Vector3 openedDoorPositionRight = Vector3.zero; + private Coroutine operatingDoor = null; private Vector3 doorTargetPositionLeft = Vector3.zero; private Vector3 doorTargetPositionRight = Vector3.zero; @@ -23,50 +20,28 @@ public class DoorsControl : MonoBehaviour public float speed = 0.1f; public float openOffset = 1; - public DoorsControl() - { - doorLeft = new Actuator(); - doorRight = new Actuator(); - } - void Start() { doorLeft.SetMovingType(Actuator.MovingType.MoveTowards); doorLeft.SetMaxSpeed(speed); + doorLeft.SetDirection(Vector3.forward); + doorLeft.SetMaxOffset(openOffset); + doorLeft.SetMinOffset(0); doorRight.SetMovingType(Actuator.MovingType.MoveTowards); doorRight.SetMaxSpeed(speed); + doorRight.SetDirection(Vector3.back); + doorRight.SetMaxOffset(openOffset); + doorRight.SetMinOffset(0); } public void SetLeftDoor(in Transform targetTransform) { doorLeft.SetTarget(targetTransform); - - if (targetTransform == null) - { - closedDoorPositionLeft = Vector3.zero; - openedDoorPositionLeft = Vector3.zero; - } - else - { - closedDoorPositionLeft = doorLeft.CurrentPosition(); - openedDoorPositionLeft = Vector3.right * openOffset; - } } public void SetRightDoor(in Transform targetTransform) { doorRight.SetTarget(targetTransform); - - if (targetTransform == null) - { - closedDoorPositionRight = Vector3.zero; - openedDoorPositionRight = Vector3.zero; - } - else - { - closedDoorPositionRight = doorRight.CurrentPosition(); - openedDoorPositionRight = Vector3.left * openOffset; - } } public Vector3 GetLeftDoorPosition() @@ -81,30 +56,32 @@ public Vector3 GetRightDoorPosition() public void Open() { - doorLeft.SetTargetPosition(Vector3.right, openOffset); - doorRight.SetTargetPosition(Vector3.left, openOffset); + doorLeft.SetTargetPosition(openOffset); + doorRight.SetTargetPosition(openOffset); - if (IsClosed() && !IsMoving()) + if (operatingDoor != null) { - StartCoroutine(MoveTo()); + StopCoroutine(operatingDoor); } + + operatingDoor = StartCoroutine(Operate()); } public void Close() { - doorLeft.SetTargetPosition(Vector3.left, openOffset, true); - doorRight.SetTargetPosition(Vector3.right, openOffset, true); + doorLeft.SetTargetPosition(-openOffset); + doorRight.SetTargetPosition(-openOffset); - if (IsOpened() && !IsMoving()) + if (operatingDoor != null) { - StartCoroutine(MoveTo()); + StopCoroutine(operatingDoor); } + operatingDoor = StartCoroutine(Operate()); } - public bool IsOpened() { - if (doorLeft.IsSamePosition(openedDoorPositionLeft) && doorRight.IsSamePosition(openedDoorPositionRight)) + if (doorLeft.IsReachedMax() && doorRight.IsReachedMax()) { return true; } @@ -114,17 +91,7 @@ public bool IsOpened() public bool IsClosed() { - if (doorLeft.IsSamePosition(closedDoorPositionLeft) && doorRight.IsSamePosition(closedDoorPositionRight)) - { - return true; - } - - return false; - } - - public bool IsMoving() - { - if (doorLeft.IsMoving || doorRight.IsMoving) + if (doorLeft.IsReachedMin() && doorRight.IsReachedMin()) { return true; } @@ -132,7 +99,7 @@ public bool IsMoving() return false; } - private IEnumerator MoveTo() + private IEnumerator Operate() { var waitForFixedUpdate = new WaitForFixedUpdate(); diff --git a/Assets/Scripts/Devices/Modules/LiftControl.cs b/Assets/Scripts/Devices/Modules/LiftControl.cs index 8c703e99..8aa8f58b 100644 --- a/Assets/Scripts/Devices/Modules/LiftControl.cs +++ b/Assets/Scripts/Devices/Modules/LiftControl.cs @@ -11,41 +11,40 @@ public class LiftControl : MonoBehaviour { - private Actuator lift; + private const float MAX_HEIGHT = 1000f; + private const float MIN_HEIGHT = -1000f; - private HashSet hashsetAllTopModels; - private HashSet hashsetLiftingObjects; + private Actuator lift = new Actuator(); - private UnityEvent finishedLiftingEvent; + private HashSet hashsetAllTopModels = new HashSet(); + private HashSet hashsetLiftingObjects = new HashSet(); + + private UnityEvent finishedLiftingEvent = new UnityEvent(); private GameObject rootModel = null; private Transform rootModelTransform = null; public string floorColliderName = string.Empty; private MeshCollider floorCollider = null; - private Vector3 targetPosition = Vector3.zero; public float speed = 1; public bool IsMoving => lift.IsMoving; - LiftControl() - { - lift = new Actuator(); - hashsetAllTopModels = new HashSet(); - hashsetLiftingObjects = new HashSet(); - } - void Awake() { - lift.SetTarget(transform); rootModel = GameObject.Find("Models"); - rootModelTransform = rootModel.transform; - finishedLiftingEvent = new UnityEvent(); } void Start() { + rootModelTransform = rootModel.transform; + + lift.SetTarget(transform); + lift.SetInitialPose(transform.localPosition); lift.SetMovingType(Actuator.MovingType.SmoothDamp); lift.SetMaxSpeed(speed); + lift.SetDirection(Vector3.up); + lift.SetMaxOffset(MAX_HEIGHT); + lift.SetMinOffset(MIN_HEIGHT); // Debug.Log(name + "::" + speed); FindFloorRegionInLift(); @@ -132,12 +131,12 @@ public void MoveTo(in float targetHeight) { DetectObjectsToLiftAndLiftIt(); - lift.SetTargetPosition(Vector3.up, targetHeight); - StartCoroutine(RunLifting()); + lift.SetTargetPosition(targetHeight); + StartCoroutine(DoLifting()); } } - private IEnumerator RunLifting() + private IEnumerator DoLifting() { var waitForFixedUpdate = new WaitForFixedUpdate(); yield return waitForFixedUpdate; diff --git a/Assets/Scripts/Services/SimulationControlService.cs b/Assets/Scripts/Services/SimulationControlService.cs index 0e7de7c5..73cea082 100644 --- a/Assets/Scripts/Services/SimulationControlService.cs +++ b/Assets/Scripts/Services/SimulationControlService.cs @@ -21,14 +21,12 @@ public class SimulationControlRequest [JsonProperty(Order = 2)] public string filter = string.Empty; - public void Print() { Debug.LogFormat("## {0}: {1}", this.GetType().Name, command); } } - public class SimulationControlResponseBase { [JsonProperty(Order = 0)] @@ -40,7 +38,6 @@ public virtual void Print() } } - public class SimulationControlResponseNormal : SimulationControlResponseBase { [JsonProperty(Order = 1)] @@ -63,7 +60,6 @@ public override void Print() } } - public class SimulationControlResponseTopicList : SimulationControlResponseBase { [JsonProperty(Order = 1)] diff --git a/Packages/manifest.json b/Packages/manifest.json index 6176c961..124f638b 100644 --- a/Packages/manifest.json +++ b/Packages/manifest.json @@ -1,6 +1,6 @@ { "dependencies": { - "com.unity.ide.vscode": "1.2.1", + "com.unity.ide.vscode": "1.2.2", "com.unity.mathematics": "1.2.1", "com.unity.render-pipelines.core": "7.3.1", "com.unity.searcher": "4.3.1", diff --git a/Packages/packages-lock.json b/Packages/packages-lock.json index 66e1af01..991a787f 100644 --- a/Packages/packages-lock.json +++ b/Packages/packages-lock.json @@ -1,7 +1,7 @@ { "dependencies": { "com.unity.ide.vscode": { - "version": "1.2.1", + "version": "1.2.2", "depth": 0, "source": "registry", "dependencies": {}, diff --git a/ProjectSettings/ProjectSettings.asset b/ProjectSettings/ProjectSettings.asset index 52d2a727..42738227 100644 --- a/ProjectSettings/ProjectSettings.asset +++ b/ProjectSettings/ProjectSettings.asset @@ -124,7 +124,7 @@ PlayerSettings: 16:10: 1 16:9: 1 Others: 1 - bundleVersion: 1.7.5 + bundleVersion: 1.7.6 preloadedAssets: [] metroInputSource: 0 wsaTransparentSwapchain: 0