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> 13 #include <visiontransfer/datachannel-imu-bno080.h> 14 #include <visiontransfer/protocol-sh2-imu-bno080.h> 35 sockaddr_in serverAddr;
37 std::shared_ptr<std::thread> receiverThread;
38 unsigned long pollDelay;
40 std::shared_ptr<ClientSideDataChannelIMUBNO080> channelBNO080;
43 void initiateHandshake();
45 void unsubscribeAll();
46 void receiverRoutine();
49 std::vector<DataChannelInfo> channelsAvailable;
50 std::map<DataChannel::Type, std::set<DataChannel::ID>> channelsAvailableByType;
55 void launch(
unsigned long pollDelayUSec);
59 return channelBNO080->lastRotationQuaternion;
61 std::vector<TimestampedQuaternion> getRotationQuaternionSeries(
int fromSec,
int fromUSec,
int untilSec,
int untilUSec) {
62 return channelBNO080->ringbufRotationQuaternion.popBetweenTimes(fromSec, fromUSec, untilSec, untilUSec);
66 return channelBNO080->lastXYZ[idx - 1];
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);
73 return channelBNO080->lastScalar[idx - 0x0a];
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);
80 class DataChannelService::Pimpl {
82 std::shared_ptr<DataChannelServiceImpl> impl;
84 impl = std::make_shared<DataChannelServiceImpl>(deviceInfo);
86 Pimpl(
const char* ipAddress) {
87 impl = std::make_shared<DataChannelServiceImpl>(ipAddress);
91 void DataChannelServiceImpl::receiverRoutine() {
93 while (threadRunning) {
95 std::this_thread::sleep_for(std::chrono::microseconds(pollDelay));
99 void DataChannelServiceImpl::launch(
unsigned long pollDelayUSec) {
101 channelBNO080 = std::make_shared<ClientSideDataChannelIMUBNO080>();
102 registerChannel(channelBNO080);
104 pollDelay = pollDelayUSec;
105 receiverThread = std::make_shared<std::thread>(std::bind(&DataChannelServiceImpl::receiverRoutine,
this));
106 receiverThread->detach();
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);
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);
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);
129 int DataChannelServiceImpl::handleChannel0Message(
DataChannelMessage& message, sockaddr_in* sender) {
130 auto cmd = DataChannelControlUtil::getCommand(message.payload, message.header.payloadSize);
132 case DataChannelControlCommands::CTLProvideAdvertisement: {
134 channelsAvailable = DataChannelControlUtil::unpackAdvertisementMessage(message.payload, message.header.payloadSize);
135 for (
auto& dci: channelsAvailable) {
136 channelsAvailableByType[dci.getChannelType()].insert(dci.getChannelID());
142 case DataChannelControlCommands::CTLProvideSubscriptions: {
152 DataChannelServiceImpl::DataChannelServiceImpl(
DeviceInfo deviceInfo)
153 : DataChannelServiceImpl::DataChannelServiceImpl(deviceInfo.
getIpAddress().c_str())
156 DataChannelServiceImpl::DataChannelServiceImpl(
const char* ipAddress)
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");
164 serverAddr.sin_addr.s_addr = result;
173 pimpl =
new DataChannelService::Pimpl(deviceInfo);
174 pimpl->impl->launch(pollDelayUSec);
178 pimpl =
new DataChannelService::Pimpl(ipAddress);
179 pimpl->impl->launch(pollDelayUSec);
183 DataChannelService::~DataChannelService() {
184 pimpl->impl->threadRunning =
false;
189 return pimpl->impl->channelsAvailableByType.count(DataChannel::Types::BNO080);
197 return pimpl->impl->getLastRotationQuaternion();
200 return pimpl->impl->getRotationQuaternionSeries(fromSec, fromUSec, untilSec, untilUSec);
204 return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_ACCELEROMETER);
207 return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_ACCELEROMETER, fromSec, fromUSec, untilSec, untilUSec);
211 return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_GYROSCOPE);
214 return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_GYROSCOPE, fromSec, fromUSec, untilSec, untilUSec);
218 return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_MAGNETOMETER);
221 return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_MAGNETOMETER, fromSec, fromUSec, untilSec, untilUSec);
225 return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_LINEAR_ACCELERATION);
228 return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_LINEAR_ACCELERATION, fromSec, fromUSec, untilSec, untilUSec);
232 return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_GRAVITY);
235 return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_GRAVITY, fromSec, fromUSec, untilSec, untilUSec);
Encapsulate a 4D (quaternion) sensor report, containing X, Y, Z, W, as well as timestamp and status f...
Encapsulate a scalar sensor measurement, containing the value, as well as timestamp and status fields...
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...
std::string getIpAddress() const
Gets the IP address of the device.
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.
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.