public virtual void processResponse(string error, bool success)
{
// copied over from Xamla
var msg = new Messages.std_msgs.String(error);
msg.Serialized = msg.Serialize();
byte[] buf;
if (success)
{
buf = new byte[msg.Serialized.Length + 1 + 4];
buf[0] = (byte)(success ? 0x01 : 0x00);
msg.Serialized.CopyTo(buf, 5);
Array.Copy(BitConverter.GetBytes(msg.Serialized.Length), 0, buf, 1, 4);
}
else
{
buf = new byte[1 + 4];
buf[0] = (byte)(success ? 0x01 : 0x00);
Array.Copy(BitConverter.GetBytes(0), 0, buf, 1, 4);
}
connection.write(buf, buf.Length, onResponseWritten, true); // Eric has immediate as true, Xamla doesn't, but this seems to work for now
// String msg = new String(error);
// msg.Serialized = msg.Serialize();
// byte[] buf = new byte[msg.Serialized.Length + 1];
// buf[0] = (byte) (success ? 0x01 : 0x00);
// msg.Serialized.CopyTo(buf, 1);
// connection.write(buf, buf.Length, onResponseWritten, true);
}
private void cb(int i, m.String msg)
{
Dispatcher.BeginInvoke(new Action(() => { if (abraCadabra.Visibility == System.Windows.Visibility.Hidden)
{
abraCadabra.Visibility = Visibility.Visible;
}
}));
string[] split = msg.data.Split('=');
if (split.Length != 2)
{
Console.WriteLine("CRAP");
return;
}
if (split[0].Equals("BA"))
{
string[] perside = split[1].Split(':');
float[] vals = new[] { 0f, 0f };
for (int j = 0; j < perside.Length; j++)
{
float.TryParse(perside[j], out vals[j]);
vals[j] /= 10.0f;
}
setSingle(i * 2, vals[i]);
setSingle(i * 2 + 1, vals[1 - i]);
}
/*StringBuilder sb = new StringBuilder("mc[" + i + "] = \n");
* lock(telem[i])
* {
* if (!telem[i].ContainsKey(split[0][0]))
* telem[i].Add(split[0][0], split[1]);
* else
* telem[i][split[0][0]] = split[1];
* List<KeyValuePair<char, string>> sorted = telem[i].ToList();
* sorted.Sort((x,y)=>x.Key.CompareTo(y.Key));
* foreach (KeyValuePair<char, string> kvp in sorted)
* {
* sb.AppendLine("" + kvp.Key + " = " + kvp.Value);
* }
* }
* Console.WriteLine(sb);*/
}
private static void chatterCallback(m.String s)
{
Console.WriteLine("RECEIVED: " + s.data);
}
private static void chatterCallback(m.String s)
{
ROS.Info("RECEIVED: " + s.data);
}
public void subCallback(Messages.std_msgs.String msg)
{
ROS.Info("Receieved:\n" + msg.data);
}
private void cb1(m.String msg)
{
cb(1, msg);
}
private void cb0(m.String msg)
{
cb(0, msg);
}
