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)); } }