state_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/state_storage.h>
00038 
00039 const std::string moveit_warehouse::RobotStateStorage::DATABASE_NAME = "moveit_robot_states";
00040 
00041 const std::string moveit_warehouse::RobotStateStorage::STATE_NAME = "state_id";
00042 const std::string moveit_warehouse::RobotStateStorage::ROBOT_NAME = "robot_id";
00043 
00044 moveit_warehouse::RobotStateStorage::RobotStateStorage(const std::string &host, const unsigned int port, double wait_seconds) :
00045   MoveItMessageStorage(host, port, wait_seconds)
00046 {
00047   createCollections();
00048   ROS_DEBUG("Connected to MongoDB '%s' on host '%s' port '%u'.", DATABASE_NAME.c_str(), db_host_.c_str(), db_port_);
00049 }
00050 
00051 void moveit_warehouse::RobotStateStorage::createCollections()
00052 {
00053   state_collection_.reset(new RobotStateCollection::element_type(DATABASE_NAME, "robot_states", db_host_, db_port_, timeout_));
00054 }
00055 
00056 void moveit_warehouse::RobotStateStorage::reset()
00057 {
00058   state_collection_.reset();
00059   MoveItMessageStorage::drop(DATABASE_NAME);
00060   createCollections();
00061 }
00062 
00063 void moveit_warehouse::RobotStateStorage::addRobotState(const moveit_msgs::RobotState &msg, const std::string &name, const std::string &robot)
00064 {
00065   bool replace = false;
00066   if (hasRobotState(name, robot))
00067   {
00068     removeRobotState(name, robot);
00069     replace = true;
00070   }
00071   mongo_ros::Metadata metadata(STATE_NAME, name,
00072                                ROBOT_NAME, robot);
00073   state_collection_->insert(msg, metadata);
00074   ROS_DEBUG("%s robot state '%s'", replace ? "Replaced" : "Added", name.c_str());
00075 }
00076 
00077 bool moveit_warehouse::RobotStateStorage::hasRobotState(const std::string &name, const std::string &robot) const
00078 {
00079   mongo_ros::Query q(STATE_NAME, name);
00080   if (!robot.empty())
00081     q.append(ROBOT_NAME, robot);
00082   std::vector<RobotStateWithMetadata> constr = state_collection_->pullAllResults(q, true);
00083   return !constr.empty();
00084 }
00085 
00086 void moveit_warehouse::RobotStateStorage::getKnownRobotStates(const std::string &regex, std::vector<std::string> &names, const std::string &robot) const
00087 {
00088   getKnownRobotStates(names, robot);
00089   filterNames(regex, names);
00090 }
00091 
00092 void moveit_warehouse::RobotStateStorage::getKnownRobotStates(std::vector<std::string> &names, const std::string &robot) const
00093 {
00094   names.clear();
00095   mongo_ros::Query q;
00096   if (!robot.empty())
00097     q.append(ROBOT_NAME, robot);
00098   std::vector<RobotStateWithMetadata> constr = state_collection_->pullAllResults(q, true, STATE_NAME, true);
00099   for (std::size_t i = 0; i < constr.size() ; ++i)
00100     if (constr[i]->metadata.hasField(STATE_NAME.c_str()))
00101       names.push_back(constr[i]->lookupString(STATE_NAME));
00102 }
00103 
00104 bool moveit_warehouse::RobotStateStorage::getRobotState(RobotStateWithMetadata &msg_m, const std::string &name, const std::string &robot) const
00105 {
00106   mongo_ros::Query q(STATE_NAME, name);
00107   if (!robot.empty())
00108     q.append(ROBOT_NAME, robot);
00109   std::vector<RobotStateWithMetadata> constr = state_collection_->pullAllResults(q, false);
00110   if (constr.empty())
00111     return false;
00112   else
00113   {
00114     msg_m = constr.front();
00115     return true;
00116   }
00117 }
00118 
00119 void moveit_warehouse::RobotStateStorage::renameRobotState(const std::string &old_name, const std::string &new_name, const std::string &robot)
00120 {
00121   mongo_ros::Query q(STATE_NAME, old_name);
00122   if (!robot.empty())
00123     q.append(ROBOT_NAME, robot);
00124   mongo_ros::Metadata m(STATE_NAME, new_name);
00125   state_collection_->modifyMetadata(q, m);
00126   ROS_DEBUG("Renamed robot state from '%s' to '%s'", old_name.c_str(), new_name.c_str());
00127 }
00128 
00129 void moveit_warehouse::RobotStateStorage::removeRobotState(const std::string &name, const std::string &robot)
00130 {
00131   mongo_ros::Query q(STATE_NAME, name);
00132   if (!robot.empty())
00133     q.append(ROBOT_NAME, robot);
00134   unsigned int rem = state_collection_->removeMessages(q);
00135   ROS_DEBUG("Removed %u RobotState messages (named '%s')", rem, name.c_str());
00136 }


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