libvisiontransfer  8.3.0
datachannelservice.cpp
1 #include <sys/types.h>
2 #include <cstring>
3 #include <stdexcept>
4 #include <fcntl.h>
5 #include <fstream>
6 
7 #include <visiontransfer/internalinformation.h>
8 #include <visiontransfer/networking.h>
9 #include <visiontransfer/datachannelservicebase.h>
10 #include <visiontransfer/datachannel-control.h>
11 #include <visiontransfer/datachannelservice.h>
12 
13 #include <visiontransfer/datachannel-imu-bno080.h>
14 #include <visiontransfer/protocol-sh2-imu-bno080.h> // for sensor constants
15 
16 #include <iostream>
17 
18 #include <memory>
19 #include <functional>
20 #include <thread>
21 #include <mutex>
22 #include <chrono>
23 
24 #ifdef _WIN32
25 #include <ws2tcpip.h>
26 #endif
27 
28 using namespace visiontransfer;
29 using namespace visiontransfer::internal;
30 
31 namespace visiontransfer {
32 
34 private:
35  sockaddr_in serverAddr;
36  //
37  std::shared_ptr<std::thread> receiverThread;
38  unsigned long pollDelay;
39  //
40  std::shared_ptr<ClientSideDataChannelIMUBNO080> channelBNO080;
41  //
42  int handleChannel0Message(DataChannelMessage& message, sockaddr_in* sender) override;
43  void initiateHandshake();
44  void subscribeAll();
45  void unsubscribeAll();
46  void receiverRoutine();
47 public:
48  bool threadRunning;
49  std::vector<DataChannelInfo> channelsAvailable;
50  std::map<DataChannel::Type, std::set<DataChannel::ID>> channelsAvailableByType;
51 public:
53  DataChannelServiceImpl(const char* ipAddr);
54  virtual ~DataChannelServiceImpl() { }
55  void launch(unsigned long pollDelayUSec);
56 public:
57  // High-level data channels API
58  TimestampedQuaternion getLastRotationQuaternion() {
59  return channelBNO080->lastRotationQuaternion;
60  }
61  std::vector<TimestampedQuaternion> getRotationQuaternionSeries(int fromSec, int fromUSec, int untilSec, int untilUSec) {
62  return channelBNO080->ringbufRotationQuaternion.popBetweenTimes(fromSec, fromUSec, untilSec, untilUSec);
63  }
64 
65  TimestampedVector getLastSensorVector(int idx) {
66  return channelBNO080->lastXYZ[idx - 1];
67  }
68  std::vector<TimestampedVector> getSensorVectorSeries(int idx, int fromSec, int fromUSec, int untilSec, int untilUSec) {
69  return channelBNO080->ringbufXYZ[idx - 1].popBetweenTimes(fromSec, fromUSec, untilSec, untilUSec);
70  }
71 
72  TimestampedScalar getLastSensorScalar(int idx) {
73  return channelBNO080->lastScalar[idx - 0x0a];
74  }
75  std::vector<TimestampedScalar> getSensorScalarSeries(int idx, int fromSec, int fromUSec, int untilSec, int untilUSec) {
76  return channelBNO080->ringbufScalar[idx - 0x0a].popBetweenTimes(fromSec, fromUSec, untilSec, untilUSec);
77  }
78 };
79 
80 class DataChannelService::Pimpl {
81 public:
82  std::shared_ptr<DataChannelServiceImpl> impl;
83  Pimpl(DeviceInfo deviceInfo) {
84  impl = std::make_shared<DataChannelServiceImpl>(deviceInfo);
85  }
86  Pimpl(const char* ipAddress) {
87  impl = std::make_shared<DataChannelServiceImpl>(ipAddress);
88  }
89 };
90 
91 void DataChannelServiceImpl::receiverRoutine() {
92  threadRunning = true;
93  while (threadRunning) {
94  process();
95  std::this_thread::sleep_for(std::chrono::microseconds(pollDelay));
96  }
97 }
98 
99 void DataChannelServiceImpl::launch(unsigned long pollDelayUSec) {
100  // Prepare our receivers (all supported channels aside from service channel 0)
101  channelBNO080 = std::make_shared<ClientSideDataChannelIMUBNO080>();
102  registerChannel(channelBNO080);
103  // Prepare our poll thread
104  pollDelay = pollDelayUSec;
105  receiverThread = std::make_shared<std::thread>(std::bind(&DataChannelServiceImpl::receiverRoutine, this));
106  receiverThread->detach();
107  // Say hello to the device to get a channel advertisement
108  initiateHandshake();
109 }
110 
111 
112 void DataChannelServiceImpl::initiateHandshake() {
113  uint16_t cmd = htons((uint16_t) DataChannelControlCommands::CTLRequestAdvertisement);
114  sendDataIsolatedPacket((DataChannel::ID) 0x00, DataChannel::Types::CONTROL, (unsigned char*) &cmd, sizeof(cmd), &serverAddr);
115 }
116 
117 void DataChannelServiceImpl::subscribeAll() {
118  unsigned char data[1024];
119  int len = DataChannelControlUtil::packSubscriptionMessage(data, 1024, DataChannelControlCommands::CTLRequestSubscriptions, {0});
120  sendDataIsolatedPacket((DataChannel::ID) 0x00, DataChannel::Types::CONTROL, data, len, &serverAddr);
121 }
122 
123 void DataChannelServiceImpl::unsubscribeAll() {
124  unsigned char data[1024];
125  int len = DataChannelControlUtil::packSubscriptionMessage(data, 1024, DataChannelControlCommands::CTLRequestUnsubscriptions, {0});
126  sendDataIsolatedPacket((DataChannel::ID) 0x00, DataChannel::Types::CONTROL, data, len, &serverAddr);
127 }
128 
129 int DataChannelServiceImpl::handleChannel0Message(DataChannelMessage& message, sockaddr_in* sender) {
130  auto cmd = DataChannelControlUtil::getCommand(message.payload, message.header.payloadSize);
131  switch (cmd) {
132  case DataChannelControlCommands::CTLProvideAdvertisement: {
133  // Update the available channels lists for run-time checks etc.
134  channelsAvailable = DataChannelControlUtil::unpackAdvertisementMessage(message.payload, message.header.payloadSize);
135  for (auto& dci: channelsAvailable) {
136  channelsAvailableByType[dci.getChannelType()].insert(dci.getChannelID());
137  }
138  // Automatic subscribeAll is suitable for now
139  subscribeAll();
140  break;
141  }
142  case DataChannelControlCommands::CTLProvideSubscriptions: {
143  break;
144  }
145  default: {
146  break;
147  }
148  }
149  return 1;
150 }
151 
152 DataChannelServiceImpl::DataChannelServiceImpl(DeviceInfo deviceInfo)
153 : DataChannelServiceImpl::DataChannelServiceImpl(deviceInfo.getIpAddress().c_str())
154 {}
155 
156 DataChannelServiceImpl::DataChannelServiceImpl(const char* ipAddress)
157 : DataChannelServiceBase(), threadRunning(false) {
158  serverAddr.sin_family = AF_INET;
159  serverAddr.sin_port = htons(InternalInformation::DATACHANNELSERVICE_PORT);
160  auto result = inet_addr(ipAddress);
161  if (result == INADDR_NONE) {
162  throw std::runtime_error("Failed to set address for DataChannelService");
163  }
164  serverAddr.sin_addr.s_addr = result;
165  //
166  //if (!inet_pton(AF_INET, deviceInfo.getIpAddress().c_str(), &(serverAddr.sin_addr))) {
167  // throw std::runtime_error("Failed to set address for DataChannelService");
168  //}
169 }
170 
171 
172 DataChannelService::DataChannelService(DeviceInfo deviceInfo, unsigned long pollDelayUSec) {
173  pimpl = new DataChannelService::Pimpl(deviceInfo);
174  pimpl->impl->launch(pollDelayUSec);
175 }
176 
177 DataChannelService::DataChannelService(const char* ipAddress, unsigned long pollDelayUSec) {
178  pimpl = new DataChannelService::Pimpl(ipAddress);
179  pimpl->impl->launch(pollDelayUSec);
180 }
181 
182 
183 DataChannelService::~DataChannelService() {
184  pimpl->impl->threadRunning = false;
185  delete pimpl;
186 }
187 
189  return pimpl->impl->channelsAvailableByType.count(DataChannel::Types::BNO080);
190 }
191 
192 
193 // High-level IMU accessors (C++-98 compatible signatures)
194 
195 // For devices not providing IMU data, these return placeholder defaults
197  return pimpl->impl->getLastRotationQuaternion();
198 }
199 std::vector<TimestampedQuaternion> DataChannelService::imuGetRotationQuaternionSeries(int fromSec, int fromUSec, int untilSec, int untilUSec) {
200  return pimpl->impl->getRotationQuaternionSeries(fromSec, fromUSec, untilSec, untilUSec);
201 }
202 
204  return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_ACCELEROMETER);
205 }
206 std::vector<TimestampedVector> DataChannelService::imuGetAccelerationSeries(int fromSec, int fromUSec, int untilSec, int untilUSec) {
207  return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_ACCELEROMETER, fromSec, fromUSec, untilSec, untilUSec);
208 }
209 
211  return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_GYROSCOPE);
212 }
213 std::vector<TimestampedVector> DataChannelService::imuGetGyroscopeSeries(int fromSec, int fromUSec, int untilSec, int untilUSec) {
214  return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_GYROSCOPE, fromSec, fromUSec, untilSec, untilUSec);
215 }
216 
218  return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_MAGNETOMETER);
219 }
220 std::vector<TimestampedVector> DataChannelService::imuGetMagnetometerSeries(int fromSec, int fromUSec, int untilSec, int untilUSec) {
221  return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_MAGNETOMETER, fromSec, fromUSec, untilSec, untilUSec);
222 }
223 
225  return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_LINEAR_ACCELERATION);
226 }
227 std::vector<TimestampedVector> DataChannelService::imuGetLinearAccelerationSeries(int fromSec, int fromUSec, int untilSec, int untilUSec) {
228  return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_LINEAR_ACCELERATION, fromSec, fromUSec, untilSec, untilUSec);
229 }
230 
232  return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_GRAVITY);
233 }
234 std::vector<TimestampedVector> DataChannelService::imuGetGravitySeries(int fromSec, int fromUSec, int untilSec, int untilUSec) {
235  return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_GRAVITY, fromSec, fromUSec, untilSec, untilUSec);
236 }
237 
238 } // namespace
239 
Encapsulate a 4D (quaternion) sensor report, containing X, Y, Z, W, as well as timestamp and status f...
Definition: sensordata.h:80
Encapsulate a scalar sensor measurement, containing the value, as well as timestamp and status fields...
Definition: sensordata.h:51
std::vector< TimestampedQuaternion > imuGetRotationQuaternionSeries(int fromSec=0, int fromUSec=0, int untilSec=0x7FFFffffl, int untilUSec=0x7FFFffffl)
Return the current contents of the rotation quaternion data buffer, optionally between specified time...
Encapsulate a 3D sensor report, containing X, Y, Z, as well as timestamp and status fields...
Definition: sensordata.h:64
std::string getIpAddress() const
Gets the IP address of the device.
Definition: deviceinfo.h:97
TimestampedVector imuGetAcceleration()
Return the most recent calibrated accelerometer reading.
std::vector< TimestampedVector > imuGetAccelerationSeries(int fromSec=0, int fromUSec=0, int untilSec=0x7FFFffffl, int untilUSec=0x7FFFffffl)
Return the current contents of the calibrated accelerometer data buffer, optionally between specified...
std::vector< TimestampedVector > imuGetLinearAccelerationSeries(int fromSec=0, int fromUSec=0, int untilSec=0x7FFFffffl, int untilUSec=0x7FFFffffl)
Return the current contents of the linear acceleration (without gravity) data buffer, optionally between specified timestamps.
DataChannelService(DeviceInfo deviceInfo, unsigned long pollDelayUSec=1000)
TimestampedVector imuGetGyroscope()
Return the most recent calibrated angular accelerations from the gyroscope.
std::vector< TimestampedVector > imuGetMagnetometerSeries(int fromSec=0, int fromUSec=0, int untilSec=0x7FFFffffl, int untilUSec=0x7FFFffffl)
Return the current contents of the magnetometer data buffer, optionally between specified timestamps...
bool imuAvailable()
Return whether the device will provide data from an Inertial Measurement Unit.
TimestampedQuaternion imuGetRotationQuaternion()
Return the most recent rotation quaternion, relative to gravity and magnetic north.
Base class for the data service (background sending and receiving, dispatching to channels) ...
std::vector< TimestampedVector > imuGetGravitySeries(int fromSec=0, int fromUSec=0, int untilSec=0x7FFFffffl, int untilUSec=0x7FFFffffl)
Return the current contents of the gravity data buffer, optionally between specified timestamps...
Aggregates information about a discovered device.
Definition: deviceinfo.h:47
std::vector< TimestampedVector > imuGetGyroscopeSeries(int fromSec=0, int fromUSec=0, int untilSec=0x7FFFffffl, int untilUSec=0x7FFFffffl)
Return the current contents of the gyroscope data buffer, optionally between specified timestamps...
TimestampedVector imuGetGravity()
Return the most recent gravity measurement.
TimestampedVector imuGetLinearAcceleration()
Return the most recent linear acceleration, i.e. with gravity factored out.
TimestampedVector imuGetMagnetometer()
Return the most recent magnetometer readings.
Nerian Vision Technologies