v01

thirdparty/ros/ros_comm/clients/roscpp/src/libros/callback_queue.cpp

@@ -0,0 +1,429 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, 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.
+ */
+
+// Make sure we use CLOCK_MONOTONIC for the condition variable wait_for if not Apple.
+#ifndef __APPLE__
+#define BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
+#endif
+
+#include "ros/callback_queue.h"
+#include "ros/assert.h"
+#include <boost/scope_exit.hpp>
+
+// check if we have really included the backported boost condition variable
+// just in case someone messes with the include order...
+#if BOOST_VERSION < 106100
+#ifndef USING_BACKPORTED_BOOST_CONDITION_VARIABLE
+#error "needs boost version >= 1.61 or the backported headers!"
+#endif
+#endif
+
+namespace ros
+{
+
+CallbackQueue::CallbackQueue(bool enabled)
+: calling_(0)
+, enabled_(enabled)
+{
+}
+
+CallbackQueue::~CallbackQueue()
+{
+  disable();
+}
+
+void CallbackQueue::enable()
+{
+  boost::mutex::scoped_lock lock(mutex_);
+  enabled_ = true;
+
+  condition_.notify_all();
+}
+
+void CallbackQueue::disable()
+{
+  boost::mutex::scoped_lock lock(mutex_);
+  enabled_ = false;
+
+  condition_.notify_all();
+}
+
+void CallbackQueue::clear()
+{
+  boost::mutex::scoped_lock lock(mutex_);
+
+  callbacks_.clear();
+}
+
+bool CallbackQueue::isEmpty()
+{
+  boost::mutex::scoped_lock lock(mutex_);
+
+  return callbacks_.empty() && calling_ == 0;
+}
+
+bool CallbackQueue::isEnabled()
+{
+  boost::mutex::scoped_lock lock(mutex_);
+
+  return enabled_;
+}
+
+void CallbackQueue::setupTLS()
+{
+  if (!tls_.get())
+  {
+    tls_.reset(new TLS);
+  }
+}
+
+void CallbackQueue::addCallback(const CallbackInterfacePtr& callback, uint64_t removal_id)
+{
+  CallbackInfo info;
+  info.callback = callback;
+  info.removal_id = removal_id;
+
+  {
+    boost::mutex::scoped_lock lock(id_info_mutex_);
+
+    M_IDInfo::iterator it = id_info_.find(removal_id);
+    if (it == id_info_.end())
+    {
+      IDInfoPtr id_info(boost::make_shared<IDInfo>());
+      id_info->id = removal_id;
+      id_info_.insert(std::make_pair(removal_id, id_info));
+    }
+  }
+
+  {
+    boost::mutex::scoped_lock lock(mutex_);
+
+    if (!enabled_)
+    {
+      return;
+    }
+
+    callbacks_.push_back(info);
+  }
+
+  condition_.notify_one();
+}
+
+CallbackQueue::IDInfoPtr CallbackQueue::getIDInfo(uint64_t id)
+{
+  boost::mutex::scoped_lock lock(id_info_mutex_);
+  M_IDInfo::iterator it = id_info_.find(id);
+  if (it != id_info_.end())
+  {
+    return it->second;
+  }
+
+  return IDInfoPtr();
+}
+
+void CallbackQueue::removeByID(uint64_t removal_id)
+{
+  setupTLS();
+
+  {
+    IDInfoPtr id_info;
+    {
+      boost::mutex::scoped_lock lock(id_info_mutex_);
+      M_IDInfo::iterator it = id_info_.find(removal_id);
+      if (it != id_info_.end())
+      {
+        id_info = it->second;
+      }
+      else
+      {
+        return;
+      }
+    }
+
+    // If we're being called from within a callback from our queue, we must unlock the shared lock we already own
+    // here so that we can take a unique lock.  We'll re-lock it later.
+    if (tls_->calling_in_this_thread == id_info->id)
+    {
+      id_info->calling_rw_mutex.unlock_shared();
+    }
+
+    {
+      boost::unique_lock<boost::shared_mutex> rw_lock(id_info->calling_rw_mutex);
+      boost::mutex::scoped_lock lock(mutex_);
+      D_CallbackInfo::iterator it = callbacks_.begin();
+      for (; it != callbacks_.end();)
+      {
+        CallbackInfo& info = *it;
+        if (info.removal_id == removal_id)
+        {
+          it = callbacks_.erase(it);
+        }
+        else
+        {
+          ++it;
+        }
+      }
+    }
+
+    if (tls_->calling_in_this_thread == id_info->id)
+    {
+      id_info->calling_rw_mutex.lock_shared();
+    }
+  }
+
+  // If we're being called from within a callback, we need to remove the callbacks that match the id that have already been
+  // popped off the queue
+  {
+    D_CallbackInfo::iterator it = tls_->callbacks.begin();
+    D_CallbackInfo::iterator end = tls_->callbacks.end();
+    for (; it != end; ++it)
+    {
+      CallbackInfo& info = *it;
+      if (info.removal_id == removal_id)
+      {
+        info.marked_for_removal = true;
+      }
+    }
+  }
+
+  {
+    boost::mutex::scoped_lock lock(id_info_mutex_);
+    id_info_.erase(removal_id);
+  }
+}
+
+CallbackQueue::CallOneResult CallbackQueue::callOne(ros::WallDuration timeout)
+{
+  setupTLS();
+  TLS* tls = tls_.get();
+
+  CallbackInfo cb_info;
+
+  {
+    boost::mutex::scoped_lock lock(mutex_);
+
+    if (!enabled_)
+    {
+      return Disabled;
+    }
+
+    if (callbacks_.empty())
+    {
+      if (!timeout.isZero())
+      {
+        condition_.wait_for(lock, boost::chrono::nanoseconds(timeout.toNSec()));
+      }
+
+      if (callbacks_.empty())
+      {
+        return Empty;
+      }
+
+      if (!enabled_)
+      {
+        return Disabled;
+      }
+    }
+
+    D_CallbackInfo::iterator it = callbacks_.begin();
+    for (; it != callbacks_.end();)
+    {
+      CallbackInfo& info = *it;
+
+      if (info.marked_for_removal)
+      {
+        it = callbacks_.erase(it);
+        continue;
+      }
+
+      if (info.callback->ready())
+      {
+        cb_info = info;
+        it = callbacks_.erase(it);
+        break;
+      }
+
+      ++it;
+    }
+
+    if (!cb_info.callback)
+    {
+      return TryAgain;
+    }
+
+    ++calling_;
+  }
+
+  bool was_empty = tls->callbacks.empty();
+  tls->callbacks.push_back(cb_info);
+  if (was_empty)
+  {
+    tls->cb_it = tls->callbacks.begin();
+  }
+
+  CallOneResult res = callOneCB(tls);
+  if (res != Empty)
+  {
+    boost::mutex::scoped_lock lock(mutex_);
+    --calling_;
+  }
+  return res;
+}
+
+void CallbackQueue::callAvailable(ros::WallDuration timeout)
+{
+  setupTLS();
+  TLS* tls = tls_.get();
+
+  {
+    boost::mutex::scoped_lock lock(mutex_);
+
+    if (!enabled_)
+    {
+      return;
+    }
+
+    if (callbacks_.empty())
+    {
+      if (!timeout.isZero())
+      {
+        condition_.wait_for(lock, boost::chrono::nanoseconds(timeout.toNSec()));
+      }
+
+      if (callbacks_.empty() || !enabled_)
+      {
+        return;
+      }
+    }
+
+    bool was_empty = tls->callbacks.empty();
+
+    tls->callbacks.insert(tls->callbacks.end(), callbacks_.begin(), callbacks_.end());
+    callbacks_.clear();
+
+    calling_ += tls->callbacks.size();
+
+    if (was_empty)
+    {
+      tls->cb_it = tls->callbacks.begin();
+    }
+  }
+
+  size_t called = 0;
+
+  while (!tls->callbacks.empty())
+  {
+    if (callOneCB(tls) != Empty)
+    {
+      ++called;
+    }
+  }
+
+  {
+    boost::mutex::scoped_lock lock(mutex_);
+    calling_ -= called;
+  }
+}
+
+CallbackQueue::CallOneResult CallbackQueue::callOneCB(TLS* tls)
+{
+  // Check for a recursive call.  If recursive, increment the current iterator.  Otherwise
+  // set the iterator it the beginning of the thread-local callbacks
+  if (tls->calling_in_this_thread == 0xffffffffffffffffULL)
+  {
+    tls->cb_it = tls->callbacks.begin();
+  }
+
+  if (tls->cb_it == tls->callbacks.end())
+  {
+    return Empty;
+  }
+
+  ROS_ASSERT(!tls->callbacks.empty());
+  ROS_ASSERT(tls->cb_it != tls->callbacks.end());
+
+  CallbackInfo info = *tls->cb_it;
+  CallbackInterfacePtr& cb = info.callback;
+
+  IDInfoPtr id_info = getIDInfo(info.removal_id);
+  if (id_info)
+  {
+    boost::shared_lock<boost::shared_mutex> rw_lock(id_info->calling_rw_mutex);
+
+    uint64_t last_calling = tls->calling_in_this_thread;
+    tls->calling_in_this_thread = id_info->id;
+
+    CallbackInterface::CallResult result = CallbackInterface::Invalid;
+
+    {
+      // Ensure that thread id gets restored, even if callback throws.
+      // This is done with RAII rather than try-catch so that the source
+      // of the original exception is not masked in a crash report.
+      BOOST_SCOPE_EXIT(&tls, &last_calling)
+      {
+        tls->calling_in_this_thread = last_calling;
+      }
+      BOOST_SCOPE_EXIT_END
+
+      if (info.marked_for_removal)
+      {
+        tls->cb_it = tls->callbacks.erase(tls->cb_it);
+      }
+      else
+      {
+        tls->cb_it = tls->callbacks.erase(tls->cb_it);
+        result = cb->call();
+      }
+    }
+
+    // Push TryAgain callbacks to the back of the shared queue
+    if (result == CallbackInterface::TryAgain && !info.marked_for_removal)
+    {
+      boost::mutex::scoped_lock lock(mutex_);
+      callbacks_.push_back(info);
+
+      return TryAgain;
+    }
+
+    return Called;
+  }
+  else
+  {
+    tls->cb_it = tls->callbacks.erase(tls->cb_it);
+  }
+
+  return Called;
+}
+
+}

thirdparty/ros/ros_comm/clients/roscpp/src/libros/common.cpp

@@ -0,0 +1,61 @@
+/*
+ * 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 "ros/common.h"
+#include <cstdlib>
+#include <cstdio>
+#include <cerrno>
+#include <cassert>
+#include <sys/types.h>
+#if !defined(WIN32)
+#include <unistd.h>
+#include <pthread.h>
+#endif
+#include <signal.h>
+
+using std::string;
+
+void ros::disableAllSignalsInThisThread()
+{
+#if !defined(WIN32)
+  // pthreads_win32, despite having an implementation of pthread_sigmask,
+  // doesn't have an implementation of sigset_t, and also doesn't expose its
+  // pthread_sigmask externally.
+  sigset_t signal_set;
+
+  /* block all signals */
+  sigfillset( &signal_set );
+  pthread_sigmask( SIG_BLOCK, &signal_set, NULL );
+#endif
+}

thirdparty/ros/ros_comm/clients/roscpp/src/libros/config.h.in

@@ -0,0 +1,3 @@
+#cmakedefine HAVE_TRUNC
+#cmakedefine HAVE_IFADDRS_H
+#cmakedefine HAVE_EPOLL

thirdparty/ros/ros_comm/clients/roscpp/src/libros/connection.cpp

@@ -0,0 +1,479 @@
+/*
+ * 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 "ros/connection.h"
+#include "ros/transport/transport.h"
+#include "ros/file_log.h"
+
+#include <ros/assert.h>
+
+#include <boost/shared_array.hpp>
+#include <boost/bind.hpp>
+
+namespace ros
+{
+
+Connection::Connection()
+: is_server_(false)
+, dropped_(false)
+, read_filled_(0)
+, read_size_(0)
+, reading_(false)
+, has_read_callback_(0)
+, write_sent_(0)
+, write_size_(0)
+, writing_(false)
+, has_write_callback_(0)
+, sending_header_error_(false)
+{
+}
+
+Connection::~Connection()
+{
+  ROS_DEBUG_NAMED("superdebug", "Connection destructing, dropped=%s", dropped_ ? "true" : "false");
+
+  drop(Destructing);
+}
+
+void Connection::initialize(const TransportPtr& transport, bool is_server, const HeaderReceivedFunc& header_func)
+{
+  ROS_ASSERT(transport);
+
+  transport_ = transport;
+  header_func_ = header_func;
+  is_server_ = is_server;
+
+  transport_->setReadCallback(boost::bind(&Connection::onReadable, this, _1));
+  transport_->setWriteCallback(boost::bind(&Connection::onWriteable, this, _1));
+  transport_->setDisconnectCallback(boost::bind(&Connection::onDisconnect, this, _1));
+
+  if (header_func)
+  {
+    read(4, boost::bind(&Connection::onHeaderLengthRead, this, _1, _2, _3, _4));
+  }
+}
+
+boost::signals2::connection Connection::addDropListener(const DropFunc& slot)
+{
+  boost::recursive_mutex::scoped_lock lock(drop_mutex_);
+  return drop_signal_.connect(slot);
+}
+
+void Connection::removeDropListener(const boost::signals2::connection& c)
+{
+  boost::recursive_mutex::scoped_lock lock(drop_mutex_);
+  c.disconnect();
+}
+
+void Connection::onReadable(const TransportPtr& transport)
+{
+  (void)transport;
+  ROS_ASSERT(transport == transport_);
+
+  readTransport();
+}
+
+void Connection::readTransport()
+{
+  boost::recursive_mutex::scoped_try_lock lock(read_mutex_);
+
+  if (!lock.owns_lock() || dropped_ || reading_)
+  {
+    return;
+  }
+
+  reading_ = true;
+
+  while (!dropped_ && has_read_callback_)
+  {
+    ROS_ASSERT(read_buffer_);
+    uint32_t to_read = read_size_ - read_filled_;
+    if (to_read > 0)
+    {
+      int32_t bytes_read = transport_->read(read_buffer_.get() + read_filled_, to_read);
+      ROS_DEBUG_NAMED("superdebug", "Connection read %d bytes", bytes_read);
+      if (dropped_)
+      {
+        return;
+      }
+      else if (bytes_read < 0)
+      {
+        // Bad read, throw away results and report error
+        ReadFinishedFunc callback;
+        callback = read_callback_;
+        read_callback_.clear();
+        read_buffer_.reset();
+        uint32_t size = read_size_;
+        read_size_ = 0;
+        read_filled_ = 0;
+        has_read_callback_ = 0;
+
+        if (callback)
+        {
+          callback(shared_from_this(), read_buffer_, size, false);
+        }
+
+        break;
+      }
+
+      read_filled_ += bytes_read;
+    }
+
+    ROS_ASSERT((int32_t)read_size_ >= 0);
+    ROS_ASSERT((int32_t)read_filled_ >= 0);
+    ROS_ASSERT_MSG(read_filled_ <= read_size_, "read_filled_ = %d, read_size_ = %d", read_filled_, read_size_);
+
+    if (read_filled_ == read_size_ && !dropped_)
+    {
+      ReadFinishedFunc callback;
+      uint32_t size;
+      boost::shared_array<uint8_t> buffer;
+
+      ROS_ASSERT(has_read_callback_);
+
+      // store off the read info in case another read() call is made from within the callback
+      callback = read_callback_;
+      size = read_size_;
+      buffer = read_buffer_;
+      read_callback_.clear();
+      read_buffer_.reset();
+      read_size_ = 0;
+      read_filled_ = 0;
+      has_read_callback_ = 0;
+
+      ROS_DEBUG_NAMED("superdebug", "Calling read callback");
+      callback(shared_from_this(), buffer, size, true);
+    }
+    else
+    {
+      break;
+    }
+  }
+
+  if (!has_read_callback_)
+  {
+    transport_->disableRead();
+  }
+
+  reading_ = false;
+}
+
+void Connection::writeTransport()
+{
+  boost::recursive_mutex::scoped_try_lock lock(write_mutex_);
+
+  if (!lock.owns_lock() || dropped_ || writing_)
+  {
+    return;
+  }
+
+  writing_ = true;
+  bool can_write_more = true;
+
+  while (has_write_callback_ && can_write_more && !dropped_)
+  {
+    uint32_t to_write = write_size_ - write_sent_;
+    ROS_DEBUG_NAMED("superdebug", "Connection writing %d bytes", to_write);
+    int32_t bytes_sent = transport_->write(write_buffer_.get() + write_sent_, to_write);
+    ROS_DEBUG_NAMED("superdebug", "Connection wrote %d bytes", bytes_sent);
+
+    if (bytes_sent < 0)
+    {
+      writing_ = false;
+      return;
+    }
+
+    write_sent_ += bytes_sent;
+
+    if (bytes_sent < (int)write_size_ - (int)write_sent_)
+    {
+      can_write_more = false;
+    }
+
+    if (write_sent_ == write_size_ && !dropped_)
+    {
+      WriteFinishedFunc callback;
+
+      {
+        boost::mutex::scoped_lock lock(write_callback_mutex_);
+        ROS_ASSERT(has_write_callback_);
+        // Store off a copy of the callback in case another write() call happens in it
+        callback = write_callback_;
+        write_callback_ = WriteFinishedFunc();
+        write_buffer_ = boost::shared_array<uint8_t>();
+        write_sent_ = 0;
+        write_size_ = 0;
+        has_write_callback_ = 0;
+      }
+
+      ROS_DEBUG_NAMED("superdebug", "Calling write callback");
+      callback(shared_from_this());
+    }
+  }
+
+  {
+    boost::mutex::scoped_lock lock(write_callback_mutex_);
+    if (!has_write_callback_)
+    {
+      transport_->disableWrite();
+    }
+  }
+
+  writing_ = false;
+}
+
+void Connection::onWriteable(const TransportPtr& transport)
+{
+  (void)transport;
+  ROS_ASSERT(transport == transport_);
+
+  writeTransport();
+}
+
+void Connection::read(uint32_t size, const ReadFinishedFunc& callback)
+{
+  if (dropped_ || sending_header_error_)
+  {
+    return;
+  }
+
+  {
+    boost::recursive_mutex::scoped_lock lock(read_mutex_);
+
+    ROS_ASSERT(!read_callback_);
+
+    read_callback_ = callback;
+    read_buffer_ = boost::shared_array<uint8_t>(new uint8_t[size]);
+    read_size_ = size;
+    read_filled_ = 0;
+    has_read_callback_ = 1;
+  }
+
+  transport_->enableRead();
+
+  // read immediately if possible
+  readTransport();
+}
+
+void Connection::write(const boost::shared_array<uint8_t>& buffer, uint32_t size, const WriteFinishedFunc& callback, bool immediate)
+{
+  if (dropped_ || sending_header_error_)
+  {
+    return;
+  }
+
+  {
+    boost::mutex::scoped_lock lock(write_callback_mutex_);
+
+    ROS_ASSERT(!write_callback_);
+
+    write_callback_ = callback;
+    write_buffer_ = buffer;
+    write_size_ = size;
+    write_sent_ = 0;
+    has_write_callback_ = 1;
+  }
+
+  transport_->enableWrite();
+
+  if (immediate)
+  {
+    // write immediately if possible
+    writeTransport();
+  }
+}
+
+void Connection::onDisconnect(const TransportPtr& transport)
+{
+  (void)transport;
+  ROS_ASSERT(transport == transport_);
+
+  drop(TransportDisconnect);
+}
+
+void Connection::drop(DropReason reason)
+{
+  ROSCPP_LOG_DEBUG("Connection::drop(%u)", reason);
+  bool did_drop = false;
+  {
+    boost::recursive_mutex::scoped_lock lock(drop_mutex_);
+    if (!dropped_)
+    {
+      dropped_ = true;
+      did_drop = true;
+    }
+  }
+
+  if (did_drop)
+  {
+    drop_signal_(shared_from_this(), reason);
+    transport_->close();
+  }
+}
+
+bool Connection::isDropped()
+{
+  boost::recursive_mutex::scoped_lock lock(drop_mutex_);
+  return dropped_;
+}
+
+void Connection::writeHeader(const M_string& key_vals, const WriteFinishedFunc& finished_callback)
+{
+  ROS_ASSERT(!header_written_callback_);
+  header_written_callback_ = finished_callback;
+
+  if (!transport_->requiresHeader())
+  {
+    onHeaderWritten(shared_from_this());
+    return;
+  }
+
+  boost::shared_array<uint8_t> buffer;
+  uint32_t len;
+  Header::write(key_vals, buffer, len);
+
+  uint32_t msg_len = len + 4;
+  boost::shared_array<uint8_t> full_msg(new uint8_t[msg_len]);
+  memcpy(full_msg.get() + 4, buffer.get(), len);
+  *((uint32_t*)full_msg.get()) = len;
+
+  write(full_msg, msg_len, boost::bind(&Connection::onHeaderWritten, this, _1), false);
+}
+
+void Connection::sendHeaderError(const std::string& error_msg)
+{
+  M_string m;
+  m["error"] = error_msg;
+
+  writeHeader(m, boost::bind(&Connection::onErrorHeaderWritten, this, _1));
+  sending_header_error_ = true;
+}
+
+void Connection::onHeaderLengthRead(const ConnectionPtr& conn, const boost::shared_array<uint8_t>& buffer, uint32_t size, bool success)
+{
+  (void)size;
+  ROS_ASSERT(conn.get() == this);
+  ROS_ASSERT(size == 4);
+
+  if (!success)
+    return;
+
+  uint32_t len = *((uint32_t*)buffer.get());
+
+  if (len > 1000000000)
+  {
+    ROS_ERROR("a header of over a gigabyte was " \
+                "predicted in tcpros. that seems highly " \
+                "unlikely, so I'll assume protocol " \
+                "synchronization is lost.");
+    conn->drop(HeaderError);
+  }
+
+  read(len, boost::bind(&Connection::onHeaderRead, this, _1, _2, _3, _4));
+}
+
+void Connection::onHeaderRead(const ConnectionPtr& conn, const boost::shared_array<uint8_t>& buffer, uint32_t size, bool success)
+{
+  ROS_ASSERT(conn.get() == this);
+
+  if (!success)
+    return;
+
+  std::string error_msg;
+  if (!header_.parse(buffer, size, error_msg))
+  {
+    drop(HeaderError);
+  }
+  else
+  {
+    std::string error_val;
+    if (header_.getValue("error", error_val))
+    {
+      ROSCPP_LOG_DEBUG("Received error message in header for connection to [%s]: [%s]", transport_->getTransportInfo().c_str(), error_val.c_str());
+      drop(HeaderError);
+    }
+    else
+    {
+      ROS_ASSERT(header_func_);
+
+      transport_->parseHeader(header_);
+
+      header_func_(conn, header_);
+    }
+  }
+
+}
+
+void Connection::onHeaderWritten(const ConnectionPtr& conn)
+{
+  ROS_ASSERT(conn.get() == this);
+  ROS_ASSERT(header_written_callback_);
+
+  header_written_callback_(conn);
+  header_written_callback_ = WriteFinishedFunc();
+}
+
+void Connection::onErrorHeaderWritten(const ConnectionPtr& conn)
+{
+  (void)conn;
+  drop(HeaderError);
+}
+
+void Connection::setHeaderReceivedCallback(const HeaderReceivedFunc& func)
+{
+  header_func_ = func;
+
+  if (transport_->requiresHeader())
+    read(4, boost::bind(&Connection::onHeaderLengthRead, this, _1, _2, _3, _4));
+}
+
+std::string Connection::getCallerId()
+{
+  std::string callerid;
+  if (header_.getValue("callerid", callerid))
+  {
+    return callerid;
+  }
+
+  return std::string("unknown");
+}
+
+std::string Connection::getRemoteString()
+{
+  std::stringstream ss;
+  ss << "callerid=[" << getCallerId() << "] address=[" << transport_->getTransportInfo() << "]";
+  return ss.str();
+}
+
+}

thirdparty/ros/ros_comm/clients/roscpp/src/libros/connection_manager.cpp

@@ -0,0 +1,228 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names 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 "ros/connection_manager.h"
+#include "ros/poll_manager.h"
+#include "ros/connection.h"
+#include "ros/transport_subscriber_link.h"
+#include "ros/service_client_link.h"
+#include "ros/transport/transport_tcp.h"
+#include "ros/transport/transport_udp.h"
+#include "ros/file_log.h"
+#include "ros/network.h"
+
+#include <ros/assert.h>
+
+namespace ros
+{
+
+const ConnectionManagerPtr& ConnectionManager::instance()
+{
+  static ConnectionManagerPtr connection_manager = boost::make_shared<ConnectionManager>();
+  return connection_manager;
+}
+
+ConnectionManager::ConnectionManager()
+: connection_id_counter_(0)
+{
+}
+
+ConnectionManager::~ConnectionManager()
+{
+  shutdown();
+}
+
+void ConnectionManager::start()
+{
+  poll_manager_ = PollManager::instance();
+  poll_conn_ = poll_manager_->addPollThreadListener(boost::bind(&ConnectionManager::removeDroppedConnections, 
+								this));
+
+  // Bring up the TCP listener socket
+  tcpserver_transport_ = boost::make_shared<TransportTCP>(&poll_manager_->getPollSet());
+  if (!tcpserver_transport_->listen(network::getTCPROSPort(), 
+				    MAX_TCPROS_CONN_QUEUE, 
+				    boost::bind(&ConnectionManager::tcprosAcceptConnection, this, _1)))
+  {
+    ROS_FATAL("Listen on port [%d] failed", network::getTCPROSPort());
+    ROS_BREAK();
+  }
+
+  // Bring up the UDP listener socket
+  udpserver_transport_ = boost::make_shared<TransportUDP>(&poll_manager_->getPollSet());
+  if (!udpserver_transport_->createIncoming(0, true))
+  {
+    ROS_FATAL("Listen failed");
+    ROS_BREAK();
+  }
+}
+
+void ConnectionManager::shutdown()
+{
+  if (udpserver_transport_)
+  {
+    udpserver_transport_->close();
+    udpserver_transport_.reset();
+  }
+
+  if (tcpserver_transport_)
+  {
+    tcpserver_transport_->close();
+    tcpserver_transport_.reset();
+  }
+
+  poll_manager_->removePollThreadListener(poll_conn_);
+
+  clear(Connection::Destructing);
+}
+
+void ConnectionManager::clear(Connection::DropReason reason)
+{
+  S_Connection local_connections;
+  {
+    boost::mutex::scoped_lock conn_lock(connections_mutex_);
+    local_connections.swap(connections_);
+  }
+
+  for(S_Connection::iterator itr = local_connections.begin();
+      itr != local_connections.end();
+      itr++)
+  {
+    const ConnectionPtr& conn = *itr;
+    conn->drop(reason);
+  }
+
+  boost::mutex::scoped_lock dropped_lock(dropped_connections_mutex_);
+  dropped_connections_.clear();
+}
+
+uint32_t ConnectionManager::getTCPPort()
+{
+  return tcpserver_transport_->getServerPort();
+}
+
+uint32_t ConnectionManager::getUDPPort()
+{
+  return udpserver_transport_->getServerPort();
+}
+
+uint32_t ConnectionManager::getNewConnectionID()
+{
+  boost::mutex::scoped_lock lock(connection_id_counter_mutex_);
+  uint32_t ret = connection_id_counter_++;
+  return ret;
+}
+
+void ConnectionManager::addConnection(const ConnectionPtr& conn)
+{
+  boost::mutex::scoped_lock lock(connections_mutex_);
+
+  connections_.insert(conn);
+  conn->addDropListener(boost::bind(&ConnectionManager::onConnectionDropped, this, _1));
+}
+
+void ConnectionManager::onConnectionDropped(const ConnectionPtr& conn)
+{
+  boost::mutex::scoped_lock lock(dropped_connections_mutex_);
+  dropped_connections_.push_back(conn);
+}
+
+void ConnectionManager::removeDroppedConnections()
+{
+  V_Connection local_dropped;
+  {
+    boost::mutex::scoped_lock dropped_lock(dropped_connections_mutex_);
+    dropped_connections_.swap(local_dropped);
+  }
+
+  boost::mutex::scoped_lock conn_lock(connections_mutex_);
+
+  V_Connection::iterator conn_it = local_dropped.begin();
+  V_Connection::iterator conn_end = local_dropped.end();
+  for (;conn_it != conn_end; ++conn_it)
+  {
+    const ConnectionPtr& conn = *conn_it;
+    connections_.erase(conn);
+  }
+}
+
+void ConnectionManager::udprosIncomingConnection(const TransportUDPPtr& transport, Header& header)
+{
+  std::string client_uri = ""; // TODO: transport->getClientURI();
+  ROSCPP_LOG_DEBUG("UDPROS received a connection from [%s]", client_uri.c_str());
+
+  ConnectionPtr conn(boost::make_shared<Connection>());
+  addConnection(conn);
+
+  conn->initialize(transport, true, NULL);
+  onConnectionHeaderReceived(conn, header);
+}
+
+void ConnectionManager::tcprosAcceptConnection(const TransportTCPPtr& transport)
+{
+  std::string client_uri = transport->getClientURI();
+  ROSCPP_LOG_DEBUG("TCPROS received a connection from [%s]", client_uri.c_str());
+
+  ConnectionPtr conn(boost::make_shared<Connection>());
+  addConnection(conn);
+
+  conn->initialize(transport, true, boost::bind(&ConnectionManager::onConnectionHeaderReceived, this, _1, _2));
+}
+
+bool ConnectionManager::onConnectionHeaderReceived(const ConnectionPtr& conn, const Header& header)
+{
+  bool ret = false;
+  std::string val;
+  if (header.getValue("topic", val))
+  {
+    ROSCPP_CONN_LOG_DEBUG("Connection: Creating TransportSubscriberLink for topic [%s] connected to [%s]", 
+		     val.c_str(), conn->getRemoteString().c_str());
+
+    TransportSubscriberLinkPtr sub_link(boost::make_shared<TransportSubscriberLink>());
+    sub_link->initialize(conn);
+    ret = sub_link->handleHeader(header);
+  }
+  else if (header.getValue("service", val))
+  {
+    ROSCPP_LOG_DEBUG("Connection: Creating ServiceClientLink for service [%s] connected to [%s]", 
+		     val.c_str(), conn->getRemoteString().c_str());
+
+    ServiceClientLinkPtr link(boost::make_shared<ServiceClientLink>());
+    link->initialize(conn);
+    ret = link->handleHeader(header);
+  }
+  else
+  {
+  	ROSCPP_LOG_DEBUG("Got a connection for a type other than 'topic' or 'service' from [%s].  Fail.", 
+			 conn->getRemoteString().c_str());
+    return false;
+  }
+
+  return ret;
+}
+
+}

thirdparty/ros/ros_comm/clients/roscpp/src/libros/file_log.cpp

@@ -0,0 +1,126 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Stanford University or 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 <cstdio>
+#include "ros/file_log.h"
+#include "ros/this_node.h"
+
+#include <ros/io.h>
+#include <ros/console.h>
+
+#include <boost/filesystem.hpp>
+
+#ifdef _MSC_VER
+  #ifdef snprintf
+    #undef snprintf
+  #endif
+  #define snprintf _snprintf_s
+#endif
+
+
+namespace fs = boost::filesystem;
+
+namespace ros
+{
+
+namespace file_log
+{
+
+std::string g_log_directory;
+
+const std::string& getLogDirectory()
+{
+  return g_log_directory;
+}
+
+void init(const M_string& remappings)
+{
+  std::string log_file_name;
+  M_string::const_iterator it = remappings.find("__log");
+  if (it != remappings.end())
+  {
+    log_file_name = it->second;
+  }
+
+  {
+    // Log filename can be specified on the command line through __log
+    // If it's been set, don't create our own name
+    if (log_file_name.empty())
+    {
+      // Setup the logfile appender
+      // Can't do this in rosconsole because the node name is not known
+      pid_t pid = getpid();
+      std::string ros_log_env;
+      if ( get_environment_variable(ros_log_env, "ROS_LOG_DIR"))
+      {
+        log_file_name = ros_log_env + std::string("/");
+      }
+      else
+      {
+        if ( get_environment_variable(ros_log_env, "ROS_HOME"))
+        {
+          log_file_name = ros_log_env + std::string("/log/");
+        }
+        else
+        {
+          // Not cross-platform?
+          if( get_environment_variable(ros_log_env, "HOME") )
+          {
+            std::string dotros = ros_log_env + std::string("/.ros/");
+            fs::create_directory(dotros);
+            log_file_name = dotros + "log/";
+            fs::create_directory(log_file_name);
+          }
+        }
+      }
+
+      // sanitize the node name and tack it to the filename
+      for (size_t i = 1; i < this_node::getName().length(); i++)
+      {
+        if (!isalnum(this_node::getName()[i]))
+        {
+          log_file_name += '_';
+        }
+        else
+        {
+          log_file_name += this_node::getName()[i];
+        }
+      }
+
+      char pid_str[100];
+      snprintf(pid_str, sizeof(pid_str), "%d", pid);
+      log_file_name += std::string("_") + std::string(pid_str) + std::string(".log");
+    }
+
+    log_file_name = fs::system_complete(log_file_name).string();
+    g_log_directory = fs::path(log_file_name).parent_path().string();
+  }
+}
+
+} // namespace file_log
+
+} // namespace ros

thirdparty/ros/ros_comm/clients/roscpp/src/libros/init.cpp

@@ -0,0 +1,612 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, 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 "ros/init.h"
+#include "ros/names.h"
+#include "ros/xmlrpc_manager.h"
+#include "ros/poll_manager.h"
+#include "ros/connection_manager.h"
+#include "ros/topic_manager.h"
+#include "ros/service_manager.h"
+#include "ros/this_node.h"
+#include "ros/network.h"
+#include "ros/file_log.h"
+#include "ros/callback_queue.h"
+#include "ros/param.h"
+#include "ros/rosout_appender.h"
+#include "ros/subscribe_options.h"
+#include "ros/transport/transport_tcp.h"
+#include "ros/internal_timer_manager.h"
+#include "xmlrpcpp/XmlRpcSocket.h"
+
+#include "roscpp/GetLoggers.h"
+#include "roscpp/SetLoggerLevel.h"
+#include "roscpp/Empty.h"
+
+#include <ros/console.h>
+#include <ros/time.h>
+#include <rosgraph_msgs/Clock.h>
+
+#include <algorithm>
+
+#include <signal.h>
+
+#include <cstdlib>
+
+namespace ros
+{
+
+namespace master
+{
+void init(const M_string& remappings);
+}
+
+namespace this_node
+{
+void init(const std::string& names, const M_string& remappings, uint32_t options);
+}
+
+namespace network
+{
+void init(const M_string& remappings);
+}
+
+namespace param
+{
+void init(const M_string& remappings);
+}
+
+namespace file_log
+{
+void init(const M_string& remappings);
+}
+
+CallbackQueuePtr g_global_queue;
+ROSOutAppender* g_rosout_appender;
+static CallbackQueuePtr g_internal_callback_queue;
+
+static bool g_initialized = false;
+static bool g_started = false;
+static bool g_atexit_registered = false;
+static boost::mutex g_start_mutex;
+static bool g_ok = false;
+static uint32_t g_init_options = 0;
+static bool g_shutdown_requested = false;
+static volatile bool g_shutting_down = false;
+static boost::recursive_mutex g_shutting_down_mutex;
+static boost::thread g_internal_queue_thread;
+
+bool isInitialized()
+{
+  return g_initialized;
+}
+
+bool isShuttingDown()
+{
+  return g_shutting_down;
+}
+
+void checkForShutdown()
+{
+  if (g_shutdown_requested)
+  {
+    // Since this gets run from within a mutex inside PollManager, we need to prevent ourselves from deadlocking with
+    // another thread that's already in the middle of shutdown()
+    boost::recursive_mutex::scoped_try_lock lock(g_shutting_down_mutex, boost::defer_lock);
+    while (!lock.try_lock() && !g_shutting_down)
+    {
+      ros::WallDuration(0.001).sleep();
+    }
+
+    if (!g_shutting_down)
+    {
+      shutdown();
+    }
+
+    g_shutdown_requested = false;
+  }
+}
+
+void requestShutdown()
+{
+  g_shutdown_requested = true;
+}
+
+void atexitCallback()
+{
+  if (ok() && !isShuttingDown())
+  {
+    ROSCPP_LOG_DEBUG("shutting down due to exit() or end of main() without cleanup of all NodeHandles");
+    g_started = false; // don't shutdown singletons, because they are already destroyed
+    shutdown();
+  }
+}
+
+void shutdownCallback(XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result)
+{
+  int num_params = 0;
+  if (params.getType() == XmlRpc::XmlRpcValue::TypeArray)
+    num_params = params.size();
+  if (num_params > 1)
+  {
+    std::string reason = params[1];
+    ROS_WARN("Shutdown request received.");
+    ROS_WARN("Reason given for shutdown: [%s]", reason.c_str());
+    requestShutdown();
+  }
+
+  result = xmlrpc::responseInt(1, "", 0);
+}
+
+bool getLoggers(roscpp::GetLoggers::Request&, roscpp::GetLoggers::Response& resp)
+{
+  std::map<std::string, ros::console::levels::Level> loggers;
+  bool success = ::ros::console::get_loggers(loggers);
+  if (success)
+  {
+    for (std::map<std::string, ros::console::levels::Level>::const_iterator it = loggers.begin(); it != loggers.end(); it++)
+    {
+      roscpp::Logger logger;
+      logger.name = it->first;
+      ros::console::levels::Level level = it->second;
+      if (level == ros::console::levels::Debug)
+      {
+        logger.level = "debug";
+      }
+      else if (level == ros::console::levels::Info)
+      {
+        logger.level = "info";
+      }
+      else if (level == ros::console::levels::Warn)
+      {
+        logger.level = "warn";
+      }
+      else if (level == ros::console::levels::Error)
+      {
+        logger.level = "error";
+      }
+      else if (level == ros::console::levels::Fatal)
+      {
+        logger.level = "fatal";
+      }
+      resp.loggers.push_back(logger);
+    }
+  }
+  return success;
+}
+
+bool setLoggerLevel(roscpp::SetLoggerLevel::Request& req, roscpp::SetLoggerLevel::Response&)
+{
+  std::transform(req.level.begin(), req.level.end(), req.level.begin(), (int(*)(int))std::toupper);
+
+  ros::console::levels::Level level;
+  if (req.level == "DEBUG")
+  {
+    level = ros::console::levels::Debug;
+  }
+  else if (req.level == "INFO")
+  {
+    level = ros::console::levels::Info;
+  }
+  else if (req.level == "WARN")
+  {
+    level = ros::console::levels::Warn;
+  }
+  else if (req.level == "ERROR")
+  {
+    level = ros::console::levels::Error;
+  }
+  else if (req.level == "FATAL")
+  {
+    level = ros::console::levels::Fatal;
+  }
+  else
+  {
+    return false;
+  }
+
+  bool success = ::ros::console::set_logger_level(req.logger, level);
+  if (success)
+  {
+    console::notifyLoggerLevelsChanged();
+  }
+
+  return success;
+}
+
+bool closeAllConnections(roscpp::Empty::Request&, roscpp::Empty::Response&)
+{
+  ROSCPP_LOG_DEBUG("close_all_connections service called, closing connections");
+  ConnectionManager::instance()->clear(Connection::TransportDisconnect);
+  return true;
+}
+
+void clockCallback(const rosgraph_msgs::Clock::ConstPtr& msg)
+{
+  Time::setNow(msg->clock);
+}
+
+CallbackQueuePtr getInternalCallbackQueue()
+{
+  if (!g_internal_callback_queue)
+  {
+    g_internal_callback_queue.reset(new CallbackQueue);
+  }
+
+  return g_internal_callback_queue;
+}
+
+void basicSigintHandler(int sig)
+{
+  (void)sig;
+  ros::requestShutdown();
+}
+
+void internalCallbackQueueThreadFunc()
+{
+  disableAllSignalsInThisThread();
+
+  CallbackQueuePtr queue = getInternalCallbackQueue();
+
+  while (!g_shutting_down)
+  {
+    queue->callAvailable(WallDuration(0.1));
+  }
+}
+
+bool isStarted()
+{
+  return g_started;
+}
+
+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;
+  std::string enable_debug_env;
+  if ( get_environment_variable(enable_debug_env,"ROSCPP_ENABLE_DEBUG") )
+  {
+    try
+    {
+      enable_debug = boost::lexical_cast<bool>(enable_debug_env.c_str());
+    }
+    catch (boost::bad_lexical_cast&)
+    {
+    }
+  }
+
+#ifdef _MSC_VER
+  if (env_ipv6)
+  {
+    free(env_ipv6);
+  }
+#endif
+
+  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);
+  }
+
+  ros::Time::init();
+
+  if (!(g_init_options & init_options::NoRosout))
+  {
+    g_rosout_appender = new ROSOutAppender;
+    ros::console::register_appender(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>(names::resolve("/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], using [%s] time", 
+		   this_node::getName().c_str(), getpid(), network::getHost().c_str(), 
+		   XMLRPCManager::instance()->getServerPort(), ConnectionManager::instance()->getTCPPort(), 
+		   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);
+  }
+}
+
+void check_ipv6_environment() {
+  char* env_ipv6 = NULL;
+#ifdef _MSC_VER
+  _dupenv_s(&env_ipv6, NULL, "ROS_IPV6");
+#else
+  env_ipv6 = getenv("ROS_IPV6");
+#endif
+
+  bool use_ipv6 = (env_ipv6 && strcmp(env_ipv6,"on") == 0);
+  TransportTCP::s_use_ipv6_ = use_ipv6;
+  XmlRpc::XmlRpcSocket::s_use_ipv6_ = use_ipv6;
+}
+
+void init(const M_string& remappings, const std::string& name, uint32_t options)
+{
+  if (!g_atexit_registered)
+  {
+    g_atexit_registered = true;
+    atexit(atexitCallback);
+  }
+
+  if (!g_global_queue)
+  {
+    g_global_queue.reset(new CallbackQueue);
+  }
+
+  if (!g_initialized)
+  {
+    g_init_options = options;
+    g_ok = true;
+
+    ROSCONSOLE_AUTOINIT;
+    // Disable SIGPIPE
+#ifndef WIN32
+    signal(SIGPIPE, SIG_IGN);
+#endif
+    check_ipv6_environment();
+    network::init(remappings);
+    master::init(remappings);
+    // names:: namespace is initialized by this_node
+    this_node::init(name, remappings, options);
+    file_log::init(remappings);
+    param::init(remappings);
+
+    g_initialized = true;
+  }
+}
+
+void init(int& argc, char** argv, const std::string& name, uint32_t options)
+{
+  M_string remappings;
+
+  int full_argc = argc;
+  // now, move the remapping argv's to the end, and decrement argc as needed
+  for (int i = 0; i < argc; )
+  {
+    std::string arg = argv[i];
+    size_t pos = arg.find(":=");
+    if (pos != std::string::npos)
+    {
+      std::string local_name = arg.substr(0, pos);
+      std::string external_name = arg.substr(pos + 2);
+
+      ROSCPP_LOG_DEBUG("remap: %s => %s", local_name.c_str(), external_name.c_str());
+      remappings[local_name] = external_name;
+
+      // shuffle everybody down and stuff this guy at the end of argv
+      char *tmp = argv[i];
+      for (int j = i; j < full_argc - 1; j++)
+        argv[j] = argv[j+1];
+      argv[argc-1] = tmp;
+      argc--;
+    }
+    else
+    {
+      i++; // move on, since we didn't shuffle anybody here to replace it
+    }
+  }
+
+  init(remappings, name, options);
+}
+
+void init(const VP_string& remappings, const std::string& name, uint32_t options)
+{
+  M_string remappings_map;
+  VP_string::const_iterator it = remappings.begin();
+  VP_string::const_iterator end = remappings.end();
+  for (; it != end; ++it)
+  {
+    remappings_map[it->first] = it->second;
+  }
+
+  init(remappings_map, name, options);
+}
+
+std::string getROSArg(int argc, const char* const* argv, const std::string& arg)
+{
+  for (int i = 0; i < argc; ++i)
+  {
+    std::string str_arg = argv[i];
+    size_t pos = str_arg.find(":=");
+    if (str_arg.substr(0,pos) == arg)
+    {
+      return str_arg.substr(pos+2);
+    }
+  }
+  return "";
+}
+
+void removeROSArgs(int argc, const char* const* argv, V_string& args_out)
+{
+  for (int i = 0; i < argc; ++i)
+  {
+    std::string arg = argv[i];
+    size_t pos = arg.find(":=");
+    if (pos == std::string::npos)
+    {
+      args_out.push_back(arg);
+    }
+  }
+}
+
+void spin()
+{
+  SingleThreadedSpinner s;
+  spin(s);
+}
+
+void spin(Spinner& s)
+{
+  s.spin();
+}
+
+void spinOnce()
+{
+  g_global_queue->callAvailable(ros::WallDuration());
+}
+
+void waitForShutdown()
+{
+  while (ok())
+  {
+    WallDuration(0.05).sleep();
+  }
+}
+
+CallbackQueue* getGlobalCallbackQueue()
+{
+  return g_global_queue.get();
+}
+
+bool ok()
+{
+  return g_ok;
+}
+
+void shutdown()
+{
+  boost::recursive_mutex::scoped_lock lock(g_shutting_down_mutex);
+  if (g_shutting_down)
+    return;
+  else
+    g_shutting_down = true;
+
+  ros::console::shutdown();
+
+  g_global_queue->disable();
+  g_global_queue->clear();
+
+  if (g_internal_queue_thread.get_id() != boost::this_thread::get_id())
+  {
+    g_internal_queue_thread.join();
+  }
+
+  g_rosout_appender = 0;
+
+  if (g_started)
+  {
+    TopicManager::instance()->shutdown();
+    ServiceManager::instance()->shutdown();
+    PollManager::instance()->shutdown();
+    ConnectionManager::instance()->shutdown();
+    XMLRPCManager::instance()->shutdown();
+  }
+
+  g_started = false;
+  g_ok = false;
+  Time::shutdown();
+}
+
+}

thirdparty/ros/ros_comm/clients/roscpp/src/libros/internal_timer_manager.cpp

@@ -0,0 +1,62 @@
+/*
+ * Copyright (C) 2010, Willow Garage, Inc.
+ *
+ * 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 names of Stanford University or 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.
+ */
+
+// Make sure we use CLOCK_MONOTONIC for the condition variable if not Apple.
+#ifndef __APPLE__
+#define BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
+#endif
+
+#include "ros/timer_manager.h"
+#include "ros/internal_timer_manager.h"
+
+// check if we have really included the backported boost condition variable
+// just in case someone messes with the include order...
+#if BOOST_VERSION < 106100
+#ifndef USING_BACKPORTED_BOOST_CONDITION_VARIABLE
+#error "steady timer needs boost version >= 1.61 or the backported headers!"
+#endif
+#endif
+
+namespace ros
+{
+
+static InternalTimerManagerPtr g_timer_manager;
+
+InternalTimerManagerPtr getInternalTimerManager()
+{
+  return g_timer_manager;
+}
+
+void initInternalTimerManager()
+{
+  if (!g_timer_manager)
+  {
+    g_timer_manager.reset(new InternalTimerManager);
+  }
+}
+
+} // namespace ros

thirdparty/ros/ros_comm/clients/roscpp/src/libros/intraprocess_publisher_link.cpp

@@ -0,0 +1,159 @@
+/*
+ * 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 "ros/intraprocess_publisher_link.h"
+#include "ros/intraprocess_subscriber_link.h"
+#include "ros/subscription.h"
+#include "ros/header.h"
+#include "ros/connection.h"
+#include "ros/transport/transport.h"
+#include "ros/this_node.h"
+#include "ros/connection_manager.h"
+#include "ros/file_log.h"
+
+#include <boost/bind.hpp>
+
+#include <sstream>
+
+namespace ros
+{
+
+IntraProcessPublisherLink::IntraProcessPublisherLink(const SubscriptionPtr& parent, const std::string& xmlrpc_uri, const TransportHints& transport_hints)
+: PublisherLink(parent, xmlrpc_uri, transport_hints)
+, dropped_(false)
+{
+}
+
+IntraProcessPublisherLink::~IntraProcessPublisherLink()
+{
+}
+
+void IntraProcessPublisherLink::setPublisher(const IntraProcessSubscriberLinkPtr& publisher)
+{
+  publisher_ = publisher;
+
+  SubscriptionPtr parent = parent_.lock();
+  ROS_ASSERT(parent);
+
+  Header header;
+  M_stringPtr values = header.getValues();
+  (*values)["callerid"] = this_node::getName();
+  (*values)["topic"] = parent->getName();
+  (*values)["type"] = publisher->getDataType();
+  (*values)["md5sum"] = publisher->getMD5Sum();
+  (*values)["message_definition"] = publisher->getMessageDefinition();
+  (*values)["latching"] = publisher->isLatching() ? "1" : "0";
+  setHeader(header);
+}
+
+void IntraProcessPublisherLink::drop()
+{
+  {
+    boost::recursive_mutex::scoped_lock lock(drop_mutex_);
+    if (dropped_)
+    {
+      return;
+    }
+
+    dropped_ = true;
+  }
+
+  if (publisher_)
+  {
+    publisher_->drop();
+    publisher_.reset();
+  }
+
+  if (SubscriptionPtr parent = parent_.lock())
+  {
+    ROSCPP_LOG_DEBUG("Connection to local publisher on topic [%s] dropped", parent->getName().c_str());
+
+    parent->removePublisherLink(shared_from_this());
+  }
+}
+
+void IntraProcessPublisherLink::handleMessage(const SerializedMessage& m, bool ser, bool nocopy)
+{
+  boost::recursive_mutex::scoped_lock lock(drop_mutex_);
+  if (dropped_)
+  {
+    return;
+  }
+
+  stats_.bytes_received_ += m.num_bytes;
+  stats_.messages_received_++;
+
+  SubscriptionPtr parent = parent_.lock();
+
+  if (parent)
+  {
+    stats_.drops_ += parent->handleMessage(m, ser, nocopy, header_.getValues(), shared_from_this());
+  }
+}
+
+std::string IntraProcessPublisherLink::getTransportType()
+{
+  return std::string("INTRAPROCESS");
+}
+
+std::string IntraProcessPublisherLink::getTransportInfo()
+{
+  // TODO: Check if we can dump more useful information here
+  return getTransportType();
+}
+
+void IntraProcessPublisherLink::getPublishTypes(bool& ser, bool& nocopy, const std::type_info& ti)
+{
+  boost::recursive_mutex::scoped_lock lock(drop_mutex_);
+  if (dropped_)
+  {
+    ser = false;
+    nocopy = false;
+    return;
+  }
+
+  SubscriptionPtr parent = parent_.lock();
+  if (parent)
+  {
+    parent->getPublishTypes(ser, nocopy, ti);
+  }
+  else
+  {
+    ser = true;
+    nocopy = false;
+  }
+}
+
+} // namespace ros
+

thirdparty/ros/ros_comm/clients/roscpp/src/libros/intraprocess_subscriber_link.cpp

@@ -0,0 +1,134 @@
+
+/*
+ * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
+ *
+ * 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 names of Stanford University or 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 "ros/intraprocess_subscriber_link.h"
+#include "ros/intraprocess_publisher_link.h"
+#include "ros/publication.h"
+#include "ros/header.h"
+#include "ros/connection.h"
+#include "ros/transport/transport.h"
+#include "ros/this_node.h"
+#include "ros/connection_manager.h"
+#include "ros/topic_manager.h"
+#include "ros/file_log.h"
+
+#include <boost/bind.hpp>
+
+namespace ros
+{
+
+IntraProcessSubscriberLink::IntraProcessSubscriberLink(const PublicationPtr& parent)
+: dropped_(false)
+{
+  ROS_ASSERT(parent);
+  parent_ = parent;
+  topic_ = parent->getName();
+}
+
+IntraProcessSubscriberLink::~IntraProcessSubscriberLink()
+{
+}
+
+void IntraProcessSubscriberLink::setSubscriber(const IntraProcessPublisherLinkPtr& subscriber)
+{
+  subscriber_ = subscriber;
+  connection_id_ = ConnectionManager::instance()->getNewConnectionID();
+  destination_caller_id_ = this_node::getName();
+}
+
+bool IntraProcessSubscriberLink::isLatching()
+{
+  if (PublicationPtr parent = parent_.lock())
+  {
+    return parent->isLatching();
+  }
+
+  return false;
+}
+
+void IntraProcessSubscriberLink::enqueueMessage(const SerializedMessage& m, bool ser, bool nocopy)
+{
+  boost::recursive_mutex::scoped_lock lock(drop_mutex_);
+  if (dropped_)
+  {
+    return;
+  }
+
+  ROS_ASSERT(subscriber_);
+  subscriber_->handleMessage(m, ser, nocopy);
+}
+
+std::string IntraProcessSubscriberLink::getTransportType()
+{
+  return std::string("INTRAPROCESS");
+}
+
+std::string IntraProcessSubscriberLink::getTransportInfo()
+{
+  // TODO: Check if we can dump more useful information here
+  return getTransportType();
+}
+
+void IntraProcessSubscriberLink::drop()
+{
+  {
+    boost::recursive_mutex::scoped_lock lock(drop_mutex_);
+    if (dropped_)
+    {
+      return;
+    }
+
+    dropped_ = true;
+  }
+
+  if (subscriber_)
+  {
+    subscriber_->drop();
+    subscriber_.reset();
+  }
+
+  if (PublicationPtr parent = parent_.lock())
+  {
+    ROSCPP_LOG_DEBUG("Connection to local subscriber on topic [%s] dropped", topic_.c_str());
+
+    parent->removeSubscriberLink(shared_from_this());
+  }
+}
+
+void IntraProcessSubscriberLink::getPublishTypes(bool& ser, bool& nocopy, const std::type_info& ti)
+{
+  boost::recursive_mutex::scoped_lock lock(drop_mutex_);
+  if (dropped_)
+  {
+    return;
+  }
+
+  subscriber_->getPublishTypes(ser, nocopy, ti);
+}
+
+} // namespace ros

thirdparty/ros/ros_comm/clients/roscpp/src/libros/io.cpp

@@ -0,0 +1,529 @@
+/*
+ * 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.
+ */
+/*****************************************************************************
+** Includes
+*****************************************************************************/
+
+#include "config.h"
+
+#include <ros/io.h>
+#include <ros/assert.h> // don't need if we dont call the pipe functions.
+#include <errno.h> // for EFAULT and co.
+#include <iostream>
+#include <sstream>
+#ifdef WIN32
+#else
+  #include <cstring> // strerror
+  #include <fcntl.h> // for non-blocking configuration
+#endif
+
+#ifdef HAVE_EPOLL
+  #include <sys/epoll.h>
+#endif
+
+/*****************************************************************************
+** Macros
+*****************************************************************************/
+
+#define UNUSED(expr) do { (void)(expr); } while (0)
+
+/*****************************************************************************
+** Namespaces
+*****************************************************************************/
+
+namespace ros {
+
+int last_socket_error() {
+	#ifdef WIN32
+		return WSAGetLastError();
+	#else
+		return errno;
+	#endif
+}
+const char* last_socket_error_string() {
+	#ifdef WIN32
+		// could fix this to use FORMAT_MESSAGE and print a real string later,
+		// but not high priority.
+		std::stringstream ostream;
+		ostream << "WSA Error: " << WSAGetLastError();
+		return ostream.str().c_str();
+	#else
+		return strerror(errno);
+	#endif
+}
+
+bool last_socket_error_is_would_block() {
+#if defined(WIN32)
+	if ( WSAGetLastError() == WSAEWOULDBLOCK ) {
+		return true;
+	} else {
+		return false;
+	}
+#else
+	if ( ( errno == EAGAIN  ) || ( errno == EWOULDBLOCK ) ) { // posix permits either
+		return true;
+	} else {
+		return false;
+	}
+#endif
+}
+
+int create_socket_watcher() {
+  int epfd = -1;
+#if defined(HAVE_EPOLL)
+  epfd = ::epoll_create1(0);
+  if (epfd < 0)
+  {
+    ROS_ERROR("Unable to create epoll watcher: %s", strerror(errno));
+  }
+#endif
+  return epfd;
+}
+
+void close_socket_watcher(int fd) {
+  if (fd >= 0)
+    ::close(fd);
+}
+
+void add_socket_to_watcher(int epfd, int fd) {
+#if defined(HAVE_EPOLL)
+  struct epoll_event ev;
+  bzero(&ev, sizeof(ev));
+
+  ev.events = 0;
+  ev.data.fd = fd;
+
+  if (::epoll_ctl(epfd, EPOLL_CTL_ADD, fd, &ev))
+  {
+    ROS_ERROR("Unable to add FD to epoll: %s", strerror(errno));
+  }
+#else
+  UNUSED(epfd);
+  UNUSED(fd);
+#endif
+}
+
+void del_socket_from_watcher(int epfd, int fd) {
+#if defined(HAVE_EPOLL)
+  if (::epoll_ctl(epfd, EPOLL_CTL_DEL, fd, NULL))
+  {
+    ROS_ERROR("Unable to remove FD to epoll: %s", strerror(errno));
+  }
+#else
+  UNUSED(epfd);
+  UNUSED(fd);
+#endif
+}
+
+void set_events_on_socket(int epfd, int fd, int events) {
+#if defined(HAVE_EPOLL)
+  struct epoll_event ev;
+  bzero(&ev, sizeof(ev));
+
+  ev.events = events;
+  ev.data.fd = fd;
+  if (::epoll_ctl(epfd, EPOLL_CTL_MOD, fd, &ev))
+  {
+    ROS_ERROR("Unable to modify FD epoll: %s", strerror(errno));
+  }
+#else
+  UNUSED(epfd);
+  UNUSED(fd);
+  UNUSED(events);
+#endif
+}
+
+
+
+/*****************************************************************************
+** Service Robotics/Libssh Functions
+*****************************************************************************/
+/**
+ * @brief A cross platform polling function for sockets.
+ *
+ * Windows doesn't have a polling function until Vista (WSAPoll) and even then
+ * its implementation is not supposed to be great. This works for a broader
+ * range of platforms and will suffice.
+ * @param epfd - the socket watcher to poll on.
+ * @param fds - the socket set (descriptor's and events) to poll for.
+ * @param nfds - the number of descriptors to poll for.
+ * @param timeout - timeout in milliseconds.
+ * @return pollfd_vector_ptr : NULL on error, empty on timeout, a list of structures with received events.
+ */
+pollfd_vector_ptr poll_sockets(int epfd, socket_pollfd *fds, nfds_t nfds, int timeout) {
+#if defined(WIN32)
+	fd_set readfds, writefds, exceptfds;
+	struct timeval tv, *ptv;
+	socket_fd_t max_fd;
+	int rc;
+	nfds_t i;
+	boost::shared_ptr<std::vector<socket_pollfd> > ofds;
+
+	UNUSED(epfd);
+
+	if (fds == NULL) {
+		errno = EFAULT;
+		return ofds;
+	}
+
+	FD_ZERO (&readfds);
+	FD_ZERO (&writefds);
+	FD_ZERO (&exceptfds);
+
+	/*********************
+	** Compute fd sets
+	**********************/
+	// also find the largest descriptor.
+	for (rc = -1, max_fd = 0, i = 0; i < nfds; i++) {
+		if (fds[i].fd == INVALID_SOCKET) {
+			continue;
+		}
+		if (fds[i].events & (POLLIN | POLLRDNORM)) {
+			FD_SET (fds[i].fd, &readfds);
+		}
+		if (fds[i].events & (POLLOUT | POLLWRNORM | POLLWRBAND)) {
+			FD_SET (fds[i].fd, &writefds);
+		}
+		if (fds[i].events & (POLLPRI | POLLRDBAND)) {
+			FD_SET (fds[i].fd, &exceptfds);
+		}
+		if (fds[i].fd > max_fd &&
+			  (fds[i].events & (POLLIN | POLLOUT | POLLPRI |
+								POLLRDNORM | POLLRDBAND |
+								POLLWRNORM | POLLWRBAND))) {
+			max_fd = fds[i].fd;
+			rc = 0;
+		}
+	}
+
+	if (rc == -1) {
+		errno = EINVAL;
+		return ofds;
+	}
+	/*********************
+	** Setting the timeout
+	**********************/
+	if (timeout < 0) {
+		ptv = NULL;
+	} else {
+		ptv = &tv;
+		if (timeout == 0) {
+			tv.tv_sec = 0;
+			tv.tv_usec = 0;
+		} else {
+			tv.tv_sec = timeout / 1000;
+			tv.tv_usec = (timeout % 1000) * 1000;
+		}
+	}
+
+	rc = select (max_fd + 1, &readfds, &writefds, &exceptfds, ptv);
+	if (rc < 0) {
+		return ofds;
+	}
+	ofds.reset(new std::vector<socket_pollfd>);
+	if ( rc == 0 ) {
+		return ofds;
+	}
+
+	for (rc = 0, i = 0; i < nfds; i++) {
+		if (fds[i].fd != INVALID_SOCKET) {
+			fds[i].revents = 0;
+
+			if (FD_ISSET(fds[i].fd, &readfds)) {
+				int save_errno = errno;
+				char data[64] = {0};
+				int ret;
+
+				/* support for POLLHUP */
+				// just check if there's incoming data, without removing it from the queue.
+				ret = recv(fds[i].fd, data, 64, MSG_PEEK);
+				#ifdef WIN32
+				if ((ret == -1) &&
+						(errno == WSAESHUTDOWN || errno == WSAECONNRESET ||
+						(errno == WSAECONNABORTED) || errno == WSAENETRESET))
+				#else
+				if ((ret == -1) &&
+						(errno == ESHUTDOWN || errno == ECONNRESET ||
+						(errno == ECONNABORTED) || errno == ENETRESET))
+				#endif
+				{
+					fds[i].revents |= POLLHUP;
+				} else {
+					fds[i].revents |= fds[i].events & (POLLIN | POLLRDNORM);
+				}
+				errno = save_errno;
+			}
+			if (FD_ISSET(fds[i].fd, &writefds)) {
+				fds[i].revents |= fds[i].events & (POLLOUT | POLLWRNORM | POLLWRBAND);
+			}
+
+			if (FD_ISSET(fds[i].fd, &exceptfds)) {
+				fds[i].revents |= fds[i].events & (POLLPRI | POLLRDBAND);
+			}
+
+			if (fds[i].revents & ~POLLHUP) {
+				rc++;
+			}
+		} else {
+				fds[i].revents = POLLNVAL;
+		}
+		ofds->push_back(fds[i]);
+	}
+	return ofds;
+#elif defined (HAVE_EPOLL)
+	UNUSED(nfds);
+	UNUSED(fds);
+	struct epoll_event ev[nfds];
+	pollfd_vector_ptr ofds;
+
+	int fd_cnt = ::epoll_wait(epfd, ev, nfds, timeout);
+
+	if (fd_cnt < 0)
+	{
+	// EINTR means that we got interrupted by a signal, and is not an error
+		if(errno != EINTR) {
+			ROS_ERROR("Error in epoll_wait! %s", strerror(errno));
+			ofds.reset();
+		}
+	}
+	else
+	{
+		ofds.reset(new std::vector<socket_pollfd>);
+		for (int i = 0; i < fd_cnt; i++)
+		{
+			socket_pollfd pfd;
+			pfd.fd = ev[i].data.fd;
+			pfd.revents = ev[i].events;
+			ofds->push_back(pfd);
+		}
+	}
+	return ofds;
+#else
+	UNUSED(epfd);
+	pollfd_vector_ptr ofds(new std::vector<socket_pollfd>);
+        // Clear the `revents` fields
+        for (nfds_t i = 0; i < nfds; i++)
+        {
+                fds[i].revents = 0;
+        }
+
+	// use an existing poll implementation
+	int result = poll(fds, nfds, timeout);
+	if ( result < 0 )
+	{
+		// EINTR means that we got interrupted by a signal, and is not an error
+		if(errno != EINTR)
+		{
+			ROS_ERROR("Error in poll! %s", strerror(errno));
+			ofds.reset();
+		}
+	} else {
+		for (nfds_t i = 0; i < nfds; i++)
+		{
+			if (fds[i].revents)
+			{
+				ofds->push_back(fds[i]);
+				fds[i].revents = 0;
+			}
+		}
+	}
+	return ofds;
+#endif // poll_sockets functions
+}
+/*****************************************************************************
+** Socket Utilities
+*****************************************************************************/
+/**
+ * Sets the socket as non blocking.
+ * @return int : 0 on success, WSAGetLastError()/errno on failure.
+ */
+int set_non_blocking(socket_fd_t &socket) {
+#ifdef WIN32
+    u_long non_blocking = 1;
+	if(ioctlsocket( socket, FIONBIO, &non_blocking ) != 0 )
+	{
+      return WSAGetLastError();
+	}
+#else
+    if(fcntl(socket, F_SETFL, O_NONBLOCK) == -1)
+    {
+      return errno;
+    }
+#endif
+    return 0;
+}
+
+/**
+ * @brief Close the socket.
+ *
+ * @return int : 0 on success, -1 on failure.
+ */
+int close_socket(socket_fd_t &socket) {
+#ifdef WIN32
+	if(::closesocket(socket) == SOCKET_ERROR ) {
+		return -1;
+	} else {
+		return 0;
+	}
+#else
+	if (::close(socket) < 0) {
+		return -1;
+	} else {
+		return 0;
+	}
+#endif //WIN32
+}
+
+/*****************************************************************************
+** Signal Pair
+*****************************************************************************/
+/**
+ * This code is primarily from the msdn socket tutorials.
+ * @param signal_pair : a pair of sockets linked to each other over localhost.
+ * @return 0 on success, -1 on failure.
+ */
+int create_signal_pair(signal_fd_t signal_pair[2]) {
+#ifdef WIN32 // use a socket pair
+	signal_pair[0] = INVALID_SOCKET;
+	signal_pair[1] = INVALID_SOCKET;
+
+    union {
+       struct sockaddr_in inaddr;
+       struct sockaddr addr;
+    } a;
+    socklen_t addrlen = sizeof(a.inaddr);
+
+    /*********************
+	** Listen Socket
+	**********************/
+	socket_fd_t listen_socket = INVALID_SOCKET;
+    listen_socket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
+	if (listen_socket == INVALID_SOCKET) {
+		return -1;
+	}
+
+    // allow it to be bound to an address already in use - do we actually need this?
+    int reuse = 1;
+    if (setsockopt(listen_socket, SOL_SOCKET, SO_REUSEADDR, (char*) &reuse, (socklen_t) sizeof(reuse)) == SOCKET_ERROR ) {
+    	::closesocket(listen_socket);
+		return -1;
+    }
+
+    memset(&a, 0, sizeof(a));
+    a.inaddr.sin_family = AF_INET;
+    a.inaddr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
+    // For TCP/IP, if the port is specified as zero, the service provider assigns
+    // a unique port to the application from the dynamic client port range.
+    a.inaddr.sin_port = 0;
+
+    if  (bind(listen_socket, &a.addr, sizeof(a.inaddr)) == SOCKET_ERROR) {
+    	::closesocket(listen_socket);
+		return -1;
+    }
+    // we need this below because the system auto filled in some entries, e.g. port #
+    if  (getsockname(listen_socket, &a.addr, &addrlen) == SOCKET_ERROR) {
+    	::closesocket(listen_socket);
+		return -1;
+    }
+    // max 1 connection permitted
+    if (listen(listen_socket, 1) == SOCKET_ERROR) {
+    	::closesocket(listen_socket);
+		return -1;
+    }
+
+    /*********************
+	** Connection
+	**********************/
+    // do we need io overlapping?
+    // DWORD flags = (make_overlapped ? WSA_FLAG_OVERLAPPED : 0);
+    DWORD overlapped_flag = 0;
+    signal_pair[0] = WSASocket(AF_INET, SOCK_STREAM, 0, NULL, 0, overlapped_flag);
+    if (signal_pair[0] == INVALID_SOCKET) {
+    	::closesocket(listen_socket);
+    	::closesocket(signal_pair[0]);
+    	return -1;
+    }
+    // reusing the information from above to connect to the listener
+    if (connect(signal_pair[0], &a.addr, sizeof(a.inaddr)) == SOCKET_ERROR) {
+    	::closesocket(listen_socket);
+    	::closesocket(signal_pair[0]);
+    	return -1;
+    }
+    /*********************
+	** Accept
+	**********************/
+    signal_pair[1] = accept(listen_socket, NULL, NULL);
+    if (signal_pair[1] == INVALID_SOCKET) {
+    	::closesocket(listen_socket);
+    	::closesocket(signal_pair[0]);
+    	::closesocket(signal_pair[1]);
+    	return -1;
+    }
+	/*********************
+	** Nonblocking
+	**********************/
+    // should we do this or should we set io overlapping?
+    if ( (set_non_blocking(signal_pair[0]) != 0) || (set_non_blocking(signal_pair[1]) != 0)  ) {
+		::closesocket(listen_socket);
+		::closesocket(signal_pair[0]);
+		::closesocket(signal_pair[1]);
+    	return -1;
+    }
+	/*********************
+	** Cleanup
+	**********************/
+    ::closesocket(listen_socket);  // the listener has done its job.
+    return 0;
+#else // use a pipe pair
+	// initialize
+	signal_pair[0] = -1;
+	signal_pair[1] = -1;
+
+	if(pipe(signal_pair) != 0) {
+		ROS_FATAL( "pipe() failed");
+		return -1;
+	}
+	if(fcntl(signal_pair[0], F_SETFL, O_NONBLOCK) == -1) {
+		ROS_FATAL( "fcntl() failed");
+		return -1;
+	}
+	if(fcntl(signal_pair[1], F_SETFL, O_NONBLOCK) == -1) {
+		ROS_FATAL( "fcntl() failed");
+		return -1;
+	}
+	return 0;
+#endif // create_pipe
+}
+
+} // namespace ros

thirdparty/ros/ros_comm/clients/roscpp/src/libros/master.cpp

@@ -0,0 +1,253 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names 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 "ros/master.h"
+#include "ros/xmlrpc_manager.h"
+#include "ros/this_node.h"
+#include "ros/init.h"
+#include "ros/network.h"
+
+#include <ros/console.h>
+#include <ros/assert.h>
+
+#include "xmlrpcpp/XmlRpc.h"
+
+namespace ros
+{
+
+namespace master
+{
+
+uint32_t g_port = 0;
+std::string g_host;
+std::string g_uri;
+ros::WallDuration g_retry_timeout;
+
+void init(const M_string& remappings)
+{
+  M_string::const_iterator it = remappings.find("__master");
+  if (it != remappings.end())
+  {
+    g_uri = it->second;
+  }
+
+  if (g_uri.empty())
+  {
+    char *master_uri_env = NULL;
+    #ifdef _MSC_VER
+      _dupenv_s(&master_uri_env, NULL, "ROS_MASTER_URI");
+    #else
+      master_uri_env = getenv("ROS_MASTER_URI");
+    #endif
+    if (!master_uri_env)
+    {
+      ROS_FATAL( "ROS_MASTER_URI is not defined in the environment. Either " \
+                 "type the following or (preferrably) add this to your " \
+                 "~/.bashrc file in order set up your " \
+                 "local machine as a ROS master:\n\n" \
+                 "export ROS_MASTER_URI=http://localhost:11311\n\n" \
+                 "then, type 'roscore' in another shell to actually launch " \
+                 "the master program.");
+      ROS_BREAK();
+    }
+
+    g_uri = master_uri_env;
+
+#ifdef _MSC_VER
+    // http://msdn.microsoft.com/en-us/library/ms175774(v=vs.80).aspx
+    free(master_uri_env);
+#endif
+  }
+
+  // Split URI into
+  if (!network::splitURI(g_uri, g_host, g_port))
+  {
+    ROS_FATAL( "Couldn't parse the master URI [%s] into a host:port pair.", g_uri.c_str());
+    ROS_BREAK();
+  }
+}
+
+const std::string& getHost()
+{
+  return g_host;
+}
+
+uint32_t getPort()
+{
+  return g_port;
+}
+
+const std::string& getURI()
+{
+  return g_uri;
+}
+
+void setRetryTimeout(ros::WallDuration timeout)
+{
+  if (timeout < ros::WallDuration(0))
+  {
+    ROS_FATAL("retry timeout must not be negative.");
+    ROS_BREAK();
+  }
+  g_retry_timeout = timeout;
+}
+
+bool check()
+{
+  XmlRpc::XmlRpcValue args, result, payload;
+  args[0] = this_node::getName();
+  return execute("getPid", args, result, payload, false);
+}
+
+bool getTopics(V_TopicInfo& topics)
+{
+  XmlRpc::XmlRpcValue args, result, payload;
+  args[0] = this_node::getName();
+  args[1] = ""; //TODO: Fix this
+
+  if (!execute("getPublishedTopics", args, result, payload, true))
+  {
+    return false;
+  }
+
+  topics.clear();
+  for (int i = 0; i < payload.size(); i++)
+  {
+    topics.push_back(TopicInfo(std::string(payload[i][0]), std::string(payload[i][1])));
+  }
+
+  return true;
+}
+
+bool getNodes(V_string& nodes)
+{
+  XmlRpc::XmlRpcValue args, result, payload;
+  args[0] = this_node::getName();
+
+  if (!execute("getSystemState", args, result, payload, true))
+  {
+    return false;
+  }
+
+  S_string node_set;
+  for (int i = 0; i < payload.size(); ++i)
+  {
+    for (int j = 0; j < payload[i].size(); ++j)
+    {
+      XmlRpc::XmlRpcValue val = payload[i][j][1];
+      for (int k = 0; k < val.size(); ++k)
+      {
+        std::string name = payload[i][j][1][k];
+        node_set.insert(name);
+      }
+    }
+  }
+
+  nodes.insert(nodes.end(), node_set.begin(), node_set.end());
+
+  return true;
+}
+
+#if defined(__APPLE__)
+boost::mutex g_xmlrpc_call_mutex;
+#endif
+
+bool execute(const std::string& method, const XmlRpc::XmlRpcValue& request, XmlRpc::XmlRpcValue& response, XmlRpc::XmlRpcValue& payload, bool wait_for_master)
+{
+  ros::SteadyTime start_time = ros::SteadyTime::now();
+
+  std::string master_host = getHost();
+  uint32_t master_port = getPort();
+  XmlRpc::XmlRpcClient *c = XMLRPCManager::instance()->getXMLRPCClient(master_host, master_port, "/");
+  bool printed = false;
+  bool slept = false;
+  bool ok = true;
+  bool b = false;
+  do
+  {
+    {
+#if defined(__APPLE__)
+      boost::mutex::scoped_lock lock(g_xmlrpc_call_mutex);
+#endif
+
+      b = c->execute(method.c_str(), request, response);
+    }
+
+    ok = !ros::isShuttingDown() && !XMLRPCManager::instance()->isShuttingDown();
+
+    if (!b && ok)
+    {
+      if (!printed && wait_for_master)
+      {
+        ROS_ERROR("[%s] Failed to contact master at [%s:%d].  %s", method.c_str(), master_host.c_str(), master_port, wait_for_master ? "Retrying..." : "");
+        printed = true;
+      }
+
+      if (!wait_for_master)
+      {
+        XMLRPCManager::instance()->releaseXMLRPCClient(c);
+        return false;
+      }
+
+      if (!g_retry_timeout.isZero() && (ros::SteadyTime::now() - start_time) >= g_retry_timeout)
+      {
+        ROS_ERROR("[%s] Timed out trying to connect to the master after [%f] seconds", method.c_str(), g_retry_timeout.toSec());
+        XMLRPCManager::instance()->releaseXMLRPCClient(c);
+        return false;
+      }
+
+      ros::WallDuration(0.05).sleep();
+      slept = true;
+    }
+    else
+    {
+      if (!XMLRPCManager::instance()->validateXmlrpcResponse(method, response, payload))
+      {
+        XMLRPCManager::instance()->releaseXMLRPCClient(c);
+
+        return false;
+      }
+
+      break;
+    }
+
+    ok = !ros::isShuttingDown() && !XMLRPCManager::instance()->isShuttingDown();
+  } while(ok);
+
+  if (ok && slept)
+  {
+    ROS_INFO("Connected to master at [%s:%d]", master_host.c_str(), master_port);
+  }
+
+  XMLRPCManager::instance()->releaseXMLRPCClient(c);
+
+  return b;
+}
+
+} // namespace master
+
+} // namespace ros

thirdparty/ros/ros_comm/clients/roscpp/src/libros/message_deserializer.cpp

@@ -0,0 +1,86 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names 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 "ros/message_deserializer.h"
+#include "ros/subscription_callback_helper.h"
+#include <ros/console.h>
+
+namespace ros
+{
+
+MessageDeserializer::MessageDeserializer(const SubscriptionCallbackHelperPtr& helper, const SerializedMessage& m, const boost::shared_ptr<M_string>& connection_header)
+: helper_(helper)
+, serialized_message_(m)
+, connection_header_(connection_header)
+{
+  if (serialized_message_.message && *serialized_message_.type_info != helper->getTypeInfo())
+  {
+    serialized_message_.message.reset();
+  }
+}
+
+VoidConstPtr MessageDeserializer::deserialize()
+{
+  boost::mutex::scoped_lock lock(mutex_);
+
+  if (msg_)
+  {
+    return msg_;
+  }
+
+  if (serialized_message_.message)
+  {
+    msg_ = serialized_message_.message;
+    return msg_;
+  }
+
+  if (!serialized_message_.buf && serialized_message_.num_bytes > 0)
+  {
+    // If the buffer has been reset it means we tried to deserialize and failed
+    return VoidConstPtr();
+  }
+
+  try
+  {
+    SubscriptionCallbackHelperDeserializeParams params;
+    params.buffer = serialized_message_.message_start;
+    params.length = serialized_message_.num_bytes - (serialized_message_.message_start - serialized_message_.buf.get());
+    params.connection_header = connection_header_;
+    msg_ = helper_->deserialize(params);
+  }
+  catch (std::exception& e)
+  {
+    ROS_ERROR("Exception thrown when deserializing message of length [%d] from [%s]: %s", (uint32_t)serialized_message_.num_bytes, (*connection_header_)["callerid"].c_str(), e.what());
+  }
+
+  serialized_message_.buf.reset();
+
+  return msg_;
+}
+
+}

thirdparty/ros/ros_comm/clients/roscpp/src/libros/names.cpp

@@ -0,0 +1,238 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Stanford University or 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 "ros/names.h"
+#include "ros/this_node.h"
+#include "ros/file_log.h"
+
+#include <ros/console.h>
+#include <ros/assert.h>
+
+#include <cstring>
+
+namespace ros
+{
+
+namespace names
+{
+
+M_string g_remappings;
+M_string g_unresolved_remappings;
+
+const M_string& getRemappings()
+{
+  return g_remappings;
+}
+
+const M_string& getUnresolvedRemappings()
+{
+  return g_unresolved_remappings;
+}
+
+bool isValidCharInName(char c)
+{
+  if (isalnum(c) || c == '/' || c == '_')
+  {
+    return true;
+  }
+
+  return false;
+}
+
+bool validate(const std::string& name, std::string& error)
+{
+  if (name.empty())
+  {
+    return true;
+  }
+
+  // First element is special, can be only ~ / or alpha
+  char c = name[0];
+  if (!isalpha(c) && c != '/' && c != '~')
+  {
+    std::stringstream ss;
+    ss << "Character [" << c << "] is not valid as the first character in Graph Resource Name [" << name << "].  Valid characters are a-z, A-Z, / and in some cases ~.";
+    error = ss.str();
+    return false;
+  }
+
+  for (size_t i = 1; i < name.size(); ++i)
+  {
+    c = name[i];
+    if (!isValidCharInName(c))
+    {
+      std::stringstream ss;
+      ss << "Character [" << c << "] at element [" << i << "] is not valid in Graph Resource Name [" << name <<"].  Valid characters are a-z, A-Z, 0-9, / and _.";
+      error = ss.str();
+
+      return false;
+    }
+  }
+
+  return true;
+}
+
+std::string clean(const std::string& name)
+{
+  std::string clean = name;
+
+  size_t pos = clean.find("//");
+  while (pos != std::string::npos)
+  {
+    clean.erase(pos, 1);
+    pos = clean.find("//", pos);
+  }
+
+  if (*clean.rbegin() == '/')
+  {
+    clean.erase(clean.size() - 1, 1);
+  }
+
+  return clean;
+}
+
+std::string append(const std::string& left, const std::string& right)
+{
+  return clean(left + "/" + right);
+}
+
+std::string remap(const std::string& name)
+{
+  std::string resolved = resolve(name, false);
+
+  M_string::const_iterator it = g_remappings.find(resolved);
+  if (it != g_remappings.end())
+  {
+    return it->second;
+  }
+
+  return name;
+}
+
+std::string resolve(const std::string& name, bool _remap)
+{
+  std::string s = resolve(this_node::getNamespace(), name, _remap);
+  return s;
+}
+
+std::string resolve(const std::string& ns, const std::string& name, bool _remap)
+{
+  std::string error;
+  if (!validate(name, error))
+  {
+  	throw InvalidNameException(error);
+  }
+
+  if (name.empty())
+  {
+    if (ns.empty())
+    {
+      return "/";
+    }
+
+    if (ns[0] == '/')
+    {
+      return ns;
+    }
+
+    return append("/", ns);
+  }
+
+  std::string copy = name;
+
+  if (copy[0] == '~')
+  {
+    copy = append(this_node::getName(), copy.substr(1));
+  }
+
+  if (copy[0] != '/')
+  {
+    copy = append("/", append(ns, copy));
+  }
+
+  copy = clean(copy);
+
+  if (_remap)
+  {
+    copy = remap(copy);
+  }
+
+  return copy;
+}
+
+void init(const M_string& remappings)
+{
+  M_string::const_iterator it = remappings.begin();
+  M_string::const_iterator end = remappings.end();
+  for (; it != end; ++it)
+  {
+    const std::string& left = it->first;
+    const std::string& right = it->second;
+
+    if (!left.empty() && left[0] != '_' && left != this_node::getName())
+    {
+      std::string resolved_left = resolve(left, false);
+      std::string resolved_right = resolve(right, false);
+      g_remappings[resolved_left] = resolved_right;
+      g_unresolved_remappings[left] = right;
+    }
+  }
+}
+
+std::string parentNamespace(const std::string& name)
+{
+  std::string error;
+  if (!validate(name, error))
+  {
+  	throw InvalidNameException(error);
+  }
+
+  if (!name.compare(""))  return "";
+  if (!name.compare("/")) return "/"; 
+
+  std::string stripped_name;
+
+  // rstrip trailing slash
+  if (name.find_last_of('/') == name.size()-1)
+    stripped_name = name.substr(0, name.size() -2);
+  else
+    stripped_name = name;
+
+  //pull everything up to the last /
+  size_t last_pos = stripped_name.find_last_of('/');
+  if (last_pos == std::string::npos)
+  {
+    return "";
+  }
+  else if (last_pos == 0)
+    return "/";
+  return stripped_name.substr(0, last_pos);
+}
+
+} // namespace names
+
+} // namespace ros

thirdparty/ros/ros_comm/clients/roscpp/src/libros/network.cpp

@@ -0,0 +1,223 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Stanford University or 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 "config.h"
+#include "ros/network.h"
+#include "ros/file_log.h"
+#include "ros/exceptions.h"
+#include "ros/io.h"     // cross-platform headers needed
+#include <ros/console.h>
+#include <ros/assert.h>
+#ifdef HAVE_IFADDRS_H
+  #include <ifaddrs.h>
+#endif
+
+#include <boost/lexical_cast.hpp>
+
+namespace ros
+{
+
+namespace network
+{
+
+std::string g_host;
+uint16_t g_tcpros_server_port = 0;
+
+const std::string& getHost()
+{
+  return g_host;
+}
+
+bool splitURI(const std::string& uri, std::string& host, uint32_t& port)
+{
+  // skip over the protocol if it's there
+  if (uri.substr(0, 7) == std::string("http://"))
+    host = uri.substr(7);
+  else if (uri.substr(0, 9) == std::string("rosrpc://"))
+    host = uri.substr(9);
+  // split out the port
+  std::string::size_type colon_pos = host.find_first_of(":");
+  if (colon_pos == std::string::npos)
+    return false;
+  std::string port_str = host.substr(colon_pos+1);
+  std::string::size_type slash_pos = port_str.find_first_of("/");
+  if (slash_pos != std::string::npos)
+    port_str = port_str.erase(slash_pos);
+  port = atoi(port_str.c_str());
+  host = host.erase(colon_pos);
+  return true;
+}
+
+uint16_t getTCPROSPort()
+{
+  return g_tcpros_server_port;
+}
+
+static bool isPrivateIP(const char *ip)
+{
+  bool b = !strncmp("192.168", ip, 7) || !strncmp("10.", ip, 3) ||
+           !strncmp("169.254", ip, 7);
+  return b;
+}
+
+std::string determineHost()
+{
+  std::string ip_env;
+  // First, did the user set ROS_HOSTNAME?
+  if ( get_environment_variable(ip_env, "ROS_HOSTNAME")) {
+    ROSCPP_LOG_DEBUG( "determineIP: using value of ROS_HOSTNAME:%s:", ip_env.c_str());
+    if (ip_env.size() == 0)
+    {
+      ROS_WARN("invalid ROS_HOSTNAME (an empty string)");
+    }
+    return ip_env;
+  }
+
+  // Second, did the user set ROS_IP?
+  if ( get_environment_variable(ip_env, "ROS_IP")) {
+    ROSCPP_LOG_DEBUG( "determineIP: using value of ROS_IP:%s:", ip_env.c_str());
+    if (ip_env.size() == 0)
+    {
+      ROS_WARN("invalid ROS_IP (an empty string)");
+    }
+    return ip_env;
+  }
+
+  // Third, try the hostname
+  char host[1024];
+  memset(host,0,sizeof(host));
+  if(gethostname(host,sizeof(host)-1) != 0)
+  {
+    ROS_ERROR("determineIP: gethostname failed");
+  }
+  // We don't want localhost to be our ip
+  else if(strlen(host) && strcmp("localhost", host))
+  {
+    return std::string(host);
+  }
+
+  // Fourth, fall back on interface search, which will yield an IP address
+
+#ifdef HAVE_IFADDRS_H
+  struct ifaddrs *ifa = NULL, *ifp = NULL;
+  int rc;
+  if ((rc = getifaddrs(&ifp)) < 0)
+  {
+    ROS_FATAL("error in getifaddrs: [%s]", strerror(rc));
+    ROS_BREAK();
+  }
+  char preferred_ip[200] = {0};
+  for (ifa = ifp; ifa; ifa = ifa->ifa_next)
+  {
+    char ip_[200];
+    socklen_t salen;
+    if (!ifa->ifa_addr)
+      continue; // evidently this interface has no ip address
+    if (ifa->ifa_addr->sa_family == AF_INET)
+      salen = sizeof(struct sockaddr_in);
+    else if (ifa->ifa_addr->sa_family == AF_INET6)
+      salen = sizeof(struct sockaddr_in6);
+    else
+      continue;
+    if (getnameinfo(ifa->ifa_addr, salen, ip_, sizeof(ip_), NULL, 0,
+                    NI_NUMERICHOST) < 0)
+    {
+      ROSCPP_LOG_DEBUG( "getnameinfo couldn't get the ip of interface [%s]", ifa->ifa_name);
+      continue;
+    }
+    //ROS_INFO( "ip of interface [%s] is [%s]", ifa->ifa_name, ip);
+    // prefer non-private IPs over private IPs
+    if (!strcmp("127.0.0.1", ip_) || strchr(ip_,':'))
+      continue; // ignore loopback unless we have no other choice
+    if (ifa->ifa_addr->sa_family == AF_INET6 && !preferred_ip[0])
+      strcpy(preferred_ip, ip_);
+    else if (isPrivateIP(ip_) && !preferred_ip[0])
+      strcpy(preferred_ip, ip_);
+    else if (!isPrivateIP(ip_) &&
+             (isPrivateIP(preferred_ip) || !preferred_ip[0]))
+      strcpy(preferred_ip, ip_);
+  }
+  freeifaddrs(ifp);
+  if (!preferred_ip[0])
+  {
+    ROS_ERROR( "Couldn't find a preferred IP via the getifaddrs() call; I'm assuming that your IP "
+        "address is 127.0.0.1.  This should work for local processes, "
+        "but will almost certainly not work if you have remote processes."
+        "Report to the ROS development team to seek a fix.");
+    return std::string("127.0.0.1");
+  }
+  ROSCPP_LOG_DEBUG( "preferred IP is guessed to be %s", preferred_ip);
+  return std::string(preferred_ip);
+#else
+  // @todo Fix IP determination in the case where getifaddrs() isn't
+  // available.
+  ROS_ERROR( "You don't have the getifaddrs() call; I'm assuming that your IP "
+             "address is 127.0.0.1.  This should work for local processes, "
+             "but will almost certainly not work if you have remote processes."
+             "Report to the ROS development team to seek a fix.");
+  return std::string("127.0.0.1");
+#endif
+}
+
+void init(const M_string& remappings)
+{
+  M_string::const_iterator it = remappings.find("__hostname");
+  if (it != remappings.end())
+  {
+    g_host = it->second;
+  }
+  else
+  {
+    it = remappings.find("__ip");
+    if (it != remappings.end())
+    {
+      g_host = it->second;
+    }
+  }
+
+  it = remappings.find("__tcpros_server_port");
+  if (it != remappings.end())
+  {
+    try
+    {
+      g_tcpros_server_port = boost::lexical_cast<uint16_t>(it->second);
+    }
+    catch (boost::bad_lexical_cast&)
+    {
+      throw ros::InvalidPortException("__tcpros_server_port [" + it->second + "] was not specified as a number within the 0-65535 range");
+    }
+  }
+
+  if (g_host.empty())
+  {
+    g_host = determineHost();
+  }
+}
+
+} // namespace network
+
+} // namespace ros

thirdparty/ros/ros_comm/clients/roscpp/src/libros/node_handle.cpp

@@ -0,0 +1,799 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Stanford University or 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 "ros/node_handle.h"
+#include "ros/this_node.h"
+#include "ros/service.h"
+#include "ros/callback_queue.h"
+
+#include "ros/time.h"
+#include "ros/rate.h"
+#include "ros/timer.h"
+#include "ros/wall_timer.h"
+#include "ros/steady_timer.h"
+
+#include "ros/xmlrpc_manager.h"
+#include "ros/topic_manager.h"
+#include "ros/service_manager.h"
+#include "ros/master.h"
+#include "ros/param.h"
+#include "ros/names.h"
+#include "ros/init.h"
+#include "ros/this_node.h"
+#include "xmlrpcpp/XmlRpc.h"
+
+#include <boost/thread.hpp>
+
+namespace ros
+{
+
+boost::mutex g_nh_refcount_mutex;
+int32_t g_nh_refcount = 0;
+bool g_node_started_by_nh = false;
+
+class NodeHandleBackingCollection
+{
+public:
+  typedef std::vector<Publisher::ImplWPtr> V_PubImpl;
+  typedef std::vector<ServiceServer::ImplWPtr> V_SrvImpl;
+  typedef std::vector<Subscriber::ImplWPtr> V_SubImpl;
+  typedef std::vector<ServiceClient::ImplWPtr> V_SrvCImpl;
+  V_PubImpl pubs_;
+  V_SrvImpl srvs_;
+  V_SubImpl subs_;
+  V_SrvCImpl srv_cs_;
+
+  boost::mutex mutex_;
+};
+
+NodeHandle::NodeHandle(const std::string& ns, const M_string& remappings)
+  : namespace_(this_node::getNamespace())
+  , callback_queue_(0)
+  , collection_(0)
+{
+  std::string tilde_resolved_ns;
+  if (!ns.empty() && ns[0] == '~')// starts with tilde
+    tilde_resolved_ns = names::resolve(ns);
+  else
+    tilde_resolved_ns = ns;
+
+  construct(tilde_resolved_ns, true);
+
+  initRemappings(remappings);
+}
+
+NodeHandle::NodeHandle(const NodeHandle& parent, const std::string& ns)
+: collection_(0)
+{
+  namespace_ = parent.getNamespace();
+  callback_queue_ = parent.callback_queue_;
+
+  remappings_ = parent.remappings_;
+  unresolved_remappings_ = parent.unresolved_remappings_;
+
+  construct(ns, false);
+}
+
+NodeHandle::NodeHandle(const NodeHandle& parent, const std::string& ns, const M_string& remappings)
+: collection_(0)
+{
+  namespace_ = parent.getNamespace();
+  callback_queue_ = parent.callback_queue_;
+
+  remappings_ = parent.remappings_;
+  unresolved_remappings_ = parent.unresolved_remappings_;
+
+  construct(ns, false);
+
+  initRemappings(remappings);
+}
+
+NodeHandle::NodeHandle(const NodeHandle& rhs)
+: collection_(0)
+{
+  callback_queue_ = rhs.callback_queue_;
+  remappings_ = rhs.remappings_;
+  unresolved_remappings_ = rhs.unresolved_remappings_;
+
+  construct(rhs.namespace_, true); 
+
+  unresolved_namespace_ = rhs.unresolved_namespace_;
+}
+
+NodeHandle::~NodeHandle()
+{
+  destruct();
+}
+
+NodeHandle& NodeHandle::operator=(const NodeHandle& rhs)
+{
+  ROS_ASSERT(collection_);
+  namespace_ = rhs.namespace_;
+  callback_queue_ = rhs.callback_queue_;
+  remappings_ = rhs.remappings_;
+  unresolved_remappings_ = rhs.unresolved_remappings_;
+
+  return *this;
+}
+
+void spinThread()
+{
+  ros::spin();
+}
+
+void NodeHandle::construct(const std::string& ns, bool validate_name)
+{
+  if (!ros::isInitialized())
+  {
+    ROS_FATAL("You must call ros::init() before creating the first NodeHandle");
+    ROS_BREAK();
+  }
+
+  collection_ = new NodeHandleBackingCollection;
+  unresolved_namespace_ = ns;
+  // if callback_queue_ is nonnull, we are in a non-nullary constructor
+
+  if (validate_name)
+    namespace_ = resolveName(ns, true);
+  else
+    {
+      namespace_ = resolveName(ns, true, no_validate());
+      // FIXME validate namespace_ now
+    }
+  ok_ = true;
+
+  boost::mutex::scoped_lock lock(g_nh_refcount_mutex);
+
+  if (g_nh_refcount == 0 && !ros::isStarted())
+  {
+    g_node_started_by_nh = true;
+    ros::start();
+  }
+
+  ++g_nh_refcount;
+}
+
+void NodeHandle::destruct()
+{
+  delete collection_;
+
+  boost::mutex::scoped_lock lock(g_nh_refcount_mutex);
+
+  --g_nh_refcount;
+
+  if (g_nh_refcount == 0 && g_node_started_by_nh)
+  {
+    ros::shutdown();
+  }
+}
+
+void NodeHandle::initRemappings(const M_string& remappings)
+{
+  {
+    M_string::const_iterator it = remappings.begin();
+    M_string::const_iterator end = remappings.end();
+    for (; it != end; ++it)
+    {
+      const std::string& from = it->first;
+      const std::string& to = it->second;
+
+      remappings_.insert(std::make_pair(resolveName(from, false), resolveName(to, false)));
+      unresolved_remappings_.insert(std::make_pair(from, to));
+    }
+  }
+}
+
+void NodeHandle::setCallbackQueue(CallbackQueueInterface* queue)
+{
+  callback_queue_ = queue;
+}
+
+std::string NodeHandle::remapName(const std::string& name) const
+{
+  std::string resolved = resolveName(name, false);
+
+  // First search any remappings that were passed in specifically for this NodeHandle
+  M_string::const_iterator it = remappings_.find(resolved);
+  if (it != remappings_.end())
+  {
+    // ROSCPP_LOG_DEBUG("found 'local' remapping: %s", it->second.c_str());
+    return it->second;
+  }
+
+  // If not in our local remappings, perhaps in the global ones
+  return names::remap(resolved);
+}
+
+std::string NodeHandle::resolveName(const std::string& name, bool remap) const
+{
+  // ROSCPP_LOG_DEBUG("resolveName(%s, %s)", name.c_str(), remap ? "true" : "false");
+  std::string error;
+  if (!names::validate(name, error))
+  {
+    throw InvalidNameException(error);
+  }
+
+  return resolveName(name, remap, no_validate());
+}
+
+std::string NodeHandle::resolveName(const std::string& name, bool remap, no_validate) const
+{
+  if (name.empty())
+  {
+    return namespace_;
+  }
+
+  std::string final = name;
+
+  if (final[0] == '~')
+  {
+    std::stringstream ss;
+    ss << "Using ~ names with NodeHandle methods is not allowed.  If you want to use private names with the NodeHandle ";
+    ss << "interface, construct a NodeHandle using a private name as its namespace.  e.g. ";
+    ss << "ros::NodeHandle nh(\"~\");  ";
+    ss << "nh.getParam(\"my_private_name\");";
+    ss << " (name = [" << name << "])";
+    throw InvalidNameException(ss.str());
+  }
+  else if (final[0] == '/')
+  {
+    // do nothing
+  }
+  else if (!namespace_.empty())
+  {
+    // ROSCPP_LOG_DEBUG("Appending namespace_ (%s)", namespace_.c_str());
+    final = names::append(namespace_, final);
+  }
+
+  // ROSCPP_LOG_DEBUG("resolveName, pre-clean: %s", final.c_str());
+  final = names::clean(final);
+  // ROSCPP_LOG_DEBUG("resolveName, post-clean: %s", final.c_str());
+
+  if (remap)
+  {
+    final = remapName(final);
+    // ROSCPP_LOG_DEBUG("resolveName, remapped: %s", final.c_str());
+  }
+
+  return names::resolve(final, false);
+}
+
+Publisher NodeHandle::advertise(AdvertiseOptions& ops)
+{
+  ops.topic = resolveName(ops.topic);
+  if (ops.callback_queue == 0)
+  {
+    if (callback_queue_)
+    {
+      ops.callback_queue = callback_queue_;
+    }
+    else
+    {
+      ops.callback_queue = getGlobalCallbackQueue();
+    }
+  }
+
+  SubscriberCallbacksPtr callbacks(boost::make_shared<SubscriberCallbacks>(ops.connect_cb, ops.disconnect_cb, 
+                                                                           ops.tracked_object, ops.callback_queue));
+
+  if (TopicManager::instance()->advertise(ops, callbacks))
+  {
+    Publisher pub(ops.topic, ops.md5sum, ops.datatype, *this, callbacks);
+
+    {
+      boost::mutex::scoped_lock lock(collection_->mutex_);
+      collection_->pubs_.push_back(pub.impl_);
+    }
+
+    return pub;
+  }
+
+  return Publisher();
+}
+
+Subscriber NodeHandle::subscribe(SubscribeOptions& ops)
+{
+  ops.topic = resolveName(ops.topic);
+  if (ops.callback_queue == 0)
+  {
+    if (callback_queue_)
+    {
+      ops.callback_queue = callback_queue_;
+    }
+    else
+    {
+      ops.callback_queue = getGlobalCallbackQueue();
+    }
+  }
+
+  if (TopicManager::instance()->subscribe(ops))
+  {
+    Subscriber sub(ops.topic, *this, ops.helper);
+
+    {
+      boost::mutex::scoped_lock lock(collection_->mutex_);
+      collection_->subs_.push_back(sub.impl_);
+    }
+
+    return sub;
+  }
+
+  return Subscriber();
+}
+
+ServiceServer NodeHandle::advertiseService(AdvertiseServiceOptions& ops)
+{
+  ops.service = resolveName(ops.service);
+  if (ops.callback_queue == 0)
+  {
+    if (callback_queue_)
+    {
+      ops.callback_queue = callback_queue_;
+    }
+    else
+    {
+      ops.callback_queue = getGlobalCallbackQueue();
+    }
+  }
+
+  if (ServiceManager::instance()->advertiseService(ops))
+  {
+    ServiceServer srv(ops.service, *this);
+
+    {
+      boost::mutex::scoped_lock lock(collection_->mutex_);
+      collection_->srvs_.push_back(srv.impl_);
+    }
+
+    return srv;
+  }
+
+  return ServiceServer();
+}
+
+ServiceClient NodeHandle::serviceClient(ServiceClientOptions& ops)
+{
+  ops.service = resolveName(ops.service);
+  ServiceClient client(ops.service, ops.persistent, ops.header, ops.md5sum);
+
+  if (client)
+  {
+    boost::mutex::scoped_lock lock(collection_->mutex_);
+    collection_->srv_cs_.push_back(client.impl_);
+  }
+
+  return client;
+}
+
+Timer NodeHandle::createTimer(Duration period, const TimerCallback& callback, 
+                              bool oneshot, bool autostart) const
+{
+  TimerOptions ops;
+  ops.period = period;
+  ops.callback = callback;
+  ops.oneshot = oneshot;
+  ops.autostart = autostart;
+  return createTimer(ops);
+}
+
+Timer NodeHandle::createTimer(TimerOptions& ops) const
+{
+  if (ops.callback_queue == 0)
+  {
+    if (callback_queue_)
+    {
+      ops.callback_queue = callback_queue_;
+    }
+    else
+    {
+      ops.callback_queue = getGlobalCallbackQueue();
+    }
+  }
+
+  Timer timer(ops);
+  if (ops.autostart)
+    timer.start();
+  return timer;
+}
+
+WallTimer NodeHandle::createWallTimer(WallDuration period, const WallTimerCallback& callback, 
+                                      bool oneshot, bool autostart) const
+{
+  WallTimerOptions ops;
+  ops.period = period;
+  ops.callback = callback;
+  ops.oneshot = oneshot;
+  ops.autostart = autostart;
+  return createWallTimer(ops);
+}
+
+WallTimer NodeHandle::createWallTimer(WallTimerOptions& ops) const
+{
+  if (ops.callback_queue == 0)
+  {
+    if (callback_queue_)
+    {
+      ops.callback_queue = callback_queue_;
+    }
+    else
+    {
+      ops.callback_queue = getGlobalCallbackQueue();
+    }
+  }
+
+  WallTimer timer(ops);
+  if (ops.autostart)
+    timer.start();
+  return timer;
+}
+
+SteadyTimer NodeHandle::createSteadyTimer(WallDuration period, const SteadyTimerCallback& callback,
+                                          bool oneshot, bool autostart) const
+{
+  SteadyTimerOptions ops;
+  ops.period = period;
+  ops.callback = callback;
+  ops.oneshot = oneshot;
+  ops.autostart = autostart;
+  return createSteadyTimer(ops);
+}
+
+SteadyTimer NodeHandle::createSteadyTimer(SteadyTimerOptions& ops) const
+{
+  if (ops.callback_queue == 0)
+  {
+    if (callback_queue_)
+    {
+      ops.callback_queue = callback_queue_;
+    }
+    else
+    {
+      ops.callback_queue = getGlobalCallbackQueue();
+    }
+  }
+
+  SteadyTimer timer(ops);
+  if (ops.autostart)
+    timer.start();
+  return timer;
+}
+
+void NodeHandle::shutdown()
+{
+  {
+    NodeHandleBackingCollection::V_SubImpl::iterator it = collection_->subs_.begin();
+    NodeHandleBackingCollection::V_SubImpl::iterator end = collection_->subs_.end();
+    for (; it != end; ++it)
+    {
+      Subscriber::ImplPtr impl = it->lock();
+
+      if (impl)
+      {
+        impl->unsubscribe();
+      }
+    }
+  }
+
+  {
+    NodeHandleBackingCollection::V_PubImpl::iterator it = collection_->pubs_.begin();
+    NodeHandleBackingCollection::V_PubImpl::iterator end = collection_->pubs_.end();
+    for (; it != end; ++it)
+    {
+      Publisher::ImplPtr impl = it->lock();
+
+      if (impl)
+      {
+        impl->unadvertise();
+      }
+    }
+  }
+
+  {
+    NodeHandleBackingCollection::V_SrvImpl::iterator it = collection_->srvs_.begin();
+    NodeHandleBackingCollection::V_SrvImpl::iterator end = collection_->srvs_.end();
+    for (; it != end; ++it)
+    {
+      ServiceServer::ImplPtr impl = it->lock();
+
+      if (impl)
+      {
+        impl->unadvertise();
+      }
+    }
+  }
+
+  {
+    NodeHandleBackingCollection::V_SrvCImpl::iterator it = collection_->srv_cs_.begin();
+    NodeHandleBackingCollection::V_SrvCImpl::iterator end = collection_->srv_cs_.end();
+    for (; it != end; ++it)
+    {
+      ServiceClient::ImplPtr impl = it->lock();
+
+      if (impl)
+      {
+        impl->shutdown();
+      }
+    }
+  }
+
+  ok_ = false;
+}
+
+void NodeHandle::setParam(const std::string& key, const XmlRpc::XmlRpcValue& v) const
+{
+  return param::set(resolveName(key), v);
+}
+
+void NodeHandle::setParam(const std::string& key, const std::string& s) const
+{
+  return param::set(resolveName(key), s);
+}
+
+void NodeHandle::setParam(const std::string& key, const char* s) const
+{
+  return param::set(resolveName(key), s);
+}
+
+void NodeHandle::setParam(const std::string& key, double d) const
+{
+  return param::set(resolveName(key), d);
+}
+
+void NodeHandle::setParam(const std::string& key, int i) const
+{
+  return param::set(resolveName(key), i);
+}
+
+void NodeHandle::setParam(const std::string& key, bool b) const
+{
+  return param::set(resolveName(key), b);
+}
+
+void NodeHandle::setParam(const std::string& key, const std::vector<std::string>& vec) const
+{
+  return param::set(resolveName(key), vec);
+}
+void NodeHandle::setParam(const std::string& key, const std::vector<double>& vec) const
+{
+  return param::set(resolveName(key), vec);
+}
+void NodeHandle::setParam(const std::string& key, const std::vector<float>& vec) const
+{
+  return param::set(resolveName(key), vec);
+}
+void NodeHandle::setParam(const std::string& key, const std::vector<int>& vec) const
+{
+  return param::set(resolveName(key), vec);
+}
+void NodeHandle::setParam(const std::string& key, const std::vector<bool>& vec) const
+{
+  return param::set(resolveName(key), vec);
+}
+
+void NodeHandle::setParam(const std::string& key, const std::map<std::string, std::string>& map) const
+{
+  return param::set(resolveName(key), map);
+}
+void NodeHandle::setParam(const std::string& key, const std::map<std::string, double>& map) const
+{
+  return param::set(resolveName(key), map);
+}
+void NodeHandle::setParam(const std::string& key, const std::map<std::string, float>& map) const
+{
+  return param::set(resolveName(key), map);
+}
+void NodeHandle::setParam(const std::string& key, const std::map<std::string, int>& map) const
+{
+  return param::set(resolveName(key), map);
+}
+void NodeHandle::setParam(const std::string& key, const std::map<std::string, bool>& map) const
+{
+  return param::set(resolveName(key), map);
+}
+
+bool NodeHandle::hasParam(const std::string& key) const
+{
+  return param::has(resolveName(key));
+}
+
+bool NodeHandle::deleteParam(const std::string& key) const
+{
+  return param::del(resolveName(key));
+}
+
+bool NodeHandle::getParamNames(std::vector<std::string>& keys) const
+{
+  return param::getParamNames(keys);
+}
+
+bool NodeHandle::getParam(const std::string& key, XmlRpc::XmlRpcValue& v) const
+{
+  return param::get(resolveName(key), v);
+}
+
+bool NodeHandle::getParam(const std::string& key, std::string& s) const
+{
+  return param::get(resolveName(key), s);
+}
+
+bool NodeHandle::getParam(const std::string& key, double& d) const
+{
+  return param::get(resolveName(key), d);
+}
+
+bool NodeHandle::getParam(const std::string& key, float& f) const
+{
+  return param::get(resolveName(key), f);
+}
+
+bool NodeHandle::getParam(const std::string& key, int& i) const
+{
+  return param::get(resolveName(key), i);
+}
+
+bool NodeHandle::getParam(const std::string& key, bool& b) const
+{
+  return param::get(resolveName(key), b);
+}
+
+
+bool NodeHandle::getParam(const std::string& key, std::vector<std::string>& vec) const
+{
+  return param::get(resolveName(key), vec);
+}
+bool NodeHandle::getParam(const std::string& key, std::vector<double>& vec) const
+{
+  return param::get(resolveName(key), vec);
+}
+bool NodeHandle::getParam(const std::string& key, std::vector<float>& vec) const
+{
+  return param::get(resolveName(key), vec);
+}
+bool NodeHandle::getParam(const std::string& key, std::vector<int>& vec) const
+{
+  return param::get(resolveName(key), vec);
+}
+bool NodeHandle::getParam(const std::string& key, std::vector<bool>& vec) const
+{
+  return param::get(resolveName(key), vec);
+}
+
+bool NodeHandle::getParam(const std::string& key, std::map<std::string, std::string>& map) const
+{
+  return param::get(resolveName(key), map);
+}
+bool NodeHandle::getParam(const std::string& key, std::map<std::string, double>& map) const
+{
+  return param::get(resolveName(key), map);
+}
+bool NodeHandle::getParam(const std::string& key, std::map<std::string, float>& map) const
+{
+  return param::get(resolveName(key), map);
+}
+bool NodeHandle::getParam(const std::string& key, std::map<std::string, int>& map) const
+{
+  return param::get(resolveName(key), map);
+}
+bool NodeHandle::getParam(const std::string& key, std::map<std::string, bool>& map) const
+{
+  return param::get(resolveName(key), map);
+}
+
+bool NodeHandle::getParamCached(const std::string& key, XmlRpc::XmlRpcValue& v) const
+{
+  return param::getCached(resolveName(key), v);
+}
+
+bool NodeHandle::getParamCached(const std::string& key, std::string& s) const
+{
+  return param::getCached(resolveName(key), s);
+}
+
+bool NodeHandle::getParamCached(const std::string& key, double& d) const
+{
+  return param::getCached(resolveName(key), d);
+}
+
+bool NodeHandle::getParamCached(const std::string& key, float& f) const
+{
+  return param::getCached(resolveName(key), f);
+}
+
+bool NodeHandle::getParamCached(const std::string& key, int& i) const
+{
+  return param::getCached(resolveName(key), i);
+}
+
+bool NodeHandle::getParamCached(const std::string& key, bool& b) const
+{
+  return param::getCached(resolveName(key), b);
+}
+
+bool NodeHandle::getParamCached(const std::string& key, std::vector<std::string>& vec) const
+{
+  return param::getCached(resolveName(key), vec);
+}
+bool NodeHandle::getParamCached(const std::string& key, std::vector<double>& vec) const
+{
+  return param::getCached(resolveName(key), vec);
+}
+bool NodeHandle::getParamCached(const std::string& key, std::vector<float>& vec) const
+{
+  return param::getCached(resolveName(key), vec);
+}
+bool NodeHandle::getParamCached(const std::string& key, std::vector<int>& vec) const
+{
+  return param::getCached(resolveName(key), vec);
+}
+bool NodeHandle::getParamCached(const std::string& key, std::vector<bool>& vec) const
+{
+  return param::getCached(resolveName(key), vec);
+}
+
+bool NodeHandle::getParamCached(const std::string& key, std::map<std::string, std::string>& map) const
+{
+  return param::getCached(resolveName(key), map);
+}
+bool NodeHandle::getParamCached(const std::string& key, std::map<std::string, double>& map) const
+{
+  return param::getCached(resolveName(key), map);
+}
+bool NodeHandle::getParamCached(const std::string& key, std::map<std::string, float>& map) const
+{
+  return param::getCached(resolveName(key), map);
+}
+bool NodeHandle::getParamCached(const std::string& key, std::map<std::string, int>& map) const
+{
+  return param::getCached(resolveName(key), map);
+}
+bool NodeHandle::getParamCached(const std::string& key, std::map<std::string, bool>& map) const
+{
+  return param::getCached(resolveName(key), map);
+}
+
+bool NodeHandle::searchParam(const std::string& key, std::string& result_out) const
+{
+  // searchParam needs a separate form of remapping -- remapping on the unresolved name, rather than the
+  // resolved one.
+
+  std::string remapped = key;
+  M_string::const_iterator it = unresolved_remappings_.find(key);
+  // First try our local remappings
+  if (it != unresolved_remappings_.end())
+  {
+    remapped = it->second;
+  }
+
+  return param::search(resolveName(""), remapped, result_out);
+}
+
+bool NodeHandle::ok() const
+{
+  return ros::ok() && ok_;
+}
+
+} // namespace ros

thirdparty/ros/ros_comm/clients/roscpp/src/libros/param.cpp

@@ -0,0 +1,882 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names 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 "ros/param.h"
+#include "ros/master.h"
+#include "ros/xmlrpc_manager.h"
+#include "ros/this_node.h"
+#include "ros/names.h"
+
+#include <ros/console.h>
+
+#include <boost/thread/mutex.hpp>
+#include <boost/lexical_cast.hpp>
+
+#include <vector>
+#include <map>
+
+namespace ros
+{
+
+namespace param
+{
+
+typedef std::map<std::string, XmlRpc::XmlRpcValue> M_Param;
+M_Param g_params;
+boost::mutex g_params_mutex;
+S_string g_subscribed_params;
+
+void invalidateParentParams(const std::string& key)
+{
+  std::string ns_key = names::parentNamespace(key);
+  while (ns_key != "" && ns_key != "/")
+  {
+    if (g_subscribed_params.find(ns_key) != g_subscribed_params.end())
+    {
+      // by erasing the key the parameter will be re-queried
+      g_params.erase(ns_key);
+    }
+    ns_key = names::parentNamespace(ns_key);
+  }
+}
+
+void set(const std::string& key, const XmlRpc::XmlRpcValue& v)
+{
+  std::string mapped_key = ros::names::resolve(key);
+
+  XmlRpc::XmlRpcValue params, result, payload;
+  params[0] = this_node::getName();
+  params[1] = mapped_key;
+  params[2] = v;
+
+  {
+    // Lock around the execute to the master in case we get a parameter update on this value between
+    // executing on the master and setting the parameter in the g_params list.
+    boost::mutex::scoped_lock lock(g_params_mutex);
+
+    if (master::execute("setParam", params, result, payload, true))
+    {
+      // Update our cached params list now so that if get() is called immediately after param::set()
+      // we already have the cached state and our value will be correct
+      if (g_subscribed_params.find(mapped_key) != g_subscribed_params.end())
+      {
+        g_params[mapped_key] = v;
+      }
+      invalidateParentParams(mapped_key);
+    }
+  }
+}
+
+void set(const std::string& key, const std::string& s)
+{
+  // construct xmlrpc_c::value object of the std::string and
+  // call param::set(key, xmlvalue);
+  XmlRpc::XmlRpcValue v(s);
+  ros::param::set(key, v);
+}
+
+void set(const std::string& key, const char* s)
+{
+  // construct xmlrpc_c::value object of the std::string and
+  // call param::set(key, xmlvalue);
+  std::string sxx = std::string(s);
+  XmlRpc::XmlRpcValue v(sxx);
+  ros::param::set(key, v);
+}
+
+void set(const std::string& key, double d)
+{
+  XmlRpc::XmlRpcValue v(d);
+  ros::param::set(key, v);
+}
+
+void set(const std::string& key, int i)
+{
+  XmlRpc::XmlRpcValue v(i);
+  ros::param::set(key, v);
+}
+
+void set(const std::string& key, bool b)
+{
+  XmlRpc::XmlRpcValue v(b);
+  ros::param::set(key, v);
+}
+
+template <class T>
+  void setImpl(const std::string& key, const std::vector<T>& vec)
+{
+  // Note: the XmlRpcValue starts off as "invalid" and assertArray turns it
+  // into an array type with the given size
+  XmlRpc::XmlRpcValue xml_vec;
+  xml_vec.setSize(vec.size());
+
+  // Copy the contents into the XmlRpcValue
+  for(size_t i=0; i < vec.size(); i++) {
+    xml_vec[i] = vec.at(i);
+  }
+
+  ros::param::set(key, xml_vec);
+}
+
+void set(const std::string& key, const std::vector<std::string>& vec)
+{
+  setImpl(key, vec);
+}
+
+void set(const std::string& key, const std::vector<double>& vec)
+{
+  setImpl(key, vec);
+}
+
+void set(const std::string& key, const std::vector<float>& vec)
+{
+  setImpl(key, vec);
+}
+
+void set(const std::string& key, const std::vector<int>& vec)
+{
+  setImpl(key, vec);
+}
+
+void set(const std::string& key, const std::vector<bool>& vec)
+{
+  setImpl(key, vec);
+}
+
+template <class T>
+  void setImpl(const std::string& key, const std::map<std::string, T>& map)
+{
+  // Note: the XmlRpcValue starts off as "invalid" and assertStruct turns it
+  // into a struct type
+  XmlRpc::XmlRpcValue xml_value;
+  xml_value.begin();
+
+  // Copy the contents into the XmlRpcValue
+  for(typename std::map<std::string, T>::const_iterator it = map.begin(); it != map.end(); ++it) {
+    xml_value[it->first] = it->second;
+  }
+
+  ros::param::set(key, xml_value);
+}
+
+void set(const std::string& key, const std::map<std::string, std::string>& map)
+{
+  setImpl(key, map);
+}
+
+void set(const std::string& key, const std::map<std::string, double>& map)
+{
+  setImpl(key, map);
+}
+
+void set(const std::string& key, const std::map<std::string, float>& map)
+{
+  setImpl(key, map);
+}
+
+void set(const std::string& key, const std::map<std::string, int>& map)
+{
+  setImpl(key, map);
+}
+
+void set(const std::string& key, const std::map<std::string, bool>& map)
+{
+  setImpl(key, map);
+}
+
+bool has(const std::string& key)
+{
+  XmlRpc::XmlRpcValue params, result, payload;
+  params[0] = this_node::getName();
+  params[1] = ros::names::resolve(key);
+  //params[1] = key;
+  // We don't loop here, because validateXmlrpcResponse() returns false
+  // both when we can't contact the master and when the master says, "I
+  // don't have that param."
+  if (!master::execute("hasParam", params, result, payload, false))
+  {
+    return false;
+  }
+
+  return payload;
+}
+
+bool del(const std::string& key)
+{
+  std::string mapped_key = ros::names::resolve(key);
+
+  {
+    boost::mutex::scoped_lock lock(g_params_mutex);
+
+    g_subscribed_params.erase(mapped_key);
+    g_params.erase(mapped_key);
+  }
+
+  XmlRpc::XmlRpcValue params, result, payload;
+  params[0] = this_node::getName();
+  params[1] = mapped_key;
+  // We don't loop here, because validateXmlrpcResponse() returns false
+  // both when we can't contact the master and when the master says, "I
+  // don't have that param."
+  if (!master::execute("deleteParam", params, result, payload, false))
+  {
+    return false;
+  }
+
+  return true;
+}
+
+bool getImpl(const std::string& key, XmlRpc::XmlRpcValue& v, bool use_cache)
+{
+  std::string mapped_key = ros::names::resolve(key);
+  if (mapped_key.empty()) mapped_key = "/";
+
+  if (use_cache)
+  {
+    boost::mutex::scoped_lock lock(g_params_mutex);
+
+    if (g_subscribed_params.find(mapped_key) != g_subscribed_params.end())
+    {
+      M_Param::iterator it = g_params.find(mapped_key);
+      if (it != g_params.end())
+      {
+        if (it->second.valid())
+        {
+          ROS_DEBUG_NAMED("cached_parameters", "Using cached parameter value for key [%s]", mapped_key.c_str());
+
+          v = it->second;
+          return true;
+        }
+        else
+        {
+          ROS_DEBUG_NAMED("cached_parameters", "Cached parameter is invalid for key [%s]", mapped_key.c_str());
+          return false;
+        }
+      }
+    }
+    else
+    {
+      // parameter we've never seen before, register for update from the master
+      if (g_subscribed_params.insert(mapped_key).second)
+      {
+        XmlRpc::XmlRpcValue params, result, payload;
+        params[0] = this_node::getName();
+        params[1] = XMLRPCManager::instance()->getServerURI();
+        params[2] = mapped_key;
+
+        if (!master::execute("subscribeParam", params, result, payload, false))
+        {
+          ROS_DEBUG_NAMED("cached_parameters", "Subscribe to parameter [%s]: call to the master failed", mapped_key.c_str());
+          g_subscribed_params.erase(mapped_key);
+          use_cache = false;
+        }
+        else
+        {
+          ROS_DEBUG_NAMED("cached_parameters", "Subscribed to parameter [%s]", mapped_key.c_str());
+        }
+      }
+    }
+  }
+
+  XmlRpc::XmlRpcValue params, result;
+  params[0] = this_node::getName();
+  params[1] = mapped_key;
+
+  // We don't loop here, because validateXmlrpcResponse() returns false
+  // both when we can't contact the master and when the master says, "I
+  // don't have that param."
+  bool ret = master::execute("getParam", params, result, v, false);
+
+  if (use_cache)
+  {
+    boost::mutex::scoped_lock lock(g_params_mutex);
+
+    ROS_DEBUG_NAMED("cached_parameters", "Caching parameter [%s] with value type [%d]", mapped_key.c_str(), v.getType());
+    g_params[mapped_key] = v;
+  }
+
+  return ret;
+}
+
+bool getImpl(const std::string& key, std::string& s, bool use_cache)
+{
+  XmlRpc::XmlRpcValue v;
+  if (!getImpl(key, v, use_cache))
+    return false;
+  if (v.getType() != XmlRpc::XmlRpcValue::TypeString)
+    return false;
+  s = std::string(v);
+  return true;
+}
+
+bool getImpl(const std::string& key, double& d, bool use_cache)
+{
+  XmlRpc::XmlRpcValue v;
+  if (!getImpl(key, v, use_cache))
+  {
+    return false;
+  }
+
+  if (v.getType() == XmlRpc::XmlRpcValue::TypeInt)
+  {
+    d = (int)v;
+  }
+  else if (v.getType() != XmlRpc::XmlRpcValue::TypeDouble)
+  {
+    return false;
+  }
+  else
+  {
+    d = v;
+  }
+
+  return true;
+}
+
+bool getImpl(const std::string& key, float& f, bool use_cache)
+{
+  double d = static_cast<double>(f);
+  bool result = getImpl(key, d, use_cache);
+  if (result)
+    f = static_cast<float>(d);
+  return result;
+}
+
+bool getImpl(const std::string& key, int& i, bool use_cache)
+{
+  XmlRpc::XmlRpcValue v;
+  if (!getImpl(key, v, use_cache))
+  {
+    return false;
+  }
+
+  if (v.getType() == XmlRpc::XmlRpcValue::TypeDouble)
+  {
+    double d = v;
+
+    if (fmod(d, 1.0) < 0.5)
+    {
+      d = floor(d);
+    }
+    else
+    {
+      d = ceil(d);
+    }
+
+    i = d;
+  }
+  else if (v.getType() != XmlRpc::XmlRpcValue::TypeInt)
+  {
+    return false;
+  }
+  else
+  {
+    i = v;
+  }
+
+  return true;
+}
+
+bool getImpl(const std::string& key, bool& b, bool use_cache)
+{
+  XmlRpc::XmlRpcValue v;
+  if (!getImpl(key, v, use_cache))
+    return false;
+  if (v.getType() != XmlRpc::XmlRpcValue::TypeBoolean)
+    return false;
+  b = v;
+  return true;
+}
+
+bool get(const std::string& key, std::string& s)
+{
+	return getImpl(key, s, false);
+}
+
+bool get(const std::string& key, double& d)
+{
+	return getImpl(key, d, false);
+}
+
+bool get(const std::string& key, float& f)
+{
+	return getImpl(key, f, false);
+}
+
+bool get(const std::string& key, int& i)
+{
+	return getImpl(key, i, false);
+}
+
+bool get(const std::string& key, bool& b)
+{
+	return getImpl(key, b, false);
+}
+
+bool get(const std::string& key, XmlRpc::XmlRpcValue& v)
+{
+	return getImpl(key, v, false);
+}
+
+bool getCached(const std::string& key, std::string& s)
+{
+	return getImpl(key, s, true);
+}
+
+bool getCached(const std::string& key, double& d)
+{
+	return getImpl(key, d, true);
+}
+
+bool getCached(const std::string& key, float& f)
+{
+	return getImpl(key, f, true);
+}
+
+bool getCached(const std::string& key, int& i)
+{
+	return getImpl(key, i, true);
+}
+
+bool getCached(const std::string& key, bool& b)
+{
+	return getImpl(key, b, true);
+}
+
+bool getCached(const std::string& key, XmlRpc::XmlRpcValue& v)
+{
+	return getImpl(key, v, true);
+}
+
+template <class T> T xml_cast(XmlRpc::XmlRpcValue xml_value) 
+{
+  return static_cast<T>(xml_value);
+}
+
+template <class T> bool xml_castable(int XmlType) 
+{
+  return false;
+}
+
+template<> bool xml_castable<std::string>(int XmlType)
+{
+  return XmlType == XmlRpc::XmlRpcValue::TypeString;
+}
+
+template<> bool xml_castable<double>(int XmlType)
+{
+  return ( 
+      XmlType == XmlRpc::XmlRpcValue::TypeDouble ||
+      XmlType == XmlRpc::XmlRpcValue::TypeInt ||
+      XmlType == XmlRpc::XmlRpcValue::TypeBoolean );
+}
+
+template<> bool xml_castable<float>(int XmlType)
+{
+  return ( 
+      XmlType == XmlRpc::XmlRpcValue::TypeDouble ||
+      XmlType == XmlRpc::XmlRpcValue::TypeInt ||
+      XmlType == XmlRpc::XmlRpcValue::TypeBoolean );
+}
+
+template<> bool xml_castable<int>(int XmlType)
+{
+  return ( 
+      XmlType == XmlRpc::XmlRpcValue::TypeDouble ||
+      XmlType == XmlRpc::XmlRpcValue::TypeInt ||
+      XmlType == XmlRpc::XmlRpcValue::TypeBoolean );
+}
+
+template<> bool xml_castable<bool>(int XmlType)
+{
+  return ( 
+      XmlType == XmlRpc::XmlRpcValue::TypeDouble ||
+      XmlType == XmlRpc::XmlRpcValue::TypeInt ||
+      XmlType == XmlRpc::XmlRpcValue::TypeBoolean );
+}
+
+template<> double xml_cast(XmlRpc::XmlRpcValue xml_value)
+{
+  using namespace XmlRpc;
+  switch(xml_value.getType()) {
+    case XmlRpcValue::TypeDouble:
+      return static_cast<double>(xml_value);
+    case XmlRpcValue::TypeInt:
+      return static_cast<double>(static_cast<int>(xml_value));
+    case XmlRpcValue::TypeBoolean:
+      return static_cast<double>(static_cast<bool>(xml_value));
+    default:
+     return 0.0;
+  };
+}
+
+template<> float xml_cast(XmlRpc::XmlRpcValue xml_value)
+{
+  using namespace XmlRpc;
+  switch(xml_value.getType()) {
+    case XmlRpcValue::TypeDouble:
+      return static_cast<float>(static_cast<double>(xml_value));
+    case XmlRpcValue::TypeInt:
+      return static_cast<float>(static_cast<int>(xml_value));
+    case XmlRpcValue::TypeBoolean:
+      return static_cast<float>(static_cast<bool>(xml_value));
+    default:
+      return 0.0f;
+  };
+}
+
+template<> int xml_cast(XmlRpc::XmlRpcValue xml_value)
+{
+  using namespace XmlRpc;
+  switch(xml_value.getType()) {
+    case XmlRpcValue::TypeDouble:
+      return static_cast<int>(static_cast<double>(xml_value));
+    case XmlRpcValue::TypeInt:
+      return static_cast<int>(xml_value);
+    case XmlRpcValue::TypeBoolean:
+      return static_cast<int>(static_cast<bool>(xml_value));
+    default:
+      return 0;
+  };
+}
+
+template<> bool xml_cast(XmlRpc::XmlRpcValue xml_value)
+{
+  using namespace XmlRpc;
+  switch(xml_value.getType()) {
+    case XmlRpcValue::TypeDouble:
+      return static_cast<bool>(static_cast<double>(xml_value));
+    case XmlRpcValue::TypeInt:
+      return static_cast<bool>(static_cast<int>(xml_value));
+    case XmlRpcValue::TypeBoolean:
+      return static_cast<bool>(xml_value);
+    default:
+      return false;
+  };
+}
+  
+template <class T>
+  bool getImpl(const std::string& key, std::vector<T>& vec, bool cached)
+{
+  XmlRpc::XmlRpcValue xml_array;
+  if(!getImpl(key, xml_array, cached)) {
+    return false;
+  }
+
+  // Make sure it's an array type
+  if(xml_array.getType() != XmlRpc::XmlRpcValue::TypeArray) {
+    return false;
+  }
+
+  // Resize the target vector (destructive)
+  vec.resize(xml_array.size());
+
+  // Fill the vector with stuff
+  for (int i = 0; i < xml_array.size(); i++) {
+    if(!xml_castable<T>(xml_array[i].getType())) {
+      return false;
+    }
+
+    vec[i] = xml_cast<T>(xml_array[i]);
+  }
+
+  return true;
+}
+
+bool get(const std::string& key, std::vector<std::string>& vec)
+{
+  return getImpl(key, vec, false);
+}
+bool get(const std::string& key, std::vector<double>& vec)
+{
+  return getImpl(key, vec, false);
+}
+bool get(const std::string& key, std::vector<float>& vec)
+{
+  return getImpl(key, vec, false);
+}
+bool get(const std::string& key, std::vector<int>& vec)
+{
+  return getImpl(key, vec, false);
+}
+bool get(const std::string& key, std::vector<bool>& vec)
+{
+  return getImpl(key, vec, false);
+}
+
+bool getCached(const std::string& key, std::vector<std::string>& vec)
+{
+  return getImpl(key, vec, true);
+}
+bool getCached(const std::string& key, std::vector<double>& vec)
+{
+  return getImpl(key, vec, true);
+}
+bool getCached(const std::string& key, std::vector<float>& vec)
+{
+  return getImpl(key, vec, true);
+}
+bool getCached(const std::string& key, std::vector<int>& vec)
+{
+  return getImpl(key, vec, true);
+}
+bool getCached(const std::string& key, std::vector<bool>& vec)
+{
+  return getImpl(key, vec, true);
+}
+
+template <class T>
+  bool getImpl(const std::string& key, std::map<std::string, T>& map, bool cached)
+{
+  XmlRpc::XmlRpcValue xml_value;
+  if(!getImpl(key, xml_value, cached)) {
+    return false;
+  }
+
+  // Make sure it's a struct type
+  if(xml_value.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
+    return false;
+  }
+
+  // Fill the map with stuff
+  for (XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = xml_value.begin();
+      it != xml_value.end();
+      ++it)
+  {
+    // Make sure this element is the right type
+    if(!xml_castable<T>(it->second.getType())) {
+      return false;
+    }
+    // Store the element
+    map[it->first] = xml_cast<T>(it->second);
+  }
+
+  return true;
+}
+
+bool get(const std::string& key, std::map<std::string, std::string>& map)
+{
+  return getImpl(key, map, false);
+}
+bool get(const std::string& key, std::map<std::string, double>& map)
+{
+  return getImpl(key, map, false);
+}
+bool get(const std::string& key, std::map<std::string, float>& map)
+{
+  return getImpl(key, map, false);
+}
+bool get(const std::string& key, std::map<std::string, int>& map)
+{
+  return getImpl(key, map, false);
+}
+bool get(const std::string& key, std::map<std::string, bool>& map)
+{
+  return getImpl(key, map, false);
+}
+
+bool getCached(const std::string& key, std::map<std::string, std::string>& map)
+{
+  return getImpl(key, map, true);
+}
+bool getCached(const std::string& key, std::map<std::string, double>& map)
+{
+  return getImpl(key, map, true);
+}
+bool getCached(const std::string& key, std::map<std::string, float>& map)
+{
+  return getImpl(key, map, true);
+}
+bool getCached(const std::string& key, std::map<std::string, int>& map)
+{
+  return getImpl(key, map, true);
+}
+bool getCached(const std::string& key, std::map<std::string, bool>& map)
+{
+  return getImpl(key, map, true);
+}
+
+bool getParamNames(std::vector<std::string>& keys)
+{
+  XmlRpc::XmlRpcValue params, result, payload;
+  params[0] = this_node::getName();
+  if (!master::execute("getParamNames", params, result, payload, false)) {
+    return false;
+  }
+  // Make sure it's an array type
+  if (result.getType() != XmlRpc::XmlRpcValue::TypeArray) {
+    return false;
+  }
+  // Make sure it returned 3 elements
+  if (result.size() != 3) {
+    return false;
+  }
+  // Get the actual parameter keys
+  XmlRpc::XmlRpcValue parameters = result[2];
+  // Resize the output
+  keys.resize(parameters.size());
+
+  // Fill the output vector with the answer
+  for (int i = 0; i < parameters.size(); ++i) {
+    if (parameters[i].getType() != XmlRpc::XmlRpcValue::TypeString) {
+      return false;
+    }
+    keys[i] = std::string(parameters[i]);
+  }
+  return true;
+}
+
+bool search(const std::string& key, std::string& result_out)
+{
+  return search(this_node::getName(), key, result_out);
+}
+
+bool search(const std::string& ns, const std::string& key, std::string& result_out)
+{
+  XmlRpc::XmlRpcValue params, result, payload;
+  params[0] = ns;
+
+  // searchParam needs a separate form of remapping -- remapping on the unresolved name, rather than the
+  // resolved one.
+
+  std::string remapped = key;
+  M_string::const_iterator it = names::getUnresolvedRemappings().find(key);
+  if (it != names::getUnresolvedRemappings().end())
+  {
+    remapped = it->second;
+  }
+
+  params[1] = remapped;
+  // We don't loop here, because validateXmlrpcResponse() returns false
+  // both when we can't contact the master and when the master says, "I
+  // don't have that param."
+  if (!master::execute("searchParam", params, result, payload, false))
+  {
+    return false;
+  }
+
+  result_out = (std::string)payload;
+
+  return true;
+}
+
+void update(const std::string& key, const XmlRpc::XmlRpcValue& v)
+{
+  std::string clean_key = names::clean(key);
+  ROS_DEBUG_NAMED("cached_parameters", "Received parameter update for key [%s]", clean_key.c_str());
+
+  boost::mutex::scoped_lock lock(g_params_mutex);
+
+  if (g_subscribed_params.find(clean_key) != g_subscribed_params.end())
+  {
+    g_params[clean_key] = v;
+  }
+  invalidateParentParams(clean_key);
+}
+
+void paramUpdateCallback(XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result)
+{
+  result[0] = 1;
+  result[1] = std::string("");
+  result[2] = 0;
+
+  ros::param::update((std::string)params[1], params[2]);
+}
+
+void init(const M_string& remappings)
+{
+  M_string::const_iterator it = remappings.begin();
+  M_string::const_iterator end = remappings.end();
+  for (; it != end; ++it)
+  {
+    const std::string& name = it->first;
+    const std::string& param = it->second;
+
+    if (name.size() < 2)
+    {
+      continue;
+    }
+
+    if (name[0] == '_' && name[1] != '_')
+    {
+      std::string local_name = "~" + name.substr(1);
+
+      bool success = false;
+
+      try
+      {
+        int32_t i = boost::lexical_cast<int32_t>(param);
+        ros::param::set(names::resolve(local_name), i);
+        success = true;
+      }
+      catch (boost::bad_lexical_cast&)
+      {
+
+      }
+
+      if (success)
+      {
+        continue;
+      }
+
+      try
+      {
+        double d = boost::lexical_cast<double>(param);
+        ros::param::set(names::resolve(local_name), d);
+        success = true;
+      }
+      catch (boost::bad_lexical_cast&)
+      {
+
+      }
+
+      if (success)
+      {
+        continue;
+      }
+
+      if (param == "true" || param == "True" || param == "TRUE")
+      {
+        ros::param::set(names::resolve(local_name), true);
+      }
+      else if (param == "false" || param == "False" || param == "FALSE")
+      {
+        ros::param::set(names::resolve(local_name), false);
+      }
+      else
+      {
+        ros::param::set(names::resolve(local_name), param);
+      }
+    }
+  }
+
+  XMLRPCManager::instance()->bind("paramUpdate", paramUpdateCallback);
+}
+
+} // namespace param
+
+} // namespace ros

thirdparty/ros/ros_comm/clients/roscpp/src/libros/poll_manager.cpp

@@ -0,0 +1,104 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names 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 "ros/poll_manager.h"
+#include "ros/common.h"
+
+#include <signal.h>
+
+namespace ros
+{
+
+const PollManagerPtr& PollManager::instance()
+{
+  static PollManagerPtr poll_manager = boost::make_shared<PollManager>();
+  return poll_manager;
+}
+
+PollManager::PollManager()
+  : shutting_down_(false)
+{
+}
+
+PollManager::~PollManager()
+{
+  shutdown();
+}
+
+void PollManager::start()
+{
+  shutting_down_ = false;
+  thread_ = boost::thread(&PollManager::threadFunc, this);
+}
+
+void PollManager::shutdown()
+{
+  if (shutting_down_) return;
+
+  shutting_down_ = true;
+  if (thread_.get_id() != boost::this_thread::get_id())
+  {
+    thread_.join();
+  }
+
+  boost::recursive_mutex::scoped_lock lock(signal_mutex_);
+  poll_signal_.disconnect_all_slots();
+}
+
+void PollManager::threadFunc()
+{
+  disableAllSignalsInThisThread();
+
+  while (!shutting_down_)
+  {
+    {
+      boost::recursive_mutex::scoped_lock lock(signal_mutex_);
+      poll_signal_();
+    }
+
+    if (shutting_down_)
+    {
+      return;
+    }
+
+    poll_set_.update(100);
+  }
+}
+
+boost::signals2::connection PollManager::addPollThreadListener(const VoidFunc& func)
+{
+  boost::recursive_mutex::scoped_lock lock(signal_mutex_);
+  return poll_signal_.connect(func);
+}
+
+void PollManager::removePollThreadListener(boost::signals2::connection c)
+{
+  boost::recursive_mutex::scoped_lock lock(signal_mutex_);
+  c.disconnect();
+}
+
+}

thirdparty/ros/ros_comm/clients/roscpp/src/libros/poll_set.cpp

@@ -0,0 +1,300 @@
+/*
+ * 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 "ros/poll_set.h"
+#include "ros/file_log.h"
+
+#include "ros/transport/transport.h"
+
+#include <ros/assert.h>
+
+#include <boost/bind.hpp>
+
+#include <fcntl.h>
+
+namespace ros
+{
+
+PollSet::PollSet()
+    : sockets_changed_(false), epfd_(create_socket_watcher())
+{
+	if ( create_signal_pair(signal_pipe_) != 0 ) {
+        ROS_FATAL("create_signal_pair() failed");
+    ROS_BREAK();
+  }
+  addSocket(signal_pipe_[0], boost::bind(&PollSet::onLocalPipeEvents, this, _1));
+  addEvents(signal_pipe_[0], POLLIN);
+}
+
+PollSet::~PollSet()
+{
+  close_signal_pair(signal_pipe_);
+  close_socket_watcher(epfd_);
+}
+
+bool PollSet::addSocket(int fd, const SocketUpdateFunc& update_func, const TransportPtr& transport)
+{
+  SocketInfo info;
+  info.fd_ = fd;
+  info.events_ = 0;
+  info.transport_ = transport;
+  info.func_ = update_func;
+
+  {
+    boost::mutex::scoped_lock lock(socket_info_mutex_);
+
+    bool b = socket_info_.insert(std::make_pair(fd, info)).second;
+    if (!b)
+    {
+      ROSCPP_LOG_DEBUG("PollSet: Tried to add duplicate fd [%d]", fd);
+      return false;
+    }
+
+    add_socket_to_watcher(epfd_, fd);
+
+    sockets_changed_ = true;
+  }
+
+  signal();
+
+  return true;
+}
+
+bool PollSet::delSocket(int fd)
+{
+  if(fd < 0)
+  {
+    return false;
+  }
+
+  boost::mutex::scoped_lock lock(socket_info_mutex_);
+  M_SocketInfo::iterator it = socket_info_.find(fd);
+  if (it != socket_info_.end())
+  {
+    socket_info_.erase(it);
+
+    {
+      boost::mutex::scoped_lock lock(just_deleted_mutex_);
+      just_deleted_.push_back(fd);
+    }
+
+    del_socket_from_watcher(epfd_, fd);
+
+    sockets_changed_ = true;
+    signal();
+
+    return true;
+  }
+
+  ROSCPP_LOG_DEBUG("PollSet: Tried to delete fd [%d] which is not being tracked", fd);
+
+  return false;
+}
+
+
+bool PollSet::addEvents(int sock, int events)
+{
+  boost::mutex::scoped_lock lock(socket_info_mutex_);
+
+  M_SocketInfo::iterator it = socket_info_.find(sock);
+
+  if (it == socket_info_.end())
+  {
+    ROSCPP_LOG_DEBUG("PollSet: Tried to add events [%d] to fd [%d] which does not exist in this pollset", events, sock);
+    return false;
+  }
+
+  it->second.events_ |= events;
+
+  set_events_on_socket(epfd_, sock, it->second.events_);
+
+  sockets_changed_ = true;
+  signal();
+
+  return true;
+}
+
+bool PollSet::delEvents(int sock, int events)
+{
+  boost::mutex::scoped_lock lock(socket_info_mutex_);
+
+  M_SocketInfo::iterator it = socket_info_.find(sock);
+  if (it != socket_info_.end())
+  {
+    it->second.events_ &= ~events;
+  }
+  else
+  {
+    ROSCPP_LOG_DEBUG("PollSet: Tried to delete events [%d] to fd [%d] which does not exist in this pollset", events, sock);
+    return false;
+  }
+
+  set_events_on_socket(epfd_, sock, it->second.events_);
+
+  sockets_changed_ = true;
+  signal();
+
+  return true;
+}
+
+void PollSet::signal()
+{
+  boost::mutex::scoped_try_lock lock(signal_mutex_);
+
+  if (lock.owns_lock())
+  {
+    char b = 0;
+    if (write_signal(signal_pipe_[1], &b, 1) < 0)
+    {
+      // do nothing... this prevents warnings on gcc 4.3
+    }
+  }
+}
+
+
+void PollSet::update(int poll_timeout)
+{
+  createNativePollset();
+
+  // Poll across the sockets we're servicing
+  boost::shared_ptr<std::vector<socket_pollfd> > ofds = poll_sockets(epfd_, &ufds_.front(), ufds_.size(), poll_timeout);
+  if (!ofds)
+  {
+    ROS_ERROR_STREAM("poll failed with error " << last_socket_error_string());
+  }
+  else
+  {
+    for (std::vector<socket_pollfd>::iterator it = ofds->begin() ; it != ofds->end(); ++it)
+    {
+      int fd = it->fd;
+      int revents = it->revents;
+      SocketUpdateFunc func;
+      TransportPtr transport;
+      int events = 0;
+
+      if (revents == 0)
+      {
+        continue;
+      }
+      {
+        boost::mutex::scoped_lock lock(socket_info_mutex_);
+        M_SocketInfo::iterator it = socket_info_.find(fd);
+        // the socket has been entirely deleted
+        if (it == socket_info_.end())
+        {
+          continue;
+        }
+
+        const SocketInfo& info = it->second;
+
+        // Store off the function and transport in case the socket is deleted from another thread
+        func = info.func_;
+        transport = info.transport_;
+        events = info.events_;
+      }
+
+      // If these are registered events for this socket, OR the events are ERR/HUP/NVAL,
+      // call through to the registered function
+      if (func
+          && ((events & revents)
+              || (revents & POLLERR)
+              || (revents & POLLHUP)
+              || (revents & POLLNVAL)))
+      {
+        bool skip = false;
+        if (revents & (POLLNVAL|POLLERR|POLLHUP))
+        {
+          // If a socket was just closed and then the file descriptor immediately reused, we can
+          // get in here with what we think is a valid socket (since it was just re-added to our set)
+          // but which is actually referring to the previous fd with the same #.  If this is the case,
+          // we ignore the first instance of one of these errors.  If it's a real error we'll
+          // hit it again next time through.
+          boost::mutex::scoped_lock lock(just_deleted_mutex_);
+          if (std::find(just_deleted_.begin(), just_deleted_.end(), fd) != just_deleted_.end())
+          {
+            skip = true;
+          }
+        }
+
+        if (!skip)
+        {
+          func(revents & (events|POLLERR|POLLHUP|POLLNVAL));
+        }
+      }
+    }
+  }
+
+  boost::mutex::scoped_lock lock(just_deleted_mutex_);
+  just_deleted_.clear();
+
+}
+
+void PollSet::createNativePollset()
+{
+  boost::mutex::scoped_lock lock(socket_info_mutex_);
+
+  if (!sockets_changed_)
+  {
+    return;
+  }
+
+  // Build the list of structures to pass to poll for the sockets we're servicing
+  ufds_.resize(socket_info_.size());
+  M_SocketInfo::iterator sock_it = socket_info_.begin();
+  M_SocketInfo::iterator sock_end = socket_info_.end();
+  for (int i = 0; sock_it != sock_end; ++sock_it, ++i)
+  {
+    const SocketInfo& info = sock_it->second;
+    socket_pollfd& pfd = ufds_[i];
+    pfd.fd = info.fd_;
+    pfd.events = info.events_;
+    pfd.revents = 0;
+  }
+  sockets_changed_ = false;
+}
+
+void PollSet::onLocalPipeEvents(int events)
+{
+  if(events & POLLIN)
+  {
+    char b;
+    while(read_signal(signal_pipe_[0], &b, 1) > 0)
+    {
+      //do nothing keep draining
+    };
+  }
+
+}
+
+}

thirdparty/ros/ros_comm/clients/roscpp/src/libros/publication.cpp

@@ -0,0 +1,507 @@
+/*
+ * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
+ *
+ * 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 names of Stanford University or 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 "ros/publication.h"
+#include "ros/subscriber_link.h"
+#include "ros/connection.h"
+#include "ros/callback_queue_interface.h"
+#include "ros/single_subscriber_publisher.h"
+#include "ros/serialization.h"
+#include <std_msgs/Header.h>
+
+namespace ros
+{
+
+class PeerConnDisconnCallback : public CallbackInterface
+{
+public:
+  PeerConnDisconnCallback(const SubscriberStatusCallback& callback, const SubscriberLinkPtr& sub_link, bool use_tracked_object, const VoidConstWPtr& tracked_object)
+  : callback_(callback)
+  , sub_link_(sub_link)
+  , use_tracked_object_(use_tracked_object)
+  , tracked_object_(tracked_object)
+  {
+  }
+
+  virtual CallResult call()
+  {
+    VoidConstPtr tracker;
+    if (use_tracked_object_)
+    {
+      tracker = tracked_object_.lock();
+
+      if (!tracker)
+      {
+        return Invalid;
+      }
+    }
+
+    SingleSubscriberPublisher pub(sub_link_);
+    callback_(pub);
+
+    return Success;
+  }
+
+private:
+  SubscriberStatusCallback callback_;
+  SubscriberLinkPtr sub_link_;
+  bool use_tracked_object_;
+  VoidConstWPtr tracked_object_;
+};
+
+Publication::Publication(const std::string &name,
+                         const std::string &datatype,
+                         const std::string &_md5sum,
+                         const std::string& message_definition,
+                         size_t max_queue,
+                         bool latch,
+                         bool has_header)
+: name_(name),
+  datatype_(datatype),
+  md5sum_(_md5sum),
+  message_definition_(message_definition),
+  max_queue_(max_queue),
+  seq_(0),
+  dropped_(false),
+  latch_(latch),
+  has_header_(has_header),
+  intraprocess_subscriber_count_(0)
+{
+}
+
+Publication::~Publication()
+{
+  drop();
+}
+
+void Publication::addCallbacks(const SubscriberCallbacksPtr& callbacks)
+{
+  boost::mutex::scoped_lock lock(callbacks_mutex_);
+
+  callbacks_.push_back(callbacks);
+
+  // Add connect callbacks for all current subscriptions if this publisher wants them
+  if (callbacks->connect_ && callbacks->callback_queue_)
+  {
+    boost::mutex::scoped_lock lock(subscriber_links_mutex_);
+    V_SubscriberLink::iterator it = subscriber_links_.begin();
+    V_SubscriberLink::iterator end = subscriber_links_.end();
+    for (; it != end; ++it)
+    {
+      const SubscriberLinkPtr& sub_link = *it;
+      CallbackInterfacePtr cb(boost::make_shared<PeerConnDisconnCallback>(callbacks->connect_, sub_link, callbacks->has_tracked_object_, callbacks->tracked_object_));
+      callbacks->callback_queue_->addCallback(cb, (uint64_t)callbacks.get());
+    }
+  }
+}
+
+void Publication::removeCallbacks(const SubscriberCallbacksPtr& callbacks)
+{
+  boost::mutex::scoped_lock lock(callbacks_mutex_);
+
+  V_Callback::iterator it = std::find(callbacks_.begin(), callbacks_.end(), callbacks);
+  if (it != callbacks_.end())
+  {
+    const SubscriberCallbacksPtr& cb = *it;
+    if (cb->callback_queue_)
+    {
+      cb->callback_queue_->removeByID((uint64_t)cb.get());
+    }
+    callbacks_.erase(it);
+  }
+}
+
+void Publication::drop()
+{
+  // grab a lock here, to ensure that no subscription callback will
+  // be invoked after we return
+  {
+    boost::mutex::scoped_lock lock(publish_queue_mutex_);
+    boost::mutex::scoped_lock lock2(subscriber_links_mutex_);
+
+    if (dropped_)
+    {
+      return;
+    }
+
+    dropped_ = true;
+  }
+
+  dropAllConnections();
+}
+
+bool Publication::enqueueMessage(const SerializedMessage& m)
+{
+  boost::mutex::scoped_lock lock(subscriber_links_mutex_);
+  if (dropped_)
+  {
+    return false;
+  }
+
+  ROS_ASSERT(m.buf);
+
+  uint32_t seq = incrementSequence();
+  if (has_header_)
+  {
+    // If we have a header, we know it's immediately after the message length
+    // Deserialize it, write the sequence, and then serialize it again.
+    namespace ser = ros::serialization;
+    std_msgs::Header header;
+    ser::IStream istream(m.buf.get() + 4, m.num_bytes - 4);
+    ser::deserialize(istream, header);
+    header.seq = seq;
+    ser::OStream ostream(m.buf.get() + 4, m.num_bytes - 4);
+    ser::serialize(ostream, header);
+  }
+
+  for(V_SubscriberLink::iterator i = subscriber_links_.begin();
+      i != subscriber_links_.end(); ++i)
+  {
+    const SubscriberLinkPtr& sub_link = (*i);
+    sub_link->enqueueMessage(m, true, false);
+  }
+
+  if (latch_)
+  {
+    last_message_ = m;
+  }
+
+  return true;
+}
+
+void Publication::addSubscriberLink(const SubscriberLinkPtr& sub_link)
+{
+  {
+    boost::mutex::scoped_lock lock(subscriber_links_mutex_);
+
+    if (dropped_)
+    {
+      return;
+    }
+
+    subscriber_links_.push_back(sub_link);
+
+    if (sub_link->isIntraprocess())
+    {
+      ++intraprocess_subscriber_count_;
+    }
+  }
+
+  if (latch_ && last_message_.buf)
+  {
+    sub_link->enqueueMessage(last_message_, true, true);
+  }
+
+  // This call invokes the subscribe callback if there is one.
+  // This must happen *after* the push_back above, in case the
+  // callback uses publish().
+  peerConnect(sub_link);
+}
+
+void Publication::removeSubscriberLink(const SubscriberLinkPtr& sub_link)
+{
+  SubscriberLinkPtr link;
+  {
+    boost::mutex::scoped_lock lock(subscriber_links_mutex_);
+
+    if (dropped_)
+    {
+      return;
+    }
+
+    if (sub_link->isIntraprocess())
+    {
+      --intraprocess_subscriber_count_;
+    }
+
+    V_SubscriberLink::iterator it = std::find(subscriber_links_.begin(), subscriber_links_.end(), sub_link);
+    if (it != subscriber_links_.end())
+    {
+      link = *it;
+      subscriber_links_.erase(it);
+    }
+  }
+
+  if (link)
+  {
+    peerDisconnect(link);
+  }
+}
+
+XmlRpc::XmlRpcValue Publication::getStats()
+{
+  XmlRpc::XmlRpcValue stats;
+  stats[0] = name_;
+  XmlRpc::XmlRpcValue conn_data;
+  conn_data.setSize(0); // force to be an array, even if it's empty
+
+  boost::mutex::scoped_lock lock(subscriber_links_mutex_);
+
+  uint32_t cidx = 0;
+  for (V_SubscriberLink::iterator c = subscriber_links_.begin();
+       c != subscriber_links_.end(); ++c, cidx++)
+  {
+    const SubscriberLink::Stats& s = (*c)->getStats();
+    conn_data[cidx][0] = (*c)->getConnectionID();
+    // todo: figure out what to do here... the bytes_sent will wrap around
+    // on some flows within a reasonable amount of time. xmlrpc++ doesn't
+    // seem to give me a nice way to do 64-bit ints, perhaps that's a
+    // limitation of xml-rpc, not sure. alternatively we could send the number
+    // of KB transmitted to gain a few order of magnitude.
+    conn_data[cidx][1] = (int)s.bytes_sent_;
+    conn_data[cidx][2] = (int)s.message_data_sent_;
+    conn_data[cidx][3] = (int)s.messages_sent_;
+    conn_data[cidx][4] = 0; // not sure what is meant by connected
+  }
+
+  stats[1] = conn_data;
+  return stats;
+}
+
+// Publisher : [(connection_id, destination_caller_id, direction, transport, topic_name, connected, connection_info_string)*]
+// e.g. [(2, '/listener', 'o', 'TCPROS', '/chatter', 1, 'TCPROS connection on port 55878 to [127.0.0.1:44273 on socket 7]')]
+void Publication::getInfo(XmlRpc::XmlRpcValue& info)
+{
+  boost::mutex::scoped_lock lock(subscriber_links_mutex_);
+
+  for (V_SubscriberLink::iterator c = subscriber_links_.begin();
+       c != subscriber_links_.end(); ++c)
+  {
+    XmlRpc::XmlRpcValue curr_info;
+    curr_info[0] = (int)(*c)->getConnectionID();
+    curr_info[1] = (*c)->getDestinationCallerID();
+    curr_info[2] = "o";
+    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;
+  }
+}
+
+void Publication::dropAllConnections()
+{
+  // Swap our publishers 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 publishers mutex
+  V_SubscriberLink local_publishers;
+
+  {
+    boost::mutex::scoped_lock lock(subscriber_links_mutex_);
+
+    local_publishers.swap(subscriber_links_);
+  }
+
+  for (V_SubscriberLink::iterator i = local_publishers.begin();
+           i != local_publishers.end(); ++i)
+  {
+    (*i)->drop();
+  }
+}
+
+void Publication::peerConnect(const SubscriberLinkPtr& sub_link)
+{
+  V_Callback::iterator it = callbacks_.begin();
+  V_Callback::iterator end = callbacks_.end();
+  for (; it != end; ++it)
+  {
+    const SubscriberCallbacksPtr& cbs = *it;
+    if (cbs->connect_ && cbs->callback_queue_)
+    {
+      CallbackInterfacePtr cb(boost::make_shared<PeerConnDisconnCallback>(cbs->connect_, sub_link, cbs->has_tracked_object_, cbs->tracked_object_));
+      cbs->callback_queue_->addCallback(cb, (uint64_t)cbs.get());
+    }
+  }
+}
+
+void Publication::peerDisconnect(const SubscriberLinkPtr& sub_link)
+{
+  V_Callback::iterator it = callbacks_.begin();
+  V_Callback::iterator end = callbacks_.end();
+  for (; it != end; ++it)
+  {
+    const SubscriberCallbacksPtr& cbs = *it;
+    if (cbs->disconnect_ && cbs->callback_queue_)
+    {
+      CallbackInterfacePtr cb(boost::make_shared<PeerConnDisconnCallback>(cbs->disconnect_, sub_link, cbs->has_tracked_object_, cbs->tracked_object_));
+      cbs->callback_queue_->addCallback(cb, (uint64_t)cbs.get());
+    }
+  }
+}
+
+size_t Publication::getNumCallbacks()
+{
+  boost::mutex::scoped_lock lock(callbacks_mutex_);
+  return callbacks_.size();
+}
+
+uint32_t Publication::incrementSequence()
+{
+  boost::mutex::scoped_lock lock(seq_mutex_);
+  uint32_t old_seq = seq_;
+  ++seq_;
+
+  return old_seq;
+}
+
+uint32_t Publication::getNumSubscribers()
+{
+  boost::mutex::scoped_lock lock(subscriber_links_mutex_);
+  return (uint32_t)subscriber_links_.size();
+}
+
+void Publication::getPublishTypes(bool& serialize, bool& nocopy, const std::type_info& ti)
+{
+  boost::mutex::scoped_lock lock(subscriber_links_mutex_);
+  V_SubscriberLink::const_iterator it = subscriber_links_.begin();
+  V_SubscriberLink::const_iterator end = subscriber_links_.end();
+  for (; it != end; ++it)
+  {
+    const SubscriberLinkPtr& sub = *it;
+    bool s = false;
+    bool n = false;
+    sub->getPublishTypes(s, n, ti);
+    serialize = serialize || s;
+    nocopy = nocopy || n;
+
+    if (serialize && nocopy)
+    {
+      break;
+    }
+  }
+}
+
+bool Publication::hasSubscribers()
+{
+  boost::mutex::scoped_lock lock(subscriber_links_mutex_);
+  return !subscriber_links_.empty();
+}
+
+void Publication::publish(SerializedMessage& m)
+{
+  if (m.message)
+  {
+    boost::mutex::scoped_lock lock(subscriber_links_mutex_);
+    V_SubscriberLink::const_iterator it = subscriber_links_.begin();
+    V_SubscriberLink::const_iterator end = subscriber_links_.end();
+    for (; it != end; ++it)
+    {
+      const SubscriberLinkPtr& sub = *it;
+      if (sub->isIntraprocess())
+      {
+        sub->enqueueMessage(m, false, true);
+      }
+    }
+
+    m.message.reset();
+  }
+
+  if (m.buf)
+  {
+    boost::mutex::scoped_lock lock(publish_queue_mutex_);
+    publish_queue_.push_back(m);
+  }
+}
+
+void Publication::processPublishQueue()
+{
+  V_SerializedMessage queue;
+  {
+    boost::mutex::scoped_lock lock(publish_queue_mutex_);
+
+    if (dropped_)
+    {
+      return;
+    }
+
+    queue.insert(queue.end(), publish_queue_.begin(), publish_queue_.end());
+    publish_queue_.clear();
+  }
+
+  if (queue.empty())
+  {
+    return;
+  }
+
+  V_SerializedMessage::iterator it = queue.begin();
+  V_SerializedMessage::iterator end = queue.end();
+  for (; it != end; ++it)
+  {
+    enqueueMessage(*it);
+  }
+}
+
+bool Publication::validateHeader(const Header& header, std::string& error_msg)
+{
+  std::string md5sum, topic, client_callerid;
+  if (!header.getValue("md5sum", md5sum)
+   || !header.getValue("topic", topic)
+   || !header.getValue("callerid", client_callerid))
+  {
+    std::string msg("Header from subscriber did not have the required elements: md5sum, topic, callerid");
+
+    ROS_ERROR("%s", msg.c_str());
+    error_msg = msg;
+
+    return false;
+  }
+
+  // Check whether the topic has been deleted from
+  // advertised_topics through a call to unadvertise(), which could
+  // have happened while we were waiting for the subscriber to
+  // provide the md5sum.
+  if(isDropped())
+  {
+    std::string msg = std::string("received a tcpros connection for a nonexistent topic [") +
+                topic + std::string("] from [" + client_callerid +"].");
+
+    ROS_ERROR("%s", msg.c_str());
+    error_msg = msg;
+
+    return false;
+  }
+
+  if (getMD5Sum() != md5sum &&
+      (md5sum != std::string("*") && getMD5Sum() != std::string("*")))
+  {
+    std::string datatype;
+    header.getValue("type", datatype);
+
+    std::string msg = std::string("Client [") + client_callerid + std::string("] wants topic ") + topic +
+                      std::string(" to have datatype/md5sum [") + datatype + "/" + md5sum +
+                      std::string("], but our version has [") + getDataType() + "/" + getMD5Sum() +
+                      std::string("]. Dropping connection.");
+
+    ROS_ERROR("%s", msg.c_str());
+    error_msg = msg;
+
+    return false;
+  }
+
+  return true;
+}
+
+} // namespace ros

thirdparty/ros/ros_comm/clients/roscpp/src/libros/publisher.cpp

@@ -0,0 +1,149 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Stanford University or 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 "ros/publisher.h"
+#include "ros/publication.h"
+#include "ros/node_handle.h"
+#include "ros/topic_manager.h"
+
+namespace ros
+{
+
+Publisher::Impl::Impl() : unadvertised_(false) { }
+
+Publisher::Impl::~Impl()
+{
+  ROS_DEBUG("Publisher on '%s' deregistering callbacks.", topic_.c_str());
+  unadvertise();
+}
+
+bool Publisher::Impl::isValid() const
+{
+  return !unadvertised_;
+}
+
+void Publisher::Impl::unadvertise()
+{
+  if (!unadvertised_)
+  {
+    unadvertised_ = true;
+    TopicManager::instance()->unadvertise(topic_, callbacks_);
+    node_handle_.reset();
+  }
+}
+
+Publisher::Publisher(const std::string& topic, const std::string& md5sum, const std::string& datatype, const NodeHandle& node_handle, const SubscriberCallbacksPtr& callbacks)
+: impl_(boost::make_shared<Impl>())
+{
+  impl_->topic_ = topic;
+  impl_->md5sum_ = md5sum;
+  impl_->datatype_ = datatype;
+  impl_->node_handle_ = boost::make_shared<NodeHandle>(node_handle);
+  impl_->callbacks_ = callbacks;
+}
+
+Publisher::Publisher(const Publisher& rhs)
+{
+  impl_ = rhs.impl_;
+}
+
+Publisher::~Publisher()
+{
+}
+
+void Publisher::publish(const boost::function<SerializedMessage(void)>& serfunc, SerializedMessage& m) const
+{
+  if (!impl_)
+  {
+    ROS_ASSERT_MSG(false, "Call to publish() on an invalid Publisher (topic [%s])", impl_->topic_.c_str());
+    return;
+  }
+
+  if (!impl_->isValid())
+  {
+    ROS_ASSERT_MSG(false, "Call to publish() on an invalid Publisher (topic [%s])", impl_->topic_.c_str());
+    return;
+  }
+
+  TopicManager::instance()->publish(impl_->topic_, serfunc, m);
+}
+
+void Publisher::incrementSequence() const
+{
+  if (impl_ && impl_->isValid())
+  {
+    TopicManager::instance()->incrementSequence(impl_->topic_);
+  }
+}
+
+void Publisher::shutdown()
+{
+  if (impl_)
+  {
+    impl_->unadvertise();
+    impl_.reset();
+  }
+}
+
+std::string Publisher::getTopic() const
+{
+  if (impl_)
+  {
+    return impl_->topic_;
+  }
+
+  return std::string();
+}
+
+uint32_t Publisher::getNumSubscribers() const
+{
+  if (impl_ && impl_->isValid())
+  {
+    return TopicManager::instance()->getNumSubscribers(impl_->topic_);
+  }
+
+  return 0;
+}
+
+bool Publisher::isLatched() const {
+  PublicationPtr publication_ptr;
+  if (impl_ && impl_->isValid()) {
+    publication_ptr =
+      TopicManager::instance()->lookupPublication(impl_->topic_);
+  } else {
+    ROS_ASSERT_MSG(false, "Call to isLatched() on an invalid Publisher");
+    throw ros::Exception("Call to isLatched() on an invalid Publisher");
+  }
+  if (publication_ptr) {
+    return publication_ptr->isLatched();
+  } else {
+    ROS_ASSERT_MSG(false, "Call to isLatched() on an invalid Publisher");
+    throw ros::Exception("Call to isLatched() on an invalid Publisher");
+  }
+}
+
+} // namespace ros

thirdparty/ros/ros_comm/clients/roscpp/src/libros/publisher_link.cpp

@@ -0,0 +1,114 @@
+/*
+ * 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 "ros/publisher_link.h"
+#include "ros/subscription.h"
+#include "ros/header.h"
+#include "ros/connection.h"
+#include "ros/transport/transport.h"
+#include "ros/this_node.h"
+#include "ros/connection_manager.h"
+#include "ros/file_log.h"
+
+#include <boost/bind.hpp>
+
+#include <sstream>
+
+namespace ros
+{
+
+PublisherLink::PublisherLink(const SubscriptionPtr& parent, const std::string& xmlrpc_uri, 
+			     const TransportHints& transport_hints)
+: parent_(parent)
+, connection_id_(0)
+, publisher_xmlrpc_uri_(xmlrpc_uri)
+, transport_hints_(transport_hints)
+, latched_(false)
+{ }
+
+PublisherLink::~PublisherLink()
+{ }
+
+bool PublisherLink::setHeader(const Header& header)
+{
+  header.getValue("callerid", caller_id_);
+
+  std::string md5sum, type, latched_str;
+  if (!header.getValue("md5sum", md5sum))
+  {
+    ROS_ERROR("Publisher header did not have required element: md5sum");
+    return false;
+  }
+
+  md5sum_ = md5sum;
+
+  if (!header.getValue("type", type))
+  {
+    ROS_ERROR("Publisher header did not have required element: type");
+    return false;
+  }
+
+  latched_ = false;
+  if (header.getValue("latching", latched_str))
+  {
+    if (latched_str == "1")
+    {
+      latched_ = true;
+    }
+  }
+
+  connection_id_ = ConnectionManager::instance()->getNewConnectionID();
+  header_ = header;
+
+  if (SubscriptionPtr parent = parent_.lock())
+  {
+    parent->headerReceived(shared_from_this(), header);
+  }
+
+  return true;
+}
+
+const std::string& PublisherLink::getPublisherXMLRPCURI()
+{
+  return publisher_xmlrpc_uri_;
+}
+
+const std::string& PublisherLink::getMD5Sum()
+{
+  ROS_ASSERT(!md5sum_.empty());
+  return md5sum_;
+}
+
+} // namespace ros
+

thirdparty/ros/ros_comm/clients/roscpp/src/libros/rosout_appender.cpp

@@ -0,0 +1,150 @@
+/*
+ * 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 "ros/rosout_appender.h"
+#include "ros/this_node.h"
+#include "ros/node_handle.h"
+#include "ros/topic_manager.h"
+#include "ros/advertise_options.h"
+#include "ros/names.h"
+
+#include <rosgraph_msgs/Log.h>
+
+namespace ros
+{
+
+ROSOutAppender::ROSOutAppender()
+: shutting_down_(false)
+, publish_thread_(boost::bind(&ROSOutAppender::logThread, this))
+{
+  AdvertiseOptions ops;
+  ops.init<rosgraph_msgs::Log>(names::resolve("/rosout"), 0);
+  ops.latch = true;
+  SubscriberCallbacksPtr cbs(boost::make_shared<SubscriberCallbacks>());
+  TopicManager::instance()->advertise(ops, cbs);
+}
+
+ROSOutAppender::~ROSOutAppender()
+{
+  shutting_down_ = true;
+
+  {
+    boost::mutex::scoped_lock lock(queue_mutex_);
+    queue_condition_.notify_all();
+  }
+
+  publish_thread_.join();
+}
+
+const std::string&  ROSOutAppender::getLastError() const
+{
+  return last_error_;
+}
+
+void ROSOutAppender::log(::ros::console::Level level, const char* str, const char* file, const char* function, int line)
+{
+  rosgraph_msgs::LogPtr msg(boost::make_shared<rosgraph_msgs::Log>());
+
+  msg->header.stamp = ros::Time::now();
+  if (level == ros::console::levels::Debug)
+  {
+    msg->level = rosgraph_msgs::Log::DEBUG;
+  }
+  else if (level == ros::console::levels::Info)
+  {
+    msg->level = rosgraph_msgs::Log::INFO;
+  }
+  else if (level == ros::console::levels::Warn)
+  {
+    msg->level = rosgraph_msgs::Log::WARN;
+  }
+  else if (level == ros::console::levels::Error)
+  {
+    msg->level = rosgraph_msgs::Log::ERROR;
+  }
+  else if (level == ros::console::levels::Fatal)
+  {
+    msg->level = rosgraph_msgs::Log::FATAL;
+  }
+  msg->name = this_node::getName();
+  msg->msg = str;
+  msg->file = file;
+  msg->function = function;
+  msg->line = line;
+  this_node::getAdvertisedTopics(msg->topics);
+
+  if (level == ::ros::console::levels::Fatal || level == ::ros::console::levels::Error)
+  {
+    last_error_ = str;
+  }
+
+  boost::mutex::scoped_lock lock(queue_mutex_);
+  log_queue_.push_back(msg);
+  queue_condition_.notify_all();
+}
+
+void ROSOutAppender::logThread()
+{
+  while (!shutting_down_)
+  {
+    V_Log local_queue;
+
+    {
+      boost::mutex::scoped_lock lock(queue_mutex_);
+
+      if (shutting_down_)
+      {
+        return;
+      }
+
+      queue_condition_.wait(lock);
+
+      if (shutting_down_)
+      {
+        return;
+      }
+
+      local_queue.swap(log_queue_);
+    }
+
+    V_Log::iterator it = local_queue.begin();
+    V_Log::iterator end = local_queue.end();
+    for (; it != end; ++it)
+    {
+      TopicManager::instance()->publish(names::resolve("/rosout"), *(*it));
+    }
+  }
+}
+
+} // namespace ros

thirdparty/ros/ros_comm/clients/roscpp/src/libros/service.cpp

@@ -0,0 +1,131 @@
+/*
+ * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
+ *
+ * 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 names of Stanford University or 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 "ros/service.h"
+#include "ros/connection.h"
+#include "ros/service_server_link.h"
+#include "ros/service_manager.h"
+#include "ros/transport/transport_tcp.h"
+#include "ros/poll_manager.h"
+#include "ros/init.h"
+#include "ros/names.h"
+#include "ros/this_node.h"
+#include "ros/header.h"
+
+using namespace ros;
+
+bool service::exists(const std::string& service_name, bool print_failure_reason)
+{
+  std::string mapped_name = names::resolve(service_name);
+
+  std::string host;
+  uint32_t port;
+
+  if (ServiceManager::instance()->lookupService(mapped_name, host, port))
+  {
+    TransportTCPPtr transport(boost::make_shared<TransportTCP>(static_cast<ros::PollSet*>(NULL), TransportTCP::SYNCHRONOUS));
+
+    if (transport->connect(host, port))
+    {
+      M_string m;
+      m["probe"] = "1";
+      m["md5sum"] = "*";
+      m["callerid"] = this_node::getName();
+      m["service"] = mapped_name;
+      boost::shared_array<uint8_t> buffer;
+      uint32_t size = 0;;
+      Header::write(m, buffer, size);
+      transport->write((uint8_t*)&size, sizeof(size));
+      transport->write(buffer.get(), size);
+      transport->close();
+
+      return true;
+    }
+    else
+    {
+      if (print_failure_reason)
+      {
+        ROS_INFO("waitForService: Service [%s] could not connect to host [%s:%d], waiting...", mapped_name.c_str(), host.c_str(), port);
+      }
+    }
+  }
+  else
+  {
+    if (print_failure_reason)
+    {
+      ROS_INFO("waitForService: Service [%s] has not been advertised, waiting...", mapped_name.c_str());
+    }
+  }
+
+  return false;
+}
+
+bool service::waitForService(const std::string& service_name, ros::Duration timeout)
+{
+  std::string mapped_name = names::resolve(service_name);
+
+  Time start_time = Time::now();
+
+  bool printed = false;
+  bool result = false;
+  while (ros::ok())
+  {
+    if (exists(service_name, !printed))
+    {
+      result = true;
+      break;
+    }
+    else
+    {
+      printed = true;
+
+      if (timeout >= Duration(0))
+      {
+        Time current_time = Time::now();
+
+        if ((current_time - start_time) >= timeout)
+        {
+          return false;
+        }
+      }
+
+      Duration(0.02).sleep();
+    }
+  }
+
+  if (printed && ros::ok())
+  {
+    ROS_INFO("waitForService: Service [%s] is now available.", mapped_name.c_str());
+  }
+
+  return result;
+}
+
+bool service::waitForService(const std::string& service_name, int32_t timeout)
+{
+  return waitForService(service_name, ros::Duration(timeout / 1000.0));
+}

thirdparty/ros/ros_comm/clients/roscpp/src/libros/service_client.cpp

@@ -0,0 +1,213 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Stanford University or 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 "ros/service_client.h"
+#include "ros/service_server_link.h"
+#include "ros/connection.h"
+#include "ros/service_manager.h"
+#include "ros/service.h"
+
+namespace ros
+{
+
+ServiceClient::Impl::Impl() 
+  : is_shutdown_(false)
+{ }
+
+ServiceClient::Impl::~Impl()
+{
+  shutdown();
+}
+
+void ServiceClient::Impl::shutdown()
+{
+  if (!is_shutdown_)
+  {
+    if (!persistent_)
+    {
+      is_shutdown_ = true;
+    }
+
+    if (server_link_)
+    {
+      server_link_->getConnection()->drop(Connection::Destructing);
+      server_link_.reset();
+    }
+  }
+}
+
+bool ServiceClient::Impl::isValid() const
+{
+  // Non-persistent connections are always valid
+  if (!persistent_)
+  {
+    return true;
+  }
+
+  if (is_shutdown_)
+  {
+    return false;
+  }
+
+  if (!server_link_)
+  {
+    return false;
+  }
+
+  return server_link_->isValid();
+}
+
+ServiceClient::ServiceClient(const std::string& service_name, bool persistent, const M_string& header_values, const std::string& service_md5sum)
+: impl_(new Impl)
+{
+  impl_->name_ = service_name;
+  impl_->persistent_ = persistent;
+  impl_->header_values_ = header_values;
+  impl_->service_md5sum_ = service_md5sum;
+
+  if (persistent)
+  {
+    impl_->server_link_ = ServiceManager::instance()->createServiceServerLink(impl_->name_, impl_->persistent_, impl_->service_md5sum_, impl_->service_md5sum_, impl_->header_values_);
+  }
+}
+
+ServiceClient::ServiceClient(const ServiceClient& rhs)
+{
+  impl_ = rhs.impl_;
+}
+
+ServiceClient::~ServiceClient()
+{
+
+}
+
+bool ServiceClient::call(const SerializedMessage& req, SerializedMessage& resp, const std::string& service_md5sum)
+{
+  if (service_md5sum != impl_->service_md5sum_)
+  {
+    ROS_ERROR("Call to service [%s] with md5sum [%s] does not match md5sum when the handle was created ([%s])", impl_->name_.c_str(), service_md5sum.c_str(), impl_->service_md5sum_.c_str());
+
+    return false;
+  }
+
+  ServiceServerLinkPtr link;
+
+  if (impl_->persistent_)
+  {
+    if (!impl_->server_link_)
+    {
+      impl_->server_link_ = ServiceManager::instance()->createServiceServerLink(impl_->name_, impl_->persistent_, service_md5sum, service_md5sum, impl_->header_values_);
+
+      if (!impl_->server_link_)
+      {
+        return false;
+      }
+    }
+
+    link = impl_->server_link_;
+  }
+  else
+  {
+    link = ServiceManager::instance()->createServiceServerLink(impl_->name_, impl_->persistent_, service_md5sum, service_md5sum, impl_->header_values_);
+
+    if (!link)
+    {
+      return false;
+    }
+  }
+
+  bool ret = link->call(req, resp);
+  link.reset();
+
+  // If we're shutting down but the node haven't finished yet, wait until we do
+  while (ros::isShuttingDown() && ros::ok())
+  {
+    ros::WallDuration(0.001).sleep();
+  }
+
+  return ret;
+}
+
+bool ServiceClient::isValid() const
+{
+  if (!impl_)
+  {
+    return false;
+  }
+
+  return impl_->isValid();
+}
+
+bool ServiceClient::isPersistent() const
+{
+  if (impl_)
+  {
+    return impl_->persistent_;
+  }
+
+  return false;
+}
+
+void ServiceClient::shutdown()
+{
+  if (impl_)
+  {
+    impl_->shutdown();
+  }
+}
+
+bool ServiceClient::waitForExistence(ros::Duration timeout)
+{
+  if (impl_)
+  {
+    return service::waitForService(impl_->name_, timeout);
+  }
+
+  return false;
+}
+
+bool ServiceClient::exists()
+{
+  if (impl_)
+  {
+    return service::exists(impl_->name_, false);
+  }
+
+  return false;
+}
+
+std::string ServiceClient::getService()
+{
+  if (impl_)
+  {
+    return impl_->name_;
+  }
+
+  return "";
+}
+
+}

thirdparty/ros/ros_comm/clients/roscpp/src/libros/service_client_link.cpp

@@ -0,0 +1,245 @@
+/*
+ * 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 "ros/service_client_link.h"
+#include "ros/service_publication.h"
+#include "ros/header.h"
+#include "ros/connection.h"
+#include "ros/service_manager.h"
+#include "ros/transport/transport.h"
+#include "ros/this_node.h"
+#include "ros/file_log.h"
+
+#include <boost/bind.hpp>
+
+namespace ros
+{
+
+ServiceClientLink::ServiceClientLink()
+: persistent_(false)
+{
+}
+
+ServiceClientLink::~ServiceClientLink()
+{
+  if (connection_)
+  {
+    if (connection_->isSendingHeaderError())
+    {
+      connection_->removeDropListener(dropped_conn_);
+    }
+    else
+    {
+      connection_->drop(Connection::Destructing);
+    }
+  }
+}
+
+bool ServiceClientLink::initialize(const ConnectionPtr& connection)
+{
+  connection_ = connection;
+  dropped_conn_ = connection_->addDropListener(boost::bind(&ServiceClientLink::onConnectionDropped, this, _1));
+
+  return true;
+}
+
+bool ServiceClientLink::handleHeader(const Header& header)
+{
+  std::string md5sum, service, client_callerid;
+  if (!header.getValue("md5sum", md5sum)
+   || !header.getValue("service", service)
+   || !header.getValue("callerid", client_callerid))
+  {
+    std::string msg("bogus tcpros header. did not have the "
+                          "required elements: md5sum, service, callerid");
+
+    ROS_ERROR("%s", msg.c_str());
+    connection_->sendHeaderError(msg);
+
+    return false;
+  }
+
+  std::string persistent;
+  if (header.getValue("persistent", persistent))
+  {
+    if (persistent == "1" || persistent == "true")
+    {
+      persistent_ = true;
+    }
+  }
+
+  ROSCPP_LOG_DEBUG("Service client [%s] wants service [%s] with md5sum [%s]", client_callerid.c_str(), service.c_str(), md5sum.c_str());
+  ServicePublicationPtr ss = ServiceManager::instance()->lookupServicePublication(service);
+  if (!ss)
+  {
+    std::string msg = std::string("received a tcpros connection for a "
+                             "nonexistent service [") +
+            service + std::string("].");
+
+    ROS_ERROR("%s", msg.c_str());
+    connection_->sendHeaderError(msg);
+
+    return false;
+  }
+  if (ss->getMD5Sum() != md5sum &&
+      (md5sum != std::string("*") && ss->getMD5Sum() != std::string("*")))
+  {
+    std::string msg = std::string("client wants service ") + service +
+            std::string(" to have md5sum ") + md5sum +
+            std::string(", but it has ") + ss->getMD5Sum() +
+            std::string(". Dropping connection.");
+
+    ROS_ERROR("%s", msg.c_str());
+    connection_->sendHeaderError(msg);
+
+    return false;
+  }
+
+  // Check whether the service (ss here) has been deleted from
+  // advertised_topics through a call to unadvertise(), which could
+  // have happened while we were waiting for the subscriber to
+  // provide the md5sum.
+  if(ss->isDropped())
+  {
+    std::string msg = std::string("received a tcpros connection for a "
+                             "nonexistent service [") +
+            service + std::string("].");
+
+    ROS_ERROR("%s", msg.c_str());
+    connection_->sendHeaderError(msg);
+
+    return false;
+  }
+  else
+  {
+    parent_ = ServicePublicationWPtr(ss);
+
+    // Send back a success, with info
+    M_string m;
+    m["request_type"] = ss->getRequestDataType();
+    m["response_type"] = ss->getResponseDataType();
+    m["type"] = ss->getDataType();
+    m["md5sum"] = ss->getMD5Sum();
+    m["callerid"] = this_node::getName();
+    connection_->writeHeader(m, boost::bind(&ServiceClientLink::onHeaderWritten, this, _1));
+
+    ss->addServiceClientLink(shared_from_this());
+  }
+
+  return true;
+}
+
+void ServiceClientLink::onConnectionDropped(const ConnectionPtr& conn)
+{
+  (void)conn;
+  ROS_ASSERT(conn == connection_);
+
+  if (ServicePublicationPtr parent = parent_.lock())
+  {
+    parent->removeServiceClientLink(shared_from_this());
+  }
+}
+
+void ServiceClientLink::onHeaderWritten(const ConnectionPtr& conn)
+{
+  (void)conn;
+  connection_->read(4, boost::bind(&ServiceClientLink::onRequestLength, this, _1, _2, _3, _4));
+}
+
+void ServiceClientLink::onRequestLength(const ConnectionPtr& conn, const boost::shared_array<uint8_t>& buffer, uint32_t size, bool success)
+{
+  (void)size;
+  if (!success)
+    return;
+
+  ROS_ASSERT(conn == connection_);
+  ROS_ASSERT(size == 4);
+
+  uint32_t len = *((uint32_t*)buffer.get());
+
+  if (len > 1000000000)
+  {
+    ROS_ERROR("a message of over a gigabyte was " \
+                "predicted in tcpros. that seems highly " \
+                "unlikely, so I'll assume protocol " \
+                "synchronization is lost.");
+    conn->drop(Connection::Destructing);
+    return;
+  }
+
+  connection_->read(len, boost::bind(&ServiceClientLink::onRequest, this, _1, _2, _3, _4));
+}
+
+void ServiceClientLink::onRequest(const ConnectionPtr& conn, const boost::shared_array<uint8_t>& buffer, uint32_t size, bool success)
+{
+  (void)conn;
+  if (!success)
+    return;
+
+  ROS_ASSERT(conn == connection_);
+
+  if (ServicePublicationPtr parent = parent_.lock())
+  {
+    parent->processRequest(buffer, size, shared_from_this());
+  }
+  else
+  {
+    ROS_BREAK();
+  }
+}
+
+void ServiceClientLink::onResponseWritten(const ConnectionPtr& conn)
+{
+  (void)conn;
+  ROS_ASSERT(conn == connection_);
+
+  if (persistent_)
+  {
+    connection_->read(4, boost::bind(&ServiceClientLink::onRequestLength, this, _1, _2, _3, _4));
+  }
+  else
+  {
+    connection_->drop(Connection::Destructing);
+  }
+}
+
+void ServiceClientLink::processResponse(bool ok, const SerializedMessage& res)
+{
+  (void)ok;
+  connection_->write(res.buf, res.num_bytes, boost::bind(&ServiceClientLink::onResponseWritten, this, _1));
+}
+
+
+} // namespace ros
+

thirdparty/ros/ros_comm/clients/roscpp/src/libros/service_manager.cpp

@@ -0,0 +1,333 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names 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 <cstdio>
+#include "ros/service_manager.h"
+#include "ros/xmlrpc_manager.h"
+#include "ros/connection_manager.h"
+#include "ros/poll_manager.h"
+#include "ros/service_publication.h"
+#include "ros/service_client_link.h"
+#include "ros/service_server_link.h"
+#include "ros/this_node.h"
+#include "ros/network.h"
+#include "ros/master.h"
+#include "ros/transport/transport_tcp.h"
+#include "ros/transport/transport_udp.h"
+#include "ros/init.h"
+#include "ros/connection.h"
+#include "ros/file_log.h"
+
+#include "xmlrpcpp/XmlRpc.h"
+
+#include <ros/console.h>
+
+using namespace XmlRpc; // A battle to be fought later
+using namespace std; // sigh
+
+namespace ros
+{
+
+const ServiceManagerPtr& ServiceManager::instance()
+{
+  static ServiceManagerPtr service_manager = boost::make_shared<ServiceManager>();
+  return service_manager;
+}
+
+ServiceManager::ServiceManager()
+: shutting_down_(false)
+{
+}
+
+ServiceManager::~ServiceManager()
+{
+  shutdown();
+}
+
+void ServiceManager::start()
+{
+  shutting_down_ = false;
+
+  poll_manager_ = PollManager::instance();
+  connection_manager_ = ConnectionManager::instance();
+  xmlrpc_manager_ = XMLRPCManager::instance();
+
+}
+
+void ServiceManager::shutdown()
+{
+  boost::recursive_mutex::scoped_lock shutdown_lock(shutting_down_mutex_);
+  if (shutting_down_)
+  {
+    return;
+  }
+
+  shutting_down_ = true;
+
+  ROSCPP_LOG_DEBUG("ServiceManager::shutdown(): unregistering our advertised services");
+  {
+    boost::mutex::scoped_lock ss_lock(service_publications_mutex_);
+
+    for (L_ServicePublication::iterator i = service_publications_.begin();
+         i != service_publications_.end(); ++i)
+    {
+      unregisterService((*i)->getName());
+      //ROSCPP_LOG_DEBUG( "shutting down service %s", (*i)->getName().c_str());
+      (*i)->drop();
+    }
+    service_publications_.clear();
+  }
+
+  L_ServiceServerLink local_service_clients;
+  {
+    boost::mutex::scoped_lock lock(service_server_links_mutex_);
+    local_service_clients.swap(service_server_links_);
+  }
+
+  {
+    L_ServiceServerLink::iterator it = local_service_clients.begin();
+    L_ServiceServerLink::iterator end = local_service_clients.end();
+    for (; it != end; ++it)
+    {
+      (*it)->getConnection()->drop(Connection::Destructing);
+    }
+
+    local_service_clients.clear();
+  }
+
+}
+
+bool ServiceManager::advertiseService(const AdvertiseServiceOptions& ops)
+{
+  boost::recursive_mutex::scoped_lock shutdown_lock(shutting_down_mutex_);
+  if (shutting_down_)
+  {
+    return false;
+  }
+
+  {
+    boost::mutex::scoped_lock lock(service_publications_mutex_);
+
+    if (isServiceAdvertised(ops.service))
+    {
+      ROS_ERROR("Tried to advertise a service that is already advertised in this node [%s]", ops.service.c_str());
+      return false;
+    }
+
+    ServicePublicationPtr pub(boost::make_shared<ServicePublication>(ops.service, ops.md5sum, ops.datatype, ops.req_datatype, ops.res_datatype, ops.helper, ops.callback_queue, ops.tracked_object));
+    service_publications_.push_back(pub);
+  }
+
+  XmlRpcValue args, result, payload;
+  args[0] = this_node::getName();
+  args[1] = ops.service;
+  char uri_buf[1024];
+  snprintf(uri_buf, sizeof(uri_buf), "rosrpc://%s:%d",
+           network::getHost().c_str(), connection_manager_->getTCPPort());
+  args[2] = string(uri_buf);
+  args[3] = xmlrpc_manager_->getServerURI();
+  master::execute("registerService", args, result, payload, true);
+
+  return true;
+}
+
+bool ServiceManager::unadvertiseService(const string &serv_name)
+{
+  boost::recursive_mutex::scoped_lock shutdown_lock(shutting_down_mutex_);
+  if (shutting_down_)
+  {
+    return false;
+  }
+
+  ServicePublicationPtr pub;
+  {
+    boost::mutex::scoped_lock lock(service_publications_mutex_);
+
+    for (L_ServicePublication::iterator i = service_publications_.begin();
+         i != service_publications_.end(); ++i)
+    {
+      if((*i)->getName() == serv_name && !(*i)->isDropped())
+      {
+        pub = *i;
+        service_publications_.erase(i);
+        break;
+      }
+    }
+  }
+
+  if (pub)
+  {
+    unregisterService(pub->getName());
+    ROSCPP_LOG_DEBUG( "shutting down service [%s]", pub->getName().c_str());
+    pub->drop();
+    return true;
+  }
+
+  return false;
+}
+
+bool ServiceManager::unregisterService(const std::string& service)
+{
+  XmlRpcValue args, result, payload;
+  args[0] = this_node::getName();
+  args[1] = service;
+  char uri_buf[1024];
+  snprintf(uri_buf, sizeof(uri_buf), "rosrpc://%s:%d",
+           network::getHost().c_str(), connection_manager_->getTCPPort());
+  args[2] = string(uri_buf);
+
+  master::execute("unregisterService", args, result, payload, false);
+
+  return true;
+}
+
+bool ServiceManager::isServiceAdvertised(const string& serv_name)
+{
+  for (L_ServicePublication::iterator s = service_publications_.begin(); s != service_publications_.end(); ++s)
+  {
+    if (((*s)->getName() == serv_name) && !(*s)->isDropped())
+    {
+      return true;
+    }
+  }
+
+  return false;
+}
+
+ServicePublicationPtr ServiceManager::lookupServicePublication(const std::string& service)
+{
+  boost::mutex::scoped_lock lock(service_publications_mutex_);
+
+  for (L_ServicePublication::iterator t = service_publications_.begin();
+       t != service_publications_.end(); ++t)
+  {
+    if ((*t)->getName() == service)
+    {
+      return *t;
+    }
+  }
+
+  return ServicePublicationPtr();
+}
+
+ServiceServerLinkPtr ServiceManager::createServiceServerLink(const std::string& service, bool persistent,
+                                             const std::string& request_md5sum, const std::string& response_md5sum,
+                                             const M_string& header_values)
+{
+
+  boost::recursive_mutex::scoped_lock shutdown_lock(shutting_down_mutex_);
+  if (shutting_down_)
+  {
+    return ServiceServerLinkPtr();
+  }
+
+  uint32_t serv_port;
+  std::string serv_host;
+  if (!lookupService(service, serv_host, serv_port))
+  {
+    return ServiceServerLinkPtr();
+  }
+
+  TransportTCPPtr transport(boost::make_shared<TransportTCP>(&poll_manager_->getPollSet()));
+
+  // Make sure to initialize the connection *before* transport->connect()
+  // is called, otherwise we might miss a connect error (see #434).
+  ConnectionPtr connection(boost::make_shared<Connection>());
+  connection_manager_->addConnection(connection);
+  connection->initialize(transport, false, HeaderReceivedFunc());
+
+  if (transport->connect(serv_host, serv_port))
+  {
+    ServiceServerLinkPtr client(boost::make_shared<ServiceServerLink>(service, persistent, request_md5sum, response_md5sum, header_values));
+
+    {
+      boost::mutex::scoped_lock lock(service_server_links_mutex_);
+      service_server_links_.push_back(client);
+    }
+
+    client->initialize(connection);
+
+    return client;
+  }
+
+  ROSCPP_LOG_DEBUG("Failed to connect to service [%s] (mapped=[%s]) at [%s:%d]", service.c_str(), service.c_str(), serv_host.c_str(), serv_port);
+
+  return ServiceServerLinkPtr();
+}
+
+void ServiceManager::removeServiceServerLink(const ServiceServerLinkPtr& client)
+{
+  // Guard against this getting called as a result of shutdown() dropping all connections (where shutting_down_mutex_ is already locked)
+  if (shutting_down_)
+  {
+    return;
+  }
+
+  boost::recursive_mutex::scoped_lock shutdown_lock(shutting_down_mutex_);
+  // Now check again, since the state may have changed between pre-lock/now
+  if (shutting_down_)
+  {
+    return;
+  }
+
+  boost::mutex::scoped_lock lock(service_server_links_mutex_);
+
+  L_ServiceServerLink::iterator it = std::find(service_server_links_.begin(), service_server_links_.end(), client);
+  if (it != service_server_links_.end())
+  {
+    service_server_links_.erase(it);
+  }
+}
+
+bool ServiceManager::lookupService(const string &name, string &serv_host, uint32_t &serv_port)
+{
+  XmlRpcValue args, result, payload;
+  args[0] = this_node::getName();
+  args[1] = name;
+  if (!master::execute("lookupService", args, result, payload, false))
+    return false;
+
+  string serv_uri(payload);
+  if (!serv_uri.length()) // shouldn't happen. but let's be sure.
+  {
+    ROS_ERROR("lookupService: Empty server URI returned from master");
+
+    return false;
+  }
+
+  if (!network::splitURI(serv_uri, serv_host, serv_port))
+  {
+    ROS_ERROR("lookupService: Bad service uri [%s]", serv_uri.c_str());
+
+    return false;
+  }
+
+  return true;
+}
+
+} // namespace ros
+

thirdparty/ros/ros_comm/clients/roscpp/src/libros/service_publication.cpp

@@ -0,0 +1,201 @@
+/*
+ * 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 "ros/service_publication.h"
+#include "ros/service_client_link.h"
+#include "ros/connection.h"
+#include "ros/callback_queue_interface.h"
+
+#include <boost/bind.hpp>
+
+#include <std_msgs/String.h>
+
+namespace ros
+{
+
+ServicePublication::ServicePublication(const std::string& name, const std::string &md5sum, const std::string& data_type, const std::string& request_data_type,
+                             const std::string& response_data_type, const ServiceCallbackHelperPtr& helper, CallbackQueueInterface* callback_queue,
+                             const VoidConstPtr& tracked_object)
+: name_(name)
+, md5sum_(md5sum)
+, data_type_(data_type)
+, request_data_type_(request_data_type)
+, response_data_type_(response_data_type)
+, helper_(helper)
+, dropped_(false)
+, callback_queue_(callback_queue)
+, has_tracked_object_(false)
+, tracked_object_(tracked_object)
+{
+  if (tracked_object)
+  {
+    has_tracked_object_ = true;
+  }
+}
+
+ServicePublication::~ServicePublication()
+{
+  drop();
+}
+
+void ServicePublication::drop()
+{
+  // grab a lock here, to ensure that no subscription callback will
+  // be invoked after we return
+  {
+    boost::mutex::scoped_lock lock(client_links_mutex_);
+    dropped_ = true;
+  }
+
+  dropAllConnections();
+
+  callback_queue_->removeByID((uint64_t)this);
+}
+
+class ServiceCallback : public CallbackInterface
+{
+public:
+  ServiceCallback(const ServiceCallbackHelperPtr& helper, const boost::shared_array<uint8_t>& buf, size_t num_bytes, const ServiceClientLinkPtr& link, bool has_tracked_object, const VoidConstWPtr& tracked_object)
+  : helper_(helper)
+  , buffer_(buf)
+  , num_bytes_(num_bytes)
+  , link_(link)
+  , has_tracked_object_(has_tracked_object)
+  , tracked_object_(tracked_object)
+  {
+  }
+
+  virtual CallResult call()
+  {
+    if (link_->getConnection()->isDropped())
+    {
+      return Invalid;
+    }
+
+    VoidConstPtr tracker;
+    if (has_tracked_object_)
+    {
+      tracker = tracked_object_.lock();
+
+      if (!tracker)
+      {
+        SerializedMessage res = serialization::serializeServiceResponse<uint32_t>(false, 0);
+        link_->processResponse(false, res);
+        return Invalid;
+      }
+    }
+
+    ServiceCallbackHelperCallParams params;
+    params.request = SerializedMessage(buffer_, num_bytes_);
+    params.connection_header = link_->getConnection()->getHeader().getValues();
+    try
+    {
+
+      bool ok = helper_->call(params);
+      if (ok != 0)
+      {
+        link_->processResponse(true, params.response);
+      }
+      else
+      {
+        SerializedMessage res = serialization::serializeServiceResponse<uint32_t>(false, 0);
+        link_->processResponse(false, res);
+      }
+    }
+    catch (std::exception& e)
+    {
+      ROS_ERROR("Exception thrown while processing service call: %s", e.what());
+      std_msgs::String error_string;
+      error_string.data = e.what();
+      SerializedMessage res = serialization::serializeServiceResponse(false, error_string);
+      link_->processResponse(false, res);
+      return Invalid;
+    }
+
+    return Success;
+  }
+
+private:
+  ServiceCallbackHelperPtr helper_;
+  boost::shared_array<uint8_t> buffer_;
+  uint32_t num_bytes_;
+  ServiceClientLinkPtr link_;
+  bool has_tracked_object_;
+  VoidConstWPtr tracked_object_;
+};
+
+void ServicePublication::processRequest(boost::shared_array<uint8_t> buf, size_t num_bytes, const ServiceClientLinkPtr& link)
+{
+  CallbackInterfacePtr cb(boost::make_shared<ServiceCallback>(helper_, buf, num_bytes, link, has_tracked_object_, tracked_object_));
+  callback_queue_->addCallback(cb, (uint64_t)this);
+}
+
+void ServicePublication::addServiceClientLink(const ServiceClientLinkPtr& link)
+{
+  boost::mutex::scoped_lock lock(client_links_mutex_);
+
+  client_links_.push_back(link);
+}
+
+void ServicePublication::removeServiceClientLink(const ServiceClientLinkPtr& link)
+{
+  boost::mutex::scoped_lock lock(client_links_mutex_);
+
+  V_ServiceClientLink::iterator it = std::find(client_links_.begin(), client_links_.end(), link);
+  if (it != client_links_.end())
+  {
+    client_links_.erase(it);
+  }
+}
+
+void ServicePublication::dropAllConnections()
+{
+  // Swap our client_links_ 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 client_links_ mutex
+  V_ServiceClientLink local_links;
+
+  {
+    boost::mutex::scoped_lock lock(client_links_mutex_);
+
+    local_links.swap(client_links_);
+  }
+
+  for (V_ServiceClientLink::iterator i = local_links.begin();
+           i != local_links.end(); ++i)
+  {
+    (*i)->getConnection()->drop(Connection::Destructing);
+  }
+}
+
+} // namespace ros

thirdparty/ros/ros_comm/clients/roscpp/src/libros/service_server.cpp

@@ -0,0 +1,92 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Stanford University or 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 "ros/service_server.h"
+#include "ros/node_handle.h"
+#include "ros/service_manager.h"
+
+namespace ros
+{
+
+ServiceServer::Impl::Impl() : unadvertised_(false) { }
+
+ServiceServer::Impl::~Impl()
+{
+  ROS_DEBUG("ServiceServer on '%s' deregistering callbacks.", service_.c_str());
+  unadvertise();
+}
+
+bool ServiceServer::Impl::isValid() const
+{
+  return !unadvertised_;
+}
+
+void ServiceServer::Impl::unadvertise()
+{
+  if (!unadvertised_)
+  {
+    unadvertised_ = true;
+    ServiceManager::instance()->unadvertiseService(service_);
+    node_handle_.reset();
+  }
+}
+
+ServiceServer::ServiceServer(const std::string& service, const NodeHandle& node_handle)
+: impl_(boost::make_shared<Impl>())
+{
+  impl_->service_ = service;
+  impl_->node_handle_ = boost::make_shared<NodeHandle>(node_handle);
+}
+
+ServiceServer::ServiceServer(const ServiceServer& rhs)
+{
+  impl_ = rhs.impl_;
+}
+
+ServiceServer::~ServiceServer()
+{
+}
+
+void ServiceServer::shutdown()
+{
+  if (impl_)
+  {
+    impl_->unadvertise();
+  }
+}
+
+std::string ServiceServer::getService() const
+{
+  if (impl_ && impl_->isValid())
+  {
+    return impl_->service_;
+  }
+
+  return std::string();
+}
+
+} // namespace ros

thirdparty/ros/ros_comm/clients/roscpp/src/libros/service_server_link.cpp

@@ -0,0 +1,390 @@
+/*
+ * 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 "ros/service_server_link.h"
+#include "ros/header.h"
+#include "ros/connection.h"
+#include "ros/service_manager.h"
+#include "ros/transport/transport.h"
+#include "ros/this_node.h"
+#include "ros/file_log.h"
+
+#include <boost/bind.hpp>
+
+#include <sstream>
+
+namespace ros
+{
+
+ServiceServerLink::ServiceServerLink(const std::string& service_name, bool persistent, const std::string& request_md5sum,
+                             const std::string& response_md5sum, const M_string& header_values)
+: service_name_(service_name)
+, persistent_(persistent)
+, request_md5sum_(request_md5sum)
+, response_md5sum_(response_md5sum)
+, extra_outgoing_header_values_(header_values)
+, header_written_(false)
+, header_read_(false)
+, dropped_(false)
+{
+}
+
+ServiceServerLink::~ServiceServerLink()
+{
+  ROS_ASSERT(connection_->isDropped());
+
+  clearCalls();
+}
+
+void ServiceServerLink::cancelCall(const CallInfoPtr& info)
+{
+  CallInfoPtr local = info;
+  {
+    boost::mutex::scoped_lock lock(local->finished_mutex_);
+    local->finished_ = true;
+    local->finished_condition_.notify_all();
+  }
+
+  if (boost::this_thread::get_id() != info->caller_thread_id_)
+  {
+    while (!local->call_finished_)
+    {
+      boost::this_thread::yield();
+    }
+  }
+}
+
+void ServiceServerLink::clearCalls()
+{
+  CallInfoPtr local_current;
+
+  {
+    boost::mutex::scoped_lock lock(call_queue_mutex_);
+    local_current = current_call_;
+  }
+
+  if (local_current)
+  {
+    cancelCall(local_current);
+  }
+
+  boost::mutex::scoped_lock lock(call_queue_mutex_);
+
+  while (!call_queue_.empty())
+  {
+    CallInfoPtr info = call_queue_.front();
+
+    cancelCall(info);
+
+    call_queue_.pop();
+  }
+}
+
+bool ServiceServerLink::initialize(const ConnectionPtr& connection)
+{
+  connection_ = connection;
+  connection_->addDropListener(boost::bind(&ServiceServerLink::onConnectionDropped, this, _1));
+  connection_->setHeaderReceivedCallback(boost::bind(&ServiceServerLink::onHeaderReceived, this, _1, _2));
+
+  M_string header;
+  header["service"] = service_name_;
+  header["md5sum"] = request_md5sum_;
+  header["callerid"] = this_node::getName();
+  header["persistent"] = persistent_ ? "1" : "0";
+  header.insert(extra_outgoing_header_values_.begin(), extra_outgoing_header_values_.end());
+
+  connection_->writeHeader(header, boost::bind(&ServiceServerLink::onHeaderWritten, this, _1));
+
+  return true;
+}
+
+void ServiceServerLink::onHeaderWritten(const ConnectionPtr& conn)
+{
+  (void)conn;
+  header_written_ = true;
+}
+
+bool ServiceServerLink::onHeaderReceived(const ConnectionPtr& conn, const Header& header)
+{
+  (void)conn;
+  std::string md5sum, type;
+  if (!header.getValue("md5sum", md5sum))
+  {
+    ROS_ERROR("TCPROS header from service server did not have required element: md5sum");
+    return false;
+  }
+
+  bool empty = false;
+  {
+    boost::mutex::scoped_lock lock(call_queue_mutex_);
+    empty = call_queue_.empty();
+
+    if (empty)
+    {
+      header_read_ = true;
+    }
+  }
+
+  if (!empty)
+  {
+    processNextCall();
+
+    header_read_ = true;
+  }
+
+  return true;
+}
+
+void ServiceServerLink::onConnectionDropped(const ConnectionPtr& conn)
+{
+  ROS_ASSERT(conn == connection_);
+  ROSCPP_LOG_DEBUG("Service client from [%s] for [%s] dropped", conn->getRemoteString().c_str(), service_name_.c_str());
+
+  dropped_ = true;
+  clearCalls();
+
+  ServiceManager::instance()->removeServiceServerLink(shared_from_this());
+}
+
+void ServiceServerLink::onRequestWritten(const ConnectionPtr& conn)
+{
+  (void)conn;
+  //ros::WallDuration(0.1).sleep();
+  connection_->read(5, boost::bind(&ServiceServerLink::onResponseOkAndLength, this, _1, _2, _3, _4));
+}
+
+void ServiceServerLink::onResponseOkAndLength(const ConnectionPtr& conn, const boost::shared_array<uint8_t>& buffer, uint32_t size, bool success)
+{
+  (void)size;
+  ROS_ASSERT(conn == connection_);
+  ROS_ASSERT(size == 5);
+
+  if (!success)
+    return;
+
+  uint8_t ok = buffer[0];
+  uint32_t len = *((uint32_t*)(buffer.get() + 1));
+
+  if (len > 1000000000)
+  {
+    ROS_ERROR("a message of over a gigabyte was " \
+                "predicted in tcpros. that seems highly " \
+                "unlikely, so I'll assume protocol " \
+                "synchronization is lost.");
+    conn->drop(Connection::Destructing);
+
+    return;
+  }
+
+  {
+    boost::mutex::scoped_lock lock(call_queue_mutex_);
+    if ( ok != 0 ) {
+    	current_call_->success_ = true;
+    } else {
+    	current_call_->success_ = false;
+    }
+  }
+
+  if (len > 0)
+  {
+    connection_->read(len, boost::bind(&ServiceServerLink::onResponse, this, _1, _2, _3, _4));
+  }
+  else
+  {
+    onResponse(conn, boost::shared_array<uint8_t>(), 0, true);
+  }
+}
+
+void ServiceServerLink::onResponse(const ConnectionPtr& conn, const boost::shared_array<uint8_t>& buffer, uint32_t size, bool success)
+{
+  (void)conn;
+  ROS_ASSERT(conn == connection_);
+
+  if (!success)
+    return;
+
+  {
+    boost::mutex::scoped_lock queue_lock(call_queue_mutex_);
+
+    if (current_call_->success_)
+    {
+      *current_call_->resp_ = SerializedMessage(buffer, size);
+    }
+    else
+    {
+      current_call_->exception_string_ = std::string(reinterpret_cast<char*>(buffer.get()), size);
+    }
+  }
+
+  callFinished();
+}
+
+void ServiceServerLink::callFinished()
+{
+  CallInfoPtr saved_call;
+  ServiceServerLinkPtr self;
+  {
+    boost::mutex::scoped_lock queue_lock(call_queue_mutex_);
+    boost::mutex::scoped_lock finished_lock(current_call_->finished_mutex_);
+
+    ROS_DEBUG_NAMED("superdebug", "Client to service [%s] call finished with success=[%s]", service_name_.c_str(), current_call_->success_ ? "true" : "false");
+
+    current_call_->finished_ = true;
+    current_call_->finished_condition_.notify_all();
+
+    saved_call = current_call_;
+    current_call_ = CallInfoPtr();
+
+    // If the call queue is empty here, we may be deleted as soon as we release these locks, so keep a shared pointer to ourselves until we return
+    // ugly
+    // jfaust TODO there's got to be a better way
+    self = shared_from_this();
+  }
+
+  saved_call = CallInfoPtr();
+
+  processNextCall();
+}
+
+void ServiceServerLink::processNextCall()
+{
+  bool empty = false;
+  {
+    boost::mutex::scoped_lock lock(call_queue_mutex_);
+
+    if (current_call_)
+    {
+      return;
+    }
+
+    if (!call_queue_.empty())
+    {
+      ROS_DEBUG_NAMED("superdebug", "[%s] Client to service [%s] processing next service call", persistent_ ? "persistent" : "non-persistent", service_name_.c_str());
+
+      current_call_ = call_queue_.front();
+      call_queue_.pop();
+    }
+    else
+    {
+      empty = true;
+    }
+  }
+
+  if (empty)
+  {
+    if (!persistent_)
+    {
+      ROS_DEBUG_NAMED("superdebug", "Dropping non-persistent client to service [%s]", service_name_.c_str());
+      connection_->drop(Connection::Destructing);
+    }
+    else
+    {
+      ROS_DEBUG_NAMED("superdebug", "Keeping persistent client to service [%s]", service_name_.c_str());
+    }
+  }
+  else
+  {
+    SerializedMessage request;
+
+    {
+      boost::mutex::scoped_lock lock(call_queue_mutex_);
+      request = current_call_->req_;
+    }
+
+    connection_->write(request.buf, request.num_bytes, boost::bind(&ServiceServerLink::onRequestWritten, this, _1));
+  }
+}
+
+bool ServiceServerLink::call(const SerializedMessage& req, SerializedMessage& resp)
+{
+  CallInfoPtr info(boost::make_shared<CallInfo>());
+  info->req_ = req;
+  info->resp_ = &resp;
+  info->success_ = false;
+  info->finished_ = false;
+  info->call_finished_ = false;
+  info->caller_thread_id_ = boost::this_thread::get_id();
+
+  //ros::WallDuration(0.1).sleep();
+
+  bool immediate = false;
+  {
+    boost::mutex::scoped_lock lock(call_queue_mutex_);
+
+    if (connection_->isDropped())
+    {
+      ROSCPP_LOG_DEBUG("ServiceServerLink::call called on dropped connection for service [%s]", service_name_.c_str());
+      info->call_finished_ = true;
+      return false;
+    }
+
+    if (call_queue_.empty() && header_written_ && header_read_)
+    {
+      immediate = true;
+    }
+
+    call_queue_.push(info);
+  }
+
+  if (immediate)
+  {
+    processNextCall();
+  }
+
+  {
+    boost::mutex::scoped_lock lock(info->finished_mutex_);
+
+    while (!info->finished_)
+    {
+      info->finished_condition_.wait(lock);
+    }
+  }
+
+  info->call_finished_ = true;
+
+  if (info->exception_string_.length() > 0)
+  {
+    ROS_ERROR("Service call failed: service [%s] responded with an error: %s", service_name_.c_str(), info->exception_string_.c_str());
+  }
+
+  return info->success_;
+}
+
+bool ServiceServerLink::isValid() const
+{
+  return !dropped_;
+}
+
+} // namespace ros
+

thirdparty/ros/ros_comm/clients/roscpp/src/libros/single_subscriber_publisher.cpp

@@ -0,0 +1,57 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Stanford University or 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 "ros/single_subscriber_publisher.h"
+#include "ros/subscriber_link.h"
+
+namespace ros
+{
+SingleSubscriberPublisher::SingleSubscriberPublisher(const SubscriberLinkPtr& link)
+: link_(link)
+{
+}
+
+SingleSubscriberPublisher::~SingleSubscriberPublisher()
+{
+}
+
+void SingleSubscriberPublisher::publish(const SerializedMessage& m) const
+{
+  link_->enqueueMessage(m, true, true);
+}
+
+std::string SingleSubscriberPublisher::getTopic() const
+{
+  return link_->getTopic();
+}
+
+std::string SingleSubscriberPublisher::getSubscriberName() const
+{
+  return link_->getDestinationCallerID();
+}
+
+} // namespace ros

thirdparty/ros/ros_comm/clients/roscpp/src/libros/spinner.cpp

@@ -0,0 +1,312 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Stanford University or 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 "ros/spinner.h"
+#include "ros/ros.h"
+#include "ros/callback_queue.h"
+
+#include <boost/thread/thread.hpp>
+#include <boost/thread/mutex.hpp>
+
+namespace {
+
+const std::string DEFAULT_ERROR_MESSAGE =
+    "Attempt to spin a callback queue from two spinners, one of them being single-threaded."
+    "\nIn the 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
+ *  spinning on the queue. Other threads accessing the callback queue will probably intercept execution order.
+
+ *  To avoid multiple SingleThreadedSpinners (started from different threads) to operate on the same callback queue,
+ *  this class stores a map of all spinned callback queues.
+ *  If the spinner is single threaded, the corresponding thread-id is stored in the map
+ *  and if other threads will try to spin the same queue, an error message is issued.
+ *
+ *  If the spinner is multi-threaded, the stored thread-id is NULL and future SingleThreadedSpinners
+ *  should not spin this queue. However, other multi-threaded spinners are allowed.
+ */
+struct SpinnerMonitor
+{
+  /* store spinner information per callback queue:
+     Only alike spinners (single-threaded or multi-threaded) are allowed on a callback queue.
+     For single-threaded spinners we store their thread id.
+     We store the number of alike spinners operating on the callback queue.
+  */
+  struct Entry
+  {
+    Entry(const boost::thread::id &tid,
+          const boost::thread::id &initial_tid) : tid(tid), initial_tid(initial_tid), num(0) {}
+
+    boost::thread::id tid; // proper thread id of single-threaded spinner
+    boost::thread::id initial_tid; // to retain old behaviour, store first spinner's thread id
+    unsigned int num; // number of (alike) spinners serving this queue
+  };
+
+  /// add a queue to the list
+  bool add(ros::CallbackQueue* queue, bool single_threaded)
+  {
+    boost::mutex::scoped_lock lock(mutex_);
+
+    boost::thread::id current_tid = boost::this_thread::get_id();
+    boost::thread::id tid; // current thread id for single-threaded spinners, zero for multi-threaded ones
+    if (single_threaded)
+      tid = current_tid;
+
+    std::map<ros::CallbackQueue*, Entry>::iterator it = spinning_queues_.find(queue);
+    bool can_spin = ( it == spinning_queues_.end() || // we will spin on any new queue
+                      it->second.tid == tid ); // otherwise spinner must be alike (all multi-threaded: 0, or single-threaded on same thread id)
+
+    if (!can_spin)
+    {
+      // 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: single-threaded spinner after multi-threaded one(s)."
+                         << DEFAULT_ERROR_MESSAGE
+                         << " Only allowed for backwards compatibility.");
+        it->second.tid = tid; // "upgrade" tid to represent single-threaded spinner
+      }
+      else
+        return false;
+    }
+
+    if (it == spinning_queues_.end())
+      it = spinning_queues_.insert(it, std::make_pair(queue, Entry(tid, current_tid)));
+
+    // increment number of active spinners
+    it->second.num += 1;
+
+    return true;
+  }
+
+  /// remove a queue from the list
+  void remove(ros::CallbackQueue* queue)
+  {
+    boost::mutex::scoped_lock lock(mutex_);
+    std::map<ros::CallbackQueue*, Entry>::iterator it = spinning_queues_.find(queue);
+    ROS_ASSERT_MSG(it != spinning_queues_.end(), "Call to SpinnerMonitor::remove() without matching call to add().");
+
+    if (it->second.tid != boost::thread::id() && it->second.tid != boost::this_thread::get_id())
+    {
+      // This doesn't harm, but isn't good practice?
+      // It was enforced by the previous implementation.
+      ROS_WARN("SpinnerMonitor::remove() called from different thread than add().");
+    }
+
+    ROS_ASSERT_MSG(it->second.num > 0, "SpinnerMonitor::remove(): Invalid spinner count (0) encountered.");
+    it->second.num -= 1;
+    if (it->second.num == 0)
+      spinning_queues_.erase(it); // erase queue entry to allow future queues with same pointer
+  }
+
+  std::map<ros::CallbackQueue*, Entry> spinning_queues_;
+  boost::mutex mutex_;
+};
+
+SpinnerMonitor spinner_monitor;
+}
+
+namespace ros
+{
+
+
+void SingleThreadedSpinner::spin(CallbackQueue* queue)
+{
+  if (!queue)
+  {
+    queue = getGlobalCallbackQueue();
+  }
+
+  if (!spinner_monitor.add(queue, true))
+  {
+    ROS_ERROR_STREAM("SingleThreadedSpinner: " << DEFAULT_ERROR_MESSAGE + " You might want to use a MultiThreadedSpinner instead.");
+    return;
+  }
+
+  ros::WallDuration timeout(0.1f);
+  ros::NodeHandle n;
+  while (n.ok())
+  {
+    queue->callAvailable(timeout);
+  }
+  spinner_monitor.remove(queue);
+}
+
+MultiThreadedSpinner::MultiThreadedSpinner(uint32_t thread_count)
+: thread_count_(thread_count)
+{
+}
+
+void MultiThreadedSpinner::spin(CallbackQueue* queue)
+{
+  AsyncSpinner s(thread_count_, queue);
+  s.start();
+
+  ros::waitForShutdown();
+}
+
+class AsyncSpinnerImpl
+{
+public:
+  AsyncSpinnerImpl(uint32_t thread_count, CallbackQueue* queue);
+  ~AsyncSpinnerImpl();
+
+  bool canStart();
+  void start();
+  void stop();
+
+private:
+  void threadFunc();
+
+  boost::mutex mutex_;
+  boost::thread_group threads_;
+
+  uint32_t thread_count_;
+  CallbackQueue* callback_queue_;
+
+  volatile bool continue_;
+
+  ros::NodeHandle nh_;
+};
+
+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();
+  }
+}
+
+AsyncSpinnerImpl::~AsyncSpinnerImpl()
+{
+  stop();
+}
+
+bool AsyncSpinnerImpl::canStart()
+{
+  return true;
+}
+
+void AsyncSpinnerImpl::start()
+{
+  boost::mutex::scoped_lock lock(mutex_);
+
+  if (continue_)
+    return; // already spinning
+
+  if (!spinner_monitor.add(callback_queue_, false))
+  {
+    ROS_ERROR_STREAM("AsyncSpinnerImpl: " << DEFAULT_ERROR_MESSAGE);
+    return;
+  }
+
+  continue_ = true;
+
+  for (uint32_t i = 0; i < thread_count_; ++i)
+  {
+    threads_.create_thread(boost::bind(&AsyncSpinnerImpl::threadFunc, this));
+  }
+}
+
+void AsyncSpinnerImpl::stop()
+{
+  boost::mutex::scoped_lock lock(mutex_);
+  if (!continue_)
+    return;
+
+  continue_ = false;
+  threads_.join_all();
+
+  spinner_monitor.remove(callback_queue_);
+}
+
+void AsyncSpinnerImpl::threadFunc()
+{
+  disableAllSignalsInThisThread();
+
+  CallbackQueue* queue = callback_queue_;
+  bool use_call_available = thread_count_ == 1;
+  WallDuration timeout(0.1);
+
+  while (continue_ && nh_.ok())
+  {
+    if (use_call_available)
+    {
+      queue->callAvailable(timeout);
+    }
+    else
+    {
+      queue->callOne(timeout);
+    }
+  }
+}
+
+AsyncSpinner::AsyncSpinner(uint32_t thread_count)
+: impl_(new AsyncSpinnerImpl(thread_count, 0))
+{
+}
+
+AsyncSpinner::AsyncSpinner(uint32_t thread_count, CallbackQueue* queue)
+: impl_(new AsyncSpinnerImpl(thread_count, queue))
+{
+}
+
+bool AsyncSpinner::canStart()
+{
+  return impl_->canStart();
+}
+
+void AsyncSpinner::start()
+{
+  impl_->start();
+}
+
+void AsyncSpinner::stop()
+{
+  impl_->stop();
+}
+
+}

thirdparty/ros/ros_comm/clients/roscpp/src/libros/statistics.cpp

@@ -0,0 +1,260 @@
+/*
+ * Copyright (C) 2013-2014, Dariush Forouher
+ *
+ * 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 names of Stanford University or 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 "ros/statistics.h"
+#include "ros/node_handle.h"
+#include <rosgraph_msgs/TopicStatistics.h>
+#include "ros/this_node.h"
+#include "ros/message_traits.h"
+#include "std_msgs/Header.h"
+#include "ros/param.h"
+
+namespace ros
+{
+
+StatisticsLogger::StatisticsLogger()
+: pub_frequency_(1.0)
+{
+}
+
+void StatisticsLogger::init(const SubscriptionCallbackHelperPtr& helper) {
+  hasHeader_ = helper->hasHeader();
+  param::param("/enable_statistics", enable_statistics, false);
+  param::param("/statistics_window_min_elements", min_elements, 10);
+  param::param("/statistics_window_max_elements", max_elements, 100);
+  param::param("/statistics_window_min_size", min_window, 4);
+  param::param("/statistics_window_max_size", max_window, 64);
+}
+
+void StatisticsLogger::callback(const boost::shared_ptr<M_string>& connection_header,
+                                const std::string& topic, const std::string& callerid, const SerializedMessage& m, const uint64_t& bytes_sent,
+                                const ros::Time& received_time, bool dropped)
+{
+  (void)connection_header;
+  struct StatData stats;
+
+  if (!enable_statistics)
+  {
+    return;
+  }
+
+  // ignore /clock for safety and /statistics to reduce noise
+  if (topic == "/statistics" || topic == "/clock")
+  {
+    return;
+  }
+
+  // callerid identifies the connection
+  std::map<std::string, struct StatData>::iterator stats_it = map_.find(callerid);
+  if (stats_it == map_.end())
+  {
+    // this is the first time, we received something on this connection
+    stats.stat_bytes_last = 0;
+    stats.dropped_msgs = 0;
+    stats.last_seq = 0;
+    stats.last_publish = ros::Time::now();
+    map_[callerid] = stats;
+  }
+  else
+  {
+    stats = map_[callerid];
+  }
+
+  stats.arrival_time_list.push_back(received_time);
+
+  if (dropped)
+  {
+    stats.dropped_msgs++;
+  }
+
+  // try to extract header, if the message has one. this fails sometimes,
+  // therefore the try-catch
+  if (hasHeader_)
+  {
+    try
+    {
+      std_msgs::Header header;
+      ros::serialization::IStream stream(m.message_start, m.num_bytes - (m.message_start - m.buf.get()));
+      ros::serialization::deserialize(stream, header);
+      if (!header.stamp.isZero())
+      {
+        stats.age_list.push_back(received_time - header.stamp);
+      }
+    }
+    catch (ros::serialization::StreamOverrunException& e)
+    {
+      ROS_DEBUG("Error during header extraction for statistics (topic=%s, message_length=%li)", topic.c_str(), m.num_bytes - (m.message_start - m.buf.get()));
+      hasHeader_ = false;
+    }
+  }
+
+  // should publish new statistics?
+  if (stats.last_publish + ros::Duration(pub_frequency_) < received_time)
+  {
+    ros::Time window_start = stats.last_publish;
+    stats.last_publish = received_time;
+
+    // fill the message with the aggregated data
+    rosgraph_msgs::TopicStatistics msg;
+    msg.topic = topic;
+    msg.node_pub = callerid;
+    msg.node_sub = ros::this_node::getName();
+    msg.window_start = window_start;
+    msg.window_stop = received_time;
+    msg.delivered_msgs = stats.arrival_time_list.size();
+    msg.dropped_msgs = stats.dropped_msgs;
+    msg.traffic = bytes_sent - stats.stat_bytes_last;
+
+    // not all message types have this
+    if (stats.age_list.size() > 0)
+    {
+      msg.stamp_age_mean = ros::Duration(0);
+      msg.stamp_age_max = ros::Duration(0);
+
+      for(std::list<ros::Duration>::iterator it = stats.age_list.begin(); it != stats.age_list.end(); it++)
+      {
+        ros::Duration age = *it;
+        msg.stamp_age_mean += age;
+
+        if (age > msg.stamp_age_max)
+        {
+            msg.stamp_age_max = age;
+        }
+      }
+
+      msg.stamp_age_mean *= 1.0 / stats.age_list.size();
+
+      double stamp_age_variance = 0.0;
+      for(std::list<ros::Duration>::iterator it = stats.age_list.begin(); it != stats.age_list.end(); it++)
+      {
+        ros::Duration t = msg.stamp_age_mean - *it;
+        stamp_age_variance += t.toSec() * t.toSec();
+      }
+      double stamp_age_stddev = sqrt(stamp_age_variance / stats.age_list.size());
+      try
+      {
+        msg.stamp_age_stddev = ros::Duration(stamp_age_stddev);
+      }
+      catch(std::runtime_error& e)
+      {
+        msg.stamp_age_stddev = ros::Duration(0);
+        ROS_WARN_STREAM("Error updating stamp_age_stddev for topic [" << topic << "]"
+          << " from node [" << callerid << "],"
+          << " likely due to the time between the mean stamp age and this message being exceptionally large."
+          << " Exception was: " << e.what());
+        ROS_DEBUG_STREAM("Mean stamp age was: " << msg.stamp_age_mean << " - std_dev of: " << stamp_age_stddev);
+      }
+
+    }
+    else
+    {
+      // in that case, set to NaN
+      msg.stamp_age_mean = ros::Duration(0);
+      msg.stamp_age_stddev = ros::Duration(0);
+      msg.stamp_age_max = ros::Duration(0);
+    }
+
+    // first, calculate the mean period between messages in this connection
+    // we need at least two messages in the window for this.
+    if (stats.arrival_time_list.size() > 1)
+    {
+      msg.period_mean = ros::Duration(0);
+      msg.period_max = ros::Duration(0);
+
+      ros::Time prev;
+      for(std::list<ros::Time>::iterator it = stats.arrival_time_list.begin(); it != stats.arrival_time_list.end(); it++) {
+
+        if (it == stats.arrival_time_list.begin()) {
+          prev = *it;
+          continue;
+        }
+
+        ros::Duration period = *it - prev;
+        msg.period_mean += period;
+        if (period > msg.period_max)
+            msg.period_max = period;
+        prev = *it;
+      }
+      msg.period_mean *= 1.0 / (stats.arrival_time_list.size() - 1);
+
+      // then, calc the stddev
+      double period_variance = 0.0;
+      for(std::list<ros::Time>::iterator it = stats.arrival_time_list.begin(); it != stats.arrival_time_list.end(); it++)
+      {
+        if (it == stats.arrival_time_list.begin())
+        {
+          prev = *it;
+          continue;
+        }
+        ros::Duration period = *it - prev;
+        ros::Duration t = msg.period_mean - period;
+        period_variance += t.toSec() * t.toSec();
+        prev = *it;
+      }
+      double period_stddev = sqrt(period_variance / (stats.arrival_time_list.size() - 1));
+      msg.period_stddev = ros::Duration(period_stddev);
+    }
+    else
+    {
+      // in that case, set to NaN
+      msg.period_mean = ros::Duration(0);
+      msg.period_stddev = ros::Duration(0);
+      msg.period_max = ros::Duration(0);
+    }
+
+    if (!pub_.getTopic().length())
+    {
+      ros::NodeHandle n("~");
+      // creating the publisher in the constructor results in a deadlock. so do it here.
+      pub_ = n.advertise<rosgraph_msgs::TopicStatistics>("/statistics", 1);
+    }
+
+    pub_.publish(msg);
+
+    // dynamic window resizing
+    if (stats.arrival_time_list.size() > static_cast<size_t>(max_elements) && pub_frequency_ * 2 <= max_window)
+    {
+      pub_frequency_ *= 2;
+    }
+    if (stats.arrival_time_list.size() < static_cast<size_t>(min_elements) && pub_frequency_ / 2 >= min_window)
+    {
+      pub_frequency_ /= 2;
+    }
+
+    // clear the window
+    stats.age_list.clear();
+    stats.arrival_time_list.clear();
+    stats.dropped_msgs = 0;
+    stats.stat_bytes_last = bytes_sent;
+
+  }
+  // store the stats for this connection
+  map_[callerid] = stats;
+}
+
+
+} // namespace ros

thirdparty/ros/ros_comm/clients/roscpp/src/libros/steady_timer.cpp

@@ -0,0 +1,226 @@
+/*
+ * Copyright (C) 2017, Felix Ruess, Roboception GmbH
+ *
+ * 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 names of Stanford University or 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.
+ */
+
+// Make sure we use CLOCK_MONOTONIC for the condition variable if not Apple.
+#ifndef __APPLE__
+#define BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
+#endif
+
+#include "ros/steady_timer.h"
+#include "ros/timer_manager.h"
+
+// check if we have really included the backported boost condition variable
+// just in case someone messes with the include order...
+#if BOOST_VERSION < 106100
+#ifndef USING_BACKPORTED_BOOST_CONDITION_VARIABLE
+#error "steady timer needs boost version >= 1.61 or the backported headers!"
+#endif
+#endif
+
+namespace ros
+{
+
+// specialization for SteadyTimer, to make sure we use a version with wait_until that uses the monotonic clock
+template<>
+void TimerManager<SteadyTime, WallDuration, SteadyTimerEvent>::threadFunc()
+{
+  SteadyTime current;
+  while (!quit_)
+  {
+    SteadyTime sleep_end;
+
+    boost::mutex::scoped_lock lock(timers_mutex_);
+
+    current = SteadyTime::now();
+
+    {
+      boost::mutex::scoped_lock waitlock(waiting_mutex_);
+
+      if (waiting_.empty())
+      {
+        sleep_end = current + WallDuration(0.1);
+      }
+      else
+      {
+        TimerInfoPtr info = findTimer(waiting_.front());
+
+        while (!waiting_.empty() && info && info->next_expected <= current)
+        {
+          current = SteadyTime::now();
+
+          //ROS_DEBUG("Scheduling timer callback for timer [%d] of period [%f], [%f] off expected", info->handle, info->period.toSec(), (current - info->next_expected).toSec());
+          CallbackInterfacePtr cb(boost::make_shared<TimerQueueCallback>(this, info, info->last_expected, info->last_real, info->next_expected));
+          info->callback_queue->addCallback(cb, (uint64_t)info.get());
+
+          waiting_.pop_front();
+
+          if (waiting_.empty())
+          {
+            break;
+          }
+
+          info = findTimer(waiting_.front());
+        }
+
+        if (info)
+        {
+          sleep_end = info->next_expected;
+        }
+      }
+    }
+
+    while (!new_timer_ && SteadyTime::now() < sleep_end && !quit_)
+    {
+      current = SteadyTime::now();
+
+      if (current >= sleep_end)
+      {
+        break;
+      }
+
+      // requires boost 1.61 for wait_until to actually use the steady clock
+      // see: https://svn.boost.org/trac/boost/ticket/6377
+      boost::chrono::steady_clock::time_point end_tp(boost::chrono::nanoseconds(sleep_end.toNSec()));
+      timers_cond_.wait_until(lock, end_tp);
+    }
+
+    new_timer_ = false;
+  }
+}
+
+SteadyTimer::Impl::Impl()
+  : started_(false)
+  , timer_handle_(-1)
+{ }
+
+SteadyTimer::Impl::~Impl()
+{
+  ROS_DEBUG("SteadyTimer deregistering callbacks.");
+  stop();
+}
+
+void SteadyTimer::Impl::start()
+{
+  if (!started_)
+  {
+    VoidConstPtr tracked_object;
+    if (has_tracked_object_)
+    {
+      tracked_object = tracked_object_.lock();
+    }
+    timer_handle_ = TimerManager<SteadyTime, WallDuration, SteadyTimerEvent>::global().add(period_, callback_, callback_queue_, tracked_object, oneshot_);
+    started_ = true;
+  }
+}
+
+void SteadyTimer::Impl::stop()
+{
+  if (started_)
+  {
+    started_ = false;
+    TimerManager<SteadyTime, WallDuration, SteadyTimerEvent>::global().remove(timer_handle_);
+    timer_handle_ = -1;
+  }
+}
+
+bool SteadyTimer::Impl::isValid()
+{
+  return !period_.isZero();
+}
+
+bool SteadyTimer::Impl::hasPending()
+{
+  if (!isValid() || timer_handle_ == -1)
+  {
+    return false;
+  }
+
+  return TimerManager<SteadyTime, WallDuration, SteadyTimerEvent>::global().hasPending(timer_handle_);
+}
+
+void SteadyTimer::Impl::setPeriod(const WallDuration& period, bool reset)
+{
+  period_ = period;
+  TimerManager<SteadyTime, WallDuration, SteadyTimerEvent>::global().setPeriod(timer_handle_, period, reset);
+}
+
+
+SteadyTimer::SteadyTimer(const SteadyTimerOptions& ops)
+: impl_(new Impl)
+{
+  impl_->period_ = ops.period;
+  impl_->callback_ = ops.callback;
+  impl_->callback_queue_ = ops.callback_queue;
+  impl_->tracked_object_ = ops.tracked_object;
+  impl_->has_tracked_object_ = (ops.tracked_object != NULL);
+  impl_->oneshot_ = ops.oneshot;
+}
+
+SteadyTimer::SteadyTimer(const SteadyTimer& rhs)
+{
+  impl_ = rhs.impl_;
+}
+
+SteadyTimer::~SteadyTimer()
+{
+}
+
+void SteadyTimer::start()
+{
+  if (impl_)
+  {
+    impl_->start();
+  }
+}
+
+void SteadyTimer::stop()
+{
+  if (impl_)
+  {
+    impl_->stop();
+  }
+}
+
+bool SteadyTimer::hasPending()
+{
+  if (impl_)
+  {
+    return impl_->hasPending();
+  }
+
+  return false;
+}
+
+void SteadyTimer::setPeriod(const WallDuration& period, bool reset)
+{
+  if (impl_)
+  {
+    impl_->setPeriod(period, reset);
+  }
+}
+
+}

thirdparty/ros/ros_comm/clients/roscpp/src/libros/subscriber.cpp

@@ -0,0 +1,107 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Stanford University or 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 "ros/subscriber.h"
+#include "ros/node_handle.h"
+#include "ros/topic_manager.h"
+
+namespace ros
+{
+
+  Subscriber::Impl::Impl()
+  : unsubscribed_(false)
+  { }
+
+  Subscriber::Impl::~Impl()
+  {
+    ROS_DEBUG("Subscriber on '%s' deregistering callbacks.", topic_.c_str());
+    unsubscribe();
+  }
+
+  bool Subscriber::Impl::isValid() const
+  {
+    return !unsubscribed_;
+  }
+
+  void Subscriber::Impl::unsubscribe()
+  {
+    if (!unsubscribed_)
+      {
+	unsubscribed_ = true;
+	TopicManager::instance()->unsubscribe(topic_, helper_);
+	node_handle_.reset();
+	helper_.reset();
+      }
+  }
+
+  Subscriber::Subscriber(const std::string& topic, const NodeHandle& node_handle, 
+			 const SubscriptionCallbackHelperPtr& helper)
+  : impl_(boost::make_shared<Impl>())
+  {
+    impl_->topic_ = topic;
+    impl_->node_handle_ = boost::make_shared<NodeHandle>(node_handle);
+    impl_->helper_ = helper;
+  }
+
+  Subscriber::Subscriber(const Subscriber& rhs)
+  {
+    impl_ = rhs.impl_;
+  }
+
+  Subscriber::~Subscriber()
+  {
+  }
+
+  void Subscriber::shutdown()
+  {
+    if (impl_)
+      {
+	impl_->unsubscribe();
+      }
+  }
+
+  std::string Subscriber::getTopic() const
+  {
+    if (impl_)
+      {
+	return impl_->topic_;
+      }
+
+    return std::string();
+  }
+
+  uint32_t Subscriber::getNumPublishers() const
+  {
+    if (impl_ && impl_->isValid())
+      {
+	return TopicManager::instance()->getNumPublishers(impl_->topic_);
+      }
+
+    return 0;
+  }
+
+} // namespace ros

thirdparty/ros/ros_comm/clients/roscpp/src/libros/subscriber_link.cpp

@@ -0,0 +1,87 @@
+/*
+ * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
+ *
+ * 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 names of Stanford University or 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 "ros/subscriber_link.h"
+#include "ros/publication.h"
+
+#include <boost/bind.hpp>
+
+namespace ros
+{
+
+SubscriberLink::SubscriberLink()
+  : connection_id_(0)
+{
+
+}
+
+SubscriberLink::~SubscriberLink()
+{
+
+}
+
+bool SubscriberLink::verifyDatatype(const std::string &datatype)
+{
+  PublicationPtr parent = parent_.lock();
+  if (!parent)
+  {
+    ROS_ERROR("Trying to verify the datatype on a publisher without a parent");
+    ROS_BREAK();
+
+    return false;
+  }
+
+  if (datatype != parent->getDataType())
+  {
+    ROS_ERROR( "tried to send a message with type %s on a " \
+                       "TransportSubscriberLink that has datatype %s",
+                datatype.c_str(), parent->getDataType().c_str());
+    return false; // todo: figure out a way to log this error
+  }
+
+  return true;
+}
+
+const std::string& SubscriberLink::getMD5Sum()
+{
+  PublicationPtr parent = parent_.lock();
+  return parent->getMD5Sum();
+}
+
+const std::string& SubscriberLink::getDataType()
+{
+  PublicationPtr parent = parent_.lock();
+  return parent->getDataType();
+}
+
+const std::string& SubscriberLink::getMessageDefinition()
+{
+  PublicationPtr parent = parent_.lock();
+  return parent->getMessageDefinition();
+}
+
+} // namespace ros

thirdparty/ros/ros_comm/clients/roscpp/src/libros/subscription.cpp

@@ -0,0 +1,853 @@
+/*
+ * 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: ";
+    {
+      boost::mutex::scoped_lock lock(publisher_links_mutex_);
+      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_CONN_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_CONN_LOG_DEBUG("Connected to publisher of topic [%s] at [%s:%d]", name_.c_str(), pub_host.c_str(), pub_port);
+    }
+    else
+    {
+      ROSCPP_CONN_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_;
+}
+
+}

thirdparty/ros/ros_comm/clients/roscpp/src/libros/subscription_queue.cpp

@@ -0,0 +1,187 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names 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 "ros/subscription_queue.h"
+#include "ros/message_deserializer.h"
+#include "ros/subscription_callback_helper.h"
+
+namespace ros
+{
+
+SubscriptionQueue::SubscriptionQueue(const std::string& topic, int32_t queue_size, bool allow_concurrent_callbacks)
+: topic_(topic)
+, size_(queue_size)
+, full_(false)
+, queue_size_(0)
+, allow_concurrent_callbacks_(allow_concurrent_callbacks)
+{}
+
+SubscriptionQueue::~SubscriptionQueue()
+{
+
+}
+
+void SubscriptionQueue::push(const SubscriptionCallbackHelperPtr& helper, const MessageDeserializerPtr& deserializer,
+                                 bool has_tracked_object, const VoidConstWPtr& tracked_object, bool nonconst_need_copy,
+                                 ros::Time receipt_time, bool* was_full)
+{
+  boost::mutex::scoped_lock lock(queue_mutex_);
+
+  if (was_full)
+  {
+    *was_full = false;
+  }
+
+  if(fullNoLock())
+  {
+    queue_.pop_front();
+    --queue_size_;
+
+    if (!full_)
+    {
+      ROS_DEBUG("Incoming queue was full for topic \"%s\". Discarded oldest message (current queue size [%d])", topic_.c_str(), (int)queue_.size());
+    }
+
+    full_ = true;
+
+    if (was_full)
+    {
+      *was_full = true;
+    }
+  }
+  else
+  {
+    full_ = false;
+  }
+
+  Item i;
+  i.helper = helper;
+  i.deserializer = deserializer;
+  i.has_tracked_object = has_tracked_object;
+  i.tracked_object = tracked_object;
+  i.nonconst_need_copy = nonconst_need_copy;
+  i.receipt_time = receipt_time;
+  queue_.push_back(i);
+  ++queue_size_;
+}
+
+void SubscriptionQueue::clear()
+{
+  boost::recursive_mutex::scoped_lock cb_lock(callback_mutex_);
+  boost::mutex::scoped_lock queue_lock(queue_mutex_);
+
+  queue_.clear();
+  queue_size_ = 0;
+}
+
+CallbackInterface::CallResult SubscriptionQueue::call()
+{
+  // The callback may result in our own destruction.  Therefore, we may need to keep a reference to ourselves
+  // that outlasts the scoped_try_lock
+  boost::shared_ptr<SubscriptionQueue> self;
+  boost::recursive_mutex::scoped_try_lock lock(callback_mutex_, boost::defer_lock);
+
+  if (!allow_concurrent_callbacks_)
+  {
+    lock.try_lock();
+    if (!lock.owns_lock())
+    {
+      return CallbackInterface::TryAgain;
+    }
+  }
+
+  VoidConstPtr tracker;
+  Item i;
+
+  {
+    boost::mutex::scoped_lock lock(queue_mutex_);
+
+    if (queue_.empty())
+    {
+      return CallbackInterface::Invalid;
+    }
+
+    i = queue_.front();
+
+    if (queue_.empty())
+    {
+      return CallbackInterface::Invalid;
+    }
+
+    if (i.has_tracked_object)
+    {
+      tracker = i.tracked_object.lock();
+
+      if (!tracker)
+      {
+        return CallbackInterface::Invalid;
+      }
+    }
+
+    queue_.pop_front();
+    --queue_size_;
+  }
+
+  VoidConstPtr msg = i.deserializer->deserialize();
+
+  // msg can be null here if deserialization failed
+  if (msg)
+  {
+    try
+    {
+      self = shared_from_this();
+    }
+    catch (boost::bad_weak_ptr&) // For the tests, where we don't create a shared_ptr
+    {}
+
+    SubscriptionCallbackHelperCallParams params;
+    params.event = MessageEvent<void const>(msg, i.deserializer->getConnectionHeader(), i.receipt_time, i.nonconst_need_copy, MessageEvent<void const>::CreateFunction());
+    i.helper->call(params);
+  }
+
+  return CallbackInterface::Success;
+}
+
+bool SubscriptionQueue::ready()
+{
+  return true;
+}
+
+bool SubscriptionQueue::full()
+{
+  boost::mutex::scoped_lock lock(queue_mutex_);
+  return fullNoLock();
+}
+
+bool SubscriptionQueue::fullNoLock()
+{
+  return (size_ > 0) && (queue_size_ >= (uint32_t)size_);
+}
+
+}
+

thirdparty/ros/ros_comm/clients/roscpp/src/libros/this_node.cpp

@@ -0,0 +1,189 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Stanford University or 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 <cstdio>
+#include "ros/this_node.h"
+#include "ros/names.h"
+#include "ros/topic_manager.h"
+#include "ros/init.h"
+
+#ifdef _MSC_VER
+  #ifdef snprintf
+    #undef snprintf
+  #endif
+  #define snprintf _snprintf_s
+#endif
+
+namespace ros
+{
+
+namespace names
+{
+void init(const M_string& remappings);
+}
+
+namespace this_node
+{
+
+// collect all static variables into a singleton to ensure proper destruction order
+class ThisNode
+{
+  std::string name_;
+  std::string namespace_;
+
+  ThisNode() : name_("empty") {}
+
+public:
+  static ThisNode& instance()
+  {
+    static ThisNode singleton;
+    return singleton;
+  }
+
+  const std::string& getName() const
+  {
+    return name_;
+  }
+
+  const std::string& getNamespace() const
+  {
+    return namespace_;
+  }
+
+  void init(const std::string& name, const M_string& remappings, uint32_t options);
+};
+
+
+const std::string& getName()
+{
+  return ThisNode::instance().getName();
+}
+
+const std::string& getNamespace()
+{
+  return ThisNode::instance().getNamespace();
+}
+
+void getAdvertisedTopics(V_string& topics)
+{
+  TopicManager::instance()->getAdvertisedTopics(topics);
+}
+
+void getSubscribedTopics(V_string& topics)
+{
+  TopicManager::instance()->getSubscribedTopics(topics);
+}
+
+void init(const std::string& name, const M_string& remappings, uint32_t options)
+{
+  ThisNode::instance().init(name, remappings, options);
+}
+
+void ThisNode::init(const std::string& name, const M_string& remappings, uint32_t options)
+{
+  char *ns_env = NULL;
+#ifdef _MSC_VER
+  _dupenv_s(&ns_env, NULL, "ROS_NAMESPACE");
+#else
+  ns_env = getenv("ROS_NAMESPACE");
+#endif
+
+  if (ns_env)
+  {
+    namespace_ = ns_env;
+#ifdef _MSC_VER
+    free(ns_env);
+#endif
+  }
+
+  if (name.empty()) {
+    throw InvalidNameException("The node name must not be empty");
+  }
+
+  name_ = name;
+
+  bool disable_anon = false;
+  M_string::const_iterator it = remappings.find("__name");
+  if (it != remappings.end())
+  {
+    name_ = it->second;
+    disable_anon = true;
+  }
+
+  it = remappings.find("__ns");
+  if (it != remappings.end())
+  {
+    namespace_ = it->second;
+  }
+
+  if (namespace_.empty())
+  {
+    namespace_ = "/";
+  }
+
+  namespace_ = (namespace_ == "/")
+    ? std::string("/") 
+    : ("/" + namespace_)
+    ;
+
+
+  std::string error;
+  if (!names::validate(namespace_, error))
+  {
+    std::stringstream ss;
+    ss << "Namespace [" << namespace_ << "] is invalid: " << error;
+    throw InvalidNameException(ss.str());
+  }
+
+  // names must be initialized here, because it requires the namespace to already be known so that it can properly resolve names.
+  // It must be done before we resolve g_name, because otherwise the name will not get remapped.
+  names::init(remappings);
+
+  if (name_.find("/") != std::string::npos)
+  {
+    throw InvalidNodeNameException(name_, "node names cannot contain /");
+  }
+  if (name_.find("~") != std::string::npos)
+  {
+    throw InvalidNodeNameException(name_, "node names cannot contain ~");
+  }
+
+  name_ = names::resolve(namespace_, name_);
+
+  if (options & init_options::AnonymousName && !disable_anon)
+  {
+    char buf[200];
+    snprintf(buf, sizeof(buf), "_%llu", (unsigned long long)WallTime::now().toNSec());
+    name_ += buf;
+  }
+
+  ros::console::setFixedFilterToken("node", name_);
+}
+
+} // namespace this_node
+
+} // namespace ros

thirdparty/ros/ros_comm/clients/roscpp/src/libros/timer.cpp

@@ -0,0 +1,150 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Stanford University or 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 "ros/timer.h"
+#include "ros/timer_manager.h"
+
+namespace ros
+{
+
+Timer::Impl::Impl()
+  : started_(false)
+  , timer_handle_(-1)
+{ }
+
+Timer::Impl::~Impl()
+{
+  ROS_DEBUG("Timer deregistering callbacks.");
+  stop();
+}
+
+bool Timer::Impl::hasStarted() const
+{
+  return started_;
+}
+
+bool Timer::Impl::isValid()
+{
+  return !period_.isZero();
+}
+
+void Timer::Impl::start()
+{
+  if (!started_)
+  {
+    VoidConstPtr tracked_object;
+    if (has_tracked_object_)
+    {
+      tracked_object = tracked_object_.lock();
+    }
+
+    timer_handle_ = TimerManager<Time, Duration, TimerEvent>::global().add(period_, callback_, callback_queue_, tracked_object, oneshot_);
+    started_ = true;
+  }
+}
+
+void Timer::Impl::stop()
+{
+  if (started_)
+  {
+    started_ = false;
+    TimerManager<Time, Duration, TimerEvent>::global().remove(timer_handle_);
+    timer_handle_ = -1;
+  }
+}
+
+bool Timer::Impl::hasPending()
+{
+  if (!isValid() || timer_handle_ == -1)
+  {
+    return false;
+  }
+
+  return TimerManager<Time, Duration, TimerEvent>::global().hasPending(timer_handle_);
+}
+
+void Timer::Impl::setPeriod(const Duration& period, bool reset)
+{
+  period_ = period;
+  TimerManager<Time, Duration, TimerEvent>::global().setPeriod(timer_handle_, period, reset);
+}
+
+Timer::Timer(const TimerOptions& ops)
+: impl_(new Impl)
+{
+  impl_->period_ = ops.period;
+  impl_->callback_ = ops.callback;
+  impl_->callback_queue_ = ops.callback_queue;
+  impl_->tracked_object_ = ops.tracked_object;
+  impl_->has_tracked_object_ = (ops.tracked_object != NULL);
+  impl_->oneshot_ = ops.oneshot;
+}
+
+Timer::Timer(const Timer& rhs)
+{
+  impl_ = rhs.impl_;
+}
+
+Timer::~Timer()
+{
+}
+
+void Timer::start()
+{
+  if (impl_)
+  {
+    impl_->start();
+  }
+}
+
+void Timer::stop()
+{
+  if (impl_)
+  {
+    impl_->stop();
+  }
+}
+
+bool Timer::hasPending()
+{
+  if (impl_)
+  {
+    return impl_->hasPending();
+  }
+
+  return false;
+}
+
+void Timer::setPeriod(const Duration& period, bool reset)
+{
+  if (impl_)
+  {
+    impl_->setPeriod(period, reset);
+  }
+}
+
+}

thirdparty/ros/ros_comm/clients/roscpp/src/libros/topic.cpp

@@ -0,0 +1,65 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, 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 "ros/topic.h"
+#include "ros/callback_queue.h"
+
+namespace ros
+{
+namespace topic
+{
+
+void waitForMessageImpl(SubscribeOptions& ops, 
+			const boost::function<bool(void)>& ready_pred, 
+			NodeHandle& nh, ros::Duration timeout)
+{
+  ros::CallbackQueue queue;
+  ops.callback_queue = &queue;
+
+  ros::Subscriber sub = nh.subscribe(ops);
+
+  ros::Time end = ros::Time::now() + timeout;
+  while (!ready_pred() && nh.ok())
+  {
+    queue.callAvailable(ros::WallDuration(0.1));
+
+    if (!timeout.isZero() && ros::Time::now() >= end)
+    {
+      return;
+    }
+  }
+}
+
+} // namespace topic
+} // namespace ros

thirdparty/ros/ros_comm/clients/roscpp/src/libros/transport/transport.cpp

@@ -0,0 +1,127 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2014, Open Source Robotics Foundation, 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 "ros/transport/transport.h"
+#include "ros/console.h"
+#include <netinet/in.h>
+#include <sys/socket.h>
+#include <netdb.h>
+
+#if !defined(__ANDROID__)
+#include <ifaddrs.h>
+#endif
+
+#ifndef NI_MAXHOST
+  #define NI_MAXHOST 1025
+#endif
+
+namespace ros
+{
+
+Transport::Transport()
+: only_localhost_allowed_(false)
+{
+  char *ros_ip_env = NULL, *ros_hostname_env = NULL;
+  #ifdef _MSC_VER
+    _dupenv_s(&ros_ip_env, NULL, "ROS_IP");
+    _dupenv_s(&ros_hostname_env, NULL, "ROS_HOSTNAME");
+  #else
+    ros_ip_env = getenv("ROS_IP");
+    ros_hostname_env = getenv("ROS_HOSTNAME");
+  #endif
+  if (ros_hostname_env && !strcmp(ros_hostname_env, "localhost"))
+    only_localhost_allowed_ = true;
+  else if (ros_ip_env && !strncmp(ros_ip_env, "127.", 4))
+    only_localhost_allowed_ = true;
+  else if (ros_ip_env && !strcmp(ros_ip_env, "::1"))
+    only_localhost_allowed_ = true;
+
+  char our_hostname[256] = {0};
+  gethostname(our_hostname, sizeof(our_hostname)-1);
+  allowed_hosts_.push_back(std::string(our_hostname));
+  allowed_hosts_.push_back("localhost");
+#if !defined(__ANDROID__)
+  // for ipv4 loopback, we'll explicitly search for 127.* in isHostAllowed()
+  // now we need to iterate all local interfaces and add their addresses
+  // from the getifaddrs manpage:  (maybe something similar for windows ?) 
+  ifaddrs *ifaddr;
+  if (-1 == getifaddrs(&ifaddr))
+  {
+    ROS_ERROR("getifaddr() failed");
+    return;
+  }
+  for (ifaddrs *ifa = ifaddr; ifa; ifa = ifa->ifa_next)
+  {
+    if(NULL == ifa->ifa_addr)
+      continue; // ifa_addr can be NULL
+    int family = ifa->ifa_addr->sa_family;
+    if (family != AF_INET && family != AF_INET6)
+      continue; // we're only looking for IP addresses
+    char addr[NI_MAXHOST] = {0};
+    if (getnameinfo(ifa->ifa_addr,
+                    (family == AF_INET) ? sizeof(sockaddr_in)
+                                        : sizeof(sockaddr_in6),
+                    addr, NI_MAXHOST,
+                    NULL, 0, NI_NUMERICHOST))
+    {
+      ROS_ERROR("getnameinfo() failed");
+      continue;
+    }
+    allowed_hosts_.push_back(std::string(addr));
+  }
+  freeifaddrs(ifaddr);
+#endif
+}
+
+bool Transport::isHostAllowed(const std::string &host) const
+{
+  if (!only_localhost_allowed_)
+    return true; // doesn't matter; we'll connect to anybody
+
+  if (host.length() >= 4 && host.substr(0, 4) == std::string("127."))
+    return true; // ipv4 localhost
+  // now, loop through the list of valid hostnames and see if we find it
+  for (std::vector<std::string>::const_iterator it = allowed_hosts_.begin(); 
+       it != allowed_hosts_.end(); ++it)
+  {
+    if (host == *it)
+      return true; // hooray
+  }
+  ROS_WARN("ROS_HOSTNAME / ROS_IP is set to only allow local connections, so "
+           "a requested connection to '%s' is being rejected.", host.c_str());
+  return false; // sadness
+}
+
+}
+

thirdparty/ros/ros_comm/clients/roscpp/src/libros/transport/transport_tcp.cpp

@@ -0,0 +1,757 @@
+/*
+ * 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 "ros/io.h"
+#include "ros/transport/transport_tcp.h"
+#include "ros/poll_set.h"
+#include "ros/header.h"
+#include "ros/file_log.h"
+#include <ros/assert.h>
+#include <sstream>
+#include <boost/bind.hpp>
+#include <fcntl.h>
+#include <errno.h>
+namespace ros
+{
+
+bool TransportTCP::s_use_keepalive_ = true;
+bool TransportTCP::s_use_ipv6_ = false;
+
+TransportTCP::TransportTCP(PollSet* poll_set, int flags)
+: sock_(ROS_INVALID_SOCKET)
+, closed_(false)
+, expecting_read_(false)
+, expecting_write_(false)
+, is_server_(false)
+, server_port_(-1)
+, local_port_(-1)
+, poll_set_(poll_set)
+, flags_(flags)
+{
+
+}
+
+TransportTCP::~TransportTCP()
+{
+  ROS_ASSERT_MSG(sock_ == -1, "TransportTCP socket [%d] was never closed", sock_);
+}
+
+bool TransportTCP::setSocket(int sock)
+{
+  sock_ = sock;
+  return initializeSocket();
+}
+
+bool TransportTCP::setNonBlocking()
+{
+  if (!(flags_ & SYNCHRONOUS))
+  {
+	  int result = set_non_blocking(sock_);
+	  if ( result != 0 ) {
+	      ROS_ERROR("setting socket [%d] as non_blocking failed with error [%d]", sock_, result);
+      close();
+      return false;
+    }
+  }
+
+  return true;
+}
+
+bool TransportTCP::initializeSocket()
+{
+  ROS_ASSERT(sock_ != ROS_INVALID_SOCKET);
+
+  if (!setNonBlocking())
+  {
+    return false;
+  }
+
+  setKeepAlive(s_use_keepalive_, 60, 10, 9);
+
+  // connect() will set cached_remote_host_ because it already has the host/port available
+  if (cached_remote_host_.empty())
+  {
+    if (is_server_)
+    {
+      cached_remote_host_ = "TCPServer Socket";
+    }
+    else
+    {
+      std::stringstream ss;
+      ss << getClientURI() << " on socket " << sock_;
+      cached_remote_host_ = ss.str();
+    }
+  }
+
+  if (local_port_ < 0)
+  {
+    la_len_ = s_use_ipv6_  ? sizeof(sockaddr_in6) : sizeof(sockaddr_in);
+    getsockname(sock_, (sockaddr *)&local_address_, &la_len_);
+    switch (local_address_.ss_family)
+    {
+      case AF_INET:
+        local_port_ = ntohs(((sockaddr_in *)&local_address_)->sin_port);
+        break;
+      case AF_INET6:
+        local_port_ = ntohs(((sockaddr_in6 *)&local_address_)->sin6_port);
+        break;
+    }
+  }
+
+#ifdef ROSCPP_USE_TCP_NODELAY
+  setNoDelay(true);
+#endif
+
+  ROS_ASSERT(poll_set_ || (flags_ & SYNCHRONOUS));
+  if (poll_set_)
+  {
+    ROS_DEBUG("Adding tcp socket [%d] to pollset", sock_);
+    poll_set_->addSocket(sock_, boost::bind(&TransportTCP::socketUpdate, this, _1), shared_from_this());
+  }
+
+  if (!(flags_ & SYNCHRONOUS))
+  {
+    //enableRead();
+  }
+
+  return true;
+}
+
+void TransportTCP::parseHeader(const Header& header)
+{
+  std::string nodelay;
+  if (header.getValue("tcp_nodelay", nodelay) && nodelay == "1")
+  {
+    ROSCPP_LOG_DEBUG("Setting nodelay on socket [%d]", sock_);
+    setNoDelay(true);
+  }
+}
+
+void TransportTCP::setNoDelay(bool nodelay)
+{
+  int flag = nodelay ? 1 : 0;
+  int result = setsockopt(sock_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag, sizeof(int));
+  if (result < 0)
+  {
+    ROS_ERROR("setsockopt failed to set TCP_NODELAY on socket [%d] [%s]", sock_, cached_remote_host_.c_str());
+  }
+}
+
+void TransportTCP::setKeepAlive(bool use, uint32_t idle, uint32_t interval, uint32_t count)
+{
+  if (use)
+  {
+    int val = 1;
+    if (setsockopt(sock_, SOL_SOCKET, SO_KEEPALIVE, reinterpret_cast<const char*>(&val), sizeof(val)) != 0)
+    {
+      ROS_DEBUG("setsockopt failed to set SO_KEEPALIVE on socket [%d] [%s]", sock_, cached_remote_host_.c_str());
+    }
+
+/* cygwin SOL_TCP does not seem to support TCP_KEEPIDLE, TCP_KEEPINTVL, TCP_KEEPCNT */
+#if defined(SOL_TCP) && defined(TCP_KEEPIDLE)
+    val = idle;
+    if (setsockopt(sock_, SOL_TCP, TCP_KEEPIDLE, &val, sizeof(val)) != 0)
+    {
+      ROS_DEBUG("setsockopt failed to set TCP_KEEPIDLE on socket [%d] [%s]", sock_, cached_remote_host_.c_str());
+    }
+#endif
+
+#if defined(SOL_TCP) && defined(TCP_KEEPINTVL)
+    val = interval;
+    if (setsockopt(sock_, SOL_TCP, TCP_KEEPINTVL, &val, sizeof(val)) != 0)
+    {
+      ROS_DEBUG("setsockopt failed to set TCP_KEEPINTVL on socket [%d] [%s]", sock_, cached_remote_host_.c_str());
+    }
+#endif
+
+#if defined(SOL_TCP) && defined(TCP_KEEPCNT)
+    val = count;
+    if (setsockopt(sock_, SOL_TCP, TCP_KEEPCNT, &val, sizeof(val)) != 0)
+    {
+      ROS_DEBUG("setsockopt failed to set TCP_KEEPCNT on socket [%d] [%s]", sock_, cached_remote_host_.c_str());
+    }
+#endif
+  }
+  else
+  {
+    int val = 0;
+    if (setsockopt(sock_, SOL_SOCKET, SO_KEEPALIVE, reinterpret_cast<const char*>(&val), sizeof(val)) != 0)
+    {
+    	ROS_DEBUG("setsockopt failed to set SO_KEEPALIVE on socket [%d] [%s]", sock_, cached_remote_host_.c_str());
+    }
+  }
+}
+
+bool TransportTCP::connect(const std::string& host, int port)
+{
+  if (!isHostAllowed(host))
+    return false; // adios amigo
+
+  sock_ = socket(s_use_ipv6_ ? AF_INET6 : AF_INET, SOCK_STREAM, 0);
+  connected_host_ = host;
+  connected_port_ = port;
+
+  if (sock_ == ROS_INVALID_SOCKET)
+  {
+    ROS_ERROR("socket() failed with error [%s]",  last_socket_error_string());
+    return false;
+  }
+
+  setNonBlocking();
+
+  sockaddr_storage sas;
+  socklen_t sas_len;
+
+  in_addr ina;
+  in6_addr in6a;
+  if (inet_pton(AF_INET, host.c_str(), &ina) == 1)
+  {
+    sockaddr_in *address = (sockaddr_in*) &sas;
+    sas_len = sizeof(sockaddr_in);
+    
+    la_len_ = sizeof(sockaddr_in);
+    address->sin_family = AF_INET;
+    address->sin_port = htons(port);
+    address->sin_addr.s_addr = ina.s_addr;
+  }
+  else if (inet_pton(AF_INET6, host.c_str(), &in6a) == 1)
+  {
+    sockaddr_in6 *address = (sockaddr_in6*) &sas;
+    sas_len = sizeof(sockaddr_in6);
+    la_len_ = sizeof(sockaddr_in6);
+    address->sin6_family = AF_INET6;
+    address->sin6_port = htons(port);
+    memcpy(address->sin6_addr.s6_addr, in6a.s6_addr, sizeof(in6a.s6_addr));
+  }
+  else
+  {
+    struct addrinfo* addr;
+    struct addrinfo hints;
+    memset(&hints, 0, sizeof(hints));
+    hints.ai_family = AF_UNSPEC;
+
+    if (getaddrinfo(host.c_str(), NULL, &hints, &addr) != 0)
+    {
+      close();
+      ROS_ERROR("couldn't resolve publisher host [%s]", host.c_str());
+      return false;
+    }
+
+    bool found = false;
+    struct addrinfo* it = addr;
+    char namebuf[128];
+    for (; it; it = it->ai_next)
+    {
+      if (!s_use_ipv6_ && it->ai_family == AF_INET)
+      {
+        sockaddr_in *address = (sockaddr_in*) &sas;
+        sas_len = sizeof(*address);
+        
+        memcpy(address, it->ai_addr, it->ai_addrlen);
+        address->sin_family = it->ai_family;
+        address->sin_port = htons(port);
+	
+        strcpy(namebuf, inet_ntoa(address->sin_addr));
+        found = true;
+        break;
+      }
+      if (s_use_ipv6_ && it->ai_family == AF_INET6)
+      {
+        sockaddr_in6 *address = (sockaddr_in6*) &sas;
+        sas_len = sizeof(*address);
+      
+        memcpy(address, it->ai_addr, it->ai_addrlen);
+        address->sin6_family = it->ai_family;
+        address->sin6_port = htons((u_short) port);
+      
+        // TODO IPV6: does inet_ntop need other includes for Windows?
+        inet_ntop(AF_INET6, (void*)&(address->sin6_addr), namebuf, sizeof(namebuf));
+        found = true;
+        break;
+      }
+    }
+
+    freeaddrinfo(addr);
+
+    if (!found)
+    {
+      ROS_ERROR("Couldn't resolve an address for [%s]\n", host.c_str());
+      return false;
+    }
+
+    ROSCPP_LOG_DEBUG("Resolved publisher host [%s] to [%s] for socket [%d]", host.c_str(), namebuf, sock_);
+  }
+
+  int ret = ::connect(sock_, (sockaddr*) &sas, sas_len);
+  // windows might need some time to sleep (input from service robotics hack) add this if testing proves it is necessary.
+  // ROS_ASSERT((flags_ & SYNCHRONOUS) || ret != 0);
+  if (((flags_ & SYNCHRONOUS) && ret != 0) || // synchronous, connect() should return 0
+      (!(flags_ & SYNCHRONOUS) && ret != 0 && last_socket_error() != ROS_SOCKETS_ASYNCHRONOUS_CONNECT_RETURN)) 
+      // asynchronous, connect() may return 0 or -1. When return -1, WSAGetLastError()=WSAEWOULDBLOCK/errno=EINPROGRESS
+  {
+    ROSCPP_CONN_LOG_DEBUG("Connect to tcpros publisher [%s:%d] failed with error [%d, %s]", host.c_str(), port, ret, last_socket_error_string());
+    close();
+
+    return false;
+  }
+
+  // from daniel stonier:
+#ifdef WIN32
+  // This is hackish, but windows fails at recv() if its slow to connect (e.g. happens with wireless)
+  // recv() needs to check if its connected or not when its asynchronous?
+  Sleep(100);
+#endif
+
+
+  std::stringstream ss;
+  ss << host << ":" << port << " on socket " << sock_;
+  cached_remote_host_ = ss.str();
+
+  if (!initializeSocket())
+  {
+    return false;
+  }
+
+  if (flags_ & SYNCHRONOUS)
+  {
+    ROSCPP_CONN_LOG_DEBUG("connect() succeeded to [%s:%d] on socket [%d]", host.c_str(), port, sock_);
+  }
+  else
+  {
+    ROSCPP_CONN_LOG_DEBUG("Async connect() in progress to [%s:%d] on socket [%d]", host.c_str(), port, sock_);
+  }
+
+  return true;
+}
+
+bool TransportTCP::listen(int port, int backlog, const AcceptCallback& accept_cb)
+{
+  is_server_ = true;
+  accept_cb_ = accept_cb;
+
+  if (s_use_ipv6_)
+  {
+    sock_ = socket(AF_INET6, SOCK_STREAM, 0);
+    sockaddr_in6 *address = (sockaddr_in6 *)&server_address_;
+    address->sin6_family = AF_INET6;
+    address->sin6_addr = isOnlyLocalhostAllowed() ? 
+                         in6addr_loopback : 
+                         in6addr_any;
+    address->sin6_port = htons(port);
+    sa_len_ = sizeof(sockaddr_in6);
+  }
+  else
+  {
+    sock_ = socket(AF_INET, SOCK_STREAM, 0);
+    sockaddr_in *address = (sockaddr_in *)&server_address_;
+    address->sin_family = AF_INET;
+    address->sin_addr.s_addr = isOnlyLocalhostAllowed() ? 
+                               htonl(INADDR_LOOPBACK) : 
+                               INADDR_ANY;
+    address->sin_port = htons(port);
+    sa_len_ = sizeof(sockaddr_in);
+  }
+
+  if (sock_ <= 0)
+  {
+    ROS_ERROR("socket() failed with error [%s]", last_socket_error_string());
+    return false;
+  }
+
+
+  if (bind(sock_, (sockaddr *)&server_address_, sa_len_) < 0)
+  {
+    ROS_ERROR("bind() failed with error [%s]", last_socket_error_string());
+    return false;
+  }
+
+  ::listen(sock_, backlog);
+  getsockname(sock_, (sockaddr *)&server_address_, &sa_len_);
+
+  switch (server_address_.ss_family)
+  {
+    case AF_INET:
+      server_port_ = ntohs(((sockaddr_in *)&server_address_)->sin_port);
+      break;
+    case AF_INET6:
+      server_port_ = ntohs(((sockaddr_in6 *)&server_address_)->sin6_port);
+      break;
+  }
+
+  if (!initializeSocket())
+  {
+    return false;
+  }
+
+  if (!(flags_ & SYNCHRONOUS))
+  {
+    enableRead();
+  }
+
+  return true;
+}
+
+void TransportTCP::close()
+{
+  Callback disconnect_cb;
+
+  if (!closed_)
+  {
+    {
+      boost::recursive_mutex::scoped_lock lock(close_mutex_);
+
+      if (!closed_)
+      {
+        closed_ = true;
+
+        ROS_ASSERT(sock_ != ROS_INVALID_SOCKET);
+
+        if (poll_set_)
+        {
+          poll_set_->delSocket(sock_);
+        }
+
+        ::shutdown(sock_, ROS_SOCKETS_SHUT_RDWR);
+        if ( close_socket(sock_) != 0 )
+        {
+          ROS_ERROR("Error closing socket [%d]: [%s]", sock_, last_socket_error_string());
+        } else
+        {
+          ROSCPP_LOG_DEBUG("TCP socket [%d] closed", sock_);
+        }
+        sock_ = ROS_INVALID_SOCKET;
+
+        disconnect_cb = disconnect_cb_;
+
+        disconnect_cb_ = Callback();
+        read_cb_ = Callback();
+        write_cb_ = Callback();
+        accept_cb_ = AcceptCallback();
+      }
+    }
+  }
+
+  if (disconnect_cb)
+  {
+    disconnect_cb(shared_from_this());
+  }
+}
+
+int32_t TransportTCP::read(uint8_t* buffer, uint32_t size)
+{
+  {
+    boost::recursive_mutex::scoped_lock lock(close_mutex_);
+
+    if (closed_)
+    {
+      ROSCPP_LOG_DEBUG("Tried to read on a closed socket [%d]", sock_);
+      return -1;
+    }
+  }
+
+  ROS_ASSERT(size > 0);
+
+  // never read more than INT_MAX since this is the maximum we can report back with the current return type
+  uint32_t read_size = std::min(size, static_cast<uint32_t>(INT_MAX));
+  int num_bytes = ::recv(sock_, reinterpret_cast<char*>(buffer), read_size, 0);
+  if (num_bytes < 0)
+  {
+	if ( !last_socket_error_is_would_block() ) // !WSAWOULDBLOCK / !EAGAIN && !EWOULDBLOCK
+    {
+      ROSCPP_LOG_DEBUG("recv() on socket [%d] failed with error [%s]", sock_, last_socket_error_string());
+      close();
+    }
+    else
+    {
+      num_bytes = 0;
+    }
+  }
+  else if (num_bytes == 0)
+  {
+    ROSCPP_LOG_DEBUG("Socket [%d] received 0/%u bytes, closing", sock_, size);
+    close();
+    return -1;
+  }
+
+  return num_bytes;
+}
+
+int32_t TransportTCP::write(uint8_t* buffer, uint32_t size)
+{
+  {
+    boost::recursive_mutex::scoped_lock lock(close_mutex_);
+
+    if (closed_)
+    {
+      ROSCPP_LOG_DEBUG("Tried to write on a closed socket [%d]", sock_);
+      return -1;
+    }
+  }
+
+  ROS_ASSERT(size > 0);
+
+  // never write more than INT_MAX since this is the maximum we can report back with the current return type
+  uint32_t writesize = std::min(size, static_cast<uint32_t>(INT_MAX));
+  int num_bytes = ::send(sock_, reinterpret_cast<const char*>(buffer), writesize, 0);
+  if (num_bytes < 0)
+  {
+    if ( !last_socket_error_is_would_block() )
+    {
+      ROSCPP_LOG_DEBUG("send() on socket [%d] failed with error [%s]", sock_, last_socket_error_string());
+      close();
+    }
+    else
+    {
+      num_bytes = 0;
+    }
+  }
+
+  return num_bytes;
+}
+
+void TransportTCP::enableRead()
+{
+  ROS_ASSERT(!(flags_ & SYNCHRONOUS));
+
+  {
+    boost::recursive_mutex::scoped_lock lock(close_mutex_);
+
+    if (closed_)
+    {
+      return;
+    }
+  }
+
+  if (!expecting_read_)
+  {
+    poll_set_->addEvents(sock_, POLLIN);
+    expecting_read_ = true;
+  }
+}
+
+void TransportTCP::disableRead()
+{
+  ROS_ASSERT(!(flags_ & SYNCHRONOUS));
+
+  {
+    boost::recursive_mutex::scoped_lock lock(close_mutex_);
+
+    if (closed_)
+    {
+      return;
+    }
+  }
+
+  if (expecting_read_)
+  {
+    poll_set_->delEvents(sock_, POLLIN);
+    expecting_read_ = false;
+  }
+}
+
+void TransportTCP::enableWrite()
+{
+  ROS_ASSERT(!(flags_ & SYNCHRONOUS));
+
+  {
+    boost::recursive_mutex::scoped_lock lock(close_mutex_);
+
+    if (closed_)
+    {
+      return;
+    }
+  }
+
+  if (!expecting_write_)
+  {
+    poll_set_->addEvents(sock_, POLLOUT);
+    expecting_write_ = true;
+  }
+}
+
+void TransportTCP::disableWrite()
+{
+  ROS_ASSERT(!(flags_ & SYNCHRONOUS));
+
+  {
+    boost::recursive_mutex::scoped_lock lock(close_mutex_);
+
+    if (closed_)
+    {
+      return;
+    }
+  }
+
+  if (expecting_write_)
+  {
+    poll_set_->delEvents(sock_, POLLOUT);
+    expecting_write_ = false;
+  }
+}
+
+TransportTCPPtr TransportTCP::accept()
+{
+  ROS_ASSERT(is_server_);
+
+  sockaddr client_address;
+  socklen_t len = sizeof(client_address);
+  int new_sock = ::accept(sock_, (sockaddr *)&client_address, &len);
+  if (new_sock >= 0)
+  {
+    ROSCPP_LOG_DEBUG("Accepted connection on socket [%d], new socket [%d]", sock_, new_sock);
+
+    TransportTCPPtr transport(boost::make_shared<TransportTCP>(poll_set_, flags_));
+    if (!transport->setSocket(new_sock))
+    {
+      ROS_ERROR("Failed to set socket on transport for socket %d", new_sock);
+    }
+
+    return transport;
+  }
+  else
+  {
+    ROS_ERROR("accept() on socket [%d] failed with error [%s]", sock_,  last_socket_error_string());
+  }
+
+  return TransportTCPPtr();
+}
+
+void TransportTCP::socketUpdate(int events)
+{
+  {
+    boost::recursive_mutex::scoped_lock lock(close_mutex_);
+    if (closed_)
+    {
+      return;
+    }
+
+    // Handle read events before err/hup/nval, since there may be data left on the wire
+    if ((events & POLLIN) && expecting_read_)
+    {
+      if (is_server_)
+      {
+        // Should not block here, because poll() said that it's ready
+        // for reading
+        TransportTCPPtr transport = accept();
+        if (transport)
+        {
+          ROS_ASSERT(accept_cb_);
+          accept_cb_(transport);
+        }
+      }
+      else
+      {
+        if (read_cb_)
+        {
+          read_cb_(shared_from_this());
+        }
+      }
+    }
+
+    if ((events & POLLOUT) && expecting_write_)
+    {
+      if (write_cb_)
+      {
+        write_cb_(shared_from_this());
+      }
+    }
+  }
+
+  if((events & POLLERR) ||
+     (events & POLLHUP) ||
+     (events & POLLNVAL))
+  {
+    uint32_t error = -1;
+    socklen_t len = sizeof(error);
+    if (getsockopt(sock_, SOL_SOCKET, SO_ERROR, reinterpret_cast<char*>(&error), &len) < 0)
+    {
+      ROSCPP_LOG_DEBUG("getsockopt failed on socket [%d]", sock_);
+    }
+  #ifdef _MSC_VER
+    char err[60];
+    strerror_s(err,60,error);
+    ROSCPP_LOG_DEBUG("Socket %d closed with (ERR|HUP|NVAL) events %d: %s", sock_, events, err);
+  #else
+    ROSCPP_LOG_DEBUG("Socket %d closed with (ERR|HUP|NVAL) events %d: %s", sock_, events, strerror(error));
+  #endif
+
+    close();
+  }
+}
+
+std::string TransportTCP::getTransportInfo()
+{
+  std::stringstream str;
+  str << "TCPROS connection on port " << local_port_ << " to [" << cached_remote_host_ << "]";
+  return str.str();
+}
+
+std::string TransportTCP::getClientURI()
+{
+  ROS_ASSERT(!is_server_);
+
+  sockaddr_storage sas;
+  socklen_t sas_len = sizeof(sas);
+  getpeername(sock_, (sockaddr *)&sas, &sas_len);
+  
+  sockaddr_in *sin = (sockaddr_in *)&sas;
+  sockaddr_in6 *sin6 = (sockaddr_in6 *)&sas;
+
+  char namebuf[128];
+  int port;
+
+  switch (sas.ss_family)
+  {
+    case AF_INET:
+      port = ntohs(sin->sin_port);
+      strcpy(namebuf, inet_ntoa(sin->sin_addr));
+      break;
+    case AF_INET6:
+      port = ntohs(sin6->sin6_port);
+      inet_ntop(AF_INET6, (void*)&(sin6->sin6_addr), namebuf, sizeof(namebuf));
+      break;
+    default:
+      namebuf[0] = 0;
+      port = 0;
+      break;
+  }
+
+  std::string ip = namebuf;
+  std::stringstream uri;
+  uri << ip << ":" << port;
+
+  return uri.str();
+}
+
+} // namespace ros

thirdparty/ros/ros_comm/clients/roscpp/src/libros/transport/transport_udp.cpp

@@ -0,0 +1,724 @@
+/*
+ * 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 "ros/transport/transport_udp.h"
+#include "ros/poll_set.h"
+#include "ros/file_log.h"
+
+#include <ros/assert.h>
+#include <boost/bind.hpp>
+
+#include <fcntl.h>
+#if defined(__APPLE__)
+  // For readv() and writev()
+  #include <sys/types.h>
+  #include <sys/uio.h>
+  #include <unistd.h>
+#elif defined(__ANDROID__)
+  // For readv() and writev() on ANDROID
+  #include <sys/uio.h>
+#elif defined(_POSIX_VERSION)
+  // For readv() and writev()
+  #include <sys/uio.h>
+#endif
+
+namespace ros
+{
+
+TransportUDP::TransportUDP(PollSet* poll_set, int flags, int max_datagram_size)
+: sock_(-1)
+, closed_(false)
+, expecting_read_(false)
+, expecting_write_(false)
+, is_server_(false)
+, server_port_(-1)
+, local_port_(-1)
+, poll_set_(poll_set)
+, flags_(flags)
+, connection_id_(0)
+, current_message_id_(0)
+, total_blocks_(0)
+, last_block_(0)
+, max_datagram_size_(max_datagram_size)
+, data_filled_(0)
+, reorder_buffer_(0)
+, reorder_bytes_(0)
+{
+  // This may eventually be machine dependent
+  if (max_datagram_size_ == 0)
+    max_datagram_size_ = 1500;
+  reorder_buffer_ = new uint8_t[max_datagram_size_];
+  reorder_start_ = reorder_buffer_;
+  data_buffer_ = new uint8_t[max_datagram_size_];
+  data_start_ = data_buffer_;
+}
+
+TransportUDP::~TransportUDP()
+{
+  ROS_ASSERT_MSG(sock_ == ROS_INVALID_SOCKET, "TransportUDP socket [%d] was never closed", sock_);
+  delete [] reorder_buffer_;
+  delete [] data_buffer_;
+}
+
+bool TransportUDP::setSocket(int sock)
+{
+  sock_ = sock;
+  return initializeSocket();
+}
+
+void TransportUDP::socketUpdate(int events)
+{
+  {
+    boost::mutex::scoped_lock lock(close_mutex_);
+
+    if (closed_)
+    {
+      return;
+    }
+  }
+
+  if((events & POLLERR) ||
+     (events & POLLHUP) ||
+     (events & POLLNVAL))
+  {
+    ROSCPP_LOG_DEBUG("Socket %d closed with (ERR|HUP|NVAL) events %d", sock_, events);
+    close();
+  }
+  else
+  {
+    if ((events & POLLIN) && expecting_read_)
+    {
+      if (read_cb_)
+      {
+        read_cb_(shared_from_this());
+      }
+    }
+
+    if ((events & POLLOUT) && expecting_write_)
+    {
+      if (write_cb_)
+      {
+        write_cb_(shared_from_this());
+      }
+    }
+  }
+
+}
+
+std::string TransportUDP::getTransportInfo()
+{
+  std::stringstream str;
+  str << "UDPROS connection on port " << local_port_ << " to [" << cached_remote_host_ << "]";
+  return str.str();
+}
+
+bool TransportUDP::connect(const std::string& host, int port, int connection_id)
+{
+  if (!isHostAllowed(host))
+    return false; // adios amigo
+
+  sock_ = socket(AF_INET, SOCK_DGRAM, 0);
+  connection_id_ = connection_id;
+
+  if (sock_ == ROS_INVALID_SOCKET)
+  {
+    ROS_ERROR("socket() failed with error [%s]",  last_socket_error_string());
+    return false;
+  }
+
+  sockaddr_in sin;
+  sin.sin_family = AF_INET;
+  if (inet_addr(host.c_str()) == INADDR_NONE)
+  {
+    struct addrinfo* addr;
+    struct addrinfo hints;
+    memset(&hints, 0, sizeof(hints));
+    hints.ai_family = AF_UNSPEC;
+
+    if (getaddrinfo(host.c_str(), NULL, &hints, &addr) != 0)
+    {
+      close();
+      ROS_ERROR("couldn't resolve host [%s]", host.c_str());
+      return false;
+    }
+
+    bool found = false;
+    struct addrinfo* it = addr;
+    for (; it; it = it->ai_next)
+    {
+      if (it->ai_family == AF_INET)
+      {
+        memcpy(&sin, it->ai_addr, it->ai_addrlen);
+        sin.sin_family = it->ai_family;
+        sin.sin_port = htons(port);
+
+        found = true;
+        break;
+      }
+    }
+
+    freeaddrinfo(addr);
+
+    if (!found)
+    {
+      ROS_ERROR("Couldn't find an AF_INET address for [%s]\n", host.c_str());
+      return false;
+    }
+
+    ROSCPP_LOG_DEBUG("Resolved host [%s] to [%s]", host.c_str(), inet_ntoa(sin.sin_addr));
+  }
+  else
+  {
+    sin.sin_addr.s_addr = inet_addr(host.c_str()); // already an IP addr
+  }
+
+  sin.sin_port = htons(port);
+
+  if (::connect(sock_, (sockaddr *)&sin, sizeof(sin)))
+  {
+    ROSCPP_LOG_DEBUG("Connect to udpros host [%s:%d] failed with error [%s]", host.c_str(), port,  last_socket_error_string());
+    close();
+
+    return false;
+  }
+
+  // from daniel stonier:
+#ifdef WIN32
+  // This is hackish, but windows fails at recv() if its slow to connect (e.g. happens with wireless)
+  // recv() needs to check if its connected or not when its asynchronous?
+  Sleep(100);
+#endif
+
+  std::stringstream ss;
+  ss << host << ":" << port << " on socket " << sock_;
+  cached_remote_host_ = ss.str();
+
+  if (!initializeSocket())
+  {
+    return false;
+  }
+
+  ROSCPP_LOG_DEBUG("Connect succeeded to [%s:%d] on socket [%d]", host.c_str(), port, sock_);
+
+  return true;
+}
+
+bool TransportUDP::createIncoming(int port, bool is_server)
+{
+  is_server_ = is_server;
+
+  sock_ = socket(AF_INET, SOCK_DGRAM, 0);
+
+  if (sock_ <= 0)
+  {
+    ROS_ERROR("socket() failed with error [%s]", last_socket_error_string());
+    return false;
+  }
+
+  server_address_.sin_family = AF_INET;
+  server_address_.sin_port = htons(port);
+  server_address_.sin_addr.s_addr = isOnlyLocalhostAllowed() ? 
+                                    htonl(INADDR_LOOPBACK) :
+                                    INADDR_ANY;
+  if (bind(sock_, (sockaddr *)&server_address_, sizeof(server_address_)) < 0)
+  {
+    ROS_ERROR("bind() failed with error [%s]",  last_socket_error_string());
+    return false;
+  }
+
+  socklen_t len = sizeof(server_address_);
+  getsockname(sock_, (sockaddr *)&server_address_, &len);
+  server_port_ = ntohs(server_address_.sin_port);
+  ROSCPP_LOG_DEBUG("UDPROS server listening on port [%d]", server_port_);
+
+  if (!initializeSocket())
+  {
+    return false;
+  }
+
+  enableRead();
+
+  return true;
+}
+
+bool TransportUDP::initializeSocket()
+{
+  ROS_ASSERT(sock_ != ROS_INVALID_SOCKET);
+
+  if (!(flags_ & SYNCHRONOUS))
+  {
+	  int result = set_non_blocking(sock_);
+	  if ( result != 0 ) {
+	      ROS_ERROR("setting socket [%d] as non_blocking failed with error [%d]", sock_, result);
+      close();
+      return false;
+    }
+  }
+
+  socklen_t len = sizeof(local_address_);
+  getsockname(sock_, (sockaddr *)&local_address_, &len);
+  local_port_ = ntohs(local_address_.sin_port);
+
+  ROS_ASSERT(poll_set_ || (flags_ & SYNCHRONOUS));
+  if (poll_set_)
+  {
+    poll_set_->addSocket(sock_, boost::bind(&TransportUDP::socketUpdate, this, _1), shared_from_this());
+  }
+
+  return true;
+}
+
+void TransportUDP::close()
+{
+  Callback disconnect_cb;
+
+  if (!closed_)
+  {
+    {
+      boost::mutex::scoped_lock lock(close_mutex_);
+
+      if (!closed_)
+      {
+        closed_ = true;
+
+        ROSCPP_LOG_DEBUG("UDP socket [%d] closed", sock_);
+
+        ROS_ASSERT(sock_ != ROS_INVALID_SOCKET);
+
+        if (poll_set_)
+        {
+          poll_set_->delSocket(sock_);
+        }
+
+        if ( close_socket(sock_) != 0 )
+        {
+          ROS_ERROR("Error closing socket [%d]: [%s]", sock_, last_socket_error_string());
+        }
+
+        sock_ = ROS_INVALID_SOCKET;
+
+        disconnect_cb = disconnect_cb_;
+
+        disconnect_cb_ = Callback();
+        read_cb_ = Callback();
+        write_cb_ = Callback();
+      }
+    }
+  }
+
+  if (disconnect_cb)
+  {
+    disconnect_cb(shared_from_this());
+  }
+}
+
+int32_t TransportUDP::read(uint8_t* buffer, uint32_t size)
+{
+  {
+    boost::mutex::scoped_lock lock(close_mutex_);
+    if (closed_)
+    {
+      ROSCPP_LOG_DEBUG("Tried to read on a closed socket [%d]", sock_);
+      return -1;
+    }
+  }
+
+  ROS_ASSERT((int32_t)size > 0);
+
+  uint32_t bytes_read = 0;
+
+  while (bytes_read < size)
+  {
+    TransportUDPHeader header;
+
+    // Get the data either from the reorder buffer or the socket
+    // copy_bytes will contain the read size.
+    // from_previous is true if the data belongs to the previous UDP datagram.
+    uint32_t copy_bytes = 0;
+    bool from_previous = false;
+    if (reorder_bytes_)
+    {
+      if (reorder_start_ != reorder_buffer_)
+      {
+        from_previous = true;
+      }
+
+      copy_bytes = std::min(size - bytes_read, reorder_bytes_);
+      header = reorder_header_;
+      memcpy(buffer + bytes_read, reorder_start_, copy_bytes);
+      reorder_bytes_ -= copy_bytes;
+      reorder_start_ += copy_bytes;
+    }
+    else
+    {
+      if (data_filled_ == 0)
+      {
+#if defined(WIN32)
+    	SSIZE_T num_bytes = 0;
+        DWORD received_bytes = 0;
+        DWORD flags = 0;
+        WSABUF iov[2];
+        iov[0].buf = reinterpret_cast<char*>(&header);
+        iov[0].len = sizeof(header);
+        iov[1].buf = reinterpret_cast<char*>(data_buffer_);
+        iov[1].len = max_datagram_size_ - sizeof(header);
+		int rc  = WSARecv(sock_, iov, 2, &received_bytes, &flags, NULL, NULL);
+		if ( rc == SOCKET_ERROR) {
+		  num_bytes = -1;
+		} else {
+			num_bytes = received_bytes;
+		}
+#else
+        ssize_t num_bytes;
+        struct iovec iov[2];
+        iov[0].iov_base = &header;
+        iov[0].iov_len = sizeof(header);
+        iov[1].iov_base = data_buffer_;
+        iov[1].iov_len = max_datagram_size_ - sizeof(header);
+        // Read a datagram with header
+        num_bytes = readv(sock_, iov, 2);
+#endif
+        if (num_bytes < 0)
+        {
+          if ( last_socket_error_is_would_block() )
+          {
+            num_bytes = 0;
+            break;
+          }
+          else
+          {
+            ROSCPP_LOG_DEBUG("readv() failed with error [%s]",  last_socket_error_string());
+            close();
+            break;
+          }
+        }
+        else if (num_bytes == 0)
+        {
+          ROSCPP_LOG_DEBUG("Socket [%d] received 0/%d bytes, closing", sock_, size);
+          close();
+          return -1;
+        }
+        else if (num_bytes < (unsigned) sizeof(header))
+        {
+          ROS_ERROR("Socket [%d] received short header (%d bytes): %s", sock_, int(num_bytes),  last_socket_error_string());
+          close();
+          return -1;
+        }
+
+		num_bytes -= sizeof(header);
+        data_filled_ = num_bytes;
+        data_start_ = data_buffer_;
+      }
+      else
+      {
+        from_previous = true;
+      }
+
+      copy_bytes = std::min(size - bytes_read, data_filled_);
+      // Copy from the data buffer, whether it has data left in it from a previous datagram or
+      // was just filled by readv()
+      memcpy(buffer + bytes_read, data_start_, copy_bytes);
+      data_filled_ -= copy_bytes;
+      data_start_ += copy_bytes;
+    }
+
+
+    if (from_previous)
+    {
+      // We are simply reading data from the last UDP datagram, nothing to
+      // parse
+      bytes_read += copy_bytes;
+    }
+    else
+    {
+      // This datagram is new, process header
+      switch (header.op_)
+      {
+        case ROS_UDP_DATA0:
+          if (current_message_id_)
+          {
+            ROS_DEBUG("Received new message [%d:%d], while still working on [%d] (block %d of %d)", header.message_id_, header.block_, current_message_id_, last_block_ + 1, total_blocks_);
+            reorder_header_ = header;
+
+            // Copy the entire data buffer to the reorder buffer, as we will
+            // need to replay this UDP datagram in the next call.
+            reorder_bytes_ = data_filled_ + (data_start_ - data_buffer_);
+            memcpy(reorder_buffer_, data_buffer_, reorder_bytes_);
+            reorder_start_ = reorder_buffer_;
+            current_message_id_ = 0;
+            total_blocks_ = 0;
+            last_block_ = 0;
+
+            data_filled_ = 0;
+            data_start_ = data_buffer_;
+            return -1;
+          }
+          total_blocks_ = header.block_;
+          last_block_ = 0;
+          current_message_id_ = header.message_id_;
+          break;
+        case ROS_UDP_DATAN:
+          if (header.message_id_ != current_message_id_)
+          {
+            ROS_DEBUG("Message Id mismatch: %d != %d", header.message_id_, current_message_id_);
+            data_filled_ = 0; // discard datagram
+            return 0;
+          }
+          if (header.block_ != last_block_ + 1)
+          {
+            ROS_DEBUG("Expected block %d, received %d", last_block_ + 1, header.block_);
+            data_filled_ = 0; // discard datagram
+            return 0;
+          }
+          last_block_ = header.block_;
+
+          break;
+        default:
+          ROS_ERROR("Unexpected UDP header OP [%d]", header.op_);
+          return -1;
+      }
+
+      bytes_read += copy_bytes;
+
+      if (last_block_ == (total_blocks_ - 1))
+      {
+        current_message_id_ = 0;
+        break;
+      }
+    }
+  }
+
+  return bytes_read;
+}
+
+int32_t TransportUDP::write(uint8_t* buffer, uint32_t size)
+{
+  {
+    boost::mutex::scoped_lock lock(close_mutex_);
+
+    if (closed_)
+    {
+      ROSCPP_LOG_DEBUG("Tried to write on a closed socket [%d]", sock_);
+      return -1;
+    }
+  }
+
+  ROS_ASSERT((int32_t)size > 0);
+
+  const uint32_t max_payload_size = max_datagram_size_ - sizeof(TransportUDPHeader);
+
+  uint32_t bytes_sent = 0;
+  uint32_t this_block = 0;
+  if (++current_message_id_ == 0)
+    ++current_message_id_;
+  while (bytes_sent < size)
+  {
+    TransportUDPHeader header;
+    header.connection_id_ = connection_id_;
+    header.message_id_ = current_message_id_;
+    if (this_block == 0)
+    {
+      header.op_ = ROS_UDP_DATA0;
+      header.block_ = (size + max_payload_size - 1) / max_payload_size;
+    }
+    else
+    {
+      header.op_ = ROS_UDP_DATAN;
+      header.block_ = this_block;
+    }
+    ++this_block;
+#if defined(WIN32)
+    WSABUF iov[2];
+	DWORD sent_bytes;
+	SSIZE_T num_bytes = 0;
+	DWORD flags = 0;
+	int rc;
+	iov[0].buf = reinterpret_cast<char*>(&header);
+	iov[0].len = sizeof(header);
+	iov[1].buf = reinterpret_cast<char*>(buffer + bytes_sent);
+	iov[1].len = std::min(max_payload_size, size - bytes_sent);
+	rc = WSASend(sock_, iov, 2, &sent_bytes, flags, NULL, NULL);
+	num_bytes = sent_bytes;
+	if (rc == SOCKET_ERROR) {
+	    num_bytes = -1;
+	}
+#else
+    struct iovec iov[2];
+    iov[0].iov_base = &header;
+    iov[0].iov_len = sizeof(header);
+    iov[1].iov_base = buffer + bytes_sent;
+    iov[1].iov_len = std::min(max_payload_size, size - bytes_sent);
+    ssize_t num_bytes = writev(sock_, iov, 2);
+#endif
+    //usleep(100);
+    if (num_bytes < 0)
+    {
+      if( !last_socket_error_is_would_block() ) // Actually EAGAIN or EWOULDBLOCK on posix
+      {
+        ROSCPP_LOG_DEBUG("writev() failed with error [%s]", last_socket_error_string());
+        close();
+        break;
+      }
+      else
+      {
+        num_bytes = 0;
+        --this_block;
+      }
+    }
+    else if (num_bytes < (unsigned) sizeof(header))
+    {
+      ROSCPP_LOG_DEBUG("Socket [%d] short write (%d bytes), closing", sock_, int(num_bytes));
+      close();
+      break;
+    }
+    else
+    {
+      num_bytes -= sizeof(header);
+    }
+    bytes_sent += num_bytes;
+  }
+
+  return bytes_sent;
+}
+
+void TransportUDP::enableRead()
+{
+  {
+    boost::mutex::scoped_lock lock(close_mutex_);
+  
+    if (closed_)
+    {
+      return;
+    }
+  }
+
+  if (!expecting_read_)
+  {
+    poll_set_->addEvents(sock_, POLLIN);
+    expecting_read_ = true;
+  }
+}
+
+void TransportUDP::disableRead()
+{
+  ROS_ASSERT(!(flags_ & SYNCHRONOUS));
+
+  {
+    boost::mutex::scoped_lock lock(close_mutex_);
+
+    if (closed_)
+    {
+      return;
+    }
+  }
+
+  if (expecting_read_)
+  {
+    poll_set_->delEvents(sock_, POLLIN);
+    expecting_read_ = false;
+  }
+}
+
+void TransportUDP::enableWrite()
+{
+  {
+    boost::mutex::scoped_lock lock(close_mutex_);
+
+    if (closed_)
+    {
+      return;
+    }
+  }
+
+  if (!expecting_write_)
+  {
+    poll_set_->addEvents(sock_, POLLOUT);
+    expecting_write_ = true;
+  }
+}
+
+void TransportUDP::disableWrite()
+{
+  {
+    boost::mutex::scoped_lock lock(close_mutex_);
+
+    if (closed_)
+    {
+      return;
+    }
+  }
+
+  if (expecting_write_)
+  {
+    poll_set_->delEvents(sock_, POLLOUT);
+    expecting_write_ = false;
+  }
+}
+
+TransportUDPPtr TransportUDP::createOutgoing(std::string host, int port, int connection_id, int max_datagram_size)
+{
+  ROS_ASSERT(is_server_);
+  
+  TransportUDPPtr transport(boost::make_shared<TransportUDP>(poll_set_, flags_, max_datagram_size));
+  if (!transport->connect(host, port, connection_id))
+  {
+    ROS_ERROR("Failed to create outgoing connection");
+    return TransportUDPPtr();
+  }
+  return transport;
+
+}
+
+std::string TransportUDP::getClientURI()
+{
+  ROS_ASSERT(!is_server_);
+
+  sockaddr_storage sas;
+  socklen_t sas_len = sizeof(sas);
+  getpeername(sock_, (sockaddr *)&sas, &sas_len);
+
+  sockaddr_in *sin = (sockaddr_in *)&sas;
+
+  char namebuf[128];
+  int port = ntohs(sin->sin_port);
+  strcpy(namebuf, inet_ntoa(sin->sin_addr));
+
+  std::string ip = namebuf;
+  std::stringstream uri;
+  uri << ip << ":" << port;
+
+  return uri.str();
+}
+
+}

thirdparty/ros/ros_comm/clients/roscpp/src/libros/transport_publisher_link.cpp

@@ -0,0 +1,318 @@
+/*
+ * 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 <ros/platform.h>  // platform dependendant requirements
+
+#include "ros/transport_publisher_link.h"
+#include "ros/subscription.h"
+#include "ros/header.h"
+#include "ros/connection.h"
+#include "ros/transport/transport.h"
+#include "ros/this_node.h"
+#include "ros/connection_manager.h"
+#include "ros/file_log.h"
+#include "ros/poll_manager.h"
+#include "ros/transport/transport_tcp.h"
+#include "ros/timer_manager.h"
+#include "ros/callback_queue.h"
+#include "ros/internal_timer_manager.h"
+
+#include <boost/bind.hpp>
+
+#include <sstream>
+
+namespace ros
+{
+
+TransportPublisherLink::TransportPublisherLink(const SubscriptionPtr& parent, const std::string& xmlrpc_uri, const TransportHints& transport_hints)
+: PublisherLink(parent, xmlrpc_uri, transport_hints)
+, retry_timer_handle_(-1)
+, needs_retry_(false)
+, dropping_(false)
+{
+}
+
+TransportPublisherLink::~TransportPublisherLink()
+{
+  dropping_ = true;
+
+  if (retry_timer_handle_ != -1)
+  {
+    getInternalTimerManager()->remove(retry_timer_handle_);
+  }
+
+  connection_->drop(Connection::Destructing);
+}
+
+bool TransportPublisherLink::initialize(const ConnectionPtr& connection)
+{
+  connection_ = connection;
+  // slot_type is used to automatically track the TransporPublisherLink class' existence
+  // and disconnect when this class' reference count is decremented to 0. It increments
+  // then decrements the shared_from_this reference count around calls to the
+  // onConnectionDropped function, preventing a coredump in the middle of execution.
+  connection_->addDropListener(Connection::DropSignal::slot_type(&TransportPublisherLink::onConnectionDropped, this, _1, _2).track(shared_from_this()));
+
+  if (connection_->getTransport()->requiresHeader())
+  {
+    connection_->setHeaderReceivedCallback(boost::bind(&TransportPublisherLink::onHeaderReceived, this, _1, _2));
+
+    SubscriptionPtr parent = parent_.lock();
+
+    M_string header;
+    header["topic"] = parent->getName();
+    header["md5sum"] = parent->md5sum();
+    header["callerid"] = this_node::getName();
+    header["type"] = parent->datatype();
+    header["tcp_nodelay"] = transport_hints_.getTCPNoDelay() ? "1" : "0";
+    connection_->writeHeader(header, boost::bind(&TransportPublisherLink::onHeaderWritten, this, _1));
+  }
+  else
+  {
+    connection_->read(4, boost::bind(&TransportPublisherLink::onMessageLength, this, _1, _2, _3, _4));
+  }
+
+  return true;
+}
+
+void TransportPublisherLink::drop()
+{
+  dropping_ = true;
+  connection_->drop(Connection::Destructing);
+
+  if (SubscriptionPtr parent = parent_.lock())
+  {
+    parent->removePublisherLink(shared_from_this());
+  }
+}
+
+void TransportPublisherLink::onHeaderWritten(const ConnectionPtr& conn)
+{
+  (void)conn;
+  // Do nothing
+}
+
+bool TransportPublisherLink::onHeaderReceived(const ConnectionPtr& conn, const Header& header)
+{
+  (void)conn;
+  ROS_ASSERT(conn == connection_);
+
+  if (!setHeader(header))
+  {
+    drop();
+    return false;
+  }
+
+  if (retry_timer_handle_ != -1)
+  {
+    getInternalTimerManager()->remove(retry_timer_handle_);
+    retry_timer_handle_ = -1;
+  }
+
+  connection_->read(4, boost::bind(&TransportPublisherLink::onMessageLength, this, _1, _2, _3, _4));
+
+  return true;
+}
+
+void TransportPublisherLink::onMessageLength(const ConnectionPtr& conn, const boost::shared_array<uint8_t>& buffer, uint32_t size, bool success)
+{
+  (void)conn;
+  (void)size;
+  if (retry_timer_handle_ != -1)
+  {
+    getInternalTimerManager()->remove(retry_timer_handle_);
+    retry_timer_handle_ = -1;
+  }
+
+  if (!success)
+  {
+    if (connection_)
+      connection_->read(4, boost::bind(&TransportPublisherLink::onMessageLength, this, _1, _2, _3, _4));
+    return;
+  }
+
+  ROS_ASSERT(conn == connection_);
+  ROS_ASSERT(size == 4);
+
+  uint32_t len = *((uint32_t*)buffer.get());
+
+  if (len > 1000000000)
+  {
+    ROS_ERROR("a message of over a gigabyte was " \
+                "predicted in tcpros. that seems highly " \
+                "unlikely, so I'll assume protocol " \
+                "synchronization is lost.");
+    drop();
+    return;
+  }
+
+  connection_->read(len, boost::bind(&TransportPublisherLink::onMessage, this, _1, _2, _3, _4));
+}
+
+void TransportPublisherLink::onMessage(const ConnectionPtr& conn, const boost::shared_array<uint8_t>& buffer, uint32_t size, bool success)
+{
+  if (!success && !conn)
+    return;
+
+  ROS_ASSERT(conn == connection_);
+
+  if (success)
+  {
+    handleMessage(SerializedMessage(buffer, size), true, false);
+  }
+
+  if (success || !connection_->getTransport()->requiresHeader())
+  {
+    connection_->read(4, boost::bind(&TransportPublisherLink::onMessageLength, this, _1, _2, _3, _4));
+  }
+}
+
+void TransportPublisherLink::onRetryTimer(const ros::SteadyTimerEvent&)
+{
+  if (dropping_)
+  {
+    return;
+  }
+
+  if (needs_retry_ && SteadyTime::now() > next_retry_)
+  {
+    retry_period_ = std::min(retry_period_ * 2, WallDuration(20));
+    needs_retry_ = false;
+    SubscriptionPtr parent = parent_.lock();
+    // TODO: support retry on more than just TCP
+    // For now, since UDP does not have a heartbeat, we do not attempt to retry
+    // UDP connections since an error there likely means some invalid operation has
+    // happened.
+    if (connection_->getTransport()->getType() == std::string("TCPROS"))
+    {
+      std::string topic = parent ? parent->getName() : "unknown";
+
+      TransportTCPPtr old_transport = boost::dynamic_pointer_cast<TransportTCP>(connection_->getTransport());
+      ROS_ASSERT(old_transport);
+      const std::string& host = old_transport->getConnectedHost();
+      int port = old_transport->getConnectedPort();
+
+      ROSCPP_CONN_LOG_DEBUG("Retrying connection to [%s:%d] for topic [%s]", host.c_str(), port, topic.c_str());
+
+      TransportTCPPtr transport(boost::make_shared<TransportTCP>(&PollManager::instance()->getPollSet()));
+      if (transport->connect(host, port))
+      {
+        ConnectionPtr connection(boost::make_shared<Connection>());
+        connection->initialize(transport, false, HeaderReceivedFunc());
+        initialize(connection);
+
+        ConnectionManager::instance()->addConnection(connection);
+      }
+      else
+      {
+        ROSCPP_CONN_LOG_DEBUG("connect() failed when retrying connection to [%s:%d] for topic [%s]", host.c_str(), port, topic.c_str());
+      }
+    }
+    else if (parent)
+    {
+      parent->removePublisherLink(shared_from_this());
+    }
+  }
+}
+
+CallbackQueuePtr getInternalCallbackQueue();
+
+void TransportPublisherLink::onConnectionDropped(const ConnectionPtr& conn, Connection::DropReason reason)
+{
+  (void)conn;
+  if (dropping_)
+  {
+    return;
+  }
+
+  ROS_ASSERT(conn == connection_);
+
+  SubscriptionPtr parent = parent_.lock();
+
+  if (reason == Connection::TransportDisconnect)
+  {
+    std::string topic = parent ? parent->getName() : "unknown";
+
+    ROSCPP_CONN_LOG_DEBUG("Connection to publisher [%s] to topic [%s] dropped", connection_->getTransport()->getTransportInfo().c_str(), topic.c_str());
+
+    ROS_ASSERT(!needs_retry_);
+    needs_retry_ = true;
+    next_retry_ = SteadyTime::now() + retry_period_;
+
+    if (retry_timer_handle_ == -1)
+    {
+      retry_period_ = WallDuration(0.1);
+      next_retry_ = SteadyTime::now() + retry_period_;
+      // shared_from_this() shared_ptr is used to ensure TransportPublisherLink is not
+      // destroyed in the middle of onRetryTimer execution
+      retry_timer_handle_ = getInternalTimerManager()->add(WallDuration(retry_period_),
+          boost::bind(&TransportPublisherLink::onRetryTimer, this, _1), getInternalCallbackQueue().get(),
+          shared_from_this(), false);
+    }
+    else
+    {
+      getInternalTimerManager()->setPeriod(retry_timer_handle_, retry_period_);
+    }
+  }
+  else
+  {
+    drop();
+  }
+}
+
+void TransportPublisherLink::handleMessage(const SerializedMessage& m, bool ser, bool nocopy)
+{
+  stats_.bytes_received_ += m.num_bytes;
+  stats_.messages_received_++;
+
+  SubscriptionPtr parent = parent_.lock();
+
+  if (parent)
+  {
+    stats_.drops_ += parent->handleMessage(m, ser, nocopy, getConnection()->getHeader().getValues(), shared_from_this());
+  }
+}
+
+std::string TransportPublisherLink::getTransportType()
+{
+  return connection_->getTransport()->getType();
+}
+
+std::string TransportPublisherLink::getTransportInfo()
+{
+  return connection_->getTransport()->getTransportInfo();
+}
+
+} // namespace ros
+

thirdparty/ros/ros_comm/clients/roscpp/src/libros/transport_subscriber_link.cpp

@@ -0,0 +1,247 @@
+
+/*
+ * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
+ *
+ * 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 names of Stanford University or 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 "ros/transport_subscriber_link.h"
+#include "ros/publication.h"
+#include "ros/header.h"
+#include "ros/connection.h"
+#include "ros/transport/transport.h"
+#include "ros/this_node.h"
+#include "ros/connection_manager.h"
+#include "ros/topic_manager.h"
+#include "ros/file_log.h"
+
+#include <boost/bind.hpp>
+
+namespace ros
+{
+
+TransportSubscriberLink::TransportSubscriberLink()
+: writing_message_(false)
+, header_written_(false)
+, queue_full_(false)
+{
+
+}
+
+TransportSubscriberLink::~TransportSubscriberLink()
+{
+  drop();
+}
+
+bool TransportSubscriberLink::initialize(const ConnectionPtr& connection)
+{
+  connection_ = connection;
+  dropped_conn_ = connection_->addDropListener(boost::bind(&TransportSubscriberLink::onConnectionDropped, this, _1));
+
+  return true;
+}
+
+bool TransportSubscriberLink::handleHeader(const Header& header)
+{
+  std::string topic;
+  if (!header.getValue("topic", topic))
+  {
+    std::string msg("Header from subscriber did not have the required element: topic");
+
+    ROS_ERROR("%s", msg.c_str());
+    connection_->sendHeaderError(msg);
+
+    return false;
+  }
+
+  // This will get validated by validateHeader below
+  std::string client_callerid;
+  header.getValue("callerid", client_callerid);
+  PublicationPtr pt = TopicManager::instance()->lookupPublication(topic);
+  if (!pt)
+  {
+    std::string msg = std::string("received a connection for a nonexistent topic [") +
+                    topic + std::string("] from [" + connection_->getTransport()->getTransportInfo() + "] [" + client_callerid +"].");
+
+    ROSCPP_LOG_DEBUG("%s", msg.c_str());
+    connection_->sendHeaderError(msg);
+
+    return false;
+  }
+
+  std::string error_msg;
+  if (!pt->validateHeader(header, error_msg))
+  {
+    ROSCPP_LOG_DEBUG("%s", error_msg.c_str());
+    connection_->sendHeaderError(error_msg);
+
+    return false;
+  }
+
+  destination_caller_id_ = client_callerid;
+  connection_id_ = ConnectionManager::instance()->getNewConnectionID();
+  topic_ = pt->getName();
+  parent_ = PublicationWPtr(pt);
+
+  // Send back a success, with info
+  M_string m;
+  m["type"] = pt->getDataType();
+  m["md5sum"] = pt->getMD5Sum();
+  m["message_definition"] = pt->getMessageDefinition();
+  m["callerid"] = this_node::getName();
+  m["latching"] = pt->isLatching() ? "1" : "0";
+  m["topic"] = topic_;
+  connection_->writeHeader(m, boost::bind(&TransportSubscriberLink::onHeaderWritten, this, _1));
+
+  pt->addSubscriberLink(shared_from_this());
+
+  return true;
+}
+
+void TransportSubscriberLink::onConnectionDropped(const ConnectionPtr& conn)
+{
+  (void)conn;
+  ROS_ASSERT(conn == connection_);
+
+  PublicationPtr parent = parent_.lock();
+
+  if (parent)
+  {
+    ROSCPP_CONN_LOG_DEBUG("Connection to subscriber [%s] to topic [%s] dropped", connection_->getRemoteString().c_str(), topic_.c_str());
+
+    parent->removeSubscriberLink(shared_from_this());
+  }
+}
+
+void TransportSubscriberLink::onHeaderWritten(const ConnectionPtr& conn)
+{
+  (void)conn;
+  header_written_ = true;
+  startMessageWrite(true);
+}
+
+void TransportSubscriberLink::onMessageWritten(const ConnectionPtr& conn)
+{
+  (void)conn;
+  writing_message_ = false;
+  startMessageWrite(true);
+}
+
+void TransportSubscriberLink::startMessageWrite(bool immediate_write)
+{
+  boost::shared_array<uint8_t> dummy;
+  SerializedMessage m(dummy, (uint32_t)0);
+
+  {
+    boost::mutex::scoped_lock lock(outbox_mutex_);
+    if (writing_message_ || !header_written_)
+    {
+      return;
+    }
+
+    if (!outbox_.empty())
+    {
+      writing_message_ = true;
+      m = outbox_.front();
+      outbox_.pop();
+    }
+  }
+
+  if (m.num_bytes > 0)
+  {
+    connection_->write(m.buf, m.num_bytes, boost::bind(&TransportSubscriberLink::onMessageWritten, this, _1), immediate_write);
+  }
+}
+
+void TransportSubscriberLink::enqueueMessage(const SerializedMessage& m, bool ser, bool nocopy)
+{
+  (void)nocopy;
+  if (!ser)
+  {
+    return;
+  }
+
+  {
+    boost::mutex::scoped_lock lock(outbox_mutex_);
+
+    int max_queue = 0;
+    if (PublicationPtr parent = parent_.lock())
+    {
+      max_queue = parent->getMaxQueue();
+    }
+
+    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());
+
+    if (max_queue > 0 && (int)outbox_.size() >= max_queue)
+    {
+      if (!queue_full_)
+      {
+        ROS_DEBUG("Outgoing queue full for topic [%s].  "
+               "Discarding oldest message\n",
+               topic_.c_str());
+      }
+
+      outbox_.pop(); // toss out the oldest thing in the queue to make room for us
+      queue_full_ = true;
+    }
+    else
+    {
+      queue_full_ = false;
+    }
+
+    outbox_.push(m);
+  }
+
+  startMessageWrite(false);
+
+  stats_.messages_sent_++;
+  stats_.bytes_sent_ += m.num_bytes;
+  stats_.message_data_sent_ += m.num_bytes;
+}
+
+std::string TransportSubscriberLink::getTransportType()
+{
+  return connection_->getTransport()->getType();
+}
+
+std::string TransportSubscriberLink::getTransportInfo()
+{
+  return connection_->getTransport()->getTransportInfo();
+}
+
+void TransportSubscriberLink::drop()
+{
+  // Only drop the connection if it's not already sending a header error
+  // If it is, it will automatically drop itself
+  if (connection_->isSendingHeaderError())
+  {
+    connection_->removeDropListener(dropped_conn_);
+  }
+  else
+  {
+    connection_->drop(Connection::Destructing);
+  }
+}
+
+} // namespace ros

thirdparty/ros/ros_comm/clients/roscpp/src/libros/wall_timer.cpp

@@ -0,0 +1,145 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Stanford University or 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 "ros/wall_timer.h"
+#include "ros/timer_manager.h"
+
+namespace ros
+{
+
+WallTimer::Impl::Impl()
+  : started_(false)
+  , timer_handle_(-1)
+{ }
+
+WallTimer::Impl::~Impl()
+{
+  ROS_DEBUG("WallTimer deregistering callbacks.");
+  stop();
+}
+
+void WallTimer::Impl::start()
+{
+  if (!started_)
+  {
+    VoidConstPtr tracked_object;
+    if (has_tracked_object_)
+    {
+      tracked_object = tracked_object_.lock();
+    }
+    timer_handle_ = TimerManager<WallTime, WallDuration, WallTimerEvent>::global().add(period_, callback_, callback_queue_, tracked_object, oneshot_);
+    started_ = true;
+  }
+}
+
+void WallTimer::Impl::stop()
+{
+  if (started_)
+  {
+    started_ = false;
+    TimerManager<WallTime, WallDuration, WallTimerEvent>::global().remove(timer_handle_);
+    timer_handle_ = -1;
+  }
+}
+
+bool WallTimer::Impl::isValid()
+{
+  return !period_.isZero();
+}
+
+bool WallTimer::Impl::hasPending()
+{
+  if (!isValid() || timer_handle_ == -1)
+  {
+    return false;
+  }
+
+  return TimerManager<WallTime, WallDuration, WallTimerEvent>::global().hasPending(timer_handle_);
+}
+
+void WallTimer::Impl::setPeriod(const WallDuration& period, bool reset)
+{
+  period_ = period;
+  TimerManager<WallTime, WallDuration, WallTimerEvent>::global().setPeriod(timer_handle_, period, reset);
+}
+
+
+WallTimer::WallTimer(const WallTimerOptions& ops)
+: impl_(new Impl)
+{
+  impl_->period_ = ops.period;
+  impl_->callback_ = ops.callback;
+  impl_->callback_queue_ = ops.callback_queue;
+  impl_->tracked_object_ = ops.tracked_object;
+  impl_->has_tracked_object_ = (ops.tracked_object != NULL);
+  impl_->oneshot_ = ops.oneshot;
+}
+
+WallTimer::WallTimer(const WallTimer& rhs)
+{
+  impl_ = rhs.impl_;
+}
+
+WallTimer::~WallTimer()
+{
+}
+
+void WallTimer::start()
+{
+  if (impl_)
+  {
+    impl_->start();
+  }
+}
+
+void WallTimer::stop()
+{
+  if (impl_)
+  {
+    impl_->stop();
+  }
+}
+
+bool WallTimer::hasPending()
+{
+  if (impl_)
+  {
+    return impl_->hasPending();
+  }
+
+  return false;
+}
+
+void WallTimer::setPeriod(const WallDuration& period, bool reset)
+{
+  if (impl_)
+  {
+    impl_->setPeriod(period, reset);
+  }
+}
+
+}

thirdparty/ros/ros_comm/clients/roscpp/src/libros/xmlrpc_manager.cpp

@@ -0,0 +1,426 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names 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 "ros/xmlrpc_manager.h"
+#include "ros/network.h"
+#include "ros/param.h"
+#include "ros/assert.h"
+#include "ros/common.h"
+#include "ros/file_log.h"
+#include "ros/io.h"
+
+using namespace XmlRpc;
+
+namespace ros
+{
+
+namespace xmlrpc
+{
+XmlRpc::XmlRpcValue responseStr(int code, const std::string& msg, const std::string& response)
+{
+  XmlRpc::XmlRpcValue v;
+  v[0] = code;
+  v[1] = msg;
+  v[2] = response;
+  return v;
+}
+
+XmlRpc::XmlRpcValue responseInt(int code, const std::string& msg, int response)
+{
+  XmlRpc::XmlRpcValue v;
+  v[0] = int(code);
+  v[1] = msg;
+  v[2] = response;
+  return v;
+}
+
+XmlRpc::XmlRpcValue responseBool(int code, const std::string& msg, bool response)
+{
+  XmlRpc::XmlRpcValue v;
+  v[0] = int(code);
+  v[1] = msg;
+  v[2] = XmlRpc::XmlRpcValue(response);
+  return v;
+}
+}
+
+class XMLRPCCallWrapper : public XmlRpcServerMethod
+{
+public:
+  XMLRPCCallWrapper(const std::string& function_name, const XMLRPCFunc& cb, XmlRpcServer *s)
+  : XmlRpcServerMethod(function_name, s)
+  , name_(function_name)
+  , func_(cb)
+  { }
+
+  void execute(XmlRpcValue &params, XmlRpcValue &result)
+  {
+    func_(params, result);
+  }
+
+private:
+  std::string name_;
+  XMLRPCFunc func_;
+};
+
+void getPid(const XmlRpcValue& params, XmlRpcValue& result)
+{
+  (void)params;
+  result = xmlrpc::responseInt(1, "", (int)getpid());
+}
+
+const ros::WallDuration CachedXmlRpcClient::s_zombie_time_(30.0); // reap after 30 seconds
+
+const XMLRPCManagerPtr& XMLRPCManager::instance()
+{
+  static XMLRPCManagerPtr xmlrpc_manager = boost::make_shared<XMLRPCManager>();
+  return xmlrpc_manager;
+}
+
+XMLRPCManager::XMLRPCManager()
+: port_(0)
+, shutting_down_(false)
+, unbind_requested_(false)
+{
+}
+
+XMLRPCManager::~XMLRPCManager()
+{
+  shutdown();
+}
+
+void XMLRPCManager::start()
+{
+  shutting_down_ = false;
+  port_ = 0;
+  bind("getPid", getPid);
+
+  bool bound = server_.bindAndListen(0);
+  (void) bound;
+  ROS_ASSERT(bound);
+  port_ = server_.get_port();
+  ROS_ASSERT(port_ != 0);
+
+  std::stringstream ss;
+  ss << "http://" << network::getHost() << ":" << port_ << "/";
+  uri_ = ss.str();
+
+  server_thread_ = boost::thread(boost::bind(&XMLRPCManager::serverThreadFunc, this));
+}
+
+void XMLRPCManager::shutdown()
+{
+  if (shutting_down_)
+  {
+    return;
+  }
+
+  shutting_down_ = true;
+  server_thread_.join();
+
+  server_.close();
+
+  // kill the last few clients that were started in the shutdown process
+  {
+    boost::mutex::scoped_lock lock(clients_mutex_);
+
+    for (V_CachedXmlRpcClient::iterator i = clients_.begin();
+         i != clients_.end();)
+    {
+      if (!i->in_use_)
+      {
+        i->client_->close();
+        delete i->client_;
+        i = clients_.erase(i);
+      }
+      else
+      {
+        ++i;
+      }
+    }
+  }
+
+  // Wait for the clients that are in use to finish and remove themselves from clients_
+  for (int wait_count = 0; !clients_.empty() && wait_count < 10; wait_count++)
+  {
+    ROSCPP_LOG_DEBUG("waiting for xmlrpc connection to finish...");
+    ros::WallDuration(0.01).sleep();
+  }
+
+  boost::mutex::scoped_lock lock(functions_mutex_);
+  functions_.clear();
+
+  {
+    S_ASyncXMLRPCConnection::iterator it = connections_.begin();
+    S_ASyncXMLRPCConnection::iterator end = connections_.end();
+    for (; it != end; ++it)
+    {
+      (*it)->removeFromDispatch(server_.get_dispatch());
+    }
+  }
+
+  connections_.clear();
+
+  {
+    boost::mutex::scoped_lock lock(added_connections_mutex_);
+    added_connections_.clear();
+  }
+
+  {
+    boost::mutex::scoped_lock lock(removed_connections_mutex_);
+    removed_connections_.clear();
+  }
+}
+
+bool XMLRPCManager::validateXmlrpcResponse(const std::string& method, XmlRpcValue &response,
+                                    XmlRpcValue &payload)
+{
+  if (response.getType() != XmlRpcValue::TypeArray)
+  {
+    ROSCPP_LOG_DEBUG("XML-RPC call [%s] didn't return an array",
+        method.c_str());
+    return false;
+  }
+  if (response.size() != 2 && response.size() != 3)
+  {
+    ROSCPP_LOG_DEBUG("XML-RPC call [%s] didn't return a 2 or 3-element array",
+        method.c_str());
+    return false;
+  }
+  if (response[0].getType() != XmlRpcValue::TypeInt)
+  {
+    ROSCPP_LOG_DEBUG("XML-RPC call [%s] didn't return a int as the 1st element",
+        method.c_str());
+    return false;
+  }
+  int status_code = response[0];
+  if (response[1].getType() != XmlRpcValue::TypeString)
+  {
+    ROSCPP_LOG_DEBUG("XML-RPC call [%s] didn't return a string as the 2nd element",
+        method.c_str());
+    return false;
+  }
+  std::string status_string = response[1];
+  if (status_code != 1)
+  {
+    ROSCPP_LOG_DEBUG("XML-RPC call [%s] returned an error (%d): [%s]",
+        method.c_str(), status_code, status_string.c_str());
+    return false;
+  }
+  if (response.size() > 2)
+  {
+    payload = response[2];
+  }
+  else
+  {
+    std::string empty_array = "<value><array><data></data></array></value>";
+    int offset = 0;
+    payload = XmlRpcValue(empty_array, &offset);
+  }
+  return true;
+}
+
+void XMLRPCManager::serverThreadFunc()
+{
+  disableAllSignalsInThisThread();
+
+  while(!shutting_down_)
+  {
+    {
+      boost::mutex::scoped_lock lock(added_connections_mutex_);
+      S_ASyncXMLRPCConnection::iterator it = added_connections_.begin();
+      S_ASyncXMLRPCConnection::iterator end = added_connections_.end();
+      for (; it != end; ++it)
+      {
+        (*it)->addToDispatch(server_.get_dispatch());
+        connections_.insert(*it);
+      }
+
+      added_connections_.clear();
+    }
+
+    // Update the XMLRPC server, blocking for at most 100ms in select()
+    {
+      boost::mutex::scoped_lock lock(functions_mutex_);
+      server_.work(0.1);
+    }
+
+    while (unbind_requested_)
+    {
+      WallDuration(0.01).sleep();
+    }
+
+    if (shutting_down_)
+    {
+      return;
+    }
+
+    {
+      S_ASyncXMLRPCConnection::iterator it = connections_.begin();
+      S_ASyncXMLRPCConnection::iterator end = connections_.end();
+      for (; it != end; ++it)
+      {
+        if ((*it)->check())
+        {
+          removeASyncConnection(*it);
+        }
+      }
+    }
+
+    {
+      boost::mutex::scoped_lock lock(removed_connections_mutex_);
+      S_ASyncXMLRPCConnection::iterator it = removed_connections_.begin();
+      S_ASyncXMLRPCConnection::iterator end = removed_connections_.end();
+      for (; it != end; ++it)
+      {
+        (*it)->removeFromDispatch(server_.get_dispatch());
+        connections_.erase(*it);
+      }
+
+      removed_connections_.clear();
+    }
+  }
+}
+
+XmlRpcClient* XMLRPCManager::getXMLRPCClient(const std::string &host, const int port, const std::string &uri)
+{
+  // go through our vector of clients and grab the first available one
+  XmlRpcClient *c = NULL;
+
+  boost::mutex::scoped_lock lock(clients_mutex_);
+
+  for (V_CachedXmlRpcClient::iterator i = clients_.begin();
+       !c && i != clients_.end(); )
+  {
+    if (!i->in_use_)
+    {
+      // see where it's pointing
+      if (i->client_->getHost() == host &&
+          i->client_->getPort() == port &&
+          i->client_->getUri()  == uri)
+      {
+        // hooray, it's pointing at our destination. re-use it.
+        c = i->client_;
+        i->in_use_ = true;
+        i->last_use_time_ = SteadyTime::now();
+        break;
+      }
+      else if (i->last_use_time_ + CachedXmlRpcClient::s_zombie_time_ < SteadyTime::now())
+      {
+        // toast this guy. he's dead and nobody is reusing him.
+        delete i->client_;
+        i = clients_.erase(i);
+      }
+      else
+      {
+        ++i; // move along. this guy isn't dead yet.
+      }
+    }
+    else
+    {
+      ++i;
+    }
+  }
+
+  if (!c)
+  {
+    // allocate a new one
+    c = new XmlRpcClient(host.c_str(), port, uri.c_str());
+    CachedXmlRpcClient mc(c);
+    mc.in_use_ = true;
+    mc.last_use_time_ = SteadyTime::now();
+    clients_.push_back(mc);
+    //ROS_INFO("%d xmlrpc clients allocated\n", xmlrpc_clients.size());
+  }
+  // ONUS IS ON THE RECEIVER TO UNSET THE IN_USE FLAG
+  // by calling releaseXMLRPCClient
+  return c;
+}
+
+void XMLRPCManager::releaseXMLRPCClient(XmlRpcClient *c)
+{
+  boost::mutex::scoped_lock lock(clients_mutex_);
+
+  for (V_CachedXmlRpcClient::iterator i = clients_.begin();
+       i != clients_.end(); ++i)
+  {
+    if (c == i->client_)
+    {
+      if (shutting_down_)
+      {
+        // if we are shutting down we won't be re-using the client
+        i->client_->close();
+        delete i->client_;
+        clients_.erase(i);
+      }
+      else
+      {
+        i->in_use_ = false;
+      }
+      break;
+    }
+  }
+}
+
+void XMLRPCManager::addASyncConnection(const ASyncXMLRPCConnectionPtr& conn)
+{
+  boost::mutex::scoped_lock lock(added_connections_mutex_);
+  added_connections_.insert(conn);
+}
+
+void XMLRPCManager::removeASyncConnection(const ASyncXMLRPCConnectionPtr& conn)
+{
+  boost::mutex::scoped_lock lock(removed_connections_mutex_);
+  removed_connections_.insert(conn);
+}
+
+bool XMLRPCManager::bind(const std::string& function_name, const XMLRPCFunc& cb)
+{
+  boost::mutex::scoped_lock lock(functions_mutex_);
+  if (functions_.find(function_name) != functions_.end())
+  {
+    return false;
+  }
+
+  FunctionInfo info;
+  info.name = function_name;
+  info.function = cb;
+  info.wrapper.reset(new XMLRPCCallWrapper(function_name, cb, &server_));
+  functions_[function_name] = info;
+
+  return true;
+}
+
+void XMLRPCManager::unbind(const std::string& function_name)
+{
+  unbind_requested_ = true;
+  boost::mutex::scoped_lock lock(functions_mutex_);
+  functions_.erase(function_name);
+  unbind_requested_ = false;
+}
+
+} // namespace ros