ParseBlock* CRGenetics::toBlock() const
{
ParseBlock *ret = resolution::CRAlgorithm::toBlock();
ret->setProperty("algorithm", "Genetic");
return ret;
}
ParseBlock *ThermalModelSimple::exportThermalData() const
{
ParseBlock* block = new ParseBlock;
for (unsigned int i = 0; i < updraft_vector.size(); i++) {
block->setBlock("updraft", updraft_vector.at(i).toBlock());
}
return block;
}
ParseBlock* TemporalConstrain::toBlock() const
{
ParseBlock *ret = new ParseBlock;
ret->setProperty("min_time", numberToString(min_time));
ret->setProperty("max_time", numberToString(max_time));
ret->setProperty("updraft_id", numberToString(updraft_id));
return ret;
}
ParseBlock* FireworksConfig::toBlock() const
{
ParseBlock *ret = resolution::CostConfig::toBlock();
ret->setProperty("am", functions::numberToString(am));
ret->setProperty("bm", functions::numberToString(bm));
ret->setProperty("mm", functions::numberToString(mm));
ret->setProperty("m", functions::numberToString(m));
ret->setProperty("amplitude_mult", functions::numberToString(amplitude_mult));
return ret;
}
void AlgorithmConfig::init(ParseBlock& block)
{
// Set the default parameters
init();
try {
if (block.hasProperty("debug")) {
debug = block("debug").as<bool>();
}
if (block.hasProperty("export_solution")) {
export_solution = block("export_solution").as<bool>();
}
if (block.hasProperty("export_catec")) {
export_catec = block("export_catec").value;
}
if (block.hasProperty("export_trajectories")) {
export_trajectories = block("export_trajectories").as<bool>();
}
if (block.hasProperty("solution_filename")) {
solution_filename = block("solution_filename").value;
}
if (block.hasProperty("trajectory_filename")) {
trajectory_filename = block("trajectory_filename").value;
}
if (block.hasProperty("double_geometry")) {
double_geometry = block("double_geometry").as<std::vector<double> >();
}
if (block.hasProperty("geometry_expansion")) {
geometry_expansion = block("geometry_expansion").as<double>();
}
} catch (std::exception &e) {
std::cerr << "AlgorithmConfig (initializer) --> failed to get the data from block. Content: ";
std::cerr << e.what() << "\n";
}
}
void GeneticConfig::init(ParseBlock& block)
{
Checker *check = getChecker();
init();
try {
block.checkUsing(check);
resolution::CostConfig::init(block);
if (block.hasProperty("initializer_type")) {
initializer_type = block("initializer_type").value;
}
if (block.hasProperty("crossover_type")) {
crossover_type = block("crossover_type").as<string>();
}
if (block.hasProperty("custom_evolution")) {
custom_evolution = block("custom_evolution").as<bool>();
}
if (block.hasProperty("mutator_type")) {
mutator_type = block("mutator_type").as<string>();
}
if (block.hasProperty("search_ratio")) {
search_ratio = block("search_ratio").as<double>();
}
if (block.hasProperty("max_checks")) {
max_checks = block("max_checks").as<int>();
}
if (block.hasProperty("mutation_probability")) {
pMutation = block("mutation_probability").as<double>();
}
if (block.hasProperty("mutation_deviation")) {
mutation_dev = block("mutation_deviation").as<double>();
}
if (block.hasProperty("crossover_probability")) {
pCrossover = block("crossover_probability").as<double>();
}
} catch (std::exception &e) {
std::cerr << "GeneticConfig::init() --> error while loading data from block\n";
throw(e);
}
delete check;
}
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;
}
ParseBlock* GeneticConfig::toBlock() const
{
ParseBlock *ret = resolution::CostConfig::toBlock();
ret->setProperty("initializer_type", initializer_type);
ret->setProperty("crossover_type", crossover_type);
ret->setProperty("mutator_type", mutator_type);
ret->setProperty("custom_evolution", boolToString(custom_evolution));
ret->setProperty("search_ratio", numberToString(search_ratio));
ret->setProperty("max_checks", numberToString(max_checks));
ret->setProperty("mutation_probability", numberToString(pMutation));
ret->setProperty("crossover_probability", numberToString(pCrossover));
ret->setProperty("mutation_deviation", numberToString(mutation_dev));
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;
}
void ThermalModelSimple::init(ParseBlock &b) {
Checker *check = getChecker();
try {
b.checkUsing(check);
// Get updraft info
ParseBlock::Blocks *updrafts = b.getBlocks("updraft");
updraft_vector.clear();
ParseBlock::Blocks::iterator it = updrafts->begin();
for ( ; it != updrafts->end(); it++) {
Updraft aux(**it);
updraft_vector.push_back(aux);
}
} catch (exception &e) {
cerr << "ThermalModelSimple::ThermalModelSimple --> Error: could not load the data.\n";
throw(e);
}
delete check;
}
ParseBlock* AlgorithmConfig::toBlock() const
{
ParseBlock *block = new ParseBlock;
block->setProperty("export_trajectories", functions::boolToString(export_trajectories));
block->setProperty("trajectory_filename", trajectory_filename);
block->setProperty("export_solution", functions::boolToString(export_solution));
block->setProperty("solution_filename", solution_filename);
block->setProperty("debug", functions::boolToString(debug));
block->setProperty("double_geometry", functions::printVector(double_geometry));
block->setProperty("geometry_expansion", functions::numberToString(geometry_expansion));
return block;
}
TemporalConstrain::TemporalConstrain(ParseBlock& data)
{
Checker *check = TemporalConstrain::getChecker();
try {
data.checkUsing(check);
max_time = data("max_time").as<double>();
min_time = data("min_time").as<double>();
updraft_id = data("updraft_id").as<int>();
uav_id = data("uav_id").as<int>();
} catch (std::exception &e) {
std::cerr << "AlgorithmConfig (initializer) --> failed to get the data from block. Content: ";
std::cerr << e.what() << "\n";
}
delete check;
}
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;
}
void CostConfig::init(ParseBlock& block)
{
Checker *check = getConfigChecker();
try {
resolution::AlgorithmConfig::init(block);
// Cost parameters
distance_cost = block["cost"]("distance").as<double>();
collision_penalty = block["cost"]("collision_penalty").as<double>();
// Sampling parameters
population = block("population").as<int>();
generations = block("generations").as<int>();
if (block.hasProperty("max_time")) {
max_time = block("max_time").as<double>();
} else {
max_time = -1.0;
}
waypoint_dimension = block("waypoint_dimension").as<int>();
intermediate_waypoints = block("intermediate_waypoints").as<int>();
if (block.hasProperty("altitude_levels")) {
altitude_levels = block("altitude_levels").as<bool>();
}
if (block.hasProperty("altitude_step")) {
altitude_step = block("altitude_step").as<double>();
}
if (block.hasProperty("export_evolution")) {
export_evolution = block("export_evolution").as<bool>();
if (export_evolution && block.hasProperty("evolution_filename")) {
evolution_file = block("evolution_filename").as<string>();
} else {
export_evolution = false;
}
}
if (block.hasProperty("cost_type")) {
objective_type = block("cost_type").as<string>();
}
if (block.hasProperty("time_exploration")) {
time_exploration = block("time_exploration").as<bool>();
}
if (block.hasProperty("time_exploration_type")) {
time_exploration_type = static_cast<resolution::TimeExplorationTypes>(block("time_exploration_type").as<int>());
}
if (block.hasProperty("init_time")) {
std::vector<double> v;
v = block("init_time").as<std::vector<double> >();
long usec = 0;
if (v.size() > 1) {
usec = v.at(1);
}
init_time.setTime(v.at(0), usec);
} else {
init_time.getTime();
}
// Bounds
upper_bounds = block["bounds"]("upper").as<std::vector<double> >();
lower_bounds = block["bounds"]("lower").as<std::vector<double> >();
if (block["bounds"].hasProperty("speed_bounds")) {
speed_bounds = block["bounds"]("speed").as<std::vector<double> >();
} else {
speed_bounds = upper_bounds;
}
if (block.hasProperty("modify_trajectory")) {
modify_trajectory = block("modify_trajectory").as<bool>();
}
if (block.hasProperty("min_control_dist")) {
min_control_dist = block("min_control_dist").as<double>();
}
if (block.hasProperty("best_individual")) {
best_individual = block("best_individual").as<string>();
}
if (block.hasProperty("initial_solution")) {
ParseBlock::Properties *iss = block.getProperties("initial_solution");
ParseBlock::Properties::iterator it = iss->begin();
for (;it != iss->end(); it++) {
std::vector <double> v = (*it)->as<std::vector<double> >();
functions::RealVector aux(v);
initial_solution.push_back(v);
}
}
if (block.hasProperty("export_all_evolution")) {
export_all_evolution = true;
}
if (block.hasProperty("manoeuvre_selection")) {
manoeuvre_selection = block("manoeuvre_selection").as<bool>();
} else {
manoeuvre_selection = false; // default value
}
if (block.hasProperty("min_z")) {
min_z = block("min_z").as<double>();
} else {
min_z = 0.0;
}
if (block.hasProperty("max_z")) {
max_z = block("max_z").as<double>();
} else {
max_z = 2.0;
}
if (block.hasProperty("max_course")) {
max_course = block("max_course").as<double>();
} else {
max_course = 1.0;
}
} catch (std::runtime_error &e) {
std::cerr << "CostConfig::init --> Exception while reading data from block. Content: " << e.what()<< "\n";
throw(e);
}
delete check;
}
ParseBlock* CostConfig::toBlock() const
{
ParseBlock *ret = resolution::AlgorithmConfig::toBlock();
// Type
ret->setProperty("cost_type", objective_type);
// Bounds
ParseBlock *bounds_block = new ParseBlock;
bounds_block->setProperty("upper", functions::printVector(upper_bounds));
bounds_block->setProperty("lower", functions::printVector(lower_bounds));
bounds_block->setProperty("speed", functions::printVector(speed_bounds));
ret->setBlock("bounds", bounds_block);
// Cost parameters
ParseBlock *cost_block = new ParseBlock;
cost_block -> setProperty("distance", numberToString(distance_cost));
cost_block -> setProperty("collision_penalty", numberToString(collision_penalty));
ret->setBlock("cost", cost_block);
// Sampling parameters
ret->setProperty("population", numberToString(population));
ret->setProperty("generations", numberToString(generations));
ret->setProperty("max_time", numberToString(max_time));
ret->setProperty("waypoint_dimension", numberToString(waypoint_dimension));
ret->setProperty("intermediate_waypoints", numberToString(intermediate_waypoints));
ret->setProperty("altitude_levels", boolToString(altitude_levels));
ret->setProperty("altitude_step", numberToString(altitude_step));
ret->setProperty("export_evolution", boolToString(export_evolution));
ret->setProperty("evolution_filename", evolution_file);
ret->setProperty("time_exploration", boolToString(time_exploration));
switch (time_exploration_type) {
case INDEPENDENT_VELOCITY:
ret->setProperty("time_exploration_type", numberToString(INDEPENDENT_VELOCITY));
break;
default:
ret->setProperty("time_exploration_type", numberToString(MANTAIN_ETA));
}
ret->setProperty("speed_factor", numberToString(speed_factor));
// MS-PSO stuff
ret->setProperty("manoeuvre_selection", boolToString(manoeuvre_selection));
ret->setProperty("min_z", numberToString(min_z));
ret->setProperty("max_z", numberToString(max_z));
ret->setProperty("max_course", numberToString(max_course));
return ret;
}
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;
}
