text: 
  
  // OMPL
  #include <ompl/base/OptimizationObjective.h>
  #include <ompl/base/SpaceInformation.h>
  #include <ompl/base/Cost.h>
  #include <ompl/base/spaces/RealVectorStateSpace.h>
  
  
  namespace ob = ompl::base;
  namespace og = ompl::geometric;
  
  
  namespace ob {
  
  
  /** \brief An optimization objective which defines path cost using the idea of mechanical work.  */
  class CostMap2DOptimizationObjective : public OptimizationObjective {
  
  
  
    ob::Cost stateCost(const State *state) const
  
  }
  
  
  
  
  
  }