void camera_ImageReceived(object sender, Magic.Common.TimestampedEventArgs <Magic.Common.Sensors.RobotImage> e)
{
int robotID = e.Message.robotID;
if (robotID >= 5)
{
robotID -= 4;
}
// e.Timestamp is not the actual time stamp you want !!
if (!robotIDToImageList.ContainsKey(robotID))
{
robotIDToImageList.Add(robotID, new List <RobotImage>(200));
mostRecentTimeStamp.Add(robotID, e.Message.timeStamp);
}
if (robotIDToImageList[robotID].Count == robotIDToImageList[robotID].Capacity)
{
robotIDToImageList[robotID].RemoveAt(0);
}
robotIDToImageList[robotID].Add(e.Message);
if (!robotImageDict.ContainsKey(robotID))
{
robotImageDict.Add(robotID, robotIDToImageList[robotID][robotIDToImageList[robotID].Count - 1]);
}
mostRecentTimeStamp[robotID] = e.Message.timeStamp; // keep track of the most recent timestamp
}
void odometryProcessor_OdometryUpdate(object sender, Magic.Common.TimestampedEventArgs <Magic.Common.RobotPose> e)
{
curTS = e.TimeStamp;
if (NewPoseAvailable != null)
{
NewPoseAvailable(this, new NewPoseAvailableEventArgs(odometryProcessor.CurrentState));
}
if (NewOdomAvailable != null)
{
NewOdomAvailable(this, new Magic.Common.TimestampedEventArgs <RobotPose>(curTS, odometryProcessor.CurrentState));
}
}
