libvisiontransfer  10.0.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 namespace internal {
33 
35 private:
36  sockaddr_in serverAddr;
37  //
38  std::shared_ptr<std::thread> receiverThread;
39  unsigned long pollDelay;
40  //
41  std::shared_ptr<ClientSideDataChannelIMUBNO080> channelBNO080;
42  //
43  int handleChannel0Message(DataChannelMessage& message, sockaddr_in* sender) override;
44  void initiateHandshake();
45  void subscribeAll();
46  void unsubscribeAll();
47  void receiverRoutine();
48 public:
49  bool threadRunning;
50  std::vector<DataChannelInfo> channelsAvailable;
51  std::map<DataChannel::Type, std::set<DataChannel::ID>> channelsAvailableByType;
52 public:
54  DataChannelServiceImpl(const char* ipAddr);
55  virtual ~DataChannelServiceImpl() { }
56  void launch(unsigned long pollDelayUSec);
57 public:
58  // High-level data channels API
59  TimestampedQuaternion getLastRotationQuaternion() {
60  return channelBNO080->lastRotationQuaternion;
61  }
62  std::vector<TimestampedQuaternion> getRotationQuaternionSeries(int fromSec, int fromUSec, int untilSec, int untilUSec) {
63  return channelBNO080->ringbufRotationQuaternion.popBetweenTimes(fromSec, fromUSec, untilSec, untilUSec);
64  }
65 
66  TimestampedVector getLastSensorVector(int idx) {
67  return channelBNO080->lastXYZ[idx - 1];
68  }
69  std::vector<TimestampedVector> getSensorVectorSeries(int idx, int fromSec, int fromUSec, int untilSec, int untilUSec) {
70  return channelBNO080->ringbufXYZ[idx - 1].popBetweenTimes(fromSec, fromUSec, untilSec, untilUSec);
71  }
72 
73  TimestampedScalar getLastSensorScalar(int idx) {
74  return channelBNO080->lastScalar[idx - 0x0a];
75  }
76  std::vector<TimestampedScalar> getSensorScalarSeries(int idx, int fromSec, int fromUSec, int untilSec, int untilUSec) {
77  return channelBNO080->ringbufScalar[idx - 0x0a].popBetweenTimes(fromSec, fromUSec, untilSec, untilUSec);
78  }
79 };
80 
81 } // internal namespace
82 
83 class DataChannelService::Pimpl {
84 public:
85  std::shared_ptr<internal::DataChannelServiceImpl> impl;
86  Pimpl(DeviceInfo deviceInfo) {
87  impl = std::make_shared<internal::DataChannelServiceImpl>(deviceInfo);
88  }
89  Pimpl(const char* ipAddress) {
90  impl = std::make_shared<internal::DataChannelServiceImpl>(ipAddress);
91  }
92 };
93 
94 void internal::DataChannelServiceImpl::receiverRoutine() {
95  threadRunning = true;
96  while (threadRunning) {
97  process();
98  std::this_thread::sleep_for(std::chrono::microseconds(pollDelay));
99  }
100 }
101 
102 void internal::DataChannelServiceImpl::launch(unsigned long pollDelayUSec) {
103  // Prepare our receivers (all supported channels aside from service channel 0)
104  channelBNO080 = std::make_shared<ClientSideDataChannelIMUBNO080>();
105  registerChannel(channelBNO080);
106  // Prepare our poll thread
107  pollDelay = pollDelayUSec;
108  receiverThread = std::make_shared<std::thread>(std::bind(&internal::DataChannelServiceImpl::receiverRoutine, this));
109  receiverThread->detach();
110  // Say hello to the device to get a channel advertisement
111  initiateHandshake();
112 }
113 
114 
115 void internal::DataChannelServiceImpl::initiateHandshake() {
116  uint16_t cmd = htons((uint16_t) DataChannelControlCommands::CTLRequestAdvertisement);
117  sendDataIsolatedPacket((DataChannel::ID) 0x00, DataChannel::Types::CONTROL, (unsigned char*) &cmd, sizeof(cmd), &serverAddr);
118 }
119 
120 void internal::DataChannelServiceImpl::subscribeAll() {
121  unsigned char data[1024];
122  int len = DataChannelControlUtil::packSubscriptionMessage(data, 1024, DataChannelControlCommands::CTLRequestSubscriptions, {0});
123  sendDataIsolatedPacket((DataChannel::ID) 0x00, DataChannel::Types::CONTROL, data, len, &serverAddr);
124 }
125 
126 void internal::DataChannelServiceImpl::unsubscribeAll() {
127  unsigned char data[1024];
128  int len = DataChannelControlUtil::packSubscriptionMessage(data, 1024, DataChannelControlCommands::CTLRequestUnsubscriptions, {0});
129  sendDataIsolatedPacket((DataChannel::ID) 0x00, DataChannel::Types::CONTROL, data, len, &serverAddr);
130 }
131 
132 int internal::DataChannelServiceImpl::handleChannel0Message(DataChannelMessage& message, sockaddr_in* sender) {
133  auto cmd = DataChannelControlUtil::getCommand(message.payload, message.header.payloadSize);
134  switch (cmd) {
135  case DataChannelControlCommands::CTLProvideAdvertisement: {
136  // Update the available channels lists for run-time checks etc.
137  channelsAvailable = DataChannelControlUtil::unpackAdvertisementMessage(message.payload, message.header.payloadSize);
138  for (auto& dci: channelsAvailable) {
139  channelsAvailableByType[dci.getChannelType()].insert(dci.getChannelID());
140  }
141  // Automatic subscribeAll is suitable for now
142  subscribeAll();
143  break;
144  }
145  case DataChannelControlCommands::CTLProvideSubscriptions: {
146  break;
147  }
148  default: {
149  break;
150  }
151  }
152  return 1;
153 }
154 
155 internal::DataChannelServiceImpl::DataChannelServiceImpl(DeviceInfo deviceInfo)
156 : DataChannelServiceImpl::DataChannelServiceImpl(deviceInfo.getIpAddress().c_str())
157 {}
158 
159 internal::DataChannelServiceImpl::DataChannelServiceImpl(const char* ipAddress)
160 : DataChannelServiceBase(), threadRunning(false) {
161  serverAddr.sin_family = AF_INET;
162  serverAddr.sin_port = htons(InternalInformation::DATACHANNELSERVICE_PORT);
163  auto result = inet_addr(ipAddress);
164  if (result == INADDR_NONE) {
165  throw std::runtime_error("Failed to set address for DataChannelService");
166  }
167  serverAddr.sin_addr.s_addr = result;
168  //
169  //if (!inet_pton(AF_INET, deviceInfo.getIpAddress().c_str(), &(serverAddr.sin_addr))) {
170  // throw std::runtime_error("Failed to set address for DataChannelService");
171  //}
172 }
173 
174 
175 DataChannelService::DataChannelService(DeviceInfo deviceInfo, unsigned long pollDelayUSec) {
176  pimpl = new DataChannelService::Pimpl(deviceInfo);
177  pimpl->impl->launch(pollDelayUSec);
178 }
179 
180 DataChannelService::DataChannelService(const char* ipAddress, unsigned long pollDelayUSec) {
181  pimpl = new DataChannelService::Pimpl(ipAddress);
182  pimpl->impl->launch(pollDelayUSec);
183 }
184 
185 
186 DataChannelService::~DataChannelService() {
187  pimpl->impl->threadRunning = false;
188  delete pimpl;
189 }
190 
192  return pimpl->impl->channelsAvailableByType.count(DataChannel::Types::BNO080);
193 }
194 
195 
196 // High-level IMU accessors (C++-98 compatible signatures)
197 
198 // For devices not providing IMU data, these return placeholder defaults
200  return pimpl->impl->getLastRotationQuaternion();
201 }
202 std::vector<TimestampedQuaternion> DataChannelService::imuGetRotationQuaternionSeries(int fromSec, int fromUSec, int untilSec, int untilUSec) {
203  return pimpl->impl->getRotationQuaternionSeries(fromSec, fromUSec, untilSec, untilUSec);
204 }
205 
207  return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_ACCELEROMETER);
208 }
209 std::vector<TimestampedVector> DataChannelService::imuGetAccelerationSeries(int fromSec, int fromUSec, int untilSec, int untilUSec) {
210  return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_ACCELEROMETER, fromSec, fromUSec, untilSec, untilUSec);
211 }
212 
214  return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_GYROSCOPE);
215 }
216 std::vector<TimestampedVector> DataChannelService::imuGetGyroscopeSeries(int fromSec, int fromUSec, int untilSec, int untilUSec) {
217  return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_GYROSCOPE, fromSec, fromUSec, untilSec, untilUSec);
218 }
219 
221  return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_MAGNETOMETER);
222 }
223 std::vector<TimestampedVector> DataChannelService::imuGetMagnetometerSeries(int fromSec, int fromUSec, int untilSec, int untilUSec) {
224  return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_MAGNETOMETER, fromSec, fromUSec, untilSec, untilUSec);
225 }
226 
228  return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_LINEAR_ACCELERATION);
229 }
230 std::vector<TimestampedVector> DataChannelService::imuGetLinearAccelerationSeries(int fromSec, int fromUSec, int untilSec, int untilUSec) {
231  return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_LINEAR_ACCELERATION, fromSec, fromUSec, untilSec, untilUSec);
232 }
233 
235  return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_GRAVITY);
236 }
237 std::vector<TimestampedVector> DataChannelService::imuGetGravitySeries(int fromSec, int fromUSec, int untilSec, int untilUSec) {
238  return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_GRAVITY, fromSec, fromUSec, untilSec, untilUSec);
239 }
240 
241 } // visiontransfer namespace
242 
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