private MessageTypes.Geometry.Point GetGeometryPoint(Vector3 position)
{
MessageTypes.Geometry.Point geometryPoint = new MessageTypes.Geometry.Point();
geometryPoint.x = position.x;
geometryPoint.y = -position.y;
geometryPoint.z = position.z;
return(geometryPoint);
}
private MessageTypes.Geometry.Point createPosition(float x, float y, float z)
{
MessageTypes.Geometry.Point v3 = new MessageTypes.Geometry.Point();
v3.x = x; //x;
v3.y = y; //y;
v3.z = 0; //z;
// Debug.Log(v3);
return(v3);
}
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);
}
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;
}
private static void GetGeometryPoint(Vector3 position, MessageTypes.Geometry.Point geometryPoint)
{
geometryPoint.x = position.x; //y in unity is z for robot
geometryPoint.y = position.y; //x in unity is y for robot
geometryPoint.z = position.z; //z in unity is x for robot
}
private static void GetGeometryPoint(Vector3 position, MessageTypes.Geometry.Point geometryPoint)
{
geometryPoint.x = position.x;
geometryPoint.y = position.y;
geometryPoint.z = position.z;
}
public static Vector3 PointMsgToVector3(RosBridgeClient.MessageTypes.Geometry.Point pointMsg)
{
return(new Vector3((float)pointMsg.x, (float)pointMsg.y, (float)pointMsg.z));
}
private static void SetGeometryPoint(Vector3 position, MessageTypes.Geometry.Point message)
{
message.x = position.x;
message.y = position.y;
message.z = position.z;
}
