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