service_action_wrappers.h

Go to the documentation of this file.

/*********************************************************************
*
*  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 the Willow Garage 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 _SERVICE_ACTION_WRAPPERS_H_
#define _SERVICE_ACTION_WRAPPERS_H_

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>

#include <string>
#include <map>

#include "object_manipulator/tools/exceptions.h"

namespace object_manipulator {


template <class ServiceDataType>
class ServiceWrapper
{
 private:
  bool initialized_;
  std::string service_name_;
  ros::NodeHandle nh_;
  ros::ServiceClient client_;
 public:
 ServiceWrapper(std::string service_name) : initialized_(false),
    service_name_(service_name),
    nh_("")
    {}

  ros::ServiceClient& client(ros::Duration timeout = ros::Duration(5.0))
  {
    if (!initialized_)
    {
      ros::Duration ping_time = ros::Duration(1.0);
      if (timeout >= ros::Duration(0) && ping_time > timeout) ping_time = timeout;
      ros::Time start_time = ros::Time::now();
      while (1)
      {
        if (ros::service::waitForService(service_name_, ping_time)) break;
        ROS_INFO_STREAM("Waiting for service " << service_name_);
        if (!ros::ok()) throw ServiceNotFoundException(service_name_);
        ros::Time current_time = ros::Time::now();
        if (timeout >= ros::Duration(0) && current_time - start_time >= timeout)
          throw ServiceNotFoundException(service_name_);
      }
      client_ = nh_.serviceClient<ServiceDataType>(service_name_);
      initialized_ = true;
    }
    return client_;
  }
};


template <class ServiceDataType>
class MultiArmServiceWrapper
{
 private:
  ros::NodeHandle nh_;

  std::string prefix_;

  std::string suffix_;

  typedef std::map<std::string, ros::ServiceClient> map_type;

  map_type clients_;


  bool resolve_names_;

 public:
 MultiArmServiceWrapper(std::string prefix, std::string suffix, bool resolve_names) :
  nh_(""), prefix_(prefix), suffix_(suffix), resolve_names_(resolve_names)
  {}


  ros::ServiceClient& client(std::string arm_name, ros::Duration timeout = ros::Duration(5.0))
    {
      //compute the name of the service
      std::string client_name = prefix_ + arm_name + suffix_;

      //check if the service is already there
      map_type::iterator it = clients_.find(client_name);
      if ( it != clients_.end() )
      {
        return it->second;
      }

      std::string service_name = client_name;
      if (resolve_names_) service_name = nh_.resolveName(client_name);

      //new service; wait for it
      ros::Duration ping_time = ros::Duration(1.0);
      if (timeout >= ros::Duration(0) && ping_time > timeout) ping_time = timeout;
      ros::Time start_time = ros::Time::now();
      while(1)
      {
        if (ros::service::waitForService(service_name, ping_time)) break;
        if (!ros::ok()) throw ServiceNotFoundException(client_name + " remapped to " + service_name);
        ros::Time current_time = ros::Time::now();
        if (timeout >= ros::Duration(0) && current_time - start_time >= timeout)
          throw ServiceNotFoundException(client_name + " remapped to " + service_name);
        ROS_INFO_STREAM("Waiting for service " << client_name << " remapped to " << service_name);
      }

      //insert new service in list
      std::pair<map_type::iterator, bool> new_pair;
      new_pair = clients_.insert(std::pair<std::string, ros::ServiceClient>
                                 (client_name, nh_.serviceClient<ServiceDataType>(service_name) ) );

      //and return it
      return new_pair.first->second;
    }
};


template <class ActionDataType>
class ActionWrapper
{
 private:
  bool initialized_;
  std::string action_name_;
  ros::NodeHandle nh_;
  actionlib::SimpleActionClient<ActionDataType> client_;
 public:
 ActionWrapper(std::string action_name, bool param) : initialized_(false),
    action_name_(action_name),
    nh_(""),
    client_(nh_, action_name, param) {}

  actionlib::SimpleActionClient<ActionDataType>& client(ros::Duration timeout = ros::Duration(5.0))
  {
    if (!initialized_)
    {
      ros::Duration ping_time = ros::Duration(1.0);
      if (timeout >= ros::Duration(0) && ping_time > timeout) ping_time = timeout;
      ros::Time start_time = ros::Time::now();
      while (1)
      {
        if (client_.waitForServer(ping_time)) break;
        if (!ros::ok()) throw ServiceNotFoundException(action_name_);
        ros::Time current_time = ros::Time::now();
        if (timeout >= ros::Duration(0) && current_time - start_time >= timeout)
          throw ServiceNotFoundException(action_name_);
        ROS_INFO_STREAM("Waiting for action client: " << action_name_);
      }
      initialized_ = true;
    }
    return client_;
  }
};



template <class ActionDataType>
class MultiArmActionWrapper
{
 private:
  ros::NodeHandle nh_;

  std::string prefix_;

  std::string suffix_;

  bool param_;

  typedef std::map<std::string, actionlib::SimpleActionClient<ActionDataType>* > map_type;

  map_type clients_;


  bool resolve_names_;

 public:
 MultiArmActionWrapper(std::string prefix, std::string suffix, bool param, bool resolve_names) :
  nh_(""), prefix_(prefix), suffix_(suffix), param_(param), resolve_names_(resolve_names)
  {}

  ~MultiArmActionWrapper()
    {
      for (typename map_type::iterator it = clients_.begin(); it!=clients_.end(); it++)
        {
          delete it->second;
        }
    }


  actionlib::SimpleActionClient<ActionDataType>& client(std::string arm_name,
                                                        ros::Duration timeout = ros::Duration(5.0))
    {
      //compute the name of the action
      std::string client_name = prefix_ + arm_name + suffix_;

      //check if the action client is already there
      typename map_type::iterator it = clients_.find(client_name);
      if ( it != clients_.end() )
      {
        return *(it->second);
      }

      std::string action_name = client_name;
      if (resolve_names_) action_name = nh_.resolveName(client_name);

      //create new action client and insert in map
      actionlib::SimpleActionClient<ActionDataType>* new_client =
        new actionlib::SimpleActionClient<ActionDataType>(nh_, action_name, param_ );

      //wait for the server
      ros::Duration ping_time = ros::Duration(1.0);
      if (timeout >= ros::Duration(0) && ping_time > timeout) ping_time = timeout;
      ros::Time start_time = ros::Time::now();
      while (1)
      {
        if (new_client->waitForServer(ping_time)) break;
        if (!ros::ok()) throw ServiceNotFoundException(client_name + " remapped to " + action_name);
        ros::Time current_time = ros::Time::now();
        if (timeout >= ros::Duration(0) && current_time - start_time >= timeout)
          throw ServiceNotFoundException(client_name + " remapped to " + action_name);
        ROS_INFO_STREAM("Waiting for action client " << client_name << ", remapped to " << action_name);
      }

      //insert new client in map
      std::pair< typename map_type::iterator, bool> new_pair =
        clients_.insert( std::pair<std::string,actionlib::SimpleActionClient<ActionDataType>* >
                         (client_name, new_client ) );

      //and return it
      return *(new_pair.first->second);

    }
};


} //namespace object_manipulator

#endif