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 WAREHOUSE_RESULT_SET_H
00040 #define WAREHOUSE_RESULT_SET_H
00041
00042 #include <warehouse/message_with_metadata.h>
00043 #include <warehouse/InitiateQuery.h>
00044 #include <boost/iterator/iterator_facade.hpp>
00045 #include <boost/utility.hpp>
00046 #include <boost/thread.hpp>
00047 #include <ros/ros.h>
00048
00049
00050 namespace warehouse
00051 {
00052
00062 template <class M>
00063 class ResultIterator :
00064 public boost::iterator_facade<ResultIterator<M>,
00065 typename MessageWithMetadata<M>::ConstPtr,
00066 boost::single_pass_traversal_tag,
00067 typename MessageWithMetadata<M>::ConstPtr >
00068 {
00069 public:
00070
00071
00072 typedef MessageWithMetadata<M> Msg;
00073 typedef typename M::ConstPtr Ptr;
00074
00076 ResultIterator (const ros::NodeHandle nh, const InitiateQuery& srv,
00077 ros::ServiceClient& pull_client, bool metadata_only);
00078
00081 ResultIterator (const ResultIterator& res_);
00082
00085 ResultIterator ();
00086
00087 private:
00088
00089 friend class boost::iterator_core_access;
00090
00092 void increment ();
00093 typename MessageWithMetadata<M>::ConstPtr dereference () const;
00094 bool equal (const ResultIterator<M>& other) const;
00095 void maybeFetchFirst () const;
00096
00097 struct QueryResultSubscriber : boost::noncopyable
00098 {
00099 QueryResultSubscriber (const ros::NodeHandle& nh, const InitiateQuery& srv,
00100 const ros::ServiceClient& pull_client_,
00101 bool metadata_only);
00102
00103 void messageCallback (Ptr msg);
00104 void metadataCallback (std_msgs::String::ConstPtr m);
00105 void fetchNext ();
00106
00107 boost::mutex mutex_;
00108 boost::condition_variable cond;
00109 ros::NodeHandle nh_;
00110 std::string query_id_, topic_base_;
00111 bool data_received_;
00112 bool metadata_received_;
00113 ros::Subscriber sub_, metadata_sub_;
00114 ros::ServiceClient pull_client_;
00115 bool metadata_only_;
00116 bool initial_fetch_;
00117 typename MessageWithMetadata<M>::Ptr next_result_;
00118 };
00119
00120 mutable boost::shared_ptr<QueryResultSubscriber> sub_;
00121 };
00122
00123
00125 template <class M>
00126 struct QueryResults
00127 {
00128 typedef std::pair<ResultIterator<M>, ResultIterator<M> > range_t;
00129 };
00130
00131
00132 }
00133
00134 #include "../../src/result_iterator_impl.h"
00135
00136 #endif // include guard