// 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;
}
}
}
/// <summary>
/// Stores Link information as a Ros message so that it will be send the next time when the user initializes to the cage
/// </summary>
/// <param name="info">the Link info.</param>
public static void StoreLinkInformation(float[] info)
{
if (info.Length % linkInfoLength != 0)
{
Debug.LogWarning("Shape mismatch: Received array length " + info.Length + " not dividable by " +
linkInfoLength);
}
int numLinks = info.Length / linkInfoLength;
linkInfo = new LinkInformation[numLinks];
for (int i = 0; i < numLinks; i++)
{
int o = i * linkInfoLength;
int id = (int)(info[o]);
string linkName = linkNames[(int)(info[o + 1])];
RosSharp.RosBridgeClient.MessageTypes.Geometry.Vector3 dimensions =
new RosSharp.RosBridgeClient.MessageTypes.Geometry.Vector3(info[o + 2], info[o + 3], info[o + 4]);
Point point = new Point(info[o + 5], info[o + 6], info[o + 7]);
RosSharp.RosBridgeClient.MessageTypes.Geometry.Quaternion quaternion =
new RosSharp.RosBridgeClient.MessageTypes.Geometry.Quaternion(info[o + 8], info[o + 9], info[o + 10],
info[o + 11]);
Pose init_pose = new Pose(point, quaternion);
linkInfo[i] = new LinkInformation(id, linkName, dimensions, init_pose);
}
}
private MessageTypes.Geometry.Vector3 GetGeometryVector3(Vector3 position)
{
MessageTypes.Geometry.Vector3 geometryVector3 = new MessageTypes.Geometry.Vector3();
geometryVector3.x = position.x;
geometryVector3.y = position.y;
geometryVector3.z = position.z;
return(geometryVector3);
}
// private static MessageTypes.Geometry.Vector3 linearVelocityToGeometryVector3(Vector3 vector3)
// {
// MessageTypes.Geometry.Vector3 geometryVector3 = new MessageTypes.Geometry.Vector3();
// geometryVector3.x = vector3.x;
// geometryVector3.y = vector3.z;
// geometryVector3.z = 0;
// return geometryVector3;
// }
// private static MessageTypes.Geometry.Vector3 angularVelocityToGeometryVector3(Vector3 vector3)
// {
// MessageTypes.Geometry.Vector3 geometryVector3 = new MessageTypes.Geometry.Vector3();
// geometryVector3.x = 0;
// geometryVector3.y = 0;
// geometryVector3.z = -vector3.z;
// return geometryVector3;
// }
private MessageTypes.Geometry.Vector3 GetGeometryPoint(Vector3 position)
{
MessageTypes.Geometry.Vector3 geometryPoint = new MessageTypes.Geometry.Vector3();
geometryPoint.x = position.x;
geometryPoint.y = -position.y;
geometryPoint.z = position.z;
return(geometryPoint);
}
private static MessageTypes.Geometry.Vector3 angularVelocityToGeometryVector3(Vector3 vector3)
{
MessageTypes.Geometry.Vector3 geometryVector3 = new MessageTypes.Geometry.Vector3();
geometryVector3.x = 0;
geometryVector3.y = 0;
geometryVector3.z = -vector3.z;
return(geometryVector3);
}
private void GetVelocityLinear(Vector3 position, MessageTypes.Geometry.Vector3 geometryPoint, float dt)
{
geometryPoint.x = (position.x - previousPosition.x) / dt;
geometryPoint.y = (position.y - previousPosition.y) / dt;
geometryPoint.z = (position.z - previousPosition.z) / dt;
previousPosition = position;
}
private static MessageTypes.Geometry.Vector3 GetGeometryVector3(Vector3 vector3)
{
MessageTypes.Geometry.Vector3 geometryVector3 = new MessageTypes.Geometry.Vector3();
geometryVector3.x = vector3.x;
geometryVector3.y = vector3.y;
geometryVector3.z = vector3.z;
return(geometryVector3);
}
private static MessageTypes.Geometry.Vector3 linearVelocityToGeometryVector3(Vector3 vector3)
{
MessageTypes.Geometry.Vector3 geometryVector3 = new MessageTypes.Geometry.Vector3();
geometryVector3.x = vector3.x;
geometryVector3.y = vector3.z;
geometryVector3.z = 0;
return(geometryVector3);
}
private MessageTypes.Geometry.Vector3 GetAngularVelocity(Rigidbody rigidbody)
{
MessageTypes.Geometry.Vector3 result = new MessageTypes.Geometry.Vector3();
result.x = rigidbody.angularVelocity.Unity2Ros().x;
result.y = rigidbody.angularVelocity.Unity2Ros().y;
result.z = rigidbody.angularVelocity.Unity2Ros().z;
return(result);
}
private static void SubscriptionCmdVelHandler(geometry.Twist message)
{
geometry.Vector3 vel_linear = message.linear;
geometry.Vector3 vel_angular = message.angular;
vel_x = vel_linear.x;
vel_y = vel_linear.y;
vel_th = vel_angular.z;
}
private static MessageTypes.Geometry.Vector3 GetGeometryVector3Linear(Vector2 vector2)
{
MessageTypes.Geometry.Vector3 geometryVector3Linear = new MessageTypes.Geometry.Vector3();
geometryVector3Linear.x = vector2.y;
geometryVector3Linear.y = -vector2.x;
geometryVector3Linear.z = 0.0f;
return(geometryVector3Linear);
}
private MessageTypes.Geometry.Vector3 createTranslation(float x, float y, float z)
{
MessageTypes.Geometry.Vector3 v3 = new MessageTypes.Geometry.Vector3();
v3.x = x; //x;
v3.y = y; //y;
v3.z = 0; //z;
// Debug.Log(v3);
return(v3);
}
private static MessageTypes.Geometry.Vector3 GetGeometryVector3Angular(Vector2 vector2)
{
float maxAngularZ = 0.5f;
MessageTypes.Geometry.Vector3 geometryVector3Angular = new MessageTypes.Geometry.Vector3();
geometryVector3Angular.x = 0.0f;
geometryVector3Angular.y = 0.0f;
geometryVector3Angular.z = -(vector2.x * maxAngularZ);
return(geometryVector3Angular);
}
private void GetVelocityAngular(Quaternion quaternion, MessageTypes.Geometry.Vector3 geometryPoint, float dt)
{
float euler = quaternion.eulerAngles[2] * (Mathf.PI / 180);
float th = (euler - previousEuler) / dt;
geometryPoint.x = 0;
geometryPoint.y = 0;
geometryPoint.z = th;
previousEuler = euler;
}
private MessageTypes.Geometry.Vector3 GetLinearAcceleration(Transform transform, Rigidbody rigidbody)
{
// move element
Vector3 localLinearVelocity = transform.InverseTransformDirection(rigidbody.velocity);
Vector3 acceleration = (localLinearVelocity - LastVelocity) / deltaTime;
LastVelocity = localLinearVelocity;
//gravity element
acceleration += transform.InverseTransformDirection(Physics.gravity);
MessageTypes.Geometry.Vector3 result = new MessageTypes.Geometry.Vector3();
result.x = acceleration.Unity2Ros().x;
result.y = acceleration.Unity2Ros().y;
result.z = acceleration.Unity2Ros().z;
return(result);
}
private void updateTargetPos(geom_msgs.Vector3 msg)
{
Debug.Log(string.Format("Updated Pos: {0}", targetPos));
targetPos = new Vector3((float)msg.x, transform.position.y, (float)msg.z);
if (rotateFirst)
{
rotate = true;
move = false;
targetDirection = (targetPos - transform.position).normalized;
targetRotation = Quaternion.LookRotation(targetDirection);
}
else
{
move = true;
}
}
private static Vector3 ToVector3(RosSharp.RosBridgeClient.MessageTypes.Geometry.Vector3 geometryVector3)
{
return(new Vector3((float)geometryVector3.x, (float)geometryVector3.y, (float)geometryVector3.z));
}
public static Vector3 Vector3MsgToVector3(RosBridgeClient.MessageTypes.Geometry.Vector3 vector3Msg)
{
return(new Vector3((float)vector3Msg.x, (float)vector3Msg.y, (float)vector3Msg.z));
}
private static void GetGyro(MessageTypes.Geometry.Vector3 geometryQuaternion)
{
geometryQuaternion.x = RMPlugin.Gyro[0];
geometryQuaternion.y = RMPlugin.Gyro[1];
geometryQuaternion.z = RMPlugin.Gyro[2];
}
private static void GetAccel(MessageTypes.Geometry.Vector3 geometryQuaternion)
{
geometryQuaternion.x = RMPlugin.Acc[0];
geometryQuaternion.y = RMPlugin.Acc[1];
geometryQuaternion.z = RMPlugin.Acc[2];
}
// Modified.
private static MessageTypes.Geometry.Vector3 GetGeometryVector3(Vector3 vector3)
{
var geometryVector3 = new MessageTypes.Geometry.Vector3(vector3.x, vector3.y, vector3.z);
return(geometryVector3);
}
private static Vector3 linearVelocityToVector3(MessageTypes.Geometry.Vector3 geometryVector3)
{
return(new Vector3((float)geometryVector3.y, -(float)geometryVector3.x, 0));
}
private static Vector3 angularVelocityToVector3(MessageTypes.Geometry.Vector3 geometryVector3)
{
return(new Vector3(0, 0, -(float)geometryVector3.z));
}
private static Vector3 ToVector3(MessageTypes.Geometry.Vector3 geometryVector3)
{
return(new Vector3((float)geometryVector3.x, (float)geometryVector3.y, (float)geometryVector3.z));
}
private static void GetGeometryPoint(Vector3 position, Vector3 origin, Quaternion rotation, MessageTypes.Geometry.Vector3 translation)
{
//Quaternion reverse = Quaternion.Inverse(rotation);
//Vector3 newposition = reverse * position;
translation.x = (origin.x + position.x);
translation.y = (origin.y + position.y);
translation.z = 0; // no vertical translation
}
