diff --git a/Assets/Scenes/MainScene.unity b/Assets/Scenes/MainScene.unity index 5216fb3c..1c159aff 100644 --- a/Assets/Scenes/MainScene.unity +++ b/Assets/Scenes/MainScene.unity @@ -771,6 +771,8 @@ MonoBehaviour: pauseOnStart: 0 clearAllOnStart: 1 worldFileName: lg_seocho.world + modelRootDirectories: [] + worldRootDirectories: [] --- !u!114 &249819590 MonoBehaviour: m_ObjectHideFlags: 0 @@ -3800,10 +3802,13 @@ MonoBehaviour: m_Name: m_EditorClassIdentifier: blockControl: 0 - mainSpeed: 10 - shiftAdd: 20 - maxShift: 50 - camSens: 0.1 + mainSpeed: 7 + shiftAdd: 10 + maxShift: 100 + camSens: 0.25 + edgeWidth: 30 + edgeSens: 0.05 + edgeSensMax: 1.7 --- !u!20 &963194227 Camera: m_ObjectHideFlags: 0 @@ -3924,12 +3929,8 @@ MonoBehaviour: m_Bits: 1 SetMoveType: 116 SetRotateType: 114 - SetScaleType: 0 SetAllTransformType: 121 SetSpaceToggle: 120 - SetPivotModeToggle: 0 - SetCenterTypeToggle: 0 - SetScaleTypeToggle: 0 translationSnapping: 306 AddSelection: 0 RemoveSelection: 0 diff --git a/Assets/Scenes/MainScene/ReflectionProbe-0.exr b/Assets/Scenes/MainScene/ReflectionProbe-0.exr deleted file mode 100644 index 2af7d91c..00000000 Binary files a/Assets/Scenes/MainScene/ReflectionProbe-0.exr and /dev/null differ diff --git a/Assets/Scenes/MainScene/ReflectionProbe-0.exr.meta b/Assets/Scenes/MainScene/ReflectionProbe-0.exr.meta deleted file mode 100644 index aba9f6f9..00000000 --- a/Assets/Scenes/MainScene/ReflectionProbe-0.exr.meta +++ /dev/null @@ -1,91 +0,0 @@ -fileFormatVersion: 2 -guid: 44fd1c569806b0663abaade849850cab -TextureImporter: - internalIDToNameTable: [] - externalObjects: {} - serializedVersion: 10 - mipmaps: - mipMapMode: 0 - enableMipMap: 1 - sRGBTexture: 1 - linearTexture: 0 - fadeOut: 0 - borderMipMap: 0 - mipMapsPreserveCoverage: 0 - alphaTestReferenceValue: 0.5 - mipMapFadeDistanceStart: 1 - mipMapFadeDistanceEnd: 3 - bumpmap: - convertToNormalMap: 0 - externalNormalMap: 0 - heightScale: 0.25 - normalMapFilter: 0 - isReadable: 0 - streamingMipmaps: 0 - streamingMipmapsPriority: 0 - grayScaleToAlpha: 0 - generateCubemap: 6 - cubemapConvolution: 1 - seamlessCubemap: 1 - textureFormat: 1 - maxTextureSize: 2048 - textureSettings: - serializedVersion: 2 - filterMode: 2 - aniso: 0 - mipBias: 0 - wrapU: 1 - wrapV: 1 - wrapW: 1 - nPOTScale: 1 - lightmap: 0 - compressionQuality: 50 - spriteMode: 0 - spriteExtrude: 1 - spriteMeshType: 1 - alignment: 0 - spritePivot: {x: 0.5, y: 0.5} - spritePixelsToUnits: 100 - spriteBorder: {x: 0, y: 0, z: 0, w: 0} - spriteGenerateFallbackPhysicsShape: 1 - alphaUsage: 1 - alphaIsTransparency: 0 - spriteTessellationDetail: -1 - textureType: 0 - textureShape: 2 - singleChannelComponent: 0 - maxTextureSizeSet: 0 - compressionQualitySet: 0 - textureFormatSet: 0 - platformSettings: - - serializedVersion: 3 - buildTarget: DefaultTexturePlatform - maxTextureSize: 2048 - resizeAlgorithm: 0 - textureFormat: -1 - textureCompression: 1 - compressionQuality: 100 - crunchedCompression: 0 - allowsAlphaSplitting: 0 - overridden: 0 - androidETC2FallbackOverride: 0 - forceMaximumCompressionQuality_BC6H_BC7: 0 - spriteSheet: - serializedVersion: 2 - sprites: [] - outline: [] - physicsShape: [] - bones: [] - spriteID: - internalID: 0 - vertices: [] - indices: - edges: [] - weights: [] - secondaryTextures: [] - spritePackingTag: - pSDRemoveMatte: 0 - pSDShowRemoveMatteOption: 0 - userData: - assetBundleName: - assetBundleVariant: diff --git a/Assets/Scripts/DevicePlugins/RobotControl.cs b/Assets/Scripts/DevicePlugins/RobotControl.cs index 1b2c1099..55879ecb 100644 --- a/Assets/Scripts/DevicePlugins/RobotControl.cs +++ b/Assets/Scripts/DevicePlugins/RobotControl.cs @@ -12,78 +12,20 @@ public class RobotControl : DevicePlugin { private MicomInput micomInput = null; private MicomSensor micomSensor = null; - private SensorDevices.IMU imuSensor = null; - private Motor motorLeft = null; - private Motor motorRight = null; - - public float wheelRadius = 0.0f; // in mether - public float divideWheelRadius = 0.0f; protected override void OnAwake() { micomInput = gameObject.AddComponent(); micomSensor = gameObject.AddComponent(); - - imuSensor = gameObject.GetComponentInChildren(); + micomSensor.SetPluginParameter(parameters); } protected override void OnStart() { - const float MM2M = 0.001f; - var updateRate = parameters.GetValue("update_rate", 20); - micomSensor.SetUpdateRate(updateRate); - - var kp = parameters.GetValue("PID/kp"); - var ki = parameters.GetValue("PID/ki"); - var kd = parameters.GetValue("PID/kd"); - - var pidControl = new PID(kp, ki, kd); - - var wheelBase = parameters.GetValue("wheel/base") * MM2M; - wheelRadius = parameters.GetValue("wheel/radius") * MM2M; - divideWheelRadius = 1.0f/wheelRadius; // for performacne. - - var wheelNameLeft = parameters.GetValue("wheel/location[@type='left']"); - var wheelNameRight = parameters.GetValue("wheel/location[@type='right']"); - var debugging = parameters.GetValue("debug", false); micomInput.EnableDebugging = debugging; - var motorFriction = parameters.GetValue("wheel/friction/motor", 0.1f); // Currently not used - var brakeFriction = parameters.GetValue("wheel/friction/brake", 0.1f); // Currently not used - - var modelList = GetComponentsInChildren(); - foreach (var model in modelList) - { - // Debug.Log(model.name); - if (model.name.Equals(wheelNameLeft)) - { - var jointWheelLeft = model.GetComponentInChildren(); - motorLeft = new Motor("Left", jointWheelLeft, pidControl); - - var wheelLeftBody = jointWheelLeft.gameObject.GetComponent(); - - // Debug.Log("joint Wheel Left found : " + jointWheelLeft.name); - // Debug.Log("joint Wheel Left max angular velocity : " + jointWheelLeft.gameObject.GetComponent().maxAngularVelocity); - } - else if (model.name.Equals(wheelNameRight)) - { - var jointWheelRight = model.GetComponentInChildren(); - motorRight = new Motor("Right", jointWheelRight, pidControl); - - var wheelRightBody = jointWheelRight.gameObject.GetComponent(); - - // Debug.Log("joint Wheel Right found : " + jointWheelRight.name); - // Debug.Log("joint Wheel Right max angular velocity : " + jointWheelRight.gameObject.GetComponent().maxAngularVelocity); - } - - if (motorLeft != null && motorRight != null) - { - break; - } - } - var txHashKey = MakeHashKey("MICOM", "_SENSOR"); if (!RegisterTxDevice(txHashKey)) { @@ -102,33 +44,26 @@ protected override void OnStart() void FixedUpdate() { - var localRotation = transform.rotation; - if (imuSensor != null) - { - micomSensor.SetIMU(imuSensor); - micomSensor.SetAccGyro(localRotation.eulerAngles); - // Debug.LogFormat("{0} {1}", localRotation, imuSensor.GetOrientation()); - } - - float linearVelocityLeft = 0; - float linearVelocityRight = 0; - - if (motorLeft != null) - { - var targetWheelVelocityLeft = micomInput.GetWheelVelocityLeft() * divideWheelRadius; - motorLeft.SetVelocityTarget(targetWheelVelocityLeft); - linearVelocityLeft = motorLeft.GetCurrentVelocity() * wheelRadius; - } - - if (motorRight != null) + if (micomInput != null && micomSensor != null) { - var targetWheelVelocityRight = micomInput.GetWheelVelocityRight() * divideWheelRadius; - motorRight.SetVelocityTarget(targetWheelVelocityRight); - linearVelocityRight = motorRight.GetCurrentVelocity() * wheelRadius; + switch (micomInput.ControlType) + { + case MicomInput.VelocityType.LinearAndAngular: + var targetLinearVelocityLeft = micomInput.GetLinearVelocity(); + var targetAngularVelocityRight = micomInput.GetAngularVelocity(); + micomSensor.SetTwistDrive(targetLinearVelocityLeft, targetAngularVelocityRight); + break; + + case MicomInput.VelocityType.LeftAndRight: + var targetWheelLeftVelocity = micomInput.GetWheelLeftVelocity(); + var targetWheelRightVelocity = micomInput.GetWheelRightVelocity(); + micomSensor.SetDifferentialDrive(targetWheelLeftVelocity, targetWheelRightVelocity); + break; + + case MicomInput.VelocityType.Unknown: + break; + } } - - // Debug.LogFormat("{0} {1}, {2} {3} | {4}", motorLeft.GetCurrentVelocity(), linearVelocityLeft, motorRight.GetCurrentVelocity(), linearVelocityRight, wheelRadius); - micomSensor.SetOdomData(linearVelocityLeft, linearVelocityRight); } private void Sender() diff --git a/Assets/Scripts/Devices/Camera.cs b/Assets/Scripts/Devices/Camera.cs index f21b402d..07c4e0bd 100644 --- a/Assets/Scripts/Devices/Camera.cs +++ b/Assets/Scripts/Devices/Camera.cs @@ -228,6 +228,7 @@ protected override void GenerateMessage() { var image = imageStamped.Image; var imageData = camData.GetTextureData(); + if (imageData != null && image.Data.Length == imageData.Length) { image.Data = imageData; diff --git a/Assets/Scripts/Devices/Contact.cs b/Assets/Scripts/Devices/Contact.cs new file mode 100644 index 00000000..d3710f75 --- /dev/null +++ b/Assets/Scripts/Devices/Contact.cs @@ -0,0 +1,185 @@ +/* + * Copyright (c) 2020 LG Electronics Inc. + * + * SPDX-License-Identifier: MIT + */ + +using System.Collections; +using System.Collections.Generic; +using System; +using UnityEngine; +using Stopwatch = System.Diagnostics.Stopwatch; +using messages = gazebo.msgs; + +namespace SensorDevices +{ + public class ContactTrigger : MonoBehaviour + { + public Action collisionEnter = null; + public Action collisionStay = null; + public Action collisionExit = null; + + private void OnCollisionEnter(Collision other) + { + if (collisionEnter != null) + { + collisionEnter.Invoke(other); + } + } + + private void OnCollisionExit(Collision other) + { + if (collisionExit != null) + { + collisionExit.Invoke(other); + } + } + + private void OnCollisionStay(Collision other) + { + if (collisionStay != null) + { + collisionStay.Invoke(other); + } + } + } + + public class Contact : Device + { + private messages.Contacts contacts = null; + + public List collision = new List(); + public string topic = string.Empty; + + private bool contacted = false; + + protected override void OnAwake() + { + deviceName = name; + } + + protected override void OnStart() + { + } + + protected override IEnumerator OnVisualize() + { + var waitForEndOfFrame = new WaitForEndOfFrame(); + var waitForSeconds = new WaitForSeconds(UpdatePeriod); + + while (true) + { + yield return waitForSeconds; + } + } + + protected override void InitializeMessages() + { + contacts = new messages.Contacts(); + contacts.Time = new messages.Time(); + } + + protected override IEnumerator MainDeviceWorker() + { + var sw = new Stopwatch(); + while (true) + { + sw.Restart(); + GenerateMessage(); + sw.Stop(); + + yield return new WaitForSeconds(WaitPeriod((float)sw.Elapsed.TotalSeconds)); + } + } + + protected override void GenerateMessage() + { + DeviceHelper.SetCurrentTime(contacts.Time); + PushData(contacts); + // if (contacts.contact.Count > 0) + // { + // Debug.Log(contacts.contact[0].Depths.Length + " : " + contacts.contact[0].Normals.Count); + // } + contacts.contact.Clear(); + } + + + public void CollisionEnter(Collision other) + { + if (other.contactCount > 0) + { + contacted = true; + } + } + + public void CollisionStay(Collision other) + { + var newContact = new messages.Contact(); + + // TODO: Need to be implemented; + // newContact.Wrenchs + newContact.Depths = new double[1]; + newContact.World = "default"; + DeviceHelper.SetCurrentTime(newContact.Time); + + foreach (var collisionContact in other.contacts) + { + var collision1 = collisionContact.thisCollider.name; + var collision2 = collisionContact.otherCollider.name; + + // find existing collision set + var existingContact = contacts.contact.Find(x => x.Collision1.Contains(collision1) && x.Collision2.Contains(collision2)); + if (existingContact != null) + { + // Debug.Log("Existing!!"); + var depths = existingContact.Depths; + var depthsLength = depths.Length; + Array.Resize(ref depths, depthsLength + 1); + depths[depthsLength] = collisionContact.separation; + existingContact.Depths = depths; + + var normal = new messages.Vector3d(); + DeviceHelper.SetVector3d(normal, collisionContact.normal); + existingContact.Normals.Add(normal); + + var position = new messages.Vector3d(); + DeviceHelper.SetVector3d(position, collisionContact.point); + existingContact.Positions.Add(position); + } + else + { + newContact.Collision1 = collisionContact.thisCollider.name; + newContact.Collision2 = collisionContact.otherCollider.name; + + newContact.Depths[0] = collisionContact.separation; + + var normal = new messages.Vector3d(); + DeviceHelper.SetVector3d(normal, collisionContact.normal); + newContact.Normals.Add(normal); + + var position = new messages.Vector3d(); + DeviceHelper.SetVector3d(position, collisionContact.point); + newContact.Positions.Add(position); + + contacts.contact.Add(newContact); + } + // Debug.DrawLine(collisionContact.point, collisionContact.normal, Color.white); + } + // Debug.Log(other.contactCount + "," + contacts.contact.Count); + // Debug.Log(contacts.contact[0].Depths.Length + " : " + contacts.contact[0].Normals.Count); + } + + public void CollisionExit(Collision other) + { + if (other.contactCount == 0) + { + contacted = false; + } + } + + public bool IsContacted() + { + return contacted; + } + } +} \ No newline at end of file diff --git a/Assets/Scripts/Tools/ProtobufMessages/robot_path.cs.meta b/Assets/Scripts/Devices/Contact.cs.meta similarity index 83% rename from Assets/Scripts/Tools/ProtobufMessages/robot_path.cs.meta rename to Assets/Scripts/Devices/Contact.cs.meta index 37200ee8..b6614926 100644 --- a/Assets/Scripts/Tools/ProtobufMessages/robot_path.cs.meta +++ b/Assets/Scripts/Devices/Contact.cs.meta @@ -1,5 +1,5 @@ fileFormatVersion: 2 -guid: 49848780bb0516c9ebd78b23a40da78f +guid: 26fd959271281d3c6bb3312cc345c380 MonoImporter: externalObjects: {} serializedVersion: 2 diff --git a/Assets/Scripts/Devices/GPS.cs b/Assets/Scripts/Devices/GPS.cs index 2ff5c279..8a92347c 100644 --- a/Assets/Scripts/Devices/GPS.cs +++ b/Assets/Scripts/Devices/GPS.cs @@ -11,7 +11,7 @@ namespace SensorDevices { - public partial class GPS : Device + public class GPS : Device { private messages.Gps gps = null; diff --git a/Assets/Scripts/Devices/IMU.cs b/Assets/Scripts/Devices/IMU.cs index c7cf2161..824f2999 100644 --- a/Assets/Scripts/Devices/IMU.cs +++ b/Assets/Scripts/Devices/IMU.cs @@ -11,7 +11,7 @@ namespace SensorDevices { - public partial class IMU : Device + public class IMU : Device { private messages.Imu imu = null; @@ -103,7 +103,7 @@ protected override void GenerateMessage() DeviceHelper.SetVector3d(imu.AngularVelocity, imuAngularVelocity * Mathf.Deg2Rad); DeviceHelper.SetVector3d(imu.LinearAcceleration, imuLinearAcceleration); DeviceHelper.SetCurrentTime(imu.Stamp); - SetMessageData(imu); + PushData(imu); } public Quaternion GetOrientation() diff --git a/Assets/Scripts/Devices/Lidar.cs b/Assets/Scripts/Devices/Lidar.cs index b6c0febc..d1a9939c 100644 --- a/Assets/Scripts/Devices/Lidar.cs +++ b/Assets/Scripts/Devices/Lidar.cs @@ -288,11 +288,15 @@ protected override IEnumerator OnVisualize() { const float visualUpdatePeriod = 0.090f; const float visualDrawDuration = visualUpdatePeriod * 1.01f; + var startAngle = defaultRotationOffset + (float)angleMin; + var waitForEndOfFrame = new WaitForEndOfFrame(); var waitForSeconds = new WaitForSeconds(visualUpdatePeriod); while (true) { + yield return waitForEndOfFrame; + var lidarSensorWorldPosition = lidarLink.position + transform.localPosition; var rangeData = GetRangeData(); diff --git a/Assets/Scripts/Devices/MicomInput.cs b/Assets/Scripts/Devices/MicomInput.cs index 30e2388e..c571b424 100644 --- a/Assets/Scripts/Devices/MicomInput.cs +++ b/Assets/Scripts/Devices/MicomInput.cs @@ -11,22 +11,31 @@ public class MicomInput : Device { private const float MM2M = 0.001f; + public enum VelocityType {Unknown, LinearAndAngular, LeftAndRight}; private messages.Param micomWritingData = null; + private VelocityType controlType = VelocityType.Unknown; + + public VelocityType ControlType => controlType; + + + // TODO: change to float type?? private int wheelVelocityLeft = 0; // linear velocity in millimeter per second private int wheelVelocityRight = 0; // linear velocity in millimeter per second + private int linearVelocity = 0; // linear velocity in millimeter per second + private int angularVelocity = 0; // angular velocit in deg per second + protected override void OnAwake() { + deviceName = "MicomInput"; } protected override void OnStart() { - deviceName = "MicomInput"; } - protected override IEnumerator MainDeviceWorker() { var waitUntil = new WaitUntil(() => GetDataStream().Length > 0); @@ -61,33 +70,93 @@ protected override void GenerateMessage() micomWritingData = GetMessageData(); if (micomWritingData.Name.Equals("control_type") && - micomWritingData.Value.IntValue == 1 && micomWritingData.Childrens.Count == 2) { - wheelVelocityLeft - = (!micomWritingData.Childrens[0].Name.Equals("nLeftWheelVelocity")) ? - 0 : micomWritingData.Childrens[0].Value.IntValue; + if (micomWritingData.Value.IntValue == 0) + { + controlType = VelocityType.LinearAndAngular; - wheelVelocityRight - = (!micomWritingData.Childrens[1].Name.Equals("nRightWheelVelocity")) ? - 0 : micomWritingData.Childrens[1].Value.IntValue; + linearVelocity + = (!micomWritingData.Childrens[0].Name.Equals("nLinearVelocity")) ? + 0 : micomWritingData.Childrens[0].Value.IntValue; + angularVelocity + = (!micomWritingData.Childrens[1].Name.Equals("nAngularVelocity")) ? + 0 : micomWritingData.Childrens[1].Value.IntValue; + } + else if (micomWritingData.Value.IntValue == 1) + { + controlType = VelocityType.LeftAndRight; + + wheelVelocityLeft + = (!micomWritingData.Childrens[0].Name.Equals("nLeftWheelVelocity")) ? + 0 : micomWritingData.Childrens[0].Value.IntValue; + + wheelVelocityRight + = (!micomWritingData.Childrens[1].Name.Equals("nRightWheelVelocity")) ? + 0 : micomWritingData.Childrens[1].Value.IntValue; + } + else + { + controlType = VelocityType.Unknown; + Debug.LogWarningFormat("MicomInput: Unsupported Control Type({0}", controlType); + } // Debug.Log("nLeftWheelVel: " + wheelVelocityLeft + ", nRightWheelVel : " + wheelVelocityRight); } // Debug.Log("MicomInput: Working OK..."); } - public float GetWheelVelocity(in int index) + public float GetWheelLeftVelocity() + { + return (float)wheelVelocityLeft * MM2M * Mathf.Rad2Deg; + } + + public float GetWheelRightVelocity() + { + return (float)wheelVelocityRight * MM2M * Mathf.Rad2Deg; + } + + public float GetLinearVelocity() + { + return (float)linearVelocity * MM2M; + } + + public float GetAngularVelocity() + { + return (float)angularVelocity * Mathf.Deg2Rad; + } + + public float GetVelocity(in int index) + { + switch (controlType) + { + case VelocityType.LinearAndAngular: + return GetType0Velocity(index); + + case VelocityType.LeftAndRight: + return GetType1Velocity(index); + + case VelocityType.Unknown: + default: + Debug.LogWarning("Unknown control type!!!! nothing to get velocity"); + break; + } + + return 0; + } + + + private float GetType1Velocity(in int index) { var velocity = 0f; switch (index) { case 0: - velocity = GetWheelVelocityLeft(); + velocity = GetWheelLeftVelocity(); break; case 1: - velocity = GetWheelVelocityRight(); + velocity = GetWheelRightVelocity(); break; default: @@ -98,14 +167,24 @@ public float GetWheelVelocity(in int index) return velocity; } - public float GetWheelVelocityLeft() + private float GetType0Velocity(in int index) { - return (float)wheelVelocityLeft * MM2M * Mathf.Rad2Deg; - } + var velocity = 0f; + switch (index) + { + case 0: + velocity = GetLinearVelocity(); + break; - public float GetWheelVelocityRight() - { - return (float)wheelVelocityRight * MM2M * Mathf.Rad2Deg; - } + case 1: + velocity = GetAngularVelocity(); + break; -} + default: + Debug.LogError("Invalid index - " + index); + break; + } + + return velocity; + } +} \ No newline at end of file diff --git a/Assets/Scripts/Devices/MicomSensor.cs b/Assets/Scripts/Devices/MicomSensor.cs index f3b48c5a..3724d849 100644 --- a/Assets/Scripts/Devices/MicomSensor.cs +++ b/Assets/Scripts/Devices/MicomSensor.cs @@ -4,21 +4,170 @@ * SPDX-License-Identifier: MIT */ +using System.Collections.Generic; using System.Collections; using UnityEngine; using messages = gazebo.msgs; public class MicomSensor : Device { + private PluginParameters parameters = null; private messages.Micom micomSensorData = null; + private Motor motorLeft = null; + private Motor motorRight = null; + + private SensorDevices.IMU imuSensor = null; + private List ussSensors = new List(); + private List irSensors = new List(); + // private List magnetSensors = null; + + private SensorDevices.Contact bumperContact = null; + private List bumperSensors = new List(); + + + private float wheelBase = 0.0f; // in meter + private float wheelRadius = 0.0f; // in meter + private float divideWheelRadius = 0.0f; // for computational performacne. + protected override void OnAwake() { + imuSensor = gameObject.GetComponentInChildren(); + deviceName = "MicomSensor"; } protected override void OnStart() { - deviceName = "MicomSensor"; + const float MM2M = 0.001f; + var modelList = GetComponentsInChildren(); + + var updateRate = parameters.GetValue("update_rate", 20); + SetUpdateRate(updateRate); + + var kp = parameters.GetValue("PID/kp"); + var ki = parameters.GetValue("PID/ki"); + var kd = parameters.GetValue("PID/kd"); + + var pidControl = new PID(kp, ki, kd); + + wheelBase = parameters.GetValue("wheel/base") * MM2M; + wheelRadius = parameters.GetValue("wheel/radius") * MM2M; + divideWheelRadius = 1.0f/wheelRadius; + + var wheelNameLeft = parameters.GetValue("wheel/location[@type='left']"); + var wheelNameRight = parameters.GetValue("wheel/location[@type='right']"); + + var motorFriction = parameters.GetValue("wheel/friction/motor", 0.1f); // Currently not used + var brakeFriction = parameters.GetValue("wheel/friction/brake", 0.1f); // Currently not used + + foreach (var model in modelList) + { + // Debug.Log(model.name); + if (model.name.Equals(wheelNameLeft)) + { + var jointWheelLeft = model.GetComponentInChildren(); + motorLeft = new Motor("Left", jointWheelLeft, pidControl); + + var wheelLeftBody = jointWheelLeft.gameObject.GetComponent(); + + // Debug.Log("joint Wheel Left found : " + jointWheelLeft.name); + // Debug.Log("joint Wheel Left max angular velocity : " + jointWheelLeft.gameObject.GetComponent().maxAngularVelocity); + } + else if (model.name.Equals(wheelNameRight)) + { + var jointWheelRight = model.GetComponentInChildren(); + motorRight = new Motor("Right", jointWheelRight, pidControl); + + var wheelRightBody = jointWheelRight.gameObject.GetComponent(); + + // Debug.Log("joint Wheel Right found : " + jointWheelRight.name); + // Debug.Log("joint Wheel Right max angular velocity : " + jointWheelRight.gameObject.GetComponent().maxAngularVelocity); + } + + if (motorLeft != null && motorRight != null) + { + break; + } + } + + if (parameters.GetValues("uss/sensor", out var ussList)) + { + foreach (var uss in ussList) + { + foreach (var model in modelList) + { + if (model.name.Equals(uss)) + { + var sonarSensor = model.GetComponentInChildren(); + ussSensors.Add(sonarSensor); + // Debug.Log("ussSensor found : " + sonarSensor.name); + } + } + } + micomSensorData.uss.Distances = new uint[ussList.Count]; + } + + if (parameters.GetValues("ir/sensor", out var irList)) + { + foreach (var ir in irList) + { + foreach (var model in modelList) + { + if (model.name.Equals(ir)) + { + var sonarSensor = model.GetComponentInChildren(); + irSensors.Add(sonarSensor); + // Debug.Log("irSensor found : " + sonarSensor.name); + } + } + } + micomSensorData.ir.Distances = new uint[irList.Count]; + } + + if (parameters.GetValues("magnet/sensor", out var magnetList)) + { + foreach (var model in modelList) + { + // TODO: to be implemented + } + } + + var targetContactName = parameters.GetAttribute("bumper", "contact"); + // Debug.Log(targetContactName); + + var contactsInChild = GetComponentsInChildren(); + + foreach (var contact in contactsInChild) + { + if (contact.name.Equals(targetContactName)) + { + bumperContact = contact; + // Debug.Log("Found"); + } + } + + if (bumperContact != null) + { + if (parameters.GetValues("bumper/joint_name", out var bumperJointNameList)) + { + var linkList = GetComponentsInChildren(); + foreach (var link in linkList) + { + foreach (var bumperJointName in bumperJointNameList) + { + if (link.jointList.TryGetValue(bumperJointName, out var jointValue)) + { + bumperSensors.Add(jointValue as ConfigurableJoint); + Debug.Log(bumperJointName); + } + } + } + } + + var bumperCount = (bumperSensors == null || bumperSensors.Count == 0) ? 1 : bumperSensors.Count; + + micomSensorData.bumper.Bumpeds = new bool[bumperCount]; + } } protected override IEnumerator MainDeviceWorker() @@ -47,6 +196,9 @@ protected override void InitializeMessages() micomSensorData.Imu.LinearAcceleration = new messages.Vector3d(); micomSensorData.Accgyro = new messages.Micom.AccGyro(); micomSensorData.Odom = new messages.Micom.Odometry(); + micomSensorData.uss = new messages.Micom.Uss(); + micomSensorData.ir = new messages.Micom.Ir(); + micomSensorData.bumper = new messages.Micom.Bumper(); } protected override void GenerateMessage() @@ -56,18 +208,99 @@ protected override void GenerateMessage() PushData(micomSensorData); } - public bool SetIMU(in SensorDevices.IMU imuSensor) + void FixedUpdate() { - var imu = micomSensorData.Imu; + UpdateIMU(); + UpdateAccGyro(); + UpdateUss(); + UpdateIr(); + UpdateBumper(); + } + + private void UpdateBumper() + { + if (micomSensorData == null || micomSensorData.bumper == null || bumperContact == null) + { + return; + } + + if (bumperContact.IsContacted()) + { + if (bumperSensors == null || bumperSensors.Count == 0) + { + micomSensorData.bumper.Bumpeds[0] = true; + } + else + { + var index = 0; + foreach (var bumperJoint in bumperSensors) + { + var threshold = bumperJoint.linearLimit.limit/2; + + var normal = bumperJoint.transform.localPosition.normalized; + // Debug.Log(index + ": " + normal.ToString("F6")); + + if (normal.x > 0 && normal.z < 0) + { + micomSensorData.bumper.Bumpeds[index] = true; + // Debug.Log("Left Bumped"); + } + else if (normal.x < 0 && normal.z < 0) + { + micomSensorData.bumper.Bumpeds[index] = true; + // Debug.Log("Right Bumped"); + } + else + { + micomSensorData.bumper.Bumpeds[index] = false; + // Debug.Log("No Bumped"); + } + + index++; + } + } + } + } + + private void UpdateUss() + { + if ((micomSensorData == null || micomSensorData.uss == null)) + { + return; + } + + const float M2MM = 1000.0f; + var index = 0; + foreach (var uss in ussSensors) + { + micomSensorData.uss.Distances[index++] = (uint)(uss.GetDetectedRange() * M2MM); + } + } + + private void UpdateIr() + { + if ((micomSensorData == null || micomSensorData.ir == null)) + { + return; + } - if (micomSensorData == null || imu == null) + const float M2MM = 1000.0f; + var index = 0; + foreach (var ir in irSensors) { - return false; + micomSensorData.ir.Distances[index++] = (uint)(ir.GetDetectedRange() * M2MM); } + } + + private void UpdateIMU() + { + var imu = micomSensorData.Imu; - if (imu.Orientation == null || imu.AngularVelocity == null || imu.LinearAcceleration == null) + if ((imuSensor == null) || + (micomSensorData == null || imu == null) || + (imu.Orientation == null || imu.AngularVelocity == null || imu.LinearAcceleration == null)) { - return false; + return; } var orientation = imuSensor.GetOrientation(); @@ -87,17 +320,18 @@ public bool SetIMU(in SensorDevices.IMU imuSensor) imu.LinearAcceleration.Z = linearAcceleration.z; DeviceHelper.SetCurrentTime(micomSensorData.Imu.Stamp); - - return true; } - public bool SetAccGyro(in Vector3 angle) + private void UpdateAccGyro() { + var localRotation = transform.rotation; + var angle = localRotation.eulerAngles; + var accGyro = micomSensorData.Accgyro; if (micomSensorData == null || accGyro == null) { - return false; + return; } accGyro.AngleX = angle.x; @@ -111,11 +345,9 @@ public bool SetAccGyro(in Vector3 angle) accGyro.AngulerRateX = 0; accGyro.AngulerRateY = 0; accGyro.AngulerRateZ = 0; - - return true; } - public bool SetOdomData(in float linearVelocityLeft, in float linearVelocityRight) + private bool SetOdomData(in float linearVelocityLeft, in float linearVelocityRight) { const float M2MM = 1000.0f; @@ -135,4 +367,43 @@ public bool SetOdomData(in float linearVelocityLeft, in float linearVelocityRigh return false; } -} + + public void SetDifferentialDrive(in float linearVelocityLeft, in float linearVelocityRight) + { + var angularVelocityLeft = linearVelocityLeft * divideWheelRadius; + var angularVelocityRight = linearVelocityRight * divideWheelRadius; + + SetMotorVelocity(angularVelocityLeft, angularVelocityRight); + } + + public void SetTwistDrive(in float linearVelocity, in float angularVelocity) + { + // m/s, deg/s + // var angularVelocityLeft = ((2 * linearVelocity) + (angularVelocity * wheelBase)) / (2 * wheelRadius); + // var angularVelocityRight = ((2 * linearVelocity) + (angularVelocity * wheelBase)) / (2 * wheelRadius); + var angularCalculation = (angularVelocity * wheelBase * 0.5f); + var angularVelocityLeft = (linearVelocity - angularCalculation) * divideWheelRadius; + var angularVelocityRight = (linearVelocity + angularCalculation) * divideWheelRadius; + + SetMotorVelocity(angularVelocityLeft, angularVelocityRight); + } + + private void SetMotorVelocity(in float angularVelocityLeft, in float angularVelocityRight) + { + if (motorLeft != null && motorRight != null) + { + motorLeft.SetVelocityTarget(angularVelocityLeft); + motorRight.SetVelocityTarget(angularVelocityRight); + + var linearJointVelocityLeft = motorLeft.GetCurrentVelocity() * wheelRadius; + var linearJointVelocityRight = motorRight.GetCurrentVelocity() * wheelRadius; + + SetOdomData(linearJointVelocityLeft, linearJointVelocityRight); + } + } + + public void SetPluginParameter(in PluginParameters pluginParams) + { + parameters = pluginParams; + } +} \ No newline at end of file diff --git a/Assets/Scripts/Devices/MultiCamera.cs b/Assets/Scripts/Devices/MultiCamera.cs index c6893e84..13a3b030 100644 --- a/Assets/Scripts/Devices/MultiCamera.cs +++ b/Assets/Scripts/Devices/MultiCamera.cs @@ -12,7 +12,7 @@ namespace SensorDevices { - public partial class MultiCamera : Device + public class MultiCamera : Device { public List cameras = new List(); diff --git a/Assets/Scripts/Devices/Sonar.cs b/Assets/Scripts/Devices/Sonar.cs index ea67e6af..bf1d3a23 100644 --- a/Assets/Scripts/Devices/Sonar.cs +++ b/Assets/Scripts/Devices/Sonar.cs @@ -12,7 +12,7 @@ namespace SensorDevices { - public partial class Sonar : Device + public class Sonar : Device { private messages.SonarStamped sonarStamped = null; @@ -81,19 +81,21 @@ protected override void OnStart() protected override IEnumerator OnVisualize() { - const float visualUpdatePeriod = 0.01f; - const float visualDrawDuration = visualUpdatePeriod * 1.01f; - var waitForSeconds = new WaitForSeconds(visualUpdatePeriod); + var waitForEndOfFrame = new WaitForEndOfFrame(); + var waitForSeconds = new WaitForSeconds(UpdatePeriod); while (true) { + yield return waitForEndOfFrame; + var direction = (GetDetectedPoint() - sensorStartPoint).normalized; var detectedRange = GetDetectedRange(); - if (detectedRange < rangeMax && !direction.Equals(Vector3.zero)) + if (detectedRange <= rangeMax && !direction.Equals(Vector3.zero)) { - Debug.DrawRay(sensorStartPoint, direction * detectedRange, Color.blue, visualDrawDuration); + Debug.DrawRay(sensorStartPoint, direction * detectedRange, Color.blue, UpdatePeriod); } + yield return waitForSeconds; } } @@ -138,7 +140,7 @@ protected override void GenerateMessage() DeviceHelper.SetVector3d(sonar.WorldPose.Position, sonarPosition); DeviceHelper.SetQuaternion(sonar.WorldPose.Orientation, sonarRotation); DeviceHelper.SetCurrentTime(sonarStamped.Time); - SetMessageData(sonarStamped); + PushData(sonarStamped); } private void ResolveSensingArea(in MeshCollider meshCollider) @@ -202,8 +204,9 @@ void OnTriggerStay(Collider other) if (Physics.Raycast(sensorStartPoint, direction, out var hitInfo)) { - Debug.DrawRay(sensorStartPoint, direction, Color.magenta, 0.01f); - // Debug.Log("Hit Point of contact: " + hitInfo.point); + // Debug.DrawRay(sensorStartPoint, direction, Color.magenta, 0.01f); + + // Debug.Log("Hit Point of contact: " + hitInfo.point + " | " + sensorStartPoint.ToString("F4")); var hitPoint = hitInfo.point; var hitDistance = Vector3.Distance(sensorStartPoint, hitPoint); diff --git a/Assets/Scripts/ModelLoader.cs b/Assets/Scripts/ModelLoader.cs index ce433349..45ee2b2a 100644 --- a/Assets/Scripts/ModelLoader.cs +++ b/Assets/Scripts/ModelLoader.cs @@ -31,20 +31,14 @@ public class ModelLoader : MonoBehaviour private string filesRootDirectory = string.Empty; - public List modelRootDirectories; - public List worldRootDirectories; + public List modelRootDirectories = new List(); + public List worldRootDirectories = new List(); private GameObject modelsRoot = null; private bool isResetting = false; private bool resetTriggered = false; - ModelLoader() - { - modelRootDirectories = new List(); - worldRootDirectories= new List(); - } - private void CleanAllModels() { foreach (var child in modelsRoot.GetComponentsInChildren()) diff --git a/Assets/Scripts/Tools/ProtobufMessages/path_info.cs b/Assets/Scripts/Tools/ProtobufMessages/path_info.cs deleted file mode 100644 index 0611e441..00000000 --- a/Assets/Scripts/Tools/ProtobufMessages/path_info.cs +++ /dev/null @@ -1,68 +0,0 @@ -// -// This file was generated by a tool; you should avoid making direct changes. -// Consider using 'partial classes' to extend these types -// Input: path_info.proto -// - -#pragma warning disable CS0612, CS1591, CS3021, IDE1006, RCS1036, RCS1057, RCS1085, RCS1192 -namespace gazebo.msgs -{ - - [global::ProtoBuf.ProtoContract(Name = @"Path_Info")] - public partial class PathInfo : global::ProtoBuf.IExtensible - { - private global::ProtoBuf.IExtension __pbn__extensionData; - global::ProtoBuf.IExtension global::ProtoBuf.IExtensible.GetExtensionObject(bool createIfMissing) - => global::ProtoBuf.Extensible.GetExtensionObject(ref __pbn__extensionData, createIfMissing); - - [global::ProtoBuf.ProtoMember(1, Name = @"datainfo", IsRequired = true)] - public int Datainfo { get; set; } - - [global::ProtoBuf.ProtoMember(2)] - public float fXmm - { - get { return __pbn__fXmm.GetValueOrDefault(); } - set { __pbn__fXmm = value; } - } - public bool ShouldSerializefXmm() => __pbn__fXmm != null; - public void ResetfXmm() => __pbn__fXmm = null; - private float? __pbn__fXmm; - - [global::ProtoBuf.ProtoMember(3)] - public float fYmm - { - get { return __pbn__fYmm.GetValueOrDefault(); } - set { __pbn__fYmm = value; } - } - public bool ShouldSerializefYmm() => __pbn__fYmm != null; - public void ResetfYmm() => __pbn__fYmm = null; - private float? __pbn__fYmm; - - [global::ProtoBuf.ProtoMember(4)] - public float fDeg - { - get { return __pbn__fDeg.GetValueOrDefault(); } - set { __pbn__fDeg = value; } - } - public bool ShouldSerializefDeg() => __pbn__fDeg != null; - public void ResetfDeg() => __pbn__fDeg = null; - private float? __pbn__fDeg; - - [global::ProtoBuf.ProtoMember(5, Name = @"corner")] - public int Corner - { - get { return __pbn__Corner.GetValueOrDefault(); } - set { __pbn__Corner = value; } - } - public bool ShouldSerializeCorner() => __pbn__Corner != null; - public void ResetCorner() => __pbn__Corner = null; - private int? __pbn__Corner; - - [global::ProtoBuf.ProtoMember(6, Name = @"index", IsRequired = true)] - public int Index { get; set; } - - } - -} - -#pragma warning restore CS0612, CS1591, CS3021, IDE1006, RCS1036, RCS1057, RCS1085, RCS1192 diff --git a/Assets/Scripts/Tools/ProtobufMessages/path_info.cs.meta b/Assets/Scripts/Tools/ProtobufMessages/path_info.cs.meta deleted file mode 100644 index a861aad0..00000000 --- a/Assets/Scripts/Tools/ProtobufMessages/path_info.cs.meta +++ /dev/null @@ -1,11 +0,0 @@ -fileFormatVersion: 2 -guid: b1533ae74b43825d0894d344083a41f6 -MonoImporter: - externalObjects: {} - serializedVersion: 2 - defaultReferences: [] - executionOrder: 0 - icon: {instanceID: 0} - userData: - assetBundleName: - assetBundleVariant: diff --git a/Assets/Scripts/Tools/ProtobufMessages/robot_path.cs b/Assets/Scripts/Tools/ProtobufMessages/robot_path.cs deleted file mode 100644 index aaa4b2a9..00000000 --- a/Assets/Scripts/Tools/ProtobufMessages/robot_path.cs +++ /dev/null @@ -1,37 +0,0 @@ -// -// This file was generated by a tool; you should avoid making direct changes. -// Consider using 'partial classes' to extend these types -// Input: robot_path.proto -// - -#pragma warning disable CS0612, CS1591, CS3021, IDE1006, RCS1036, RCS1057, RCS1085, RCS1192 -namespace gazebo.msgs -{ - - [global::ProtoBuf.ProtoContract(Name = @"Robot_Path")] - public partial class RobotPath : global::ProtoBuf.IExtensible - { - private global::ProtoBuf.IExtension __pbn__extensionData; - global::ProtoBuf.IExtension global::ProtoBuf.IExtensible.GetExtensionObject(bool createIfMissing) - => global::ProtoBuf.Extensible.GetExtensionObject(ref __pbn__extensionData, createIfMissing); - - [global::ProtoBuf.ProtoMember(1, Name = @"datasize", IsRequired = true)] - public int Datasize { get; set; } - - [global::ProtoBuf.ProtoMember(2, Name = @"fXmm")] - public float[] fXmms { get; set; } - - [global::ProtoBuf.ProtoMember(3, Name = @"fYmm")] - public float[] fYmms { get; set; } - - [global::ProtoBuf.ProtoMember(4, Name = @"fDeg")] - public float[] fDegs { get; set; } - - [global::ProtoBuf.ProtoMember(5, Name = @"corner")] - public int[] Corners { get; set; } - - } - -} - -#pragma warning restore CS0612, CS1591, CS3021, IDE1006, RCS1036, RCS1057, RCS1085, RCS1192 diff --git a/Assets/Scripts/Tools/SDF/Entity.cs b/Assets/Scripts/Tools/SDF/Entity.cs index 7e10511a..20e6d152 100644 --- a/Assets/Scripts/Tools/SDF/Entity.cs +++ b/Assets/Scripts/Tools/SDF/Entity.cs @@ -5,6 +5,7 @@ */ using System.Collections.Generic; +using System.Linq; using System.Xml; using System; @@ -40,10 +41,10 @@ protected void LoadData(XmlNode node) //Console.WriteLine("Load {0} Info", typeof(T).Name); - XmlNodeList nodeList = node.SelectNodes(targetTag); + var nodeList = node.SelectNodes(targetTag); // Console.WriteLine("Num Of model nodes: " + nodeList.Count); - foreach (XmlNode nodeItem in nodeList) + foreach (var nodeItem in nodeList) { //Console.WriteLine(" NAME: " + node.Attributes["name"].Value); items.Add((T)Activator.CreateInstance(typeof(T), nodeItem)); @@ -124,6 +125,29 @@ protected XmlNodeList GetNodes(in string xpath) return GetValue(GetNode(xpath), defaultValue); } + protected bool GetValues(in string xpath, out List valueList) + { + var nodeList = new List(GetNodes("contact/collision").Cast()); + if (nodeList == null) + { + valueList = null; + return false; + } + + valueList = nodeList.ConvertAll(node => + { + if (node == null) + { + return default(T); + } + + var value = node.InnerXml.Trim(); + return ConvertValueType(value); + }); + + return true; + } + protected T GetAttribute(in string attributeName, in T defaultValue = default(T)) { var targetAttribute = attributes[attributeName]; @@ -185,7 +209,7 @@ public static T ConvertValueType(string value) return (T)Convert.ChangeType(value, code); } - private static T ConvertXmlNodeToValue(in XmlNode node) + public static T ConvertXmlNodeToValue(in XmlNode node) { if (node == null) { diff --git a/Assets/Scripts/Tools/SDF/Importer.cs b/Assets/Scripts/Tools/SDF/Importer.cs index 97e96716..ab241fa0 100644 --- a/Assets/Scripts/Tools/SDF/Importer.cs +++ b/Assets/Scripts/Tools/SDF/Importer.cs @@ -91,7 +91,7 @@ private void ImportVisuals(IReadOnlyList items, in Object parentObject) foreach (var item in items) { // Console.WriteLine("[Visual] {0}", item.Name); - Object createdObject = ImportVisual(item, parentObject); + var createdObject = ImportVisual(item, parentObject); ImportGeometry(item.GetGeometry(), createdObject); @@ -108,7 +108,7 @@ private void ImportCollisions(IReadOnlyList items, in Object parentOb foreach (var item in items) { // Console.WriteLine("[Collision] {0}", item.Name); - Object createdObject = ImportCollision(item, parentObject); + var createdObject = ImportCollision(item, parentObject); ImportGeometry(item.GetGeometry(), createdObject); @@ -120,7 +120,7 @@ private void ImportSensors(IReadOnlyList items, in Object parentObject) { foreach (var item in items) { - Object createdObject = ImportSensor(item, parentObject); + var createdObject = ImportSensor(item, parentObject); ImportPlugins(item.GetPlugins(), createdObject); } @@ -131,7 +131,7 @@ private void ImportLinks(IReadOnlyList items, in Object parentObject) foreach (var item in items) { // Console.WriteLine("[Link] {0}", item.Name); - Object createdObject = ImportLink(item, parentObject); + var createdObject = ImportLink(item, parentObject); ImportVisuals(item.GetVisuals(), createdObject); diff --git a/Assets/Scripts/Tools/SDF/Joint.cs b/Assets/Scripts/Tools/SDF/Joint.cs index ba84d09d..2a0b0001 100644 --- a/Assets/Scripts/Tools/SDF/Joint.cs +++ b/Assets/Scripts/Tools/SDF/Joint.cs @@ -44,6 +44,11 @@ public bool UseLimit() } } + public class OdePhysics + { + public double max_force = double.PositiveInfinity; + } + public class Joint : Entity { private string parent = string.Empty; @@ -57,6 +62,8 @@ public class Joint : Entity private Axis axis2 = null; // for revolute2(second axis)/universal joints // : TBD + private OdePhysics odePhysics = null; + // : TBD, ??? public string ParentLinkName => parent; @@ -66,9 +73,13 @@ public class Joint : Entity public Axis Axis => axis; public Axis Axis2 => axis2; + public OdePhysics OdePhysics => odePhysics; + public Joint(XmlNode _node) : base(_node) { + odePhysics = new OdePhysics(); + if (root != null) { ParseElements(); @@ -122,7 +133,14 @@ protected override void ParseElements() axis2.limit_upper = GetValue("axis2/limit/upper"); } } - } + if (IsValidNode("physics/ode")) + { + if (IsValidNode("physics/ode/max_force")) + { + odePhysics.max_force = GetValue("physics/ode/max_force"); + } + } + } } } \ No newline at end of file diff --git a/Assets/Scripts/Tools/SDF/Root.cs b/Assets/Scripts/Tools/SDF/Root.cs index 7b07df6c..ea514f3b 100644 --- a/Assets/Scripts/Tools/SDF/Root.cs +++ b/Assets/Scripts/Tools/SDF/Root.cs @@ -38,8 +38,6 @@ public void SetWorldFileName(in string filename) { worldFileName = filename; } - - worldFileName = "/" + worldFileName; } public World World() @@ -74,15 +72,21 @@ public bool DoParse() if (doc != null && worldFileName != null && worldFileName.Length > 0) { - // Console.WriteLine("World SDF FILE PATH: " + worldFileName); - foreach (var worlPath in worldDefaultPath) + // Console.WriteLine("World file, PATH: " + worldFileName); + foreach (var worldPath in worldDefaultPath) { - var fullFilePath = worlPath + worldFileName; + var fullFilePath = worldPath + "/" + worldFileName; if (File.Exists(@fullFilePath)) { - doc.Load(worlPath + worldFileName); + doc.Load(fullFilePath); break; } + else + { + (Console.Out as DebugLogWriter).SetWarning(true); + Console.WriteLine("World file not exist: " + worldFileName); + (Console.Out as DebugLogWriter).SetWarning(false); + } } } diff --git a/Assets/Scripts/Tools/SDF/Scene.cs b/Assets/Scripts/Tools/SDF/Scene.cs index cd0a53c7..86bd58e0 100644 --- a/Assets/Scripts/Tools/SDF/Scene.cs +++ b/Assets/Scripts/Tools/SDF/Scene.cs @@ -12,13 +12,13 @@ public class Scene { private XmlNode root = null; - // ambient - // background - // sky - // shadows - // fog - // grid - // origin_visual + // : TBD + // : TBD + // : TBD + // : TBD + // : TBD + // : TBD + // : TBD public Scene(XmlNode _node) { diff --git a/Assets/Scripts/Tools/SDF/Sensor.Parse.cs b/Assets/Scripts/Tools/SDF/Sensor.Parse.cs index 1af234a4..e31a72a2 100644 --- a/Assets/Scripts/Tools/SDF/Sensor.Parse.cs +++ b/Assets/Scripts/Tools/SDF/Sensor.Parse.cs @@ -22,7 +22,9 @@ private Ray ParseRay() ray.horizontal.max_angle = GetValue("ray/scan/horizontal/max_angle"); if (ray.horizontal.max_angle < ray.horizontal.min_angle) + { Console.WriteLine("Must be greater or equal to min_angle"); + } if (IsValidNode("ray/scan/vertical")) { @@ -32,10 +34,14 @@ private Ray ParseRay() ray.vertical.max_angle = GetValue("ray/scan/vertical/max_angle"); if (ray.vertical.samples == 0) + { Console.WriteLine("vertical sample cannot be zero"); + } if (ray.vertical.max_angle < ray.vertical.min_angle) + { Console.WriteLine("Must be greater or equal to min_angle"); + } } } @@ -262,5 +268,19 @@ private GPS ParseGPS() return gps; } + + private Contact ParseContact() + { + var contact = new Contact(); + + if (GetValues("contact/collision", out var collisionList)) + { + contact.collision = collisionList; + } + + contact.topic = GetValue("contact/topic"); + + return contact; + } } } \ No newline at end of file diff --git a/Assets/Scripts/Tools/SDF/Sensor.cs b/Assets/Scripts/Tools/SDF/Sensor.cs index ab952c5b..432e4f0f 100644 --- a/Assets/Scripts/Tools/SDF/Sensor.cs +++ b/Assets/Scripts/Tools/SDF/Sensor.cs @@ -65,7 +65,7 @@ protected override void ParseElements() // Console.WriteLine("[{0}] P:{1} C:{2}", GetType().Name, parent, child); - if (IsValidNode("ray") && (Type.Equals("gpu_ray") || Type.Equals("ray"))) + if (IsValidNode("ray") && (Type.Equals("gpu_ray") || Type.Equals("ray") || Type.Equals("lidar"))) { sensor = ParseRay(); } @@ -73,7 +73,7 @@ protected override void ParseElements() { if (Type.Equals("multicamera")) { - Cameras cameras = new Cameras(); + var cameras = new Cameras(); cameras.name = "multiple_camera"; var nodes = GetNodes("camera"); @@ -107,6 +107,10 @@ protected override void ParseElements() { sensor = ParseGPS(); } + else if (IsValidNode("contact") && Type.Equals("contact")) + { + sensor = ParseContact(); + } else { Console.WriteLine("Not supported sensor type!!!!! => " + Type); diff --git a/Assets/Scripts/Tools/SDF/SensorType.cs b/Assets/Scripts/Tools/SDF/SensorType.cs index c91b6daa..b2e39e7b 100644 --- a/Assets/Scripts/Tools/SDF/SensorType.cs +++ b/Assets/Scripts/Tools/SDF/SensorType.cs @@ -144,7 +144,11 @@ public class Intrinsics public Lens lens = new Lens(); } - // : TBD + public class Contact : SensorType + { + public List collision = new List(); + public string topic; + } public class GPS : SensorType { diff --git a/Assets/Scripts/Tools/SDFImporter/SDF2Unity.cs b/Assets/Scripts/Tools/SDFImporter/SDF2Unity.cs index 25c3582d..5d9011c8 100644 --- a/Assets/Scripts/Tools/SDFImporter/SDF2Unity.cs +++ b/Assets/Scripts/Tools/SDFImporter/SDF2Unity.cs @@ -9,7 +9,7 @@ public class SDF2Unity { public static Vector3 GetPosition(double x, double y, double z) { - Vector3 pos = new Vector3(); + var pos = new Vector3(); pos.x = (float)x; pos.y = (float)z; pos.z = (float)y; @@ -19,7 +19,7 @@ public static Vector3 GetPosition(double x, double y, double z) public static Vector3 GetPosition(SDF.Vector3 value) { - Vector3 pos = new Vector3(); + var pos = new Vector3(); pos.x = (float)(value.X); pos.y = (float)(value.Z); pos.z = (float)(value.Y); @@ -43,7 +43,7 @@ public static Vector3 GetScale(SDF.Vector3 value) public static Vector3 GetScale(double radius) { - Vector3 pos = new Vector3(); + var pos = new Vector3(); pos.x = (float)(radius);// * 2.0f; pos.y = pos.x; pos.z = pos.y; @@ -52,7 +52,7 @@ public static Vector3 GetScale(double radius) public static void LoadObjMesh(in GameObject targetObject, in string objPath, in string mtlPath) { - GameObject loadedObject = new Dummiesman.OBJLoader().Load(objPath, mtlPath); + var loadedObject = new Dummiesman.OBJLoader().Load(objPath, mtlPath); var meshRotation = new Vector3(90f, 180f, 0f); foreach (var meshFilter in loadedObject.GetComponentsInChildren()) @@ -74,7 +74,7 @@ public static void LoadStlMesh(in GameObject targetObject, in string objPath) { const string commonShader = "Standard (Specular setup)"; - Mesh[] multipleMesh = Parabox.Stl.Importer.Import(objPath, Parabox.Stl.CoordinateSpace.Right, Parabox.Stl.UpAxis.Z, true); + var multipleMesh = Parabox.Stl.Importer.Import(objPath, Parabox.Stl.CoordinateSpace.Right, Parabox.Stl.UpAxis.Z, true); var meshRotation = new Vector3(0.0f, 90.0f, 0.0f); for (int i = 0; i < multipleMesh.Length; i++) diff --git a/Assets/Scripts/Tools/SDFImporter/SDFImplement.Joint.cs b/Assets/Scripts/Tools/SDFImporter/SDFImplement.Joint.cs index eadc4ffc..96e9e058 100644 --- a/Assets/Scripts/Tools/SDFImporter/SDFImplement.Joint.cs +++ b/Assets/Scripts/Tools/SDFImporter/SDFImplement.Joint.cs @@ -13,20 +13,39 @@ public partial class SDFImplement { public class Joint { - private static Vector3 GetAxis(SDF.Vector3 value) + private static Vector3 GetAxis(SDF.Vector3 value, SDF.Quaternion rot = null) { - Vector3 pos = new Vector3(); + var pos = new Vector3(); pos.x = -value.X; pos.y = -value.Z; pos.z = -value.Y; + + if (rot != null) + { + if (pos.x != 0) + { + pos.y = pos.x * Mathf.Sin((float)rot.Pitch); + pos.z = pos.x * Mathf.Sin((float)rot.Yaw); + } + else if (pos.y != 0) + { + pos.x = pos.y * Mathf.Sin((float)rot.Roll); + pos.z = pos.y * Mathf.Sin((float)rot.Pitch); + } + else if (pos.z != 0) + { + pos.x = pos.z * Mathf.Sin((float)rot.Roll); + pos.y = pos.z * Mathf.Sin((float)rot.Yaw); + } + } return pos; } - public static UEJoint AddRevolute(in SDF.Axis jointInfo, in GameObject targetObject, in Rigidbody connectBody) + public static UEJoint AddRevolute(in SDF.Axis axisInfo, in GameObject targetObject, in Rigidbody connectBody) { var hingeJointComponent = targetObject.AddComponent(); hingeJointComponent.connectedBody = connectBody; - hingeJointComponent.axis = GetAxis(jointInfo.xyz); + hingeJointComponent.axis = GetAxis(axisInfo.xyz); hingeJointComponent.useMotor = false; var jointMotor = new JointMotor(); @@ -36,11 +55,11 @@ public static UEJoint AddRevolute(in SDF.Axis jointInfo, in GameObject targetObj hingeJointComponent.motor = jointMotor; - if (jointInfo.UseLimit()) + if (axisInfo.UseLimit()) { - JointLimits jointLimits = new JointLimits(); - jointLimits.min = (float)jointInfo.limit_lower * Mathf.Rad2Deg; - jointLimits.max = (float)jointInfo.limit_upper * Mathf.Rad2Deg; + var jointLimits = new JointLimits(); + jointLimits.min = (float)axisInfo.limit_lower * Mathf.Rad2Deg; + jointLimits.max = (float)axisInfo.limit_upper * Mathf.Rad2Deg; hingeJointComponent.useLimits = true; hingeJointComponent.limits = jointLimits; } @@ -48,19 +67,19 @@ public static UEJoint AddRevolute(in SDF.Axis jointInfo, in GameObject targetObj return hingeJointComponent; } - public static UEJoint AddRevolute2(in SDF.Axis jointInfo1, in SDF.Axis jointInfo2, in GameObject targetObject, in Rigidbody connectBody) + public static UEJoint AddRevolute2(in SDF.Axis axisInfo1, in SDF.Axis axisInfo2, in GameObject targetObject, in Rigidbody connectBody) { var jointComponent = targetObject.AddComponent(); var confJointComponent = targetObject.AddComponent(); - confJointComponent.axis = GetAxis(jointInfo1.xyz); - confJointComponent.secondaryAxis = GetAxis(jointInfo2.xyz); + confJointComponent.axis = GetAxis(axisInfo1.xyz); + confJointComponent.secondaryAxis = GetAxis(axisInfo2.xyz); - // jointInfo1.limit_lower; - // jointInfo1.limit_upper; + // axisInfo1.limit_lower; + // axisInfo1.limit_upper; - // jointInfo2.limit_lower; - // jointInfo2.limit_upper; + // axisInfo2.limit_lower; + // axisInfo2.limit_upper; return jointComponent; } @@ -100,21 +119,21 @@ public static UEJoint AddBall(in GameObject targetObject, in Rigidbody connectBo return jointComponent; } - public static UEJoint AddPrismatic(in SDF.Axis jointInfo, in GameObject targetObject, in Rigidbody connectBody) + public static UEJoint AddPrismatic(in SDF.Axis axisInfo, in SDF.OdePhysics physicsInfo, in SDF.Pose pose, in GameObject targetObject, in Rigidbody connectBody) { var jointComponent = targetObject.AddComponent(); jointComponent.connectedBody = connectBody; jointComponent.secondaryAxis = Vector3.zero; - jointComponent.axis = GetAxis(jointInfo.xyz); + jointComponent.axis = GetAxis(axisInfo.xyz, pose.Rot); var configurableJointMotion = ConfigurableJointMotion.Free; - if (jointInfo.UseLimit()) + if (axisInfo.UseLimit()) { - // Debug.LogWarningFormat("limit uppper{0}, lower{1}", jointInfo.limit_upper, jointInfo.limit_lower); + // Debug.LogWarningFormat("limit uppper{0}, lower{1}", axisInfo.limit_upper, axisInfo.limit_lower); configurableJointMotion = ConfigurableJointMotion.Limited; var linearLimit = new SoftJointLimit(); - linearLimit.limit = (float)(jointInfo.limit_upper); + linearLimit.limit = (float)(axisInfo.limit_upper); jointComponent.linearLimit = linearLimit; } @@ -125,15 +144,15 @@ public static UEJoint AddPrismatic(in SDF.Axis jointInfo, in GameObject targetOb jointComponent.linearLimitSpring = linearLimitSpring; var softJointLimit = new SoftJointLimit(); - softJointLimit.limit = (float)(jointInfo.limit_upper - jointInfo.limit_lower); + softJointLimit.limit = (float)(axisInfo.limit_upper - axisInfo.limit_lower); softJointLimit.bounciness = 0.000f; softJointLimit.contactDistance = 0.0f; jointComponent.linearLimit = softJointLimit; var jointDrive = new JointDrive(); - jointDrive.positionSpring = (float)(jointInfo.dynamics_spring_stiffness); - jointDrive.positionDamper = (float)(jointInfo.dynamics_damping); - jointDrive.maximumForce = Mathf.Infinity; // 3.402823e+38 + jointDrive.positionSpring = (float)axisInfo.dynamics_spring_stiffness; + jointDrive.positionDamper = (float)axisInfo.dynamics_damping; + jointDrive.maximumForce = (float)physicsInfo.max_force; var zeroJointDriver = new JointDrive(); zeroJointDriver.positionSpring = 0; @@ -158,16 +177,16 @@ public static UEJoint AddPrismatic(in SDF.Axis jointInfo, in GameObject targetOb return jointComponent; } - public static void SetCommonConfiguration(UEJoint jointComponent, in GameObject linkObject) + public static void SetCommonConfiguration(UEJoint jointComponent, in SDF.Vector3 jointPosition, in GameObject linkObject) { var linkTransform = linkObject.transform; - jointComponent.anchor = Vector3.zero; + jointComponent.anchor = SDF2Unity.GetPosition(jointPosition); jointComponent.autoConfigureConnectedAnchor = false; if (jointComponent.autoConfigureConnectedAnchor == false) { - var connectedAnchor = new Vector3(); + var connectedAnchor = Vector3.zero; var connectedBody = jointComponent.connectedBody; var modelObject = linkTransform.parent; @@ -178,7 +197,7 @@ public static void SetCommonConfiguration(UEJoint jointComponent, in GameObject if (Array.Exists(childRigidBodies, element => element == connectedBody)) { // Debug.Log("Sibling connected!!!: " + linkTransform.name); - connectedAnchor = linkTransform.localPosition; + connectedAnchor = SDF2Unity.GetPosition(jointPosition) + linkTransform.localPosition; } else { @@ -213,7 +232,6 @@ public static void SetCommonConfiguration(UEJoint jointComponent, in GameObject connectedAnchor = finalAnchor; } - // Debug.LogFormat("JointObject({0}) connectedAnchor: " + connectedAnchor.ToString("F4"), linkObject.name); jointComponent.connectedAnchor = connectedAnchor; } diff --git a/Assets/Scripts/Tools/SDFImporter/SDFImplement.Sensor.cs b/Assets/Scripts/Tools/SDFImporter/SDFImplement.Sensor.cs index 7233e69c..672d852a 100644 --- a/Assets/Scripts/Tools/SDFImporter/SDFImplement.Sensor.cs +++ b/Assets/Scripts/Tools/SDFImporter/SDFImplement.Sensor.cs @@ -27,7 +27,7 @@ private static string GetFrameName(in GameObject currentObject) return frameName.Substring(2); } - private static void AttachSensor(in GameObject sensorObject, in GameObject targetObject) + private static void AttachSensor(in GameObject sensorObject, in GameObject targetObject, SDF.Pose sensorPose = null) { try { @@ -36,22 +36,17 @@ private static void AttachSensor(in GameObject sensorObject, in GameObject targe sensorTransform.rotation = Quaternion.identity; sensorTransform.SetParent(targetObject.transform, false); sensorTransform.localScale = Vector3.one; - sensorTransform.localPosition = Vector3.zero; - sensorTransform.localRotation = Quaternion.identity; - } - catch - { - Debug.Log("sensorObject is null or Invalid obejct exist"); - } - } - public static void TransformSensor(in GameObject sensorObject, SDF.Pose sensorPose) - { - try - { - var sensorTransform = sensorObject.transform; - sensorTransform.localPosition = SDF2Unity.GetPosition(sensorPose.Pos); - sensorTransform.localRotation = SDF2Unity.GetRotation(sensorPose.Rot); + if (sensorPose == null) + { + sensorTransform.localPosition = Vector3.zero; + sensorTransform.localRotation = Quaternion.identity; + } + else + { + sensorTransform.localPosition = SDF2Unity.GetPosition(sensorPose.Pos); + sensorTransform.localRotation = SDF2Unity.GetRotation(sensorPose.Rot); + } } catch { @@ -84,8 +79,8 @@ public static Device AddLidar(in SDF.Ray element, in GameObject targetObject) public static Device AddCamera(in SDF.Camera element, in GameObject targetObject) { var newSensorObject = new GameObject(); - AttachSensor(newSensorObject, targetObject); - TransformSensor(newSensorObject, element.Pose); + AttachSensor(newSensorObject, targetObject, element.Pose); + var camera = newSensorObject.AddComponent(); camera.deviceName = GetFrameName(newSensorObject); camera.parameters = element; @@ -95,8 +90,7 @@ public static Device AddCamera(in SDF.Camera element, in GameObject targetObject public static Device AddDepthCamera(in SDF.Camera element, in GameObject targetObject) { var newSensorObject = new GameObject(); - AttachSensor(newSensorObject, targetObject); - TransformSensor(newSensorObject, element.Pose); + AttachSensor(newSensorObject, targetObject, element.Pose); var depthCamera = newSensorObject.AddComponent(); depthCamera.deviceName = GetFrameName(newSensorObject); @@ -153,5 +147,23 @@ public static Device AddGps(in SDF.GPS element, in GameObject targetObject) return gps; } + + public static Device AddContact(in SDF.Contact element, in GameObject targetObject) + { + var newSensorObject = new GameObject(); + AttachSensor(newSensorObject, targetObject); + + var contact = newSensorObject.AddComponent(); + contact.deviceName = GetFrameName(newSensorObject); + contact.collision = element.collision; + contact.topic = element.topic; + + var contactTrigger = targetObject.AddComponent(); + contactTrigger.collisionEnter = contact.CollisionEnter; + contactTrigger.collisionExit = contact.CollisionExit; + contactTrigger.collisionStay = contact.CollisionStay; + + return contact; + } } } diff --git a/Assets/Scripts/Tools/SDFImporter/SDFImporter.Joint.cs b/Assets/Scripts/Tools/SDFImporter/SDFImporter.Joint.cs index d6ea3e8f..57d932bf 100644 --- a/Assets/Scripts/Tools/SDFImporter/SDFImporter.Joint.cs +++ b/Assets/Scripts/Tools/SDFImporter/SDFImporter.Joint.cs @@ -49,8 +49,8 @@ protected override void ImportJoint(in SDF.Joint joint, in System.Object parentO var transformParent = (parentObject as GameObject).transform; - GameObject linkObjectParent = FindObjectByName(linkNameParent, transformParent); - GameObject linkObjectChild = FindObjectByName(linkNameChild, transformParent); + var linkObjectParent = FindObjectByName(linkNameParent, transformParent); + var linkObjectChild = FindObjectByName(linkNameChild, transformParent); if (linkObjectChild is null || linkObjectParent is null) { @@ -71,13 +71,13 @@ protected override void ImportJoint(in SDF.Joint joint, in System.Object parentO if (joint.Type.Equals("ball")) { - var ballJointComponent = SDFImplement.Joint.AddBall(linkObjectChild, rigidBodyParent); - jointComponent = ballJointComponent as Joint; + var ballJoint = SDFImplement.Joint.AddBall(linkObjectChild, rigidBodyParent); + jointComponent = ballJoint as Joint; } else if (joint.Type.Equals("prismatic")) { - var prismaticJointComponent = SDFImplement.Joint.AddPrismatic(joint.Axis, linkObjectChild, rigidBodyParent); - jointComponent = prismaticJointComponent as Joint; + var prismaticJoint = SDFImplement.Joint.AddPrismatic(joint.Axis, joint.OdePhysics, joint.Pose, linkObjectChild, rigidBodyParent); + jointComponent = prismaticJoint as Joint; } else if (joint.Type.Equals("revolute")) { @@ -112,7 +112,13 @@ protected override void ImportJoint(in SDF.Joint joint, in System.Object parentO if (jointComponent != null) { - SDFImplement.Joint.SetCommonConfiguration(jointComponent, linkObjectChild); + SDFImplement.Joint.SetCommonConfiguration(jointComponent, joint.Pose.Pos, linkObjectChild); + + var linkPlugin = linkObjectChild.GetComponent(); + if (linkPlugin != null) + { + linkPlugin.jointList.Add(joint.Name, jointComponent); + } } } } diff --git a/Assets/Scripts/Tools/SDFImporter/SDFImporter.Sensor.cs b/Assets/Scripts/Tools/SDFImporter/SDFImporter.Sensor.cs index 3f523771..c7e6dc05 100644 --- a/Assets/Scripts/Tools/SDFImporter/SDFImporter.Sensor.cs +++ b/Assets/Scripts/Tools/SDFImporter/SDFImporter.Sensor.cs @@ -56,8 +56,12 @@ protected override System.Object ImportSensor(in SDF.Sensor item, in System.Obje var gps = item.GetSensor() as SDF.GPS; sensor = SDFImplement.Sensor.AddGps(gps, targetObject); } - else if (sensorType.Equals("air_pressure") || - sensorType.Equals("altimeter") || sensorType.Equals("contact") || + else if (sensorType.Equals("contact")) + { + var contact = item.GetSensor() as SDF.Contact; + sensor = SDFImplement.Sensor.AddContact(contact, targetObject); + } + else if (sensorType.Equals("air_pressure") || sensorType.Equals("altimeter") || sensorType.Equals("force_torque") || sensorType.Equals("logical_camera") || sensorType.Equals("magnetometer") || sensorType.Equals("rfidtag") || @@ -82,7 +86,8 @@ protected override System.Object ImportSensor(in SDF.Sensor item, in System.Obje { newSensorObject.tag = "Sensor"; newSensorObject.name = item.Name; - SDFImplement.Sensor.TransformSensor(newSensorObject, item.Pose); + newSensorObject.transform.localPosition += SDF2Unity.GetPosition(item.Pose.Pos); + newSensorObject.transform.localRotation *= SDF2Unity.GetRotation(item.Pose.Rot); #if UNITY_EDITOR SceneVisibilityManager.instance.ToggleVisibility(newSensorObject, true); SceneVisibilityManager.instance.DisablePicking(newSensorObject, true); diff --git a/Assets/Scripts/Tools/SDFPlugins/LinkPlugin.cs b/Assets/Scripts/Tools/SDFPlugins/LinkPlugin.cs index 898b5883..e7f46e9d 100644 --- a/Assets/Scripts/Tools/SDFPlugins/LinkPlugin.cs +++ b/Assets/Scripts/Tools/SDFPlugins/LinkPlugin.cs @@ -4,6 +4,7 @@ * SPDX-License-Identifier: MIT */ +using System.Collections.Generic; using UnityEngine; public class LinkPlugin : MonoBehaviour @@ -14,13 +15,13 @@ public class LinkPlugin : MonoBehaviour private PoseControl poseControl = null; - LinkPlugin() - { - poseControl = new PoseControl(); - } + public Dictionary jointList; void Awake() { + poseControl = new PoseControl(); + jointList = new Dictionary(); + tag = "Link"; var modelObject = transform.parent; modelPlugin = modelObject.GetComponent(); @@ -148,26 +149,28 @@ private void MakeStaticLink() private void ScaleMassOnJoint() { - var thisJoint = this.GetComponent(); - - if (thisJoint && thisJoint.GetType() != typeof(FixedJoint)) + var thisJoints = this.GetComponents(); + foreach (var thisJoint in thisJoints) { - var connectedBody = thisJoint.connectedBody; - var connectedBodyObject = thisJoint.connectedBody.gameObject; - - if (connectedBody && connectedBodyObject && connectedBodyObject.tag.Equals("Link")) + if (thisJoint && thisJoint.GetType() != typeof(FixedJoint)) { - var thisBody = GetComponent(); + var connectedBody = thisJoint.connectedBody; + var connectedBodyObject = thisJoint.connectedBody.gameObject; - if (connectedBody.mass > thisBody.mass) - { - thisJoint.massScale = 1; - thisJoint.connectedMassScale = connectedBody.mass/thisBody.mass; - } - else + if (connectedBody && connectedBodyObject && connectedBodyObject.tag.Equals("Link")) { - thisJoint.massScale = thisBody.mass/connectedBody.mass; - thisJoint.connectedMassScale = 1; + var thisBody = GetComponent(); + + if (connectedBody.mass > thisBody.mass) + { + thisJoint.massScale = 1; + thisJoint.connectedMassScale = connectedBody.mass / thisBody.mass; + } + else + { + thisJoint.massScale = thisBody.mass / connectedBody.mass; + thisJoint.connectedMassScale = 1; + } } } } diff --git a/Assets/Scripts/Tools/SDFPlugins/PluginParameters.cs b/Assets/Scripts/Tools/SDFPlugins/PluginParameters.cs index d4fcfd5c..3c47fdda 100644 --- a/Assets/Scripts/Tools/SDFPlugins/PluginParameters.cs +++ b/Assets/Scripts/Tools/SDFPlugins/PluginParameters.cs @@ -19,16 +19,6 @@ public void SetRootData(in XmlNode node) parameters = node.SelectSingleNode("."); } - private static T XmlNodeToValue(in XmlNode node) - { - if (node == null) - { - return default(T); - } - var value = node.InnerXml.Trim(); - return SDF.Entity.ConvertValueType(value); - } - public T GetAttribute(in string xpath, in string attributeName, in T defaultValue = default(T)) { var node = parameters.SelectSingleNode(xpath); @@ -56,7 +46,7 @@ private static T XmlNodeToValue(in XmlNode node) try { var node = parameters.SelectSingleNode(xpath); - return XmlNodeToValue(node); + return SDF.Entity.ConvertXmlNodeToValue(node); } catch (XmlException ex) { @@ -70,7 +60,7 @@ public bool GetValues(in string xpath, out List valueList) valueList = null; var result = GetValues(xpath, out var nodeList); - valueList = nodeList.ConvertAll(s => XmlNodeToValue(s)); + valueList = nodeList.ConvertAll(s => SDF.Entity.ConvertXmlNodeToValue(s)); return result; } @@ -110,8 +100,8 @@ public void PrintData() else { // Print all SDF contents - StringWriter sw = new StringWriter(); - XmlTextWriter xw = new XmlTextWriter(sw); + var sw = new StringWriter(); + var xw = new XmlTextWriter(sw); parameters.WriteTo(xw); Debug.Log(sw.ToString()); } diff --git a/Assets/Scripts/UI/CameraControl.cs b/Assets/Scripts/UI/CameraControl.cs index 8cc52817..b50234d2 100644 --- a/Assets/Scripts/UI/CameraControl.cs +++ b/Assets/Scripts/UI/CameraControl.cs @@ -3,7 +3,7 @@ * * SPDX-License-Identifier: MIT */ - + using UnityEngine; public class CameraControl : MonoBehaviour @@ -24,8 +24,15 @@ Converted to C# 27-02-13 - no credit wanted. public float shiftAdd = 20.0f; //multiplied by how long shift is held. Basically running public float maxShift = 50.0f; //Maximum speed when holding shift public float camSens = 0.1f; //How sensitive it with mouse + public float edgeWidth = 100.0f; + public float edgeSens = 0.02f; + public float edgeSensMax = 1.0f; + private Vector3 lastMouse = new Vector3(255, 255, 255); //kind of in the middle of the screen, rather than at the top (play) private float totalRun = 1.0f; + private float edgeSensAccumlated = 0.0f; + + private Vector3 lastMousePrevious = Vector3.zero; void LateUpdate() { @@ -36,13 +43,52 @@ void LateUpdate() lastMouse = Input.mousePosition - lastMouse; lastMouse.Set(-lastMouse.y * camSens, lastMouse.x * camSens, 0); - lastMouse.Set(transform.eulerAngles.x + lastMouse.x, transform.eulerAngles.y + lastMouse.y, 0); + lastMouse.Set(transform.eulerAngles.x + lastMouse.x, transform.eulerAngles.y + lastMouse.y , 0); + + //Mouse camera angle done. + if (Input.GetMouseButton(2) || Input.GetMouseButton(1)) + { + // perspective move during the right or wheel click + // Debug.Log(lastMouse.ToString("F4")); + + if (edgeSensAccumlated < edgeSensMax) + { + edgeSensAccumlated += edgeSens; + } - //Mouse camera angle done. - if (Input.GetMouseButton(2)) + if (Input.mousePosition.x < edgeWidth) + { + // Debug.Log("rotate camera left here"); + lastMouse.y -= edgeSensAccumlated; + transform.eulerAngles = lastMouse; + } + else if (Input.mousePosition.x > Screen.width - edgeWidth) + { + // Debug.Log("rotate camera right here"); + lastMouse.y += edgeSensAccumlated; + transform.eulerAngles = lastMouse; + } + else if (Input.mousePosition.y < edgeWidth) + { + // Debug.Log("rotate camera down here"); + lastMouse.x += edgeSensAccumlated; + transform.eulerAngles = lastMouse; + } + else if (Input.mousePosition.y > Screen.height - edgeWidth) + { + // Debug.Log("rotate camera up here"); + lastMouse.x -= edgeSensAccumlated; + transform.eulerAngles = lastMouse; + } + else + { + edgeSensAccumlated = 0.0f; + transform.eulerAngles = lastMouse; + } + } + else { - // perspective move during the left click - transform.eulerAngles = lastMouse; + edgeSensAccumlated = 0.0f; } lastMouse = Input.mousePosition; diff --git a/Assets/Scripts/UI/RuntimeGizmo/TransformGizmo.cs b/Assets/Scripts/UI/RuntimeGizmo/TransformGizmo.cs index 935beec2..120c0fbe 100644 --- a/Assets/Scripts/UI/RuntimeGizmo/TransformGizmo.cs +++ b/Assets/Scripts/UI/RuntimeGizmo/TransformGizmo.cs @@ -63,15 +63,15 @@ public partial class TransformGizmo : MonoBehaviour //These are the same as the unity editor hotkeys [Header("Key configurations")] - public KeyCode SetMoveType = KeyCode.W; - public KeyCode SetRotateType = KeyCode.E; - public KeyCode SetScaleType = KeyCode.R; - //public KeyCode SetRectToolType = KeyCode.T; + public KeyCode SetMoveType = KeyCode.T; + public KeyCode SetRotateType = KeyCode.R; + // public KeyCode SetScaleType = KeyCode.L; + // public KeyCode SetRectToolType = KeyCode.G; public KeyCode SetAllTransformType = KeyCode.Y; public KeyCode SetSpaceToggle = KeyCode.X; - public KeyCode SetPivotModeToggle = KeyCode.Z; - public KeyCode SetCenterTypeToggle = KeyCode.C; - public KeyCode SetScaleTypeToggle = KeyCode.S; + // public KeyCode SetPivotModeToggle = KeyCode.Z; + // public KeyCode SetCenterTypeToggle = KeyCode.C; + // public KeyCode SetScaleTypeToggle = KeyCode.S; public KeyCode translationSnapping = KeyCode.LeftControl; public KeyCode AddSelection = KeyCode.LeftShift; public KeyCode RemoveSelection = KeyCode.LeftControl; @@ -268,29 +268,29 @@ void SetSpaceAndType() transformType = TransformType.Move; else if (Input.GetKeyDown(SetRotateType)) transformType = TransformType.Rotate; - else if (Input.GetKeyDown(SetScaleType)) - transformType = TransformType.Scale; + // else if (Input.GetKeyDown(SetScaleType)) + // transformType = TransformType.Scale; //else if (Input.GetKeyDown(SetRectToolType)) type = TransformType.RectTool; else if (Input.GetKeyDown(SetAllTransformType)) transformType = TransformType.All; if (!isTransforming) translatingType = transformType; - if (Input.GetKeyDown(SetPivotModeToggle)) - { - if (pivot == TransformPivot.Pivot) pivot = TransformPivot.Center; - else if (pivot == TransformPivot.Center) pivot = TransformPivot.Pivot; + // if (Input.GetKeyDown(SetPivotModeToggle)) + // { + // if (pivot == TransformPivot.Pivot) pivot = TransformPivot.Center; + // else if (pivot == TransformPivot.Center) pivot = TransformPivot.Pivot; - SetPivotPoint(); - } + // SetPivotPoint(); + // } - if (Input.GetKeyDown(SetCenterTypeToggle)) - { - if (centerType == CenterType.All) centerType = CenterType.Solo; - else if (centerType == CenterType.Solo) centerType = CenterType.All; + // if (Input.GetKeyDown(SetCenterTypeToggle)) + // { + // if (centerType == CenterType.All) centerType = CenterType.Solo; + // else if (centerType == CenterType.Solo) centerType = CenterType.All; - SetPivotPoint(); - } + // SetPivotPoint(); + // } if (Input.GetKeyDown(SetSpaceToggle)) { @@ -300,11 +300,11 @@ void SetSpaceAndType() space = TransformSpace.Global; } - if (Input.GetKeyDown(SetScaleTypeToggle)) - { - if (scaleType == ScaleType.FromPoint) scaleType = ScaleType.FromPointOffset; - else if (scaleType == ScaleType.FromPointOffset) scaleType = ScaleType.FromPoint; - } + // if (Input.GetKeyDown(SetScaleTypeToggle)) + // { + // if (scaleType == ScaleType.FromPoint) scaleType = ScaleType.FromPointOffset; + // else if (scaleType == ScaleType.FromPointOffset) scaleType = ScaleType.FromPoint; + // } if (transformType == TransformType.Scale) { diff --git a/LICENSES/third_party_license.txt b/LICENSE-3RD-PARTY similarity index 100% rename from LICENSES/third_party_license.txt rename to LICENSE-3RD-PARTY diff --git a/LICENSES/MIT.txt b/LICENSES/MIT.txt deleted file mode 100644 index f9e457d2..00000000 --- a/LICENSES/MIT.txt +++ /dev/null @@ -1,19 +0,0 @@ -MIT License Copyright (c) 2020 LG Electronics Inc. - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is furnished -to do so, subject to the following conditions: - -The above copyright notice and this permission notice (including the next -paragraph) shall be included in all copies or substantial portions of the -Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS -FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS -OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, -WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF -OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. diff --git a/Packages/manifest.json b/Packages/manifest.json index 4d3a166c..7a61594d 100644 --- a/Packages/manifest.json +++ b/Packages/manifest.json @@ -16,8 +16,6 @@ "com.unity.modules.physics": "1.0.0", "com.unity.modules.screencapture": "1.0.0", "com.unity.modules.terrain": "1.0.0", - "com.unity.modules.terrainphysics": "1.0.0", - "com.unity.modules.tilemap": "1.0.0", "com.unity.modules.ui": "1.0.0", "com.unity.modules.uielements": "1.0.0", "com.unity.modules.umbra": "1.0.0", diff --git a/Packages/packages-lock.json b/Packages/packages-lock.json index 5fe46ffc..57325115 100644 --- a/Packages/packages-lock.json +++ b/Packages/packages-lock.json @@ -95,12 +95,6 @@ "source": "builtin", "dependencies": {} }, - "com.unity.modules.physics2d": { - "version": "1.0.0", - "depth": 1, - "source": "builtin", - "dependencies": {} - }, "com.unity.modules.screencapture": { "version": "1.0.0", "depth": 0, @@ -115,23 +109,6 @@ "source": "builtin", "dependencies": {} }, - "com.unity.modules.terrainphysics": { - "version": "1.0.0", - "depth": 0, - "source": "builtin", - "dependencies": { - "com.unity.modules.physics": "1.0.0", - "com.unity.modules.terrain": "1.0.0" - } - }, - "com.unity.modules.tilemap": { - "version": "1.0.0", - "depth": 0, - "source": "builtin", - "dependencies": { - "com.unity.modules.physics2d": "1.0.0" - } - }, "com.unity.modules.ui": { "version": "1.0.0", "depth": 0, diff --git a/ProjectSettings/ProjectSettings.asset b/ProjectSettings/ProjectSettings.asset index 6909d0eb..6d39ea86 100644 --- a/ProjectSettings/ProjectSettings.asset +++ b/ProjectSettings/ProjectSettings.asset @@ -42,8 +42,8 @@ PlayerSettings: m_SplashScreenLogos: [] m_VirtualRealitySplashScreen: {fileID: 0} m_HolographicTrackingLossScreen: {fileID: 0} - defaultScreenWidth: 1024 - defaultScreenHeight: 768 + defaultScreenWidth: 1280 + defaultScreenHeight: 1024 defaultScreenWidthWeb: 960 defaultScreenHeightWeb: 600 m_StereoRenderingPath: 0 @@ -111,17 +111,20 @@ PlayerSettings: switchNVNShaderPoolsGranularity: 33554432 switchNVNDefaultPoolsGranularity: 16777216 switchNVNOtherPoolsGranularity: 16777216 + switchNVNMaxPublicTextureIDCount: 0 + switchNVNMaxPublicSamplerIDCount: 0 stadiaPresentMode: 0 stadiaTargetFramerate: 0 vulkanNumSwapchainBuffers: 3 vulkanEnableSetSRGBWrite: 0 + vulkanEnableLateAcquireNextImage: 0 m_SupportedAspectRatios: 4:3: 1 5:4: 1 16:10: 1 16:9: 1 Others: 1 - bundleVersion: 1.4.2 + bundleVersion: 1.5.0 preloadedAssets: [] metroInputSource: 0 wsaTransparentSwapchain: 0 diff --git a/ProjectSettings/ProjectVersion.txt b/ProjectSettings/ProjectVersion.txt index 291a7485..d518aa37 100644 --- a/ProjectSettings/ProjectVersion.txt +++ b/ProjectSettings/ProjectVersion.txt @@ -1,2 +1,2 @@ -m_EditorVersion: 2019.4.5f1 -m_EditorVersionWithRevision: 2019.4.5f1 (81610f64359c) +m_EditorVersion: 2019.4.6f1 +m_EditorVersionWithRevision: 2019.4.6f1 (a7aea80e3716) diff --git a/UIElementsSchema/UIElements.xsd b/UIElementsSchema/UIElements.xsd deleted file mode 100644 index c0520737..00000000 --- a/UIElementsSchema/UIElements.xsd +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - \ No newline at end of file diff --git a/UIElementsSchema/UnityEditor.PackageManager.UI.xsd b/UIElementsSchema/UnityEditor.PackageManager.UI.xsd deleted file mode 100644 index 433aa4c1..00000000 --- a/UIElementsSchema/UnityEditor.PackageManager.UI.xsd +++ /dev/null @@ -1,294 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/UIElementsSchema/UnityEditor.UIElements.Debugger.xsd b/UIElementsSchema/UnityEditor.UIElements.Debugger.xsd deleted file mode 100644 index 39551353..00000000 --- a/UIElementsSchema/UnityEditor.UIElements.Debugger.xsd +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/UIElementsSchema/UnityEditor.UIElements.xsd b/UIElementsSchema/UnityEditor.UIElements.xsd deleted file mode 100644 index 3a808f49..00000000 --- a/UIElementsSchema/UnityEditor.UIElements.xsd +++ /dev/null @@ -1,887 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/UIElementsSchema/UnityEngine.UIElements.xsd b/UIElementsSchema/UnityEngine.UIElements.xsd deleted file mode 100644 index af7823d1..00000000 --- a/UIElementsSchema/UnityEngine.UIElements.xsd +++ /dev/null @@ -1,525 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/oss-pkg-info.yaml b/oss-pkg-info.yaml index 9a2f7149..a4f95b75 100644 --- a/oss-pkg-info.yaml +++ b/oss-pkg-info.yaml @@ -1,6 +1,6 @@ Open Source Package: - name: VSCode - version: 2.7 + version: 2.9 source: https://github.com/dotBunny/VSCode license: MIT @@ -10,7 +10,7 @@ Open Source Package: license: MPL-2.0 - name: NetMQ - version: 4.0.0 + version: 4.0.0.207 source: https://github.com/zeromq/netmq license: LGPL-3.0