Esempio n. 1
0
    // 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;
            }
        }
    }
Esempio n. 2
0
    /// <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);
        }
    }
Esempio n. 3
0
 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);
 }
Esempio n. 4
0
        // 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;
        }
Esempio n. 7
0
 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);
 }
Esempio n. 9
0
 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);
 }
Esempio n. 10
0
 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);
 }
Esempio n. 12
0
 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;
        }
Esempio n. 15
0
        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);
        }
Esempio n. 16
0
    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));
 }
Esempio n. 18
0
 public static Vector3 Vector3MsgToVector3(RosBridgeClient.MessageTypes.Geometry.Vector3 vector3Msg)
 {
     return(new Vector3((float)vector3Msg.x, (float)vector3Msg.y, (float)vector3Msg.z));
 }
Esempio n. 19
0
 private static void GetGyro(MessageTypes.Geometry.Vector3 geometryQuaternion)
 {
     geometryQuaternion.x = RMPlugin.Gyro[0];
     geometryQuaternion.y = RMPlugin.Gyro[1];
     geometryQuaternion.z = RMPlugin.Gyro[2];
 }
Esempio n. 20
0
 private static void GetAccel(MessageTypes.Geometry.Vector3 geometryQuaternion)
 {
     geometryQuaternion.x = RMPlugin.Acc[0];
     geometryQuaternion.y = RMPlugin.Acc[1];
     geometryQuaternion.z = RMPlugin.Acc[2];
 }
Esempio n. 21
0
        // Modified.
        private static MessageTypes.Geometry.Vector3 GetGeometryVector3(Vector3 vector3)
        {
            var geometryVector3 = new MessageTypes.Geometry.Vector3(vector3.x, vector3.y, vector3.z);

            return(geometryVector3);
        }
Esempio n. 22
0
 private static Vector3 linearVelocityToVector3(MessageTypes.Geometry.Vector3 geometryVector3)
 {
     return(new Vector3((float)geometryVector3.y, -(float)geometryVector3.x, 0));
 }
Esempio n. 23
0
 private static Vector3 angularVelocityToVector3(MessageTypes.Geometry.Vector3 geometryVector3)
 {
     return(new Vector3(0, 0, -(float)geometryVector3.z));
 }
Esempio n. 24
0
 private static Vector3 ToVector3(MessageTypes.Geometry.Vector3 geometryVector3)
 {
     return(new Vector3((float)geometryVector3.x, (float)geometryVector3.y, (float)geometryVector3.z));
 }
Esempio n. 25
0
 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
 }