Exemple #1
0
    public override void HandleEvent(QMsg msg)
    {
        PointMsg pm = msg as PointMsg;

        switch (pm.controllmode)
        {
        case PointControll_E.Edit:
            break;

        case PointControll_E.SaveMatchingpoint:
            break;

        case PointControll_E.Cancle:
            break;

        case PointControll_E.AutoNext:
            break;

        case PointControll_E.Load:
            LoadPointer(pm.parent, pm.pointsmap);
            break;

        case PointControll_E.ChoiseGroup:
        case PointControll_E.AllDisplay:
            RefreshDisplay(pm.controllmode, pm);
            break;

        default:
            break;
        }
    }
Exemple #2
0
    //点控制
    void ControllPoint(QMsg msg)
    {
        PointMsg        pm  = msg as PointMsg;
        PointControll_E pce = pm.controllmode;

        currentmode = pce;
        switch (pce)
        {
        case PointControll_E.Edit:
            CheckLastEdit(currentgroup, currentindex);
            this.currentgroup = pm.group;
            this.currentindex = pm.index;
            break;

        case PointControll_E.SaveMatchingpoint:
            SaveGizmo();
            break;

        case PointControll_E.Cancle:
            CancleGizmo();
            break;

        case PointControll_E.AutoNext:
            break;

        default:
            break;
        }
    }
    public void InitCameraManager()
    {
        GameObject camera = new GameObject("GameCamera", typeof(Camera));

        camerarotatesphere = camera.AddComponent <CameraRotateSphere>();
        camera.transform.SetParent(transform);

        QMsg Normalmsg = new ModelMsg()
        {
            EventID = (int)Model_E.Normal, modelevent = Model_E.SetCamera, selfcamera = camera.GetComponent <Camera>()
        };

        SendMsg(Normalmsg);
        QMsg Usermsg = new ModelMsg()
        {
            EventID = (int)Model_E.UserImport, modelevent = Model_E.SetCamera, selfcamera = camera.GetComponent <Camera>()
        };

        SendMsg(Usermsg);
        QMsg normalPointmsg = new PointMsg()
        {
            EventID = (int)Point_E.Normal, controllmode = PointControll_E.SetCamra, selfcamra = camera.GetComponent <Camera>()
        };

        SendMsg(normalPointmsg);
        QMsg userPointmsg = new PointMsg()
        {
            EventID = (int)Point_E.Userimport, controllmode = PointControll_E.SetCamra, selfcamra = camera.GetComponent <Camera>()
        };

        SendMsg(userPointmsg);

        RegetsSelfEvent();
    }
Exemple #4
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 Vector3 From(this PointMsg 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>());
            }
        }
Exemple #6
0
    void ResetLastGroupItem(Button currentlistbutton, int group)
    {
        if (lastkey)
        {
            lastkey.GetComponent <Image>().color = Color.white;
        }
        currentlistbutton.GetComponent <Image>().color = Color.green;

        //判断当前是分组显示
        if (dropdownindex == 0)
        {
            QMsg usermsg = new PointMsg()
            {
                EventID      = (int)Point_E.Userimport,
                group        = group,
                controllmode = PointControll_E.ChoiseGroup
            };
            PointerManager.Instance.SendMsg(usermsg);
            QMsg normalmsg = new PointMsg()
            {
                EventID      = (int)Point_E.Normal,
                group        = group,
                controllmode = PointControll_E.ChoiseGroup
            };
            PointerManager.Instance.SendMsg(normalmsg);
        }
        currentgroup = group;
        // lastkey = currentlistbutton;
    }
Exemple #7
0
 public static Vector3 ToVector3(this PointMsg pointMsg)
 {
     return(new Vector3(
                -pointMsg.GetY(),
                pointMsg.GetZ(),
                pointMsg.GetX()));
 }
    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();
    }
Exemple #9
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);
            }
Exemple #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);
    }
Exemple #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);
     }
 }
Exemple #12
0
        public static void GUI(this PointMsg message, string name)
        {
            string body = $"[{message.x:F2}, {message.y:F2}, {message.z:F2}]";

            if (name == null || name == "")
            {
                GUILayout.Label(body);
            }
            else
            {
                GUILayout.Label($"{name}: {body}");
            }
        }
 public DetectionMsg(JSONNode msg)
 {
     scores = new ObjectHypothesisMsg[msg["scores"].Count];
     for (int i = 0; i < scores.Length; i++)
     {
         scores[i] = new ObjectHypothesisMsg(msg["scores"][i]);
     }
     corners = new PointMsg[msg["corners"].Count];
     for (int i = 0; i < corners.Length; i++)
     {
         corners[i] = new PointMsg(msg["corners"][i]);
     }
     occluded_corners = byte.Parse(msg["occluded_corners"], System.Globalization.CultureInfo.InvariantCulture);
 }
Exemple #14
0
 public NavStsMsg(JSONNode msg)
 {
     _header               = new HeaderMsg(msg["header"]);
     _global_position      = new DecimalLatLonMsg(msg["global_position"]);
     _origin               = new DecimalLatLonMsg(msg["origin"]);
     _position             = new NEDMsg(msg["position"]);
     _altitude             = new Float32Msg(msg["altitude"]);
     _body_velocity        = new PointMsg(msg["body_velocity"]);
     _orientation          = new RPYMsg(msg["orientation"]);
     _orientation_rate     = new RPYMsg(msg["orientation_rate"]);
     _position_variance    = new NEDMsg(msg["position_variance"]);
     _orientation_variance = new RPYMsg(msg["orientation_variance"]);
     _status               = new UInt8Msg(msg["status"]);
 }
Exemple #15
0
    //封装发送
    private void Send(PointControll_E pcontroller)
    {
        QMsg normalmsg = new PointMsg()
        {
            EventID = (int)Point_E.Normal, controllmode = pcontroller, group = this.group, index = this.index
        };

        PointerManager.Instance.SendMsg(normalmsg);
        QMsg usermsg = new PointMsg()
        {
            EventID = (int)Point_E.Userimport, controllmode = pcontroller, group = this.group, index = this.index
        };

        PointerManager.Instance.SendMsg(usermsg);
    }
Exemple #16
0
    //封装发送
    private void Send(PointControll_E pcontroller)
    {
        QMsg normalmsg = new PointMsg()
        {
            EventID = (int)Point_E.Normal, controllmode = pcontroller
        };

        PointerManager.Instance.SendMsg(normalmsg);
        QMsg usermsg = new PointMsg()
        {
            EventID = (int)Point_E.Userimport, controllmode = pcontroller
        };

        PointerManager.Instance.SendMsg(usermsg);

        lastchoiseitem.HandleEvent(pcontroller);
        toolpanel.SetActive(pcontroller == PointControll_E.Edit);
    }
Exemple #17
0
            public NavStsMsg(JSONNode msg)
            {
                //Debug.Log("NavStsMsg with " + msg.ToString());

                _header               = new HeaderMsg(msg["header"]);
                _global_position      = new DecimalLatLonMsg(msg["global_position"]);
                _origin               = new DecimalLatLonMsg(msg["origin"]);
                _position             = new NEDMsg(msg["position"]);
                _altitude             = new Float32Msg(msg["altitude"]);
                _body_velocity        = new PointMsg(msg["body_velocity"]);
                _orientation          = new RPYMsg(msg["orientation"]);
                _orientation_rate     = new RPYMsg(msg["orientation_rate"]);
                _position_variance    = new NEDMsg(msg["position_variance"]);
                _orientation_variance = new RPYMsg(msg["orientation_variance"]);
                _status               = new UInt8Msg(msg["status"]);

                //Debug.Log("NavStsMsg done and it looks like " + this.ToString());
            }
    protected virtual void RefreshDisplay(PointControll_E pce, QMsg msg)
    {
        PointMsg pm = msg as PointMsg;

        switch (pce)
        {
        case PointControll_E.ChoiseGroup:
            Refreshdis(pm.group);
            break;

        case PointControll_E.AllDisplay:
            Refreshdis(allactive: true);
            break;

        default:
            break;
        }
    }
Exemple #19
0
    /// <summary>
    /// ("分组显示", "全部显示")下拉列表,事件定义
    /// </summary>
    /// <param name="index"></param>
    void DropdownCallBack(int index)
    {
        dropdownindex = index;
        QMsg usermsg = new PointMsg()
        {
            EventID      = (int)Point_E.Userimport,
            group        = currentgroup,
            controllmode = index == 0 ? PointControll_E.ChoiseGroup : PointControll_E.AllDisplay
        };

        PointerManager.Instance.SendMsg(usermsg);
        QMsg normalmsg = new PointMsg()
        {
            EventID      = (int)Point_E.Normal,
            group        = currentgroup,
            controllmode = index == 0 ? PointControll_E.ChoiseGroup : PointControll_E.AllDisplay
        };

        PointerManager.Instance.SendMsg(normalmsg);

        //Enums.PointControll pcontroll = (Enums.PointControll)index;
        //MSGCenter.Execute(pcontroll.ToString(),choiseindex.ToString());
    }
Exemple #20
0
 public NavStsMsg(HeaderMsg header,
                  DecimalLatLonMsg global_position,
                  DecimalLatLonMsg origin,
                  NEDMsg position,
                  Float32Msg altitude,
                  PointMsg body_velocity,
                  RPYMsg orientation,
                  RPYMsg orientation_rate,
                  NEDMsg position_variance,
                  RPYMsg orientation_variance,
                  UInt8Msg status)
 {
     _header               = header;
     _global_position      = global_position;
     _origin               = origin;
     _position             = position;
     _altitude             = altitude;
     _body_velocity        = body_velocity;
     _orientation          = orientation;
     _orientation_rate     = orientation_rate;
     _position_variance    = position_variance;
     _orientation_variance = orientation_variance;
     _status               = status;
 }
Exemple #21
0
    /// <summary>
    /// 加载用户点
    /// </summary>
    protected virtual void LoadGizmopointer(Dictionary <int, Dictionary <int, Vector3> > _pointmap, Transform _parent, Tool.PointMode mode)
    {
        int eventid = -1;

        switch (mode)
        {
        case Tool.PointMode.Normal:
            eventid = (int)Point_E.Normal;
            break;

        case Tool.PointMode.UserImport:
            eventid = (int)Point_E.Userimport;
            break;

        default:
            break;
        }
        QMsg msg = new PointMsg()
        {
            EventID = eventid, controllmode = PointControll_E.Load, parent = _parent, pointsmap = _pointmap
        };

        PointerManager.Instance.SendMsg(msg);
    }
 public PoseMsg(Vector3 _position, Quaternion _orientation, bool _fromUnity = false)
 {
     position    = new PointMsg(_position, _fromUnity);
     orientation = new QuaternionMsg(_orientation, _fromUnity);
 }
 public PoseMsg(Transform _tf)
 {
     this.position    = new PointMsg(_tf.position, true);
     this.orientation = new QuaternionMsg(_tf.localRotation, true);
 }
 public PoseMsg(PointMsg _translation, QuaternionMsg _rotation)
 {
     position    = _translation;
     orientation = _rotation;
 }
 public PoseMsg(JSONNode msg)
 {
     position    = new PointMsg(msg["position"]);
     orientation = new QuaternionMsg(msg["orientation"]);
 }
 public TransformMsg(PointMsg p, QuaternionMsg q)
 {
     _translation = p;
     _rotation    = q;
 }
 public TransformMsg(JSONNode msg)
 {
     _translation = new PointMsg(msg["translation"]);
     _rotation    = new QuaternionMsg(msg["rotation"]);
 }
Exemple #28
0
 public override void Deserialize(JSONNode msg)
 {
     _position    = new PointMsg(msg["position"]);
     _orientation = new QuaternionMsg(msg["orientation"]);
 }
Exemple #29
0
 public static void GUI(this PointMsg message)
 {
     GUILayout.Label($"[{message.x:F2}, {message.y:F2}, {message.z:F2}]");
 }
Exemple #30
0
        /// <summary>
        /// Finds the world coordinates of an image coordinate based on the CameraInfo matrices
        /// K matrix     P matrix
        /// 0 1 2        0 1 2 3
        /// 3 4 5        4 5 6 7
        /// 6 7 8        8 9 10 11
        /// </summary>
        /// <param name="x">x coord to map</param>
        /// <param name="y">y coord to map</param>
        /// <param name="P">Projection matrix (3x4)</param>
        /// <param name="K">Camera intrinsics (3x3)</param>
        /// <returns></returns>
        static PointMsg FindPixelProjection(int x, int y, double[] P, double[][] invK)
        {
            PointMsg worldCoord = new PointMsg(x, y, 1);

            return(new PointMsg((float)invK[0][0] * worldCoord.x + (float)invK[0][1] * worldCoord.y + (float)invK[0][2] * worldCoord.z, (float)invK[1][0] * worldCoord.x + (float)invK[1][1] * worldCoord.y + (float)invK[1][2] * worldCoord.z, (float)invK[2][0] * worldCoord.x + (float)invK[2][1] * worldCoord.y + (float)invK[2][2] * worldCoord.z));
        }