void nRF5xCharacteristicDescriptorDiscoverer::terminate(Discovery* discovery, ble_error_t err) {
// temporary copy, user code can try to launch a new discovery in the onTerminate
// callback. So, this discovery should not appear in such case.
Discovery tmp = *discovery;
*discovery = Discovery();
tmp.terminate(err);
}
/// GSourceFunc function to invoke discover member function at the timed interval.
/// This function is called by the g_main_loop mechanism when the rediscover timeout elapses.
FSTATIC gboolean
_discovery_rediscover(gpointer vself) ///<[in/out] Object to perform discovery on
{
Discovery* self = CASTTOCLASS(Discovery, vself);
return self->discover(self);
}
void nRF5xCharacteristicDescriptorDiscoverer::process(uint16_t connectionHandle, const ble_gattc_evt_desc_disc_rsp_t& descriptors) {
Discovery* discovery = findRunningDiscovery(connectionHandle);
// the discovery has been removed
if(!discovery) {
return;
}
for (uint16_t i = 0; i < descriptors.count; ++i) {
discovery->process(
descriptors.descs[i].handle, UUID(descriptors.descs[i].uuid.uuid)
);
}
// prepare the next discovery request (if needed)
uint16_t startHandle = descriptors.descs[descriptors.count - 1].handle + 1;
uint16_t endHandle = discovery->getCharacteristic().getLastHandle();
if(startHandle > endHandle) {
terminate(discovery, BLE_ERROR_NONE);
return;
}
ble_error_t err = gattc_descriptors_discover(connectionHandle, startHandle, endHandle);
if(err) {
terminate(discovery, err);
return;
}
}
void runAsClient() {
timeStampServerLast = Thread::getTimeStampMs();
ThroughputReceiver tpRcvr;
DiscoveryTimeGreeter discGreeter;
reporter = Publisher("reports");
reporter.setGreeter(&discGreeter);
Subscriber tcpSub("throughput.tcp");
tcpSub.setReceiver(&tpRcvr);
SubscriberConfigMCast mcastConfig("throughput.mcast");
mcastConfig.setMulticastIP("224.1.2.3");
mcastConfig.setMulticastPortbase(22022);
Subscriber mcastSub(&mcastConfig);
mcastSub.setReceiver(&tpRcvr);
SubscriberConfigRTP rtpConfig("throughput.rtp");
// rtpConfig.setPortbase(40042);
Subscriber rtpSub(&rtpConfig);
rtpSub.setReceiver(&tpRcvr);
node.addSubscriber(tcpSub);
node.addSubscriber(mcastSub);
node.addSubscriber(rtpSub);
node.addPublisher(reporter);
disc.add(node);
timeStampStartedPublishing = Thread::getTimeStampMs();
// do nothing here, we reply only when we received a message
while(true) {
#if 0
Thread::sleepMs(1000);
#else
// disconnect / reconnect client on key-press
getchar();
node.removeSubscriber(tcpSub);
node.removeSubscriber(mcastSub);
node.removeSubscriber(rtpSub);
node.removePublisher(reporter);
disc.remove(node);
getchar();
disc.add(node);
node.addSubscriber(tcpSub);
node.addSubscriber(mcastSub);
node.addSubscriber(rtpSub);
node.addPublisher(reporter);
#endif
}
}
void nRF5xCharacteristicDescriptorDiscoverer::processAttributeInformation(
uint16_t connectionHandle, const ble_gattc_evt_attr_info_disc_rsp_t& infos) {
Discovery* discovery = findRunningDiscovery(connectionHandle);
// the discovery has been removed
if(!discovery) {
return;
}
#if (NRF_SD_BLE_API_VERSION <= 2)
// for all UUIDS found, process the discovery
for (uint16_t i = 0; i < infos.count; ++i) {
bool use_16bits_uuids = infos.format == BLE_GATTC_ATTR_INFO_FORMAT_16BIT;
const ble_gattc_attr_info_t& attr_info = infos.attr_info[i];
UUID uuid = use_16bits_uuids ? UUID(attr_info.info.uuid16.uuid) : UUID(attr_info.info.uuid128.uuid128, UUID::LSB);
discovery->process(attr_info.handle, uuid);
}
// prepare the next round of descriptors discovery
uint16_t startHandle = infos.attr_info[infos.count - 1].handle + 1;
#else
uint16_t startHandle;
// for all UUIDS found, process the discovery
if (infos.format == BLE_GATTC_ATTR_INFO_FORMAT_16BIT) {
for (uint16_t i = 0; i < infos.count; ++i) {
UUID uuid = UUID(infos.info.attr_info16[i].uuid.uuid);
discovery->process(infos.info.attr_info16[i].handle, uuid);
}
// prepare the next round of descriptors discovery
startHandle = infos.info.attr_info16[infos.count - 1].handle + 1;
} else {
for (uint16_t i = 0; i < infos.count; ++i) {
UUID uuid = UUID(infos.info.attr_info128[i].uuid.uuid128, UUID::LSB);
discovery->process(infos.info.attr_info128[i].handle, uuid);
}
// prepare the next round of descriptors discovery
startHandle = infos.info.attr_info128[infos.count - 1].handle + 1;
}
#endif
uint16_t endHandle = discovery->getCharacteristic().getLastHandle();
if(startHandle > endHandle) {
terminate(discovery, BLE_ERROR_NONE);
return;
}
ble_error_t err = gattc_descriptors_discover(connectionHandle, startHandle, endHandle);
if(err) {
terminate(discovery, err);
return;
}
}
////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Processes message received by the Service. If not supported, then
/// message is passed to inheriting services depending on what
/// type of control has been established for the component.
///
/// This Service supports AccessControl related messages only.
///
/// \param[in] message Message data to process.
///
////////////////////////////////////////////////////////////////////////////////////
void AccessControl::Receive(const Message* message)
{
if(AcceptCommandMessage(message) == false)
{
return;
}
if(mControllerID == message->GetSourceID())
{
mControllerUpdateTime.SetCurrentTime();
}
switch(message->GetMessageCode())
{
case CONFIRM_CONTROL:
{
const ConfirmControl* command = dynamic_cast<const ConfirmControl*>(message);
if(command == NULL)
return;
if(command->GetResponseCode() == ConfirmControl::ControlAccepted)
{
bool firstTime = true;
// Lock data.
mControlMutex.Lock();
// See if we already had control, and if not, then
// we re-acquired control of this component and must
// signal via callbacks, etc. of this event.
Address::Set::iterator controlledComponent = mControlledComponents.find(message->GetSourceID());
if(controlledComponent != mControlledComponents.end())
{
// If we get here, we have been trying to control this
// component. Check the control status flag.
std::map<Address, bool>::iterator flag = mControlFlags.find(message->GetSourceID());
if(flag->second == true)
{
// We already had control, nothing new here.
// So don't trigger events.
firstTime = false;
}
}
mControlledComponents.insert(message->GetSourceID());
mControlConfirmTimes[message->GetSourceID()].SetCurrentTime();
mControlFlags[message->GetSourceID()] = true;
// Unlock data.
mControlMutex.Unlock();
if(mDebugMessagesFlag)
{
Mutex::ScopedLock plock(&mDebugMessagesMutex);
if(firstTime)
{
std::cout << "[" << GetServiceID().ToString() << "-" << mComponentID.ToString()
<< "] - Acquired Control of " << message->GetSourceID().ToString() << " at " << Time::GetUtcTime().ToString() << "\n";
}
else
{
std::cout << "[" << GetServiceID().ToString() << "-" << mComponentID.ToString()
<< "] - Maintained Control of " << message->GetSourceID().ToString() << " at " << Time::GetUtcTime().ToString() << "\n";
}
}
// Trigger callbacks if needed.
if(firstTime)
{
Service::Map children = GetChildServices();
Service::Map::iterator child;
for(child = children.begin();
child != children.end();
child++)
{
Child* controlChild = dynamic_cast<Child *>(child->second);
if(controlChild)
{
controlChild->ProcessAcquisitionOfControl(message->GetSourceID());
}
}
Mutex::ScopedLock cbLock(&mCallbacksMutex);
Callback::Set::iterator cb;
for(cb = mCallbacks.begin();
cb != mCallbacks.end();
cb++)
{
(*cb)->ProcessAcquisitionfControl(message->GetSourceID());
}
}
}
else
{
Address::Set::iterator component;
// Lock data.
mControlMutex.Lock();
component = mControlledComponents.find(message->GetSourceID());
if(component != mControlledComponents.end())
{
std::map<Address, bool>::iterator flag;
flag = mMaintainFlags.find(message->GetSourceID());
if(flag != mMaintainFlags.end())
{
// If we didn't get control, and we are not
// trying to maintain control, remove the component
// from our lists. If we don't have high enough authority
// then don't bother trying to re-acquire control.
if(flag->second == false || command->GetMessageCode() == ConfirmControl::InsufficientAuthority)
{
EraseComponentControlInfo(message->GetSourceID());
}
else // Update control status
{
mControlFlags[message->GetSourceID()] = false;
}
}
// Delete junk data.
else
{
EraseComponentControlInfo(message->GetSourceID());
}
}
// Unlock data.
mControlMutex.Unlock();
}
}
break;
case QUERY_AUTHORITY:
{
if(mShutdownServiceFlag)
{
break;
}
Mutex::ScopedLock lock(&mControlMutex);
ReportAuthority report(message->GetSourceID(), GetComponentID());
report.SetAuthorityCode(mAuthorityCode);
Send(&report);
}
break;
case QUERY_CONTROL:
{
Mutex::ScopedLock lock(&mControlMutex);
ReportControl report(message->GetSourceID(), GetComponentID());
report.SetAuthorityCode(mControllerAuthorityCode);
report.SetControllingComponent(mControllerID);
Send(&report);
}
break;
case QUERY_TIMEOUT:
{
Mutex::ScopedLock lock(&mControlMutex);
ReportTimeout report(message->GetSourceID(), GetComponentID());
report.SetTimeoutSeconds(mTimeoutPeriod);
Send(&report);
}
break;
case REJECT_CONTROL:
{
const RejectControl* command = dynamic_cast<const RejectControl*>(message);
if(mShutdownServiceFlag || command == NULL)
return;
Mutex::ScopedLock lock(&mControlMutex);
Address::Set::iterator component;
component = mControlledComponents.find(message->GetSourceID());
if(component != mControlledComponents.end())
{
std::map<Address, bool>::iterator flag;
flag = mMaintainFlags.find(message->GetSourceID());
if(flag != mMaintainFlags.end())
{
// If we lost control, and we are not
// trying to maintain control, remove the component
// from our lists.
if(flag->second == false)
{
mMaintainFlags.erase(flag);
mControlFlags.erase(mControlFlags.find(message->GetSourceID()));
mControlConfirmTimes.erase(mControlConfirmTimes.find(message->GetSourceID()));
mControlledComponents.erase(component);
}
else // Update control status
{
mControlFlags[message->GetSourceID()] = false;
}
// Generate events and callbacks.
Mutex::ScopedLock clock(&mCallbacksMutex);
Callback::Set::iterator cb;
for(cb = mCallbacks.begin();
cb != mCallbacks.end();
cb++)
{
(*cb)->ProcessLossOfControl(message->GetSourceID());
}
}
// Delete junk data.
else
{
if(mControlFlags.find(*component) != mControlFlags.end())
{
mControlFlags.erase(*component);
}
mControlledComponents.erase(*component);
}
}
if(mDebugMessagesFlag)
{
Mutex::ScopedLock plock(&mDebugMessagesMutex);
std::cout << "[" << GetServiceID().ToString() << "-" << mComponentID.ToString()
<< "] - Lost Control of " << message->GetSourceID().ToString() << " at " << Time::GetUtcTime().ToString() << "\n";
}
}
break;
case RELEASE_CONTROL:
{
const ReleaseControl* command = dynamic_cast<const ReleaseControl*>(message);
if(mShutdownServiceFlag || command == NULL)
return;
Mutex::ScopedLock lock(&mControlMutex);
if(command->GetSourceID() == mControllerID)
{
RejectControl response(mControllerID, GetComponentID());
bool controlReleased = true;
Service::Map children = GetChildServices();
Service::Map::iterator child;
for(child = children.begin();
child != children.end();
child++)
{
Child* controlChild = dynamic_cast<Child *>(child->second);
if(controlChild)
{
if(controlChild->ReleaseControl())
{
controlReleased;
}
}
}
if(controlReleased)
{
response.SetResponseCode(RejectControl::ControlReleased);
Send(&response);
mControllerID = Address();
mControllerAuthorityCode = 0;
mControllerUpdateTime.Clear();
mControllerCheckTime.Clear();
if(mDebugMessagesFlag)
{
Mutex::ScopedLock plock(&mDebugMessagesMutex);
std::cout << "[" << GetServiceID().ToString() << "-" << mComponentID.ToString()
<< "] - Control Released from " << message->GetSourceID().ToString() << " at " << Time::GetUtcTime().ToString() << "\n";
}
// Generate events and callbacks.
Mutex::ScopedLock clock(&mCallbacksMutex);
Callback::Set::iterator cb;
for(cb = mCallbacks.begin();
cb != mCallbacks.end();
cb++)
{
(*cb)->ProcessReleaseOfControl(message->GetSourceID());
}
SignalEvent(REPORT_CONTROL);
}
else
{
response.SetResponseCode(RejectControl::NotAvailable);
Send(&response);
}
}
}
break;
case REPORT_AUTHORITY:
{
const JAUS::ReportAuthority* report = dynamic_cast<const JAUS::ReportAuthority*>(message);
if(report)
{
// Update Discovery Contents.
try
{
Discovery* discovery = GetComponent()->DiscoveryService();
if(discovery)
{
discovery->GetSubsystem(report->GetSourceID().mSubsystem)->GetComponent(report->GetSourceID())->mAuthorityLevel = (int)report->GetAuthorityCode();
if( discovery->GetVehicle(report->GetSourceID().mSubsystem).IsValid() &&
discovery->GetVehicle(report->GetSourceID().mSubsystem)->mAuthority < (int)report->GetAuthorityCode())
{
discovery->GetVehicle(report->GetSourceID().mSubsystem)->mAuthority = (int)report->GetAuthorityCode();
}
}
}
catch(Exception& e)
{
e.Print();
}
}
}
break;
case REPORT_CONTROL:
// Not used.
break;
case REPORT_TIMEOUT:
{
const ReportTimeout* report = dynamic_cast<const ReportTimeout*>(message);
if(report == NULL)
return;
Mutex::ScopedLock lock(&mControlMutex);
mTimeoutPeriods[message->GetSourceID()] = report->GetTimeoutSeconds();
if(mTimeoutPeriods[message->GetSourceID()] == 0)
{
// We always want to check for control.
mTimeoutPeriods[message->GetSourceID()] = 2;
}
}
break;
case REQUEST_CONTROL:
{
const RequestControl* command = dynamic_cast<const RequestControl*>(message);
if(mShutdownServiceFlag || command == NULL)
return;
ConfirmControl response(message->GetSourceID(), GetComponentID());
Mutex::ScopedLock lock(&mControlMutex);
// If they have minimum authority and we are
// not under control or component has greater authority than
// the current controller, accept.
if(mControllableFlag &&
command->GetAuthorityCode() >= mAuthorityCode &&
(mControllerID == command->GetSourceID() || mControllerID.IsValid() == false || command->GetAuthorityCode() > mControllerAuthorityCode))
{
bool controlAccepted = true;
Service::Map children = GetChildServices();
Service::Map::iterator child;
for(child = children.begin();
child != children.end();
child++)
{
Child* controlChild = dynamic_cast<Child *>(child->second);
if(controlChild)
{
controlAccepted = controlChild->RequestControl();
if(controlAccepted)
{
break;
}
}
}
// This check is needed in case any child
// service (Management) is in an Emergency state
// and cannot transition.
if(controlAccepted == false)
{
response.SetResponseCode(ConfirmControl::NotAvailable);
}
else
{
response.SetResponseCode(ConfirmControl::ControlAccepted);
if(mControllerID != command->GetSourceID() && mControllerID.IsValid())
{
// Notify controller that they have lost control.
RejectControl reject(mControllerID, GetComponentID());
reject.SetResponseCode(RejectControl::ControlReleased);
Send(&reject);
}
mControllerID = message->GetSourceID();
mControllerAuthorityCode = command->GetAuthorityCode();
mControllerCheckTime.SetCurrentTime();
mControllerUpdateTime.SetCurrentTime();
if(mDebugMessagesFlag)
{
Mutex::ScopedLock plock(&mDebugMessagesMutex);
std::cout << "[" << GetServiceID().ToString() << "-" << mComponentID.ToString()
<< "] - Control Granted to " << message->GetSourceID().ToString() << " at " << Time::GetUtcTime().ToString() << "\n";
}
}
}
else
{
if(mControllableFlag == false)
{
response.SetResponseCode(ConfirmControl::NotAvailable);
}
else
{
response.SetResponseCode(ConfirmControl::InsufficientAuthority);
}
if(mDebugMessagesFlag)
{
Mutex::ScopedLock plock(&mDebugMessagesMutex);
std::cout << "[" << GetServiceID().ToString() << "-" << mComponentID.ToString()
<< "] - Control Rejected from " << message->GetSourceID().ToString() << " at " << Time::GetUtcTime().ToString() << "\n";
}
}
Send(&response);
}
break;
case SET_AUTHORITY:
{
const SetAuthority* command = dynamic_cast<const SetAuthority*>(message);
if(command == NULL)
return;
Mutex::ScopedLock lock(&mControlMutex);
mAuthorityCode = command->GetAuthorityCode();
SignalEvent(REPORT_AUTHORITY);
// Update Discovery Contents.
try
{
Discovery* discovery = GetComponent()->DiscoveryService();
if(discovery)
{
discovery->GetSubsystem(command->GetDestinationID().mSubsystem)->GetComponent(command->GetDestinationID())->mAuthorityLevel = (int)command->GetAuthorityCode();
if(discovery->GetVehicle(command->GetDestinationID().mSubsystem)->mAuthority < (int)command->GetAuthorityCode())
{
discovery->GetVehicle(command->GetDestinationID().mSubsystem)->mAuthority = (int)command->GetAuthorityCode();
}
}
}
catch(Exception& e)
{
e.Print();
}
}
break;
default:
break;
};
}
void runAsServer() {
ThroughputGreeter tpGreeter;
Publisher pub;
switch (type) {
case PUB_RTP: {
PublisherConfigRTP config("throughput.rtp");
config.setTimestampIncrement(166);
pub = Publisher(&config);
pub.setGreeter(&tpGreeter);
break;
}
case PUB_MCAST: {
PublisherConfigMCast config("throughput.mcast");
config.setTimestampIncrement(166);
// config.setPortbase(42142);
pub = Publisher(&config);
pub.setGreeter(&tpGreeter);
break;
}
case PUB_TCP: {
pub = Publisher("throughput.tcp");
pub.setGreeter(&tpGreeter);
break;
}
}
node.addPublisher(pub);
disc.add(node);
timeStampStartedAt = Thread::getTimeStampMs();
if (duration > 0)
duration *= 1000; // convert to ms
pub.waitForSubscribers(waitForSubs);
// reserve 20 bytes for timestamp, sequence number and report interval
size_t dataSize = (std::max)(mtu, (size_t)20);
char* data = (char*)malloc(dataSize);
Message* msg = NULL;
if (useZeroCopy) {
msg = new Message(data, dataSize, doneCallback, (void*)NULL);
} else {
msg = new Message();
}
uint64_t lastReportAt = Thread::getTimeStampMs();
while(1) {
uint64_t now = Thread::getTimeStampMs();
if (duration > 0 && now - timeStampStartedAt > duration)
break;
// first 16 bytes are seqNr and timestamp
Message::write(&data[0], ++currSeqNr);
Message::write(&data[8], now);
Message::write(&data[16], reportInterval);
if (!useZeroCopy) {
msg->setData(data, dataSize);
}
pub.send(msg);
intervalFactor = 1000.0 / (double)reportInterval;
bytesWritten += dataSize;
bytesTotal += dataSize;
packetsWritten++;
// sleep just enough to reach the desired bps
{
RScopeLock lock(mutex);
size_t packetsPerSecond = (std::max)(bytesPerSecond / dataSize, (size_t)1);
delay = (std::max)((1000000) / (packetsPerSecond), (size_t)1);
}
// every report interval we are recalculating bandwith
if (now - lastReportAt > reportInterval) {
RScopeLock lock(mutex);
std::string scaleInfo("--");
// and recalculate bytes to send
if (reports.size() > 0) {
double decreasePressure = 0;
// print report messages
std::map<std::string, Report>::iterator repIter = reports.begin();
// bandwidth is not fixed
if (fixedBytesPerSecond == 0) {
while(repIter != reports.end()) {
const Report& report = repIter->second;
double reportPressure = 0;
// see if we need to decrease bandwidth
if (report.pktsLate > packetsWritten / 2) {
// client is lagging more than half a second, scale back somewhat
reportPressure = (std::max)(1.1, reportPressure);
}
if (report.pcntLoss > pcntLossOk) {
// we lost packages, scale back somewhat
reportPressure = (std::max)((double)report.pktsDropped / report.pktsRcvd, reportPressure);
reportPressure = (std::min)(reportPressure, 1.4);
}
if (report.pktsLate > 3 * packetsWritten) {
// queues explode! scale back alot!
reportPressure = (std::max)((double)report.pktsLate / packetsWritten, reportPressure);
reportPressure = (std::min)(reportPressure, 2.0);
}
if (reportPressure > decreasePressure)
decreasePressure = reportPressure;
repIter++;
}
if (decreasePressure > 1) {
bytesPerSecond *= (1.0 / decreasePressure);
bytesPerSecond = (std::max)(bytesPerSecond, (size_t)(1024));
scaleInfo = "down " + toStr(1.0 / decreasePressure);
} else {
bytesPerSecond *= 1.2;
scaleInfo = "up 1.2";
}
std::cout << std::endl;
} else {
if (bytesWritten * intervalFactor < fixedBytesPerSecond) {
bytesPerSecond *= 1.05;
} else if (bytesWritten * intervalFactor > fixedBytesPerSecond) {
bytesPerSecond *= 0.95;
}
}
}
currReportNr++;
writeReports(scaleInfo);
bytesWritten = 0;
packetsWritten = 0;
lastReportAt = Thread::getTimeStampMs();
}
// now we sleep until we have to send another packet
Thread::sleepUs(delay);
}
pub.setGreeter(NULL);
delete(msg);
}
void runAsServer() {
ThroughputGreeter tpGreeter;
Publisher pub;
PublisherConfig* config = NULL;
switch (type) {
case PUB_RTP: {
config = new PublisherConfigRTP("throughput.rtp");
((PublisherConfigRTP*)config)->setTimestampIncrement(166);
break;
}
case PUB_MCAST: {
config = new PublisherConfigMCast("throughput.mcast");
((PublisherConfigMCast*)config)->setTimestampIncrement(166);
// config.setPortbase(42142);
break;
}
case PUB_TCP: {
config = new PublisherConfigTCP ("throughput.tcp");
break;
}
}
if (compressionType.size() != 0)
config->enableCompression(compressionType, compressionWithState, compressionLevel, compressionRefreshInterval);
pub = Publisher(config);
pub.setGreeter(&tpGreeter);
delete config;
node.addPublisher(pub);
disc.add(node);
timeStampStartedDiscovery = Thread::getTimeStampMs();
if (duration > 0)
duration *= 1000; // convert to ms
// open file with data
std::ifstream fin(streamFile, std::ios::in | std::ios::binary);
if (fin) {
// streamFile exists on filesyste,, slurp content into streamData
streamData = std::string((std::istreambuf_iterator<char>(fin) ),
(std::istreambuf_iterator<char>() ));
} else {
// stream file is no actual file, interpret as "size:compressability"
char* compArg = streamFile;
// http://stackoverflow.com/a/53878/990120
std::list<std::string> compArgs;
do {
const char *begin = compArg;
while(*compArg != ':' && *compArg)
compArg++;
compArgs.push_back(std::string(begin, compArg - begin));
} while(0 != *compArg++);
size_t streamDataSize = 0;
double compressibility = 50;
if (compArgs.size() < 1)
printUsageAndExit();
streamDataSize = displayToBytes(compArgs.front());
compArgs.pop_front();
if (!compArgs.empty()) {
compressibility = strTo<double>(compArgs.front());
if (toStr(compressibility) != compArgs.front())
printUsageAndExit();
compArgs.pop_front();
}
if (!compArgs.empty()) {
printUsageAndExit();
}
streamData.resize(streamDataSize);
RDG_genBuffer(&streamData[0], streamDataSize, 1 - (compressibility/(double)100), 0.0, 0);
}
{
// determine compressionActualRatio
Message msg(streamData.data(), streamData.size());
size_t compressMaxPayload = msg.getCompressBounds("lz4", NULL, Message::Compression::PAYLOAD);
char* compressPayloadData = (char*)malloc(compressMaxPayload);
size_t compressActualPayload = msg.compress("lz4", NULL, compressPayloadData, compressMaxPayload, Message::Compression::PAYLOAD);
compressionActualRatio = 100 * ((double)compressActualPayload / streamData.size());
}
pub.waitForSubscribers(waitForSubs);
timeStampStartedPublishing = Thread::getTimeStampMs();
uint64_t lastReportAt = Thread::getTimeStampMs();
size_t streamDataOffset = 0;
while(1) {
// fill data from stream data
Message* msg = new Message((char*)malloc(mtu), mtu);
size_t msgDataOffset = 0;
while(true) {
size_t toRead = (mtu - msgDataOffset <= streamData.size() - streamDataOffset ? mtu - msgDataOffset : streamData.size() - streamDataOffset);
memcpy(&msg->data()[msgDataOffset], &streamData[streamDataOffset], toRead);
msgDataOffset += toRead;
streamDataOffset += toRead;
// UM_LOG_WARN("%d: %d / %d", streamData.size(), msgDataOffset, streamDataOffset);
if (msgDataOffset == mtu)
break;
if (streamData.size() == streamDataOffset)
streamDataOffset = 0;
}
// UM_LOG_WARN("%d: %s", msg->size(), md5(msg->data(), msg->size()).c_str());
uint64_t now = Thread::getTimeStampMs();
if (duration > 0 && now - timeStampStartedPublishing > duration)
break;
// first 16 bytes are seqNr and timestamp
Message::write(&msg->data()[0], ++currSeqNr);
Message::write(&msg->data()[8], now);
Message::write(&msg->data()[16], reportInterval);
// msg->putMeta("md5", md5(msg->data(), msg->size()));
intervalFactor = 1000.0 / (double)reportInterval;
bytesWritten += msg->size();
bytesTotal += msg->size();
packetsWritten++;
// sleep just enough to reach the desired bps
{
RScopeLock lock(mutex);
size_t packetsPerSecond = (std::max)(bytesPerSecond / msg->size(), (size_t)1);
delay = (std::max)((1000000) / (packetsPerSecond), (size_t)1);
}
// sending with compression will alter msg->size(); not anymore ...
pub.send(msg);
// every report interval we are recalculating bandwith
if (now - lastReportAt > reportInterval) {
RScopeLock lock(mutex);
std::string scaleInfo("--");
// and recalculate bytes to send
if (reports.size() > 0) {
double decreasePressure = 0;
// print report messages
std::map<std::string, Report>::iterator repIter = reports.begin();
// bandwidth is fixed
if (fixedBytesPerSecond == 0) {
while(repIter != reports.end()) {
const Report& report = repIter->second;
double reportPressure = 0;
// see if we need to decrease bandwidth
if (report.pktsLate > packetsWritten / 2) {
// client is lagging more than half a second, scale back somewhat
reportPressure = (std::max)(1.1, reportPressure);
}
if (report.pcntLoss > pcntLossOk) {
// we lost packages, scale back somewhat
reportPressure = (std::max)((double)report.pktsDropped / report.pktsRcvd, reportPressure);
reportPressure = (std::min)(reportPressure, 1.4);
}
if (report.pktsLate > 3 * packetsWritten) {
// queues explode! scale back alot!
reportPressure = (std::max)((double)report.pktsLate / packetsWritten, reportPressure);
reportPressure = (std::min)(reportPressure, 2.0);
}
if (reportPressure > decreasePressure)
decreasePressure = reportPressure;
repIter++;
}
if (decreasePressure > 1) {
bytesPerSecond *= (1.0 / decreasePressure);
bytesPerSecond = (std::max)(bytesPerSecond, (size_t)(1024));
scaleInfo = "down " + toStr(1.0 / decreasePressure);
} else {
bytesPerSecond *= 1.3;
scaleInfo = "up 1.3";
}
std::cout << std::endl;
} else if (fixedBytesPerSecond == (size_t)-1) {
// do nothing
} else {
if (bytesWritten * intervalFactor < fixedBytesPerSecond) {
bytesPerSecond *= 1.05;
} else if (bytesWritten * intervalFactor > fixedBytesPerSecond) {
bytesPerSecond *= 0.95;
}
}
}
currReportNr++;
writeReports(scaleInfo);
bytesWritten = 0;
packetsWritten = 0;
lastReportAt = Thread::getTimeStampMs();
}
delete(msg);
// now we sleep until we have to send another packet
if (delay > 50 && fixedBytesPerSecond != (size_t) - 1)
Thread::sleepUs(delay);
}
pub.setGreeter(NULL);
}
int main(int argc, char** argv) {
uint16_t modulo = 1;
Freenect::Freenect freenect;
FreenectBridge* device;
Discovery disc;
Node node;
Publisher pubDepthTCP;
Publisher pubDepthRTP;
Publisher pubVideoTCP;
Publisher pubVideoRTP;
printf("umundo-kinect-pub version " UMUNDO_VERSION " (" CMAKE_BUILD_TYPE " build)\n");
int option;
while ((option = getopt(argc, argv, "m:")) != -1) {
switch(option) {
case 'm':
modulo = atoi(optarg);
break;
default:
printUsageAndExit();
break;
}
}
std::cout << "Sending every " << modulo << ". frame..." << std::endl << std::flush;
{
PublisherConfigRTP pubConfigRTP("kinect.depth.rtp");
pubConfigRTP.setTimestampIncrement(1);
pubDepthRTP = Publisher(&pubConfigRTP);
PublisherConfigTCP pubConfigTCP("kinect.depth.tcp");
pubConfigTCP.enableCompression();
pubDepthTCP = Publisher(&pubConfigTCP);
}
{
PublisherConfigRTP pubConfigRTP("kinect.video.rtp");
pubConfigRTP.setTimestampIncrement(1);
pubVideoRTP = Publisher(&pubConfigRTP);
PublisherConfigTCP pubConfigTCP("kinect.video.tcp");
pubConfigTCP.enableCompression();
pubVideoTCP = Publisher(&pubConfigTCP);
}
disc = Discovery(Discovery::MDNS);
disc.add(node);
node.addPublisher(pubDepthRTP);
node.addPublisher(pubDepthTCP);
node.addPublisher(pubVideoRTP);
node.addPublisher(pubVideoTCP);
// try to instantiate freenect device
while(true) {
try {
// constructor is somewhat fragile
device = &freenect.createDevice<FreenectBridge>(0);
device->pubDepthRTP = pubDepthRTP;
device->pubDepthTCP = pubDepthTCP;
device->pubVideoRTP = pubVideoRTP;
device->pubVideoTCP = pubVideoTCP;
device->setModulo(modulo);
// this is actually the default
device->setVideoFormat(FREENECT_VIDEO_BAYER, FREENECT_RESOLUTION_MEDIUM);
while(true) {
try {
device->startVideo(); // not blocking on recent versions only?
while(true)
Thread::sleepMs(2000);
} catch(std::runtime_error e) {
std::cout << "An exception occured while trying to start video: " << e.what() << " - retrying after 5s" << std::endl;
Thread::sleepMs(5000);
}
}
} catch(std::runtime_error e) {
std::cout << "An exception occured: " << e.what() << " - retrying" << std::endl;
Thread::sleepMs(5000);
}
}
return 0;
}
