Skip to content
Snippets Groups Projects
Commit 06c07ac6 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Use smart pointers in Prb::autoSetup

parent 9e561ff9
No related branches found
No related tags found
2 merge requests!436Release to start wolf public,!433After 2nd RA-L submission
Pipeline #8066 failed
......@@ -113,35 +113,37 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
#else
std::string lib_extension = ".so";
#endif
// Problem structure and dimension
std::string frame_structure = _server.getParam<std::string> ("problem/frame_structure");
int dim = _server.getParam<int> ("problem/dimension");
auto problem = Problem::create(frame_structure, dim);
//
// cout << "PRINTING SERVER MAP" << endl;
// _server.print();
// cout << "-----------------------------------" << endl;
int dim = _server.getParam<int> ("problem/dimension");
auto problem = Problem::create(frame_structure, dim);
WOLF_TRACE("Setting up problem with frame structure {" + frame_structure + "} and dimension " + std::to_string(dim) + "D");
// Load plugins
auto loaders = std::vector<Loader *>();
auto loaders = std::vector<std::shared_ptr<Loader>>();
std::string plugins_path;
try{
plugins_path = _server.getParam<std::string>("plugins_path");
}
catch(MissingValueException& e){
WOLF_WARN(e.what());
WOLF_WARN("Setting '/usr/local/lib/iri-algorithms/' as plugins path...");
plugins_path="/usr/local/lib/iri-algorithms/";
WOLF_WARN("Setting '/usr/local/lib/' as plugins path...");
plugins_path="/usr/local/lib/";
}
for (auto plugin_name : _server.getParam<std::vector<std::string>>("plugins")) {
for (auto plugin_name : _server.getParam<std::vector<std::string>>("plugins"))
{
if (plugin_name == "core" or plugin_name == "wolf" or plugin_name == "") continue; // ignore plugin "core"
std::string plugin = plugins_path + "libwolf" + plugin_name + lib_extension;
WOLF_TRACE("Loading plugin " + plugin);
auto l = new LoaderRaw(plugin);
WOLF_TRACE("Loading plugin " + plugin_name + " via " + plugin);
auto l = std::make_shared<LoaderRaw>(plugin);
l->load();
loaders.push_back(l);
}
// load Packages for subscribers and publishers
std::string packages_path;
try {
packages_path = _server.getParam<std::string>("packages_path");
......@@ -149,20 +151,22 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
WOLF_WARN(e.what());
WOLF_WARN("Support for subscribers disabled...");
}
for (auto it : _server.getParam<std::vector<std::string>>("packages_subscriber")) {
std::string subscriber = packages_path + "/libsubscriber_" + it + lib_extension;
WOLF_TRACE("Loading subscriber " + subscriber);
auto l = new LoaderRaw(subscriber);
for (auto subscriber_name : _server.getParam<std::vector<std::string>>("packages_subscriber")) {
std::string subscriber = packages_path + "/libsubscriber_" + subscriber_name + lib_extension;
WOLF_TRACE("Loading subscriber " + subscriber_name + " via " + subscriber);
auto l = std::make_shared<LoaderRaw>(subscriber);
l->load();
loaders.push_back(l);
}
for (auto it : _server.getParam<std::vector<std::string>>("packages_publisher")) {
std::string subscriber = packages_path + "/libpublisher_" + it + lib_extension;
WOLF_TRACE("Loading publisher " + subscriber);
auto l = new LoaderRaw(subscriber);
for (auto publisher_name : _server.getParam<std::vector<std::string>>("packages_publisher")) {
std::string publisher = packages_path + "/libpublisher_" + publisher_name + lib_extension;
WOLF_TRACE("Loading publisher " + publisher_name + " via " + publisher);
auto l = std::make_shared<LoaderRaw>(publisher);
l->load();
loaders.push_back(l);
}
// load raw libs
std::vector<std::string> raw_libs;
try {
raw_libs = _server.getParam<std::vector<std::string>>("raw_libs");
......@@ -172,22 +176,25 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
}
for (auto lib : raw_libs) {
WOLF_TRACE("Loading raw lib " + lib);
auto l = new LoaderRaw(lib);
auto l = std::make_shared<LoaderRaw>(lib);
l->load();
loaders.push_back(l);
}
// Install sensors and processors
auto sensorMap = std::map<std::string, SensorBasePtr>();
auto procesorMap = std::map<std::string, ProcessorBasePtr>();
auto sensors = _server.getParam<std::vector<std::map<std::string, std::string>>>("sensors");
for(auto sen : sensors){
sensorMap.insert(std::pair<std::string, SensorBasePtr>(sen["name"], problem->installSensor(sen["type"], sen["name"], _server)));
}
auto processors = _server.getParam<std::vector<std::map<std::string, std::string>>>("processors");
for(auto prc : processors){
procesorMap.insert(std::pair<std::string, ProcessorBasePtr>(prc["name"], problem->installProcessor(prc["type"], prc["name"], prc["sensor_name"], _server)));
}
auto sensors = _server.getParam<std::vector<std::map<std::string, std::string>>>("sensors");
for(auto sen : sensors)
problem->installSensor(sen["type"],
sen["name"],
_server);
auto processors = _server.getParam<std::vector<std::map<std::string, std::string>>>("processors");
for(auto prc : processors)
problem->installProcessor(prc["type"],
prc["name"],
prc["sensor_name"],
_server);
// Tree manager
std::string tree_manager_type = _server.getParam<std::string>("problem/tree_manager/type");
......@@ -195,7 +202,7 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
if (tree_manager_type != "None" and tree_manager_type != "none")
problem->setTreeManager(AutoConfFactoryTreeManager::create(tree_manager_type, "tree manager", _server));
// Prior
// Set problem prior -- first keyframe
std::string prior_mode = _server.getParam<std::string>("problem/prior/mode");
assert((prior_mode == "nothing" || prior_mode == "initial_guess" || prior_mode == "fix" || prior_mode == "factor") && "wrong _mode value, it should be: 'nothing', 'initial_guess', 'fix' or 'factor'");
WOLF_TRACE("Prior mode: ", prior_mode);
......@@ -214,7 +221,6 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
}
else
{
WOLF_TRACE("Prior mode: ", prior_mode);
problem->setPriorOptions(prior_mode,
_server.getParam<VectorComposite>("problem/prior/state"));
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment