int RobotPilotServer(deque<float> &pars, CSimpleInConnection &connection, tcp::iostream &client, ofstream &logFile)
{
cout << "Connected to RobotPilot Client." << endl;
logFile << "Connected to RobotPilot Client." << endl;
PathPlannerParamsTypedef serverPars;
PathMessage path;
ParsePathPlannerPars(pars, serverPars);
//Forward parameters to Client
ForwardPathPlannerPars(client, serverPars);
//Receive (sampled) path from PathPlanner
path.receive(client);
vector<float> path_vrep;
for (auto &s : path.path)
{
for (auto &p : s.path)
{
path_vrep.push_back(p.p.x);
path_vrep.push_back(p.p.y);
}
}
//Send (sampled) path to V-Rep Client
if (!connection.replyToReceivedData((char*)path_vrep._Myfirst, path_vrep.size()*sizeof(float)))
return -1;
RobotLoop(connection, client, logFile);
client.close();
return 0;
}
void _retry_write_message(tcp::iostream& stream, const T& msg) {
while (true) {
if (_write_message(stream, msg)) {
return;
}
DEBUG("write failed, sleep and retry\n");
stream.close();
stream.clear();
sleep(1);
stream.connect("127.0.0.1", "1481");
}
}
void sendMessages(unsigned sleepMillis) {
std::cout << "In SimpleACoreDataServer sendMessages()" << std::endl;
//WaveformSimulator wave_sim_;
try
{
//wave_sim_.start(this);
WaveformsMessage waveMessage;
running_ = true;
while (running_ && connected_)
{
if (stream_.good()) {
//sending a waveform message
waveMessage = waveQ_.popWait();
std::cout << "GUIMessageSenderThread sending a waveform message" << std::endl;
stream_ << waveMessage.toXML() << std::endl;
while (!numericsQ_.isEmpty()) {
std::cout << "GUIMessageSenderThread sending a numerics message!" << std::endl;
stream_ << numericsQ_.popWait().toXML() << std::endl;
}
}
else {
connected_ = false;
}
//now wait
//timer.expires_from_now(boost::posix_time::milliseconds(100));
//timer.wait();
//send a numerics message
//std::cout << "sending a numerics message!" << std::endl;
//stream << numerics_sim_.getNextNumericsMessage().toXML() << std::endl;
//now wait
//timer.expires_from_now(boost::posix_time::milliseconds(100));
//timer.wait();
}
}
catch (std::exception& e)
{
std::cerr << "Error sending messages: " << e.what() << std::endl;
}
//wave_sim_.stop();
stream_.clear();
stream_.close();
std::cout << "done SimpleACoreDataServer sendMessages()" << std::endl;
}
int CarRobotPilotServer(deque<float> &pars, CSimpleInConnection &connection, tcp::iostream &client, ofstream &logFile)
{
cout << "Connected to PathPlanner Client." << endl;
logFile << "Connected to PathPlanner Client." << endl;
CarPathPlannerParamsTypedef serverPars;
PathMessage path;
CarParsePathPlannerPars(pars, serverPars);
//Forward parameters to Client
CarForwardPathPlannerPars(client, serverPars);
if(serverPars.app.isPathFollow())
{
//Receive, forward path from V-Rep Client
ForwardPath(connection, client);
}
if(serverPars.app.isPathPlanner())
{
//Receive (sampled) path from PathPlanner
path.receive(client);
vector<float> path_vrep = ConvertVrepPath(path);
//Send (sampled) path to V-Rep Client
if(!connection.replyToReceivedData((char*) path_vrep._Myfirst, path_vrep.size()*sizeof(float)))
return -1;
}
//Receive (sampled) path from PathPlanner
path.path.clear();
path.receive(client);
vector<float> path_vrep2 = ConvertVrepPath(path);
//Send (sampled) path to V-Rep Client
if(!connection.replyToReceivedData((char*) path_vrep2._Myfirst, path_vrep2.size()*sizeof(float)))
return -1;
CarRobotPilotLoop(serverPars.PathFollow, connection, client, logFile);
client.close();
return 0;
}
void run(unsigned sleepMillis)
{
std::cout << "in GUIMessageSenderThread: run "
<< sleepMillis << "ms"
<< std::endl;
while (running_) {
if (!connected_) {
std::cout << "In GUIMessageSenderThread trying to connect to: " << host_ << " " << port_ << std::endl;
//tcp::endpoint endpoint_(boost::asio::ip::address::from_string(host_), port_);
//tcp::endpoint endpoint_(boost::asio::ip::address::from_string(host_), port_);
endpoint_.address(boost::asio::ip::address::from_string(host_));
endpoint_.port(port_);
tcp::acceptor acceptor(*io_service_, endpoint_);
getClientConnection(acceptor);
try {
sendMessages(sleepMillis);
acceptor.close();
stream_.clear();
stream_.close();
}
catch (std::exception& e)
{
std::cerr << "GUIMessageSenderThread Error in connection: " << e.what() << std::endl;
}
connected_ = false;
//boost::asio::deadline_timer timer(*io_service_,boost::posix_time::milliseconds(1000));
//timer.expires_from_now(boost::posix_time::milliseconds(1000));
//timer.wait();
}
}
std::cout << "done GUIMessageSenderThread: run "
<< sleepMillis << "ms"
<< std::endl;
}
