static void Main(string[] args)
{
RosBridgeUtility.RosBridgeConfig conf1 = new RosBridgeUtility.RosBridgeConfig();
conf1.readConfig("XMLFile1.xml");
RosBridgeUtility.RosBridgeLogic logic = new RosBridgeUtility.RosBridgeLogic();
logic.AddStoppedListener(logic_MonitoringStopped);
String fos = "[10, 10]";
Console.WriteLine(Array.ConvertAll(fos.Replace("[", "").Replace("]", "").Split(','), Double.Parse));
logic.Initialize(conf1.URI);
logic.Connect();
logic.InitializeCollection();
/*
* Dictionary<String, double> lin = new Dictionary<string, double>()
* {
* {"x",1.5},
* {"y",0.0},
* {"z",0.0},
* };
* Dictionary<String, double> ang = new Dictionary<string, double>()
* {
* {"x",0.0},
* {"y",0.0},
* {"z",3.14},
* };
* Object[] vals = { lin, ang };
*/
//logic.PublishMessage("/turtle1/command_velocity", keys, vals);
//logic.PublishMessage("/turtle1/cmd_vel", keys, vals);
Object[] lin = { 2.0, 0.0, 0.0 };
Object[] ang = { 0.0, 0.0, 3.14 };
foreach (var item in conf1.getPublicationList())
{
logic.PublishTwistMsg(item, lin, ang);
}
logic.StartCollections(conf1.getTopicList());
Object[] linNeo = { 2.1, 2.1 };
Object[] driveActive = { true, true };
Object[] quickStop = { false, false };
Object[] disableBrake = { true, true };
//logic.PublishNeobotixCommandMsg("/DriveCommands", linNeo, driveActive, quickStop, disableBrake);
System.Threading.Thread.Sleep(1000);
logic.RemoveCollections(conf1.getTopicList());
var x = logic.StopCollection();
foreach (var attr in conf1.ProjectedAttributes())
{
Console.Out.WriteLine(attr.Item2);
foreach (var item in logic.getResponseAttribute(attr.Item1, attr.Item2))
{
Console.WriteLine(item);
//Console.Out.WriteLine(((List<Double>)item)[0]);
//Console.Out.WriteLine("{0}: {1} {2}", attr.Item1, ((List<Double>)item)[0], ((List<Double>)item)[1]);
}
}
}
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)));
}
}
}
}
