Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Revert "Silence ZMQ related warnings" #101

Merged
merged 1 commit into from
Mar 7, 2022
Merged

Revert "Silence ZMQ related warnings" #101

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion include/mujincontrollerclient/mujinjson.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -164,7 +164,7 @@ inline void ParseJson(rapidjson::Document& d, const std::string& str) {
inline void ParseJson(rapidjson::Document& d, std::istream& is) {
rapidjson::IStreamWrapper isw(is);
// see note in: void ParseJson(rapidjson::Document& d, const std::string& str)
rapidjson::Document tempDoc;
rapidjson::Document(tempDoc);
tempDoc.ParseStream<rapidjson::kParseFullPrecisionFlag>(isw); // parse float in full precision mode
if (tempDoc.HasParseError()) {
throw MujinJSONException(boost::str(boost::format("Json stream is invalid (offset %u) %s")%((unsigned)d.GetErrorOffset())%GetParseError_En(d.GetParseError())), MJE_Failed);
Expand Down
47 changes: 27 additions & 20 deletions src/binpickingtask.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1785,12 +1785,14 @@ void BinPickingTaskResource::_HeartbeatMonitorThread(const double reinitializeti
socket.reset();
}
socket.reset(new zmq::socket_t((*_zmqcontext.get()),ZMQ_SUB));
socket->set(zmq::sockopt::tcp_keepalive, 1); // turn on tcp keepalive, do these configuration before connect
socket->set(zmq::sockopt::tcp_keepalive_idle, 2); // the interval between the last data packet sent (simple ACKs are not considered data) and the first keepalive probe; after the connection is marked to need keepalive, this counter is not used any further
socket->set(zmq::sockopt::tcp_keepalive_intvl, 2); // the interval between subsequential keepalive probes, regardless of what the connection has exchanged in the meantime
socket->set(zmq::sockopt::tcp_keepalive_cnt, 2); // the number of unacknowledged probes to send before considering the connection dead and notifying the application layer
socket->connect("tcp://" + _mujinControllerIp + ":" + std::to_string(_heartbeatPort));
socket->set(zmq::sockopt::subscribe, "");
socket->setsockopt(ZMQ_TCP_KEEPALIVE, 1); // turn on tcp keepalive, do these configuration before connect
socket->setsockopt(ZMQ_TCP_KEEPALIVE_IDLE, 2); // the interval between the last data packet sent (simple ACKs are not considered data) and the first keepalive probe; after the connection is marked to need keepalive, this counter is not used any further
socket->setsockopt(ZMQ_TCP_KEEPALIVE_INTVL, 2); // the interval between subsequential keepalive probes, regardless of what the connection has exchanged in the meantime
socket->setsockopt(ZMQ_TCP_KEEPALIVE_CNT, 2); // the number of unacknowledged probes to send before considering the connection dead and notifying the application layer
std::stringstream ss; ss << std::setprecision(std::numeric_limits<double>::digits10+1);
ss << _heartbeatPort;
socket->connect(("tcp://"+ _mujinControllerIp+":"+ss.str()).c_str());
socket->setsockopt(ZMQ_SUBSCRIBE, "", 0);

zmq::pollitem_t pollitem;
memset(&pollitem, 0, sizeof(zmq::pollitem_t));
Expand All @@ -1799,12 +1801,13 @@ void BinPickingTaskResource::_HeartbeatMonitorThread(const double reinitializeti

unsigned long long lastheartbeat = GetMilliTime();
while (!_bShutdownHeartbeatMonitor && (GetMilliTime() - lastheartbeat) / 1000.0f < reinitializetimeout) {
zmq::poll(&pollitem, 1, std::chrono::milliseconds{50}); // wait 50 ms for message
zmq::poll(&pollitem,1, 50); // wait 50 ms for message
if (pollitem.revents & ZMQ_POLLIN) {
zmq::message_t reply;
socket->recv(reply);
socket->recv(&reply);
std::string replystring((char *)reply.data (), (size_t)reply.size());
//if ((size_t)reply.size() == 1 && ((char *)reply.data())[0]==255) {
if (reply.to_string() == "255") {
if (replystring == "255") {
lastheartbeat = GetMilliTime();
}
}
Expand All @@ -1824,36 +1827,37 @@ void BinPickingTaskResource::_HeartbeatMonitorThread(const double reinitializeti
std::string utils::GetHeartbeat(const std::string& endpoint) {
zmq::context_t zmqcontext(1);
zmq::socket_t socket(zmqcontext, ZMQ_SUB);
socket.connect(endpoint);
socket.set(zmq::sockopt::subscribe, "");
socket.connect(endpoint.c_str());
socket.setsockopt(ZMQ_SUBSCRIBE, "", 0);

zmq::pollitem_t pollitem;
memset(&pollitem, 0, sizeof(zmq::pollitem_t));
pollitem.socket = socket;
pollitem.events = ZMQ_POLLIN;

zmq::poll(&pollitem, 1, std::chrono::milliseconds{50}); // wait 50 ms for message
zmq::poll(&pollitem,1, 50); // wait 50 ms for message
if (!(pollitem.revents & ZMQ_POLLIN)) {
return "";
}

zmq::message_t reply;
socket.recv(reply);
socket.recv(&reply);
const std::string received((char *)reply.data (), (size_t)reply.size());
#ifndef _WIN32
return reply.to_string();
return received;
#else
// sometimes buffer can container \n or \\, which windows does not like
std::string newbuffer;
std::vector< std::pair<std::string, std::string> > serachpairs(2);
serachpairs[0].first = "\n"; serachpairs[0].second = "";
serachpairs[1].first = "\\"; serachpairs[1].second = "";
SearchAndReplace(newbuffer, reply.to_string(), serachpairs);
SearchAndReplace(newbuffer, received, serachpairs);
return newbuffer;
#endif
}

namespace {

namespace {
std::string FindSmallestSlaveRequestId(const rapidjson::Value& pt) {
// get all slave request ids
std::vector<std::string> slavereqids;
Expand Down Expand Up @@ -1894,11 +1898,13 @@ std::string FindSmallestSlaveRequestId(const rapidjson::Value& pt) {
return slavereqids[smallest_suffix_index];
}

std::string GetValueForSmallestSlaveRequestId(const std::string& heartbeat, const std::string& key)
std::string GetValueForSmallestSlaveRequestId(const std::string& heartbeat,
const std::string& key)
{

rapidjson::Document pt(rapidjson::kObjectType);
ParseJson(pt, heartbeat);
std::stringstream ss(heartbeat);
ParseJson(pt, ss.str());
try {
const std::string slavereqid = FindSmallestSlaveRequestId(pt);
std::string result;
Expand All @@ -1910,8 +1916,8 @@ std::string GetValueForSmallestSlaveRequestId(const std::string& heartbeat, cons
}

}
}

} // anonymous namespace

std::string mujinclient::utils::GetScenePkFromHeartbeat(const std::string& heartbeat) {
static const std::string prefix("mujin:/");
Expand All @@ -1920,7 +1926,8 @@ std::string mujinclient::utils::GetScenePkFromHeartbeat(const std::string& heart

std::string utils::GetSlaveRequestIdFromHeartbeat(const std::string& heartbeat) {
rapidjson::Document pt;
ParseJson(pt, heartbeat);
std::stringstream ss(heartbeat);
ParseJson(pt, ss.str());
try {
static const std::string prefix("slaverequestid-");
return FindSmallestSlaveRequestId(pt).substr(prefix.length());
Expand Down
26 changes: 16 additions & 10 deletions src/binpickingtaskzmq.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -267,26 +267,30 @@ void BinPickingTaskZmqResource::_HeartbeatMonitorThread(const double reinitializ
socket.reset();
}
socket.reset(new zmq::socket_t((*_zmqcontext.get()),ZMQ_SUB));
socket->set(zmq::sockopt::tcp_keepalive, 1); // turn on tcp keepalive, do these configuration before connect
socket->set(zmq::sockopt::tcp_keepalive_idle, 2); // the interval between the last data packet sent (simple ACKs are not considered data) and the first keepalive probe; after the connection is marked to need keepalive, this counter is not used any further
socket->set(zmq::sockopt::tcp_keepalive_intvl, 2); // the interval between subsequential keepalive probes, regardless of what the connection has exchanged in the meantime
socket->set(zmq::sockopt::tcp_keepalive_cnt, 2); // the number of unacknowledged probes to send before considering the connection dead and notifying the application layer
socket->connect("tcp://" + _mujinControllerIp + ":" + std::to_string(_heartbeatPort));
socket->set(zmq::sockopt::subscribe, "");
socket->setsockopt(ZMQ_TCP_KEEPALIVE, 1); // turn on tcp keepalive, do these configuration before connect
socket->setsockopt(ZMQ_TCP_KEEPALIVE_IDLE, 2); // the interval between the last data packet sent (simple ACKs are not considered data) and the first keepalive probe; after the connection is marked to need keepalive, this counter is not used any further
socket->setsockopt(ZMQ_TCP_KEEPALIVE_INTVL, 2); // the interval between subsequential keepalive probes, regardless of what the connection has exchanged in the meantime
socket->setsockopt(ZMQ_TCP_KEEPALIVE_CNT, 2); // the number of unacknowledged probes to send before considering the connection dead and notifying the application layer
std::stringstream ss; ss << std::setprecision(std::numeric_limits<double>::digits10+1);
ss << _heartbeatPort;
socket->connect (("tcp://"+ _mujinControllerIp+":"+ss.str()).c_str());
socket->setsockopt(ZMQ_SUBSCRIBE, "", 0);

zmq::pollitem_t pollitem;
memset(&pollitem, 0, sizeof(zmq::pollitem_t));
pollitem.socket = socket->operator void*();
pollitem.events = ZMQ_POLLIN;
unsigned long long lastheartbeat = GetMilliTime();
while (!_bShutdownHeartbeatMonitor && (GetMilliTime() - lastheartbeat) / 1000.0f < reinitializetimeout) {
zmq::poll(&pollitem, 1, std::chrono::milliseconds{50}); // wait 50 ms for message
zmq::poll(&pollitem,1, 50); // wait 50 ms for message
if (pollitem.revents & ZMQ_POLLIN) {
zmq::message_t reply;
socket->recv(reply);
socket->recv(&reply);
std::string replystring((char *)reply.data (), (size_t)reply.size());
rapidjson::Document pt(rapidjson::kObjectType);
try{
ParseJson(pt, reply.to_string());
std::stringstream replystring_ss(replystring);
ParseJson(pt, replystring_ss.str());
heartbeat.Parse(pt);
{
boost::mutex::scoped_lock lock(_mutexTaskState);
Expand All @@ -300,7 +304,7 @@ void BinPickingTaskZmqResource::_HeartbeatMonitorThread(const double reinitializ
}
catch (std::exception const &e) {
MUJIN_LOG_ERROR("HeartBeat reply is not JSON");
MUJIN_LOG_ERROR(reply.to_string());
MUJIN_LOG_ERROR(replystring);
MUJIN_LOG_ERROR(e.what());
continue;
}
Expand All @@ -315,4 +319,6 @@ void BinPickingTaskZmqResource::_HeartbeatMonitorThread(const double reinitializ
MUJIN_LOG_DEBUG(str(boost::format("Stopped controller %s monitoring thread on port %d for slaverequestid=%s.")%_mujinControllerIp%_heartbeatPort%_slaverequestid));
}



} // end namespace mujinclient
Loading