diff --git a/server/worldstorage/src/ETSI.ARF.OpenAPI.WorldStorage/ETSI-ARF/ControllersImpl/RelocalizationInformationImpl.cs b/server/worldstorage/src/ETSI.ARF.OpenAPI.WorldStorage/ETSI-ARF/ControllersImpl/RelocalizationInformationImpl.cs index 4a9c9dc27019e845dcebeb7cd8a3004735b646bc..9a377b30adec12cab328cbdf7b14cdf9e249f9da 100644 --- a/server/worldstorage/src/ETSI.ARF.OpenAPI.WorldStorage/ETSI-ARF/ControllersImpl/RelocalizationInformationImpl.cs +++ b/server/worldstorage/src/ETSI.ARF.OpenAPI.WorldStorage/ETSI-ARF/ControllersImpl/RelocalizationInformationImpl.cs @@ -55,6 +55,7 @@ namespace ETSI.ARF.OpenAPI.WorldStorage.Controllers public override IActionResult GetRelocalizationInformation([FromQuery(Name = "uuids")][Required()] List<GetRelocalizationInformationUuidsParameterInner> uuids, [FromQuery(Name = "capabilities")][Required()] List<Capability> capabilities, [FromHeader(Name = "token")] string token) { WorldAnchorService _worldAnchorService = WorldAnchorService.Singleton as WorldAnchorService; + TrackableService _trackableService = TrackableService.Singleton as TrackableService; var history = new List<Guid>(); var result = new GetRelocalizationInformation200Response(); @@ -63,6 +64,13 @@ namespace ETSI.ARF.OpenAPI.WorldStorage.Controllers foreach (var request in uuids) { var anchor = _worldAnchorService.Get(request.Uuid); + Trackable trackable = null; + + if (anchor == null) + { + // anchor not found, try to find uuid as a trackable + trackable = _trackableService.Get(request.Uuid); + } if (anchor != null) { @@ -77,6 +85,37 @@ namespace ETSI.ARF.OpenAPI.WorldStorage.Controllers result.RelocInfo.Add(relocalizationInformation); } + else if (trackable != null) + { + history.Add(trackable.UUID); + + Matrix4x4 matrix = Matrix4x4.Identity; + + var relocalizationInformation = new RelocalizationInformation(); + relocalizationInformation.RequestUUID = trackable.UUID; + relocalizationInformation.RequestType = TypeWorldStorage.TRACKABLEEnum; + relocalizationInformation.RelocObjects = new List<RelocalizationInformationRelocObjectsInner>(); + + if (trackable.Match(capabilities)) + { + // add itself with matrix identity as Transform3D + var itself = new RelocalizationInformationRelocObjectsInner(); + itself.Trackable = trackable; + itself.Mode = request.Mode; + itself.Transform3D = new List<float>() + { + matrix.M11, matrix.M12, matrix.M13, matrix.M14, + matrix.M21, matrix.M22, matrix.M23, matrix.M24, + matrix.M31, matrix.M32, matrix.M33, matrix.M34, + matrix.M41, matrix.M42, matrix.M43, matrix.M44, + }; + relocalizationInformation.RelocObjects.Add(itself); + } + + relocalizationInformation.RelocObjects.AddRange(FindRelocInfo(trackable.UUID, request.Mode, matrix, capabilities, ref history)); + + result.RelocInfo.Add(relocalizationInformation); + } else { Console.WriteLine($"WorldAnchor with UUID {request.Uuid} does not exist in database");