// 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");
}
});
}
