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,
00046                                                          double wait_seconds)
00047   : MoveItMessageStorage(host, port, wait_seconds)
00048 {
00049   createCollections();
00050   ROS_DEBUG("Connected to MongoDB '%s' on host '%s' port '%u'.", DATABASE_NAME.c_str(), db_host_.c_str(), db_port_);
00051 }
00052 
00053 void moveit_warehouse::ConstraintsStorage::createCollections()
00054 {
00055   constraints_collection_.reset(
00056       new ConstraintsCollection::element_type(DATABASE_NAME, "constraints", db_host_, db_port_, timeout_));
00057 }
00058 
00059 void moveit_warehouse::ConstraintsStorage::reset()
00060 {
00061   constraints_collection_.reset();
00062   MoveItMessageStorage::drop(DATABASE_NAME);
00063   createCollections();
00064 }
00065 
00066 void moveit_warehouse::ConstraintsStorage::addConstraints(const moveit_msgs::Constraints& msg, const std::string& robot,
00067                                                           const std::string& group)
00068 {
00069   bool replace = false;
00070   if (hasConstraints(msg.name, robot, group))
00071   {
00072     removeConstraints(msg.name, robot, group);
00073     replace = true;
00074   }
00075   mongo_ros::Metadata metadata(CONSTRAINTS_ID_NAME, msg.name, ROBOT_NAME, robot, CONSTRAINTS_GROUP_NAME, group);
00076   constraints_collection_->insert(msg, metadata);
00077   ROS_DEBUG("%s constraints '%s'", replace ? "Replaced" : "Added", msg.name.c_str());
00078 }
00079 
00080 bool moveit_warehouse::ConstraintsStorage::hasConstraints(const std::string& name, const std::string& robot,
00081                                                           const std::string& group) const
00082 {
00083   mongo_ros::Query q(CONSTRAINTS_ID_NAME, name);
00084   if (!robot.empty())
00085     q.append(ROBOT_NAME, robot);
00086   if (!group.empty())
00087     q.append(CONSTRAINTS_GROUP_NAME, group);
00088   std::vector<ConstraintsWithMetadata> constr = constraints_collection_->pullAllResults(q, true);
00089   return !constr.empty();
00090 }
00091 
00092 void moveit_warehouse::ConstraintsStorage::getKnownConstraints(const std::string& regex,
00093                                                                std::vector<std::string>& names,
00094                                                                const std::string& robot, const std::string& group) const
00095 {
00096   getKnownConstraints(names, robot, group);
00097   filterNames(regex, names);
00098 }
00099 
00100 void moveit_warehouse::ConstraintsStorage::getKnownConstraints(std::vector<std::string>& names,
00101                                                                const std::string& robot, const std::string& group) const
00102 {
00103   names.clear();
00104   mongo_ros::Query q;
00105   if (!robot.empty())
00106     q.append(ROBOT_NAME, robot);
00107   if (!group.empty())
00108     q.append(CONSTRAINTS_GROUP_NAME, group);
00109   std::vector<ConstraintsWithMetadata> constr =
00110       constraints_collection_->pullAllResults(q, true, CONSTRAINTS_ID_NAME, true);
00111   for (std::size_t i = 0; i < constr.size(); ++i)
00112     if (constr[i]->metadata.hasField(CONSTRAINTS_ID_NAME.c_str()))
00113       names.push_back(constr[i]->lookupString(CONSTRAINTS_ID_NAME));
00114 }
00115 
00116 bool moveit_warehouse::ConstraintsStorage::getConstraints(ConstraintsWithMetadata& msg_m, const std::string& name,
00117                                                           const std::string& robot, const std::string& group) const
00118 {
00119   mongo_ros::Query q(CONSTRAINTS_ID_NAME, name);
00120   if (!robot.empty())
00121     q.append(ROBOT_NAME, robot);
00122   if (!group.empty())
00123     q.append(CONSTRAINTS_GROUP_NAME, group);
00124   std::vector<ConstraintsWithMetadata> constr = constraints_collection_->pullAllResults(q, false);
00125   if (constr.empty())
00126     return false;
00127   else
00128   {
00129     msg_m = constr.back();
00130     // in case the constraints were renamed, the name in the message may be out of date
00131     const_cast<moveit_msgs::Constraints*>(static_cast<const moveit_msgs::Constraints*>(msg_m.get()))->name = name;
00132     return true;
00133   }
00134 }
00135 
00136 void moveit_warehouse::ConstraintsStorage::renameConstraints(const std::string& old_name, const std::string& new_name,
00137                                                              const std::string& robot, const std::string& group)
00138 {
00139   mongo_ros::Query q(CONSTRAINTS_ID_NAME, old_name);
00140   if (!robot.empty())
00141     q.append(ROBOT_NAME, robot);
00142   if (!group.empty())
00143     q.append(CONSTRAINTS_GROUP_NAME, group);
00144   mongo_ros::Metadata m(CONSTRAINTS_ID_NAME, new_name);
00145   constraints_collection_->modifyMetadata(q, m);
00146   ROS_DEBUG("Renamed constraints from '%s' to '%s'", old_name.c_str(), new_name.c_str());
00147 }
00148 
00149 void moveit_warehouse::ConstraintsStorage::removeConstraints(const std::string& name, const std::string& robot,
00150                                                              const std::string& group)
00151 {
00152   mongo_ros::Query q(CONSTRAINTS_ID_NAME, name);
00153   if (!robot.empty())
00154     q.append(ROBOT_NAME, robot);
00155   if (!group.empty())
00156     q.append(CONSTRAINTS_GROUP_NAME, group);
00157   unsigned int rem = constraints_collection_->removeMessages(q);
00158   ROS_DEBUG("Removed %u Constraints messages (named '%s')", rem, name.c_str());
00159 }


warehouse
Author(s): Ioan Sucan
autogenerated on Wed Jan 17 2018 03:32:42