HW 03 - Single robot mapping

In the previous assignment (HW02), you implemented a fixed-size occupancy grid where a stationary robot updated the map using sensor measurements. In this assignment, you will extend that concept further: the robot will be able to move, and the map will grow dynamically as needed to accommodate new explored areas.

The focus is on practising dynamic memory management and correctly handling a map that must be resized at runtime. The robot starts in a known area, moves one grid cell at a time, and continues to receive laser-like measurements.

Same as before, the map represents a rectangular area in the real world, with:

• the origin (0, 0) at the bottom-left corner of the map,
• the x-axis increasing to the right,
• the y-axis increasing upward,
• the map stores the occupancy probability $P$ for each cell ($0 ≤ P ≤ 1$).

The map must be stored on the heap. Its size is not known in advance and must be expanded dynamically whenever the robot moves or sensor beams reach beyond the current boundaries.

• the map grows only in positive X and Y directions (no negative indices).
• when expansion is needed:
  1. allocate a new, larger grid
  2. copy existing data
  3. initialize new cells to $P = 0.5$.
  4. free the old map.

Note: Always reallocate the map to the smallest sufficient size. For example, if the current map is 2 × 2 and a measurement hits cell (1,3), the new map should be 2 × 4. In general:

$$ width_{new} = max(width_{current}, x_{max} + 1) $$ $$ height_{new} = max(height_{current}, y_{max} + 1) $$

The robot moves on the grid (not in continuous space). Each movement command shifts its position by exactly one cell in the 4-neighbourhood (up, down, left, or right). The robot never rotates, its orientation is fixed throughout execution.

Whenever the robot enters a new cell:

• that cell becomes known free ($P = 0.0$)
• if it lies beyond the current map, expand the map before updating

Movement commands correspond to integer offsets in grid coordinates (x, y):

Assume that the given robot move is always valid, you don't have to check whether the area is occupied or free.

At the start of the program, you should allocate 2 × 2 occupancy map:

where:

• the robot's grid position will be (1,1) (indices starting from zero)
• the cells (0,0), (0,1) and (1,0) are occupied ($P = 1.0$)
• the cell occupied by the robot (1,1) is free ($P = 0.0$)

In memory, the map is stored as a 2D array map[rows][cols], where:

• row indices increase downward (top → bottom),
• col indices increase to the right.

Because the array indexing and map coordinate system differ in orientation, you must invert the y-coordinates accordingly when converting from map coordinates to array indices. Think of coordinate (0, 0) as the bottom-left corner of the map in the world, but in memory, row index 0 corresponds to the top of the grid. (see HW02)

For each sensor measurement, you will get a set of cells that the laser beam passes through:

• all but the last cell are marked as free (measurement = 0)
• the last cell (the obstacle) is occupied (measurement = 1)

Each affected cell’s probability must be updated using the same Bayesian update rule as in HW02:

• If measurement = 1:

$$ P_{next} = \dfrac{0.8 \cdot P}{0.8 \cdot P + 0.2 \cdot (1-P)} $$

• If measurement = 0:

$$ P_{next} = \dfrac{0.2 \cdot P}{0.2 \cdot P + 0.8 \cdot (1-P)} $$

Implement an occupancy grid mapping system for a moving robot in C that receives distance measurements from its sensors. Your program will construct a 2D map from these measurements by updating the occupancy probabilities of grid cells.

1. Load command-line arguments
2. Create an initial occupancy map of size 2 × 2 with robot in the grid position (1,1) and initialize its values (see above)
3. Sequentially read sensor measurements (angle and distance) and robot movements (up, left, down, right) from the standard input
   • If movement is read from input
     • Move the robot by one cell
     • Mark the cell as known free ($P = 0.0$)
   • If sensor measurement is read from input
     • Compute the absolute beam angle
     • Use the provided raycast() function to obtain all grid cells along the beam
     • If the obtained grid cells are out of the allocated occupancy grid, reallocate it to the smallest sufficient size
     • Apply the Bayesian update to all affected cells (free vs. occupied) to obtain the new probability estimates in the occupancy grid
4. After processing all measurements and robot movements, write the final occupancy grid to a file named out.txt

You will get 2 configuration parameters provided via the command-line arguments (argv):

$ ./main cell_size alpha_robot

where:

• cell_size - float (in meters): physical size of one map cell
• alpha_robot - float value (in degrees, 0-360): robot’s orientation, measured counterclockwise

Note: Invalid or missing arguments must result in an error (see Error handling section).

You will get the sensor measurements and the robot movements via the standard input as a single continuous stream of tokens. You must distinguish between two input types, the sensor measurement:

<angle> <distance>

and the robot movement, in format:

<right / left / up / down>

where:

• angle - float value (in degrees, range -180 to +180): direction of the beam relative to the robot’s current orientation
• distance - float value (in meters), distance from the robot to the detected obstacle
• direction - string value “right”/“left”/“up”/“down”, direction of the robot movement (always in lowercase)

Tokens will appear on one line. Use the scanf() function, read all measurements/robot movements until the end of file (EOF).

Note: If reading from stdin fails, print an error and terminate the program (see Error handling section).

A helper function raycast() is provided to compute which grid cells a laser beam passes through as well as the obstacle cell. It is based on a Bresenham-style algorithm that traces discrete cells along a continuous line. The function prototype is:

int raycast(double angle_deg, double distance_m, double cell_size_m, int ***out_cells, int *out_count)

where:

• angle_deg (double): beam angle in degrees (in map coordinates)
• distance_m (double): beam length in meters (sensor distance)
• cell_size_m (double): size of one map cell in meters
• out_cells (int * * *): output pointer to a dynamically allocated 2D array of cell coordinates [N][2]
• out_count (int *): output pointer for the number of returned cells N

You will need to copy paste the function implementation to your solution, the full implementation is the following:

/**
 * @brief Performs a grid-based raycast using Bresenham's algorithm.
 * 
 * The ray starts at (0,0) and travels in the specified angle
 * and distance. Each grid cell has size `cell_size_m`. Requires stdlib.h, math.h
 * Variable out_cells is dynamically allocated memory, don't forget to free() it after the use.
 * 
 * @param angle_deg   Angle of the ray (in degrees)
 * @param distance_m  Length of the ray (in meters)
 * @param cell_size_m Size of one grid cell (in meters)
 * @param out_cells   Pointer to store dynamically allocated 2D array [N][2] of grid coords, undefined on failure
 * @param out_count   Pointer to store number of cells (N), undefined on failure
 * @return            1 on success, 0 on failure (e.g., allocation failure, 'distance_m' is less than 'cell_size_m')
 */
int raycast(double angle_deg, double distance_m, double cell_size_m, int ***out_cells, int *out_count)
{
    if (!out_cells || !out_count || distance_m < cell_size_m) {
        return 0;
    }
 
    double angle_rad = angle_deg * 3.14159265358979323846 / 180.0;
    double end_x = distance_m * cos(angle_rad);
    double end_y = distance_m * sin(angle_rad);
    int x0 = 0, y0 = 0;
    int x1 = (int)(end_x / cell_size_m);
    int y1 = (int)(end_y / cell_size_m);
    int dx = abs(x1 - x0);
    int dy = abs(y1 - y0);
    int sx = (x0 < x1) ? 1 : -1;
    int sy = (y0 < y1) ? 1 : -1;
    int err = dx - dy;
 
    int max_cells = dx + dy + 1;
    int **cells = malloc(max_cells * sizeof(int *));
    if (!cells) {
        return 0;
    }
 
    int count = 0;
 
    while (1) {
        cells[count] = malloc(2 * sizeof(int));
        if (!cells[count]) {
            // Clean up previously allocated memory
            for (int i = 0; i < count; i++) {
                free(cells[i]);
                cells[i] = NULL;
            }
            free(cells);
            cells = NULL;
            return 0;
        }
        cells[count][0] = x0;
        cells[count][1] = y0;
        count++;
 
        if (x0 == x1 && y0 == y1) {
            break;
        }
 
        int e2 = 2 * err;
        if (e2 > -dy) {
            err -= dy;
            x0 += sx;
        }
        if (e2 < dx) {
            err += dx;
            y0 += sy;
        }
    }
 
    *out_cells = cells;
    *out_count = count;
    return 1;
}

$\qquad\,$ You can also find the implementation, together with a usage example and a simple visualisation here.

This function computes all grid cells intersected by a laser beam starting at the robot’s position (local origin (0,0)) and travelling in the specified direction and distance. The last cell in the array is the obstacle cell. It allocates a dynamic 2D array of integer cell coordinates [N][2] and stores:

• out_cells[i][0] → x offset (integer)
• out_cells[i][1] → y offset (integer)

both relative to the robot’s cell. The last element of the array is the obstacle, all cells up to that point should be considered free.

The function returns 1 on success, 0 on failure (e.g., allocation failure).

Note: These coordinates are relative, not absolute map indices. You must offset them by the robot’s actual grid cell position before accessing the map array. The out_cells array is dynamically allocated, don't forget to free() the memory after the use.

After all sensor data have been processed, write the final occupancy grid (in the same way as in HW02) to a file named out.txt, using the following format:

• Cells with $P > 0.5$ are printed as hashtag symbol # (occupied)
• Cells with $P < 0.5$ are printed as space symbol (free)
• Cells with $P == 0.5$ are printed as dash symbol - (unknown)
• The robot position should be printed as a star symbol *

Write the map starting from the top row down to the bottom, so that it corresponds to the map coordinate system (x → right, y → up).

After successful execution, your program should return 0, if an error was encountered, it should return with an appropriate exit value and print an error message to the stderr (see Error handling section).

If any error occurs, print an error message to stderr and exit with the corresponding return code:

Raycast function failure

101

ERROR: raycast failure

Invalid sensor data

102

ERROR: invalid sensor data

Invalid command-line arguments

103

ERROR: invalid arguments

Any other error

100

ERROR: unknown

Otherwise, when no error is encountered, your program should return 0 on successful completion.

You can find four examples of running the program TODO placeholder: