subscription.cpp

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

#include <sstream>
#include <fcntl.h>
#include <cerrno>
#include <cstring>
#include <typeinfo>

#include "ros/common.h"
#include "ros/io.h"
#include "ros/subscription.h"
#include "ros/publication.h"
#include "ros/transport_publisher_link.h"
#include "ros/intraprocess_publisher_link.h"
#include "ros/intraprocess_subscriber_link.h"
#include "ros/connection.h"
#include "ros/transport/transport_tcp.h"
#include "ros/transport/transport_udp.h"
#include "ros/callback_queue_interface.h"
#include "ros/this_node.h"
#include "ros/network.h"
#include "ros/poll_manager.h"
#include "ros/connection_manager.h"
#include "ros/message_deserializer.h"
#include "ros/subscription_queue.h"
#include "ros/file_log.h"
#include "ros/transport_hints.h"
#include "ros/subscription_callback_helper.h"

#include <boost/make_shared.hpp>

using XmlRpc::XmlRpcValue;

namespace ros
{

Subscription::Subscription(const std::string &name, const std::string& md5sum, const std::string& datatype, const TransportHints& transport_hints)
: name_(name)
, md5sum_(md5sum)
, datatype_(datatype)
, nonconst_callbacks_(0)
, dropped_(false)
, shutting_down_(false)
, transport_hints_(transport_hints)
{
}

Subscription::~Subscription()
{
  pending_connections_.clear();
  callbacks_.clear();
}

void Subscription::shutdown()
{
  {
    boost::mutex::scoped_lock lock(shutdown_mutex_);
    shutting_down_ = true;
  }

  drop();
}

XmlRpcValue Subscription::getStats()
{
  XmlRpcValue stats;
  stats[0] = name_;
  XmlRpcValue conn_data;
  conn_data.setSize(0);

  boost::mutex::scoped_lock lock(publisher_links_mutex_);

  uint32_t cidx = 0;
  for (V_PublisherLink::iterator c = publisher_links_.begin();
       c != publisher_links_.end(); ++c)
  {
    const PublisherLink::Stats& s = (*c)->getStats();
    conn_data[cidx][0] = (*c)->getConnectionID();
    conn_data[cidx][1] = (int)s.bytes_received_;
    conn_data[cidx][2] = (int)s.messages_received_;
    conn_data[cidx][3] = (int)s.drops_;
    conn_data[cidx][4] = 0; // figure out something for this. not sure.
  }

  stats[1] = conn_data;
  return stats;
}

// [(connection_id, publisher_xmlrpc_uri, direction, transport, topic_name, connected, connection_info_string)*]
// e.g. [(1, 'http://host:54893/', 'i', 'TCPROS', '/chatter', 1, 'TCPROS connection on port 59746 to [host:34318 on socket 11]')]
void Subscription::getInfo(XmlRpc::XmlRpcValue& info)
{
  boost::mutex::scoped_lock lock(publisher_links_mutex_);

  for (V_PublisherLink::iterator c = publisher_links_.begin();
       c != publisher_links_.end(); ++c)
  {
    XmlRpcValue curr_info;
    curr_info[0] = (int)(*c)->getConnectionID();
    curr_info[1] = (*c)->getPublisherXMLRPCURI();
    curr_info[2] = "i";
    curr_info[3] = (*c)->getTransportType();
    curr_info[4] = name_;
    curr_info[5] = true; // For length compatibility with rospy
    curr_info[6] = (*c)->getTransportInfo();
    info[info.size()] = curr_info;
  }
}

uint32_t Subscription::getNumPublishers()
{
        boost::mutex::scoped_lock lock(publisher_links_mutex_);
        return (uint32_t)publisher_links_.size();
}

void Subscription::drop()
{
  if (!dropped_)
  {
    dropped_ = true;

    dropAllConnections();
  }
}

void Subscription::dropAllConnections()
{
  // Swap our subscribers list with a local one so we can only lock for a short period of time, because a
  // side effect of our calling drop() on connections can be re-locking the subscribers mutex
  V_PublisherLink localsubscribers;

  {
    boost::mutex::scoped_lock lock(publisher_links_mutex_);

    localsubscribers.swap(publisher_links_);
  }

  V_PublisherLink::iterator it = localsubscribers.begin();
  V_PublisherLink::iterator end = localsubscribers.end();
  for (;it != end; ++it)
  {
    (*it)->drop();
  }
}

void Subscription::addLocalConnection(const PublicationPtr& pub)
{
  boost::mutex::scoped_lock lock(publisher_links_mutex_);
  if (dropped_)
  {
    return;
  }

  ROSCPP_LOG_DEBUG("Creating intraprocess link for topic [%s]", name_.c_str());

  IntraProcessPublisherLinkPtr pub_link(boost::make_shared<IntraProcessPublisherLink>(shared_from_this(), XMLRPCManager::instance()->getServerURI(), transport_hints_));
  IntraProcessSubscriberLinkPtr sub_link(boost::make_shared<IntraProcessSubscriberLink>(pub));
  pub_link->setPublisher(sub_link);
  sub_link->setSubscriber(pub_link);

  addPublisherLink(pub_link);
  pub->addSubscriberLink(sub_link);
}

bool urisEqual(const std::string& uri1, const std::string& uri2)
{
  std::string host1, host2;
  uint32_t port1 = 0, port2 = 0;
  network::splitURI(uri1, host1, port1);
  network::splitURI(uri2, host2, port2);
  return port1 == port2 && host1 == host2;
}

bool Subscription::pubUpdate(const V_string& new_pubs)
{
  boost::mutex::scoped_lock lock(shutdown_mutex_);

  if (shutting_down_ || dropped_)
  {
    return false;
  }

  bool retval = true;

  {
    std::stringstream ss;

    for (V_string::const_iterator up_i = new_pubs.begin();
         up_i != new_pubs.end(); ++up_i)
    {
      ss << *up_i << ", ";
    }

    ss << " already have these connections: ";
    for (V_PublisherLink::iterator spc = publisher_links_.begin();
         spc!= publisher_links_.end(); ++spc)
    {
      ss << (*spc)->getPublisherXMLRPCURI() << ", ";
    }

    boost::mutex::scoped_lock lock(pending_connections_mutex_);
    S_PendingConnection::iterator it = pending_connections_.begin();
    S_PendingConnection::iterator end = pending_connections_.end();
    for (; it != end; ++it)
    {
      ss << (*it)->getRemoteURI() << ", ";
    }

    ROSCPP_LOG_DEBUG("Publisher update for [%s]: %s", name_.c_str(), ss.str().c_str());
  }

  V_string additions;
  V_PublisherLink subtractions;
  V_PublisherLink to_add;
  // could use the STL set operations... but these sets are so small
  // it doesn't really matter.
  {
    boost::mutex::scoped_lock lock(publisher_links_mutex_);

    for (V_PublisherLink::iterator spc = publisher_links_.begin();
         spc!= publisher_links_.end(); ++spc)
    {
      bool found = false;
      for (V_string::const_iterator up_i = new_pubs.begin();
           !found && up_i != new_pubs.end(); ++up_i)
      {
        if (urisEqual((*spc)->getPublisherXMLRPCURI(), *up_i))
        {
          found = true;
          break;
        }
      }

      if (!found)
      {
        subtractions.push_back(*spc);
      }
    }

    for (V_string::const_iterator up_i  = new_pubs.begin(); up_i != new_pubs.end(); ++up_i)
    {
      bool found = false;
      for (V_PublisherLink::iterator spc = publisher_links_.begin();
           !found && spc != publisher_links_.end(); ++spc)
      {
        if (urisEqual(*up_i, (*spc)->getPublisherXMLRPCURI()))
        {
          found = true;
          break;
        }
      }

      if (!found)
      {
        boost::mutex::scoped_lock lock(pending_connections_mutex_);
        S_PendingConnection::iterator it = pending_connections_.begin();
        S_PendingConnection::iterator end = pending_connections_.end();
        for (; it != end; ++it)
        {
          if (urisEqual(*up_i, (*it)->getRemoteURI()))
          {
            found = true;
            break;
          }
        }
      }

      if (!found)
      {
        additions.push_back(*up_i);
      }
    }
  }

  for (V_PublisherLink::iterator i = subtractions.begin(); i != subtractions.end(); ++i)
  {
        const PublisherLinkPtr& link = *i;
    if (link->getPublisherXMLRPCURI() != XMLRPCManager::instance()->getServerURI())
    {
      ROSCPP_LOG_DEBUG("Disconnecting from publisher [%s] of topic [%s] at [%s]",
                        link->getCallerID().c_str(), name_.c_str(), link->getPublisherXMLRPCURI().c_str());
                  link->drop();
          }
          else
          {
                  ROSCPP_LOG_DEBUG("Disconnect: skipping myself for topic [%s]", name_.c_str());
          }
        }

  for (V_string::iterator i = additions.begin();
            i != additions.end(); ++i)
  {
    // this function should never negotiate a self-subscription
    if (XMLRPCManager::instance()->getServerURI() != *i)
    {
      retval &= negotiateConnection(*i);
    }
    else
    {
      ROSCPP_LOG_DEBUG("Skipping myself (%s, %s)", name_.c_str(), XMLRPCManager::instance()->getServerURI().c_str());
    }
  }

  return retval;
}

bool Subscription::negotiateConnection(const std::string& xmlrpc_uri)
{
  XmlRpcValue tcpros_array, protos_array, params;
  XmlRpcValue udpros_array;
  TransportUDPPtr udp_transport;
  int protos = 0;
  V_string transports = transport_hints_.getTransports();
  if (transports.empty())
  {
    transport_hints_.reliable();
    transports = transport_hints_.getTransports();
  }
  for (V_string::const_iterator it = transports.begin();
       it != transports.end();
       ++it)
  {
    if (*it == "UDP")
    {
      int max_datagram_size = transport_hints_.getMaxDatagramSize();
      udp_transport = boost::make_shared<TransportUDP>(&PollManager::instance()->getPollSet());
      if (!max_datagram_size)
        max_datagram_size = udp_transport->getMaxDatagramSize();
      udp_transport->createIncoming(0, false);
      udpros_array[0] = "UDPROS";
      M_string m;
      m["topic"] = getName();
      m["md5sum"] = md5sum();
      m["callerid"] = this_node::getName();
      m["type"] = datatype();
      boost::shared_array<uint8_t> buffer;
      uint32_t len;
      Header::write(m, buffer, len);
      XmlRpcValue v(buffer.get(), len);
      udpros_array[1] = v;
      udpros_array[2] = network::getHost();
      udpros_array[3] = udp_transport->getServerPort();
      udpros_array[4] = max_datagram_size;

      protos_array[protos++] = udpros_array;
    }
    else if (*it == "TCP")
    {
      tcpros_array[0] = std::string("TCPROS");
      protos_array[protos++] = tcpros_array;
    }
    else
    {
      ROS_WARN("Unsupported transport type hinted: %s, skipping", it->c_str());
    }
  }
  params[0] = this_node::getName();
  params[1] = name_;
  params[2] = protos_array;
  std::string peer_host;
  uint32_t peer_port;
  if (!network::splitURI(xmlrpc_uri, peer_host, peer_port))
  {
    ROS_ERROR("Bad xml-rpc URI: [%s]", xmlrpc_uri.c_str());
    return false;
  }

  XmlRpc::XmlRpcClient* c = new XmlRpc::XmlRpcClient(peer_host.c_str(),
                                                     peer_port, "/");
 // if (!c.execute("requestTopic", params, result) || !g_node->validateXmlrpcResponse("requestTopic", result, proto))

  // Initiate the negotiation.  We'll come back and check on it later.
  if (!c->executeNonBlock("requestTopic", params))
  {
    ROSCPP_LOG_DEBUG("Failed to contact publisher [%s:%d] for topic [%s]",
              peer_host.c_str(), peer_port, name_.c_str());
    delete c;
    if (udp_transport)
    {
      udp_transport->close();
    }

    return false;
  }

  ROSCPP_LOG_DEBUG("Began asynchronous xmlrpc connection to [%s:%d]", peer_host.c_str(), peer_port);

  // The PendingConnectionPtr takes ownership of c, and will delete it on
  // destruction.
  PendingConnectionPtr conn(boost::make_shared<PendingConnection>(c, udp_transport, shared_from_this(), xmlrpc_uri));

  XMLRPCManager::instance()->addASyncConnection(conn);
  // Put this connection on the list that we'll look at later.
  {
    boost::mutex::scoped_lock pending_connections_lock(pending_connections_mutex_);
    pending_connections_.insert(conn);
  }

  return true;
}

void closeTransport(const TransportUDPPtr& trans)
{
  if (trans)
  {
    trans->close();
  }
}

void Subscription::pendingConnectionDone(const PendingConnectionPtr& conn, XmlRpcValue& result)
{
  boost::mutex::scoped_lock lock(shutdown_mutex_);
  if (shutting_down_ || dropped_)
  {
    return;
  }

  {
    boost::mutex::scoped_lock pending_connections_lock(pending_connections_mutex_);
    pending_connections_.erase(conn);
  }

  TransportUDPPtr udp_transport;

  std::string peer_host = conn->getClient()->getHost();
  uint32_t peer_port = conn->getClient()->getPort();
  std::stringstream ss;
  ss << "http://" << peer_host << ":" << peer_port << "/";
  std::string xmlrpc_uri = ss.str();
  udp_transport = conn->getUDPTransport();

  XmlRpc::XmlRpcValue proto;
  if(!XMLRPCManager::instance()->validateXmlrpcResponse("requestTopic", result, proto))
  {
        ROSCPP_LOG_DEBUG("Failed to contact publisher [%s:%d] for topic [%s]",
              peer_host.c_str(), peer_port, name_.c_str());
        closeTransport(udp_transport);
        return;
  }

  if (proto.size() == 0)
  {
        ROSCPP_LOG_DEBUG("Couldn't agree on any common protocols with [%s] for topic [%s]", xmlrpc_uri.c_str(), name_.c_str());
        closeTransport(udp_transport);
        return;
  }

  if (proto.getType() != XmlRpcValue::TypeArray)
  {
        ROSCPP_LOG_DEBUG("Available protocol info returned from %s is not a list.", xmlrpc_uri.c_str());
        closeTransport(udp_transport);
        return;
  }
  if (proto[0].getType() != XmlRpcValue::TypeString)
  {
        ROSCPP_LOG_DEBUG("Available protocol info list doesn't have a string as its first element.");
        closeTransport(udp_transport);
        return;
  }

  std::string proto_name = proto[0];
  if (proto_name == "TCPROS")
  {
    if (proto.size() != 3 ||
        proto[1].getType() != XmlRpcValue::TypeString ||
        proto[2].getType() != XmlRpcValue::TypeInt)
    {
        ROSCPP_LOG_DEBUG("publisher implements TCPROS, but the " \
                "parameters aren't string,int");
      return;
    }
    std::string pub_host = proto[1];
    int pub_port = proto[2];
    ROSCPP_LOG_DEBUG("Connecting via tcpros to topic [%s] at host [%s:%d]", name_.c_str(), pub_host.c_str(), pub_port);

    TransportTCPPtr transport(boost::make_shared<TransportTCP>(&PollManager::instance()->getPollSet()));
    if (transport->connect(pub_host, pub_port))
    {
      ConnectionPtr connection(boost::make_shared<Connection>());
      TransportPublisherLinkPtr pub_link(boost::make_shared<TransportPublisherLink>(shared_from_this(), xmlrpc_uri, transport_hints_));

      connection->initialize(transport, false, HeaderReceivedFunc());
      pub_link->initialize(connection);

      ConnectionManager::instance()->addConnection(connection);

      boost::mutex::scoped_lock lock(publisher_links_mutex_);
      addPublisherLink(pub_link);

      ROSCPP_LOG_DEBUG("Connected to publisher of topic [%s] at [%s:%d]", name_.c_str(), pub_host.c_str(), pub_port);
    }
    else
    {
        ROSCPP_LOG_DEBUG("Failed to connect to publisher of topic [%s] at [%s:%d]", name_.c_str(), pub_host.c_str(), pub_port);
    }
  }
  else if (proto_name == "UDPROS")
  {
    if (proto.size() != 6 ||
        proto[1].getType() != XmlRpcValue::TypeString ||
        proto[2].getType() != XmlRpcValue::TypeInt ||
        proto[3].getType() != XmlRpcValue::TypeInt ||
        proto[4].getType() != XmlRpcValue::TypeInt ||
        proto[5].getType() != XmlRpcValue::TypeBase64)
    {
      ROSCPP_LOG_DEBUG("publisher implements UDPROS, but the " \
                       "parameters aren't string,int,int,int,base64");
      closeTransport(udp_transport);
      return;
    }
    std::string pub_host = proto[1];
    int pub_port = proto[2];
    int conn_id = proto[3];
    int max_datagram_size = proto[4];
    std::vector<char> header_bytes = proto[5];
    boost::shared_array<uint8_t> buffer(new uint8_t[header_bytes.size()]);
    memcpy(buffer.get(), &header_bytes[0], header_bytes.size());
    Header h;
    std::string err;
    if (!h.parse(buffer, header_bytes.size(), err))
    {
      ROSCPP_LOG_DEBUG("Unable to parse UDPROS connection header: %s", err.c_str());
      closeTransport(udp_transport);
      return;
    }
    ROSCPP_LOG_DEBUG("Connecting via udpros to topic [%s] at host [%s:%d] connection id [%08x] max_datagram_size [%d]", name_.c_str(), pub_host.c_str(), pub_port, conn_id, max_datagram_size);

    std::string error_msg;
    if (h.getValue("error", error_msg))
    {
      ROSCPP_LOG_DEBUG("Received error message in header for connection to [%s]: [%s]", xmlrpc_uri.c_str(), error_msg.c_str());
      closeTransport(udp_transport);
      return;
    }

    TransportPublisherLinkPtr pub_link(boost::make_shared<TransportPublisherLink>(shared_from_this(), xmlrpc_uri, transport_hints_));
    if (pub_link->setHeader(h))
    {
      ConnectionPtr connection(boost::make_shared<Connection>());
      connection->initialize(udp_transport, false, NULL);
      connection->setHeader(h);
      pub_link->initialize(connection);

      ConnectionManager::instance()->addConnection(connection);

      boost::mutex::scoped_lock lock(publisher_links_mutex_);
      addPublisherLink(pub_link);

      ROSCPP_LOG_DEBUG("Connected to publisher of topic [%s] at [%s:%d]", name_.c_str(), pub_host.c_str(), pub_port);
    }
    else
    {
      ROSCPP_LOG_DEBUG("Failed to connect to publisher of topic [%s] at [%s:%d]", name_.c_str(), pub_host.c_str(), pub_port);
      closeTransport(udp_transport);
      return;
    }
  }
  else
  {
        ROSCPP_LOG_DEBUG("Publisher offered unsupported transport [%s]", proto_name.c_str());
  }
}

uint32_t Subscription::handleMessage(const SerializedMessage& m, bool ser, bool nocopy, const boost::shared_ptr<M_string>& connection_header, const PublisherLinkPtr& link)
{
  boost::mutex::scoped_lock lock(callbacks_mutex_);

  uint32_t drops = 0;

  // Cache the deserializers by type info.  If all the subscriptions are the same type this has the same performance as before.  If
  // there are subscriptions with different C++ type (but same ROS message type), this now works correctly rather than passing
  // garbage to the messages with different C++ types than the first one.
  cached_deserializers_.clear();

  ros::Time receipt_time = ros::Time::now();

  for (V_CallbackInfo::iterator cb = callbacks_.begin();
       cb != callbacks_.end(); ++cb)
  {
    const CallbackInfoPtr& info = *cb;

    ROS_ASSERT(info->callback_queue_);

    const std::type_info* ti = &info->helper_->getTypeInfo();

    if ((nocopy && m.type_info && *ti == *m.type_info) || (ser && (!m.type_info || *ti != *m.type_info)))
    {
      MessageDeserializerPtr deserializer;

      V_TypeAndDeserializer::iterator des_it = cached_deserializers_.begin();
      V_TypeAndDeserializer::iterator des_end = cached_deserializers_.end();
      for (; des_it != des_end; ++des_it)
      {
        if (*des_it->first == *ti)
        {
          deserializer = des_it->second;
          break;
        }
      }

      if (!deserializer)
      {
        deserializer = boost::make_shared<MessageDeserializer>(info->helper_, m, connection_header);
        cached_deserializers_.push_back(std::make_pair(ti, deserializer));
      }

      bool was_full = false;
      bool nonconst_need_copy = false;
      if (callbacks_.size() > 1)
      {
        nonconst_need_copy = true;
      }

      info->subscription_queue_->push(info->helper_, deserializer, info->has_tracked_object_, info->tracked_object_, nonconst_need_copy, receipt_time, &was_full);

      if (was_full)
      {
        ++drops;
      }
      else
      {
        info->callback_queue_->addCallback(info->subscription_queue_, (uint64_t)info.get());
      }
    }
  }

  // measure statistics
  statistics_.callback(connection_header, name_, link->getCallerID(), m, link->getStats().bytes_received_, receipt_time, drops > 0);

  // If this link is latched, store off the message so we can immediately pass it to new subscribers later
  if (link->isLatched())
  {
    LatchInfo li;
    li.connection_header = connection_header;
    li.link = link;
    li.message = m;
    li.receipt_time = receipt_time;
    latched_messages_[link] = li;
  }

  cached_deserializers_.clear();

  return drops;
}

bool Subscription::addCallback(const SubscriptionCallbackHelperPtr& helper, const std::string& md5sum, CallbackQueueInterface* queue, int32_t queue_size, const VoidConstPtr& tracked_object, bool allow_concurrent_callbacks)
{
  ROS_ASSERT(helper);
  ROS_ASSERT(queue);

  statistics_.init(helper);

  // Decay to a real type as soon as we have a subscriber with a real type
  {
    boost::mutex::scoped_lock lock(md5sum_mutex_);
    if (md5sum_ == "*" && md5sum != "*")
    {

      md5sum_ = md5sum;
    }
  }

  if (md5sum != "*" && md5sum != this->md5sum())
  {
    return false;
  }

  {
    boost::mutex::scoped_lock lock(callbacks_mutex_);

    CallbackInfoPtr info(boost::make_shared<CallbackInfo>());
    info->helper_ = helper;
    info->callback_queue_ = queue;
    info->subscription_queue_ = boost::make_shared<SubscriptionQueue>(name_, queue_size, allow_concurrent_callbacks);
    info->tracked_object_ = tracked_object;
    info->has_tracked_object_ = false;
    if (tracked_object)
    {
      info->has_tracked_object_ = true;
    }

    if (!helper->isConst())
    {
      ++nonconst_callbacks_;
    }

    callbacks_.push_back(info);
    cached_deserializers_.reserve(callbacks_.size());

    // if we have any latched links, we need to immediately schedule callbacks
    if (!latched_messages_.empty())
    {
      boost::mutex::scoped_lock lock(publisher_links_mutex_);

      V_PublisherLink::iterator it = publisher_links_.begin();
      V_PublisherLink::iterator end = publisher_links_.end();
      for (; it != end;++it)
      {
        const PublisherLinkPtr& link = *it;
        if (link->isLatched())
        {
          M_PublisherLinkToLatchInfo::iterator des_it = latched_messages_.find(link);
          if (des_it != latched_messages_.end())
          {
            const LatchInfo& latch_info = des_it->second;

            MessageDeserializerPtr des(boost::make_shared<MessageDeserializer>(helper, latch_info.message, latch_info.connection_header));
            bool was_full = false;
            info->subscription_queue_->push(info->helper_, des, info->has_tracked_object_, info->tracked_object_, true, latch_info.receipt_time, &was_full);
            if (!was_full)
            {
              info->callback_queue_->addCallback(info->subscription_queue_, (uint64_t)info.get());
            }
          }
        }
      }
    }
  }

  return true;
}

void Subscription::removeCallback(const SubscriptionCallbackHelperPtr& helper)
{
  CallbackInfoPtr info;
  {
    boost::mutex::scoped_lock cbs_lock(callbacks_mutex_);
    for (V_CallbackInfo::iterator it = callbacks_.begin();
         it != callbacks_.end(); ++it)
    {
      if ((*it)->helper_ == helper)
      {
        info = *it;
        callbacks_.erase(it);

        if (!helper->isConst())
        {
          --nonconst_callbacks_;
        }

        break;
      }
    }
  }

  if (info)
  {
    info->subscription_queue_->clear();
    info->callback_queue_->removeByID((uint64_t)info.get());
  }
}

void Subscription::headerReceived(const PublisherLinkPtr& link, const Header& h)
{
  (void)h;
  boost::mutex::scoped_lock lock(md5sum_mutex_);
  if (md5sum_ == "*")
  {
    md5sum_ = link->getMD5Sum();
  }
}

void Subscription::addPublisherLink(const PublisherLinkPtr& link)
{
  publisher_links_.push_back(link);
}

void Subscription::removePublisherLink(const PublisherLinkPtr& pub_link)
{
  boost::mutex::scoped_lock lock(publisher_links_mutex_);

  V_PublisherLink::iterator it = std::find(publisher_links_.begin(), publisher_links_.end(), pub_link);
  if (it != publisher_links_.end())
  {
    publisher_links_.erase(it);
  }

  if (pub_link->isLatched())
  {
    latched_messages_.erase(pub_link);
  }
}

void Subscription::getPublishTypes(bool& ser, bool& nocopy, const std::type_info& ti)
{
  boost::mutex::scoped_lock lock(callbacks_mutex_);
  for (V_CallbackInfo::iterator cb = callbacks_.begin();
       cb != callbacks_.end(); ++cb)
  {
    const CallbackInfoPtr& info = *cb;
    if (info->helper_->getTypeInfo() == ti)
    {
      nocopy = true;
    }
    else
    {
      ser = true;
    }

    if (nocopy && ser)
    {
      return;
    }
  }
}

const std::string Subscription::datatype()
{
  return datatype_;
}

const std::string Subscription::md5sum()
{
  boost::mutex::scoped_lock lock(md5sum_mutex_);
  return md5sum_;
}

}