private void ReportNewAsaRelPose(Odometry relPose)
{
var anchor = missioncontroller.GetAnchorControllerById(relPose.header.frame_id);
if (anchor == null)
{
Debug.Log("Anchor for pose was null");
return;
}
TransformHelper.transform.SetParent(anchor.transform, false);
var position = relPose.pose.pose.position;
var orientation = relPose.pose.pose.orientation;
(var pos, var or) = PoseExtensions.Ros2Unity(position, orientation);
TransformHelper.transform.localPosition = pos;
TransformHelper.transform.localRotation = or;
if (anchor != null)
{
//Calculate the relative position to the current anchor
//pos = anchor.transform.InverseTransformPoint(TransformHelper.transform.position);
//or = (Quaternion.Inverse(anchor.transform.rotation) * TransformHelper.transform.rotation);
robotState.Pose.SetFromPosition(pos);
robotState.Pose.SetFromRotation(or);
}
else
{
//we do not have a current anchor yet, so assume that the anchor is still the one at origin
robotState.Pose.SetFromRosSharp(position, orientation);
}
//Animate the robot's base odometry
if (robotModelActive)
{
RobotBase.transform.SetParent(anchor.transform, false);
RobotBase.transform.localPosition = pos;
RobotBase.transform.localRotation = or;
}
//if(targetPose != null)
//{
// Debug.Log($"{robotState.Pose.X - targetPose.X} | {robotState.Pose.Z - targetPose.Z}");
//}
Robot.Delegates.UpdateRobotState(robotState);
}
private void UpdateStereoPerspective()
{
Camera camera = this.Camera;
// Apply camera settings to the frustum.
this._frustum.NearClip = camera.nearClipPlane;
this._frustum.FarClip = camera.farClipPlane;
this._frustum.CameraOffset = this._cameraOffset;
this._frustum.Ipd = Mathf.Lerp(
0, camera.stereoSeparation, this._stereoWeight);
this._frustum.HeadPose = PoseExtensions.Lerp(
this._frustum.DefaultHeadPose,
this._headTarget.Pose,
this._stereoWeight);
// Update the camera's view matrices for the
// center, left, and right eyes.
camera.transform.SetLocalPose(this.GetLocalPose(ZEye.Center));
camera.SetStereoViewMatrix(
Camera.StereoscopicEye.Left,
this._frustum.GetViewMatrix(ZEye.Left, this.WorldScale) *
this._monoWorldToCameraMatrix);
camera.SetStereoViewMatrix(
Camera.StereoscopicEye.Right,
this._frustum.GetViewMatrix(ZEye.Right, this.WorldScale) *
this._monoWorldToCameraMatrix);
// Update the camera's projection matrices for the
// center, left, and right eyes.
camera.projectionMatrix =
this._frustum.GetProjectionMatrix(ZEye.Center);
camera.SetStereoProjectionMatrix(
Camera.StereoscopicEye.Left,
this._frustum.GetProjectionMatrix(ZEye.Left));
camera.SetStereoProjectionMatrix(
Camera.StereoscopicEye.Right,
this._frustum.GetProjectionMatrix(ZEye.Right));
}
void Update()
{
var pose = PoseExtensions.Slerp(transform.Pose(), other.Pose(), trackingSpeed);
transform.SetPositionAndRotation(pose);
}