/*********** * File : navigation.cpp * * TODO : * - add pid_control() * - change navigate to use vec_line instead * - Clear the map fails every time you restart a navigation * - Check if path optimisation is working every time, sometimes is not that great * * TOTEST * - Different front sensor map modification * * Notice : */ /****************************************************** includes */ #include #include #include #include #include #include #include #include #include #include #include "ros/ros.h" #include #include #include #include "discovery/TagObservationPose.h" #include "discovery/Odometry.h" #include "discovery/MotorRotate.h" #include "discovery/MotorDistance.h" #include "discovery/MotorSpeeds.h" #include "discovery/IdentifiedTag.h" #include "discovery/MapSrv.h" #include "discovery/ExploreSrv.h" //#include "discovery/GotoPoseSrv.h" #include #include #include #include #include "common.h" #include "C_robot.cpp" using namespace std; //#define SIMUWORLD #define ODOMETRY /****************************************************** Constants */ #define NAV_CELL_SIZE 1 #define EXPAND_SIZE 12 // 1 => always square wall expansion. 0 => circle #define SQUARE_ANGLE 0 #define ERR_DIST_MAX 4.0 #define ERR_DIST_PERCENTAGE 0.90 #define NAV_MAP_HEIGHT MAP_HEIGHT/NAV_CELL_SIZE #define NAV_MAP_WIDTH MAP_WIDTH/NAV_CELL_SIZE int ROBOT_ORIGIN_X = 70; int ROBOT_ORIGIN_Y = 70; #define TARGET_CLEAR_DISTANCE 6 /****************************************************** Types *****/ /** @type t_item : Object in the grid referential */ typedef struct _item { float x; float y; float theta; std::string name; int color; int shape; float x_tag; float y_tag; float theta_tag; float turret_angle; float camera_angle; } t_item; /** @type t_line : */ typedef struct _line { float delta_x; float delta_y; int start_x; int start_y; } t_line; /** @type t_cell_state : differents states of a cell in the navigation occupancy grid */ typedef enum {FREE, BUSY, UNKNOWN } t_cell_state; /** @type t_cell_block : block of a cell in the navigation occupancy grid */ typedef struct _cell_block { t_cell_state state; int distance; // used for pathfinding algorithm } t_cell_block; /** @type route : vector of pointers over the grid */ typedef vector route; /** @type vec_line : vector of t_line */ typedef vector vec_line; /****************************************************** Globals */ // original occupancy grid grid float m[MAP_HEIGHT][MAP_WIDTH]; float l[MAP_HEIGHT][MAP_WIDTH]; // New occupancy grid t_cell_block grid[NAV_MAP_HEIGHT][NAV_MAP_WIDTH]; // objects on the grid vector items; // order to visit the items vector travel; // differents publishers ros::Publisher tagObsPose_pub; ros::Publisher map_pub; ros::Publisher sensors_pub; ros::Publisher traj_pub; ros::Publisher drive_dist_pub; ros::Publisher drive_speed_pub; ros::Publisher rotate_pub; ros::Publisher tagId_pub; ros::Publisher obs_pub; enum {MOVING, STOP, ROTATING, GO_FORWARD, RECOVERING, STOP_WHILE_RECOVERING} state; float last_x = ROBOT_ORIGIN_X; float last_y = ROBOT_ORIGIN_Y; float last_theta = 0; float last_x_particles = ROBOT_ORIGIN_X; float last_y_particles = ROBOT_ORIGIN_Y; float last_theta_particles = 0; // visualizations containers visualization_msgs::Marker line_walls; visualization_msgs::MarkerArray tagIDarr; visualization_msgs::MarkerArray obsArray; // Contain all possibles travels between the items vector all_travels; // ok to move bool is_build = false; C_robot robot; /****************************************************** Functions */ void print_grid() { for(int i =10 ; i < NAV_MAP_HEIGHT-10 ; i++) { for(int j = 10 ; j < NAV_MAP_WIDTH-10 ; j++) { if(i == 30 && j == 50) { cout << "++" ; } else if(grid[i][j].distance < 10 && grid[i][j].distance >= 0 ) { cout << " " << grid[i][j].distance ; }else if (grid[i][j].distance > 99) { cout << "--" ; } else { cout << grid[i][j].distance ; } cout << " "; } cout << endl; } } void publish_grid() { cout << "publish Grid" << endl; nav_msgs::OccupancyGrid msg; msg.header.frame_id = "/map"; msg.header.stamp = ros::Time::now(); msg.info.map_load_time = ros::Time::now(); msg.info.resolution = (float)NAV_CELL_SIZE/100; msg.info.width = NAV_MAP_WIDTH; msg.info.height = NAV_MAP_HEIGHT; //std::cout << "Pose: x=" << robotPose_x << " y=" << robotPose_y << " theta=" << robotPose_theta<< std::endl; msg.info.origin.position.x = (float)-MAP_WIDTH/200; msg.info.origin.position.y = (float)-MAP_HEIGHT/200; msg.info.origin.position.z = 0; msg.info.origin.orientation.x = 0; msg.info.origin.orientation.y = 0; msg.info.origin.orientation.z = 0; //sin(robotPose_theta/2); msg.info.origin.orientation.w = 1; //cos(robotPose_theta/2); msg.data.resize(NAV_MAP_WIDTH*NAV_MAP_HEIGHT); for (unsigned int i=0; ipose.x + ROBOT_ORIGIN_X; last_y= msg->pose.y + ROBOT_ORIGIN_Y; last_theta= fmod(msg->pose.theta, 2*PI) ;*/ } void update_pos_particles(const discovery::OdometryConstPtr & msg) { last_x = msg->pose.x + ROBOT_ORIGIN_X; last_y = msg->pose.y+ ROBOT_ORIGIN_Y; last_theta = fmod(msg->pose.theta, 2*PI) ; } //---------------------------- copied functions from wallFollow void do_stop() { #ifndef SIMUWORLD std_srvs::Empty empty; ros::service::call("stop_motors", empty); #else state = STOP; #endif } void do_rotate(float rad) { #ifndef SIMUWORLD if(fabs(rad) > 5/180*PI) { discovery::MotorRotate msg; msg.angle = rad; msg.speed = 50; rotate_pub.publish(msg); } else { state = STOP; } #else last_theta += rad; while(last_theta < -PI) last_theta += 2*PI; while(last_theta > PI) last_theta -= 2*PI; state = STOP; // printf("Change theta : X=%f Y=%f theta=%f \n", last_x, last_y, last_theta); #endif } void do_drive_dist(float dist, int speed) { #ifndef SIMUWORLD discovery::MotorDistance msg; msg.distance = dist; msg.speed = speed; printf("Publishing dist %f %d\n", dist, speed); drive_dist_pub.publish(msg); #else state = STOP ; #endif } void do_drive_speeds(int left, int right) { #ifndef SIMUWORLD discovery::MotorSpeeds msg; msg.left = left; msg.right = right; drive_speed_pub.publish(msg); #else float coeff = (float)abs(left + right)/40.0; state = STOP ; last_x += coeff*cos(last_theta); last_y += coeff*sin(last_theta); // printf("Change pos : X=%f Y=%f theta=%f : coeff=%f lspeed=%d rspeed=%d \n", last_x, last_y, last_theta,coeff, left,right); #endif } /**************************** VISUALIZATION CRAP */ void init_line_walls() { line_walls.header.frame_id = "/map"; line_walls.header.stamp = ros::Time::now(); line_walls.ns = "traj_lines"; line_walls.action = visualization_msgs::Marker::ADD; line_walls.pose.orientation.w = 1.0; line_walls.id = 2; line_walls.type = visualization_msgs::Marker::LINE_STRIP; line_walls.scale.x = 0.010; line_walls.color.r = 1.0; line_walls.color.a = 1.0; line_walls.color.b = 1.0; } void addTagIdToMap(const t_item & item) { static int tagID = 1; std::cout << "Let's print this new Tag receive !" << std::endl; uint32_t shape_i = visualization_msgs::Marker::TEXT_VIEW_FACING; visualization_msgs::Marker marker; // Set the frame ID and timestamp. marker.header.frame_id = "/map"; marker.header.stamp = ros::Time::now(); // Set the namespace and id for this marker. This serves to create a unique ID // Any marker sent with the same namespace and id will overwrite the old one marker.ns = "TagID"; marker.id = tagID++; marker.text = item.name; // Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER marker.type = shape_i; // Set the marker action. Options are ADD and DELETE marker.action = visualization_msgs::Marker::ADD; // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header marker.pose.position.x = (float)NAV_MAP_HEIGHT/200 - item.y/100 ; marker.pose.position.y = -(float)NAV_MAP_HEIGHT/200 + item.x/100 ; marker.pose.position.z = 0.20; // Set the scale of the marker -- 1x1x1 here means 1m on a side marker.scale.z = 0.12; // Set the color -- be sure to set alpha to something non-zero! marker.color.r = 0.0f; marker.color.g = 1.0f; marker.color.b = 0.0f; marker.color.a = 1.0; marker.lifetime = ros::Duration(); // Publish the marker //tag_pub.publish(marker); tagIDarr.markers.push_back(marker); tagId_pub.publish(tagIDarr); if(1) { static int tagID = 0; uint32_t shape = visualization_msgs::Marker::SPHERE; visualization_msgs::Marker marker; // Set the frame ID and timestamp. marker.header.frame_id = "/map"; marker.header.stamp = ros::Time::now(); // Set the namespace and id for this marker. This serves to create a unique ID // Any marker sent with the same namespace and id will overwrite the old one marker.ns = "basic_shapes2"; marker.id = tagID++; // Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER marker.type = shape; // Set the marker action. Options are ADD and DELETE marker.action = visualization_msgs::Marker::ADD; // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header marker.pose.position.x = (float)NAV_MAP_WIDTH/200 - item.y/100 ; marker.pose.position.y = -(float)NAV_MAP_HEIGHT/200 + item.x/100 ; marker.pose.position.z = 0.15; marker.pose.orientation.x = 0.0; marker.pose.orientation.y = 0.0; marker.pose.orientation.z = 0.0; marker.pose.orientation.w = 1; // Set the scale of the marker -- 1x1x1 here means 1m on a side marker.scale.x = 0.04; marker.scale.y = 0.04; marker.scale.z = 0.04; // Set the color -- be sure to set alpha to something non-zero! marker.color.r = 1.0f; marker.color.g = 0.0f; marker.color.b = 0.6f; marker.color.a = 1.0; marker.lifetime = ros::Duration(); obsArray.markers.push_back(marker); obs_pub.publish(obsArray); } } /****************************************************************************************************/ int track_dist(double dist, double begin_x, double begin_y, int max_speed=120) { double dx = last_x - begin_x; double dy = last_y - begin_y; double dist_travelled = sqrt(dx * dx + dy * dy); printf("travelled: %lf / %lf\n", dist_travelled, dist); double dist_left = fmin(dist - dist_travelled, 15); if(dist_left > 0.2 && dist_left < 3) return 5; if(dist_left <= 0) return 0; double stopDist = 1; double startSlowDownDist = 15; return (dist_left - stopDist) / (startSlowDownDist - stopDist) * max_speed; } void got_stopped(const std_msgs::Empty::ConstPtr &msg) { if(state == GO_FORWARD || state == ROTATING || state == MOVING) { printf("GOT STOPPED!!!!!!#####\n"); state = STOP; } else if(state == RECOVERING) { state = STOP_WHILE_RECOVERING; } } void clear_position(int cell_i, int cell_j, float distance = CLEAR_ROBOT_SIZE /2) { // Clear the actual position of the robot // Min-max to prevent out of bounds for(int i =max(0,(cell_i-distance)/NAV_CELL_SIZE) ; i <= min((cell_i+distance)/NAV_CELL_SIZE,NAV_MAP_HEIGHT) ; i++) for(int j = max(0,(cell_j-distance)/NAV_CELL_SIZE) ; j < min((cell_j+distance)/NAV_CELL_SIZE, NAV_MAP_WIDTH) ; j++) { grid[i][j].distance = -1; grid[i][j].state = FREE; } } void clear_actual_position() { clear_position(last_x, last_y); } pair getPos(t_cell_block* cell) { pair ret; ret.first = (cell - &(grid[0][0]))/(NAV_MAP_WIDTH); ret.second = (cell - &(grid[0][0]))%(NAV_MAP_WIDTH); return ret; } vector getNeighbors(int i, int j) { vector ret; if(i > 0) ret.push_back(&(grid[i-1][j])); if(j > 0) ret.push_back(&(grid[i][j-1])); if(i < NAV_MAP_HEIGHT-1) ret.push_back(&(grid[i+1][j])); if(j < NAV_MAP_WIDTH) ret.push_back(&(grid[i][j+1])); return ret; } vector getNeighborsi(int i, int j) { vector ret; if(i > 0) ret.push_back(&(grid[i-1][j])); if(i < NAV_MAP_HEIGHT-1) ret.push_back(&(grid[i+1][j])); if(j > 0) ret.push_back(&(grid[i][j-1])); if(j < NAV_MAP_WIDTH) ret.push_back(&(grid[i][j+1])); return ret; } vector getNeighborsj(int i, int j) { vector ret; if(j > 0) ret.push_back(&(grid[i][j-1])); if(j < NAV_MAP_WIDTH) ret.push_back(&(grid[i][j+1])); if(i > 0) ret.push_back(&(grid[i-1][j])); if(i < NAV_MAP_HEIGHT-1) ret.push_back(&(grid[i+1][j])); return ret; } route revertRoute(route arg) { route ret(arg.size()); for(size_t i = 0 ; i < arg.size() ; i++) { ret[arg.size()-1-i] = arg[i]; } return ret; } bool isFreeAndDist0(t_cell_block& cell) { if(cell.distance == -1 && cell.state == FREE) { return true; } else { return false; } } vector getNeighborsFree(int i, int j) { vector ret; if(i > 0 && isFreeAndDist0(grid[i-1][j])) { ret.push_back(&(grid[i-1][j])); } if(j > 0 && isFreeAndDist0(grid[i][j-1])) { ret.push_back(&(grid[i][j-1])); } if(i < NAV_MAP_HEIGHT-1 && isFreeAndDist0(grid[i+1][j])){ ret.push_back(&(grid[i+1][j])); } if(j < NAV_MAP_WIDTH-1 && isFreeAndDist0(grid[i][j+1])) { ret.push_back(&(grid[i][j+1])); } return ret; } int loadMap(const std::string filename = DEFAULT_MAP_FILE) { std::ifstream is(filename.c_str(), ios::binary); if(!is.is_open()) { printf("SUPER FAIL; NO MAP LOADED BE ADVISED!!!!!!! WILL QUIT\n"); exit(1); return -1; // fail } else { is.read((char*)&l, sizeof(l)); is.read((char*)&m, sizeof(m)); return 0; } } void getLatestMap() { discovery::MapSrv request; ros::service::call("map/request_map", request); for(int i=0; i toExpand; for(int i = 0 ; i < NAV_MAP_HEIGHT ; i++ ) { for(int j = 0 ; j < NAV_MAP_WIDTH ; j++) { // for each NAV cell, check the state of the real occupancy grid int i2 = i*NAV_CELL_SIZE/CELL_SIZE; int j2 = j*NAV_CELL_SIZE/CELL_SIZE; t_cell_state state = UNKNOWN; while(state != BUSY && i2 < (i+1)*NAV_CELL_SIZE/CELL_SIZE) { if(m[i2][j2] > 0.6) state = BUSY; else if (m[i2][j2] < 0.3) state = FREE; if(++j2 == (j+1)*NAV_CELL_SIZE/CELL_SIZE) { j2 = j*NAV_CELL_SIZE/CELL_SIZE; i2++; } } grid[i][j].state = state; if(state == BUSY) { static int nbCells = 0; // add cell to expand after the import for(int i3 = max(0,i-EXPAND_SIZE/NAV_CELL_SIZE) ; // min for border flow i3 < min(i+EXPAND_SIZE/NAV_CELL_SIZE, NAV_MAP_HEIGHT) ; i3++) { for(int j3 = max(0,j-EXPAND_SIZE/NAV_CELL_SIZE) ; // min for border flow j3 < min(j+EXPAND_SIZE/NAV_CELL_SIZE, NAV_MAP_WIDTH) ; j3++) { if(SQUARE_ANGLE || SQUARE((float)((i3-i)*NAV_CELL_SIZE))+SQUARE((float)((j3-j)*NAV_CELL_SIZE)) < SQUARE((float)EXPAND_SIZE)) { toExpand.push_back(&(grid[i3][j3])); } } } } } } // and now we expand ! for(int i = 0 ; i < toExpand.size() ; i++ ) { toExpand[i]->state = BUSY; } } void clear_distance() { for(int i =0 ; i < NAV_MAP_HEIGHT ; i++) for(int j = 0 ; j < NAV_MAP_WIDTH ; j++) grid[i][j].distance = -1; } void list_empty() { for(int i =0 ; i < NAV_MAP_HEIGHT ; i++) for(int j = 0 ; j < NAV_MAP_WIDTH ; j++) if(grid[i][j].state == FREE) cout << "Free cell (" << i << "," << j << ")." << endl ; } void compute_distance_base(int i_cell, int j_cell) { if(grid[i_cell][j_cell].distance == 0) { cout << "distance already computed" << endl; return; } else { cout << "Compute distance for point : " << i_cell << "," << j_cell << endl; } clear_distance(); vector todo = getNeighborsFree(i_cell,j_cell); vector toadd; int distance = 0; grid[i_cell][j_cell].distance = distance; while(todo.size() > 0) { distance++; // We parse every cell to update for this turn for(vector::iterator it = todo.begin(); it < todo.end() ; it++){ if((**it).distance == -1 ) { (**it).distance = distance; vector tmp = getNeighborsFree(getPos((*it)).first , getPos((*it)).second ) ; if(tmp.size() > 0) toadd.insert(toadd.end(), tmp.begin(), tmp.end()); } } // now we invert to do and toadd vectors; todo.clear(); todo = toadd; toadd.clear(); } } void compute_distance(t_item item) { int i_cell = item.x/NAV_CELL_SIZE; int j_cell = item.y/NAV_CELL_SIZE; compute_distance_base(i_cell, j_cell); } route findRouteDirect_base(int start_i, int start_j, int end_i, int end_j) { route ret; bool ok = true; int begin_i = min(start_i, end_i); int begin_j = min(start_j, end_j); int target_i = max(start_i, end_i); int target_j = max(start_j, end_j); for(int i = begin_i ; i <= target_i && ok; i++) { for(int j = begin_j ; j <= target_j && ok; j++) { if(grid[i][j].state == FREE) { ret.push_back(&(grid[i][j])); } else { ok = false; ret.clear(); } } } return ret; } /***************************************** line optimisation crap */ vec_line get_lines(const route & path) { vec_line lines; if(path.size() < 2) { return lines; //nothing to compute } bool endofnav = false; int dx = 0; int dy = 0; state = STOP; t_line newline; newline.start_x = getPos(path[0]).first; newline.start_y = getPos(path[0]).second; for(unsigned int cell= 0 ; !endofnav && cell < path.size()-1 && ros::ok(); cell++) { int ddx = getPos(path[cell]).first - getPos(path[cell+1]).first; int ddy = getPos(path[cell]).second - getPos(path[cell+1]).second ; if(dx + ddx == 0 || dy + ddy== 0) { // We are still on the same line. Add the distance to do dx += ddx; dy += ddy; } else { newline.delta_x = -dx; newline.delta_y = -dy; lines.push_back(newline); newline = { 0,0,0,0 }; // start a new line dx = ddx; dy = ddy; newline.start_x = getPos(path[cell]).first; newline.start_y = getPos(path[cell]).second; } } newline.delta_x = -dx; newline.delta_y = -dy; lines.push_back(newline); cout << "Number of lines : "<< lines.size() << endl; for(unsigned int i = 0 ; i < lines.size() ; i++) { cout << "[" << i << "]" << lines[i].start_x << "," << lines[i].start_y << " Delta : " << lines[i].delta_x << ", " << lines[i].delta_y << endl; } return lines; } route linesToRoute(vec_line & lines) { route ret; int i,j; cout << "NEW : Number of lines : "<< lines.size() << endl; for(unsigned int i = 0 ; i < lines.size() ; i++) { cout << "[" << i << "]" << lines[i].start_x << "," << lines[i].start_y << " Delta : " << lines[i].delta_x << ", " << lines[i].delta_y << endl; } for(vec_line::iterator it = lines.begin() ; it < lines.end() ; it++) { i=it->start_x; j=it->start_y; ret.push_back(&(grid[i][j])); for(int cell = 0 ; cell < (it->delta_x + it->delta_y) ; cell++ ) { if(it->delta_x <0) { i--; } else if(it->delta_x >0) { i++; } if(it->delta_y <0) { j--; } else if(it->delta_y >0) { j++; } ret.push_back(&(grid[i][j])); } } return ret; } route path_optimisation(const route & path) { route ret; if(path.size() < 2) { return ret; //nothing to compute } vec_line lines = get_lines(path); for(unsigned int i = 1 ; i < lines.size() -1 ; i++) { // Never compute the first and last line // check if the delta before and after is the same if((lines[i-1].delta_x != 0 && lines[i+1].delta_x != 0 && lines[i-1].delta_y == 0 && lines[i-1].delta_y == 0) || (lines[i-1].delta_y != 0 && lines[i+1].delta_y != 0 && lines[i-1].delta_x == 0 && lines[i-1].delta_x == 0)) { // compute the min and max parrallels line from the existing line int start_i = lines[i].start_x; int start_j = lines[i].start_y; int end_i = lines[i].start_x + lines[i].delta_x; int end_j = lines[i].start_y+ lines[i].delta_y; while(findRouteDirect_base(start_i,start_j, end_i, end_j).size() == lines[i].delta_x+ lines[i].delta_y + 1 ) { if(lines[i].delta_x == 0) { start_i--; end_i--; } else { start_j--; end_j--; } } int min_start_i = start_i; int min_start_j = start_j; int min_end_i = end_i; int min_end_j = end_j; start_i = lines[i].start_x; start_j = lines[i].start_y; end_i = lines[i].start_x + lines[i].delta_x; end_j = lines[i].start_y+ lines[i].delta_y; cout << "Target : " << lines[i].delta_x+ lines[i].delta_y + 1 << endl; while(findRouteDirect_base(start_i,start_j, end_i, end_j).size() == lines[i].delta_x+ lines[i].delta_y + 1 ) { if(lines[i].delta_x == 0) { start_i++; end_i++; } else { start_j++; end_j++; } } int max_start_i = start_i; int max_start_j = start_j; int max_end_i = end_i; int max_end_j = end_j; // Get average start_i = (min_start_i + max_start_i) / 2; start_j = (min_start_j + max_start_j) / 2; end_i = (min_end_i + max_end_i) / 2; end_j = (min_end_j + max_end_j) / 2; // Update line path lines[i-1].delta_x = start_i - lines[i-1].start_x ; lines[i-1].delta_y = start_j - lines[i-1].start_y ; lines[i].start_x = start_i; lines[i].start_y = start_j; lines[i+1].delta_x = lines[i+1].start_x - end_i + lines[i+1].delta_x ; lines[i+1].delta_y = lines[i+1].start_y - end_j + lines[i+1].delta_y ; lines[i+1].start_x = end_i; lines[i+1].start_y = end_j; } } ret = linesToRoute(lines); // compute to route format cout << "End of optimisation" << endl; cout << "Size of input data" < 0 ; distance--) { // We add this cell ret.push_back(&(grid[i][j])); //cout <<"[" << distance << "] Add cell (" << i<< "," << j << ")." << endl ; route tmp ; // Test if we can go in the same direction int di = getPos(ret[ret.size()-2]).first - getPos(ret[ret.size()-1]).first ; int dj = getPos(ret[ret.size()-2]).second - getPos(ret[ret.size()-1]).second; if(di!=0 && dj == 0) { tmp = getNeighborsi(i,j); } else if (di==0 && dj != 0){ tmp = getNeighborsj(i,j); } else { tmp = getNeighbors(i,j); } // for each cell, we get the neighbors route::iterator it = tmp.begin(); //cout <<">>Neighbor (" << getPos(*it).first<< "," << getPos(*it).second << ") Distance : " << (*it)->distance << endl ; while((*it)->distance != distance-1) { it++; //cout <<">>Neighbor (" << getPos(*it).first<< "," << getPos(*it).second << ") Distance : " << (*it)->distance << endl ; } i = getPos(*it).first; j = getPos(*it).second; } if(ret.size() > 0) { ret.push_back(&(grid[end_i][end_j])); ret = path_optimisation(ret); } return ret; } route findRoute(t_item &start, t_item &end) { int start_i = start.x/NAV_CELL_SIZE; int start_j = start.y/NAV_CELL_SIZE; int end_i = end.x/NAV_CELL_SIZE; int end_j = end.y/NAV_CELL_SIZE; return findRoute_base(start_i, start_j, end_i, end_j); } route findRoute_toItem_fromHere(const t_item &end) { return findRoute_base(last_x/NAV_CELL_SIZE, last_y/NAV_CELL_SIZE, end.x/NAV_CELL_SIZE, end.y/NAV_CELL_SIZE )); } void transform_path_for_visualisation(const route & chemin) { for(unsigned int l = 0 ; l < chemin.size() ; l++ ) { geometry_msgs::Point p; float y = (getPos(chemin[l]).second +0.5)*NAV_CELL_SIZE ; float x = (getPos(chemin[l]).first+0.5)*NAV_CELL_SIZE ; p.x = (float)MAP_WIDTH/200 - (y) /100; p.y = -(float)MAP_HEIGHT/200 + (x ) /100 ; p.z = 0.035; line_walls.points.push_back(p); // cout <<"[" << l << "] Free cell (" << getPos(chemin[l]).first << "," << getPos(chemin[l]).second << ")." << endl ; } } bool check_reliability() { return false; } void correct_orientation(float dx, float dy) { bool ok = false; // Correct starting rotation static ros::Rate loop_rate(5); while(!ok) { float torotateFromOrigin = 0; if(dx > 0) { torotateFromOrigin = PI; } else if(dx < 0 ) { torotateFromOrigin = 0; } else if(dy > 0 ) { torotateFromOrigin = -PI/2; } else if(dy < 0 ) { torotateFromOrigin = PI/2; } cout << " torotateFromOrigin : " << torotateFromOrigin << endl; float todo = torotateFromOrigin - last_theta; while(todo <= -PI) { todo += 2*PI;} while(todo >= PI) { todo -= 2*PI;} state = MOVING; cout << " pos : " << last_theta << endl; cout << " initial correction : " << todo << endl; if(fabs(todo) < 0.3) { ok = true; } do_rotate(todo); loop_rate.reset(); while(ros::ok() && state == MOVING) { ros::spinOnce(); loop_rate.sleep(); } if(state != STOP || fabs(torotateFromOrigin-last_theta) < 0.3) { ok = true; } } } void goto_orientation(float rad) { bool ok = false; // Correct starting rotation static ros::Rate loop_rate(5); while(!ok) { cout << " target angle : " << rad << endl; float todo = rad - last_theta; while(todo <= -PI) { todo += 2*PI;} while(todo >= PI) { todo -= 2*PI;} state = MOVING; cout << " pos : " << last_theta << endl; cout << " initial correction : " << todo << endl; if(fabs(todo) < 0.3) { ok = true; } do_rotate(todo); loop_rate.reset(); while(ros::ok() && state == MOVING) { ros::spinOnce(); loop_rate.sleep(); } if(state != STOP || fabs(rad-last_theta) < 0.3) { ok = true; } } } bool is_drifting(t_cell_block * cell) { cout << "check if drifting" << endl; cout << "Pos- should : " << getPos(cell).first*NAV_CELL_SIZE << "," << getPos(cell).second*NAV_CELL_SIZE << endl; cout << "Pos- real : " << last_x << "," << last_y << endl; float err_dist_square = SQUARE(getPos(cell).first - last_x) + SQUARE(getPos(cell).second - last_y ); if(err_dist_square < SQUARE((float)ERR_DIST_MAX)) { return false; } else { cout << "drift detected ! " << sqrt(err_dist_square) << endl; return true; } } void do_pidControl(float refDistWall) { static float kpDist = 3; static float kiDist = 0; static float kdDist = 0; static float kpAng = 80; static float kiAng = 0; static float kdAng = 0; //Scale between the two pids, 0.5 -> equally important, 1 -> only distance important static float pidRatio = 0.5; static float distError; static float distErrorSum; static float distErrorDiff; static float angError; static float angErrorSum; static float angErrorDiff; //Memorize last value to calculate derivative float previousDistError = distError; //If we can not trust the wall, drive straight if (robot.wallDistRight() == C_robot::NO_DISTANCE || robot.wallAngleRight() == C_robot::NO_DISTANCE || robot.didRightWallFrontJump()) { do_drive_speeds(robot.speedFactor, robot.speedFactor); printf("No sensor data! Screwing out\n"); return; } /******* CALCULATE ERRORS IN DISTANCE AND ANGLE *******/ printf("Dist Wall : %f - Target %f \n",robot.wallDistRight(),refDistWall); distError = robot.wallDistRight() - refDistWall; distErrorSum += distError; distErrorDiff = distError - previousDistError; float previousAngError = angError; angError = robot.wallAngleRight() - ANGLE_RIGHT_WALL_SETPOINT; angErrorSum += angError; angErrorDiff = previousAngError - angError; //printf("Distance error is: %f\n", distError); //printf("Angle error is: %f\n", angError); //Calculate PID control values float pidDist = kpDist * distError + kiDist * distErrorSum + kdDist * distErrorDiff; float pidAng = -(kpAng * angError + kiAng * angErrorSum + kdAng * angErrorDiff); float pidFinal = pidRatio * pidDist + (1 - pidRatio) * pidAng; float rightWheelSpeed = robot.speedFactor; float leftWheelSpeed = robot.speedFactor; //printf("Speed: %f\n", robot.speedFactor); //Subtract the pid value from the current speed if (pidFinal > 0) rightWheelSpeed -= pidFinal; else leftWheelSpeed += pidFinal; printf("Publish speeds left: %.f right %.f\n", leftWheelSpeed,rightWheelSpeed); do_drive_speeds(leftWheelSpeed, rightWheelSpeed); } void nav_drive_line(float dx, float dy, t_cell_block * target) { static ros::Rate loop_rate(5); float begin_x = last_x; float begin_y = last_y; float dist_todo = sqrt(SQUARE(dx*NAV_CELL_SIZE) + SQUARE(dy*NAV_CELL_SIZE)); float refDistWall = -1; state = GO_FORWARD; loop_rate.reset(); ros::spinOnce(); robot.STATE = C_robot::MOVING_WITH_WALL; robot.speedFactor = 100; if(robot.isBothRightInRange() && robot.lastWallDistRight() < 15) { refDistWall = robot.lastWallDistRight(); // TODO has to be improved cout << "Track target : " << refDistWall << endl; } while(ros::ok() && state == GO_FORWARD) { ros::spinOnce(); //---- do we reach the point ? if((SQUARE(last_x- getPos(target).first*NAV_CELL_SIZE) + SQUARE(last_y - getPos(target).second*NAV_CELL_SIZE)) < min(SQUARE(ERR_DIST_MAX), SQUARE(ERR_DIST_PERCENTAGE*dist_todo)) ) { do_stop(); cout << "We are arrived ! " << endl; } else { //---- distance tracking cout << "waiting ! " << endl; robot.speedFactor = min(robot.speedFactor,track_dist(dist_todo, begin_x, begin_y)); if(robot.speedFactor>0 ) { if(refDistWall > 0) do_pidControl(refDistWall); else do_drive_speeds(robot.speedFactor, robot.speedFactor); } if(robot.speedFactor < 5) { printf("Reached destination, rotation-time\n"); break; } loop_rate.sleep(); } } } void nav_rotate(float rad) { static ros::Rate loop_rate(5); ros::spinOnce(); printf("I just spun, lol\n"); state = ROTATING; do_rotate(rad); loop_rate.reset(); while(state == ROTATING && ros::ok()) { ros::spinOnce(); cout << "rotating ! " << rad << endl; loop_rate.sleep(); } } /** Calculate the rotate between the 2 deltas lines in parameters * * @param dx : delta x arrival line * @param dy : delta y arrival line * @param ddx : delta x next line * @param ddy : delta y next line * @return float : angle to rotate */ float nav_get_rotate(float dx, float dy,float ddx, float ddy) { float ret = 0; if(dx != 0) { if((dx > 0 && ddy > 0) || (dx < 0 && ddy < 0) ) { ret = PI/2; } else { ret = -PI/2; } } else { if((dy > 0 && ddx < 0) || (dy < 0 && ddx > 0) ) { ret = PI/2; } else { ret = -PI/2; } } return ret; } void nav_wait_stop() { static ros::Rate loop_rate(5); ros::spinOnce(); printf("Wait stopping\n"); state = MOVING; do_stop(); loop_rate.reset(); while(state == MOVING && ros::ok()) { ros::spinOnce(); printf("Wait stopping\n"); loop_rate.sleep(); } } bool check_recovering_state() { bool result = false; static ros::Rate loop_rate(5); loop_rate.reset(); while(state == RECOVERING && ros::ok()) { result = true; loop_rate.sleep(); } return result; } bool navigate(const route &path) { if(path.size() == 0 ) { cout << "Nothing to compute" << endl; return true; } else { cout << "Nav start" << path.size() << "Orders todo" << endl; line_walls.points.clear(); transform_path_for_visualisation(path); traj_pub.publish(line_walls); ros::spinOnce(); bool endofnav = false; int dx = 0; int dy = 0; state = STOP; for(unsigned int cell= 0 ; !endofnav && cell < path.size()-1 && ros::ok(); cell++) { int ddx = getPos(path[cell]).first - getPos(path[cell+1]).first; int ddy = getPos(path[cell]).second - getPos(path[cell+1]).second ; if(dx + ddx == 0 || dy + ddy== 0) { // We are still on the same line. Add the distance to do dx += ddx; dy += ddy; } else { if(check_recovering_state()) { cout << "we just fail !" << endl; clear_distance(); clear_actual_position(); route new_route = findRoute_base(last_x/NAV_CELL_SIZE, last_y/NAV_CELL_SIZE, getPos(path[path.size()-1]).first, getPos(path[path.size()-1]).second ); cout << "New path : " << new_route.size() << endl; endofnav = true; } else { // Check if we are drifting from the path correct_orientation(dx,dy); cout << "ToDrive : dx=" << dx*NAV_CELL_SIZE << " dy= " << dy*NAV_CELL_SIZE << endl; nav_drive_line(dx, dy,path[cell]); //Prevent stopped message nav_wait_stop(); // Check if we are drifting from the path if(is_drifting(path[cell])) { clear_distance(); clear_actual_position(); route new_route = findRoute_base(last_x/NAV_CELL_SIZE, last_y/NAV_CELL_SIZE, getPos(path[path.size()-1]).first, getPos(path[path.size()-1]).second ); cout << "New path : " << new_route.size() << endl; endofnav = true; } else { // And start a new line dx = ddx; dy = ddy; } } } } if(!endofnav) { correct_orientation(dx,dy); cout << "ToDrive : dx=" << dx*NAV_CELL_SIZE << " dy= " << dy*NAV_CELL_SIZE << endl; nav_drive_line(dx, dy,path[path.size()-1]); if(is_drifting(path[path.size()-1])) endofnav = true; } return !endofnav; } } void find_paths() { // TODO do a graph path finding algorithm for(unsigned int i = 0 ; i < items.size()-1 ; i++) { cout << "----- Item : " << items[i].name << "--------------" << endl ; cout << "PosX:" << items[i].x << endl; cout << "PosY:" << items[i].y << endl; int ii = items[i].x/NAV_CELL_SIZE; int jj = items[i].y/NAV_CELL_SIZE; if(grid[ii][jj].state != FREE) { cout << "FAILURE : the cell is not free" << endl; } else { compute_distance(items[i]); // print_grid(); for(size_t j = i+1 ; j < items.size() ; j++) { cout << "To : " << items[j].name << std::endl; route path = findRoute(items[j],items[i]); path = revertRoute(path); for(unsigned int cell= 0 ; cell < path.size() && ros::ok(); cell++) { pair w; w = getPos(path[cell]); printf("(%d ; %d)\n", w.first, w.second); } if(path.size() > 0) { cout << " - Path find. Distance : " << path.size() << " cells" << endl; all_travels.push_back(path); transform_path_for_visualisation(path); if(i == 0 ) travel.push_back(items[j]); // TODO to improve. Add only target reacheable from origin } else { cout << " - Path not found ! " << endl; } } } } } //IF CHANGING, SEE and FIX goto_unexplored_pose!! void goto_pose(t_item & item) { clear_position(item.x, item.y); bool finish = false; while(!finish) { route way = findRoute_toItem_fromHere(item); if(way.size() > 0) { cout << "Path find" << endl; finish = navigate(way); if(finish) { cout << " Destination Reach !" << endl; goto_orientation(item.theta); } } else { cout << " No way find !" << endl; finish = true; } } } void goto_unexplored_pose(geometry_msgs::Pose2D &pose) { t_item item; item.x = pose.x; item.y = pose.y; item.theta = pose.theta; item.name = 'unexplored'; goto_pose(item); } /* Grass fire stuff */ bool within_nav_bounds(int x, int y) { return !(x < 0 || x >= NAV_MAP_HEIGHT || y < 0 || y >= NAV_MAP_WIDTH); } typedef struct { int length; int pose_x; int pose_y; float pose_theta; } pose_and_length; pose_and_length region_cell_pose_and_length(int x, int y) { pose_and_length ret; ret.length = -1; int n_x, n_y; n_x = x+1; n_y = y; if(within_nav_bounds(n_x, n_y)) { if(grid[n_x][n_y].state != UNKNOWN) { ret.length = grid[n_x][n_y].distance; ret.pose_x = n_x; ret.pose_y = n_y; ret.pose_theta = M_PI; return ret; } } n_x = x-1; n_y = y; if(within_nav_bounds(n_x, n_y)) { if(grid[n_x][n_y].state != UNKNOWN) { ret.length = grid[n_x][n_y].distance; ret.pose_x = n_x; ret.pose_y = n_y; ret.pose_theta = 0; return ret; } } n_x = x; n_y = y+1; if(within_nav_bounds(n_x, n_y)) { if(grid[n_x][n_y].state != UNKNOWN) { ret.length = grid[n_x][n_y].distance; ret.pose_x = n_x; ret.pose_y = n_y; ret.pose_theta = 3*M_PI/2; return ret; } } n_x = x; n_y = y-1; if(within_nav_bounds(n_x, n_y)) { if(grid[n_x][n_y].state != UNKNOWN) { ret.length = grid[n_x][n_y].distance; ret.pose_x = n_x; ret.pose_y = n_y; ret.pose_theta = M_PI/2; return ret; } } return ret; } pose_and_length find_region_to_explore() { getLatestMap(); //DEBUG STUFF //loadMap(); //last_x = 234; //last_y = 186; expandGrid(); int count = 0; for(int i=0; i > q; q.push(make_pair(i, j)); //Calculate region statistics int region_size = 0; pose_and_length region_pal; region_pal.length = INT_MAX; while(!q.empty()) { pair curr = q.front(); q.pop(); ++region_size; //Check all surrounding cells for(int k=-1; k<=1; ++k) { for(int l=-1; l<=1; ++l) { //Skip ourselves if(!k && !l) continue; //Neighbouring cell's (x, y) int neighbour_x = curr.first + k; int neighbour_y = curr.second + l; //Check bounds if(!within_nav_bounds(neighbour_x, neighbour_y)) continue; //Skip visisted if(visited[neighbour_x][neighbour_y]) continue; visited[neighbour_x][neighbour_y] = true; //Ignore explored, not part of region if(grid[neighbour_x][neighbour_y].state != UNKNOWN) continue; //Calculate length to robot, if reachable pose_and_length pal = region_cell_pose_and_length(neighbour_x, neighbour_y); if(pal.length != -1) if(pal.length < region_pal.length) region_pal = pal; //Push q.push(make_pair(neighbour_x, neighbour_y)); } } } printf("Found region (size: %d), closest at x %d y %d (length: %d)\n", region_size, region_pal.pose_x, region_pal.pose_y, region_pal.length); //TODO: Heuristics to save the best region here! /* region_size is the number of cells in the region region_pal contains length to the region as well as robot pose to navigate to */ if(region_size > 20*20 && region_pal.length < best_region_length) best_region_pal = region_pal; } } //TODO: Take the best region, find path and go there //TODO: Verify not INT_MAX! return best_region_pal; } /* End of grass fire stuff */ void visit_all() { for(size_t i = 0 ; i < travel.size() && ros::ok() ; i++) { cout << " travelling to" << travel[i].name << endl; cout << "X=" << travel[i].x << "Y=" << travel[i].y << endl; clear_actual_position(); clear_position(travel[i].x, travel[i].y); route way = findRoute_toItem_fromHere(travel[i]); if(way.size() > 0) { cout << "Path find" << endl; navigate(way); cout << " Destination Reach !" << endl; } else { cout << " No way find !" << endl; } } } void receive_start(const std_msgs::EmptyConstPtr &msg) { static bool hasWork = false; if(!hasWork) { hasWork = true; //---- initVisualization stuff init_line_walls(); // --- load the occupancy grid map loadMap(); // --- Compute and expand the original Map expandGrid(); clear_actual_position(); // --- load items load_items(); // ---Loop over Items to pathFind find_paths(); is_build = true; // --- and work ! /* if(all_travels.size() > 0) {*/ // } } } bool receive_explore(discovery::ExploreSrv::Request& request, discovery::ExploreSrv::Response& response) { pose_and_length pal = find_region_to_explore(); printf("Best exploration pose: x %d, y %d, t %lf\n", pal.pose_x, pal.pose_y, pal.pose_theta); response.pose.x = pal.pose_x; response.pose.y = pal.pose_y; response.pose.theta = pal.pose_theta; goto_unexplored_pose(response.pose); return true; } ////bool receive_goto_pose(discovery::GotoPoseSrv::Request& request, discovery::GotoPoseSrv::Response& response) { // goto_unexplored_pose(request.pose); // return true; //} #ifdef UNTESTED_WTD void wait_stop_recoverymove() { static ros::Rate loop_rate(5); loop_rate.reset(); while(state != STOP_WHILE_RECOVERING) { ros::spinOnce(); loop_rate.sleep(); } state = RECOVERING; } #endif void recieve_whisker_right(const std_msgs::Empty::ConstPtr& msg) { if(state == GO_FORWARD ) { cout << "/!\\ Receive whisker /!\\" << endl; state = RECOVERING; do_stop(); #ifdef UNTESTED_WTF ros::Duration(0.5).sleep(); do_drive_dist(-10, 40); wait_stop_recoverymove(); do_rotate(PI / 2); wait_stop_recoverymove(); do_drive_dist(6, 50); wait_stop_recoverymove(); #else ros::Duration(0.5).sleep(); do_drive_dist(-10,50); ros::Duration(5).sleep(); #endif ros::spinOnce(); state = STOP; } } void recieve_whisker_left(const std_msgs::Empty::ConstPtr& msg) { if(state == GO_FORWARD ) { cout << "/!\\ Receive whisker /!\\" << endl; state = RECOVERING; do_stop(); #ifdef UNTESTED_WTF ros::Duration(0.5).sleep(); do_drive_dist(-10, 40); wait_stop_recoverymove(); do_rotate(-PI / 2); wait_stop_recoverymove(); do_drive_dist(6, 50); wait_stop_recoverymove(); #else ros::Duration(0.5).sleep(); do_drive_dist(-10,50); ros::Duration(5).sleep(); #endif ros::spinOnce(); state = STOP; } } void recieve_front_bumper(const std_msgs::Empty::ConstPtr& msg) { if(state == GO_FORWARD ) { cout << "/!\\ Receive Front/!\\" << endl; block_cell(last_x + cos(last_theta)*(ROBOT_SIZE/2 +3 ), last_y + sin(last_theta)*(ROBOT_SIZE/2 + 3), EXPAND_SIZE/3); state = RECOVERING; do_stop(); ros::Duration(0.5).sleep(); do_drive_dist(-10,50); #ifdef UNTESTED_WTF wait_stop_recoverymove(); #else ros::Duration(5).sleep(); #endif ros::spinOnce(); state = STOP; } } void receive_sensors(const discovery::SensorMessageConstPtr& msg) { robot.setNewIRValues(msg); if(state == GO_FORWARD && robot.isTooCloseFromAWall() ) { state = RECOVERING; cout << "/!\\ Emergency stop /!\\" << endl; do_stop(); ros::Duration(1).sleep(); ros::spinOnce(); if(robot.isFrontWallClose() ) { if(robot.wallDistFront() != C_robot::NO_DISTANCE) { /* if(robot.isFrontIRLTooClose()) { block_cell(last_x + cos(last_theta)* (ROBOT_SIZE/2 + robot.wallDistFront() + msg->sensorPose_X[msg->SIR_F_L]), last_y + sin(last_theta)* (ROBOT_SIZE/2 + robot.wallDistFront() + + msg->sensorPose_Y[msg->SIR_F_L]), EXPAND_SIZE/3); } if(robot.isFrontIRRTooClose()) { block_cell(last_x + cos(last_theta)* (ROBOT_SIZE/2 + robot.wallDistFront() + msg->sensorPose_X[msg->SIR_F_R]), last_y + sin(last_theta)* (ROBOT_SIZE/2 + robot.wallDistFront() + + msg->sensorPose_Y[msg->SIR_F_R]), EXPAND_SIZE/3); } if(robot.isFrontIRLTooClose()) { block_cell(last_x + cos(last_theta)* (ROBOT_SIZE/2 + robot.wallDistFront() + msg->sensorPose_X[msg->SONAR]), last_y + sin(last_theta)* (ROBOT_SIZE/2 + robot.wallDistFront() + + msg->sensorPose_Y[msg->SONAR]), EXPAND_SIZE/3); }*/ } } else if(robot.isTooCloseLeft()) { do_rotate(-PI/2); ros::Duration(4).sleep(); do_drive_dist(5,50); ros::Duration(4).sleep(); }else if(robot.isTooCloseRight()) { do_rotate(PI/2); ros::Duration(4).sleep(); do_drive_dist(5,50); ros::Duration(4).sleep(); } state = STOP; // TODO cout << "Scanning..." << endl; } } /****************************************************** Main */ int main(int argc, char **argv) { ros::init(argc, argv, "NavigationNode"); ros::NodeHandle n; robot.setNodeHandle(&n); tagObsPose_pub = n.advertise ("/CV/tagObservationPose", 1); ros::Subscriber start_sub = n.subscribe("/navigation/build", 1, receive_start); ros::Subscriber sensors_sub = n.subscribe ("/sensor/data", 1, receive_sensors); #ifdef ODOMETRY ros::Subscriber pose2_sub = n.subscribe("/odometry/pose", 1, update_pos_particles ); #else ros::Subscriber pose2_sub = n.subscribe("/localization/pose", 1, update_pos_particles); #endif ros::Subscriber stopped_sub = n.subscribe ("/motor/stopped", 100, got_stopped); //ros::ServiceServer save_service = n.advertiseService("load_tags", load_tags); ros::Subscriber whisker_sub = n.subscribe ("/sensor/right_whisker", 1, recieve_whisker_right); ros::Subscriber whisker2_sub = n.subscribe ("/sensor/left_whisker", 1, recieve_whisker_left); ros::Subscriber front_bumper_sub = n.subscribe ("/sensor/front_bumper", 1, recieve_front_bumper); ros::ServiceServer map_request_service = n.advertiseService("get_explore_region", receive_explore); bool moved = false; // and finally print the map ! map_pub = n.advertise("/roboard/nav_occu_grid",1000000); traj_pub = n.advertise("/map/traj", 10000); drive_dist_pub = n.advertise("/motor/drive_dist", 1); rotate_pub = n.advertise("/motor/rotate", 1); drive_speed_pub = n.advertise ("/motor/drive_speed", 2); tagId_pub = n.advertise("/map/targets_names", 10); obs_pub = n.advertise("/map/targets_points", 1); ros::Rate loop_rate(1); int i = 0; cout << "Start main" << endl; while (ros::ok()) { ros::spinOnce(); if(all_travels.size() > 0) { cout << "pub paths" << endl; traj_pub.publish(line_walls); } publish_grid(); tagId_pub.publish(tagIDarr); obs_pub.publish(obsArray); cout << "do main loop" << endl; if(i++ > 4) { ros::spinOnce(); if (is_build) { cout << "Let's visit everybody !" << endl; visit_all(); } } loop_rate.sleep(); } }