void waitForShutdown()
{
while (ok())
{
WallDuration(0.05).sleep();
}
}
bool WallRate::sleep()
{
WallTime expected_end = start_ + expected_cycle_time_;
WallTime actual_end = WallTime::now();
// detect backward jumps in time
if (actual_end < start_)
{
expected_end = actual_end + expected_cycle_time_;
}
//calculate the time we'll sleep for
WallDuration sleep_time = expected_end - actual_end;
//set the actual amount of time the loop took in case the user wants to know
actual_cycle_time_ = actual_end - start_;
//make sure to reset our start time
start_ = expected_end;
//if we've taken too much time we won't sleep
if(sleep_time <= WallDuration(0.0))
{
// if we've jumped forward in time, or the loop has taken more than a full extra
// cycle, reset our cycle
if (actual_end > expected_end + expected_cycle_time_)
{
start_ = actual_end;
}
return false;
}
return sleep_time.sleep();
}
bool WallTime::sleepUntil(const WallTime& end)
{
WallDuration d(end - WallTime::now());
if (d > WallDuration(0))
{
return d.sleep();
}
return true;
}
void internalCallbackQueueThreadFunc()
{
disableAllSignalsInThisThread();
CallbackQueuePtr queue = getInternalCallbackQueue();
while (!g_shutting_down)
{
queue->callAvailable(WallDuration(0.1));
}
}
void TransportPublisherLink::onConnectionDropped(const ConnectionPtr& conn, Connection::DropReason reason)
{
if (dropping_)
{
return;
}
ROS_ASSERT(conn == connection_);
SubscriptionPtr parent = parent_.lock();
if (reason == Connection::TransportDisconnect)
{
std::string topic = parent ? parent->getName() : "unknown";
ROSCPP_LOG_DEBUG("Connection to publisher [%s] to topic [%s] dropped", connection_->getTransport()->getTransportInfo().c_str(), topic.c_str());
ROS_ASSERT(!needs_retry_);
needs_retry_ = true;
next_retry_ = WallTime::now() + retry_period_;
if (retry_timer_handle_ == -1)
{
retry_period_ = WallDuration(0.1);
next_retry_ = WallTime::now() + retry_period_;
retry_timer_handle_ = getInternalTimerManager()->add(WallDuration(retry_period_),
boost::bind(&TransportPublisherLink::onRetryTimer, this, _1), getInternalCallbackQueue().get(),
VoidConstPtr(), false);
}
else
{
getInternalTimerManager()->setPeriod(retry_timer_handle_, retry_period_);
}
}
else
{
drop();
}
}
void TransportPublisherLink::onRetryTimer(const ros::WallTimerEvent&)
{
if (dropping_)
{
return;
}
if (needs_retry_ && WallTime::now() > next_retry_)
{
retry_period_ = std::min(retry_period_ * 2, WallDuration(20));
needs_retry_ = false;
SubscriptionPtr parent = parent_.lock();
// TODO: support retry on more than just TCP
// For now, since UDP does not have a heartbeat, we do not attempt to retry
// UDP connections since an error there likely means some invalid operation has
// happened.
if (connection_->getTransport()->getType() == std::string("TCPROS"))
{
std::string topic = parent ? parent->getName() : "unknown";
TransportTCPPtr old_transport = boost::dynamic_pointer_cast<TransportTCP>(connection_->getTransport());
ROS_ASSERT(old_transport);
const std::string& host = old_transport->getConnectedHost();
int port = old_transport->getConnectedPort();
ROSCPP_LOG_DEBUG("Retrying connection to [%s:%d] for topic [%s]", host.c_str(), port, topic.c_str());
TransportTCPPtr transport(new TransportTCP(&PollManager::instance()->getPollSet()));
if (transport->connect(host, port))
{
ConnectionPtr connection(new Connection);
connection->initialize(transport, false, HeaderReceivedFunc());
initialize(connection);
ConnectionManager::instance()->addConnection(connection);
}
else
{
ROSCPP_LOG_DEBUG("connect() failed when retrying connection to [%s:%d] for topic [%s]", host.c_str(), port, topic.c_str());
}
}
else if (parent)
{
parent->removePublisherLink(shared_from_this());
}
}
}
void XMLRPCManager::serverThreadFunc()
{
disableAllSignalsInThisThread();
while(!shutting_down_)
{
{
boost::mutex::scoped_lock lock(added_connections_mutex_);
S_ASyncXMLRPCConnection::iterator it = added_connections_.begin();
S_ASyncXMLRPCConnection::iterator end = added_connections_.end();
for (; it != end; ++it)
{
(*it)->addToDispatch(server_.get_dispatch());
connections_.insert(*it);
}
added_connections_.clear();
}
// Update the XMLRPC server, blocking for at most 10ms in select()
{
boost::mutex::scoped_lock lock(functions_mutex_);
server_.work(0.01);
}
while (unbind_requested_)
{
WallDuration(0.01).sleep();
}
if (shutting_down_)
{
return;
}
{
S_ASyncXMLRPCConnection::iterator it = connections_.begin();
S_ASyncXMLRPCConnection::iterator end = connections_.end();
for (; it != end; ++it)
{
if ((*it)->check())
{
removeASyncConnection(*it);
}
}
}
{
boost::mutex::scoped_lock lock(removed_connections_mutex_);
S_ASyncXMLRPCConnection::iterator it = removed_connections_.begin();
S_ASyncXMLRPCConnection::iterator end = removed_connections_.end();
for (; it != end; ++it)
{
(*it)->removeFromDispatch(server_.get_dispatch());
connections_.erase(*it);
}
removed_connections_.clear();
}
}
}