EReturn AICOProblem::initDerived(tinyxml2::XMLHandle & handle)
{
EReturn ret_value = SUCCESS;
tinyxml2::XMLElement* xmltmp;
bool hastime = false;
XML_CHECK("T");
XML_OK(getInt(*xmltmp, T));
if (T <= 2)
{
INDICATE_FAILURE
;
return PAR_ERR;
}
xmltmp = handle.FirstChildElement("duration").ToElement();
if (xmltmp)
{
XML_OK(getDouble(*xmltmp, tau));
tau = tau / ((double) T);
hastime = true;
}
if (hastime)
{
xmltmp = handle.FirstChildElement("tau").ToElement();
if (xmltmp)
WARNING("Duration has already been specified, tau is ignored.");
}
else
{
XML_CHECK("tau");
XML_OK(getDouble(*xmltmp, tau));
}
XML_CHECK("Qrate");
XML_OK(getDouble(*xmltmp, Q_rate));
XML_CHECK("Hrate");
XML_OK(getDouble(*xmltmp, H_rate));
XML_CHECK("Wrate");
XML_OK(getDouble(*xmltmp, W_rate));
{
Eigen::VectorXd tmp;
XML_CHECK("W");
XML_OK(getVector(*xmltmp, tmp));
W = Eigen::MatrixXd::Identity(tmp.rows(), tmp.rows());
W.diagonal() = tmp;
}
for (TaskDefinition_map::const_iterator it = task_defs_.begin();
it != task_defs_.end() and ok(ret_value); ++it)
{
if (it->second->type().compare(std::string("TaskSqrError")) == 0)
ERROR("Task variable " << it->first << " is not an squared error!");
}
// Set number of time steps
return setTime(T);
}
void Identity::initDerived(tinyxml2::XMLHandle & handle)
{
// Load the goal
if (handle.FirstChildElement("Ref").ToElement())
{
getVector(*(handle.FirstChildElement("Ref").ToElement()), jointRef);
jointMap.resize(jointRef.rows());
for (int i = 0; i < jointRef.rows(); i++)
jointMap[i] = i;
useRef = true;
}
}
void TaskTerminationCriterion::initDerived(tinyxml2::XMLHandle & handle)
{
TaskSqrError::initDerived(handle);
double thr;
if (handle.FirstChildElement("Threshold").ToElement())
{
getDouble(*(handle.FirstChildElement("Threshold").ToElement()),thr);
threshold0_(0) = thr;
}
else
{
throw_named("Threshold was not specified");
}
setTimeSteps(1);
}
void IMesh::initDerived(tinyxml2::XMLHandle & handle)
{
EParam<std_msgs::String> ref;
tinyxml2::XMLHandle tmp_handle = handle.FirstChildElement("ReferenceFrame");
server_->registerParam(ns_, tmp_handle, ref);
if (!tmp_handle.ToElement()) ref->data = "/world";
initDebug(ref->data);
eff_size_ = scene_->getMapSize(object_name_);
weights_.setOnes(eff_size_, eff_size_);
initialised_ = true;
}
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;
}
}
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 IKProblem::initDerived(tinyxml2::XMLHandle & handle)
{
tinyxml2::XMLElement* xmltmp;
xmltmp = handle.FirstChildElement("W").ToElement();
if (xmltmp)
{
Eigen::VectorXd tmp;
getVector(*xmltmp, tmp);
config_w_ = Eigen::MatrixXd::Identity(tmp.rows(), tmp.rows());
config_w_.diagonal() = tmp;
}
xmltmp = handle.FirstChildElement("Tolerance").ToElement();
if (xmltmp)
{
getDouble(*xmltmp, tau_);
}
xmltmp = handle.FirstChildElement("T").ToElement();
if (xmltmp)
{
try
{
getInt(*xmltmp, T_);
}
catch (Exception e)
{
T_ = 1;
}
}
else
{
T_ = 1;
}
for (auto& it : task_defs_)
{
it.second->setTimeSteps(T_);
}
}
void OMPLsolver::initDerived(tinyxml2::XMLHandle & handle)
{
tinyxml2::XMLHandle tmp_handle = handle.FirstChildElement(
"TrajectorySmooth");
server_->registerParam<std_msgs::Bool>(ns_, tmp_handle, smooth_);
tmp_handle = handle.FirstChildElement("Solver");
server_->registerParam<std_msgs::String>(ns_, tmp_handle, solver_);
tmp_handle = handle.FirstChildElement("SolverPackage");
server_->registerParam<std_msgs::String>(ns_, tmp_handle,solver_package_);
try
{
HIGHLIGHT_NAMED(object_name_,
"Using ["<<solver_->data<<"] from package ["<<solver_package_->data<<"].");
base_solver_ = OMPLBaseSolver::base_solver_loader.createInstance(
"ompl_solver/" + solver_->data);
} catch (pluginlib::PluginlibException& ex)
{
throw_named("EXOTica-OMPL plugin failed to load solver "<<solver_->data<<". Solver package: '"<<solver_package_->data<< "'. \nError: " << ex.what());
}
base_solver_->initialiseBaseSolver(handle, server_);
}
EReturn Initialiser::initialise(tinyxml2::XMLHandle root_handle,
Server_ptr & server)
{
server = Server::Instance();
tinyxml2::XMLHandle server_handle(root_handle.FirstChildElement("Server"));
if (server_handle.ToElement())
{
if (ok(server->initialise(server_handle)))
{
return SUCCESS;
}
else
{
INDICATE_FAILURE
return FAILURE;
}
}
else
{
EReturn OMPLsolver::initDerived(tinyxml2::XMLHandle & handle)
{
tinyxml2::XMLHandle tmp_handle = handle.FirstChildElement(
"TrajectorySmooth");
server_->registerParam<std_msgs::Bool>(ns_, tmp_handle, smooth_);
tmp_handle = handle.FirstChildElement("SaveResults");
server_->registerParam<std_msgs::Bool>(ns_, tmp_handle, saveResults_);
tinyxml2::XMLElement* xmltmp;
std::string txt;
XML_CHECK("algorithm");
{
txt = std::string(xmltmp->GetText());
bool known = false;
std::vector<std::string> nm = getPlannerNames();
for (std::string s : nm)
{
if (txt.compare(s.substr(11)) == 0)
{
selected_planner_ = s;
INFO(
"Using planning algorithm: "<<selected_planner_<<". Trajectory smoother is "<<(smooth_->data?"Enabled":"Disabled"));
known = true;
break;
}
}
if (!known)
{
ERROR("Unknown planning algorithm ["<<txt<<"]");
HIGHLIGHT_NAMED(object_name_, "Available algorithms:");
for (std::string s : nm)
HIGHLIGHT_NAMED(object_name_, s);
return FAILURE;
}
}
XML_CHECK("max_goal_sampling_attempts");
XML_OK(getInt(*xmltmp, goal_ampling_max_attempts_));
projection_components_.clear();
tmp_handle = handle.FirstChildElement("Projection");
if (tmp_handle.ToElement())
{
projector_ = tmp_handle.ToElement()->Attribute("type");
tinyxml2::XMLHandle jnt_handle = tmp_handle.FirstChildElement(
"component");
while (jnt_handle.ToElement())
{
const char* atr = jnt_handle.ToElement()->Attribute("name");
if (atr)
{
projection_components_.push_back(std::string(atr));
}
else
{
INDICATE_FAILURE
;
return FAILURE;
}
jnt_handle = jnt_handle.NextSiblingElement("component");
}
}
tmp_handle = handle.FirstChildElement("Range");
if (tmp_handle.ToElement())
{
server_->registerParam<std_msgs::String>(ns_, tmp_handle, range_);
}
if (saveResults_->data)
{
std::string path = ros::package::getPath("ompl_solver") + "/result/" + txt
+ ".txt";
result_file_.open(path);
if (!result_file_.is_open())
{
ERROR("Error open "<<path);
return FAILURE;
}
}
succ_cnt_ = 0;
return SUCCESS;
}
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)
}