int main() { // push waypoints to a routes' map waypoints.push_back(waypoint(-4.0, -3.0)); // 0 waypoints.push_back(waypoint(-3.0, -1.0)); // 1 waypoints.push_back(waypoint(1.0, -4.0)); // 2 waypoints.push_back(waypoint(3.0, -2.0)); // 3 waypoints.push_back(waypoint(-1.0, 1.0)); // 4 waypoints.push_back(waypoint(4.0, -3.0)); // 5 waypoints.push_back(waypoint(2.0, 1.0)); // 6 waypoints.push_back(waypoint(-2.0, 2.0)); // 7 waypoints.push_back(waypoint(5.0, -1.0)); // 8 waypoints.push_back(waypoint(6.0, 2.0)); // 9 waypoints.push_back(waypoint(-1.0, 4.0)); // 10 waypoints.push_back(waypoint(4.0, 4.0)); // 11 waypoints.push_back(waypoint(8.0, 3.0)); // 12 waypoints.push_back(waypoint(1.0, 6.0)); // 13 waypoints.push_back(waypoint(6.0, 5.0)); // 14 waypoints.push_back(waypoint(2.0, 7.0)); // 15 waypoints.push_back(waypoint(7.0, 7.0)); // 16 waypoints.push_back(waypoint(4.0, 7.0)); // 17 waypoints.push_back(waypoint(6.0, 9.0)); // 18 waypoints.push_back(waypoint(8.0, 9.0)); // 19 obstacles.push_back(obstacle(0.0,5.0,1.0)); // 0 obstacles.push_back(obstacle(3.0,3.0,2.0)); // 1 obstacles.push_back(obstacle(3.5,0.0,1.0)); // 2 obstacles.push_back(obstacle(8.0,7.0,2.5)); // 3 // define exponential traverser type and create an exemplar typedef ExpTr<node_value,cost_value,expand,less<cost_value> > ExpTr_t; ExpTr_t j; cout << "in exp traverser forward search ...\n"; j = ExpTr_t(0); // set a maximal lag length j.getexpfunc().setMaxLagLen(5.0); // or 6.0 // add two defined handlers to enable calculation of an expansions number j.set_handler_on_expand_root(OnExpandRoot); j.set_handler_on_select_cursor(OnSelectCursor); // launch A* algorithm (combination of a predefined heuristics with a best - first // search that is provided by exponential traverser is A*!) ForwardSearch(j, goal); ShowPath(j); // show founded path cout << "number of expansions = " << num_expansions << '\n'; cout << "...out \n\n"; cout << "Search is completed.\n"; return 0; }
void sonarCallback(const sensor_msgs::PointCloud::ConstPtr& msg) { double pf_Start = ros::Time::now().toSec(); std::cout << "I was called : (ms)" << (pf_Start - lastTimeCalled)*1000 << std::endl ; lastTimeCalled = pf_Start; obstacles_positions_current.clear(); for(int i=0; i< msg->points.size(); ++i) { Eigen::Vector3d obstacle(msg->points[i].x,msg->points[i].y,0.0); if(obstacle.norm()<laser_max_distance-0.01) //if((obstacle.norm()>robot_radius)&&(obstacle.norm()<laser_max_distance-0.01)) // check if measurement is between the laser range and the robot { //ROS_INFO_STREAM("INSIDE THE LIMITS:"<<obstacle.norm()); obstacles_positions_current.push_back(obstacle); } } if(!init_flag) { init_flag=true; obstacles_positions_previous=obstacles_positions_current; return; } //ROS_INFO_STREAM("obstacles:" << obstacles_positions.size()); //ROS_INFO("I heard sensor data : [%f, %f , %f]", msg->points[0].x , msg->points[0].y , msg->points[0].z ); computeForceField(); // feedbackMaster(); //feedbackSlave(); obstacles_positions_previous=obstacles_positions_current; double pf_End = ros::Time::now().toSec(); //std::cout << "Call back took : (ms)" << (pf_End - pf_Start)*1000 << std::endl ; }
void addObstaclesAndPropositions(oc::PropositionalTriangularDecomposition* decomp) { Polygon obstacle(4); obstacle.pts[0] = Vertex(0.,.9); obstacle.pts[1] = Vertex(1.1,.9); obstacle.pts[2] = Vertex(1.1,1.1); obstacle.pts[3] = Vertex(0.,1.1); decomp->addHole(obstacle); Polygon p0(4); p0.pts[0] = Vertex(.9,.3); p0.pts[1] = Vertex(1.1,.3); p0.pts[2] = Vertex(1.1,.5); p0.pts[3] = Vertex(.9,.5); decomp->addProposition(p0); Polygon p1(4); p1.pts[0] = Vertex(1.5,1.6); p1.pts[1] = Vertex(1.6,1.6); p1.pts[2] = Vertex(1.6,1.7); p1.pts[3] = Vertex(1.5,1.7); decomp->addProposition(p1); Polygon p2(4); p2.pts[0] = Vertex(.2,1.7); p2.pts[1] = Vertex(.3,1.7); p2.pts[2] = Vertex(.3,1.8); p2.pts[3] = Vertex(.2,1.8); decomp->addProposition(p2); }
int main(){ int i; pt debut = 0; pt fin = _largeur/2 +1; double* mesh = init_tab_mesh(); obstacle(mesh); //double bottom_left[2] = { 10, 10 }; //double top_right[2] = { 15, 15 }; //generate_rectangle(bottom_left, top_right, mesh); double* dist = tab_distance(mesh, debut, fin); printf("\n"); for(i=0; i<_largeur*_largeur;i++){ if(i%_largeur == 0) printf("\n"); printf("%d\t", (int)dist[i]); } printf("\n"); save_dist(dist, "distance.vtk"); pt* chemin = court_chemin_bis(dist, fin); save_path(dist, fin, chemin, "path.vtk"); printf("\n"); for(i=0; i< dist[fin]; i++) printf("%d \t %d \n", chemin[i]/_largeur, chemin[i]%_largeur); free(mesh); free(dist); return 0; }
void step(double& t){ int nx, ny, nz, alarm; double *A, *Bx, *By, *Bz, dt, *parx, *pary, *parz; nx = n[0]; ny = n[1]; nz = n[2]; parx = new double[(nz)*(ny)*5]; pary = new double[(nz)*5]; parz = new double[5]; A = new double[5]; Bx = new double[5]; By = new double[5]; Bz = new double[5]; init_aux(parx, pary, parz); dt = 42; for(int ix = 2; ix < nx+2; ix++){ for(int iy = 2; iy < ny+2; iy++){ for(int iz = 2; iz < nz+2;iz++){ if(obstacle(ix,iy,iz,0) > 0){ flux(&parx[(iy-2)*nz + (iz-2)], ix, iy, iz, Bx, 'X', dt); flux(&pary[iz-2], ix, iy, iz, By, 'Y', dt); flux(parz, ix, iy, iz, Bz, 'Z', dt); f(ix, iy, iz, A); for(int i = 0; i < 5; i++){ A[i] = A[i] + dt*(Bx[i]/dif[0][ix-2] + By[i]/dif[1][iy-2] + Bz[i]/dif[2][iz-2]); } f_inv(ix, iy, iz, A); } } } } recalc_dif_p(); t = t + dt; }
std::vector<Obstacle> XMLExtractor::extractObstacles(std::string gameName, unsigned int level, Hole& hole, Point& ballPosition, float& radius) { std::vector<Obstacle> obstacleArray; pugi::xml_document doc; std::string fileName = "..\\DataManager\\Courses\\" + gameName + ".xml"; if (doc.load_file(fileName.c_str())) { std::string s = "Course" + std::to_string(level); pugi::xml_node obstacles = doc.child(s.c_str()); for (pugi::xml_node obstacleNode = obstacles.first_child(); obstacleNode; obstacleNode = obstacleNode.next_sibling()) { std::string obstaclestr = std::string("Obstacle").c_str(); std::string holestr = std::string("Hole").c_str(); std::string ballstr = std::string("Ball").c_str(); if (obstacleNode.name() == obstaclestr) { double x = atof(obstacleNode.child_value("X")); double y = atof(obstacleNode.child_value("Y")); double width = atof(obstacleNode.child_value("Width")); double height = atof(obstacleNode.child_value("Height")); double resistance = atof(obstacleNode.child_value("Resistance")); double rotation = atof(obstacleNode.child_value("Rotation")); Obstacle obstacle(x, y, width, height); obstacle.setAngle(rotation); obstacle.setSurfaceResistance(resistance); //Ajouter le nouveau point au vecteur obstacleArray.emplace_back(obstacle); } else if (obstacleNode.name() == holestr) { hole.setPosition(atof(obstacleNode.child_value("X")), atof(obstacleNode.child_value("Y"))); hole.setWidth(atof(obstacleNode.child_value("Width"))); hole.setHeight(atof(obstacleNode.child_value("Height"))); } else if (obstacleNode.name() == ballstr) { ballPosition = Point(atof(obstacleNode.child_value("X")), atof(obstacleNode.child_value("Y"))); radius = atof(obstacleNode.child_value("Radius")); } } } else { std::cout << "ERREUR: Le fichier '" + fileName + "' est introuvable. \n"; } return obstacleArray; }
void flux(double* par_ax, int ix, int iy, int iz, double* B, int ax, double& ddeltat){ int alarm; double *par, rho1, u1, p1, ltan1, ltan2, rho2, u2, p2, rtan1, rtan2; par = new double[12]; //forward riemparam2(par, ix, iy, iz, ax); riemi(par, &alarm); if(alarm){ std::cerr<<"function 'riemi' sends an alarm at ("<<ix<<" "<<iy<<" "<<iz<<'\n';} tanvel(par[10], rtan1, rtan2, ax, ix, iy, iz); updatetimestep(ax, ix, iy, iz, par[9], par[11], ddeltat); rho2 = par[6]; u2 = par[7]; p2 = par[8]; //backward if(obstacle(ix, iy, iz, -ax) == 1){ //tangential velosity components conserve near an obstacle riemparam2(par, ix, iy, iz, -ax); riemi(par, &alarm); if(alarm){ std::cerr<<"function 'riemi' sends an alarm at ("<<ix<<" "<<iy<<" "<<iz<<'\n';} updatetimestep(-ax, ix, iy, iz, par[9], par[11], ddeltat); par_ax[0] = par[6]; par_ax[1] = par[7]; par_ax[2] = par[8]; par_ax[3] = p[tan1(ax)][ix][iy][iz]; par_ax[4] = p[tan2(ax)][ix][iy][iz]; } rho1 = par_ax[0]; u1 = par_ax[1]; p1 = par_ax[2]; ltan1 = par_ax[3]; ltan2 = par_ax[4]; //fluxes B[0] = u2*rho2 - rho1*u1; switch(ax){ case 'X': B[1] = rho2*u2*u2 + p2 - (rho1*u1*u1 + p1); B[2] = rho2*u2*rtan1 - (rho1*u1*ltan1); B[3] = rho2*u2*rtan2 - (rho1*u1*ltan2); break; case 'Y': B[1] = rho2*u2*rtan1 - (rho1*u1*ltan1); B[2] = rho2*u2*u2 + p2 - (rho1*u1*u1 + p1); B[3] = rho2*u2*rtan2 - (rho1*u1*ltan2); break; case 'Z': B[1] = rho2*u2*rtan1 - rho1*u1*ltan1; B[2] = rho2*u2*rtan2 - rho1*u1*ltan2; B[3] = rho2*u2*u2 + p2 - (rho1*u1*u1 + p1); break; } B[4] = u2*(rho2*(inenergy(rho2,p2) + 0.5*(u2*u2+rtan1*rtan1+rtan2*rtan2)) + p2) - u1*(rho1*(inenergy(rho1,p1) + 0.5*(u1*u1+ltan1*ltan1+ltan2*ltan2)) + p1); delete[] par; //parameters' update par_ax[0] = rho2; par_ax[1] = u2; par_ax[2] = p2; par_ax[3] = rtan1; par_ax[4] = rtan2; return; }
task main() { ClearMessage(); float widthOfRoom=perimeterX(); //calculate the width of the room float distanceY=perimeterY(); //calculate the length of the room int repitition=reps(widthOfRoom); //calculate how many repititions of the cleaning cycle it needs to do float distanceX= distX(widthOfRoom); //calculate the distance per strafe on the repititions bool hit= false; int counter=0; while((counter<repitition-3)&&hit!=1)// run while it hasn't hit the bottom, or has not completed all of its repititions { hit=drive(1,distanceY-40); //drive forward -20cm for clearance from wall if(hit) //if it hit something go through obstacle functions { float lengthOfObstacle=obstacle(1); //drive around the obstacle drive(1, distanceY-lengthOfObstacle +10); //drive forward the remainder of the distance hit=false; //reset hit } drive(3, distanceX); //drive right hit=drive(2, (distanceY+20) ); //drive backwards -20cm for clearance from wall if(hit) //if it hits something go through obstacle functions { wait1Msec(2000); //wait for sensor to rotate float lengthOfObstacle=obstacle(2); //drive around the obstacle drive(2, distanceY+lengthOfObstacle); //drive the remainder of the distance hit=false; //reset hit } hit=drive(3, distanceX);//drive right counter++; } motor[motorA]=0; motor[motorB]=0; motor[motorC]=0; }
Map::Map (Game* game) { const auto nodes = 20; _ground.reserve(nodes); for (auto i = 0; i < nodes; i++) _ground.push_back(vec2f( math::random(width()), math::random(height()))); _obstacles.push_back( obstacle({ { 30, 30 }, { 320, 35 }, { 310, 300 }, { 30, 310 } })); for (auto& b : _obstacles) b.createBody(game); }
int main(int argc, char * argv[]) { std::cout<<"Common LAAS Raster library"<<std::endl; if (argc < 3) { std::cerr<<"usage: "<<argv[0]<<" flat.tiff obstacle.tiff region_out.tif"<<std::endl; return EXIT_FAILURE; } gladys::gdal flat(argv[1]); gladys::gdal obstacle(argv[2]); gladys::gdal region; region.copy_meta(flat, clara::region::N_RASTER); region.bands_name = {"NO_3D_CLASS", "FLAT", "OBSTACLE", "ROUGH", "SLOPE"}; region.bands[clara::region::FLAT] = flat.bands[0]; region.bands[clara::region::OBSTACLE] = obstacle.bands[0]; region.save(argv[3]); return EXIT_SUCCESS; }
//return REACHED if position reached, OBSTACLE if stopped because of obstacle A CUSTOM STOP MESSAGE MUST BE SENT, CUSTOM_STOP_MESSAGE if stopped because a custom message was sent //robot position is updated //position is reached running for distance in steps <=200 (first the dist%200 than n steps dist/200) //it sleeps 1s after each step int go_to_position(pos_t target_pos, int margin, int speed){ int32_t tacho_dist, start_tacho, end_tacho; int angle, dist; int i; if(robot_rank == 0) while(wait); compute_polar_coord(target_pos, &dist, &angle,margin); target_pos.x = robot.x + dist*sin(angle*M_PI/180); target_pos.y = robot.y + dist*cos(angle*M_PI/180); //start movement, in two steps if bigger than 200 int div = dist/200; int mod = dist%200; int step; for(i = 0; i < div+1 ; i++){ if(i == div) step = mod; else step = 200; if(robot_rank == 0 && send_action_flag) send_action(next, angle, step, speed/2); //vado alla posizione set_motors_speed(SLOW_SPEED); turn_absolute_fb(angle, get_cal_location(robot.x,robot.y)); //aggiorno posizione final; robot.t = angle;//get_gyro_value(); printf("robot.t = %d\n",robot.t); //save initial tacho counts start_tacho=get_motorR_position(); set_motors_speed(speed); run_relative((int)(step*360.0/18)); while(!command_finish()){ if(cancel){ //cancel = 0; run_relative(0); end_tacho = get_motorR_position(); tacho_dist = end_tacho-start_tacho; dist = tacho_dist/20.0; robot.x = robot.x + dist*sin(robot.t*M_PI/180); robot.y = robot.y + dist*cos(robot.t*M_PI/180); printf("\n\n>>>>>>>>pos %d %d %d\n\n",robot.x,robot.y,robot.t); if(send_action_flag) send_cancel(next, dist); // Send to ourself an action in order came closer act_val.dist = cancel - dist; act_val.angle = robot.t; act++; return CUSTOM_STOP_MESSAGE; } if(obstacle()){ run_relative(0); end_tacho = get_motorR_position(); tacho_dist = end_tacho-start_tacho; dist = tacho_dist/20.0; printf("step %d dist %d\n",step,dist); if(step-dist < STOP_DIST*18/360.0){ set_motors_speed(speed); run_relative((int)((step-dist)*360.0/18)); return REACHED; } printf("tacho dist %ld dist %ld\n",tacho_dist,dist); robot.x = robot.x + dist*sin(robot.t*M_PI/180); robot.y = robot.y + dist*cos(robot.t*M_PI/180); printf("\n\n>>>>>>>>pos %d %d %d\n\n",robot.x,robot.y,robot.t); if(send_action_flag) send_cancel(next,dist); // HERE WE MUST SEND A CUSTOM MESSAGE STOP TO FOLLOWING ROBOT!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! // HERE WE MUST SEND A CUSTOM MESSAGE STOP TO FOLLOWING ROBOT!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! return OBSTACLE; } } sleep(1); } //update pos robot.x = target_pos.x; robot.y = target_pos.y; printf("\n\n>>>>>>>>pos %d %d %d\n\n",robot.x,robot.y,robot.t); return REACHED; }
void sched (Info * info) { //mylogfd (SCHFD, "%d\n", info->state); // if (!info->video_info->is_valid) return; // mylogfd(SCHFD, "%d %d\n", IsEdgeValid(info), getEdgeX(info)); // mylogfd(SCHFD, "IsDoorSeen %d IsDoorAllSeen %d IsDoorLeft %d IsDoorRight %d IsDoorMiddle %d DoorMidX %d DoorMidY %d\n", // IsDoorSeen(info), IsDoorAllSeen(info), IsDoorLeft(info), IsDoorRight(info), IsDoorMiddle(info), getDoorMidx(info), getDoorMidy(info)); /*static int wait_edge = -1; if (wait_edge >= 0) { wait_edge ++; if (wait_edge >= 5) { mylogfd(SCHFD, "Touch edges->Stop\n"); do_stop(info); wait_edge = -1; } return; }*/ // mylogfd(SCHFD, "IsDoorSeen:%d IsEdgeValid:%d IsOutSide:%d edgeX %d\n", IsDoorAllSeen(info), IsEdgeValid(info), IsOutSide(info), getEdgeX(info)); estimate_seen_state(info); // mylogfd(SCHFD, "IsRobotSeen %d RobotX %d RobotY %d\n", IsRobotSeen(info), getRobotX(info), getRobotY(info)); if (info->last_cmd == CMD_FORWARD && info->state != SHOOTING && IsOutSide(info)) { mylogfd(SCHFD, "Outside\n"); info->last_cmd = CMD_STOP; do_stop(info); info->state = OUTSIDE; return; } /* if (info->last_cmd == CMD_FORWARD && IsOutSide(info) && IsEdgeValid(info)) { if (getEdgeX(info) < 50 && getEdgeX(info) >= 0) { mylogfd(SCHFD, "Touch edges %d\n", getEdgeX(info)); do_stop(info); info->state = DONE; //wait_edge = 0; return; } }*/ /* if (info->last_cmd == CMD_FORWARD && IsRobotSeen(info)) { if (IsRobotAcc(info)) { if (getRobotY(info) < 30) { do_stop(info); info->state = SEARCH_BALL; } else if (getRobotY(info) < 60) { info->destP.x = 0; info->destP.y = getRobotY(info) + 50; info->obstacle[0].x = getRobotY(info); info->obstacle[0].y = - getRobotX(info); do_obstacle(info); info->state = OBSTACLE; } } }*/ //mylogfd(-1, "s"); switch (info->state) { case START: //chang in state machine -- redwalker start(info); case FIND_PLACE: // find_place(info); break; case SEARCH_BALL: search_ball(info); break; case SEARCHING_BALL: searching_ball(info); break; case SEARCHING_ADJUST: searching_adjust(info); break; case FORWARDING_BALL: forwarding_ball(info); break; case CATCHING_BALL: catching_ball(info); break; case BALL_CATCHED: ball_catched(info); break; case SHOOT: shoot(info); break; case SHOOTING: shooting(info); break; case MOVING: moving(info); break; case CHANGE_PLACE: change_place(info); break; case FINDING_DIR: finding_dir(info); break; case CHANGING_PLACE: changing_place(info); break; case OBSTACLE: obstacle(info); break; case PROTECT_DOOR: protect_door(info); break; case OUTSIDE: if (!IsOutSide(info)) { mylogfd(SCHFD, "Inside restart to search_ball\n"); info->state = SEARCH_BALL; } break; case FACE_DOOR: face_door(info); break; case DONE: break; case GAME: //game(info); break; case SHOOTED: shooted(info); default : break; } }
void CGameObject::on_matrix_change (const Fmatrix &previous) { obstacle().on_move (); }