$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 #ifndef ROSRT_PUBLISHER_H 00036 #define ROSRT_PUBLISHER_H 00037 00038 #include <lockfree/object_pool.h> 00039 #include "detail/pool_gc.h" 00040 00041 #include <ros/publisher.h> 00042 #include <ros/node_handle.h> 00043 #include <boost/utility.hpp> 00044 00045 namespace rosrt 00046 { 00047 00048 typedef boost::shared_ptr<void const> VoidConstPtr; 00049 00050 typedef void(*PublishFunc)(const ros::Publisher& pub, const VoidConstPtr& msg); 00051 typedef VoidConstPtr(*CloneFunc)(const VoidConstPtr& msg); 00052 00053 namespace detail 00054 { 00055 template<typename M> 00056 void publishMessage(const ros::Publisher& pub, const VoidConstPtr& msg) 00057 { 00058 boost::shared_ptr<M const> m = boost::static_pointer_cast<M const>(msg); 00059 pub.publish(m); 00060 } 00061 00062 template<typename M> 00063 VoidConstPtr cloneMessage(const VoidConstPtr& msg) 00064 { 00065 boost::shared_ptr<M> clone(new M); 00066 *clone = *boost::static_pointer_cast<M const>(msg); 00067 return clone; 00068 } 00069 00070 bool publish(const ros::Publisher& pub, const VoidConstPtr& msg, PublishFunc pub_func, CloneFunc clone_func); 00071 00072 template<typename M> 00073 bool publish(const ros::Publisher& pub, const VoidConstPtr& msg) 00074 { 00075 return publish(pub, msg, publishMessage<M>, cloneMessage<M>); 00076 } 00077 } // namespace detail 00078 00082 template<typename M> 00083 class Publisher : public boost::noncopyable 00084 { 00085 typedef boost::shared_ptr<M const> MConstPtr; 00086 00087 public: 00092 Publisher() 00093 : pool_(NULL) 00094 { 00095 } 00096 00103 Publisher(const ros::Publisher& pub, uint32_t message_pool_size, const M& tmpl) 00104 { 00105 initialize(pub, message_pool_size, tmpl); 00106 } 00107 00116 Publisher(ros::NodeHandle& nh, const std::string& topic, uint32_t ros_publisher_queue_size, uint32_t message_pool_size, const M& tmpl) 00117 { 00118 initialize(nh, topic, ros_publisher_queue_size, message_pool_size, tmpl); 00119 } 00120 00121 ~Publisher() 00122 { 00123 if (pool_) 00124 detail::addPoolToGC((void*)pool_, detail::deletePool<M>, detail::poolIsDeletable<M>); 00125 } 00126 00133 void initialize(const ros::Publisher& pub, uint32_t message_pool_size, const M& tmpl) 00134 { 00135 pub_ = pub; 00136 pool_ = new lockfree::ObjectPool<M>(); 00137 pool_->initialize(message_pool_size, tmpl); 00138 } 00139 00148 void initialize(ros::NodeHandle& nh, const std::string& topic, uint32_t ros_publisher_queue_size, uint32_t message_pool_size, const M& tmpl) 00149 { 00150 initialize(nh.advertise<M>(topic, ros_publisher_queue_size), message_pool_size, tmpl); 00151 } 00152 00156 bool publish(const MConstPtr& msg) 00157 { 00158 return detail::publish<M>(pub_, msg); 00159 } 00160 00165 boost::shared_ptr<M> allocate() 00166 { 00167 assert(pool_); 00168 return pool_->allocateShared(); 00169 } 00170 00174 const ros::Publisher& getPublisher() { return pub_; } 00175 00176 private: 00177 ros::Publisher pub_; 00178 lockfree::ObjectPool<M>* pool_; 00179 }; 00180 00181 } // namespace rosrt 00182 00183 #endif // ROSRT_PUBLISHER_H