Yo Fleyt / Mbed 2 deprecated roboticLab_withclass_3_July

test morning

Dependencies:   ISR_Mini-explorer mbed

Fork of roboticLab_withclass_3_July by Georgios Tsamis

Map.cpp@14:696187e74411, 2017-07-12 (annotated)

Committer:
Ludwigfr
Date:
Wed Jul 12 18:08:07 2017 +0000
Revision:
14:696187e74411
Parent:
5:19f24c363418 
lol

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Ludwigfr 0:9f7ee7ed13e4 1 #include "Map.hpp"
Ludwigfr 0:9f7ee7ed13e4 2
Ludwigfr 0:9f7ee7ed13e4 3 Map::Map(float widthRealMap, float heightRealMap, int nbCellWidth, int nbCellHeight){
Ludwigfr 0:9f7ee7ed13e4 4 this->widthRealMap=widthRealMap;
Ludwigfr 0:9f7ee7ed13e4 5 this->heightRealMap=heightRealMap;
Ludwigfr 0:9f7ee7ed13e4 6 this->nbCellWidth=nbCellWidth;
Ludwigfr 0:9f7ee7ed13e4 7 this->nbCellHeight=nbCellHeight;
Ludwigfr 0:9f7ee7ed13e4 8 this->sizeCellWidth=widthRealMap/(float)nbCellWidth;
Ludwigfr 0:9f7ee7ed13e4 9 this->sizeCellHeight=heightRealMap/(float)nbCellHeight;
Ludwigfr 0:9f7ee7ed13e4 10
Ludwigfr 0:9f7ee7ed13e4 11 this->cellsLogValues= new float*[nbCellWidth];
Ludwigfr 0:9f7ee7ed13e4 12 for(int i = 0; i < nbCellWidth; ++i)
Ludwigfr 0:9f7ee7ed13e4 13 this->cellsLogValues[i] = new float[nbCellHeight];
Ludwigfr 0:9f7ee7ed13e4 14
Ludwigfr 0:9f7ee7ed13e4 15 this->initialLogValues= new float*[nbCellWidth];
Ludwigfr 0:9f7ee7ed13e4 16 for(int i = 0; i < nbCellWidth; ++i)
Ludwigfr 0:9f7ee7ed13e4 17 this->initialLogValues[i] = new float[nbCellHeight];
Ludwigfr 0:9f7ee7ed13e4 18
Ludwigfr 0:9f7ee7ed13e4 19 this->fill_initialLogValues();
Ludwigfr 3:37345c109dfc 20 this->fill_map_with_empty();
Ludwigfr 0:9f7ee7ed13e4 21 }
Ludwigfr 0:9f7ee7ed13e4 22
Ludwigfr 0:9f7ee7ed13e4 23 //fill initialLogValues with the values we already know (here the bordurs)
Ludwigfr 0:9f7ee7ed13e4 24 void Map::fill_initialLogValues(){
Ludwigfr 0:9f7ee7ed13e4 25 //Fill map, we know the border are occupied
Ludwigfr 0:9f7ee7ed13e4 26 for (int i = 0; i<this->nbCellWidth; i++) {
Ludwigfr 0:9f7ee7ed13e4 27 for (int j = 0; j<this->nbCellHeight; j++) {
Ludwigfr 0:9f7ee7ed13e4 28 if(j==0 || j==this->nbCellHeight-1 || i==0 || i==this->nbCellWidth-1)
Ludwigfr 0:9f7ee7ed13e4 29 this->initialLogValues[i][j] = this->proba_to_log(1);
Ludwigfr 0:9f7ee7ed13e4 30 else
Ludwigfr 0:9f7ee7ed13e4 31 this->initialLogValues[i][j] = this->proba_to_log(0.5);
Ludwigfr 0:9f7ee7ed13e4 32 }
Ludwigfr 0:9f7ee7ed13e4 33 }
Ludwigfr 0:9f7ee7ed13e4 34 }
Ludwigfr 0:9f7ee7ed13e4 35
Ludwigfr 3:37345c109dfc 36
Ludwigfr 3:37345c109dfc 37 //fill initialLogValues with the values we already know (here the bordurs)
Ludwigfr 3:37345c109dfc 38 void Map::fill_initialLogValuesLab4(){
Ludwigfr 3:37345c109dfc 39 //Fill map, we know the border are occupied
Ludwigfr 3:37345c109dfc 40 for (int i = 0; i<this->nbCellWidth; i++) {
Ludwigfr 3:37345c109dfc 41 for (int j = 0; j<this->nbCellHeight; j++) {
Ludwigfr 3:37345c109dfc 42 if(j==0 || j==this->nbCellHeight-1 || i==0 || i==this->nbCellWidth-1)
Ludwigfr 3:37345c109dfc 43 this->initialLogValues[i][j] = this->proba_to_log(1);
Ludwigfr 3:37345c109dfc 44 else
Ludwigfr 3:37345c109dfc 45 this->initialLogValues[i][j] = this->proba_to_log(0.5);
Ludwigfr 3:37345c109dfc 46 }
Ludwigfr 3:37345c109dfc 47 }
Ludwigfr 3:37345c109dfc 48 }
Ludwigfr 3:37345c109dfc 49
Ludwigfr 3:37345c109dfc 50 void Map::fill_map_with_empty(){
Ludwigfr 0:9f7ee7ed13e4 51 for (int i = 0; i<this->nbCellWidth; i++) {
Ludwigfr 0:9f7ee7ed13e4 52 for (int j = 0; j<this->nbCellHeight; j++) {
Ludwigfr 3:37345c109dfc 53 this->cellsLogValues[i][j] = this->proba_to_log(0.5);
Ludwigfr 0:9f7ee7ed13e4 54 }
Ludwigfr 0:9f7ee7ed13e4 55 }
Ludwigfr 0:9f7ee7ed13e4 56 }
Ludwigfr 0:9f7ee7ed13e4 57
Ludwigfr 0:9f7ee7ed13e4 58 //returns the probability [0,1] that the cell is occupied from the log valAue lt
Ludwigfr 0:9f7ee7ed13e4 59 float Map::log_to_proba(float lt){
Ludwigfr 0:9f7ee7ed13e4 60 return 1-1/(1+exp(lt));
Ludwigfr 0:9f7ee7ed13e4 61 }
Ludwigfr 0:9f7ee7ed13e4 62
Ludwigfr 0:9f7ee7ed13e4 63 void Map::update_cell_value(int widthIndice,int heightIndice ,float proba){
Ludwigfr 0:9f7ee7ed13e4 64 this->cellsLogValues[widthIndice][heightIndice]=this->cellsLogValues[widthIndice][heightIndice]+this->proba_to_log(proba)+this->initialLogValues[widthIndice][heightIndice];//map is filled as map[0][0] get the data for the point closest to the origin
Ludwigfr 0:9f7ee7ed13e4 65 }
Ludwigfr 0:9f7ee7ed13e4 66
Ludwigfr 0:9f7ee7ed13e4 67 float Map::cell_width_coordinate_to_world(int i){
Ludwigfr 0:9f7ee7ed13e4 68 return this->sizeCellWidth/2+i*this->sizeCellWidth;
Ludwigfr 0:9f7ee7ed13e4 69 }
Ludwigfr 0:9f7ee7ed13e4 70
Ludwigfr 0:9f7ee7ed13e4 71 float Map::cell_height_coordinate_to_world(int j){
Ludwigfr 0:9f7ee7ed13e4 72 return this->sizeCellHeight/2+j*this->sizeCellHeight;
Ludwigfr 0:9f7ee7ed13e4 73 }
Ludwigfr 0:9f7ee7ed13e4 74
Ludwigfr 0:9f7ee7ed13e4 75 float Map::get_proba_cell(int widthIndice, int heightIndice){
Ludwigfr 0:9f7ee7ed13e4 76 return this->log_to_proba(this->cellsLogValues[widthIndice][heightIndice]);
Ludwigfr 0:9f7ee7ed13e4 77 }
Ludwigfr 0:9f7ee7ed13e4 78
Ludwigfr 0:9f7ee7ed13e4 79 //returns the log value that the cell is occupied from the probability value [0,1]
Ludwigfr 0:9f7ee7ed13e4 80 float Map::proba_to_log(float p){
Ludwigfr 0:9f7ee7ed13e4 81 return log(p/(1-p));
Ludwigfr 0:9f7ee7ed13e4 82 }
Ludwigfr 0:9f7ee7ed13e4 83
Ludwigfr 5:19f24c363418 84 void Map::fill_map_with_kalman_knowledge(){
Ludwigfr 5:19f24c363418 85 float xCenterRect=0;
Ludwigfr 5:19f24c363418 86 float yCenterRect=0;
Ludwigfr 5:19f24c363418 87 for (int i = 0; i<this->nbCellWidth; i++) {
Ludwigfr 5:19f24c363418 88 for (int j = 0; j<this->nbCellHeight; j++) {
Ludwigfr 5:19f24c363418 89 if(j==0 || j==this->nbCellHeight-1 || i==0 || i==this->nbCellWidth-1)
Ludwigfr 5:19f24c363418 90 this->cellsLogValues[i][j] = this->proba_to_log(1);
Ludwigfr 5:19f24c363418 91 else{
Ludwigfr 5:19f24c363418 92 //rectangle in center 30 width, 20 height
Ludwigfr 5:19f24c363418 93 if(((this->cell_width_coordinate_to_world(i) >= this->widthRealMap/2-15) && (this->cell_width_coordinate_to_world(i) <= this->widthRealMap/2+15)) && ((this->cell_height_coordinate_to_world(j) >= this->heightRealMap/2-10) && (this->cell_height_coordinate_to_world(j) <= this->heightRealMap/2+10)) ){
Ludwigfr 5:19f24c363418 94 this->cellsLogValues[i][j] = this->proba_to_log(1);
Ludwigfr 5:19f24c363418 95 }else
Ludwigfr 5:19f24c363418 96 this->cellsLogValues[i][j] = this->proba_to_log(0.5);
Ludwigfr 5:19f24c363418 97 }
Ludwigfr 5:19f24c363418 98
Ludwigfr 5:19f24c363418 99 }
Ludwigfr 5:19f24c363418 100 }
Ludwigfr 5:19f24c363418 101 }
Ludwigfr 5:19f24c363418 102
Ludwigfr 0:9f7ee7ed13e4 103 /*
Ludwigfr 0:9f7ee7ed13e4 104
Ludwigfr 0:9f7ee7ed13e4 105 float Map::robot_x_coordinate_in_world(float robot_x, float robot_y){
Ludwigfr 0:9f7ee7ed13e4 106 return this->nbCellWidth*this->sizeCellWidth-robot_y;
Ludwigfr 0:9f7ee7ed13e4 107 }
Ludwigfr 0:9f7ee7ed13e4 108
Ludwigfr 0:9f7ee7ed13e4 109 float Map::robot_y_coordinate_in_world(float robot_x, float robot_y){
Ludwigfr 0:9f7ee7ed13e4 110 return robot_x;
Ludwigfr 0:9f7ee7ed13e4 111 }
Ludwigfr 0:9f7ee7ed13e4 112
Ludwigfr 0:9f7ee7ed13e4 113
Ludwigfr 0:9f7ee7ed13e4 114 void MiniExplorerCoimbra::print_final_map() {
Ludwigfr 0:9f7ee7ed13e4 115 float currProba;
Ludwigfr 0:9f7ee7ed13e4 116 pc.printf("\n\r");
Ludwigfr 0:9f7ee7ed13e4 117 for (int y = this->nbCellHeight -1; y>-1; y--) {
Ludwigfr 0:9f7ee7ed13e4 118 for (int x= 0; x<this->nbCellWidth; x++) {
Ludwigfr 0:9f7ee7ed13e4 119 currProba=this->log_to_proba(this->cellsLogValues[x][y]);
Ludwigfr 0:9f7ee7ed13e4 120 if ( currProba < 0.5) {
Ludwigfr 0:9f7ee7ed13e4 121 pc.printf(" ");
Ludwigfr 0:9f7ee7ed13e4 122 } else {
Ludwigfr 0:9f7ee7ed13e4 123 if(currProba==0.5)
Ludwigfr 0:9f7ee7ed13e4 124 pc.printf(" . ");
Ludwigfr 0:9f7ee7ed13e4 125 else
Ludwigfr 0:9f7ee7ed13e4 126 pc.printf(" X ");
Ludwigfr 0:9f7ee7ed13e4 127 }
Ludwigfr 0:9f7ee7ed13e4 128 }
Ludwigfr 0:9f7ee7ed13e4 129 pc.printf("\n\r");
Ludwigfr 0:9f7ee7ed13e4 130 }
Ludwigfr 0:9f7ee7ed13e4 131 }
Ludwigfr 0:9f7ee7ed13e4 132
Ludwigfr 0:9f7ee7ed13e4 133
Ludwigfr 0:9f7ee7ed13e4 134 void Map::print_final_map_with_robot_position(float robot_x,float robot_y) {
Ludwigfr 0:9f7ee7ed13e4 135 float currProba;
Ludwigfr 0:9f7ee7ed13e4 136 float Xrobot=this->robot_x_coordinate_in_world(robot_x,robot_y);
Ludwigfr 0:9f7ee7ed13e4 137 float Yrobot=this->robot_y_coordinate_in_world(robot_x,robot_y);
Ludwigfr 0:9f7ee7ed13e4 138
Ludwigfr 0:9f7ee7ed13e4 139 float heightIndiceInOrthonormal;
Ludwigfr 0:9f7ee7ed13e4 140 float widthIndiceInOrthonormal;
Ludwigfr 0:9f7ee7ed13e4 141
Ludwigfr 0:9f7ee7ed13e4 142 float widthMalus=-(3*sizeCellWidth/2);
Ludwigfr 0:9f7ee7ed13e4 143 float widthBonus=sizeCellWidth/2;
Ludwigfr 0:9f7ee7ed13e4 144
Ludwigfr 0:9f7ee7ed13e4 145 float heightMalus=-(3*sizeCellHeight/2);
Ludwigfr 0:9f7ee7ed13e4 146 float heightBonus=sizeCellHeight/2;
Ludwigfr 0:9f7ee7ed13e4 147
Ludwigfr 0:9f7ee7ed13e4 148 pc.printf("\n\r");
Ludwigfr 0:9f7ee7ed13e4 149 for (int y = this->nbCellHeight -1; y>-1; y--) {
Ludwigfr 0:9f7ee7ed13e4 150 for (int x= 0; x<this->nbCellWidth; x++) {
Ludwigfr 0:9f7ee7ed13e4 151 heightIndiceInOrthonormal=this->cell_height_coordinate_to_world(y);
Ludwigfr 0:9f7ee7ed13e4 152 widthIndiceInOrthonormal=this->cell_width_coordinate_to_world(x);
Ludwigfr 0:9f7ee7ed13e4 153 if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 0:9f7ee7ed13e4 154 pc.printf(" R ");
Ludwigfr 0:9f7ee7ed13e4 155 else{
Ludwigfr 0:9f7ee7ed13e4 156 currProba=this->log_to_proba(this->cellsLogValues[x][y]);
Ludwigfr 0:9f7ee7ed13e4 157 if ( currProba < 0.5)
Ludwigfr 0:9f7ee7ed13e4 158 pc.printf(" ");
Ludwigfr 0:9f7ee7ed13e4 159 else{
Ludwigfr 0:9f7ee7ed13e4 160 if(currProba==0.5)
Ludwigfr 0:9f7ee7ed13e4 161 pc.printf(" . ");
Ludwigfr 0:9f7ee7ed13e4 162 else
Ludwigfr 0:9f7ee7ed13e4 163 pc.printf(" X ");
Ludwigfr 0:9f7ee7ed13e4 164 }
Ludwigfr 0:9f7ee7ed13e4 165 }
Ludwigfr 0:9f7ee7ed13e4 166 }
Ludwigfr 0:9f7ee7ed13e4 167 pc.printf("\n\r");
Ludwigfr 0:9f7ee7ed13e4 168 }
Ludwigfr 0:9f7ee7ed13e4 169 }
Ludwigfr 0:9f7ee7ed13e4 170
Ludwigfr 0:9f7ee7ed13e4 171 void Map::print_final_map_with_robot_position_and_target(float robot_x,float robot_y,float targetXWorld, float targetYWorld) {
Ludwigfr 0:9f7ee7ed13e4 172 float currProba;
Ludwigfr 0:9f7ee7ed13e4 173 float Xrobot=this->robot_x_coordinate_in_world(robot_x,robot_y);
Ludwigfr 0:9f7ee7ed13e4 174 float Yrobot=this->robot_y_coordinate_in_world(robot_x,robot_y);
Ludwigfr 0:9f7ee7ed13e4 175
Ludwigfr 0:9f7ee7ed13e4 176 float heightIndiceInOrthonormal;
Ludwigfr 0:9f7ee7ed13e4 177 float widthIndiceInOrthonormal;
Ludwigfr 0:9f7ee7ed13e4 178
Ludwigfr 0:9f7ee7ed13e4 179 float widthMalus=-(3*sizeCellWidth/2);
Ludwigfr 0:9f7ee7ed13e4 180 float widthBonus=sizeCellWidth/2;
Ludwigfr 0:9f7ee7ed13e4 181
Ludwigfr 0:9f7ee7ed13e4 182 float heightMalus=-(3*sizeCellHeight/2);
Ludwigfr 0:9f7ee7ed13e4 183 float heightBonus=sizeCellHeight/2;
Ludwigfr 0:9f7ee7ed13e4 184
Ludwigfr 0:9f7ee7ed13e4 185 pc.printf("\n\r");
Ludwigfr 0:9f7ee7ed13e4 186 for (int y = this->nbCellHeight -1; y>-1; y--) {
Ludwigfr 0:9f7ee7ed13e4 187 for (int x= 0; x<this->nbCellWidth; x++) {
Ludwigfr 0:9f7ee7ed13e4 188 heightIndiceInOrthonormal=this->cell_height_coordinate_to_world(y);
Ludwigfr 0:9f7ee7ed13e4 189 widthIndiceInOrthonormal=this->cell_width_coordinate_to_world(x);
Ludwigfr 0:9f7ee7ed13e4 190 if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 0:9f7ee7ed13e4 191 pc.printf(" R ");
Ludwigfr 0:9f7ee7ed13e4 192 else{
Ludwigfr 0:9f7ee7ed13e4 193 if(targetYWorld >= (heightIndiceInOrthonormal+heightMalus) && targetYWorld <= (heightIndiceInOrthonormal+heightBonus) && targetXWorld >= (widthIndiceInOrthonormal+widthMalus) && targetXWorld <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 0:9f7ee7ed13e4 194 pc.printf(" T ");
Ludwigfr 0:9f7ee7ed13e4 195 else{
Ludwigfr 0:9f7ee7ed13e4 196 currProba=this->log_to_proba(this->cellsLogValues[x][y]);
Ludwigfr 0:9f7ee7ed13e4 197 if ( currProba < 0.5)
Ludwigfr 0:9f7ee7ed13e4 198 pc.printf(" ");
Ludwigfr 0:9f7ee7ed13e4 199 else{
Ludwigfr 0:9f7ee7ed13e4 200 if(currProba==0.5)
Ludwigfr 0:9f7ee7ed13e4 201 pc.printf(" . ");
Ludwigfr 0:9f7ee7ed13e4 202 else
Ludwigfr 0:9f7ee7ed13e4 203 pc.printf(" X ");
Ludwigfr 0:9f7ee7ed13e4 204 }
Ludwigfr 0:9f7ee7ed13e4 205 }
Ludwigfr 0:9f7ee7ed13e4 206 }
Ludwigfr 0:9f7ee7ed13e4 207 }
Ludwigfr 0:9f7ee7ed13e4 208 pc.printf("\n\r");
Ludwigfr 0:9f7ee7ed13e4 209 }
Ludwigfr 0:9f7ee7ed13e4 210 }
Ludwigfr 0:9f7ee7ed13e4 211 */