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" 37 constexpr
int ParameterTransfer::SOCKET_TIMEOUT_MS;
39 ParameterTransfer::ParameterTransfer(
const char* address,
const char* service)
40 : socket(INVALID_SOCKET) {
42 tabTokenizer.collapse(
false).separators({
"\t"});
44 Networking::initNetworking();
45 addrinfo* addressInfo = Networking::resolveAddress(address, service);
47 socket = Networking::connectTcpSocket(addressInfo);
48 Networking::setSocketTimeout(socket, SOCKET_TIMEOUT_MS);
52 receiverThread = std::make_shared<std::thread>(std::bind(&ParameterTransfer::receiverRoutine,
this));
55 size_t written = send(socket,
"A\n", 2, 0);
57 TransferException ex(
"Error sending GetAllParameter request: " + Networking::getLastErrorString());
61 freeaddrinfo(addressInfo);
64 ParameterTransfer::~ParameterTransfer() {
65 threadRunning =
false;
66 if (receiverThread->joinable()) {
67 receiverThread->join();
70 if(socket != INVALID_SOCKET) {
71 Networking::closeSocket(socket);
75 void ParameterTransfer::waitNetworkReady() {
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) {
86 void ParameterTransfer::readParameter(
unsigned char messageType,
const char*
id,
unsigned char* dest,
int length) {
90 throw TransferException(
"Error caused termination of ParameterTransfer: " + networkErrorString);
93 for (
int i=0; i<length; ++i) { dest[i] =
'\0'; }
101 throw TransferException(
"Error caused termination of ParameterTransfer: " + networkErrorString);
103 if (!paramSet.count(
id)) {
106 blockingCallThisThread([
this, &
id, &value](){
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());
115 auto result = lastSetRequestResult[getThreadId()];
116 if (result.first ==
false) {
128 throw TransferException(
"Error caused termination of ParameterTransfer: " + networkErrorString);
130 if (!paramSet.count(
id)) {
133 blockingCallThisThread([
this, &
id, &value](){
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());
142 auto result = lastSetRequestResult[getThreadId()];
143 if (result.first ==
false) {
151 if (!paramSet.count(
id)) {
159 if (!paramSet.count(
id)) {
167 if (!paramSet.count(
id)) {
187 std::map<std::string, ParameterInfo> compatMap;
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>();
201 if (param.hasIncrement()) {
202 increment = param.getIncrement<
int>();
204 compatMap[name] = ParameterInfo::fromInt(name, writeable, param.getCurrent<
int>(), min, max, increment);
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>();
213 if (param.hasIncrement()) {
214 increment = param.getIncrement<
double>();
216 compatMap[name] = ParameterInfo::fromDouble(name, writeable, param.getCurrent<
double>(), min, max, increment);
219 case param::ParameterValue::TYPE_BOOL: {
220 compatMap[name] = ParameterInfo::fromBool(name, writeable, param.getCurrent<
bool>());
232 void ParameterTransfer::receiverRoutine() {
234 threadRunning =
true;
235 [[maybe_unused]]
int internalThreadId = getThreadId();
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) {
245 networkErrorString = std::string(
"Error receiving network packet: ") + Networking::getLastErrorString();
246 threadRunning =
false;
249 }
else if (bytesReceived == 0) {
251 networkErrorString =
"Connection closed";
252 threadRunning =
false;
255 recvBufBytes += bytesReceived;
257 unsigned int start=0;
258 for (
unsigned int i=0; i<recvBufBytes; ++i) {
259 unsigned char c = recvBuf[i];
261 std::string currentLine((
const char*) recvBuf+start, i-start);
262 auto toks = tabTokenizer.tokenize(currentLine);
264 const std::string& cmd = toks[0];
268 if(atol(toks[1].c_str()) !=
static_cast<unsigned int>(InternalInformation::CURRENT_PARAMETER_PROTOCOL_VERSION)) {
270 networkErrorString = std::string(
"Protocol version mismatch, expected ") + std::to_string(InternalInformation::CURRENT_PARAMETER_PROTOCOL_VERSION) +
" but got " + toks[1];
271 threadRunning =
false;
276 networkErrorString =
"Incomplete transfer of protocol version";
277 threadRunning =
false;
280 }
else if (cmd==
"I") {
282 Parameter param = ParameterSerialization::deserializeParameterFullUpdate(toks);
283 paramSet[param.
getUid()] = param;
284 }
else if (cmd==
"V") {
285 if (toks.size() < 3) {
288 if (paramSet.count(toks[1])) {
290 ParameterSerialization::deserializeParameterValueChange(toks, paramSet[toks[1]]);
292 std::cerr <<
"Parameter not received yet - not updating value of: " << toks[1] << std::endl;;
294 }
else if (cmd==
"R") {
295 if (toks.size() < 4) {
298 std::unique_lock<std::mutex> globalLock(mapMutex);
299 int replyThreadId = atol(toks[1].c_str());
300 if (waitConds.count(replyThreadId)) {
302 std::lock_guard<std::mutex> localLock(waitCondMutexes[replyThreadId]);
303 lastSetRequestResult[replyThreadId] = {toks[2] ==
"1", toks[3]};
304 waitConds[replyThreadId].notify_all();
306 std::cerr <<
"Ignoring unexpected request result for thread " << replyThreadId << std::endl;
308 }
else if (cmd==
"E") {
312 std::lock_guard<std::mutex> readyLock(readyMutex);
313 readyCond.notify_all();
316 networkErrorString = std::string(
"Unknown update command received: ") + cmd;
317 threadRunning =
false;
325 if (start>=recvBufBytes) {
328 std::memmove(recvBuf, recvBuf+start, recvBufBytes-start);
329 recvBufBytes = recvBufBytes-start;
334 int ParameterTransfer::getThreadId() {
336 static std::atomic_int threadCount{0};
337 thread_local
int threadId = threadCount.fetch_add(1);
341 void ParameterTransfer::blockingCallThisThread(std::function<
void()> fn,
int waitMaxMilliseconds) {
342 auto tid = getThreadId();
343 std::unique_lock<std::mutex> globalLock(mapMutex);
345 auto& localWaitCond = waitConds[tid];
346 auto& localWaitCondMutex = waitCondMutexes[tid];
347 std::unique_lock<std::mutex> localLock(localWaitCondMutex);
354 auto status = localWaitCond.wait_for(localLock, std::chrono::milliseconds(waitMaxMilliseconds));
357 waitConds.erase(tid);
358 waitCondMutexes.erase(tid);
361 if (status == std::cv_status::timeout) {
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.
std::string getUid() const
T getCurrent(const std::string &key)
Convenience function for safe bulk parameter access (throws for invalid UIDs). Will return any defaul...
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.
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.