// 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 }