private void ParseState(USARParser msg)
{
float[] curPose = USARParser.parseFloats(msg.getSegment("Pose").Get("Pose"), ",");
x = curPose[0];
y = curPose[1];
theta = curPose[2];
}
public Laser(USARParser msg)
{
if (msg.size == 0 || msg.segments == null)
{
return;
}
ParseState(msg);
}
public GroundTruth(USARParser msg)
{
if (msg.size == 0 || msg.segments == null)
{
return;
}
ParseState(msg);
}
private void ParseState(USARParser msg)
{
float[] curLocation = USARParser.parseFloats(msg.getSegment("Location").Get("Location"), ",");
float[] curRotation = USARParser.parseFloats(msg.getSegment("Orientation").Get("Orientation"), ",");
normalRotation(curRotation);
p3Loc = new Vector3(curLocation[0], curLocation[1], curLocation[2]);
p3Ori = new Vector3(curRotation[0], curRotation[1], curRotation[2]);
}
private void ParseState(USARParser msg)
{
fName = msg.getSegment("Name").Get("Name");
fFov = float.Parse(msg.getSegment("FOV").Get("FOV"));
fResolution = float.Parse(msg.getSegment("Resolution").Get("Resolution"));
rangesString = msg.getSegment("Range").Get("Range");
fRanges = new List <double>();
fRanges.AddRange(USARParser.parseDoubles(rangesString, ","));
validateString = msg.getString("Valid");
if (!string.IsNullOrEmpty(validateString))
{
bFilters = new List <bool>();
bFilters.AddRange(USARParser.parseBools(validateString, ",", true));
}
else
{
bFilters = new List <bool>();
foreach (float range in fRanges)
{
bFilters.Add(range <min_range || range> max_range);
}
}
if (!string.IsNullOrEmpty(thetaString))
{
fAng0 = (float)fTheta[0];
}
else
{
fAng0 = ((float)System.Math.PI / 2 - fFov / 2);
}
thetaString = msg.getString("Theta");
if (!string.IsNullOrEmpty(thetaString))
{
fTheta = new List <double>();
fTheta.AddRange(USARParser.parseDoubles(thetaString, ","));
}
else
{
fTheta = new List <double>();
for (int i = 0; i < fRanges.Count; i++)
{
fTheta.Add(fAng0 + i * fResolution);
}
}
if (msg.getSegment("Time") != null)
{
fTime = float.Parse(msg.getSegment("Time").Get("Time"));
}
scan_mask = 1;
}
private void ParseMessage(string message)
{
USARParser msg = new USARParser(message);
try
{
if (msg.type.Equals("SEN"))
{
string SensorType = msg.getSegment("Type").Get("Type");
if (SensorType.Equals("GroundTruth"))
{
SimMsgType = (int)eSimulationMessageType.GroundTruth;
SimMsgData = new GroundTruth(msg);
}
else if (SensorType.Equals("Odometry"))
{
SimMsgType = (int)eSimulationMessageType.Odometdy;
SimMsgData = new Odometry(msg);
}
else if (SensorType.Equals("INS"))
{
SimMsgType = (int)eSimulationMessageType.INS;
SimMsgData = new INS(msg);
}
else if (SensorType.Equals("RangeScanner"))
{
SimMsgType = (int)eSimulationMessageType.Laser;
SimMsgData = new Laser(msg);
}
else
{
SimMsgData = null;
SimMsgType = -1;
}
}
}
catch (Exception)
{
throw new Exception("Unable parse message <" + message + "> into object: ");
}
}