2
0

FrontWheelTrackerLogger.cs 3.9 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980
  1. using System.Collections.Generic;
  2. using System.Globalization;
  3. using Logging.Base;
  4. using Tracking;
  5. using UnityEngine;
  6. namespace Logging.Data
  7. {
  8. public readonly struct FrontWheelTrackerData : ISerializable
  9. {
  10. private readonly long timestamp;
  11. private readonly float debugLegTrackerPositionX;
  12. private readonly float debugLegTrackerPositionY;
  13. private readonly float debugLegTrackerPositionZ;
  14. private readonly float debugPosOnBikePlaneX;
  15. private readonly float debugPosOnBikePlaneY;
  16. private readonly float debugPosOnBikePlaneZ;
  17. private readonly float debugFrontWheelTrackerRelativePositionX;
  18. private readonly float debugFrontWheelTrackerRelativePositionY;
  19. private readonly float debugFrontWheelTrackerRelativePositionZ;
  20. public FrontWheelTrackerData(long timestamp, float debugLegTrackerPositionX, float debugLegTrackerPositionY,
  21. float debugLegTrackerPositionZ, float debugPosOnBikePlaneX, float debugPosOnBikePlaneY,
  22. float debugPosOnBikePlaneZ, float debugFrontWheelTrackerRelativePositionX,
  23. float debugFrontWheelTrackerRelativePositionY, float debugFrontWheelTrackerRelativePositionZ)
  24. {
  25. this.timestamp = timestamp;
  26. this.debugLegTrackerPositionX = debugLegTrackerPositionX;
  27. this.debugLegTrackerPositionY = debugLegTrackerPositionY;
  28. this.debugLegTrackerPositionZ = debugLegTrackerPositionZ;
  29. this.debugPosOnBikePlaneX = debugPosOnBikePlaneX;
  30. this.debugPosOnBikePlaneY = debugPosOnBikePlaneY;
  31. this.debugPosOnBikePlaneZ = debugPosOnBikePlaneZ;
  32. this.debugFrontWheelTrackerRelativePositionX = debugFrontWheelTrackerRelativePositionX;
  33. this.debugFrontWheelTrackerRelativePositionY = debugFrontWheelTrackerRelativePositionY;
  34. this.debugFrontWheelTrackerRelativePositionZ = debugFrontWheelTrackerRelativePositionZ;
  35. }
  36. public KeyValuePair<long, string[]> Serialize() => new KeyValuePair<long, string[]>(timestamp, new[]
  37. {
  38. debugLegTrackerPositionX.ToString("F4", CultureInfo.InvariantCulture),
  39. debugLegTrackerPositionY.ToString("F4", CultureInfo.InvariantCulture),
  40. debugLegTrackerPositionZ.ToString("F4", CultureInfo.InvariantCulture),
  41. debugPosOnBikePlaneX.ToString("F4", CultureInfo.InvariantCulture),
  42. debugPosOnBikePlaneY.ToString("F4", CultureInfo.InvariantCulture),
  43. debugPosOnBikePlaneZ.ToString("F4", CultureInfo.InvariantCulture),
  44. debugFrontWheelTrackerRelativePositionX.ToString("F4", CultureInfo.InvariantCulture),
  45. debugFrontWheelTrackerRelativePositionY.ToString("F4", CultureInfo.InvariantCulture),
  46. debugFrontWheelTrackerRelativePositionZ.ToString("F4", CultureInfo.InvariantCulture)
  47. });
  48. }
  49. public class FrontWheelTrackerLogger : SensorDataLogger<FrontWheelTrackerData>
  50. {
  51. public FrontWheelTracker frontWheelTracker;
  52. public override string Key => "front_wheel_tracker";
  53. private void Update()
  54. {
  55. var legTrackerPos = frontWheelTracker.legTracker.RelativePosition;
  56. var posOnBikePlane = frontWheelTracker.DebugPosOnBikePlane;
  57. var fwtRelativePos = frontWheelTracker.RelativePosition;
  58. Log(new FrontWheelTrackerData(Helpers.RoundToLong(Time.time * 1000f),
  59. legTrackerPos.x,
  60. legTrackerPos.y,
  61. legTrackerPos.z,
  62. posOnBikePlane.x,
  63. posOnBikePlane.y,
  64. posOnBikePlane.z,
  65. fwtRelativePos.x,
  66. fwtRelativePos.y,
  67. fwtRelativePos.z));
  68. }
  69. public override IEnumerable<FrontWheelTrackerData> ReadLog(IEnumerable<IEnumerable<string>> lines)
  70. {
  71. throw new System.NotImplementedException(); //TODO
  72. }
  73. }
  74. }