Пример #1
0
    IEnumerator SendImu()
    {
        yield return(new WaitForEndOfFrame());

        var now            = DateTime.Now;
        var timeSpan       = now - lastFrame;
        var timeSinceStart = now - camStart;
        var timeMessage    = new TimeMsg(timeSinceStart.Seconds, timeSinceStart.Milliseconds);
        var headerMessage  = new HeaderMsg(count, timeMessage, "imu");

        Quaternion vehicleImu;

        vehicleImu = vehicle.transform.rotation;
        double xey     = vehicleImu.x;
        double yey     = vehicleImu.y;
        double zey     = vehicleImu.z;
        double wey     = vehicleImu.w;
        var    imuData = new QuaternionMsg(xey, yey, zey, wey);

        double[] arg3 = { 0.00, 0.00, 0.00 };

        var vect3 = new Vector3Msg(0.00, 0.00, 0.00);

        var imumessage = new ImuMessage(headerMessage, imuData, arg3, vect3, arg3, vect3, arg3);

        ros.Publish(imuPublisher.GetMessageTopic(), imumessage);
        ros.Render();
    }
Пример #2
0
    void Update()
    {
        ros.Render();

        if (!_running)
        {
            return;
        }

        timer -= Time.deltaTime;

        if (timer <= 0)
        {
            timer = pollRate;

            PointMsg               point  = new PointMsg(1, 2, 3);
            QuaternionMsg          quat   = new QuaternionMsg(1, 2, 3, 4);
            PoseMsg                pose   = new PoseMsg(point, quat);
            PoseWithCovarianceMsg  posec  = new PoseWithCovarianceMsg(pose);
            Vector3Msg             vec3   = new Vector3Msg(1, 2, 3);
            TwistMsg               twist  = new TwistMsg(vec3, vec3);
            TwistWithCovarianceMsg twistc = new TwistWithCovarianceMsg(twist, new double[36]);
            HeaderMsg              header = new HeaderMsg(1, new TimeMsg(1, 1), "0");

            PoseStampedMsg ps  = new PoseStampedMsg(header, pose);
            PathMsg        msg = new PathMsg(header, new PoseStampedMsg[] { ps, ps, ps });

            BoolMsg             boolmsg = new BoolMsg(true);
            StringMsg           str     = new StringMsg("This is a test");
            RegionOfInterestMsg roi     = new RegionOfInterestMsg(0, 1, 2, 3, true);
            CameraInfoMsg       caminfo = new CameraInfoMsg(header, 100, 200, "plumb_bob", new double[5], new double[9], new double[9], new double[12], 14, 123, roi);

            _genericPub.PublishData(caminfo);
        }
    }
        public static Quaternion From(this QuaternionMsg self, CoordinateSpaceSelection selection)
        {
            switch (selection)
            {
            case CoordinateSpaceSelection.RUF:
                return(self.From <RUF>());

            case CoordinateSpaceSelection.FLU:
                return(self.From <FLU>());

            case CoordinateSpaceSelection.FRD:
                return(self.From <FRD>());

            case CoordinateSpaceSelection.ENU:
                return(self.From <ENU>());

            case CoordinateSpaceSelection.NED:
                return(self.From <NED>());

            case CoordinateSpaceSelection.ENULocal:
                return(self.From <ENULocal>());

            case CoordinateSpaceSelection.NEDLocal:
                return(self.From <NEDLocal>());

            default:
                Debug.LogError("Invalid coordinate space " + selection);
                return(self.From <RUF>());
            }
        }
    public new static void CallBack(ROSBridgeMsg msg)
    {
        OccupancyGridMsg   occupancy = (OccupancyGridMsg)msg;
        MapMetaDataInfoMsg meta      = occupancy.GetInfo();

        int           width = meta.GetWidth(), height = meta.GetHeight();
        float         resolution = meta.GetResolution();
        PointMsg      origin     = meta.GetOrigin().GetPosition();
        QuaternionMsg pose       = meta.GetOrigin().GetOrientation();

        Debug.Log("Width: " + width + "\nHeight: " + height + "\nResolution: " + resolution);

        RawImage  texObj = GameObject.Find(objectName).GetComponent <RawImage>();
        Texture2D tex    = texObj.texture as Texture2D;

        if (tex == null || tex.width != width || tex.height != height)
        {
            tex            = new Texture2D(width, height, TextureFormat.RGBA32, false);
            texObj.texture = tex;
        }

        NativeArray <Color32> pixels = tex.GetRawTextureData <Color32>();

        byte[] raw = occupancy.GetData();
        for (int i = 0; i < raw.Length; i++)
        {
            pixels[i] = new Color32((byte)(255 - raw[i]), (byte)(255 - raw[i]), (byte)(255 - raw[i]), (byte)Mathf.Min(255, 350 - raw[i]));
        }

        tex.Apply();
    }
Пример #5
0
 public static Quaternion ToQuaternion(this QuaternionMsg quaternionMsg)
 {
     return(new Quaternion(
                quaternionMsg.GetX(),
                quaternionMsg.GetZ(),
                quaternionMsg.GetY(),
                quaternionMsg.GetW()));
 }
Пример #6
0
 public static Quaternion ToQuaternion(this QuaternionMsg quaternionMsg)
 {
     return(new Quaternion(
                -quaternionMsg.GetY(),
                quaternionMsg.GetZ(),
                quaternionMsg.GetX(),
                -quaternionMsg.GetW())); // negating because ROS is right handed and Unity is left handed
 }
Пример #7
0
            public PoseMsg(Transform tf)
            {
                Vector3 position    = tf.position;
                Vector3 orientation = tf.rotation.eulerAngles;

                _position    = new PointMsg(position.x, position.z, position.y);
                _orientation = new QuaternionMsg(orientation.x, orientation.y, orientation.z);
            }
Пример #8
0
 public static void GUI(this QuaternionMsg message, string label)
 {
     if (label != "" && label != null)
     {
         label += ": ";
     }
     GUILayout.Label($"{label}[{message.x:F2}, {message.y:F2}, {message.z:F2}, {message.w:F2}]");
 }
Пример #9
0
 public ImuMessage(HeaderMsg header, QuaternionMsg quaternionIMU, double[] orientation_c, Vector3Msg angularVelocity, double[] angular_velocity_c, Vector3Msg linearAcceleration, double[] linear_acceleration_c)
 {
     _header                = header;
     _quaternionIMU         = quaternionIMU;
     _orientation_c         = orientation_c;
     _angularVelocity       = angularVelocity;
     _angular_velocity_c    = angular_velocity_c;
     _linearAcceleration    = linearAcceleration;
     _linear_acceleration_c = linear_acceleration_c;
 }
Пример #10
0
    public void sendPingerPos()
    {
        Vector3    posTmp = pinger.transform.position;
        Quaternion rotTmp = pinger.transform.rotation;

        pingerPoint      = new PointMsg(posTmp.x, posTmp.y, posTmp.z);
        pingerQuaternion = new QuaternionMsg(rotTmp.x, rotTmp.y, rotTmp.z, rotTmp.w);
        pingerMsg        = new PoseMsg(pingerPoint, pingerQuaternion);
        obj.GetComponent <ROSInitializerSAUVC>().rosSAUVC.Publish(PingerPublisher.GetMessageTopic(), pingerMsg);
    }
Пример #11
0
 public void SendPos(int id)
 {
     if (id != 0 && id <= props.Length)
     {
         Vector3    posTmp = props[id - 1].transform.position;
         Quaternion rotTmp = props[id - 1].transform.rotation;
         pt     = new PointMsg(posTmp.x, posTmp.y, posTmp.z);
         qt     = new QuaternionMsg(rotTmp.x, rotTmp.y, rotTmp.z, rotTmp.w);
         posMsg = new PoseMsg(pt, qt);
         obj.GetComponent <ROSInitializerSAUVC>().rosSAUVC.Publish(PosPublisher.GetMessageTopic(), posMsg);
     }
 }
Пример #12
0
            public ImuMessage(JSONNode msg)
            {
                _header        = new HeaderMsg(msg ["header"]);
                _quaternionIMU = new QuaternionMsg(msg ["orientation"]);

                /*
                 * for (int i = 0; i < 8; i++) {
                 *      _orientation_c [i] = double.Parse (msg ["orientation_covariance"] [i]);
                 *      _angular_velocity_c [i]= double.Parse (msg ["angular_velocity_covariance"][i]);
                 *      _linear_acceleration_c [i]= double.Parse (msg ["linear_acceleration_covariance"][i]);
                 * }
                 */
                _angularVelocity    = new Vector3Msg(msg ["angular_velocity"]);
                _linearAcceleration = new Vector3Msg(msg ["linear_acceleration"]);
                //Debug.Log ("IMU Message Done");
            }
Пример #13
0
 public PoseMsg(PointMsg _translation, QuaternionMsg _rotation)
 {
     position    = _translation;
     orientation = _rotation;
 }
Пример #14
0
 public PoseMsg(JSONNode msg)
 {
     position    = new PointMsg(msg["position"]);
     orientation = new QuaternionMsg(msg["orientation"]);
 }
 public TransformMsg(Transform tf)
 {
     _translation = new Vector3Msg(tf.localPosition, true);
     _rotation    = new QuaternionMsg(tf.localRotation, true);
 }
Пример #16
0
 public PoseMsg(Vector3 position, Quaternion orientation)
 {
     _position    = new PointMsg(position);
     _orientation = new QuaternionMsg(orientation);
 }
Пример #17
0
 public PoseMsg(PointMsg position, QuaternionMsg orientation)
 {
     _position    = position;
     _orientation = orientation;
 }
 public static Quaternion From <C>(this QuaternionMsg self) where C : ICoordinateSpace, new()
 {
     return(new Quaternion <C>((float)self.x, (float)self.y, (float)self.z, (float)self.w).toUnity);
 }
Пример #19
0
 public PoseMsg(Transform _tf)
 {
     this.position    = new PointMsg(_tf.position, true);
     this.orientation = new QuaternionMsg(_tf.localRotation, true);
 }
 public TransformMsg(JSONNode msg)
 {
     _translation = new Vector3Msg(msg["translation"]);
     _rotation    = new QuaternionMsg(msg["rotation"]);
 }
Пример #21
0
 public static void GUI(this QuaternionMsg message)
 {
     GUILayout.Label($"[{message.x:F2}, {message.y:F2}, {message.z:F2}, {message.w:F2}]");
 }
Пример #22
0
 public PoseMsg(PointMsg translation, QuaternionMsg rotation)
 {
     _position    = translation;
     _orientation = rotation;
 }
Пример #23
0
        public static void DrawAxisVectors <C>(Drawing3d drawing, Vector3Msg position, QuaternionMsg rotation, float size, bool drawUnityAxes) where C : ICoordinateSpace, new()
        {
            Vector3    unityPosition = position.From <C>();
            Quaternion unityRotation = rotation.From <C>();
            Vector3    x, y, z;

            if (drawUnityAxes)
            {
                x = unityRotation * new Vector3(1, 0, 0) * size;
                y = unityRotation * new Vector3(0, 1, 0) * size;
                z = unityRotation * new Vector3(0, 0, 1) * size;
            }
            else
            {
                x = unityRotation * new Vector3 <C>(1, 0, 0).toUnity *size;
                y = unityRotation * new Vector3 <C>(0, 1, 0).toUnity *size;
                z = unityRotation * new Vector3 <C>(0, 0, 1).toUnity *size;
            }
            drawing.DrawLine(unityPosition, unityPosition + x, Color.red, size * 0.1f);
            drawing.DrawLine(unityPosition, unityPosition + y, Color.green, size * 0.1f);
            drawing.DrawLine(unityPosition, unityPosition + z, Color.blue, size * 0.1f);
        }
Пример #24
0
 public override void Deserialize(JSONNode msg)
 {
     _position    = new PointMsg(msg["position"]);
     _orientation = new QuaternionMsg(msg["orientation"]);
 }
Пример #25
0
 public PoseMsg(PointMsg p, QuaternionMsg q)
 {
     _position    = p;
     _orientation = q;
 }
Пример #26
0
 public PoseMsg(Vector3 _position, Quaternion _orientation, bool _fromUnity = false)
 {
     position    = new PointMsg(_position, _fromUnity);
     orientation = new QuaternionMsg(_orientation, _fromUnity);
 }
Пример #27
0
 public TransformMsg(PointMsg p, QuaternionMsg q)
 {
     _translation = p;
     _rotation    = q;
 }
 public TransformMsg(Vector3Msg translation, QuaternionMsg rotation)
 {
     _translation = translation;
     _rotation    = rotation;
 }
Пример #29
0
    public ROSBridgeMsg OnReceiveMessage(string topic, JSONNode raw_msg, ROSBridgeMsg parsed = null)
    {
        ROSBridgeMsg result = null;

        // Writing all code in here for now. May need to move out to separate handler functions when it gets too unwieldy.
        switch (topic)
        {
        case "/dji_sdk/attitude":
            QuaternionMsg attitudeMsg = (parsed == null) ? new QuaternionMsg(raw_msg["quaternion"]) : (QuaternionMsg)parsed;
            attitude = offset * (new Quaternion(attitudeMsg.GetX(), attitudeMsg.GetY(), attitudeMsg.GetZ(), attitudeMsg.GetW()));
            result   = attitudeMsg;
            break;

        case "/dji_sdk/battery_state":
            battery_state = (parsed == null) ? new BatteryStateMsg(raw_msg) : (BatteryStateMsg)parsed;
            result        = battery_state;
            break;

        case "/dji_sdk/flight_status":
            if (isM100)
            {
                m100_flight_status = (FlightStatusM100)(new UInt8Msg(raw_msg)).GetData();
            }
            else
            {
                flight_status = (FlightStatus)(new UInt8Msg(raw_msg)).GetData();
            }
            break;

        case "/dji_sdk/gimbal_angle":
            Vector3Msg gimbleAngleMsg = (parsed == null) ? new Vector3Msg(raw_msg["vector"]) : (Vector3Msg)parsed;
            gimble_joint_angles = new Vector3((float)gimbleAngleMsg.GetX(), (float)gimbleAngleMsg.GetY(), (float)gimbleAngleMsg.GetZ());
            result = gimbleAngleMsg;
            break;

        case "/dji_sdk/gps_health":
            gps_health = (parsed == null) ? (new UInt8Msg(raw_msg)).GetData() : ((UInt8Msg)parsed).GetData();
            break;

        case "/dji_sdk/gps_position":
            gps_position = (parsed == null) ? new NavSatFixMsg(raw_msg) : (NavSatFixMsg)parsed;
            result       = gps_position;
            break;

        case "/dji_sdk/imu":
            imu    = (parsed == null) ? new IMUMsg(raw_msg) : (IMUMsg)parsed;
            result = imu;
            break;

        case "/dji_sdk/rc":
            remote_controller_msg = (parsed == null) ? new JoyMsg(raw_msg) : (JoyMsg)parsed;
            result = remote_controller_msg;
            break;

        case "/dji_sdk/velocity":
            Vector3Msg velocityMsg = (parsed == null) ? new Vector3Msg(raw_msg["vector"]) : (Vector3Msg)parsed;
            velocity = new Vector3((float)velocityMsg.GetX(), (float)velocityMsg.GetY(), (float)velocityMsg.GetZ());
            result   = velocityMsg;
            break;

        case "/dji_sdk/height_above_takeoff":
            relative_altitude = (parsed == null) ? (new Float32Msg(raw_msg)).GetData() : ((Float32Msg)parsed).GetData();
            break;

        case "/dji_sdk/local_position":
            PointMsg pointMsg = (parsed == null) ? new PointMsg(raw_msg["point"]) : (PointMsg)parsed;
            local_position = new Vector3(pointMsg.GetX(), pointMsg.GetY(), pointMsg.GetZ());
            result         = pointMsg;
            Debug.Log(result);
            break;

        default:
            Debug.LogError("Topic not implemented: " + topic);
            break;
        }
        return(result);
    }