Skip to content

GPS and Drone Modules #1

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
ArjunGupte1 wants to merge 14 commits into
base: master
Choose a base branch
from
Open

GPS and Drone Modules #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
14 commits
Select commit Hold shift + click to select a range
74dc51f
Added new headers/cases for gps and drone
ArjunGupte1 Jun 16, 2022
922038c
Added implementation for car's position
ArjunGupte1 Jun 17, 2022
cdf74d1
Added C# script and meta file for the drone
ArjunGupte1 Jun 18, 2022
0456bfa
Added drone functionality
ArjunGupte1 Jun 19, 2022
5f370c5
Added render texture for the drone
ArjunGupte1 Jun 19, 2022
20a9c18
Updated PythonInterface.cs with feedback from PR
ArjunGupte1 Jul 23, 2022
31a839e
Updated PhysicsModule.cs with feedback from PR
ArjunGupte1 Jul 23, 2022
97a8bc1
Updated Racecar.cs with feedback from PR
ArjunGupte1 Jul 23, 2022
251c1da
Removed duplicate logic from Drone.cs and CameraModule.cs
ArjunGupte1 Jul 23, 2022
b7a285d
Removed duplicate logic from CameraModule.cs and Drone.cs
ArjunGupte1 Jul 23, 2022
50fcb56
Added new helper class to eliminate duplicate logic
ArjunGupte1 Jul 23, 2022
df9b01b
Added meta file for ImageCaptureHelper.cs
ArjunGupte1 Jul 23, 2022
af826b4
Fixed error in variable name
ArjunGupte1 Jul 23, 2022
73160da
Fixed error in variable scope
ArjunGupte1 Jul 23, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
49 changes: 44 additions & 5 deletions RacecarSim/Assets/Scripts/NonMonoBehavior/PythonInterface.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
/// </remarks>
private const int version = 1;
private const int version = 2;

/// <summary>
/// The UDP port used by the Unity simulation (this program).
Expand All @@ -40,7 +40,7 @@ public class PythonInterface
/// <summary>
Copy link
Contributor

@MatthewCalligaro MatthewCalligaro Jun 28, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Above, you need to change private const int version = 1; to 2 because you are updating the communication protocol

/// The time (in ms) to wait for Python to respond.
/// </summary>
private const int timeoutTime = 5000;
private const int timeoutTime = 5000; //Need to fix this.

/// <summary>
/// The maximum UDP packet size allowed on Windows.
Expand Down Expand Up @@ -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,
}

/// <summary>
Expand Down Expand Up @@ -226,7 +231,7 @@ private enum Error
return null;
}
}

LevelManager.UpdateConnectedPrograms();
return index;
}
Expand Down Expand Up @@ -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:
Expand All @@ -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;

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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:
Expand All @@ -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;
Expand Down
126 changes: 19 additions & 107 deletions RacecarSim/Assets/Scripts/Racecar/CameraModule.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
using System;
using System;
using System.Threading;
using UnityEngine;

Expand Down Expand Up @@ -62,39 +62,12 @@ public class CameraModule : RacecarModule
/// <summary>
/// The width (in pixels) of the depth images captured by the camera.
/// </summary>
public static int DepthWidth { get { return CameraModule.ColorWidth / Settings.DepthDivideFactor; } }
public static int DepthWidth { get { return ImageCaptureHelper.ColorWidth / Settings.DepthDivideFactor; } }

/// <summary>
/// The height (in pixels) of the depth images captured by the camera.
/// </summary>
public static int DepthHeight { get { return CameraModule.ColorHeight / Settings.DepthDivideFactor; } }

/// <summary>
/// The GPU-side texture to which the color camera renders.
/// </summary>
public RenderTexture ColorImage
{
get
{
return this.colorCamera.targetTexture;
}
}

/// <summary>
/// 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.
/// </summary>
public byte[] ColorImageRaw
{
get
{
if (!isColorImageRawValid)
{
this.UpdateColorImageRaw();
}
return this.colorImageRaw;
}
}
public static int DepthHeight { get { return ImageCaptureHelper.ColorHeight / Settings.DepthDivideFactor; } }

/// <summary>
/// The depth values (in cm) captured by the depth camera this frame, from top left to bottom right.
Expand Down Expand Up @@ -161,18 +134,6 @@ public void VisualizeDepth(Texture2D texture)
texture.Apply();
}

/// <summary>
/// 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.
/// </summary>
/// <returns>The color image captured by the camera.</returns>
public byte[] GetColorImageRawAsync()
{
this.mustUpdateColorImageRaw = true;
Thread.Sleep(CameraModule.asyncWaitTime);
return this.colorImageRaw;
}

/// <summary>
/// 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.
Expand All @@ -186,16 +147,6 @@ public byte[] GetDepthImageRawAsync()
}
#endregion

/// <summary>
/// Private member for the ColorImageRaw accessor.
/// </summary>
private byte[] colorImageRaw;

/// <summary>
/// True if colorImageRaw is up to date with the color image rendered for the current frame.
/// </summary>
private bool isColorImageRawValid = false;

/// <summary>
/// Private member for the DepthImage accessor.
/// </summary>
Expand All @@ -216,26 +167,26 @@ public byte[] GetDepthImageRawAsync()
/// </summary>
private bool isDepthImageRawValid = false;

/// <summary>
/// The color camera on the car.
/// </summary>
private Camera colorCamera;

/// <summary>
/// The depth camera on the car.
/// This is currently unused, but a future goal is to use this instead of raycasts.
/// </summary>
private Camera depthCamera;

/// <summary>
/// If true, colorImageRaw is updated next frame.
/// If true, depthImageRaw is updated next frame.
/// </summary>
private bool mustUpdateColorImageRaw;
private bool mustUpdateDepthImageRaw;

/// <summary>
/// If true, depthImageRaw is updated next frame.
/// The color camera on the car.
/// </summary>
private bool mustUpdateDepthImageRaw;
private Camera colorCamera;

/// <summary>
/// Helper object for the color camera.
/// </summary>
public ImageCaptureHelper colorCameraHelper;

protected override void Awake()
{
Expand All @@ -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();
}

Expand All @@ -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)
Expand All @@ -288,7 +233,7 @@ private void Update()

private void LateUpdate()
{
this.isColorImageRawValid = false;
colorCameraHelper.isRawImageValid = false;
this.isDepthImageValid = false;
this.isDepthImageRawValid = false;
}
Expand Down Expand Up @@ -322,39 +267,6 @@ private static Color InterpolateDepthColor(float depth)
}
}

/// <summary>
/// Update colorImageRaw by rendering the color camera on the GPU and copying to the CPU.
/// Warning: this operation is very expensive.
/// </summary>
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;
}

/// <summary>
/// Update depthImage by performing a ray cast for each depth pixel.
/// Warning: this operation is very expensive.
Expand All @@ -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;
}
Expand Down
Loading