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");
}
}
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 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;
}
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;
}
void getClientConnection (tcp::acceptor &acceptor) {
std::cout << "In GUIMessageSenderThread getClientConnection()" << std::endl;
try
{
std::cout << "waiting for gui client to connect..." << std::endl;
acceptor.accept(*stream_.rdbuf());
std::cout << "...gui client connected" << std::endl;
connected_ = true;
}
catch (std::exception& e)
{
std::cerr << "Error in gui client connection: " << e.what() << std::endl;
}
std::cout << "done GUIMessageSenderThread getClientConnection()" << 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;
}
/**
* sends a request to the local HTTP server
*
* @param http_stream open stream to send the request via
* @param resource name of the HTTP resource to request
* @param content_length bytes available in the response, if successful
*/
inline unsigned int sendRequest(tcp::iostream& http_stream,
const std::string& resource,
unsigned long& content_length)
{
const boost::regex regex_get_response_code("^HTTP/1\\.1\\s(\\d+)\\s.*");
const boost::regex regex_response_header("^[A-Za-z0-9_-]+:\\s.*");
const boost::regex regex_content_length_header("^Content-Length:\\s(\\d+).*", boost::regex::icase);
const boost::regex regex_response_end("^\\s*$");
// send HTTP request to the server
http_stream << "GET " << resource << " HTTP/1.1" << HTTPTypes::STRING_CRLF << HTTPTypes::STRING_CRLF;
http_stream.flush();
// receive response from the server
std::string rsp_line;
boost::smatch rx_matches;
unsigned int response_code = 0;
BOOST_REQUIRE(std::getline(http_stream, rsp_line));
BOOST_REQUIRE(boost::regex_match(rsp_line, rx_matches, regex_get_response_code));
BOOST_REQUIRE(rx_matches.size() == 2);
// extract response status code
response_code = boost::lexical_cast<unsigned int>(rx_matches[1]);
BOOST_REQUIRE(response_code != 0);
// read response headers
content_length = 0;
while (true) {
BOOST_REQUIRE(std::getline(http_stream, rsp_line));
// check for end of response headers (empty line)
if (boost::regex_match(rsp_line, rx_matches, regex_response_end))
break;
// check validity of response header
BOOST_REQUIRE(boost::regex_match(rsp_line, rx_matches, regex_response_header));
// check for content-length response header
if (boost::regex_match(rsp_line, rx_matches, regex_content_length_header)) {
if (rx_matches.size() == 2)
content_length = boost::lexical_cast<unsigned long>(rx_matches[1]);
}
}
return response_code;
}
/**
* checks response content validity for the local HTTP server
*
* @param http_stream open stream to send the request via
* @param resource name of the HTTP resource to request
* @param content_regex regex that the response content should match
*/
inline void checkWebServerResponseContent(tcp::iostream& http_stream,
const std::string& resource,
const boost::regex& content_regex,
unsigned int expectedResponseCode = 200)
{
// send valid request to the server
unsigned int response_code;
unsigned long content_length = 0;
response_code = sendRequest(http_stream, resource, content_length);
BOOST_CHECK(response_code == expectedResponseCode);
BOOST_REQUIRE(content_length > 0);
// read in the response content
boost::scoped_array<char> content_buf(new char[content_length+1]);
BOOST_CHECK(http_stream.read(content_buf.get(), content_length));
content_buf[content_length] = '\0';
// check the response content
BOOST_CHECK(boost::regex_match(content_buf.get(), content_regex));
}