client.h

Go to the documentation of this file.

#include "sick_scan/sick_scan_base.h" /* Base definitions included in all header files, added by add_sick_scan_base_header.py. Do not edit this line. */
/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2015-2016, Myrmex, 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 Myrmex, 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 __CLIENT_H__
#define __CLIENT_H__
 
//#include <boost/chrono.hpp>
//#include <boost/function.hpp>
#include <thread>
#include <ros/node_handle.h>
#include <dynamic_reconfigure/ConfigDescription.h>
#include <dynamic_reconfigure/Reconfigure.h>
 
namespace dynamic_reconfigure {
 
template <class ConfigType>
class Client {
 public:
  Client(
      const std::string& name,
      const std::function<void(const ConfigType&)> config_callback = 0,
      const std::function<void(const dynamic_reconfigure::ConfigDescription&)>
          description_callback = 0)
      : name_(name),
        nh_(name),
        received_configuration_(false),
        received_description_(false),
        config_callback_(config_callback),
        description_callback_(description_callback) {
    set_service_ =
        nh_.serviceClient<dynamic_reconfigure::Reconfigure>("set_parameters");
 
    config_sub_ =
        nh_.subscribe("parameter_updates", 1,
                      &Client<ConfigType>::configurationCallback, this);
 
    descr_sub_ = nh_.subscribe("parameter_descriptions", 1,
                               &Client<ConfigType>::descriptionCallback, this);
  }
  Client(
      const std::string& name, const ros::NodeHandle& nh,
      const std::function<void(const ConfigType&)> config_callback = 0,
      const std::function<void(const dynamic_reconfigure::ConfigDescription&)>
          description_callback = 0)
      : name_(name),
        nh_(nh),
        received_configuration_(false),
        received_description_(false),
        config_callback_(config_callback),
        description_callback_(description_callback) {
    set_service_ =
        nh_.serviceClient<dynamic_reconfigure::Reconfigure>("set_parameters");
 
    config_sub_ =
        nh_.subscribe("parameter_updates", 1,
                      &Client<ConfigType>::configurationCallback, this);
 
    descr_sub_ = nh_.subscribe("parameter_descriptions", 1,
                               &Client<ConfigType>::descriptionCallback, this);
  }
  void setConfigurationCallback(
      const std::function<void(const ConfigType&)>& config_callback) {
    config_callback_ = config_callback;
  }
  void setDescriptionCallback(const std::function<void(
      const dynamic_reconfigure::ConfigDescription&)>& description_callback) {
    description_callback_ = description_callback;
  }
  bool setConfiguration(const ConfigType& configuration) {
    ConfigType temp = configuration;
    return setConfiguration(temp);
  }
  bool setConfiguration(ConfigType& configuration) {
    dynamic_reconfigure::Reconfigure srv;
    received_configuration_ = false;
    configuration.__toMessage__(srv.request.config);
    if (set_service_.call(srv)) {
      configuration.__fromMessage__(srv.response.config);
      return true;
    } else {
      ROS_WARN("Could not set configuration");
      return false;
    }
  }
  bool getCurrentConfiguration(
      ConfigType& configuration,
      const ros::Duration& timeout = ros::Duration(0)) {
    if (timeout == ros::Duration(0)) {
      ROS_INFO_ONCE("Waiting for configuration...");
      std::lock_guard<std::mutex> lock(mutex_);
      while (!received_configuration_) {
        if (!ros::ok()) return false;
        cv_.wait(lock);
      }
    } else {
      ros::Time start_time = ros::Time::now();
      std::lock_guard<std::mutex> lock(mutex_);
      while (!received_configuration_) {
        if (!ros::ok()) return false;
        ros::Duration time_left = timeout - (ros::Time::now() - start_time);
        if (time_left.toSec() <= 0.0) return false;
        cv_.wait_for(lock, boost::chrono::nanoseconds(time_left.toNSec()));
      }
    }
    configuration = latest_configuration_;
    return true;
  }
  bool getDefaultConfiguration(
      ConfigType& configuration,
      const ros::Duration& timeout = ros::Duration(0)) {
    ConfigDescription answer;
    if (getDescription(answer, timeout)) {
      configuration.__fromMessage__(answer.dflt);
      return true;
    } else {
      return false;
    }
  }
  bool getMinConfiguration(ConfigType& configuration,
                           const ros::Duration& timeout = ros::Duration(0)) {
    ConfigDescription answer;
    if (getDescription(answer, timeout)) {
      configuration.__fromMessage__(answer.min);
      return true;
    } else {
      return false;
    }
  }
  bool getMaxConfiguration(ConfigType& configuration,
                           const ros::Duration& timeout = ros::Duration(0)) {
    ConfigDescription answer;
    if (getDescription(answer, timeout)) {
      configuration.__fromMessage__(answer.max);
      return true;
    } else {
      return false;
    }
  }
  std::string getName() const { return name_; }
 
 private:
  void configurationCallback(const dynamic_reconfigure::Config& configuration) {
    std::lock_guard<std::mutex> lock(mutex_);
    dynamic_reconfigure::Config temp_config = configuration;
    received_configuration_ = true;
    latest_configuration_.__fromMessage__(temp_config);
    cv_.notify_all();
 
    if (config_callback_) {
      try {
        config_callback_(latest_configuration_);
      } catch (std::exception& e) {
        ROS_WARN("Configuration callback failed with exception %s", e.what());
      } catch (...) {
        ROS_WARN("Configuration callback failed with unprintable exception");
      }
    } else {
      ROS_DEBUG(
          "Unable to call Configuration callback because none was set.\n" \
          "See setConfigurationCallback");
    }
  }
 
  void descriptionCallback(
      const dynamic_reconfigure::ConfigDescription& description) {
    std::lock_guard<std::mutex> lock(mutex_);
    received_description_ = true;
    latest_description_ = description;
    cv_.notify_all();
 
    if (description_callback_) {
      try {
        description_callback_(description);
      } catch (std::exception& e) {
        ROS_WARN("Description callback failed with exception %s", e.what());
      } catch (...) {
        ROS_WARN("Description callback failed with unprintable exception");
      }
    } else {
      ROS_DEBUG(
          "Unable to call Description callback because none was set.\n" \
          "See setDescriptionCallback");
    }
  }
 
  bool getDescription(ConfigDescription& configuration,
                      const ros::Duration& timeout) {
    if (timeout == ros::Duration(0)) {
      ROS_INFO_ONCE("Waiting for configuration...");
      std::lock_guard<std::mutex> lock(mutex_);
      while (!received_description_) {
        if (!ros::ok()) return false;
        cv_.wait(lock);
      }
    } else {
      ros::Time start_time = ros::Time::now();
      std::lock_guard<std::mutex> lock(mutex_);
      while (!received_description_) {
        if (!ros::ok()) return false;
        ros::Duration time_left = timeout - (ros::Time::now() - start_time);
        if (time_left.toSec() <= 0.0) return false;
        cv_.wait_for(lock, boost::chrono::nanoseconds(time_left.toNSec()));
      }
    }
    configuration = latest_description_;
    return true;
  }
 
  std::string name_;
  bool received_configuration_;
  ConfigType latest_configuration_;
  bool received_description_;
  dynamic_reconfigure::ConfigDescription latest_description_;
  std::condition_variable cv_;
  std::mutex mutex_;
  ros::NodeHandle nh_;
  ros::ServiceClient set_service_;
  ros::Subscriber descr_sub_;
  ros::Subscriber config_sub_;
 
  std::function<void(const ConfigType&)> config_callback_;
  std::function<void(const dynamic_reconfigure::ConfigDescription&)>
      description_callback_;
};
}
 
#endif  // __CLIENT_H__