libvisiontransfer  7.2.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  DeviceInfo serverInfo;
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  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 };
87 
88 void DataChannelServiceImpl::receiverRoutine() {
89  threadRunning = true;
90  while (threadRunning) {
91  process();
92  std::this_thread::sleep_for(std::chrono::microseconds(pollDelay));
93  }
94 }
95 
96 void DataChannelServiceImpl::launch(unsigned long pollDelayUSec) {
97  // Prepare our receivers (all supported channels aside from service channel 0)
98  channelBNO080 = std::make_shared<ClientSideDataChannelIMUBNO080>();
99  registerChannel(channelBNO080);
100  // Prepare our poll thread
101  pollDelay = pollDelayUSec;
102  receiverThread = std::make_shared<std::thread>(std::bind(&DataChannelServiceImpl::receiverRoutine, this));
103  receiverThread->detach();
104  // Say hello to the device to get a channel advertisement
105  initiateHandshake();
106 }
107 
108 
109 void DataChannelServiceImpl::initiateHandshake() {
110  uint16_t cmd = htons((uint16_t) DataChannelControlCommands::CTLRequestAdvertisement);
111  sendDataIsolatedPacket((DataChannel::ID) 0x00, DataChannel::Types::CONTROL, (unsigned char*) &cmd, sizeof(cmd), &serverAddr);
112 }
113 
114 void DataChannelServiceImpl::subscribeAll() {
115  unsigned char data[1024];
116  int len = DataChannelControlUtil::packSubscriptionMessage(data, 1024, DataChannelControlCommands::CTLRequestSubscriptions, {0});
117  sendDataIsolatedPacket((DataChannel::ID) 0x00, DataChannel::Types::CONTROL, data, len, &serverAddr);
118 }
119 
120 void DataChannelServiceImpl::unsubscribeAll() {
121  unsigned char data[1024];
122  int len = DataChannelControlUtil::packSubscriptionMessage(data, 1024, DataChannelControlCommands::CTLRequestUnsubscriptions, {0});
123  sendDataIsolatedPacket((DataChannel::ID) 0x00, DataChannel::Types::CONTROL, data, len, &serverAddr);
124 }
125 
126 int DataChannelServiceImpl::handleChannel0Message(DataChannelMessage& message, sockaddr_in* sender) {
127  auto cmd = DataChannelControlUtil::getCommand(message.payload, message.header.payloadSize);
128  switch (cmd) {
129  case DataChannelControlCommands::CTLProvideAdvertisement: {
130  // Update the available channels lists for run-time checks etc.
131  channelsAvailable = DataChannelControlUtil::unpackAdvertisementMessage(message.payload, message.header.payloadSize);
132  for (auto& dci: channelsAvailable) {
133  channelsAvailableByType[dci.getChannelType()].insert(dci.getChannelID());
134  }
135  // Automatic subscribeAll is suitable for now
136  subscribeAll();
137  break;
138  }
139  case DataChannelControlCommands::CTLProvideSubscriptions: {
140  break;
141  }
142  default: {
143  break;
144  }
145  }
146  return 1;
147 }
148 
149 DataChannelServiceImpl::DataChannelServiceImpl(DeviceInfo deviceInfo)
150 : DataChannelServiceBase(), serverInfo(deviceInfo), threadRunning(false) {
151  serverAddr.sin_family = AF_INET;
152  serverAddr.sin_port = htons(InternalInformation::DATACHANNELSERVICE_PORT);
153  auto result = inet_addr(deviceInfo.getIpAddress().c_str());
154  if (result == INADDR_NONE) {
155  throw std::runtime_error("Failed to set address for DataChannelService");
156  }
157  serverAddr.sin_addr.s_addr = result;
158  //
159  //if (!inet_pton(AF_INET, deviceInfo.getIpAddress().c_str(), &(serverAddr.sin_addr))) {
160  // throw std::runtime_error("Failed to set address for DataChannelService");
161  //}
162 }
163 
164 
165 DataChannelService::DataChannelService(DeviceInfo deviceInfo, unsigned long pollDelayUSec) {
166  pimpl = new DataChannelService::Pimpl(deviceInfo);
167  pimpl->impl->launch(pollDelayUSec);
168 }
169 
170 DataChannelService::~DataChannelService() {
171  pimpl->impl->threadRunning = false;
172  delete pimpl;
173 }
174 
176  return pimpl->impl->channelsAvailableByType.count(DataChannel::Types::BNO080);
177 }
178 
179 
180 // High-level IMU accessors (C++-98 compatible signatures)
181 
182 // For devices not providing IMU data, these return placeholder defaults
184  return pimpl->impl->getLastRotationQuaternion();
185 }
186 std::vector<TimestampedQuaternion> DataChannelService::imuGetRotationQuaternionSeries(int fromSec, int fromUSec, int untilSec, int untilUSec) {
187  return pimpl->impl->getRotationQuaternionSeries(fromSec, fromUSec, untilSec, untilUSec);
188 }
189 
191  return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_ACCELEROMETER);
192 }
193 std::vector<TimestampedVector> DataChannelService::imuGetAccelerationSeries(int fromSec, int fromUSec, int untilSec, int untilUSec) {
194  return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_ACCELEROMETER, fromSec, fromUSec, untilSec, untilUSec);
195 }
196 
198  return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_GYROSCOPE);
199 }
200 std::vector<TimestampedVector> DataChannelService::imuGetGyroscopeSeries(int fromSec, int fromUSec, int untilSec, int untilUSec) {
201  return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_GYROSCOPE, fromSec, fromUSec, untilSec, untilUSec);
202 }
203 
205  return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_MAGNETOMETER);
206 }
207 std::vector<TimestampedVector> DataChannelService::imuGetMagnetometerSeries(int fromSec, int fromUSec, int untilSec, int untilUSec) {
208  return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_MAGNETOMETER, fromSec, fromUSec, untilSec, untilUSec);
209 }
210 
212  return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_LINEAR_ACCELERATION);
213 }
214 std::vector<TimestampedVector> DataChannelService::imuGetLinearAccelerationSeries(int fromSec, int fromUSec, int untilSec, int untilUSec) {
215  return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_LINEAR_ACCELERATION, fromSec, fromUSec, untilSec, untilUSec);
216 }
217 
219  return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_GRAVITY);
220 }
221 std::vector<TimestampedVector> DataChannelService::imuGetGravitySeries(int fromSec, int fromUSec, int untilSec, int untilUSec) {
222  return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_GRAVITY, fromSec, fromUSec, untilSec, untilUSec);
223 }
224 
225 } // namespace
226 
Encapsulate a 4D (quaternion) sensor report, containing i, j, k, r, as well as timestamp and status f...
Definition: sensordata.h:66
Encapsulate a scalar sensor measurement, containing the value, as well as timestamp and status fields...
Definition: sensordata.h:37
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:50
std::string getIpAddress() const
Gets the IP address of the device.
Definition: deviceinfo.h:96
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