grid_perception.h
Go to the documentation of this file.
00001 #ifndef JSK_FOOTSTEP_PLANNER_GRID_PERCEPTION_H_
00002 #define JSK_FOOTSTEP_PLANNER_GRID_PERCEPTION_H_
00003 
00004 #include <pcl/pcl_base.h>
00005 #include <pcl/point_cloud.h>
00006 #include <pcl/point_types.h>
00007 #include <pcl/point_representation.h>
00008 
00009 #include "jsk_footstep_planner/grid_astar_solver.h"
00010 #include "jsk_footstep_planner/grid_graph.h"
00011 
00012 namespace jsk_footstep_planner {
00013 #if 0
00014   class PerceptionGridState : public CostedGridState
00015   {
00016   public:
00017     typedef boost::shared_ptr<PerceptionGridState> Ptr;
00018 
00019     PerceptionGridState(int x, int y) : CostedGridState(x, y), updated_(false)
00020     {
00021     }
00022     inline virtual bool isValid() {
00023       // TODO update for using cost
00024       if(occupancy_ == 0) {
00025         return true;
00026       }
00027       return false;
00028     }
00029   protected:
00030     bool updated_;
00031   };
00032 #endif
00033   class PerceptionGridMap : public GridMap <CostedGridState>
00034   {
00035   public:
00036     typedef boost::shared_ptr< PerceptionGridMap > Ptr;
00037     typedef boost::function<bool(GridState::Ptr)> UpdateCostFunction;
00038 
00039     PerceptionGridMap(int _x, int _y) : GridMap()
00040     {
00041       size_x_ = _x;
00042       size_y_ = _y;
00043       resetGrid();
00044     }
00045 
00046     inline virtual bool checkState(int ix, int iy)
00047     {
00048       StatePtr pt = state_list_.at(index(ix, iy));
00049       if(!pt) {
00050         return false;
00051       }
00052       return true;
00053     }
00054 
00055     inline virtual StatePtr getState(int ix, int iy)
00056     {
00057       StatePtr pt = state_list_.at(index(ix, iy));
00058       if(!pt) {
00059         StatePtr new_pt(new MapState(ix, iy));
00060         state_list_[index(ix, iy)] = new_pt;
00061         updateCost(new_pt);
00062         return new_pt;
00063       }
00064       return pt;
00065     }
00066 
00067     virtual bool updateCost(StatePtr st)
00068     {
00069 #if 0
00070       // update around the state
00071       int ix = st->indexX();
00072       int iy = st->indexY();
00073       for(int y = -1; y < 2; y++) {
00074         for(int x = -1; x < 2; x++) {
00075           if(inRange(ix + x, iy + y)) {
00076             StatePtr pt = state_list_.at(index(ix+x, iy+y));
00077             if(!pt) {
00078               StatePtr new_pt(new MapState(ix+x, iy+y));
00079               state_list_[index(ix, iy)] = new_pt;
00080               cost_func_(new_pt);
00081             }
00082           }
00083         }
00084       }
00085 #endif
00086       return cost_func_(st);
00087     }
00088 
00089     virtual void setCostFunction(UpdateCostFunction func)
00090     {
00091       cost_func_ = func;
00092     }
00093 
00094   protected:
00095     virtual void resetGrid() {
00096       state_list_.clear();
00097       state_list_.resize(size_x_ * size_y_);
00098     }
00099 
00100     UpdateCostFunction cost_func_;
00101   };
00102 
00103   typedef GridGraph<CostedGridState> PerceptionGridGraph;
00104 
00105   // default heuristic function
00106   double gridPerceptionHeuristicDistance(SolverNode<PerceptionGridGraph::State, PerceptionGridGraph>::Ptr node,
00107                                          PerceptionGridGraph::Ptr graph) {
00108 
00109     int ix = node->getState()->indexX();
00110     int iy = node->getState()->indexY();
00111     double gx = graph->getGoalState()->indexX() - ix;
00112     double gy = graph->getGoalState()->indexY() - iy;
00113 
00114     return std::sqrt(gx * gx + gy * gy);
00115   }
00116 }
00117 
00118 #endif


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Fri Apr 19 2019 03:45:28