一个简单的订阅程序如下：
File: /home/lgy/ros_catkin_ws/src/ros_tutorials/roscpp_tutorials/listener/listener.cpp
29: #include "ros/ros.h"
30: #include "std_msgs/String.h"
36: void chatterCallback(const std_msgs::String::ConstPtr& msg)
37: {
38: ROS_INFO("I heard: [%s]", msg->data.c_str());
39: }
42: int main(int argc, char **argv)
43: {
54: ros::init(argc, argv, "listener");
61: ros::NodeHandle n;
79: ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
88: ros::spin();
91: return 0;
92: }
我们分析
ros::Subscriber sub = n.subscribe(“chatter”, 1000, chatterCallback);订阅消息时做了什么？
订阅的消息时如何收到并如何处理的？
ros::spin();做了什么？
一、先分析subscribe
ros::Subscriber sub = n.subscribe(“chatter”, 1000, chatterCallback);
函数调用如下：
File: /home/lgy/ros_catkin_ws/src/ros_comm/roscpp/include/ros/node_handle.h
401: template<class M, class T>
402: Subscriber subscribe(const std::string& topic, uint32_t queue_size, void(T::*fp)(M), T* obj,
403: const TransportHints& transport_hints = TransportHints())
404: {
405: SubscribeOptions ops;
406: ops.template initByFullCallbackType<M>(topic, queue_size, boost::bind(fp, obj, _1));
407: ops.transport_hints = transport_hints;
408: return subscribe(ops);
409: }
File: /home/lgy/ros_catkin_ws/src/ros_comm/roscpp/include/ros/subscribe_options.h
81: template<class P>
82: void initByFullCallbackType(const std::string& _topic, uint32_t _queue_size,
83: const boost::function<void (P)>& _callback,
84: const boost::function<boost::shared_ptr<typename ParameterAdapter<P>::Message>(void)>& factory_fn = DefaultMessageCreator<typename ParameterAdapter<P>::Message>())
85: {
86: typedef typename ParameterAdapter<P>::Message MessageType;
87: topic = _topic;
88: queue_size = _queue_size;
89: md5sum = message_traits::md5sum<MessageType>();
90: datatype = message_traits::datatype<MessageType>();
91: helper = boost::make_shared<SubscriptionCallbackHelperT<P> >(_callback, factory_fn);
92: }
File: /home/lgy/ros_catkin_ws/src/ros_comm/roscpp/include/ros/subscription_callback_helper.h
102: SubscriptionCallbackHelperT(const Callback& callback,
103: const CreateFunction& create = DefaultMessageCreator<NonConstType>())
104: : callback_(callback)
105: , create_(create)
106: { }
函数调用关系总结如下：
Subscriber subscribe(const std::string& topic, uint32_t queue_size, void(T::*fp)(M), T* obj,...)
subscribe_options::initByFullCallbackType
helper = boost::make_shared<SubscriptionCallbackHelperT<P> >(_callback, factory_fn);
SubscriptionCallbackHelperT(const Callback& callback, const CreateFunction& create = DefaultMessageCreator<NonConstType>())//这里请留意callback通过构造函数最终赋值给callback_中。
subscribe(ops);
这里请留意callback通过构造函数最终赋值给callback_中。
接着向下分析
File: /home/lgy/ros_catkin_ws/src/ros_comm/roscpp/src/libros/node_handle.cpp
322: Subscriber NodeHandle::subscribe(SubscribeOptions& ops)
323: {
324: ops.topic = resolveName(ops.topic);
325: if (ops.callback_queue == 0)
326: {
327: if (callback_queue_)
328: {
329: ops.callback_queue = callback_queue_;
330: }
331: else
332: {
333: ops.callback_queue = getGlobalCallbackQueue();
334: }
335: }
336:
337: if (TopicManager::instance()->subscribe(ops))
338: {
339: Subscriber sub(ops.topic, *this, ops.helper);
340:
341: {
342: boost::mutex::scoped_lock lock(collection_->mutex_);
343: collection_->subs_.push_back(sub.impl_);
344: }
345:
346: return sub;
347: }
348:
349: return Subscriber();
350: }
File: /home/lgy/ros_catkin_ws/src/ros_comm/roscpp/src/libros/topic_manager.cpp
245: // this function has the subscription code that doesn't need to be templated.
246:
247: {
248: boost::mutex::scoped_lock lock(subs_mutex_);
249:
250: if (addSubCallback(ops))
251: {
252: return true;
253: }
254:
255: if (isShuttingDown())
256: {
257: return false;
258: }
259:
260: if (ops.md5sum.empty())
261: {
262: throw InvalidParameterException("Subscribing to topic [" + ops.topic + "] with an empty md5sum");
263: }
264:
265: if (ops.datatype.empty())
266: {
267: throw InvalidParameterException("Subscribing to topic [" + ops.topic + "] with an empty datatype");
268: }
269:
270: if (!ops.helper)
271: {
272: throw InvalidParameterException("Subscribing to topic [" + ops.topic + "] without a callback");
273: }
274:
275: const std::string& md5sum = ops.md5sum;
276: std::string datatype = ops.datatype;
277:
278: SubscriptionPtr s(boost::make_shared<Subscription>(ops.topic, md5sum, datatype, ops.transport_hints));
279: s->addCallback(ops.helper, ops.md5sum, ops.callback_queue, ops.queue_size, ops.tracked_object, ops.allow_concurrent_callbacks);
280:
281: if (!registerSubscriber(s, ops.datatype))
282: {
283: ROS_WARN("couldn't register subscriber on topic [%s]", ops.topic.c_str());
284: s->shutdown();
285: return false;
286: }
287:
288: subscriptions_.push_back(s);
289:
290: return true;
291: }
File: /home/lgy/ros_catkin_ws/src/ros_comm/roscpp/src/libros/topic_manager.cpp
198: bool TopicManager::addSubCallback(const SubscribeOptions& ops)
199: {
200: // spin through the subscriptions and see if we find a match. if so, use it.
201: bool found = false;
202: bool found_topic = false;
203:
204: SubscriptionPtr sub;
205:
206: {
207: if (isShuttingDown())
208: {
209: return false;
210: }
211:
212: for (L_Subscription::iterator s = subscriptions_.begin();
213: s != subscriptions_.end() && !found; ++s)
214: {
215: sub = *s;
216: if (!sub->isDropped() && sub->getName() == ops.topic)
217: {
218: found_topic = true;
219: if (md5sumsMatch(ops.md5sum, sub->md5sum()))
220: {
221: found = true;
222: }
223: break;
224: }
225: }
226: }
227:
228: if (found_topic && !found)
229: {
230: std::stringstream ss;
231: ss << "Tried to subscribe to a topic with the same name but different md5sum as a topic that was already subscribed [" << ops.datatype << "/" << ops.md5sum << " vs. " << sub->datatype() << "/" << sub->md5sum() << "]";
232: throw ConflictingSubscriptionException(ss.str());
233: }
234: else if (found)
235: {
236: if (!sub->addCallback(ops.helper, ops.md5sum, ops.callback_queue, ops.queue_size, ops.tracked_object, ops.allow_concurrent_callbacks))
237: {
238: return false;
239: }
240: }
241:
242: return found;
243: }
我们看看函数调用关系：
ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
Subscriber subscribe(const std::string& topic, uint32_t queue_size, void(T::*fp)(M), T* obj, const TransportHints& transport_hints = TransportHints())\
Subscriber NodeHandle::subscribe(SubscribeOptions& ops)
//情况1：掉用addSubCallback增加callback,如果是已经注册过的可以添加成功，否则失败继续向下执行
bool TopicManager::addSubCallback(const SubscribeOptions& ops)
bool Subscription::addCallback(const SubscriptionCallbackHelperPtr& helper, const std::string& md5sum, CallbackQueueInterface* queue, int32_t queue_size, const VoidConstPtr& tracked_object, bool allow_concurrent_callbacks)
//情况2：这里创建订阅器 增加回调函数 并保存下来
subscriptionPtr s(boost::make_shared<Subscription>(ops.topic, md5sum, datatype, ops.transport_hints));
bool Subscription::addCallback(const SubscriptionCallbackHelperPtr& helper, const std::string& md5sum, CallbackQueueInterface* queue, int32_t queue_size, const VoidConstPtr& tracked_object, bool allow_concurrent_callbacks)
s->addCallback(ops.helper, ops.md5sum, ops.callback_queue, ops.queue_size, ops.tracked_object, ops.allow_concurrent_callbacks);
subscriptions_.push_back(s);
subscribe会调用addSubCallback增加callback,如果是已经注册过的可以添加成功；否则创建订阅器 增加回调函数。
两种情况最终都调用Subscription::addCallback(…)，该函数如下：
File: /home/lgy/ros_catkin_ws/src/ros_comm/roscpp/src/libros/subscription.cpp
681: bool Subscription::addCallback(const SubscriptionCallbackHelperPtr& helper, const std::string& md5sum, CallbackQueueInterface* queue, int32_t queue_size, const VoidConstPtr& tracked_object, bool allow_concurrent_callbacks)
682: {
683: ROS_ASSERT(helper);
684: ROS_ASSERT(queue);
685:
686: statistics_.init(helper);
687:
688: // Decay to a real type as soon as we have a subscriber with a real type
689: {
690: boost::mutex::scoped_lock lock(md5sum_mutex_);
691: if (md5sum_ == "*" && md5sum != "*")
692: {
693:
694: md5sum_ = md5sum;
695: }
696: }
697:
698: if (md5sum != "*" && md5sum != this->md5sum())
699: {
700: return false;
701: }
702:
703: {
704: boost::mutex::scoped_lock lock(callbacks_mutex_);
705:
706: CallbackInfoPtr info(boost::make_shared<CallbackInfo>());
707: info->helper_ = helper;
708: info->callback_queue_ = queue;
709: info->subscription_queue_ = boost::make_shared<SubscriptionQueue>(name_, queue_size, allow_concurrent_callbacks);
710: info->tracked_object_ = tracked_object;
711: info->has_tracked_object_ = false;
712: if (tracked_object)
713: {
714: info->has_tracked_object_ = true;
715: }
716:
717: if (!helper->isConst())
718: {
719: ++nonconst_callbacks_;
720: }
721:
722: callbacks_.push_back(info);
723: cached_deserializers_.reserve(callbacks_.size());
724:
725: // if we have any latched links, we need to immediately schedule callbacks
726: if (!latched_messages_.empty())
727: {
728: boost::mutex::scoped_lock lock(publisher_links_mutex_);
729:
730: V_PublisherLink::iterator it = publisher_links_.begin();
731: V_PublisherLink::iterator end = publisher_links_.end();
732: for (; it != end;++it)
733: {
734: const PublisherLinkPtr& link = *it;
735: if (link->isLatched())
736: {
737: M_PublisherLinkToLatchInfo::iterator des_it = latched_messages_.find(link);
738: if (des_it != latched_messages_.end())
739: {
740: const LatchInfo& latch_info = des_it->second;
741:
742: MessageDeserializerPtr des(boost::make_shared<MessageDeserializer>(helper, latch_info.message, latch_info.connection_header));
743: bool was_full = false;
744: info->subscription_queue_->push(info->helper_, des, info->has_tracked_object_, info->tracked_object_, true, latch_info.receipt_time, &was_full);
745: if (!was_full)
746: {
747: info->callback_queue_->addCallback(info->subscription_queue_, (uint64_t)info.get());
748: }
749: }
750: }
751: }
752: }
753: }
754:
755: return true;
756: }
722:
Subscription::addCallback先检查参数合法性，然后创建一个CallbackInfoptr info,最终调用callbacks_.push_back(info)。
二、订阅消息是怎么接收并处理的？
在之前的文章中我们分析了同进程消息发布与订阅的处理流程：
void IntraProcessSubscriberLink::enqueueMessage(const SerializedMessage& m, bool ser, bool nocopy)
void IntraProcessPublisherLink::handleMessage(const SerializedMessage& m, bool ser, bool nocopy)
uint32_t Subscription::handleMessage(const SerializedMessage& m, bool ser, bool nocopy, const boost::shared_ptr<M_string>& connection_header, const PublisherLinkPtr& link)
我们可以看到同进程的publish消息一路调用，直接调用到订阅器的消息处理了。
在另外一篇文章中我们分析了通过TCP/UDP发布与订阅消息处理流程：
TransportPublisherLink::onHeaderReceived-->TransportPublisherLink::onMessageLength-->TransportPublisherLink::onMessage-->TransportPublisherLink::handleMessage-->Subscription::handleMessage
这样就完成了数据subscription节点的数据接收并调用Subscription::handleMessage，后续文章分析。
可以看到这两种情况最终又调用到Subscription::handleMessage，我们分析如下流程：
File: /home/lgy/ros_catkin_ws/src/ros_comm/roscpp/src/libros/subscription.cpp
599: uint32_t Subscription::handleMessage(const SerializedMessage& m, bool ser, bool nocopy, const boost::shared_ptr<M_string>& connection_header, const PublisherLinkPtr& link)
600: {
601: boost::mutex::scoped_lock lock(callbacks_mutex_);
602:
603: uint32_t drops = 0;
604:
605: // Cache the deserializers by type info. If all the subscriptions are the same type this has the same performance as before. If
606: // there are subscriptions with different C++ type (but same ROS message type), this now works correctly rather than passing
607: // garbage to the messages with different C++ types than the first one.
608: cached_deserializers_.clear();
609:
610: ros::Time receipt_time = ros::Time::now();
611:
//遍历callbacks_
612: for (V_CallbackInfo::iterator cb = callbacks_.begin();
613: cb != callbacks_.end(); ++cb)
614: {
615: const CallbackInfoPtr& info = *cb;
616:
617: ROS_ASSERT(info->callback_queue_);
618:
619: const std::type_info* ti = &info->helper_->getTypeInfo();
620:
621: if ((nocopy && m.type_info && *ti == *m.type_info) || (ser && (!m.type_info || *ti != *m.type_info)))
622: {
623: MessageDeserializerPtr deserializer;
624:
625: V_TypeAndDeserializer::iterator des_it = cached_deserializers_.begin();
626: V_TypeAndDeserializer::iterator des_end = cached_deserializers_.end();
627: for (; des_it != des_end; ++des_it)
628: {
629: if (*des_it->first == *ti)
630: {
631: deserializer = des_it->second;
632: break;
633: }
634: }
635:
636: if (!deserializer)
637: {
638: deserializer = boost::make_shared<MessageDeserializer>(info->helper_, m, connection_header);
639: cached_deserializers_.push_back(std::make_pair(ti, deserializer));
640: }
641:
642: bool was_full = false;
643: bool nonconst_need_copy = false;
644: if (callbacks_.size() > 1)
645: {
646: nonconst_need_copy = true;
647: }
648:
649: info->subscription_queue_->push(info->helper_, deserializer, info->has_tracked_object_, info->tracked_object_, nonconst_need_copy, receipt_time, &was_full);
650:
651: if (was_full)
652: {
653: ++drops;
654: }
655: else
656: {
657: info->callback_queue_->addCallback(info->subscription_queue_, (uint64_t)info.get());
658: }
659: }
660: }
661:
662: // measure statistics
663: statistics_.callback(connection_header, name_, link->getCallerID(), m, link->getStats().bytes_received_, receipt_time, drops > 0);
664:
665: // If this link is latched, store off the message so we can immediately pass it to new subscribers later
666: if (link->isLatched())
667: {
668: LatchInfo li;
669: li.connection_header = connection_header;
670: li.link = link;
671: li.message = m;
672: li.receipt_time = receipt_time;
673: latched_messages_[link] = li;
674: }
675:
676: cached_deserializers_.clear();
677:
678: return drops;
679: }
通过代码看这里遍历callbacks_，判断队列是否满：如果满的话就丢弃了，否则加入到处理队列。消息处理就结束了？怎么没有调用我们的回调函数呢？我们继续向后看。
三、ros::spin();做了什么？
File: /home/lgy/ros_catkin_ws/src/ros_comm/roscpp/src/libros/init.cpp
560: void spin()
561: {
562: SingleThreadedSpinner s;
563: spin(s);
564: }
565:
566: void spin(Spinner& s)
567: {
568: s.spin();
569: }
File: /home/lgy/ros_catkin_ws/src/ros_comm/roscpp/src/libros/spinner.cpp
123: void SingleThreadedSpinner::spin(CallbackQueue* queue)
124: {
125: if (!queue)
126: {
127: queue = getGlobalCallbackQueue();
128: }
129:
130: if (!spinner_monitor.add(queue, true))
131: {
132: std::string errorMessage = "SingleThreadedSpinner: " + DEFAULT_ERROR_MESSAGE + " You might want to use a MultiThreadedSpinner instead.";
133: ROS_FATAL_STREAM(errorMessage);
134: throw std::runtime_error(errorMessage);
135: }
136:
137: ros::WallDuration timeout(0.1f);
138: ros::NodeHandle n;
139: while (n.ok())
140: {
141: queue->callAvailable(timeout);
142: }
143: spinner_monitor.remove(queue);
144: }
File: /home/lgy/ros_catkin_ws/src/ros_comm/roscpp/src/libros/callback_queue.cpp
301: void CallbackQueue::callAvailable(ros::WallDuration timeout)
302: {
303: setupTLS();
304: TLS* tls = tls_.get();
305:
306: {
307: boost::mutex::scoped_lock lock(mutex_);
308:
309: if (!enabled_)
310: {
311: return;
312: }
313:
314: if (callbacks_.empty())
315: {
316: if (!timeout.isZero())
317: {
318: condition_.wait_for(lock, boost::chrono::nanoseconds(timeout.toNSec()));
319: }
320:
321: if (callbacks_.empty() || !enabled_)
322: {
323: return;
324: }
325: }
326:
327: bool was_empty = tls->callbacks.empty();
328:
329: tls->callbacks.insert(tls->callbacks.end(), callbacks_.begin(), callbacks_.end());
330: callbacks_.clear();
331:
332: calling_ += tls->callbacks.size();
333:
334: if (was_empty)
335: {
336: tls->cb_it = tls->callbacks.begin();
337: }
338: }
339:
340: size_t called = 0;
341:
342: while (!tls->callbacks.empty())
343: {
344: if (callOneCB(tls) != Empty)
345: {
346: ++called;
347: }
348: }
349:
350: {
351: boost::mutex::scoped_lock lock(mutex_);
352: calling_ -= called;
353: }
354: }
File: /home/lgy/ros_catkin_ws/src/ros_comm/roscpp/src/libros/callback_queue.cpp
356: CallbackQueue::CallOneResult CallbackQueue::callOneCB(TLS* tls)
357: {
358: // Check for a recursive call. If recursive, increment the current iterator. Otherwise
359: // set the iterator it the beginning of the thread-local callbacks
360: if (tls->calling_in_this_thread == 0xffffffffffffffffULL)
361: {
362: tls->cb_it = tls->callbacks.begin();
363: }
364:
365: if (tls->cb_it == tls->callbacks.end())
366: {
367: return Empty;
368: }
369:
370: ROS_ASSERT(!tls->callbacks.empty());
371: ROS_ASSERT(tls->cb_it != tls->callbacks.end());
372:
373: CallbackInfo info = *tls->cb_it;
374: CallbackInterfacePtr& cb = info.callback;
375:
376: IDInfoPtr id_info = getIDInfo(info.removal_id);
377: if (id_info)
378: {
379: boost::shared_lock<boost::shared_mutex> rw_lock(id_info->calling_rw_mutex);
380:
381: uint64_t last_calling = tls->calling_in_this_thread;
382: tls->calling_in_this_thread = id_info->id;
383:
384: CallbackInterface::CallResult result = CallbackInterface::Invalid;
385:
386: {
387: // Ensure that thread id gets restored, even if callback throws.
388: // This is done with RAII rather than try-catch so that the source
389: // of the original exception is not masked in a crash report.
390: BOOST_SCOPE_EXIT(&tls, &last_calling)
391: {
392: tls->calling_in_this_thread = last_calling;
393: }
394: BOOST_SCOPE_EXIT_END
395:
396: if (info.marked_for_removal)
397: {
398: tls->cb_it = tls->callbacks.erase(tls->cb_it);
399: }
400: else
401: {
402: tls->cb_it = tls->callbacks.erase(tls->cb_it);
403: result = cb->call();
404: if (result == CallbackInterface::Success)
405: {
406: condition_.notify_one();
407: }
408: }
409: }
410:
411: // Push TryAgain callbacks to the back of the shared queue
412: if (result == CallbackInterface::TryAgain && !info.marked_for_removal)
413: {
414: boost::mutex::scoped_lock lock(mutex_);
415: callbacks_.push_back(info);
416:
417: return TryAgain;
418: }
419:
420: return Called;
421: }
422: else
423: {
424: tls->cb_it = tls->callbacks.erase(tls->cb_it);
425: }
426:
427: return Called;
428: }
函数调用关系如下：
void spin()
void SingleThreadedSpinner::spin(CallbackQueue* queue)
void CallbackQueue::callAvailable(ros::WallDuration timeout)
CallbackQueue::CallOneResult CallbackQueue::callOneCB(TLS* tls)
CallbackInterfacePtr& cb = info.callback; result = cb->call();
函数一路调用到cb->call()。CallbackInterfacePtr& cb是CallbackInterface指针。
class ROSCPP_DECL SubscriptionQueue : public CallbackInterface, public boost::enable_shared_from_this<SubscriptionQueue>
SubscriptionQueue继承自CallbackInterface，接着看subscriptionQueue::call()
File: /home/lgy/ros_catkin_ws/src/ros_comm/roscpp/src/libros/subscription_queue.cpp
102: CallbackInterface::CallResult SubscriptionQueue::call()
103: {
104: // The callback may result in our own destruction. Therefore, we may need to keep a reference to ourselves
105: // that outlasts the scoped_try_lock
106: boost::shared_ptr<SubscriptionQueue> self;
107: boost::recursive_mutex::scoped_try_lock lock(callback_mutex_, boost::defer_lock);
108:
109: if (!allow_concurrent_callbacks_)
110: {
111: lock.try_lock();
112: if (!lock.owns_lock())
113: {
114: return CallbackInterface::TryAgain;
115: }
116: }
117:
118: VoidConstPtr tracker;
119: Item i;
120:
121: {
122: boost::mutex::scoped_lock lock(queue_mutex_);
123:
124: if (queue_.empty())
125: {
126: return CallbackInterface::Invalid;
127: }
128:
129: i = queue_.front();
130:
131: if (queue_.empty())
132: {
133: return CallbackInterface::Invalid;
134: }
135:
136: if (i.has_tracked_object)
137: {
138: tracker = i.tracked_object.lock();
139:
140: if (!tracker)
141: {
142: return CallbackInterface::Invalid;
143: }
144: }
145:
146: queue_.pop_front();
147: --queue_size_;
148: }
149:
150: VoidConstPtr msg = i.deserializer->deserialize();
151:
152: // msg can be null here if deserialization failed
153: if (msg)
154: {
155: try
156: {
157: self = shared_from_this();
158: }
159: catch (boost::bad_weak_ptr&) // For the tests, where we don't create a shared_ptr
160: {}
161:
162: SubscriptionCallbackHelperCallParams params;
163: params.event = MessageEvent<void const>(msg, i.deserializer->getConnectionHeader(), i.receipt_time, i.nonconst_need_copy, MessageEvent<void const>::CreateFunction());
164: i.helper->call(params);
165: }
166:
167: return CallbackInterface::Success;
168: }
File: /home/lgy/ros_catkin_ws/src/ros_comm/roscpp/include/ros/subscription_callback_helper.h
141: virtual void call(SubscriptionCallbackHelperCallParams& params)
142: {
143: Event event(params.event, create_);
144: callback_(ParameterAdapter<P>::getParameter(event));
145: }
函数调用如下：
CallbackInterface::CallResult SubscriptionQueue::call()
i = queue_.front();
VoidConstPtr msg = i.deserializer->deserialize();
i.helper->call(params);
callback_(ParameterAdapter<P>::getParameter(event));
请再次留意下subscribe做的工作：
Subscriber subscribe(const std::string& topic, uint32_t queue_size, void(T::*fp)(M), T* obj,...)
subscribe_options::initByFullCallbackType
helper = boost::make_shared<SubscriptionCallbackHelperT<P> >(_callback, factory_fn);
SubscriptionCallbackHelperT(const Callback& callback, const CreateFunction& create = DefaultMessageCreator<NonConstType>())//这里请留意callback通过构造函数最终赋值给callback_中。
这里的callback_就是Subscriber subscribe时的callback,这样终于调用到了用户的回调函数。
1664
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
