00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00039 #ifndef POSE_GRAPH_GRAPH_DB_H
00040 #define POSE_GRAPH_GRAPH_DB_H
00041
00042 #include <pose_graph/constraint_graph.h>
00043 #include <warehouse/warehouse_client.h>
00044 #include <yaml-cpp/yaml.h>
00045
00046 namespace pose_graph
00047 {
00048
00051 std::string nodeMetadata (unsigned n);
00052
00056 template <class M>
00057 typename M::ConstPtr
00058 getNodeData (warehouse::Collection<M>* coll, unsigned n);
00059
00061 inline
00062 std::vector<std::string> nodeIndex ()
00063 {
00064 std::vector<std::string> v;
00065 v.push_back("node");
00066 return v;
00067 }
00068
00069
00070
00074 template <class M>
00075 class CachedNodeMap
00076 {
00077 public:
00078
00082 CachedNodeMap (warehouse::WarehouseClient* client, const std::string& name);
00083
00087 CachedNodeMap (warehouse::Collection<M>* coll);
00088
00090 CachedNodeMap (const CachedNodeMap& m);
00091
00096 typename M::ConstPtr get (unsigned n) const;
00097
00102 void set (unsigned n, typename M::ConstPtr);
00103
00108 void set (unsigned n, const M& m);
00109
00112 bool hasData (unsigned n) const;
00113
00114 private:
00115
00117 void initialLoad ();
00118
00120 void removeExisting (unsigned n);
00121
00123 void updateCB (const warehouse::UpdateNotification& m);
00124
00125
00126 mutable boost::mutex mutex_;
00127
00128
00129 bool initialized_;
00130
00131
00132 boost::shared_ptr<warehouse::Collection<M> > coll_ptr_;
00133 warehouse::Collection<M>* coll_;
00134
00135
00136 mutable std::map<unsigned, typename M::ConstPtr> map_;
00137
00138 ros::NodeHandle nh_;
00139
00140
00141 ros::Subscriber update_sub_;
00142
00143 };
00144
00145
00146
00147 }
00148
00149 #include "../../src/graph_db_impl.h"
00150
00151 #endif // include guard