continualPlanning.h

Go to the documentation of this file.

#ifndef CONTINUAL_PLANNING_H
#define CONTINUAL_PLANNING_H

#include <vector>
#include <ros/ros.h>
#include "continual_planning_executive/symbolicState.h"
#include "continual_planning_executive/stateCreator.h"
#include "continual_planning_executive/goalCreator.h"
#include "continual_planning_executive/plannerInterface.h"
#include "continual_planning_executive/statusPublisher.h"
#include "planExecutor.h"

// constantly do the following:
// estimate current state
// check that curState + curPlan will lead to goal
// if not:
//    replan
// executros
//  check/keep running actions
//  preempt if conflicting? planner always assumes no running actions?
//  should never decide: oops s.th. changed lets redo everything
//  -- make monitoring nice/safe
//
//  Plan monitoring does not work, if move_base/make_plan fails during movement
//  also: Execution failures should trigger replan somehow: i.e. drive to gap -> fail -> replan
//  probably need partial/subgoal plan then to get away and make new situation
//
// check contiplan paper


class ContinualPlanning
{
    public:
        friend bool loadStateCreators(ros::NodeHandle & nh);
        friend bool loadGoalCreators(ros::NodeHandle & nh);
        friend bool loadActionExecutors(ros::NodeHandle & nh);
        friend bool loadPlanner(ros::NodeHandle & nh);
        friend void signal_handler(int);

        enum ContinualPlanningState {
            Running,
            FinishedNoPlanToGoal,
            FinishedAtGoal
        };

        enum ReplanningTriggerMethod {
            ReplanAlways,
            ReplanIfLogicalStateChanged,
            ReplanByMonitoring
        };

        ContinualPlanning();
        ~ContinualPlanning();


        ContinualPlanningState loop();

        bool estimateCurrentState();

        bool isGoalFulfilled() const;


        bool executeActionDirectly(const DurativeAction & a, bool publishStatus);

        void forceReplanning() { _forceReplan = true; }

    protected:

        Plan monitorAndReplan(bool & atGoal);


        bool needReplanning(bool & atGoal) const;

    protected:
        std::vector<continual_planning_executive::StateCreator*> _stateCreators;
        continual_planning_executive::PlannerInterface* _planner;
        PlanExecutor _planExecutor;

        SymbolicState _currentState;
        SymbolicState _goal;
        Plan _currentPlan;

        bool _forceReplan;      
        ReplanningTriggerMethod _replanningTrigger;

        bool _allowDirectGoalCheck;     

        StatusPublisher _status;
};

#endif