ompl_interface::ConstraintApproximationConstructionResults
ompl_interface::ConstraintsLibrary::addConstraintApproximation(const moveit_msgs::Constraints &constr_sampling, const moveit_msgs::Constraints &constr_hard,
const std::string &group, const std::string &state_space_parameterization,
const planning_scene::PlanningSceneConstPtr &scene, unsigned int samples, unsigned int edges_per_sample)
{
ConstraintApproximationConstructionResults res;
ModelBasedPlanningContextPtr pc = context_manager_.getPlanningContext(group, state_space_parameterization);
if (pc)
{
pc->clear();
pc->setPlanningScene(scene);
pc->setCompleteInitialState(scene->getCurrentState());
std::map<std::string, ConstraintApproximationFactoryPtr>::const_iterator it = constraint_factories_.find(constr_hard.name);
ConstraintApproximationFactory *fct = NULL;
ConstraintStateStorageOrderFn order;
if (it != constraint_factories_.end())
{
fct = it->second.get();
order = fct->getOrderFunction();
}
ros::WallTime start = ros::WallTime::now();
ompl::base::StateStoragePtr ss = constructConstraintApproximation(pc, constr_sampling, constr_hard, order, samples, edges_per_sample, res);
logInform("Spent %lf seconds constructing the database", (ros::WallTime::now() - start).toSec());
if (ss)
{
ConstraintApproximationPtr ca;
if (fct)
ca = fct->allocApproximation(context_manager_.getRobotModel(), group, state_space_parameterization, constr_hard, group + "_" +
boost::posix_time::to_iso_extended_string(boost::posix_time::microsec_clock::universal_time()) + ".ompldb", ss);
else
ca.reset(new ConstraintApproximation(context_manager_.getRobotModel(), group, state_space_parameterization, constr_hard, group + "_" +
boost::posix_time::to_iso_extended_string(boost::posix_time::microsec_clock::universal_time()) + ".ompldb", ss));
if (ca)
{
if (constraint_approximations_.find(ca->getName()) != constraint_approximations_.end())
logWarn("Overwriting constraint approximation named '%s'", ca->getName().c_str());
constraint_approximations_[ca->getName()] = ca;
res.approx = ca;
}
}
else
logError("Unable to construct constraint approximation for group '%s'", group.c_str());
}
return res;
}
ompl_interface::ConstraintApproximationConstructionResults
ompl_interface::ConstraintsLibrary::addConstraintApproximation(
const moveit_msgs::Constraints& constr_sampling, const moveit_msgs::Constraints& constr_hard,
const std::string& group, const planning_scene::PlanningSceneConstPtr& scene,
const ConstraintApproximationConstructionOptions& options)
{
ConstraintApproximationConstructionResults res;
ModelBasedPlanningContextPtr pc = context_manager_.getPlanningContext(group, options.state_space_parameterization);
if (pc)
{
pc->clear();
pc->setPlanningScene(scene);
pc->setCompleteInitialState(scene->getCurrentState());
ros::WallTime start = ros::WallTime::now();
ompl::base::StateStoragePtr ss = constructConstraintApproximation(pc, constr_sampling, constr_hard, options, res);
ROS_INFO_NAMED("constraints_library", "Spent %lf seconds constructing the database",
(ros::WallTime::now() - start).toSec());
if (ss)
{
ConstraintApproximationPtr ca(new ConstraintApproximation(
group, options.state_space_parameterization, options.explicit_motions, constr_hard,
group + "_" + boost::posix_time::to_iso_extended_string(boost::posix_time::microsec_clock::universal_time()) +
".ompldb",
ss, res.milestones));
if (constraint_approximations_.find(ca->getName()) != constraint_approximations_.end())
ROS_WARN_NAMED("constraints_library", "Overwriting constraint approximation named '%s'", ca->getName().c_str());
constraint_approximations_[ca->getName()] = ca;
res.approx = ca;
}
else
ROS_ERROR_NAMED("constraints_library", "Unable to construct constraint approximation for group '%s'",
group.c_str());
}
return res;
}
