public void UnrealCallback(object sender, UnrealMsgArgs ua) { //Console.WriteLine(msgNum); if (ua.getMsgNum() % 5 == 0) { steerNormalize(); speedNormalize(); } }
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); } } } } }