private Vector3 GetPosition(GeometryPoseStamped message)
{
return(new Vector3(
message.pose.position.x,
message.pose.position.y,
message.pose.position.z));
}
private Quaternion GetRotation(GeometryPoseStamped message)
{
return(new Quaternion(
message.pose.orientation.x,
message.pose.orientation.y,
message.pose.orientation.z,
message.pose.orientation.w));
}