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;
}
