int VNCServer::Disconnect(WebSocket *socket)
{
Log("-VNCServer::Disconnect [ws:%p]\n",socket);
//Lock
use.WaitUnusedAndLock();
//Get client
Client *client = (Client *)socket->GetUserData();
//If no client was attached
if (!client)
{
//Unlock
use.Unlock();
//Nothing more
return Error("Already deleted");
}
//Remove it
clients.erase(client->GetId());
//Unlock
use.Unlock();
//Let it finish
client->Disconnect();
//Delete it
delete(client);
//OK
return 1;
}
static PyObject* pyvicon_disconnect(PyObject* self, PyObject* args) {
//inputs
PyObject* capsule;
//parse
if (!PyArg_ParseTuple(args, "O", &capsule)) return NULL;
Client* client = (Client*)PyCapsule_GetPointer(capsule, NULL);
//dumb disconnect, who needs errors for this?
client->Disconnect();
Py_RETURN_NONE;
}
int main()
{
Client FTPClient ;
char szCommand[256] ;
szCommand[0] = '\0' ;
while (stricmp(szCommand, "EXIT") != 0)
{
scanf("%256[^\n\0\t]", szCommand) ;
fflush(stdin) ;
HandleCommand(FTPClient, szCommand) ;
}
FTPClient.Disconnect() ;
return 0 ;
}
void viconExit()
{
MyClient.DisableSegmentData();
// MyClient.DisableMarkerData();
// MyClient.DisableUnlabeledMarkerData();
// MyClient.DisableDeviceData();
// TODO: Disconnect seems to cause a hang. -Scott Kuhl
// Disconnect and dispose
int t = clock();
std::cout << " Disconnecting..." << std::endl;
MyClient.Disconnect();
int dt = clock() - t;
double secs = (double) (dt)/(double)CLOCKS_PER_SEC;
std::cout << " Disconnect time = " << secs << " secs" << std::endl;
}
int DERPEditor::net_client_disconnect(lua_State *L)
{
if (lua_gettop(L) == 1)
{
lua_getfield(L, -1, "instance");
Client* client = *(Client**)lua_touserdata(L, -1);
client->Disconnect();
return 0;
}
else
{
lua_pushstring(L, "Invalid arguments passed to disconnect function!");
lua_error(L);
}
return 0;
}
void HandleCommand(Client &FTPClient, char *szCommand)
{
char szBuffer[256] ;
int iLen = 0 ;
while (szCommand[iLen] != '\0')
{
if (szCommand[iLen] == ' ')
{
szCommand[iLen] = '\0' ;
break ;
}
iLen ++ ;
}
if (stricmp(szCommand, "OPEN") == 0)
{
if (FTPClient.Connect(szCommand+5))
printf("Connected successfully to %s\n", szCommand+5) ;
else
{
printf("Connection to %s failed\n", szCommand+5) ;
return ;
}
szBuffer[0] = '\0' ;
while (1)
{
FTPClient.RecieveMain(szBuffer, 256) ;
char *temp = (szBuffer + strlen(szBuffer) - 6) ;
if (strlen(szBuffer) > 6 && stricmp(szBuffer + strlen(szBuffer) - 6, "ready.") == 0)
break ;
if (GetCode(szBuffer) / 100 == 5)
{
FTPClient.Disconnect() ;
return ;
}
}
return ;
} else if (stricmp(szCommand, "CLOSE") == 0)
{
FTPClient.Disconnect() ;
return ;
} else if (stricmp(szCommand, "EXIT") == 0)
{
return ;
} else if (stricmp(szCommand, "CD") == 0)
{
if (stricmp(szCommand+3, "..") == 0)
FTPClient.SendMain("CDUP", 4) ;
else
{
sprintf(szBuffer, "CWD %s", szCommand+3) ;
FTPClient.SendMain(szBuffer) ;
}
FTPClient.RecieveMain(szBuffer, 256) ;
return ;
} else if (stricmp(szCommand, "DELETE") == 0)
{
sprintf(szBuffer, "DELE %s", szCommand+7) ;
FTPClient.SendMain(szBuffer) ;
FTPClient.RecieveMain(szBuffer, 256) ;
} else if (stricmp(szCommand, "USER") == 0)
{
szCommand[4] = ' ' ;
FTPClient.SendMain(szCommand) ;
FTPClient.RecieveMain(szBuffer, 256) ;
} else if (stricmp(szCommand, "PASS") == 0)
{
szCommand[4] = ' ' ;
FTPClient.SendMain(szCommand) ;
FTPClient.RecieveMain(szBuffer, 256) ;
} else if (stricmp(szCommand, "HELP") == 0)
{
FTPClient.SendMain("HELP", 4) ;
FTPClient.RecieveMain(szBuffer, 256) ;
return ;
} else if (stricmp(szCommand, "MKDIR") == 0)
{
sprintf(szBuffer, "MKD %s", szCommand+6) ;
FTPClient.SendMain(szBuffer) ;
FTPClient.RecieveMain(szBuffer, 256) ;
} else if (stricmp(szCommand, "RMDIR") == 0)
{
sprintf(szBuffer, "RMD %s", szCommand+6) ;
FTPClient.SendMain(szBuffer) ;
FTPClient.RecieveMain(szBuffer, 256) ;
} else if (stricmp(szCommand, "LS") == 0)
{
FTPClient.PORTRequest() ;
FTPClient.RecieveMain(szBuffer, 256) ;
if (GetCode(szBuffer) / 100 != 5)
{
FTPClient.SendMain("LIST", 4) ;
FTPClient.RecieveMain(szBuffer, 256) ;
if (GetCode(szBuffer) / 100 != 5)
{
FTPClient.PORTAccept() ;
int iRecieved = 256 ;
while (iRecieved > 2)
{
iRecieved = FTPClient.RecievePORT(szBuffer, 256) ;
// printf("%s\n", szBuffer) ;
}
FTPClient.PORTClose() ;
FTPClient.RecieveMain(szBuffer, 256) ;
} else
{
printf("Error with LIST command\n") ;
}
} else
{
printf("PORT request failed, cannot LIST\n") ;
}
return ;
} else if (stricmp(szCommand, "PWD") == 0)
{
FTPClient.SendMain("PWD", 3) ;
FTPClient.RecieveMain(szBuffer, 256) ;
return ;
} else if (stricmp(szCommand, "RETR") == 0)
{
FTPClient.PORTRequest() ;
FTPClient.RecieveMain(szBuffer, 256) ;
if (GetCode(szBuffer) / 100 != 5)
{
szCommand[4] = ' ' ;
FTPClient.SendMain(szCommand) ;
FTPClient.RecieveMain(szBuffer, 256) ;
if (GetCode(szBuffer) / 100 != 5)
{
FILE *pFile ;
if (FTPClient.GetType() == I_TYPE) pFile = fopen(szCommand + 5, "wb") ;
else pFile = fopen(szCommand + 5, "w") ;
FTPClient.PORTAccept() ;
/* char cTemp = ' ' ;
char szBuffer[2] ;
while (szBuffer[0] != 255 && szBuffer[0] != 0)
{
FTPClient.RecievePORT(szBuffer, 2) ;
fputc(szBuffer[0], pFile) ;
fputc(szBuffer[1], pFile) ;
}
*/
char cTemp = ' ' ;
int iRecieve = 1;
while (iRecieve > 0)
{
iRecieve = FTPClient.RecievePORT(&cTemp, 1) ;
fputc(cTemp, pFile) ;
}
fclose(pFile) ;
FTPClient.PORTClose() ;
FTPClient.RecieveMain(szBuffer, 256) ;
} else
{
printf("Error with RETR command\n") ;
}
}
} else if (stricmp(szCommand, "STOR") == 0)
{
FTPClient.PORTRequest() ;
FTPClient.RecieveMain(szBuffer, 256) ;
if (GetCode(szBuffer) / 100 != 5)
{
szCommand[4] = ' ' ;
FTPClient.SendMain(szCommand) ;
FTPClient.RecieveMain(szBuffer, 256) ;
if (GetCode(szBuffer) / 100 != 5)
{
FILE *pFile ;
if (FTPClient.GetType() == I_TYPE) pFile = fopen(szCommand + 5, "rb") ;
else pFile = fopen(szCommand + 5, "r") ;
if (pFile == NULL) printf("Error opening %s\n", szCommand+5) ;
FTPClient.PORTAccept() ;
SOCKET hSocket = FTPClient.GetPORT() ;
char szBuffer[256] ;
int iRead ;
if (pFile != NULL) while (!feof(pFile))
{
iRead = fread(szBuffer, sizeof(char), 256, pFile) ;
send(hSocket, szBuffer, iRead, 0) ;
}
fclose(pFile) ;
FTPClient.PORTClose() ;
FTPClient.RecieveMain(szBuffer, 256) ;
} else
{
printf("Error with STOR command\n") ;
}
return ;
} else
{
printf("PORT request failed, cannot RETR\n") ;
return ;
}
return ;
} else if (stricmp(szCommand, "BINARY") == 0)
{
FTPClient.SetType(I_TYPE) ;
return ;
} else if (stricmp(szCommand, "ASCII") == 0)
{
FTPClient.SetType(A_TYPE) ;
return ;
} else if (stricmp(szCommand, "REN") == 0)
{
int iTrav = 4 ;
while (szCommand[iTrav] != '\0')
{
if (szCommand[iTrav] == ' ')
szCommand[iTrav] = '\0' ;
iTrav ++ ;
}
sprintf(szBuffer, "RNFR %s", szCommand+4) ;
FTPClient.SendMain(szBuffer) ;
FTPClient.RecieveMain(szBuffer, 256) ;
sprintf(szBuffer, "RNTO %s", szCommand+strlen(szCommand+4)+5) ;
FTPClient.SendMain(szBuffer) ;
FTPClient.RecieveMain(szBuffer, 256) ;
} else if (stricmp(szCommand, "?") == 0)
{
printf("OPEN CLOSE USER PASS EXIT LS PWD CD MKDIR RMDIR RETR STOR REN DELETE HELP\n") ;
return ;
} else
{
printf("Invalid command\n") ;
}
}
int main( int argc, char* argv[] )
{
//lcm
lcm::LCM lcm;
if(!lcm.good())
return 1;
vicon_state_t vicon_msg;
// Program options
std::string HostName = "192.168.0.102:801";
if( argc > 1 )
{
HostName = argv[1];
}
// log contains:
// version number
// log of framerate over time
// --multicast
// kill off internal app
std::string LogFile = "";
std::string MulticastAddress = "244.0.0.0:44801";
bool ConnectToMultiCast = false;
bool EnableMultiCast = false;
bool EnableHapticTest = false;
bool bReadCentroids = false;
std::vector<std::string> HapticOnList(0);
for(int a=2; a < argc; ++a)
{
std::string arg = argv[a];
if(arg == "--help")
{
std::cout << argv[0] << " <HostName>: allowed options include:\n --log_file <LogFile> --enable_multicast <MulticastAddress:Port> --connect_to_multicast <MulticastAddress:Port> --help --enable_haptic_test <DeviceName> --centroids" << std::endl;
return 0;
}
else if (arg=="--log_file")
{
if(a < argc)
{
LogFile = argv[a+1];
std::cout << "Using log file <"<< LogFile << "> ..." << std::endl;
++a;
}
}
else if (arg=="--enable_multicast")
{
EnableMultiCast = true;
if(a < argc)
{
MulticastAddress = argv[a+1];
std::cout << "Enabling multicast address <"<< MulticastAddress << "> ..." << std::endl;
++a;
}
}
else if (arg=="--connect_to_multicast")
{
ConnectToMultiCast = true;
if(a < argc)
{
MulticastAddress = argv[a+1];
std::cout << "connecting to multicast address <"<< MulticastAddress << "> ..." << std::endl;
++a;
}
}
else if (arg=="--enable_haptic_test")
{
EnableHapticTest = true;
++a;
if ( a < argc )
{
//assuming no haptic device name starts with "--"
while( a < argc && strncmp( argv[a], "--", 2 ) !=0 )
{
HapticOnList.push_back( argv[a] );
++a;
}
}
}
else if( arg=="--centroids" )
{
bReadCentroids = true;
}
else
{
std::cout << "Failed to understand argument <" << argv[a] << ">...exiting" << std::endl;
return 1;
}
}
std::ofstream ofs;
if(!LogFile.empty())
{
ofs.open(LogFile.c_str());
if(!ofs.is_open())
{
std::cout << "Could not open log file <" << LogFile << ">...exiting" << std::endl;
return 1;
}
}
// Make a new client
Client MyClient;
for(int i=0; i != 3; ++i) // repeat to check disconnecting doesn't wreck next connect
{
// Connect to a server
std::cout << "Connecting to " << HostName << " ..." << std::flush;
while( !MyClient.IsConnected().Connected )
{
// Direct connection
bool ok = false;
if(ConnectToMultiCast)
{
// Multicast connection
ok = ( MyClient.ConnectToMulticast( HostName, MulticastAddress ).Result == Result::Success );
}
else
{
ok =( MyClient.Connect( HostName ).Result == Result::Success );
}
if(!ok)
{
std::cout << "Warning - connect failed..." << std::endl;
}
std::cout << ".";
#ifdef WIN32
Sleep( 1000 );
#else
sleep(1);
#endif
}
// std::cout << std::endl;
// Enable some different data types
MyClient.EnableSegmentData();
MyClient.EnableMarkerData();
MyClient.EnableUnlabeledMarkerData();
MyClient.EnableDeviceData();
if( bReadCentroids )
{
MyClient.EnableCentroidData();
}
std::cout << "Segment Data Enabled: " << Adapt( MyClient.IsSegmentDataEnabled().Enabled ) << std::endl;
std::cout << "Marker Data Enabled: " << Adapt( MyClient.IsMarkerDataEnabled().Enabled ) << std::endl;
std::cout << "Unlabeled Marker Data Enabled: " << Adapt( MyClient.IsUnlabeledMarkerDataEnabled().Enabled ) << std::endl;
std::cout << "Device Data Enabled: " << Adapt( MyClient.IsDeviceDataEnabled().Enabled ) << std::endl;
std::cout << "Centroid Data Enabled: " << Adapt( MyClient.IsCentroidDataEnabled().Enabled ) << std::endl;
// Set the streaming mode
//MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ClientPull );
// MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ClientPullPreFetch );
MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ServerPush );
// Set the global up axis
MyClient.SetAxisMapping( Direction::Forward,
Direction::Left,
Direction::Up ); // Z-up
// MyClient.SetGlobalUpAxis( Direction::Forward,
// Direction::Up,
// Direction::Right ); // Y-up
Output_GetAxisMapping _Output_GetAxisMapping = MyClient.GetAxisMapping();
std::cout << "Axis Mapping: X-" << Adapt( _Output_GetAxisMapping.XAxis )
<< " Y-" << Adapt( _Output_GetAxisMapping.YAxis )
<< " Z-" << Adapt( _Output_GetAxisMapping.ZAxis ) << std::endl;
// Discover the version number
Output_GetVersion _Output_GetVersion = MyClient.GetVersion();
std::cout << "Version: " << _Output_GetVersion.Major << "."
<< _Output_GetVersion.Minor << "."
<< _Output_GetVersion.Point << std::endl;
if( EnableMultiCast )
{
assert( MyClient.IsConnected().Connected );
MyClient.StartTransmittingMulticast( HostName, MulticastAddress );
}
size_t FrameRateWindow = 1000; // frames
size_t Counter = 0;
clock_t LastTime = clock();
// Loop until a key is pressed
#ifdef WIN32
while( !Hit() )
#else
while( true)
#endif
{
// Get a frame
// output_stream << "Waiting for new frame...";
while( MyClient.GetFrame().Result != Result::Success )
{
// Sleep a little so that we don't lumber the CPU with a busy poll
#ifdef WIN32
Sleep( 200 );
#else
sleep(1);
#endif
// output_stream << ".";
}
// output_stream << std::endl;
if(++Counter == FrameRateWindow)
{
clock_t Now = clock();
double FrameRate = (double)(FrameRateWindow * CLOCKS_PER_SEC) / (double)(Now - LastTime);
if(!LogFile.empty())
{
time_t rawtime;
struct tm * timeinfo;
time ( &rawtime );
timeinfo = localtime ( &rawtime );
ofs << "Frame rate = " << FrameRate << " at " << asctime (timeinfo)<< std::endl;
}
LastTime = Now;
Counter = 0;
}
// Get the frame number
Output_GetFrameNumber _Output_GetFrameNumber = MyClient.GetFrameNumber();
// output_stream << "Frame Number: " << _Output_GetFrameNumber.FrameNumber << std::endl;
if( EnableHapticTest == true )
{
for (size_t i = 0; i < HapticOnList.size(); ++ i)
{
if( Counter % 2 == 0 )
{
Output_SetApexDeviceFeedback Output= MyClient.SetApexDeviceFeedback( HapticOnList[i], true );
if( Output.Result == Result::Success )
{
// output_stream<< "Turn haptic feedback on for device: " << HapticOnList[i]<<std::endl;
}
else if( Output.Result == Result::InvalidDeviceName )
{
output_stream<< "Device doesn't exist: "<< HapticOnList[i]<<std::endl;
}
}
if( Counter % 20 == 0 )
{
Output_SetApexDeviceFeedback Output = MyClient.SetApexDeviceFeedback( HapticOnList[i], false);
if( Output.Result == Result::Success )
{
output_stream<< "Turn haptic feedback off for device: " << HapticOnList[i]<<std::endl;
}
}
}
}
// Count the number of subjects
unsigned int SubjectCount = MyClient.GetSubjectCount().SubjectCount;
// output_stream << "Subjects (" << SubjectCount << "):" << std::endl;
for( unsigned int SubjectIndex = 0 ; SubjectIndex < SubjectCount ; ++SubjectIndex )
{
// output_stream << " Subject #" << SubjectIndex << std::endl;
// Get the subject name
std::string SubjectName = MyClient.GetSubjectName( SubjectIndex ).SubjectName;
// output_stream << " Name: " << SubjectName << std::endl;
// Get the root segment
// std::string RootSegment = MyClient.GetSubjectRootSegmentName( SubjectName ).SegmentName;
// output_stream << " Root Segment: " << RootSegment << std::endl;
// Count the number of segments
unsigned int SegmentCount = MyClient.GetSegmentCount( SubjectName ).SegmentCount;
// output_stream << " Segments (" << SegmentCount << "):" << std::endl;
for( unsigned int SegmentIndex = 0 ; SegmentIndex < SegmentCount ; ++SegmentIndex )
{
// output_stream << " Segment #" << SegmentIndex << std::endl;
// Get the segment name
std::string SegmentName = MyClient.GetSegmentName( SubjectName, SegmentIndex ).SegmentName;
//chose the the object by name
if(SegmentName != OBJ_TO_TRACK) continue;
// output_stream << " Name: " << SegmentName << std::endl;
// Get the segment parent
std::string SegmentParentName = MyClient.GetSegmentParentName( SubjectName, SegmentName ).SegmentName;
// output_stream << " Parent: " << SegmentParentName << std::endl;
// Get the segment's children
unsigned int ChildCount = MyClient.GetSegmentChildCount( SubjectName, SegmentName ).SegmentCount;
// output_stream << " Children (" << ChildCount << "):" << std::endl;
for( unsigned int ChildIndex = 0 ; ChildIndex < ChildCount ; ++ChildIndex )
{
std::string ChildName = MyClient.GetSegmentChildName( SubjectName, SegmentName, ChildIndex ).SegmentName;
// output_stream << " " << ChildName << std::endl;
}
// Get the global segment translation
Output_GetSegmentGlobalTranslation _Output_GetSegmentGlobalTranslation =
MyClient.GetSegmentGlobalTranslation( SubjectName, SegmentName );
// output_stream << " Global Translation: (" << _Output_GetSegmentGlobalTranslation.Translation[ 0 ] << ", "
// << _Output_GetSegmentGlobalTranslation.Translation[ 1 ] << ", "
// << _Output_GetSegmentGlobalTranslation.Translation[ 2 ] << ") "
// << Adapt( _Output_GetSegmentGlobalTranslation.Occluded ) << std::endl;
// Get the global segment rotation in helical co-ordinates
// Output_GetSegmentGlobalRotationHelical _Output_GetSegmentGlobalRotationHelical =
// MyClient.GetSegmentGlobalRotationHelical( SubjectName, SegmentName );
// output_stream << " Global Rotation Helical: (" << _Output_GetSegmentGlobalRotationHelical.Rotation[ 0 ] << ", "
// << _Output_GetSegmentGlobalRotationHelical.Rotation[ 1 ] << ", "
// << _Output_GetSegmentGlobalRotationHelical.Rotation[ 2 ] << ") "
// << Adapt( _Output_GetSegmentGlobalRotationHelical.Occluded ) << std::endl;
// Get the global segment rotation as a matrix
Output_GetSegmentGlobalRotationMatrix _Output_GetSegmentGlobalRotationMatrix =
MyClient.GetSegmentGlobalRotationMatrix( SubjectName, SegmentName );
for (int iii = 0; iii < 9; iii ++) {
vicon_msg.DCM[iii] = _Output_GetSegmentGlobalRotationMatrix.Rotation[ iii ];
}
// output_stream << " Global Rotation Matrix: (" << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 0 ] << ", "
// << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 1 ] << ", "
// << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 2 ] << ", "
// << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 3 ] << ", "
// << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 4 ] << ", "
// << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 5 ] << ", "
// << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 6 ] << ", "
// << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 7 ] << ", "
// << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 8 ] << ") "
// << Adapt( _Output_GetSegmentGlobalRotationMatrix.Occluded ) << std::endl;
// Get the global segment rotation in quaternion co-ordinates
Output_GetSegmentGlobalRotationQuaternion _Output_GetSegmentGlobalRotationQuaternion =
MyClient.GetSegmentGlobalRotationQuaternion( SubjectName, SegmentName );
// output_stream << " Global Rotation Quaternion: (" << _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 0 ] << ", "
// << _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 1 ] << ", "
// << _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 2 ] << ", "
// << _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 3 ] << ") "
// << Adapt( _Output_GetSegmentGlobalRotationQuaternion.Occluded ) << std::endl;
// Get the global segment rotation in EulerXYZ co-ordinates
Output_GetSegmentGlobalRotationEulerXYZ _Output_GetSegmentGlobalRotationEulerXYZ =
MyClient.GetSegmentGlobalRotationEulerXYZ( SubjectName, SegmentName );
// std::cout<<"global: "<<_Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 0 ]<<", "<<_Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 1 ]<<", "<<_Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 2 ]<<std::endl;
// output_stream << " Global Rotation EulerXYZ: (" << _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 0 ] << ", "
// << _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 1 ] << ", "
// << _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 2 ] << ") "
// << Adapt( _Output_GetSegmentGlobalRotationEulerXYZ.Occluded ) << std::endl;
number = distribution(generator);
_Output_GetSegmentGlobalTranslation.Translation[ 0 ]+= 1000.0*number;
_Output_GetSegmentGlobalTranslation.Translation[ 1 ]+= 1000.0*number;
_Output_GetSegmentGlobalTranslation.Translation[ 2 ]+= 1000.0*number;
//lcm publish data
vicon_msg.timestamp = utime_now();
///////////////////filter position
// std::cout <<abs(_Output_GetSegmentGlobalTranslation.Translation[ 2 ] - pre_z)<<std::endl;
// if(abs(_Output_GetSegmentGlobalTranslation.Translation[ 0 ] - pre_x) <= POS_JUMP_THRES)
addArr(pos_x, _Output_GetSegmentGlobalTranslation.Translation[ 0 ], NUM_SAMPLES_MED);
// if(abs(_Output_GetSegmentGlobalTranslation.Translation[ 1 ] - pre_y) <= POS_JUMP_THRES)
addArr(pos_y, _Output_GetSegmentGlobalTranslation.Translation[ 1 ], NUM_SAMPLES_MED);
// if(abs(_Output_GetSegmentGlobalTranslation.Translation[ 2 ] - pre_z) <= POS_JUMP_THRES)
addArr(pos_z, _Output_GetSegmentGlobalTranslation.Translation[ 2 ], NUM_SAMPLES_MED);
addArr(pos_x_med, median(pos_x, NUM_SAMPLES_MED), NUM_SAMPLES_AVG);
addArr(pos_y_med, median(pos_y, NUM_SAMPLES_MED), NUM_SAMPLES_AVG);
addArr(pos_z_med, median(pos_z, NUM_SAMPLES_MED), NUM_SAMPLES_AVG);
addArr(pos_x_avg, average(pos_x_med, NUM_SAMPLES_AVG), NUM_SAMPLES_AVG);
addArr(pos_y_avg, average(pos_y_med, NUM_SAMPLES_AVG), NUM_SAMPLES_AVG);
addArr(pos_z_avg, average(pos_z_med, NUM_SAMPLES_AVG), NUM_SAMPLES_AVG);
vicon_msg.position[0] = pos_x_avg[NUM_SAMPLES_AVG - 1];
vicon_msg.position[1] = pos_y_avg[NUM_SAMPLES_AVG - 1];
vicon_msg.position[2] = pos_z_avg[NUM_SAMPLES_AVG - 1];
/////////////////filter attitude
///
///it seems that attitude is refreshed with half of the frequency as position
///
///
addArr(att_x, _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 0 ], NUM_SAMPLES_MED);
addArr(att_y, _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 1 ], NUM_SAMPLES_MED);
addArr(att_z, _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 2 ], NUM_SAMPLES_MED);
addArr(att_x_med, median(att_x, NUM_SAMPLES_MED), NUM_SAMPLES_AVG);
addArr(att_y_med, median(att_y, NUM_SAMPLES_MED), NUM_SAMPLES_AVG);
addArr(att_z_med, median(att_z, NUM_SAMPLES_MED), NUM_SAMPLES_AVG);
addArr(att_x_avg, average(att_x_med, NUM_SAMPLES_AVG), NUM_SAMPLES_AVG);
addArr(att_y_avg, average(att_y_med, NUM_SAMPLES_AVG), NUM_SAMPLES_AVG);
addArr(att_z_avg, average(att_z_med, NUM_SAMPLES_AVG), NUM_SAMPLES_AVG);
vicon_msg.attitude[0] = att_x_avg[NUM_SAMPLES_AVG - 1];
vicon_msg.attitude[1] = att_y_avg[NUM_SAMPLES_AVG - 1];
vicon_msg.attitude[2] = att_z_avg[NUM_SAMPLES_AVG - 1];
// std::cout << "attitude[0]: "<<_Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 0 ]<<std::endl;
// std::cout << "attitude[1]: "<<_Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 1 ]<<std::endl;
// std::cout << "attitude[2]: "<<_Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 2 ]<<std::endl;
dt = (static_cast<double>(vicon_msg.timestamp - time_tmp))/1000; //dt in mseconds
// differentiate the positions to get velocities
dx_in_mm = vicon_msg.position[0] - pre_x;
dy_in_mm = vicon_msg.position[1] - pre_y;
dz_in_mm = vicon_msg.position[2] - pre_z;
// std::cout<<"dz: "<<dz_in_mm<<"\tdt: "<<dt <<std::endl;
vx = dx_in_mm/dt;
vy = dy_in_mm/dt;
vz = dz_in_mm/dt;
//differentiate the attitudes to get Euler rate
v_rho = ((double)vicon_msg.attitude[0] - pre_rho)/(dt/1e3);
v_theta = ((double)vicon_msg.attitude[1] - pre_theta)/(dt/1e3);
v_psi = ((double)vicon_msg.attitude[2] - pre_psi)/(dt/1e3);
///////////////////////filter the velocity
//collect data in to stack
//
//filter out unreasonable jump
// if(abs(vx - prev_vx) <= JUMP_THRESHOLD) {
addArr(vel_x, vx, NUM_SAMPLES_MED);
addArr(vel_x_med, median(vel_x, NUM_SAMPLES_MED), NUM_SAMPLES_AVG);
addArr(vel_x_avg, average(vel_x_med, NUM_SAMPLES_AVG), NUM_SAMPLES_AVG);
vx = vel_x_avg[NUM_SAMPLES_AVG-1];
prev_vx = vx;
// } else vx = prev_vx;
// if(abs(vy - prev_vy) <= JUMP_THRESHOLD) {
addArr(vel_y, vy, NUM_SAMPLES_MED);
addArr(vel_y_med, median(vel_y, NUM_SAMPLES_MED), NUM_SAMPLES_AVG);
addArr(vel_y_avg, average(vel_y_med, NUM_SAMPLES_AVG), NUM_SAMPLES_AVG);
vy = vel_y_avg[NUM_SAMPLES_AVG-1];
prev_vy = vy;
// } else vy = prev_vy;
// if(abs(vz - prev_vz) <= JUMP_THRESHOLD) {
addArr(vel_z, vz, NUM_SAMPLES_MED);
addArr(vel_z_med, median(vel_z, NUM_SAMPLES_MED), NUM_SAMPLES_AVG);
addArr(vel_z_avg, average(vel_z_med, NUM_SAMPLES_AVG), NUM_SAMPLES_AVG);
vz = vel_z_avg[NUM_SAMPLES_AVG-1];
prev_vz = vz;
// } else vz = prev_vz;
//filter the Euler rate
// if(abs(v_rho - v_rho_pre) <= deg2rad(JUMP_THRESHOLD_ATTITUDE)) {
if(abs(v_rho - v_rho_pre) <= 3) addArr(rho_vel, v_rho, NUM_SAMPLES_MED); // Jump between -180 to 180
addArr(rho_vel_med, median(rho_vel, NUM_SAMPLES_MED), NUM_SAMPLES_AVG);
addArr(rho_vel_avg, average(rho_vel_med, NUM_SAMPLES_AVG), NUM_SAMPLES_AVG);
v_rho = rho_vel_avg[NUM_SAMPLES_AVG - 1];
v_rho_pre = v_rho;
// } else v_rho = v_rho_pre;
// if(abs(v_theta - v_theta_pre) <= deg2rad(JUMP_THRESHOLD_ATTITUDE)) {
if(abs(v_theta - v_theta_pre) <= 3) addArr(theta_vel, v_theta, NUM_SAMPLES_MED);
addArr(theta_vel_med, median(theta_vel, NUM_SAMPLES_MED), NUM_SAMPLES_AVG);
addArr(theta_vel_avg, average(theta_vel_med, NUM_SAMPLES_AVG), NUM_SAMPLES_AVG);
v_theta = theta_vel_avg[NUM_SAMPLES_AVG - 1];
v_theta_pre = v_theta;
// } else v_theta = v_theta_pre;
// if(abs(v_psi - v_psi_pre) <= deg2rad(JUMP_THRESHOLD_ATTITUDE)) {
if(abs(v_psi - v_psi_pre) <= 3) addArr(psi_vel, v_psi, NUM_SAMPLES_MED);
addArr(psi_vel_med, median(psi_vel, NUM_SAMPLES_MED), NUM_SAMPLES_AVG);
addArr(psi_vel_avg, average(psi_vel_med, NUM_SAMPLES_AVG), NUM_SAMPLES_AVG);
v_psi = psi_vel_avg[NUM_SAMPLES_AVG - 1];
v_psi_pre = v_psi;
// } else v_psi = v_psi_pre;
vicon_msg.velocity[0] = vx;
vicon_msg.velocity[1] = vy;
vicon_msg.velocity[2] = vz;
vicon_msg.angular_vel[0] = v_rho;
vicon_msg.angular_vel[1] = v_theta;
vicon_msg.angular_vel[2] = v_psi;
lcm.publish("vicon_state", &vicon_msg);
//previous state
pre_x = (double)vicon_msg.position[0];
pre_y = (double)vicon_msg.position[1];
pre_z = (double)vicon_msg.position[2];
pre_rho = (double)vicon_msg.attitude[0];
pre_theta = (double)vicon_msg.attitude[1];
pre_psi = (double)vicon_msg.attitude[2];
time_tmp = vicon_msg.timestamp;
//usleep is crucial for accuracy
usleep(1e4);
}
}
if( bReadCentroids )
{
unsigned int CameraCount = MyClient.GetCameraCount().CameraCount;
output_stream << "Cameras(" << CameraCount << "):" << std::endl;
for( unsigned int CameraIndex = 0; CameraIndex < CameraCount; ++CameraIndex )
{
output_stream << " Camera #" << CameraIndex << ":" << std::endl;
const std::string CameraName = MyClient.GetCameraName( CameraIndex ).CameraName;
output_stream << " Name: " << CameraName << std::endl;
unsigned int CentroidCount = MyClient.GetCentroidCount( CameraName ).CentroidCount;
output_stream << " Centroids(" << CentroidCount << "):" << std::endl;
for( unsigned int CentroidIndex = 0; CentroidIndex < CentroidCount; ++CentroidIndex )
{
output_stream << " Centroid #" << CentroidIndex << ":" << std::endl;
Output_GetCentroidPosition _Output_GetCentroidPosition = MyClient.GetCentroidPosition( CameraName, CentroidIndex );
output_stream << " Position: (" << _Output_GetCentroidPosition.CentroidPosition[0] << ", "
<< _Output_GetCentroidPosition.CentroidPosition[1] << ")" << std::endl;
output_stream << " Radius: (" << _Output_GetCentroidPosition.Radius << ")" << std::endl;
}
}
}
}
if( EnableMultiCast )
{
MyClient.StopTransmittingMulticast();
}
MyClient.DisableSegmentData();
MyClient.DisableMarkerData();
MyClient.DisableUnlabeledMarkerData();
MyClient.DisableDeviceData();
if( bReadCentroids )
{
MyClient.DisableCentroidData();
}
// Disconnect and dispose
int t = clock();
std::cout << " Disconnecting..." << std::endl;
MyClient.Disconnect();
int dt = clock() - t;
double secs = (double) (dt)/(double)CLOCKS_PER_SEC;
std::cout << " Disconnect time = " << secs << " secs" << std::endl;
}
}
int main( int argc, char* argv[] )
{
// Program options
std::string HostName = "192.168.10.1:801";
int startup_delay = 10;
ros::init(argc, argv, "udp_enable");
ros::NodeHandle n;
ros::Rate loop_rate(1);
// log contains:
// version number
// log of framerate over time
// --multicast
// kill off internal app
std::string LogFile = "";
std::string MulticastAddress = "224.0.0.0:44801";
bool ConnectToMultiCast = false;
bool EnableMultiCast = true;
bool EnableHapticTest = false;
bool bReadCentroids = false;
std::vector<std::string> HapticOnList(0);
std::ofstream ofs;
// ROS parameters
std::string s;
// mutlicast address
if (n.getParam("vicon_multicast_address", s)) {
MulticastAddress = s;
//std::cout << "Got multicast " << MulticastAddress << " from parameter." << std::endl;
}
// own hostname
if (n.getParam("vicon_server_hostname", s)) {
HostName = s;
//std::cout << "Got hostname " << HostName << " from parameter." << std::endl;
}
// tracking object (this one is private as unique to each node)
// try the private thing using the "bare" method
if (ros::param::has("~startup_delay")) {
ros::param::get("~startup_delay", startup_delay);
}
ROS_INFO("Waiting for start-up delay of %d s", startup_delay);
sleep(startup_delay);
// Make a new client
Client MyClient;
// Connect to a server
ROS_INFO("Connecting to server %s", HostName.c_str());
while( !MyClient.IsConnected().Connected )
{
// Direct connection
bool ok = false;
if(ConnectToMultiCast)
{
// Multicast connection
ok = ( MyClient.ConnectToMulticast( HostName, MulticastAddress ).Result == Result::Success );
}
else
{
ok =( MyClient.Connect( HostName ).Result == Result::Success );
}
if(!ok)
{
std::cout << "Warning - connect failed..." << std::endl;
}
std::cout << ".";
sleep(1);
}
std::cout << std::endl;
// Enable some different data types
MyClient.EnableSegmentData();
MyClient.EnableMarkerData();
MyClient.EnableUnlabeledMarkerData();
MyClient.EnableDeviceData();
// Set the streaming mode
// MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ClientPull );
// MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ClientPullPreFetch );
MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ServerPush );
// Set the global up axis
MyClient.SetAxisMapping( Direction::Forward,
Direction::Left,
Direction::Up ); // Z-up
// MyClient.SetGlobalUpAxis( Direction::Forward,
// Direction::Up,
// Direction::Right ); // Y-up
Output_GetAxisMapping _Output_GetAxisMapping = MyClient.GetAxisMapping();
std::cout << "Axis Mapping: X-" << Adapt( _Output_GetAxisMapping.XAxis )
<< " Y-" << Adapt( _Output_GetAxisMapping.YAxis )
<< " Z-" << Adapt( _Output_GetAxisMapping.ZAxis ) << std::endl;
// Discover the version number
Output_GetVersion _Output_GetVersion = MyClient.GetVersion();
std::cout << "Version: " << _Output_GetVersion.Major << "."
<< _Output_GetVersion.Minor << "."
<< _Output_GetVersion.Point << std::endl;
if( EnableMultiCast )
{
assert( MyClient.IsConnected().Connected );
MyClient.StopTransmittingMulticast();
sleep(1);
MyClient.StartTransmittingMulticast( HostName, MulticastAddress );
}
ROS_INFO("Starting multicast on address %s", MulticastAddress.c_str());
// wait for a key to be pressed
std::cout << "Multicast has been started" << std::endl;
std::cout << "Press Ctrl+C to stop" << std::endl;
while( ros::ok() )
{
// get ROS stuff done first
ros::spinOnce();
// a pause
loop_rate.sleep();
}
// shutting down again
if( EnableMultiCast )
{
MyClient.StopTransmittingMulticast();
}
MyClient.DisableSegmentData();
MyClient.DisableMarkerData();
MyClient.DisableUnlabeledMarkerData();
MyClient.DisableDeviceData();
if( bReadCentroids )
{
MyClient.DisableCentroidData();
}
// Disconnect and dispose
int t = clock();
std::cout << " Disconnecting..." << std::endl;
MyClient.Disconnect();
int dt = clock() - t;
double secs = (double) (dt)/(double)CLOCKS_PER_SEC;
std::cout << " Disconnect time = " << secs << " secs" << std::endl;
}
int main( int argc, char* argv[] )
{
// Program options
bool TransmitMulticast = false;
std::string HostName = "localhost:801";
if( argc == 2 )
{
HostName = argv[1];
}
// Make a new client
Client MyClient;
for(int i=0; i != 3; ++i) // repeat to check disconnecting doesn't wreck next connect
{
// Connect to a server
std::cout << "Connecting to " << HostName << " ..." << std::flush;
while( !MyClient.IsConnected().Connected )
{
// Direct connection
MyClient.Connect( HostName );
// Multicast connection
// MyClient.ConnectToMulticast( HostName, "224.0.0.0" );
std::cout << ".";
#ifdef WIN32
Sleep( 200 );
#else
sleep(1);
#endif
}
std::cout << std::endl;
// Enable some different data types
MyClient.EnableSegmentData();
MyClient.EnableMarkerData();
MyClient.EnableUnlabeledMarkerData();
MyClient.EnableDeviceData();
std::cout << "Segment Data Enabled: " << Adapt( MyClient.IsSegmentDataEnabled().Enabled ) << std::endl;
std::cout << "Marker Data Enabled: " << Adapt( MyClient.IsMarkerDataEnabled().Enabled ) << std::endl;
std::cout << "Unlabeled Marker Data Enabled: " << Adapt( MyClient.IsUnlabeledMarkerDataEnabled().Enabled ) << std::endl;
std::cout << "Device Data Enabled: " << Adapt( MyClient.IsDeviceDataEnabled().Enabled ) << std::endl;
// Set the streaming mode
MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ClientPull );
// MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ClientPullPreFetch );
// MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ServerPush );
// Set the global up axis
MyClient.SetAxisMapping( Direction::Forward,
Direction::Left,
Direction::Up ); // Z-up
// MyClient.SetGlobalUpAxis( Direction::Forward,
// Direction::Up,
// Direction::Right ); // Y-up
Output_GetAxisMapping _Output_GetAxisMapping = MyClient.GetAxisMapping();
std::cout << "Axis Mapping: X-" << Adapt( _Output_GetAxisMapping.XAxis )
<< " Y-" << Adapt( _Output_GetAxisMapping.YAxis )
<< " Z-" << Adapt( _Output_GetAxisMapping.ZAxis ) << std::endl;
// Discover the version number
Output_GetVersion _Output_GetVersion = MyClient.GetVersion();
std::cout << "Version: " << _Output_GetVersion.Major << "."
<< _Output_GetVersion.Minor << "."
<< _Output_GetVersion.Point << std::endl;
if( TransmitMulticast )
{
MyClient.StartTransmittingMulticast( "localhost", "224.0.0.0" );
}
// Loop until a key is pressed
#ifdef WIN32
while( !Hit() )
#else
while( true)
#endif
{
// Get a frame
std::cout << "Waiting for new frame...";
while( MyClient.GetFrame().Result != Result::Success )
{
// Sleep a little so that we don't lumber the CPU with a busy poll
#ifdef WIN32
Sleep( 200 );
#else
sleep(1);
#endif
std::cout << ".";
}
std::cout << std::endl;
// Get the frame number
Output_GetFrameNumber _Output_GetFrameNumber = MyClient.GetFrameNumber();
std::cout << "Frame Number: " << _Output_GetFrameNumber.FrameNumber << std::endl;
// Get the timecode
Output_GetTimecode _Output_GetTimecode = MyClient.GetTimecode();
std::cout << "Timecode: "
<< _Output_GetTimecode.Hours << "h "
<< _Output_GetTimecode.Minutes << "m "
<< _Output_GetTimecode.Seconds << "s "
<< _Output_GetTimecode.Frames << "f "
<< _Output_GetTimecode.SubFrame << "sf "
<< Adapt( _Output_GetTimecode.FieldFlag ) << " "
<< _Output_GetTimecode.Standard << " "
<< _Output_GetTimecode.SubFramesPerFrame << " "
<< _Output_GetTimecode.UserBits << std::endl << std::endl;
// Get the latency
std::cout << "Latency: " << MyClient.GetLatencyTotal().Total << "s" << std::endl;
for( unsigned int LatencySampleIndex = 0 ; LatencySampleIndex < MyClient.GetLatencySampleCount().Count ; ++LatencySampleIndex )
{
std::string SampleName = MyClient.GetLatencySampleName( LatencySampleIndex ).Name;
double SampleValue = MyClient.GetLatencySampleValue( SampleName ).Value;
std::cout << " " << SampleName << " " << SampleValue << "s" << std::endl;
}
std::cout << std::endl;
// Count the number of subjects
unsigned int SubjectCount = MyClient.GetSubjectCount().SubjectCount;
std::cout << "Subjects (" << SubjectCount << "):" << std::endl;
for( unsigned int SubjectIndex = 0 ; SubjectIndex < SubjectCount ; ++SubjectIndex )
{
std::cout << " Subject #" << SubjectIndex << std::endl;
// Get the subject name
std::string SubjectName = MyClient.GetSubjectName( SubjectIndex ).SubjectName;
std::cout << " Name: " << SubjectName << std::endl;
// Get the root segment
std::string RootSegment = MyClient.GetSubjectRootSegmentName( SubjectName ).SegmentName;
std::cout << " Root Segment: " << RootSegment << std::endl;
// Count the number of segments
unsigned int SegmentCount = MyClient.GetSegmentCount( SubjectName ).SegmentCount;
std::cout << " Segments (" << SegmentCount << "):" << std::endl;
for( unsigned int SegmentIndex = 0 ; SegmentIndex < SegmentCount ; ++SegmentIndex )
{
std::cout << " Segment #" << SegmentIndex << std::endl;
// Get the segment name
std::string SegmentName = MyClient.GetSegmentName( SubjectName, SegmentIndex ).SegmentName;
std::cout << " Name: " << SegmentName << std::endl;
// Get the segment parent
std::string SegmentParentName = MyClient.GetSegmentParentName( SubjectName, SegmentName ).SegmentName;
std::cout << " Parent: " << SegmentParentName << std::endl;
// Get the segment's children
unsigned int ChildCount = MyClient.GetSegmentChildCount( SubjectName, SegmentName ).SegmentCount;
std::cout << " Children (" << ChildCount << "):" << std::endl;
for( unsigned int ChildIndex = 0 ; ChildIndex < ChildCount ; ++ChildIndex )
{
std::string ChildName = MyClient.GetSegmentChildName( SubjectName, SegmentName, ChildIndex ).SegmentName;
std::cout << " " << ChildName << std::endl;
}
// Get the static segment translation
Output_GetSegmentStaticTranslation _Output_GetSegmentStaticTranslation =
MyClient.GetSegmentStaticTranslation( SubjectName, SegmentName );
std::cout << " Static Translation: (" << _Output_GetSegmentStaticTranslation.Translation[ 0 ] << ", "
<< _Output_GetSegmentStaticTranslation.Translation[ 1 ] << ", "
<< _Output_GetSegmentStaticTranslation.Translation[ 2 ] << ") " << std::endl;
// Get the static segment rotation in helical co-ordinates
Output_GetSegmentStaticRotationHelical _Output_GetSegmentStaticRotationHelical =
MyClient.GetSegmentStaticRotationHelical( SubjectName, SegmentName );
std::cout << " Static Rotation Helical: (" << _Output_GetSegmentStaticRotationHelical.Rotation[ 0 ] << ", "
<< _Output_GetSegmentStaticRotationHelical.Rotation[ 1 ] << ", "
<< _Output_GetSegmentStaticRotationHelical.Rotation[ 2 ] << ") " << std::endl;
// Get the static segment rotation as a matrix
Output_GetSegmentStaticRotationMatrix _Output_GetSegmentStaticRotationMatrix =
MyClient.GetSegmentStaticRotationMatrix( SubjectName, SegmentName );
std::cout << " Static Rotation Matrix: (" << _Output_GetSegmentStaticRotationMatrix.Rotation[ 0 ] << ", "
<< _Output_GetSegmentStaticRotationMatrix.Rotation[ 1 ] << ", "
<< _Output_GetSegmentStaticRotationMatrix.Rotation[ 2 ] << ", "
<< _Output_GetSegmentStaticRotationMatrix.Rotation[ 3 ] << ", "
<< _Output_GetSegmentStaticRotationMatrix.Rotation[ 4 ] << ", "
<< _Output_GetSegmentStaticRotationMatrix.Rotation[ 5 ] << ", "
<< _Output_GetSegmentStaticRotationMatrix.Rotation[ 6 ] << ", "
<< _Output_GetSegmentStaticRotationMatrix.Rotation[ 7 ] << ", "
<< _Output_GetSegmentStaticRotationMatrix.Rotation[ 8 ] << ") " << std::endl;
// Get the static segment rotation in quaternion co-ordinates
Output_GetSegmentStaticRotationQuaternion _Output_GetSegmentStaticRotationQuaternion =
MyClient.GetSegmentStaticRotationQuaternion( SubjectName, SegmentName );
std::cout << " Static Rotation Quaternion: (" << _Output_GetSegmentStaticRotationQuaternion.Rotation[ 0 ] << ", "
<< _Output_GetSegmentStaticRotationQuaternion.Rotation[ 1 ] << ", "
<< _Output_GetSegmentStaticRotationQuaternion.Rotation[ 2 ] << ", "
<< _Output_GetSegmentStaticRotationQuaternion.Rotation[ 3 ] << ") " << std::endl;
// Get the static segment rotation in EulerXYZ co-ordinates
Output_GetSegmentStaticRotationEulerXYZ _Output_GetSegmentStaticRotationEulerXYZ =
MyClient.GetSegmentStaticRotationEulerXYZ( SubjectName, SegmentName );
std::cout << " Static Rotation EulerXYZ: (" << _Output_GetSegmentStaticRotationEulerXYZ.Rotation[ 0 ] << ", "
<< _Output_GetSegmentStaticRotationEulerXYZ.Rotation[ 1 ] << ", "
<< _Output_GetSegmentStaticRotationEulerXYZ.Rotation[ 2 ] << ") " << std::endl;
// Get the global segment translation
Output_GetSegmentGlobalTranslation _Output_GetSegmentGlobalTranslation =
MyClient.GetSegmentGlobalTranslation( SubjectName, SegmentName );
std::cout << " Global Translation: (" << _Output_GetSegmentGlobalTranslation.Translation[ 0 ] << ", "
<< _Output_GetSegmentGlobalTranslation.Translation[ 1 ] << ", "
<< _Output_GetSegmentGlobalTranslation.Translation[ 2 ] << ") "
<< Adapt( _Output_GetSegmentGlobalTranslation.Occluded ) << std::endl;
// Get the global segment rotation in helical co-ordinates
Output_GetSegmentGlobalRotationHelical _Output_GetSegmentGlobalRotationHelical =
MyClient.GetSegmentGlobalRotationHelical( SubjectName, SegmentName );
std::cout << " Global Rotation Helical: (" << _Output_GetSegmentGlobalRotationHelical.Rotation[ 0 ] << ", "
<< _Output_GetSegmentGlobalRotationHelical.Rotation[ 1 ] << ", "
<< _Output_GetSegmentGlobalRotationHelical.Rotation[ 2 ] << ") "
<< Adapt( _Output_GetSegmentGlobalRotationHelical.Occluded ) << std::endl;
// Get the global segment rotation as a matrix
Output_GetSegmentGlobalRotationMatrix _Output_GetSegmentGlobalRotationMatrix =
MyClient.GetSegmentGlobalRotationMatrix( SubjectName, SegmentName );
std::cout << " Global Rotation Matrix: (" << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 0 ] << ", "
<< _Output_GetSegmentGlobalRotationMatrix.Rotation[ 1 ] << ", "
<< _Output_GetSegmentGlobalRotationMatrix.Rotation[ 2 ] << ", "
<< _Output_GetSegmentGlobalRotationMatrix.Rotation[ 3 ] << ", "
<< _Output_GetSegmentGlobalRotationMatrix.Rotation[ 4 ] << ", "
<< _Output_GetSegmentGlobalRotationMatrix.Rotation[ 5 ] << ", "
<< _Output_GetSegmentGlobalRotationMatrix.Rotation[ 6 ] << ", "
<< _Output_GetSegmentGlobalRotationMatrix.Rotation[ 7 ] << ", "
<< _Output_GetSegmentGlobalRotationMatrix.Rotation[ 8 ] << ") "
<< Adapt( _Output_GetSegmentGlobalRotationMatrix.Occluded ) << std::endl;
// Get the global segment rotation in quaternion co-ordinates
Output_GetSegmentGlobalRotationQuaternion _Output_GetSegmentGlobalRotationQuaternion =
MyClient.GetSegmentGlobalRotationQuaternion( SubjectName, SegmentName );
std::cout << " Global Rotation Quaternion: (" << _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 0 ] << ", "
<< _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 1 ] << ", "
<< _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 2 ] << ", "
<< _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 3 ] << ") "
<< Adapt( _Output_GetSegmentGlobalRotationQuaternion.Occluded ) << std::endl;
// Get the global segment rotation in EulerXYZ co-ordinates
Output_GetSegmentGlobalRotationEulerXYZ _Output_GetSegmentGlobalRotationEulerXYZ =
MyClient.GetSegmentGlobalRotationEulerXYZ( SubjectName, SegmentName );
std::cout << " Global Rotation EulerXYZ: (" << _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 0 ] << ", "
<< _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 1 ] << ", "
<< _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 2 ] << ") "
<< Adapt( _Output_GetSegmentGlobalRotationEulerXYZ.Occluded ) << std::endl;
// Get the local segment translation
Output_GetSegmentLocalTranslation _Output_GetSegmentLocalTranslation =
MyClient.GetSegmentLocalTranslation( SubjectName, SegmentName );
std::cout << " Local Translation: (" << _Output_GetSegmentLocalTranslation.Translation[ 0 ] << ", "
<< _Output_GetSegmentLocalTranslation.Translation[ 1 ] << ", "
<< _Output_GetSegmentLocalTranslation.Translation[ 2 ] << ") "
<< Adapt( _Output_GetSegmentLocalTranslation.Occluded ) << std::endl;
// Get the local segment rotation in helical co-ordinates
Output_GetSegmentLocalRotationHelical _Output_GetSegmentLocalRotationHelical =
MyClient.GetSegmentLocalRotationHelical( SubjectName, SegmentName );
std::cout << " Local Rotation Helical: (" << _Output_GetSegmentLocalRotationHelical.Rotation[ 0 ] << ", "
<< _Output_GetSegmentLocalRotationHelical.Rotation[ 1 ] << ", "
<< _Output_GetSegmentLocalRotationHelical.Rotation[ 2 ] << ") "
<< Adapt( _Output_GetSegmentLocalRotationHelical.Occluded ) << std::endl;
// Get the local segment rotation as a matrix
Output_GetSegmentLocalRotationMatrix _Output_GetSegmentLocalRotationMatrix =
MyClient.GetSegmentLocalRotationMatrix( SubjectName, SegmentName );
std::cout << " Local Rotation Matrix: (" << _Output_GetSegmentLocalRotationMatrix.Rotation[ 0 ] << ", "
<< _Output_GetSegmentLocalRotationMatrix.Rotation[ 1 ] << ", "
<< _Output_GetSegmentLocalRotationMatrix.Rotation[ 2 ] << ", "
<< _Output_GetSegmentLocalRotationMatrix.Rotation[ 3 ] << ", "
<< _Output_GetSegmentLocalRotationMatrix.Rotation[ 4 ] << ", "
<< _Output_GetSegmentLocalRotationMatrix.Rotation[ 5 ] << ", "
<< _Output_GetSegmentLocalRotationMatrix.Rotation[ 6 ] << ", "
<< _Output_GetSegmentLocalRotationMatrix.Rotation[ 7 ] << ", "
<< _Output_GetSegmentLocalRotationMatrix.Rotation[ 8 ] << ") "
<< Adapt( _Output_GetSegmentLocalRotationMatrix.Occluded ) << std::endl;
// Get the local segment rotation in quaternion co-ordinates
Output_GetSegmentLocalRotationQuaternion _Output_GetSegmentLocalRotationQuaternion =
MyClient.GetSegmentLocalRotationQuaternion( SubjectName, SegmentName );
std::cout << " Local Rotation Quaternion: (" << _Output_GetSegmentLocalRotationQuaternion.Rotation[ 0 ] << ", "
<< _Output_GetSegmentLocalRotationQuaternion.Rotation[ 1 ] << ", "
<< _Output_GetSegmentLocalRotationQuaternion.Rotation[ 2 ] << ", "
<< _Output_GetSegmentLocalRotationQuaternion.Rotation[ 3 ] << ") "
<< Adapt( _Output_GetSegmentLocalRotationQuaternion.Occluded ) << std::endl;
// Get the local segment rotation in EulerXYZ co-ordinates
Output_GetSegmentLocalRotationEulerXYZ _Output_GetSegmentLocalRotationEulerXYZ =
MyClient.GetSegmentLocalRotationEulerXYZ( SubjectName, SegmentName );
std::cout << " Local Rotation EulerXYZ: (" << _Output_GetSegmentLocalRotationEulerXYZ.Rotation[ 0 ] << ", "
<< _Output_GetSegmentLocalRotationEulerXYZ.Rotation[ 1 ] << ", "
<< _Output_GetSegmentLocalRotationEulerXYZ.Rotation[ 2 ] << ") "
<< Adapt( _Output_GetSegmentLocalRotationEulerXYZ.Occluded ) << std::endl;
}
// Count the number of markers
unsigned int MarkerCount = MyClient.GetMarkerCount( SubjectName ).MarkerCount;
std::cout << " Markers (" << MarkerCount << "):" << std::endl;
for( unsigned int MarkerIndex = 0 ; MarkerIndex < MarkerCount ; ++MarkerIndex )
{
// Get the marker name
std::string MarkerName = MyClient.GetMarkerName( SubjectName, MarkerIndex ).MarkerName;
// Get the marker parent
std::string MarkerParentName = MyClient.GetMarkerParentName( SubjectName, MarkerName ).SegmentName;
// Get the global marker translation
Output_GetMarkerGlobalTranslation _Output_GetMarkerGlobalTranslation =
MyClient.GetMarkerGlobalTranslation( SubjectName, MarkerName );
std::cout << " Marker #" << MarkerIndex << ": "
<< MarkerName << " ("
<< _Output_GetMarkerGlobalTranslation.Translation[ 0 ] << ", "
<< _Output_GetMarkerGlobalTranslation.Translation[ 1 ] << ", "
<< _Output_GetMarkerGlobalTranslation.Translation[ 2 ] << ") "
<< Adapt( _Output_GetMarkerGlobalTranslation.Occluded ) << std::endl;
}
}
// Get the unlabeled markers
unsigned int UnlabeledMarkerCount = MyClient.GetUnlabeledMarkerCount().MarkerCount;
std::cout << " Unlabeled Markers (" << UnlabeledMarkerCount << "):" << std::endl;
for( unsigned int UnlabeledMarkerIndex = 0 ; UnlabeledMarkerIndex < UnlabeledMarkerCount ; ++UnlabeledMarkerIndex )
{
// Get the global marker translation
Output_GetUnlabeledMarkerGlobalTranslation _Output_GetUnlabeledMarkerGlobalTranslation =
MyClient.GetUnlabeledMarkerGlobalTranslation( UnlabeledMarkerIndex );
std::cout << " Marker #" << UnlabeledMarkerIndex << ": ("
<< _Output_GetUnlabeledMarkerGlobalTranslation.Translation[ 0 ] << ", "
<< _Output_GetUnlabeledMarkerGlobalTranslation.Translation[ 1 ] << ", "
<< _Output_GetUnlabeledMarkerGlobalTranslation.Translation[ 2 ] << ") " << std::endl;
}
// Count the number of devices
unsigned int DeviceCount = MyClient.GetDeviceCount().DeviceCount;
std::cout << " Devices (" << DeviceCount << "):" << std::endl;
for( unsigned int DeviceIndex = 0 ; DeviceIndex < DeviceCount ; ++DeviceIndex )
{
std::cout << " Device #" << DeviceIndex << ":" << std::endl;
// Get the device name and type
Output_GetDeviceName _Output_GetDeviceName = MyClient.GetDeviceName( DeviceIndex );
std::cout << " Name: " << _Output_GetDeviceName.DeviceName << std::endl;
std::cout << " Type: " << Adapt( _Output_GetDeviceName.DeviceType ) << std::endl;
// Count the number of device outputs
unsigned int DeviceOutputCount = MyClient.GetDeviceOutputCount( _Output_GetDeviceName.DeviceName ).DeviceOutputCount;
std::cout << " Device Outputs (" << DeviceOutputCount << "):" << std::endl;
for( unsigned int DeviceOutputIndex = 0 ; DeviceOutputIndex < DeviceOutputCount ; ++DeviceOutputIndex )
{
// Get the device output name and unit
Output_GetDeviceOutputName _Output_GetDeviceOutputName =
MyClient.GetDeviceOutputName( _Output_GetDeviceName.DeviceName, DeviceOutputIndex );
// Get the device output value
Output_GetDeviceOutputValue _Output_GetDeviceOutputValue =
MyClient.GetDeviceOutputValue( _Output_GetDeviceName.DeviceName, _Output_GetDeviceOutputName.DeviceOutputName );
std::cout << " Device Output #" << DeviceOutputIndex << ": "
<< _Output_GetDeviceOutputName.DeviceOutputName << " "
<< _Output_GetDeviceOutputValue.Value << " "
<< Adapt( _Output_GetDeviceOutputName.DeviceOutputUnit ) << " "
<< Adapt( _Output_GetDeviceOutputValue.Occluded ) << std::endl;
}
}
// Output the force plate information.
unsigned int ForcePlateCount = MyClient.GetForcePlateCount().ForcePlateCount;
std::cout << "Force Plates: " << ForcePlateCount << std::endl;
for( unsigned int ForcePlateIndex = 0 ; ForcePlateIndex < ForcePlateCount ; ++ForcePlateIndex )
{
std::cout << " Force Plate #" << ForcePlateIndex << ":" << std::endl;
Output_GetGlobalForceVector _Output_GetForceVector = MyClient.GetGlobalForceVector( ForcePlateIndex );
std::cout << " Force:" << std::endl;
std::cout << " X: " << _Output_GetForceVector.ForceVector[ 0 ] << std::endl;
std::cout << " Y: " << _Output_GetForceVector.ForceVector[ 1 ] << std::endl;
std::cout << " Z: " << _Output_GetForceVector.ForceVector[ 2 ] << std::endl;
Output_GetGlobalMomentVector _Output_GetMomentVector = MyClient.GetGlobalMomentVector( ForcePlateIndex );
std::cout << " Moment:" << std::endl;
std::cout << " X: " << _Output_GetMomentVector.MomentVector[ 0 ] << std::endl;
std::cout << " Y: " << _Output_GetMomentVector.MomentVector[ 1 ] << std::endl;
std::cout << " Z: " << _Output_GetMomentVector.MomentVector[ 2 ] << std::endl;
Output_GetGlobalCentreOfPressure _Output_GetCentreOfPressure = MyClient.GetGlobalCentreOfPressure( ForcePlateIndex );
std::cout << " Centre Of Pressure:" << std::endl;
std::cout << " X: " << _Output_GetCentreOfPressure.CentreOfPressure[ 0 ] << std::endl;
std::cout << " Y: " << _Output_GetCentreOfPressure.CentreOfPressure[ 1 ] << std::endl;
std::cout << " Z: " << _Output_GetCentreOfPressure.CentreOfPressure[ 2 ] << std::endl;
}
}
if( TransmitMulticast )
{
MyClient.StopTransmittingMulticast();
}
MyClient.DisableSegmentData();
MyClient.DisableMarkerData();
MyClient.DisableUnlabeledMarkerData();
MyClient.DisableDeviceData();
// Disconnect and dispose
int t = clock();
std::cout << " Disconnecting..." << std::endl;
MyClient.Disconnect();
int dt = clock() - t;
double secs = (double) (dt)/(double)CLOCKS_PER_SEC;
std::cout << " Disconnect time = " << secs << " secs" << std::endl;
}
}
void ClientsManager::run(void)
{
//Make sure the thread stays running
while (!threadShouldExit())//as long as the thread should stay running
{
if (indexpointer <= -1)
{
juce::Thread::yield();//give some more time back to other threads before processing clients again
lockProcess->enterRead();//make sure size() is properly readable
maxsize = clientslist->size();//Let's see if there is anything to process
lockProcess->exitRead();//Reading is done, no need to keep it mutex locked
if (maxsize <= 0)//No clients...
{
juce::Thread::sleep(1);//no clients, wait 1 millisecond
}
else//We have clients to process!
{
//possibly initialise codes before client looping starts?
laststamp = juce::Time::currentTimeMillis();
//assign indexpointer to last client
indexpointer = (maxsize - 1);
}
}
else //There are clients to process as long indexpointer >= 0
{
lockProcess->enterWrite();//lock the clientslist so this client pointer doesn't magically dissappear
maxsize = clientslist->size();//Let's see if the next client didn't suddenly dissappear
if (indexpointer >= maxsize)//The wanted index is not set..
{
indexpointer = (maxsize - 1);//take the last used index and use that one instead
}
//Get client
Client* c = clientslist->getUnchecked(indexpointer);
int currentindex = indexpointer;
//move to next client next iteration or if process is somehow interrupted
--indexpointer;
//process the client..
if (c->IsConnectionStateExpired(laststamp))
{
if (c->CheckIfDisconnected())
{
c->MarkForDeletion();
}
}
if (c->IsMarkedForDeletion())
{
c->Disconnect();
if (serverprocessor != NULL)//special exception for server functions
{
serverprocessor->ClientWasRemoved(c);
}
#ifdef FEEDBACK_CLIENTSMANAGER_REMOVED_CLIENTS
juce::String message("Client `");
const std::string* clientname = c->GetClientName();
if (clientname->length() > 0)//if name is set
{
message += *clientname;
}
else
{
message += CLIENT_UNKNOWN_NAME_TEXT;
}
message += "` (";
message += *c->GetAddress();
message += ':';
message += *c->GetPort();
message += ") disconnected";
//possibly a switch case is needed here, for windows vs mac vs linux
message += '\n';//newline
std::cout << message;
#endif
arraylock.enter();
clientslist->remove(currentindex, true);
arraylock.exit();
}
else
{
/*
Here, a mechanic could be added to stop execution time of thread after client processing time.
If time exceeds the time threshol, restart thread.
*/
//process incoming data
c->ProcessBuffers();
//check if there is data to process
if (c->HasBufferedCommunicationObjects())
{
juce::OwnedArray<CommunicationObject>* messages = c->GetBufferedCommunicationObjects();
for (int i = (messages->size() - 1); i >= 0; i--)
{
CommunicationObject* message = messages->getUnchecked(i);
messages->remove(i, false);
/*
std::cout << "TEST: " << message->ToString(c->GetLastClassPresentationProtocol()) << std::endl;
*/
CommunicationObjectType* devicename = message->ClearDataIndex(0);
if (devicename != NULL)
{
juce::juce_wchar type = devicename->GetType();
if (type == 's' || type == 'S' || type == 'A')
{
char* devicenamecharpointer = (char*)devicename->GetRawDataCopy();
juce::String devicenamestring(devicenamecharpointer);
delete devicenamecharpointer;
delete devicename;
switch (devicenamestring[0])//this message is meant for a group
{
case '#':
{
devicenamestring = devicenamestring.substring(1);
if (serverprocessor != NULL)//special exception for server functions
{
if (devicenamestring.equalsIgnoreCase("server"))
{
serverprocessor->ProcesssFunction(c, message);
continue;
}
}
//a group of clients
{
if (c->IsFullyIdentified())//only clients that are identified are allowed to do this
{
const std::string* this_clientname = c->GetClientName();
message->ReplaceDataIndex(0, new CommunicationObjectType(*this_clientname));
for (int u = (maxsize - 1); u >= 0; u--)//and iterate backwards
{
Client* t = clientslist->getUnchecked(u);
if (t != c && t->IsFullyIdentified())//do not self to self and target must be identified
{
if (!t->MarkedSenderOnly() && t->IsClientPartOfGroup(devicenamestring))
{
t->Send(message);
}
}
}
}
}
break;//#
}
case '?'://meant for every client (send message to every client)
{
if (c->IsFullyIdentified())//only clients that are identified are allowed to do this
{
const std::string* this_clientname = c->GetClientName();
message->ReplaceDataIndex(0, new CommunicationObjectType(*this_clientname));
for (int u = (maxsize - 1); u >= 0; u--)//and iterate backwards
{
Client* t = clientslist->getUnchecked(u);
if (!t->MarkedSenderOnly() && t != c)//do not send to self
{
t->Send(message);
}
}
}
break;//?
}
case '!'://meant for every registered client (send message to every registered client)
{
if (c->IsFullyIdentified())//only clients that are identified are allowed to do this
{
const std::string* this_clientname = c->GetClientName();
message->ReplaceDataIndex(0, new CommunicationObjectType(*this_clientname));
for (int u = (maxsize - 1); u >= 0; u--)//and iterate backwards
{
Client* t = clientslist->getUnchecked(u);
if (!t->MarkedSenderOnly() && t != c && t->IsFullyIdentified())//do not send to self
{
t->Send(message);
}
}
}
break;//!
}
default://compare name
{
if (c->IsFullyIdentified())//only clients that are identified are allowed to do this
{
const std::string* this_clientname = c->GetClientName();
message->ReplaceDataIndex(0, new CommunicationObjectType(*this_clientname));
for (int u = (maxsize - 1); u >= 0; u--)//and iterate backwards
{
Client* t = clientslist->getUnchecked(u);
if (!t->MarkedSenderOnly() && t != c && t->IsFullyIdentified())//do not self to self and target must be identified
{
if (devicenamestring.compareNatural(juce::StringRef(*t->GetClientName())))
{
t->Send(message);
break;
}
}
}
}
break;
}
}
}
else
{
delete devicename;
}
}
delete message;
}
messages->clear(false);
delete messages;
//messages = NULL;
}
}
lockProcess->exitWrite();//unlock mutex
//Give other threads more processing time by yielding after X time
//The following algorithm checks whether the amount of clients superseeds or underseeds the load processing
//If there are many clients, the yield will happen less often
if (++AdaptiveLoad_CurrentBatchSizeClientProcessing >= AdaptiveLoad_BatchSizeClientProcessing)
{
//after processing X clients, reset counter so yielding won't happen immediately after
AdaptiveLoad_CurrentBatchSizeClientProcessing = 0;
if (maxsize - AdaptiveLoad_BatchSizeClientProcessing > AdaptiveLoad_HeavyLoadNumClientsThreshhold)//if is bigger
{
AdaptiveLoad_BatchSizeClientProcessing += AdaptiveLoad_HeavyLoadNumClientsThreshhold;
}
else if (AdaptiveLoad_BatchSizeClientProcessing - maxsize > AdaptiveLoad_HeavyLoadNumClientsThreshhold)
{
AdaptiveLoad_BatchSizeClientProcessing -= AdaptiveLoad_HeavyLoadNumClientsThreshhold;
if (AdaptiveLoad_BatchSizeClientProcessing < AdaptiveLoad_HeavyLoadNumClientsThreshhold)
{
AdaptiveLoad_BatchSizeClientProcessing = AdaptiveLoad_HeavyLoadNumClientsThreshhold;//make sure at least some clients are processed
}
}
juce::Thread::yield();//yield once to provide an additional time slice for other threads
}
}
}
}
int main( int argc, char* argv[] )
{
//std::cout << "Starting Vicon UDP connector..." << std::endl << std::flush;
/* int ii;
for (ii=0; ii<=argc; ii++) {
//std::cout << argv[ii] << std::endl;
} */
// Program options
std::string HostName = "192.168.10.81";
std::string TargetSubjectName = "QAV_GREEN";
std::string ViconBaseFrame = "/world";
std::string MulticastAddress = "224.0.0.0:44801";
// scaling constant
double pos_scale = 1e-3;
// do the ROS setup
ros::init(argc, argv, "udp_client");
ros::NodeHandle n;
// ROS parameters
std::string s;
// mutlicast address
if (n.getParam("vicon_multicast_address", s)) {
MulticastAddress = s;
//std::cout << "Got multicast " << MulticastAddress << " from parameter." << std::endl;
}
// own hostname
if (n.getParam("vicon_client_hostname", s)) {
HostName = s;
//std::cout << "Got hostname " << HostName << " from parameter." << std::endl;
}
// vicon frame ID
if (n.getParam("vicon_tf_parent_frame", s)) {
ViconBaseFrame = s;
//std::cout << "Got frame ID " << ViconBaseFrame << " from parameter." << std::endl;
}
// tracking object (this one is private as unique to each node)
// try the private thing using the "bare" method
if (ros::param::has("~vicon_target_subject")) {
ros::param::get("~vicon_target_subject", TargetSubjectName);
}
// set up ROS publishing
std::string TopicName = "vicon/" + TargetSubjectName + "/" + TargetSubjectName;
ros::Publisher pose_pub = n.advertise<geometry_msgs::TransformStamped>(TopicName, 1000);
ros::Rate loop_rate(300);
//std::cout << "HostName: " << HostName << std::endl;
//std::cout << "Multicast: " << MulticastAddress << std::endl;
//std::cout << "Parent frame: " << ViconBaseFrame << std::endl;
//std::cout << "Target subject: " << TargetSubjectName << std::endl;
// initialize the transform
geometry_msgs::TransformStamped MyTransform;
MyTransform.header.frame_id = ViconBaseFrame;
//ROS_INFO("Publishing vicon object %s/%s on topic %s", TargetSubjectName.c_str(), TargetSubjectName.c_str(), TopicName.c_str());
// initialize transform broadcaster
tf::TransformBroadcaster TfBroadcaster;
tf::Transform MyTfTransform;
// log contains:
// version number
// log of framerate over time
// --multicast
// kill off internal app
bool ConnectToMultiCast = true;
bool EnableMultiCast = false;
bool EnableHapticTest = false;
bool bReadCentroids = false;
std::vector<std::string> HapticOnList(0);
// variables for data retrieval
Output_GetSegmentGlobalRotationQuaternion _Output_GetSegmentGlobalRotationQuaternion;
Output_GetSegmentGlobalTranslation _Output_GetSegmentGlobalTranslation;
Output_GetFrame _Output_GetFrame;
// Make a new client
Client MyClient;
// Connect to a server
ROS_INFO("Connecting to multicast %s as host %s", MulticastAddress.c_str(), HostName.c_str());
//std::cout << "Connecting to " << HostName << " ..." << std::flush;
while( !MyClient.IsConnected().Connected )
{
// Direct connection
bool ok = false;
if(ConnectToMultiCast)
{
// Multicast connection
ok = ( MyClient.ConnectToMulticast( HostName, MulticastAddress ).Result == Result::Success );
}
else
{
ok =( MyClient.Connect( HostName ).Result == Result::Success );
}
if(!ok)
{
////std::cout << "Warning - connect failed..." << std::endl;
ROS_WARN("Vicon connection failed...");
}
//std::cout << ".";
sleep(1);
}
//std::cout << "Connected" << std::endl;
// Enable some different data types
MyClient.EnableSegmentData();
//MyClient.EnableMarkerData();
//MyClient.EnableUnlabeledMarkerData();
//MyClient.EnableDeviceData();
if( bReadCentroids )
{
MyClient.EnableCentroidData();
}
//std::cout << "Segment Data Enabled: " << Adapt( MyClient.IsSegmentDataEnabled().Enabled ) << std::endl;
//std::cout << "Marker Data Enabled: " << Adapt( MyClient.IsMarkerDataEnabled().Enabled ) << std::endl;
//std::cout << "Unlabeled Marker Data Enabled: " << Adapt( MyClient.IsUnlabeledMarkerDataEnabled().Enabled ) << std::endl;
//std::cout << "Device Data Enabled: " << Adapt( MyClient.IsDeviceDataEnabled().Enabled ) << std::endl;
//std::cout << "Centroid Data Enabled: " << Adapt( MyClient.IsCentroidDataEnabled().Enabled ) << std::endl;
// Set the streaming mode
// MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ClientPull );
// MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ClientPullPreFetch );
MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ServerPush );
// Set the global up axis
MyClient.SetAxisMapping( Direction::Forward,
Direction::Left,
Direction::Up ); // Z-up
// MyClient.SetGlobalUpAxis( Direction::Forward,
// Direction::Up,
// Direction::Right ); // Y-up
Output_GetAxisMapping _Output_GetAxisMapping = MyClient.GetAxisMapping();
//std::cout << "Axis Mapping: X-" << Adapt( _Output_GetAxisMapping.XAxis )
// << " Y-" << Adapt( _Output_GetAxisMapping.YAxis )
// << " Z-" << Adapt( _Output_GetAxisMapping.ZAxis ) << std::endl;
// Discover the version number
Output_GetVersion _Output_GetVersion = MyClient.GetVersion();
//std::cout << "Version: " << _Output_GetVersion.Major << "."
// << _Output_GetVersion.Minor << "."
// << _Output_GetVersion.Point << std::endl;
if( EnableMultiCast )
{
assert( MyClient.IsConnected().Connected );
MyClient.StartTransmittingMulticast( HostName, MulticastAddress );
}
//ROS_INFO("Publishing vicon object %s relative to %s", TargetSubjectName.c_str(), ViconBaseFrame.c_str());
ROS_INFO("Publishing vicon object %s", TargetSubjectName.c_str());
size_t FrameRateWindow = 1000; // frames
size_t Counter = 0;
clock_t LastTime = clock();
// Loop until a key is pressed
while( ros::ok() )
{
// get ROS stuff done first
ros::spinOnce();
// a pause
loop_rate.sleep();
//try {
// Get a frame
//ROS_INFO("Getting new frame...");
_Output_GetFrame = MyClient.GetFrame();
//ROS_INFO("Got frame");
if (_Output_GetFrame.Result != Result::Success) {
ROS_INFO("Missed frame");
//std::cout << "Didn't get new frame..." << std::endl << std::flush;
continue;
}
// Get the frame number
//Output_GetFrameNumber _Output_GetFrameNumber = MyClient.GetFrameNumber();
////std::cout << "Frame Number: " << _Output_GetFrameNumber.FrameNumber << std::endl;
//Output_GetFrameRate Rate = MyClient.GetFrameRate();
////std::cout << "Frame rate: " << Rate.FrameRateHz << std::endl;
// Get the timecode
//Output_GetTimecode _Output_GetTimecode = MyClient.GetTimecode();
/* //std::cout << "Timecode: "
<< _Output_GetTimecode.Hours << "h "
<< _Output_GetTimecode.Minutes << "m "
<< _Output_GetTimecode.Seconds << "s "
<< _Output_GetTimecode.Frames << "f "
<< _Output_GetTimecode.SubFrame << "sf "
<< Adapt( _Output_GetTimecode.FieldFlag ) << " "
<< _Output_GetTimecode.Standard << " "
<< _Output_GetTimecode.SubFramesPerFrame << " "
<< _Output_GetTimecode.UserBits << std::endl << std::endl;
*/
// Get the global rotation of the target segment
_Output_GetSegmentGlobalRotationQuaternion =
MyClient.GetSegmentGlobalRotationQuaternion( TargetSubjectName, TargetSubjectName );
// test if we got it OK
if (_Output_GetSegmentGlobalRotationQuaternion.Result == Result::Success) {
/* std::cout << "+=+= Global Rotation Quaternion of " << TargetSubjectName << "/" << TargetSubjectName
<< ": (" << _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 0 ] << ", "
<< _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 1 ] << ", "
<< _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 2 ] << ", "
<< _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 3 ] << ") "
<< Adapt( _Output_GetSegmentGlobalRotationQuaternion.Occluded ) << std::endl; */
}
else {
//std::cout << "Failed to get quaternion" << std::endl;
continue;
}
// Get the global segment translation
_Output_GetSegmentGlobalTranslation =
MyClient.GetSegmentGlobalTranslation( TargetSubjectName, TargetSubjectName );
// test if we got it OK
if (_Output_GetSegmentGlobalTranslation.Result == Result::Success) {
/* std::cout << "+=+= Global Translation of " << TargetSubjectName << "/" << TargetSubjectName
<< ": (" << _Output_GetSegmentGlobalTranslation.Translation[ 0 ] << ", "
<< _Output_GetSegmentGlobalTranslation.Translation[ 1 ] << ", "
<< _Output_GetSegmentGlobalTranslation.Translation[ 2 ] << ") "
<< Adapt( _Output_GetSegmentGlobalTranslation.Occluded ) << std::endl; */
}
else {
//std::cout << "Failed to get translation" << std::endl;
continue;
}
// populate the transform
MyTransform.transform.translation.x = _Output_GetSegmentGlobalTranslation.Translation[ 0 ] * pos_scale;
MyTransform.transform.translation.y = _Output_GetSegmentGlobalTranslation.Translation[ 1 ] * pos_scale;
MyTransform.transform.translation.z = _Output_GetSegmentGlobalTranslation.Translation[ 2 ] * pos_scale;
MyTransform.transform.rotation.x = _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 0 ];
MyTransform.transform.rotation.y = _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 1 ];
MyTransform.transform.rotation.z = _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 2 ];
MyTransform.transform.rotation.w = _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 3 ];
// publish the thing
pose_pub.publish(MyTransform);
// and the TF broadcast
MyTfTransform.setOrigin( tf::Vector3(_Output_GetSegmentGlobalTranslation.Translation[ 0 ] * pos_scale,
_Output_GetSegmentGlobalTranslation.Translation[ 1 ] * pos_scale,
_Output_GetSegmentGlobalTranslation.Translation[ 2 ] * pos_scale) );
MyTfTransform.setRotation( tf::Quaternion(_Output_GetSegmentGlobalRotationQuaternion.Rotation[ 0 ],
_Output_GetSegmentGlobalRotationQuaternion.Rotation[ 1 ],
_Output_GetSegmentGlobalRotationQuaternion.Rotation[ 2 ],
_Output_GetSegmentGlobalRotationQuaternion.Rotation[ 3 ]) );
TfBroadcaster.sendTransform(tf::StampedTransform(MyTfTransform, ros::Time::now(), ViconBaseFrame, TopicName));
//}
/* catch (...) {
ROS_INFO("Vicon problem");
}
*/
}
MyClient.DisableSegmentData();
MyClient.DisableMarkerData();
MyClient.DisableUnlabeledMarkerData();
MyClient.DisableDeviceData();
ROS_INFO("Disconnecting from Vicon...");
// Disconnect and dispose
int t = clock();
//std::cout << " Disconnecting..." << std::endl;
MyClient.Disconnect();
int dt = clock() - t;
double secs = (double) (dt)/(double)CLOCKS_PER_SEC;
//std::cout << " Disconnect time = " << secs << " secs" << std::endl;
}
