/// <summary>
/// Use this to remove a waypoint from the path and from the scene
/// </summary>
/// <param name="deletedWaypoint"> The waypoint which is to be deleted </param>
public void DeleteWaypoint(Waypoint deletedWaypoint)
{
//Sending a ROS DELETE Update
string curr_id = deletedWaypoint.id;
string prev_id = deletedWaypoint.prevPathPoint.id;
float x = deletedWaypoint.gameObjectPointer.transform.localPosition.x;
float y = deletedWaypoint.gameObjectPointer.transform.localPosition.y;
float z = deletedWaypoint.gameObjectPointer.transform.localPosition.z;
UserpointInstruction msg = new UserpointInstruction(curr_id, prev_id, x, y, z, "DELETE");
WorldProperties.worldObject.GetComponent <ROSDroneConnection>().PublishWaypointUpdateMessage(msg);
// Removing the new waypoint from the dictionary, waypoints array and placement order
waypointsDict.Remove(deletedWaypoint.id);
waypoints.Remove(deletedWaypoint);
waypointsOrder.Remove(deletedWaypoint);
// Removing from the path linked list by adjusting the next and previous pointers of the surrounding waypoints
deletedWaypoint.prevPathPoint.nextPathPoint = deletedWaypoint.nextPathPoint;
// Need to check if this is the last waypoint in the list -- if it has a next or not
if (deletedWaypoint.nextPathPoint != null)
{
deletedWaypoint.nextPathPoint.prevPathPoint = deletedWaypoint.prevPathPoint;
}
// Removing line collider
WaypointProperties tempProperties = deletedWaypoint.gameObjectPointer.GetComponent <WaypointProperties>();
tempProperties.deleteLineCollider();
// Deleting the waypoint gameObject
Object.Destroy(deletedWaypoint.gameObjectPointer);
}
/// <summary>
/// This handles the primary placement states
/// </summary>
private void PrimaryPlacementChecks()
{
// Checks for right index Pressed
if (currentControllerState == ControllerState.IDLE && OVRInput.GetDown(OVRInput.Button.SecondaryIndexTrigger))
{
currentWaypoint = CreateWaypoint(placePoint.transform.position);
//Check to make sure we have successfully placed a waypoint
if (currentWaypoint != null)
{
currentControllerState = ControllerState.PLACING_WAYPOINT;
}
}
// Updates new waypoint location as long as the index is held
if (currentControllerState == ControllerState.PLACING_WAYPOINT)
{
currentWaypoint.gameObjectPointer.transform.position = placePoint.transform.position;
currentWaypoint.gameObjectPointer.GetComponent <WaypointProperties>().UpdateGroundpointLine();
}
// Releases the waypoint when the right index is released
if (currentControllerState == ControllerState.PLACING_WAYPOINT && OVRInput.GetUp(OVRInput.Button.SecondaryIndexTrigger))
{
UserpointInstruction msg = new UserpointInstruction(currentWaypoint, "MODIFY");
WorldProperties.worldObject.GetComponent <ROSDroneConnection>().PublishWaypointUpdateMessage(msg);
currentControllerState = ControllerState.IDLE;
}
}
/// <summary>
/// Handles the controller state switch to grabbing
/// </summary>
private void GrabbingChecks()
{
if (currentControllerState == ControllerState.IDLE &&
controller_right.GetComponent <VRTK_InteractGrab>().GetGrabbedObject() != null)
{
// Updating to note that we are currently grabbing a waypoint
grabbedWaypoint = controller_right.GetComponent <VRTK_InteractGrab>().GetGrabbedObject().GetComponent <WaypointProperties>().classPointer;
currentControllerState = ControllerState.GRABBING;
Debug.Log("Grabbing!");
}
else if (currentControllerState == ControllerState.GRABBING &&
controller_right.GetComponent <VRTK_InteractGrab>().GetGrabbedObject() == null)
{
// Updating the line colliders
grabbedWaypoint.UpdateLineColliders();
// Sending a ROS MODIFY Update
UserpointInstruction msg = new UserpointInstruction(grabbedWaypoint, "MODIFY");
WorldProperties.worldObject.GetComponent <ROSDroneConnection>().PublishWaypointUpdateMessage(msg);
// Updating the controller state and noting that we are not grabbing anything
grabbedWaypoint = null;
currentControllerState = ControllerState.IDLE;
}
}
/// <summary>
/// Use this to add a new Waypoint to the end of the drone's path
/// </summary>
/// <param name="newWaypoint"> The Waypoint which is to be added to the end of path </param>
public void AddWaypoint(Waypoint newWaypoint)
{
string prev_id;
// Check to see if we need to add the starter waypoint
if (waypoints.Count < 1)
{
//Creating the starter waypoint
Waypoint startWaypoint = new Waypoint(this, gameObjectPointer.transform.TransformPoint(new Vector3(0, 1, 0)));
// Otherwise, this is the first waypoint.
startWaypoint.prevPathPoint = null; // This means the previous point of the path is the Drone.
// Storing this for the ROS message
prev_id = "DRONE";
// Swapping the ids so the order makes sense
string tempId = startWaypoint.id;
startWaypoint.id = newWaypoint.id;
Debug.Log(startWaypoint.id);
newWaypoint.id = tempId;
// Adding to dictionary, order, and path list
waypointsDict.Add(startWaypoint.id, startWaypoint);
waypoints.Add(startWaypoint);
waypointsOrder.Add(startWaypoint);
// Send a special Userpoint message marking this as the start
UserpointInstruction msg = new UserpointInstruction(
startWaypoint.id, "DRONE", 0, 1, 0, "ADD");
WorldProperties.worldObject.GetComponent <ROSDroneConnection>().PublishWaypointUpdateMessage(msg);
}
else
{
// Otherwise we can add as normal
Waypoint prevWaypoint = (Waypoint)waypoints[waypoints.Count - 1]; // Grabbing the waypoint at the end of our waypoints path
newWaypoint.prevPathPoint = prevWaypoint; // setting the previous of the new waypoint
prevWaypoint.nextPathPoint = newWaypoint; // setting the next of the previous waypoint
// Storing this for the ROS message
prev_id = prevWaypoint.id;
// Adding to dictionary, order, and path list
waypointsDict.Add(newWaypoint.id, newWaypoint);
waypoints.Add(newWaypoint);
waypointsOrder.Add(newWaypoint);
}
// Send a generic ROS ADD Update only if this is not the initial waypoint
if (prev_id != "DRONE")
{
UserpointInstruction msg = new UserpointInstruction(newWaypoint, "ADD");
WorldProperties.worldObject.GetComponent <ROSDroneConnection>().PublishWaypointUpdateMessage(msg);
}
else
{
// Otherwise we have just set the starter waypoint and still need to create the real waypoint
this.AddWaypoint(newWaypoint);
}
}
public void tappedHandler(object sender, EventArgs e)
{
// Can only add if in Add_State.
if (GameObject.Find("Master").GetComponent <StateHandler> ().add_state)
{
var gesture = sender as TapGesture;
HitData hit = gesture.GetScreenPositionHitData();
//print ("Tapped Position: " + hit.Point);
// Switch Add_State off.
GameObject master = GameObject.Find("Master");
master.GetComponent <StateHandler> ().add_state = false;
StartCoroutine(WaitHeight(hit));
}
// Delete Functionality.
if (GameObject.Find("Master").GetComponent <StateHandler> ().delete_state)
{
var gesture = sender as TapGesture;
HitData hit = gesture.GetScreenPositionHitData();
Vector3 worldHitPoint = new Vector3(hit.Point.x, 2f, hit.Point.z);
Vector3 origin = new Vector3(worldHitPoint.x, 3f, worldHitPoint.z);
RaycastHit h = new RaycastHit();
Ray r = new Ray(origin, Camera.main.transform.forward);
// Debug.DrawRay (origin, Camera.main.transform.forward,Color.red, 100, true);
if (Physics.Raycast(origin, Camera.main.transform.forward))
{
if (Physics.Raycast(r, out h))
{
if (h.collider.gameObject.CompareTag("Waypoint"))
{
GameObject waypoint = h.collider.gameObject;
if (!waypoint.GetComponent <Waypoint>().WaypointComplete)
{
// ROS COMMUNICATION
string prevID = GetPrevID(waypoint.name, "delete");
float worldX = waypoint.transform.position.x;
float worldZ = waypoint.transform.position.z;
float height = waypoint.GetComponent <Waypoint>().worldPos.y;
UserpointInstruction msg = new UserpointInstruction(waypoint.name, prevID, worldX, height, worldZ, "DELETE");
GameObject.Find("Master").GetComponent <ROSDroneConnection>().PublishWaypointUpdateMessage(msg);
DeleteHouseKeeping(waypoint);
// Switch delete_state off.
GameObject master = GameObject.Find("Master");
master.GetComponent <StateHandler>().delete_state = false;
}
}
}
}
}
}
private void SendROSModifyMessage()
{
//TODO: Determine Why Tom has height being set to worldPos on edit (Kind of breaks the ability to edit)
WorldScript ws = GameObject.Find("World").GetComponent <WorldScript>();
string prevID = ws.GetPrevID(this.name, "modify");
float worldX = this.transform.localPosition.x;
float worldZ = this.transform.localPosition.z;
float height = this.worldPos.y;
UserpointInstruction msg = new UserpointInstruction(this.name, prevID, worldX, height, worldZ, "MODIFY");
GameObject.Find("Master").GetComponent <ROSDroneConnection>().PublishWaypointUpdateMessage(msg);
}
private void longPressHandler(object sender, EventArgs e)
{
// Delete Functionality.
var gesture = sender as LongPressGesture;
HitData hit = gesture.GetScreenPositionHitData();
// Debug.Log (hit);
Vector3 worldHitPoint = new Vector3(hit.Point.x, 2f, hit.Point.z);
// Debug.Log (worldHitPoint);
// Raycast Approach
Vector3 origin = new Vector3(worldHitPoint.x, 3f, worldHitPoint.z);
RaycastHit h = new RaycastHit();
Ray r = new Ray(origin, Camera.main.transform.forward);
// Debug.DrawRay (origin, Camera.main.transform.forward,Color.red, 100, true);
if (Physics.Raycast(origin, Camera.main.transform.forward))
{
if (Physics.Raycast(r, out h))
{
if (h.collider.gameObject.CompareTag("Waypoint"))
{
GameObject waypoint = h.collider.gameObject;
if (!waypoint.GetComponent <Waypoint>().WaypointComplete)
{
// ROS COMMUNICATION
string prevID = GetPrevID(waypoint.name, "delete");
float worldX = waypoint.transform.position.x;
float worldZ = waypoint.transform.position.z;
float height = waypoint.GetComponent <Waypoint>().worldPos.y;
UserpointInstruction msg = new UserpointInstruction(waypoint.name, prevID, worldX, height, worldZ, "DELETE");
GameObject.Find("Master").GetComponent <ROSDroneConnection>().PublishWaypointUpdateMessage(msg);
DeleteHouseKeeping(waypoint);
}
}
}
}
}
/// <summary>
/// Use this to insert a new waypoint into the path (between two existing waypoints)
/// </summary>
/// <param name="newWaypoint"> The Waypoint which is to be added to the path </param>
/// <param name="prevWaypoint"> The existing Waypoint just before the one which is to be added to the path </param>
public void InsertWaypoint(Waypoint newWaypoint, Waypoint prevWaypoint)
{
// Adding the new waypoint to the dictionary and placement order
waypointsDict.Add(newWaypoint.id, newWaypoint);
waypointsOrder.Add(newWaypoint);
// Adding the waypoint to the array
int previousIndex = Mathf.Max(0, waypoints.IndexOf(prevWaypoint));
int newIndex = previousIndex + 1;
waypoints.Insert(newIndex, newWaypoint);
// Inserting into the path linked list by adjusting the next and previous pointers of the surrounding waypoints
newWaypoint.prevPathPoint = prevWaypoint;
newWaypoint.nextPathPoint = prevWaypoint.nextPathPoint;
newWaypoint.prevPathPoint.nextPathPoint = newWaypoint;
newWaypoint.nextPathPoint.prevPathPoint = newWaypoint;
//Sending a ROS INSERT Update
UserpointInstruction msg = new UserpointInstruction(newWaypoint, "INSERT");
WorldProperties.worldObject.GetComponent <ROSDroneConnection>().PublishWaypointUpdateMessage(msg);
}
public void PublishWaypointUpdateMessage(UserpointInstruction msg)
{
Debug.Log("Published new userpoint instruction: " + msg.ToYAMLString());
ros.Publish(UserpointPublisher.GetMessageTopic(), msg);
}
public static string ToYAMLString(UserpointInstruction msg)
{
return(msg.ToYAMLString());
}
IEnumerator WaitHeight(HitData hit)
{
// Instantiating waypoint in right place.
var waypoint = Instantiate(WaypointPrefab) as Transform;
waypoint.parent = Container;
String id = CreateWaypointID("A");
waypoint.name = id;
// Add more components to waypoint.
waypoint.gameObject.AddComponent <TapGesture>();
waypoint.gameObject.AddComponent <TransformGesture>();
waypoint.gameObject.AddComponent <Waypoint> ();
waypoint.gameObject.AddComponent <Transformer> ();
waypoint.localScale = Vector3.one * Scale;// * waypoint.localScale.x;
waypoint.position = new Vector3(hit.Point.x, 2f, hit.Point.z);
waypoint.gameObject.GetComponent <Waypoint>().setFlatPos(waypoint.position);
waypoint.gameObject.GetComponent <Waypoint>().setLastPos(waypoint.position);
waypoint.gameObject.GetComponent <Waypoint>().setCurrPos(waypoint.position);
AddWaypointHouseKeeping(waypoint.name);
// Waiting player for height.
print("Waiting for a height.");
yield return(new WaitUntil(() => TempHeight != 0));
AddWaypointUndoHouseKeeping(waypoint.name);
print("Height received.");
// Hit coordinates are unreliable...
float worldX = waypoint.transform.localPosition.x;
float worldZ = waypoint.transform.localPosition.z;
//Paxtan: Storing the waypoint with the feet units in the Y value because it displays the waypoint value after letting go of the slider
float correctHeight;
if (ButtonHelper.Units == "Metric")
{
correctHeight = TempHeight * 3.05f;
}
else
{
correctHeight = TempHeight * 10f;
}
Debug.Log(TempHeight);
waypoint.gameObject.GetComponent <Waypoint> ().setWorldPos(new Vector3(worldX, correctHeight, worldZ));
waypoint.gameObject.GetComponent <Waypoint>().SetID(id);
waypoint.GetComponent <Waypoint>().CreateHoverText();
string prevID = GetPrevID(waypoint.name, "add");
// ROS COMMUNICATION
float ROSHeight = TempHeight * 3.05f - 0.148f;
UserpointInstruction msg = new UserpointInstruction(waypoint.name, prevID, worldX, ROSHeight, worldZ, "ADD");
GameObject.Find("Master").GetComponent <ROSDroneConnection>().PublishWaypointUpdateMessage(msg);
// Could be buggy to only add the GameObject.
wayPoints[currPointIndex] = waypoint.gameObject;
currPointIndex += 1;
numWaypoints += 1;
// print ("Stored point: " + store);
// Get waypoint.
waypoint.gameObject.AddComponent <SphereCollider> ();
// Color filled = new Color(0.278F, 0.874F, 1F, 0.9F);
// waypoint.GetComponent<Renderer>().material.color = filled;
// Housekeeping
TempHeight = 0f;
}
