footstep_state_discrete_close_list.h
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2015, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 
37 #ifndef JSK_FOOTSTEP_PLANNER_FOOTSTEP_STATE_DISCRETE_CLOSE_LIST_H_
38 #define JSK_FOOTSTEP_PLANNER_FOOTSTEP_STATE_DISCRETE_CLOSE_LIST_H_
39 
41 #include <boost/tuple/tuple.hpp>
42 #include <boost/tuple/tuple_comparison.hpp>
43 
44 namespace jsk_footstep_planner
45 {
47  {
48  // x_offset, x_offset + 1, ...., x_offset + X - 1
49  public:
52  int x_offset, int y_offset, int theta_offset,
53  size_t x_num, size_t y_num, size_t theta_num);
54 
55  inline FootstepState::Ptr get(int x, int y, int theta)
56  {
57  return data_[x - x_offset_][y - y_offset_][theta - theta_offset_];
58  }
59 
60  inline void add(FootstepState::Ptr state)
61  {
62  int x = state->indexX();
63  int y = state->indexY();
64  int theta = state->indexT();
65  if (!data_[x - x_offset_][y - y_offset_][theta - theta_offset_]) {
66  size_++;
67  }
68  data_[x - x_offset_][y - y_offset_][theta - theta_offset_] = state;
69  }
70 
71  // Use push_back to inser points
72  template <class PointT>
73  void insertToPointCloud(pcl::PointCloud<PointT>& output)
74  {
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]) {
79  FootstepState::Ptr state = data_[xi][yi][thetai];
80  PointT p = state->toPoint<PointT>();
81  output.points.push_back(p);
82  }
83  }
84  }
85  }
86  }
87 
88  inline size_t size() { return size_; }
89  protected:
90  size_t size_;
91  const size_t x_num_;
92  const size_t y_num_;
93  const size_t theta_num_;
94  const int x_offset_;
95  const int y_offset_;
96  const int theta_offset_;
97  std::vector<std::vector<std::vector<FootstepState::Ptr> > > data_;
98  private:
99 
100  };
101 
109  {
110  public:
112  typedef boost::tuple<int, int, int> VolumeKey;
113  FootstepStateDiscreteCloseList(const size_t local_x_num,
114  const size_t local_y_num,
115  const size_t local_theta_num);
116  inline int keyDivide(int x, int y)
117  {
118  if (x < 0) {
119  // return -1, while -1 <= x <= -local_num
120  // return -2, while -local_num -1 <= x <= - 2*local_num
121  // ...
122  return (x + 1)/ y - 1;
123  }
124  else {
125  // return 0, while 0 <= x <= local_num -1
126  // return 1, while local_num <= x <= 2*local_num -1
127  // ...
128  return x / y;
129  }
130  }
131  inline VolumeKey volumeKey(
132  int xi, int yi, int ti)
133  {
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);
138  }
139 
140  inline size_t size()
141  {
142  std::map<VolumeKey, FootstepStateDiscreteCloseListLocal::Ptr>::iterator it;
143  size_t s = 0;
144  for (it = local_volumes_.begin(); it != local_volumes_.end(); ++it) {
145  s += it->second->size();
146  }
147  return s;
148  }
149 
150  inline void push_back(FootstepState::Ptr state)
151  {
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()) { // found!
159  it->second->add(state);
160  }
161  else {
162  // add new local volume
164  new FootstepStateDiscreteCloseListLocal(local_volume_key.get<0>() * local_x_num_,
165  local_volume_key.get<1>() * local_y_num_,
166  local_volume_key.get<2>() * local_theta_num_,
167  local_x_num_,
168  local_y_num_,
169  local_theta_num_));
170  local_volumes_[local_volume_key] = new_local;
171  new_local->add(state);
172  }
173  }
174 
175 
176  inline bool find(FootstepState::Ptr state)
177  {
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()) { // found!
185  return it->second->get(xi, yi, ti);
186  }
187  else {
188  return false;
189  }
190  }
191 
192  // This method may be slow
193  template <class PointT>
194  void toPointCloud(pcl::PointCloud<PointT>& output)
195  {
196  output.points.reserve(size());
197  for (std::map<VolumeKey, FootstepStateDiscreteCloseListLocal::Ptr>::iterator it
198  = local_volumes_.begin();
199  it != local_volumes_.end();
200  ++it) {
201  it->second->insertToPointCloud<PointT>(output);
202  }
203  }
204 
205  protected:
206  const size_t local_x_num_;
207  const size_t local_y_num_;
208  const size_t local_theta_num_;
209  // local volume := [int][int][int] -> FootstepState::Ptr
210  // global volume := [int][int][int] -> local volume
211  std::map<VolumeKey, FootstepStateDiscreteCloseListLocal::Ptr> local_volumes_;
212  private:
213 
214  };
215 }
216 
217 #endif
FootstepStateDiscreteCloseListLocal(int x_offset, int y_offset, int theta_offset, size_t x_num, size_t y_num, size_t theta_num)
double y
std::vector< std::vector< std::vector< FootstepState::Ptr > > > data_
std::map< VolumeKey, FootstepStateDiscreteCloseListLocal::Ptr > local_volumes_
state
pcl::PointXYZRGB PointT
FootstepStateDiscreteCloseList is a special clas to use for close list of FootstepState.
boost::shared_ptr< FootstepStateDiscreteCloseListLocal > Ptr
boost::shared_ptr< FootstepStateDiscreteCloseList > Ptr
double x


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Thu Nov 14 2019 03:53:28