public static string AgentPath(MapAgentPath path, bool addBoundingBoxex = true)
{
var result = new GeoJsonMultiLine();
for(int i = 0; i < path.Points.Count - 2; i++)
{
var p0 = path.Points[i];
var p1 = path.Points[i + 1];
var segment = new List<List<double>>
{
new List<double> { p0.Lon, p0.Lat },
new List<double> { p1.Lon, p1.Lat }
};
result.Coordinates.Add(segment);
}
if (addBoundingBoxex)
{
foreach (var tileId in path.TileIds)
{
var bbox = new List<List<double>>();
var (lon0, lat0, lon1, lat1) = TileMath.GetTileBounds(tileId, WorldGraph.Zoom);
bbox.Add(new List<double> { lon0, lat0 });
bbox.Add(new List<double> { lon1, lat0 });
bbox.Add(new List<double> { lon1, lat1 });
bbox.Add(new List<double> { lon0, lat1 });
bbox.Add(new List<double> { lon0, lat0 });
result.Coordinates.Add(bbox);
}
}
return JsonSerializer.Serialize(result, SerializerOptions);
}
public async Task UpdateAgent(string agentId, AgentCommand command)
{
_logger.LogInformation($"Updating agent {agentId}");
string agentJson = await _blobStoreService.GetText($"agents/{agentId}.json").ConfigureAwait(false);
if (string.IsNullOrWhiteSpace(agentJson))
{
throw new ArgumentException($"Unknown agent: {agentId}");
}
var agent = JsonSerializer.Deserialize <MapAgent>(agentJson);
string pathJson = await _blobStoreService.GetText(BuildPathPath(agentId), throwIfNotFound : false).ConfigureAwait(false);
var oldPath = string.IsNullOrWhiteSpace(pathJson) ? new MapAgentPath() : JsonSerializer.Deserialize <MapAgentPath>(pathJson);
var newPath = new MapAgentPath();
long pathUnixMs = GeoMath.UnixMs();
long pathMs = 0;
double pathMeters = 0;
// Keep all points that are in the future - and one point in the past so we can interpolate the line segment
List <GeoCoord> visitedPoints = new List <GeoCoord>();
long msPrev = -1;
for (int i = 0; i < oldPath.PointAbsTimestampMs.Count; ++i)
{
bool pointIsInTheFuture = oldPath.PointAbsTimestampMs[i] >= pathUnixMs;
bool nextPointIsInTheFuture = i < oldPath.PointAbsTimestampMs.Count - 1 && oldPath.PointAbsTimestampMs[i + 1] >= pathUnixMs;
bool keepPoint = pointIsInTheFuture || nextPointIsInTheFuture;
if (keepPoint)
{
pathUnixMs = oldPath.PointAbsTimestampMs[i];
newPath.PointAbsTimestampMs.Add(pathUnixMs);
newPath.Points.Add(oldPath.Points[i]);
long msCurrent = oldPath.PointAbsTimestampMs[i];
if (msPrev != -1)
{
long segmentMs = msCurrent - msPrev;
pathMs += segmentMs;
}
msPrev = msCurrent;
}
else
{
visitedPoints.Add(oldPath.Points[i]);
}
}
// Make sure all visited lines are drawn by adding the points that are cut off.
if (newPath.Points.Count > 1)
{
visitedPoints.Add(newPath.Points[0]);
visitedPoints.Add(newPath.Points[1]);
}
await _mapCoverage.UpdateCoverage(visitedPoints).ConfigureAwait(false);
{
// If stuck, clear newPath and add a single point at or near a valid location. Then run update once from Cmd. A new valid path should now be written.
//newPath.Points.Clear();
//newPath.Points.Add(new GeoCoord(6.598364, 50.078052));
//newPath.PointAbsTimestampMs.Clear();
//newPath.PointAbsTimestampMs.Add(GeoMath.UnixMs());
}
if (newPath.Points.Count == 0)
{
// Path is out of date, nothing to keep. Reuse latest known position.
int oldCount = oldPath.Points.Count;
newPath.Points.Clear();
newPath.PointAbsTimestampMs.Clear();
newPath.Points.Add(oldPath.Points[oldCount - 1]);
newPath.Points.Add(oldPath.Points[oldCount - 1]);
newPath.PointAbsTimestampMs.Add(GeoMath.UnixMs());
}
// Start path here
var startPoint = newPath.Points.Last();
var startNode = await _worldGraph.GetNearbyNode(startPoint).ConfigureAwait(false);
List <GeoCoord> deadEndDebugSegments = new List <GeoCoord>();
List <GeoCoord> possibleWayoutDebugSegments = new List <GeoCoord>();
double prevBearing = double.MaxValue;
List <WayTileNode> conn = null;
WayTileNode prevNode = null;
WayTileNode node = startNode;
WayTileNode nextNode = null;
while (true)
{
conn = await _worldGraph.GetNodeConnections(node).ConfigureAwait(false);
var options = conn.Select(x => new Option {
Node = x
}).ToList();
// Don't go back if we can go forward
if (options.Count > 1)
{
options = options.Where(x => x.Node != prevNode).ToList();
}
foreach (var option in options)
{
var(deadEndFound, unexploredNodeFound, visitedNodesFound, unvisitedNodesFound, totalVisitCount, exploredSegments) = await _graphPeek.Peek(node, option.Node).ConfigureAwait(false);
double bearing = GeoMath.CalculateBearing(node.Point, option.Node.Point);
option.BearingDiff = Math.Abs(bearing - prevBearing);
option.IsDeadEnd = deadEndFound;
option.UnvisitedCount = unvisitedNodesFound;
option.VisitedCount = visitedNodesFound;
option.TotalVisitCount = totalVisitCount;
option.UnvisitedPct = visitedNodesFound == 0 ? 100 : (double)unvisitedNodesFound / visitedNodesFound;
// Absolute score - number of new nodes scores high.
option.Score = (unvisitedNodesFound - totalVisitCount) + _rnd.Next(0, 5); // Add a little randomness
// Percentage score - small dead ends scores high.
//option.Score = ((unvisitedNodesFound + visitedNodesFound) / ((double)totalVisitCount + 1)) + _rnd.NextDouble() * 0.1; // Add a little randomness
}
// Scores should reflect the direction with the most unexplored nodes. Higher = better.
// If nothing was explored score = unvisitedNodesFound + visitedNodesFound
// If everything was explored exactly once, score = 1. Will go below 1 when visitcount gets bigger than 1.
// If there are no detected unexplored areas nearby, prefer to go straight ahead to cover some ground quickly.
bool preferStraight = false;
bool onlyAlreadyExploredOptions = !options.Any(x => x.Score > 1.0);
if (onlyAlreadyExploredOptions)
{
long minVisitCount = options.Min(x => x.Node.VisitCount ?? 0);
bool allEqual = options.All(x => x.Node.VisitCount == minVisitCount);
if (allEqual)
{
preferStraight = onlyAlreadyExploredOptions && _rnd.NextDouble() < 0.9;
}
else
{
// All are already explored, but some have higher count than others. Pick from nodes with lowest count.
options = options.Where(x => x.Node.VisitCount == minVisitCount).ToList();
}
}
else
{
// Pick best score
options = options.OrderByDescending(x => x.Score).ToList();
}
// Most of the time select element 0, but sometimes randomly select another
int maxIdx = options.Count > 1 ? 1 : 0;
int selectedIdx = _rnd.NextDouble() < 0.75 ? 0 : _rnd.Next(0, maxIdx + 1);
nextNode = options[selectedIdx].Node;
prevNode = node;
node = nextNode;
prevBearing = GeoMath.CalculateBearing(prevNode.Point, node.Point);
double segmentMeters = GeoMath.MetersDistanceTo(prevNode.Point, node.Point);
long segmentMs = (long)((segmentMeters / agent.MetersPerSecond) * 1000);
newPath.Points.Add(node.Point);
pathUnixMs += segmentMs;
newPath.PointAbsTimestampMs.Add(pathUnixMs);
pathMeters += segmentMeters;
pathMs += segmentMs;
if (pathMs >= 60 * 10 * 1000) // 10 minutes per path
{
break;
}
}
//if (deadEndDebugSegments.Count > 0)
//{
// string geo = GeoJsonBuilder.Segments(deadEndDebugSegments);
// await _blobStoreService.StoreText($"debug/skipped-deadends/deadends-{deadEndDebugSegments.Count}-{DateTime.UtcNow.Ticks}.json", geo).ConfigureAwait(false);
//}
//if (possibleWayoutDebugSegments.Count > 0)
//{
// string geo = GeoJsonBuilder.Segments(possibleWayoutDebugSegments);
// await _blobStoreService.StoreText($"debug/possible-way-out/wayout-{possibleWayoutDebugSegments.Count}-{DateTime.UtcNow.Ticks}.json", geo).ConfigureAwait(false);
//}
await _worldGraph.StoreUpdatedVisitCounts().ConfigureAwait(false);
newPath.PathMs = pathMs;
newPath.TileIds = _worldGraph.GetLoadedTiles().Select(tile => tile.Id).ToList();
newPath.EncodedPolyline = GooglePolylineConverter.Encode(newPath.Points);
string newPathJson = JsonSerializer.Serialize(newPath);
await _blobStoreService.StoreText(BuildPathPath(agentId), newPathJson, overwriteExisting : true).ConfigureAwait(false);
//string geoJson = GeoJsonBuilder.AgentPath(newPath);
//await _blobStoreService.StoreText(BuildGeoJsonPathPath(agentId), geoJson, overwriteExisting: true).ConfigureAwait(false);
var clientPath = new AgentClientPath
{
MsStart = newPath.PointAbsTimestampMs.First(),
MsEnd = newPath.PointAbsTimestampMs.Last(),
EncodedPolyline = newPath.EncodedPolyline
};
string clientPathJson = JsonSerializer.Serialize(clientPath);
await _blobStoreService.StoreText(BuildClientPathPath(agentId), clientPathJson, overwriteExisting : true).ConfigureAwait(false);
await TryTakeSelfie(oldPath.Points).ConfigureAwait(false);
}
