// this sets the property of a puck to a value
osaPuck::Errno osaPuck::SetProperty( Barrett::ID propid,
Barrett::Value propval,
bool verify){
// empty CAN frame
osaCANBusFrame frame;
// pack the property ID and value in a "set" CAN frame
if( PackProperty( frame, Barrett::SET, propid, propval )!=osaPuck::ESUCCESS ){
CMN_LOG_RUN_WARNING << LogPrefix() << "Failed to pack property " << propid
<< std::endl;
return osaPuck::EFAILURE;
}
// send the CAN frame
if( canbus->Send( frame ) != osaCANBus::ESUCCESS ){
CMN_LOG_RUN_ERROR << LogPrefix() << "Failed to send the CAN frame."
<< std::endl;
return osaPuck::EFAILURE;
}
// do we double check that the value was set?
if( verify ){
// If we just changed the status of the puck, give it a bit of time to
// initialize itself
if( propid == Barrett::STATUS && propval == osaPuck::STATUS_READY )
osaSleep( 1.0 );
else
osaSleep( 0.01 );
// query the puck to make sure that the property is set
Barrett::Value recvpropval = rand();
if( GetProperty( propid, recvpropval ) != osaPuck::ESUCCESS ){
CMN_LOG_RUN_WARNING << LogPrefix()<<"Failed to get puck property"
<< std::endl;
return osaPuck::EFAILURE;
}
if( propval != recvpropval ){
CMN_LOG_RUN_WARNING << LogPrefix() << "Oop! Unexpected property value. "
<< "Expected " << propval << " got " << recvpropval
<< std::endl;
return osaPuck::EFAILURE;
}
}
return osaPuck::ESUCCESS;
}
int main( int argc, char** argv ) {
cmnLogger::SetMask( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskFunction( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskDefaultLog( CMN_LOG_ALLOW_ALL );
if( argc != 3 ) {
std::cerr << "Usage: " << argv[0] << " can[?] PID" << std::endl;
return -1;
}
osaSocketCAN can( argv[1], osaCANBus::RATE_1000 );
if( can.Open() != osaCANBus::ESUCCESS ) {
std::cerr << argv[0] << ": Failed to open device " << argv[1] << std::endl;
return -1;
}
std::istringstream iss( argv[2] );
int pid;
iss >> pid;
osaPuck puck( (osaPuck::ID)pid, &can );
// Reset the firmware
if( puck.Reset() != osaPuck::ESUCCESS ) {
std::cerr << ": Failed to reset the puck." << std::endl;
return -1;
}
osaSleep( 1. );
// Ready the puck
if( puck.Ready() != osaPuck::ESUCCESS ) {
std::cerr << ": Failed to ready the puck." << std::endl;
return -1;
}
osaSleep( 1.0 );
// configure the puck
if( puck.InitializeMotor() != osaPuck::ESUCCESS ) {
std::cerr << argv[0] << ": Failed to initialize puck" << std::endl;
}
if( can.Close() != osaCANBus::ESUCCESS ) {
std::cerr << argv[0] << ": Failed to close device " << argv[1] << std::endl;
return -1;
}
return 0;
}
void mtsQtApplication::Run(void)
{
QtApp->exec();
osaSleep(1.0 * cmn_s);
Timer->stop();
Kill();
}
bool mtsSartoriusSerial::GetWeight(double & weightInGrams, bool & stable)
{
CMN_LOG_CLASS_RUN_DEBUG << "GetWeight: entering method" << std::endl;
unsigned int nbTrials;
nbTrials = 10;
bool enoughData = false;
while ((!enoughData) && (nbTrials > 0)) {
nbTrials--;
// add whatever we can read to main buffer, read potentialy as
// many characters as space left in buffer
if (this->NbBytesReadSoFar < BUFFER_SIZE) {
this->NbBytesReadSoFar +=
this->SerialPort.Read(this->BytesReadSoFar + this->NbBytesReadSoFar,
BUFFER_SIZE - this->NbBytesReadSoFar);
CMN_LOG_CLASS_RUN_DEBUG << "GetWeight: buffer now contains "
<< this->NbBytesReadSoFar << " characters"
<< std::endl;
enoughData = this->ProcessBuffer();
weightInGrams = this->Weight.Data;
} else {
// looks like we have an empty buffer, empty it and hope to re-sync with scale
CMN_LOG_CLASS_RUN_ERROR << "GetWeight: buffer is full (" << BUFFER_SIZE << " chars)" << std::endl;
this->BytesReadSoFar[BUFFER_SIZE - 1] = '\0';
CMN_LOG_CLASS_RUN_DEBUG << "GetWeight: buffer contains: " << this->BytesReadSoFar << std::endl;
this->NbBytesReadSoFar = 0;
}
osaSleep(1.0 * cmn_ms); // tiny sleep
}
if (!enoughData) {
CMN_LOG_RUN_VERBOSE << "GetWeight failed, not enought data" << std::endl;
return false;
}
return true;
}
void mtsSartoriusSerial::Run(void)
{
this->ProcessQueuedCommands();
bool stable;
this->GetWeight(this->Weight.Data, stable);
osaSleep(1.0 * cmn_ms); // way smaller than delay from scale
}
vctDynamicNArray<unsigned char,3> osaOSGStereo::GetRGBPlanarImage(size_t idx){
// Get the left/right slave
const osg::View::Slave& slave = getSlave( idx );
osg::Camera* camera = slave._camera.get();
// get the camera final draw callback
osg::Camera::DrawCallback* dcb = NULL;
dcb = camera->getFinalDrawCallback();
osg::ref_ptr<osg::Referenced> ref = dcb->getUserData();
osg::ref_ptr<osaOSGCamera::FinalDrawCallback::Data> data = NULL;
data = dynamic_cast<osaOSGCamera::FinalDrawCallback::Data*>( ref.get() );
if( data != NULL ){
data->RequestRGBImage();
osaSleep( 1.0 );
cv::Mat rgbimage = data->GetRGBImage();
cv::Size size = rgbimage.size();
vctDynamicNArray<unsigned char, 3> x;
x.SetSize( vctDynamicNArray<unsigned char, 3>::nsize_type( size.height,
size.width, 3 ) );
memcpy( x.Pointer(),
rgbimage.ptr<unsigned char>(),
size.height*size.width*3 );
return x;
}
return vctDynamicNArray<unsigned char, 3>();
}
osaOSGMono::Errno osaOSGMono::GetDepthImage( cv::Mat& depth ){
// get the camera final draw callback
osg::Camera::DrawCallback* dcb = NULL;
dcb = getCamera()->getFinalDrawCallback();
if( dcb != NULL ){
osg::ref_ptr<osg::Referenced> ref = dcb->getUserData();
osg::ref_ptr<osaOSGCamera::FinalDrawCallback::Data> data = NULL;
data = dynamic_cast<osaOSGCamera::FinalDrawCallback::Data*>( ref.get() );
if( data != NULL ){
data->RequestDepthImage();
osaSleep( 1.0 );
depth = data->GetDepthImage();
}
else{
CMN_LOG_RUN_ERROR << "Could not get the user data from the callback"
<< std::endl;
return osaOSGMono::EFAILURE;
}
return osaOSGMono::ESUCCESS;
}
else{
CMN_LOG_RUN_ERROR << "Could not get the drawing callback" << std::endl;
return osaOSGMono::EFAILURE;
}
}
void * Method(ThreadSignalMethodArguments * argument) {
unsigned int index;
osaSleep(argument->DelayToStart);
for (index = 0;
index < argument->NumberOfIterations;
index++) {
argument->TimerWait.Start();
argument->ThreadSignal->Wait();
argument->TimerWait.Stop();
argument->TimerBlock.Start();
osaSleep(argument->DelayToUnlock);
argument->TimerBlock.Stop();
argument->ThreadSignal->Raise();
osaSleep(argument->DelayToLock);
}
return 0;
}
//#define _COMMUNICATION_TEST_
void mtsManagerProxyClient::ManagerClientI::Run()
{
#ifdef _COMMUNICATION_TEST_
int count = 0;
while (IsActiveProxy()) {
osaSleep(1 * cmn_s);
std::cout << "\tClient [" << ManagerProxyClient->GetProxyName() << "] running (" << ++count << ")" << std::endl;
std::stringstream ss;
ss << "Msg " << count << " from Client " << ManagerProxyClient->GetProxyName();
ManagerProxyClient->SendTestMessageFromClientToServer(ss.str());
}
#else
double lastTickChecked = 0.0, now;
while (IsActiveProxy()) {
now = osaGetTime();
if (now < lastTickChecked + mtsProxyConfig::RefreshPeriodForManagers) {
osaSleep(10 * cmn_ms);
continue;
}
lastTickChecked = now;
try {
Server->Refresh();
} catch (const ::Ice::Exception & ex) {
LogError(mtsManagerProxyClient, "refresh failed (" << Server->ice_toString() << ")" << std::endl << ex);
if (ManagerProxyClient) {
ManagerProxyClient->OnServerDisconnect(ex);
}
}
}
#endif
#ifdef ENABLE_DETAILED_MESSAGE_EXCHANGE_LOG
LogPrint(mtsManagerProxyClient, "mtsManagerProxyClient::ManagerClientI - terminated");
#endif
}
int main(int CMN_UNUSED(argc), char ** CMN_UNUSED(argv))
{
// log configuration
cmnLogger::SetMask(CMN_LOG_ALLOW_ALL);
cmnLogger::SetMaskDefaultLog(CMN_LOG_ALLOW_ALL);
cmnLogger::AddChannel(std::cerr, CMN_LOG_ALLOW_ERRORS_AND_WARNINGS);
mtsManagerLocal * manager = mtsManagerLocal::GetInstance();
// components
mtsKeyboard * keyboard = new mtsKeyboard;
mtsTextToSpeech * textToSpeech = new mtsTextToSpeech;
// add an interface to handle keyboard events "Key"
textToSpeech->AddInterfaceRequiredForEventCharacter("Keyboard", "Key");
manager->AddComponent(keyboard);
manager->AddComponent(textToSpeech);
manager->Connect(textToSpeech->GetName(), "Keyboard",
keyboard->GetName(), "Keyboard");
// create the component threads
manager->CreateAll();
manager->WaitForStateAll(mtsComponentState::READY, 2.0 * cmn_s);
// start the periodic Run
manager->StartAll();
manager->WaitForStateAll(mtsComponentState::ACTIVE , 2.0 * cmn_s);
std::cout << "Hit 'q' to quit. Any other key should trigger text to speech." << std::endl;
while (!keyboard->Done()) {
std::cout << "." << std::flush;
osaSleep(1.0 * cmn_s);
}
std::cout << std::endl;
// cleanup
manager->KillAll();
manager->WaitForStateAll(mtsComponentState::FINISHED, 2.0 * cmn_s);
manager->Cleanup();
// delete component
delete textToSpeech;
delete keyboard;
// stop all logs
cmnLogger::Kill();
return 0;
}
int main(void)
{
// log configuration
cmnLogger::SetMask(CMN_LOG_ALLOW_ALL);
cmnLogger::AddChannel(std::cout, CMN_LOG_ALLOW_ERRORS_AND_WARNINGS);
// specify a higher, more verbose log level for these classes
cmnClassRegister::SetLogMaskClassMatching("mts", CMN_LOG_ALLOW_ALL);
// create our two tasks
const double PeriodDisplay = 10.0; // in milliseconds
mtsComponentManager * componentManager = mtsComponentManager::GetInstance();
displayTask * displayTaskObject =
new displayTask("DISP", PeriodDisplay * cmn_ms);
displayTaskObject->Configure();
componentManager->AddComponent(displayTaskObject);
// name as defined in Sensable configuration
mtsNovintHDL * robotObject = new mtsNovintHDL("Novint", "Novint");
componentManager->AddComponent(robotObject);
// connect the tasks
componentManager->Connect("DISP", "Robot", "Novint", "Novint");
componentManager->Connect("DISP", "Button1", "Novint", "NovintButton1");
componentManager->Connect("DISP", "Button2", "Novint", "NovintButton2");
// create the tasks, i.e. find the commands
componentManager->CreateAll();
componentManager->WaitForStateAll(mtsComponentState::READY, 2.0 * cmn_s);
// start the periodic Run
componentManager->StartAll();
componentManager->WaitForStateAll(mtsComponentState::ACTIVE, 2.0 * cmn_s);
// wait until the close button of the UI is pressed
while (true) {
osaSleep(10.0 * cmn_ms); // sleep to save CPU
if (displayTaskObject->GetExitFlag()) {
break;
}
}
// cleanup
componentManager->KillAll();
componentManager->WaitForStateAll(mtsComponentState::FINISHED, 2.0 * cmn_s);
componentManager->Cleanup();
cmnLogger::Kill();
return 0;
}
int svlFilterCapFramerate::Process(svlProcInfo * procInfo, svlSample * syncInput, svlSample * & syncOutput)
{
syncOutput = syncInput;
_OnSingleThread(procInfo)
{
const double currentTime = mtsComponentManager::GetInstance()->GetTimeServer().GetRelativeTime();
const double timeSinceLastFrame = currentTime - this->TimeForLastFrame;
this->TimeForLastFrame = currentTime;
if (timeSinceLastFrame < this->DesiredFrameInterval) {
osaSleep(this->DesiredFrameInterval - timeSinceLastFrame);
}
}
return SVL_OK;
}
int main(int argc, char * argv[])
{
// Enable system-wide thread-safe Logger
mtsManagerLocal::SetLogForwarding(true);
std::string globalComponentManagerIP;
// Set global component manager's ip address
if (argc < 2)
globalComponentManagerIP = "localhost";
else
globalComponentManagerIP = argv[1];
// Get the TaskManager instance and set operation mode
mtsManagerLocal * taskManager;
try {
taskManager = mtsManagerLocal::GetInstance(globalComponentManagerIP, "ProcessIRE");
} catch (...) {
CMN_LOG_INIT_ERROR << "Failed to initialize local component manager" << std::endl;
return 1;
}
cmnObjectRegister::Register("TaskManager", taskManager);
IRE_Shell shell = IRE_WXPYTHON;
if ((argc > 2) && (strncmp(argv[2], "ipy", 3) == 0))
shell = IRE_IPYTHON;
ireTask *ire = new ireTask("IRE", shell); // Could add startup string as third parameter
taskManager->AddComponent(ire);
if (!taskManager->Connect(
"ProcessIRE",
"IRE",
mtsManagerComponentBase::InterfaceNames::InterfaceSystemLoggerRequired,
mtsManagerLocal::ProcessNameOfLCMWithGCM,
mtsManagerComponentBase::ComponentNames::ManagerComponentServer,
mtsManagerComponentBase::InterfaceNames::InterfaceSystemLoggerProvided))
CMN_LOG_INIT_ERROR << "Failed to connect system-wide thread-safe logger" << std::endl;
taskManager->CreateAll();
taskManager->StartAll();
// Loop until IRE is exited
while (!ire->IsTerminated())
osaSleep(0.5 * cmn_s); // Wait 0.5 seconds
taskManager->Cleanup();
return 0;
}
void mtsManagerComponentServer::InterfaceGCMCommands_GetAbsoluteTimeDiffs(const std::vector<std::string> & processNames,
std::vector<double> & processTimes) const
{
double sleepTime = 0.1;
unsigned int numOfTrials = 10;
vctDynamicVector<double> trialTimes(numOfTrials);
double time;
mtsExecutionResult result;
// Make sure return vector has correct size
processTimes.resize(processNames.size());
mtsManagerLocal * LCM = mtsManagerLocal::GetInstance();
const std::string thisProcess = LCM->GetProcessName();
for (unsigned int i = 0; i < processNames.size(); i++) {
processTimes[i] = 0.0;
if (processNames[i] == thisProcess)
continue;
InterfaceGCMFunctionType * functionSet = InterfaceGCMFunctionMap.GetItem(processNames[i], CMN_LOG_LEVEL_NONE);
if (!functionSet) {
CMN_LOG_CLASS_RUN_ERROR << "GetAbsoluteTimeDiffs: could not find functions for process " << processNames[i] << std::endl;
continue;
}
trialTimes.Zeros();
for (unsigned int t = 0; t < numOfTrials; t++) {
double tic = LCM->GetTimeServer().GetAbsoluteTimeInSeconds();
result = functionSet->GetAbsoluteTimeInSeconds(time);
double roundTripTime = LCM->GetTimeServer().GetAbsoluteTimeInSeconds() - tic;
if (result.IsOK()) {
trialTimes[i] = (tic + roundTripTime/2.0) - time;
}
else {
CMN_LOG_CLASS_RUN_ERROR << "GetAbsoluteTimeDiffs: could not execute command for process " << processNames[i]
<< ", result = " << result << std::endl;
trialTimes.Zeros();
break;
}
osaSleep(sleepTime);
}
processTimes[i] = trialTimes.SumOfElements()/numOfTrials;
}
}
bool mtsComponentViewer::ConnectToUDrawGraph(void)
{
// Try to connect to UDrawGraph
std::vector<std::string> arguments;
arguments.push_back("-pipe");
UDrawPipeConnected = UDrawPipe.Open("uDrawGraph", arguments, "rw");
// wait for initial OK
if (UDrawPipeConnected) {
WaitingForResponse = true;
UDrawResponse = "";
// Create reader thread.
ReaderThread.Create<mtsComponentViewer, int>(this, &mtsComponentViewer::ReadFromUDrawGraph, 0);
// Wait for initial OK
for (int i = 0; (i < 100) && (UDrawResponse == ""); i++)
osaSleep(0.01);
ProcessResponse();
if (WaitingForResponse) {
CMN_LOG_CLASS_RUN_ERROR << "ConnectToUDrawGraph: Failed to receive response" << std::endl;
}
/// Now initialize the system
WriteString(UDrawPipe, "window(title(\"CISST Component Viewer\"))\n");
WriteString(UDrawPipe, "app_menu(create_menus([menu_entry(\"redraw\", \"Redraw graph\"), "
#if CISST_MTS_HAS_ICE
"menu_entry(\"showproxies\", \"Show proxies\"), "
"menu_entry(\"hideproxies\", \"Hide proxies\"), "
#endif
"blank, "
"menu_entry(\"connectStart\", \"Connect Start...\"), "
"menu_entry(\"connectFinish\", \"Connect Finish\"), "
"menu_entry(\"connectCancel\", \"Connect Cancel\")]))\n");
// Activate menu items (context-sensitive)
ActivateMenuItems();
// Default node rules (nr) could be removed -- not currently used
WriteString(UDrawPipe, "visual(new_rules([er(\"CONNECTION\", [m([menu_entry(\"disconnect\", \"Disconnect\")])]), "
"nr(\"USER\", [m([menu_entry(\"start\", \"Start\"), "
"menu_entry(\"stop\", \"Stop\")])])]))\n");
WriteString(UDrawPipe, "drag_and_drop(dragging_on)\n");
// Following is an example of how to create a TCL/TK window; in this case, a window with a single button "Connect"
// that returns the message "connectPressed".
// WriteString(UDrawPipe, "tcl(eval(\"button .b -text Connect -command {uDrawGraph tcl_answer connectPressed} ; pack .b\"))\n");
CMN_LOG_CLASS_INIT_VERBOSE << "Connected to UDraw(Graph)" << std::endl;
}
return UDrawPipeConnected;
}
int svlVideoCodecTCPStream::Open(const std::string &filename, unsigned int &width, unsigned int &height, double &framerate)
{
if (Opened || ParseFilename(filename) != SVL_OK) return SVL_FAIL;
std::string extensionlist(GetExtensions());
std::string extension;
svlVideoIO::GetExtension(filename, extension);
if (extension == "ncvi") Compressor = CVI;
else if (extension == "njpg") Compressor = JPEG;
else return SVL_FAIL;
while (1) {
Opened = true;
Writing = false;
Width = Height = 0;
// Releasing existing buffers
delete [] yuvBuffer;
yuvBuffer = 0;
yuvBufferSize = 0;
AccumulatedSize = 0;
// Start data receiving thread
ReceiveInitialized = false;
KillReceiveThread = false;
ReceiveThread = new osaThread;
ReceiveInitEvent = new osaThreadSignal;
ReceiveThread->Create<svlVideoCodecTCPStream, int>(this, &svlVideoCodecTCPStream::ReceiveProc, 0);
ReceiveInitEvent->Wait();
if (ReceiveInitialized == false) break;
// Wait until header is received
while (Width == 0 || Height == 0) osaSleep(0.1);
Pos = BegPos = EndPos = 0;
width = Width;
height = Height;
framerate = -1.0;
return SVL_OK;
}
Close();
return SVL_FAIL;
}
cv::Mat osaOSGStereo::GetRGBImage( size_t idx ) {
// Get the left/right slave
const osg::View::Slave& slave = getSlave( idx );
osg::ref_ptr< osg::Camera > camera = slave._camera.get();
osg::ref_ptr< osg::Camera::DrawCallback > dcb;
dcb = getCamera()->getFinalDrawCallback();
//
osg::ref_ptr<osg::Referenced> ref = dcb->getUserData();
osg::ref_ptr<osaOSGCamera::FinalDrawCallback::Data> data = NULL;
data = dynamic_cast<osaOSGCamera::FinalDrawCallback::Data*>( ref.get() );
if( data != NULL ){
data->RequestRGBImage();
osaSleep( 1.0 );
return data->GetRGBImage();
}
return cv::Mat();
}
void osaTripleBufferTest::TestMultiThreading(void)
{
value_type referenceVector;
referenceVector.SetSize(TestVectorSize);
referenceVector.SetAll(0);
osaTripleBuffer<value_type > tripleBuffer(referenceVector);
CPPUNIT_ASSERT_EQUAL(TestVectorSize, tripleBuffer.LastWriteNode->Pointer->size());
CPPUNIT_ASSERT_EQUAL(TestVectorSize, tripleBuffer.LastWriteNode->Next->Pointer->size());
CPPUNIT_ASSERT_EQUAL(TestVectorSize, tripleBuffer.LastWriteNode->Next->Next->Pointer->size());
osaThread readThread;
readThread.Create(osaTripleBufferTestReadThread, &tripleBuffer);
osaThread writeThread;
writeThread.Create(osaTripleBufferTestWriteThread, &tripleBuffer);
while (!(ReadThreadDone && WriteThreadDone)) {
osaSleep(1.0 * cmn_ms);
}
CPPUNIT_ASSERT(!ErrorFoundInRead);
}
_ParallelLoop(procInfo, idx, videochannels)
{
if (Codec[idx]) {
// Seek (if needed) to frame that needs to be extracted
pos = Codec[idx]->GetPos();
Position[idx] = pos;
if (UseRange[idx]) {
// Check if position is outside of the playback segment
if (pos < Range[idx][0]) {
Codec[idx]->SetPos(Range[idx][0]);
ResetTimer = true;
}
else if (pos > Range[idx][1]) {
if (!GetLoop()) {
ret = SVL_STOP_REQUEST;
break;
}
else {
Codec[idx]->SetPos(Range[idx][0]);
ResetTimer = true;
}
}
}
// Extract frame
ret = Codec[idx]->Read(0, *OutputImage, idx, true);
// Manage looped playback and errors
if (ret == SVL_VID_END_REACHED) {
if (!GetLoop()) {
ret = SVL_STOP_REQUEST;
break;
}
else {
// Loop around
ret = Codec[idx]->Read(0, *OutputImage, idx, true);
}
}
if (ret != SVL_OK) {
CMN_LOG_CLASS_INIT_ERROR << "Process: failed to read video frame on channel: " << idx << std::endl;
break;
}
// Run timer based in the first video channel
if (idx == 0) {
// Get timestamp stored in the video file
timestamp = Codec[idx]->GetTimestamp();
if (timestamp > 0.0) {
if (!IsTargetTimerRunning()) {
// Try to keep orignal frame intervals
if (ResetTimer || Codec[idx]->GetPos() == 1) {
FirstTimestamp = timestamp;
Timer.Reset();
Timer.Start();
ResetTimer = false;
}
else {
timespan = (timestamp - FirstTimestamp) - Timer.GetElapsedTime();
if (timespan > 0.0) osaSleep(timespan);
}
}
}
OutputImage->SetTimestamp(timestamp);
}
}
}
int main(int argc, char * argv[])
{
if (argc != 2) {
std::cerr << "Usage: " << argv[0] << " GlobalManagerIP" << std::endl;
exit(-1);
}
//*
// before we start, estimate overhead related to serialization and de-serialization of parameters
osaTimeServer timeServer;
std::stringstream serializationStream;
cmnSerializer serialization(serializationStream);
cmnDeSerializer deSerialization(serializationStream);
unsigned int index;
prmPositionCartesianGet parameter;
cmnGenericObject * generated;
timeServer.SetTimeOrigin();
for (index = 0; index < confNumberOfSamples; index++) {
serialization.Serialize(parameter);
generated = deSerialization.DeSerialize();
if (generated->Services() != parameter.Services()) {
std::cout << "Serialization test failed!" << std::endl;
exit(0);
}
// delete generated object created by de-serialization
delete generated;
}
double elapsedTime = timeServer.GetRelativeTime();
std::cout << std::endl << std::endl
<< "Serialization, dynamic creation and deserialization time for "
<< confNumberOfSamples << " samples of " << parameter.Services()->GetName()
<< ": " << cmnInternalTo_ms(elapsedTime) << " (ms)" << std::endl
<< "Per sample: " << cmnInternalTo_ms(elapsedTime / confNumberOfSamples) << " (ms)" << std::endl
<< std::endl << std::endl;
//*/
// networking part
std::string globalTaskManagerIP(argv[1]);
// Log configuration
cmnLogger::SetMask(CMN_LOG_ALLOW_ERRORS_AND_WARNINGS);
cmnLogger::SetMaskFunction(CMN_LOG_ALLOW_ERRORS_AND_WARNINGS);
// create our server task
clientTask * client = new clientTask("Client", confClientPeriod);
// Get the TaskManager instance and set operation mode
mtsTaskManager * taskManager = mtsTaskManager::GetInstance();
taskManager->AddComponent(client);
// Connect the tasks across networks
taskManager->Connect("Client", "Required", "Server", "Provided");
// create the tasks, i.e. find the commands
taskManager->CreateAll();
// start the periodic Run
taskManager->StartAll();
// run while the benchmarks are not over
while (!(client->IsBenchmarkCompleted())) {
osaSleep(10.0 * cmn_ms);
}
// cleanup
taskManager->Cleanup();
taskManager->KillAll();
// To prevent crash due to CMN_LOG_CLASS (cmnLODOutputMultiplexer).
osaSleep(0.5 * cmn_s);
return 0;
}
int main(int argc, char * argv[])
{
// log configuration
cmnLogger::SetMask(CMN_LOG_ALLOW_ALL);
cmnLogger::SetMaskFunction(CMN_LOG_ALLOW_ALL);
cmnLogger::SetMaskDefaultLog(CMN_LOG_ALLOW_ALL);
cmnLogger::AddChannel(std::cout, CMN_LOG_ALLOW_ERRORS_AND_WARNINGS);
// set the log level of detail on select components
cmnLogger::SetMaskClassMatching("mts", CMN_LOG_ALLOW_ALL);
// enable system-wide thread-safe logging
#ifdef MTS_LOGGING
mtsManagerLocal::SetLogForwarding(true);
#endif
std::string globalComponentManagerIP;
int stateCollectionFlag = 0;
if (argc != 2) {
std::cerr << "Usage: " << argv[0] << " [GlobalManagerIP] [flagForDataCollection]" << std::endl;
std::cerr << " GlobalManagerIP is set as 127.0.0.1 by default" << std::endl;
}
// Set global component manager's ip and argument type
// If flag is not specified, or not 1, then use generic type (mtsDouble)
if (argc == 1) {
globalComponentManagerIP = "localhost";
} else if (argc == 2) {
globalComponentManagerIP = argv[1];
}else if (argc == 3) {
globalComponentManagerIP = argv[1];
stateCollectionFlag = atoi(argv[2]);
} else {
exit(-1);
}
std::cout << "Starting server, IP = " << globalComponentManagerIP << " 'q' to stop" << std::endl;
if(stateCollectionFlag){
std::cout << " with data collection. 's' to start/stop data collection" << std::endl;
}
// Get the TaskManager instance and set operation mode
mtsManagerLocal * componentManager;
try {
componentManager = mtsManagerLocal::GetInstance(globalComponentManagerIP, "ProcessClient");
} catch (...) {
CMN_LOG_INIT_ERROR << "Failed to initialize local component manager" << std::endl;
return 1;
}
// client
mtsMedtronicStealthlinkExampleComponent * componentExample;
if(stateCollectionFlag)
componentExample= new mtsMedtronicStealthlinkExampleComponent("Example", 50 * cmn_ms, true);
else
componentExample = new mtsMedtronicStealthlinkExampleComponent("Example", 50 * cmn_ms);
// add the components to the component manager
componentManager->AddComponent(componentExample);
// Connect the test component to the Stealthlink component
if (!componentManager->Connect("ProcessClient", componentExample->GetName(), "Stealthlink",
"ProcessServer", "Stealthlink", "Controller")) {
CMN_LOG_INIT_ERROR << "Could not connect test component to Stealthlink component." << std::endl;
return 0;
}
// Now, connect to the tools (we assume these are pre-defined in the XML file)
if (!componentManager->Connect("ProcessClient", componentExample->GetName(), "Pointer",
"ProcessServer", "Stealthlink", "Pointer")) {
CMN_LOG_INIT_WARNING << "Could not connect test component to Pointer tool." << std::endl;
}
if (!componentManager->Connect("ProcessClient", componentExample->GetName(), "Frame",
"ProcessServer", "Stealthlink", "Frame")) {
CMN_LOG_INIT_WARNING << "Could not connect test component to Frame tool." << std::endl;
}
// Connect the registration interface
if (!componentManager->Connect("ProcessClient", componentExample->GetName(), "Registration",
"ProcessServer", "Stealthlink", "Registration")) {
CMN_LOG_INIT_ERROR << "Could not connect test component to Registration interface." << std::endl;
return 0;
}
// Connect the exam information interface
if (!componentManager->Connect("ProcessClient", componentExample->GetName(), "ExamInformation",
"ProcessServer", "Stealthlink", "ExamInformation")) {
CMN_LOG_INIT_ERROR << "Could not connect test component to ExamInformation interface." << std::endl;
return 0;
}
if(stateCollectionFlag)
{
if (!componentManager->Connect("ProcessClient", componentExample->GetName(), "CollectorState",
"ProcessServer", "StealthlinkStateCollector", "Control")) {
CMN_LOG_INIT_ERROR << "Could not connect test component to Control interface." << std::endl;
return 0;
}
}
// create the tasks, i.e. find the commands
componentManager->CreateAll();
componentManager->WaitForStateAll(mtsComponentState::READY);
// start the periodic Run
componentManager->StartAll();
componentManager->WaitForStateAll(mtsComponentState::ACTIVE);
// wait for keyboard input in command window
int ch;
// execution result used by all functions
mtsExecutionResult executionResult;
bool GCMActive = true;
bool started = false;
while (GCMActive && ch != 'q') {
osaSleep(5.0 * cmn_ms);
GCMActive = componentManager->IsGCMActive();
ch = cmnGetChar();
switch (ch) {
case 's':
if(started)
{
if(componentExample->CollectorState.StopCollection.IsValid())
{
executionResult = componentExample->CollectorState.StopCollection();
osaSleep(0.1 * cmn_s);
std::cout << "Stop data collection" << std::endl;
started = false;
}
}
else
{
if(componentExample->CollectorState.StartCollection.IsValid())
{
executionResult = componentExample->CollectorState.StartCollection();
osaSleep(0.1 * cmn_s);
if(executionResult.IsOK())
{
std::cout << "Start data collection" << std::endl;
started = true;
}
else
std::cout << "StartCollection failed to execute." << std::endl;
}
}
break;
default:
break;
}
}
if (!GCMActive) {
CMN_LOG_RUN_ERROR << "Global Component Manager is disconnected" << std::endl;
}
// cleanup
componentManager->KillAll();
componentManager->WaitForStateAll(mtsComponentState::FINISHED, 20.0 * cmn_s);
// delete components
delete componentExample;
componentManager->Cleanup();
return 0;
}
void mtsOptoforce3D::Run(void)
{
struct optopacket {
unsigned char header[4];
unsigned short count;
unsigned short status;
short fx;
short fy;
short fz;
unsigned short checksum;
};
union PacketDataType {
unsigned char bytes[16];
optopacket packet;
};
PacketDataType buffer;
ProcessQueuedCommands();
if (connected) {
bool found = false;
unsigned short recvChecksum;
#if (CISST_OS == CISST_WINDOWS)
// On Windows, serialPort.Read seems to always return the requested number
// of characters, which is sizeof(buffer).
// Thus, we have to check whether part of the expected packet has been combined
// with another packet, such as the 7 byte response to the command sent to the sensor.
int n = serialPort.Read((char *)&buffer, sizeof(buffer));
while (!found) {
for (int i = 0; i < n - 3; i++) {
if ((buffer.bytes[i] == 170) && (buffer.bytes[i + 1] == 7)
&& (buffer.bytes[i + 2] == 8) && (buffer.bytes[i + 3] == 10)) {
if (i != 0) { // If pattern not found at beginning of buffer
memmove(buffer.bytes, buffer.bytes + i, n - i); // shift so that 170 is in buffer[0]
serialPort.Read(buffer.bytes + n - i, i); // fill the rest of the buffer
}
found = true;
break;
}
}
if (!found) { // If pattern not yet found
memmove(buffer.bytes, buffer.bytes + n - 4, 4); // move last 4 characters to beginning of buffer
serialPort.Read(buffer.bytes + 4, sizeof(buffer.bytes) - 4); // get another 12 characters
}
}
// Now, process the data
RawSensor.X() = (double)static_cast<short>(_byteswap_ushort(buffer.packet.fx)) * scale.X();
RawSensor.Y() = (double)static_cast<short>(_byteswap_ushort(buffer.packet.fy)) * scale.Y();
RawSensor.Z() = (double)static_cast<short>(_byteswap_ushort(buffer.packet.fz)) * scale.Z();
Count = _byteswap_ushort(buffer.packet.count);
Status = _byteswap_ushort(buffer.packet.status);
recvChecksum = _byteswap_ushort(buffer.packet.checksum);
#else
// On Linux, serialPort.Read seems to return a complete packet, even if it is less than the
// requested size.
// Thus, we can discard packets that are not the correct size.
while (!found) {
if (serialPort.Read((char *)&buffer, sizeof(buffer)) == sizeof(buffer)) {
// Check for expected 4 byte packet header
found = ((buffer.bytes[0] == 170) && (buffer.bytes[1] == 7)
&& (buffer.bytes[2] == 8) && (buffer.bytes[3] == 10));
}
}
// Now, process the data
RawSensor.X() = (double)static_cast<short>(bswap_16(buffer.packet.fx)) * scale.X();
RawSensor.Y() = (double)static_cast<short>(bswap_16(buffer.packet.fy)) * scale.Y();
RawSensor.Z() = (double)static_cast<short>(bswap_16(buffer.packet.fz)) * scale.Z();
Count = bswap_16(buffer.packet.count);
Status = bswap_16(buffer.packet.status);
recvChecksum = bswap_16(buffer.packet.checksum);
#endif
// Verify the checksum (last 2 bytes).
unsigned short checksum = buffer.bytes[0];
for (size_t i = 1; i < sizeof(buffer) - 2; i++)
checksum += buffer.bytes[i];
// (Status == 0) means no errors or overload warnings.
// For now, we check ((Status&0xFC00) == 0), which ignores overload warnings.
bool valid = (checksum == recvChecksum) && ((Status & 0xFC00) == 0);
ForceTorque.SetValid(valid);
if (valid) {
if (calib_valid) {
Force = RawSensor;
}
else if (matrix_a_valid) {
// Obtain forces by applying the calibration matrix, which depends on sensor-specific calibration
// values (A) and the assumed length to where the forces are applied (Length), which determines matrix_l (L).
// The calibration matrix, matrix_cal, is equal to inv(A*L)
Force = matrix_cal*RawSensor - bias; // F = inv(A*L)*S - bias
// Stablize the sensor readings
ForceCurrent.Assign(Force);
for (int i=0; i<3; i++) {
if(fabs(ForceCurrent.Element(i)) < 0.2)
ForceCurrent.Element(i) = 0.0;
}
Force.Assign(ForceCurrent);
for (int i=0; i<3; i++)
Force.Element(i) = 0.5*ForceCurrent.Element(i) +
0.5*ForcePrevious.Element(i);
ForcePrevious.Assign(Force);
}
else {
Force = RawSensor - bias;
}
ForceTorque.SetForce(vctDouble6(Force.X(), Force.Y(), Force.Z(), 0.0, 0.0, 0.0));
}
}
else {
ForceTorque.SetValid(false);
osaSleep(0.1); // If not connected, wait
}
}
int main(int argc, char *argv[])
{
srand(time(NULL));
cmnLogger::AddChannel(std::cout, CMN_LOG_ALLOW_ERRORS_AND_WARNINGS);
// Enable safety framework
mtsComponentManager::InstallSafetyCoordinator();
mtsManagerLocal * ComponentManager = 0;
try {
ComponentManager = mtsComponentManager::GetInstance();
} catch (...) {
std::cerr << "Failed to initialize local component manager" << std::endl;
return 1;
}
// Create and setup components
SensorWrapper sensorWrapper("Sensor", PERIOD);
Control control("Control", PERIOD);
UI ui("UI", PERIOD);
SCASSERT(ComponentManager->AddComponent(&sensorWrapper));
SCASSERT(ComponentManager->AddComponent(&control));
SCASSERT(ComponentManager->AddComponent(&ui));
// Read json file
std::string fileName(JSON_FOLDER"/Sensor.json");
if (!GetSafetyCoordinator->ReadConfigFile(fileName)) {
std::cerr << "Failed to read json file: " << fileName << std::endl;
return 1;
}
fileName = JSON_FOLDER"/Control.json";
if (!GetSafetyCoordinator->ReadConfigFile(fileName)) {
std::cerr << "Failed to read json file: " << fileName << std::endl;
return 1;
}
CONNECT_LOCAL(sensorWrapper.GetName(), "SensorValue",
control.GetName(), "ReadSensorValue");
CONNECT_LOCAL(control.GetName(), "ControlValue",
ui.GetName(), "ReadControlValue");
// Install data collector
mtsCollectorState collectorSensor(sensorWrapper.GetName(),
sensorWrapper.GetDefaultStateTableName(),
mtsCollectorBase::COLLECTOR_FILE_FORMAT_CSV);
collectorSensor.AddSignal("SensorValue");
collectorSensor.AddSignal("SensorValue2");
SCASSERT(ComponentManager->AddComponent(&collectorSensor));
collectorSensor.Connect();
ComponentManager->CreateAll();
ComponentManager->WaitForStateAll(mtsComponentState::READY);
ComponentManager->StartAll();
ComponentManager->WaitForStateAll(mtsComponentState::ACTIVE);
// start data collection
collectorSensor.StartCollection(0.0);
std::cout << "Press 'q' to quit." << std::endl;
// loop until 'q' is pressed
int key = ' ';
while (key != 'q') {
key = cmnGetChar();
osaSleep(100 * cmn_ms);
}
std::cout << std::endl;
// stop data collection
collectorSensor.StopCollection(0.0);
std::cout << "Cleaning up... " << std::flush;
ComponentManager->KillAll();
ComponentManager->WaitForStateAll(mtsComponentState::FINISHED, 2.0 * cmn_s);
ComponentManager->Cleanup();
std::cout << "Done\n";
return 0;
}
int svlFilterSourceVideoFile::Process(svlProcInfo* procInfo, svlSample* &syncOutput)
{
syncOutput = OutputImage;
// Try to keep target frequency
_OnSingleThread(procInfo) WaitForTargetTimer();
unsigned int idx, videochannels = OutputImage->GetVideoChannels();
double timestamp, timespan;
int pos, ret = SVL_OK;
// TO DO: add a little more sophisticated logic here
if (Codec[0]->IsMultithreaded()) {
// Codecs are multithreaded, so it's worth
// splitting work between all threads
for (idx = 0; idx < videochannels; idx ++) {
if (Codec[idx]) {
// Seek (if needed) to frame that needs to be extracted
_OnSingleThread(procInfo)
{
pos = Codec[idx]->GetPos();
Position[idx] = pos;
if (UseRange[idx]) {
// Check if position is outside of the playback segment
if (pos < Range[idx][0]) {
Codec[idx]->SetPos(Range[idx][0]);
ResetTimer = true;
}
else if (pos > Range[idx][1]) {
if (!GetLoop()) {
ret = SVL_STOP_REQUEST;
break;
}
else {
Codec[idx]->SetPos(Range[idx][0]);
ResetTimer = true;
}
}
}
Status = SVL_OK;
}
_SynchronizeThreads(procInfo);
// Extract frame
ret = Codec[idx]->Read(procInfo, *OutputImage, idx, true);
if (ret != SVL_OK) Status = ret;
if (ret == SVL_VID_END_REACHED && !GetLoop()) Status = SVL_STOP_REQUEST;
_SynchronizeThreads(procInfo);
if (Status == SVL_STOP_REQUEST) {
ret = SVL_STOP_REQUEST;
break;
}
// Manage looped playback and frame timing
_OnSingleThread(procInfo)
{
if (Status == SVL_VID_END_REACHED || Status == SVL_VID_RETRY) {
// Loop around
Status = Codec[idx]->Read(0, *OutputImage, idx, true);
}
if (Status != SVL_OK) {
CMN_LOG_CLASS_INIT_ERROR << "Process: failed to read video frame on channel: " << idx << std::endl;
break;
}
// Run timer based in the first video channel
if (idx == 0) {
// Get timestamp stored in the video file
timestamp = Codec[idx]->GetTimestamp();
if (timestamp > 0.0) {
if (!IsTargetTimerRunning()) {
// Try to keep orignal frame intervals
if (ResetTimer || Codec[idx]->GetPos() == 1) {
FirstTimestamp = timestamp;
Timer.Reset();
Timer.Start();
ResetTimer = false;
}
else {
timespan = (timestamp - FirstTimestamp) - Timer.GetElapsedTime();
if (timespan > 0.0) osaSleep(timespan);
}
}
}
OutputImage->SetTimestamp(timestamp);
}
}
}
}
}
else {
int main(int argc, char * argv[])
{
#if (CISST_OS == CISST_LINUX_XENOMAI)
mlockall(MCL_CURRENT|MCL_FUTURE);
#endif
// log configuration
cmnLogger::SetMask(CMN_LOG_ALLOW_ALL);
cmnLogger::SetMaskFunction(CMN_LOG_ALLOW_ALL);
// get all message to log file
cmnLogger::SetMaskDefaultLog(CMN_LOG_ALLOW_ALL);
// get only errors and warnings to std::cout
cmnLogger::AddChannel(std::cout, CMN_LOG_ALLOW_ERRORS_AND_WARNINGS);
// specify a higher, more verbose log level for these classes
cmnLogger::SetMaskClassMatching("mts", CMN_LOG_ALLOW_ALL);
cmnLogger::SetMaskClassMatching("clientTask", CMN_LOG_ALLOW_ALL);
// enable system-wide thread-safe logging
#ifdef MTS_LOGGING
mtsManagerLocal::SetLogForwarding(true);
#endif
bool useGeneric;
#if CISST_MTS_HAS_ICE
std::string globalComponentManagerIP;
if (argc != 3) {
std::cerr << "Usage: " << argv[0] << " [GlobalManagerIP] [flag]" << std::endl;
std::cerr << " GlobalManagerIP is set as 127.0.0.1 by default" << std::endl;
std::cerr << " flag = 1 to use double instead of mtsDouble" << std::endl;
}
// Set global component manager's ip and argument type
// If flag is not specified, or not 1, then use generic type (mtsDouble)
if (argc == 1) {
globalComponentManagerIP = "localhost";
useGeneric = true;
} else if (argc == 2) {
globalComponentManagerIP = argv[1];
useGeneric = true;
} else if (argc == 3) {
globalComponentManagerIP = argv[1];
useGeneric = (argv[2][0] != '1');
} else {
exit(-1);
}
std::cout << "Starting client, IP = " << globalComponentManagerIP << std::endl;
std::cout << "Using " << (useGeneric ? "mtsDouble" : "double") << std::endl;
// Get the TaskManager instance and set operation mode
mtsManagerLocal * componentManager;
try {
componentManager = mtsManagerLocal::GetInstance(globalComponentManagerIP, "ProcessClient");
} catch (...) {
CMN_LOG_INIT_ERROR << "Failed to initialize local component manager" << std::endl;
return 1;
}
#else
useGeneric = true;
if ((argc > 1) && (argv[1][0] == '1'))
useGeneric = false;
mtsManagerLocal * componentManager = mtsManagerLocal::GetInstance();
std::cout << "Starting client" << std::endl;
std::cout << "Using " << (useGeneric ? "mtsDouble" : "double") << std::endl;
#endif
// create our client task
clientTaskBase * client1;
clientTaskBase * client2;
if (useGeneric) {
client1 = new clientTask<mtsDouble>("Client1");
client2 = new clientTask<mtsDouble>("Client2");
} else {
client1 = new clientTask<double>("Client1");
client2 = new clientTask<double>("Client2");
}
client1->Configure();
client2->Configure();
componentManager->AddComponent(client1);
componentManager->AddComponent(client2);
#if CISST_MTS_HAS_ICE
// Connect the tasks across networks
if (!componentManager->Connect("ProcessClient", "Client1", "Required", "ProcessServer", "Server", "Provided")) {
CMN_LOG_INIT_ERROR << "Connect failed for client 1" << std::endl;
return 1;
}
if (!componentManager->Connect("ProcessClient", "Client2", "Required", "ProcessServer", "Server", "Provided")) {
CMN_LOG_INIT_ERROR << "Connect failed for client 2" << std::endl;
return 1;
}
#else
mtsSocketProxyClient * clientProxy = new mtsSocketProxyClient("MyClientProxy", "localhost", 1234);
componentManager->AddComponent(clientProxy);
if (!componentManager->Connect("Client1", "Required", "MyClientProxy", "Provided")) {
CMN_LOG_INIT_ERROR << "Connect failed for client 1" << std::endl;
return 1;
}
if (!componentManager->Connect("Client2", "Required", "MyClientProxy", "Provided")) {
CMN_LOG_INIT_ERROR << "Connect failed for client 2" << std::endl;
return 1;
}
#endif
// create the tasks, i.e. find the commands
componentManager->CreateAll();
componentManager->WaitForStateAll(mtsComponentState::READY);
// start the periodic Run
componentManager->StartAll();
componentManager->WaitForStateAll(mtsComponentState::ACTIVE);
bool GCMActive = true;
while (client1->UIOpened() || client2->UIOpened()) {
Fl::lock();
{
Fl::check();
}
Fl::unlock();
Fl::awake();
osaSleep(5.0 * cmn_ms);
GCMActive = componentManager->IsGCMActive();
if (!GCMActive)
break;
}
if (!GCMActive) {
CMN_LOG_RUN_ERROR << "Global Component Manager is disconnected" << std::endl;
}
// cleanup
componentManager->KillAll();
componentManager->WaitForStateAll(mtsComponentState::FINISHED, 20.0 * cmn_s);
// delete components
delete client1;
delete client2;
componentManager->Cleanup();
return 0;
}
int svlWindowManagerX11::DoModal(bool show, bool fullscreen)
{
Destroy();
DestroyFlag = false;
unsigned int i, atom_count;
int x, y, prevright, prevbottom;
unsigned int lastimage = 0;
unsigned long black, white;
XSizeHints wsh;
#if CISST_SVL_HAS_XV
Atom atoms[3];
unsigned int xvadaptorcount;
XvAdaptorInfo *xvai;
XVisualInfo xvvinfo;
bool xvupdateimage = true;
#else // CISST_SVL_HAS_XV
Atom atoms[2];
#endif // CISST_SVL_HAS_XV
// setting decoration hints for borderless mode
struct {
unsigned long flags;
unsigned long functions;
unsigned long decorations;
signed long input_mode;
unsigned long status;
} mwm;
mwm.flags = MWM_HINTS_DECORATIONS;
mwm.decorations = 0;
mwm.functions = 0;
mwm.input_mode = 0;
mwm.status = 0;
// resetting DestroyedSignal event
if (DestroyedSignal) delete(DestroyedSignal);
DestroyedSignal = new osaThreadSignal();
if (DestroyedSignal == 0) goto labError;
// initialize display and pick default screen
xDisplay = XOpenDisplay(reinterpret_cast<char*>(0));
xScreen = DefaultScreen(xDisplay);
#if CISST_SVL_HAS_XV
// check if 24bpp is suppoted by the display
if (XMatchVisualInfo(xDisplay, xScreen, 24, TrueColor, &xvvinfo) == 0) goto labError;
#endif // CISST_SVL_HAS_XV
// pick colors
black = BlackPixel(xDisplay, xScreen);
white = WhitePixel(xDisplay, xScreen);
// create windows
xWindows = new Window[NumOfWins];
memset(xWindows, 0, NumOfWins * sizeof(Window));
xGCs = new GC[NumOfWins];
memset(xGCs, 0, NumOfWins * sizeof(GC));
// create atoms for overriding default window behaviours
atoms[0] = XInternAtom(xDisplay, "WM_DELETE_WINDOW", False);
atoms[1] = XInternAtom(xDisplay, "_MOTIF_WM_HINTS", False);
#if CISST_SVL_HAS_XV
atoms[2] = XInternAtom(xDisplay, "XV_SYNC_TO_VBLANK", False);
#endif // CISST_SVL_HAS_XV
// create title strings
Titles = new std::string[NumOfWins];
CustomTitles = new std::string[NumOfWins];
CustomTitleEnabled = new int[NumOfWins];
#if CISST_SVL_HAS_XV
xvImg = new XvImage*[NumOfWins];
xvShmInfo = new XShmSegmentInfo[NumOfWins];
xvPort = new XvPortID[NumOfWins];
if (xvImg == 0 || xvShmInfo == 0 || xvPort == 0) goto labError;
memset(xvImg, 0, NumOfWins * sizeof(XvImage*));
memset(xvShmInfo, 0, NumOfWins * sizeof(XShmSegmentInfo));
memset(xvPort, 0, NumOfWins * sizeof(XvPortID));
#else // CISST_SVL_HAS_XV
// create images
xImageBuffers = new unsigned char*[NumOfWins];
for (i = 0; i < NumOfWins; i ++) {
xImageBuffers[i] = new unsigned char[Width[i] * Height[i] * 4];
}
xImg = new XImage*[NumOfWins];
memset(xImg, 0, NumOfWins * sizeof(XImage*));
if (xImg == 0) goto labError;
for (i = 0; i < NumOfWins; i ++) {
xImg[i] = XCreateImage(xDisplay,
DefaultVisual(xDisplay, xScreen),
24,
ZPixmap,
0,
reinterpret_cast<char*>(xImageBuffers[i]),
Width[i],
Height[i],
32,
0);
}
#endif // CISST_SVL_HAS_XV
prevright = prevbottom = 0;
for (i = 0; i < NumOfWins; i ++) {
if (PosX == 0 || PosY == 0) {
if (fullscreen) {
x = prevright;
y = 0;
prevright += Width[i];
}
else {
x = prevright;
y = prevbottom;
prevright += 50;
prevbottom += 50;
}
}
else {
x = PosX[i];
y = PosY[i];
}
xWindows[i] = XCreateSimpleWindow(xDisplay, DefaultRootWindow(xDisplay),
x,
y,
Width[i],
Height[i],
0,
black,
white);
if (xWindows[i] == 0) goto labError;
// overriding default behaviours:
// - borderless mode
// - closing window
if (fullscreen) {
XChangeProperty(xDisplay, xWindows[i],
atoms[1], atoms[1], 32,
PropModeReplace, reinterpret_cast<unsigned char*>(&mwm), 5);
atom_count = 2;
}
else {
atom_count = 1;
}
XSetWMProtocols(xDisplay, xWindows[i], atoms, atom_count);
wsh.flags = PPosition|PSize;
wsh.x = x;
wsh.y = y;
wsh.width = Width[i];
wsh.height = Height[i];
XSetNormalHints(xDisplay, xWindows[i], &wsh);
// set window title
CustomTitleEnabled[i] = 0;
std::ostringstream ostring;
if (Title.length() > 0) {
if (NumOfWins > 0) ostring << Title << " #" << i;
else ostring << Title;
}
else {
if (NumOfWins > 0) ostring << Title << "svlImageWindow #" << i;
else ostring << "svlImageWindow";
}
Titles[i] = ostring.str();
XSetStandardProperties(xDisplay, xWindows[i],
Titles[i].c_str(), Titles[i].c_str(),
None, NULL, 0, NULL);
// set even mask
XSelectInput(xDisplay, xWindows[i], ExposureMask|PointerMotionMask|ButtonPressMask|KeyPressMask);
// set window colormap
XSetWindowColormap(xDisplay, xWindows[i], DefaultColormapOfScreen(DefaultScreenOfDisplay(xDisplay)));
#if CISST_SVL_HAS_XV
// query shared memory extension
if (!XShmQueryExtension(xDisplay)) goto labError;
// query video adaptors
if (XvQueryAdaptors(xDisplay, DefaultRootWindow(xDisplay), &xvadaptorcount, &xvai) != Success) goto labError;
xvPort[i] = xvai->base_id + i;
XvFreeAdaptorInfo(xvai);
// overriding default Xvideo vertical sync behavior
XvSetPortAttribute (xDisplay, xvPort[i], atoms[2], 1);
#endif // CISST_SVL_HAS_XV
// create graphics context
xGCs[i] = XCreateGC(xDisplay, xWindows[i], 0, 0);
// set default colors
XSetBackground(xDisplay, xGCs[i], white);
XSetForeground(xDisplay, xGCs[i], black);
#if CISST_SVL_HAS_XV
// create image in shared memory
xvImg[i] = XvShmCreateImage(xDisplay, xvPort[i], 0x32595559/*YUV2*/, 0, Width[i], Height[i], &(xvShmInfo[i]));
if (xvImg[i]->width < static_cast<int>(Width[i]) || xvImg[i]->height < static_cast<int>(Height[i])) {
CMN_LOG_INIT_ERROR << "DoModal - image too large for XV to display (requested="
<< Width[i] << "x" << Height[i] << "; allowed="
<< xvImg[i]->width << "x" << xvImg[i]->height << ")" << std::endl;
goto labError;
}
xvShmInfo[i].shmid = shmget(IPC_PRIVATE, xvImg[i]->data_size, IPC_CREAT | 0777);
xvShmInfo[i].shmaddr = xvImg[i]->data = reinterpret_cast<char*>(shmat(xvShmInfo[i].shmid, 0, 0));
xvShmInfo[i].readOnly = False;
if (!XShmAttach(xDisplay, &(xvShmInfo[i]))) goto labError;
#endif // CISST_SVL_HAS_XV
// clear window
XClearWindow(xDisplay, xWindows[i]);
// show window if requested
if (show) XMapRaised(xDisplay, xWindows[i]);
}
// signal that initialization is done
if (InitReadySignal) InitReadySignal->Raise();
// main message loop
XEvent event;
KeySym code;
unsigned int winid;
while (1) {
osaSleep(0.001);
#if CISST_SVL_HAS_XV
if (!xvupdateimage) {
for (int events = XPending(xDisplay); events > 0; events --) {
#else // CISST_SVL_HAS_XV
if (XPending(xDisplay)) {
#endif // CISST_SVL_HAS_XV
XNextEvent(xDisplay, &event);
// find recipient
for (winid = 0; winid < NumOfWins; winid ++) {
if (event.xany.window == xWindows[winid]) break;
}
if (winid == NumOfWins) continue;
// override default window behaviour
if (event.type == ClientMessage) {
if (static_cast<unsigned long>(event.xclient.data.l[0]) == atoms[0]) {
// X11 server wants to close window
// Do nothing.... we will destroy it ourselves later
}
continue;
}
// window should be closed
if (event.type == UnmapNotify) {
printf("destroy\n");
if (xGCs[winid]) {
XFreeGC(xDisplay, xGCs[winid]);
xGCs[winid] = 0;
}
xWindows[winid] = 0;
continue;
}
// window should be updated
if (event.type == Expose && event.xexpose.count == 0) {
XClearWindow(xDisplay, xWindows[winid]);
continue;
}
if (event.type == KeyPress) {
code = XLookupKeysym(&event.xkey, 0);
if (code >= 48 && code <= 57) { // ascii numbers
OnUserEvent(winid, true, code);
continue;
}
if (code >= 97 && code <= 122) { // ascii letters
OnUserEvent(winid, true, code);
continue;
}
if (code == 13 ||
code == 32) { // special characters with correct ascii code
OnUserEvent(winid, true, code);
continue;
}
if (code >= 0xffbe && code <= 0xffc9) { // F1-F12
OnUserEvent(winid, false, winInput_KEY_F1 + (code - 0xffbe));
continue;
}
switch (code) {
case 0xFF55:
OnUserEvent(winid, false, winInput_KEY_PAGEUP);
break;
case 0xFF56:
OnUserEvent(winid, false, winInput_KEY_PAGEDOWN);
break;
case 0xFF50:
OnUserEvent(winid, false, winInput_KEY_HOME);
break;
case 0xFF57:
OnUserEvent(winid, false, winInput_KEY_END);
break;
case 0xFF63:
OnUserEvent(winid, false, winInput_KEY_INSERT);
break;
case 0xFFFF:
OnUserEvent(winid, false, winInput_KEY_DELETE);
break;
case 0xFF51:
OnUserEvent(winid, false, winInput_KEY_LEFT);
break;
case 0xFF53:
OnUserEvent(winid, false, winInput_KEY_RIGHT);
break;
case 0xFF52:
OnUserEvent(winid, false, winInput_KEY_UP);
break;
case 0xFF54:
OnUserEvent(winid, false, winInput_KEY_DOWN);
break;
}
continue;
}
if (event.type == ButtonPress) {
if (event.xbutton.button == Button1) {
if (!LButtonDown && !RButtonDown) {
LButtonDown = true;
XGrabPointer(xDisplay, xWindows[winid], false,
PointerMotionMask|ButtonReleaseMask,
GrabModeAsync, GrabModeAsync,
None,
None,
CurrentTime);
}
OnUserEvent(winid, false, winInput_LBUTTONDOWN);
}
else if (event.xbutton.button == Button3) {
if (!LButtonDown && !RButtonDown) {
RButtonDown = true;
XGrabPointer(xDisplay, xWindows[winid], false,
PointerMotionMask|ButtonReleaseMask,
GrabModeAsync, GrabModeAsync,
None,
None,
CurrentTime);
}
OnUserEvent(winid, false, winInput_RBUTTONDOWN);
}
}
if (event.type == ButtonRelease) {
if (event.xbutton.button == Button1) {
OnUserEvent(winid, false, winInput_LBUTTONUP);
if (LButtonDown && !RButtonDown) {
LButtonDown = false;
XUngrabPointer(xDisplay, CurrentTime);
}
}
else if (event.xbutton.button == Button3) {
OnUserEvent(winid, false, winInput_RBUTTONUP);
if (!LButtonDown && RButtonDown) {
RButtonDown = false;
XUngrabPointer(xDisplay, CurrentTime);
}
}
}
if (event.type == MotionNotify) {
SetMousePos(static_cast<short>(event.xmotion.x), static_cast<short>(event.xmotion.y));
OnUserEvent(winid, false, winInput_MOUSEMOVE);
}
#if CISST_SVL_HAS_XV
// image update complete
if (event.type == XShmGetEventBase(xDisplay) + ShmCompletion) {
xvupdateimage = true;
continue;
}
}
if (!xvupdateimage) signalImage.Wait(0.01);
#endif // CISST_SVL_HAS_XV
}
else {
if (DestroyFlag) break;
if (ImageCounter > lastimage) {
csImage.Enter();
lastimage = ImageCounter;
#if CISST_SVL_HAS_XV
for (i = 0; i < NumOfWins; i ++) {
XvShmPutImage(xDisplay,
xvPort[i],
xWindows[i],
xGCs[i],
xvImg[i],
0, 0, Width[i], Height[i],
0, 0, Width[i], Height[i], True);
}
xvupdateimage = false;
#else // CISST_SVL_HAS_XV
for (i = 0; i < NumOfWins; i ++) {
xImg[i]->data = reinterpret_cast<char*>(xImageBuffers[i]);
XPutImage(xDisplay,
xWindows[i],
xGCs[i],
xImg[i],
0, 0, 0, 0,
Width[i], Height[i]);
}
#endif // CISST_SVL_HAS_XV
/*
for (i = 0; i < NumOfWins; i ++) {
if (CustomTitleEnabled[i] < 0) {
// Restore original timestamp
XSetStandardProperties(xDisplay, xWindows[i],
Titles[i].c_str(), Titles[i].c_str(),
None, NULL, 0, NULL);
}
else if (CustomTitleEnabled[i] > 0) {
// Set custom timestamp
XSetStandardProperties(xDisplay, xWindows[i],
CustomTitles[i].c_str(), CustomTitles[i].c_str(),
None, NULL, 0, NULL);
}
}
*/
csImage.Leave();
}
}
}
labError:
#if CISST_SVL_HAS_XV
if (xvShmInfo) {
for (i = 0; i < NumOfWins; i ++) {
XShmDetach(xDisplay, &(xvShmInfo[i]));
shmdt(xvShmInfo[i].shmaddr);
}
delete [] xvShmInfo;
xvShmInfo = 0;
}
#endif // CISST_SVL_HAS_XV
XSync(xDisplay, 0);
for (i = 0; i < NumOfWins; i ++) {
if (xGCs[i]) XFreeGC(xDisplay, xGCs[i]);
if (xWindows[i]) XDestroyWindow(xDisplay, xWindows[i]);
}
XCloseDisplay(xDisplay);
#if CISST_SVL_HAS_XV
if (xvImg) {
for (i = 0; i < NumOfWins; i ++) {
if (xvImg[i]) XFree(xvImg[i]);
}
delete [] xvImg;
xvImg = 0;
}
if (xvPort) {
delete [] xvPort;
xvPort = 0;
}
#else // CISST_SVL_HAS_XV
if (xImg) {
for (i = 0; i < NumOfWins; i ++) {
if (xImg[i]) XDestroyImage(xImg[i]);
}
delete [] xImg;
xImg = 0;
}
if (xImageBuffers) {
delete [] xImageBuffers;
xImageBuffers = 0;
}
#endif // CISST_SVL_HAS_XV
if (xGCs) {
delete [] xGCs;
xGCs = 0;
}
if (xWindows) {
delete [] xWindows;
xWindows = 0;
}
if (Titles) {
delete [] Titles;
Titles = 0;
}
if (CustomTitles) {
delete [] CustomTitles;
CustomTitles = 0;
}
if (CustomTitleEnabled) {
delete [] CustomTitleEnabled;
CustomTitleEnabled = 0;
}
xScreen = 0;
xDisplay = 0;
if (DestroyedSignal) DestroyedSignal->Raise();
return 0;
}
void* svlStreamProc::Proc(svlStreamManager* baseref)
{
svlSample *inputsample, *outputsample;
svlFilterBase *filter, *prevfilter;
svlFilterSourceBase* source = baseref->StreamSource;
svlFilterOutput* output;
svlFilterInput* input;
svlProcInfo info;
svlSyncPoint *sync = baseref->SyncPoint;
unsigned int counter = 0;
osaTimeServer* timeserver = 0;
double timestamp;
int status = SVL_OK;
// Initializing thread info structure
info.count = ThreadCount;
info.ID = ThreadID;
info.sync = sync;
info.cs = baseref->CS;
if (ThreadID == 0) {
// Execute only on one thread - BEGIN
// Initialize time server for accessing absolute time
timeserver = new osaTimeServer;
timeserver->SetTimeOrigin();
// Execute only on one thread - END
}
while (baseref->StopThread == false) {
source->FrameCounter = counter;
outputsample = 0;
///////////////////////////////////////
// Handle stream control (pause/play)
if (source->PauseAtFrameID == static_cast<int>(counter)) {
if (ThreadID == 0) {
// Wait until playback resumed or stream stopped
while (source->PlayCounter == 0 && baseref->StopThread == false) {
osaSleep(0.1); // check 10 times a second
}
if (baseref->StopThread) {
CMN_LOG_INIT_DEBUG << "svlStreamProc::Proc (ThreadID=" << ThreadID << ", Filter=\"" << source->GetName() << "\"): stream stopped while paused" << std::endl;
break;
}
}
if (ThreadCount > 1) {
// Execute only if multi-threaded - BEGIN
// Synchronization point, wait for other threads
if (sync->Sync(ThreadID) != SVL_SYNC_OK) {
CMN_LOG_INIT_ERROR << "svlStreamProc::Proc (ThreadID=" << ThreadID << ", Filter=\"" << source->GetName() << "\"): Sync() returned error (#1)" << std::endl;
break;
}
// Execute only if multi-threaded - END
}
}
if (ThreadID == 0) {
if (source->PlayCounter > 0) source->PlayCounter --;
if (source->PlayCounter == 0) {
// Pause when the next frame arrives
source->PauseAtFrameID = static_cast<int>(counter) + 1;
}
}
////////////////////////////////////
// Starting from the stream source
status = source->Process(&info, outputsample);
if (status == SVL_STOP_REQUEST) {
CMN_LOG_INIT_DEBUG << "svlStreamProc::Proc (ThreadID=" << ThreadID << ", Filter=\"" << source->GetName() << "\"): SVL_STOP_REQUEST received" << std::endl;
break;
}
else if (status < 0) {
CMN_LOG_INIT_ERROR << "svlStreamProc::Proc (ThreadID=" << ThreadID << ", Filter=\"" << source->GetName() << "\"): svlFilterSourceBase::Process() returned error (" << status << ")" << std::endl;
break;
}
if (ThreadID == 0) {
// Execute only on one thread - BEGIN
if (outputsample && (source->AutoTimestamp || outputsample->GetTimestamp() < 0.0)) {
// Get fresh timestamp and assign it to the output sample
outputsample->SetTimestamp(GetAbsoluteTime(timeserver));
}
// Execute only on one thread - END
}
if (ThreadCount > 1) {
// Execute only if multi-threaded - BEGIN
// Synchronization point, wait for other threads
if (sync->Sync(ThreadID) != SVL_SYNC_OK) {
CMN_LOG_INIT_ERROR << "svlStreamProc::Proc (ThreadID=" << ThreadID << ", Filter=\"" << source->GetName() << "\"): Sync() returned error (#2)" << std::endl;
break;
}
// Execute only if multi-threaded - END
}
// Enabled/Disabled flag to be ignored in case of
// source filters. Use Pause and Play instead.
// Check for errors and stop request
if (baseref->StopThread) {
CMN_LOG_INIT_DEBUG << "svlStreamProc::Proc (ThreadID=" << ThreadID << ", Filter=\"" << source->GetName() << "\"): StopThread flag is true (#1)" << std::endl;
break;
}
else if (baseref->StreamStatus != SVL_OK) {
CMN_LOG_INIT_ERROR << "svlStreamProc::Proc (ThreadID=" << ThreadID << ", Filter=\"" << source->GetName() << "\"): StreamStatus signals error (" << baseref->StreamStatus << ") (#1)" << std::endl;
break;
}
prevfilter = source;
// Get next filter in the chain
output = source->GetOutput();
filter = 0;
// Check if trunk output exists
if (output) {
input = output->Connection;
// Check if trunk output is connected
if (input) {
// If connected input is trunk
if (input->Trunk) filter = input->Filter;
// If connected input is not trunk
else if (ThreadID == 0 && outputsample) input->Buffer->Push(outputsample);
// Store timestamps on both the filter input and the filter output
if (outputsample) {
timestamp = outputsample->GetTimestamp();
output->Timestamp = timestamp;
input->Timestamp = timestamp;
}
}
}
////////////////////////////////////////////
// Going downstream filter by filter
while (filter != 0) {
filter->FrameCounter = counter;
// Pass samples downstream
inputsample = outputsample; outputsample = 0;
// Check if the previous output is valid input for the next filter
status = filter->IsDataValid(filter->GetInput()->Type, inputsample);
if (status != SVL_OK) {
CMN_LOG_INIT_ERROR << "svlStreamProc::Proc (ThreadID=" << ThreadID << ", Filter=\"" << filter->GetName() << "\"): svlFilterBase::IsDataValid() returned error (" << status << ")" << std::endl;
break;
}
status = filter->Process(&info, inputsample, outputsample);
if (status < 0) {
CMN_LOG_INIT_ERROR << "svlStreamProc::Proc (ThreadID=" << ThreadID << ", Filter=\"" << filter->GetName() << "\"): svlFilterBase::Process() returned error (" << status << ")" << std::endl;
break;
}
if (ThreadCount > 1) {
// Execute only if multi-threaded - BEGIN
// Synchronization point, wait for other threads
if (sync->Sync(ThreadID) != SVL_SYNC_OK) {
CMN_LOG_INIT_ERROR << "svlStreamProc::Proc (ThreadID=" << ThreadID << ", Filter=\"" << filter->GetName() << "\"): Sync() returned error (#3)" << std::endl;
break;
}
// Execute only if multi-threaded - END
}
// Thread-safe propagation of Enabled flag to EnabledInternal.
// This step introduces at most 1 frame delay to the Enabled/Disabled state.
if (ThreadID == 0) {
filter->EnabledInternal = filter->Enabled;
}
// Check for errors and stop request
if (baseref->StopThread) {
CMN_LOG_INIT_DEBUG << "svlStreamProc::Proc (ThreadID=" << ThreadID << ", Filter=\"" << filter->GetName() << "\"): StopThread flag is true (#2)" << std::endl;
break;
}
else if (baseref->StreamStatus != SVL_OK) {
CMN_LOG_INIT_ERROR << "svlStreamProc::Proc (ThreadID=" << ThreadID << ", Filter=\"" << filter->GetName() << "\"): StreamStatus signals error (" << baseref->StreamStatus << ") (#2)" << std::endl;
break;
}
// Store input time stamp
filter->PrevInputTimestamp = inputsample->GetTimestamp();
// Pass input timestamp to output sample
if (outputsample) outputsample->SetTimestamp(filter->PrevInputTimestamp);
prevfilter = filter;
// Get next filter in the chain
output = filter->GetOutput();
filter = 0;
// Check if trunk output exists
if (output) {
input = output->Connection;
// Check if trunk output is connected
if (input) {
// If connected input is trunk
if (input->Trunk) filter = input->Filter;
// If connected input is not trunk
else if (ThreadID == 0 && outputsample) input->Buffer->Push(outputsample);
// Store timestamps on both the filter input and the filter output
if (outputsample) {
timestamp = outputsample->GetTimestamp();
output->Timestamp = timestamp;
input->Timestamp = timestamp;
}
}
}
}
if (status < 0) break;
// incrementing frame counter
counter ++;
}
if (ThreadID == 0) {
// Execute only on one thread - BEGIN
// Delete time server
if (timeserver) delete timeserver;
// Execute only on one thread - END
}
// Signal the error status
if (baseref->StopThread == false) {
// Internal shutdown
baseref->StreamStatus = status;
}
if (ThreadCount > 1) {
// Execute only if multi-threaded - BEGIN
sync->ReleaseAll();
// Execute only if multi-threaded - END
}
// Run InternalStop() method in case of internal shutdown
if (baseref->StopThread == false && ThreadID == 0) {
baseref->InternalStop(ThreadID);
}
return this;
}
int main(int argc, char * argv[])
{
#if (CISST_OS == CISST_LINUX_XENOMAI)
mlockall(MCL_CURRENT | MCL_FUTURE);
#endif
// set global component manager IP
std::string globalComponentManagerIP;
if (argc == 1) {
std::cerr << "Using default, i.e. 127.0.0.1 to find global component manager" << std::endl;
globalComponentManagerIP = "127.0.0.1";
} else if (argc == 2) {
globalComponentManagerIP = argv[1];
} else {
std::cerr << "Usage: " << argv[0] << " (global component manager IP)" << std::endl;
return -1;
}
// log configuration
cmnLogger::SetMask(CMN_LOG_ALLOW_ALL);
cmnLogger::SetMaskDefaultLog(CMN_LOG_ALLOW_ALL);
cmnLogger::AddChannel(std::cout, CMN_LOG_ALLOW_ERRORS_AND_WARNINGS);
cmnLogger::SetMaskClassMatching("mts", CMN_LOG_ALLOW_ALL);
cmnLogger::SetMaskClass("serverQtComponent", CMN_LOG_ALLOW_ALL);
// create a Qt user interface
QApplication application(argc, argv);
// create our server component
serverQtComponent * server = new serverQtComponent("Server");
// Get the ComponentManager instance and set operation mode
mtsComponentManager * componentManager;
try {
componentManager = mtsComponentManager::GetInstance(globalComponentManagerIP, "cisstMultiTaskCommandsAndEventsQtServer");
} catch (...) {
CMN_LOG_INIT_ERROR << "Failed to initialize component manager" << std::endl;
return 1;
}
componentManager->AddComponent(server);
//
// TODO: Hide this waiting routine inside mtsComponentManager using events or other things.
//
osaSleep(0.5 * cmn_s);
// create and start all components
componentManager->CreateAll();
componentManager->WaitForStateAll(mtsComponentState::READY);
componentManager->StartAll();
componentManager->WaitForStateAll(mtsComponentState::ACTIVE);
// run Qt user interface
application.exec();
// kill all components and perform cleanup
componentManager->KillAll();
componentManager->WaitForStateAll(mtsComponentState::FINISHED, 2.0 * cmn_s);
componentManager->Cleanup();
return 0;
}
void mtsCollectorStateTest::TestExecution(_serverType * server,
double serverExecutionDelay)
{
const std::string filename = "StateDataCollectionUnitTest";
// execution result used by all functions
mtsExecutionResult executionResult;
// we assume both server and servers use the same type
mtsComponentManager * manager = mtsComponentManager::GetInstance();
// add to manager and start all
CPPUNIT_ASSERT(manager->AddComponent(server));
mtsCollectorState * stateCollector = new mtsCollectorState("CollectorStateTest");
stateCollector->SetOutput(filename, mtsCollectorBase::COLLECTOR_FILE_FORMAT_CSV);
CPPUNIT_ASSERT(stateCollector);
CPPUNIT_ASSERT(stateCollector->SetStateTable(server->GetName(),
server->InterfaceProvided1.StateTable->GetName()));
CPPUNIT_ASSERT(stateCollector->AddSignal(""));
CPPUNIT_ASSERT(manager->AddComponent(stateCollector));
CPPUNIT_ASSERT(stateCollector->Connect());
// add test device to control execution
mtsCollectorStateTestDevice * stateCollectorTestDevice = new mtsCollectorStateTestDevice();
std::string stateTableName = server->InterfaceProvided1.StateTable->GetName();
std::string stateTableInterfaceName = "StateTable" + server->InterfaceProvided1.StateTable->GetName();
CPPUNIT_ASSERT(stateCollectorTestDevice);
CPPUNIT_ASSERT(manager->AddComponent(stateCollectorTestDevice));
CPPUNIT_ASSERT(manager->Connect(stateCollectorTestDevice->GetName(), "TestComponent",
server->GetName(), "p1"));
CPPUNIT_ASSERT(manager->Connect(stateCollectorTestDevice->GetName(), "StateTable",
server->GetName(), stateTableInterfaceName));
CPPUNIT_ASSERT(manager->Connect(stateCollectorTestDevice->GetName(), "CollectorState",
stateCollector->GetName(), "Control"));
manager->CreateAll();
CPPUNIT_ASSERT(manager->WaitForStateAll(mtsComponentState::READY, StateTransitionMaximumDelay));
manager->StartAll();
CPPUNIT_ASSERT(manager->WaitForStateAll(mtsComponentState::ACTIVE, StateTransitionMaximumDelay));
// test that functions are connected
CPPUNIT_ASSERT(stateCollectorTestDevice->TestComponent.StateTableAdvance.IsValid());
CPPUNIT_ASSERT(stateCollectorTestDevice->CollectorState.StartCollection.IsValid());
CPPUNIT_ASSERT(stateCollectorTestDevice->CollectorState.StopCollection.IsValid());
// actual testing, preliminary conditions
size_t numberOfRows = 0;
mtsStateTable::IndexRange range;
CPPUNIT_ASSERT(!stateCollectorTestDevice->CollectionRunning);
CPPUNIT_ASSERT_EQUAL(0u, stateCollectorTestDevice->BatchReadyEventCounter);
CPPUNIT_ASSERT_EQUAL(0u, stateCollectorTestDevice->SamplesCollected);
// try a start/stop without any state table advance
executionResult = stateCollectorTestDevice->CollectorState.StartCollection.ExecuteBlocking();
CPPUNIT_ASSERT(executionResult.IsOK());
// commands are sent to the collector which then sends to the
// server which owns the state table. the later command being
// queued we need to wait a bit
osaSleep(serverExecutionDelay);
// collection will not send the start event until the next state table advance
CPPUNIT_ASSERT(!stateCollectorTestDevice->CollectionRunning);
// advance state table, this will trigger the start event
executionResult = stateCollectorTestDevice->TestComponent.StateTableAdvance.ExecuteBlocking();
numberOfRows++;
CPPUNIT_ASSERT(executionResult.IsOK());
CPPUNIT_ASSERT(stateCollectorTestDevice->CollectionRunning);
// to make sure, not collection should have been performed so far
CPPUNIT_ASSERT_EQUAL(0u, stateCollectorTestDevice->BatchReadyEventCounter);
CPPUNIT_ASSERT_EQUAL(0u, stateCollectorTestDevice->SamplesCollected);
// now try to stop
executionResult = stateCollectorTestDevice->CollectorState.StopCollection.ExecuteBlocking();
CPPUNIT_ASSERT(executionResult.IsOK());
// see previous comment re. osaSleep
osaSleep(serverExecutionDelay);
// collection will not send the stop event until the next state table advance
executionResult = stateCollectorTestDevice->TestComponent.StateTableAdvance.ExecuteBlocking();
CPPUNIT_ASSERT(executionResult.IsOK());
CPPUNIT_ASSERT(!stateCollectorTestDevice->CollectionRunning);
// since we had to do one advance, the collector should send a range event
CPPUNIT_ASSERT_EQUAL(1u, stateCollectorTestDevice->BatchReadyEventCounter);
CPPUNIT_ASSERT_EQUAL(1u, stateCollectorTestDevice->SamplesCollected);
#if 0
// now collect 300 elements so we can test replay
executionResult = stateCollectorTestDevice->CollectorState.StartCollection.ExecuteBlocking();
CPPUNIT_ASSERT(executionResult.IsOK());
// see previous comment re. osaSleep
osaSleep(serverExecutionDelay);
for (size_t index = 0;
index < 300;
index++) {
executionResult = stateCollectorTestDevice->TestComponent.StateTableAdvance.ExecuteBlocking();
numberOfRows++;
CPPUNIT_ASSERT(executionResult.IsOK());
CPPUNIT_ASSERT(stateCollectorTestDevice->CollectionRunning);
}
// stop collection
executionResult = stateCollectorTestDevice->CollectorState.StopCollection.ExecuteBlocking();
CPPUNIT_ASSERT(executionResult.IsOK());
// see previous comment re. osaSleep
osaSleep(serverExecutionDelay);
// collection will not send the stop event until the next state table advance
executionResult = stateCollectorTestDevice->TestComponent.StateTableAdvance.ExecuteBlocking();
CPPUNIT_ASSERT(executionResult.IsOK());
// since we had to do one advance, the collector should send a range event
CPPUNIT_ASSERT_EQUAL(11u, stateCollectorTestDevice->BatchReadyEventCounter);
CPPUNIT_ASSERT_EQUAL(300u, stateCollectorTestDevice->SamplesCollected);
// create a new component of the same type for replay
_serverType * replayComponent = new _serverType("replay-component");
CPPUNIT_ASSERT(manager->AddComponent(replayComponent));
// should fail since we are not in replay mode
CPPUNIT_ASSERT(!replayComponent->SetReplayData(stateTableName, filename));
CPPUNIT_ASSERT(replayComponent->SetReplayMode());
// shouldn't find that "dummy" state table
CPPUNIT_ASSERT(!replayComponent->SetReplayData("dummy", filename));
std::string dataReplayFile = filename + ".csv";
CPPUNIT_ASSERT(replayComponent->SetReplayData(stateTableName, dataReplayFile));
// create and connect component to read from replay state table
typedef typename _serverType::value_type value_type;
mtsTestDevice1<value_type> * replayReader = new mtsTestDevice1<value_type>("replay-reader");
CPPUNIT_ASSERT(manager->AddComponent(replayReader));
CPPUNIT_ASSERT(manager->Connect(replayReader->GetName(), "r1",
replayComponent->GetName(), "p1"));
replayComponent->Start();
replayReader->Start();
replayComponent->Create();
replayReader->Create();
value_type data;
for (size_t index = 0;
index < 300;
index++) {
CPPUNIT_ASSERT(replayComponent->InterfaceProvided1.StateTable->ReplayAdvance());
replayReader->InterfaceRequired1.FunctionStateTableRead(data);
std::cerr << index << " -> " << data << std::endl;
}
#endif
// stop all and cleanup
manager->KillAll();
CPPUNIT_ASSERT(manager->WaitForStateAll(mtsComponentState::FINISHED, StateTransitionMaximumDelay));
CPPUNIT_ASSERT(stateCollector->Disconnect());
CPPUNIT_ASSERT(manager->Disconnect(stateCollectorTestDevice->GetName(), "TestComponent",
server->GetName(), "p1"));
CPPUNIT_ASSERT(manager->Disconnect(stateCollectorTestDevice->GetName(), "StateTable",
server->GetName(), stateTableInterfaceName));
CPPUNIT_ASSERT(manager->Disconnect(stateCollectorTestDevice->GetName(), "CollectorState",
stateCollector->GetName(), "Control"));
CPPUNIT_ASSERT(manager->RemoveComponent(server));
CPPUNIT_ASSERT(manager->RemoveComponent(stateCollector));
CPPUNIT_ASSERT(manager->RemoveComponent(stateCollectorTestDevice));
delete stateCollector;
delete stateCollectorTestDevice;
// the manager singleton needs to be cleaned up, adeguet1
std::cerr << "temporary hack " << CMN_LOG_DETAILS << std::endl;
manager->RemoveComponent("LCM_MCC");
manager->RemoveComponent("MCS");
}
