diff --git a/RacecarSim/Assets/Scripts/NonMonoBehavior/PythonInterface.cs b/RacecarSim/Assets/Scripts/NonMonoBehavior/PythonInterface.cs index d4e23d0..88ca21c 100644 --- a/RacecarSim/Assets/Scripts/NonMonoBehavior/PythonInterface.cs +++ b/RacecarSim/Assets/Scripts/NonMonoBehavior/PythonInterface.cs @@ -20,7 +20,7 @@ public class PythonInterface /// should be incremented both here and in racecar_core_sim.py. This allows us to immediately detect /// if a user attempts to use incompatible versions of RacecarSim and racecar_core. /// - private const int version = 1; + private const int version = 2; /// /// The UDP port used by the Unity simulation (this program). @@ -40,7 +40,7 @@ public class PythonInterface /// /// The time (in ms) to wait for Python to respond. /// - private const int timeoutTime = 5000; + private const int timeoutTime = 5000; //Need to fix this. /// /// The maximum UDP packet size allowed on Windows. @@ -159,6 +159,11 @@ private enum Header lidar_get_samples, physics_get_linear_acceleration, physics_get_angular_velocity, + physics_get_position, + drone_get_image, + drone_get_height, + drone_set_height, + drone_return_to_car, } /// @@ -226,7 +231,7 @@ private enum Error return null; } } - + LevelManager.UpdateConnectedPrograms(); return index; } @@ -323,7 +328,8 @@ private void PythonCall(Header function) break; case Header.camera_get_color_image: - pythonFinished = !this.SendFragmented(racecar.Camera.ColorImageRaw, 32, endPoint); + pythonFinished = !this.SendFragmented(racecar.Camera.colorCameraHelper.RawImage, 32, endPoint); + //pythonFinished = !this.SendFragmented(racecar.Camera.ColorImageRaw, 32, endPoint); break; case Header.camera_get_depth_image: @@ -333,11 +339,13 @@ private void PythonCall(Header function) case Header.camera_get_width: sendData = BitConverter.GetBytes(CameraModule.ColorWidth); + //sendData = BitConverter.GetBytes(CameraModule.ColorWidth); this.udpClient.Send(sendData, sendData.Length, endPoint); break; case Header.camera_get_height: sendData = BitConverter.GetBytes(CameraModule.ColorHeight); + //sendData = BitConverter.GetBytes(CameraModule.ColorHeight); this.udpClient.Send(sendData, sendData.Length, endPoint); break; @@ -413,6 +421,31 @@ private void PythonCall(Header function) this.udpClient.Send(sendData, sendData.Length, endPoint); break; + case Header.physics_get_position: + Vector3 position = racecar.Physics.Position; + sendData = new byte[sizeof(float) * 3]; + Buffer.BlockCopy(new float[] { position.x, position.y, position.z }, 0, sendData, 0, sendData.Length); + this.udpClient.Send(sendData, sendData.Length, endPoint); + break; + + case Header.drone_get_image: + pythonFinished = !this.SendFragmented(racecar.Drone.droneCameraHelper.RawImage, 32, endPoint); + //pythonFinished = !this.SendFragmented(racecar.Drone.DroneImageRaw, 32, endPoint); + break; + + case Header.drone_get_height: + sendData = BitConverter.GetBytes(racecar.Drone.CurrentPosition.y); + this.udpClient.Send(sendData, sendData.Length, endPoint); + break; + + case Header.drone_set_height: + racecar.Drone.TargetHeight = BitConverter.ToSingle(data, 4); + break; + + case Header.drone_return_to_car: + racecar.Drone.Land(); + break; + default: Debug.LogError($">> Error: The function {header} is not supported by RacecarSim."); pythonFinished = true; @@ -578,7 +611,8 @@ private void ProcessAsyncCalls() break; case Header.camera_get_color_image: - this.SendFragmentedAsync(racecar.Camera.GetColorImageRawAsync(), 32, receiveEndPoint); + this.SendFragmentedAsync(racecar.Camera.colorCameraHelper.GetRawImageAsync(), 32, receiveEndPoint); + //this.SendFragmentedAsync(racecar.Camera.GetColorImageRawAsync(), 32, receiveEndPoint); break; case Header.camera_get_depth_image: @@ -592,6 +626,11 @@ private void ProcessAsyncCalls() this.udpClientAsync.Send(sendData, sendData.Length, receiveEndPoint); break; + case Header.drone_get_image: + this.SendFragmentedAsync(racecar.Drone.droneCameraHelper.GetRawImageAsync(), 32, receiveEndPoint); + //this.SendFragmentedAsync(racecar.Drone.GetDroneImageRawAsync(), 32, receiveEndPoint); + break; + default: Debug.LogError($">> Error: The function {header} is not supported by RacecarSim for async calls."); break; diff --git a/RacecarSim/Assets/Scripts/Racecar/CameraModule.cs b/RacecarSim/Assets/Scripts/Racecar/CameraModule.cs index bf46d64..3a67286 100644 --- a/RacecarSim/Assets/Scripts/Racecar/CameraModule.cs +++ b/RacecarSim/Assets/Scripts/Racecar/CameraModule.cs @@ -1,4 +1,4 @@ -using System; +using System; using System.Threading; using UnityEngine; @@ -62,39 +62,12 @@ public class CameraModule : RacecarModule /// /// The width (in pixels) of the depth images captured by the camera. /// - public static int DepthWidth { get { return CameraModule.ColorWidth / Settings.DepthDivideFactor; } } + public static int DepthWidth { get { return ImageCaptureHelper.ColorWidth / Settings.DepthDivideFactor; } } /// /// The height (in pixels) of the depth images captured by the camera. /// - public static int DepthHeight { get { return CameraModule.ColorHeight / Settings.DepthDivideFactor; } } - - /// - /// The GPU-side texture to which the color camera renders. - /// - public RenderTexture ColorImage - { - get - { - return this.colorCamera.targetTexture; - } - } - - /// - /// The raw bytes of the color image captured by the color camera this frame. - /// Each pixel is stored in the ARGB 32-bit format, from top left to bottom right. - /// - public byte[] ColorImageRaw - { - get - { - if (!isColorImageRawValid) - { - this.UpdateColorImageRaw(); - } - return this.colorImageRaw; - } - } + public static int DepthHeight { get { return ImageCaptureHelper.ColorHeight / Settings.DepthDivideFactor; } } /// /// The depth values (in cm) captured by the depth camera this frame, from top left to bottom right. @@ -161,18 +134,6 @@ public void VisualizeDepth(Texture2D texture) texture.Apply(); } - /// - /// Asynchronously updates and returns the color image captured by the camera. - /// Warning: This method blocks for asyncWaitTime ms to wait for the new image to load. - /// - /// The color image captured by the camera. - public byte[] GetColorImageRawAsync() - { - this.mustUpdateColorImageRaw = true; - Thread.Sleep(CameraModule.asyncWaitTime); - return this.colorImageRaw; - } - /// /// Asynchronously updates and returns the depth image captured by the camera. /// Warning: This method blocks for asyncWaitTime ms to wait for the new image to load. @@ -186,16 +147,6 @@ public byte[] GetDepthImageRawAsync() } #endregion - /// - /// Private member for the ColorImageRaw accessor. - /// - private byte[] colorImageRaw; - - /// - /// True if colorImageRaw is up to date with the color image rendered for the current frame. - /// - private bool isColorImageRawValid = false; - /// /// Private member for the DepthImage accessor. /// @@ -216,11 +167,6 @@ public byte[] GetDepthImageRawAsync() /// private bool isDepthImageRawValid = false; - /// - /// The color camera on the car. - /// - private Camera colorCamera; - /// /// The depth camera on the car. /// This is currently unused, but a future goal is to use this instead of raycasts. @@ -228,14 +174,19 @@ public byte[] GetDepthImageRawAsync() private Camera depthCamera; /// - /// If true, colorImageRaw is updated next frame. + /// If true, depthImageRaw is updated next frame. /// - private bool mustUpdateColorImageRaw; + private bool mustUpdateDepthImageRaw; /// - /// If true, depthImageRaw is updated next frame. + /// The color camera on the car. /// - private bool mustUpdateDepthImageRaw; + private Camera colorCamera; + + /// + /// Helper object for the color camera. + /// + public ImageCaptureHelper colorCameraHelper; protected override void Awake() { @@ -250,13 +201,7 @@ protected override void Awake() } this.depthImageRaw = new byte[sizeof(float) * CameraModule.DepthHeight * CameraModule.DepthWidth]; - this.colorImageRaw = new byte[sizeof(float) * CameraModule.ColorWidth * CameraModule.ColorHeight]; - - if (Settings.HideCarsInColorCamera) - { - this.colorCamera.cullingMask &= ~(1 << LayerMask.NameToLayer("Player")); - } - + colorCameraHelper = new ImageCaptureHelper(this.colorCamera); base.Awake(); } @@ -268,10 +213,10 @@ private void Start() private void Update() { - if (this.mustUpdateColorImageRaw) + if (colorCameraHelper.mustUpdateRawImage) { - this.UpdateColorImageRaw(); - this.mustUpdateColorImageRaw = false; + colorCameraHelper.UpdateRawImage(); + colorCameraHelper.mustUpdateRawImage = false; } if (this.mustUpdateDepthImageRaw) @@ -288,7 +233,7 @@ private void Update() private void LateUpdate() { - this.isColorImageRawValid = false; + colorCameraHelper.isRawImageValid = false; this.isDepthImageValid = false; this.isDepthImageRawValid = false; } @@ -322,39 +267,6 @@ private static Color InterpolateDepthColor(float depth) } } - /// - /// Update colorImageRaw by rendering the color camera on the GPU and copying to the CPU. - /// Warning: this operation is very expensive. - /// - private void UpdateColorImageRaw() - { - RenderTexture activeRenderTexture = RenderTexture.active; - - // Tell GPU to render the image captured by the color camera - RenderTexture.active = this.ColorImage; - this.colorCamera.Render(); - - // Copy this image from the GPU to a Texture2D on the CPU - Texture2D image = new Texture2D(this.ColorImage.width, this.ColorImage.height); - image.ReadPixels(new Rect(0, 0, this.ColorImage.width, this.ColorImage.height), 0, 0); - image.Apply(); - - // Restore the previous GPU render target - RenderTexture.active = activeRenderTexture; - - // Copy the bytes from the Texture2D to this.colorImageRaw, reversing row order - // (Unity orders bottom-to-top, we want top-to-bottom) - byte[] bytes = image.GetRawTextureData(); - int bytesPerRow = CameraModule.ColorWidth * 4; - for (int r = 0; r < CameraModule.ColorHeight; r++) - { - Buffer.BlockCopy(bytes, (CameraModule.ColorHeight - r - 1) * bytesPerRow, this.colorImageRaw, r * bytesPerRow, bytesPerRow); - } - - Destroy(image); - this.isColorImageRawValid = true; - } - /// /// Update depthImage by performing a ray cast for each depth pixel. /// Warning: this operation is very expensive. @@ -372,8 +284,8 @@ private void UpdateDepthImage() if (Physics.Raycast(ray, out RaycastHit raycastHit, CameraModule.maxRange, Constants.IgnoreUIMask)) { - float distance = Settings.IsRealism - ? raycastHit.distance * NormalDist.Random(1, CameraModule.averageErrorFactor) + float distance = Settings.IsRealism + ? raycastHit.distance * NormalDist.Random(1, CameraModule.averageErrorFactor) : raycastHit.distance; this.depthImage[r][c] = distance > CameraModule.minRange ? distance * 10 : CameraModule.minCode; } diff --git a/RacecarSim/Assets/Scripts/Racecar/Drone.cs b/RacecarSim/Assets/Scripts/Racecar/Drone.cs new file mode 100644 index 0000000..01a0791 --- /dev/null +++ b/RacecarSim/Assets/Scripts/Racecar/Drone.cs @@ -0,0 +1,131 @@ +using System.Collections; +using System.Collections.Generic; +using System.Threading; +using System; +using UnityEngine; + +public class Drone : RacecarModule +{ + + #region Constants + /// + /// The width (in pixels) of the color images captured by the camera. + /// + public const int ColorWidth = 640; + + /// + /// The height (in pixels) of the color images captured by the camera. + /// + public const int ColorHeight = 480; + + /// + /// The starting position of the drone. + /// + public static Vector3 startingPosition = new Vector3(0f, 0f, 0f); + + /// + /// The target position of the drone - initially set to the starting position. + /// + public Vector3 targetPosition = startingPosition; + + /// + /// The field of view (in degrees) of the camera. + /// Based on the Intel RealSense D435i datasheet. + /// + private static readonly Vector2 fieldOfView = new Vector2(69.4f, 42.5f); + + /// + /// The average relative error of distance measurements. + /// Based on the Intel RealSense D435i datasheet. + /// + private const float averageErrorFactor = 0.02f; + + /// + /// Time (in ms) to wait for the color or depth image to update during an async call. + /// + private const int asyncWaitTime = 200; + + /// + /// Controls the rate at which the drone flies up and down. + /// + private const int droneSpeed = 20; + #endregion + + #region Public Interface + /// + /// The camera on the drone. + /// + private Camera droneCamera; + + /// + /// Helper object for the drone camera. + /// + public ImageCaptureHelper droneCameraHelper; + + /// + /// The current position of the drone. + /// + public Vector3 CurrentPosition + { + get + { + return this.transform.position; + } + } + + /// + /// The target height the drone should fly to. + /// + public float TargetHeight + { + private get + { + return this.targetPosition.y; + } + + set + { + this.targetPosition = new Vector3(this.targetPosition.x, value, this.targetPosition.z); + } + } + + /// + /// Has the drone descend to its starting position when called. + /// + /// Null. + public void Land() + { + this.targetPosition = startingPosition; + } + #endregion + + protected override void Awake() + { + Camera[] cameras = this.GetComponentsInChildren(); + this.droneCamera = cameras[0]; + droneCameraHelper = new ImageCaptureHelper(this.droneCamera); + base.Awake(); + } + + private void Start() + { + this.droneCamera.fieldOfView = Drone.fieldOfView.y; + startingPosition = new Vector3(transform.localPosition.x, transform.localPosition.y, transform.localPosition.z); + this.targetPosition = startingPosition; + } + + private void Update() + { + if (droneCameraHelper.mustUpdateRawImage) + { + droneCameraHelper.UpdateRawImage(); + droneCameraHelper.mustUpdateRawImage = false; + } + transform.localPosition = Vector3.Lerp(transform.localPosition, targetPosition, (Time.deltaTime * droneSpeed) / Vector3.Distance(targetPosition, transform.localPosition)); + } + + private void LateUpdate() + { + droneCameraHelper.isRawImageValid = false; + } +} diff --git a/RacecarSim/Assets/Scripts/Racecar/Drone.cs.meta b/RacecarSim/Assets/Scripts/Racecar/Drone.cs.meta new file mode 100644 index 0000000..90464c7 --- /dev/null +++ b/RacecarSim/Assets/Scripts/Racecar/Drone.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 2864254d0b2ca9f448a27eebeac21179 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/RacecarSim/Assets/Scripts/Racecar/ImageCaptureHelper.cs b/RacecarSim/Assets/Scripts/Racecar/ImageCaptureHelper.cs new file mode 100644 index 0000000..7daed83 --- /dev/null +++ b/RacecarSim/Assets/Scripts/Racecar/ImageCaptureHelper.cs @@ -0,0 +1,131 @@ +using System; +using System.Threading; +using System.Collections; +using System.Collections.Generic; +using UnityEngine; + + +public class ImageCaptureHelper : RacecarModule { + + /// + /// The width (in pixels) of the color images captured by the camera. + /// + public const int ColorWidth = 640; + + /// + /// The height (in pixels) of the color images captured by the camera. + /// + public const int ColorHeight = 480; + + /// + /// The average relative error of distance measurements. + /// Based on the Intel RealSense D435i datasheet. + /// + private const float averageErrorFactor = 0.02f; + + /// + /// Time (in ms) to wait for the color or depth image to update during an async call. + /// + private const int asyncWaitTime = 200; + + /// + /// The camera that the ImageCaptureHelper object uses. + /// + private Camera camera; + + /// + /// Private member for the RawImage accessor. + /// + private byte[] rawImage; + + /// + /// True if RawImage is up to date with the color image rendered for the current frame. + /// + public bool isRawImageValid = false; + + /// + /// If true, RawImage is updated next frame. + /// + public bool mustUpdateRawImage; + + /// + /// Constructor for this class. + /// + public ImageCaptureHelper(Camera camera) { + this.camera = camera; + this.rawImage = new byte[sizeof(float) * ImageCaptureHelper.ColorWidth * ImageCaptureHelper.ColorHeight]; + } + + /// + /// The GPU-side texture to which the camera renders. + /// + public RenderTexture ImageTexture + { + get + { + return this.camera.targetTexture; + } + } + + /// + /// The raw bytes of the color image captured by the camera this frame. + /// Each pixel is stored in the ARGB 32-bit format, from top left to bottom right. + /// + public byte[] RawImage + { + get + { + if (!isRawImageValid) + { + this.UpdateRawImage(); + } + return this.rawImage; + } + } + + /// + /// Asynchronously updates and returns the color image captured by the camera. + /// Warning: This method blocks for asyncWaitTime ms to wait for the new image to load. + /// + /// The color image captured by the drone's camera. + public byte[] GetRawImageAsync() + { + this.mustUpdateRawImage = true; + Thread.Sleep(ImageCaptureHelper.asyncWaitTime); + return this.rawImage; + } + + /// + /// Update rawImage by rendering the color camera on the GPU and copying to the CPU. + /// Warning: this operation is very expensive. + /// + public void UpdateRawImage() + { + RenderTexture activeRenderTexture = RenderTexture.active; + + // Tell GPU to render the image captured by the camera + RenderTexture.active = this.ImageTexture; + this.camera.Render(); + + // Copy this image from the GPU to a Texture2D on the CPU + Texture2D image = new Texture2D(this.ImageTexture.width, this.ImageTexture.height); + image.ReadPixels(new Rect(0, 0, this.ImageTexture.width, this.ImageTexture.height), 0, 0); + image.Apply(); + + // Restore the previous GPU render target + RenderTexture.active = activeRenderTexture; + + // Copy the bytes from the Texture2D to this.colorImageRaw, reversing row order + // (Unity orders bottom-to-top, we want top-to-bottom) + byte[] bytes = image.GetRawTextureData(); + int bytesPerRow = ImageCaptureHelper.ColorWidth * 4; + for (int r = 0; r < ImageCaptureHelper.ColorHeight; r++) + { + Buffer.BlockCopy(bytes, (ImageCaptureHelper.ColorHeight - r - 1) * bytesPerRow, this.rawImage, r * bytesPerRow, bytesPerRow); + } + + Destroy(image); + this.isRawImageValid = true; + } + +} diff --git a/RacecarSim/Assets/Scripts/Racecar/ImageCaptureHelper.cs.meta b/RacecarSim/Assets/Scripts/Racecar/ImageCaptureHelper.cs.meta new file mode 100644 index 0000000..d3aed89 --- /dev/null +++ b/RacecarSim/Assets/Scripts/Racecar/ImageCaptureHelper.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 508f5b66d360ac848842679ca548f61b +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/RacecarSim/Assets/Scripts/Racecar/PhysicsModule.cs b/RacecarSim/Assets/Scripts/Racecar/PhysicsModule.cs index 63c2e97..dc22494 100644 --- a/RacecarSim/Assets/Scripts/Racecar/PhysicsModule.cs +++ b/RacecarSim/Assets/Scripts/Racecar/PhysicsModule.cs @@ -34,6 +34,18 @@ public class PhysicsModule : RacecarModule /// This value is made up (it is NOT specified in the Intel RealSense D435i datasheet). /// private const float angularErrorFixed = 0.005f; + + /// + /// The average relative error of position measurements. + /// This value is made up. + /// + private const float positionErrorFactor = 0.001f; + + /// + /// The average fixed error applied to all position measurements. + /// This value is made up. + /// + private const float positionErrorFixed = 0.005f; #endregion #region Public Interface @@ -82,6 +94,32 @@ public Vector3 AngularVelocity return this.angularVelocity.Value; } } + + /// + /// The position of the car relative to the Unity scene's global origin (in meters). + /// + public Vector3 Position + { + get + { + if (!this.position.HasValue) + { + // Unity uses a left-handed coordinate system, but our API is right-handed + Vector3 pos = -this.rBody.position; + + if (Settings.IsRealism) + { + pos *= NormalDist.Random(1, PhysicsModule.positionErrorFactor); + pos.x += NormalDist.Random(0, PhysicsModule.positionErrorFixed); + pos.y += NormalDist.Random(0, PhysicsModule.positionErrorFixed); + pos.z += NormalDist.Random(0, PhysicsModule.positionErrorFixed); + } + + this.position = pos; + } + return this.position.Value; + } + } #endregion /// @@ -104,6 +142,11 @@ public Vector3 AngularVelocity /// private Vector3? angularVelocity = null; + /// + /// Private member for the Position accessor + /// + private Vector3? position = null; + protected override void Awake() { this.rBody = this.GetComponent(); @@ -146,5 +189,6 @@ private void LateUpdate() { this.linearVelocity = null; this.angularVelocity = null; + this.position = null; } } diff --git a/RacecarSim/Assets/Scripts/Racecar/Racecar.cs b/RacecarSim/Assets/Scripts/Racecar/Racecar.cs index c09e033..a8479bd 100644 --- a/RacecarSim/Assets/Scripts/Racecar/Racecar.cs +++ b/RacecarSim/Assets/Scripts/Racecar/Racecar.cs @@ -67,6 +67,11 @@ public class Racecar : MonoBehaviour /// Exposes the RealSense D435i IMU. /// public PhysicsModule Physics { get; private set; } + + /// + /// Exposes the drone model. + /// + public Drone Drone { get; private set; } /// /// The heads-up display controlled by this car, if any. @@ -177,6 +182,7 @@ private void Awake() this.Drive = this.GetComponent(); this.Lidar = this.GetComponentInChildren(); this.Physics = this.GetComponent(); + this.Drone = this.GetComponentInChildren(); // Begin with main player camera (0th camera) if (this.playerCameras.Length > 0) diff --git a/RacecarSim/Assets/Textures/DroneImg.renderTexture b/RacecarSim/Assets/Textures/DroneImg.renderTexture new file mode 100644 index 0000000..d92b965 --- /dev/null +++ b/RacecarSim/Assets/Textures/DroneImg.renderTexture @@ -0,0 +1,37 @@ +%YAML 1.1 +%TAG !u! tag:unity3d.com,2011: +--- !u!84 &8400000 +RenderTexture: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_Name: DroneImg + m_ImageContentsHash: + serializedVersion: 2 + Hash: 00000000000000000000000000000000 + m_ForcedFallbackFormat: 4 + m_DownscaleFallback: 0 + serializedVersion: 3 + m_Width: 640 + m_Height: 480 + m_AntiAliasing: 1 + m_MipCount: -1 + m_DepthFormat: 2 + m_ColorFormat: 8 + m_MipMap: 0 + m_GenerateMips: 1 + m_SRGB: 0 + m_UseDynamicScale: 0 + m_BindMS: 0 + m_EnableCompatibleFormat: 1 + m_TextureSettings: + serializedVersion: 2 + m_FilterMode: 1 + m_Aniso: 0 + m_MipBias: 0 + m_WrapU: 1 + m_WrapV: 1 + m_WrapW: 1 + m_Dimension: 2 + m_VolumeDepth: 1 diff --git a/RacecarSim/Assets/Textures/DroneImg.renderTexture.meta b/RacecarSim/Assets/Textures/DroneImg.renderTexture.meta new file mode 100644 index 0000000..77f93dd --- /dev/null +++ b/RacecarSim/Assets/Textures/DroneImg.renderTexture.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: df776d7c6bb9dc14cb34e7fa995a3a07 +NativeFormatImporter: + externalObjects: {} + mainObjectFileID: 0 + userData: + assetBundleName: + assetBundleVariant: