void ClusterServer::acceptClient(void)
{
BinaryMessage msg;
DgramSocket serviceSock;
SocketAddress addr;
std::string service;
std::string connectionType;
UInt32 readable;
bool connected=false;
std::string address;
bool bound = false;
SINFO << "Waiting for request of "
<< _serviceName
<< std::endl;
try
{
if(!_requestAddress.empty())
{
// create connection
_connection = ConnectionFactory::the()->createPoint(
_connectionType);
if(_connection)
{
// set interface
_connection->setInterface(_interface);
// bind connection
try
{
// bind to requested address
_boundAddress = _connection->bind(_requestAddress);
bound = true;
}
catch(...)
{
SINFO << "Unable to bind, use name as symbolic "
<< "service name"
<< std::endl;
}
}
}
serviceSock.open();
serviceSock.setReusePort(true);
// join to multicast group
if(!_serviceGroup.empty())
{
SocketAddress groupAddress =
SocketAddress(_serviceGroup.c_str(),
_servicePort);
if(groupAddress.isMulticast())
{
SINFO << "wait for request on multicast:"
<< _serviceGroup << std::endl;
serviceSock.bind(SocketAddress(SocketAddress::ANY,
_servicePort));
serviceSock.join(SocketAddress(groupAddress));
}
else
{
SINFO << "wait for request by broadcast:"
<< _serviceGroup << std::endl;
serviceSock.bind(SocketAddress(groupAddress));
}
}
else
{
SINFO << "wait for request by broadcast" << std::endl;
serviceSock.bind(SocketAddress(SocketAddress::ANY,
_servicePort));
}
while(!connected)
{
try
{
if(_connection)
readable = serviceSock.waitReadable(.01);
else
readable = true;
if(readable)
{
serviceSock.recvFrom(msg,addr);
service = msg.getString();
connectionType = msg.getString();
SINFO << "Request for "
<< service << " "
<< connectionType
<< std::endl;
if(service == _serviceName)
{
// remove old connection if typename missmaches
if(_connection &&
_connection->getType()->getName() != connectionType)
{
delete _connection;
_connection = NULL;
}
// try to create connection
if(!_connection)
{
// create connection
_connection =
ConnectionFactory::the()->createPoint(
connectionType);
if(_connection)
{
// set interface
_connection->setInterface(_interface);
// bind connection
_boundAddress = _connection->bind(
_requestAddress);
bound = true;
}
else
{
SINFO << "Unknown connection type '"
<< connectionType << "'" << std::endl;
}
}
if(_connection)
{
msg.clear ( );
msg.putString(_serviceName );
msg.putString(_boundAddress);
serviceSock.sendTo(msg, addr);
SINFO << "Response "
<< connectionType << ":"
<< _boundAddress
<< std::endl;
}
}
}
}
catch(SocketConnReset &e)
{
// ignore if there is a connection. This can happen, if
// a client has send a request. The server has send an
// answer meanwile the client has send a second request
// the client gets the answer to the first request and
// the server tries to send a second answer. The second
// answer can not be delivered because the client has
// closed its service port. This is a win-socket problem.
SWARNING << e.what() << std::endl;
// if there is no connection, then its a real problem
if(!_connection)
throw;
}
catch(OSG_STDEXCEPTION_NAMESPACE::exception &e)
{
SWARNING << e.what() << std::endl;
}
try
{
// try to accept
if(bound && _connection && _connection->acceptGroup(0.2) >= 0)
{
connected = true;
SINFO << "Connection accepted "
<< _boundAddress
<< std::endl;
}
}
catch(OSG_STDEXCEPTION_NAMESPACE::exception &e)
{
SWARNING << e.what() << std::endl;
}
}
serviceSock.close();
}
catch(OSG_STDEXCEPTION_NAMESPACE::exception &)
{
throw;
}
}
void TCPServer::listen(port_t port)
{
listen(SocketAddress(InetAddress(), port));
}
bool TCPSocket::connect(const InetAddress& addr, port_t port)
{
return connect(SocketAddress(addr, port));
}
int main(int argc, char* argv[])
{
//init Pathplanner
cout << "start planner ... " << endl;
std::tr1::function<float(float, float, float)> callback;
callback = &computeVelo;
SILIA_Pathplanner planner(callback);
vector<float> vec1; //<! start configuration
vector<float> vec2; //<! goal configuration
vector< pair<vector<float>, bool> > path; //<! target path, consists of start and goal configuration
/*
vec1.push_back(90);
vec1.push_back(30);
vec1.push_back(30);
vec1.push_back(0);
vec1.push_back(0);
vec1.push_back(0);
vec2.push_back(0);
vec2.push_back(90);
vec2.push_back(-90);
vec2.push_back(90);
vec2.push_back(90);
vec2.push_back(90);
path.push_back(pair<vector<float>, bool> (vec1, true));
path.push_back(pair<vector<float>, bool> (vec2, true));
*/
vector<float> a2,b2;
a2.push_back(-26.7);
a2.push_back(-72.4);
a2.push_back(78.7);
a2.push_back(-0.5);
a2.push_back(82.3);
a2.push_back(17.9);
/*b2.push_back(35.1);
b2.push_back(-75.1);
b2.push_back(84.3);
b2.push_back(0.1);
b2.push_back(79.3);
b2.push_back(-3.6);
*/
b2.push_back(-39.1);
b2.push_back(-45.1);
b2.push_back(34.3);
b2.push_back(-0.1);
b2.push_back(97.3);
b2.push_back(-3.8);
path.push_back(pair<vector<float>, bool> (a2, true));
path.push_back(pair<vector<float>, bool> (b2, true));
vector<pair<vector<float>,float>> solution = planner.computePath(path); //<! solution path consists of start, goal and intermediate configurations
cout << "Path: " << endl;
for(unsigned int i=0; i<solution.size(); i++)
{
for(unsigned int k=0; k<solution[i].first.size(); k++)
{
cout << solution[i].first[k] << " ";
}
cout << "|| Velocity: " << solution[i].second;
cout << endl;
}
LogManager * _logManager;
_logManager = new LogManager(NULL);
KukaEthernetClient client;
cout << "start client..." << endl;
getchar();
client.Initialize("EKIServerFrame.xml", SocketAddress("192.1.10.20", 49152));
//client.Initialize("TestKRL.xml", SocketAddress("localhost", 9978));
client.setCallbackFcn(testCallback);
cout << "start sending messages..." << endl;
getchar();
//client.movePTP(currentMsgID, KukaAxis(-33, -113, 100, 0, 100, 11), 10);// oben
for(unsigned int i=0; i<solution.size(); i++)
{
currentMsgID = client.getMessageID();
client.movePTP(currentMsgID, KukaAxis(solution[i].first), solution[i].second * 100);// oben
}
//float axSpeed_base = 10;
//for (float i = 1; i <= 10; i++)
//{
// currentMsgID = client.getMessageID();
// client.movePTP(currentMsgID, KukaAxis(-33, -113, 100, 0, 100, 11), axSpeed);// oben
// //client.movePTP(client.getMessageID(), KukaAxis(-53, -114, 110, 0, 92,-9), axSpeed);// mitte rechts
// //client.movePTP(client.getMessageID(), KukaAxis(-33, -113, 118, 0, 83, 11), axSpeed);//unten
// //client.movePTP(client.getMessageID(), KukaAxis(-20, -109, 107, 0, 90, 24), axSpeed);// mitte links
//}
//getchar();
////cout << "Press to close..." << endl;
getchar();
return 0;
}
operator SocketAddress() const {
return SocketAddress(reinterpret_cast<const struct sockaddr *>(&address),
size);
}
void RTMFPServer::run() {
try {
_startDatetimeStr = DateTimeFormatter::format(LocalDateTime(), "%b %d %Y %H:%M:%S");
_pSocket->bind(SocketAddress("0.0.0.0",_port));
NOTE("RTMFP server sendbufsize %d recvbufsize %d recvtmo %d sendtmo %d",
_pSocket->getSendBufferSize(),
_pSocket->getReceiveBufferSize(),
_pSocket->getReceiveTimeout().milliseconds(),
_pSocket->getSendTimeout().milliseconds()
);
sockets.add(*_pSocket,*this); //_mainSockets
NOTE("RTMFP server starts on %u port",_port);
if(_shellPort > 0) {
_shellSocket.bind(SocketAddress("0.0.0.0", _shellPort));
sockets.add(_shellSocket, *this);
NOTE("RTMFP server shell command portal on %u prot", _shellPort);
}
onStart();
RTMFPManager manager(*this);
bool terminate=false;
while(!terminate)
handle(terminate);
} catch(Exception& ex) {
FATAL("RTMFPServer, %s",ex.displayText().c_str());
} catch (exception& ex) {
FATAL("RTMFPServer, %s",ex.what());
} catch (...) {
FATAL("RTMFPServer, unknown error");
}
sockets.remove(*_pSocket); // _mainSockets
// terminate handle
terminate();
// clean sessions, and send died message if need
_handshake.clear();
_sessions.clear();
// stop receiving and sending engine (it waits the end of sending last session messages)
poolThreads.clear();
// close UDP socket
_pSocket->close();
// close shell command port
if(_shellPort > 0) {
_shellSocket.close();
}
sockets.clear();
//_mainSockets.clear();
_port=0;
onStop();
if(_pCirrus) {
delete _pCirrus;
_pCirrus = NULL;
}
if(_pSocket) {
delete _pSocket;
_pSocket = NULL;
}
NOTE("RTMFP server stops");
}
