std::string
ReplayConductor::toString( const IceUtil::Time &t )
{
stringstream ss;
ss << t.toSeconds() << ":" << t.toMicroSeconds()-t.toSeconds()*1e6;
return ss.str();
}
void laserClient::run(){
IceUtil::Time last;
last=IceUtil::Time::now();
while (!(_done)){
if (pauseStatus){
IceUtil::Mutex::Lock sync(this->controlMutex);
this->sem.wait(sync);
}
jderobot::LaserDataPtr localLaser=this->prx->getLaserData();
this->controlMutex.lock();
this->data.resize(localLaser->distanceData.size());
std::copy( localLaser->distanceData.begin(), localLaser->distanceData.end(), this->data.begin() );
this->controlMutex.unlock();
if ((IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds()) > this->cycle ){
if (this->debug)
std::cout<< prefix << ": pointCloud adquisition timeout-" << std::endl;
}
else{
usleep(this->cycle - (IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds()));
}
this->refreshRate=(int)(1000000/(IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds()));
last=IceUtil::Time::now();
}
}
bool
CPatchDlg::patchProgress(Ice::Long, Ice::Long, Ice::Long totalProgress, Ice::Long totalSize)
{
IceUtil::Time elapsed = IceUtil::Time::now(IceUtil::Time::Monotonic) - _startTime;
if(elapsed.toSeconds() > 0)
{
CString speed;
speed.Format(L" %s/s", convertSize(totalProgress / elapsed.toSeconds()));
_speed->SetWindowText(speed);
}
int pcnt = 100;
if(totalSize > 0)
{
pcnt = static_cast<int>(totalProgress * 100 / totalSize);
}
CString percent;
percent.Format(L"%d%%", pcnt);
_percent->SetWindowText(percent);
CString total;
total.Format(L" %s / %s", convertSize(totalProgress), convertSize(totalSize));
_total->SetWindowText(total);
_progress->SetPos(pcnt);
processMessages();
return !_isCancel;
}
void
Driver::read( hydrointerfaces::LaserScanner2d::Data &data )
{
// tmp data storage
hokuyo_aist::HokuyoData hokuyoData;
try {
laser_->GetRanges( &hokuyoData );
// set the time stamp right away
IceUtil::Time t = IceUtil::Time::now();
data.timeStampSec = (int)t.toSeconds();
data.timeStampUsec = (int)t.toMicroSeconds() - data.timeStampSec*1000000;
//TODO: Michael 23/Sept/08
//this timeStamp is actually at the _end_ of the scan; also
//scan-time is _not_ negligable for the hokuyos (10-30Hz --> 100-33ms)
//
//--> need to keep track of delta time between scans, and use fieldOfView
//to calculate a backcorrection, assuming 1 rotation per scan (true for
//URG and UHG, unknown for TopURG). Also should put something like
//durationOfScan into the interface.
//
//This would allow to interpolate senor pose per individual range-reading.
//Yes, I really think this is neccessary.
}
catch (hokuyo_aist::HokuyoError &e)
{
std::stringstream ss;
ss << "Read on urg_nz failed. Errorcode: "
<< e.Code()
<< ", Errordescription: "
<< e.what()
<< " continuing";
//context_.tracer().error( ss.str() ,2 );
throw gbxutilacfr::HardwareException( ERROR_INFO, ss.str() );
}
const uint32_t* ranges = hokuyoData.Ranges();
//const uint32_t* intensities = hokuyoData.Intensities();
size_t length = hokuyoData.Length();
assert(config_.numberOfSamples == (int)length); // make sure the receiving end has enough space
double minRangeHokuyo = config_.minRange*1000.0; // convert to mm
for (unsigned int i=0; i<length; ++i )
{
if(ranges[i] < minRangeHokuyo){
//these guys are error-codes (per reading); setting to maxRange to get behavior similar to SICK
data.ranges[i] = config_.maxRange;
}else{
data.ranges[i] = ranges[i] / 1000.0;
}
}
}
void pointcloudClient::run(){
IceUtil::Time last;
last=IceUtil::Time::now();
while (!(_done)){
if (pauseStatus){
IceUtil::Mutex::Lock sync(this->controlMutex);
this->sem.wait(sync);
}
try{
jderobot::pointCloudDataPtr localCloud=this->prx->getCloudData();
this->controlMutex.lock();
this->newData=true;
this->data.resize(localCloud->p.size());
std::copy( localCloud->p.begin(), localCloud->p.end(), this->data.begin() );
this->controlMutex.unlock();
this->semBlock.broadcast();
}
catch(...){
jderobot::Logger::getInstance()->warning(prefix +"error during request (connection error)");
usleep(50000);
}
int process = this->cycle - (IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds());
if (process > (int)cycle ){
jderobot::Logger::getInstance()->warning(prefix + ": pointCloud adquisition timeout-");
}
else{
int delay = (int)cycle - process;
if (delay <1 || delay > (int)cycle)
delay = 1;
usleep(delay);
}
this->refreshRate=(int)(1000000/(IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds()));
last=IceUtil::Time::now();
usleep(100);
}
this->data.clear();
}
jderobot::ImageDataPtr CameraUtils::convert(const cv::Mat &image) {
jderobot::ImageDataPtr reply=jderobot::ImageDataPtr(new jderobot::ImageData());
reply->description = jderobot::ImageDescriptionPtr(new jderobot::ImageDescription());
IceUtil::Time t = IceUtil::Time::now();
reply->timeStamp.seconds = (long)t.toSeconds();
reply->timeStamp.useconds = (long)t.toMicroSeconds() - reply->timeStamp.seconds*1000000;
reply->description->format = colorspaces::ImageRGB8::FORMAT_RGB8.get()->name;
reply->description->width=image.size().width;
reply->description->height=image.size().height;
reply->pixelData.resize(image.rows*image.cols * image.channels());
memcpy(&(reply->pixelData[0]),(unsigned char *) image.data, image.rows*image.cols * image.channels());
return reply;
}
void
Driver::read( hydrointerfaces::Image::Data &data )
{
context_.tracer().debug( "Copying image file data..." );
//Qt uses a uint32_t array not a byte aligned array so we must cut off the end of each scanline.
uint32_t bytesPerLine = static_cast<uint32_t>(image_.width()) * 3;
for(uint32_t i = 0; i < static_cast<uint32_t>(image_.height()); i++)
{
memcpy(data.pixelData+i*bytesPerLine, image_.scanLine(i), bytesPerLine);
}
IceUtil::Time t = IceUtil::Time::now();
data.timeStampSec = (int)t.toSeconds();
data.timeStampUsec = (int)t.toMicroSeconds() - data.timeStampSec*1000000;
}
void Pose3dIceClient::run(){
JdeRobotTypes::Pose3d pose3d;
IceUtil::Time last;
last=IceUtil::Time::now();
while (this->on){
if (pauseStatus){
IceUtil::Mutex::Lock sync(this->controlMutex);
this->sem.wait(sync);
}
try{
jderobot::Pose3DDataPtr pose3ddata = this->prx->getPose3DData();
pose3d.x = pose3ddata->x;
pose3d.y = pose3ddata->y;
pose3d.z = pose3ddata->z;
pose3d.q[0] = pose3ddata->q0;
pose3d.q[1] = pose3ddata->q1;
pose3d.q[2] = pose3ddata->q2;
pose3d.q[3] = pose3ddata->q3;
pose3d.yaw = this->quat2Yaw(pose3d.q);
pose3d.pitch = this->quat2Pitch(pose3d.q);
pose3d.roll = this->quat2Roll(pose3d.q);
this->controlMutex.lock();
this->pose = pose3d;
this->controlMutex.unlock();
}
catch(...){
std::cerr << prefix +"error during request (connection error)" << std::endl;
usleep(5000);
}
if ((IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds()) <= this->cycle ){
usleep(this->cycle - (IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds()));
}
last=IceUtil::Time::now();
}
}
void ReplyTask::run() {
jderobot::ImageDataPtr reply(new jderobot::ImageData);
::jderobot::Time t;
while (1) {
IceUtil::Time t = IceUtil::Time::now();
reply->timeStamp.seconds = (long) t.toSeconds();
reply->timeStamp.useconds = (long) t.toMicroSeconds()
- reply->timeStamp.seconds * 1000000;
{ //critical region start
IceUtil::Mutex::Lock sync(requestsMutex);
while (!requests.empty()) {
jderobot::AMD_ImageProvider_getImageDataPtr cb = requests.front();
requests.pop_front();
cb->ice_response(reply);
}
}
}
}
void
ReplayConductor::handleRewind( const IceUtil::Time &deltaT )
{
bool wasPlaying = isPlaying_;
if ( wasPlaying )
handlePause();
int sec, usec;
masterFileReader_.getCursorTime( sec, usec );
IceUtil::Time tNew = orcalog::iceUtilTime( sec, usec );
tNew -= deltaT;
masterFileReader_.placeCursorAtOrAfterTime( tNew.toSeconds(),
(int)(tNew.toMicroSeconds()-tNew.toSeconds()*1e6) );
if ( wasPlaying )
handleStart();
}
bool
IceUtilInternal::Semaphore::timedWait(const IceUtil::Time& timeout) const
{
long msec = (long)timeout.toMilliSeconds();
int rc = WaitForSingleObject(_sem, msec);
if(rc != WAIT_TIMEOUT && rc != WAIT_OBJECT_0)
{
throw IceUtil::ThreadSyscallException(__FILE__, __LINE__, GetLastError());
}
return rc != WAIT_TIMEOUT;
}
chrono::system_clock::time_point
#else
IceUtil::Time
#endif
getX509Date(SecCertificateRef cert, CFTypeRef key)
{
assert(key == kSecOIDX509V1ValidityNotAfter || key == kSecOIDX509V1ValidityNotBefore);
UniqueRef<CFDictionaryRef> property(getCertificateProperty(cert, key));
CFAbsoluteTime seconds = 0;
if(property)
{
CFNumberRef date = static_cast<CFNumberRef>(CFDictionaryGetValue(property.get(), kSecPropertyKeyValue));
CFNumberGetValue(date, kCFNumberDoubleType, &seconds);
}
IceUtil::Time time = IceUtil::Time::secondsDouble(kCFAbsoluteTimeIntervalSince1970 + seconds);
#ifdef ICE_CPP11_MAPPING
return chrono::system_clock::time_point(chrono::microseconds(time.toMicroSeconds()));
#else
return time;
#endif
}
void laserClient::run(){
IceUtil::Time last;
last=IceUtil::Time::now();
while (!(_done)){
if (pauseStatus){
IceUtil::Mutex::Lock sync(this->controlMutex);
this->sem.wait(sync);
}
try{
jderobot::LaserDataPtr localLaser=this->prx->getLaserData();
this->controlMutex.lock();
this->data.resize(localLaser->distanceData.size());
std::copy( localLaser->distanceData.begin(), localLaser->distanceData.end(), this->data.begin() );
this->controlMutex.unlock();
}
catch(...){
jderobot::Logger::getInstance()->warning(prefix +"error during request (connection error)");
usleep(5000);
}
if ((IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds()) > this->cycle ){
jderobot::Logger::getInstance()->warning(prefix + ": pointCloud adquisition timeout-");
}
else{
usleep(this->cycle - (IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds()));
}
this->refreshRate=(int)(1000000/(IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds()));
last=IceUtil::Time::now();
}
}
void
Driver::read( hydrointerfaces::MultiImage::Data& data )
{
context_.tracer().debug( "Grabbing frame(s) from camera(s)..." );
for( unsigned int i=0; i<cameras_.size(); ++i )
{
// This performs the grab and the retrieve with one cv function call
frames_.at(i) = cvQueryFrame( cameras_.at(i) );
if( frames_.at(i) == NULL )
{
stringstream ss;
ss << "Failed to retrieve frame from Camera " << i;
throw gbxutilacfr::Exception( ERROR_INFO, ss.str() );
}
memcpy( data.at(i).pixelData, frames_.at(i)->imageData, frames_.at(i)->imageSize );
IceUtil::Time t = IceUtil::Time::now();
data.at(i).timeStampSec = (int)t.toSeconds();
data.at(i).timeStampUsec = (int)t.toMicroSeconds() - data.at(i).timeStampSec*1000000;
}
}
bool
IceUtilInternal::Semaphore::timedWait(const IceUtil::Time& timeout) const
{
IceUtil::Int64 msTimeout = timeout.toMilliSeconds();
if(msTimeout < 0 || msTimeout > 0x7FFFFFFF)
{
throw IceUtil::InvalidTimeoutException(__FILE__, __LINE__, timeout);
}
DWORD rc = WaitForSingleObject(_sem, static_cast<DWORD>(msTimeout));
if(rc != WAIT_TIMEOUT && rc != WAIT_OBJECT_0)
{
throw IceUtil::ThreadSyscallException(__FILE__, __LINE__, GetLastError());
}
return rc != WAIT_TIMEOUT;
}
void checkedSleep( gbxutilacfr::Stoppable* activity, const IceUtil::Time& duration, int checkIntervalMs )
{
assert( activity && "Null activity pointer" );
// Handle durations less than the check interval
if ( duration.toMilliSeconds() < checkIntervalMs )
{
IceUtil::ThreadControl::sleep( duration );
return;
}
IceUtil::Time wakeupTime = IceUtil::Time::now() + duration;
IceUtil::Time checkInterval = IceUtil::Time::milliSeconds( checkIntervalMs );
while ( !activity->isStopping() && IceUtil::Time::now() < wakeupTime )
{
IceUtil::ThreadControl::sleep( checkInterval );
}
}
void
CameraIceClient::run(){
jderobot::ImageDataPtr dataPtr;
colorspaces::Image::FormatPtr fmt;
IceUtil::Time last;
int iterIndex = 0;
int totalRefreshRate = 0;
int refrRate = 0;
JdeRobotTypes::Image img;
last=IceUtil::Time::now();
while (this->on){
iterIndex ++;
if (pauseStatus){
IceUtil::Mutex::Lock sync(this->controlMutex);
this->semWait.wait(sync);
}
try{
dataPtr = this->prx->getImageData(this->mImageFormat);
// Putting image data
img.data = CameraUtils::getImageFromCameraProxy(dataPtr);
img.format = dataPtr->description->format;
img.width = dataPtr->description->width;
img.height = dataPtr->description->height;
img.timeStamp = dataPtr->timeStamp.seconds + dataPtr->timeStamp.useconds * 1e-6;
}
catch(std::exception& e){
LOG(WARNING) << prefix +"error during request (connection error): " << e.what() << std::endl;
usleep(50000);
}
int process = (IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds());
if (process > (int)cycle ){
DLOG(WARNING) << "--------" + prefix + " adquisition timeout-";
}
else{
int delay = (int)cycle - process;
if (delay <1 || delay > (int)cycle)
delay = 1;
usleep(delay);
}
int rate =(int)(1000000/(IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds()));
totalRefreshRate = totalRefreshRate + rate;
refrRate = totalRefreshRate / iterIndex;
last=IceUtil::Time::now();
if (iterIndex == INT_MAX)
{
iterIndex = 0;
DLOG(INFO) << "*** Counter reset";
}
this->controlMutex.lock();
this->image = img;
this->refreshRate = refrRate;
this->controlMutex.unlock();
}
this->image.data.release();
}
void
LatencyPingerI::calcStats( const int objSize )
{
// RRT values
std::vector<double> rrt;
int lostCount = 0;
for ( int i=0; i<config_.count; ++i ) {
if ( ids_[i] != -1 ) {
IceUtil::Time dt = receives_[i] - sends_[i];
if ( dt < IceUtil::Time() ) {
std::cout<<i<<" : negative time! "<<std::endl;
}
rrt.push_back( dt.toSecondsDouble() / double(config_.preload) );
// debug
std::cout<<i<<" snd "<<sends_[i].toSeconds()<<":"<<sends_[i].toMicroSeconds()-sends_[i].toSeconds()*1000000
<<" rcv "<<receives_[i].toSeconds()<<":"<<receives_[i].toMicroSeconds()-receives_[i].toSeconds()*1000000
<<" rtt "<<rrt.back()<<" "<<std::endl;
// debug
//double dd = rrt.back();
//std::cout<<"object "<<i<<" rtt: "<<rrt.back()<<"s "<<dd<<"s "<<1000.0*dd<<"ms"<<std::endl;
}
else {
++lostCount;
//std::cout<<"object "<<i<<" was lost"<<std::endl;
}
}
// check for empty vector
if ( rrt.empty() ) {
std::cout<<"No statistics. All packets lost?"<<std::endl;
return;
}
#ifdef WIN32
// under windows, times under 1ms cannot be measured apparently
for ( unsigned int i=0; i<rrt.size(); ++i ) {
if ( rrt[i] == 0 ) {
// set to sensing limit: 1ms
rrt[i] = 1.0e-3;
}
}
#endif
// find min / max RRT's
double rrtmin = *(std::min_element( rrt.begin()+1, rrt.end() ));
double rrtmax = *(std::max_element( rrt.begin()+1, rrt.end() ));
// find average RRT
double rrtavg = 0.0;
// ignore the first result, it's always an outlier (because the Serializer is adjusting its size)
for ( unsigned int i=1; i<rrt.size(); ++i ) {
rrtavg += rrt[i];
}
rrtavg /= (rrt.size()-1);
// print out results (ping style)
std::cout<<"size "<<objSize<<" bytes"<<std::endl;
std::cout<<config_.count<<" packets transmitted, "
<<config_.count-lostCount<<" received, "
<<(double)lostCount/(double)config_.count*100.00<<"% packet loss, time "<<"?"<<"ms"<<std::endl;
std::cout<<"rtt min/avg/max/mdev = "
<<rrtmin*1000.0<<"/"<<rrtavg*1000.0<<"/"<<rrtmax*1000.0<<"/? ms"<<std::endl<<std::endl;
// log results
*logfile_ <<objSize<<" "
<< (rrtmin*1000.0)<<" "<<(rrtavg*1000.0)<<" "<<(rrtmax*1000.0)<<" "<<-1.0;
// log individual rrt's
for ( unsigned int i=1; i<rrt.size(); ++i ) {
*logfile_ << " " << rrt[i]*1000.0;
// debug
//std::cout<<rrt[i]<<" ";
}
for ( unsigned int i=0; i<(config_.count-rrt.size()); ++i ) {
*logfile_<< " " << -1;
}
*logfile_ << std::endl;
std::cout<<std::endl;
std::cout<<std::endl;
}
int
main(int argc, char* argv[])
{
Ice::InitializationData initData;
initData.properties = Ice::createProperties();
initData.properties->load("config");
Ice::CommunicatorPtr communicator = Ice::initialize(argc, argv, initData);
if(argc > 1)
{
cerr << argv[0] << ": too many arguments" << endl;
return EXIT_FAILURE;
}
Freeze::ConnectionPtr connection = Freeze::createConnection(communicator, "backup");
IntLongMap m(connection, "IntLongMap", true);
const int size = 10000;
if(m.size() == 0)
{
cout << "********* Creating new map ***********" << endl;
Freeze::TransactionHolder txHolder(connection);
IceUtil::Time time = IceUtil::Time::now();
IceUtil::Int64 ms = time.toMilliSeconds();
for(int i = 0; i < size; ++i)
{
m.insert(IntLongMap::value_type(i, ms));
}
txHolder.commit();
}
cout << "Updating map" << endl;
for(;;)
{
int count = 0;
Freeze::TransactionHolder txHolder(connection);
IceUtil::Time time = IceUtil::Time::now();
IceUtil::Int64 ms = time.toMilliSeconds();
IntLongMap::iterator p = m.begin();
IceUtil::Int64 oldMs = p->second;
do
{
if(p->second != oldMs)
{
cerr << "old time (ms) == " << oldMs << endl;
cerr << "current current (ms) == " << p->second << endl;
}
test(p->second == oldMs);
p.set(ms);
count++;
} while(++p != m.end());
cout << "Read " << IceUtil::Time::milliSeconds(oldMs).toDateTime() << " in all records;"
<< " updating with " << time.toDateTime() << " ... " << flush;
txHolder.commit();
cout << "done" << endl;
test(count == size);
}
connection->close();
communicator->destroy();
return EXIT_SUCCESS;
}
int
ThroughputClient::run(int argc, char* argv[])
{
if(argc > 1)
{
cerr << appName() << ": too many arguments" << endl;
return EXIT_FAILURE;
}
ThroughputPrx throughput = ThroughputPrx::checkedCast(communicator()->propertyToProxy("Throughput.Proxy"));
if(!throughput)
{
cerr << argv[0] << ": invalid proxy" << endl;
return EXIT_FAILURE;
}
ThroughputPrx throughputOneway = ThroughputPrx::uncheckedCast(throughput->ice_oneway());
ByteSeq byteSeq(ByteSeqSize);
pair<const Ice::Byte*, const Ice::Byte*> byteArr;
byteArr.first = &byteSeq[0];
byteArr.second = byteArr.first + byteSeq.size();
StringSeq stringSeq(StringSeqSize, "hello");
StringDoubleSeq structSeq(StringDoubleSeqSize);
int i;
for(i = 0; i < StringDoubleSeqSize; ++i)
{
structSeq[i].s = "hello";
structSeq[i].d = 3.14;
}
FixedSeq fixedSeq(FixedSeqSize);
for(i = 0; i < FixedSeqSize; ++i)
{
fixedSeq[i].i = 0;
fixedSeq[i].j = 0;
fixedSeq[i].d = 0;
}
//
// To allow cross-language tests we may need to "warm up" the
// server. The warm up is to ensure that any JIT compiler will
// have converted any hotspots to native code. This ensures an
// accurate throughput measurement.
//
if(throughput->needsWarmup())
{
throughput->startWarmup();
ByteSeq emptyBytesBuf(1);
emptyBytesBuf.resize(1);
pair<const Ice::Byte*, const Ice::Byte*> emptyBytes;
emptyBytes.first = &emptyBytesBuf[0];
emptyBytes.second = emptyBytes.first + emptyBytesBuf.size();
StringSeq emptyStrings(1);
emptyStrings.resize(1);
StringDoubleSeq emptyStructs(1);
emptyStructs.resize(1);
FixedSeq emptyFixed(1);
emptyFixed.resize(1);
cout << "warming up the server... " << flush;
for(int i = 0; i < 10000; i++)
{
throughput->sendByteSeq(emptyBytes);
throughput->sendStringSeq(emptyStrings);
throughput->sendStructSeq(emptyStructs);
throughput->sendFixedSeq(emptyFixed);
throughput->recvByteSeq();
throughput->recvStringSeq();
throughput->recvStructSeq();
throughput->recvFixedSeq();
throughput->echoByteSeq(emptyBytes);
throughput->echoStringSeq(emptyStrings);
throughput->echoStructSeq(emptyStructs);
throughput->echoFixedSeq(emptyFixed);
}
throughput->endWarmup();
cout << " ok" << endl;
}
else
{
throughput->ice_ping(); // Initial ping to setup the connection.
}
menu();
//
// By default use byte sequence.
//
char currentType = '1';
int seqSize = ByteSeqSize;
char c;
do
{
try
{
cout << "==> ";
cin >> c;
IceUtil::Time tm = IceUtil::Time::now(IceUtil::Time::Monotonic);
const int repetitions = 1000;
if(c == '1' || c == '2' || c == '3' || c == '4')
{
currentType = c;
switch(c)
{
case '1':
{
cout << "using byte sequences" << endl;
seqSize = ByteSeqSize;
break;
}
case '2':
{
cout << "using string sequences" << endl;
seqSize = StringSeqSize;
break;
}
case '3':
{
cout << "using variable-length struct sequences" << endl;
seqSize = StringDoubleSeqSize;
break;
}
case '4':
{
cout << "using fixed-length struct sequences" << endl;
seqSize = FixedSeqSize;
break;
}
}
}
else if(c == 't' || c == 'o' || c == 'r' || c == 'e')
{
switch(c)
{
case 't':
case 'o':
{
cout << "sending";
break;
}
case 'r':
{
cout << "receiving";
break;
}
case 'e':
{
cout << "sending and receiving";
break;
}
}
cout << ' ' << repetitions;
switch(currentType)
{
case '1':
{
cout << " byte";
break;
}
case '2':
{
cout << " string";
break;
}
case '3':
{
cout << " variable-length struct";
break;
}
case '4':
{
cout << " fixed-length struct";
break;
}
}
cout << " sequences of size " << seqSize;
if(c == 'o')
{
cout << " as oneway";
}
cout << "..." << endl;
for(int i = 0; i < repetitions; ++i)
{
switch(currentType)
{
case '1':
{
switch(c)
{
case 't':
{
throughput->sendByteSeq(byteArr);
break;
}
case 'o':
{
throughputOneway->sendByteSeq(byteArr);
break;
}
case 'r':
{
throughput->recvByteSeq();
break;
}
case 'e':
{
throughput->echoByteSeq(byteArr);
break;
}
}
break;
}
case '2':
{
switch(c)
{
case 't':
{
throughput->sendStringSeq(stringSeq);
break;
}
case 'o':
{
throughputOneway->sendStringSeq(stringSeq);
break;
}
case 'r':
{
throughput->recvStringSeq();
break;
}
case 'e':
{
throughput->echoStringSeq(stringSeq);
break;
}
}
break;
}
case '3':
{
switch(c)
{
case 't':
{
throughput->sendStructSeq(structSeq);
break;
}
case 'o':
{
throughputOneway->sendStructSeq(structSeq);
break;
}
case 'r':
{
throughput->recvStructSeq();
break;
}
case 'e':
{
throughput->echoStructSeq(structSeq);
break;
}
}
break;
}
case '4':
{
switch(c)
{
case 't':
{
throughput->sendFixedSeq(fixedSeq);
break;
}
case 'o':
{
throughputOneway->sendFixedSeq(fixedSeq);
break;
}
case 'r':
{
throughput->recvFixedSeq();
break;
}
case 'e':
{
throughput->echoFixedSeq(fixedSeq);
break;
}
}
break;
}
}
}
tm = IceUtil::Time::now(IceUtil::Time::Monotonic) - tm;
cout << "time for " << repetitions << " sequences: " << tm * 1000 << "ms" << endl;
cout << "time per sequence: " << tm * 1000 / repetitions << "ms" << endl;
int wireSize = 0;
switch(currentType)
{
case '1':
{
wireSize = 1;
break;
}
case '2':
{
wireSize = static_cast<int>(stringSeq[0].size());
break;
}
case '3':
{
wireSize = static_cast<int>(structSeq[0].s.size());
wireSize += 8; // Size of double on the wire.
break;
}
case '4':
{
wireSize = 16; // Size of two ints and a double on the wire.
break;
}
}
double mbit = repetitions * seqSize * wireSize * 8.0 / tm.toMicroSeconds();
if(c == 'e')
{
mbit *= 2;
}
cout << "throughput: " << setprecision(5) << mbit << "Mbps" << endl;
}
else if(c == 's')
{
throughput->shutdown();
}
else if(c == 'x')
{
// Nothing to do
}
else if(c == '?')
{
menu();
}
else
{
cout << "unknown command `" << c << "'" << endl;
menu();
}
}
catch(const Ice::Exception& ex)
{
cerr << ex << endl;
}
}
while(cin.good() && c != 'x');
return EXIT_SUCCESS;
}
void
cameraClient::run(){
jderobot::ImageDataPtr dataPtr;
IceUtil::Time last;
int iterIndex = 0;
int totalRefreshRate = 0;
last=IceUtil::Time::now();
while (!(_done)){
iterIndex ++;
if (pauseStatus){
IceUtil::Mutex::Lock sync(this->controlMutex);
this->semWait.wait(sync);
}
try{
dataPtr = this->prx->getImageData(this->mImageFormat);
cv::Mat image = CameraUtils::getImageFromCameraProxy(dataPtr);
this->controlMutex.lock();
image.copyTo(this->data);
this->newData=true;
this->semBlock.broadcast();
this->controlMutex.unlock();
}
catch(...){
LOG(WARNING) << prefix +"error during request (connection error)";
usleep(50000);
}
int process = (IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds());
if (process > (int)cycle ){
DLOG(WARNING) << "--------" + prefix + " adquisition timeout-";
}
else{
int delay = (int)cycle - process;
if (delay <1 || delay > (int)cycle)
delay = 1;
usleep(delay);
}
int rate =(int)(1000000/(IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds()));
totalRefreshRate = totalRefreshRate + rate;
this->refreshRate= totalRefreshRate / iterIndex;
last=IceUtil::Time::now();
if (iterIndex == INT_MAX)
{
iterIndex = 0;
DLOG(INFO) << "*** Counter reset";
}
}
this->data.release();
}
void
NodeI::check()
{
{
Lock sync(*this);
if(_destroy)
{
return;
}
assert(!_mergeTask);
if(_state == NodeStateElection || _state == NodeStateReorganization || _coord != _id)
{
assert(_checkTask);
_timer->schedule(_checkTask, _electionTimeout);
return;
}
// Next get the set of nodes that were detected as unreachable
// from the replica and remove them from our slave list.
vector<int> dead;
_observers->getReapedSlaves(dead);
if(!dead.empty())
{
for(vector<int>::const_iterator p = dead.begin(); p != dead.end(); ++p)
{
set<GroupNodeInfo>::iterator q = _up.find(GroupNodeInfo(*p));
if(q != _up.end())
{
if(_traceLevels->election > 0)
{
Ice::Trace out(_traceLevels->logger, _traceLevels->electionCat);
out << "node " << _id << ": reaping slave " << *p;
}
_up.erase(q);
}
}
// If we no longer have the majority of the nodes under our
// care then we need to stop our replica.
if(_up.size() < _nodes.size()/2)
{
if(_traceLevels->election > 0)
{
Ice::Trace out(_traceLevels->logger, _traceLevels->electionCat);
out << "node " << _id << ": stopping replica";
}
// Clear _checkTask -- recovery() will reset the
// timer.
assert(_checkTask);
_checkTask = 0;
recovery();
return;
}
}
}
// See if other groups exist for possible merge.
set<int> tmpset;
int max = -1;
for(map<int, NodePrx>::const_iterator p = _nodes.begin(); p != _nodes.end(); ++p)
{
if(p->first == _id)
{
continue;
}
try
{
if(p->second->areYouCoordinator())
{
if(p->first > max)
{
max = p->first;
}
tmpset.insert(p->first);
}
}
catch(const Ice::Exception& ex)
{
if(_traceLevels->election > 0)
{
Ice::Trace out(_traceLevels->logger, _traceLevels->electionCat);
out << "node " << _id << ": call on node " << p->first << " failed: " << ex;
}
}
}
Lock sync(*this);
// If the node state has changed while the mutex has been released
// then bail. We don't schedule a re-check since we're either
// destroyed in which case we're going to terminate or the end of
// the election/reorg will re-schedule the check.
if(_destroy || _state == NodeStateElection || _state == NodeStateReorganization || _coord != _id)
{
_checkTask = 0;
return;
}
// If we didn't find any coordinators then we're done. Reschedule
// the next check and terminate.
if(tmpset.empty())
{
assert(_checkTask);
_timer->schedule(_checkTask, _electionTimeout);
return;
}
// _checkTask == 0 means that the check isn't scheduled.
_checkTask = 0;
if(_traceLevels->election > 0)
{
Ice::Trace out(_traceLevels->logger, _traceLevels->electionCat);
out << "node " << _id << ": highest priority node count: " << max;
}
IceUtil::Time delay = IceUtil::Time::seconds(0);
if(_id < max)
{
// Reschedule timer proportial to p-i.
delay = _mergeTimeout + _mergeTimeout * (max - _id);
if(_traceLevels->election > 0)
{
Ice::Trace out(_traceLevels->logger, _traceLevels->electionCat);
out << "node " << _id << ": scheduling merge in " << delay.toDuration()
<< " seconds";
}
}
assert(!_mergeTask);
_mergeTask = new MergeTask(this, tmpset);
_timer->schedule(_mergeTask, delay);
}