libvisiontransfer  10.0.0
parametertransfer.cpp
1 /*******************************************************************************
2  * Copyright (c) 2022 Nerian Vision GmbH
3  *
4  * Permission is hereby granted, free of charge, to any person obtaining a copy
5  * of this software and associated documentation files (the "Software"), to deal
6  * in the Software without restriction, including without limitation the rights
7  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
8  * copies of the Software, and to permit persons to whom the Software is
9  * furnished to do so, subject to the following conditions:
10  *
11  * The above copyright notice and this permission notice shall be included in
12  * all copies or substantial portions of the Software.
13  *******************************************************************************/
14 
15 #include <iostream>
16 
17 #include "visiontransfer/parametertransfer.h"
18 #include "visiontransfer/exceptions.h"
19 #include "visiontransfer/internalinformation.h"
20 #include "visiontransfer/standardparameterids.h"
21 #include "visiontransfer/parametertransferdata.h"
22 #include "visiontransfer/parameterserialization.h"
23 
24 #include <cstring>
25 #include <string>
26 #include <functional>
27 #include <atomic>
28 
29 using namespace std;
30 using namespace visiontransfer;
31 using namespace visiontransfer::internal;
32 using namespace visiontransfer::param;
33 
34 namespace visiontransfer {
35 namespace internal {
36 
37 constexpr int ParameterTransfer::SOCKET_TIMEOUT_MS;
38 
39 ParameterTransfer::ParameterTransfer(const char* address, const char* service)
40  : socket(INVALID_SOCKET) {
41 
42  tabTokenizer.collapse(false).separators({"\t"});
43 
44  Networking::initNetworking();
45  addrinfo* addressInfo = Networking::resolveAddress(address, service);
46 
47  socket = Networking::connectTcpSocket(addressInfo);
48  Networking::setSocketTimeout(socket, SOCKET_TIMEOUT_MS);
49 
50  networkError = false;
51  pollDelay = 1000;
52  receiverThread = std::make_shared<std::thread>(std::bind(&ParameterTransfer::receiverRoutine, this));
53 
54  // Initial 'GetAll' command
55  size_t written = send(socket, "A\n", 2, 0);
56  if(written != 2) {
57  TransferException ex("Error sending GetAllParameter request: " + Networking::getLastErrorString());
58  throw ex;
59  }
60 
61  freeaddrinfo(addressInfo);
62 }
63 
64 ParameterTransfer::~ParameterTransfer() {
65  threadRunning = false;
66  if (receiverThread->joinable()) {
67  receiverThread->join();
68  }
69 
70  if(socket != INVALID_SOCKET) {
71  Networking::closeSocket(socket);
72  }
73 }
74 
75 void ParameterTransfer::waitNetworkReady() {
76  if (!networkReady) {
77  // Block for network to become ready
78  std::unique_lock<std::mutex> readyLock(readyMutex);
79  auto status = readyCond.wait_for(readyLock, std::chrono::milliseconds(2000));
80  if (status == std::cv_status::timeout) {
81  throw TransferException("Timeout waiting for parameter server ready state");
82  }
83  }
84 }
85 
86 void ParameterTransfer::readParameter(unsigned char messageType, const char* id, unsigned char* dest, int length) {
87  waitNetworkReady();
88  if (networkError) {
89  // collecting deferred error from background thread
90  throw TransferException("Error caused termination of ParameterTransfer: " + networkErrorString);
91  }
92 
93  for (int i=0; i<length; ++i) { dest[i] = '\0'; } // PLACEHOLDER
94 }
95 
96 template<typename T>
97 void ParameterTransfer::writeParameter(const char* id, const T& value) {
98  waitNetworkReady();
99  if (networkError) {
100  // collecting deferred error from background thread
101  throw TransferException("Error caused termination of ParameterTransfer: " + networkErrorString);
102  }
103  if (!paramSet.count(id)) {
104  throw ParameterException("Invalid parameter: " + std::string(id));
105  }
106  blockingCallThisThread([this, &id, &value](){
107  // Emit a set request with our thread id - the receiver thread will unblock us based on that ID
108  std::stringstream ss;
109  ss << "S" << "\t" << getThreadId() << "\t" << id << "\t" << value << "\n";
110  size_t written = send(socket, ss.str().c_str(), (int) ss.str().size(), 0);
111  if(written != ss.str().size()) {
112  throw TransferException("Error sending parameter set request: " + Networking::getLastErrorString());
113  }
114  });
115  auto result = lastSetRequestResult[getThreadId()];
116  if (result.first == false) {
117  // There was a remote error, append its info to the exception
118  throw ParameterException("Remote parameter error: " + result.second);
119  }
120 }
121 
122 // Explicit instantiation for std::string
123 template<>
124 void ParameterTransfer::writeParameter(const char* id, const std::string& value) {
125  waitNetworkReady();
126  if (networkError) {
127  // collecting deferred error from background thread
128  throw TransferException("Error caused termination of ParameterTransfer: " + networkErrorString);
129  }
130  if (!paramSet.count(id)) {
131  throw ParameterException("Invalid parameter: " + std::string(id));
132  }
133  blockingCallThisThread([this, &id, &value](){
134  // Emit a set request with our thread id - the receiver thread will unblock us based on that ID
135  std::stringstream ss;
136  ss << "S" << "\t" << getThreadId() << "\t" << id << "\t" << value << "\n";
137  size_t written = send(socket, ss.str().c_str(), (int) ss.str().size(), 0);
138  if(written != ss.str().size()) {
139  throw TransferException("Error sending parameter set request: " + Networking::getLastErrorString());
140  }
141  });
142  auto result = lastSetRequestResult[getThreadId()];
143  if (result.first == false) {
144  // There was a remote error, append its info to the exception
145  throw ParameterException("Remote parameter error: " + result.second);
146  }
147 }
148 
150  waitNetworkReady();
151  if (!paramSet.count(id)) {
152  throw ParameterException("Invalid parameter: " + std::string(id));
153  }
154  return paramSet[id].getCurrent<int>();
155 }
156 
158  waitNetworkReady();
159  if (!paramSet.count(id)) {
160  throw ParameterException("Invalid parameter: " + std::string(id));
161  }
162  return paramSet[id].getCurrent<double>();
163 }
164 
166  waitNetworkReady();
167  if (!paramSet.count(id)) {
168  throw ParameterException("Invalid parameter: " + std::string(id));
169  }
170  return paramSet[id].getCurrent<bool>();
171 }
172 
173 void ParameterTransfer::writeIntParameter(const char* id, int value) {
174  writeParameter(id, value);
175 }
176 
177 void ParameterTransfer::writeDoubleParameter(const char* id, double value) {
178  writeParameter(id, value);
179 }
180 
181 void ParameterTransfer::writeBoolParameter(const char* id, bool value) {
182  writeParameter(id, value);
183 }
184 
185 std::map<std::string, ParameterInfo> ParameterTransfer::getAllParameters() {
186  waitNetworkReady();
187  std::map<std::string, ParameterInfo> compatMap;
188  {
189  std::unique_lock<std::mutex> globalLock(mapMutex);
190  for (auto kv: paramSet) {
191  auto& name = kv.first;
192  auto& param = kv.second;
193  bool writeable = param.getAccessForApi() == param::Parameter::ACCESS_READWRITE;
194  switch(param.getType()) {
195  case param::ParameterValue::TYPE_INT: {
196  int min = -1, max = -1, increment = -1;
197  if (param.hasRange()) {
198  min = param.getMin<int>();
199  max = param.getMax<int>();
200  }
201  if (param.hasIncrement()) {
202  increment = param.getIncrement<int>();
203  }
204  compatMap[name] = ParameterInfo::fromInt(name, writeable, param.getCurrent<int>(), min, max, increment);
205  break;
206  }
207  case param::ParameterValue::TYPE_DOUBLE: {
208  double min = -1, max = -1, increment = -1;
209  if (param.hasRange()) {
210  min = param.getMin<double>();
211  max = param.getMax<double>();
212  }
213  if (param.hasIncrement()) {
214  increment = param.getIncrement<double>();
215  }
216  compatMap[name] = ParameterInfo::fromDouble(name, writeable, param.getCurrent<double>(), min, max, increment);
217  break;
218  }
219  case param::ParameterValue::TYPE_BOOL: {
220  compatMap[name] = ParameterInfo::fromBool(name, writeable, param.getCurrent<bool>());
221  break;
222  }
223  default:
224  // Omit parameters with other types from legacy compatibility API
225  break;
226  }
227  }
228  }
229  return compatMap;
230 }
231 
232 void ParameterTransfer::receiverRoutine() {
233  recvBufBytes = 0;
234  threadRunning = true;
235  [[maybe_unused]] int internalThreadId = getThreadId(); // we just reserve ID 0 for the receiver
236  while (threadRunning) {
237  int bytesReceived = recv(socket, recvBuf+recvBufBytes, (RECV_BUF_SIZE - recvBufBytes), 0);
238  if (bytesReceived < 0) {
239  auto err = Networking::getErrno();
240  if(err == EAGAIN || err == EWOULDBLOCK) {
241  // No event or reply - no problem
242  continue;
243  } else {
244  networkError = true;
245  networkErrorString = std::string("Error receiving network packet: ") + Networking::getLastErrorString();
246  threadRunning = false;
247  continue;
248  }
249  } else if (bytesReceived == 0) {
250  networkError = true;
251  networkErrorString = "Connection closed";
252  threadRunning = false;
253  continue;
254  } else {
255  recvBufBytes += bytesReceived;
256  }
257  unsigned int start=0;
258  for (unsigned int i=0; i<recvBufBytes; ++i) {
259  unsigned char c = recvBuf[i];
260  if (c=='\n') {
261  std::string currentLine((const char*) recvBuf+start, i-start);
262  auto toks = tabTokenizer.tokenize(currentLine);
263  if (toks.size()>0) {
264  const std::string& cmd = toks[0];
265  if (cmd=="P") {
266  if (toks.size()>1) {
267  // Check of protocol version - old firmwares do not send newline-terminated version, which will just time out waitNetworkReady()
268  if(atol(toks[1].c_str()) != static_cast<unsigned int>(InternalInformation::CURRENT_PARAMETER_PROTOCOL_VERSION)) {
269  networkError = true;
270  networkErrorString = std::string("Protocol version mismatch, expected ") + std::to_string(InternalInformation::CURRENT_PARAMETER_PROTOCOL_VERSION) + " but got " + toks[1];
271  threadRunning = false;
272  break;
273  }
274  } else {
275  networkError = true;
276  networkErrorString = "Incomplete transfer of protocol version";
277  threadRunning = false;
278  break;
279  }
280  } else if (cmd=="I") {
281  // Add or overwrite local parameter
282  Parameter param = ParameterSerialization::deserializeParameterFullUpdate(toks);
283  paramSet[param.getUid()] = param;
284  } else if (cmd=="V") {
285  if (toks.size() < 3) {
286  throw TransferException("Received malformed parameter value update");
287  }
288  if (paramSet.count(toks[1])) {
289  // In-place update
290  ParameterSerialization::deserializeParameterValueChange(toks, paramSet[toks[1]]);
291  } else {
292  std::cerr << "Parameter not received yet - not updating value of: " << toks[1] << std::endl;;
293  }
294  } else if (cmd=="R") {
295  if (toks.size() < 4) {
296  throw TransferException("Received malformed reply for parameter set request");
297  }
298  std::unique_lock<std::mutex> globalLock(mapMutex);
299  int replyThreadId = atol(toks[1].c_str());
300  if (waitConds.count(replyThreadId)) {
301  // Reanimating the waiting thread - it will clean up after itself
302  std::lock_guard<std::mutex> localLock(waitCondMutexes[replyThreadId]);
303  lastSetRequestResult[replyThreadId] = {toks[2] == "1", toks[3]};
304  waitConds[replyThreadId].notify_all();
305  } else {
306  std::cerr << "Ignoring unexpected request result for thread " << replyThreadId << std::endl;
307  }
308  } else if (cmd=="E") {
309  // 'End of Transmission' - at least one full enumeration has arrived - we are ready
310  networkReady = true;
311  // Wake any sleeping threads that were blocked until network became ready
312  std::lock_guard<std::mutex> readyLock(readyMutex);
313  readyCond.notify_all();
314  } else {
315  networkError = true;
316  networkErrorString = std::string("Unknown update command received: ") + cmd;
317  threadRunning = false;
318  break;
319  }
320  }
321  start = i+1;
322  }
323  }
324  // Move any incomplete line to front of recv buffer
325  if (start>=recvBufBytes) {
326  recvBufBytes = 0;
327  } else {
328  std::memmove(recvBuf, recvBuf+start, recvBufBytes-start);
329  recvBufBytes = recvBufBytes-start;
330  }
331  }
332 }
333 
334 int ParameterTransfer::getThreadId() {
335  // Always returns an int type (which may not be the case for std::thread::id)
336  static std::atomic_int threadCount{0};
337  thread_local int threadId = threadCount.fetch_add(1);
338  return threadId;
339 }
340 
341 void ParameterTransfer::blockingCallThisThread(std::function<void()> fn, int waitMaxMilliseconds) {
342  auto tid = getThreadId();
343  std::unique_lock<std::mutex> globalLock(mapMutex);
344  // Populate maps
345  auto& localWaitCond = waitConds[tid];
346  auto& localWaitCondMutex = waitCondMutexes[tid];
347  std::unique_lock<std::mutex> localLock(localWaitCondMutex);
348  // First do the actual handshake setup, like emitting the network message
349  // (The current thread is protected against a reply race at this point)
350  fn();
351  // Allow receiver thread to access its checks (it is still blocked by our specific localLock)
352  globalLock.unlock();
353  // Wait for receiver thread to notify us with the reply
354  auto status = localWaitCond.wait_for(localLock, std::chrono::milliseconds(waitMaxMilliseconds));
355  // Cleanup, so that any spurious network replies can get detected and discarded
356  globalLock.lock();
357  waitConds.erase(tid);
358  waitCondMutexes.erase(tid);
359  globalLock.unlock();
360  // Outcome
361  if (status == std::cv_status::timeout) {
362  TransferException ex("Timeout waiting for request reply from parameter server");
363  throw ex;
364  }
365 }
366 
368  waitNetworkReady();
369  return paramSet;
370 }
371 
372 }} // namespace
373 
void writeIntParameter(const char *id, int value)
Writes an integer value to a parameter of the parameter server.
Exception class that is used for all parameter-related exceptions.
Definition: exceptions.h:41
std::string getUid() const
Definition: parameter.h:72
T getCurrent(const std::string &key)
Convenience function for safe bulk parameter access (throws for invalid UIDs). Will return any defaul...
Definition: parameterset.h:52
void writeParameter(const char *id, const T &value)
Writes a scalar value to a parameter of the parameter server.
bool readBoolParameter(const char *id)
Reads a boolean value from the parameter server.
std::map< std::string, ParameterInfo > getAllParameters()
Enumerates all parameters as reported by the device.
param::ParameterSet & getParameterSet()
Returns a reference to the internal parameter set (once the network handshake is complete) ...
double readDoubleParameter(const char *id)
Reads a double precision floating point value from the parameter server.
Exception class that is used for all transfer exceptions.
Definition: exceptions.h:33
void writeBoolParameter(const char *id, bool value)
Writes a boolean value to a parameter of the parameter server.
int readIntParameter(const char *id)
Reads an integer value from the parameter server.
void writeDoubleParameter(const char *id, double value)
Writes a double precision floating point value to a parameter of the parameter server.
Nerian Vision Technologies