void planning_models::KinematicModel::clearAllAttachedBodyModels()
{
exclusiveLock();
for(unsigned int i =0; i < link_model_vector_.size(); i++) {
link_model_vector_[i]->clearAttachedBodyModels();
}
exclusiveUnlock();
}
void planning_models::KinematicModel::clearLinkAttachedBodyModels(const std::string& link_name)
{
exclusiveLock();
if(link_model_map_.find(link_name) == link_model_map_.end()) {
exclusiveUnlock();
return;
}
link_model_map_[link_name]->clearAttachedBodyModels();
exclusiveUnlock();
}
void planning_models::KinematicModel::addAttachedBodyModel(const std::string& link_name,
planning_models::KinematicModel::AttachedBodyModel* ab)
{
exclusiveLock();
if(link_model_map_.find(link_name) == link_model_map_.end()) {
ROS_WARN_STREAM("Model has no link named " << link_name << " to attach body to. This is probably going to introduce a memory leak");
exclusiveUnlock();
return;
}
link_model_map_[link_name]->addAttachedBodyModel(ab);
exclusiveUnlock();
}
void planning_models::KinematicModel::replaceAttachedBodyModels(const std::string& link_name,
std::vector<AttachedBodyModel*>& attached_body_vector)
{
exclusiveLock();
if(link_model_map_.find(link_name) == link_model_map_.end())
{
ROS_WARN_STREAM("Model has no link named " << link_name << ". This is probably going to introduce a memory leak");
exclusiveUnlock();
return;
}
link_model_map_[link_name]->replaceAttachedBodyModels(attached_body_vector);
exclusiveUnlock();
}
bool AccessManager::TryAccess(const UString::String& key)
{
std::shared_lock<std::shared_timed_mutex> lock(listMutex);
auto it(std::find(list.begin(), list.end(), key));
if (it == list.end())
{
AccessUpgrader exclusiveLock(lock);
if (std::find(list.begin(), list.end(), key) == list.end())
{
list.push_back(key);
return true;
}
}
return false;
}
