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 #ifndef ROSCPP_PUBLISHER_HANDLE_H
00029 #define ROSCPP_PUBLISHER_HANDLE_H
00030
00031 #include "ros/forwards.h"
00032 #include "ros/common.h"
00033 #include "ros/message.h"
00034 #include "ros/serialization.h"
00035 #include <boost/bind.hpp>
00036
00037 namespace ros
00038 {
00047 class ROSCPP_DECL Publisher
00048 {
00049 public:
00050 Publisher() {}
00051 Publisher(const Publisher& rhs);
00052 ~Publisher();
00053
00062 template <typename M>
00063 void publish(const boost::shared_ptr<M>& message) const
00064 {
00065 using namespace serialization;
00066
00067 if (!impl_)
00068 {
00069 ROS_ASSERT_MSG(false, "Call to publish() on an invalid Publisher");
00070 return;
00071 }
00072
00073 if (!impl_->isValid())
00074 {
00075 ROS_ASSERT_MSG(false, "Call to publish() on an invalid Publisher (topic [%s])", impl_->topic_.c_str());
00076 return;
00077 }
00078
00079 ROS_ASSERT_MSG(impl_->md5sum_ != "*" || impl_->md5sum_ == mt::md5sum<M>(*message),
00080 "Trying to publish message of type [%s/%s] on a publisher with type [%s/%s]",
00081 impl_->datatype_.c_str(), impl_->md5sum_.c_str(),
00082 mt::datatype<M>(*message), mt::md5sum<M>(*message));
00083
00084 SerializedMessage m;
00085 m.type_info = &typeid(M);
00086 m.message = message;
00087
00088 publish(boost::bind(serializeMessage<M>, boost::ref(*message)), m);
00089 }
00090
00094 template <typename M>
00095 void publish(const M& message) const
00096 {
00097 using namespace serialization;
00098 namespace mt = ros::message_traits;
00099
00100 if (!impl_)
00101 {
00102 ROS_ASSERT_MSG(false, "Call to publish() on an invalid Publisher");
00103 return;
00104 }
00105
00106 if (!impl_->isValid())
00107 {
00108 ROS_ASSERT_MSG(false, "Call to publish() on an invalid Publisher (topic [%s])", impl_->topic_.c_str());
00109 return;
00110 }
00111
00112 ROS_ASSERT_MSG(impl_->md5sum_ != "*" || impl_->md5sum_ == mt::md5sum<M>(message),
00113 "Trying to publish message of type [%s/%s] on a publisher with type [%s/%s]",
00114 impl_->datatype_.c_str(), impl_->md5sum_.c_str(),
00115 mt::datatype<M>(message), mt::md5sum<M>(message));
00116
00117 SerializedMessage m;
00118 publish(boost::bind(serializeMessage<M>, boost::ref(message)), m);
00119 }
00120
00131 void shutdown();
00132
00136 std::string getTopic() const;
00137
00141 uint32_t getNumSubscribers() const;
00142
00146 bool isLatched() const;
00147
00148 operator void*() const { return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0; }
00149
00150 bool operator<(const Publisher& rhs) const
00151 {
00152 return impl_ < rhs.impl_;
00153 }
00154
00155 bool operator==(const Publisher& rhs) const
00156 {
00157 return impl_ == rhs.impl_;
00158 }
00159
00160 bool operator!=(const Publisher& rhs) const
00161 {
00162 return impl_ != rhs.impl_;
00163 }
00164
00165 private:
00166
00167 Publisher(const std::string& topic, const std::string& md5sum,
00168 const std::string& datatype, const NodeHandle& node_handle,
00169 const SubscriberCallbacksPtr& callbacks);
00170
00171 void publish(const boost::function<SerializedMessage(void)>& serfunc, SerializedMessage& m) const;
00172 void incrementSequence() const;
00173
00174 class ROSCPP_DECL Impl
00175 {
00176 public:
00177 Impl();
00178 ~Impl();
00179
00180 void unadvertise();
00181 bool isValid() const;
00182
00183 std::string topic_;
00184 std::string md5sum_;
00185 std::string datatype_;
00186 NodeHandlePtr node_handle_;
00187 SubscriberCallbacksPtr callbacks_;
00188 bool unadvertised_;
00189 };
00190 typedef boost::shared_ptr<Impl> ImplPtr;
00191 typedef boost::weak_ptr<Impl> ImplWPtr;
00192
00193 ImplPtr impl_;
00194
00195 friend class NodeHandle;
00196 friend class NodeHandleBackingCollection;
00197 };
00198
00199 typedef std::vector<Publisher> V_Publisher;
00200 }
00201
00202 #endif // ROSCPP_PUBLISHER_HANDLE_H
00203