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