AsyncSpinnerImpl::AsyncSpinnerImpl(uint32_t thread_count, CallbackQueue* queue)
: thread_count_(thread_count)
, callback_queue_(queue)
, continue_(false)
{
if (thread_count == 0)
{
thread_count_ = boost::thread::hardware_concurrency();
if (thread_count_ == 0)
{
thread_count_ = 1;
}
}
if (!queue)
{
callback_queue_ = getGlobalCallbackQueue();
}
}
void SingleThreadedSpinner::spin(CallbackQueue* queue)
{
boost::recursive_mutex::scoped_try_lock spinlock(spinmutex);
if(not spinlock.owns_lock()) {
ROS_ERROR("SingleThreadedSpinner: You've attempted to call spin "
"from multiple threads. Use a MultiThreadedSpinner instead.");
return;
}
ros::WallDuration timeout(0.1f);
if (!queue)
{
queue = getGlobalCallbackQueue();
}
ros::NodeHandle n;
while (n.ok())
{
queue->callAvailable(timeout);
}
}
void start()
{
boost::mutex::scoped_lock lock(g_start_mutex);
if (g_started)
{
return;
}
g_shutdown_requested = false;
g_shutting_down = false;
g_started = true;
g_ok = true;
bool enable_debug = false;
const char* enable_debug_env = getenv("ROSCPP_ENABLE_DEBUG");
if (enable_debug_env)
{
try
{
enable_debug = boost::lexical_cast<bool>(enable_debug_env);
}
catch (boost::bad_lexical_cast&)
{
}
}
param::param("/tcp_keepalive", TransportTCP::s_use_keepalive_, TransportTCP::s_use_keepalive_);
PollManager::instance()->addPollThreadListener(checkForShutdown);
XMLRPCManager::instance()->bind("shutdown", shutdownCallback);
initInternalTimerManager();
TopicManager::instance()->start();
ServiceManager::instance()->start();
ConnectionManager::instance()->start();
PollManager::instance()->start();
XMLRPCManager::instance()->start();
if (!(g_init_options & init_options::NoSigintHandler))
{
signal(SIGINT, basicSigintHandler);
}
if (!(g_init_options & init_options::NoRosout))
{
g_rosout_appender = new ROSOutAppender;
const log4cxx::LoggerPtr& logger = log4cxx::Logger::getLogger(ROSCONSOLE_ROOT_LOGGER_NAME);
logger->addAppender(g_rosout_appender);
}
if (g_shutting_down) goto end;
{
ros::AdvertiseServiceOptions ops;
ops.init<roscpp::GetLoggers>(names::resolve("~get_loggers"), getLoggers);
ops.callback_queue = getInternalCallbackQueue().get();
ServiceManager::instance()->advertiseService(ops);
}
if (g_shutting_down) goto end;
{
ros::AdvertiseServiceOptions ops;
ops.init<roscpp::SetLoggerLevel>(names::resolve("~set_logger_level"), setLoggerLevel);
ops.callback_queue = getInternalCallbackQueue().get();
ServiceManager::instance()->advertiseService(ops);
}
if (g_shutting_down) goto end;
if (enable_debug)
{
ros::AdvertiseServiceOptions ops;
ops.init<roscpp::Empty>(names::resolve("~debug/close_all_connections"), closeAllConnections);
ops.callback_queue = getInternalCallbackQueue().get();
ServiceManager::instance()->advertiseService(ops);
}
if (g_shutting_down) goto end;
{
bool use_sim_time = false;
param::param("/use_sim_time", use_sim_time, use_sim_time);
if (use_sim_time)
{
Time::setNow(ros::Time());
}
if (g_shutting_down) goto end;
if (use_sim_time)
{
ros::SubscribeOptions ops;
ops.init<rosgraph_msgs::Clock>("/clock", 1, clockCallback);
ops.callback_queue = getInternalCallbackQueue().get();
TopicManager::instance()->subscribe(ops);
}
}
if (g_shutting_down) goto end;
g_internal_queue_thread = boost::thread(internalCallbackQueueThreadFunc);
getGlobalCallbackQueue()->enable();
ROSCPP_LOG_DEBUG("Started node [%s], pid [%d], bound on [%s], xmlrpc port [%d], tcpros port [%d], logging to [%s], using [%s] time", this_node::getName().c_str(), getpid(), network::getHost().c_str(), XMLRPCManager::instance()->getServerPort(), ConnectionManager::instance()->getTCPPort(), file_log::getLogFilename().c_str(), Time::useSystemTime() ? "real" : "sim");
// Label used to abort if we've started shutting down in the middle of start(), which can happen in
// threaded code or if Ctrl-C is pressed while we're initializing
end:
// If we received a shutdown request while initializing, wait until we've shutdown to continue
if (g_shutting_down)
{
boost::recursive_mutex::scoped_lock lock(g_shutting_down_mutex);
}
}
