pnf/Flow.hpp

Go to the documentation of this file.

/* 
 * Copyright (C) 2004
 * Swiss Federal Institute of Technology, Lausanne. All rights reserved.
 * 
 * Author: Roland Philippsen <roland dot philippsen at gmx net>
 *         Autonomous Systems Lab <http://asl.epfl.ch/>
 * 
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
 * USA
 */


#ifndef PNF_FLOW_HPP
#define PNF_FLOW_HPP


#include <estar/util.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/scoped_ptr.hpp>
#include <map>
#include <vector>


namespace estar {
  class Facade;
  class RiskMap;
  class Sprite;
  class Region;
  class Grid;
}


namespace local {
  class Object;
  class Robot;
}


namespace pnf {
  
  
  class BufferZone;
  

  class Flow
  {
  private:
    Flow(ssize_t xsize, ssize_t ysize, double resolution,
         bool perform_convolution, bool alternate_worst_case);
    
    
  public:
    typedef std::pair<const estar::array<double> *, double> array_info_t;
    
    const ssize_t xsize, ysize;
    const double resolution;
    const double half_diagonal;
    const bool perform_convolution;
    const bool alternate_worst_case; // just for (hacked) trials...
    
    static Flow * Create(ssize_t xsize, ssize_t ysize, double resolution,
                         bool perform_convolution, bool alternate_worst_case);
    ~Flow();
    
    bool HaveEnvdist() const;
    void PropagateEnvdist(bool step);
    
    void MapEnvdist();
    
    bool HaveObjdist(size_t id) const;
    void PropagateObjdist(size_t id);
    bool HaveAllObjdist() const;
    void PropagateAllObjdist();
    bool HaveRobdist() const;
    void PropagateRobdist();
    
    void ComputeAllCooc(double static_buffer_factor,
                        double static_buffer_degree);
    void ComputeRisk(const estar::RiskMap & risk_map);
    
    bool HavePNF() const;
    void PropagatePNF();
    
    void AddStaticObject(ssize_t ix, ssize_t iy);
    void RemoveStaticObject(ssize_t ix, ssize_t iy);
    void RemoveDynamicObject(size_t id);
    
    bool AddStaticObject(double x, double y);
    bool RemoveStaticObject(double x, double y);
    bool SetDynamicObject(size_t id, double x, double y, double r, double v);
    bool SetRobot(double x, double y,double r,  double v);
    bool SetGoal(double x, double y, double r);
    
    void DumpEnvdist(FILE * stream) const;
    void DumpRobdist(FILE * stream) const;
    void DumpObjdist(size_t id, FILE * stream) const;
    void DumpObjCooc(size_t id, FILE * stream) const;
    void DumpRisk(FILE * stream) const;
    void DumpPNF(FILE * stream) const;
    
    const estar::Facade * GetObjdist(size_t id) const
    { return GetObjdist(id, 0); }
    
    const estar::Region * GetRegion(size_t id) const;
    
    const estar::Region * GetGoal() const { return m_goal.get(); }
    
    const estar::Region * GetRobot() const;
    
    const estar::Facade * GetRobdist() const;
    
    estar::Facade & GetEnvdist() { return * m_envdist; }
      
    boost::shared_ptr<estar::Grid const> GetEnvdistGrid() const;
      
    const estar::Facade & GetPNF()     const { return * m_pnf; }
    
    array_info_t GetRobotLambda() const;
    
    array_info_t GetObjectLambda(size_t id) const;
    
    array_info_t GetEnvCooc() const;
    
    array_info_t GetObjCooc(size_t id) const;
    
    array_info_t GetDynamicCooc() const;
    
    array_info_t GetRisk() const;
    
    
  private:
    typedef std::map<size_t, boost::shared_ptr<local::Object> > objectmap_t;
    
    boost::scoped_ptr<estar::Facade>  m_envdist;
    boost::scoped_ptr<local::Robot>   m_robot;
    objectmap_t                       m_object;
    boost::scoped_ptr<estar::Facade>  m_pnf;
    boost::scoped_ptr<estar::Region>  m_goal;
    
    boost::scoped_ptr<estar::array<double> > m_env_cooc;
    double m_max_env_cooc;
    boost::scoped_ptr<estar::array<double> > m_dynamic_cooc;
    double m_max_dynamic_cooc;
    boost::scoped_ptr<estar::array<double> > m_risk;
    double m_max_risk;
    
    bool CompIndices(double x, double y, ssize_t & ix, ssize_t & iy) const;
    estar::Facade * GetObjdist(size_t id, FILE * verbose_stream) const;

    bool DoSetRobot(double x, double y, ssize_t ix, ssize_t iy,
                    double r, double v);
    void DoComputeLambda(local::Robot & obj);
    void DoComputeCooc(local::Object & obj);
  };
  
}

#endif // PNF_FLOW_HPP

---

E* Interpolated Graph Replanner Wed Dec 12 18:55:40 2007