public static void SetFromRosSharp(this RobotUtilities.IPose pose, RosSharp.RosBridgeClient.MessageTypes.Geometry.Point pos, RosSharp.RosBridgeClient.MessageTypes.Geometry.Quaternion quat, bool useRos2Unity = true)
{
Vector3 posV = new Vector3((float)pos.x, (float)pos.y, (float)pos.z);
Quaternion quatQ = new Quaternion((float)quat.x, (float)quat.y, (float)quat.z, (float)quat.w);
if (useRos2Unity)
{
posV = RosSharp.TransformExtensions.Ros2Unity(posV);
quatQ = RosSharp.TransformExtensions.Ros2Unity(quatQ);
}
pose.X = posV.x;
pose.Y = posV.y;
pose.Z = posV.z;
pose.i = quatQ.x;
pose.j = quatQ.y;
pose.k = quatQ.z;
pose.w = quatQ.w;
}
public static void SetFromTransform(this RobotUtilities.IPose pose, Transform transform, bool useLocal = true)
{
if (useLocal)
{
pose.X = transform.localPosition.x;
pose.Y = transform.localPosition.y;
pose.Z = transform.localPosition.z;
pose.i = transform.localRotation.x;
pose.j = transform.localRotation.y;
pose.k = transform.localRotation.z;
pose.w = transform.localRotation.w;
}
else
{
pose.X = transform.position.x;
pose.Y = transform.position.y;
pose.Z = transform.position.z;
pose.i = transform.rotation.x;
pose.j = transform.rotation.y;
pose.k = transform.rotation.z;
pose.w = transform.rotation.w;
}
}
public static void SetFromTransformRotation(this RobotUtilities.IPose pose, Transform transform)
{
SetFromRotation(pose, transform.rotation);
}
public static Quaternion ToRotationQuaternion(this RobotUtilities.IPose pose)
{
return(new Quaternion((float)pose.i, (float)pose.j, (float)pose.k, (float)pose.w));
}
public static Vector3 ToPositionVector(this RobotUtilities.IPose pose)
{
return(new Vector3((float)pose.X, (float)pose.Y, (float)pose.Z));
}