OMPLStateValidityChecker::OMPLStateValidityChecker(
const ob::SpaceInformationPtr &si, const OMPLProblem_ptr &prob)
: ob::StateValidityChecker(si), prob_(prob)
{
Server_ptr server = Server::Instance();
if (server->hasParam(server->getName() + "/SafetyMargin"))
server->getParam(server->getName() + "/SafetyMargin", margin_);
else
{
margin_.reset(new std_msgs::Float64());
margin_->data = 0.0;
server->setParam(server->getName() + "/SafetyMargin", margin_);
}
HIGHLIGHT_NAMED("OMPLStateValidityChecker",
"Safety Margin: " << margin_->data);
}
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 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 OMPLsolver::specifyProblem(PlanningProblem_ptr goals,
PlanningProblem_ptr costs, PlanningProblem_ptr goalBias,
PlanningProblem_ptr samplingBias)
{
if (goals->type().compare(std::string("exotica::OMPLProblem")) != 0)
{
ERROR("This solver can't use problem of type '" << goals->type() << "'!");
return PAR_INV;
}
if (costs
&& costs->type().compare(std::string("exotica::OMPLProblem")) != 0)
{
ERROR("This solver can't use problem of type '" << costs->type() << "'!");
return PAR_INV;
}
if (goalBias
&& goalBias->type().compare(std::string("exotica::OMPLProblem")) != 0)
{
ERROR(
"This solver can't use problem of type '" << goalBias->type() << "'!");
return PAR_INV;
}
if (samplingBias
&& samplingBias->type().compare(std::string("exotica::OMPLProblem"))
!= 0)
{
ERROR(
"This solver can't use problem of type '" << samplingBias->type() << "'!");
return PAR_INV;
}
MotionSolver::specifyProblem(goals);
prob_ = boost::static_pointer_cast<OMPLProblem>(goals);
costs_ = boost::static_pointer_cast<OMPLProblem>(costs);
goalBias_ = boost::static_pointer_cast<OMPLProblem>(goalBias);
samplingBias_ = boost::static_pointer_cast<OMPLProblem>(samplingBias);
for (auto & it : prob_->getScenes())
{
if (!ok(it.second->activateTaskMaps()))
{
INDICATE_FAILURE
;
return FAILURE;
}
}
if (costs)
{
for (auto & it : costs_->getScenes())
{
if (!ok(it.second->activateTaskMaps()))
{
INDICATE_FAILURE
;
return FAILURE;
}
}
}
if (goalBias_)
{
for (auto & it : goalBias_->getScenes())
{
if (!ok(it.second->activateTaskMaps()))
{
INDICATE_FAILURE
;
return FAILURE;
}
}
}
if (samplingBias_)
{
for (auto & it : samplingBias_->getScenes())
{
if (!ok(it.second->activateTaskMaps()))
{
INDICATE_FAILURE
;
return FAILURE;
}
}
}
compound_ = prob_->isCompoundStateSpace();
if (compound_)
{
state_space_ = ob::StateSpacePtr(
OMPLSE3RNCompoundStateSpace::FromProblem(prob_, server_));
}
else
{
state_space_ = OMPLStateSpace::FromProblem(prob_);
}
HIGHLIGHT_NAMED(object_name_,
"Using state space: "<<state_space_->getName());
ompl_simple_setup_.reset(new og::SimpleSetup(state_space_));
ompl_simple_setup_->setStateValidityChecker(
ob::StateValidityCheckerPtr(new OMPLStateValidityChecker(this)));
ompl_simple_setup_->setPlannerAllocator(
boost::bind(known_planners_[selected_planner_], _1,
this->getObjectName() + "_" + selected_planner_));
if (compound_)
ompl_simple_setup_->getStateSpace()->setStateSamplerAllocator(
boost::bind(&allocSE3RNStateSampler,
(ob::StateSpace*) state_space_.get()));
ompl_simple_setup_->getSpaceInformation()->setup();
std::vector<std::string> jnts;
prob_->getScenes().begin()->second->getJointNames(jnts);
if (projector_.compare("Joints") == 0)
{
bool projects_ok_ = true;
std::vector<int> vars(projection_components_.size());
for (int i = 0; i < projection_components_.size(); i++)
{
bool found = false;
for (int j = 0; j < jnts.size(); j++)
{
if (projection_components_[i].compare(jnts[j]) == 0)
{
vars[i] = j;
found = true;
break;
}
}
if (!found)
{
WARNING(
"Projection joint ["<<projection_components_[i]<<"] does not exist, OMPL Projection Evaluator not used");
projects_ok_ = false;
break;
}
}
if (projects_ok_)
{
ompl_simple_setup_->getStateSpace()->registerDefaultProjection(
ompl::base::ProjectionEvaluatorPtr(
new exotica::OMPLProjection(state_space_, vars)));
std::string tmp;
for (int i = 0; i < projection_components_.size(); i++)
tmp = tmp + "[" + projection_components_[i] + "] ";
HIGHLIGHT_NAMED(object_name_, " Using projection joints "<<tmp);
}
}
else if (projector_.compare("DMesh") == 0)
{
// Construct default DMesh projection relationship
std::vector<std::pair<int, int> > tmp_index(
projection_components_.size() - 1);
for (int i = 0; i < tmp_index.size(); i++)
{
tmp_index[i].first = i;
tmp_index[i].second = tmp_index.size();
}
ompl_simple_setup_->getStateSpace()->registerDefaultProjection(
ompl::base::ProjectionEvaluatorPtr(
new exotica::DMeshProjection(state_space_, projection_components_,
tmp_index, prob_->getScenes().begin()->second)));
std::string tmp;
for (int i = 0; i < projection_components_.size(); i++)
tmp = tmp + "[" + projection_components_[i] + "] ";
HIGHLIGHT_NAMED(object_name_, " Using DMesh Projection links "<<tmp);
}
else
{
WARNING_NAMED(object_name_,
"Unknown projection type "<<projector_<<". Setting ProjectionEvaluator failed.");
}
ompl_simple_setup_->setGoal(constructGoal());
ompl_simple_setup_->setup();
if (isFlexiblePlanner())
{
INFO_NAMED(object_name_,
"Setting up FRRT Local planner from file\n"<<prob_->local_planner_config_);
if (!ompl_simple_setup_->getPlanner()->as<ompl::geometric::FlexiblePlanner>()->setUpLocalPlanner(
prob_->local_planner_config_, prob_->scenes_.begin()->second))
{
INDICATE_FAILURE
return FAILURE;
}
}
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;
}
EReturn OMPLsolver::getSimplifiedPath(ompl::geometric::PathGeometric &pg,
Eigen::MatrixXd & traj, ob::PlannerTerminationCondition &ptc)
{
if (smooth_->data)
{
HIGHLIGHT("Simplifying solution");
int original_cnt = pg.getStateCount();
ros::Time start = ros::Time::now();
//ompl_simple_setup_->simplifySolution(d);
// Lets do our own simplifier ~:)
if (original_cnt >= 3)
{
og::PathSimplifierPtr psf_ = ompl_simple_setup_->getPathSimplifier();
const ob::SpaceInformationPtr &si =
ompl_simple_setup_->getSpaceInformation();
bool tryMore = false;
if (ptc == false) tryMore = psf_->reduceVertices(pg);
if (ptc == false) psf_->collapseCloseVertices(pg);
int times = 0;
while (tryMore && ptc == false)
{
tryMore = psf_->reduceVertices(pg);
times++;
}
if (si->getStateSpace()->isMetricSpace())
{
if (ptc == false)
tryMore = psf_->shortcutPath(pg);
else
tryMore = false;
times = 0;
while (tryMore && ptc == false)
{
tryMore = psf_->shortcutPath(pg);
times++;
}
}
std::vector<ob::State*> &states = pg.getStates();
// Calculate number of states required
unsigned int length = 0;
const int n1 = states.size() - 1;
for (int i = 0; i < n1; ++i)
length += si->getStateSpace()->validSegmentCount(states[i],
states[i + 1]);
// // Forward reducing
// HIGHLIGHT("States before forward reducing: "<<pg.getStateCount());
// pg.interpolate(length);
//
// bool need_backward = true;
// for (int i = states.size() - 1; i > 0; i--)
// {
// if (si->checkMotion(states[0], states[i]))
// {
// ob::State *start = si->cloneState(states[0]);
// pg.keepAfter(states[i]);
// pg.prepend(start);
// need_backward = (i == states.size() - 1) ? false : true;
// break;
// }
// }
//
// // Backward reducing
// ob::State *mid;
// if (need_backward)
// {
// HIGHLIGHT("States before backward reducing: "<<pg.getStateCount());
// pg.interpolate(length);
// for (int i = 1; i < states.size(); i++)
// {
// if (si->checkMotion(states[states.size() - 1], states[i]))
// {
// ob::State *goal = si->cloneState(states[states.size() - 1]);
// pg.keepBefore(states[i]);
// pg.append(goal);
// mid = si->cloneState(states[i]);
// break;
// }
// }
// }
pg.interpolate(length);
}
// if (ompl_simple_setup_->haveSolutionPath())
// {
// pg.interpolate();
// }
HIGHLIGHT_NAMED("OMPLSolver",
"Simplification took "<<ros::Duration(ros::Time::now()-start).toSec()<<"sec. States: "<<original_cnt<<"->"<<pg.getStateCount());
}
convertPath(pg, traj);
return SUCCESS;
}