// Use this for initialization void Start() { lineRender = GetComponent <LineRenderer>(); lineRender.startWidth = 0.01f; lineRender.endWidth = 0.01f; lineRender.useWorldSpace = true; traj = new List <Vector3>(); lineRender.positionCount = 0; arrows = new List <GameObject>(); buffer = new ros.geometry_msgs.PoseArray(); Advertise("WaypointPub", "/hololens/navigation/waypoints", 5, out waypointPub); StartCoroutine(WaitForSpeechInit()); }
private IEnumerator WaitForSpeechInit() { yield return(new WaitUntil(() => RosGazeManager.Instance != null && RosUserSpeechManager.Instance != null)); RosUserSpeechManager.Instance.AddNewPhrase("Move here", () => { if (AddingMultipleWaypoints) { RosUserSpeechManager.Instance.voicebox.StartSpeaking("Removing previous set of waypoints"); AddingMultipleWaypoints = false; } traj.Clear(); buffer = new ros.geometry_msgs.PoseArray(); buffer.header.frame_id = "/Unity"; if (RosGazeManager.Instance.Focused) { Quaternion camRot = Camera.main.transform.rotation * Quaternion.Euler(0, -90, 0); Quaternion pointRot = Quaternion.Euler(0, camRot.eulerAngles.y, 0); ros.geometry_msgs.Pose newPoint = new ros.geometry_msgs.Pose(RosGazeManager.Instance.position, pointRot); buffer.poses.Add(newPoint); Publish(waypointPub, buffer); RosUserSpeechManager.Instance.voicebox.StartSpeaking("Moving to new location"); traj.Add(new Vector3(Camera.main.transform.position.x, Parameters.FloorDepth, Camera.main.transform.position.z)); traj.Add(newPoint.position.AsUnityVector); lineRender.positionCount = traj.Count; lineRender.SetPositions(traj.ToArray()); } else { RosUserSpeechManager.Instance.voicebox.StartSpeaking("Unable to set a waypoint, please scan the room first"); } }); RosUserSpeechManager.Instance.AddNewPhrase("Add point", () => { if (!AddingMultipleWaypoints) { buffer = new ros.geometry_msgs.PoseArray(); buffer.header.frame_id = "Unity"; AddingMultipleWaypoints = true; } if (RosGazeManager.Instance.Focused) { Quaternion pointRot; if (buffer.poses.Count > 0) { Vector3 prevPoint = buffer.poses[buffer.poses.Count - 1].position.AsUnityVector; Vector3 delta = (RosGazeManager.Instance.position - prevPoint).normalized; float angle = -(Mathf.Rad2Deg * Mathf.Atan2(delta.z, delta.x)); pointRot = Quaternion.Euler(0, angle, 0); } else { Quaternion camRot = Camera.main.transform.rotation * Quaternion.Euler(0, -90, 0); pointRot = Quaternion.Euler(0, camRot.eulerAngles.y, 0); } ros.geometry_msgs.Pose newPoint = new ros.geometry_msgs.Pose(RosGazeManager.Instance.position, pointRot); buffer.poses.Add(newPoint); traj.Clear(); traj.Add(new Vector3(Camera.main.transform.position.x, Parameters.FloorDepth, Camera.main.transform.position.z)); foreach (var p in buffer.poses) { traj.Add(p.position.AsUnityVector); } lineRender.positionCount = traj.Count; lineRender.SetPositions(traj.ToArray()); RosUserSpeechManager.Instance.voicebox.StartSpeaking("New waypoint set"); } else { RosUserSpeechManager.Instance.voicebox.StartSpeaking("Unable to set a waypoint"); } }); RosUserSpeechManager.Instance.AddNewPhrase("start moving", () => { if (AddingMultipleWaypoints) { Publish(waypointPub, buffer); AddingMultipleWaypoints = false; RosUserSpeechManager.Instance.voicebox.StartSpeaking("Moving along specified path"); } }); }