ros melodic源码分析之消息发布msg publish

一个简单的消息发布使用案例如下代码所示

File: src/ros_tutorials/roscpp_tutorials/talker/talker.cpp

29: #include "ros/ros.h"
32: #include "std_msgs/String.h"
35: #include <sstream>

40: int main(int argc, char **argv)
41: {
53:   ros::init(argc, argv, "talker");

62:   ros::NodeHandle n;

83:   ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
85: 
87:   ros::Rate loop_rate(10);

95:   int count = 0;
96:   while (ros::ok())
97:   {
103:     std_msgs::String msg;
104: 
105:     std::stringstream ss;
106:     ss << "hello world " << count;
107:     msg.data = ss.str();

111:     ROS_INFO("%s", msg.data.c_str());

121:     chatter_pub.publish(msg);
123: 
125:     ros::spinOnce();
127: 
129:     loop_rate.sleep();
131:     ++count;
132:   }
135:   return 0;
136: }

今天我们关注

1. 62: ros::NodeHandle n; ros节点是怎么被初始化的？
2. 83: ros::Publisher chatter_pub = n.advertise<std_msgs::String>(“chatter”, 1000); advertise调用背后有什么故事？
3. 121: chatter_pub.publish(msg); publish消息是怎么被发布的？

一、ros::NodeHandle n; ros节点是怎么被初始化的

从NodeHadle类声明开始

File: src/ros_comm/roscpp/include/ros/node_handle.h
87:   class ROSCPP_DECL NodeHandle
88:   {
89:   public:
104:     NodeHandle(const std::string& ns = std::string(), const M_string& remappings = M_string());
...
}

File: src/ros_comm/roscpp/src/libros/node_handle.cpp
77: NodeHandle::NodeHandle(const std::string& ns, const M_string& remappings)
78:   : namespace_(this_node::getNamespace())
79:   , callback_queue_(0)
80:   , collection_(0)
81: {
82:   std::string tilde_resolved_ns;
83:   if (!ns.empty() && ns[0] == '~')// starts with tilde
84:     tilde_resolved_ns = names::resolve(ns);
85:   else
86:     tilde_resolved_ns = ns;
87: 
88:   construct(tilde_resolved_ns, true);
89: 
90:   initRemappings(remappings);
91: }

File: src/ros_comm/roscpp/src/libros/node_handle.cpp
152: void NodeHandle::construct(const std::string& ns, bool validate_name)
153: {
154:   if (!ros::isInitialized())
155:   {
156:     ROS_FATAL("You must call ros::init() before creating the first NodeHandle");
157:     ROS_BREAK();
158:   }
159: 
160:   collection_ = new NodeHandleBackingCollection;
161:   unresolved_namespace_ = ns;
162:   // if callback_queue_ is nonnull, we are in a non-nullary constructor
163: 
164:   if (validate_name)
165:     namespace_ = resolveName(ns, true);
166:   else
167:     {
168:       namespace_ = resolveName(ns, true, no_validate());
169:       // FIXME validate namespace_ now
170:     }
171:   ok_ = true;
172: 
173:   boost::mutex::scoped_lock lock(g_nh_refcount_mutex);
174: 
175:   if (g_nh_refcount == 0 && !ros::isStarted())
176:   {
177:     g_node_started_by_nh = true;
178:     ros::start();
179:   }
180: 
181:   ++g_nh_refcount;
182: }

File: src/ros_comm/roscpp/src/libros/node_handle.cpp
198: void NodeHandle::initRemappings(const M_string& remappings)
199: {
200:   {
201:     M_string::const_iterator it = remappings.begin();
202:     M_string::const_iterator end = remappings.end();
203:     for (; it != end; ++it)
204:     {
205:       const std::string& from = it->first;
206:       const std::string& to = it->second;
207: 
208:       remappings_.insert(std::make_pair(resolveName(from, false), resolveName(to, false)));
209:       unresolved_remappings_.insert(std::make_pair(from, to));
210:     }
211:   }
212: }

函数调用关系如下(缩进表示调用关系)

NodeHandle调用构造函数
    construct函数
        如果！ros::isStarted() ros没有启动会调用，ros::start()启动ros，本文章后续会做简单分析。
    initRemappings初始化映射map，后续使用。

这样完成了NodeHandle的初始化。

二、 advertise调用背后有什么故事？

 83: ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000); 

File: src/ros_comm/roscpp/include/ros/node_handle.h
248:     template <class M>
249:     Publisher advertise(const std::string& topic, uint32_t queue_size, bool latch = false)
250:     {
251:       AdvertiseOptions ops;
252:       ops.template init<M>(topic, queue_size);
253:       ops.latch = latch;
254:       return advertise(ops);
255:     }

File: src/ros_comm/roscpp/src/libros/node_handle.cpp
289: Publisher NodeHandle::advertise(AdvertiseOptions& ops)
290: {
291:   ops.topic = resolveName(ops.topic);
292:   if (ops.callback_queue == 0)
293:   {
294:     if (callback_queue_)
295:     {
296:       ops.callback_queue = callback_queue_;
297:     }
298:     else
299:     {
300:       ops.callback_queue = getGlobalCallbackQueue();
301:     }
302:   }
303: 
304:   SubscriberCallbacksPtr callbacks(boost::make_shared<SubscriberCallbacks>(ops.connect_cb, ops.disconnect_cb, 
305:                                                                            ops.tracked_object, ops.callback_queue));
306: 
307:   if (TopicManager::instance()->advertise(ops, callbacks))
308:   {
309:     Publisher pub(ops.topic, ops.md5sum, ops.datatype, *this, callbacks);
310: 
311:     {
312:       boost::mutex::scoped_lock lock(collection_->mutex_);
313:       collection_->pubs_.push_back(pub.impl_);
314:     }
315: 
316:     return pub;
317:   }
318: 
319:   return Publisher();
320: }

函数调用关系如下：

NodeHandle::advertise(const std::string& topic, uint32_t queue_size, bool latch = false)
    NodeHandle::advertise(AdvertiseOptions& ops)
        TopicManager::instance()->advertise(ops, callbacks)

我们对TopicManager::instance()->advertise(ops, callbacks)进行分析：

File: src/ros_comm/roscpp/src/libros/topic_manager.cpp
289: bool TopicManager::advertise(const AdvertiseOptions& ops, const SubscriberCallbacksPtr& callbacks)
290: {
291:   if (ops.datatype == "*")
292:   {
293:     std::stringstream ss;
294:     ss << "Advertising with * as the datatype is not allowed.  Topic [" << ops.topic << "]";
295:     throw InvalidParameterException(ss.str());
296:   }
297: 
298:   if (ops.md5sum == "*")
299:   {
300:     std::stringstream ss;
301:     ss << "Advertising with * as the md5sum is not allowed.  Topic [" << ops.topic << "]";
302:     throw InvalidParameterException(ss.str());
303:   }
304: 
305:   if (ops.md5sum.empty())
306:   {
307:     throw InvalidParameterException("Advertising on topic [" + ops.topic + "] with an empty md5sum");
308:   }
309: 
310:   if (ops.datatype.empty())
311:   {
312:     throw InvalidParameterException("Advertising on topic [" + ops.topic + "] with an empty datatype");
313:   }
314: 
315:   if (ops.message_definition.empty())
316:   {
317:     ROS_WARN("Advertising on topic [%s] with an empty message definition.  Some tools (e.g. rosbag) may not work correctly.", ops.topic.c_str());
318:   }
319: 
320:   PublicationPtr pub;
321: 
322:   {
323:     boost::recursive_mutex::scoped_lock lock(advertised_topics_mutex_);
324: 
325:     if (isShuttingDown())
326:     {
327:       return false;
328:     }
329: 
330:     pub = lookupPublicationWithoutLock(ops.topic);
331:     if (pub && pub->getNumCallbacks() == 0)
332:     {
333:       pub.reset();
334:     }
335: 
336:     if (pub)
337:     {
338:       if (pub->getMD5Sum() != ops.md5sum)
339:       {
340:         ROS_ERROR("Tried to advertise on topic [%s] with md5sum [%s] and datatype [%s], but the topic is already advertised as md5sum [%s] and datatype [%s]",
341:                   ops.topic.c_str(), ops.md5sum.c_str(), ops.datatype.c_str(), pub->getMD5Sum().c_str(), pub->getDataType().c_str());
342:         return false;
343:       }
344: 
345:       pub->addCallbacks(callbacks);
346: 
347:       return true;
348:     }
349: 
350:     pub = PublicationPtr(boost::make_shared<Publication>(ops.topic, ops.datatype, ops.md5sum, ops.message_definition, ops.queue_size, ops.latch, ops.has_header));
351:     pub->addCallbacks(callbacks);
352:     advertised_topics_.push_back(pub);
353:   }
354: 
355: 
356:   {
357:     boost::mutex::scoped_lock lock(advertised_topic_names_mutex_);
358:     advertised_topic_names_.push_back(ops.topic);
359:   }
360: 
361:   // Check whether we've already subscribed to this topic.  If so, we'll do
362:   // the self-subscription here, to avoid the deadlock that would happen if
363:   // the ROS thread later got the publisherUpdate with its own XMLRPC URI.
364:   // The assumption is that advertise() is called from somewhere other
365:   // than the ROS thread.
366:   bool found = false;
367:   SubscriptionPtr sub;
368:   {
369:     boost::mutex::scoped_lock lock(subs_mutex_);
370: 
371:     for (L_Subscription::iterator s = subscriptions_.begin();
372:          s != subscriptions_.end() && !found; ++s)
373:     {
374:       if ((*s)->getName() == ops.topic && md5sumsMatch((*s)->md5sum(), ops.md5sum) && !(*s)->isDropped())
375:       {
376:         found = true;
377:         sub = *s;
378:         break;
379:       }
380:     }
381:   }
382: 
383:   if(found)
384:   {
385:     sub->addLocalConnection(pub);
386:   }
387: 
388:   XmlRpcValue args, result, payload;
389:   args[0] = this_node::getName();
390:   args[1] = ops.topic;
391:   args[2] = ops.datatype;
392:   args[3] = xmlrpc_manager_->getServerURI();
393:   master::execute("registerPublisher", args, result, payload, true);
394: 
395:   return true;
396: }

TopicManager::advertise函数中：

1. 首先做一堆合法性校验，不合法时抛出异常
2. 调用 pub = lookupPublicationWithoutLock(ops.topic);从已注册的发布节点中查找节点；
   如果找到进行MD5校验，合法则加入回调函数，非法就报错返回。
   如果没有找到时创建新的发布器，加入回调函数。将pub加入到记录中advertised_topics_.push_back(pub);
3. 查找是否自己订阅了自己，如果订阅了就将自己sub，避免死锁。
4. 向master节点注册发布器。

三、 publish消息是怎么被发布的？

File: src/ros_comm/roscpp/include/ros/publisher.h
92:     /**
93:      * \brief Publish a message on the topic associated with this Publisher.
94:      */
95:     template <typename M>
96:       void publish(const M& message) const
97:     {
98:       using namespace serialization;
99:       namespace mt = ros::message_traits;
100: 
101:       if (!impl_)
102:         {
103:           ROS_ASSERT_MSG(false, "Call to publish() on an invalid Publisher");
104:           return;
105:         }
106: 
107:       if (!impl_->isValid())
108:         {
109:           ROS_ASSERT_MSG(false, "Call to publish() on an invalid Publisher (topic [%s])", impl_->topic_.c_str());
110:           return;
111:         }
112: 
113:       ROS_ASSERT_MSG(impl_->md5sum_ == "*" || std::string(mt::md5sum<M>(message)) == "*" || impl_->md5sum_ == mt::md5sum<M>(message),
114:                      "Trying to publish message of type [%s/%s] on a publisher with type [%s/%s]",
115:                      mt::datatype<M>(message), mt::md5sum<M>(message),
116:                      impl_->datatype_.c_str(), impl_->md5sum_.c_str());
117: 
118:       SerializedMessage m;
119:       publish(boost::bind(serializeMessage<M>, boost::ref(message)), m);
120:     }

File: src/ros_comm/roscpp/src/libros/publisher.cpp
78: void Publisher::publish(const boost::function<SerializedMessage(void)>& serfunc, SerializedMessage& m) const
79: {
80:   if (!impl_)
81:   {
82:     ROS_ASSERT_MSG(false, "Call to publish() on an invalid Publisher (topic [%s])", impl_->topic_.c_str());
83:     return;
84:   }
85: 
86:   if (!impl_->isValid())
87:   {
88:     ROS_ASSERT_MSG(false, "Call to publish() on an invalid Publisher (topic [%s])", impl_->topic_.c_str());
89:     return;
90:   }
91: 
92:   TopicManager::instance()->publish(impl_->topic_, serfunc, m);
93: }

File: src/ros_comm/roscpp/src/libros/topic_manager.cpp
700: void TopicManager::publish(const std::string& topic, const boost::function<SerializedMessage(void)>& serfunc, SerializedMessage& m)
701: {
702:   boost::recursive_mutex::scoped_lock lock(advertised_topics_mutex_);
703: 
704:   if (isShuttingDown())
705:   {
706:     return;
707:   }
708: 
709:   PublicationPtr p = lookupPublicationWithoutLock(topic);
710:   if (p->hasSubscribers() || p->isLatching())
711:   {
712:     ROS_DEBUG_NAMED("superdebug", "Publishing message on topic [%s] with sequence number [%d]", p->getName().c_str(), p->getSequence());
713: 
714:     // Determine what kinds of subscribers we're publishing to.  If they're intraprocess with the same C++ type we can
715:     // do a no-copy publish.
716:     bool nocopy = false;
717:     bool serialize = false;
718: 
719:     // We can only do a no-copy publish if a shared_ptr to the message is provided, and we have type information for it
720:     if (m.type_info && m.message)
721:     {
722:       p->getPublishTypes(serialize, nocopy, *m.type_info);
723:     }
724:     else
725:     {
726:       serialize = true;
727:     }
728: 
729:     if (!nocopy)
730:     {
731:       m.message.reset();
732:       m.type_info = 0;
733:     }
734: 
735:     if (serialize || p->isLatching())
736:     {
737:       SerializedMessage m2 = serfunc();
738:       m.buf = m2.buf;
739:       m.num_bytes = m2.num_bytes;
740:       m.message_start = m2.message_start;
741:     }
742: 
743:     p->publish(m);
744: 
745:     // If we're not doing a serialized publish we don't need to signal the pollset.  The write()
746:     // call inside signal() is actually relatively expensive when doing a nocopy publish.
747:     if (serialize)
748:     {
749:       poll_manager_->getPollSet().signal();
750:     }
751:   }
752:   else
753:   {
754:     p->incrementSequence();
755:   }
756: }

File: src/ros_comm/roscpp/src/libros/publication.cpp
407: void Publication::publish(SerializedMessage& m)
408: {
409:   if (m.message)
410:   {
411:     boost::mutex::scoped_lock lock(subscriber_links_mutex_);
412:     V_SubscriberLink::const_iterator it = subscriber_links_.begin();
413:     V_SubscriberLink::const_iterator end = subscriber_links_.end();
414:     for (; it != end; ++it)
415:     {
416:       const SubscriberLinkPtr& sub = *it;
417:       if (sub->isIntraprocess())
418:       {
419:         sub->enqueueMessage(m, false, true);
420:       }
421:     }
422: 
423:     m.message.reset();
424:   }
425: 
426:   if (m.buf)
427:   {
428:     boost::mutex::scoped_lock lock(publish_queue_mutex_);
429:     publish_queue_.push_back(m);
430:   }
431: }

函数调用关系如下:

template <typename M> void publisher::void publish(const M& message) const
    void Publisher::publish(c...) const
        void TopicManager::publish(...) 查找已经注册的发布器,判断有订阅者或者是latch的再做发布
             void Publication::publish(SerializedMessage& m) 判断是否为同进程，同进程的直接调用sub的enqueueMessage入队列；不同进程则入队列publish_queue_

1. 我们简单看一下同进程如何处理的？

File: src/ros_comm/roscpp/src/libros/intraprocess_subscriber_link.cpp
74: void IntraProcessSubscriberLink::enqueueMessage(const SerializedMessage& m, bool ser, bool nocopy)
75: {
76:   boost::recursive_mutex::scoped_lock lock(drop_mutex_);
77:   if (dropped_)
78:   {
79:     return;
80:   }
81: 
82:   ROS_ASSERT(subscriber_);
83:   subscriber_->handleMessage(m, ser, nocopy);
84: }

File: src/ros_comm/roscpp/src/libros/intraprocess_publisher_link.cpp
106: void IntraProcessPublisherLink::handleMessage(const SerializedMessage& m, bool ser, bool nocopy)
107: {
108:   boost::recursive_mutex::scoped_lock lock(drop_mutex_);
109:   if (dropped_)
110:   {
111:     return;
112:   }
113: 
114:   stats_.bytes_received_ += m.num_bytes;
115:   stats_.messages_received_++;
116: 
117:   SubscriptionPtr parent = parent_.lock();
118: 
119:   if (parent)
120:   {
121:     stats_.drops_ += parent->handleMessage(m, ser, nocopy, header_.getValues(), shared_from_this());
122:   }
123: }

File: 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: 
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: }
680: 

函数调用如下：

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消息一路调用，直接调用到订阅器的消息处理了。

2.那不同进程则入队列publish_queue_后又怎么处理的呢？

File: src/ros_comm/roscpp/src/libros/init.cpp
293: void start()
294: {
295:   boost::mutex::scoped_lock lock(g_start_mutex);
296:   if (g_started)
297:   {
298:     return;
299:   }
300: 
301:   g_shutdown_requested = false;
302:   g_shutting_down = false;
303:   g_started = true;
304:   g_ok = true;
305: 
306:   bool enable_debug = false;
307:   std::string enable_debug_env;
308:   if ( get_environment_variable(enable_debug_env,"ROSCPP_ENABLE_DEBUG") )
309:   {
310:     try
311:     {
312:       enable_debug = boost::lexical_cast<bool>(enable_debug_env.c_str());
313:     }
314:     catch (boost::bad_lexical_cast&)
315:     {
316:     }
317:   }
318: 
319:   param::param("/tcp_keepalive", TransportTCP::s_use_keepalive_, TransportTCP::s_use_keepalive_);
320: 
321:   PollManager::instance()->addPollThreadListener(checkForShutdown);
322:   XMLRPCManager::instance()->bind("shutdown", shutdownCallback);
323: 
324:   initInternalTimerManager();
325: 
326:   TopicManager::instance()->start();
    //...
}

File: src/ros_comm/roscpp/src/libros/topic_manager.cpp
56: const TopicManagerPtr& TopicManager::instance()
57: {
58:   static TopicManagerPtr topic_manager = boost::make_shared<TopicManager>();
59:   return topic_manager;
60: }

File: src/ros_comm/roscpp/src/libros/topic_manager.cpp
68: void TopicManager::start()
69: {
70:   boost::mutex::scoped_lock shutdown_lock(shutting_down_mutex_);
71:   shutting_down_ = false;
72: 
73:   poll_manager_ = PollManager::instance();
74:   connection_manager_ = ConnectionManager::instance();
75:   xmlrpc_manager_ = XMLRPCManager::instance();
76: 
77:   xmlrpc_manager_->bind("publisherUpdate", boost::bind(&TopicManager::pubUpdateCallback, this, _1, _2));
78:   xmlrpc_manager_->bind("requestTopic", boost::bind(&TopicManager::requestTopicCallback, this, _1, _2));
79:   xmlrpc_manager_->bind("getBusStats", boost::bind(&TopicManager::getBusStatsCallback, this, _1, _2));
80:   xmlrpc_manager_->bind("getBusInfo", boost::bind(&TopicManager::getBusInfoCallback, this, _1, _2));
81:   xmlrpc_manager_->bind("getSubscriptions", boost::bind(&TopicManager::getSubscriptionsCallback, this, _1, _2));
82:   xmlrpc_manager_->bind("getPublications", boost::bind(&TopicManager::getPublicationsCallback, this, _1, _2));
83: 
84:   poll_manager_->addPollThreadListener(boost::bind(&TopicManager::processPublishQueues, this));
85: }

本文章前面提到NodeHandle构造时，如果！ros::isStarted() ros没有启动会调用，ros::start()启动ros，我们关心如下调用流程：

void start()
    TopicManager::instance()->start();
        poll_manager_->addPollThreadListener(boost::bind(&TopicManager::processPublishQueues, this));

poll_manager在之前的文章中有分析，结果就是processPublishQueues会循环被调用。

File: src/ros_comm/roscpp/src/libros/topic_manager.cpp
145: void TopicManager::processPublishQueues()
146: {
147:   boost::recursive_mutex::scoped_lock lock(advertised_topics_mutex_);
148: 
149:   V_Publication::iterator it = advertised_topics_.begin();
150:   V_Publication::iterator end = advertised_topics_.end();
151:   for (; it != end; ++it)
152:   {
153:     const PublicationPtr& pub = *it;
154:     pub->processPublishQueue();
155:   }
156: }

File: src/ros_comm/roscpp/src/libros/publication.cpp
433: void Publication::processPublishQueue()
434: {
435:   V_SerializedMessage queue;
436:   {
437:     boost::mutex::scoped_lock lock(publish_queue_mutex_);
438: 
439:     if (dropped_)
440:     {
441:       return;
442:     }
443: 
444:     queue.insert(queue.end(), publish_queue_.begin(), publish_queue_.end());
445:     publish_queue_.clear();
446:   }
447: 
448:   if (queue.empty())
449:   {
450:     return;
451:   }
452: 
453:   V_SerializedMessage::iterator it = queue.begin();
454:   V_SerializedMessage::iterator end = queue.end();
455:   for (; it != end; ++it)
456:   {
457:     enqueueMessage(*it);
458:   }
459: }

File: src/ros_comm/roscpp/src/libros/publication.cpp
157: bool Publication::enqueueMessage(const SerializedMessage& m)
158: {
159:   boost::mutex::scoped_lock lock(subscriber_links_mutex_);
160:   if (dropped_)
161:   {
162:     return false;
163:   }
164: 
165:   ROS_ASSERT(m.buf);
166: 
167:   uint32_t seq = incrementSequence();
168:   if (has_header_)
169:   {
170:     // If we have a header, we know it's immediately after the message length
171:     // Deserialize it, write the sequence, and then serialize it again.
172:     namespace ser = ros::serialization;
173:     std_msgs::Header header;
174:     ser::IStream istream(m.buf.get() + 4, m.num_bytes - 4);
175:     ser::deserialize(istream, header);
176:     header.seq = seq;
177:     ser::OStream ostream(m.buf.get() + 4, m.num_bytes - 4);
178:     ser::serialize(ostream, header);
179:   }
180: 
181:   for(V_SubscriberLink::iterator i = subscriber_links_.begin();
182:       i != subscriber_links_.end(); ++i)
183:   {
184:     const SubscriberLinkPtr& sub_link = (*i);
185:     sub_link->enqueueMessage(m, true, false);
186:   }
187: 
188:   if (latch_)
189:   {
190:     last_message_ = m;
191:   }
192: 
193:   return true;
194: }

File: src/ros_comm/roscpp/src/libros/transport_subscriber_link.cpp
177: void TransportSubscriberLink::enqueueMessage(const SerializedMessage& m, bool ser, bool nocopy)
178: {
179:   (void)nocopy;
180:   if (!ser)
181:   {
182:     return;
183:   }
184: 
185:   {
186:     boost::mutex::scoped_lock lock(outbox_mutex_);
187: 
188:     int max_queue = 0;
189:     if (PublicationPtr parent = parent_.lock())
190:     {
191:       max_queue = parent->getMaxQueue();
192:     }
193: 
194:     ROS_DEBUG_NAMED("superdebug", "TransportSubscriberLink on topic [%s] to caller [%s], queueing message (queue size [%d])", topic_.c_str(), destination_caller_id_.c_str(), (int)outbox_.size());
195: 
196:     if (max_queue > 0 && (int)outbox_.size() >= max_queue)
197:     {
198:       if (!queue_full_)
199:       {
200:         ROS_DEBUG("Outgoing queue full for topic [%s].  "
201:                "Discarding oldest message",
202:                topic_.c_str());
203:       }
204: 
205:       outbox_.pop(); // toss out the oldest thing in the queue to make room for us
206:       queue_full_ = true;
207:     }
208:     else
209:     {
210:       queue_full_ = false;
211:     }
212: 
213:     outbox_.push(m);
214:   }
215: 
216:   startMessageWrite(false);
217: 
218:   stats_.messages_sent_++;
219:   stats_.bytes_sent_ += m.num_bytes;
220:   stats_.message_data_sent_ += m.num_bytes;
221: }

File: src/ros_comm/roscpp/src/libros/transport_subscriber_link.cpp
151: void TransportSubscriberLink::startMessageWrite(bool immediate_write)
152: {
153:   boost::shared_array<uint8_t> dummy;
154:   SerializedMessage m(dummy, (uint32_t)0);
155: 
156:   {
157:     boost::mutex::scoped_lock lock(outbox_mutex_);
158:     if (writing_message_ || !header_written_)
159:     {
160:       return;
161:     }
162: 
163:     if (!outbox_.empty())
164:     {
165:       writing_message_ = true;
166:       m = outbox_.front();
167:       outbox_.pop();
168:     }
169:   }
170: 
171:   if (m.num_bytes > 0)
172:   {
173:     connection_->write(m.buf, m.num_bytes, boost::bind(&TransportSubscriberLink::onMessageWritten, this, _1), immediate_write);
174:   }
175: }

函数调用如下:

void TopicManager::processPublishQueues()
    void Publication::processPublishQueue()
        bool Publication::enqueueMessage(const SerializedMessage& m)
            TransportSubscriberLink::enqueueMessage(const SerializedMessage& m, bool ser, bool nocopy)
                void TransportSubscriberLink::startMessageWrite(bool immediate_write)

队列处理函数函数最终回调用到transport的写消息函数。
写数据又是如何持续的呢？

File: src/ros_comm/roscpp/src/libros/transport_subscriber_link.cpp
144: void TransportSubscriberLink::onMessageWritten(const ConnectionPtr& conn)
145: {
146:   (void)conn;
147:   writing_message_ = false;
148:   startMessageWrite(true);
149: }
150: 
151: void TransportSubscriberLink::startMessageWrite(bool immediate_write)
152: {
153:   boost::shared_array<uint8_t> dummy;
154:   SerializedMessage m(dummy, (uint32_t)0);
155: 
156:   {
157:     boost::mutex::scoped_lock lock(outbox_mutex_);
158:     if (writing_message_ || !header_written_)
159:     {
160:       return;
161:     }
162: 
163:     if (!outbox_.empty())
164:     {
165:       writing_message_ = true;
166:       m = outbox_.front();
167:       outbox_.pop();
168:     }
169:   }
170: 
171:   if (m.num_bytes > 0)
172:   {
173:     connection_->write(m.buf, m.num_bytes, boost::bind(&TransportSubscriberLink::onMessageWritten, this, _1), immediate_write);
174:   }
175: }
···

```cpp
调用关系如下：
connection_->write(m.buf, m.num_bytes, boost::bind(&TransportSubscriberLink::onMessageWritten, this, _1), 
    void TransportSubscriberLink::onMessageWritten(const ConnectionPtr& conn)
        startMessageWrite(true);
            connection_->write(m.buf, m.num_bytes, boost::bind(&TransportSubscriberLink::onMessageWritten, this, _1), immediate_write);

写完成时会调用onMessageWritten，会导致onMessageWritten循环被调用，直到outbox_队列写完。