00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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 ®ex, 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 }