client.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2010, 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 the 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 <rve_rpc/client.h>

#include <ros/ros.h>
#include <ros/callback_queue.h>

#include <rve_rpc/Request.h>
#include <rve_rpc/Response.h>
#include <rve_rpc/ProtocolMethodsAdded.h>
#include <rve_rpc/ProtocolClientConnect.h>

#include <boost/unordered_set.hpp>

#define REQUEST_DEBUG 1

namespace rve_rpc
{

struct Client::Impl
{
  Impl(const std::string& name, const ros::NodeHandle& nh);
  ~Impl();

  void connect();
  void connectAsync();
  bool isConnected() { return connected_; }
  void disconnect();
  void call(const RequestWrapperPtr& req, const boost::function<void(const ResponseWrapperConstPtr&)>& response_cb);
  void pump();
  void flush();
  void cb(const ros::MessageEvent<ResponseWrapper const>& res);
  void addMethod(const MethodInfo& method);
  void handleMethodsAdded(const ros::MessageEvent<ResponseWrapper const>& evt);

  void subAndPub();
  void waitForConnection();
  void sendConnected();

  ros::CallbackQueue cbqueue_;
  std::vector<ros::Subscriber> subs_;
  ros::Publisher pub_;
  ros::NodeHandle nh_;

  struct RequestInfo
  {
    RequestInfo()
    {}

    rve_common::UUID id;
    boost::function<void(const ResponseWrapperConstPtr&)> response_cb;

#if REQUEST_DEBUG
    std::string method;
#endif
  };

  typedef std::map<rve_common::UUID, RequestInfo> M_RequestInfo;
  M_RequestInfo requests_;
  boost::mutex requests_mutex_;

  typedef std::map<std::string, MethodInfo> M_MethodInfo;
  M_MethodInfo methods_;

  typedef std::set<std::string> S_string;
  S_string broken_methods_;

  bool connected_;

  void onQueueTimer(const ros::WallTimerEvent&);
  ros::WallTimer queue_timer_;
};

Client::Impl::Impl(const std::string& name, const ros::NodeHandle& nh)
: nh_(nh, name)
, connected_(false)
{
}

Client::Impl::~Impl()
{
  // Ensure that by the time we hit the automatic destructor we're not getting any callbacks
  pub_.shutdown();
  subs_.clear();
  queue_timer_.stop();
}

void Client::Impl::subAndPub()
{
  ROS_ASSERT(!pub_);
  ROS_ASSERT(subs_.empty());
  ROS_ASSERT(!queue_timer_);

  pub_ = nh_.advertise<RequestWrapper>("request", 0);

  // We subscribe to "response" twice, once with our internal callback queue, and once with the queue used by
  // the NodeHandle we were initialized with.  This is so that we're not limited by the 50ms update rate of the
  // internal queue for receiving responses -- however fast the external queue is being serviced, we'll be
  // serviced that quickly as well
  //
  // This creates a little bit of extra work when we receive a response, since technically we'll receive response twice,
  // but it's a relatively small amount of work.  If it turns out to be a problem, another solution is to spin a separate
  // thread that's servicing the queue
  ros::SubscribeOptions sub_ops;
  sub_ops.initByFullCallbackType<const ros::MessageEvent<ResponseWrapper>&>("response", 0, boost::bind(&Impl::cb, this, _1));
  sub_ops.callback_queue = &cbqueue_;
  subs_.push_back(nh_.subscribe(sub_ops));
  subs_.push_back(nh_.subscribe("response", 0, &Impl::cb, this));

  queue_timer_ = nh_.createWallTimer(ros::WallDuration(0.05), &Impl::onQueueTimer, this);
}

void Client::Impl::connect()
{
  subAndPub();
  waitForConnection();
}

void Client::Impl::disconnect()
{
  connected_ = false;
  subs_.clear();
  pub_.shutdown();
  queue_timer_ = ros::WallTimer();
}

void Client::Impl::waitForConnection()
{
  // TODO this may break if there are multiple servers (which is a broken situation anyway).
  // we assume only one server
  while (!pub_.getNumSubscribers() || !subs_[0].getNumPublishers())
  {
    cbqueue_.callAvailable(ros::WallDuration(0.1));
  }

  sendConnected();

  while (nh_.ok() && !connected_)
  {
    cbqueue_.callAvailable(ros::WallDuration(0.1));
  }
}

void Client::Impl::onQueueTimer(const ros::WallTimerEvent&)
{
  if (!connected_)
  {
    if (pub_.getNumSubscribers() && subs_[0].getNumPublishers())
    {
      sendConnected();
    }
  }

  cbqueue_.callAvailable();
}

void Client::Impl::connectAsync()
{
  subAndPub();
}

void Client::Impl::sendConnected()
{
  std::pair<RequestWrapperPtr, boost::shared_ptr<ProtocolClientConnect> > pair = makeRequest<ProtocolClientConnect>();
  pair.first->request_id = rve_common::UUID::generate();
  pair.first->protocol = Request::PROTOCOL_CLIENT_CONNECT;
  pub_.publish(pair.first);
}

void Client::Impl::handleMethodsAdded(const ros::MessageEvent<ResponseWrapper const>& evt)
{
  ResponseWrapperConstPtr res = evt.getMessage();

  try
  {
    ProtocolMethodsAddedConstPtr header = res->instantiate<ProtocolMethodsAdded>();
    for (size_t i = 0; i < header->methods.size(); ++i)
    {
      const MethodSpec& spec = header->methods[i];

      // Already been notified about this one, and we know it's broken, don't warn about it again
      if (broken_methods_.find(spec.name) != broken_methods_.end())
      {
        continue;
      }

      M_MethodInfo::iterator it = methods_.find(spec.name);
      if (it != methods_.end())
      {
        MethodInfo& info = it->second;
        if (info.request_md5sum != spec.request_md5sum)
        {
          ROS_ERROR("RPC Server wants method [%s:%s] to have request of type [%s/%s] but I have [%s/%s]", nh_.getNamespace().c_str(), spec.name.c_str(),
              spec.request_datatype.c_str(), spec.request_md5sum.c_str(), info.request_datatype.c_str(), info.request_md5sum.c_str());

          broken_methods_.insert(info.name);
        }
        else if (info.response_md5sum != spec.response_md5sum)
        {
          ROS_ERROR("RPC Server wants method [%s:%s] to have response of type [%s/%s] but I have [%s/%s]", nh_.getNamespace().c_str(), spec.name.c_str(),
              spec.response_datatype.c_str(), spec.response_md5sum.c_str(), info.response_datatype.c_str(), info.response_md5sum.c_str());

          broken_methods_.insert(info.name);
        }
      }
      else
      {
        ROS_DEBUG("RPC Server for [%s] at [%s] has extra method [%s]", nh_.getNamespace().c_str(), evt.getPublisherName().c_str(), spec.name.c_str());
      }
    }
  }
  catch (std::exception& e)
  {
    ROS_ERROR("Exception thrown while processing header from rpc server for [%s] at [%s]", nh_.getNamespace().c_str(), evt.getPublisherName().c_str());
  }

  connected_ = true;
}

void Client::Impl::cb(const ros::MessageEvent<ResponseWrapper const>& evt)
{
  ResponseWrapperConstPtr res = evt.getMessage();
  if (res->protocol != 0)
  {
    switch (res->protocol)
    {
    case Response::PROTOCOL_METHODS_ADDED:
      handleMethodsAdded(evt);
      break;
    default:
      ROS_ERROR("Unknown protocol message type [%d]", res->protocol);
    }
  }
  else
  {
    rve_common::UUID id = res->request_id;

    {
      boost::mutex::scoped_lock lock(requests_mutex_);
      M_RequestInfo::iterator it = requests_.find(id);
      if (it != requests_.end())
      {
        RequestInfo& ri = it->second;
        ri.response_cb(res);
        requests_.erase(it);
      }
    }
  }
}

void Client::Impl::addMethod(const MethodInfo& method)
{
  ROS_ASSERT(methods_.count(method.name) == 0);

  methods_[method.name] = method;
}

void Client::Impl::pump()
{
  cbqueue_.callOne(ros::WallDuration(0.1));
}

void Client::Impl::call(const RequestWrapperPtr& req, const boost::function<void(const ResponseWrapperConstPtr&)>& response_cb)
{
  ROS_ASSERT(req);
  ROS_ASSERT(response_cb);

  if (!connected_)
  {
    throw CallException("Client to [" + nh_.getNamespace() + "] is not connected");
  }

  if (broken_methods_.find(req->method) != broken_methods_.end())
  {
    throw CallException("Unconnected method [" + req->method + "], see previously generated errors for more details");
  }

  req->request_id = rve_common::UUID::generate();

  RequestInfo info;
  info.id = req->request_id;
  info.response_cb = response_cb;
#if REQUEST_DEBUG
  info.method = req->method;
#endif

  {
    boost::mutex::scoped_lock lock(requests_mutex_);
    requests_[info.id] = info;
  }

  pub_.publish(req);
}

void Client::Impl::flush()
{
  // Copy the ids of all the requests.  Use that to determine when we're done
  std::vector<rve_common::UUID> uuids;
  {
    boost::mutex::scoped_lock lock(requests_mutex_);
    M_RequestInfo::const_iterator it = requests_.begin();
    M_RequestInfo::const_iterator end = requests_.end();
    for (; it != end; ++it)
    {
      uuids.push_back(it->first);
    }
  }

  while (true)
  {
    if (uuids.empty())
    {
      return;
    }

    ros::WallDuration(0.001).sleep();

    pump();

    bool erased = true;
    while (erased && !uuids.empty())
    {
      erased = false;

      boost::mutex::scoped_lock lock(requests_mutex_);
      rve_common::UUID& id = uuids.back();
      M_RequestInfo::iterator it = requests_.find(id);
      if (it == requests_.end())
      {
        uuids.pop_back();
        erased = true;
      }
      else
      {
#if REQUEST_DEBUG
        ROS_INFO("Waiting for request on method '%s'", it->second.method.c_str());
#endif
      }
    }
  }
}


Client::Client(const std::string& name, const ros::NodeHandle& nh)
: impl_(new Impl(name, nh))
{

}

void Client::connect()
{
  impl_->connect();
}

void Client::connectAsync()
{
  impl_->connectAsync();
}

bool Client::isConnected()
{
  return impl_->isConnected();
}

void Client::waitForConnection()
{
  impl_->waitForConnection();
}

void Client::call(const RequestWrapperPtr& req, const boost::function<void(const ResponseWrapperConstPtr&)>& response_cb)
{
  impl_->call(req, response_cb);
}

void Client::addMethod(const MethodInfo& method)
{
  impl_->addMethod(method);
}

void Client::pump()
{
  impl_->pump();
}

void Client::disconnect()
{
  impl_->disconnect();
}

void Client::flush()
{
  impl_->flush();
}

} // namespace rve_rpc