Beispiel #1
0
 public void UnrealCallback(object sender, UnrealMsgArgs ua)
 {
     //Console.WriteLine(msgNum);
     if (ua.getMsgNum() % 5 == 0)
     {
         steerNormalize();
         speedNormalize();
     }
 }
Beispiel #2
0
        public void UnrealCallback(object sender, UnrealMsgArgs ua)
        {
            if (ua.getMsgType() == Messaging.MSG_SEN_RFID)
            {
                if (msgSys.getMsgResponse(Messaging.MSG_SEN_RFID) != null &&
                    msgSys.getMsgResponse(Messaging.MSG_SEN_RFID)["Mem"] != null)
                {
                    string rfMem = (string)msgSys.getMsgResponse(Messaging.MSG_SEN_RFID)["Mem"];
                    if (rfMem.IndexOf('_') > 0)
                    {
                        string signId = rfMem.Split('_')[0];
                        string preOrPost = rfMem.Split('_')[1];
                        //Console.WriteLine("S:" + signId + " P:" + preOrPost);

                        string esOut = "";
                        switch (signId)
                        {
                            case "13":
                                if (selectedES == 0)
                                {
                                    if (preOrPost == "0")
                                        esOut += expert.assertFact("rfid_detected(pre,no_turn_left)", "1");
                                    else
                                        esOut += expert.assertFact("rfid_detected(post,no_turn_left)", "1");
                                    break;
                                }
                                else
                                {
                                    if (preOrPost == "0")
                                        esOut += expert.assertFact("NoTurn", "Pre_RFID_Detected", "Pre_No_Turn_Left", "1");
                                    else
                                        esOut += expert.assertFact("NoTurn", "Post_RFID_Detected", "Post_No_Turn_Left", "1");
                                    break;
                                }
                        }
                        //Console.WriteLine(esOut);
                        queryViolation();

                    }
                }
            }
            else if (ua.getMsgType() == Messaging.MSG_SEN_GPS)
            {
                if (ua.getMsgNum() % 5 == 0) {
                    if (msgSys.getMsgResponse(Messaging.MSG_SEN_GPS) != null &&
                        msgSys.getMsgResponse(Messaging.MSG_SEN_GPS)["Latitude"] != null)
                    {
                        String lat = (string)msgSys.getMsgResponse(Messaging.MSG_SEN_GPS)["Latitude"];
                        String lon = (string)msgSys.getMsgResponse(Messaging.MSG_SEN_GPS)["Longitude"];

                        int yOffset = 160;
                        int xOffset = 60;
                        double yp2gp = 770;
                        double xp2gp = 500;
                        double ns = Double.Parse(lat.Split(new char[] { ',' })[1]);
                        double ew = Double.Parse(lon.Split(new char[] { ',' })[1]);
                        if (!lat.EndsWith("N"))
                        {
                            ns = -ns;
                        }
                        if (!lon.EndsWith("E"))
                        {
                            ew = -ew;
                        }
                        //Console.WriteLine(ns + " !!! " + ew);
                        int x = (int)(ew * xp2gp) + xOffset;
                        int y = yOffset - (int)(ns * yp2gp);
                        Graphics g = pnlMap.CreateGraphics();
                        if (map == null)
                        {
                            map = pnlMap.BackgroundImage;
                        }
                        g.DrawImage(map, 0, 0);
                        g.FillEllipse(mapPen.Brush, x - 4, y - 4, 8, 8);

                        String rules = "";
                        string esOut = "";
                        Double prob = Math.Pow(Double.Parse("0." + lat.Substring(lat.IndexOf('.') + 3, 2)), 2) + Math.Pow(Double.Parse("0." + lon.Substring(lat.IndexOf('.') + 3, 2)), 2);
                        prob = Math.Sqrt(prob);
                        prob = 1 - prob / 10d;
                        string latGIS = lat.Remove(lat.IndexOf('.') + 3, 2);
                        string lonGIS = lon.Remove(lat.IndexOf('.') + 3, 2);
                        if (!lastNode.Equals(latGIS + lonGIS))
                        {
                            lastNode = latGIS + lonGIS;
                            XmlNodeList nl = gis.SelectNodes("//NodeProp[@lat='" + latGIS + "' and @lon='" + lonGIS + "']");
                            if (nl.Count > 0)
                            {
                                foreach (XmlElement elem in nl)
                                {
                                    String rule = elem.GetAttribute("rule");
                                    String type = elem.GetAttribute("type");
                                    rules += rule + ",";
                                    if (rule == "1")
                                    {
                                        if (selectedES == 0)
                                        {
                                            esOut += expert.assertFact("gps_node_property(speed_limit,30)", prob.ToString("0.0000"));
                                        }
                                        else
                                        {
                                            esOut += expert.assertFact("SpeedLimit", "GPS_Node_Property", "Limit_30", prob.ToString("0.0000"));
                                        }
                                    }
                                    else if (rule == "13")
                                    {
                                        if (selectedES == 0)
                                        {
                                            esOut += expert.assertFact("gps_node_property(" + type + ",no_turn_left)", prob.ToString("0.0000"));
                                        }
                                        else
                                        {
                                            if (type == "pre")
                                                esOut += expert.assertFact("NoTurn", "Pre_GPS_Node_Property", "Pre_No_Turn_Left", prob.ToString("0.0000"));
                                            else
                                                esOut += expert.assertFact("NoTurn", "Post_GPS_Node_Property", "Post_No_Turn_Left", prob.ToString("0.0000"));

                                        }
                                    }
                                }
                                queryViolation();
                            }
                            else
                            {
                                if (selectedES == 0)
                                {
                                    esOut += expert.retractFact("gps_node_property(speed_limit,Y)");
                                    esOut += expert.retractFact("gps_node_property(post,Y)");
                                }
                                else
                                {
                                    esOut += expert.retractFact("SpeedLimit", "GPS_Node_Property");
                                    esOut += expert.retractFact("NoTurn", "Post_GPS_Node_Property");
                                }

                            }
                            String rtbText = "GPS INFO:\n\nLatitude:\n" + lat + "\nLongitude:\n" + lon + "\n\nRule:" + rules + "\nProb:" + prob.ToString("0.0000");
                            this.BeginInvoke(new ExpertWriteDelegate(ExpertWrite), rtbGPS, rtbText);
                            //Console.WriteLine(esOut);
                        }
                    }
                }
            }
        }