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);
}
}
}
}
}