void start_client(std::string url, bool *bexit)
{
Client c;
auto proto = c.GetProtocol();
c.SetReceiveCb(
[&, proto](SessionId sess, const void* data, size_t bytes)
{
g_client_recv += bytes;
proto->Send(sess, data, bytes, [&, bytes](boost_ec ec){
if (ec) g_client_send_err += bytes;
else g_client_send += bytes;
});
});
boost_ec ec = c.Connect(url);
if (ec) {
if (!*bexit) {
sleep(1);
go [=]{ start_client(url, bexit); };
}
return ;
}
c.Send(g_data, sizeof(g_data));
while (!*bexit) {
sleep(1);
if (!proto->IsEstab(c.GetSessId())) {
go [=]{ start_client(url, bexit); };
return ;
}
}
// printf("client exit\n");
}
int main()
{
// Step1: 创建一个Client对象
Client client;
// Step2: 设置收到数据的处理函数
client.SetReceiveCb(&OnMessage);
// Step3: 连接
// * 连接接口goStart接受一个url, 规则与Server的goStart接口相同,
// 参见tcpserver.cpp教程
boost_ec ec = client.Connect("tcp://127.0.0.1:3030");
// Step4: 处理Connect返回值, 检测是否连接成功
if (ec) {
printf("client connect error %d:%s\n", ec.value(), ec.message().c_str());
return 1;
} else {
printf("connected to %s:%d\n", client.LocalAddr().address().to_string().c_str(),
client.LocalAddr().port());
std::string s = "Hello libgonet!";
client.Send(s.c_str(), s.size(), [](boost_ec ec) {
printf("send ec:%s\n", ec.message().c_str());
});
}
// Step5: 启动协程调度器
co_sched.RunUntilNoTask();
return 0;
}
int DERPEditor::net_client_connect(lua_State *L)
{
if (lua_gettop(L) == 3)
{
lua_getfield(L, -3, "instance");
Client* client = *(Client**)lua_touserdata(L, -1);
bool ret = client->Connect(lua_tolstring(L, -3, NULL), lua_tonumber(L, -2));
// Message("aoeu", "connecting to %s", lua_tolstring(L, -3, NULL));
if (!ret)
{
lua_pushstring(L, "Couldn't connect.");
return 1;
}
return 0;
}
else
{
lua_pushstring(L, "Invalid arguments passed to connect function!");
lua_error(L);
}
return 0;
}
static PyObject* pyvicon_connect(PyObject* self, PyObject* args) {
//inputs
PyObject* capsule;
char* address;
//parse
if (!PyArg_ParseTuple(args, "Os", &capsule, &address)) return NULL;
Client* client = (Client*)PyCapsule_GetPointer(capsule, NULL);
//thread waits here
Output_Connect out;
Py_BEGIN_ALLOW_THREADS
out = client->Connect(address);
Py_END_ALLOW_THREADS
//true if connected, false if failed
switch (out.Result) {
case Result::Success:
Py_RETURN_TRUE;
case Result::ClientAlreadyConnected:
Py_RETURN_TRUE;
case Result::ClientConnectionFailed:
Py_RETURN_FALSE;
default:
break;
}
//raise errors for everything else
if (handleError(out.Result)) return NULL;
//catch the rest
Py_RETURN_FALSE;
}
int main()
{
// Create Client object.
Client ClientObj;
// Create Client Socket.
ClientObj.CreateSocket(TCPSOCKET);
ClientObj.SetSocketOptions();
// Initialise and bind Client address.
ClientObj.InitialiseAddress(6001);
ClientObj.Bind();
char ServerName[24];
int ServerPort;
cout << "Enter Server name or IP: "; // Use localhost or 127.0.0.1 for local server.
cin.getline(ServerName, 24);
cout << "Enter Server port: ";
cin >> ServerPort;
// Connect to Server. Server name/IP and port are provided as arguments.
ClientObj.Connect(ServerName, ServerPort);
// Send and receive.
char ClientMessage[] = "Hello from client.";
ClientObj.Send((void*)ClientMessage, (unsigned int)strlen(ClientMessage));
ClientObj.Receive();
cout << "Server says: " << ClientObj.GetBuffer() << endl;
ClientObj.CloseClientSocket();
return 0;
}
int main()
{
std::pair<std::string, std::string> con;
Client clt;
Map map;
Menu *menu;
try
{
menu = new Menu();
con = menu->run(map);
delete menu;
clt.Connect(con.first, con.second);
}
catch (std::runtime_error &err)
{
std::cerr << "Error: " << err.what() <<std::endl;
return (1);
}
catch (std::exception &err)
{
return (0);
}
clt.run(map);
return (0);
}
void viconInit()
{
// Connect to a server
std::cout << "Connecting to " << HostName << " ..." << std::flush;
int attemptConnectCount = 0;
const int MAX_CONNECT_ATTEMPTS=2;
while( !MyClient.IsConnected().Connected && attemptConnectCount < MAX_CONNECT_ATTEMPTS)
{
attemptConnectCount++;
bool ok = false;
ok =( MyClient.Connect( HostName ).Result == Result::Success );
if(!ok)
std::cout << "Warning - connect failed..." << std::endl;
std::cout << ".";
sleep(1);
}
if(attemptConnectCount == MAX_CONNECT_ATTEMPTS)
{
printf("Giving up making connection to Vicon system\n");
return;
}
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;
}
bool StorageClient::isConnected(){
Client* client = new Client(_ip,_port);
client->Connect();
bool connected = client->isConnect();
client->sendMessage("disconnect");
delete client;
return connected;
}
int main()
{
// Create Client object.
Client ClientObj;
// Create Client Socket.
ClientObj.CreateSocket(TCPSOCKET);
ClientObj.SetSocketOptions();
// Initialise and bind Client address.
ClientObj.InitialiseAddress(6001);
ClientObj.Bind();
char ServerName[24];
int ServerPort;
cout << "Enter Server name or IP: "; // Use localhost or 127.0.0.1 for local server.
cin.getline(ServerName, 24);
cout << "Enter Server port: ";
cin >> ServerPort;
// Connect to Server. Server name/IP and port are provided as arguments.
ClientObj.Connect(ServerName, ServerPort);
// Send and receive.
int Offset;
fstream File;
File.open("received.pdf", ios::in|ios::out|ios::binary);
if (File.is_open()==true)
{
File.seekp(0, ios::end);
Offset = (int)File.tellp();
}
else
{
// If file does not exist then create a new file.
File.open("received.pdf", ios::out|ios::binary);
Offset = 0;
}
ClientObj.Send((void*)&Offset, sizeof(int));
int filesize;
ClientObj.Receive((void*)&filesize, sizeof(int));
cout << "File size: " << filesize << "B (" << (float)filesize/1024 << "KB)." << endl;
if (Offset != 0)
cout << "Resuming download from " << Offset << "B" << endl;
char c;
for (int i=0; i<(filesize-Offset); i++)
{
ClientObj.Receive((void*)&c, sizeof(char));
File.write((char*)&c, sizeof(char));
}
File.close();
ClientObj.CloseClientSocket();
return 0;
}
std::string StorageClient::getDiskSize(int pDiskID)
{
Client* client = new Client(_ip,_port);
client->Connect();
std::string message = std::string("getDiskSize ") + std::to_string(_sessionID) + std::string(" ") + std::to_string(pDiskID);
client->sendMessage(message);
std::string messagea = client->readMessage();
client->sendMessage("disconnect");
delete client;
return messagea;
}
int StorageClient::writeBlock(int pDiskID, std::string pData)
{
Client* client = new Client(_ip,_port);
client->Connect();
std::string message = std::string("writeBlock ") + std::to_string(_sessionID) + std::string(" ") + std::to_string(pDiskID) + std::string(" ") + std::string(pData);
client->sendMessage(message);
std::string messagea = client->readMessage();
client->sendMessage("disconnect");
delete client;
return std::stoi((messagea[0]!='?'?messagea:"0"));
}
std::string StorageClient::readBytes(int pDiskID, int pBlock, int pOffSet, int pSize)
{
Client* client = new Client(_ip,_port);
client->Connect();
std::string message = std::string("readBytes ") + std::to_string(_sessionID) + std::string(" ") + std::to_string(pDiskID) + std::string(" ") + std::to_string(pBlock) + std::string(" ") + std::to_string(pOffSet) + std::string(" ") + std::to_string(pSize);
client->sendMessage(message);
std::string messagea = client->readMessage();
client->sendMessage("disconnect");
delete client;
return messagea;
}
bool StorageClient::isAlive(int pDiskID)
{
Client* client = new Client(_ip,_port);
client->Connect();
std::string message = std::string("isAlive ") + std::to_string(pDiskID);
client->sendMessage(message);
std::string messagea = client->readMessage();
client->sendMessage("disconnect");
delete client;
std::cout<<"isAlive request. Disk: '"<<pDiskID<<"' Alive: "<<messagea<<std::endl;
return messagea == "true";
}
std::string StorageClient::connect(int pDiskID, std::string pSecurityKeyMD5)
{
Client* client = new Client(_ip,_port);
client->Connect();
std::string message = std::string("connect ") + std::to_string(_sessionID) + std::string(" ") + std::to_string(pDiskID) + std::string(" ") + std::string(pSecurityKeyMD5);
client->sendMessage(message);
std::string messagea = client->readMessage();
client->sendMessage("disconnect");
delete client;
return messagea;
}
void initialize_VICON( void )
{
int trying = 0;
cout << " Connecting to Giganet......" << endl;
// ============================= Connect to Giganet (Vicon motion systems) =============================
while( !MyClient.IsConnected().Connected )
{
if(trying++ > 3)
break;
Output_Connect _Output_Connect = MyClient.Connect( "localhost:801" );
Sleep(200);
}
if(trying > 3)
return;
Output_EnableMarkerData _Output_EnableMarkerData = MyClient.EnableMarkerData();
Output_EnableUnlabeledMarkerData _Output_EnableUnlabeledMarkerData = MyClient.EnableUnlabeledMarkerData();
Output_EnableDeviceData _Output_EnableDeviceData = MyClient.EnableDeviceData();
Output_SetStreamMode _Output_SetStreamMode = MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ClientPull );
cout << " Giganet successfully connected." << endl << endl;
/// Wait until frame is arrived.
while( MyClient.GetFrame().Result != Result::Success );
/// Get number of object shown in vicon
unsigned int SubjectCount = MyClient.GetSubjectCount().SubjectCount;
cout << "[NOTICE] " << SubjectCount << " objects found" << endl;
for( unsigned int SubjectIndex = 0 ; SubjectIndex < SubjectCount ; ++SubjectIndex )
{
cout << "Object name : " << MyClient.GetSubjectName(SubjectIndex).SubjectName << endl;
if(MyClient.GetSubjectName(SubjectIndex).SubjectName == "pitta")
{
cout << "Pitta found ID = " << SubjectIndex << endl;
pitta_obj_id = SubjectIndex;
pitta_name = MyClient.GetSubjectName(SubjectIndex).SubjectName;
}
}
}
int VNCServer::Connect(int partId,const std::wstring &name,WebSocket *socket,const std::string &to)
{
Log(">VNCServer connecting participant viewer [id:%d,to:%s]\n",partId,to.c_str());
//Lock
use.WaitUnusedAndLock();
//Create new client
Client *client = new Client(partId,name,this);
//Set client as as user data
socket->SetUserData(client);
//Check if there was one already ws connected for that user
Clients::iterator it = clients.find(partId);
//If it was
if (it!=clients.end())
{
//Get old client
Client *old = it->second;
//End it
old->Close();
//Delete it
delete(old);
//Set new one
it->second = client;
} else {
//Append to client list
clients[partId] = client;
}
//If it was editor
if (partId==editorId)
//Set client editor
client->SetViewOnly(false);
//Check if it is freezed
if (to.rfind("vnc-freeze")!=std::string::npos)
//Freeze
client->FreezeUpdate(true);
//Unlock clients list
use.Unlock();
//Accept incoming connection and add us as listeners
socket->Accept(this);
//Connect to socket
return client->Connect(socket);
}
std::string StorageClient::startClient()
{
Client* client = new Client(_ip, _port);
client->Connect();
std::string message = std::string("startClient");
client->sendMessage(message);
std::string messagea = client->readMessage();
_sessionID = std::stoi(messagea);
client->sendMessage("disconnect");
delete client;
return messagea;
}
void AnOgreAppliCation()
{
int mode = MODE_TESTING;
if(enet_initialize() != 0) {
std::cout << "Unable to initialize enet" << std::endl;
return;
}
atexit(enet_deinitialize);
if(mode==MODE_SERVER) {
Server* server = new Server(64);
delete server;
} else if(mode==MODE_CLIENT) {
Client* client = new Client();
client->Connect("localhost", 1234);
client->Play();
} else if(mode==MODE_TESTING) {
Server* server = new Server(64);
Client* client = new Client();
client->Connect("localhost", 1234);
client->Play();
delete server;
}
return;
}
int main()
{
// Create Client object.
Client ClientObj;
// Create Client Socket.
ClientObj.CreateSocket(TCPSOCKET);
ClientObj.SetSocketOptions();
// Initialise and bind Client address.
ClientObj.InitialiseAddress(6001);
ClientObj.Bind();
char ServerName[24];
int ServerPort;
cout << "Enter Server name or IP: "; // Use localhost or 127.0.0.1 for local server.
cin.getline(ServerName, 24);
cout << "Enter Server port: ";
cin >> ServerPort;
// Connect to Server. Server name/IP and port are provided as arguments.
ClientObj.Connect(ServerName, ServerPort);
// Send and receive.
fstream File;
File.open("received.pdf", ios::out|ios::binary);
int filesize;
ClientObj.Receive((void*)&filesize, sizeof(int));
cout << "File size: " << filesize << "B." << endl;
char c;
for (int i=0; i<filesize; i++)
{
ClientObj.Receive((void*)&c, sizeof(char));
File.write((char*)&c, sizeof(char));
}
File.close();
ClientObj.CloseClientSocket();
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 Patafour::Main()
{
startTimeout.restart();
string nick,pass;
while (window.isOpen())
{
window.clear(sf::Color::Black);
sf::Vector2u windowsize = window.getSize();
int width = windowsize.x;
int height = windowsize.y;
view.setCenter(sf::Vector2f(width/2, height/2));
view.setSize(sf::Vector2f(width, height));
switch(state) {
case -3:
return 0;
break;
case -2:
static LoginDialog loginDialog;
loginDialog.Draw(window);
nick=loginDialog.login;
pass=loginDialog.pass;
break;
case -1:
static LoadingSplash loadingSplash;
loadingSplash.Draw(window);
if(startTimeout.getElapsedTime().asSeconds()>=2)
{
state = 0;
loadingSplash.Stop();
}
break;
case 0:
static Opening opening(window);
opening.Draw(window,this);
break;
case 1:
static MainMenu mainmenu(window);
mainmenu.Draw(window,this);
break;
case 2:
cout << "options" << endl;
break;
case 3:
cout << "Loading save" << endl;
///Loading here
cout << "Loaded" << endl;
state = 4;
break;
case 4:
static Tips tipsp(this);
if(patapolisloaded == false)
if(tipsp.needsrefresh == true) {
cout << "refresh'd" << endl;
tipsp.refresh(this);
waiting.restart();
res.LoadAllWeapons();
}
if(patapolisloaded==false)
tipsp.Draw(window,this,1);
static int i = 0;
if(i==0) {
res.LoadAllWeapons();
waiting.restart();
i+=1;
}
if(waiting.getElapsedTime().asSeconds()>1) {
if(saveloaded==false) {
savecontainer.Synchronize(nick,pass,0);
saveloaded=true; }
static Patapolis patapolis(window,savecontainer);
if(patapolisloaded==true) {
patapolis.Draw(window,savecontainer,this); }}
break;
case 5:
static NewGameIntro newgameintro;
newgameintro.Draw(window,this);
break;
case 6:
cout << "Patapon oath" << endl;
break;
case 7:
static Tips tipsm(this);
static int miau = 0;
if(miau == 0) {
pataponload = false;
miau += 1;
}
if(missionloaded==false)
if(tipsm.needsrefresh == true){
cout << "refresh'd" << endl;
tipsm.refresh(this);
pataponload = false;
}
if(missionloaded==false) {
tipsm.Draw(window,this,2);
hatapon.x = 0;
hatapon.action = "idle";
}
if(tipsm.finished==true) {
static Mission* mission = new Mission();
if(pataponload == false)
{
cout << "creating patapons" << endl;
for(int i=0; i<=5; i++)
{
if(savecontainer.yaripons[i][1]!="")
{
troops[1] = true;
Patapon temppon;
temppon.PataponType = temppon.YARIPON;
temppon.x = (hatapon.x+100)+50*i;
temppon.baseposition = (hatapon.x+100)+50*i;
yaripons.push_back(temppon);
cout << "YARIPONS SIZE: " << yaripons.size() << endl;
}
}
for(int i=0; i<=5; i++)
{
if(savecontainer.tatepons[i][1]!="")
{
troops[2] = true;
Patapon temppon;
temppon.PataponType = temppon.TATEPON;
temppon.x = 300+30*i;
temppon.baseposition = 300+30*i;
tatepons.push_back(temppon);
}
}
for(int i=0; i<=5; i++)
{
if(savecontainer.yumipons[i][1]!="")
{
troops[3] = true;
Patapon temppon;
temppon.PataponType = temppon.YUMIPON;
temppon.x = 0+30*i;
temppon.baseposition = 0+30*i;
yumipons.push_back(temppon);
}
}
cout << "patapons created" << endl;
pataponload = true;
}
static PataponRes res;
if(mission->loaded==false) {
mission->Load(missionfile,window);
cout << "loading patapons" << endl;
for(int i=0; i<=yaripons.size(); i++)
{
if(savecontainer.yaripons[i][1]!="")
yaripons[i].LoadResources(res);
}
for(int i=0; i<=tatepons.size(); i++)
{
if(savecontainer.tatepons[i][1]!="")
tatepons[i].LoadResources(res);
}
for(int i=0; i<=yumipons.size(); i++)
{
if(savecontainer.yumipons[i][1]!="")
yumipons[i].LoadResources(res);
}
cout << "loaded patapons" << endl;
}
if(missionloaded==true) {
mission->Draw(window,commandbuffer,this);
hatapon.WeaponID = atoi(savecontainer.hatapon[6].c_str());
hatapon.HatID = atoi(savecontainer.hatapon[7].c_str());
hatapon.draw(window);
hatapon.baseposition = hatapon.x+0;
hatapon.commandbuffer = mission->commandsystem.Play();
hatapon.commandclock = mission->commandClock.getElapsedTime();
mission->camera.x = hatapon.x;
/**
for(int i=0; i<=4; i++)
{
if(troops[i]==true)
{
if(i==0)
{
cout << "hatapon" << endl;
int temp = window.getSize().x/2;
circle[i].setPosition((4+i*70)-temp+mission->camera.x,10);
hpoutline[i].setPosition((8+i*70)-temp+mission->camera.x,2);
hpbar[i].setPosition((9+i*70)-temp+mission->camera.x,3);
sf::Sprite Mini_Head;
sf::Sprite Mini_Eye;
sf::Sprite Mini_Arm_L;
sf::Sprite Mini_Arm_R;
sf::Sprite Mini_Leg_L;
sf::Sprite Mini_Leg_R;
sf::Sprite Mini_Hat;
sf::Sprite Mini_Weapon;
Mini_Head.setTexture(*hatapon.Head.getTexture(),true);
Mini_Eye.setTexture(*hatapon.Eye.getTexture(),true);
Mini_Arm_L.setTexture(*hatapon.Arm_L.getTexture(),true);
Mini_Arm_R.setTexture(*hatapon.Arm_R.getTexture(),true);
Mini_Leg_L.setTexture(*hatapon.Leg_L.getTexture(),true);
Mini_Leg_R.setTexture(*hatapon.Leg_R.getTexture(),true);
///Mini_Hat.setTexture(temptex[6],true);
Mini_Weapon.setTexture(*hatapon.Weapon.getTexture(),true);
Mini_Head.setScale(0.25,0.25);
Mini_Eye.setScale(0.25,0.25);
Mini_Arm_L.setScale(0.25,0.25);
Mini_Arm_R.setScale(0.25,0.25);
Mini_Leg_L.setScale(0.25,0.25);
Mini_Leg_R.setScale(0.25,0.25);
Mini_Hat.setScale(0.25,0.25);
Mini_Weapon.setScale(0.25,0.25);
Mini_Head.setPosition((4+i*70)-temp+mission->camera.x+17,26);
Mini_Eye.setPosition((4+i*70)-temp+mission->camera.x+26+((hatapon.Eye.getPosition().x-hatapon.x)/2),35+((hatapon.Eye.getPosition().y-hatapon.y)/2));
Mini_Arm_L.setPosition((4+i*70)-temp+mission->camera.x+9+((hatapon.Arm_L.getPosition().x-hatapon.x)/2)+24,41+((hatapon.Arm_L.getPosition().y-hatapon.y)/2));
Mini_Arm_R.setPosition((4+i*70)-temp+mission->camera.x+9+((hatapon.Arm_R.getPosition().x-hatapon.x)/2)+20,41+((hatapon.Arm_R.getPosition().y-hatapon.y)/2));
Mini_Arm_L.setRotation(hatapon.Arm_L.getRotation());
Mini_Arm_R.setRotation(hatapon.Arm_R.getRotation());
Mini_Arm_L.setOrigin(hatapon.Arm_L.getOrigin().x,hatapon.Arm_L.getOrigin().y);
Mini_Arm_R.setOrigin(hatapon.Arm_R.getOrigin().x,hatapon.Arm_R.getOrigin().y);
Mini_Leg_L.setPosition((4+i*70)-temp+mission->camera.x+11+((hatapon.Leg_L.getPosition().x-hatapon.x)/2)+21,41+((hatapon.Leg_L.getPosition().y-hatapon.y)/2));
Mini_Leg_R.setPosition((4+i*70)-temp+mission->camera.x+11+((hatapon.Leg_R.getPosition().x-hatapon.x)/2)+21,41+((hatapon.Leg_R.getPosition().y-hatapon.y)/2));
Mini_Weapon.setPosition((4+i*70)-temp+mission->camera.x+11+((hatapon.Weapon.getPosition().x-hatapon.x)/2)+21,30+((hatapon.Weapon.getPosition().y-hatapon.y)/2));
Mini_Weapon.setRotation(hatapon.Weapon.getRotation());
Mini_Weapon.setOrigin(hatapon.Weapon.getOrigin().x,hatapon.Weapon.getOrigin().y);
window.draw(circle[i]);
window.draw(Mini_Head);
window.draw(Mini_Eye);
window.draw(Mini_Arm_L);
window.draw(Mini_Arm_R);
window.draw(Mini_Leg_L);
window.draw(Mini_Leg_R);
window.draw(Mini_Hat);
window.draw(Mini_Weapon);
window.draw(hpoutline[i]);
window.draw(hpbar[i]);
}
if(i==1)
{
cout << "yaripon" << endl;
int temp = window.getSize().x/2;
circle[i].setPosition((4+i*70)-temp+mission->camera.x,10);
hpoutline[i].setPosition((8+i*70)-temp+mission->camera.x,2);
hpbar[i].setPosition((9+i*70)-temp+mission->camera.x,3);
float ratio;
float currentsum,maxsum;
if(yaripons.size()!=0)
for(int i=0; i<yaripons.size(); i++)
{
if(savecontainer.yaripons[i][1]!="")
{
currentsum += atof(savecontainer.yaripons[i][2].c_str());
maxsum += atof(savecontainer.yaripons[i][3].c_str());
}
}
cout << "test1" << endl;
if(currentsum==0)
currentsum = 1;
if(maxsum==0)
maxsum = 1;
ratio = currentsum / maxsum;
hpbar[i].setScale(ratio,1);
cout << "sprites" << endl;
sf::Sprite Mini_Head;
sf::Sprite Mini_Eye;
sf::Sprite Mini_Arm_L;
sf::Sprite Mini_Arm_R;
sf::Sprite Mini_Leg_L;
sf::Sprite Mini_Leg_R;
sf::Sprite Mini_Hat;
sf::Sprite Mini_Weapon;
cout << "pointers (" << yaripons.size()-1 << ")" << endl;
Mini_Head.setTexture(*yaripons[0].Head.getTexture(),true);
Mini_Eye.setTexture(*yaripons[0].Eye.getTexture(),true);
Mini_Arm_L.setTexture(*yaripons[0].Arm_L.getTexture(),true);
Mini_Arm_R.setTexture(*yaripons[0].Arm_R.getTexture(),true);
Mini_Leg_L.setTexture(*yaripons[0].Leg_L.getTexture(),true);
Mini_Leg_R.setTexture(*yaripons[0].Leg_R.getTexture(),true);
///Mini_Hat.setTexture(temptex[6],true);
///Mini_Weapon.setTexture(hatapon.Weapon.getTexture(),true);
cout << "scale" << endl;
Mini_Head.setScale(0.25,0.25);
Mini_Eye.setScale(0.25,0.25);
Mini_Arm_L.setScale(0.25,0.25);
Mini_Arm_R.setScale(0.25,0.25);
Mini_Leg_L.setScale(0.25,0.25);
Mini_Leg_R.setScale(0.25,0.25);
Mini_Hat.setScale(0.25,0.25);
cout << "position" << endl;
Mini_Head.setPosition((4+i*70)-temp+mission->camera.x+17,26);
Mini_Eye.setPosition((4+i*70)-temp+mission->camera.x+26+((yaripons[yaripons.size()-1].Eye.getPosition().x-yaripons[yaripons.size()-1].x)/2),35+((yaripons[yaripons.size()-1].Eye.getPosition().y-yaripons[yaripons.size()-1].y)/2));
Mini_Arm_L.setPosition((4+i*70)-temp+mission->camera.x+9+((yaripons[yaripons.size()-1].Arm_L.getPosition().x-yaripons[yaripons.size()-1].x)/2)+25,39+((yaripons[yaripons.size()-1].Arm_L.getPosition().y-yaripons[yaripons.size()-1].y)/2));
Mini_Arm_R.setPosition((4+i*70)-temp+mission->camera.x+9+((yaripons[yaripons.size()-1].Arm_R.getPosition().x-yaripons[yaripons.size()-1].x)/2)+21,39+((yaripons[yaripons.size()-1].Arm_R.getPosition().y-yaripons[yaripons.size()-1].y)/2));
Mini_Leg_L.setPosition((4+i*70)-temp+mission->camera.x+11+((yaripons[yaripons.size()-1].Leg_L.getPosition().x-yaripons[yaripons.size()-1].x)/2)+21,41+((yaripons[yaripons.size()-1].Leg_L.getPosition().y-yaripons[yaripons.size()-1].y)/2));
Mini_Leg_R.setPosition((4+i*70)-temp+mission->camera.x+11+((yaripons[yaripons.size()-1].Leg_R.getPosition().x-yaripons[yaripons.size()-1].x)/2)+21,41+((yaripons[yaripons.size()-1].Leg_R.getPosition().y-yaripons[yaripons.size()-1].y)/2));
window.draw(circle[i]);
window.draw(Mini_Head);
window.draw(Mini_Eye);
window.draw(Mini_Arm_L);
window.draw(Mini_Arm_R);
window.draw(Mini_Leg_L);
window.draw(Mini_Leg_R);
window.draw(Mini_Hat);
window.draw(hpoutline[i]);
window.draw(hpbar[i]);
cout << "yaripon is done" << endl;
}
if(i==2)
{
cout << "tatepon" << endl;
int temp = window.getSize().x/2;
circle[i].setPosition((4+i*70)-temp+mission->camera.x,10);
hpoutline[i].setPosition((8+i*70)-temp+mission->camera.x,2);
hpbar[i].setPosition((9+i*70)-temp+mission->camera.x,3);
float ratio;
float currentsum,maxsum;
if(tatepons.size()!=0)
for(int i=0; i<tatepons.size(); i++)
{
if(savecontainer.tatepons[i][1]!="")
{
currentsum += atof(savecontainer.tatepons[i][2].c_str());
maxsum += atof(savecontainer.tatepons[i][3].c_str());
}
}
if(currentsum==0)
currentsum = 1;
if(maxsum==0)
maxsum = 1;
ratio = currentsum / maxsum;
hpbar[i].setScale(ratio,1);
window.draw(circle[i]);
window.draw(hpoutline[i]);
window.draw(hpbar[i]);
}
if(i==3)
{
cout << "yumipon" << endl;
int temp = window.getSize().x/2;
circle[i].setPosition((4+i*70)-temp+mission->camera.x,10);
hpoutline[i].setPosition((8+i*70)-temp+mission->camera.x,2);
hpbar[i].setPosition((9+i*70)-temp+mission->camera.x,3);
float ratio;
float currentsum,maxsum;
if(yumipons.size()!=0)
for(int i=0; i<yumipons.size(); i++)
{
if(savecontainer.yumipons[i][1]!="")
{
currentsum += atof(savecontainer.yumipons[i][2].c_str());
maxsum += atof(savecontainer.yumipons[i][3].c_str());
}
}
if(currentsum==0)
currentsum = 1;
if(maxsum==0)
maxsum = 1;
ratio = currentsum / maxsum;
hpbar[i].setScale(ratio,1);
window.draw(circle[i]);
window.draw(hpoutline[i]);
window.draw(hpbar[i]);
}
}
}*/
if(yaripons.size()!=0)
for(int i=0; i<yaripons.size(); i++)
{
if(savecontainer.yaripons[i][1]!="")
{
yaripons[i].commandbuffer = mission->commandsystem.Play();
yaripons[i].commandclock = mission->commandClock.getElapsedTime();
yaripons[i].baseposition = (hatapon.x+100)+50*i;
yaripons[i].WeaponID = atoi(savecontainer.yaripons[i][6].c_str());
yaripons[i].HatID = atoi(savecontainer.yaripons[i][7].c_str());
yaripons[i].draw(window);
}
}
if(tatepons.size()!=0)
for(int i=0; i<=tatepons.size(); i++)
{
if(savecontainer.tatepons[i][1]!="")
{
tatepons[i].commandbuffer = mission->commandsystem.Play();
yaripons[i].commandclock = mission->commandClock.getElapsedTime();
yaripons[i].WeaponID = atoi(savecontainer.tatepons[i][6].c_str());
yaripons[i].HatID = atoi(savecontainer.tatepons[i][7].c_str());
tatepons[i].draw(window);
}
}
if(yumipons.size()!=0)
for(int i=0; i<=yumipons.size(); i++)
{
if(savecontainer.yumipons[i][1]!="")
{
yumipons[i].commandbuffer = mission->commandsystem.Play();
yaripons[i].commandclock = mission->commandClock.getElapsedTime();
yaripons[i].WeaponID = atoi(savecontainer.yumipons[i][6].c_str());
yaripons[i].HatID = atoi(savecontainer.yumipons[i][7].c_str());
yumipons[i].draw(window);
}
}
cout << mission->camera.x << endl;
for(int i=0; i<yaripons.size(); i++)
{
for(int i2=0; i2<mission->entities.size(); i2++)
{
if(yaripons[i].spear.spear.getPosition().x>mission->entities[i2].GetObjectLocation().x-mission->entities[i2].objectamount*5)
{
if(yaripons[i].spear.spear.getPosition().y>mission->entities[i2].GetObjectLocation().y-mission->entities[i2].objectamount*3)
{
if(yaripons[i].spear.spear.getPosition().x<mission->entities[i2].GetObjectLocation().x+mission->entities[i2].objectamount*5)
{
if(yaripons[i].spear.spear.getPosition().y<mission->entities[i2].GetObjectLocation().y+mission->entities[i2].objectamount*3)
{
yaripons[i].spear.dmg.dmgtext.setPosition(yaripons[i].spear.spear.getPosition());
yaripons[i].spear.dmg.alpha = 255;
yaripons[i].spear.dmg.dmg = round(res.spearmindmg[atof(savecontainer.yaripons[i][6].c_str())]+res.hatdmg[atof(savecontainer.yaripons[i][6].c_str())]+res.spearmindmg[atof(savecontainer.yaripons[i][6].c_str())]/3*res.spearmindmg[atof(savecontainer.yaripons[i][8].c_str())]+(atof(savecontainer.yaripons[i][0].c_str())-1)*2);
yaripons[i].spear.spear.setPosition(0,20000);
cout << "GOT HIT" << endl;
}
}
}
}
}
}
if(hatapon.action == "move")
{
if(mission->entities.size()-1>=0) {
for(int i=0; i<=mission->entities.size()-1; i++) {
if(mission->entities[i].collide == true) {
for(int i2=0; i2<yaripons.size(); i2++) {
if(mission->entities[i].GetObjectLocation().x-200>yaripons[i2].x) {
///mission->entities[i].Move(-1,0);
if(yaripons.size()!=0)
if(savecontainer.yaripons[i2][1]!="")
{
yaripons[i2].x += 2;
}
mission->cancameramove = true;
yaripons[i2].cangotobase = true;
} else {
yaripons[i2].cangotobase = false;
mission->cancameramove = false;
}
}
if(mission->entities[i].GetObjectLocation().x-200>hatapon.x) {
hatapon.x += 2;
mission->cancameramove = true;
hatapon.cangotobase = true;
for(int i=0; i<=mission->backgroundcount; i++)
{
mission->backSize[i].x-=mission->backMoveX[i];
}
} else {
mission->cancameramove = false;
hatapon.cangotobase = false;
}
for(int i2=0; i2<yaripons.size(); i2++)
{
if(mission->entities[i].GetObjectLocation().x-200>yaripons[i2].x) {
yaripons[i2].cangotobase = true;
} else {
yaripons[i2].cangotobase = false;
}}
} else {
for(int i2=0; i2<yaripons.size(); i2++) {
if(yaripons.size()!=0)
if(savecontainer.yaripons[i2][1]!="")
{
yaripons[i2].x += 2;
}
}
hatapon.x += 2;
for(int i=0; i<=mission->backgroundcount; i++)
{
mission->backSize[i].x-=mission->backMoveX[i];
}
mission->cancameramove = true;
if(mission->entities[i].completeafterwalk == true)
if(hatapon.x>mission->entities[i].GetObjectLocation().x) {
m1selected = 0;
m2selected = 0;
menu2open = false;
mission->loaded = false;
dmenureset = true;
yaripons.clear();
tatepons.clear();
yumipons.clear();
state = 34;
}
}
}
}
}
}
}
break;
case 8:
cout << "Mission overview (Like in Patapon 2 yay)" << endl;
break;
case 9:
cout << "World map biatch" << endl;
break;
case 13:
static Organization organization(savecontainer);
if(oplaysnd == true)
if(organization.bgm.getStatus() != sf::Music::Playing)
organization.bgm.play();
organization.selected = oselected;
organization.menu1selected = m1selected;
organization.menu2selected = m2selected;
organization.menu2open = menu2open;
organization.selected2 = pataselected;
organization.Draw(window,this);
break;
case 34:
static DemoMenu demomenu;
cout << "uh, hi?" << endl;
missionloaded = false;
if(dmenureset == true) {
organization.bgm.stop();
m1selected = 0;
m2selected = 0;
menu2open = false;
demomenu.azito.play();
demomenu.clock.restart();
dmenureset = false;
}
demomenu.completed = savecontainer.demopass;
demomenu.sel = dmenuselect;
demomenu.Draw(window,this);
break;
case 69:
static Client client;
static int c = 0;
if(c==0) {
missionloaded = false;
string ip;
cout << "input ip you want to connect" << endl;
cin >> ip;
client.Connect(ip.c_str());
c++;
}
static Tips tipsmulti(this);
if(missionloaded==false)
if(tipsmulti.needsrefresh == true){
cout << "refresh'd" << endl;
tipsmulti.refresh(this);
}
if(missionloaded==false)
tipsmulti.Draw(window,this,2);
if(tipsmulti.finished==true) {
static Mission mission;
if(mission.loaded==false)
mission.Load("multimission_1.map",window);
if(missionloaded==true) {
mission.Draw(window,commandbuffer,this);
client.Multiplayer();
client.Draw(window,mission);
}}
break;
case 666:
static Server server;
static int s = 0;
if(s==0) {
missionloaded = false;
string ip;
cout << "input ip you want to host" << endl;
cin >> ip;
server.Connect(ip.c_str());
s++;
}
static Tips tipsmulti2(this);
if(missionloaded==false)
if(tipsmulti2.needsrefresh == true){
cout << "refresh'd" << endl;
tipsmulti2.refresh(this);
}
if(missionloaded==false)
tipsmulti2.Draw(window,this,2);
if(tipsmulti2.finished==true) {
static Mission mission;
if(mission.loaded==false)
mission.Load("multimission_1.map",window);
if(missionloaded==true) {
mission.Draw(window,commandbuffer,this);
server.Multiplayer();
server.Draw(window,mission);
}}
break;
}
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[] )
{
//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;
}
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;
}
}
