15 #include "visiontransfer/parametertransfer.h" 16 #include "visiontransfer/exceptions.h" 17 #include "visiontransfer/internalinformation.h" 18 #include "visiontransfer/standardparameterids.h" 19 #include "visiontransfer/parametertransferdata.h" 30 constexpr
int ParameterTransfer::SOCKET_TIMEOUT_MS;
32 ParameterTransfer::ParameterTransfer(
const char* address,
const char* service)
33 : socket(INVALID_SOCKET) {
35 Networking::initNetworking();
36 addrinfo* addressInfo = Networking::resolveAddress(address, service);
38 socket = Networking::connectTcpSocket(addressInfo);
39 Networking::setSocketTimeout(socket, SOCKET_TIMEOUT_MS);
40 checkProtocolVersion();
42 freeaddrinfo(addressInfo);
45 ParameterTransfer::~ParameterTransfer() {
46 if(socket != INVALID_SOCKET) {
47 Networking::closeSocket(socket);
51 std::map<std::string, ParameterInfo> ParameterTransfer::recvEnumeration() {
52 std::map<std::string, ParameterInfo> pi;
53 const size_t bufsize = 4096;
57 int bytesReceived = recv(socket, recv_buf, 4, 0);
58 if(bytesReceived < 0) {
59 TransferException ex(
"Error receiving network packet: " +
string(strerror(errno)));
61 }
else if (bytesReceived == 0) {
64 }
else if (bytesReceived < 4) {
71 uint32_t num_params = ntohl(reinterpret_cast<uint32_t*>(buf)[0]);
74 if (expected_remaining_size > bufsize - 4) {
75 TransferException ex(
"Remote parameter enumeration exceeds expected maximum size");
78 while (expected_remaining_size > 0) {
79 bytesReceived = recv(socket, recv_buf, expected_remaining_size, 0);
80 if (bytesReceived < 0) {
81 TransferException ex(
"Error receiving network packet: " +
string(strerror(errno)));
83 }
else if (bytesReceived == 0) {
87 expected_remaining_size -= bytesReceived;
88 recv_buf += bytesReceived;
93 for (
unsigned int i = 0; i < num_params; ++i) {
94 StandardParameterIDs::ParameterID
id = (StandardParameterIDs::ParameterID) ntohl(tpi->id);
95 ParameterInfo::ParameterType type = (ParameterInfo::ParameterType) ntohl(tpi->type);
96 bool writeable = ntohl(tpi->flags & StandardParameterIDs::ParameterFlags::PARAMETER_WRITEABLE) != 0;
98 auto nameIt = internal::StandardParameterIDs::parameterNameByID.find(
id);
99 if (nameIt == StandardParameterIDs::parameterNameByID.end()) {
100 ParameterException ex(
"Enumeration contained a ParameterID for which no name is known: "+std::to_string(
id));
104 case ParameterInfo::TYPE_INT: {
105 pi[nameIt->second] = visiontransfer::ParameterInfo::fromInt(nameIt->second, writeable,
106 ntohl(tpi->value.intVal), ntohl(tpi->min.intVal), ntohl(tpi->max.intVal), ntohl(tpi->inc.intVal)
110 case ParameterInfo::TYPE_BOOL: {
111 pi[nameIt->second] = visiontransfer::ParameterInfo::fromBool(nameIt->second, writeable, ntohl(tpi->value.boolVal) != 0);
114 case ParameterInfo::TYPE_DOUBLE: {
115 pi[nameIt->second] = visiontransfer::ParameterInfo::fromDouble(nameIt->second, writeable,
116 tpi->value.doubleVal, tpi->min.doubleVal, tpi->max.doubleVal, tpi->inc.doubleVal
128 void ParameterTransfer::recvData(
unsigned char* dest,
int length) {
129 int bytesReceived = recv(socket, reinterpret_cast<char*>(dest), length, 0);
130 if(bytesReceived < 0) {
131 TransferException ex(
"Error receiving network packet: " +
string(strerror(errno)));
133 }
else if(bytesReceived < length) {
138 void ParameterTransfer::checkProtocolVersion() {
139 unsigned int version = 0;
140 recvData(reinterpret_cast<unsigned char*>(&version),
sizeof(version));
142 if(ntohl(version) != static_cast<unsigned int>(InternalInformation::CURRENT_PROTOCOL_VERSION)) {
144 + std::to_string(InternalInformation::CURRENT_PROTOCOL_VERSION)
145 +
" but received " + std::to_string(ntohl(version)));
149 void ParameterTransfer::readParameter(
unsigned char messageType, int32_t
id,
unsigned char* dest,
int length) {
154 unsigned int networkId = htonl(
id);
155 unsigned char messageBuf[13];
156 memset(messageBuf, 0,
sizeof(messageBuf));
158 messageBuf[0] = messageType;
159 memcpy(&messageBuf[1], &networkId, 4);
161 int written = send(socket, reinterpret_cast<char*>(messageBuf),
sizeof(messageBuf), 0);
162 if(written !=
sizeof(messageBuf)) {
163 TransferException ex(
"Error sending parameter read request: " +
string(strerror(errno)));
167 unsigned char replyBuf[8];
168 recvData(replyBuf,
sizeof(replyBuf));
169 memcpy(dest, replyBuf, length);
173 void ParameterTransfer::writeParameter(
unsigned char messageType, int32_t
id, T value) {
174 static_assert(
sizeof(T) <= 8,
"Parameter type musst be smaller or equal to 8 bytes");
176 unsigned int networkId = htonl(
id);
177 unsigned char messageBuf[13];
179 memset(messageBuf, 0,
sizeof(messageBuf));
180 messageBuf[0] = messageType;
181 memcpy(&messageBuf[1], &networkId, 4);
182 memcpy(&messageBuf[5], &value,
sizeof(value));
184 int written = send(socket, reinterpret_cast<char*>(messageBuf),
sizeof(messageBuf), 0);
185 if(written !=
sizeof(messageBuf)) {
186 TransferException ex(
"Error sending parameter write request: " +
string(strerror(errno)));
190 unsigned char replyBuf[8];
191 recvData(replyBuf,
sizeof(replyBuf));
193 if(replyBuf[0] == 0 && replyBuf[1] == 0 && replyBuf[2] == 0 && replyBuf[3] == 0) {
200 readParameter(MESSAGE_READ_INT,
id, reinterpret_cast<unsigned char*>(&data),
sizeof(data));
201 return static_cast<int>(ntohl(data));
206 readParameter(MESSAGE_READ_DOUBLE,
id, reinterpret_cast<unsigned char*>(&data),
sizeof(data));
212 readParameter(MESSAGE_READ_BOOL,
id, reinterpret_cast<unsigned char*>(&data),
sizeof(data));
217 writeParameter(MESSAGE_WRITE_INT,
id, htonl(static_cast<uint32_t>(value)));
221 writeParameter(MESSAGE_WRITE_DOUBLE,
id, value);
225 writeParameter(MESSAGE_WRITE_BOOL,
id, htonl(static_cast<uint32_t>(value)));
229 unsigned char messageBuf[13];
230 memset(messageBuf, 0,
sizeof(messageBuf));
231 messageBuf[0] = MESSAGE_ENUMERATE_PARAMS;
233 int written = send(socket, reinterpret_cast<char*>(messageBuf),
sizeof(messageBuf), 0);
234 if(written !=
sizeof(messageBuf)) {
235 TransferException ex(
"Error sending parameter enumeration request: " +
string(strerror(errno)));
238 auto enumeration = recvEnumeration();
bool readBoolParameter(int32_t id)
Reads a boolean value from the parameter server.
int readIntParameter(int32_t id)
Reads an integer value from the parameter server.
void writeBoolParameter(int32_t id, int32_t value)
Writes a boolean value to a parameter of the parameter server.
Exception class that is used for all parameter-related exceptions.
void writeIntParameter(int32_t id, int32_t value)
Writes an integer value to a parameter of the parameter server.
void writeDoubleParameter(int32_t id, double value)
Writes a double precision floating point value to a parameter of the parameter server.
std::map< std::string, ParameterInfo > getAllParameters()
Enumerates all parameters as reported by the device.
double readDoubleParameter(int32_t id)
Reads a double precision floating point value from the parameter server.
Exception class that is used for all transfer exceptions.