void ompl_interface::ConstraintsLibrary::loadConstraintApproximations(const std::string &path)
{
constraint_approximations_.clear();
std::ifstream fin((path + "/manifest").c_str());
if (!fin.good())
{
logDebug("Manifest not found in folder '%s'. Not loading constraint approximations.", path.c_str());
return;
}
logInform("Loading constrained space approximations from '%s'", path.c_str());
while (fin.good() && !fin.eof())
{
std::string group, state_space_parameterization, serialization, filename;
fin >> group;
if (fin.eof())
break;
fin >> state_space_parameterization;
if (fin.eof())
break;
fin >> serialization;
if (fin.eof())
break;
fin >> filename;
const ModelBasedPlanningContextPtr &pc = context_manager_.getPlanningContext(group, state_space_parameterization);
if (pc)
{
moveit_msgs::Constraints msg;
hexToMsg(serialization, msg);
ConstraintApproximationStateStorage *cass = new ConstraintApproximationStateStorage(pc->getOMPLSimpleSetup().getStateSpace());
cass->load((path + "/" + filename).c_str());
ConstraintApproximationPtr cap;
if (constraint_factories_.find(msg.name) != constraint_factories_.end())
cap = constraint_factories_[msg.name]->allocApproximation(context_manager_.getRobotModel(),
group, state_space_parameterization, msg, filename, ompl::base::StateStoragePtr(cass));
else
cap.reset(new ConstraintApproximation(context_manager_.getRobotModel(),
group, state_space_parameterization, msg, filename, ompl::base::StateStoragePtr(cass)));
if (cap)
{
if (constraint_approximations_.find(cap->getName()) != constraint_approximations_.end())
logWarn("Overwriting constraint approximation named '%s'", cap->getName().c_str());
constraint_approximations_[cap->getName()] = cap;
std::size_t sum = 0;
for (std::size_t i = 0 ; i < cass->size() ; ++i)
sum += cass->getMetadata(i).size();
logInform("Loaded %lu states and %lu connections (%0.1lf per state) from %s", cass->size(), sum, (double)sum / (double)cass->size(), filename.c_str());
}
}
}
}
void ompl_interface::ConstraintsLibrary::loadConstraintApproximations(const std::string& path)
{
constraint_approximations_.clear();
std::ifstream fin((path + "/manifest").c_str());
if (!fin.good())
{
ROS_WARN_NAMED("constraints_library", "Manifest not found in folder '%s'. Not loading constraint approximations.",
path.c_str());
return;
}
ROS_INFO_NAMED("constraints_library", "Loading constrained space approximations from '%s'...", path.c_str());
while (fin.good() && !fin.eof())
{
std::string group, state_space_parameterization, serialization, filename;
bool explicit_motions;
unsigned int milestones;
fin >> group;
if (fin.eof())
break;
fin >> state_space_parameterization;
if (fin.eof())
break;
fin >> explicit_motions;
if (fin.eof())
break;
fin >> milestones;
if (fin.eof())
break;
fin >> serialization;
if (fin.eof())
break;
fin >> filename;
ROS_INFO_NAMED("constraints_library", "Loading constraint approximation of type '%s' for group '%s' from '%s'...",
state_space_parameterization.c_str(), group.c_str(), filename.c_str());
const ModelBasedPlanningContextPtr& pc = context_manager_.getPlanningContext(group, state_space_parameterization);
if (pc)
{
moveit_msgs::Constraints msg;
hexToMsg(serialization, msg);
auto* cass = new ConstraintApproximationStateStorage(pc->getOMPLSimpleSetup()->getStateSpace());
cass->load((path + "/" + filename).c_str());
ConstraintApproximationPtr cap(new ConstraintApproximation(group, state_space_parameterization, explicit_motions,
msg, filename, ompl::base::StateStoragePtr(cass),
milestones));
if (constraint_approximations_.find(cap->getName()) != constraint_approximations_.end())
ROS_WARN_NAMED("constraints_library", "Overwriting constraint approximation named '%s'",
cap->getName().c_str());
constraint_approximations_[cap->getName()] = cap;
std::size_t sum = 0;
for (std::size_t i = 0; i < cass->size(); ++i)
sum += cass->getMetadata(i).first.size();
ROS_INFO_NAMED("constraints_library", "Loaded %lu states (%lu milestones) and %lu connections (%0.1lf per state) "
"for constraint named '%s'%s",
cass->size(), cap->getMilestoneCount(), sum, (double)sum / (double)cap->getMilestoneCount(),
msg.name.c_str(), explicit_motions ? ". Explicit motions included." : "");
}
}
ROS_INFO_NAMED("constraints_library", "Done loading constrained space approximations.");
}
