std::string extract(tinyxml2::XMLHandle handle) {
tinyxml2::XMLElement* element = handle.ToElement();
if (element == nullptr) {
throw std::runtime_error("Missing element!");
}
const char* str = element->GetText();
if (str == nullptr) {
throw std::runtime_error("Missing text!");
}
return std::string(str);
}
void Server::initialise(tinyxml2::XMLHandle & handle)
{
static bool first = true;
if (!singleton_server_)
{
name_ = handle.ToElement()->Attribute("name");
std::string ns = name_;
}
tinyxml2::XMLHandle param_handle(handle.FirstChildElement("Parameters"));
tinyxml2::XMLHandle tmp_handle = param_handle.FirstChild();
while (tmp_handle.ToElement())
{
createParam(name_, tmp_handle);
tmp_handle = tmp_handle.NextSibling();
}
if (first)
{
HIGHLIGHT_NAMED(name_, "EXOTica Server Initialised");
first = false;
}
}
void Server::createParam(const std::string & ns,
tinyxml2::XMLHandle & tmp_handle)
{
std::string name = ns + "/" + tmp_handle.ToElement()->Name();
std::string type = tmp_handle.ToElement()->Attribute("type");
if (type.compare("int") == 0)
{
double dou;
getDouble(*tmp_handle.FirstChildElement("default").ToElement(), dou);
std_msgs::Int64 val;
val.data = dou;
params_[name] = boost::shared_ptr<std_msgs::Int64>(
new std_msgs::Int64(val));
}
else if (type.compare("double") == 0)
{
double dou;
getDouble(*tmp_handle.FirstChildElement("default").ToElement(), dou);
std_msgs::Float64 val;
val.data = dou;
params_[name] = boost::shared_ptr<std_msgs::Float64>(
new std_msgs::Float64(val));
}
else if (type.compare("vector") == 0)
{
exotica::Vector vec;
getStdVector(*tmp_handle.FirstChildElement("default").ToElement(),
vec.data);
params_[name] = boost::shared_ptr<exotica::Vector>(
new exotica::Vector(vec));
}
else if (type.compare("bool") == 0)
{
bool val;
getBool(*tmp_handle.FirstChildElement("default").ToElement(), val);
std_msgs::Bool ros_bool;
ros_bool.data = val;
params_[name] = boost::shared_ptr<std_msgs::Bool>(
new std_msgs::Bool(ros_bool));
}
else if (type.compare("boollist") == 0)
{
exotica::BoolList boollist;
std::vector<bool> vec;
getBoolVector(*tmp_handle.FirstChildElement("default").ToElement(),
vec);
boollist.data.resize(vec.size());
for (int i = 0; i < vec.size(); i++)
boollist.data[i] = vec[i];
params_[name] = boost::shared_ptr<exotica::BoolList>(
new exotica::BoolList(boollist));
}
else if (type.compare("string") == 0)
{
std::string str =
tmp_handle.FirstChildElement("default").ToElement()->GetText();
if (str.size() == 0)
{
throw_pretty("Invalid string!");
}
std_msgs::String ros_s;
ros_s.data =
tmp_handle.FirstChildElement("default").ToElement()->GetText();
params_[name] = boost::shared_ptr<std_msgs::String>(
new std_msgs::String(ros_s));
}
else if (type.compare("stringlist") == 0)
{
exotica::StringList list;
getList(*tmp_handle.FirstChildElement("default").ToElement(),
list.strings);
params_[name] = boost::shared_ptr<exotica::StringList>(
new exotica::StringList(list));
}
else
{
throw_pretty("Unknown type!");
}
INFO("Register new paramter "<<name)
}