diff --git a/Assets/Scripts/CLOiSimPlugins/Messages/micom.cs b/Assets/Scripts/CLOiSimPlugins/Messages/micom.cs index cf2e1dbc..962e981d 100644 --- a/Assets/Scripts/CLOiSimPlugins/Messages/micom.cs +++ b/Assets/Scripts/CLOiSimPlugins/Messages/micom.cs @@ -19,7 +19,7 @@ public partial class Micom : global::ProtoBuf.IExtensible [global::ProtoBuf.ProtoMember(1, Name = @"time", IsRequired = true)] public Time Time { get; set; } - [global::ProtoBuf.ProtoMember(2, Name = @"odom", IsRequired = true)] + [global::ProtoBuf.ProtoMember(2, Name = @"odom")] public Odometry Odom { get; set; } [global::ProtoBuf.ProtoMember(3)] diff --git a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs index bcad1dbb..b7594a45 100644 --- a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs +++ b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs @@ -26,12 +26,13 @@ protected void PublishTfThread(System.Object threadObject) tfMessage.Transform.Orientation = new messages.Quaternion(); var deviceMessage = new DeviceMessage(); - if (publisher != null && tfList.Count > 0) + if (publisher != null) { + const int EmptyTfPublishPeriod = 2000; const float publishFrequency = 50; const int updatePeriod = (int)(1f / publishFrequency * 1000f); - int updatePeriodPerEachTf = (int)(updatePeriod / tfList.Count); - // Debug.Log(updatePeriod + " , " + updatePeriodPerEachTf); + var updatePeriodPerEachTf = (tfList.Count == 0) ? int.MaxValue : (int)(updatePeriod / tfList.Count); + // Debug.Log("PublishTfThread: " + updatePeriod + " , " + updatePeriodPerEachTf); while (PluginThread.IsRunning) { @@ -58,6 +59,16 @@ protected void PublishTfThread(System.Object threadObject) } CLOiSimPluginThread.Sleep(updatePeriodPerEachTf); } + + if (tfList.Count == 0) + { + deviceMessage.SetMessage(tfMessage); + if (publisher.Publish(deviceMessage) == false) + { + Debug.Log(tfMessage.Header.StrId + ", " + tfMessage.Transform.Name + " error to send TF!!"); + } + CLOiSimPluginThread.Sleep(EmptyTfPublishPeriod); + } } } } diff --git a/Assets/Scripts/Devices/MicomSensor.cs b/Assets/Scripts/Devices/MicomSensor.cs index c8800d32..b70ae048 100644 --- a/Assets/Scripts/Devices/MicomSensor.cs +++ b/Assets/Scripts/Devices/MicomSensor.cs @@ -17,8 +17,8 @@ public partial class MicomSensor : Device private MotorControl _motorControl = null; private SensorDevices.Battery battery = null; private SensorDevices.IMU _imuSensor = null; - private List ussSensors = new List(); - private List irSensors = new List(); + private List _ussSensors = new List(); + private List _irSensors = new List(); // private List magnetSensors = null; private List _bumperSensors = new List(); @@ -48,10 +48,11 @@ public void SetIMU(in string sensorName) var imuList = gameObject.GetComponentsInChildren(); foreach (var imu in imuList) { - if (imu.DeviceName.Contains("::" + sensorName + "::") || + if (imu.DeviceName.Contains($"::{sensorName}::") || // Model name + imu.DeviceName.EndsWith($"::{sensorName}") || // Link name imu.name.Equals(sensorName)) { - Debug.Log(imu.DeviceName + " attached to Micom"); + Debug.Log($"IMU: {imu.DeviceName} attached to Micom"); _imuSensor = imu; break; } @@ -60,38 +61,46 @@ public void SetIMU(in string sensorName) public void SetUSS(in List ussList) { - var modelList = GetComponentsInChildren(); - foreach (var uss in ussList) + var sonarList = GetComponentsInChildren(); + + foreach (var ussName in ussList) { - foreach (var model in modelList) + foreach (var sonar in sonarList) { - if (model.name.Equals(uss)) + if (sonar.DeviceName.Contains($"::{ussName}::") || // Model name + sonar.DeviceName.EndsWith($"::{ussName}") || // Link name + sonar.name.Equals(ussName)) { - var sonarSensor = model.GetComponentInChildren(); - ussSensors.Add(sonarSensor); - // Debug.Log("ussSensor found : " + sonarSensor.name); + Debug.Log($"USS: {sonar.DeviceName} attached to Micom"); + _ussSensors.Add(sonar); + break; } } } - micomSensorData.uss.Distances = new double[ussList.Count]; + + micomSensorData.uss.Distances = new double[_ussSensors.Count]; } public void SetIRSensor(in List irList) { - var modelList = GetComponentsInChildren(); - foreach (var ir in irList) + var sonarList = GetComponentsInChildren(); + + foreach (var irName in irList) { - foreach (var model in modelList) + foreach (var sonar in sonarList) { - if (model.name.Equals(ir)) + if (sonar.DeviceName.Contains($"::{irName}::") || // Model name + sonar.DeviceName.EndsWith($"::{irName}") || // Link name + sonar.name.Equals(irName)) { - var sonarSensor = model.GetComponentInChildren(); - irSensors.Add(sonarSensor); - // Debug.Log("irSensor found : " + sonarSensor.name); + Debug.Log($"IR: {sonar.DeviceName} attached to Micom"); + _irSensors.Add(sonar); + break; } } } - micomSensorData.ir.Distances = new double[irList.Count]; + + micomSensorData.ir.Distances = new double[_irSensors.Count]; } public void SetMagnet(in List magnetList) @@ -106,24 +115,24 @@ public void SetMagnet(in List magnetList) public void SetBumper(in List bumperList) { - var contactsInChild = GetComponentsInChildren(); + var contactList = GetComponentsInChildren(); - var bumperCount = 0; - foreach (var bumper in bumperList) + foreach (var bumperName in bumperList) { - foreach (var contact in contactsInChild) + foreach (var contact in contactList) { - if (contact.name.Equals(bumper)) + if (contact.DeviceName.Contains($"::{bumperName}::") || // Model name + contact.DeviceName.EndsWith($"::{bumperName}") || // Link name + contact.name.Equals(bumperName)) { + Debug.Log($"Bumper: {contact.DeviceName} attached to Micom"); _bumperSensors.Add(contact); - bumperCount++; - Debug.Log("Found " + contact.name); break; } } } - micomSensorData.bumper.Bumpeds = new bool[bumperCount]; + micomSensorData.bumper.Bumpeds = new bool[_bumperSensors.Count]; } public void SetBattery(in SensorDevices.Battery targetBattery) @@ -146,15 +155,20 @@ protected override void InitializeMessages() { micomSensorData = new messages.Micom(); micomSensorData.Time = new messages.Time(); + micomSensorData.Odom = null; + micomSensorData.uss = new messages.Micom.Uss(); + micomSensorData.ir = new messages.Micom.Ir(); + micomSensorData.bumper = new messages.Micom.Bumper(); + } + + private void InitializeOdometryMessage() + { micomSensorData.Odom = new messages.Micom.Odometry(); micomSensorData.Odom.AngularVelocity = new messages.Micom.Odometry.Wheel(); micomSensorData.Odom.LinearVelocity = new messages.Micom.Odometry.Wheel(); micomSensorData.Odom.Pose = new messages.Vector3d(); micomSensorData.Odom.TwistLinear = new messages.Vector3d(); micomSensorData.Odom.TwistAngular = new messages.Vector3d(); - micomSensorData.uss = new messages.Micom.Uss(); - micomSensorData.ir = new messages.Micom.Ir(); - micomSensorData.bumper = new messages.Micom.Bumper(); } protected override void GenerateMessage() @@ -179,6 +193,11 @@ void FixedUpdate() if (_motorControl != null) { + if (micomSensorData.Odom == null) + { + InitializeOdometryMessage(); + } + if (_motorControl.Update(micomSensorData.Odom, deltaTime, _imuSensor) == false) { Debug.LogWarning("Update failed in MotorControl"); @@ -215,9 +234,9 @@ private void UpdateUss() return; } - for (var index = 0; index < ussSensors.Count; index++) + for (var index = 0; index < _ussSensors.Count; index++) { - micomSensorData.uss.Distances[index] = ussSensors[index].GetDetectedRange(); + micomSensorData.uss.Distances[index] = _ussSensors[index].GetDetectedRange(); } } @@ -228,9 +247,9 @@ private void UpdateIr() return; } - for (var index = 0; index < irSensors.Count; index++) + for (var index = 0; index < _irSensors.Count; index++) { - micomSensorData.ir.Distances[index] = irSensors[index].GetDetectedRange(); + micomSensorData.ir.Distances[index] = _irSensors[index].GetDetectedRange(); } } diff --git a/Assets/Scripts/Devices/Modules/Base/Device.cs b/Assets/Scripts/Devices/Modules/Base/Device.cs index 6cb07f53..3b89450b 100644 --- a/Assets/Scripts/Devices/Modules/Base/Device.cs +++ b/Assets/Scripts/Devices/Modules/Base/Device.cs @@ -20,11 +20,15 @@ public enum ModeType { NONE, TX, RX, TX_THREAD, RX_THREAD }; private SDF.Plugin pluginParameters = null; + [SerializeField] private string deviceName = string.Empty; + [SerializeField] private float _updateRate = 1; private bool debuggingOn = true; + + [SerializeField] private bool visualize = true; private float transportingTimeSeconds = 0; diff --git a/Assets/Scripts/Devices/Sonar.cs b/Assets/Scripts/Devices/Sonar.cs index c3f6cae9..ac03f26e 100644 --- a/Assets/Scripts/Devices/Sonar.cs +++ b/Assets/Scripts/Devices/Sonar.cs @@ -13,6 +13,8 @@ namespace SensorDevices { public class Sonar : Device { + private static readonly float Margin = 0.001f; + private messages.SonarStamped sonarStamped = null; public string geometry = string.Empty; @@ -26,33 +28,29 @@ public class Sonar : Device [Range(0, 100)] public double radius = 0; - public Vector3 sensorStartPoint = Vector3.zero; - - private List meshSensorRegionVertices = new List(); + public Vector3 _sensorStartPoint = Vector3.zero; - private int pingpongindex = 0; + private List _meshSensorRegionVertices = new List(); - private Transform sonarLink = null; + private Transform _sonarLink = null; - private float sensorStartOffset = 0; + private float _sensorStartOffset = 0; protected override void OnAwake() { Mode = ModeType.TX; DeviceName = name; - sonarLink = transform.parent; + _sonarLink = transform.parent; } protected override void OnStart() { - var visualMesh = sonarLink.GetComponentInChildren(); - sensorStartOffset = (visualMesh == null) ? 0f : visualMesh.sharedMesh.bounds.max.y; + var visualMesh = _sonarLink.GetComponentInChildren(); + _sensorStartOffset = (visualMesh == null) ? 0f : visualMesh.sharedMesh.bounds.max.y; // Create a new sensing area - var meshCollider = gameObject.AddComponent(); - - float sensorMeshOffset = 0; Mesh mesh = null; + var sensorMeshOffset = 0f; if (geometry.Equals("sphere")) { mesh = ProceduralMesh.CreateSphere((float)radius); @@ -64,8 +62,10 @@ protected override void OnStart() sensorMeshOffset = (float)rangeMax / 2; } - TranslateDetectionArea(mesh, 0.0001f + sensorStartOffset + sensorMeshOffset + (float)rangeMin); + var translationOffset = Margin + _sensorStartOffset + sensorMeshOffset; // + (float)rangeMin; + TranslateDetectionArea(mesh, translationOffset); + var meshCollider = gameObject.AddComponent(); meshCollider.sharedMesh = mesh; meshCollider.convex = true; meshCollider.isTrigger = true; @@ -89,9 +89,10 @@ protected override IEnumerator OnVisualize() if (!detectedPoint.Equals(Vector3.zero)) { - var direction = (GetDetectedPoint() - sensorStartPoint).normalized; + var direction = (GetDetectedPoint() - _sensorStartPoint).normalized; var detectedRange = GetDetectedRange(); - Debug.DrawRay(sensorStartPoint, direction * detectedRange, Color.blue, UpdatePeriod); + Debug.DrawRay(_sensorStartPoint, direction * detectedRange, Color.blue, UpdatePeriod); + // Debug.Log($"{name} direction={direction} detectedRange{detectedRange}"); } yield return waitForSeconds; @@ -111,8 +112,8 @@ protected override void InitializeMessages() protected override void GenerateMessage() { - var sonarPosition = sonarLink.position; - var sonarRotation = sonarLink.rotation; + var sonarPosition = _sonarLink.position; + var sonarRotation = _sonarLink.rotation; sonarStamped.Time.SetCurrentTime(); @@ -129,13 +130,12 @@ private void ResolveSensingArea(Mesh targetMesh) for (var i = 0; i < targetMesh.vertices.Length; i++) { var targetPoint = targetMesh.vertices[i]; - var distance = (Vector3.zero - targetPoint).magnitude; + var distance = targetPoint.magnitude; if (distance < (float)rangeMin) { continue; } - - meshSensorRegionVertices.Add(targetMesh.vertices[i]); + _meshSensorRegionVertices.Add(targetPoint); } } @@ -150,54 +150,58 @@ private void TranslateDetectionArea(Mesh mesh, in float offset) mesh.vertices = vertices; } - private float sensorTimeElapsed = 0.0f; + private int _pingPongIndex = 0; + private float _sensorTimeElapsed = 0.0f; void OnTriggerStay(Collider other) { - if (meshSensorRegionVertices.Count == 0) + if (_meshSensorRegionVertices.Count == 0) { return; } - if ((sensorTimeElapsed += Time.fixedDeltaTime) < UpdatePeriod * 2) + if ((_sensorTimeElapsed += Time.fixedDeltaTime) < UpdatePeriod * 2) { return; } else { - sensorTimeElapsed = 0.0f; + _sensorTimeElapsed = 0.0f; } - var detectedRange = (float)rangeMax; + var detectedRange = float.NegativeInfinity; var contactPoint = Vector3.zero; var contactDirection = Vector3.zero; - var localToWorld = sonarLink.localToWorldMatrix; + var localToWorld = this.transform.localToWorldMatrix; - sensorStartPoint.Set(0, -(sensorStartOffset + (float)rangeMin), 0); - sensorStartPoint = localToWorld.MultiplyPoint3x4(sensorStartPoint); + _sensorStartPoint.Set(0, -(Margin + _sensorStartOffset), 0); + _sensorStartPoint = this.transform.localRotation * _sensorStartPoint; + _sensorStartPoint += localToWorld.GetPosition(); - // Debug.Log("Hit Points: " + meshSensorRegionVertices.Count); - for (var i = pingpongindex; i < meshSensorRegionVertices.Count; i += 2) + // Debug.Log("Hit Points: " + _meshSensorRegionVertices.Count); + for (var i = _pingPongIndex; i < _meshSensorRegionVertices.Count; i += 2) { - var targetPoint = localToWorld.MultiplyPoint3x4(meshSensorRegionVertices[i]); - var direction = (targetPoint - sensorStartPoint); + var targetPoint = localToWorld.MultiplyPoint3x4(_meshSensorRegionVertices[i]); + var direction = targetPoint - _sensorStartPoint; - if (Physics.Raycast(sensorStartPoint, direction, out var hitInfo, (float)rangeMax)) + // Debug.DrawLine(_sensorStartPoint, targetPoint, Color.red, 0.5f); + if (Physics.Raycast(_sensorStartPoint, direction, out var hitInfo, (float)rangeMax)) { - // Debug.DrawRay(sensorStartPoint, direction, Color.magenta, 0.01f); - // Debug.Log("Hit Point of contact: " + hitInfo.point + " | " + sensorStartPoint.ToString("F4")); + // 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); + var hitDistance = Vector3.Distance(_sensorStartPoint, hitPoint); var hitCollider = hitInfo.collider; - // Debug.Log(hitCollider.name + " <=> " + name + "," + DeviceName); - // Debug.Log(hitCollider.transform.parent.name + " <=> " + sonarLink.name + "," + DeviceName); - + // Debug.Log($"Hit Point {hitCollider.name}<->{name} | {hitCollider.transform.parent.name}<->{_sonarLink.name}"); // ignore itself - if (hitCollider.name.Equals(name) && hitCollider.transform.parent.name.Equals(sonarLink.name)) + if (hitCollider.name.Equals(name) && hitCollider.transform.parent.name.Equals(_sonarLink.name)) + { continue; + } - if ((hitDistance < detectedRange) && (hitDistance > (float)rangeMin)) + if ((hitDistance <= (float)rangeMax) && (hitDistance > (float)rangeMin)) { // Debug.Log("Hit Point " + i + " of contacts: " + hitCollider.name + "," + hitInfo.point + "|" + hitDistance.ToString("F4")); detectedRange = hitDistance; @@ -210,10 +214,10 @@ void OnTriggerStay(Collider other) var sonar = sonarStamped.Sonar; sonar.Range = detectedRange; sonar.Contact.Set(contactPoint); - // Debug.Log(DeviceName + ": " + other.name + " |Stay| " + detectedRange.ToString("F5") + " | " + contactPoint); + // Debug.Log($"{DeviceName}: |Stay| {detectedRange.ToString("F5")} | {contactPoint}"); - pingpongindex++; - pingpongindex %= 2; + _pingPongIndex++; + _pingPongIndex %= 2; } void OnTriggerExit(Collider other) diff --git a/ProjectSettings/ProjectSettings.asset b/ProjectSettings/ProjectSettings.asset index eaec2a83..2923d86b 100644 --- a/ProjectSettings/ProjectSettings.asset +++ b/ProjectSettings/ProjectSettings.asset @@ -140,7 +140,7 @@ PlayerSettings: loadStoreDebugModeEnabled: 0 visionOSBundleVersion: 1.0 tvOSBundleVersion: 1.0 - bundleVersion: 4.9.2 + bundleVersion: 4.9.3 preloadedAssets: [] metroInputSource: 0 wsaTransparentSwapchain: 0