server.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/server.h>

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

#include <rve_rpc/ProtocolMethodsAdded.h>

namespace rve_rpc
{

struct Server::Impl
{
  Impl(const std::string& name, const ros::NodeHandle& nh);
  ~Impl();
  void ready();
  void addMethod(const std::string& name, const CallbackHelperPtr& helper);

  void callback(const ros::MessageEvent<RequestWrapper const>& evt);

  void subscriberConnected(const ros::SingleSubscriberPublisher& pub);
  ResponseWrapperPtr createClientConnectedResponse();

  void handleProtocolMessage(const RequestWrapperConstPtr& req);
  void handleClientConnected(const RequestWrapperConstPtr& req);

  void respond(const ResponseWrapperConstPtr& res);

  ros::NodeHandle nh_;
  ros::Publisher pub_;
  ros::Subscriber sub_;
  std::string name_;
  bool ready_;

  typedef std::map<std::string, CallbackHelperPtr> M_Method;
  M_Method methods_;
  boost::mutex methods_mutex_;

  bool shutting_down_;
  boost::mutex shutting_down_mutex_;
};

Server::Impl::Impl(const std::string& name, const ros::NodeHandle& nh)
: nh_(nh, name)
, name_(name)
, ready_(false)
, shutting_down_(false)
{
}

Server::Impl::~Impl()
{
  {
    boost::mutex::scoped_lock lock(shutting_down_mutex_);
    shutting_down_ = true;
  }
  // Make sure we don't get any callbacks while destructing
  pub_.shutdown();
  sub_.shutdown();
}

void fillMethodSpec(const std::string& name, const CallbackHelperPtr& helper, MethodSpec& spec)
{
  spec.name = name;
  spec.request_md5sum = helper->getRequestMD5Sum();
  spec.request_datatype = helper->getRequestDataType();
  spec.request_definition = helper->getRequestDefinition();
  spec.response_md5sum = helper->getResponseMD5Sum();
  spec.response_datatype = helper->getResponseDataType();
  spec.response_definition = helper->getResponseDefinition();
}

ResponseWrapperPtr Server::Impl::createClientConnectedResponse()
{
  std::pair<ResponseWrapperPtr, boost::shared_ptr<ProtocolMethodsAdded> > pair = makeResponse<ProtocolMethodsAdded>();
  ResponseWrapperPtr& res = pair.first;
  ProtocolMethodsAddedPtr& header = pair.second;

  res->request_id = rve_common::UUID::generate();
  res->protocol = Response::PROTOCOL_METHODS_ADDED;

  M_Method::const_iterator it = methods_.begin();
  M_Method::const_iterator end = methods_.end();
  for (; it != end; ++it)
  {
    const std::string& name = it->first;
    const CallbackHelperPtr& helper = it->second;
    MethodSpec spec;
    fillMethodSpec(name, helper, spec);
    header->methods.push_back(spec);
  }

  return res;
}

void Server::Impl::subscriberConnected(const ros::SingleSubscriberPublisher& pub)
{
  pub.publish(createClientConnectedResponse());
}

void Server::Impl::handleClientConnected(const RequestWrapperConstPtr& req)
{
  pub_.publish(createClientConnectedResponse());
}

void Server::Impl::handleProtocolMessage(const RequestWrapperConstPtr& req)
{
  switch (req->protocol)
  {
    case Request::PROTOCOL_CLIENT_CONNECT:
      handleClientConnected(req);
      break;
    default:
      ROS_ERROR("Unknown protocol type %d", req->protocol);
  }
}

void Server::Impl::ready()
{
  ROS_ASSERT(!pub_);
  ROS_ASSERT(!sub_);
  pub_ = nh_.advertise<ResponseWrapper>("response", 0, boost::bind(&Impl::subscriberConnected, this, _1));
  sub_ = nh_.subscribe("request", 0, &Impl::callback, this);
  ready_ = true;
}

void Server::Impl::addMethod(const std::string& name, const CallbackHelperPtr& helper)
{
  {
    boost::mutex::scoped_lock lock(methods_mutex_);
    ROS_ASSERT(methods_.count(name) == 0);
    methods_[name] = helper;
  }

  if (ready_)
  {
    MethodSpec spec;
    fillMethodSpec(name, helper, spec);

    std::pair<ResponseWrapperPtr, boost::shared_ptr<ProtocolMethodsAdded> > pair = makeResponse<ProtocolMethodsAdded>();
    ResponseWrapperPtr& res = pair.first;
    res->request_id = rve_common::UUID::generate();
    res->protocol = Response::PROTOCOL_METHODS_ADDED;

    ProtocolMethodsAddedPtr& methods_added = pair.second;
    methods_added->methods.push_back(spec);
    pub_.publish(res);
  }
}

void Server::Impl::callback(const ros::MessageEvent<RequestWrapper const>& evt)
{
  RequestWrapperConstPtr req = evt.getMessage();
  if (req->protocol > 0)
  {
    handleProtocolMessage(req);
  }
  else
  {
    CallbackHelperPtr helper;

    {
      boost::mutex::scoped_lock lock(methods_mutex_);
      M_Method::iterator it = methods_.find(req->method);
      if (it == methods_.end())
      {
        ResponseWrapperPtr res(new ResponseWrapper);
        res->request_id = req->request_id;
        res->error_code = Response::UNKNOWN_METHOD;
        res->error_string = req->method;
        pub_.publish(res);
        return;
      }

      helper = it->second;
    }


    helper->call(evt, boost::bind(&Server::Impl::respond, this, _1));
  }
}

void Server::Impl::respond(const ResponseWrapperConstPtr& res)
{
  boost::mutex::scoped_lock lock(shutting_down_mutex_);
  if (shutting_down_)
  {
    return;
  }

  pub_.publish(res);
}

// Interface

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

}

void Server::ready()
{
  impl_->ready();
}

void Server::addMethod(const std::string& name, const CallbackHelperPtr& helper)
{
  impl_->addMethod(name, helper);
}

} // namespace rve_rpc