public static gm.Quaternion SetQuaternionFromRPY(double roll, double pitch, double yaw)
{
double halfRoll = roll / 2;
double halfPitch = pitch / 2;
double halfYaw = yaw / 2;
double sinRoll2 = Math.Sin(halfRoll);
double sinPitch2 = Math.Sin(halfPitch);
double sinYaw2 = Math.Sin(halfYaw);
double cosRoll2 = Math.Cos(halfRoll);
double cosPitch2 = Math.Cos(halfPitch);
double cosYaw2 = Math.Cos(halfYaw);
gm.Quaternion q = new gm.Quaternion();
q.x = cosPitch2 * sinRoll2 * cosYaw2 - sinPitch2 * cosRoll2 * sinYaw2;
q.y = cosPitch2 * sinRoll2 * sinYaw2 + sinPitch2 * cosRoll2 * cosYaw2;
q.z = cosPitch2 * cosRoll2 * sinYaw2 - sinPitch2 * sinRoll2 * cosYaw2;
q.w = cosPitch2 * cosRoll2 * cosYaw2 + sinPitch2 * sinRoll2 * sinYaw2;
return(q);
}
public emQuaternion(gm.Quaternion shallow)
: this(shallow.x, shallow.y, shallow.z, shallow.w)
{
}
public emTransform transformFrame(string source, string target, out gm.Vector3 vec, out gm.Quaternion quat)
{
emTransform trans = new emTransform();
try
{
transformer.lookupTransform(target, source, new Time(new TimeData()), out trans);
}
catch (Exception e)
{
ROS.Error(e.ToString());
trans = null;
}
if (trans != null)
{
vec = trans.translation != null?trans.translation.ToMsg() : new emVector3().ToMsg();
quat = trans.rotation != null?trans.rotation.ToMsg() : new emQuaternion().ToMsg();
}
else
{
vec = null;
quat = null;
}
return(trans);
}
private void SetDimensions(uint w, uint h, float res, Messages.geometry_msgs.Point position, Messages.geometry_msgs.Quaternion orientation)
{
pwidth = w;
pheight = h;
width = w * res;
height = h * res;
mapPos = new Vector3((float)position.x + (width / 2f), (float)position.y + (height / 2f), (float)position.z);
//mapRot = new Quaternion((float) orientation.x, (float) orientation.y, (float) orientation.z, (float) orientation.w);
mapScale = new Vector3(height, width, 1f);
}