====== HW 03 - Single robot mapping ====== ===== Background ===== 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. ----- ==== 1. Dynamic Map Representation ==== 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: - allocate a new, larger grid - copy existing data - initialize new cells to $P = 0.5$. - 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) $$ ----- ==== 2. Robot Movement ==== 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): ^ Direction ^ Offset in x ^ Offset in y ^ | right | +1 | 0 | | left | -1 | 0 | | up | 0 | +1 | | down | 0 | -1 | //Assume that the given robot move is always valid, you don't have to check whether the area is occupied or free.// ----- ==== 3. Map at the Start of the Program ==== At the start of the program, you should allocate ''2 × 2'' occupancy map: | 1.0 | 0.0 | | 1.0 | 1.0 | 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$) == Reminder: == 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 [[courses:be5b99cpl:hw:hw02|HW02]]) ----- ==== 4. Bayesian Update of Cell Probabilities ==== 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 [[courses:be5b99cpl:hw:hw02|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)} $$ ---- ---- ===== Assignment ===== 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. - Load command-line arguments - Create an initial occupancy map of size ''2 × 2'' with robot in the grid position ''(1,1)'' and initialize its values (see above) - 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 - After processing all measurements and robot movements, write the final occupancy grid to **a file** named ''out.txt'' ---- ==== Input ==== === Command-line arguments === 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).// === Standard input (sensor measurements, robot movement) === You will get the sensor measurements and the robot movements via the standard input as a single continuous stream of tokens (all separated by a single space) You must distinguish between two input types, the sensor measurement: and the robot movement, in format: 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. Each input type (sensor measurement, robot movement) may appear at an arbitrary frequency, even not at all. 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).// Hint: You may, for example, alternate between //scanf()// with ''%f'' and ''%s'' format specifiers, or use //fgets()// with //strtok()// to parse the input. Other parsing methods are also acceptable. You can assume that the float values are given with up to 3 decimal places and are in range $(-9999, 9999)$. ---- ==== Raycast function ==== 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 {{:courses:be5b99cpl:hw:hw02_raycast.zip | 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.// ---- ==== Output ==== 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). Remember that comparing floating-point numbers for **equality** in C is unsafe due to limited machine precision. Always compare using a small tolerance, for example: ''fabs(a - b) < 1e-6'' ---- ==== Error handling ==== If any error occurs, print an error message to **stderr** and exit with the corresponding return code: ^ Reason ^ Exit code ^ Stderr output ^ | 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. ---- ==== Examples ==== You can find four examples of running the program {{:courses:be5b99cpl:hw:hw03_sample_data.zip | here}}. Expected ''out.txt'' outputs are provided with the suffix ''.map.out'' for each public example, e.g. ''pub00.map.out''. ^ Standard input ^ Expected file ''out.txt'' ^ Return value ^ CMD argument ^ Error message ^ | -135.000 2.121 -30.964 2.915 3.366 8.515 57.095 10.124 22.380 9.192 | ------#--- ----- ---- ----- ---- ---- ----- --- ------ --- ---- # -- --- -- -- ---- #* # ##-#------ | 0 | 1.0 0.0 | | up right up right up right up right 37.125 8.551 158.660 6.792 17.471 9.779 | -----#---- ----- ---- ----- ---- ----- ---- ---- *---- --- - -- -- -- - - - ---- -# # ----- -- ##------#- | 0 | 1.5 285.0 | | 143.0 14.0 | | 103 | 2.0 | ERROR: invalid arguments | | 9.0 a | | 102 | 1.0 0.0 | ERROR: invalid sensor data | Upload your solution into [[https://cw.felk.cvut.cz/brute/|BRUTE]] as a zip archive containing only the file ''main.c''