private static void SetGeometryQuaternion(Quaternion quaternion, MessageTypes.Geometry.Quaternion message)
{
message.x = quaternion.x;
message.y = quaternion.y;
message.z = quaternion.z;
message.w = quaternion.w;
}
private static void GetGeometryQuaternion(Quaternion quaternion, MessageTypes.Geometry.Quaternion geometryQuaternion)
{
geometryQuaternion.x = quaternion.x;
geometryQuaternion.y = quaternion.y;
geometryQuaternion.z = quaternion.z;
geometryQuaternion.w = quaternion.w;
}
// Update is called once per frame
void Update()
{
if (receivedMsg)
{
receivedMsg = false;
RosSharp.RosBridgeClient.MessageTypes.Geometry.Vector3 newRosSharpPos = lastMsg.transform.translation;
RosSharp.RosBridgeClient.MessageTypes.Geometry.Quaternion newRosSharpRot = lastMsg.transform.rotation;
UnityEngine.Vector3 newPos = new UnityEngine.Vector3((float)newRosSharpPos.x, (float)newRosSharpPos.y, (float)newRosSharpPos.z);
UnityEngine.Quaternion newRot = new UnityEngine.Quaternion((float)newRosSharpRot.x, (float)newRosSharpRot.y, (float)newRosSharpRot.z, (float)newRosSharpRot.w);
if (useRos2Unity)
{
newPos = newPos.Ros2Unity();
newRot = newRot.Ros2Unity();
}
if (useLocalTransform)
{
transformToApply.localPosition = newPos;
transformToApply.localRotation = newRot;
}
else
{
transformToApply.position = newPos;
transformToApply.rotation = newRot;
}
}
}
private MessageTypes.Geometry.Quaternion GetGeometryQuaternion(Quaternion quaternion)
{
MessageTypes.Geometry.Quaternion geometryQuaternion = new MessageTypes.Geometry.Quaternion();
geometryQuaternion.x = quaternion.x;
geometryQuaternion.y = -quaternion.y;
geometryQuaternion.z = quaternion.z;
geometryQuaternion.w = quaternion.w;
return(geometryQuaternion);
}
private MessageTypes.Geometry.Quaternion GetGeometryQuaternion(Transform transform)
{
MessageTypes.Geometry.Quaternion geometryQuaternion = new MessageTypes.Geometry.Quaternion();
geometryQuaternion.x = transform.rotation.Unity2Ros().x;
geometryQuaternion.y = transform.rotation.Unity2Ros().y;
geometryQuaternion.z = transform.rotation.Unity2Ros().z;
geometryQuaternion.w = transform.rotation.Unity2Ros().w;
return(geometryQuaternion);
}
private MessageTypes.Geometry.Quaternion createQuaternion(Quaternion input)
{
MessageTypes.Geometry.Quaternion q = new MessageTypes.Geometry.Quaternion();
q.z = -input.y;
q.y = 0;
q.x = 0;
q.w = input.w;
return(q);
}
private MessageTypes.Geometry.Quaternion createQuaternionMsgFromYaw(float th)
{
Quaternion odom_quat = Quaternion.Euler(0, th * 180 / 3.14f, 0);
// Debug.Log(odom_quat);
MessageTypes.Geometry.Quaternion q = new MessageTypes.Geometry.Quaternion();
q.x = 0;
q.y = 0;
q.z = odom_quat.y;
q.w = odom_quat.w;
// Debug.Log(q);
return(q);
}
public static Quaternion QuaternionMsgToQuaternion(RosBridgeClient.MessageTypes.Geometry.Quaternion quaternionMsg)
{
return(new Quaternion((float)quaternionMsg.x, (float)quaternionMsg.y, (float)quaternionMsg.z, (float)quaternionMsg.w));
}
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 (Vector3, Quaternion) Ros2Unity(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);
}
return(posV, quatQ);
}
private static void GetGeometryQuaternion(Quaternion quaternion, Quaternion origin_quaternion, MessageTypes.Geometry.Quaternion geometryQuaternion)
{
// We don't do anything with the origin quaternion right now
// Quaternion reverse = Quaternion.Inverse(quaternion);
geometryQuaternion.x = quaternion.x;
geometryQuaternion.y = quaternion.y;
geometryQuaternion.z = quaternion.z;
geometryQuaternion.w = quaternion.w;
}
