TParam* TSection::add(std::string strName, std::string strComment) { std::map<std::string, TParam*>::iterator found = m_mParams.find(trim(strName)); if(found == m_mParams.end()) { TParam* param = new TParam(strName); param->comment() = strComment; m_mParams[strName] = param; return param; } return found->second; }
bool TParam::addChild(const TParam &n) { if (n.name.empty()) { std::cerr << "Error in " << __PRETTY_FUNCTION__ << ": Empty names are not allowed\n"; return false; } if (n.name.find(" ") != std::string::npos) { std::cerr << "Error in " << __PRETTY_FUNCTION__ << ": White spaces in names are not allowed. Got '" << n.name << "'.\n"; return false; } if (n.name.find("/") != std::string::npos) { std::cerr << "Error in " << __PRETTY_FUNCTION__ << ": Slashes '/' in names are not allowed. Got '" << n.name << "'.\n"; return false; } if (getTypeByName(n.name) != Invalid) { std::cerr << "Error in " << __PRETTY_FUNCTION__ << ": Names of Variant Types are not allowed. Got '" << n.name << "'.\n"; return false; } if (getType() != Invalid) { std::cerr << "Error in " << __PRETTY_FUNCTION__ << ": Trying to add a child to leaf node.\n"; return false; } if (children.count(n.name) > 0) { if (children[n.name]->getType() != n.getType()) { std::cerr << "Error in " << __PRETTY_FUNCTION__ << ": Trying to replace Param " << n.name << " of type " << children[n.name]->getTypeName() << " by type " << n.getTypeName() << "\n"; return false; } delete children[n.name]; } children[n.name] = new TParam(n); children[n.name]->parent = this; return true; }
/** * Проверяет потенциальное совпадение двух элементов массива * Значения индексов для этой процедуры считаются окончательными * даже если граф строится для итерации (проверяется только 1 итерация * а не их последовательность). */ bool equal(const TParam &in, const TParam &out) { // FIXME - only for non scalar value if (in. getName() != out. getName()) return false; if (!in. isArrayElement() || !out. isArrayElement()) return true; // array and subarray // the dimension must be equal - fixme int dim1 = in. getDimension(); int dim2 = out.getDimension(); int common = (dim1 >= dim2) ? dim2 : dim1; TIndex *ind_in = in. getIndexes(); TIndex *ind_out = out. getIndexes(); for (int i = 0; i < common; i++) { if (ind_in[i]. isAtom() || ind_out[i]. isAtom()) continue; if (isIntersect(ind_in[i].diap, ind_out[i].diap)) { continue; } else { delete[] ind_in; delete[] ind_out; return false; } } delete[] ind_in; delete[] ind_out; return true; }
bool rpi_configure_devices(TIniFile& file) { TSection* sec = file["devices"]; if(sec) { std::string str_aux; short s_aux; TParam* param; TSection* sc_dev; TRPiDevice* dev; for(size_t i = 0; i < sec->size(); ++i) { param = (*sec)[i]; sc_dev = file[std::string("dev_") + param->name()]; if(sc_dev) { param->value() >> s_aux; dev = new TRPiDevice(uav_id(s_aux)); s_aux = UI_INVALID_ID; dev->name() = param->name(); dev->enabled() = false; // dev->enabled() = param->value().str().find("disabled") == std::string::npos; dev->kind() = str2Kind((*sc_dev)["kind"]->value().str()); if(dev->kind() == UI_INVALID_ID) { (*sc_dev)["kind"]->value() >> s_aux; dev->kind() = uav_id(s_aux); } (*sc_dev)["min"]->value() >> dev->minimum(); (*sc_dev)["max"]->value() >> dev->maximum(); // if(dev->kind() == UI_INVALID_ID) // dev->enabled() = false; Devices.insert(dev); } else { //-- TODO: No information for declared device!!! } }
stringSet sub(const TParam &l, const stringSet &minus) { stringSet temp = toLocalMemoryUse(l.unroll()); stringSet result, empty; // search whole array stringSet::const_iterator pos = find(minus. begin(), minus.end(), l); if (pos != minus. end()) return result; // search successful intersect(minus, temp, empty, result); return result; }
bool TParam::read(std::istream &s) { if (getType() != Invalid) { std::cerr << "Error in " << __PRETTY_FUNCTION__ << ": Leaf Param can not be read as tree\n"; return false; } clear(); s >> name; std::string t = Variant::tryRead(s); if (getType() == Invalid) { std::stringstream ss(t); unsigned int csize; ss >> csize; if (ss.fail()) return false; for (unsigned int i=0; i<csize; i++) { TParam p; p.read(s); assert(!s.fail()); addChild(p); } }
void packUnifySet(stringSet &l) { // Находим все массивы как скалярные переменные и // удаляем все их частные вхождения // также удаляем повторяющиеся элементы stringSet result; stringSet::const_iterator beg = l.begin(); stringSet::const_iterator end = l.end(); while (beg != end) { TParam cur = (*--end); if (!cur.isArrayElement()) { result. push_back(cur); continue; } if (!find(cur, result)) // удаляем повторения элементов result. push_back(cur); } packSet(result); // удаляем частные вхождения l = result; }
bool TParam::parseTree(std::istream &is, std::stack<int> &indentation) { // read newline separated parameter file if (is.eof()) return true; std::string line; getline(is, line); std::size_t pos = line.find_first_not_of(' '); if (pos == std::string::npos || line[pos] == '#') return parseTree(is, indentation); assert(line[pos] != '\t'); std::istringstream linestream(line); std::string pName; linestream >> pName; assert(!linestream.fail()); // find parent depending on indentation and adapt indentation TParam *pParent = this; while ((int)pos <= indentation.top()) { pParent = pParent->parent; indentation.pop(); } // read parameter Variant pValue; std::string t = pValue.tryRead(linestream); if (pValue.getType() == Invalid) { if (!t.empty() && t[0] != '#') std::cerr << "Warning: Unknown type '" << t << "' of parameter '" << pName << "'. Parsing as category.\n"; indentation.push(pos); // no leaf node } TParam p(pName, pValue); pParent->addChild(p); if (p.getType() == Invalid) return pParent->children.find(p.name)->second->parseTree(is, indentation); return pParent->parseTree(is, indentation); }
void SelectDrawWidget::GoToWWWDocumentation(DrawInfo *d) { if (d->IsValid() == false) return; TParam *p = d->GetParam()->GetIPKParam(); TSzarpConfig* sc = p->GetSzarpConfig(); wxString link = sc->GetDocumentationBaseURL(); if (link.IsEmpty()) link = _T("http://www.szarp.com"); link += _T("/cgi-bin/param_docs.py?"); link << _T("prefix=") << d->GetBasePrefix().c_str(); link << _T("¶m=") << encode_string(p->GetName().c_str()); TUnit* u; if ((u = p->GetParentUnit())) { TDevice* dev = u->GetDevice(); link << _T("&path=") << encode_string(dev->GetPath().c_str()); } int validity = 8 * 3600; std::wstringstream tss; tss << (wxDateTime::Now().GetTicks() / validity * validity); link << _T("&id=") << sz_md5(tss.str()); #if __WXMSW__ if (wxLaunchDefaultBrowser(link) == false) #else if (wxExecute(wxString::Format(_T("xdg-open %s"), link.c_str())) == 0) #endif wxMessageBox(_("I was not able to start default browser"), _("Error"), wxICON_ERROR | wxOK, this); }
/** * Создает полностью детерминированную зависимость * Все недетерминированные выражения в индексах заменяются на * диапазон 0..размерность */ stringSet createActual(const stringSet &arg) { stringSet result; stringSet::const_iterator beg = arg.begin(); stringSet::const_iterator end = arg.end(); while (beg != end) { TParam cur = *--end; if (!cur. isArrayElement()) { result. push_back(cur); // fixme - remember arrayScalar continue; } // index case int dim = cur. getDimension(); TIndex *indexes = cur. getIndexes(); TDimensionList list = cur. getTypeInfo(). second; TDimensionList::iterator beg = list. begin(); for (int i = dim - 1; i >= 0; i--, beg++) { if (indexes[i]. isAtom()) { indexes[i] = TIndex(TRange(0, (*beg - 1))); continue; } } // этот элемент будет прозрачный в силу изначальной недетерминированности его индекса TParam res(cur. getName(), indexes, dim, cur.getTypeInfo(), true); //cout<<"Create Actual :"<<res<<endl; concat(result, toLocalMemoryUse(res)); delete[] indexes; } packUnifySet(result); return result; }
bool TParam::update(const TParam &other) { if (name != other.name) { std::cerr << "Error in " << __PRETTY_FUNCTION__ << ": Trying to update Param " << name << " with " << other.name << ".\n"; return false; } if (getType() != other.getType()) { std::cerr << "Error in " << __PRETTY_FUNCTION__ << ": Trying to update Param " << name << " of type " << getTypeName() << " with type " << other.getTypeName() << ".\n"; return false; } Variant::operator=(other); for (std::map<std::string, TParam*>::const_iterator it = other.children.begin(); it != other.children.end(); it++) { if (children.count(it->first) > 0) { if (!children[it->first]->update(*it->second)) return false; } else { if (!addChild(*it->second)) return false; } } return true; }
int main(int argc, char **argv) { Random::randomize(); std::string paramfile = "multi.ini"; int test = 1; char c; while ((c = getopt(argc, argv, "p:h")) != EOF) { switch (c) { case 'p': paramfile = optarg; break; case 'h': default: std::cerr << "Usage: " << argv[0] << " [options]\n"; std::cerr << "\nOptions:\n"; std::cerr << "-p <file>: use the given parameter file\n"; std::cerr << "-h: this help\n"; return 1; } } TParam p; p.loadTree(paramfile); MultiAgentMotionModel *motionModel; std::string typestr = p("multi_rotor_control/type").toString(); if (typestr == "point2d") { motionModel = new Point2dMotionModel(); } else if (typestr == "rotor2d") { motionModel = new Rotor2dMotionModel(); } else { std::cerr << "Error: Unknown type '" << typestr << "' - exiting\n"; exit(-1); } motionModel->init(p); TargetTrackingController ttc; ttc.init(p); std::vector<const SensorModel*> sensorModels = ttc.getTopology().getSensorModels(); if (test == 1) { // online test TargetTrajectory tt; tt.init(p); EKF ekf(motionModel); ekf.init(p); std::ofstream topo_out("topo.out"); std::ofstream multi_out("multi.out"); unsigned int nA = motionModel->getNumAgents(); unsigned int nT = motionModel->getNumTargets(); unsigned int aSD = motionModel->getAgentStateDim(); unsigned int cSD = motionModel->getAgentControlDim(); unsigned int tSD = motionModel->getTargetStateDim(); // test output Eigen::VectorXd state = p("estimation/initialState").toVectorXd(); for (unsigned int i=0; !tt.atEnd(); ++i) { Eigen::VectorXd mean = ekf.getMean(); Eigen::MatrixXd cov = ekf.getCovariance(); // Eigen::VectorXd control(nA*cSD); // Eigen::VectorXd targetPosition = state.segment(aSD*nA, cSD); // for (unsigned int i=0; i<nA; ++i) { // control.segment(cSD*i, cSD) = (targetPosition - state.segment(aSD*i, cSD))/p("estimation/motionDt").toDouble(); // } Eigen::VectorXd control = ttc.getControlTopo(&ekf, motionModel, sensorModels); multi_out << aSD << " " << cSD << " " << tSD << " 0 0 " << nA << " " << nT << " 0 0 0" << " " << state.transpose() << " " << control.transpose() << " " << mean.transpose(); multi_out << " " << Eigen::Map<Eigen::MatrixXd>(cov.data(), 1, cov.cols()*cov.cols()); // for (int i=0; i<N+1; ++i) { // multi_out << " " << cov(2*agentStateDim, 2*agentStateDim) << " " << cov(2*agentStateDim, 2*agentStateDim+1) << " " << cov(2*agentStateDim+1, 2*agentStateDim) << " " << cov(2*agentStateDim+1, 2*agentStateDim+1); // } multi_out << "\n"; ttc.getTopology().write(topo_out, state); // simulation state = motionModel->move(state, control, motionModel->sampleNoise(state, control)); if (Random::uniform() < p("multi_rotor_control/kidnap/target").toDouble()) { // kidnap target state.segment(aSD*nA, 2) = tt.randomJump(); ekf.getCovariance().block(aSD*nA, aSD*nA, 4, 4) = Eigen::MatrixXd::Identity(4, 4)*4.0; } else { state.segment(aSD*nA, 2) = tt.step(); } if (Random::uniform() < p("multi_rotor_control/kidnap/agent").toDouble()) { // kidnap agent int agent = rand()%nA; state.segment(aSD*agent, 2) = Eigen::Vector2d(Random::uniform(-1, 1), Random::uniform(-0.5, 0.5)); ekf.getCovariance().block(aSD*agent, aSD*agent, 2, 2) = Eigen::MatrixXd::Identity(2, 2)*4.0; } // state estimation ekf.predict(control); for (std::vector<const SensorModel*>::const_iterator it = sensorModels.begin(); it != sensorModels.end(); ++it) { if ((*it)->measurementAvailable(state)) { Eigen::VectorXd noiseSample = (*it)->sampleNoise(state, (*it)->sense(state)); Eigen::VectorXd measurementSample = (*it)->sense(state, noiseSample); ekf.correct(measurementSample, *(*it)); } } } } return 0; }
void TIniFile::saveFile(std::string strFilename) { std::fstream file; file.open(trim(strFilename).c_str(), std::fstream::out | std::fstream::trunc); if(!file.is_open()) return; std::map<std::string, TSection*>::iterator iter = m_mSections.begin(); TParam* param = NULL; if(!m_strHeader.empty()) { size_t pos; std::string str_aux = m_strHeader; while((pos = str_aux.find('\n')) != std::string::npos) { file << "# " << str_aux.substr(0, pos) << std::endl; str_aux = str_aux.substr(pos + 1); } file << "# " << str_aux.substr(0, pos) << std::endl << std::endl; } while(iter != m_mSections.end()) { if(iter->second) { if(iter != m_mSections.begin()) file << std::endl; if(iter->second->comment().size()) { size_t pos; std::string comment = iter->second->comment(); while((pos = comment.find('\n')) != std::string::npos) { file << "# " << comment.substr(0, pos) << std::endl; comment = comment.substr(pos + 1); } file << "# " << comment.substr(0, pos) << std::endl; } file << "[" << iter->second->name() << "]" << std::endl; for(size_t i = 0; i < iter->second->size(); ++i) { param = (*(iter->second))[i]; if(param) { file << param->name() << "=" << param->value().str(); if(param->comment().size()) file << " # " << param->comment(); file << std::endl; } } } ++iter; } file.close(); }
void* QueryExecutor::Entry() { DatabaseQuery *q = NULL; #if 0 try { #endif while ((q = queue->GetQuery())) { bool post_response = false; if (q->type == DatabaseQuery::STARTING_CONFIG_RELOAD) { szbase->RemoveConfig(q->reload_prefix.prefix, true); post_response = true; } else if (q->type == DatabaseQuery::COMPILE_FORMULA) { std::wstring error; bool ret; #ifndef NO_LUA ret = szbase->CompileLuaFormula(q->compile_formula.formula, error); q->compile_formula.ok = ret; if (ret == false) q->compile_formula.error = wcsdup(error.c_str()); #else q->compile_formula.ok = false; q->compile_formula.error = strdup(error.c_str()); #endif post_response = true; #ifndef NO_LUA } else if (q->type == DatabaseQuery::ADD_PARAM) { TParam *p = q->defined_param.p; wchar_t *prefix = q->defined_param.prefix; szbase->AddExtraParam(prefix, p); post_response = false; free(prefix); } else if (q->type == DatabaseQuery::REMOVE_PARAM) { TParam *p = q->defined_param.p; wchar_t *prefix = q->defined_param.prefix; szbase->RemoveExtraParam(prefix, p); post_response = true; #endif } else if (q->type == DatabaseQuery::CHECK_CONFIGURATIONS_CHANGE) { szbase->NotifyAboutConfigurationChanges(); post_response = false; } else if (q->type == DatabaseQuery::SET_PROBER_ADDRESS) { szbase->SetProberAddress(q->prefix, q->prober_address.address, q->prober_address.port); free(q->prober_address.address); free(q->prober_address.port); post_response = false; } else if (q->type == DatabaseQuery::EXTRACT_PARAM_VALUES) { ExecuteExtractParametersQuery(q->extraction_parameters); post_response = false; } else { TParam *p = NULL; TSzarpConfig *cfg = NULL; szb_buffer_t *szb = NULL; if (q->param) { p = q->param; cfg = p->GetSzarpConfig(); szb = szbase->GetBuffer(cfg->GetPrefix()); } switch (q->type) { case DatabaseQuery::SEARCH_DATA: szbase->NextQuery(); ExecuteSearchQuery(szb, p, q->search_data); post_response = true; break; case DatabaseQuery::GET_DATA: szbase->NextQuery(); ExecuteDataQuery(szb, p, q); post_response = false; break; case DatabaseQuery::RESET_BUFFER: if (szb) szb_reset_buffer(szb); break; case DatabaseQuery::CLEAR_CACHE: if (cfg) szbase->ClearCacheDir(cfg->GetPrefix().c_str()); break; default: break; } } if (post_response) { DatabaseResponse r(q); wxPostEvent(response_receiver, r); } else delete q; } #if 0 } catch (std::exception &e) { std::cout << e.what() << std::endl; } #endif Szbase::Destroy(); return NULL; }