diff --git a/clients/roscpp/src/libros/spinner.cpp b/clients/roscpp/src/libros/spinner.cpp index 9d179feb66..8d417b0c2e 100644 --- a/clients/roscpp/src/libros/spinner.cpp +++ b/clients/roscpp/src/libros/spinner.cpp @@ -34,6 +34,11 @@ namespace { +const std::string DEFAULT_ERROR_MESSAGE = + "\nAttempt to spin a callback queue from two spinners, one of them being single-threaded." + "\nThis will probably result in callbacks being executed out-of-order." + "\nIn future this will throw an exception!"; + /** class to monitor running single-threaded spinners. * * Calling the callbacks of a callback queue _in order_, requires a unique SingleThreadedSpinner @@ -55,9 +60,11 @@ struct SpinnerMonitor */ struct Entry { - Entry(const boost::thread::id &tid) : tid(tid), num_multi_threaded(0) {} + Entry(const boost::thread::id &tid, + const boost::thread::id &initial_tid) : tid(tid), initial_tid(initial_tid), num_multi_threaded(0) {} boost::thread::id tid; // thread id of single-threaded spinner + boost::thread::id initial_tid; // to retain old, but wrong behaviour unsigned int num_multi_threaded; // number of multi-threaded spinners }; @@ -66,17 +73,29 @@ struct SpinnerMonitor { boost::mutex::scoped_lock lock(mutex_); + boost::thread::id current_tid = boost::this_thread::get_id(); boost::thread::id tid; if (single_threaded) - tid = boost::this_thread::get_id(); + tid = current_tid; std::map::iterator it = spinning_queues_.find(queue); bool can_spin = ( it == spinning_queues_.end() || it->second.tid == tid ); + if (!can_spin) - return false; + { + // Previous behavior (up to Kinetic) was to accept multiple spinners on a queue + // as long as they were started from the same thread. Although this is wrong behavior, + // we retain it here for backwards compatibility, i.e. we allow spinning of a + // single-threaded spinner after several multi-threaded ones, given that they + // were started from the same initial thread + if (it->second.initial_tid == tid) + ROS_ERROR_STREAM("SpinnerMonitor: " << DEFAULT_ERROR_MESSAGE); + else + return false; + } if (it == spinning_queues_.end()) - it = spinning_queues_.insert(it, std::make_pair(queue, Entry(tid))); + it = spinning_queues_.insert(it, std::make_pair(queue, Entry(tid, current_tid))); if (!single_threaded) // increment number of multi-threaded spinners it->second.num_multi_threaded += 1; @@ -114,9 +133,6 @@ struct SpinnerMonitor }; SpinnerMonitor spinner_monitor; -const std::string DEFAULT_ERROR_MESSAGE = - "Attempt to spin a callback queue from two spinners, one of them being single-threaded. " - "This will probably result in callbacks being executed out-of-order."; } namespace ros @@ -132,8 +148,8 @@ void SingleThreadedSpinner::spin(CallbackQueue* queue) if (!spinner_monitor.add(queue, true)) { - ROS_FATAL_STREAM("SingleThreadedSpinner: " << DEFAULT_ERROR_MESSAGE); - throw std::runtime_error("There is already another spinner on this queue"); + ROS_ERROR_STREAM("SingleThreadedSpinner: " << DEFAULT_ERROR_MESSAGE); + return; } ros::WallDuration timeout(0.1f); @@ -222,8 +238,8 @@ void AsyncSpinnerImpl::start() if (!spinner_monitor.add(callback_queue_, false)) { - ROS_FATAL_STREAM("AsyncSpinnerImpl: " << DEFAULT_ERROR_MESSAGE); - throw std::runtime_error("There is already a single-threaded spinner on this queue"); + ROS_ERROR_STREAM("AsyncSpinnerImpl: " << DEFAULT_ERROR_MESSAGE); + return; } continue_ = true; diff --git a/test/test_roscpp/test/launch/spinners.xml b/test/test_roscpp/test/launch/spinners.xml index 0e522dcc56..15650a8b3f 100644 --- a/test/test_roscpp/test/launch/spinners.xml +++ b/test/test_roscpp/test/launch/spinners.xml @@ -1,8 +1,4 @@ - - - -