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>
46 class FootstepStateDiscreteCloseListLocal
52 int x_offset,
int y_offset,
int theta_offset,
53 size_t x_num,
size_t y_num,
size_t theta_num);
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++) {
81 output.points.push_back(
p);
97 std::vector<std::vector<std::vector<FootstepState::Ptr> > >
data_;
108 class FootstepStateDiscreteCloseList
112 typedef boost::tuple<int, int, int>
VolumeKey;
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)
137 return boost::make_tuple(kx, ky, kt);
142 std::map<VolumeKey, FootstepStateDiscreteCloseListLocal::Ptr>::iterator it;
145 s += it->second->size();
153 int yi =
state->indexY();
156 std::map<VolumeKey, FootstepStateDiscreteCloseListLocal::Ptr>::iterator it
159 it->second->add(state);
171 new_local->add(state);
178 int xi =
state->indexX();
179 int yi =
state->indexY();
180 int ti =
state->indexT();
182 std::map<VolumeKey, FootstepStateDiscreteCloseListLocal::Ptr>::iterator it
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
201 it->second->insertToPointCloud<
PointT>(output);
211 std::map<VolumeKey, FootstepStateDiscreteCloseListLocal::Ptr>
local_volumes_;