client.h

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.
 */

#ifndef RVIZ_RPC_CLIENT_H
#define RVIZ_RPC_CLIENT_H


#include "exceptions.h"
#include "request_wrapper.h"
#include "response_wrapper.h"
#include "method_response.h"
#include <rve_common/uuid.h>

#include <string>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/function.hpp>
#include <boost/bind.hpp>

namespace ros
{
class NodeHandle;
}

namespace rve_rpc
{

template<typename Req, typename Res>
class Method
{
private:
  struct SyncCallHelper
  {
    SyncCallHelper()
    : received(false)
    {}

    void cb(const MethodResponse<Res>& res)
    {
      response = res;
      received = true;
    }

    bool received;
    MethodResponse<Res> response;
  };

public:
  typedef boost::shared_ptr<Req> ReqPtr;
  typedef boost::shared_ptr<Req const> ReqConstPtr;
  typedef boost::shared_ptr<Res> ResPtr;
  typedef boost::shared_ptr<Res const> ResConstPtr;

  typedef boost::function<void(const RequestWrapperPtr&, const boost::function<void(const ResponseWrapperConstPtr&)>)> CallFn;
  typedef boost::function<void(const MethodResponse<Res>&)> ResponseCallback;
  typedef boost::function<void()> PumpFn;

  Method() {}

  Method(const std::string& name, const CallFn& c, const PumpFn& pump)
  {
    name_ = name;
    call_fn_ = c;
    pump_fn_ = pump;
  }

  void callAsync(const ReqConstPtr& req, const ResponseCallback& cb)
  {
    ROS_ASSERT(!name_.empty());

    RequestWrapperPtr request(boost::make_shared<RequestWrapper>());
    request->method = name_;
    request->message.message = req;
    request->message.serialize = serialize<Req>;
    request->message.serialized_length = serializedLength<Req>;
    request->message.ti = &typeid(Req);

    call_fn_(request, boost::bind(&Method::responseCallback, this, _1, cb));
  }

  void callAsync(const ReqConstPtr& req)
  {
    callAsync(req, ResponseCallback());
  }

  ResConstPtr call(const ReqConstPtr& req)
  {
    SyncCallHelper h;
    callAsync(req, boost::bind(&SyncCallHelper::cb, &h, _1));

    while (!h.received)
    {
      pump_fn_();
    }

    uint32_t error_code = h.response.error_code;
    if (error_code == Response::SUCCESS)
    {
      return h.response.response;
    }
    else if (error_code == Response::EXCEPTION)
    {
      throw CallException(h.response.error_string);
    }
    else if (error_code == Response::UNKNOWN_METHOD)
    {
      std::stringstream ss;
      ss << "Server does not have method [" << h.response.error_string << "]";
      throw CallException(ss.str());
    }
    else
    {
      std::stringstream ss;
      ss << "Unknown error code [" << error_code << "]";
      throw CallException(ss.str());
    }
  }

private:
  void responseCallback(const ResponseWrapperConstPtr& res, const ResponseCallback& cb)
  {
    if (cb)
    {
      MethodResponse<Res> mr;
      if (res->error_code == 0)
      {
        mr.response = res->instantiate<Res>();
      }
      mr.error_code = res->error_code;
      mr.error_string = res->error_string;
      cb(mr);
    }
  }

  CallFn call_fn_;
  PumpFn pump_fn_;
  std::string name_;
};

class Client
{
private:
  struct MethodInfo
  {
    std::string name;
    std::string request_md5sum;
    std::string request_datatype;
    std::string request_definition;
    std::string response_md5sum;
    std::string response_datatype;
    std::string response_definition;
  };

public:
  Client(const std::string& name, const ros::NodeHandle& nh);
  void connect();
  void connectAsync();
  void disconnect();
  void waitForConnection();
  bool isConnected();
  void flush();

  template<typename Req, typename Res>
  Method<Req, Res> addMethod(const std::string& name)
  {
    Method<Req, Res> method(name, boost::bind(&Client::call, this, _1, _2), boost::bind(&Client::pump, this));
    MethodInfo info;
    info.name = name;
    info.request_md5sum = ros::message_traits::md5sum<Req>();
    info.request_datatype = ros::message_traits::datatype<Req>();
    info.request_definition = ros::message_traits::definition<Req>();
    info.response_md5sum = ros::message_traits::md5sum<Res>();
    info.response_datatype = ros::message_traits::datatype<Res>();
    info.response_definition = ros::message_traits::definition<Res>();
    addMethod(info);
    return method;
  }

private:
  void addMethod(const MethodInfo& name);
  void call(const RequestWrapperPtr& req, const boost::function<void(const ResponseWrapperConstPtr&)>& response_cb);
  void pump();

  struct Impl;
  typedef boost::shared_ptr<Impl> ImplPtr;
  ImplPtr impl_;
};
typedef boost::shared_ptr<Client> ClientPtr;

} // namespace rve_rpc

#endif // RVIZ_RPC_CLIENT_H