private void pushView(JObject jsonData)
{
if (jsonData["topic"].ToString().Equals(showState))
{
Dispatcher.Invoke(new Action(() => labelX.Content = "x: " + valueToView(jsonData, bridgeConfig.ProjectedAttributes()[0].Item2)));
Dispatcher.Invoke(new Action(() => labelY.Content = "y: " + valueToView(jsonData, bridgeConfig.ProjectedAttributes()[1].Item2)));
}
else if (jsonData["topic"].ToString().Replace("\"", "").Equals(laserScan))
{
/*
* var x1 = jsonData["msg"]["ranges"].ToList();
* foreach (var itemx in ((IList<JToken>)x1))
* {
* Console.WriteLine(itemx);
* }
*/
Double angle_inc;
Double min_angle;
if (jsonData["msg"]["angle_increment"].ToString().Contains(","))
{
Double.TryParse(jsonData["msg"]["angle_increment"].ToString(),
NumberStyles.Number,
CultureInfo.CreateSpecificCulture("hu-HU"),
out angle_inc);
Double.TryParse(jsonData["msg"]["angle_min"].ToString(),
NumberStyles.Number,
CultureInfo.CreateSpecificCulture("hu-HU"),
out min_angle);
}
else
{
Double.TryParse(jsonData["msg"]["angle_increment"].ToString(),
NumberStyles.Number,
CultureInfo.CreateSpecificCulture("en-US"),
out angle_inc);
Double.TryParse(jsonData["msg"]["angle_min"].ToString(),
NumberStyles.Number,
CultureInfo.CreateSpecificCulture("en-US"),
out min_angle);
}
Dispatcher.Invoke(new Action(() =>
laserScanCanvas(jsonData["msg"]["ranges"].ToList(), angle_inc, min_angle)));
}
else if (jsonData["topic"].ToString().Replace("\"", "").Equals(odometry))
{
odometryCount++;
Double measuredDistance = 0;
Double measuredAngle = 0;
if (bridgeConfig.target == "neobotix_mp500" || bridgeConfig.target == "pr2")
{
/*Console.WriteLine(Double.TryParse(jsonData["msg"]["pose"]["pose"].ToString(),
* NumberStyles.Number,
* CultureInfo.CreateSpecificCulture("en-US"),
* out measuredDistance));
*/
Double x, y, angleX, angleY, angleZ, angleW = 0;
if (jsonData["msg"]["pose"]["pose"]["position"]["x"].ToString().Contains(","))
{
Double.TryParse(jsonData["msg"]["pose"]["pose"]["position"]["x"].ToString(),
NumberStyles.Number,
CultureInfo.CreateSpecificCulture("de-DE"),
out x);
Double.TryParse(jsonData["msg"]["pose"]["pose"]["position"]["y"].ToString(),
NumberStyles.Number,
CultureInfo.CreateSpecificCulture("de-DE"),
out y);
Double.TryParse(jsonData["msg"]["pose"]["pose"]["orientation"]["x"].ToString(),
NumberStyles.Number,
CultureInfo.CreateSpecificCulture("de-DE"),
out angleX);
Double.TryParse(jsonData["msg"]["pose"]["pose"]["orientation"]["y"].ToString(),
NumberStyles.Number,
CultureInfo.CreateSpecificCulture("de-DE"),
out angleY);
Double.TryParse(jsonData["msg"]["pose"]["pose"]["orientation"]["z"].ToString(),
NumberStyles.Number,
CultureInfo.CreateSpecificCulture("de-DE"),
out angleZ);
Double.TryParse(jsonData["msg"]["pose"]["pose"]["orientation"]["w"].ToString(),
NumberStyles.Number,
CultureInfo.CreateSpecificCulture("de-DE"),
out angleW);
}
else
{
Double.TryParse(jsonData["msg"]["pose"]["pose"]["position"]["x"].ToString(),
NumberStyles.Number,
CultureInfo.CreateSpecificCulture("en-US"),
out x);
Double.TryParse(jsonData["msg"]["pose"]["pose"]["position"]["y"].ToString(),
NumberStyles.Number,
CultureInfo.CreateSpecificCulture("en-US"),
out y);
Double.TryParse(jsonData["msg"]["pose"]["pose"]["orientation"]["x"].ToString(),
NumberStyles.Number,
CultureInfo.CreateSpecificCulture("en-US"),
out angleX);
Double.TryParse(jsonData["msg"]["pose"]["pose"]["orientation"]["y"].ToString(),
NumberStyles.Number,
CultureInfo.CreateSpecificCulture("en-US"),
out angleY);
Double.TryParse(jsonData["msg"]["pose"]["pose"]["orientation"]["z"].ToString(),
NumberStyles.Number,
CultureInfo.CreateSpecificCulture("en-US"),
out angleZ);
Double.TryParse(jsonData["msg"]["pose"]["pose"]["orientation"]["w"].ToString(),
NumberStyles.Number,
CultureInfo.CreateSpecificCulture("en-US"),
out angleW);
}
if (odometryCount % 100 == 0)
{
//Console.WriteLine("X: {0}, Y: {1}, Z: {2}, W: {3}", angleX, angleY, angleZ, angleW);
currRot = bridgeLogic.convertQuaternionToEuler(angleX, angleY, angleZ, angleW);
//Console.WriteLine("Roll: {0}, Pitch: {1}, Yaw: {2}", currRot.roll, currRot.pitch, currRot.yaw);
Dispatcher.Invoke(new Action(() => visualizeOdometry(x, y, currRot.yaw)));
}
}
else
{
if (jsonData["msg"]["distance"].ToString().Contains(","))
{
Double.TryParse(jsonData["msg"]["distance"].ToString(),
NumberStyles.Number,
CultureInfo.CreateSpecificCulture("de-DE"),
out measuredDistance);
Double.TryParse(jsonData["msg"]["angle"].ToString(),
NumberStyles.Number,
CultureInfo.CreateSpecificCulture("de-DE"),
out measuredAngle);
Dispatcher.Invoke(new Action(() => visualizeOdometry(measuredDistance, measuredAngle)));
}
{
Double.TryParse(jsonData["msg"]["distance"].ToString(),
NumberStyles.Number,
CultureInfo.CreateSpecificCulture("en-US"),
out measuredDistance);
Double.TryParse(jsonData["msg"]["angle"].ToString(),
NumberStyles.Number,
CultureInfo.CreateSpecificCulture("en-US"),
out measuredAngle);
Dispatcher.Invoke(new Action(() => visualizeOdometry(measuredDistance, measuredAngle)));
}
}
}
}