public Waypoint nextPathPoint; // Refers to next waypoint
/// <summary>
/// Waypoint class object constructor
/// </summary>
/// <param name="myDrone"> This is the drone that the new waypoint should belong to (remember that you still need to add the waypoint to the path using this drone's methods) </param>
/// <param name="position"> This is the 3d location at which the new waypoint gameObject should be placed. </param>
public Waypoint(Drone myDrone, Vector3 position)
{
// Linking this waypoint to its drone
referenceDrone = myDrone;
// Setting up all the related gameObject parameters
GameObject baseObject = (GameObject)WorldProperties.worldObject.GetComponent <WorldProperties>().waypointBaseObject;
gameObjectPointer = Object.Instantiate(baseObject, position, Quaternion.identity);
gameObjectPointer.GetComponent <VRTK_InteractableObject>().ignoredColliders[0] = GameObject.Find("controller_right").GetComponent <SphereCollider>(); //Ignoring Collider from Controller so that WayPoint Zone is used
gameObjectPointer.GetComponent <WaypointProperties>().classPointer = this; // Connect the gameObject back to the classObject
gameObjectPointer.tag = "waypoint";
gameObjectPointer.name = baseObject.name;
gameObjectPointer.transform.localScale = WorldProperties.actualScale / 100;
gameObjectPointer.transform.parent = WorldProperties.worldObject.transform;
WorldProperties.AddClipShader(gameObjectPointer.transform);
// Initializing the ROSpoints Arraylist
ROSpoints = new ArrayList(0);
// Establishing the unique waypoint identifier
id = "" + referenceDrone.id + referenceDrone.nextWaypointId;
referenceDrone.nextWaypointId++;
//Debug.Log("Created new waypoint with id: " + id);
}
public Dictionary <string, Waypoint> waypointsDict; // Collection of the waypoints in this drone's path
/// <summary>
/// Constructor method for Drone class objects
/// </summary>
/// <param name="drone_obj"> We pass in a Gameobject for the drone -- this will be phased out and the new drone_obj gameObject will be instantiated in this method </param>
public Drone(Vector3 position)
{
// Create gameObject at position
GameObject baseObject = (GameObject)WorldProperties.worldObject.GetComponent <WorldProperties>().droneBaseObject;
gameObjectPointer = Object.Instantiate(baseObject, position, Quaternion.identity);
gameObjectPointer.GetComponent <DroneProperties>().classPointer = this; // Connect the gameObject back to the classObject
gameObjectPointer.tag = "Drone";
gameObjectPointer.name = baseObject.name;
gameObjectPointer.transform.localScale = WorldProperties.actualScale / 5;
gameObjectPointer.transform.parent = WorldProperties.worldObject.transform;
WorldProperties.AddClipShader(gameObjectPointer.transform);
// Initialize path and placement order lists
waypoints = new ArrayList(0);
waypointsOrder = new ArrayList(0);
// Add waypoints container
nextWaypointId = 0;
waypointsDict = new Dictionary <string, Waypoint>();
// Updating the world properties to reflect a new drone being added
id = WorldProperties.nextDroneId;
WorldProperties.dronesDict.Add(id, this);
WorldProperties.nextDroneId++;
// Select this drone
this.Select();
Debug.Log("Created new drone with id: " + id);
}
/// <summary>
/// This method handles the undo and delete functionality
/// Removes the waypoint from the scene and from the drone's path
/// </summary>
public void Undo() /// TODO: add undo for moving around
{
Drone currentlySelectedDrone = WorldProperties.GetSelectedDrone();
if (currentlySelectedDrone != null)
{
// Make sure the currently selected drone has waypoints
if (currentlySelectedDrone.WaypointsCount() >= 2)
{
// Otherwise we default to removing the last waypoint (UNDO)
Debug.Log("Removing most recently placed waypoint");
Waypoint lastWaypoint = currentlySelectedDrone.PopWaypoint();
// Remove from collisions list
//currentCollisions.RemoveAll(collision => collision.waypoint == lastWaypoint &&
// collision.type == CollisionType.WAYPOINT);
// currentCollisions.RemoveAll(collision => collision.waypoint == lastWaypoint &&
// collision.type == CollisionType.LINE);
// Catching edge case in which most recent collision was the last waypoint
//if (lastWaypoint == mostRecentCollision.waypoint)
//{
// mostRecentCollision.type = CollisionType.NOTHING;
// mostRecentCollision.waypoint = null;
// }
}
}
}
public void Redo()
{
Drone currentlySelectedDrone = WorldProperties.GetSelectedDrone();
if (currentlySelectedDrone != null)
{
currentlySelectedDrone.RestoreLastDeletedWaypoint();
}
}
/// <summary>
/// This handles setting the height of the new waypoint
/// </summary>
/// <param name="newWaypoint"></param>
private void AdjustHeight(Waypoint newWaypoint)
{
GameObject newWaypointGameObject = newWaypoint.gameObjectPointer;
float groundX = newWaypointGameObject.transform.position.x;
float groundY = newWaypointGameObject.transform.position.y;
float groundZ = newWaypointGameObject.transform.position.z;
float localX = controller.transform.position.x;
float localY = controller.transform.position.y;
float localZ = controller.transform.position.z;
float height = 2.147f + (float)Distance(groundX, groundZ, 0f, 0f, localX, localZ) * (float)Math.Tan(Math.PI * (ControllerInteractions.getLocalControllerRotation(OVRInput.Controller.RTouch).x));
float heightMin = 2.3f + WorldProperties.actualScale.y / 200; //mesh height = 2.147
height = Math.Min(WorldProperties.GetMaxHeight(), Math.Max(heightMin, height));
newWaypointGameObject.transform.position = new Vector3(groundX, height, groundZ);
}
public WaypointProperties waypointProperties; // Points to attached Waypoint Properties script
/// <summary>
/// Waypoint class object constructor
/// </summary>
/// <param name="myDrone"> This is the drone that the new waypoint should belong to (remember that you still need to add the waypoint to the path using this drone's methods) </param>
/// <param name="position"> This is the 3d location at which the new waypoint gameObject should be placed. </param>
public Waypoint(Drone myDrone, Vector3 position, bool takeoffWaypoint = false)
{
// Linking this waypoint to its drone
referenceDrone = myDrone;
// Setting up all the related gameObject parameters
GameObject baseObject = (GameObject)WorldProperties.worldObject.GetComponent <WorldProperties>().waypointBaseObject;
gameObjectPointer = Object.Instantiate(baseObject, position, Quaternion.identity);
gameObjectPointer.GetComponent <VRTK_InteractableObject>().ignoredColliders[0] = GameObject.Find("controller_right").GetComponent <SphereCollider>(); //Ignoring Collider from Controller so that WayPoint Zone is used
waypointProperties = gameObjectPointer.GetComponent <WaypointProperties>();
waypointProperties.classPointer = this; // Connect the gameObject back to the classObject
gameObjectPointer.tag = "waypoint";
gameObjectPointer.name = baseObject.name;
gameObjectPointer.transform.localScale = WorldProperties.actualScale / 100;
gameObjectPointer.transform.parent = WorldProperties.worldObject.transform;
WorldProperties.AddClipShader(gameObjectPointer.transform);
}
/// <summary>
/// Constructor method for Drone class objects
/// </summary>
/// <param name="drone_obj"> We pass in a Gameobject for the drone -- this will be phased out and the new drone_obj gameObject will be instantiated in this method </param>
public Drone(Vector3 position, int uniqueID)
{
// Create gameObject at position
GameObject baseObject = (GameObject)WorldProperties.worldObject.GetComponent <WorldProperties>().droneBaseObject;
gameObjectPointer = Object.Instantiate(baseObject, position, Quaternion.identity);
Debug.Log("Position init: " + position.ToString());
droneProperties = gameObjectPointer.GetComponent <DroneProperties>();
droneProperties.enabled = true;
droneProperties.droneClassPointer = this; // Connect the gameObject back to the classObject
droneProperties.selectedMeshRenderer = gameObjectPointer.transform.Find("group3/Outline").GetComponent <MeshRenderer>();
gameObjectPointer.tag = "Drone";
gameObjectPointer.name = baseObject.name;
gameObjectPointer.transform.localScale = WorldProperties.actualScale / 5;
gameObjectPointer.transform.parent = WorldProperties.worldObject.transform;
WorldProperties.AddClipShader(gameObjectPointer.transform);
// Initialize path and placement order lists
waypoints = new List <Waypoint>(); // All waypoints held by the drone
deletedWaypoints = new List <Waypoint>(); // All waypoints that were deleted, in case the player wants to redo them.
// Initialize the actions stack.
actions = new Stack <string>();
// Updating the world properties to reflect a new drone being added
id = uniqueID;
WorldProperties.AddDrone(this);
Debug.Log("Created new drone with id: " + id);
// Initilize the sensor list
// @Jasmine: this is populated by ROS Manager when initilzing the drone and can be used for the UI
// @Jasmine: feel free to add/remove functionality as needed, it's a very rough structure right now
attachedSensors = new List <ROSSensorConnectionInterface>();
// Init as unselected
gameObjectPointer.transform.Find("group3/Outline").GetComponent <MeshRenderer>().material = droneProperties.deselectedMaterial;
selected = false;
Debug.Log("Created new drone with id: " + id);
}
/******************************/
// Waypoint Flight Logic //
/******************************/
/// <summary>
/// To be called by ROSConnection when waypoints have been successfully uploaded.
/// Inform each waypoint that it has been uploaded for state change and user feedback.
/// </summary>
public void WaypointsUploaded(MissionWaypointMsg[] uploadedWaypoints)
{
foreach (MissionWaypointMsg waypoint in uploadedWaypoints)
{
Vector3 waypoint_coord = WorldProperties.GPSCoordToUnityCoord(new GPSCoordinate(waypoint.GetLatitude(), waypoint.GetLongitude(), waypoint.GetAltitude()));
// TODO: refine search to accound for order.
for (int i = 1; i < waypoints.Count; i++)
{
Waypoint unityWaypoint = waypoints[i];
float distance = Vector3.Distance(unityWaypoint.gameObjectPointer.transform.localPosition, waypoint_coord);
if (distance < 0.2f)
{
unityWaypoint.waypointProperties.WaypointUploaded();
}
}
}
}
void OnClickEvent()
{
WorldProperties.NewDrone();
}
// Fixed update is called n times per frame, where n is the physics step (and can be different from the progression of video frames).
void FixedUpdate()
{
// Query for the controller state, determined by user input.
switch (controllerState)
{
// If nothing is held down, default to the idle state.
case ControllerState.IDLE:
{
if (controllerInput.RightIsTouchingWaypoint())
{
controllerInput.HideWaypointPlacementVisualizer();
}
else
{
controllerInput.ShowWaypointPlacementVisualizer();
}
if (controllerInput.BothGrip())
{
controllerState = ControllerState.SCALING;
controllerInput.DisableBothUIs();
controllerInput.DisableRightPointer();
ScaleWorld();
break;
}
if (controllerInput.RightGrip())
{
controllerState = ControllerState.AIMING_SELECTOR;
controllerInput.DisableRightUI();
break;
}
if (controllerInput.RightTrigger())
{
controllerState = ControllerState.PLACING_WAYPOINT;
controllerInput.DisableRightPointer();
/// TODO: start linecollider
break;
}
if (controllerInput.RightA())
{
controllerState = ControllerState.UNDOING;
controllerInput.DisableRightPointer();
/// TODO: make waypoint slightly fade
break;
}
if (controllerInput.RightB())
{
controllerState = ControllerState.REDOING;
controllerInput.DisableRightPointer();
/// TODO: make waypoint slightly appear
break;
}
if (controllerInput.RightStickMoved()) /// Rotate the world
{
RotateWorld();
}
if (controllerInput.LeftStickMoved()) /// Move the world
{
MoveWorld();
}
if (controllerInput.LeftX()) /// Cycle through drones
{
controllerState = ControllerState.SELECTING_DRONE;
}
if (controllerInput.LeftY()) /// Cycle through sensors
{
controllerState = ControllerState.SELECTING_SENSOR;
}
break;
}
case ControllerState.SCALING:
{
if (controllerInput.BothGrip())
{
ScaleWorld();
}
else
{
controllerState = ControllerState.IDLE;
controllerInput.EnableBothUIs();
controllerInput.EnableRightPointer();
}
break;
}
case ControllerState.SELECTING_DRONE:
{
if (!controllerInput.LeftX())
{
controllerState = ControllerState.IDLE;
WorldProperties.SelectNextDrone();
}
break;
}
case ControllerState.SELECTING_SENSOR:
{
if (!controllerInput.LeftY())
{
controllerState = ControllerState.IDLE;
WorldProperties.sensorManager.ShowNextSensor();
}
break;
}
case ControllerState.AIMING_SELECTOR:
{
/// TODO: set wp height if GetRightIndex()
if (controllerInput.BothGrip())
{
controllerState = ControllerState.SCALING;
controllerInput.DisableBothUIs();
controllerInput.DisableRightPointer();
ScaleWorld();
break;
}
if (!controllerInput.RightGrip())
{
controllerState = ControllerState.IDLE;
controllerInput.EnableRightUI();
}
break;
}
case ControllerState.SETTING_WAYPOINT_HEIGHT:
{
break;
}
case ControllerState.PLACING_WAYPOINT:
{
if (controllerInput.RightIsGrabbingWaypoint())
{
controllerState = ControllerState.MOVING_WAYPOINT;
break;
}
if (controllerInput.RightGrip()) /// Cancel waypoint placement
{
/// TODO: stop showing line
controllerState = ControllerState.IDLE;
controllerInput.EnableRightPointer();
break;
}
if (!controllerInput.RightTrigger())
{
controllerState = ControllerState.IDLE;
Drone currentlySelectedDrone = WorldProperties.GetSelectedDrone();
currentlySelectedDrone.AddWaypoint(controllerInput.RightUITransform().position);
controllerInput.EnableRightPointer();
}
else
{
/// TODO: continue line showing and slightly faded wp
}
break;
}
case ControllerState.MOVING_WAYPOINT:
{
if (!controllerInput.RightIsGrabbingWaypoint())
{
controllerState = ControllerState.IDLE;
controllerInput.EnableRightPointer();
break;
}
break;
}
case ControllerState.UNDOING:
{
if (!controllerInput.RightA())
{
controllerState = ControllerState.IDLE;
Undo();
break;
}
if (controllerInput.RightGrip())
{
controllerState = ControllerState.IDLE;
controllerInput.EnableRightPointer();
/// TODO: make waypoint disappear
break;
}
break;
}
case ControllerState.REDOING:
{
if (!controllerInput.RightB())
{
controllerState = ControllerState.IDLE;
Redo();
break;
}
if (controllerInput.RightGrip())
{
controllerState = ControllerState.IDLE;
controllerInput.EnableRightPointer();
/// TODO: make waypoint disappear
break;
}
break;
}
}
}
