Main Code for Robot
#include <cmath>
#include <iostream>
#include <random>
#include <vector>
#include <fstream>
#include <utility>
#include <iomanip>
#include "utils.h"
#include "render.h"+
using namespace std;
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++++++++++++++Modify my_robot class here+++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
class my_robot:public Object {
// lidar radius for mapping around robot
private:
int lidar_range = 50;
public:
// allocates 800 x800 occupancy grid set to -1 (unknown)
my_robot(int a,int b,int c,int d,int e,int f) : Object(a, b, c, d, e, f)
{
grid = std::vector<std::vector<int>>(800, std::vector<int>(800, -1));
}
std::vector<std::vector<int>> grid;
//finds the center of the robot
//Also colours in the map for the robot using the i and j values around the robot and marking them as true
void updateSensor(grid_util grid_instance) {
int x_c = 10 + x;
int y_c = y + 10;
for (int i = x_c - lidar_range; i <= x_c + lidar_range; ++i) {
for (int j = y_c - lidar_range; j <= y_c + lidar_range; ++j) {
if ((i - x_c) * (i - x_c) + (j - y_c) * (j - y_c) <= lidar_range * lidar_range) {
int tru_grid_value = Object::grid_value(grid_instance, this, i, j, lidar_range);
grid[i][j] = tru_grid_value;
}
}
}
}
// save grid
void save_grid_csv() {
std::string filename = "grid_pred.csv";
std::ofstream file(filename);
if (!file.is_open()) {
std::cerr << "Error: Could not open file " << filename << std::endl;
return;
}
size_t maxRowSize = 0;
for (const auto& col : grid) {
if (col.size() > maxRowSize) {
maxRowSize = col.size();
}
}
for (size_t row = 0; row < maxRowSize; ++row) {
for (size_t col = 0; col < grid.size(); ++col) {
if (row < grid[col].size()) {
file << grid[col][row];
}
if (col < grid.size() - 1) {
file << ",";
}
}
file << "\n";
}
file.close();
std::cout << "Robot's grid written to " << filename << std::endl;
}
};
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//===== Main parameters =====
const int env_width {800}, env_height {800}; //Width and height of the environment
const int radius {10}; //Radius of the robot
const int min_obj_size {50};
const int max_obj_size {100};
int lidar_range{50}; //Lidar range
// Grid utility class
grid_util grid(env_width, env_height, min_obj_size, max_obj_size);
// Random generator
random_generator rand_gen;
// Vector of velocity commands
std::vector<std::vector<int>> robot_pos;
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++DEFINE ANY GLOBAL VARIABLES/FUNCTIONS HERE+++++++++++++++
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
int main(int argc, char const *argv[])
{
//==========CREATE ROBOT AND WALLS==========
// read config file
std::tuple<std::string, bool, int, int> config = read_csv();
// create the walls
std::vector<Object*> walls;
// normal perpendicular walls
if (std::get<3>(config) == 4) {
walls = grid.create_walls(std::get<0>(config));
}
// angled walls
else {
walls = grid.create_angled_walls(std::get<0>(config));
}
// get minimum/maximum y values for the robot to spawn
int min_y_spawn = grid.get_min_y();
int max_y_spawn = grid.get_max_y();
// Uncomment this line to write the grid to csv to see the grid as a csv
// grid.writeGridToCSV("grid.csv");
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++++++++DEFINE ANY LOCAL VARIABLES HERE+++++++++++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// saves wall information around robot
int checkWall[4] = {0,0,0,0};
int wall_LR , wall_UD, wall_D1, wall_D2, countDown , startY;
int TL_X = 0, TL_Y = 999, bottomy = 0;
vector<pair<int, int>> pos_checked;
bool done_loop = false, donemapping = false;
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++++++++Modify the instantiation of robot+++++++++++++++++++++++++++
//+++++robot should be a my_robot class instead of an Object class++++++++++++++++
//+++++++++++The constructor signature can be however you like++++++++++++++++++++
//+++Make sure to pass min_y_spawn and max_y_spawn to the constructor of Object+++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
my_robot robot(2*radius, 2*radius, env_width, min_y_spawn, max_y_spawn, radius+5); // 2*radius is used for width/height of robot
my_robot robot_init = robot; // create a copy. change this to a my_robot class as well
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// push the initial position onto robot_pos
robot_pos.push_back({robot.x, robot.y});
int limit_count = 0;
// run the program indefinitely until robot hits the goal or an obstacle
while (donemapping != true && limit_count <5000)
{
limit_count++;
//+++++++++++++++WRITE YOUR MAIN LOOP CODE HERE++++++++++++++++++++++
//++++++++++++++EXAMPLE: ROBOT SIMPLY MOVES LEFT+++++++++++++++++++++
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
while(done_loop == false){
pos_checked.push_back({robot.x, robot.y});
//To find robot center
int rc_x = robot.x + 10;
int rc_y = robot.y + 10;
for(int o; o < 4; o++){
checkWall[o] = 0;
}
if ((robot.grid[rc_x - 25][rc_y] == 1)){
checkWall[0] = -1;
}
if ((robot.grid[rc_x][rc_y - 25] == 1)){
checkWall[1] = -1;
}
if ((robot.grid[rc_x + 25][rc_y] == 1)){
checkWall[0] = 1;
}
if ((robot.grid[rc_x][rc_y + 25] == 1)){
checkWall[1] = 1;
}
if(robot.grid[rc_x-15][rc_y-15]== 1){
checkWall[2] = -1;
}
if(robot.grid[rc_x+15][rc_y-15]== 1){
checkWall[3] = 1;
}
if(robot.grid[rc_x+15][rc_y+15]== 1){
checkWall[2] = 1;
}
if(robot.grid[rc_x-15][rc_y+15]== 1){
checkWall[3] = -1;
}
wall_LR = checkWall[0];
wall_UD = checkWall[1];
wall_D1 = checkWall[2];
wall_D2 = checkWall[3];
if((wall_LR == 0)&&(wall_UD == 0)&&(wall_D1 == 0)&&(wall_D2 == 0)){
robot.x = robot.x-1;
}
if(wall_LR == -1){
if(robot.grid[robot.x][robot.y - 5]!= 1){
robot.y = robot.y - 1;
}
}
if(wall_D1 == -1){
if(robot.grid[robot.x+25][robot.y]!= 1){
robot.x = robot.x + 1;
}
if(robot.grid[robot.x][robot.y - 5]!= 1){
robot.y = robot.y - 1;
}
}
if(wall_UD == -1){
if(robot.grid[robot.x+25][robot.y]!= 1){
robot.x = robot.x + 1;
}
}
if(wall_D2 == 1){
if(robot.grid[robot.x][robot.y+25]!= 1){
robot.y = 1 + robot.y;
}
if(robot.grid[robot.x+25][robot.y]!= 1){
robot.x = 1 + robot.x;
}
}
if(wall_LR == 1){
if(robot.grid[robot.x][robot.y+25]!= 1){
robot.y = 1 + robot.y;
}
}
if(wall_D1 == 1){
if(robot.grid[robot.x][robot.y+25]!= 1){
robot.y = 1 + robot.y;
}
if(robot.grid[robot.x - 5][robot.y]!= 1){
robot.x = robot.x - 1;
}
}
if(wall_UD == 1){
if(robot.grid[robot.x - 5][robot.y]!= 1){
robot.x = robot.x - 1;
}
}
if(wall_D2 == -1){
if(robot.grid[robot.x][robot.y - 5]!= 1){
robot.y = robot.y - 1;
}
if(robot.grid[robot.x - 5][robot.y]!= 1){
robot.x = robot.x - 1;
}
}
if(robot.y < TL_Y){
TL_Y = robot.y;
TL_X = robot.x;
}
if(robot.y > bottomy){
bottomy = robot.y;
}
robot.updateSensor(grid);
robot_pos.push_back({robot.x, robot.y});
for (const auto& pos : pos_checked) {
if (pos.first == robot.x && pos.second == robot.y && TL_Y == robot.y) {
done_loop = true;
}
}
}
startY = robot.y;
while(robot.y - 50 <= startY){
int rc_x = robot.x + 10;
int rc_y = robot.y + 10;
for(int o; o < 4; o++){
checkWall[o] = 0;
}
if ((robot.grid[rc_x - 25][rc_y] == 1)){
checkWall[0] = -1;
}
if ((robot.grid[rc_x][rc_y - 25] == 1)){
checkWall[1] = -1;
}
if ((robot.grid[rc_x + 25][rc_y] == 1)){
checkWall[0] = 1;
}
if ((robot.grid[rc_x][rc_y + 25] == 1)){
checkWall[1] = 1;
}
if(robot.grid[rc_x-15][rc_y-15]== 1){
checkWall[2] = -1;
}
if(robot.grid[rc_x+15][rc_y-15]== 1){
checkWall[3] = 1;
}
if(robot.grid[rc_x+15][rc_y+15]== 1){
checkWall[2] = 1;
}
if(robot.grid[rc_x-15][rc_y+15]== 1){
checkWall[3] = -1;
}
wall_LR = checkWall[0];
wall_UD = checkWall[1];
wall_D1 = checkWall[2];
wall_D2 = checkWall[3];
while(robot.grid[robot.x + 25][robot.y + 10] != 1){
robot.x = robot.x+1;
robot.updateSensor(grid);
robot_pos.push_back({robot.x, robot.y});
}
if(robot.y >= bottomy - 20){
donemapping = true;
break;
}
if(wall_LR == -1){
if(robot.grid[robot.x][robot.y - 5]!= 1){
robot.y = robot.y + 1;
countDown = countDown + 1;
}
}
if(wall_D1 == -1){
if(robot.grid[robot.x+25][robot.y]!= 1){
robot.x = robot.x - 1;
}
if(robot.grid[robot.x][robot.y - 5]!= 1){
robot.y = robot.y + 1;
countDown = countDown + 1;
}
}
if(wall_D2 == 1){
if(robot.grid[robot.x][robot.y+25]!= 1){
robot.y = robot.y + 1;
countDown = countDown + 1;
}
if(robot.grid[robot.x+25][robot.y]!= 1){
robot.x = robot.x + 1;
}
}
if(wall_LR == 1){
if(robot.grid[robot.x][robot.y+25]!= 1){
robot.y = robot.y + 1;
countDown = countDown + 1;
}
}
if(wall_D1 == 1){
if(robot.grid[robot.x - 5][robot.y]!= 1){
robot.x = robot.x - 1;
}
if(robot.grid[robot.x- 5][robot.y+25]!= 1){
robot.y = robot.y + 1;
countDown = countDown + 1;
}
}
if(wall_UD == 1){
if(robot.grid[robot.x - 5][robot.y+20]!= 1){
robot.x = robot.x - 1;
}}
robot.updateSensor(grid);
robot_pos.push_back({robot.x, robot.y});
}
startY = robot.y;
while(robot.y - 50 <= startY){
int rc_x = robot.x + 10;
int rc_y = robot.y + 10;
for(int o; o < 4; o++){
checkWall[o] = 0;
}
if ((robot.grid[rc_x - 25][rc_y] == 1)){
checkWall[0] = -1;
}
if ((robot.grid[rc_x][rc_y - 25] == 1)){
checkWall[1] = -1;
}
if ((robot.grid[rc_x + 25][rc_y] == 1)){
checkWall[0] = 1;
}
if ((robot.grid[rc_x][rc_y + 25] == 1)){
checkWall[1] = 1;
}
if(robot.grid[rc_x-15][rc_y-15]== 1){
checkWall[2] = -1;
}
if(robot.grid[rc_x+15][rc_y-15]== 1){
checkWall[3] = 1;
}
if(robot.grid[rc_x+15][rc_y+15]== 1){
checkWall[2] = 1;
}
if(robot.grid[rc_x-15][rc_y+15]== 1){
checkWall[3] = -1;
}
wall_LR = checkWall[0];
wall_UD = checkWall[1];
wall_D1 = checkWall[2];
wall_D2 = checkWall[3];
while(robot.grid[robot.x - 5][robot.y + 10] != 1){
robot.x = robot.x-1;
robot.updateSensor(grid);
robot_pos.push_back({robot.x, robot.y});
}
if(robot.y >= bottomy - 20){
donemapping = true;
break;
}
if(wall_LR == -1){
if(robot.grid[robot.x][robot.y - 5]!= 1){
robot.y = robot.y + 1;
countDown = countDown + 1;
}
}
if(wall_D1 == -1){
if(robot.grid[robot.x][robot.y + 25]!= 1){
robot.y = robot.y + 1;
countDown = countDown + 1;
if(robot.grid[robot.x-5][robot.y]!= 1){
robot.x = robot.x - 1;
}
}
}
if(wall_D2 == -1){
if(robot.grid[robot.x + 25][robot.y]!= 1){
robot.x = robot.x + 1;
}
if(robot.grid[robot.x+25][robot.y + 25]!= 1){
robot.y = robot.y + 1;
countDown = countDown + 1;
}
}
if(wall_UD == 1){
if(robot.grid[robot.x + 25][robot.y+20]!= 1){
robot.x = robot.x + 1;
}}
robot.updateSensor(grid);
robot_pos.push_back({robot.x, robot.y});
}
if(donemapping){
while(robot.grid[robot.x - 15][robot.y + 10] != 1){
robot.x = robot.x-1;
robot.updateSensor(grid);
robot_pos.push_back({robot.x, robot.y});
}
while(robot.grid[robot.x + 35][robot.y + 10] != 1){
robot.x = robot.x+1;
robot.updateSensor(grid);
robot_pos.push_back({robot.x, robot.y});
}
break;
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
robot_pos.push_back({robot.x, robot.y});
if (false) {
std::cout << "====Program terminated after 3600 iterations====" << std::endl;
break;
}
}
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++For now, an 800x800 vector, vec, initialized to -1 is placed here+++++++++
//+Modify lines 180-181 so robot.grid is passed to both functions instead of vec++
//++++++Modify line 187 so the third argument to render_grid() is robot.grid++++++
//++++++After you make the robot instance in line 139 a my_robot class with
// a grid member, lines 182-183 will have robot.grid as its argument
// and line 189 will have robot.grid as its third argument.++++++++++++++++++
//++++++++++++++++++++++++++Then, you can remove vec++++++++++++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
std::vector<std::vector<int>> vec(800, std::vector<int>(800, -1));
std::cout << std::fixed << std::setprecision(2); // set precision for printing
float wall_accuracy = grid.wall_accuracy(robot.grid); // for task 1: outer walls. replace vec with your robot's grid
float accuracy = grid.grid_accuracy(robot.grid); // for task 2: entire environment inside walls. replace vec with your robot's grid
std::cout << "Percent of walls correctly mapped: " << wall_accuracy*100.0 << "%" << std::endl;
std::cout << "Percent of environment correctly mapped: " << accuracy*100.0 << "%" << std::endl;
if (std::get<1>(config)){
render_window(robot_pos, walls, robot_init, env_width, env_height, std::get<2>(config));
}
render_grid(robot_init, robot_pos, robot.grid, env_width, env_height, radius, lidar_range, std::get<2>(config));
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
return 0;
}
