void collision_detection::CollisionRobotFCL::constructFCLObject(const planning_models::KinematicState &state, FCLObject &fcl_obj) const
{
const std::vector<planning_models::KinematicState::LinkState*> &link_states = state.getLinkStateVector();
fcl_obj.collision_objects_.reserve(geoms_.size());
for (std::size_t i = 0 ; i < geoms_.size() ; ++i)
{
if (geoms_[i] && geoms_[i]->collision_geometry_)
{
fcl::CollisionObject *collObj = new fcl::CollisionObject(geoms_[i]->collision_geometry_, transform2fcl(link_states[i]->getGlobalCollisionBodyTransform()));
fcl_obj.collision_objects_.push_back(boost::shared_ptr<fcl::CollisionObject>(collObj));
// the CollisionGeometryData is already stored in the class member geoms_, so we need not copy it
}
std::vector<const planning_models::KinematicState::AttachedBody*> ab;
link_states[i]->getAttachedBodies(ab);
for (std::size_t j = 0 ; j < ab.size() ; ++j)
{
std::vector<FCLGeometryConstPtr> objs;
getAttachedBodyObjects(ab[j], objs);
const EigenSTL::vector_Affine3d &ab_t = ab[j]->getGlobalCollisionBodyTransforms();
for (std::size_t k = 0 ; k < objs.size() ; ++k)
if (objs[k]->collision_geometry_)
{
fcl::CollisionObject *collObj = new fcl::CollisionObject(objs[k]->collision_geometry_, transform2fcl(ab_t[k]));
fcl_obj.collision_objects_.push_back(boost::shared_ptr<fcl::CollisionObject>(collObj));
// we copy the shared ptr to the CollisionGeometryData, as this is not stored by the class itself,
// and would be destroyed when objs goes out of scope.
fcl_obj.collision_geometry_.push_back(objs[k]);
}
}
}
}
void planning_environment::getAllKinematicStateStampedTransforms(const planning_models::KinematicState& state,
std::vector<geometry_msgs::TransformStamped>& trans_vector,
const ros::Time& stamp)
{
trans_vector.clear();
for(unsigned int i = 0; i < state.getLinkStateVector().size(); i++) {
const planning_models::KinematicState::LinkState* ls = state.getLinkStateVector()[i];
geometry_msgs::TransformStamped ts;
ts.header.stamp = stamp;
ts.header.frame_id = state.getKinematicModel()->getRoot()->getParentFrameId();
ts.child_frame_id = ls->getName();
if(ts.header.frame_id == ts.child_frame_id) continue;
tf::transformTFToMsg(ls->getGlobalLinkTransform(),ts.transform);
trans_vector.push_back(ts);
}
}