void collision_detection::CollisionRobotFCL::getAttachedBodyObjects(const planning_models::KinematicState::AttachedBody *ab,
std::vector<FCLGeometryConstPtr> &geoms) const
{
const std::vector<shapes::ShapeConstPtr> &shapes = ab->getShapes();
for (std::size_t i = 0 ; i < shapes.size() ; ++i)
{
FCLGeometryConstPtr co = createCollisionGeometry(shapes[i], ab);
if (co)
geoms.push_back(co);
}
}
void collision_detection::CollisionWorldFCL::constructFCLObject(const World::Object *obj, FCLObject &fcl_obj) const
{
for (std::size_t i = 0 ; i < obj->shapes_.size() ; ++i)
{
FCLGeometryConstPtr g = createCollisionGeometry(obj->shapes_[i], obj);
if (g)
{
fcl::CollisionObject *co = new fcl::CollisionObject(g->collision_geometry_, transform2fcl(obj->shape_poses_[i]));
fcl_obj.collision_objects_.push_back(boost::shared_ptr<fcl::CollisionObject>(co));
fcl_obj.collision_geometry_.push_back(g);
}
}
}
void collision_detection::CollisionRobotFCL::updatedPaddingOrScaling(const std::vector<std::string> &links)
{
for (std::size_t i = 0 ; i < links.size() ; ++i)
{
std::map<std::string, std::size_t>::const_iterator it = index_map_.find(links[i]);
const planning_models::KinematicModel::LinkModel *lmodel = kmodel_->getLinkModel(links[i]);
if (it != index_map_.end() && lmodel)
{
FCLGeometryConstPtr g = createCollisionGeometry(lmodel->getShape(), getLinkScale(links[i]), getLinkPadding(links[i]), lmodel);
geoms_[it->second] = g;
}
else
ROS_ERROR("Updating padding or scaling for unknown link: '%s'", links[i].c_str());
}
}
collision_detection::CollisionRobotFCL::CollisionRobotFCL(const planning_models::KinematicModelConstPtr &kmodel, double padding, double scale) : CollisionRobot(kmodel, padding, scale)
{
links_ = kmodel_->getLinkModels();
// we keep the same order of objects as what KinematicState::getLinkState() returns
for (std::size_t i = 0 ; i < links_.size() ; ++i)
if (links_[i] && links_[i]->getShape())
{
FCLGeometryConstPtr g = createCollisionGeometry(links_[i]->getShape(), getLinkScale(links_[i]->getName()), getLinkPadding(links_[i]->getName()), links_[i]);
if (g)
index_map_[links_[i]->getName()] = geoms_.size();
else
links_[i] = NULL;
geoms_.push_back(g);
}
else
{
links_[i] = NULL;
geoms_.push_back(FCLGeometryConstPtr());
}
}
