/// <summary>
/// Update the pose information from incoming message
/// </summary>
/// <param name="pose"></param>
public void SetPose(Pose pose)
{
if (pose != null)
{
Position = pose.position;
Orientation = pose.orientation;
}
}
/// <summary>
/// Set the world reference frame transformation
/// </summary>
/// <param name="pose">World frame transformation</param>
public void SetWorldFrame(Pose pose)
{
if (pose != null)
{
_worldFrame.position = pose.position;
_worldFrame.orientation = pose.orientation;
}
}
private static IList<Pose> LogTorsoInfo(object info)
{
List<Pose> poseList = new List<Pose>();
Node nodeInfo = info as Node;
if (nodeInfo != null)
{
Subscribe subscriber = nodeInfo.subscriber.FirstOrDefault(subscribe => subscribe.msg_type == "Humans");
if (subscriber != null)
{
Console.WriteLine("Creating context");
using (var context = NetMQContext.Create())
{
Console.WriteLine("Creating socket");
using (var socket = context.CreateSubscriberSocket())
{
var addr = string.Format("{0}:{1}", subscriber.host, subscriber.port);
Console.WriteLine("Connecting to {0}", addr);
socket.Connect(addr);
Console.WriteLine("Subscribing to {0}", subscriber.topic);
socket.Subscribe(subscriber.topic);
uint id = 1;
while (!_stopTask)
{
var topic = socket.ReceiveString(new TimeSpan(0, 0, 0, 0, 100));
if (topic != null)
{
byte[] msg = socket.Receive();
if (msg != null)
{
using (var memStream = new MemoryStream(msg))
{
var humans = Serializer.Deserialize<Humans>(memStream);
foreach (var human in humans.human)
{
var pose = new Pose()
{
id = (uint) human.id,
position = human.torso_position,
orientation = human.orientation
};
poseList.Add(pose);
}
}
}
}
}
}
System.Threading.Thread.Sleep(50);
}
}
else
{
Console.WriteLine("No valid publisher of Pose");
}
}
else
{
Console.WriteLine("Node info empty");
}
Console.WriteLine("Completing the logging task");
return poseList;
}