From 74dc51f6dfe49b338b2b952930130ebbfdcecfb8 Mon Sep 17 00:00:00 2001 From: ArjunGupte1 <43100142+ArjunGupte1@users.noreply.github.com> Date: Thu, 16 Jun 2022 16:58:46 -0700 Subject: [PATCH 01/14] Added new headers/cases for gps and drone --- .../NonMonoBehavior/PythonInterface.cs | 37 ++++++++++++++++++- 1 file changed, 35 insertions(+), 2 deletions(-) diff --git a/RacecarSim/Assets/Scripts/NonMonoBehavior/PythonInterface.cs b/RacecarSim/Assets/Scripts/NonMonoBehavior/PythonInterface.cs index d4e23d0..92e103b 100644 --- a/RacecarSim/Assets/Scripts/NonMonoBehavior/PythonInterface.cs +++ b/RacecarSim/Assets/Scripts/NonMonoBehavior/PythonInterface.cs @@ -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 = 20000; //changed this from 5000 to 20000 since image processing takes > 5s /// /// 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_drone_image, + drone_get_drone_height, + drone_set_height, + drone_return_home, } /// @@ -226,7 +231,7 @@ private enum Error return null; } } - + LevelManager.UpdateConnectedPrograms(); return index; } @@ -413,6 +418,30 @@ 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_drone_image: + pythonFinished = !this.SendFragmented(racecar.Drone.DroneImageRaw, 32, endPoint); + break; + + case Header.drone_get_drone_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_home: + racecar.Drone.Land(); + break; + default: Debug.LogError($">> Error: The function {header} is not supported by RacecarSim."); pythonFinished = true; @@ -592,6 +621,10 @@ private void ProcessAsyncCalls() this.udpClientAsync.Send(sendData, sendData.Length, receiveEndPoint); break; + case Header.drone_get_drone_image: + this.SendFragmentedAsync(racecar.Drone.GetDroneImageRawAsync(), 32, receiveEndPoint); + break; + default: Debug.LogError($">> Error: The function {header} is not supported by RacecarSim for async calls."); break; From 922038c900269168beb7e1abe18ad9419a2d471d Mon Sep 17 00:00:00 2001 From: ArjunGupte1 <43100142+ArjunGupte1@users.noreply.github.com> Date: Thu, 16 Jun 2022 17:07:14 -0700 Subject: [PATCH 02/14] Added implementation for car's position --- .../Assets/Scripts/Racecar/PhysicsModule.cs | 44 +++++++++++++++++++ 1 file changed, 44 insertions(+) diff --git a/RacecarSim/Assets/Scripts/Racecar/PhysicsModule.cs b/RacecarSim/Assets/Scripts/Racecar/PhysicsModule.cs index 63c2e97..7a83f17 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 (it is NOT specified in the Intel RealSense D435i datasheet). + /// + private const float positionErrorFactor = 0.001f; + + /// + /// The average fixed error applied to all position measurements. + /// This value is made up (it is NOT specified in the Intel RealSense D435i datasheet). + /// + private const float positionErrorFixed = 0.005f; //old value => 4.90f #endregion #region Public Interface @@ -82,6 +94,32 @@ public Vector3 AngularVelocity return this.angularVelocity.Value; } } + + /// + /// The position of the car (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; } } From cdf74d1690514032fdfea2e6bd376b7c15d1030b Mon Sep 17 00:00:00 2001 From: ArjunGupte1 <43100142+ArjunGupte1@users.noreply.github.com> Date: Sat, 18 Jun 2022 15:24:43 -0700 Subject: [PATCH 03/14] Added C# script and meta file for the drone --- RacecarSim/Assets/Scripts/Racecar/Drone.cs | 210 ++++++++++++++++++ .../Assets/Scripts/Racecar/Drone.cs.meta | 11 + 2 files changed, 221 insertions(+) create mode 100644 RacecarSim/Assets/Scripts/Racecar/Drone.cs create mode 100644 RacecarSim/Assets/Scripts/Racecar/Drone.cs.meta diff --git a/RacecarSim/Assets/Scripts/Racecar/Drone.cs b/RacecarSim/Assets/Scripts/Racecar/Drone.cs new file mode 100644 index 0000000..7de31e0 --- /dev/null +++ b/RacecarSim/Assets/Scripts/Racecar/Drone.cs @@ -0,0 +1,210 @@ +using System.Collections; +using System.Collections.Generic; +using System.Threading; +using System; +using UnityEngine; + +public class Drone : RacecarModule +{ + + #region Constants + /// + /// 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 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 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 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 GPU-side texture to which the color camera renders. + /// + public RenderTexture DroneImg + { + get + { + return this.droneCamera.targetTexture; + } + } + + /// + /// The raw bytes of the color image captured by the drone's camera this frame. + /// Each pixel is stored in the ARGB 32-bit format, from top left to bottom right. + /// + public byte[] DroneImageRaw + { + get + { + if (!isDroneImageRawValid) + { + this.UpdateDroneImageRaw(); + } + return this.droneImageRaw; + } + } + + /// + /// Asynchronously updates and returns the color image captured by the drone's 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[] GetDroneImageRawAsync() + { + this.mustUpdateDroneImageRaw = true; + Thread.Sleep(Drone.asyncWaitTime); + return this.droneImageRaw; + } + + /// + /// 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 + + /// + /// Private member for the DroneImageRaw accessor. + /// + private byte[] droneImageRaw; + + /// + /// True if droneImageRaw is up to date with the color image rendered for the current frame. + /// + private bool isDroneImageRawValid = false; + + /// + /// The color camera on the drone. + /// + private Camera droneCamera; + + /// + /// If true, droneImageRaw is updated next frame. + /// + private bool mustUpdateDroneImageRaw; + + private void UpdateDroneImageRaw() + { + RenderTexture activeRenderTexture = RenderTexture.active; + + // Tell GPU to render the image captured by the drone camera + RenderTexture.active = this.DroneImg; + this.droneCamera.Render(); + + // Copy this image from the GPU to a Texture2D on the CPU + Texture2D image = new Texture2D(this.DroneImg.width, this.DroneImg.height); + image.ReadPixels(new Rect(0, 0, this.DroneImg.width, this.DroneImg.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 = Drone.ColorWidth * 4; + for (int r = 0; r < Drone.ColorHeight; r++) + { + Buffer.BlockCopy(bytes, (Drone.ColorHeight - r - 1) * bytesPerRow, this.droneImageRaw, r * bytesPerRow, bytesPerRow); + } + + Destroy(image); + this.isDroneImageRawValid = true; + } + + protected override void Awake() + { + Camera[] cameras = this.GetComponentsInChildren(); + this.droneCamera = cameras[0]; + this.droneImageRaw = new byte[sizeof(float) * Drone.ColorWidth * Drone.ColorHeight]; + + 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 (this.mustUpdateDroneImageRaw) + { + this.UpdateDroneImageRaw(); + this.mustUpdateDroneImageRaw = false; + } + transform.localPosition = Vector3.Lerp(transform.localPosition, targetPosition, (Time.deltaTime * droneSpeed) / Vector3.Distance(targetPosition, transform.localPosition)); + } + + private void LateUpdate() + { + this.isDroneImageRawValid = 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: From 0456bfa80090fb9dea43eebc405f9110c1cdb037 Mon Sep 17 00:00:00 2001 From: ArjunGupte1 <43100142+ArjunGupte1@users.noreply.github.com> Date: Sun, 19 Jun 2022 00:48:27 -0700 Subject: [PATCH 04/14] Added drone functionality --- RacecarSim/Assets/Scripts/Racecar/Racecar.cs | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/RacecarSim/Assets/Scripts/Racecar/Racecar.cs b/RacecarSim/Assets/Scripts/Racecar/Racecar.cs index c09e033..157338c 100644 --- a/RacecarSim/Assets/Scripts/Racecar/Racecar.cs +++ b/RacecarSim/Assets/Scripts/Racecar/Racecar.cs @@ -72,6 +72,11 @@ public class Racecar : MonoBehaviour /// The heads-up display controlled by this car, if any. /// public Hud Hud { get; set; } + + /// + /// Exposes the drone model. + /// + public Drone Drone { get; private set; } /// /// The center point of the car. @@ -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) From 5f370c5261cbef17d508516d9b8696331138a546 Mon Sep 17 00:00:00 2001 From: ArjunGupte1 <43100142+ArjunGupte1@users.noreply.github.com> Date: Sun, 19 Jun 2022 00:50:08 -0700 Subject: [PATCH 05/14] Added render texture for the drone --- .../Assets/Textures/DroneImg.renderTexture | 37 +++++++++++++++++++ .../Textures/DroneImg.renderTexture.meta | 8 ++++ 2 files changed, 45 insertions(+) create mode 100644 RacecarSim/Assets/Textures/DroneImg.renderTexture create mode 100644 RacecarSim/Assets/Textures/DroneImg.renderTexture.meta 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: From 20a9c189489e63ada0ba933fc33f156468972662 Mon Sep 17 00:00:00 2001 From: ArjunGupte1 <43100142+ArjunGupte1@users.noreply.github.com> Date: Sat, 23 Jul 2022 00:49:51 -0700 Subject: [PATCH 06/14] Updated PythonInterface.cs with feedback from PR --- .../NonMonoBehavior/PythonInterface.cs | 32 +++++++++++-------- 1 file changed, 19 insertions(+), 13 deletions(-) diff --git a/RacecarSim/Assets/Scripts/NonMonoBehavior/PythonInterface.cs b/RacecarSim/Assets/Scripts/NonMonoBehavior/PythonInterface.cs index 92e103b..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 = 20000; //changed this from 5000 to 20000 since image processing takes > 5s + private const int timeoutTime = 5000; //Need to fix this. /// /// The maximum UDP packet size allowed on Windows. @@ -160,10 +160,10 @@ private enum Header physics_get_linear_acceleration, physics_get_angular_velocity, physics_get_position, - drone_get_drone_image, - drone_get_drone_height, + drone_get_image, + drone_get_height, drone_set_height, - drone_return_home, + drone_return_to_car, } /// @@ -328,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: @@ -338,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; @@ -425,11 +428,12 @@ private void PythonCall(Header function) this.udpClient.Send(sendData, sendData.Length, endPoint); break; - case Header.drone_get_drone_image: - pythonFinished = !this.SendFragmented(racecar.Drone.DroneImageRaw, 32, endPoint); + 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_drone_height: + case Header.drone_get_height: sendData = BitConverter.GetBytes(racecar.Drone.CurrentPosition.y); this.udpClient.Send(sendData, sendData.Length, endPoint); break; @@ -438,7 +442,7 @@ private void PythonCall(Header function) racecar.Drone.TargetHeight = BitConverter.ToSingle(data, 4); break; - case Header.drone_return_home: + case Header.drone_return_to_car: racecar.Drone.Land(); break; @@ -607,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: @@ -621,8 +626,9 @@ private void ProcessAsyncCalls() this.udpClientAsync.Send(sendData, sendData.Length, receiveEndPoint); break; - case Header.drone_get_drone_image: - this.SendFragmentedAsync(racecar.Drone.GetDroneImageRawAsync(), 32, receiveEndPoint); + case Header.drone_get_image: + this.SendFragmentedAsync(racecar.Drone.droneCameraHelper.GetRawImageAsync(), 32, receiveEndPoint); + //this.SendFragmentedAsync(racecar.Drone.GetDroneImageRawAsync(), 32, receiveEndPoint); break; default: From 31a839e46e4fe5300f79cf536d5415e0dd29c308 Mon Sep 17 00:00:00 2001 From: ArjunGupte1 <43100142+ArjunGupte1@users.noreply.github.com> Date: Sat, 23 Jul 2022 00:50:41 -0700 Subject: [PATCH 07/14] Updated PhysicsModule.cs with feedback from PR --- RacecarSim/Assets/Scripts/Racecar/PhysicsModule.cs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/RacecarSim/Assets/Scripts/Racecar/PhysicsModule.cs b/RacecarSim/Assets/Scripts/Racecar/PhysicsModule.cs index 7a83f17..dc22494 100644 --- a/RacecarSim/Assets/Scripts/Racecar/PhysicsModule.cs +++ b/RacecarSim/Assets/Scripts/Racecar/PhysicsModule.cs @@ -37,15 +37,15 @@ public class PhysicsModule : RacecarModule /// /// The average relative error of position measurements. - /// This value is made up (it is NOT specified in the Intel RealSense D435i datasheet). + /// 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 (it is NOT specified in the Intel RealSense D435i datasheet). + /// This value is made up. /// - private const float positionErrorFixed = 0.005f; //old value => 4.90f + private const float positionErrorFixed = 0.005f; #endregion #region Public Interface @@ -96,7 +96,7 @@ public Vector3 AngularVelocity } /// - /// The position of the car (in meters). + /// The position of the car relative to the Unity scene's global origin (in meters). /// public Vector3 Position { From 97a8bc16554d1bd887affcc161f650a898fc1471 Mon Sep 17 00:00:00 2001 From: ArjunGupte1 <43100142+ArjunGupte1@users.noreply.github.com> Date: Sat, 23 Jul 2022 00:52:34 -0700 Subject: [PATCH 08/14] Updated Racecar.cs with feedback from PR --- RacecarSim/Assets/Scripts/Racecar/Racecar.cs | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/RacecarSim/Assets/Scripts/Racecar/Racecar.cs b/RacecarSim/Assets/Scripts/Racecar/Racecar.cs index 157338c..a8479bd 100644 --- a/RacecarSim/Assets/Scripts/Racecar/Racecar.cs +++ b/RacecarSim/Assets/Scripts/Racecar/Racecar.cs @@ -67,17 +67,17 @@ public class Racecar : MonoBehaviour /// Exposes the RealSense D435i IMU. /// public PhysicsModule Physics { get; private set; } - - /// - /// The heads-up display controlled by this car, if any. - /// - public Hud Hud { get; set; } /// /// Exposes the drone model. /// public Drone Drone { get; private set; } + /// + /// The heads-up display controlled by this car, if any. + /// + public Hud Hud { get; set; } + /// /// The center point of the car. /// From 251c1da80750b01b43c5d004f1a748599655020a Mon Sep 17 00:00:00 2001 From: ArjunGupte1 <43100142+ArjunGupte1@users.noreply.github.com> Date: Sat, 23 Jul 2022 00:54:15 -0700 Subject: [PATCH 09/14] Removed duplicate logic from Drone.cs and CameraModule.cs --- RacecarSim/Assets/Scripts/Racecar/Drone.cs | 115 ++++----------------- 1 file changed, 18 insertions(+), 97 deletions(-) diff --git a/RacecarSim/Assets/Scripts/Racecar/Drone.cs b/RacecarSim/Assets/Scripts/Racecar/Drone.cs index 7de31e0..01a0791 100644 --- a/RacecarSim/Assets/Scripts/Racecar/Drone.cs +++ b/RacecarSim/Assets/Scripts/Racecar/Drone.cs @@ -9,24 +9,24 @@ public class Drone : RacecarModule #region Constants /// - /// The starting position of the drone. + /// The width (in pixels) of the color images captured by the camera. /// - public static Vector3 startingPosition = new Vector3(0f, 0f, 0f); + public const int ColorWidth = 640; /// - /// The target position of the drone - initially set to the starting position. + /// The height (in pixels) of the color images captured by the camera. /// - public Vector3 targetPosition = startingPosition; + public const int ColorHeight = 480; /// - /// The width (in pixels) of the color images captured by the camera. + /// The starting position of the drone. /// - public const int ColorWidth = 640; + public static Vector3 startingPosition = new Vector3(0f, 0f, 0f); /// - /// The height (in pixels) of the color images captured by the camera. + /// The target position of the drone - initially set to the starting position. /// - public const int ColorHeight = 480; + public Vector3 targetPosition = startingPosition; /// /// The field of view (in degrees) of the camera. @@ -41,7 +41,7 @@ public class Drone : RacecarModule private const float averageErrorFactor = 0.02f; /// - /// Time (in ms) to wait for the color image to update during an async call. + /// Time (in ms) to wait for the color or depth image to update during an async call. /// private const int asyncWaitTime = 200; @@ -53,43 +53,14 @@ public class Drone : RacecarModule #region Public Interface /// - /// The GPU-side texture to which the color camera renders. + /// The camera on the drone. /// - public RenderTexture DroneImg - { - get - { - return this.droneCamera.targetTexture; - } - } - - /// - /// The raw bytes of the color image captured by the drone's camera this frame. - /// Each pixel is stored in the ARGB 32-bit format, from top left to bottom right. - /// - public byte[] DroneImageRaw - { - get - { - if (!isDroneImageRawValid) - { - this.UpdateDroneImageRaw(); - } - return this.droneImageRaw; - } - } + private Camera droneCamera; /// - /// Asynchronously updates and returns the color image captured by the drone's camera. - /// Warning: This method blocks for asyncWaitTime ms to wait for the new image to load. + /// Helper object for the drone camera. /// - /// The color image captured by the drone's camera. - public byte[] GetDroneImageRawAsync() - { - this.mustUpdateDroneImageRaw = true; - Thread.Sleep(Drone.asyncWaitTime); - return this.droneImageRaw; - } + public ImageCaptureHelper droneCameraHelper; /// /// The current position of the drone. @@ -128,61 +99,11 @@ public void Land() } #endregion - /// - /// Private member for the DroneImageRaw accessor. - /// - private byte[] droneImageRaw; - - /// - /// True if droneImageRaw is up to date with the color image rendered for the current frame. - /// - private bool isDroneImageRawValid = false; - - /// - /// The color camera on the drone. - /// - private Camera droneCamera; - - /// - /// If true, droneImageRaw is updated next frame. - /// - private bool mustUpdateDroneImageRaw; - - private void UpdateDroneImageRaw() - { - RenderTexture activeRenderTexture = RenderTexture.active; - - // Tell GPU to render the image captured by the drone camera - RenderTexture.active = this.DroneImg; - this.droneCamera.Render(); - - // Copy this image from the GPU to a Texture2D on the CPU - Texture2D image = new Texture2D(this.DroneImg.width, this.DroneImg.height); - image.ReadPixels(new Rect(0, 0, this.DroneImg.width, this.DroneImg.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 = Drone.ColorWidth * 4; - for (int r = 0; r < Drone.ColorHeight; r++) - { - Buffer.BlockCopy(bytes, (Drone.ColorHeight - r - 1) * bytesPerRow, this.droneImageRaw, r * bytesPerRow, bytesPerRow); - } - - Destroy(image); - this.isDroneImageRawValid = true; - } - protected override void Awake() { Camera[] cameras = this.GetComponentsInChildren(); this.droneCamera = cameras[0]; - this.droneImageRaw = new byte[sizeof(float) * Drone.ColorWidth * Drone.ColorHeight]; - + droneCameraHelper = new ImageCaptureHelper(this.droneCamera); base.Awake(); } @@ -195,16 +116,16 @@ private void Start() private void Update() { - if (this.mustUpdateDroneImageRaw) + if (droneCameraHelper.mustUpdateRawImage) { - this.UpdateDroneImageRaw(); - this.mustUpdateDroneImageRaw = false; + droneCameraHelper.UpdateRawImage(); + droneCameraHelper.mustUpdateRawImage = false; } transform.localPosition = Vector3.Lerp(transform.localPosition, targetPosition, (Time.deltaTime * droneSpeed) / Vector3.Distance(targetPosition, transform.localPosition)); } private void LateUpdate() { - this.isDroneImageRawValid = false; + droneCameraHelper.isRawImageValid = false; } } From b7a285d816dcb80e418828b2e6bbb288be5ce6e0 Mon Sep 17 00:00:00 2001 From: ArjunGupte1 <43100142+ArjunGupte1@users.noreply.github.com> Date: Sat, 23 Jul 2022 00:55:03 -0700 Subject: [PATCH 10/14] Removed duplicate logic from CameraModule.cs and Drone.cs --- .../Assets/Scripts/Racecar/CameraModule.cs | 126 +++--------------- 1 file changed, 19 insertions(+), 107 deletions(-) diff --git a/RacecarSim/Assets/Scripts/Racecar/CameraModule.cs b/RacecarSim/Assets/Scripts/Racecar/CameraModule.cs index bf46d64..30cf5e8 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 ImageProcessingSM.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 ImageProcessingSM.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; } From 50fcb561b957547f4b98a984ce3ecd048dff8e61 Mon Sep 17 00:00:00 2001 From: ArjunGupte1 <43100142+ArjunGupte1@users.noreply.github.com> Date: Sat, 23 Jul 2022 00:57:34 -0700 Subject: [PATCH 11/14] Added new helper class to eliminate duplicate logic --- .../Scripts/Racecar/ImageCaptureHelper.cs | 131 ++++++++++++++++++ 1 file changed, 131 insertions(+) create mode 100644 RacecarSim/Assets/Scripts/Racecar/ImageCaptureHelper.cs diff --git a/RacecarSim/Assets/Scripts/Racecar/ImageCaptureHelper.cs b/RacecarSim/Assets/Scripts/Racecar/ImageCaptureHelper.cs new file mode 100644 index 0000000..b2ca532 --- /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. + /// + private const int ColorWidth = 640; + + /// + /// The height (in pixels) of the color images captured by the camera. + /// + private 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; + } + +} From df9b01bfeee94b8137a60718b3e1b035dcdefaa8 Mon Sep 17 00:00:00 2001 From: ArjunGupte1 <43100142+ArjunGupte1@users.noreply.github.com> Date: Sat, 23 Jul 2022 00:58:24 -0700 Subject: [PATCH 12/14] Added meta file for ImageCaptureHelper.cs --- .../Assets/Scripts/Racecar/ImageCaptureHelper.cs.meta | 11 +++++++++++ 1 file changed, 11 insertions(+) create mode 100644 RacecarSim/Assets/Scripts/Racecar/ImageCaptureHelper.cs.meta 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: From af826b4d5458464e72a54a633c1ac5d93cf157f9 Mon Sep 17 00:00:00 2001 From: ArjunGupte1 <43100142+ArjunGupte1@users.noreply.github.com> Date: Sat, 23 Jul 2022 01:11:51 -0700 Subject: [PATCH 13/14] Fixed error in variable name --- RacecarSim/Assets/Scripts/Racecar/CameraModule.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/RacecarSim/Assets/Scripts/Racecar/CameraModule.cs b/RacecarSim/Assets/Scripts/Racecar/CameraModule.cs index 30cf5e8..3a67286 100644 --- a/RacecarSim/Assets/Scripts/Racecar/CameraModule.cs +++ b/RacecarSim/Assets/Scripts/Racecar/CameraModule.cs @@ -62,12 +62,12 @@ public class CameraModule : RacecarModule /// /// The width (in pixels) of the depth images captured by the camera. /// - public static int DepthWidth { get { return ImageProcessingSM.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 ImageProcessingSM.ColorHeight / Settings.DepthDivideFactor; } } + 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. From 73160dacdd515c96363f9792bad9198a37cc49ce Mon Sep 17 00:00:00 2001 From: ArjunGupte1 <43100142+ArjunGupte1@users.noreply.github.com> Date: Sat, 23 Jul 2022 01:12:36 -0700 Subject: [PATCH 14/14] Fixed error in variable scope --- RacecarSim/Assets/Scripts/Racecar/ImageCaptureHelper.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/RacecarSim/Assets/Scripts/Racecar/ImageCaptureHelper.cs b/RacecarSim/Assets/Scripts/Racecar/ImageCaptureHelper.cs index b2ca532..7daed83 100644 --- a/RacecarSim/Assets/Scripts/Racecar/ImageCaptureHelper.cs +++ b/RacecarSim/Assets/Scripts/Racecar/ImageCaptureHelper.cs @@ -10,12 +10,12 @@ public class ImageCaptureHelper : RacecarModule { /// /// The width (in pixels) of the color images captured by the camera. /// - private const int ColorWidth = 640; + public const int ColorWidth = 640; /// /// The height (in pixels) of the color images captured by the camera. /// - private const int ColorHeight = 480; + public const int ColorHeight = 480; /// /// The average relative error of distance measurements.