void localMapRequestClient_MsgReceived(object sender, MsgReceivedEventArgs <LocalMapRequestMessage> e) { if (e.message == null) { return; } UpdateMapDataMessage mapToSend = Diff(e.message.RobotID, gaussianMixMapAlgorithm.UhatGM, e.message.CurrentPose, e.message.ExtentX, e.message.ExtentY); if (mapToSend != null) { int segmentLength = 2000; int numIteration = mapToSend.CellData.Count / segmentLength; int numLeftOver = mapToSend.CellData.Count - (numIteration * segmentLength); int lastIndex = 0; for (int i = 0; i < numIteration + 1; i++) { if (i == numIteration) { localMapResponseServer.SendUnreliably(new UpdateMapDataMessage(mapToSend.CellData.GetRange(lastIndex, numLeftOver))); } else { localMapResponseServer.SendUnreliably(new UpdateMapDataMessage(mapToSend.CellData.GetRange(lastIndex, segmentLength))); lastIndex += segmentLength; } } } }
void gaussMixMapClient_MsgReceived(object sender, MsgReceivedEventArgs <GlobalMapCell> e) { isWorking = true; if (globalHeightMap == null) { double globalMapResolution = e.message.DataList[0].largeU; // first index has grid information double globalMapExtent = e.message.DataList[0].largeSig; globalHeightMap = new OccupancyGrid2D(globalMapResolution, globalMapResolution, globalMapExtent, globalMapExtent); globalSigMap = new OccupancyGrid2D(globalHeightMap); globalPijMap = new OccupancyGrid2D(globalHeightMap); laserHitMap = new OccupancyGrid2D(globalHeightMap); thresholdGlobalHeightMap = new OccupancyGrid2D(globalHeightMap); } // if message is received, update the 3 gridmap GlobalGaussianMixMap.ConvertToOcGrid(ref globalHeightMap, ref globalSigMap, ref globalPijMap, ref laserHitMap, e.message.DataList); for (int i = 0; i < globalHeightMap.NumCellX; i++) { for (int j = 0; j < globalHeightMap.NumCellY; j++) { if (globalPijMap.GetCellByIdx(i, j) > 1) { thresholdGlobalHeightMap.SetCellByIdx(i, j, globalHeightMap.GetCellByIdx(i, j)); } else { thresholdGlobalHeightMap.SetCellByIdx(i, j, 0); } } } }
public async void IncomingMsgAsync(object sender, MsgReceivedEventArgs e) { //check if message is from this peer if (e.RemoteIp == this.LocalIpAddress) { //from this peer. //no futher proccessing needed if (forwardAll == false) { return; } } //check if its from a new peer if (e.BindingType == TransportType.UDP) { string remotePeeripAddress = e.RemoteIp; bool peerKnown = DoesPeerExistByIp(remotePeeripAddress); if (!peerKnown) { //not a known peer await DirectConnectTCPAsync(remotePeeripAddress); } } //check if its a blank UDP packet //These are used as heart beats if (e.Message.Length <= 0) { return; } //trigger sent message MsgReceived?.Invoke(this, e); }
void gaussMixMapClient_MsgReceived(object sender, MsgReceivedEventArgs<GlobalMapCell> e) { isWorking = true; if (globalHeightMap == null) { double globalMapResolution = e.message.DataList[0].largeU; // first index has grid information double globalMapExtent = e.message.DataList[0].largeSig; globalHeightMap = new OccupancyGrid2D(globalMapResolution, globalMapResolution, globalMapExtent, globalMapExtent); globalSigMap = new OccupancyGrid2D(globalHeightMap); globalPijMap = new OccupancyGrid2D(globalHeightMap); laserHitMap = new OccupancyGrid2D(globalHeightMap); thresholdGlobalHeightMap = new OccupancyGrid2D(globalHeightMap); } // if message is received, update the 3 gridmap GlobalGaussianMixMap.ConvertToOcGrid(ref globalHeightMap, ref globalSigMap, ref globalPijMap, ref laserHitMap, e.message.DataList); for (int i = 0; i < globalHeightMap.NumCellX; i++) { for (int j = 0; j < globalHeightMap.NumCellY; j++) { if (globalPijMap.GetCellByIdx(i, j) > 1) { thresholdGlobalHeightMap.SetCellByIdx(i, j, globalHeightMap.GetCellByIdx(i, j)); } else { thresholdGlobalHeightMap.SetCellByIdx(i, j, 0); } } } }
static void poseClient_MsgReceived(object sender, MsgReceivedEventArgs <RobotPoseMessage> e) { if (e.message == null) { return; } pose = e.message.Pose; protoPose.x = pose.x; protoPose.y = pose.y; protoPose.z = pose.z; protoPose.yaw = pose.yaw; protoPose.pitch = pose.pitch; protoPose.roll = pose.roll; protoPose.timeStamp = pose.TimeStamp; protoPoseServer.SendUnreliably(protoPose); // Clear the screen Console.Clear(); Console.SetCursorPosition(Console.CursorLeft, Console.CursorTop); Console.WriteLine("Robot pose:"); Console.WriteLine("X:\t" + pose.x); Console.WriteLine("Y:\t" + pose.y); Console.WriteLine("Z:\t" + pose.z); Console.WriteLine("yaw:\t" + pose.yaw); Console.WriteLine("pitch:\t" + pose.pitch); Console.WriteLine("roll:\t" + pose.roll); Console.WriteLine("timestamp:\t" + pose.timestamp); }
void simLidarClient_MsgReceived(object sender, MsgReceivedEventArgs <SimMessage <ILidarScan <ILidar2DPoint> > > e) { if (e.message.RobotID == robotID) { ScanReceived.Invoke(sender, new ILidarScanEventArgs <ILidarScan <ILidar2DPoint> >(e.message.Message)); } }
void simSegwayFeedback_MsgReceived(object sender, MsgReceivedEventArgs <SimMessage <IRobotTwoWheelStatus> > e) { if (e.message.RobotID == robotID && WheelPositionUpdate != null) { WheelPositionUpdate(this, new TimestampedEventArgs <IRobotTwoWheelStatus>(e.message.Timestamp, e.message.Message)); } }
void simLidarClient_MsgReceived(object sender, MsgReceivedEventArgs<SimMessage<ILidarScan<ILidar2DPoint>>> e) { if (e.message.RobotID == robotID) { ScanReceived.Invoke(sender, new ILidarScanEventArgs<ILidarScan<ILidar2DPoint>>(e.message.Message)); } }
static void poseClient_MsgReceived(object sender, MsgReceivedEventArgs<RobotPoseMessage> e) { if (e.message == null) return; pose = e.message.Pose; protoPose.x = pose.x; protoPose.y = pose.y; protoPose.z = pose.z; protoPose.yaw = pose.yaw; protoPose.pitch = pose.pitch; protoPose.roll = pose.roll; protoPose.timeStamp = pose.TimeStamp; protoPoseServer.SendUnreliably(protoPose); // Clear the screen Console.Clear(); Console.SetCursorPosition(Console.CursorLeft, Console.CursorTop); Console.WriteLine("Robot pose:"); Console.WriteLine("X:\t" + pose.x); Console.WriteLine("Y:\t" + pose.y); Console.WriteLine("Z:\t" + pose.z); Console.WriteLine("yaw:\t" + pose.yaw); Console.WriteLine("pitch:\t" + pose.pitch); Console.WriteLine("roll:\t" + pose.roll); Console.WriteLine("timestamp:\t" + pose.timestamp); }
void indivTargetClient_MsgReceived(object sender, MsgReceivedEventArgs <TargetMessage> e) { //if (e.message.RobotID == 3) return; for (int i = 0; i < e.message.TargetState.Count; i++) { int targetID = e.message.RobotID * 1000 + i; lock (locker) { if (!targetIDToInformation.ContainsKey(targetID)) { targetIDToInformation.Add(targetID, new TargetInformation(targetID, e.message.TargetState[i], e.message.TargetCov[i], e.message.LastRobotPose[i], e.message.TargetTypes[i])); } else { targetIDToInformation[targetID].targetState = e.message.TargetState[i]; targetIDToInformation[targetID].targetCov = e.message.TargetCov[i]; targetIDToInformation[targetID].detectedPose = e.message.LastRobotPose[i]; if (targetIDToInformation[targetID].type == TargetTypes.Unconfirmed) { targetIDToInformation[targetID].type = e.message.TargetTypes[i]; } } } } }
private void onMessageReceived(object sender, MsgReceivedEventArgs e) { if (e.Sender == ActiveContact.Login) { chatbox1.AddMessage(e.Message.Data, e.Message.SentTime.ToString("MM/dd HH:mm")); } }
void simIMUClient_MsgReceived(object sender, MsgReceivedEventArgs <SimMessage <IMUData> > e) { e.message.Message.DataType = "IMU"; if (IMUUpdate != null) { IMUUpdate(sender, new TimestampedEventArgs <IMUData>(e.message.Timestamp, e.message.Message)); } }
void localMapRequestClient_MsgReceived(object sender, MsgReceivedEventArgs <LocalMapRequestMessage> e) { if (e.message == null) { return; } UpdateMapDataMessage mapToSend = Diff(e.message.RobotID, globalOcGrid, e.message.CurrentPose, e.message.ExtentX, e.message.ExtentY); localMapReponseServer.SendUnreliably(mapToSend); }
private void OnReceived(object sender, MqttMsgPublishEventArgs e) { var message = new MsgReceivedEventArgs { Value = Encoding.Default.GetString((e).Message), Chanel = GetChanel(e.Topic) }; MsgReceived(sender, message); }
void filterLidarClient_MsgReceived(object sender, MsgReceivedEventArgs <LidarFilterPackageMessage> e) { // only update with SICK lidar, which has ID = 1 if (e.message.LidarScan.ScannerID != 1) { return; } // Update ocGridLogOdd.SetRobotPose(e.message.Pose.ToRobotPose()); ocGridLogOdd.SetLidarPose(e.message.SensorPose); ocGridLogOdd.UpdateLidarScan(e.message.LidarScan); }
void clearTargetListClient_MsgReceived(object sender, MsgReceivedEventArgs<ClearTargetListMessage> e) { if (e.message.RobotID == 0) { robotIDToPackage.Clear(); targetIDToInformation.Clear(); associatedTargetID2Information.Clear(); missingTargets.Clear(); Console.WriteLine("Clear Targets message received! and cleared!"); } else Console.WriteLine("Clear Targets message received! but didn't do anything."); }
void clearTargetListClient_MsgReceived(object sender, MsgReceivedEventArgs <ClearTargetListMessage> e) { if (e.message.RobotID == 0) { robotIDToPackage.Clear(); targetIDToInformation.Clear(); associatedTargetID2Information.Clear(); missingTargets.Clear(); Console.WriteLine("Clear Targets message received! and cleared!"); } else { Console.WriteLine("Clear Targets message received! but didn't do anything."); } }
public void SensorManager_ConfigListener_MsgReceived() { var json = @"{'" + nameof(AppSettings.Room) + "':'bogus', '" + nameof(AppSettings.BrokerUrl) + "':'sugob'}"; var args = new MsgReceivedEventArgs() { Message = json, DupFlag = false, QosLevel = QosLevel.ExactlyOnce, Retain = false, Topic = "blah/Moreblah/what", }; moqFactory.Listener.Raise(x => x.MsgReceived += null, args); Assert.AreEqual("bogus", settings.Room); Assert.AreEqual("sugob", settings.BrokerUrl); VerifyUpdateFromSettings(true, settings); }
void confirmOOIClient_MsgReceived(object sender, MsgReceivedEventArgs <ConfirmOOIMessage> e) { lock (locker) { // when confirmation gets received, update the target information we have in TargetListPackage if (robotIDToPackage.ContainsKey(e.message.RobotID) && robotIDToPackage[e.message.RobotID].targetType.ContainsKey(e.message.TargetId)) { targetIDToInformation[e.message.TargetId].type = e.message.TargetType; robotIDToPackage[e.message.RobotID].targetType[e.message.TargetId] = e.message.TargetType; // update the information } // also update the associated targetID with the confirmation if (associationDictionary.ContainsKey(e.message.TargetId)) { int associatedTargetIdx = associationDictionary[e.message.TargetId] % 1000; int robotID = associationDictionary[e.message.TargetId] / 1000; robotIDToPackage[robotID].targetType[associatedTargetIdx] = e.message.TargetType; } } }
void confirmOOIClient_MsgReceived(object sender, MsgReceivedEventArgs<ConfirmOOIMessage> e) { lock (locker) { // when confirmation gets received, update the target information we have in TargetListPackage if (robotIDToPackage.ContainsKey(e.message.RobotID) && robotIDToPackage[e.message.RobotID].targetType.ContainsKey(e.message.TargetId)) { targetIDToInformation[e.message.TargetId].type = e.message.TargetType; robotIDToPackage[e.message.RobotID].targetType[e.message.TargetId] = e.message.TargetType; // update the information } // also update the associated targetID with the confirmation if (associationDictionary.ContainsKey(e.message.TargetId)) { int associatedTargetIdx = associationDictionary[e.message.TargetId] % 1000; int robotID = associationDictionary[e.message.TargetId] / 1000; robotIDToPackage[robotID].targetType[associatedTargetIdx] = e.message.TargetType; } } }
void lidarFilterPackageClient_MsgReceived(object sender, MsgReceivedEventArgs <LidarFilterPackageMessage> e) { // measurement rejection for unreasonable covariance matrix. //if (e.message.Pose.Covariance.Determinant <= 0) //{ // Console.WriteLine("GlobalGaussianMixMapQ.cs: Invalid Covariance..!! Ignore."); // return; //} // measurement update lock (locker) { if (e.message.Pose != null && e.message.LidarScan != null && e.message.SensorPose != null) { if (!robotIDToSensorPose.ContainsKey(e.message.RobotID)) { robotIDToPose.Add(e.message.RobotID, e.message.Pose); //gaussianMixMapAlgorithm.UpdateCovariance(e.message.Pose.covariance); robotIDToSensorPose.Add(e.message.RobotID, e.message.SensorPose); robotIDToScan.Add(e.message.RobotID, e.message.LidarScan); robotIDToTimestamp.Add(e.message.RobotID, e.message.LidarScan.Timestamp); robotIDToPastTimestamp.Add(e.message.RobotID, 0); // each robot's global OCgrid globalOcGridByEachRobot.Add(e.message.RobotID, new OccupancyGrid2D(globalOcGrid)); globalOcGridByEachRobotAlgorithm.Add(e.message.RobotID, new GaussianMixMappingQ(globalOcGridByEachRobot[e.message.RobotID], e.message.SensorPose, 4)); } robotIDToSensorPose[e.message.RobotID] = e.message.SensorPose; robotIDToPose[e.message.RobotID] = e.message.Pose; robotIDToScan[e.message.RobotID] = e.message.LidarScan; robotIDToTimestamp[e.message.RobotID] = e.message.Pose.timestamp; //gaussianMixMapAlgorithm.UpdateCovariance(e.message.Pose.covariance); } if (!otherRobotPolygon.ContainsKey(e.message.RobotID)) { otherRobotPolygon.Add(e.message.RobotID, Polygon.VehiclePolygonWithRadius(0.5, e.message.Pose.ToVector2())); } else { otherRobotPolygon[e.message.RobotID] = Polygon.VehiclePolygonWithRadius(0.5, e.message.Pose.ToVector2()); } } }
void lidarPosePackageClient_MsgReceived(object sender, MsgReceivedEventArgs <LidarPosePackageMessage> e) { // measurement rejection for unreasonable covariance matrix. Matrix covariance = e.message.Pose.covariance; for (int i = 0; i < 6; i++) { if (covariance[i, i] == 0) { covariance[i, i] = 0.01; } } if (covariance.Determinant <= 0) { return; } // measurement update lock (locker) { if (e.message.Pose != null && e.message.LidarScan != null && e.message.SensorPose != null) { if (!robotIDToSensorPose.ContainsKey(e.message.RobotID)) { robotIDToPose.Add(e.message.RobotID, e.message.Pose); //gaussianMixMapAlgorithm.UpdateCovariance(e.message.Pose.covariance); robotIDToSensorPose.Add(e.message.RobotID, e.message.SensorPose); robotIDToScan.Add(e.message.RobotID, e.message.LidarScan); robotIDToTimestamp.Add(e.message.RobotID, e.message.LidarScan.Timestamp); robotIDToPastTimestamp.Add(e.message.RobotID, 0); // each robot's global OCgrid globalOcGridByEachRobot.Add(e.message.RobotID, new OccupancyGrid2D(globalOcGrid)); globalOcGridByEachRobotAlgorithm.Add(e.message.RobotID, new GaussianMixMapping(globalOcGridByEachRobot[e.message.RobotID], e.message.SensorPose)); } robotIDToSensorPose[e.message.RobotID] = e.message.SensorPose; robotIDToPose[e.message.RobotID] = e.message.Pose; robotIDToScan[e.message.RobotID] = e.message.LidarScan; robotIDToTimestamp[e.message.RobotID] = e.message.LidarScan.Timestamp; //gaussianMixMapAlgorithm.UpdateCovariance(e.message.Pose.covariance); } } }
/// <summary> /// Occurs when a message was received from a peer /// </summary> /// <param name="sender">Sender of the event</param> /// <param name="e">Event arguments</param> private void TransportManagerOnMsgReceived(object sender, MsgReceivedEventArgs e) { var dataString = Encoding.UTF8.GetString(e.Message); var obj = JsonConvert.DeserializeObject <MessageContainer>(dataString); _logger.Information("Received {MessageType} message from {IpAddress}", obj.MessageType, e.RemoteIp); Task.Run(() => { switch (obj.MessageType) { case MessageType.NewBlockMined: HandleNewMinedBlock(obj); break; case MessageType.RequestLastBlock: HandleLastBlockRequest(e.RemoteIp); break; case MessageType.LastKnownBlock: HandleLastBlockResponse(obj, e.RemoteIp); break; case MessageType.RequestFullChain: HandleRequestForFullChain(e.RemoteIp); break; case MessageType.FullChain: HandleReceiptOfFullChain(obj); break; case MessageType.BlockToMine: HandleBlockToMine(obj, e.RemoteIp); break; } }); }
private void NewPeer_MsgReceived(object sender, MsgReceivedEventArgs e) { MsgReceived?.Invoke(this, e); }
void targetNoImageClient_MsgReceived(object sender, MsgReceivedEventArgs<TargetListNoImageMessage> e) { int robotID = e.message.RobotID; //if (robotID == 3) return; if (!robotIDToPackage.ContainsKey(robotID)) robotIDToPackage.Add(robotID, new TargetListPackage()); for (int i = 0; i < e.message.TargetIDs.Count; i++) { lock (locker) { int targetID = e.message.TargetIDs[i]; if (!robotIDToPackage[robotID].timeStamp.ContainsKey(targetID)) // if data doesn't exist yet { robotIDToPackage[robotID].timeStamp.Add(targetID, e.message.TimeStamp); robotIDToPackage[robotID].pixelLTCorner.Add(targetID, e.message.PixelLTCorner[i]); robotIDToPackage[robotID].pixelRBCorner.Add(targetID, e.message.PixelRBCorner[i]); robotIDToPackage[robotID].targetType.Add(targetID, TargetTypes.Unconfirmed); robotIDToPackage[robotID].robotID = robotID; robotIDToPackage[robotID].lidarScan.Add(targetID, e.message.LidarScan); } robotIDToPackage[robotID].timeStamp[targetID] = e.message.TimeStamp; robotIDToPackage[robotID].pixelLTCorner[targetID] = e.message.PixelLTCorner[i]; robotIDToPackage[robotID].pixelRBCorner[targetID] = e.message.PixelRBCorner[i]; robotIDToPackage[robotID].robotID = robotID; robotIDToPackage[robotID].lidarScan[targetID] = e.message.LidarScan; // checking missing stuff if (!targetIDfromTracker.Contains(targetID)) targetIDfromTracker.Add(targetID); } } #region oldcode - remove when done //if (!robotIDToPackage.ContainsKey(robotID)) //{ // TargetListPackage package = new TargetListPackage(); // package.robotID = e.message.RobotID; // package.pixelLTCorner = e.message.PixelLTCorner; // package.pixelRBCorner = e.message.PixelRBCorner; // package.timeStamp = e.message.TimeStamp; // robotIDToPackage.Add(robotID, package); //} //else //{ // robotIDToPackage[robotID].robotID = e.message.RobotID; // robotIDToPackage[robotID].pixelLTCorner = e.message.PixelLTCorner; // robotIDToPackage[robotID].pixelRBCorner = e.message.PixelRBCorner; // robotIDToPackage[robotID].timeStamp = e.message.TimeStamp; //} //List<int> targetID = new List<int>(e.message.PixelLTCorner.Count); //for (int i = 0; i < targetID.Capacity; i++) // targetID.Add(robotID * 1000 + i); // target ID will be assigned as robotNumber * 1000 + localTargetID. i.e) for target 3 for Robot2 = 2003 //robotIDToPackage[robotID].targetID = targetID; #endregion }
void localMapRequestClient_MsgReceived(object sender, MsgReceivedEventArgs<LocalMapRequestMessage> e) { if (e.message == null) return; UpdateMapDataMessage mapToSend = Diff(e.message.RobotID, gaussianMixMapAlgorithm.UhatGM, e.message.CurrentPose, e.message.ExtentX, e.message.ExtentY); if (mapToSend != null) { int segmentLength = 2000; int numIteration = mapToSend.CellData.Count / segmentLength; int numLeftOver = mapToSend.CellData.Count - (numIteration * segmentLength); int lastIndex = 0; for (int i = 0; i < numIteration + 1; i++) { if (i == numIteration) { localMapResponseServer.SendUnreliably(new UpdateMapDataMessage(mapToSend.CellData.GetRange(lastIndex, numLeftOver))); } else { localMapResponseServer.SendUnreliably(new UpdateMapDataMessage(mapToSend.CellData.GetRange(lastIndex, segmentLength))); lastIndex += segmentLength; } } } }
void lidarFilterPackageClient_MsgReceived(object sender, MsgReceivedEventArgs<LidarFilterPackageMessage> e) { // measurement rejection for unreasonable covariance matrix. //if (e.message.Pose.Covariance.Determinant <= 0) //{ // Console.WriteLine("GlobalGaussianMixMapQ.cs: Invalid Covariance..!! Ignore."); // return; //} // measurement update lock (locker) { if (e.message.Pose != null && e.message.LidarScan != null && e.message.SensorPose != null) { if (!robotIDToSensorPose.ContainsKey(e.message.RobotID)) { robotIDToPose.Add(e.message.RobotID, e.message.Pose); //gaussianMixMapAlgorithm.UpdateCovariance(e.message.Pose.covariance); robotIDToSensorPose.Add(e.message.RobotID, e.message.SensorPose); robotIDToScan.Add(e.message.RobotID, e.message.LidarScan); robotIDToTimestamp.Add(e.message.RobotID, e.message.LidarScan.Timestamp); robotIDToPastTimestamp.Add(e.message.RobotID, 0); // each robot's global OCgrid globalOcGridByEachRobot.Add(e.message.RobotID, new OccupancyGrid2D(globalOcGrid)); globalOcGridByEachRobotAlgorithm.Add(e.message.RobotID, new GaussianMixMappingQ(globalOcGridByEachRobot[e.message.RobotID], e.message.SensorPose, 4)); } robotIDToSensorPose[e.message.RobotID] = e.message.SensorPose; robotIDToPose[e.message.RobotID] = e.message.Pose; robotIDToScan[e.message.RobotID] = e.message.LidarScan; robotIDToTimestamp[e.message.RobotID] = e.message.Pose.timestamp; //gaussianMixMapAlgorithm.UpdateCovariance(e.message.Pose.covariance); } if (!otherRobotPolygon.ContainsKey(e.message.RobotID)) otherRobotPolygon.Add(e.message.RobotID, Polygon.VehiclePolygonWithRadius(0.5, e.message.Pose.ToVector2())); else otherRobotPolygon[e.message.RobotID] = Polygon.VehiclePolygonWithRadius(0.5, e.message.Pose.ToVector2()); } }
void queryClient_MsgReceived(object sender, MsgReceivedEventArgs <TargetQueryMessage> e) { int robotIDRequested = e.message.RobotID; // this would be just zero... now let's reassign this. Bitmap gg; if (robotIDToPackage.Count == 0) // send anything. The robot ID would be zero. //if (!robotIDToPackage.ContainsKey(robotIDRequested)) { Console.WriteLine("No detection found yet"); string fileLocation = Directory.GetParent(Directory.GetParent(Directory.GetCurrentDirectory()).ToString()) + "\\noDetection.png"; gg = new Bitmap(fileLocation); RobotImage noImg = new RobotImage(robotIDRequested, gg, 0.0); targetListServer.SendUnreliably(new TargetListMessage(robotIDRequested, noImg, 0.0, -1, TargetTypes.Junk)); return; } int targetID = 0; //robotIDRequested = (int)Math.Ceiling(r.NextDouble() * robotIDToPackage.Count); //List<int> robotIDList = robotIDToPackage.Keys.ToList(); robotIDRequested = robotIDToPackage.Keys.ToList()[(int)Math.Floor(r.NextDouble() * robotIDToPackage.Count)]; RobotImage img = robotIDToPackage[robotIDRequested].GetUnconfirmedRobotImage(ref targetID, missingTargets); if (targetID == 0) { Console.WriteLine("TargetID is not valid."); } if (img == null) { Console.WriteLine("No Image Queue to process OR no more unconfirmed image"); string fileLocation = Directory.GetParent(Directory.GetParent(Directory.GetCurrentDirectory()).ToString()) + "\\allConfirmed.png"; gg = new Bitmap(fileLocation); RobotImage noImg = new RobotImage(robotIDRequested, gg, 0.0); targetListServer.SendUnreliably(new TargetListMessage(robotIDRequested, noImg, 0.0, -1, TargetTypes.Junk)); return; } else { gg = new Bitmap(img.image.Clone() as Image); } //if (targetIDToInformation[targetID].targetCov[0, 0] > 0.5 && targetIDToInformation[targetID].targetCov[1, 1] > 0.5) //{ // Console.WriteLine("No Good Target with small covariance"); // string fileLocation = Directory.GetParent(Directory.GetParent(Directory.GetCurrentDirectory()).ToString()) + "\\noGoodTarget.png"; // gg = new Bitmap(fileLocation); // RobotImage noImg = new RobotImage(robotIDRequested, gg, 0.0); // targetListServer.SendUnreliably(new TargetListMessage(robotIDRequested, noImg, 0.0, -1)); // return; //} Graphics g = Graphics.FromImage(gg); Vector2 RBCorner = robotIDToPackage[robotIDRequested].pixelRBCorner[targetID]; Vector2 LTCorner = robotIDToPackage[robotIDRequested].pixelLTCorner[targetID]; //if (e.message.RobotID == 1) g.DrawRectangle(new Pen(Color.Red, 3.0f), new Rectangle((int)(LTCorner.X / 2 * 1) - 10, (int)((LTCorner.Y / 2 * 1) - 10), (int)(RBCorner.X / 2 - LTCorner.X / 2) * 1 + 10, (int)((RBCorner.Y - LTCorner.Y) / 2 * 1 + 10))); /* * else * g.DrawRectangle(new Pen(Color.Red, 3.0f), new Rectangle((int)LTCorner.X - 10, (int)LTCorner.Y - 10, * (int)(RBCorner.X - LTCorner.X) + 10, (int)(RBCorner.Y - LTCorner.Y) + 10)); */ Bitmap toSend = new Bitmap(160, 40); Graphics gr = Graphics.FromImage(toSend); gr.DrawImage(gg, 0, 0, 160, 40); RobotImage rImg = new RobotImage(img.robotID, toSend, img.timeStamp); targetListServer.SendUnreliably(new TargetListMessage(robotIDRequested, rImg, robotIDToPackage[robotIDRequested].timeStamp[targetID], targetID, targetIDToInformation[targetID].targetState.ToVector2(), targetIDToInformation[targetID].targetCov, targetIDToInformation[targetID].type)); detectionStateScanServer.SendUnreliably(new LidarPoseTargetMessage(robotIDRequested, targetID, targetIDToInformation[targetID].detectedPose, robotIDToPackage[robotIDRequested].lidarScan[targetID])); }
void targetNoImageClient_MsgReceived(object sender, MsgReceivedEventArgs <TargetListNoImageMessage> e) { int robotID = e.message.RobotID; //if (robotID == 3) return; if (!robotIDToPackage.ContainsKey(robotID)) { robotIDToPackage.Add(robotID, new TargetListPackage()); } for (int i = 0; i < e.message.TargetIDs.Count; i++) { lock (locker) { int targetID = e.message.TargetIDs[i]; if (!robotIDToPackage[robotID].timeStamp.ContainsKey(targetID)) // if data doesn't exist yet { robotIDToPackage[robotID].timeStamp.Add(targetID, e.message.TimeStamp); robotIDToPackage[robotID].pixelLTCorner.Add(targetID, e.message.PixelLTCorner[i]); robotIDToPackage[robotID].pixelRBCorner.Add(targetID, e.message.PixelRBCorner[i]); robotIDToPackage[robotID].targetType.Add(targetID, TargetTypes.Unconfirmed); robotIDToPackage[robotID].robotID = robotID; robotIDToPackage[robotID].lidarScan.Add(targetID, e.message.LidarScan); } robotIDToPackage[robotID].timeStamp[targetID] = e.message.TimeStamp; robotIDToPackage[robotID].pixelLTCorner[targetID] = e.message.PixelLTCorner[i]; robotIDToPackage[robotID].pixelRBCorner[targetID] = e.message.PixelRBCorner[i]; robotIDToPackage[robotID].robotID = robotID; robotIDToPackage[robotID].lidarScan[targetID] = e.message.LidarScan; // checking missing stuff if (!targetIDfromTracker.Contains(targetID)) { targetIDfromTracker.Add(targetID); } } } #region oldcode - remove when done //if (!robotIDToPackage.ContainsKey(robotID)) //{ // TargetListPackage package = new TargetListPackage(); // package.robotID = e.message.RobotID; // package.pixelLTCorner = e.message.PixelLTCorner; // package.pixelRBCorner = e.message.PixelRBCorner; // package.timeStamp = e.message.TimeStamp; // robotIDToPackage.Add(robotID, package); //} //else //{ // robotIDToPackage[robotID].robotID = e.message.RobotID; // robotIDToPackage[robotID].pixelLTCorner = e.message.PixelLTCorner; // robotIDToPackage[robotID].pixelRBCorner = e.message.PixelRBCorner; // robotIDToPackage[robotID].timeStamp = e.message.TimeStamp; //} //List<int> targetID = new List<int>(e.message.PixelLTCorner.Count); //for (int i = 0; i < targetID.Capacity; i++) // targetID.Add(robotID * 1000 + i); // target ID will be assigned as robotNumber * 1000 + localTargetID. i.e) for target 3 for Robot2 = 2003 //robotIDToPackage[robotID].targetID = targetID; #endregion }
void simLidarPosePackageClient_MsgReceived(object sender, MsgReceivedEventArgs <SimMessage <LidarPosePackageMessage> > e) { lidarPosePackageClient_MsgReceived(sender, new MsgReceivedEventArgs <LidarPosePackageMessage>(e.message.Message, e.sender)); }
void queryClient_MsgReceived(object sender, MsgReceivedEventArgs<TargetQueryMessage> e) { int robotIDRequested = e.message.RobotID; // this would be just zero... now let's reassign this. Bitmap gg; if (robotIDToPackage.Count == 0) // send anything. The robot ID would be zero. //if (!robotIDToPackage.ContainsKey(robotIDRequested)) { Console.WriteLine("No detection found yet"); string fileLocation = Directory.GetParent(Directory.GetParent(Directory.GetCurrentDirectory()).ToString()) + "\\noDetection.png"; gg = new Bitmap(fileLocation); RobotImage noImg = new RobotImage(robotIDRequested, gg, 0.0); targetListServer.SendUnreliably(new TargetListMessage(robotIDRequested, noImg, 0.0, -1, TargetTypes.Junk)); return; } int targetID = 0; //robotIDRequested = (int)Math.Ceiling(r.NextDouble() * robotIDToPackage.Count); //List<int> robotIDList = robotIDToPackage.Keys.ToList(); robotIDRequested = robotIDToPackage.Keys.ToList()[(int)Math.Floor(r.NextDouble() * robotIDToPackage.Count)]; RobotImage img = robotIDToPackage[robotIDRequested].GetUnconfirmedRobotImage(ref targetID, missingTargets); if (targetID == 0) Console.WriteLine("TargetID is not valid."); if (img == null) { Console.WriteLine("No Image Queue to process OR no more unconfirmed image"); string fileLocation = Directory.GetParent(Directory.GetParent(Directory.GetCurrentDirectory()).ToString()) + "\\allConfirmed.png"; gg = new Bitmap(fileLocation); RobotImage noImg = new RobotImage(robotIDRequested, gg, 0.0); targetListServer.SendUnreliably(new TargetListMessage(robotIDRequested, noImg, 0.0, -1, TargetTypes.Junk)); return; } else gg = new Bitmap(img.image.Clone() as Image); //if (targetIDToInformation[targetID].targetCov[0, 0] > 0.5 && targetIDToInformation[targetID].targetCov[1, 1] > 0.5) //{ // Console.WriteLine("No Good Target with small covariance"); // string fileLocation = Directory.GetParent(Directory.GetParent(Directory.GetCurrentDirectory()).ToString()) + "\\noGoodTarget.png"; // gg = new Bitmap(fileLocation); // RobotImage noImg = new RobotImage(robotIDRequested, gg, 0.0); // targetListServer.SendUnreliably(new TargetListMessage(robotIDRequested, noImg, 0.0, -1)); // return; //} Graphics g = Graphics.FromImage(gg); Vector2 RBCorner = robotIDToPackage[robotIDRequested].pixelRBCorner[targetID]; Vector2 LTCorner = robotIDToPackage[robotIDRequested].pixelLTCorner[targetID]; //if (e.message.RobotID == 1) g.DrawRectangle(new Pen(Color.Red, 3.0f), new Rectangle((int)(LTCorner.X / 2 * 1) - 10, (int)((LTCorner.Y / 2 * 1) - 10), (int)(RBCorner.X / 2 - LTCorner.X / 2) * 1 + 10, (int)((RBCorner.Y - LTCorner.Y) / 2 * 1 + 10))); /* * else g.DrawRectangle(new Pen(Color.Red, 3.0f), new Rectangle((int)LTCorner.X - 10, (int)LTCorner.Y - 10, (int)(RBCorner.X - LTCorner.X) + 10, (int)(RBCorner.Y - LTCorner.Y) + 10)); */ Bitmap toSend = new Bitmap(160, 40); Graphics gr = Graphics.FromImage(toSend); gr.DrawImage(gg, 0, 0, 160, 40); RobotImage rImg = new RobotImage(img.robotID, toSend, img.timeStamp); targetListServer.SendUnreliably(new TargetListMessage(robotIDRequested, rImg, robotIDToPackage[robotIDRequested].timeStamp[targetID], targetID, targetIDToInformation[targetID].targetState.ToVector2(), targetIDToInformation[targetID].targetCov, targetIDToInformation[targetID].type)); detectionStateScanServer.SendUnreliably(new LidarPoseTargetMessage(robotIDRequested, targetID, targetIDToInformation[targetID].detectedPose, robotIDToPackage[robotIDRequested].lidarScan[targetID])); }
void simSegwayFeedback_MsgReceived(object sender, MsgReceivedEventArgs<SimMessage<IRobotTwoWheelStatus>> e) { if (e.message.RobotID == robotID && WheelPositionUpdate != null) WheelPositionUpdate(this, new TimestampedEventArgs<IRobotTwoWheelStatus>(e.message.Timestamp, e.message.Message)); }
void simIMUClient_MsgReceived(object sender, MsgReceivedEventArgs<SimMessage<IMUData>> e) { e.message.Message.DataType = "IMU"; if (IMUUpdate != null) IMUUpdate(sender, new TimestampedEventArgs<IMUData>(e.message.Timestamp, e.message.Message)); }
void simLidarPosePackageClient_MsgReceived(object sender, MsgReceivedEventArgs<SimMessage<LidarPosePackageMessage>> e) { lidarPosePackageClient_MsgReceived(sender, new MsgReceivedEventArgs<LidarPosePackageMessage>(e.message.Message, e.sender)); }
void indivTargetClient_MsgReceived(object sender, MsgReceivedEventArgs<TargetMessage> e) { //if (e.message.RobotID == 3) return; for (int i = 0; i < e.message.TargetState.Count; i++) { int targetID = e.message.RobotID * 1000 + i; lock (locker) { if (!targetIDToInformation.ContainsKey(targetID)) { targetIDToInformation.Add(targetID, new TargetInformation(targetID, e.message.TargetState[i], e.message.TargetCov[i], e.message.LastRobotPose[i], e.message.TargetTypes[i])); } else { targetIDToInformation[targetID].targetState = e.message.TargetState[i]; targetIDToInformation[targetID].targetCov = e.message.TargetCov[i]; targetIDToInformation[targetID].detectedPose = e.message.LastRobotPose[i]; if (targetIDToInformation[targetID].type == TargetTypes.Unconfirmed) targetIDToInformation[targetID].type = e.message.TargetTypes[i]; } } } }
void lidarPosePackageClient_MsgReceived(object sender, MsgReceivedEventArgs<LidarPosePackageMessage> e) { // measurement rejection for unreasonable covariance matrix. Matrix covariance = e.message.Pose.covariance; for (int i = 0; i < 6; i++) { if (covariance[i, i] == 0) covariance[i, i] = 0.01; } if (covariance.Determinant <= 0) return; // measurement update lock (locker) { if (e.message.Pose != null && e.message.LidarScan != null && e.message.SensorPose != null) { if (!robotIDToSensorPose.ContainsKey(e.message.RobotID)) { robotIDToPose.Add(e.message.RobotID, e.message.Pose); //gaussianMixMapAlgorithm.UpdateCovariance(e.message.Pose.covariance); robotIDToSensorPose.Add(e.message.RobotID, e.message.SensorPose); robotIDToScan.Add(e.message.RobotID, e.message.LidarScan); robotIDToTimestamp.Add(e.message.RobotID, e.message.LidarScan.Timestamp); robotIDToPastTimestamp.Add(e.message.RobotID, 0); // each robot's global OCgrid globalOcGridByEachRobot.Add(e.message.RobotID, new OccupancyGrid2D(globalOcGrid)); globalOcGridByEachRobotAlgorithm.Add(e.message.RobotID, new GaussianMixMapping(globalOcGridByEachRobot[e.message.RobotID], e.message.SensorPose)); } robotIDToSensorPose[e.message.RobotID] = e.message.SensorPose; robotIDToPose[e.message.RobotID] = e.message.Pose; robotIDToScan[e.message.RobotID] = e.message.LidarScan; robotIDToTimestamp[e.message.RobotID] = e.message.LidarScan.Timestamp; //gaussianMixMapAlgorithm.UpdateCovariance(e.message.Pose.covariance); } } }
void localMapRequestClient_MsgReceived(object sender, MsgReceivedEventArgs<LocalMapRequestMessage> e) { if (e.message == null) return; UpdateMapDataMessage mapToSend = Diff(e.message.RobotID, globalOcGrid, e.message.CurrentPose, e.message.ExtentX, e.message.ExtentY); localMapReponseServer.SendUnreliably(mapToSend); }
private void IncomingMsg(object sender, MsgReceivedEventArgs e) { //send message out MsgReceived?.Invoke(this, e); }
private void clientHandler_MsgReceived(object sender, MsgReceivedEventArgs e) { SendMessageToAllAsync(new Message { Data = e, MessageType = MessageType.Text }).ConfigureAwait(false); }