Skip to content
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.

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

Allow spawning multiple prefabs #8

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
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
84 changes: 56 additions & 28 deletions Assets/Flightmare/Scripts/CameraController.cs
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ namespace RPGFlightmare
public class CameraController : MonoBehaviour
{
// ==============================================================================
// Default Parameters
// Default Parameters
// ==============================================================================
[HideInInspector]
public const int pose_client_default_port = 10253;
Expand Down Expand Up @@ -89,15 +89,15 @@ public class CameraController : MonoBehaviour
private Texture2D rendered_frame;
private object socket_lock;
// Unity Image Synthesis
// post processing including object/category segmentation,
// optical flow, depth image.
// post processing including object/category segmentation,
// optical flow, depth image.
private RPGImageSynthesis img_post_processing;
private sceneSchedule scene_schedule;
private Vector3 thirdPV_cam_offset;
private int activate_vehicle_cam = 0;

/* =====================
* UNITY PLAYER EVENT HOOKS
* UNITY PLAYER EVENT HOOKS
* =====================
*/
// Function called when Unity Player is loaded.
Expand Down Expand Up @@ -163,7 +163,7 @@ public void Start()
StartCoroutine(WaitForRender());
}

// Co-routine in Unity, executed every frame.
// Co-routine in Unity, executed every frame.
public IEnumerator WaitForRender()
{
// Wait until end of frame to transmit images
Expand Down Expand Up @@ -242,10 +242,10 @@ public void ConnectToClient(string inputIPString)

}

/*
/*
* Update is called once per frame
* Take the most recent ZMQ message and use it to position the cameras.
* If there has not been a recent message, the renderer should probably pause rendering until a new request is received.
* If there has not been a recent message, the renderer should probably pause rendering until a new request is received.
*/
void Update()
{
Expand Down Expand Up @@ -300,12 +300,12 @@ void Update()
{
sendReady();
}
return; // no need to worry about the rest if not ready.
return; // no need to worry about the rest if not ready.
}
else
{
pub_message = new PubMessage_t(settings);
// after initialization, we only receive sub_message message of the vehicle.
// after initialization, we only receive sub_message message of the vehicle.
sub_message = JsonConvert.DeserializeObject<SubMessage_t>(msg[1].ConvertToString());
// Ensure that dynamic object settings such as depth-scaling and color are set correctly.
updateDynamicObjectSettings();
Expand Down Expand Up @@ -342,7 +342,7 @@ void Update()
}

/* ==================================
* FlightGoggles High Level Functions
* FlightGoggles High Level Functions
* ==================================
*/
// Tries to initialize uninitialized objects multiple times until the object is initialized.
Expand Down Expand Up @@ -459,7 +459,7 @@ void instantiateCameras()
Debug.Log(camera.ID);
// Get camera object
GameObject obj = internal_state.getGameobject(camera.ID, HD_camera);
//
//
var currentCam = obj.GetComponent<Camera>();
currentCam.fieldOfView = camera.fov;
currentCam.nearClipPlane = camera.nearClipPlane[0];
Expand Down Expand Up @@ -516,7 +516,7 @@ void updateObjectPositions()
// string camera_ID = vehicle_i.ID + "_" + camera.ID;
// Get camera object
GameObject obj = internal_state.getGameobject(camera.ID, HD_camera);
//
//
var currentCam = obj.GetComponent<Camera>();
currentCam.fieldOfView = camera.fov;
// apply translation and rotation;
Expand Down Expand Up @@ -562,11 +562,30 @@ void updateObjectPositions()

foreach (Object_t obj_state in sub_message.objects)
{
Debug.Log("Updating object with id " + obj_state.ID);
// Fallback to this template if loading the requested object is impossible
// NOTE: it would be better to have a "404-template"
GameObject template = gate_template;
// Check if the object exists. If not, the prefab 'obj_state.prefabID'
// should be loaded and used as template.
if(!internal_state.contains(obj_state.ID)) {
// Try loading the requested resource
GameObject prefab = Resources.Load(obj_state.prefabID) as GameObject;
if(prefab == null)
{
Debug.Log("Failed loading object, using default");
}
else
{
// ok, use it :)
template = prefab;
}
}
// Get the object (it is created in case it is missing)
GameObject obj = internal_state.getGameobject(obj_state.ID, template);
// Apply translation, rotation, and scaling
// GameObject prefab = Resources.Load(obj_state.prefabID) as GameObject;
GameObject other_obj = internal_state.getGameobject(obj_state.ID, gate_template);
other_obj.transform.SetPositionAndRotation(ListToVector3(obj_state.position), ListToQuaternion(obj_state.rotation));
other_obj.transform.localScale = ListToVector3(obj_state.size);
obj.transform.SetPositionAndRotation(ListToVector3(obj_state.position), ListToQuaternion(obj_state.rotation));
obj.transform.localScale = ListToVector3(obj_state.size);
}
{
// third person view camera
Expand Down Expand Up @@ -653,14 +672,14 @@ void updateLidarData()
{

float angle_i = lidar.start_angle + angle_resolution * beam_i;
// Find direction and origin of raytrace for LIDAR.
// Find direction and origin of raytrace for LIDAR.
var raycastDirection = lidar_rotation * new Vector3((float)Math.Cos(angle_i), 0, (float)Math.Sin(angle_i));
// Run the raytrace
bool hasHit = Physics.Raycast(lidar_position, raycastDirection, out lidarHit, lidar.max_distance);
// Get distance. Return max+1 if out of range
float raycastDistance = hasHit ? lidarHit.distance : lidar.max_distance + 1;
// Save the result of the raycast.
// vehicle_collisions.Add(raycastDistance); // don't use add..
// vehicle_collisions.Add(raycastDistance); // don't use add..
pub_message.pub_vehicles[vehicle_count - 1].lidar_ranges.Add(raycastDistance);
Debug.DrawLine(lidar_position, lidar_position + raycastDirection * raycastDistance, Color.red);
}
Expand All @@ -669,7 +688,7 @@ void updateLidarData()
}

/* =============================================
* FlightGoggles Initialization Functions
* FlightGoggles Initialization Functions
* =============================================
*/
void resizeScreen()
Expand Down Expand Up @@ -705,11 +724,20 @@ void instantiateObjects()
// Initialize additional objects
foreach (var obj_state in settings.objects)
{
// GameObject prefab = Resources.Load(obj_state.prefabID) as GameObject;
Debug.Log("obj_state id : " + obj_state.ID);
GameObject obj = internal_state.getGameobject(obj_state.ID, gate_template);
Debug.Log("Instantiating obj_state id : " + obj_state.ID);
// Try loading the given prefab.
GameObject prefab = Resources.Load(obj_state.prefabID) as GameObject;
// If the operation fails, fallback to the "template"
if(prefab == null)
{
Debug.Log("Failed loading object, using default");
// NOTE: it would be better to have a "404-template"
prefab = gate_template;
}
// Add the object and set its pose / size
GameObject obj = internal_state.getGameobject(obj_state.ID, prefab);
obj.transform.SetPositionAndRotation(ListToVector3(obj_state.position), ListToQuaternion(obj_state.rotation));
obj.transform.localScale = ListToVector3(obj_state.size);
// obj.layer = 9;
}
foreach (var vehicle in settings.vehicles)
{
Expand All @@ -733,7 +761,7 @@ void setCameraViewports()
GameObject obj = internal_state.getGameobject(camera.ID, HD_camera);
var currentCam = obj.GetComponent<Camera>();
// Make sure camera renders to the correct portion of the screen.
// currentCam.pixelRect = new Rect(settings.camWidth * camera.outputIndex, 0,
// currentCam.pixelRect = new Rect(settings.camWidth * camera.outputIndex, 0,
// settings.camWidth * (camera.outputIndex+1), settings.camHeight);
currentCam.pixelRect = new Rect(0, 0,
settings.camWidth, settings.camHeight);
Expand Down Expand Up @@ -839,7 +867,7 @@ void sendFrameOnWire()
}
byte[] readImageFromScreen(Camera_t cam_config)
{
// rendered_frame.ReadPixels(new Rect(cam_config.outputIndex*(cam_config.width), 0,
// rendered_frame.ReadPixels(new Rect(cam_config.outputIndex*(cam_config.width), 0,
// (cam_config.outputIndex+1)*cam_config.width, cam_config.height), 0, 0);
rendered_frame.ReadPixels(new Rect(0, 0,
cam_config.width, cam_config.height), 0, 0);
Expand All @@ -857,7 +885,7 @@ byte[] readImageFromHiddenCamera(Camera subcam, Camera_t cam_config)
RenderTexture.active = templRT;
subcam.targetTexture = templRT;
//
// subcam.pixelRect = new Rect(cam_config.width * cam_config.outputIndex, 0,
// subcam.pixelRect = new Rect(cam_config.width * cam_config.outputIndex, 0,
// cam_config.width * ( cam_config.outputIndex+1), cam_config.height);
subcam.pixelRect = new Rect(0, 0,
cam_config.width, cam_config.height);
Expand All @@ -880,7 +908,7 @@ byte[] readImageFromHiddenCamera(Camera subcam, Camera_t cam_config)
}

/* ==================================
* FlightGoggles Helper Functions
* FlightGoggles Helper Functions
* ==================================
*/

Expand Down Expand Up @@ -1162,4 +1190,4 @@ void FlyingCamSettings()
// writer.Close();
// }

// }
// }
12 changes: 9 additions & 3 deletions Assets/Flightmare/Scripts/MessageSpec.cs
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,12 @@ public class UnityState_t

private Dictionary<string, ObjectState_t> objects;

// Tells if the given object exists inside the environment.
public bool contains(string ID)
{
return objects.ContainsKey(ID);
}

// Advanced getters/setters
// Get Wrapper object, defaulting to a passed in template if it does not exist.
public ObjectState_t getWrapperObject(string ID, GameObject template)
Expand Down Expand Up @@ -164,7 +170,7 @@ public class Object_t
// =============================
public class SettingsMessage_t
{
// Startup parameters.
// Startup parameters.
// public bool sceneIsInternal { get; set; }
public int scene_id { get; set; }

Expand All @@ -178,7 +184,7 @@ public class SettingsMessage_t
// ==============================================================================
public int numVehicles { get { return vehicles.Count(); } }
public Vehicle_t mainVehicle { get; set; }
// we noly count the number of camera on the main vehicle.
// we noly count the number of camera on the main vehicle.
public int numCameras { get; set; }
public Camera_t mainCamera { get; set; }
// public List<Camera_t> cameras{ get; set; }
Expand All @@ -190,7 +196,7 @@ public class SettingsMessage_t
public void InitParamsters()
{
// kind of ugly, the purpose is to handle
// the vehile that has no cameras.
// the vehile that has no cameras.
if (numVehicles > 0)
{
mainVehicle = vehicles[(int)(numVehicles / 2)];
Expand Down