constraints_storage.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #include <moveit/warehouse/constraints_storage.h>
00038 
00039 const std::string moveit_warehouse::ConstraintsStorage::DATABASE_NAME = "moveit_constraints";
00040 
00041 const std::string moveit_warehouse::ConstraintsStorage::CONSTRAINTS_ID_NAME = "constraints_id";
00042 const std::string moveit_warehouse::ConstraintsStorage::CONSTRAINTS_GROUP_NAME = "group_id";
00043 const std::string moveit_warehouse::ConstraintsStorage::ROBOT_NAME = "robot_id";
00044 
00045 moveit_warehouse::ConstraintsStorage::ConstraintsStorage(const std::string &host, const unsigned int port, double wait_seconds) :
00046   MoveItMessageStorage(host, port, wait_seconds)
00047 {
00048   createCollections();
00049   ROS_DEBUG("Connected to MongoDB '%s' on host '%s' port '%u'.", DATABASE_NAME.c_str(), db_host_.c_str(), db_port_);
00050 }
00051 
00052 void moveit_warehouse::ConstraintsStorage::createCollections()
00053 {
00054   constraints_collection_.reset(new ConstraintsCollection::element_type(DATABASE_NAME, "constraints", db_host_, db_port_, timeout_));
00055 }
00056 
00057 void moveit_warehouse::ConstraintsStorage::reset()
00058 {
00059   constraints_collection_.reset();
00060   MoveItMessageStorage::drop(DATABASE_NAME);
00061   createCollections();
00062 }
00063 
00064 void moveit_warehouse::ConstraintsStorage::addConstraints(const moveit_msgs::Constraints &msg, const std::string &robot, const std::string &group)
00065 {
00066   bool replace = false;
00067   if (hasConstraints(msg.name, robot, group))
00068   {
00069     removeConstraints(msg.name, robot, group);
00070     replace = true;
00071   }
00072   mongo_ros::Metadata metadata(CONSTRAINTS_ID_NAME, msg.name,
00073                                ROBOT_NAME, robot,
00074                                CONSTRAINTS_GROUP_NAME, group);
00075   constraints_collection_->insert(msg, metadata);
00076   ROS_DEBUG("%s constraints '%s'", replace ? "Replaced" : "Added", msg.name.c_str());
00077 }
00078 
00079 bool moveit_warehouse::ConstraintsStorage::hasConstraints(const std::string &name, const std::string &robot, const std::string &group) const
00080 {
00081   mongo_ros::Query q(CONSTRAINTS_ID_NAME, name);
00082   if (!robot.empty())
00083     q.append(ROBOT_NAME, robot);
00084   if (!group.empty())
00085     q.append(CONSTRAINTS_GROUP_NAME, group);
00086   std::vector<ConstraintsWithMetadata> constr = constraints_collection_->pullAllResults(q, true);
00087   return !constr.empty();
00088 }
00089 
00090 void moveit_warehouse::ConstraintsStorage::getKnownConstraints(const std::string &regex, std::vector<std::string> &names, const std::string &robot, const std::string &group) const
00091 {
00092   getKnownConstraints(names, robot, group);
00093   filterNames(regex, names);
00094 }
00095 
00096 void moveit_warehouse::ConstraintsStorage::getKnownConstraints(std::vector<std::string> &names, const std::string &robot, const std::string &group) const
00097 {
00098   names.clear();
00099   mongo_ros::Query q;
00100   if (!robot.empty())
00101     q.append(ROBOT_NAME, robot);
00102   if (!group.empty())
00103     q.append(CONSTRAINTS_GROUP_NAME, group);
00104   std::vector<ConstraintsWithMetadata> constr = constraints_collection_->pullAllResults(q, true, CONSTRAINTS_ID_NAME, true);
00105   for (std::size_t i = 0; i < constr.size() ; ++i)
00106     if (constr[i]->metadata.hasField(CONSTRAINTS_ID_NAME.c_str()))
00107       names.push_back(constr[i]->lookupString(CONSTRAINTS_ID_NAME));
00108 }
00109 
00110 bool moveit_warehouse::ConstraintsStorage::getConstraints(ConstraintsWithMetadata &msg_m, const std::string &name, const std::string &robot, const std::string &group) const
00111 {
00112   mongo_ros::Query q(CONSTRAINTS_ID_NAME, name);
00113   if (!robot.empty())
00114     q.append(ROBOT_NAME, robot);
00115   if (!group.empty())
00116     q.append(CONSTRAINTS_GROUP_NAME, group);
00117   std::vector<ConstraintsWithMetadata> constr = constraints_collection_->pullAllResults(q, false);
00118   if (constr.empty())
00119     return false;
00120   else
00121   {
00122     msg_m = constr.back();
00123     // in case the constraints were renamed, the name in the message may be out of date
00124     const_cast<moveit_msgs::Constraints*>(static_cast<const moveit_msgs::Constraints*>(msg_m.get()))->name = name;
00125     return true;
00126   }
00127 }
00128 
00129 void moveit_warehouse::ConstraintsStorage::renameConstraints(const std::string &old_name, const std::string &new_name, const std::string &robot, const std::string &group)
00130 {
00131   mongo_ros::Query q(CONSTRAINTS_ID_NAME, old_name);
00132   if (!robot.empty())
00133     q.append(ROBOT_NAME, robot);
00134   if (!group.empty())
00135     q.append(CONSTRAINTS_GROUP_NAME, group);
00136   mongo_ros::Metadata m(CONSTRAINTS_ID_NAME, new_name);
00137   constraints_collection_->modifyMetadata(q, m);
00138   ROS_DEBUG("Renamed constraints from '%s' to '%s'", old_name.c_str(), new_name.c_str());
00139 }
00140 
00141 void moveit_warehouse::ConstraintsStorage::removeConstraints(const std::string &name, const std::string &robot, const std::string &group)
00142 {
00143   mongo_ros::Query q(CONSTRAINTS_ID_NAME, name);
00144   if (!robot.empty())
00145     q.append(ROBOT_NAME, robot);
00146   if (!group.empty())
00147     q.append(CONSTRAINTS_GROUP_NAME, group);
00148   unsigned int rem = constraints_collection_->removeMessages(q);
00149   ROS_DEBUG("Removed %u Constraints messages (named '%s')", rem, name.c_str());
00150 }


warehouse
Author(s): Ioan Sucan
autogenerated on Wed Aug 26 2015 12:43:45