property_node.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_properties/property_node.h>

#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/join.hpp>
#include <boost/algorithm/string/trim.hpp>
#include <boost/bind.hpp>

#include <vector>

#include <ros/assert.h>

#include <ros/callback_queue_interface.h>

namespace rve_properties
{

PropertyNode::PropertyNode()
: parent_(0)
, children_(0)
, change_callbacks_(0)
{
}

PropertyNode::~PropertyNode()
{
  delete children_;
  delete change_callbacks_;
}

void PropertyNode::setParent(PropertyNode* parent, const std::string& name, bool remove_from_previous)
{
  PropertyNodePtr this_ptr = shared_from_this();

  if (parent_)
  {
    parent_->fireChanged(this_ptr, name_, PropertyRemoved);

    if (remove_from_previous)
    {
      parent_->removeNode(name_);
    }
  }

  parent_ = parent;
  name_ = name;

  if (parent_)
  {
    parent_->fireChanged(this_ptr, name_, PropertyAdded);
  }
}

bool PropertyNode::hasValue()
{
  return data_.value.filled();
}

PropertyNodePtr PropertyNode::getNode(const std::string& _path, bool create_if_necessary)
{
  std::string path = _path;
  boost::trim(path);
  if (path.empty())
  {
    return shared_from_this();
  }

  typedef std::vector<std::string> V_string;
  V_string split_path;
  boost::split(split_path, path, boost::is_any_of("/"), boost::token_compress_on);

  if (split_path.empty())
  {
    return shared_from_this();
  }

  PropertyNodePtr node = shared_from_this();

  V_string::iterator it = split_path.begin();
  V_string::iterator end = split_path.end();
  for (; it != end; ++it)
  {
    const std::string& prop = *it;

    bool created_new_node = false;
    PropertyNodePtr parent;
    {
      boost::recursive_mutex::scoped_lock lock(node->mutex_);

      if (!node->children_)
      {
        if (!create_if_necessary)
        {
          return PropertyNodePtr();
        }

        node->children_ = new M_PropertyNode;
      }

      M_PropertyNode::iterator child_it = node->children_->find(prop);
      if (child_it != node->children_->end())
      {
        node = child_it->second;
      }
      else
      {
        if (!create_if_necessary)
        {
          return PropertyNodePtr();
        }

        PropertyNodePtr new_node(new PropertyNode);
        node->children_->insert(std::make_pair(prop, new_node));
        parent = node;
        node = new_node;
        created_new_node = true;
      }
    }

    if (created_new_node)
    {
      node->setParent(parent.get(), prop, true);
    }
  }

  return node;
}

std::pair<PropertyNodePtr, std::string> PropertyNode::getParentOf(const std::string& _path)
{
  std::string path = _path;
  boost::trim(path);
  if (path.empty())
  {
    throw BadPathException("Cannot get the parent of the current node!");
  }

  // This is an inefficient way of doing this
  typedef std::vector<std::string> V_string;
  V_string split_path;
  boost::split(split_path, path, boost::is_any_of("/"), boost::token_compress_on);
  std::string final_name = split_path.back();
  split_path.pop_back();

  if (split_path.empty())
  {
    return std::make_pair(shared_from_this(), final_name);
  }
  else
  {
    std::string parent_path = boost::join(split_path, "/");
    return std::make_pair(getNode(parent_path), final_name);
  }
}

void PropertyNode::setChild(const std::string& name, const PropertyNodePtr& node)
{
  boost::recursive_mutex::scoped_lock lock(mutex_);

  if (!children_)
  {
    children_ = new M_PropertyNode;
  }

  M_PropertyNode::iterator it = children_->find(name);
  if (it != children_->end())
  {
    it->second = node;
  }
  else
  {
    children_->insert(std::make_pair(name, node));
  }

  node->setParent(this, name, true);
}

void PropertyNode::setNode(const std::string& _path, const PropertyNodePtr& node)
{
  std::string path = _path;
  boost::trim(path);
  if (path.empty())
  {
    throw BadPathException("Cannot set the pointer for the current node");
  }

  std::pair<PropertyNodePtr, std::string> pair = getParentOf(path);
  const PropertyNodePtr& parent = pair.first;
  const std::string& child_name = pair.second;
  parent->setChild(child_name, node);
}

void PropertyNode::removeNode(const std::string& _path)
{
  std::string path = _path;
  boost::trim(path);
  if (path.empty())
  {
    throw BadPathException("Cannot delete the current node!");
  }

  std::pair<PropertyNodePtr, std::string> pair = getParentOf(path);
  const PropertyNodePtr& parent = pair.first;
  const std::string& child_name = pair.second;

  PropertyNodePtr child;

  {
    boost::recursive_mutex::scoped_lock lock(mutex_);
    if (!parent->children_)
    {
      return;
    }

    M_PropertyNode::iterator it = parent->children_->find(child_name);
    if (it != parent->children_->end())
    {
      PropertyNodePtr child = it->second;
      parent->children_->erase(it);
    }
  }

  if (child)
  {
    child->setParent(0, "", false);
  }
}

bool PropertyNode::exists(const std::string& path)
{
  return getNode(path, false);
}

PropertyNodePtr PropertyNode::getParent()
{
  if (!parent_)
  {
    return PropertyNodePtr();
  }

  return parent_->shared_from_this();
}

struct CBQueueChangeCallback : public ros::CallbackInterface
{
public:
  CBQueueChangeCallback(const PropertyChangeCallback& cb, const PropertyNodePtr& node, const std::string& path, PropertyChangeType type)
  : cb_(cb)
  , node_(node)
  , path_(path)
  , type_(type)
  {}

  virtual CallResult call()
  {
    cb_(node_, path_, type_);
    return Success;
  }

private:
  PropertyChangeCallback cb_;
  PropertyNodePtr node_;
  std::string path_;
  PropertyChangeType type_;
};

void PropertyNode::fireChanged(const PropertyNodePtr& node, const std::string& path, PropertyChangeType type)
{
  boost::recursive_mutex::scoped_lock lock(mutex_);
  std::string full_path;
  if (path.empty())
  {
    full_path = name_;
  }
  else
  {
    if (name_.empty())
    {
      ROS_ASSERT(!parent_);
      full_path = path;
    }
    else
    {
      full_path = name_ + "/" + path;
    }
  }

  if (parent_)
  {
    parent_->childChanged(node, full_path, type);
  }

  if (!change_callbacks_)
  {
    return;
  }

  V_ChangeCallback::iterator it = change_callbacks_->begin();
  V_ChangeCallback::iterator end = change_callbacks_->end();
  for (; it != end; ++it)
  {
    const ChangeCallback& cc = *it;
    if (cc.queue)
    {
      cc.queue->addCallback(ros::CallbackInterfacePtr(new CBQueueChangeCallback(cc.cb, node, path, type)), (uint64_t)cc.owner);
    }
    else
    {
      cc.cb(node, full_path, type);
    }
  }
}

void PropertyNode::childChanged(const PropertyNodePtr& child, const std::string& child_path, PropertyChangeType type)
{
  fireChanged(child, child_path, type);
}

void PropertyNode::addChangeCallback(const PropertyChangeCallback& cb, void* owner, ros::CallbackQueueInterface* callback_queue)
{
  boost::recursive_mutex::scoped_lock lock(mutex_);

  if (!change_callbacks_)
  {
    change_callbacks_ = new V_ChangeCallback;
  }

  ChangeCallback cc;
  cc.owner = owner;
  cc.cb = cb;
  cc.queue = callback_queue;
  change_callbacks_->push_back(cc);
}

void PropertyNode::removeChangeCallback(void* owner)
{
  boost::recursive_mutex::scoped_lock lock(mutex_);

  if (!change_callbacks_)
  {
    return;
  }

  V_ChangeCallback::iterator it = change_callbacks_->begin();
  for (; it != change_callbacks_->end();)
  {
    const ChangeCallback& cc = *it;
    if (cc.owner == owner)
    {
      if (cc.queue)
      {
        cc.queue->removeByID((uint64_t)cc.owner);
      }
      it = change_callbacks_->erase(it);
    }
    else
    {
      ++it;
    }
  }
}

} // namespace rve_properties