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 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 using warehouse_ros::Metadata;
00046 using warehouse_ros::Query;
00047 
00048 moveit_warehouse::TrajectoryConstraintsStorage::TrajectoryConstraintsStorage(
00049     warehouse_ros::DatabaseConnection::Ptr conn)
00050   : MoveItMessageStorage(conn)
00051 {
00052   createCollections();
00053 }
00054 
00055 void moveit_warehouse::TrajectoryConstraintsStorage::createCollections(void)
00056 {
00057   constraints_collection_ =
00058       conn_->openCollectionPtr<moveit_msgs::TrajectoryConstraints>(DATABASE_NAME, "trajectory_constraints");
00059 }
00060 
00061 void moveit_warehouse::TrajectoryConstraintsStorage::reset(void)
00062 {
00063   constraints_collection_.reset();
00064   conn_->dropDatabase(DATABASE_NAME);
00065   createCollections();
00066 }
00067 
00068 void moveit_warehouse::TrajectoryConstraintsStorage::addTrajectoryConstraints(
00069     const moveit_msgs::TrajectoryConstraints& msg, const std::string& name, const std::string& robot,
00070     const std::string& group)
00071 {
00072   bool replace = false;
00073   if (hasTrajectoryConstraints(name, robot, group))
00074   {
00075     removeTrajectoryConstraints(name, robot, group);
00076     replace = true;
00077   }
00078   Metadata::Ptr metadata = constraints_collection_->createMetadata();
00079   metadata->append(CONSTRAINTS_ID_NAME, name);
00080   metadata->append(ROBOT_NAME, robot);
00081   metadata->append(CONSTRAINTS_GROUP_NAME, group);
00082   constraints_collection_->insert(msg, metadata);
00083   ROS_DEBUG("%s constraints '%s'", replace ? "Replaced" : "Added", name.c_str());
00084 }
00085 
00086 bool moveit_warehouse::TrajectoryConstraintsStorage::hasTrajectoryConstraints(const std::string& name,
00087                                                                               const std::string& robot,
00088                                                                               const std::string& group) const
00089 {
00090   Query::Ptr q = constraints_collection_->createQuery();
00091   q->append(CONSTRAINTS_ID_NAME, name);
00092   if (!robot.empty())
00093     q->append(ROBOT_NAME, robot);
00094   if (!group.empty())
00095     q->append(CONSTRAINTS_GROUP_NAME, group);
00096   std::vector<TrajectoryConstraintsWithMetadata> constr = constraints_collection_->queryList(q, true);
00097   return !constr.empty();
00098 }
00099 
00100 void moveit_warehouse::TrajectoryConstraintsStorage::getKnownTrajectoryConstraints(const std::string& regex,
00101                                                                                    std::vector<std::string>& names,
00102                                                                                    const std::string& robot,
00103                                                                                    const std::string& group) const
00104 {
00105   getKnownTrajectoryConstraints(names, robot, group);
00106   filterNames(regex, names);
00107 }
00108 
00109 void moveit_warehouse::TrajectoryConstraintsStorage::getKnownTrajectoryConstraints(std::vector<std::string>& names,
00110                                                                                    const std::string& robot,
00111                                                                                    const std::string& group) const
00112 {
00113   names.clear();
00114   Query::Ptr q = constraints_collection_->createQuery();
00115   if (!robot.empty())
00116     q->append(ROBOT_NAME, robot);
00117   if (!group.empty())
00118     q->append(CONSTRAINTS_GROUP_NAME, group);
00119   std::vector<TrajectoryConstraintsWithMetadata> constr =
00120       constraints_collection_->queryList(q, true, CONSTRAINTS_ID_NAME, true);
00121   for (std::size_t i = 0; i < constr.size(); ++i)
00122     if (constr[i]->lookupField(CONSTRAINTS_ID_NAME))
00123       names.push_back(constr[i]->lookupString(CONSTRAINTS_ID_NAME));
00124 }
00125 
00126 bool moveit_warehouse::TrajectoryConstraintsStorage::getTrajectoryConstraints(TrajectoryConstraintsWithMetadata& msg_m,
00127                                                                               const std::string& name,
00128                                                                               const std::string& robot,
00129                                                                               const std::string& group) const
00130 {
00131   Query::Ptr q = constraints_collection_->createQuery();
00132   q->append(CONSTRAINTS_ID_NAME, name);
00133   if (!robot.empty())
00134     q->append(ROBOT_NAME, robot);
00135   if (!group.empty())
00136     q->append(CONSTRAINTS_GROUP_NAME, group);
00137   std::vector<TrajectoryConstraintsWithMetadata> constr = constraints_collection_->queryList(q, false);
00138   if (constr.empty())
00139     return false;
00140   else
00141   {
00142     msg_m = constr.back();
00143     return true;
00144   }
00145 }
00146 
00147 void moveit_warehouse::TrajectoryConstraintsStorage::renameTrajectoryConstraints(const std::string& old_name,
00148                                                                                  const std::string& new_name,
00149                                                                                  const std::string& robot,
00150                                                                                  const std::string& group)
00151 {
00152   Query::Ptr q = constraints_collection_->createQuery();
00153   q->append(CONSTRAINTS_ID_NAME, old_name);
00154   if (!robot.empty())
00155     q->append(ROBOT_NAME, robot);
00156   if (!group.empty())
00157     q->append(CONSTRAINTS_GROUP_NAME, group);
00158   Metadata::Ptr m = constraints_collection_->createMetadata();
00159   m->append(CONSTRAINTS_ID_NAME, new_name);
00160   constraints_collection_->modifyMetadata(q, m);
00161   ROS_DEBUG("Renamed constraints from '%s' to '%s'", old_name.c_str(), new_name.c_str());
00162 }
00163 
00164 void moveit_warehouse::TrajectoryConstraintsStorage::removeTrajectoryConstraints(const std::string& name,
00165                                                                                  const std::string& robot,
00166                                                                                  const std::string& group)
00167 {
00168   Query::Ptr q = constraints_collection_->createQuery();
00169   q->append(CONSTRAINTS_ID_NAME, name);
00170   if (!robot.empty())
00171     q->append(ROBOT_NAME, robot);
00172   if (!group.empty())
00173     q->append(CONSTRAINTS_GROUP_NAME, group);
00174   unsigned int rem = constraints_collection_->removeMessages(q);
00175   ROS_DEBUG("Removed %u TrajectoryConstraints messages (named '%s')", rem, name.c_str());
00176 }


warehouse
Author(s): Ioan Sucan
autogenerated on Mon Jul 24 2017 02:21:59