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


warehouse
Author(s): Ioan Sucan
autogenerated on Wed Jun 19 2019 19:24:47