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