/**
* Saves all settings to config file.
* @return true if success, false otherwise
*/
bool SettingsManager::save()
{
QVariantMap map = convertToMap();
if (!QDir().mkpath(_configDir))
return false;
QFile f(_configPath);
if (!f.open(QIODevice::WriteOnly | QIODevice::Truncate))
return false;
bool ok;
QJson::Serializer s;
s.setIndentMode(QJson::IndentFull);
s.serialize(map, &f, &ok);
LOG_MSG("Settings saved to: " + _configPath, mongo::LL_INFO);
return ok;
}
/**
* Saves all settings to config file.
* @return true if success, false otherwise
*/
bool SettingsManager::save()
{
QVariantMap map = convertToMap();
if (!QDir().mkpath(_configDir)) {
LOG_MSG("ERROR: Could not create settings path: " + _configDir, mongo::logger::LogSeverity::Error());
return false;
}
QFile f(_configPath);
if (!f.open(QIODevice::WriteOnly | QIODevice::Truncate)) {
LOG_MSG("ERROR: Could not write settings to: " + _configPath, mongo::logger::LogSeverity::Error());
return false;
}
bool ok;
QJson::Serializer s;
s.setIndentMode(QJson::IndentFull);
s.serialize(map, &f, &ok);
LOG_MSG("Settings saved to: " + _configPath, mongo::logger::LogSeverity::Info());
return ok;
}
PropertyRecord::PropertyRecord(const PropertyMap& propmap)
: PropertyHolder()
{
mProperties.clear();
convertToMap(propmap, mProperties);
}
void SonarMap::addScan(double sonar_x, double sonar_y, double sonar_theta, double fov, double max_range, double distance, double uncertainty)
{
int cx=0, cy=0;
double wx = 0.0, wy = 0.0;
convertToCell(sonar_x, sonar_y, cx, cy);
mapstore::MapStoreCone msc(double(cx), double(cy), sonar_theta, fov, distance/resolution_);
std::list<int> occxs;
std::list<int> occys;
std::list<double> occ_vals;
double sum_occupied = 0;
if (distance<uncertainty/2) {
//fixes crashing on wall entering
return;
}
while (msc.nextCell(cx,cy)) {
if (convertToMap(cx, cy, wx, wy)) {
double tmp_lin_dist = computeEuclideanDistance(wx, wy, sonar_x, sonar_y);
double tmp_ang_dist = computeAngularDistance(sonar_theta, atan2(wy-sonar_y, wx-sonar_x));
double p_linear;
double p_angular = this->Ea(fov, tmp_ang_dist);
double tmp_occ = 0;
double tmp_free = 0;
if (abs(distance-max_range)<uncertainty) {
//beams do not hit any obstacle
p_linear = this->ErFree(distance, tmp_lin_dist, uncertainty);
tmp_free = p_linear*p_angular;
} else {
if (tmp_lin_dist<distance) {
p_linear = this->ErFree(distance, tmp_lin_dist, uncertainty);
tmp_free = p_linear*p_angular;
} else {
double p_linear = this->ErOcc(distance, tmp_lin_dist, uncertainty);
tmp_occ = p_linear*p_angular;
}
}
double free_val_prev = map_free_.get(cx,cy);
double occ_val_prev = map_occupied_.get(cx,cy);
double free_val_new = free_val_prev + tmp_free - free_val_prev*tmp_free;
double occ_val_new = tmp_occ*(1-free_val_new);
sum_occupied += occ_val_new;
occ_vals.push_front(occ_val_new);
occxs.push_front(cx);
occys.push_front(cy);
map_free_.set(cx,cy, free_val_new);
}
}
//Normalization pass over stored x and y's
while (!occ_vals.empty()) {
cx = occxs.back();
cy = occys.back();
double tmp_val = occ_vals.back();
if (sum_occupied>0.05) {
//normalization not possible otherwise
double normalized_occ = tmp_val/sum_occupied;
double occ_prev = map_occupied_.get(cx,cy);
map_occupied_.set(cx,cy, occ_prev + normalized_occ - occ_prev*normalized_occ);
}
double occ = map_occupied_.get(cx,cy);
double free = map_free_.get(cx,cy);
if (occ>free) {
map_.set(cx, cy, occ);
} else {
map_.set(cx,cy, -free);
}
occxs.pop_back();
occys.pop_back();
occ_vals.pop_back();
}
}
