00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #ifndef ROSCPP_TOPIC_H
00036 #define ROSCPP_TOPIC_H
00037
00038 #include "node_handle.h"
00039 #include <boost/shared_ptr.hpp>
00040
00041 namespace ros
00042 {
00043 namespace topic
00044 {
00045
00049 void waitForMessageImpl(SubscribeOptions& ops, const boost::function<bool(void)>& ready_pred, NodeHandle& nh, ros::Duration timeout);
00050
00051 template<class M>
00052 class SubscribeHelper
00053 {
00054 public:
00055 typedef boost::shared_ptr<M const> MConstPtr;
00056 void callback(const MConstPtr& message)
00057 {
00058 message_ = message;
00059 }
00060
00061 bool hasMessage()
00062 {
00063 return message_;
00064 }
00065
00066 MConstPtr getMessage()
00067 {
00068 return message_;
00069 }
00070
00071 private:
00072 MConstPtr message_;
00073 };
00074
00084 template<class M>
00085 boost::shared_ptr<M const> waitForMessage(const std::string& topic, NodeHandle& nh, ros::Duration timeout)
00086 {
00087 SubscribeHelper<M> helper;
00088 SubscribeOptions ops;
00089 ops.template init<M>(topic, 1, boost::bind(&SubscribeHelper<M>::callback, &helper, _1));
00090
00091 waitForMessageImpl(ops, boost::bind(&SubscribeHelper<M>::hasMessage, &helper), nh, timeout);
00092
00093 return helper.getMessage();
00094 }
00095
00103 template<class M>
00104 boost::shared_ptr<M const> waitForMessage(const std::string& topic)
00105 {
00106 ros::NodeHandle nh;
00107 return waitForMessage<M>(topic, nh, ros::Duration());
00108 }
00109
00118 template<class M>
00119 boost::shared_ptr<M const> waitForMessage(const std::string& topic, ros::Duration timeout)
00120 {
00121 ros::NodeHandle nh;
00122 return waitForMessage<M>(topic, nh, timeout);
00123 }
00124
00133 template<class M>
00134 boost::shared_ptr<M const> waitForMessage(const std::string& topic, ros::NodeHandle& nh)
00135 {
00136 return waitForMessage<M>(topic, nh, ros::Duration());
00137 }
00138
00139 }
00140 }
00141
00142 #endif // ROSCPP_TOPIC_H