ReplanningSimulator.hpp

/*
 *  ReplanningSimulator.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 REPLANNING_SIMULATOR_HPP
#define REPLANNING_SIMULATOR_HPP

#include "Replanner.hpp"

class ReplanningSimulator 
{   
public:
    ReplanningSimulator();
    ~ReplanningSimulator();
    
    p3d_rob* getRobot();
    
    void setMultithreadGraphical(bool enable);
    
    void storeTrajectoryToVect(API::Trajectory& traj, double step);
    void storeExploration(const API::Trajectory& traj, double lPrev, double lNext, std::tr1::shared_ptr<Configuration> qNew);
    void storeTrajectoryToDraw(const API::Trajectory& traj, double step);
    void storeGraphToDraw(const Graph& graph);
    
    int executeSimpleSimulation( int (*fct)(p3d_rob* robot, p3d_localpath* localpathPt) );
    int executeSoftmotionSimulation( int (*fct)(p3d_rob* robot, p3d_localpath* localpathPt) );
    
    double timeSinceLastCall(bool& is_first_call, double& t_init);
    
    void setDrawStep(double step) { drawStep = step; }
    void resetTrajectoriesToDraw() {   currentLine.clear(); deviateLine.clear(); lastLine.clear(); }
    void draw();
    
private:
    bool initSimulator();
    void init(std::string robotName);
    bool initFindRobotBasename( std::string& robotBaseName );
    void initDeactivateAllCntrts( p3d_rob* rob );
    bool initRobotSimulatedCntrtsAndCollisions();
    
    void optimizeCurrentTraj();
    
    void storeTrajectoryToVect(SM_TRAJ& smTraj, double current, double step);
    void storeShortcut(const API::Trajectory& traj, double lPrev, double lNext);
    void storeHumanPosition();
    
    bool setExecutedTrajectoryToCurrent(API::Trajectory& traj);
    bool timeSwitchAndId(double s, double s_rep, int& id_switch, API::Trajectory& traj, double &s_switch);
    
    //----------------------------------------------------
    bool initDone;
    
    Replanner* replanner;
    
    int switchId;
    
    Robot* robot;
    Robot* robotSimulated;
    Robot* robotCylinder;
    Robot* human;
    
    confPtr_t qEnd;
    
    API::Trajectory executedTraj;
    
    bool isWritingDisplay;
    bool isReadingDisplay;
    bool drawFinalConfig;
    
    double drawStep;
    
    std::vector< std::vector<double> > currentLine;
    std::vector< std::vector<double> > lastLine;
    std::vector< std::vector<double> > deviateLine;
};

extern ReplanningSimulator* globalRePlanningEnv;
extern Graph* globalRePlanningGraph;

extern SM_TRAJ globalManipPlannerLastTraj;

#endif