managed_serviceserver.h

Go to the documentation of this file.

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2018, University of Luxembourg
 *  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 University of Luxembourg 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.
 *
 * Author: Maciej Zurad
 *********************************************************************/
#ifndef ROBOT_ACTIVITY_RESOURCE_MANAGED_SERVICESERVER_H
#define ROBOT_ACTIVITY_RESOURCE_MANAGED_SERVICESERVER_H

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

#include <robot_activity/resource/managed_resource.h>

#include <string>

namespace robot_activity
{
namespace resource
{

class ManagedServiceServer : public Managed<ManagedServiceServer, ros::ServiceServer>
{
public:
  using Managed<ManagedServiceServer, ros::ServiceServer>::Managed;

  void advertiseService(const ros::NodeHandlePtr& node_handle)
  {
    acquire(node_handle);
  }

  void shutdown()
  {
    release();
  }

  template <typename ...Args>
  using ServiceCallback = boost::function<bool(Args...)>;

  template<class MReq, class MRes>
  LazyAcquirer makeLazyAcquirer(
    const std::string& service,
    const ServiceCallback<MReq&, MRes&>& callback,
    const ros::VoidConstPtr& tracked_object = ros::VoidConstPtr()) const
  {
    ROS_DEBUG("makeLazyAcquirer ServiceCallback<MReq&, MRes&>&");
    return [ = ](const ros::NodeHandlePtr & nh) -> ros::ServiceServer
    {
      ROS_DEBUG("Advertising...");
      return nh->advertiseService(
        service,
        static_cast<ServiceCallback<MReq&, MRes&>>(wrapServiceCallback(callback)),
        tracked_object);
    };
  }

  template<class ServiceEvent>
  LazyAcquirer makeLazyAcquirer(
    const std::string& service,
    const ServiceCallback<ServiceEvent&>& callback,
    const ros::VoidConstPtr& tracked_object = ros::VoidConstPtr()) const
  {
    ROS_DEBUG("makeLazyAcquirer ServiceEventCallback<ServiceEvent&>&");
    return [ = ](const ros::NodeHandlePtr & nh) -> ros::ServiceServer
    {
      ROS_DEBUG("Advertising...");
      return nh->advertiseService(
        service,
        static_cast<ServiceCallback<ServiceEvent&>>(wrapServiceCallback(callback)),
        tracked_object);
    };
  }

  template<class T, class MReq, class MRes>
  LazyAcquirer makeLazyAcquirer(
    const std::string& service,
    bool(T::*srv_func)(MReq&, MRes&),
    T *obj) const
  {
    ServiceCallback<MReq&, MRes&> callback = boost::bind(srv_func, obj, _1, _2);
    return makeLazyAcquirer(service, callback);
  }

  template<class T, class MReq, class MRes>
  LazyAcquirer makeLazyAcquirer(
    const std::string& service,
    bool(T::*srv_func)(ros::ServiceEvent<MReq, MRes>&),
    T *obj) const
  {
    ServiceCallback<ros::ServiceEvent<MReq, MRes>&> callback
      = boost::bind(srv_func, obj, _1);
    return makeLazyAcquirer(service, callback);
  }

  template<class T, class MReq, class MRes>
  LazyAcquirer makeLazyAcquirer(
    const std::string& service,
    bool(T::*srv_func)(MReq &, MRes &),
    const boost::shared_ptr<T>& obj) const
  {
    ServiceCallback<MReq&, MRes&> callback = boost::bind(srv_func, obj.get(), _1, _2);
    return makeLazyAcquirer(service, callback, obj);
  }

  template<class T, class MReq, class MRes>
  LazyAcquirer makeLazyAcquirer(
    const std::string& service,
    bool(T::*srv_func)(ros::ServiceEvent<MReq, MRes>&),
    const boost::shared_ptr<T>& obj) const
  {
    ServiceCallback<ros::ServiceEvent<MReq, MRes>&> callback
      = boost::bind(srv_func, obj.get(), _1);
    return makeLazyAcquirer(service, callback, obj);
  }

  template<class MReq, class MRes>
  LazyAcquirer makeLazyAcquirer(
    const std::string& service,
    bool(*srv_func)(MReq&, MRes&)) const
  {
    ServiceCallback<MReq&, MRes&> callback = boost::bind(srv_func);
    return makeLazyAcquirer(service, callback);
  }

  template<class MReq, class MRes>
  LazyAcquirer makeLazyAcquirer(
    const std::string& service,
    bool(*srv_func)(ros::ServiceEvent<MReq, MRes>&)) const
  {
    ServiceCallback<ros::ServiceEvent<MReq, MRes>&> callback
      = boost::bind(srv_func);
    return makeLazyAcquirer(service, callback);
  }


  /* TODO - remove if unneccessary
  template<class MReq, class MRes>
  LazyAcquirer makeLazyAcquirer(
    const std::string& service,
    const boost::function<bool(MReq&, MRes&)>& callback,
    const ros::VoidConstPtr& tracked_object = ros::VoidConstPtr())
  {
    AdvertiseServiceOptions ops;
    ops.template init<MReq, MRes>(service, callback);
    ops.tracked_object = tracked_object;
    return makeLazyAcquirer(ops);
  }

  template<class S>
  LazyAcquirer makeLazyAcquirer(
    const std::string& service,
    const boost::function<bool(S&)>& callback,
    const ros::VoidConstPtr& tracked_object = ros::VoidConstPtr())
  {
    AdvertiseServiceOptions ops;
    ops.template initBySpecType<S>(service, callback);
    ops.tracked_object = tracked_object;
    return makeLazyAcquirer(ops);
  }
  */
private:
  template <typename ...Args>
  ServiceCallback<Args...> wrapServiceCallback(
    const ServiceCallback<Args...>& callback) const
  {
    return [this, &callback](Args ... args) -> bool
    {
      if (paused_)
      {
        ROS_DEBUG("service is paused!");
        return false;
      }
      return callback(std::forward<Args>(args)...);
    };
  }
};

}  // namespace resource
}  // namespace robot_activity

#endif  // ROBOT_ACTIVITY_RESOURCE_MANAGED_SERVICESERVER_H