private void Update()
{
if (mustInit)
{
for (int i = toAdd.Count - 1; i >= 0; i--)
{
Marker m = toAdd[i];
markers[m.id] = (GameObject)Instantiate(marker, parent: transform, instantiateInWorldSpace: true);
markers[m.id].transform.position = TransformExtensions.GetPosition(m.pose);
Debug.Log(TransformExtensions.GetPosition(m.pose));
markers[m.id].transform.rotation = GetRotation(m.pose).Ros2Unity();
markers[m.id].GetComponent <Renderer>().material.color = m.color.toColor();
toAdd.RemoveAt(i);
TextToSpeechManager.Instance.beaconDetected();
}
mustInit = false;
}
if (mustUpdate)
{
for (int i = toUpdate.Count - 1; i >= 0; i--)
{
Marker m = toUpdate[i];
markers[m.id].transform.localPosition = TransformExtensions.GetPosition(m.pose);
markers[m.id].GetComponent <Renderer>().material.color = m.color.toColor();
Debug.Log("updated m");
}
mustUpdate = false;
}
}
public void OnInputClicked(InputClickedEventData eventData)
{
message.header.stamp.secs = (int)DateTime.Now.Subtract(new DateTime(1970, 1, 1, 0, 0, 0)).TotalSeconds;
Vector3 point = TransformExtensions.Unity2Ros(target.transform.position);
// point = Quaternion.Euler(0, -45, 0) * point;
point.z = 0;
message.pose.position.x = point.x;
message.pose.position.y = point.y;
message.pose.position.z = point.y;
publisher.message = message;
publisher.publishMessage();
}
