Replanner.hpp

/*
 *  Replanner.hpp
 *  libmove3D-planners 
 *
 *  Created by Benjamin Vadant on 01/02/13.
 *  Based on Jim Mainprice replanningAlgorithms.hpp/cpp
 *  Copyright 2013 LAAS/CNRS. All rights reserved.
 *
 */

#ifndef REPLANNER_HPP
#define REPLANNER_HPP

#include "API/Trajectory/trajectory.hpp"
#include "API/ConfigSpace/configuration.hpp"
#include "API/Roadmap/graph.hpp"

#include "HRI_costspace/HRICS_Navigation.hpp"

#include "utils/ConfGenerator.h"

#include "P3d-pkg.h"
#include "LightPlanner-pkg.h"

#include <string>
#include <tr1/memory>

void p3d_deactivate_all_cntrts( Robot* r );

class Replanner 
{   
public:
    Replanner(Robot* r);
    ~Replanner();
    
    void setHuman(Robot* human) { this->human=human; }
    void setSwitchData( confPtr_t qSwitch, int idSwitch, double positionSwitch, double timeLimitRePlanning, double localPathAverageLength, double initialStep );
    
    confPtr_t getQSwitch() { return qSwitch; }
    confPtr_t getQGoal() { return qGoal; }
    API::Trajectory& getCurrentTrajectory() { return currentTrajectory; }
    
    bool isPlanSucceeded() const { return planningSucceeded; }
    bool isPlanningUndergo() const { return isPlanning; }

    virtual bool init() = 0;
    virtual void run() = 0;
    
protected:
    bool initMultiLocalPath();
    p3d_traj* concatenateToCurrentTrajectory(const std::vector<p3d_traj*>& trajs);//DEPRECATED
    std::pair<bool,API::Trajectory> concatenateToCurrentTrajectory(const API::Trajectory& newPortion);
    //std::pair<bool,API::Trajectory> concatenateToCurrentTrajectory(const vector<API::Trajectory> trajectories);
    
    Robot*      robot;
    Robot*      human;
    
    confPtr_t   qGoal;
    API::Trajectory currentTrajectory;

    
    bool        useMultiLocalPath;
    int         baseMultiLocalPath;
    int         baseSoftMotionMultiLocalPath;
    int         headMultiLocalPath;
    int         upBodyMultiLocalPath;
    int         upBodySoftMotionMultiLocalPath;
    
    bool        isPlanning;
    bool        planningSucceeded;
    bool        firstRun;
    int         idRun;
    
    
    //Switch data
    confPtr_t   qSwitch;
    int         idSwitch;
    double      positionSwitch;
    double      timeLimitRePlanning; 
    double      localPathAverageLength;
    double      initialStep;
};

#endif