Model* ModelSimpleUAVPolarWind::createFromFile(const string& filename) const
{
  Model *ret = NULL;
  
  try {
		ParseBlock modelData;
		modelData.load(filename.c_str());
		ret = createFromBlock(modelData);
	} catch (std::runtime_error &e) {
		std::cout << "ModelSimpleUAVPolarWind::create_from_file --> Error while loading data from file: ";
		std::cout << e.what() << std::endl;
		throw(e);
	}
    
  return ret;
}
ControllerSimpleGlider * ControllerSimpleGlider::createFromFile(const std::string& fileName) const {
  //! \Note Dummy
  ControllerSimpleGlider *ret = NULL;
	
	try {
		ParseBlock data;
		data.load(fileName.c_str());
		ret = createFromBlock(data);
	} catch (std::runtime_error &e) {
		std::cout << "ControllerSimpleGlider::createFromFile --> Error while loading data from file: ";
		std::cout << e.what() << std::endl;
		throw(e);
	}
	
	return ret;
}
results solveProblem(ArgumentData& arg, int max_cont, int id, int particles)
{
  results ret_val;
  ret_val.id = id;
  
  FormattedTime t0, t1, t2;
  
  t0.getTime();
  t1.getTime();
 // 1. Montecarlo simulation --> First make a montecarlo simulation of the whole system
  Simulator *s = NULL;
  try {
    ParseBlock b;
    b.load(arg[1].c_str());
    
    s = new Simulator(b["simulator"]);
  } catch (exception &e) {
    cerr << "Error while loading the file. Content: " << e.what() << endl;
  }
  
  bool collision;
  RealVector max_dev;
  CRAlgorithmFactory alg_fac;
  s->montecarlo(collision, particles, max_dev, false, false);
  t2.getTime();
  cout << "First simulation done. Spended time: " << t2 - t1 << endl;
  ret_val.montecarlo.push_back(t2 - t1);
  
  cout << "Max deviations: " << max_dev.toString() << endl;
  
  if (arg.isOption("export_original")) {
    string s_;
    arg.getOption("export_original", s_);
    cout << "Exporting original trajectories to: " << s_ << endl;
    s->exportTrajectory(s_);
  }
  
  if (!collision) {
    cout << "No collision has been found.\n";
    return ret_val;
  }

  CRAlgorithm *alg = alg_fac.createFromFile(arg.at(1));
  CRGenetics *algorithm = dynamic_cast<CRGenetics *>(alg);
  
  int cont = 0;
  while (cont < max_cont && algorithm != NULL) {
    double expansion = max_dev.at(0) + max_dev.at(1);
    if (!collision) {
      expansion = max_dev.at(0);
      cout << "No collision in the last solution --> setting the expansion to: " << expansion << endl;
    } else {
      cout << "Collision --> setting the expansion to: " << expansion << endl;
    }
    algorithm->getSimulator()->expandGeometries(expansion);
    vector<double > original_geo = algorithm->getSimulator()->getGeometries().at(0);
    
    t1.getTime();
    CRAlgorithmStatistics stats = algorithm->execute_one_vs_all(s->getTrajectories(), cont == 0);
    t2.getTime();
    
    cout << "Algorithm executed. Expended time: " << t2 - t1 << "\n";
    cout << "New plan: " << endl,
    cout << algorithm->getSimulator()->getFlightPlans().at(0).toString() << endl; 
    ret_val.ga.push_back(t2 - t1);
    ret_val.cost.push_back(stats.getMinObjetive());
    
    t1.getTime();
    // Set the new plan and simulate again!
    s->setFlightPlans(algorithm->getSimulator()->getFlightPlans());
    s->montecarlo_1_vs_all(collision, particles, max_dev, false);
    t2.getTime();
    
    cout << "Montecarlo executed. Expended time: " << t2 - t1 << ". ";
    ret_val.montecarlo.push_back(t2 - t1);
    
    if (collision) {
      cout << "Collision found\n";
      ret_val.collision.push_back(1);
    } else {
      cout << "NO Collision found\n";
      ret_val.collision.push_back(0);
    }
    cout << "Max deviations: " << max_dev.toString() << endl;
    
    cont++;
    algorithm->getSimulator()->expandGeometries(-expansion);
  }
  
  t2.getTime();
  cout << endl << "TOTAL Time: " << t2 - t0 << endl;
  ret_val.time = t2-t0;
  
  if (algorithm == NULL) {
    cerr << "Could not create the algorithm instance of the problem\n";
  }
  
  // Free memory
  delete algorithm;
  delete s;
  
  return ret_val;
}
Beispiel #4
0
int main(int argc, char **argv) {
    ArgumentData arg(argc,argv);
    if (argc < 2) {
        cerr << "Use: " << arg.at(0) << "<input_filename>" << endl;
        return 1;
    } else {
      if (arg.isOption("simulation")) {
	UAVFactory fac;
	
	UAV *uav = NULL;
	
	try {
	  ParseBlock block; 
	  block.load(arg.at(1).c_str());
	  uav = fac.create_from_block(block["uav"]);
	} catch (exception &e) {
	  cerr << "main:: Exception while loading data\n";
	}
	
	if (uav != NULL) {
	  cout << uav->toString() << endl;
	  for (int i = 0; i < 100; i++) {
	    uav->actualize();
	  }
	  cout << uav->toString() << endl;
	}
	
	
      } else if (arg.isOption("time-slot")) {
	cout << "Performing time slot testing.\n";
	TimeSlotList list;
	
	double min_dist = 5.0;
	double max_altitude = 250.0;
	try {
	  ParseBlock block; 
	  block.load(arg.at(1).c_str());
	  ParseBlock::Blocks *slots = block.getBlocks("timeslot");
	  
	  if (block.hasProperty("min_dist")) {
	    min_dist = block("min_dist").as<double>();
	  }
	  cout << "Min dist = " << min_dist << endl;
	  
	  if (block.hasProperty("max_altitude")) {
	    max_altitude = block("max_altitude").as<double>();
	  }
	  cout << "Maximum altitude = " << max_altitude << endl;
	  
	  ParseBlock::Blocks::iterator sl = slots->begin();
	  for (;sl != slots->end(); sl++) {
	    TimeSlot s(**sl);
	    cout << "Timeslot loaded: " << s.toString() << endl;
	    list.push_back(s);
	  }
	  
	  
	} catch (exception &e) {
	  cerr << "main:: Exception while loading data\n";
	  exit(1);
	}
	
	
	
	
	// Perform the tests. Check the first with all the other
	TimeSlotList::iterator it = list.begin();
	it++;
	TimeSlot &first = *list.begin();
	cout << "Checking this with all the others. " <<first.toString() << endl;
	for (;it != list.end(); it++) {
	  cout << "With: " << it->toString();
	  if (it->check(first.updraft_id, first.min_time, first.initial_altitude, first.ascending_rate, min_dist, max_altitude, first.uav_id)) {
	    cout << "Passes." << endl;
	  } else {
	   cout << "Fails." << endl; 
	  }
	}
      } else {
	
	URM *urm = NULL;
// 	double sleep_time;
	
	cout << "Performing normal test.\n";
	
	try {
	  ParseBlock data;
	  data.load(arg.at(1).c_str());
    
	  // Get the parameters of URM manager (updrafts, etc.)
	  if (!data.hasBlock("urm")) {
	    cerr << "The data faile has not urm data.\n";
	    exit(1);
	  }
	  urm = new URM(data["urm"]);

// 	  sleep_time = 1.0; // Default sleep time
// 	  if (data.hasProperty("uav_sleep")) {
// 	    sleep_time = data("uav_sleep").as<double>();
// 	  }

	  // Get the UAV data
	  ParseBlock::Blocks *uav_blocks = data.getBlocks("uav");

	  ParseBlock::Blocks::iterator uav_it = uav_blocks->begin();
	  int cont = 0;
// 	  bool error = false;
	  simulator::FlightPlan plan;
	  
	  int n_points = 0;
	  if (arg.isOption("random_wp")) {
	    arg.getOption("random_wp", n_points);
	    
	    for (int i = 0; i < n_points; i++) {
	      functions::RealVector v(2, true);
	      v = v * 1000.0;
	      
	      urm->addWaypoint(v);
	    }
	    
	  }
    
	  for (;uav_it != uav_blocks->end(); uav_it++, cont++) {
	    // Get the UAV parameters
	    UAVFactory fac;
	    UAV *aux = fac.create_from_block(**uav_it);
	    
	    if (!aux) {
	      cerr << "Error while loading UAV " << cont << " data.\n";
	      continue;
	    }
	    
	    aux->id = cont;

	    // Then the planner parameters
      
	    SoaringPlanner *aux_planner = NULL;
      
	    if ((*uav_it)->hasBlock("planner")) {
	      SoaringPlannerFactory fac_planner;
	      aux_planner = fac_planner.createFromBlock( (**uav_it)["planner"] );
	      if (aux_planner != NULL) {
		aux_planner->setURM(urm);
		aux_planner->setUAV(aux);
	      }
	      functions::FormattedTime t0;
	      t0.getTime();
	      if (aux_planner != NULL && aux_planner->execute(plan)) {
		functions::FormattedTime t1;
		t1.getTime();
		cout << "Output flight plan: " << plan.toString() << endl;
		cout << "Expended time: " << t1 - t0 << endl;
		
	      } else {
		cout << "The planner failed.\n";
	      }
	    }
      
	  }
	} catch (exception &e) {
	  cerr << "main:: Exception while loading data\n";
	}
      
	
      }
    }
    
    return 0;
}