Initial commit

srcs/RobotNet10/Shared/RobotNet10.Shared/Localization/OccupancyGrid.cs

@@ -0,0 +1,153 @@
+using RobotNet10.Shared.Geometry;
+
+namespace RobotNet10.Shared.Localization;
+
+/// <summary>
+/// Occupancy grid for representing map as a 2D grid of cells
+/// Compatible with ROS navigation_msgs/OccupancyGrid
+/// </summary>
+public class OccupancyGrid
+{
+    /// <summary>
+    /// Resolution of the grid (meters per pixel)
+    /// </summary>
+    public double Resolution { get; set; }
+
+    /// <summary>
+    /// Width of the grid (number of cells)
+    /// </summary>
+    public int Width { get; set; }
+
+    /// <summary>
+    /// Height of the grid (number of cells)
+    /// </summary>
+    public int Height { get; set; }
+
+    /// <summary>
+    /// Origin of the grid in map frame (bottom-left corner)
+    /// </summary>
+    public Pose Origin { get; set; }
+
+    /// <summary>
+    /// Grid data: -1 = Unknown, 0-100 = Probability of occupancy (0 = free, 100 = occupied)
+    /// Stored in row-major order (index = y * width + x)
+    /// </summary>
+    public sbyte[] Data { get; set; }
+
+    /// <summary>
+    /// Creates a new empty occupancy grid
+    /// </summary>
+    public OccupancyGrid()
+    {
+        Resolution = 0.05;
+        Width = 0;
+        Height = 0;
+        Origin = new Pose();
+        Data = [];
+    }
+
+    /// <summary>
+    /// Creates a new occupancy grid with specified dimensions
+    /// </summary>
+    public OccupancyGrid(double resolution, int width, int height, Pose origin)
+    {
+        Resolution = resolution;
+        Width = width;
+        Height = height;
+        Origin = origin;
+        Data = new sbyte[width * height];
+        
+        // Initialize all cells as unknown (-1)
+        Array.Fill(Data, (sbyte)-1);
+    }
+
+    /// <summary>
+    /// Gets the occupancy value at the specified cell coordinates
+    /// </summary>
+    /// <param name="x">X coordinate (0 to Width-1)</param>
+    /// <param name="y">Y coordinate (0 to Height-1)</param>
+    /// <returns>-1 = Unknown, 0-100 = Occupancy probability</returns>
+    public sbyte GetCell(int x, int y)
+    {
+        if (x < 0 || x >= Width || y < 0 || y >= Height)
+        {
+            return -1; // Out of bounds = unknown
+        }
+        return Data[y * Width + x];
+    }
+
+    /// <summary>
+    /// Sets the occupancy value at the specified cell coordinates
+    /// </summary>
+    /// <param name="x">X coordinate (0 to Width-1)</param>
+    /// <param name="y">Y coordinate (0 to Height-1)</param>
+    /// <param name="value">-1 = Unknown, 0-100 = Occupancy probability</param>
+    public void SetCell(int x, int y, sbyte value)
+    {
+        if (x < 0 || x >= Width || y < 0 || y >= Height)
+        {
+            return; // Out of bounds, ignore
+        }
+        Data[y * Width + x] = value;
+    }
+
+    /// <summary>
+    /// Converts world coordinates (meters) to grid cell coordinates
+    /// </summary>
+    /// <param name="worldX">X coordinate in meters</param>
+    /// <param name="worldY">Y coordinate in meters</param>
+    /// <returns>Cell coordinates (x, y)</returns>
+    public (int x, int y) WorldToGrid(double worldX, double worldY)
+    {
+        // Convert world coordinates relative to origin
+        var relativeX = worldX - Origin.Position.X;
+        var relativeY = worldY - Origin.Position.Y;
+
+        // Convert to grid coordinates
+        var gridX = (int)Math.Floor(relativeX / Resolution);
+        var gridY = (int)Math.Floor(relativeY / Resolution);
+
+        return (gridX, gridY);
+    }
+
+    /// <summary>
+    /// Converts grid cell coordinates to world coordinates (meters)
+    /// </summary>
+    /// <param name="gridX">X coordinate in grid cells</param>
+    /// <param name="gridY">Y coordinate in grid cells</param>
+    /// <returns>World coordinates (x, y) in meters</returns>
+    public (double x, double y) GridToWorld(int gridX, int gridY)
+    {
+        var worldX = Origin.Position.X + (gridX * Resolution);
+        var worldY = Origin.Position.Y + (gridY * Resolution);
+        return (worldX, worldY);
+    }
+
+    /// <summary>
+    /// Gets the bounds of the grid in world coordinates
+    /// </summary>
+    public BoundingBox GetBounds()
+    {
+        var (minX, minY) = GridToWorld(0, 0);
+        var (maxX, maxY) = GridToWorld(Width, Height);
+        return new BoundingBox
+        {
+            MinX = Math.Min(minX, maxX),
+            MinY = Math.Min(minY, maxY),
+            MaxX = Math.Max(minX, maxX),
+            MaxY = Math.Max(minY, maxY)
+        };
+    }
+}
+
+/// <summary>
+/// Bounding box for representing map bounds
+/// </summary>
+public class BoundingBox
+{
+    public double MinX { get; set; }
+    public double MinY { get; set; }
+    public double MaxX { get; set; }
+    public double MaxY { get; set; }
+}
+