RealTimePlanner.h

#ifndef REAL_TIME_PLANNER_H
#define REAL_TIME_PLANNER_H

#include "RobotCSpace.h"
#include "PlannerSettings.h"
#include "PlannerObjective.h"
#include "RampCSpace.h"
#include "Modeling/DynamicPath.h"
#include <KrisLibrary/utils/StatCollector.h>
#include <KrisLibrary/utils/threadutils.h>
#include <KrisLibrary/Timer.h>

class MotionQueueInterface;

class DynamicMotionPlannerBase
{
 public:
  enum { Failure=0, Success=1, Timeout=2 };

  DynamicMotionPlannerBase();
  virtual ~DynamicMotionPlannerBase();
  virtual void Init(CSpace* space,Robot* robot,WorldPlannerSettings* settings);
  virtual void SetGoal(shared_ptr<PlannerObjectiveBase> newgoal);
  virtual void SetTime(Real tstart);
  virtual void SetDefaultLimits();
  virtual void SetLimits(Real qScale=1.0,Real vScale=1.0,Real aScale=1.0);

  bool LogBegin(const char* fn="realtimeplanner.log");
  bool LogEnd();


  virtual int PlanFrom(ParabolicRamp::DynamicPath& path,Real cutoff)
  {
    return Failure;
  }

  int PlanFrom(const Config& qstart,Real cutoff,ParabolicRamp::DynamicPath& result)
  {
    result.ramps.resize(1);
    result.ramps[0].SetConstant(qstart);
    return PlanFrom(result,cutoff);
  }

  bool StopPlanning();

  int Shortcut(ParabolicRamp::DynamicPath& path,Real timeLimit);
  int SmartShortcut(Real tstart,ParabolicRamp::DynamicPath& path,Real timeLimit);

  bool GetMilestoneRamp(const Config& q0,const Vector& dq0,const Config& q1,ParabolicRamp::DynamicPath& ramp) const;
  bool GetMilestoneRamp(const ParabolicRamp::DynamicPath& curPath,const Config& q,ParabolicRamp::DynamicPath& ramp) const;

  //returns true if the ramp from the current config to q is collision free
  bool CheckMilestoneRamp(const ParabolicRamp::DynamicPath& curPath,const Config& q,ParabolicRamp::DynamicPath& ramp) const;

  Real EvaluateDirectPathCost(const ParabolicRamp::DynamicPath& curPath,const Config& q);

  Real EvaluatePathCost(const ParabolicRamp::DynamicPath& path,Real tStart=-1.0);

  Real EvaluateTerminalCost(const Config& q,Real tEnd);

  Robot* robot;
  WorldPlannerSettings* settings;
  CSpace* cspace;
  //objective function
  shared_ptr<PlannerObjectiveBase> goal;
  //configuration, velocity, and acceleration limits
  ParabolicRamp::Vector qMin,qMax,velMax,accMax;

  //planning start time
  Real tstart;
  //flag: another thread may set this to true when the planner should
  //stop during a long planning cycle.  The subclass needs to continually
  //monitor this flag
  bool stopPlanning;

  //log file
  FILE* flog;
};


class SendPathCallbackBase
{
 public:
  virtual ~SendPathCallbackBase() {}
  virtual bool Send(Real tplanstart,Real tcut,const ParabolicRamp::DynamicPath& path) =0;
};


class RealTimePlanner
{
public:
  RealTimePlanner();
  virtual ~RealTimePlanner();

  void SetSpace(SingleRobotCSpace* space);

  void SetConstantPath(const Config& q);
  void SetCurrentPath(Real tglobal,const ParabolicRamp::DynamicPath& path);

  virtual void Reset(shared_ptr<PlannerObjectiveBase> newgoal);

  shared_ptr<PlannerObjectiveBase> Objective() const;

  virtual bool PlanUpdate(Real tglobal,Real& splitTime,Real& planTime);

  bool StopPlanning() { 
    if(!planner) return true;
    return planner->StopPlanning(); 
  }

  virtual void MarkSendFailure();


  shared_ptr<DynamicMotionPlannerBase> planner;

  Real pathStartTime; 
  ParabolicRamp::DynamicPath currentPath;

  shared_ptr<SendPathCallbackBase> sendPathCallback;

  Real cognitiveMultiplier;

  bool acceptTimeOverruns;

  enum SplitUpdateProtocol { Constant, ExponentialBackoff, Learning };
  SplitUpdateProtocol protocol;
  Real currentSplitTime,currentPadding,currentExternalPadding;
  Real maxPadding;

  StatCollector planFailTimeStats,planSuccessTimeStats,planTimeoutTimeStats;
};

class RealTimePlanningThread
{
 public:
  RealTimePlanningThread();
  ~RealTimePlanningThread();

  void SetStartConfig(const Config& qstart);
  void SetCSpace(SingleRobotCSpace* space);
  void SetPlanner(const shared_ptr<DynamicMotionPlannerBase>& planner);
  void SetPlanner(const shared_ptr<RealTimePlanner>& planner);
  void SetObjective(shared_ptr<PlannerObjectiveBase> newgoal);
  PlannerObjectiveBase* GetObjective() const;
  void ResetCurrentPath(Real tglobal,const ParabolicRamp::DynamicPath& path);
  bool Start();
  void Stop();
  bool IsPlanning();
  void PausePlanning();
  void BreakPlanning();
  void StopPlanning();
  void ResumePlanning();
  bool HasUpdate();
  bool SendUpdate(MotionQueueInterface* interface);
  Real ObjectiveValue();

  void* internal;
  shared_ptr<RealTimePlanner> planner;
  Thread thread;
};

#endif