planner_family.h

Go to the documentation of this file.

namespace or_lemur
{

class FamilyPlanner : public OpenRAVE::PlannerBase
{
public:

   struct SetCache
   {
      std::string name;
      std::string filename;
      std::string set_header; // from start of file, opaque (passed to module)
      std::string roadmap_header; // read from file
      FamilyModule::SetPtr set;
   };

   // stateful data
   // must be consistent between calls to InitPlan()
   // without a call to ResetFamily()
   // things that change per-plan are:
   // - family indicators and their costs (as current set changes)
   // - the planner's problem definition
   // - internal planner data (of course)
   struct CurrentFamily
   {
      // the family module we're using
      boost::weak_ptr<or_lemur::FamilyModule> mod_family;
      // details about the robot
      // TODO: relax same-robot requirement to family-compatibility
      boost::weak_ptr<OpenRAVE::RobotBase> robot;
      std::vector<int> active_dofs;
      // caches (from params), key is filename
      std::map<std::string, SetCache> setcaches;
      // current set
      FamilyModule::SetPtr set_current;
      // current family specification (from family module)
      or_lemur::FamilyModule::Family familyspec;
      std::map<or_lemur::FamilyModule::SetPtr,std::string> familyspec_names;
      // the objects used to interface with ompl
      ompl::base::SpaceInformationPtr ompl_si;
      boost::shared_ptr<ompl_lemur::Family> ompl_family;
      ompl_lemur::FamilyUtilityCheckerPtr ompl_family_checker;
      boost::shared_ptr<
         ompl_lemur::FamilyTagCache<ompl_lemur::LEMUR::VIdxTagMap,ompl_lemur::LEMUR::EIdxTagsMap>
         > ompl_tag_cache;
      boost::shared_ptr<ompl_lemur::LEMUR> ompl_lemur;
      // roadmap header (known once planner initializes)
      std::string roadmap_header;
      
      // put this here for now
      FamilyParametersConstPtr params_last;
   };
   boost::shared_ptr<CurrentFamily> _current_family;
   
   bool _initialized;
   
   FamilyPlanner(OpenRAVE::EnvironmentBasePtr env);
   ~FamilyPlanner();
   
   bool InitPlan(OpenRAVE::RobotBasePtr robot, std::istream & isParameters);
   bool InitPlan(OpenRAVE::RobotBasePtr robot, OpenRAVE::PlannerBase::PlannerParametersConstPtr params);
   OpenRAVE::PlannerBase::PlannerParametersConstPtr GetParameters() const;
   
   OpenRAVE::PlannerStatus PlanPath(OpenRAVE::TrajectoryBasePtr);
   
   bool CmdSaveSetCaches(std::ostream & sout, std::istream & sin);
   
   bool CmdResetFamily(std::ostream & sout, std::istream & sin);
   
   bool CmdGetTimes(std::ostream & sout, std::istream & sin) const;
};

} // namespace or_lemur