From 66c6b11a09ba265612a45a515b07c589c82f2db5 Mon Sep 17 00:00:00 2001 From: Connor Brew Date: Tue, 25 Aug 2015 12:05:09 -0700 Subject: [PATCH] Refactored Warehouse ROS for pluginlib --- CMakeLists.txt | 60 ++-- README.md | 4 +- cmake/FindMongoDB.cmake | 67 ----- .../impl/message_collection_impl.hpp | 267 ----------------- include/mongo_ros/metadata.h | 270 ------------------ include/mongo_ros/mongo_ros.h | 73 ----- include/warehouse_ros/database_connection.h | 90 ++++++ include/warehouse_ros/database_loader.h | 83 ++++++ .../{mongo_ros => warehouse_ros}/exceptions.h | 39 ++- .../impl/database_connection_impl.hpp} | 39 +-- .../impl/message_collection_impl.hpp | 163 +++++++++++ .../impl/query_results_impl.hpp | 82 +++--- .../message_collection.h | 102 +++---- .../message_with_metadata.h | 59 ++-- include/warehouse_ros/metadata.h | 103 +++++++ .../query_results.h | 55 ++-- .../transform_collection.h | 67 +++-- package.xml | 22 +- setup.py | 12 - src/database_loader.cpp | 124 ++++++++ src/mongo_ros.cpp | 135 --------- src/mongo_ros_dummy.cpp | 50 ---- src/mongo_wrapper_ros.py | 111 ------- src/test_dbloader.cpp | 17 ++ src/transform_collection.cpp | 58 ++-- src/warehouse_ros/__init__.py | 1 - src/warehouse_ros/message_collection.py | 198 ------------- src/warehouse_ros/util.py | 47 --- test/test_mongo_helpers.h | 91 ------ test/test_mongo_ros.cpp | 157 ---------- test/test_mongo_rospy.py | 125 -------- 31 files changed, 841 insertions(+), 1930 deletions(-) delete mode 100644 cmake/FindMongoDB.cmake delete mode 100644 include/mongo_ros/impl/message_collection_impl.hpp delete mode 100644 include/mongo_ros/metadata.h delete mode 100644 include/mongo_ros/mongo_ros.h create mode 100644 include/warehouse_ros/database_connection.h create mode 100644 include/warehouse_ros/database_loader.h rename include/{mongo_ros => warehouse_ros}/exceptions.h (63%) rename include/{mongo_ros/config.h.in => warehouse_ros/impl/database_connection_impl.hpp} (61%) create mode 100644 include/warehouse_ros/impl/message_collection_impl.hpp rename include/{mongo_ros => warehouse_ros}/impl/query_results_impl.hpp (56%) rename include/{mongo_ros => warehouse_ros}/message_collection.h (60%) rename include/{mongo_ros => warehouse_ros}/message_with_metadata.h (70%) create mode 100644 include/warehouse_ros/metadata.h rename include/{mongo_ros => warehouse_ros}/query_results.h (72%) rename include/{mongo_ros => warehouse_ros}/transform_collection.h (70%) delete mode 100644 setup.py create mode 100644 src/database_loader.cpp delete mode 100644 src/mongo_ros.cpp delete mode 100644 src/mongo_ros_dummy.cpp delete mode 100755 src/mongo_wrapper_ros.py create mode 100644 src/test_dbloader.cpp delete mode 100644 src/warehouse_ros/__init__.py delete mode 100644 src/warehouse_ros/message_collection.py delete mode 100644 src/warehouse_ros/util.py delete mode 100644 test/test_mongo_helpers.h delete mode 100644 test/test_mongo_ros.cpp delete mode 100755 test/test_mongo_rospy.py diff --git a/CMakeLists.txt b/CMakeLists.txt index a797b3b..a69225c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,61 +1,37 @@ cmake_minimum_required(VERSION 2.8.3) project(warehouse_ros) -set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake/" ${CMAKE_MODULE_PATH}) - -find_package(catkin REQUIRED COMPONENTS - geometry_msgs +find_package(catkin REQUIRED roscpp rostime - std_msgs) -if (CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) -endif() -find_package(Boost REQUIRED COMPONENTS system) -find_package(OpenSSL) -find_package(MongoDB) - -set(MONGO_EXPORT) -if("${MongoDB_LIBRARIES}" MATCHES "\\.so$") - set(MONGO_EXPORT MongoDB) -endif() - -file(MAKE_DIRECTORY "${CATKIN_DEVEL_PREFIX}/include") - -catkin_python_setup() + std_msgs + geometry_msgs + pluginlib + tf +) +find_package(Boost COMPONENTS system filesystem thread) catkin_package( - INCLUDE_DIRS ${CATKIN_DEVEL_PREFIX}/include include test + INCLUDE_DIRS include LIBRARIES warehouse_ros - CATKIN_DEPENDS roscpp geometry_msgs rostime std_msgs - DEPENDS Boost ${MONGO_EXPORT} - ) - -include(cmake/FindMongoDB.cmake) + CATKIN_DEPENDS roscpp rostime std_msgs geometry_msgs + DEPENDS Boost +) -if (NOT MongoDB_EXPOSE_MACROS) - add_definitions(-DMONGO_EXPOSE_MACROS) -endif() -configure_file("include/mongo_ros/config.h.in" "${CATKIN_DEVEL_PREFIX}/include/mongo_ros/config.h") - -include_directories(${CATKIN_DEVEL_PREFIX}/include include test ${catkin_INCLUDE_DIRS} ${MongoDB_INCLUDE_DIR} ${Boost_INCLUDE_DIRS}) +include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) link_directories(${catkin_LINK_DIRS}) link_directories(${Boost_LIBRARY_DIRS}) set(warehouse_srcs - src/mongo_ros.cpp - src/mongo_ros_dummy.cpp) + src/database_loader.cpp + src/transform_collection.cpp) add_library(warehouse_ros SHARED ${warehouse_srcs}) -target_link_libraries(warehouse_ros ${catkin_LIBRARIES} ${MongoDB_LIBRARIES} ${OPENSSL_LIBRARIES} ${Boost_LIBRARIES}) -if (CATKIN_ENABLE_TESTING) - add_executable(test_mongo_roscpp test/test_mongo_ros.cpp) - target_link_libraries(test_mongo_roscpp ${catkin_LIBRARIES} ${MongoDB_LIBRARIES} ${OPENSSL_LIBRARIES} ${Boost_LIBRARIES}) - add_rostest(test/mongo_ros.test) -endif() +target_link_libraries(warehouse_ros ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +add_executable(warehouse_test_dbloader src/test_dbloader.cpp) +target_link_libraries(warehouse_test_dbloader warehouse_ros ${catkin_LIBRARIES} ${Boost_LIBRARIES}) install(TARGETS warehouse_ros LIBRARY DESTINATION lib) install(DIRECTORY include/ DESTINATION include FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp") -install(PROGRAMS src/mongo_wrapper_ros.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) -install(FILES "${CATKIN_DEVEL_PREFIX}/include/mongo_ros/config.h" DESTINATION include/mongo_ros) diff --git a/README.md b/README.md index e5a4120..1227907 100644 --- a/README.md +++ b/README.md @@ -1,3 +1 @@ -Code for persisting ROS message data using MongoDB. Contains C++ and Python libraries to serialize ROS data to MongoDB, as well as some handy scripts to record data from the command line. See http://www.ros.org/wiki/warehousewg for more. - -master branch: [![Build Status](https://travis-ci.org/ros-planning/warehouse_ros.png?branch=master)](https://travis-ci.org/ros-planning/warehouse_ros) +Abstract interface for persisting ROS message data. Implementations are loaded using pluginlib. Currently has one implementation, using MongoDB, warehouse_ros_mongo. See http://www.ros.org/wiki/warehousewg for more. diff --git a/cmake/FindMongoDB.cmake b/cmake/FindMongoDB.cmake deleted file mode 100644 index bfbea00..0000000 --- a/cmake/FindMongoDB.cmake +++ /dev/null @@ -1,67 +0,0 @@ -# - Find MongoDB; NOTE: this is specific to warehouse_ros! -# -# Find the MongoDB includes and client library -# This module defines -# MongoDB_INCLUDE_DIR, where to find mongo/client/dbclient.h -# MongoDB_LIBRARIES, the libraries needed to use MongoDB. -# MongoDB_FOUND, If false, do not try to use MongoDB. -# MongoDB_EXPOSE_MACROS, If true, mongo_ros should use '#define MONGO_EXPOSE_MACROS' - -set(MongoDB_EXPOSE_MACROS "NO") - -set(MongoDB_PossibleIncludePaths - /usr/include/ - /usr/local/include/ - /usr/include/mongo/ - /usr/local/include/mongo/ - /opt/mongo/include/ - $ENV{ProgramFiles}/Mongo/*/include - $ENV{SystemDrive}/Mongo/*/include - ) -find_path(MongoDB_INCLUDE_DIR mongo/client/dbclient.h - ${MongoDB_PossibleIncludePaths}) - -if(MongoDB_INCLUDE_DIR) - find_path(MongoDB_dbclientinterface_Path mongo/client/dbclientinterface.h - ${MongoDB_PossibleIncludePaths}) - if (MongoDB_dbclientinterface_Path) - set(MongoDB_EXPOSE_MACROS "YES") - endif() -endif() - -if(WIN32) - find_library(MongoDB_LIBRARIES NAMES mongoclient - PATHS - $ENV{ProgramFiles}/Mongo/*/lib - $ENV{SystemDrive}/Mongo/*/lib - ) -else(WIN32) - find_library(MongoDB_LIBRARIES NAMES mongoclient - PATHS - /usr/lib - /usr/lib64 - /usr/lib/mongo - /usr/lib64/mongo - /usr/local/lib - /usr/local/lib64 - /usr/local/lib/mongo - /usr/local/lib64/mongo - /opt/mongo/lib - /opt/mongo/lib64 - ) -endif(WIN32) - -if(MongoDB_INCLUDE_DIR AND MongoDB_LIBRARIES) - set(MongoDB_FOUND TRUE) - message(STATUS "Found MongoDB: ${MongoDB_INCLUDE_DIR}, ${MongoDB_LIBRARIES}") - message(STATUS "MongoDB using new interface: ${MongoDB_EXPOSE_MACROS}") -else(MongoDB_INCLUDE_DIR AND MongoDB_LIBRARIES) - set(MongoDB_FOUND FALSE) - if (MongoDB_FIND_REQUIRED) - message(FATAL_ERROR "MongoDB not found.") - else (MongoDB_FIND_REQUIRED) - message(STATUS "MongoDB not found.") - endif (MongoDB_FIND_REQUIRED) -endif(MongoDB_INCLUDE_DIR AND MongoDB_LIBRARIES) - -mark_as_advanced(MongoDB_INCLUDE_DIR MongoDB_LIBRARIES MongoDB_EXPOSE_MACROS) diff --git a/include/mongo_ros/impl/message_collection_impl.hpp b/include/mongo_ros/impl/message_collection_impl.hpp deleted file mode 100644 index eab5270..0000000 --- a/include/mongo_ros/impl/message_collection_impl.hpp +++ /dev/null @@ -1,267 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - */ - -/** - * \file - * - * Implementation of template methods of MessageCollection - * Only to be included by message_collection.h - * - * TODO: this should really be in include/impl rather than src/ - * - * \author Bhaskara Marthi - */ - -#include -#include -#include -#include -#include - -namespace mongo_ros -{ - -using std::string; -using std::vector; -namespace ser=ros::serialization; -namespace mt=ros::message_traits; - - -template -MessageCollection::MessageCollection (const string& db, - const string& coll, - const string& db_host, - const unsigned db_port, - const float timeout) : - ns_(db+"."+coll), - md5sum_matches_(true), - insertion_pub_(nh_.advertise("warehouse/"+db+"/"+coll+\ - "/inserts", 100, true)) -{ - initialize(db, coll, db_host, db_port, timeout); -} - - -template -void MessageCollection::initialize (const string& db, const string& coll, - const string& host, const unsigned port, - const float timeout) -{ - conn_ = makeDbConnection(nh_, host, port, timeout); - - gfs_.reset(new mongo::GridFS(*conn_, db)); - ROS_DEBUG_NAMED ("create_collection", "Constructed collection"); - - ensureIndex("creation_time"); - - // Add to the metatable - const string meta_ns = db+".ros_message_collections"; - if (!conn_->count(meta_ns, BSON("name" << coll))) - { - ROS_DEBUG_NAMED ("create_collection", "Inserting metadata"); - typedef typename mt::DataType DataType; - const string datatype = DataType().value(); - typedef typename mt::MD5Sum Md5; - const string md5 = Md5().value(); - conn_->insert(meta_ns, BSON("name" << coll << "type" << datatype - << "md5sum" << md5)); - } - else - { - ROS_DEBUG_NAMED("create_collection", "Not inserting metadata"); - typedef typename mt::MD5Sum Md5; - const string md5 = Md5().value(); - if (!conn_->count(meta_ns, BSON("name" << coll << "md5sum" << md5))) - { - md5sum_matches_ = false; - typedef typename mt::DataType DataType; - const string datatype = DataType().value(); - ROS_ERROR("The md5 sum for message %s changed to %s. Only reading metadata.", - datatype.c_str(), md5.c_str()); - } - } - - if (insertion_pub_.getNumSubscribers()==0) - { - ros::WallDuration d(0.1); - ROS_DEBUG_STREAM_NAMED ("create_collection", - "Waiting " << d.toSec() << - " for any additional notification subscribers"); - d.sleep(); - } -} - -template -MessageCollection& MessageCollection::ensureIndex -(const string& field) -{ - conn_->ensureIndex(ns_, BSON(field << 1)); - return *this; -} - -template -void MessageCollection::insert -(const M& msg, const Metadata& metadata) -{ - if (!md5sum_matches_) - throw Md5SumException("Cannot insert additional elements."); - - /// Get the BSON and id from the metadata - const mongo::BSONObj bson = metadata; - mongo::OID id; - bson["_id"].Val(id); - - /// Serialize the message into a buffer - const size_t serial_size = ser::serializationLength(msg); - boost::shared_array buffer(new uint8_t[serial_size]); - ser::OStream stream(buffer.get(), serial_size); - ser::serialize(stream, msg); - const char* data = (char*) buffer.get(); - - // Store in grid fs - mongo::BSONObj file_obj = gfs_->storeFile(data, serial_size, id.toString()); - - // Add blob id to metadata and store it in the message collection - mongo::BSONObjBuilder builder; - builder.appendElements(bson); - mongo::OID blob_id; - file_obj["_id"].Val(blob_id); - builder.append("blob_id", blob_id); - mongo::BSONObj entry = builder.obj(); - conn_->insert(ns_, entry); - - - // Publish ROS notification - std_msgs::String notification; - notification.data = entry.jsonString(); - insertion_pub_.publish(notification); -} - -template -typename QueryResults::range_t -MessageCollection::queryResults (const mongo::Query& query, - const bool metadata_only, - const string& sort_by, - const bool ascending) const -{ - if (!md5sum_matches_ && !metadata_only) - throw Md5SumException("Can only query metadata."); - - mongo::Query copy(query.obj); - ROS_DEBUG_NAMED("query", "Sending query %s to %s", copy.toString().c_str(), - ns_.c_str()); - - if (sort_by.size() > 0) - copy.sort(sort_by, ascending ? 1 : -1); - return typename QueryResults::range_t - (ResultIterator(conn_, ns_, copy, gfs_, metadata_only), - ResultIterator()); -} - - -template -vector ::ConstPtr> -MessageCollection::pullAllResults (const mongo::Query& query, - const bool metadata_only, - const string& sort_by, - const bool ascending) const -{ - typename QueryResults::range_t res = queryResults(query, metadata_only, - sort_by, ascending); - return vector::ConstPtr> - (res.first, res.second); -} - - -template -typename MessageWithMetadata::ConstPtr -MessageCollection::findOne (const Query& q,const bool metadata_only) const -{ - typename QueryResults::range_t res = queryResults(q, metadata_only); - if (res.first==res.second) - throw NoMatchingMessageException(ns_); - return *res.first; -} - - -template -unsigned MessageCollection::removeMessages (const mongo::Query& query) -{ - unsigned num_removed = 0; - typedef typename MessageWithMetadata::ConstPtr Msg; - vector msgs = pullAllResults(query, true); - conn_->remove(ns_, query); - - // Also remove the raw messages from gridfs - BOOST_FOREACH (Msg m, msgs) - { - mongo::OID id; - m->metadata["_id"].Val(id); - gfs_->removeFile(id.toString()); - num_removed++; - } - - return num_removed; -} - -template -void MessageCollection::modifyMetadata (const Query& q, const Metadata& m) -{ - typename MessageWithMetadata::ConstPtr orig = findOne(q, true); - - mongo::BSONObjBuilder new_meta_builder; - - std::set fields; - m.getFieldNames(fields); - - BOOST_FOREACH (const string& f, fields) - { - if ((f!="_id") && (f!="creation_time")) - new_meta_builder.append(BSON("$set" << BSON(f << m.getField(f))).\ - getField("$set")); - } - - mongo::BSONObj new_meta = new_meta_builder.obj().copy(); - conn_->update(ns_, q, new_meta); -} - -template -unsigned MessageCollection::count () -{ - return conn_->count(ns_); -} - -template -bool MessageCollection::md5SumMatches () const -{ - return md5sum_matches_; -} - -} // namespace diff --git a/include/mongo_ros/metadata.h b/include/mongo_ros/metadata.h deleted file mode 100644 index 00e6c97..0000000 --- a/include/mongo_ros/metadata.h +++ /dev/null @@ -1,270 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - */ - -/** - * \file - * - * Define a couple of classes that wrap Mongo's BSON type - * - * \author Bhaskara Marthi - */ - -#ifndef MONGO_ROS_METADATA_H -#define MONGO_ROS_METADATA_H - -// We have to include this top-level include here because -// the mongo c++ library is not robust to reincludes -#ifdef __APPLE__ -#include -#else -#include -#endif -#include -#include - - -namespace mongo_ros -{ - -using mongo::LT; -using mongo::LTE; -using mongo::GT; -using mongo::GTE; -using mongo::BSONObj; -using mongo::BSONObjBuilder; - -/// \brief Internal parent class -/// -/// This allows the user to not have to deal with separate BSONObj and -/// BSONObj builder objects -class WrappedBSON : public BSONObj -{ -public: - WrappedBSON () : - BSONObj(), builder_(new BSONObjBuilder()) - {} - - WrappedBSON (const WrappedBSON& other) : - BSONObj(), builder_(other.builder_) - { - update(); - } - - WrappedBSON (const std::string& json) : - BSONObj(), builder_(new BSONObjBuilder()) - { - builder_->appendElements(mongo::fromjson(json.c_str())); - update(); - } - -protected: - - boost::shared_ptr builder_; - - void update () - { - BSONObj::operator=(builder_->asTempObj()); - } -}; - - -/// \brief Represents a query to the db -/// -/// Usage: -/// Query q("foo", 42); -/// Query q2("bar", LT, 24); // bar less than 24 -/// Templated so you can have different types of values -/// -/// Or: -/// q = Query().append("foo", 42).append("bar", LT, 24); -class Query : public WrappedBSON -{ -public: - Query () : WrappedBSON () - {} - - Query (const Query& other) : - WrappedBSON(other) - {} - - template - Query (const std::string& name, const T& val) : - WrappedBSON () - { - append(name, val); - } - - template - Query (const std::string& name, const S& rel, const T& val) : - WrappedBSON () - { - append(name, rel, val); - } - - template - Query& append (const std::string& name, - const T& val) - { - *builder_ << name << val; - update(); - return *this; - } - - template - Query& append (const std::string& name, - const T& rel, - const S& val) - { - *builder_ << name << rel << val; - update(); - return *this; - } - - -}; - - - -/// \brief Represents metadata attached to a message. Automatically -/// includes a unique id and creation time. -/// -/// Usage: -/// -/// Metadata m("x", 24, "y", 42); -/// (templated so you can use varying number of fields, numeric or string values) -/// -/// Or: -/// m = Metadata().append("x", 24).append("name", "foo"); -class Metadata : public WrappedBSON -{ -public: - Metadata() : - WrappedBSON () - { - initialize(); - } - - Metadata(const std::string& json) : - WrappedBSON (json) - { - update(); - } - - Metadata (const Metadata& other) : - WrappedBSON(other) - { - update(); - } - - template - Metadata (const std::string& name, const T& val) : - WrappedBSON () - { - initialize(); - append(name, val); - } - - template - Metadata (const std::string& n, const T& v, - const std::string& n2, const T2& v2) : - WrappedBSON () - { - initialize(); - append(n, v); - append(n2, v2); - } - - template - Metadata (const std::string& n1, const T1& v1, - const std::string& n2, const T2& v2, - const std::string& n3, const T3& v3) : - WrappedBSON () - { - initialize(); - append(n1, v1); - append(n2, v2); - append(n3, v3); - } - - template - Metadata (const std::string& n1, const T1& v1, - const std::string& n2, const T2& v2, - const std::string& n3, const T3& v3, - const std::string& n4, const T3& v4) : - WrappedBSON () - { - initialize(); - append(n1, v1); - append(n2, v2); - append(n3, v3); - append(n4, v4); - } - - template - Metadata (const std::string& n1, const T1& v1, - const std::string& n2, const T2& v2, - const std::string& n3, const T3& v3, - const std::string& n4, const T3& v4, - const std::string& n5, const T3& v5) : - WrappedBSON () - { - initialize(); - append(n1, v1); - append(n2, v2); - append(n3, v3); - append(n4, v4); - append(n5, v5); - } - - template - Metadata& append (const std::string& name, - const T& val) - { - builder_->append(name, val); - update(); - return *this; - } - -private: - - void initialize() - { - builder_->genOID(); - builder_->append("creation_time", ros::Time::now().toSec()); - update(); - } -}; - - - - -} // namespace - -#endif // include guard diff --git a/include/mongo_ros/mongo_ros.h b/include/mongo_ros/mongo_ros.h deleted file mode 100644 index 72923cc..0000000 --- a/include/mongo_ros/mongo_ros.h +++ /dev/null @@ -1,73 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - */ - -/** - * \file - * - * Db-level operations. Most operations are in message_collection.h - * - * \author Bhaskara Marthi - */ - -#ifndef MONGO_ROS_MONGO_ROS_H -#define MONGO_ROS_MONGO_ROS_H - -#include -#include -#include - -namespace mongo_ros -{ - -boost::shared_ptr -makeDbConnection (const ros::NodeHandle& nh, const std::string& host="", - const unsigned& port=0, float timeout=300.0); - - -/// Return the ROS Message type of a given collection -std::string messageType (mongo::DBClientConnection& conn, - const std::string& db, - const std::string& coll); - -/// Drop a db and all its collections -/// Uses the connection parameters given by the warehouse_host and -/// warehouse_port ROS parameters -void dropDatabase (const std::string& db); - -/// Drop a db and its collections, given host and port parameters, -/// and a timeout. A DbClientConnection exception will be thrown if -/// we can't connect in time. -void dropDatabase (const std::string& db, const std::string& host, - unsigned port, const float timeout); - - -} // namespace - -#endif // include guard diff --git a/include/warehouse_ros/database_connection.h b/include/warehouse_ros/database_connection.h new file mode 100644 index 0000000..34b9852 --- /dev/null +++ b/include/warehouse_ros/database_connection.h @@ -0,0 +1,90 @@ +/* + * Copyright (c) 2015, Fetch Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +/** + * \file + * + * The DatabaseConnection class + * + * \author Connor Brew + */ + +#ifndef WAREHOUSE_ROS_DATABASE_CONNECTION_H +#define WAREHOUSE_ROS_DATABASE_CONNECTION_H + +#include + +namespace warehouse_ros +{ + +class DatabaseConnection +{ +public: + /// \brief Set database connection params. + virtual bool setParams(const std::string& host, unsigned port, float timeout=60.0) = 0; + + /// \brief Set database connection params. + virtual bool setTimeout(float timeout) = 0; + + /// Setup the database connection. This call assumes setParams() has been previously called. + /// Returns true if the connection was succesfully established. + virtual bool connect() = 0; + + /// Returns whether the database is connected. + virtual bool isConnected() = 0; + + /// \brief Drop a db and all its collections. + /// A DbClientConnection exception will be thrown if the database is not connected. + virtual void dropDatabase(const std::string& db_name) = 0; + + /// \brief Return the ROS Message type of a given collection + virtual std::string messageType(const std::string& db_name, const std::string& collection_name) = 0; + + /// \brief Open a collection on the DB. The collection is created if it doesn't exist. + /// A DbClientConnection exception will be thrown if the database is not connected. + template + MessageCollection openCollection(const std::string& db_name, const std::string& collection_name); + + /// \brief Open a collection on the DB. The collection is created if it doesn't exist. + /// A DbClientConnection exception will be thrown if the database is not connected. + template + typename MessageCollection::Ptr openCollectionPtr(const std::string& db_name, const std::string& collection_name); + + typedef boost::shared_ptr Ptr; +protected: + virtual MessageCollectionHelper::Ptr openCollectionHelper(const std::string& db_name, + const std::string& collection_name) = 0; +}; + +} // namespace + +#include "impl/database_connection_impl.hpp" + +#endif // include guard \ No newline at end of file diff --git a/include/warehouse_ros/database_loader.h b/include/warehouse_ros/database_loader.h new file mode 100644 index 0000000..1fa390d --- /dev/null +++ b/include/warehouse_ros/database_loader.h @@ -0,0 +1,83 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, Fetch Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Connor Brew */ + +#ifndef WAREHOUSE_ROS_DATABASE_LOADER_ +#define WAREHOUSE_ROS_LOADER_ + +#include +#include + +namespace warehouse_ros +{ + +class DBConnectionStub : public DatabaseConnection { +public: + bool setParams(const std::string& host, unsigned port, float timeout) {return false;} + bool setTimeout(float timeout) {return false;} + bool connect() {return false;} + bool isConnected() {return false;} + void dropDatabase(const std::string& db_name) {throw warehouse_ros::DbConnectException("Database is stub");} + std::string messageType(const std::string& db_name, const std::string& collection_name) {throw warehouse_ros::DbConnectException("Database is stub");} +protected: + typename MessageCollectionHelper::Ptr openCollectionHelper(const std::string& db_name, + const std::string& collection_name) {}; +}; + +/** \brief This class provides the mechanism to connect to a database and reads needed ROS parameters when appropriate. */ +class DatabaseLoader +{ +public: + /// \brief Takes a warehouse_ros DatabaseConnection. The DatabaseConnection is expected to have already been initialized. + DatabaseLoader(); + + ~DatabaseLoader(); + + /// \brief Initialize the DatabaseLoader + void initialize(); + + /** \brief Load a database connection using pluginlib + Looks for ROS params specifying which plugin/host/port to use. NodeHandle::searchParam() + is used starting from ~ to look for warehouse_plugin, warehouse_host and warehouse_port. */ + typename DatabaseConnection::Ptr loadDatabase(); + +private: + ros::NodeHandle nh_; + boost::scoped_ptr > db_plugin_loader_; +}; + +} + +#endif diff --git a/include/mongo_ros/exceptions.h b/include/warehouse_ros/exceptions.h similarity index 63% rename from include/mongo_ros/exceptions.h rename to include/warehouse_ros/exceptions.h index 17b91c4..e1147e6 100644 --- a/include/mongo_ros/exceptions.h +++ b/include/warehouse_ros/exceptions.h @@ -31,55 +31,52 @@ /** * \file * - * Exceptions thrown by mongo_ros + * Exceptions thrown by warehouse_ros * * \author Bhaskara Marthi */ -#ifndef MONGO_ROS_EXCEPTIONS_H -#define MONGO_ROS_EXCEPTIONS_H +#ifndef WAREHOUSE_ROS_EXCEPTIONS_H +#define WAREHOUSE_ROS_EXCEPTIONS_H #include #include #include -namespace mongo_ros +namespace warehouse_ros { using boost::format; -using std::string; -/// A base class for all pose_graph exceptions; provides a handy boost::format parent constructor -class MongoRosException: public std::runtime_error +/// A base class for all warehouse_ros exceptions; provides a handy boost::format parent constructor +class WarehouseRosException: public std::runtime_error { public: - MongoRosException (const format& error_string) : std::runtime_error(error_string.str()) {}; - MongoRosException (const char* str) : std::runtime_error(str) {}; + WarehouseRosException(const format& error_string) : std::runtime_error(error_string.str()) {}; + WarehouseRosException(const char* str) : std::runtime_error(str) {}; }; - /// \brief Couldn't find matching message in collection -struct NoMatchingMessageException: public MongoRosException +struct NoMatchingMessageException: public WarehouseRosException { - NoMatchingMessageException (const string& coll) : - MongoRosException (format ("Couldn't find message in %1% matching query") % coll) {} + NoMatchingMessageException(const std::string& coll) : + WarehouseRosException(format("Couldn't find message in %1% matching query") % coll) {} }; -/// \brief Couldn't find matching message in collection -struct DbConnectException: public MongoRosException +/// \brief Couldn't connect to database +struct DbConnectException: public WarehouseRosException { - DbConnectException () : - MongoRosException ("Couldn't connect to MongoDB instance") {} + DbConnectException(const std::string& failure) : + WarehouseRosException(format("Not connected to the database. %1%") % failure) {} }; /// \brief Different md5 sum for messages -struct Md5SumException: public MongoRosException +struct Md5SumException: public WarehouseRosException { - Md5SumException (const string& failure) : - MongoRosException (format("The md5 sum for the ROS messages saved in the database differs from that of the compiled message. %1%") % failure) {} + Md5SumException(const std::string& failure) : + WarehouseRosException(format("The md5 sum for the ROS messages saved in the database differs from that of the compiled message. %1%") % failure) {} }; - } // namespace #endif // include guard diff --git a/include/mongo_ros/config.h.in b/include/warehouse_ros/impl/database_connection_impl.hpp similarity index 61% rename from include/mongo_ros/config.h.in rename to include/warehouse_ros/impl/database_connection_impl.hpp index 37d9de5..e95a399 100644 --- a/include/mongo_ros/config.h.in +++ b/include/warehouse_ros/impl/database_connection_impl.hpp @@ -1,5 +1,5 @@ /* - * Copyright (c) 2013, Willow Garage, Inc. + * Copyright (c) 2015, Fetch Robotics * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -31,24 +31,31 @@ /** * \file * - * Mongo has different magic on how it deals with macros starting with Ubuntu Raring - * so we need to be careful on how we include the headers we need + * Implementation of template methods of DatabaseConnection + * Only to be included by database_connection.h * - * \author Ioan Sucan + * \author Connor Brew */ -#ifndef MONGO_ROS_CONFIG_H -#define MONGO_ROS_CONFIG_H +namespace warehouse_ros +{ -#cmakedefine01 MongoDB_EXPOSE_MACROS +template +MessageCollection DatabaseConnection::openCollection(const std::string& db_name, + const std::string& collection_name) +{ + if (!isConnected()) + throw DbConnectException("Cannot open collection."); + return MessageCollection(openCollectionHelper(db_name, collection_name)); +} -#if MongoDB_EXPOSE_MACROS -# define MONGO_EXPOSE_MACROS -# include -# include -# undef MONGO_EXPOSE_MACROS -#else -# include -#endif +template +typename MessageCollection::Ptr DatabaseConnection::openCollectionPtr(const std::string& db_name, + const std::string& collection_name) +{ + if (!isConnected()) + throw DbConnectException("Cannot open collection."); + return typename MessageCollection::Ptr(new MessageCollection(openCollectionHelper(db_name, collection_name))); +} -#endif +} // namespace \ No newline at end of file diff --git a/include/warehouse_ros/impl/message_collection_impl.hpp b/include/warehouse_ros/impl/message_collection_impl.hpp new file mode 100644 index 0000000..e2af86e --- /dev/null +++ b/include/warehouse_ros/impl/message_collection_impl.hpp @@ -0,0 +1,163 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +/** + * \file + * + * Implementation of template methods of MessageCollection + * Only to be included by message_collection.h + * + * \author Bhaskara Marthi + */ + +namespace warehouse_ros +{ + +template +MessageCollection::MessageCollection(MessageCollectionHelper::Ptr collection) : + collection_(collection) +{ + typedef typename ros::message_traits::DataType DataType; + const std::string datatype = DataType().value(); + typedef typename ros::message_traits::MD5Sum Md5; + const std::string md5 = Md5().value(); + md5sum_matches_ = collection_->initialize(datatype, md5); +} + +template +MessageCollection::MessageCollection(const MessageCollection& other) : + collection_(other.collection_), md5sum_matches_(other.md5sum_matches_) +{ +} + +template +MessageCollection::~MessageCollection() +{ +} + +template +MessageCollection& MessageCollection::operator=(const MessageCollection& other) +{ + collection_ = other.collection_; + md5sum_matches_ = other.md5sum_matches_; + return *this; +} + +template +void MessageCollection::insert(const M& msg, Metadata::Ptr metadata) +{ + if (!md5sum_matches_) + throw Md5SumException("Cannot insert additional elements."); + + metadata->append("creation_time", ros::Time::now().toSec()); + + /// Serialize the message into a buffer + size_t serial_size = ros::serialization::serializationLength(msg); + boost::shared_array buffer(new uint8_t[serial_size]); + ros::serialization::OStream stream(buffer.get(), serial_size); + ros::serialization::serialize(stream, msg); + char* data = (char*) buffer.get(); + + collection_->insert(data, serial_size, metadata); +} + +template +typename QueryResults::range_t +MessageCollection::query(Query::ConstPtr query, + bool metadata_only, + const std::string& sort_by, + bool ascending) const +{ + if (!md5sum_matches_ && !metadata_only) + throw Md5SumException("Can only query metadata."); + + ResultIteratorHelper::Ptr results = collection_->query(query, sort_by, ascending); + return typename QueryResults::range_t + (ResultIterator(results, metadata_only), ResultIterator()); +} + +template +std::vector::ConstPtr> +MessageCollection::queryList(Query::ConstPtr query, + bool metadata_only, + const std::string& sort_by, + bool ascending) const +{ + typename QueryResults::range_t res = this->query(query, metadata_only, sort_by, ascending); + return std::vector::ConstPtr>(res.first, res.second); +} + +template +typename MessageWithMetadata::ConstPtr +MessageCollection::findOne(Query::ConstPtr query, const bool metadata_only) const +{ + typename QueryResults::range_t res = this->query(query, metadata_only); + if (res.first==res.second) + throw NoMatchingMessageException(collection_->collectionName()); + return *res.first; +} + +template +unsigned MessageCollection::removeMessages(Query::ConstPtr query) +{ + return collection_->removeMessages(query); +} + +template +void MessageCollection::modifyMetadata(Query::ConstPtr q, Metadata::ConstPtr m) +{ + collection_->modifyMetadata(q, m); +} + +template +unsigned MessageCollection::count() +{ + return collection_->count(); +} + +template +bool MessageCollection::md5SumMatches() const +{ + return md5sum_matches_; +} + +template +Query::Ptr MessageCollection::createQuery() const +{ + return collection_->createQuery(); +} + +template +Metadata::Ptr MessageCollection::createMetadata() const +{ + return collection_->createMetadata(); +} + +} // namespace diff --git a/include/mongo_ros/impl/query_results_impl.hpp b/include/warehouse_ros/impl/query_results_impl.hpp similarity index 56% rename from include/mongo_ros/impl/query_results_impl.hpp rename to include/warehouse_ros/impl/query_results_impl.hpp index 6cea2eb..bcf68ef 100644 --- a/include/mongo_ros/impl/query_results_impl.hpp +++ b/include/warehouse_ros/impl/query_results_impl.hpp @@ -38,85 +38,73 @@ */ -namespace mongo_ros +namespace warehouse_ros { -using std::vector; -using std::string; +template +ResultIterator::ResultIterator(ResultIteratorHelper::Ptr results, bool metadata_only) : + results_(results), metadata_only_(metadata_only) +{ + if (!results_->hasData()) + results_.reset(); +} template -ResultIterator::ResultIterator -(boost::shared_ptr conn, - const string& ns, const mongo::Query& query, - boost::shared_ptr gfs, - const bool metadata_only) : - metadata_only_(metadata_only), - cursor_(new Cursor(conn->query(ns, query))), - gfs_(gfs) +ResultIterator::ResultIterator(const ResultIterator& other) : + results_(other.results_), metadata_only_(other.metadata_only_) { - if ((*cursor_)->more()) - next_ = (*cursor_)->nextSafe(); } template -ResultIterator::ResultIterator () : +ResultIterator::ResultIterator() : metadata_only_(false) { } template -ResultIterator::ResultIterator (const ResultIterator& res) : - metadata_only_(res.metadata_only_), cursor_(res.cursor_), - next_(res.next_), gfs_(res.gfs_) +ResultIterator::~ResultIterator() { } template -void ResultIterator::increment () +ResultIterator& ResultIterator::operator=(const ResultIterator& other) { - ROS_ASSERT (next_); - if ((*cursor_)->more()) - next_ = (*cursor_)->nextSafe(); - else - next_.reset(); + results_ = other.results_; + metadata_only_ = other.metadata_only_; + return *this; +} + +template +void ResultIterator::increment() +{ + if (!results_->next()) + { + results_.reset(); + } } template typename MessageWithMetadata::ConstPtr -ResultIterator::dereference () const +ResultIterator::dereference() const { - ROS_ASSERT (next_); - // Get the raw message if necessary, else - // use a default constructed one - typename MessageWithMetadata::Ptr m(new MessageWithMetadata(next_->copy())); + ROS_ASSERT(results_); + + typename MessageWithMetadata::Ptr msg(new MessageWithMetadata(results_->metadata())); if (!metadata_only_) { - mongo::OID blob_id; - (*next_)["blob_id"].Val(blob_id); - mongo::BSONObj q = BSON ("_id" << blob_id); - mongo::GridFile f = gfs_->findFile(q); - ROS_ASSERT(f.exists()); - std::stringstream ss (std::ios_base::out); - f.write(ss); - std::string str = ss.str(); - + std::string str = results_->message(); uint8_t* buf = (uint8_t*) str.c_str(); ros::serialization::IStream istream(buf, str.size()); - ros::serialization::Serializer::read(istream, *m); + ros::serialization::Serializer::read(istream, *msg); } - - return m; + return msg; } template -bool ResultIterator::equal (const ResultIterator& other) const +bool ResultIterator::equal(const ResultIterator& other) const { - // Incomplete; the only case we care about is whether we're at the end yet - if (next_ && other.next_) - ROS_WARN ("Unexpected case of equality check of two not-past-the-end " - "iterators in ResultIterator"); - return (!next_ && !other.next_); + // Incomplete, the only case we care about is whether iter is at the end + return (!results_ && !other.results_); } - } // namespace diff --git a/include/mongo_ros/message_collection.h b/include/warehouse_ros/message_collection.h similarity index 60% rename from include/mongo_ros/message_collection.h rename to include/warehouse_ros/message_collection.h index 93932e3..1b7c559 100644 --- a/include/mongo_ros/message_collection.h +++ b/include/warehouse_ros/message_collection.h @@ -36,104 +36,106 @@ * \author Bhaskara Marthi */ -#ifndef MONGO_ROS_MESSAGE_COLLECTION_H -#define MONGO_ROS_MESSAGE_COLLECTION_H +#ifndef WAREHOUSE_ROS_MESSAGE_COLLECTION_H +#define WAREHOUSE_ROS_MESSAGE_COLLECTION_H -#include -#include +#include -namespace mongo_ros +namespace warehouse_ros { -/// Represents a collection of ROS Messages stored in a MongoDB database. +class MessageCollectionHelper +{ +public: + virtual bool initialize(const std::string& datatype, const std::string& md5) = 0; + virtual void insert(char* msg, size_t msg_size, Metadata::ConstPtr metadata) = 0; + virtual ResultIteratorHelper::Ptr query(Query::ConstPtr query, + const std::string& sort_by = "", + bool ascending=true) const = 0; + virtual unsigned removeMessages(Query::ConstPtr query) = 0; + virtual void modifyMetadata(Query::ConstPtr q, Metadata::ConstPtr m) = 0; + virtual unsigned count() = 0; + virtual Query::Ptr createQuery() const = 0; + virtual Metadata::Ptr createMetadata() const = 0; + virtual std::string collectionName() const = 0; + + typedef boost::shared_ptr Ptr; +}; + +/// Represents a collection of ROS Messages stored in a database. /// Each stored message in the db has a unique id, a creation time, and /// optional additional metadata stored as a dictionary template class MessageCollection { public: - /// \brief Will connect to given database and collection. Collection is /// created if it doesn't exist. - /// \param db_host If provided, will be used instead of looking up the - /// warehouse_host ros parameter. - /// \param db_port If provided, will be used instead of looking up the - /// warehouse_port ros parameter. - /// \param timeout Throw a DbConnectException if can't connect within - /// this many seconds - MessageCollection (const std::string& db_name, - const std::string& collection_name, - const std::string& db_host="", - unsigned db_port=0, - float timeout=300.0); + MessageCollection(MessageCollectionHelper::Ptr collection); + + /// \brief Copy constructor + MessageCollection(const MessageCollection& rhs); + + /// \brief Destructor + ~MessageCollection(); + + MessageCollection& operator=(const MessageCollection& other); /// \brief Insert a ROS message, together with some optional metadata, /// into the db /// \throws mongo::DBException if unable to insert /// \param metadata Metadata to insert. Note that a unique id field /// _id and a field creation_time will be autogenerated for all messages. - /// - /// As a secondary effect, publishes a notification message on the topic - /// warehouse/db/collection/inserts - void insert (const M& msg, const Metadata& metadata = Metadata()); + void insert(const M& msg, Metadata::Ptr metadata); /// \retval Iterator range over matching messages - /// \param query A metadata object representing a query. + /// \param query A query object /// \param metadata_only If this is true, only retrieve the metadata /// (returned message objects will just be default constructed) typename QueryResults::range_t - queryResults (const mongo::Query& query, bool metadata_only = false, - const std::string& sort_by = "", bool ascending=true) const; + query(Query::ConstPtr query, bool metadata_only = false, + const std::string& sort_by = "", bool ascending=true) const; /// \brief Convenience function that calls queryResult and /// puts the results into a vector - std::vector::ConstPtr > - pullAllResults (const mongo::Query& query, bool metadata_only = false, - const std::string& sort_by = "", bool ascending=true) const; + std::vector::ConstPtr> + queryList(Query::ConstPtr query, bool metadata_only = false, + const std::string& sort_by = "", bool ascending=true) const; - /// \brief Convenience function that returns a single matching result /// for the query /// \throws NoMatchingMessageException typename MessageWithMetadata::ConstPtr - findOne (const Query& query, bool metadata_only = false) const; + findOne(Query::ConstPtr query, bool metadata_only = false) const; /// \brief Remove messages matching query - unsigned removeMessages (const mongo::Query& query); - - /// \post Ensure that there's an index on the given field. - /// Note that index on _id and creation_time are always created. - MessageCollection& ensureIndex (const std::string& field); + unsigned removeMessages(Query::ConstPtr query); /// \brief Modify metadata /// Find message matching \a q and update its metadata using \a m /// In other words, overwrite keys in the message using \a m, but /// keep keys that don't occur in \a m. - void modifyMetadata (const Query& q, const Metadata& m); + void modifyMetadata(Query::ConstPtr q, Metadata::ConstPtr m); /// \brief Count messages in collection - unsigned count (); + unsigned count(); /// \brief Check if the md5 sum of the messages previously stored in /// the database matches that of the compiled message - bool md5SumMatches () const; - -private: + bool md5SumMatches() const; - // Called by constructors - void initialize (const std::string& db, const std::string& coll, - const std::string& host, unsigned port, - float timeout); + Query::Ptr createQuery() const; - const std::string ns_; - boost::shared_ptr conn_; - boost::shared_ptr gfs_; + Metadata::Ptr createMetadata() const; + + typedef boost::shared_ptr > Ptr; + typedef M message_type; + +private: + MessageCollectionHelper::Ptr collection_; bool md5sum_matches_; - ros::NodeHandle nh_; - ros::Publisher insertion_pub_; }; - } // namespace #include "impl/message_collection_impl.hpp" diff --git a/include/mongo_ros/message_with_metadata.h b/include/warehouse_ros/message_with_metadata.h similarity index 70% rename from include/mongo_ros/message_with_metadata.h rename to include/warehouse_ros/message_with_metadata.h index 93a6652..6650a9d 100644 --- a/include/mongo_ros/message_with_metadata.h +++ b/include/warehouse_ros/message_with_metadata.h @@ -37,70 +37,71 @@ * \author Bhaskara Marthi */ -#ifndef MONGO_ROS_MESSAGE_WITH_METADATA_H -#define MONGO_ROS_MESSAGE_WITH_METADATA_H +#ifndef WAREHOUSE_ROS_MESSAGE_WITH_METADATA_H +#define WAREHOUSE_ROS_MESSAGE_WITH_METADATA_H -#include +#include -namespace mongo_ros +namespace warehouse_ros { - /************************************************************ * MessageWithMetadata ***********************************************************/ - /// \brief Class that wraps (via inheritance) a ROS message type, together /// with additional metadata (a yaml dictionary) /// \tparam M the message type being wrapped template struct MessageWithMetadata : public M { - MessageWithMetadata (const Metadata& metadata, - const M& msg = M()) : - M(msg), metadata(metadata) - { - } - - MessageWithMetadata (const mongo::BSONObj& metadata, +public: + MessageWithMetadata(Metadata::ConstPtr metadata, const M& msg = M()) : - M(msg), metadata(metadata) + M(msg), metadata_(metadata) {} - MessageWithMetadata (const MessageWithMetadata& m) : - M(m), metadata(m.metadata) + MessageWithMetadata(const MessageWithMetadata& m) : + M(m), metadata_(m.metadata_) {} - MessageWithMetadata () + MessageWithMetadata() {} - mongo::BSONObj metadata; + Metadata::ConstPtr metadata_; + + std::string lookupString(const std::string& name) const + { + return metadata_->lookupString(name); + } + + double lookupDouble(const std::string& name) const + { + return metadata_->lookupDouble(name); + } - std::string lookupString (const std::string& name) const + int lookupInt(const std::string& name) const { - return metadata.getStringField(name.c_str()); + return metadata_->lookupInt(name); } - double lookupDouble (const std::string& name) const + bool lookupBool(const std::string& name) const { - double d; - metadata[name.c_str()].Val(d); - return d; + return metadata_->lookupBool(name); } - int lookupInt (const std::string& name) const + bool lookupField(const std::string& name) const { - return metadata.getIntField(name.c_str()); + return metadata_->lookupField(name); } - bool lookupBool (const std::string& name) const + std::set lookupFieldNames() const { - return metadata.getBoolField(name.c_str()); + return metadata_->lookupFieldNames(); } typedef boost::shared_ptr > Ptr; - typedef boost::shared_ptr const> ConstPtr; + typedef boost::shared_ptr > ConstPtr; }; diff --git a/include/warehouse_ros/metadata.h b/include/warehouse_ros/metadata.h new file mode 100644 index 0000000..0185315 --- /dev/null +++ b/include/warehouse_ros/metadata.h @@ -0,0 +1,103 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +/** + * \file + * + * Define a couple of classes for wrapping queries and metadata + * + * \author Bhaskara Marthi + */ + +#ifndef WAREHOUSE_ROS_METADATA_H +#define WAREHOUSE_ROS_METADATA_H + +#include +#include +#include + +namespace warehouse_ros +{ + +/// \brief Represents a query to the db +/// +/// Usage: +/// q = Query().append("foo", 42).appendLT("bar", 24); +class Query +{ +public: + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + virtual ~Query() {} + virtual void append(const std::string& name, const std::string& val) = 0; + virtual void append(const std::string& name, const double val) = 0; + virtual void append(const std::string& name, const int val) = 0; + virtual void append(const std::string& name, const bool val) = 0; + virtual void appendLT(const std::string& name, const double val) = 0; + virtual void appendLT(const std::string& name, const int val) = 0; + virtual void appendLTE(const std::string& name, const double val) = 0; + virtual void appendLTE(const std::string& name, const int val) = 0; + virtual void appendGT(const std::string& name, const double val) = 0; + virtual void appendGT(const std::string& name, const int val) = 0; + virtual void appendGTE(const std::string& name, const double val) = 0; + virtual void appendGTE(const std::string& name, const int val) = 0; + virtual void appendRange(const std::string& name, const double lower, const double upper) = 0; + virtual void appendRange(const std::string& name, const int lower, const int upper) = 0; + virtual void appendRangeInclusive(const std::string& name, const double lower, const double upper) = 0; + virtual void appendRangeInclusive(const std::string& name, const int lower, const int upper) = 0; +}; + +/// \brief Represents metadata attached to a message. +/// +/// Usage: +/// m = Metadata().append("x", 24).append("name", "foo"); +class Metadata +{ +public: + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + virtual ~Metadata() {} + virtual void append(const std::string& name, const std::string& val) = 0; + virtual void append(const std::string& name, const double val) = 0; + virtual void append(const std::string& name, const int val) = 0; + virtual void append(const std::string& name, const bool val) = 0; + virtual std::string lookupString(const std::string& name) const = 0; + virtual double lookupDouble(const std::string& name) const = 0; + virtual int lookupInt(const std::string& name) const = 0; + virtual bool lookupBool(const std::string& name) const = 0; + virtual bool lookupField(const std::string& name) const = 0; + virtual std::set lookupFieldNames() const = 0; +}; + +} // namespace + +#endif // include guard diff --git a/include/mongo_ros/query_results.h b/include/warehouse_ros/query_results.h similarity index 72% rename from include/mongo_ros/query_results.h rename to include/warehouse_ros/query_results.h index 22f1c01..3ae73b8 100644 --- a/include/mongo_ros/query_results.h +++ b/include/warehouse_ros/query_results.h @@ -36,19 +36,26 @@ * \author Bhaskara Marthi */ -#ifndef MONGO_ROS_QUERY_RESULTS_H -#define MONGO_ROS_QUERY_RESULTS_H +#ifndef WAREHOUSE_ROS_QUERY_RESULTS_H +#define WAREHOUSE_ROS_QUERY_RESULTS_H -#include -#include +#include +#include +#include -namespace mongo_ros +namespace warehouse_ros { -// To avoid some const-correctness issues we wrap Mongo's returned auto_ptr in -// another pointer -typedef std::auto_ptr Cursor; -typedef boost::shared_ptr CursorPtr; +class ResultIteratorHelper +{ +public: + virtual bool next() = 0; + virtual bool hasData() const = 0; + virtual Metadata::ConstPtr metadata() const = 0; + virtual std::string message() const = 0; + + typedef boost::shared_ptr Ptr; +}; template class ResultIterator : @@ -58,44 +65,38 @@ class ResultIterator : typename MessageWithMetadata::ConstPtr > { public: - /// \brief Constructor - ResultIterator (boost::shared_ptr conn, - const std::string& ns, - const mongo::Query& query, - boost::shared_ptr gfs, - bool metadata_only); + ResultIterator(ResultIteratorHelper::Ptr results, bool metadata_only); /// \brief Copy constructor - ResultIterator (const ResultIterator& rhs); + ResultIterator(const ResultIterator& rhs); /// \brief Constructor for past_the_end iterator - ResultIterator (); + ResultIterator(); -private: + /// \brief Destructor + ~ResultIterator(); + + ResultIterator& operator=(const ResultIterator& other); +private: friend class boost::iterator_core_access; // Member functions needed to be an iterator - void increment (); - typename MessageWithMetadata::ConstPtr dereference () const; - bool equal (const ResultIterator& other) const; + void increment(); + typename MessageWithMetadata::ConstPtr dereference() const; + bool equal(const ResultIterator& other) const; + ResultIteratorHelper::Ptr results_; const bool metadata_only_; - CursorPtr cursor_; - boost::optional next_; - boost::shared_ptr gfs_; }; -/// A templated typedef for convenience template struct QueryResults { typedef std::pair, ResultIterator > range_t; }; - - } // namespace #include "impl/query_results_impl.hpp" diff --git a/include/mongo_ros/transform_collection.h b/include/warehouse_ros/transform_collection.h similarity index 70% rename from include/mongo_ros/transform_collection.h rename to include/warehouse_ros/transform_collection.h index 621b256..4dce9ad 100644 --- a/include/mongo_ros/transform_collection.h +++ b/include/warehouse_ros/transform_collection.h @@ -36,14 +36,14 @@ * \author Bhaskara Marthi */ -#ifndef MONGO_ROS_TF_COLLECTION_H -#define MONGO_ROS_TF_COLLECTION_H +#ifndef WAREHOUSE_ROS_TF_COLLECTION_H +#define WAREHOUSE_ROS_TF_COLLECTION_H -#include +#include #include #include -namespace mongo_ros +namespace warehouse_ros { /// This abstract base class just makes it easier to write code that works for @@ -51,12 +51,11 @@ namespace mongo_ros class TransformSource { public: - /// Get the transform between two frames at a given timepoint. Can throw /// all the usual tf exceptions if the transform is unavailable. - virtual tf::StampedTransform lookupTransform (const std::string& target_frame, - const std::string& source_frame, - double t) const = 0; + virtual tf::StampedTransform lookupTransform(const std::string& target_frame, + const std::string& source_frame, + double t) const = 0; }; /// The setup is that you have a db containing a collection with tf messages, @@ -69,56 +68,54 @@ class TransformSource class TransformCollection : public TransformSource { public: - TransformCollection (const std::string& db, - const std::string& coll="tf", - const std::string& host="localhost", - const unsigned port=27017, - const double history_length=10.0) : + TransformCollection(MessageCollection &coll, + const double search_back=10.0, + const double search_forward=1.0) : TransformSource(), - coll_(db, coll, host, port), history_length_(history_length) + coll_(coll), search_back_(search_back), search_forward_(search_forward) {} /// Get the transform between two frames at a given timepoint. Can throw /// all the exceptions tf::lookupTransform can. - virtual - tf::StampedTransform lookupTransform (const std::string& target_frame, - const std::string& source_frame, - double t) const; - + virtual tf::StampedTransform lookupTransform(const std::string& target_frame, + const std::string& source_frame, + double t) const; -private: - - /// Return a query that returns transforms relevant to query at time t - mongo::Query transformQuery (double t) const; + /// Put the transform into the collection. + void putTransform(tf::StampedTransform); +private: MessageCollection coll_; - double history_length_; - + double search_back_; + double search_forward_; }; - /// This wraps a tf transform listener so it can be used interchangeably /// with a TransformCollection. class LiveTransformSource : public TransformSource { public: - /// \param timeout: Maximum timeout /// /// ros::init must be called before creating an instance - LiveTransformSource (double timeout = 0) : + LiveTransformSource(double timeout = 0) : TransformSource(), tf_(new tf::TransformListener()), timeout_(timeout) {} - + /// Will return the transform if it becomes available before the timeout /// expires, else throw a tf exception - virtual - tf::StampedTransform lookupTransform (const std::string& target, - const std::string& source, - double t) const; - -private: + virtual tf::StampedTransform lookupTransform(const std::string& target, + const std::string& source, + double t) const + { + ros::Time tm(t); + tf_->waitForTransform(target, source, tm, ros::Duration(timeout_)); + tf::StampedTransform trans; + tf_->lookupTransform(target, source, tm, trans); // Can throw + return trans; + } +private: ros::NodeHandle nh_; boost::shared_ptr tf_; ros::Duration timeout_; diff --git a/package.xml b/package.xml index 0dd38b6..6cb4dc6 100644 --- a/package.xml +++ b/package.xml @@ -2,9 +2,10 @@ warehouse_ros 0.8.8 - Persistent storage of ROS data using MongoDB + Persistent storage of ROS messages Bhaskara Marthi + Connor Brew Ioan Sucan BSD @@ -12,28 +13,21 @@ catkin - boost - rospy roscpp - geometry_msgs rostime std_msgs - mongodb-dev - python-pymongo - curl - libssl-dev + geometry_msgs + pluginlib + tf boost roscpp rostime - geometry_msgs std_msgs - mongodb-dev - python-pymongo - curl - libssl-dev + geometry_msgs + pluginlib + tf gtest rostest - diff --git a/setup.py b/setup.py deleted file mode 100644 index 577ad7f..0000000 --- a/setup.py +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['warehouse_ros'], - package_dir={'': 'src'}, - requires=['roslib', 'rospy'] -) - -setup(**d) diff --git a/src/database_loader.cpp b/src/database_loader.cpp new file mode 100644 index 0000000..c31bf81 --- /dev/null +++ b/src/database_loader.cpp @@ -0,0 +1,124 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, Fetch Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Connor Brew */ + +#include + +namespace warehouse_ros +{ + +using std::string; + +DatabaseLoader::DatabaseLoader() : + nh_("~") +{ + initialize(); +} + +DatabaseLoader::~DatabaseLoader() +{ +} + +void DatabaseLoader::initialize() +{ + // Create the plugin loader. + try + { + db_plugin_loader_.reset(new pluginlib::ClassLoader("warehouse_ros", "warehouse_ros::DatabaseConnection")); + } + catch(pluginlib::PluginlibException& ex) + { + ROS_FATAL_STREAM("Exception while creating database_connection plugin loader " << ex.what()); + } +} + +typename DatabaseConnection::Ptr DatabaseLoader::loadDatabase() +{ + if (!db_plugin_loader_) + { + return typename DatabaseConnection::Ptr(new DBConnectionStub()); + } + + // Search for the warehouse_plugin parameter in the local namespace of the node, and up the tree of namespaces. + // If the desired param is not found, make a final attempt to look for the param in the default namespace + string paramName; + if (!nh_.searchParam("warehouse_plugin", paramName)) + paramName = "warehouse_plugin"; + string db_plugin; + if (!nh_.getParamCached(paramName, db_plugin)) + { + ROS_ERROR("Could not find parameter for database plugin name"); + return typename DatabaseConnection::Ptr(new DBConnectionStub()); + } + + DatabaseConnection::Ptr db; + try + { + db.reset(db_plugin_loader_->createUnmanagedInstance(db_plugin)); + } + catch(pluginlib::PluginlibException& ex) + { + ROS_ERROR_STREAM("Exception while loading database plugin '" << db_plugin << "': " << ex.what() << std::endl); + return typename DatabaseConnection::Ptr(new DBConnectionStub()); + } + + bool hostFound = false; + bool portFound = false; + + if (!nh_.searchParam("warehouse_host", paramName)) + paramName = "warehouse_host"; + std::string host; + if (nh_.getParamCached(paramName, host)) + { + hostFound = true; + } + + if (!nh_.searchParam("warehouse_port", paramName)) + paramName = "warehouse_port"; + int port; + if (nh_.getParamCached(paramName, port)) + { + portFound = true; + } + + if (hostFound && portFound) + { + db->setParams(host, port); + } + + return db; +} + +} diff --git a/src/mongo_ros.cpp b/src/mongo_ros.cpp deleted file mode 100644 index 2dd4827..0000000 --- a/src/mongo_ros.cpp +++ /dev/null @@ -1,135 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - */ - -/** - * \file - * - * Implementation of mongo_ros.h - * - * \author Bhaskara Marthi - */ - -#include -#include -#include - -namespace mongo_ros -{ - -using std::string; - -/// Get a parameter, with default values -template -P getParam (const ros::NodeHandle& nh, const string& name, const P& default_val) -{ - P val; - nh.param(name, val, default_val); - ROS_DEBUG_STREAM_NAMED ("init", "Initialized " << name << " to " << val << - " (default was " << default_val << ")"); - return val; -} - -string getHost (ros::NodeHandle nh, const string& host="") -{ - const string db_host = - (host=="") ? - getParam(nh, "warehouse_host", "localhost") : - host; - return db_host; -} - -int getPort (ros::NodeHandle nh, const int port=0) -{ - const int db_port = - (port==0) ? - getParam(nh, "warehouse_port", 27017) : - port; - return db_port; -} - -boost::shared_ptr -makeDbConnection (const ros::NodeHandle& nh, const string& host, - const unsigned& port, const float timeout) -{ - // The defaults should match the ones used by mongodb/wrapper.py - const string db_host = getHost(nh, host); - const int db_port = getPort(nh, port); - - const string db_address = (boost::format("%1%:%2%") % db_host % db_port).str(); - boost::shared_ptr conn; - - const ros::WallTime end = ros::WallTime::now() + ros::WallDuration(timeout); - - while (ros::ok() && ros::WallTime::now()connect(db_address); - if (!conn->isFailed()) - break; - } - catch (mongo::ConnectException& e) - { - ros::Duration(1.0).sleep(); - } - } - if (conn->isFailed() || ros::WallTime::now()>end) - throw DbConnectException(); - - ROS_DEBUG_STREAM_NAMED("init", "Successfully connected to db"); - return conn; -} - -void dropDatabase (const string& db_name) -{ - dropDatabase(db_name, "", 0, 60.0); -} - -void dropDatabase (const string& db, const string& host, const unsigned port, - const float timeout) -{ - ros::NodeHandle nh; - boost::shared_ptr c = - makeDbConnection(nh, host, port, timeout); - c->dropDatabase(db); -} - -string messageType (mongo::DBClientConnection& conn, - const string& db, const string& coll) -{ - const string ns = db+".ros_message_collections"; - std::auto_ptr cursor = conn.query(ns, BSON("name" << coll)); - mongo::BSONObj obj = cursor->next(); - return obj.getStringField("type"); -} - -} // namespace diff --git a/src/mongo_ros_dummy.cpp b/src/mongo_ros_dummy.cpp deleted file mode 100644 index 857d1ff..0000000 --- a/src/mongo_ros_dummy.cpp +++ /dev/null @@ -1,50 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - */ - -#include - -// add this dummy function so the .so file copies everything we need from the -// libmongoclient.a file at link time. We need this because Ubuntu does not install -// a .so file for libmongoclient and the wrappers we have in this lib are templated. -// make this function globally accessible so strip --strip-unneeded does not remove symbols -void _thisFunctionShouldNeverBeCalled_MakeMongoROSincludeTheSymbolsWeNeed__(void) -{ - ros::NodeHandle nh; - boost::shared_ptr conn = mongo_ros::makeDbConnection(nh, "", 0, 0.0f); - mongo::GridFS *gfs = new mongo::GridFS(*conn, ""); - mongo::BSONObj q; - mongo::GridFile f = gfs->findFile(q); - f.write(std::cout); - gfs->removeFile(""); - q = gfs->storeFile(NULL, 0, ""); - mongo::LT; - mongo::GT; - delete gfs; -} diff --git a/src/mongo_wrapper_ros.py b/src/mongo_wrapper_ros.py deleted file mode 100755 index 8e54f25..0000000 --- a/src/mongo_wrapper_ros.py +++ /dev/null @@ -1,111 +0,0 @@ -#!/usr/bin/env python - -# This is a ros node wrapper for the mongod database server that -# sets various things using ROS parameters: -# - Parent namespace -# - warehouse_host: hostname used by db -# - warehouse_port: port used by db -# - Private namespace -# - ~database location: where the db is stored. Defaults to /tmp/db. -# - ~overwrite: whether to overwrite existing db. Defaults to false. - -import roslib; roslib.load_manifest('warehouse_ros') -import rospy -import subprocess as sp -import sys -import os -from os import path -import shutil - -def is_oneiric(): - if path.exists('/etc/issue'): - rospy.logdebug('/etc/issue exists') - with open('/etc/issue') as f: - contents = f.readline() - rospy.logdebug('contents are {0}'.format(contents)) - return '11.10' in contents - else: - rospy.logdebug('/etc/issue does not exist') - - - -def is_lucid_or_maverick(): - if path.exists('/etc/issue'): - rospy.logdebug('/etc/issue exists') - with open('/etc/issue') as f: - contents = f.readline() - rospy.logdebug('contents are {0}'.format(contents)) - return '10.04' in contents or '10.10' in contents - else: - rospy.logdebug('/etc/issue does not exist') - -def print_help_message(): - print """ -Usage: rosrun mongodb wrapper.py. -Start the mongodb database server. Configured using the following ROS parameters. - -Parameters in parent namespace -- warehouse_port: port used by db. Defaults to 27017. -- warehouse_host: doesn't directly affect server, but used by clients to know where the db is. - -Parameters in parent namespace -- db_path: where the db is stored on the filesystem. Defaults to /tmp/db. -- database_path: same as db_path. For backward compatibility. -- overwrite: whether to overwrite existing database if it exists. Defaults to false. -""" - -if __name__ == '__main__': - - if '--help' in sys.argv: - print_help_message() - sys.exit() - - rospy.init_node('mongodb') - path_param = rospy.get_param('~database_path' , '/tmp/db') - if path_param=='/tmp/db': - path_param = rospy.get_param('~db_path' , '/tmp/db') - dbpath = path.expanduser(path_param) - overwrite = rospy.get_param('~overwrite', False) - - if '--repair' in sys.argv: - rospy.loginfo("Repairing database") - lock_file = '{0}/mongod.lock'.format(dbpath) - if path.exists(lock_file): - rospy.loginfo(" Removing lock file") - os.remove(lock_file) - sp.check_call(['mongodb', 'mongod', '--repair', '--dbpath', dbpath.format(dbpath)]) - rospy.loginfo(" Successfully repaired.") - sys.exit(0) - - # The defaults here should match the ones used by each client library - port = rospy.get_param('warehouse_port', 27017) - host = rospy.get_param('warehouse_host', 'localhost') - - rospy.loginfo('Starting mongodb with db location {0} listening on {2}:{1}'.\ - format(dbpath, port, host)) - - if overwrite and path.exists(dbpath): - shutil.rmtree(dbpath) - rospy.loginfo('Removed existing db at %s', dbpath) - - if not path.exists(dbpath): - rospy.loginfo('{0} did not exist; creating it.'.format(dbpath)) - os.makedirs(dbpath) - - - - - # OK, now actually run mongod - try: - cmd = "mongod" - sp.check_call("{2} --dbpath {0} --port {1}".\ - format(dbpath, port, cmd).split()) - except sp.CalledProcessError as e: - if e.returncode==12: - rospy.loginfo("Ignoring mongod's nonstandard return code of 12") - else: - rospy.logerr("Mongo process exited with error code {0}".format(e.returncode)) - except OSError, e: - rospy.logerr("Execution failed: %s", e) - - diff --git a/src/test_dbloader.cpp b/src/test_dbloader.cpp new file mode 100644 index 0000000..98dc8f9 --- /dev/null +++ b/src/test_dbloader.cpp @@ -0,0 +1,17 @@ + +#include +#include + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "db_loader_test", ros::init_options::AnonymousName); + + warehouse_ros::DatabaseLoader dbloader; + warehouse_ros::DatabaseConnection::Ptr conn = dbloader.loadDatabase(); + //conn->setParams("localhost", 27017, 10.0); + conn->setTimeout(10.0); + if (!conn->connect()) + ROS_ERROR("Failed to connect to DB"); + + ros::shutdown(); +} \ No newline at end of file diff --git a/src/transform_collection.cpp b/src/transform_collection.cpp index b5d08b6..e22abb6 100644 --- a/src/transform_collection.cpp +++ b/src/transform_collection.cpp @@ -36,48 +36,29 @@ * \author Bhaskara Marthi */ -#include +#include #include -#include -namespace mongo_ros +namespace warehouse_ros { -namespace gm=geometry_msgs; -using std::string; -using std::vector; -using boost::format; - - -// We'll look at all transforms between t-history_length and t+INC -const double INC=1.0; - -mongo::Query TransformCollection::transformQuery (const double t) const +tf::StampedTransform TransformCollection::lookupTransform(const std::string& target, + const std::string& src, + const double t) const { - format query("{stamp : { $gte : %.9f, $lte : %.9f } }"); - const double start = t-history_length_; - const double end = t+INC; - string s = (query % start % end).str(); - mongo::Query q = mongo::fromjson(s); - return q; -} - -tf::StampedTransform TransformCollection::lookupTransform (const string& target, - const string& src, - const double t) const -{ - // Form the query - const mongo::Query q = transformQuery(t); + // Query all transforms between t-search_back_ and t+search_forward_ + Query::Ptr q = coll_.createQuery(); + q->appendRangeInclusive("stamp", t-search_back_, t+search_forward_); // Iterate over the messages and add them to a Transformer - tf::Transformer buffer(true, ros::Duration(history_length_+INC*1.1)); - typedef MessageWithMetadata::ConstPtr MsgPtr; - BOOST_FOREACH (MsgPtr m, coll_.queryResults(q)) + tf::Transformer buffer(true, ros::Duration(search_back_+search_forward_*1.1)); + typename QueryResults::range_t res = coll_.query(q); + for (ResultIterator it = res.first; it != res.second; ++it) { - BOOST_FOREACH (const gm::TransformStamped& trans, m->transforms) + BOOST_FOREACH (const geometry_msgs::TransformStamped& trans, (*it)->transforms) { - const gm::Vector3& v = trans.transform.translation; - const gm::Quaternion& q = trans.transform.rotation; + const geometry_msgs::Vector3& v = trans.transform.translation; + const geometry_msgs::Quaternion& q = trans.transform.rotation; const std_msgs::Header& h = trans.header; const tf::Transform tr(tf::Quaternion(q.x, q.y, q.z, q.w), tf::Vector3(v.x, v.y, v.z)); @@ -94,16 +75,9 @@ tf::StampedTransform TransformCollection::lookupTransform (const string& target, return result; } - -tf::StampedTransform LiveTransformSource::lookupTransform (const string& target, - const string& src, - const double tm) const +void TransformCollection::putTransform(tf::StampedTransform) { - ros::Time t(tm); - tf_->waitForTransform(target, src, t, ros::Duration(timeout_)); - tf::StampedTransform trans; - tf_->lookupTransform(target, src, t, trans); // Can throw - return trans; + } } // namespace diff --git a/src/warehouse_ros/__init__.py b/src/warehouse_ros/__init__.py deleted file mode 100644 index 211d8cb..0000000 --- a/src/warehouse_ros/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from .message_collection import * diff --git a/src/warehouse_ros/message_collection.py b/src/warehouse_ros/message_collection.py deleted file mode 100644 index 145eab3..0000000 --- a/src/warehouse_ros/message_collection.py +++ /dev/null @@ -1,198 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2008, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Bhaskara Marthi - -# Collection of messages stored in a Mongo table and GridFS - -import pymongo as pm -import gridfs as gfs -import rospy -import StringIO -import std_msgs.msg -import json -import bson.json_util - -class MessageCollection: - - def __init__(self, db, coll, msg_class, - db_host=None, db_port=None, indexes=[]): - """ - @param db: Name of database - @param coll: Name of collection - @param indexes: List of fields to build indexes on. - @param msg_class: The class of the message object being stored - @param db_host: The host where the db server is listening. - @param db_port: The port on which the db server is listening. - - Creates collection, db, and indexes if don't already exist. - The database host and port are set to the provided values if given. - If not, the ROS parameters warehouse_host and warehouse_port are used, - and these in turn default to localhost and 27017. - """ - - # Connect to mongo - self.host = db_host or rospy.get_param('warehouse_host', 'localhost') - self.port = db_port or rospy.get_param('warehouse_port', 27017) - while not rospy.is_shutdown(): - try: - self.conn = pm.Connection(self.host, self.port) - break - except: - rospy.loginfo( "Attempting to connect to mongodb @ {0}:{1}".\ - format(self.host,self.port)) - rospy.sleep(2.0) - - # Set up db, collection, gridfs - self.db = self.conn[db] - self.coll = self.db[coll] - self.fs = gfs.GridFS(self.db) - self.msg_class = msg_class - - # Indexes - for ind in indexes: - self.ensure_index(ind) - self.ensure_index('creation_time') - - # Add to the metatable - - # Set up insertion pub - insertion_topic = 'warehouse/{0}/{1}/inserts'.format(db, coll) - self.insertion_pub = rospy.Publisher(insertion_topic, std_msgs.msg.String, - latch=True, queue_size=5) - - - def ensure_index(self, ind, **kwargs): - info = self.coll.index_information() - if ind in info: - rospy.logdebug("Index {0} already exists".format(ind)) - else: - kwargs['name'] = ind - self.coll.ensure_index(ind, **kwargs) - - - def insert(self, m, metadata={}, **kwargs): - """ - @param m: Message to insert - @param metadata: Dictionary of metadata to associate with message - """ - # Insert raw message into gridFS - buff = StringIO.StringIO() - m.serialize(buff) - v = buff.getvalue() - msg_id = self.fs.put(v) - - # Create db entry - entry= metadata.copy() - entry['blob_id'] = msg_id - entry['creation_time'] = rospy.Time.now().to_sec() - - # Insert message info - self.coll.insert(entry, **kwargs) - - # Publish ros notification - s = json.dumps(entry, default=bson.json_util.default) - self.insertion_pub.publish(s) - - - def query(self, query, metadata_only=False, sort_by='', ascending=True): - """ - Perform a query. - - @return: Iterator over tuples (message, metadata) if metadata_only is - False, or iterator over metadata if it's true - """ - if sort_by: - results = self.coll.find(query, sort=[(sort_by, pm.ASCENDING if - ascending else pm.DESCENDING)]) - else: - results = self.coll.find(query) - - if metadata_only: - return results - else: - return (self.process_entry(r) for r in results) - - def find_one(self, query, metadata_only=False, sort_by='', ascending=True): - """ - Like query except returns a single matching item, or None if - no item exists - """ - return next(self.query(query, metadata_only, sort_by, ascending), None) - - def remove(self, query): - "Remove items matching query and return number of removed items." - num_removed = 0 - for item in self.query(query, metadata_only=True): - self.coll.remove(item['_id']) - num_removed += 1 - self.fs.delete(item['blob_id']) - return num_removed - - def process_entry(self, r): - blob = self.fs.get(r['blob_id']) - msg = self.msg_class() - msg.deserialize(blob.read()) - return msg, r - - def update(self, entry, metadata=None, msg=None): - """ - Update a message and/or metadata. - - @param entry: The existing metadata entry - @param metadata: Updates to metadata. These are merged with the existing dictionary entries. - @param msg: If specified, a new message object to store in place of the current one. - """ - old_blob_id = None - if msg: - buf = StringIO.StringIO() - msg.serialize(buf) - v = buf.getvalue() - new_msg_id = self.fs.put(v) - old_blob_id = entry['blob_id'] - entry['blob_id'] = new_msg_id - if metadata: - entry.update(metadata) - self.coll.save(entry, safe=True) - if old_blob_id: - self.fs.delete(old_blob_id) - - def count(self): - return self.coll.count() - - - - - - - - diff --git a/src/warehouse_ros/util.py b/src/warehouse_ros/util.py deleted file mode 100644 index 5913c61..0000000 --- a/src/warehouse_ros/util.py +++ /dev/null @@ -1,47 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2008, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Bhaskara Marthi - -# Utilities - -import roslib - -def load_message_class(pkg, msg): - """ - Given a ROS package and message name, returns the corresponding Python - class object. - """ - roslib.load_manifest(pkg) - m = __import__(pkg+'.msg') - mod = getattr(m, 'msg') - return getattr(mod, msg) diff --git a/test/test_mongo_helpers.h b/test/test_mongo_helpers.h deleted file mode 100644 index bf423f5..0000000 --- a/test/test_mongo_helpers.h +++ /dev/null @@ -1,91 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - */ - -#include -#include -#include - -const double TOL=1e-3; -using std::ostream; - -namespace geometry_msgs -{ - -inline -bool operator== (const Pose& p1, const Pose& p2) -{ - const Point& pos1 = p1.position; - const Point& pos2 = p2.position; - const Quaternion& q1 = p1.orientation; - const Quaternion& q2 = p2.orientation; - return (pos1.x == pos2.x) && (pos1.y == pos2.y) && (pos1.z == pos2.z) && - (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); -} - -} - -template -ostream& operator<< (ostream& str, const mongo_ros::MessageWithMetadata& s) -{ - const T& msg = s; - str << "Message: " << msg; - str << "\nMetadata: " << s.metadata.toString(); - return str; -} - - -geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw) -{ - geometry_msgs::Quaternion q; - q.w = cos(yaw/2); - q.z = sin(yaw/2); - q.x = 0; - q.y = 0; - return q; -} - - -inline -geometry_msgs::Pose makePose (const double x, const double y, const double theta) -{ - geometry_msgs::Pose p; - p.position.x = x; - p.position.y = y; - p.orientation = createQuaternionMsgFromYaw(theta); - return p; -} - -using std::string; - -bool contains (const string& s1, const string& s2) -{ - return strstr(s1.c_str(), s2.c_str())!=NULL; -} - diff --git a/test/test_mongo_ros.cpp b/test/test_mongo_ros.cpp deleted file mode 100644 index 69b4390..0000000 --- a/test/test_mongo_ros.cpp +++ /dev/null @@ -1,157 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - */ - -/** - * \file - * - * Test script for Mongo ros c++ interface - * - * \author Bhaskara Marthi - */ - -// %Tag(CPP_CLIENT)% - -#include "test_mongo_helpers.h" -#include -#include -#include -#include -#include - -namespace mr=mongo_ros; -namespace gm=geometry_msgs; -using std::vector; -using std::string; -using std::cout; - -typedef mr::MessageWithMetadata PoseWithMetadata; -typedef PoseWithMetadata::ConstPtr PoseMetaPtr; - -// Helper function that creates metadata for a message. -// Here we'll use the x and y position, as well as a 'name' -// field that isn't part of the original message. -mr::Metadata makeMetadata (const gm::Pose& p, const string& v) -{ - return mr::Metadata("x", p.position.x, "y", p.position.y, "name", v); -} - -TEST(MongoRos, MongoRos) -{ - // Symbols used in queries to denote binary predicates for < and > - // Note that equality is the default, so we don't need a symbol for it - using mr::LT; - using mr::GT; - - // Clear existing data if any - mr::dropDatabase("my_db", "localhost", 27019, 60.0); - - // Set up db - mr::MessageCollection coll("my_db", "poses", "localhost", - 27019, 60.0); - - // Arrange to index on metadata fields 'x' and 'name' - coll.ensureIndex("name"); - coll.ensureIndex("x"); - - // Add some poses and metadata - const gm::Pose p1 = makePose(24, 42, 0); - const gm::Pose p2 = makePose(10, 532, 3); - const gm::Pose p3 = makePose(53, 22, 5); - const gm::Pose p4 = makePose(22, -5, 33); - coll.insert(p1, makeMetadata(p1, "bar")); - coll.insert(p2, makeMetadata(p2, "baz")); - coll.insert(p3, makeMetadata(p3, "qux")); - coll.insert(p1, makeMetadata(p1, "oof")); - coll.insert(p4, makeMetadata(p4, "ooof")); - EXPECT_EQ(5u, coll.count()); - - // Simple query: find the pose with name 'qux' and return just its metadata - // Since we're doing an equality check, we don't explicitly specify a predicate - vector res = coll.pullAllResults(mr::Query("name", "qux"), true); - EXPECT_EQ(1u, res.size()); - EXPECT_EQ("qux", res[0]->lookupString("name")); - EXPECT_DOUBLE_EQ(53, res[0]->lookupDouble("x")); - - // Set up query: position.x < 40 and position.y > 0. Reverse order - // by the "name" metadata field. Also, here we pull the message itself, not - // just the metadata. Finally, we can't use the simplified construction - // syntax here because it's too long - mr::Query q = mr::Query().append("x", mr::LT, 40).append("y", mr::GT, 0); - vector poses = coll.pullAllResults(q, false, "name", false); - - // Verify poses. - EXPECT_EQ(3u, poses.size()); - EXPECT_EQ(p1, *poses[0]); - EXPECT_EQ(p2, *poses[1]); - EXPECT_EQ(p1, *poses[2]); - - EXPECT_EQ("oof", poses[0]->lookupString("name")); - EXPECT_EQ("baz", poses[1]->lookupString("name")); - EXPECT_EQ("bar", poses[2]->lookupString("name")); - - // Set up query to delete some poses. - mr::Query q2 ("y", mr::LT, 30); - - EXPECT_EQ(5u, coll.count()); - EXPECT_EQ(2u, coll.removeMessages(q2)); - EXPECT_EQ(3u, coll.count()); - - // Test findOne - mr::Query q4("name", "bar"); - EXPECT_EQ(p1, *coll.findOne(q4, false)); - EXPECT_DOUBLE_EQ(24, coll.findOne(q4, true)->lookupDouble("x")); - - mr::Query q5("name", "barbar"); - EXPECT_THROW(coll.findOne(q5, true), mr::NoMatchingMessageException); - EXPECT_THROW(coll.findOne(q5, false), mr::NoMatchingMessageException); - - // Test update - coll.modifyMetadata(q4, mr::Metadata("name", "barbar")); - EXPECT_EQ(3u, coll.count()); - EXPECT_THROW(coll.findOne(q4, false), mr::NoMatchingMessageException); - ROS_INFO("here"); - EXPECT_EQ(p1, *coll.findOne(q5, false)); - - // Check stored metadata - boost::shared_ptr conn = - mr::makeDbConnection(ros::NodeHandle()); - EXPECT_EQ("geometry_msgs/Pose", mr::messageType(*conn, "my_db", "poses")); -} - - -int main (int argc, char** argv) -{ - ros::init(argc, argv, "client_test"); - ros::NodeHandle nh; - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - -// %EndTag(CPP_CLIENT)% diff --git a/test/test_mongo_rospy.py b/test/test_mongo_rospy.py deleted file mode 100755 index ccd961b..0000000 --- a/test/test_mongo_rospy.py +++ /dev/null @@ -1,125 +0,0 @@ -#!/usr/bin/env python -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the Willow Garage nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -# Author: Bhaskara Marthi -# -# 1 - -## %Tag(PYTHON_CLIENT)% - -import roslib; roslib.load_manifest('warehouse_ros') -import rospy -import warehouse_ros as wr -import unittest -import math -import geometry_msgs.msg as gm - -def make_pose(x, y, th): - return gm.Pose(gm.Point(x, y, 0), - gm.Quaternion(0, 0, math.sin(th/2), math.cos(th/2))) - -def make_metadata(p, str): - return {'x': p.position.x, 'y': p.position.y, 'name': str} - -def to_tuple(p): - return (p.position.x, p.position.y, p.position.z, p.orientation.x, - p.orientation.y, p.orientation.z, p.orientation.w) - -def eq_poses(p1, p2): - return to_tuple(p1)==to_tuple(p2) - - -class TestMongoRospy(unittest.TestCase): - - def test_basic(self): - - # Set up collection - coll = wr.MessageCollection("my_db", "poses", gm.Pose) - p1 = make_pose(24, 42, 0) - p2 = make_pose(10, 532, 3) - p3 = make_pose(53, 22, 5) - p4 = make_pose(22, -5, 33) - - # Insert pose objects with accompanying string metadata - coll.insert(p1, make_metadata(p1, "bar")) - coll.insert(p2, make_metadata(p2, "baz")) - coll.insert(p3, make_metadata(p3, "qux")) - coll.insert(p1, make_metadata(p1, "oof")) - coll.insert(p4, make_metadata(p4, "ooof")) - - # Query poses s.t x < 40, y > ), in descending order of name - results = coll.query({'x': {'$lt': 40}, 'y': {'$gt': 0}},\ - sort_by='name', ascending=False) - - # Turn list of pairs into pair of lists - poses, metadata = zip(*list(results)) - - self.assertEqual(len(poses), 3) - self.assertTrue(eq_poses(p1, poses[0])) - self.assertTrue(eq_poses(p2, poses[1])) - self.assertTrue(eq_poses(p1, poses[2])) - - self.assertEqual(metadata[0]['name'], 'oof') - self.assertEqual(metadata[1]['name'], 'baz') - self.assertEqual(metadata[2]['name'], 'bar') - - # Update some messages - coll.update(metadata[0], metadata={'name': 'bat'}) - coll.update(metadata[2], msg=p2) - res2 = coll.query({'y': {'$gt': 40}}, sort_by='name') - poses, metadata = zip(*list(res2)) - - self.assertEqual(metadata[0]['name'], 'bar') - self.assertEqual(metadata[1]['name'], 'bat') - self.assertEqual(metadata[2]['name'], 'baz') - self.assertTrue(eq_poses(p2, poses[0])) - self.assertTrue(eq_poses(p1, poses[1])) - self.assertTrue(eq_poses(p2, poses[2])) - - - # Remove entries s.t. y<30 - self.assertEqual(5, coll.count()) - self.assertEqual(2, coll.remove({'y': {'$lt': 30}})) - self.assertEqual(3, coll.count()) - - # Test find_one - self.assertTrue(coll.find_one({'y': {'$gt': 30}})) - self.assertFalse(coll.find_one({'y': {'$lt': 30}})) - - -if __name__ == "__main__": - rospy.init_node('test_mongo_rospy') - import rostest - rostest.rosrun('mongo_ros', 'test_mongo_rospy', TestMongoRospy) - -## %EndTag(PYTHON_CLIENT)%