ArmarDEConverter.cpp
Go to the documentation of this file.
1 #include "ArmarDEConverter.h"
2 
3 #include <cmath>
4 #include <string>
5 
6 #include <SimoxUtility/algorithm/advanced.h>
7 #include <SimoxUtility/algorithm/get_map_keys_values.h>
8 
9 #include "ArmarXCore/core/exceptions/local/ExpressionException.h"
10 
11 #include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
12 #include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
13 #include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
14 #include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
15 
16 #include "ConverterTools.h"
17 
18 namespace armarx::armem::server::robot_state::exteroception
19 {
20 
21  ArmarDEConverter::ArmarDEConverter(const Properties& properties) : properties_(properties)
22  {
23  }
24 
25  ArmarDEConverter::~ArmarDEConverter() = default;
26 
27  aron::data::DictPtr
28  ArmarDEConverter::convert(const RobotUnitDataStreaming::TimeStep& data,
29  const RobotUnitDataStreaming::DataStreamingDescription& description)
30  {
31  arondto::Exteroception dto;
32  dto.iterationID = data.iterationId;
33 
34  // initialize fields
35  const std::size_t singleDimSize = static_cast<std::size_t>(std::sqrt(properties_.tofSize));
36  ARMARX_CHECK_EQUAL(singleDimSize * singleDimSize, properties_.tofSize);
37 
38  // Left
39  // FIXME change if unsigned
40  dto.tof["Left"].depth.resize(singleDimSize, singleDimSize);
41  dto.tof["Left"].depth.setConstant(-1); // in order to unset fields faults
42 
43  // FIXME change if unsigned
44  dto.tof["Left"].sigma.resize(singleDimSize, singleDimSize);
45  dto.tof["Left"].sigma.setConstant(-1); // in order to unset fields faults
46 
47  // FIXME change if unsigned
48  dto.tof["Left"].targetDetected.resize(singleDimSize, singleDimSize);
49  dto.tof["Left"].targetDetected.setConstant(-1); // in order to unset fields faults
50 
51  // Right
52  // FIXME change if unsigned
53  dto.tof["Right"].depth.resize(singleDimSize, singleDimSize);
54  dto.tof["Right"].depth.setConstant(-1); // in order to unset fields faults
55 
56  // FIXME change if unsigned
57  dto.tof["Right"].sigma.resize(singleDimSize, singleDimSize);
58  dto.tof["Right"].sigma.setConstant(-1); // in order to unset fields faults
59 
60  // FIXME change if unsigned
61  dto.tof["Right"].targetDetected.resize(singleDimSize, singleDimSize);
62  dto.tof["Right"].targetDetected.setConstant(-1); // in order to unset fields faults
63 
64  for (const auto& [dataEntryName, dataEntry] : description.entries)
65  {
66  process(dto, dataEntryName, {data, dataEntry});
67  }
68 
69  // resize to square
70  // for (auto& [_, tof] : dto.tof)
71  // {
72  // const int sr = std::sqrt(tof.array.size());
73  // const bool isSquare = (sr * sr == tof.array.size());
74 
75  // if (tof.array.size() > 0 and isSquare)
76  // {
77  // tof.array.resize(sr, sr);
78  // }
79  // }
80 
81 
82  return dto.toAron();
83  }
84 
85  void
86  ArmarDEConverter::process(arondto::Exteroception& dto,
87  const std::string& entryName,
88  const ConverterValue& value)
89  {
90  const std::vector<std::string> split = simox::alg::split(entryName, ".", false, false);
91  ARMARX_CHECK_GREATER_EQUAL(split.size(), 3);
92  const std::set<size_t> acceptedSizes{3, 4, 5};
93  ARMARX_CHECK_GREATER(acceptedSizes.count(split.size()), 0)
94  << "Data entry name could not be parsed (exected 3 or 4 or 5 components between '.'): "
95  << "\n- split: '" << split << "'";
96 
97  const std::string& category = split.at(0);
98  // const std::string& name = split.at(1);
99  const std::string& field = split.at(2);
100  ARMARX_CHECK_EQUAL(category, "sens") << category << " | " << entryName;
101 
102  if (simox::alg::contains(field, "tofDepth"))
103  {
104  // ARMARX_DEBUG << "Processing ToF sensor data";
105  processToFDepthEntry(dto.tof, split, value);
106  }
107 
108  if (simox::alg::contains(field, "tofSigma"))
109  {
110  // ARMARX_DEBUG << "Processing ToF sensor data";
111  processToFSigmaEntry(dto.tof, split, value);
112  }
113 
114  if (simox::alg::contains(field, "tofTargetDetected"))
115  {
116  // ARMARX_DEBUG << "Processing ToF sensor data";
117  processToFTargetDetectedEntry(dto.tof, split, value);
118  }
119  }
120 
121  void
122  ArmarDEConverter::processToFDepthEntry(
123  std::map<std::string, armarx::armem::exteroception::arondto::ToF>& tofs,
124  const std::vector<std::string>& split,
125  const ConverterValue& value)
126  {
127  // split e.g. "sens.LeftHand.tofDepth.element_15" (split at dot)
128 
129  ARMARX_CHECK_EQUAL(split.size(), 4);
130  ARMARX_CHECK_EQUAL(split.at(2), "tofDepth");
131 
132  const std::string& name = split.at(1);
133 
134  // element 0 sens
135  // element 1 is either LeftHand or RightHand
136 
137  const std::map<std::string, std::string> sidePrefixMap{{"LeftHand", "Left"},
138  {"RightHand", "Right"}};
139 
140  auto it = sidePrefixMap.find(name);
141  ARMARX_CHECK(it != sidePrefixMap.end()) << name;
142 
143  const std::string& side = it->second;
144  processToFDepthEntry(tofs[side], split, value);
145  }
146 
147  void
148  ArmarDEConverter::processToFDepthEntry(armarx::armem::exteroception::arondto::ToF& tof,
149  const std::vector<std::string>& split,
150  const ConverterValue& value)
151  {
152  // split, e.g., element_12
153  const std::vector<std::string> elements = simox::alg::split(split.back(), "_");
154  ARMARX_CHECK_EQUAL(elements.size(), 2);
155 
156  const int idx = std::stoi(elements.at(1));
157 
158  ARMARX_CHECK_LESS(idx, tof.depth.size());
159 
160  // we don't allow dynamic ToF size atm
161  // if (tof.array.size() < (idx + 1))
162  // {
163  // tof.array.resize(idx + 1, 1);
164  // }
165 
166  tof.depth(idx) = getValueAs<float>(value);
167  }
168 
169  void
170  ArmarDEConverter::processToFSigmaEntry(
171  std::map<std::string, armarx::armem::exteroception::arondto::ToF>& tofs,
172  const std::vector<std::string>& split,
173  const ConverterValue& value)
174  {
175  // split e.g. "sens.LeftHand.tofDepth.element_15" (split at dot)
176 
177  ARMARX_CHECK_EQUAL(split.size(), 4);
178  ARMARX_CHECK_EQUAL(split.at(2), "tofSigma");
179 
180  const std::string& name = split.at(1);
181 
182  // element 0 sens
183  // element 1 is either LeftHand or RightHand
184 
185  const std::map<std::string, std::string> sidePrefixMap{{"LeftHand", "Left"},
186  {"RightHand", "Right"}};
187 
188  auto it = sidePrefixMap.find(name);
189  ARMARX_CHECK(it != sidePrefixMap.end()) << name;
190 
191  const std::string& side = it->second;
192  processToFSigmaEntry(tofs[side], split, value);
193  }
194 
195  void
196  ArmarDEConverter::processToFSigmaEntry(armarx::armem::exteroception::arondto::ToF& tof,
197  const std::vector<std::string>& split,
198  const ConverterValue& value)
199  {
200  // split, e.g., element_12
201  const std::vector<std::string> elements = simox::alg::split(split.back(), "_");
202  ARMARX_CHECK_EQUAL(elements.size(), 2);
203 
204  const int idx = std::stoi(elements.at(1));
205 
206  ARMARX_CHECK_LESS(idx, tof.sigma.size());
207 
208  // we don't allow dynamic ToF size atm
209  // if (tof.array.size() < (idx + 1))
210  // {
211  // tof.array.resize(idx + 1, 1);
212  // }
213 
214  tof.sigma(idx) = getValueAs<float>(value);
215  }
216 
217  void
218  ArmarDEConverter::processToFTargetDetectedEntry(
219  std::map<std::string, armarx::armem::exteroception::arondto::ToF>& tofs,
220  const std::vector<std::string>& split,
221  const ConverterValue& value)
222  {
223  // split e.g. "sens.LeftHand.tofDepth.element_15" (split at dot)
224 
225  ARMARX_CHECK_EQUAL(split.size(), 4);
226  ARMARX_CHECK_EQUAL(split.at(2), "tofTargetDetected");
227 
228  const std::string& name = split.at(1);
229 
230  // element 0 sens
231  // element 1 is either LeftHand or RightHand
232 
233  const std::map<std::string, std::string> sidePrefixMap{{"LeftHand", "Left"},
234  {"RightHand", "Right"}};
235 
236  auto it = sidePrefixMap.find(name);
237  ARMARX_CHECK(it != sidePrefixMap.end()) << name;
238 
239  const std::string& side = it->second;
240  processToFTargetDetectedEntry(tofs[side], split, value);
241  }
242 
243  void
244  ArmarDEConverter::processToFTargetDetectedEntry(armarx::armem::exteroception::arondto::ToF& tof,
245  const std::vector<std::string>& split,
246  const ConverterValue& value)
247  {
248  // split, e.g., element_12
249  const std::vector<std::string> elements = simox::alg::split(split.back(), "_");
250  ARMARX_CHECK_EQUAL(elements.size(), 2);
251 
252  const int idx = std::stoi(elements.at(1));
253 
254  ARMARX_CHECK_LESS(idx, tof.targetDetected.size());
255 
256  // we don't allow dynamic ToF size atm
257  // if (tof.array.size() < (idx + 1))
258  // {
259  // tof.array.resize(idx + 1, 1);
260  // }
261 
262  tof.targetDetected(idx) = getValueAs<float>(value);
263  }
264 
265 
266 } // namespace armarx::armem::server::robot_state::exteroception
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition: VectorXD.h:662
armarx::armem::server::robot_state::exteroception::detail::Properties
Definition: ArmarDEConverter.h:20
ARMARX_CHECK_GREATER
#define ARMARX_CHECK_GREATER(lhs, rhs)
This macro evaluates whether lhs is greater (>) than rhs and if it turns out to be false it will thro...
Definition: ExpressionException.h:116
ConverterTools.h
ARMARX_CHECK_LESS
#define ARMARX_CHECK_LESS(lhs, rhs)
This macro evaluates whether lhs is less (<) than rhs and if it turns out to be false it will throw a...
Definition: ExpressionException.h:102
ArmarDEConverter.h
armarx::armem::contains
bool contains(const MemoryID &general, const MemoryID &specific)
Indicates whether general is "less specific" than, or equal to, specific, i.e.
Definition: MemoryID.cpp:558
Dict.h
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
armarx::armem::server::robot_state::exteroception::detail::Properties::tofSize
std::size_t tofSize
Definition: ArmarDEConverter.h:22
RobotUnitDataStreamingReceiver.h
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:926
data
uint8_t data[1]
Definition: EtherCATFrame.h:68
armarx::armem::server::robot_state::exteroception
Definition: ArmarDEConverter.cpp:18
armarx::armem::server::robot_state::exteroception::ConverterValue
Definition: ConverterTools.h:20
ARMARX_CHECK_GREATER_EQUAL
#define ARMARX_CHECK_GREATER_EQUAL(lhs, rhs)
This macro evaluates whether lhs is greater or equal (>=) rhs and if it turns out to be false it will...
Definition: ExpressionException.h:123
aron_conversions.h
armarx::armem::server::robot_state::exteroception::ArmarDEConverter::ArmarDEConverter
ArmarDEConverter(const Properties &properties=Properties())
Definition: ArmarDEConverter.cpp:21
ExpressionException.h
armarx::aron::data::DictPtr
std::shared_ptr< Dict > DictPtr
Definition: Dict.h:41
armarx::armem::server::robot_state::exteroception::ArmarDEConverter::convert
aron::data::DictPtr convert(const RobotUnitDataStreaming::TimeStep &data, const RobotUnitDataStreaming::DataStreamingDescription &description) override
Definition: ArmarDEConverter.cpp:28
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition: ExpressionException.h:130
armarx::armem::server::robot_state::exteroception::ArmarDEConverter::~ArmarDEConverter
~ArmarDEConverter() override
armarx::armem::server::robot_state::exteroception::ArmarDEConverter::process
void process(arondto::Exteroception &dtoExteroception, const std::string &entryName, const ConverterValue &value)
Definition: ArmarDEConverter.cpp:86
armarx::split
std::vector< std::string > split(const std::string &source, const std::string &splitBy, bool trimElements=false, bool removeEmptyElements=false)
Definition: StringHelpers.cpp:36