private void Update()
{
if (Input.GetKeyDown(KeyCode.T))
{
ToastUtil.Toast(this, "Toast message.");
}
}
//called by some event
//Set the AR camera on real robot and do ResetFrame(), "Unity"TF will fixed with the offset of transform as PoseStamped message (in ROS side script)
public void ResetFrameBoth(Transform RefTransform = null)
{
GameObject go = new GameObject();
if (RefTransform is null)
{
go.transform.parent = ReferenceTransform;
}
else
{
go.transform.parent = RefTransform;
}
if (BasePlane.IsValid())
{
Vector3 normal = BasePlane.rotation * Vector3.down;
float b = Vector3.Dot(normal, BasePlane.position);
float a = Vector3.Dot(normal, ReferenceTransform.position);
go.transform.position = ReferenceTransform.position - (a - b) * normal;
//normal direction must be same as base plane.
normal = BasePlane.rotation * Vector3.up;
Vector3 rotVec = new Vector3(
ReferenceTransform.transform.rotation.x,
ReferenceTransform.transform.rotation.y,
ReferenceTransform.transform.rotation.z);
float r = Vector3.Dot(normal, rotVec);
go.transform.rotation = Quaternion.AxisAngle(normal, r);
}
else
{
go.transform.localPosition = -1 * ResetOffset;
go.transform.rotation = ReferenceTransform.rotation;
}
Matrix4x4 v = TargetTransform.localToWorldMatrix * go.transform.worldToLocalMatrix;
Vector3 p = v.GetColumn(3);
message.header.Update();
message.pose.position = GetGeometryPoint(p.Unity2Ros());
message.pose.orientation = GetGeometryQuaternion(v.rotation.Unity2Ros());
Publish(message);
ToastUtil.Toast(this, "Reset Frame.");
Debug.Log("Reset FrameBoth");
}
async void Update()
{
timeElapsed += Time.deltaTime;
if (timeElapsed < NtpSpan)
{
return;
}
timeElapsed = 0;
TimeSpan tmp;
bool isOk = false;
await Task.Run(() =>
{
try
{
NtpClient ntp;
ntp = new NtpClient(Dns.GetHostEntry(Host).AddressList[0], Port);
tmp = ntp.GetCorrectionOffset();
isOk = true;
}
catch (Exception e)
{
Debug.LogError("Ntp error:" + e.ToString());
isOk = false;
}
});
if (!isOk)
{
return;
}
if (!synced)
{
offset = tmp;
synced = true;
Debug.Log("NtpTime is started offset is " + offset.ToString());
ToastUtil.Toast(this, "NtpTime is started offset is " + offset.ToString());
}
else
{
offset = TimeSpan.FromSeconds(0.9 * offset.TotalSeconds + 0.1 * tmp.TotalSeconds);
}
}
