PathPlanner.h

Go to the documentation of this file.

//-*- C++ -*-

#ifndef __PATH_PLANNER_H__
#define __PATH_PLANNER_H__

#include <string>
#include <map>
#include <iostream>
#include <sstream>
#include <boost/function.hpp>
#include "hrpUtil/TimeMeasure.h"

#include "exportdef.h"
#include "Algorithm.h"
#include "Configuration.h"
#include "ConfigurationSpace.h"
#include "Mobility.h"
#include "Optimizer.h"
#include "CollisionDetector.h"
#include "hrpCollision/ColdetModelPair.h"
#undef random

#include <hrpCorba/ORBwrap.h>
#include <hrpCorba/ModelLoader.hh>
#include <hrpCorba/CollisionDetector.hh>
#include <hrpCorba/OnlineViewer.hh>

#include <hrpModel/World.h>
#include <hrpModel/ConstraintForceSolver.h>
#include <hrpUtil/TimeMeasure.h>

namespace PathEngine {
    class Algorithm;
    class Mobility;
    class PathPlanner;

    typedef boost::function2<bool, PathPlanner *, const Configuration &> applyConfigFunc;
    typedef boost::shared_ptr<hrp::World<hrp::ConstraintForceSolver> > WorldPtr;
    class HRPPLANNER_API PathPlanner {

    private:
        applyConfigFunc m_applyConfigFunc;
        typedef std::map<const std::string, std::pair<AlgorithmNewFunc, AlgorithmDeleteFunc> > AlgorithmFactory;
        typedef AlgorithmFactory::value_type AlgorithmFactoryValueType;
        AlgorithmFactory algorithmFactory_;

        typedef std::map<std::string, std::pair<MobilityNewFunc, MobilityDeleteFunc> > MobilityFactory;
        typedef MobilityFactory::value_type MobilityFactoryValueType;
        MobilityFactory mobilityFactory_;

        typedef std::map<std::string, std::pair<OptimizerNewFunc, OptimizerDeleteFunc> > OptimizerFactory;
        typedef OptimizerFactory::value_type OptimizerFactoryValueType;
        OptimizerFactory optimizerFactory_;

        OpenHRP::ModelLoader_var modelLoader_;

        OpenHRP::OnlineViewer_var onlineViewer_;

        std::string algorithmName_;

        Algorithm* algorithm_;

        std::string mobilityName_;

        Mobility* mobility_;

        ConfigurationSpace cspace_;

        hrp::BodyPtr model_;

        bool debug_;

        double dt_;

        template<typename X, typename X_ptr>
        X_ptr checkCorbaServer(const std::string &n, CosNaming::NamingContext_var &cxt);

        std::vector<Configuration> path_;

        unsigned int countCollisionCheck_;

        TimeMeasure timeCollisionCheck_, timeForwardKinematics_;

        CORBA::ORB_var orb_;

        WorldPtr world_;

        OpenHRP::CollisionDetector_var collisionDetector_;
        OpenHRP::CharacterPositionSequence_var allCharacterPositions_;

        void _setupCharacterData();
        void _updateCharacterPositions();

        bool bboxMode_;

        std::vector<hrp::ColdetModelPair> checkPairs_;
        //< point cloud created by vision or range sensor
        std::vector<hrp::Vector3> pointCloud_; 
        double radius_; 

        std::pair<std::string, std::string> collidingPair_;

        CollisionDetector *customCollisionDetector_;

        bool defaultCheckCollision();
    public:
        WorldPtr world();

        hrp::BodyPtr robot();

        void setApplyConfigFunc(applyConfigFunc i_func);

        bool setConfiguration(const Configuration &pos);

        void getWorldState(OpenHRP::WorldState_out wstate);

        void setPointCloud(const std::vector<hrp::Vector3>& i_cloud, double i_radius);

        PathPlanner(unsigned int dim, 
                    WorldPtr world = WorldPtr(),
                    bool isDebugMode = false);

        ~PathPlanner();

        void registerAlgorithm(const std::string &algorithmName, AlgorithmNewFunc newFunc, AlgorithmDeleteFunc deleteFunc);

        void registerMobility(const std::string &mobilityName, MobilityNewFunc newFunc, MobilityDeleteFunc deleteFunc);
    
        void registerOptimizer(const std::string &optimizerName, OptimizerNewFunc newFunc, OptimizerDeleteFunc deleteFunc);
    
        void initPlanner(const std::string &nameServer);

        hrp::BodyPtr registerCharacter(const char* name,OpenHRP::BodyInfo_ptr cInfo);

        hrp::BodyPtr registerCharacter(const char *name, hrp::BodyPtr i_body);

        hrp::BodyPtr registerCharacterByURL(const char* name, const char* url);

        void setCharacterPosition(const char* character, 
                                  const OpenHRP::DblSequence& pos);

        void registerIntersectionCheckPair(const char* char1, 
                                           const char* name1, 
                                           const char* char2,
                                           const char* name2,
                                           CORBA::Double tolerance);


        void initSimulation();

        void getMobilityNames(std::vector<std::string> &mobilitys);

        void getAlgorithmNames(std::vector<std::string> &algorithms);

        void getOptimizerNames(std::vector<std::string> &optimizers);

        bool getProperties(const std::string &algorithm,
                           std::vector<std::string> &names,
                           std::vector<std::string> &values);
  
        void setRobotName(const std::string &model);

      
        void setAlgorithmName(const std::string &algorithm);

      
        bool setMobilityName(const std::string &mobility);

        void setProperties(const std::map<std::string, std::string> &properties) {algorithm_->setProperties(properties);}
        void setStartConfiguration(const Configuration &pos) {algorithm_->setStartConfiguration(pos);}

        void setGoalConfiguration(const Configuration &pos) {algorithm_->setGoalConfiguration(pos);}

        bool calcPath();

        void stopPlanning() {algorithm_->stopPlanning();}
    
        RoadmapPtr getRoadmap() { return algorithm_->getRoadmap();}

        std::vector<Configuration> getPath();

        std::vector<Configuration>& getWayPoints();

        Mobility* getMobility() {return mobility_;}

        Algorithm* getAlgorithm() {return algorithm_;}

        ConfigurationSpace* getConfigurationSpace() { return &cspace_; }

        bool checkCollision();
        bool checkCollision(const Configuration &pos);

        bool checkCollision(const std::vector<Configuration> &path);

        void setDebug(bool debug) {debug_ = debug;}

        bool optimize(const std::string& optimizer);

        unsigned int countCollisionCheck() const { return timeCollisionCheck_.numCalls();}

        double timeCollisionCheck() const;

        double timeForwardKinematics() const;

        void boundingBoxMode(bool mode) { bboxMode_ = mode; } 

        const std::pair<std::string, std::string> &collidingPair() { return collidingPair_; }

        void setCollisionDetector(CollisionDetector *i_cd){
            customCollisionDetector_ = i_cd; 
        }
    };
};
#endif // __PATH_PLANNER_H__