コード例 #1
0
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]);
                }
        }
    }
}
コード例 #2
0
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);
  }
}