Gazebo Common

API Reference

5.7.0
DataFrame.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2022 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 #ifndef GZ_COMMON_DATAFRAME_HH_
18 #define GZ_COMMON_DATAFRAME_HH_
19 
20 #include <array>
21 #include <numeric>
22 #include <sstream>
23 #include <string>
24 #include <stdexcept>
25 #include <unordered_map>
26 #include <vector>
27 
28 #include <gz/common/CSVStreams.hh>
29 #include <gz/common/IOBase.hh>
30 
32 
33 namespace gz
34 {
35  namespace common
36  {
41  template <typename K, typename V>
42  class DataFrame
43  {
48  public: bool Has(const K &_key) const
49  {
50  return this->storage.count(_key) > 0;
51  }
52 
57  public: V &operator[](const K &_key)
58  {
59  return this->storage[_key];
60  }
61 
66  public: const V &operator[](const K &_key) const
67  {
68  return this->storage.at(_key);
69  }
70 
73  public: const std::vector<K> Keys() const
74  {
75  std::vector<K> keyList;
76  for (auto &[k, _] : this->storage)
77  {
78  keyList.push_back(k);
79  }
80  return keyList;
81  }
82 
84  private: std::unordered_map<K, V> storage;
85  };
86 
94  template <typename K, typename T, typename V, typename P>
95  struct IO<DataFrame<K, math::InMemoryTimeVaryingVolumetricGrid<T, V, P>>>
96  {
111  CSVIStreamIterator _end,
112  const std::string &_timeColumnName,
113  const std::array<std::string, 3> &_spatialColumnNames)
114  {
115  if (_begin == _end)
116  {
117  throw std::invalid_argument("CSV data stream is empty");
118  }
119  const std::vector<std::string> &header = *_begin;
120  if (header.empty())
121  {
122  throw std::invalid_argument("CSV data stream has no header");
123  }
124 
125  auto it = std::find(header.begin(), header.end(), _timeColumnName);
126  if (it == header.end())
127  {
128  std::stringstream sstream;
129  sstream << "CSV data stream has no '"
130  << _timeColumnName << "' column";
131  throw std::invalid_argument(sstream.str());
132  }
133  const size_t timeIndex = it - header.begin();
134 
135  std::array<size_t, 3> spatialColumnIndices;
136  for (size_t i = 0; i < _spatialColumnNames.size(); ++i)
137  {
138  it = std::find(header.begin(), header.end(), _spatialColumnNames[i]);
139  if (it == header.end())
140  {
141  std::stringstream sstream;
142  sstream << "CSV data stream has no '"
143  << _spatialColumnNames[i] << "' column";
144  throw std::invalid_argument(sstream.str());
145  }
146  spatialColumnIndices[i] = it - header.begin();
147  }
148 
149  return ReadFrom(_begin, _end, timeIndex, spatialColumnIndices);
150  }
151 
166  CSVIStreamIterator _end,
167  const size_t &_timeColumnIndex = 0,
168  const std::array<size_t, 3> &_spatialColumnIndices = {1, 2, 3})
169  {
170  if (_begin == _end)
171  {
172  throw std::invalid_argument("CSV data stream is empty");
173  }
174  std::vector<size_t> dataColumnIndices(_begin->size());
175  std::iota(dataColumnIndices.begin(), dataColumnIndices.end(), 0);
176  auto last = dataColumnIndices.end();
177  for (size_t index : {_timeColumnIndex, _spatialColumnIndices[0],
178  _spatialColumnIndices[1], _spatialColumnIndices[2]})
179  {
180  auto it = std::find(dataColumnIndices.begin(), last, index);
181  if (it == last)
182  {
183  std::stringstream sstream;
184  sstream << "Column index " << index << " is"
185  << "out of range for CSV data stream";
186  throw std::invalid_argument(sstream.str());
187  }
188  *it = *(--last);
189  }
190  dataColumnIndices.erase(last, dataColumnIndices.end());
191 
192  using FactoryT =
193  math::InMemoryTimeVaryingVolumetricGridFactory<T, V>;
194  std::vector<FactoryT> factories(dataColumnIndices.size());
195  for (auto it = std::next(_begin); it != _end; ++it)
196  {
197  const T time = IO<T>::ReadFrom(it->at(_timeColumnIndex));
198  const math::Vector3<P> position{
199  IO<P>::ReadFrom(it->at(_spatialColumnIndices[0])),
200  IO<P>::ReadFrom(it->at(_spatialColumnIndices[1])),
201  IO<P>::ReadFrom(it->at(_spatialColumnIndices[2]))};
202 
203  for (size_t i = 0; i < dataColumnIndices.size(); ++i)
204  {
205  const V value = IO<V>::ReadFrom(it->at(dataColumnIndices[i]));
206  factories[i].AddPoint(time, position, value);
207  }
208  }
209 
210  DataFrame<K, math::InMemoryTimeVaryingVolumetricGrid<T, V, P>> df;
211  for (size_t i = 0; i < dataColumnIndices.size(); ++i)
212  {
213  const std::string key = !_begin->empty() ?
214  _begin->at(dataColumnIndices[i]) :
215  "var" + std::to_string(dataColumnIndices[i]);
216  df[IO<K>::ReadFrom(key)] = factories[i].Build();
217  }
218  return df;
219  }
220  };
221  }
222 }
223 #endif