RigidBodyGeometry.h

/*********************************************************************
* Rice University Software Distribution License
*
* Copyright (c) 2010, Rice University
* All Rights Reserved.
*
* For a full description see the file named LICENSE.
*
*********************************************************************/
 
/* Author: Ioan Sucan */
 
#ifndef OMPLAPP_GEOMETRY_RIGID_BODY_GEOMETRY_
#define OMPLAPP_GEOMETRY_RIGID_BODY_GEOMETRY_
 
#include "omplapp/config.h"
#include "omplapp/geometry/GeometrySpecification.h"
#include <ompl/base/SpaceInformation.h>
#include <ompl/base/spaces/RealVectorBounds.h>
#include <memory>
#include <assimp/scene.h>
#include <assimp/Importer.hpp>
#include <boost/filesystem.hpp>
#include <string>
#include <vector>
 
namespace ompl
{
 
    namespace app
    {
        enum CollisionChecker
            { PQP, FCL };
 
        class RigidBodyGeometry
        {
        public:
 
            explicit
            RigidBodyGeometry(MotionModel mtype, CollisionChecker ctype) : mtype_(mtype), factor_(1.0), add_(0.0), ctype_(ctype)
            {
            }
 
            explicit
            RigidBodyGeometry(MotionModel mtype) : mtype_(mtype), factor_(1.0), add_(0.0), ctype_(FCL)
            {
            }
 
            virtual ~RigidBodyGeometry() = default;
 
            MotionModel getMotionModel() const
            {
                return mtype_;
            }
 
            CollisionChecker getCollisionCheckerType() const
            {
                return ctype_;
            }
 
            bool hasEnvironment() const
            {
                return !importerEnv_.empty();
            }
 
            bool hasRobot() const
            {
                return !importerRobot_.empty();
            }
 
            unsigned int getLoadedRobotCount() const
            {
                return importerRobot_.size();
            }
 
            aiVector3D getRobotCenter(unsigned int robotIndex) const;
 
            virtual bool setEnvironmentMesh(const std::string &env);
 
            virtual bool addEnvironmentMesh(const std::string &env);
 
            virtual bool setRobotMesh(const std::string &robot);
 
            virtual bool addRobotMesh(const std::string &robot);
 
            virtual void setStateValidityCheckerType (CollisionChecker ctype);
 
            const base::StateValidityCheckerPtr& allocStateValidityChecker(const base::SpaceInformationPtr &si, const GeometricStateExtractor &se, bool selfCollision);
 
            const GeometrySpecification& getGeometrySpecification() const;
 
            void setBoundsFactor(double factor)
            {
                factor_ = factor;
            }
 
            double getBoundsFactor() const
            {
                return factor_;
            }
 
            void setBoundsAddition(double add)
            {
                add_ = add;
            }
 
            double getBoundsAddition() const
            {
                return add_;
            }
 
            base::RealVectorBounds inferEnvironmentBounds() const;
 
            void setMeshPath(const std::vector<boost::filesystem::path>& path)
            {
                for (const auto &p : path)
                    if (!boost::filesystem::is_directory(p))
                        OMPL_WARN("Mesh path '%s' is not an existing directory", p.c_str());
                meshPath_ = path;
            }
 
        protected:
            boost::filesystem::path findMeshFile(const std::string& fname);
 
            void computeGeometrySpecification();
 
            MotionModel         mtype_;
 
            double              factor_;
 
            double              add_;
 
            std::vector< std::shared_ptr<Assimp::Importer> > importerEnv_;
 
            std::vector< std::shared_ptr<Assimp::Importer> > importerRobot_;
 
            GeometrySpecification         geom_;
 
            base::StateValidityCheckerPtr validitySvc_;
 
            CollisionChecker              ctype_;
 
            std::vector<boost::filesystem::path> meshPath_{OMPLAPP_RESOURCE_DIR};
 
        };
 
    }
}
 
#endif