private static Vector3 getPosition(NavigationOdometry navigationOdometry)
{
return(new Vector3(
-navigationOdometry.pose.pose.position.y,
navigationOdometry.pose.pose.position.z,
navigationOdometry.pose.pose.position.x));
}
private Vector3 GetPosition(NavigationOdometry message)
{
return(new Vector3(
message.pose.pose.position.x,
message.pose.pose.position.y,
message.pose.pose.position.z));
}
private static Quaternion getRotation(NavigationOdometry navigationOdometry)
{
return(new Quaternion(
navigationOdometry.pose.pose.orientation.x,
-navigationOdometry.pose.pose.orientation.z,
navigationOdometry.pose.pose.orientation.y,
navigationOdometry.pose.pose.orientation.w));
}
private Quaternion GetRotation(NavigationOdometry message)
{
return(new Quaternion(
message.pose.pose.orientation.x,
message.pose.pose.orientation.y,
message.pose.pose.orientation.z,
message.pose.pose.orientation.w));
}
private void updateOdometry(Message message)
{
NavigationOdometry navigationOdometry = (NavigationOdometry)message;
odometryTransformManager.updateTransform(getPosition(navigationOdometry), getRotation(navigationOdometry));
}
