protected override void ReceiveMessage(OccupancyGrid message)
{
if (mapReader.visualizeMap)
{
Task.Run(() => mapReader.VisualizeMap(message));
}
}
void Start()
{
rays = new float[nSightRays];
hit = new bool[nSightRays];
occupancy = new OccupancyGrid(1024, rect);
mapTexture = new Texture2D(1024, 1024);
mapMaterial.SetTexture("_Main", mapTexture);
}
public GridMarcher(OccupancyGrid occupancyGrid, ComputeShader computeShader)
{
this.occupancyGrid = occupancyGrid;
this.marchingCube = new MarchingCube();
marchingCube.Init();
this.computeShader = computeShader;
}
public PRM(Vector3 bounds, OccupancyGrid occupancy, float radius, int neighbours, ConnectionMethod method)
{
graph = new StateConfGraph();
this.bounds = bounds;
this.occupancy = occupancy;
this.radius = radius;
this.neighbours = neighbours;
this.method = method;
this.search = new AStar();
}
public static bool[,] LoadOccupancyGrid(string pngFile, double tileSize)
{
using (var stream = File.OpenRead(pngFile))
{
using (var image = Image.FromStream(stream))
{
var bitmap = new Bitmap(image);
return(OccupancyGrid.GenerateOccupancyGrid(bitmap, tileSize));
}
}
}
public bool IsOccupied(int x, int y)
{
if (x < 0 ||
y < 0 ||
x >= OccupancyGrid.GetLength(0) ||
y >= OccupancyGrid.GetLength(1))
{
return(true);
}
return(!OccupancyGrid[x, y]);
}
//initalize from editor
//void OnValidate()
//{
// Initialize();
//}
public void Initialize()
{
meshRenderer = GetComponent <MeshRenderer>();
meshFilter = GetComponent <MeshFilter>();
var occupancyDescriptor = new NoiseOccupancyDescriptor(noiseOccupancySettings);
var occupancyGrid = new OccupancyGrid(occupancyGridSettings, occupancyDescriptor);
gridMarcher = new GridMarcher(occupancyGrid, computeShader);
meshFilter.mesh = gridMarcher.BuildMesh();
}
public void VisualizeMap(OccupancyGrid message)
{
try
{
var mapByteArray = message.data;
mapWidth = message.info.width;
mapHeight = message.info.height;
mapColorArray = new Color[mapByteArray.Length];
for (var i = 0; i < mapByteArray.Length; i++)
{
Color color;
switch (mapByteArray[i])
{
default:
case -1:
continue;
case 0:
color = new Color((float)0.5, (float)0.5, (float)0.5);
break;
case 100:
color = new Color(0, 0, 1);
break;
}
mapColorArray[i] = color;
}
setMap = true;
}
catch (Exception e)
{
Debug.Log(e.Message);
}
}
public ITrack ToTrack()
{
var tmpOccupancyGrid = OccupancyGrid.Select(line => line.ToCharArray().Select(symbol => symbol == ' ').ToArray()).ToArray();
var occupancyGrid = new bool[tmpOccupancyGrid[0].Length, tmpOccupancyGrid.Length];
for (int i = 0; i < occupancyGrid.GetLength(0); i++)
{
for (int j = 0; j < occupancyGrid.GetLength(1); j++)
{
occupancyGrid[i, j] = tmpOccupancyGrid[j][i];
}
}
return(new RaceTrack(
new DeserialziedCircuit
{
Start = new Vector(Circuit.Start.X, Circuit.Start.Y),
Radius = Circuit.Radius,
WayPoints = Circuit.WayPoints.Select(point => new RadialGoal(new Vector(point.X, point.Y), Circuit.Radius)).ToList <IGoal>(),
StartingPosition = StartingPosition
},
occupancyGrid,
TileSize));
}
public OccupancyGridRenderer(OccupancyGrid grid)
{
this.grid = grid;
this.doRender = true;
}
public GetMapResponse()
{
this.map = new OccupancyGrid();
}
private void mapcb(OccupancyGrid msg)
{
SetDimensions(msg.info.width, msg.info.height, msg.info.resolution, msg.info.origin.position, msg.info.origin.orientation);
createARGB(msg.data, ref imageData);
textureMutex.Set();
}
public GetMapResult(OccupancyGrid map)
{
this.map = map;
}
public GetMapResult()
{
this.map = new OccupancyGrid();
}
public void generateRRTObstaclesWithPath(int numNodes, OccupancyGrid occupancy, int pathSearchDepth)
{
// Create a new tree which we will populate with nodes, with the root of the tree
// set to the initial configuration
tree = new Tree(initialConfiguration);
// Loop until the graph is the requested size
for (int i = 0; i < numNodes; i++) {
bool clearConf = false;
StateConfig randConf = null;
StateConfig newConf = null;
TreeNode<StateConfig> closestNode = null;
// Generate a random configuration within the configuration space until one is
// found which does not fall in any occupied cell in the occupancy grid
while (!clearConf){
randConf = getRandomConfiguration(limits);
Tuple<float,float> pos = randConf.getPosition();
// If the random configuration is occupied, generate a new one.
if(occupancy.isOccupied(pos.first, pos.second))
continue;
// Find the closest node on the current graph to the point that was generated
closestNode = tree.findClosest(randConf, Tree.SearchMethod.BFS);
StateConfig closestConf = closestNode.content;
// Generate a new configuration by moving along the hyper-edge connecting
// the closest configuration and the randomly generated one.
newConf = closestConf + (randConf - closestConf) * increment;
Tuple<float,float> newPos = newConf.getPosition();
if (occupancy.isOccupied(newPos.first, newPos.second))
continue;
if (!occupancy.obstacleFreeStraightPath(closestConf.getPosition(), newConf.getPosition(), pathSearchDepth))
continue;
clearConf = true;
}
// Add the new configuration as a child of the closest node.
TreeNode<StateConfig> newNode = tree.addNode(newConf, closestNode);
// If we are within range of the goal with the new node, stop expanding the tree.
if (checkGoalDist(newNode))
break;
}
}
// Creates an occupancy grid and computes the occupancy for all cells.
// Also requests the camera to draw the grid on the screen.
public void constructOccupancyGrid()
{
grid = new OccupancyGrid(-xBound, -yBound, xBound * 2, yBound * 2, gridRes);
if (robot != null){
grid.moverExtents = robot.collider.bounds.extents;
}
grid.recomputeOccupied("ObstacleCube");
grid.drawGrid();
}
/*
* Trims the path within this object such that any extraneous nodes are
* removed. This results in changes in direction only when unavoidable
* as a result of obstacles.
* The original path is not modified.
*/
public NavigationPath trimPath(OccupancyGrid grid)
{
List<GraphNode<StateConfig>> newPath = new List<GraphNode<StateConfig>>();
newPath.Add(path[0]);
//Debug.Log("Path length: " + path.Count);
GraphNode<StateConfig> startNode = null;
GraphNode<StateConfig> endNode = null;
// for (int i = 0; i < path.Count; i++) {
// Debug.Log("node " + i + ": " + path[i]);
// }
bool connected = true;
for (int startIndex = 0; startIndex < path.Count - 1;) {
// If it was not possible to make a connection from the previous node, then
// try looking from the next node in the path. Otherwise, a connection was made
// and the trimming will continue from the node at the endIndex we got last time
if (!connected)
startIndex++;
startNode = path[startIndex];
//Debug.Log("StartIndex: " + startIndex + ", node: " + startNode);
for (int endIndex = path.Count - 1; endIndex > startIndex; endIndex--) {
endNode = path[endIndex];
// Debug.Log("EndIndex: " + endIndex + ", node: " + endNode);
if (grid.obstacleFreeStraightPath(startNode.getPosition(), endNode.getPosition())){
newPath.Add(endNode);
// Once we make a connection, the next node we start from is the one we just added.
startIndex = endIndex;
connected = true;
break;
}
connected = false;
}
}
return new NavigationPath(newPath);
}
public GetMapResponse(OccupancyGrid map)
{
this.map = map;
}
