HW 04 - Multi-robot mapping

In the previous assignment (HW03), you implemented a mapping pipeline where a single moving robot updated the map using sensor measurements. In this assignment, you will extend the concept further: there will be multiple robots exploring concurrently.

Each robot will run in its own thread, and they will all build one shared map together. The focus is on practising thread programming and handling access to a shared dynamic occupancy map and using a third-party library for image export. All robots start in known positions, move one grid cell at a time, and 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 and is shared by all robots (threads). Its size is not known in advance and must be expanded dynamically whenever any 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 (reallocation): 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) $$

Note (sharing memory): Because the map is shared among multiple threads, every operation (read/write/resize) must be protected by a mutex (or an equivalent synchronisation mechanism) to prevent data races and accessing freed memory.

There are N robots, each running in its own thread. Every 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 robots never rotate, their orientation is fixed throughout execution.

Whenever a 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 map coordinates (x, y):

Important: Assume that the given robot move is always valid, you don't have to check whether the area is occupied or free. Robots do not see each other and never collide (you don't need any robot-robot collision detection). Multiple robots can be in the same cell simultaneously without requiring any special handling.

(Same as in HW03)

Click to view details

(Same as in HW03)

Click to view details

Implement a multi-robot occupancy grid mapping system in C, which combines sensor measurements from multiple moving robots to construct a 2D map by updating the occupancy probabilities of grid cells.

1. Load command-line arguments
2. Create the initial occupancy map of size 2 × 2 and initialize its values as described above
3. Spawn N robot threads, each thread:
   • Ensures that the robot's position is inside the occupancy map, resizing if necessary (using the shared map + mutex)
   • Reads its own input file
   • Sequentially processes sensor measurements (angle and distance) and robot movements (up, left, down, right) from its input file
     • If movement is read from the input file
       • Move the robot by one cell
       • Mark the cell as known free ($P = 0.0$)
     • If sensor measurement is read from the input file
       • 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 all threads finish processing all sensor measurements and robot movements, convert the final occupancy grid into an image (using the provided image library) and write it to a file named out.bmp

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

$ ./main cell_size basename

where:

• cell_size - float (in meters): physical size of one map cell
• basename - string: base name of per-robot data files

Note: Invalid or missing arguments must result in an error ERROR: invalid arguments to stderr and terminate the program with exit code 103 (see Error handling section).

The robots' starting positions are provided via the standard input (stdin). Each line corresponds to a position of one robot and contains three space-separated values:

x_robot_i y_robot_i alpha_robot_i

where for robot i (i = 0, 1, …, N-1):

• x_robot_i - integer (>=0): x map coordinate of robot i at the start
• y_robot_i - integer (>=0): y map coordinate of robot i at the start
• alpha_robot_i - float value (in degrees, 0-360): orientation of robot i at the start, measured counterclockwise

The number of robots is not known in advance and is determined by the number of lines on the stdin. Each line corresponds to one thread/robot. You should read from the stdin until the end of file (EOF).

The robot ID is given by the line index (starting from zero). The first line on stdin corresponds to robot ID 0, the second line to robot ID 1, and so on. The same ID is then used to determine the per-robot input filename (see below).

Note: If reading starting positions from stdin fails (e.g. missing or malformed values), you must print ERROR: invalid stdin data and exit with code 104 (see Error handling section).

Each robot reads its input from a separate file. The file name for the robot with ID i is:

<basename>_<i>.txt

for example, if basename = “sensor” and ID = 6 then its input file is “sensor_6.txt”.

Each file contains a single continuous stream of tokens all separated by a single space). You must distinguish between two types of tokens, 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)

Each input type (sensor measurement, robot movement) may appear at an arbitrary frequency, even not at all. Each robot thread must read all measurements/robot movements from its own file until the end of file (EOF).

Note: If any per-robot input file cannot be opened, print an error ERROR: missing sensor data file to stderr and terminate the program with exit code 105. If reading from any file fails, print an error ERROR: invalid sensor data to stderr and terminate the program with exit code 102 (see Error handling section).

(Same as in HW03)

Click to view details

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

/**
 * @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;
}

You must split your program into multiple source files. The thread entry function (the function you pass to pthread_create()) must be defined in a separate module robot.c and declared in robot.h.

A possible structure could look like this:

• main.c is responsible for:
  • parsing command-line arguments
  • reading robot starting positions from stdin
  • allocating and initializing the shared occupancy map
  • setting up synchronization primitives (mutex)
  • preparing per-robot argument structures
  • creating and joining threads

• robot.c / robot.h are responsible for:
  • defining a struct that contains all arguments needed by one robot thread (robot ID, starting pose, pointers to the shared map and mutex, cell_size, basename, etc.)
  • implementing the thread entry function that:
    • opens the per-robot input file
    • reads and parses tokens
    • updates the robot pose
    • updates the shared occupancy map (with proper synchronization)

After all measurements and robot movements are processed by all threads (and no error occurred), the program must write the final occupancy map to a BMP image file named out.bmp.

You are provided with an external library for image creation simage (containing simage.h and simage.c). The library provides 2 functions to export a BMP image (you should use one of them, depending on whether you represent the occupancy map as a 1D or 2D array):

int saveMapAsImage1D(char* name, double* map, int width, int height);
int saveMapAsImage2D(char* name, double** map, int width, int height);

where:

• name (char *): name of the output file
• map (double * or double * *): pointer to a dynamically allocated 1D/2D occupancy grid
• height (int): height of the map (number of rows)
• width (int): width of the map (number of columns)

The function returns 0 on success, -1 on failure.

You must threshold the 2D occupancy map before calling the saveMapAsImage1D()/saveMapAsImage2D() function so that:

• Cells with $P > 0.5$ have value 1.0 (occupied)
• Cells with $P < 0.5$ have value 0.0 (free)
• Cells with $P == 0.5$ have value 0.5 (unknown)

The functions assume the same memory layout as in HW02/HW03: the map is stored row-by-row from the top row downwards, so that the image 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 exit code 106 and print an error message ERROR: BMP image export failure to the stderr (see Error handling section).

If any error occurs, print an error message to stderr and exit with the corresponding return code. They are ordered in decreasing priority.

Raycast function failure

101

ERROR: raycast failure

Invalid sensor data

102

ERROR: invalid sensor data

Invalid command-line arguments

103

ERROR: invalid arguments

Invalid stdin data

104

ERROR: invalid stdin data

Invalid sensor data file

105

ERROR: invalid sensor data file

BMP image export failure

106

ERROR: BMP image export failure

Any other error

100

ERROR: unknown

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

Standard input	Content of sensor_0.txt	Content of sensor_1.txt	Return value	CMD argument	Error message	Output out.bmp
1 1 0.00 1 1 0.00	right right down 0 6	up up right right 0 5	0	1.0 sensor
1 1 0.00 180 1 0.00	0 60 30 70 60 70 90 60	180 60 150 70 120 70 90 60	0	2.0 sensor
1 1 0.00			103	2.0	ERROR: invalid arguments
1 0 f			104	1.0 sensor	ERROR: invalid stdin data

You can find all public examples here (placeholder). Expected out.bmp outputs are provided with the suffix .map.bmp for each public example, e.g. pub00.map.bmp.