37 #ifndef JSK_FOOTSTEP_PLANNER_FOOTSTEP_STATE_DISCRETE_CLOSE_LIST_H_ 38 #define JSK_FOOTSTEP_PLANNER_FOOTSTEP_STATE_DISCRETE_CLOSE_LIST_H_ 41 #include <boost/tuple/tuple.hpp> 42 #include <boost/tuple/tuple_comparison.hpp> 52 int x_offset,
int y_offset,
int theta_offset,
53 size_t x_num,
size_t y_num,
size_t theta_num);
62 int x = state->indexX();
63 int y = state->indexY();
64 int theta = state->indexT();
72 template <
class Po
intT>
75 for (
size_t xi = 0; xi <
x_num_; xi++) {
76 for (
size_t yi = 0; yi <
y_num_; yi++) {
77 for (
size_t thetai = 0; thetai <
theta_num_; thetai++) {
78 if (
data_[xi][yi][thetai]) {
81 output.points.push_back(p);
97 std::vector<std::vector<std::vector<FootstepState::Ptr> > >
data_;
114 const size_t local_y_num,
115 const size_t local_theta_num);
122 return (x + 1)/ y - 1;
132 int xi,
int yi,
int ti)
134 int kx = keyDivide(xi, local_x_num_);
135 int ky = keyDivide(yi, local_y_num_);
136 int kt = keyDivide(ti, local_theta_num_);
137 return boost::make_tuple(kx, ky, kt);
142 std::map<VolumeKey, FootstepStateDiscreteCloseListLocal::Ptr>::iterator it;
144 for (it = local_volumes_.begin(); it != local_volumes_.end(); ++it) {
145 s += it->second->size();
152 int xi = state->indexX();
153 int yi = state->indexY();
154 int ti = state->indexT();
155 VolumeKey local_volume_key = volumeKey(xi, yi, ti);
156 std::map<VolumeKey, FootstepStateDiscreteCloseListLocal::Ptr>::iterator it
157 = local_volumes_.find(local_volume_key);
158 if (it != local_volumes_.end()) {
159 it->second->add(state);
165 local_volume_key.get<1>() * local_y_num_,
166 local_volume_key.get<2>() * local_theta_num_,
170 local_volumes_[local_volume_key] = new_local;
171 new_local->add(state);
178 int xi = state->indexX();
179 int yi = state->indexY();
180 int ti = state->indexT();
181 VolumeKey key = volumeKey(xi, yi, ti);
182 std::map<VolumeKey, FootstepStateDiscreteCloseListLocal::Ptr>::iterator it
183 = local_volumes_.find(key);
184 if (it != local_volumes_.end()) {
185 return (
bool)it->second->get(xi, yi, ti);
193 template <
class Po
intT>
196 output.points.reserve(
size());
197 for (std::map<VolumeKey, FootstepStateDiscreteCloseListLocal::Ptr>::iterator it
198 = local_volumes_.begin();
199 it != local_volumes_.end();
201 it->second->insertToPointCloud<
PointT>(output);