hrplib/hrpPlanner/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__