trajectory_constraints_storage.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2013, 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 the 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: Mario Prats, Ioan Sucan */
00036 
00037 #include <moveit/warehouse/trajectory_constraints_storage.h>
00038 
00039 const std::string moveit_warehouse::TrajectoryConstraintsStorage::DATABASE_NAME = "moveit_trajectory_constraints";
00040 
00041 const std::string moveit_warehouse::TrajectoryConstraintsStorage::CONSTRAINTS_ID_NAME = "constraints_id";
00042 const std::string moveit_warehouse::TrajectoryConstraintsStorage::CONSTRAINTS_GROUP_NAME = "group_id";
00043 const std::string moveit_warehouse::TrajectoryConstraintsStorage::ROBOT_NAME = "robot_id";
00044 
00045 moveit_warehouse::TrajectoryConstraintsStorage::TrajectoryConstraintsStorage(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::TrajectoryConstraintsStorage::createCollections(void)
00053 {
00054   constraints_collection_.reset(new TrajectoryConstraintsCollection::element_type(DATABASE_NAME, "trajectory_constraints", db_host_, db_port_, timeout_));
00055 }
00056 
00057 void moveit_warehouse::TrajectoryConstraintsStorage::reset(void)
00058 {
00059   constraints_collection_.reset();
00060   MoveItMessageStorage::drop(DATABASE_NAME);
00061   createCollections();
00062 }
00063 
00064 void moveit_warehouse::TrajectoryConstraintsStorage::addTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints &msg, const std::string &name, const std::string &robot, const std::string &group)
00065 {
00066   bool replace = false;
00067   if (hasTrajectoryConstraints(name, robot, group))
00068   {
00069     removeTrajectoryConstraints(name, robot, group);
00070     replace = true;
00071   }
00072   mongo_ros::Metadata metadata(CONSTRAINTS_ID_NAME, 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", name.c_str());
00077 }
00078 
00079 bool moveit_warehouse::TrajectoryConstraintsStorage::hasTrajectoryConstraints(const std::string &name, const std::string &robot, const std::string &group) const
00080 {
00081 
00082   mongo_ros::Query q(CONSTRAINTS_ID_NAME, name);
00083   if (!robot.empty())
00084     q.append(ROBOT_NAME, robot);
00085   if (!group.empty())
00086     q.append(CONSTRAINTS_GROUP_NAME, group);
00087   std::vector<TrajectoryConstraintsWithMetadata> constr = constraints_collection_->pullAllResults(q, true);
00088   return !constr.empty();
00089 }
00090 
00091 void moveit_warehouse::TrajectoryConstraintsStorage::getKnownTrajectoryConstraints(const std::string &regex, std::vector<std::string> &names, const std::string &robot, const std::string &group) const
00092 {
00093   getKnownTrajectoryConstraints(names, robot, group);
00094   filterNames(regex, names);
00095 }
00096 
00097 void moveit_warehouse::TrajectoryConstraintsStorage::getKnownTrajectoryConstraints(std::vector<std::string> &names, const std::string &robot, const std::string &group) const
00098 {
00099   names.clear();
00100   mongo_ros::Query q;
00101   if (!robot.empty())
00102     q.append(ROBOT_NAME, robot);
00103   if (!group.empty())
00104     q.append(CONSTRAINTS_GROUP_NAME, group);
00105   std::vector<TrajectoryConstraintsWithMetadata> constr = constraints_collection_->pullAllResults(q, true, CONSTRAINTS_ID_NAME, true);
00106   for (std::size_t i = 0; i < constr.size() ; ++i)
00107     if (constr[i]->metadata.hasField(CONSTRAINTS_ID_NAME.c_str()))
00108       names.push_back(constr[i]->lookupString(CONSTRAINTS_ID_NAME));
00109 }
00110 
00111 bool moveit_warehouse::TrajectoryConstraintsStorage::getTrajectoryConstraints(TrajectoryConstraintsWithMetadata &msg_m, const std::string &name, const std::string &robot, const std::string &group) const
00112 {
00113   mongo_ros::Query q(CONSTRAINTS_ID_NAME, name);
00114   if (!robot.empty())
00115     q.append(ROBOT_NAME, robot);
00116   if (!group.empty())
00117     q.append(CONSTRAINTS_GROUP_NAME, group);
00118   std::vector<TrajectoryConstraintsWithMetadata> constr = constraints_collection_->pullAllResults(q, false);
00119   if (constr.empty())
00120     return false;
00121   else
00122   {
00123     msg_m = constr.back();
00124     return true;
00125   }
00126 }
00127 
00128 void moveit_warehouse::TrajectoryConstraintsStorage::renameTrajectoryConstraints(const std::string &old_name, const std::string &new_name, const std::string &robot, const std::string &group)
00129 {
00130   mongo_ros::Query q(CONSTRAINTS_ID_NAME, old_name);
00131   if (!robot.empty())
00132     q.append(ROBOT_NAME, robot);
00133   if (!group.empty())
00134     q.append(CONSTRAINTS_GROUP_NAME, group);
00135   mongo_ros::Metadata m(CONSTRAINTS_ID_NAME, new_name);
00136   constraints_collection_->modifyMetadata(q, m);
00137   ROS_DEBUG("Renamed constraints from '%s' to '%s'", old_name.c_str(), new_name.c_str());
00138 }
00139 
00140 void moveit_warehouse::TrajectoryConstraintsStorage::removeTrajectoryConstraints(const std::string &name, const std::string &robot, const std::string &group)
00141 {
00142   mongo_ros::Query q(CONSTRAINTS_ID_NAME, name);
00143   if (!robot.empty())
00144     q.append(ROBOT_NAME, robot);
00145   if (!group.empty())
00146     q.append(CONSTRAINTS_GROUP_NAME, group);
00147   unsigned int rem = constraints_collection_->removeMessages(q);
00148   ROS_DEBUG("Removed %u TrajectoryConstraints messages (named '%s')", rem, name.c_str());
00149 }


warehouse
Author(s): Ioan Sucan
autogenerated on Mon Oct 6 2014 02:32:09