Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef JSK_FOOTSTEP_PLANNER_FOOTSTEP_STATE_DISCRETE_CLOSE_LIST_H_
00038 #define JSK_FOOTSTEP_PLANNER_FOOTSTEP_STATE_DISCRETE_CLOSE_LIST_H_
00039
00040 #include "jsk_footstep_planner/footstep_state.h"
00041 #include <boost/tuple/tuple.hpp>
00042 #include <boost/tuple/tuple_comparison.hpp>
00043
00044 namespace jsk_footstep_planner
00045 {
00046 class FootstepStateDiscreteCloseListLocal
00047 {
00048
00049 public:
00050 typedef boost::shared_ptr<FootstepStateDiscreteCloseListLocal> Ptr;
00051 FootstepStateDiscreteCloseListLocal(
00052 int x_offset, int y_offset, int theta_offset,
00053 size_t x_num, size_t y_num, size_t theta_num);
00054
00055 inline FootstepState::Ptr get(int x, int y, int theta)
00056 {
00057 return data_[x - x_offset_][y - y_offset_][theta - theta_offset_];
00058 }
00059
00060 inline void add(FootstepState::Ptr state)
00061 {
00062 int x = state->indexX();
00063 int y = state->indexY();
00064 int theta = state->indexT();
00065 if (!data_[x - x_offset_][y - y_offset_][theta - theta_offset_]) {
00066 size_++;
00067 }
00068 data_[x - x_offset_][y - y_offset_][theta - theta_offset_] = state;
00069 }
00070
00071
00072 template <class PointT>
00073 void insertToPointCloud(pcl::PointCloud<PointT>& output)
00074 {
00075 for (size_t xi = 0; xi < x_num_; xi++) {
00076 for (size_t yi = 0; yi < y_num_; yi++) {
00077 for (size_t thetai = 0; thetai < theta_num_; thetai++) {
00078 if (data_[xi][yi][thetai]) {
00079 FootstepState::Ptr state = data_[xi][yi][thetai];
00080 PointT p = state->toPoint<PointT>();
00081 output.points.push_back(p);
00082 }
00083 }
00084 }
00085 }
00086 }
00087
00088 inline size_t size() { return size_; }
00089 protected:
00090 size_t size_;
00091 const size_t x_num_;
00092 const size_t y_num_;
00093 const size_t theta_num_;
00094 const int x_offset_;
00095 const int y_offset_;
00096 const int theta_offset_;
00097 std::vector<std::vector<std::vector<FootstepState::Ptr> > > data_;
00098 private:
00099
00100 };
00101
00108 class FootstepStateDiscreteCloseList
00109 {
00110 public:
00111 typedef boost::shared_ptr<FootstepStateDiscreteCloseList> Ptr;
00112 typedef boost::tuple<int, int, int> VolumeKey;
00113 FootstepStateDiscreteCloseList(const size_t local_x_num,
00114 const size_t local_y_num,
00115 const size_t local_theta_num);
00116 inline int keyDivide(int x, int y)
00117 {
00118 if (x < 0) {
00119 return x / y - 1;
00120 }
00121 else {
00122 return x / y;
00123 }
00124 }
00125 inline VolumeKey volumeKey(
00126 int xi, int yi, int ti)
00127 {
00128 int kx = keyDivide(xi, local_x_num_);
00129 int ky = keyDivide(yi, local_y_num_);
00130 int kt = keyDivide(ti, local_theta_num_);
00131 return boost::make_tuple(kx, ky, kt);
00132 }
00133
00134 inline size_t size()
00135 {
00136 std::map<VolumeKey, FootstepStateDiscreteCloseListLocal::Ptr>::iterator it;
00137 size_t s = 0;
00138 for (it = local_volumes_.begin(); it != local_volumes_.end(); ++it) {
00139 s += it->second->size();
00140 }
00141 return s;
00142 }
00143
00144 inline void push_back(FootstepState::Ptr state)
00145 {
00146 int xi = state->indexX();
00147 int yi = state->indexY();
00148 int ti = state->indexT();
00149 VolumeKey local_volume_key = volumeKey(xi, yi, ti);
00150 std::map<VolumeKey, FootstepStateDiscreteCloseListLocal::Ptr>::iterator it
00151 = local_volumes_.find(local_volume_key);
00152 if (it != local_volumes_.end()) {
00153 it->second->add(state);
00154 }
00155 else {
00156
00157 FootstepStateDiscreteCloseListLocal::Ptr new_local(
00158 new FootstepStateDiscreteCloseListLocal(local_volume_key.get<0>() * local_x_num_,
00159 local_volume_key.get<1>() * local_y_num_,
00160 local_volume_key.get<2>() * local_theta_num_,
00161 local_x_num_,
00162 local_y_num_,
00163 local_theta_num_));
00164 local_volumes_[local_volume_key] = new_local;
00165 new_local->add(state);
00166 }
00167 }
00168
00169
00170 inline bool find(FootstepState::Ptr state)
00171 {
00172 int xi = state->indexX();
00173 int yi = state->indexY();
00174 int ti = state->indexT();
00175 VolumeKey key = volumeKey(xi, yi, ti);
00176 std::map<VolumeKey, FootstepStateDiscreteCloseListLocal::Ptr>::iterator it
00177 = local_volumes_.find(key);
00178 if (it != local_volumes_.end()) {
00179 return it->second->get(xi, yi, ti);
00180 }
00181 else {
00182 return false;
00183 }
00184 }
00185
00186
00187 template <class PointT>
00188 void toPointCloud(pcl::PointCloud<PointT>& output)
00189 {
00190 output.points.reserve(size());
00191 for (std::map<VolumeKey, FootstepStateDiscreteCloseListLocal::Ptr>::iterator it
00192 = local_volumes_.begin();
00193 it != local_volumes_.end();
00194 ++it) {
00195 it->second->insertToPointCloud<PointT>(output);
00196 }
00197 }
00198
00199 protected:
00200 const size_t local_x_num_;
00201 const size_t local_y_num_;
00202 const size_t local_theta_num_;
00203
00204
00205 std::map<VolumeKey, FootstepStateDiscreteCloseListLocal::Ptr> local_volumes_;
00206 private:
00207
00208 };
00209 }
00210
00211 #endif