00001 #ifndef SHAREDMEM_TRANSPORT_PUBLISHER_H 00002 #define SHAREDMEM_TRANSPORT_PUBLISHER_H 00003 00004 00005 #include <roslib/Header.h> 00006 #include "message_transport/simple_publisher_plugin.h" 00007 #include "sharedmem_transport/SharedMemHeader.h" 00008 #include "sharedmem_transport/SharedMemoryBlock.h" 00009 00010 namespace sharedmem_transport { 00011 00012 class SharedmemPublisherImpl 00013 { 00014 public: 00015 SharedmemPublisherImpl(); 00016 virtual ~SharedmemPublisherImpl(); 00017 00018 uint32_t initialise(const std::string & topic); 00019 00020 void setNodeHandle(ros::NodeHandle & nh) { 00021 nh_ = nh; 00022 } 00023 00024 00025 void publish_msg(const ros::Message& message) ; 00026 protected: 00027 boost::interprocess::managed_shared_memory *segment_ ; 00028 SharedMemoryBlock *blockmgr_; 00029 bool clientRegistered; 00030 00031 // This will be modified after the first image is received, so we 00032 // mark them mutable and publish stays "const" 00033 shm_handle shm_handle_; 00034 ros::NodeHandle nh_; 00035 00036 }; 00037 00038 template <class Base> 00039 class SharedmemPublisher : 00040 public message_transport::SimplePublisherPlugin<Base,sharedmem_transport::SharedMemHeader> 00041 { 00042 public: 00043 SharedmemPublisher() : 00044 message_transport::SimplePublisherPlugin<Base,sharedmem_transport::SharedMemHeader>(true), // force latch 00045 first_run_(true) {} 00046 virtual ~SharedmemPublisher() {} 00047 00048 virtual std::string getTransportName() const 00049 { 00050 return "sharedmem"; 00051 } 00052 00053 protected: 00054 virtual void postAdvertiseInit() { 00055 impl.setNodeHandle(this->getNodeHandle()); 00056 } 00057 00058 virtual void publish(const Base& message, 00059 const typename message_transport::SimplePublisherPlugin<Base,sharedmem_transport::SharedMemHeader>::PublishFn& publish_fn) const { 00060 if (first_run_) { 00061 ROS_INFO("First publish run"); 00062 SharedMemHeader header; 00063 header.handle = impl.initialise(this->getTopic()); 00064 ROS_INFO("Publishing latched header"); 00065 publish_fn(header); 00066 first_run_ = false; 00067 } 00068 ROS_DEBUG("Publishing shm message"); 00069 impl.publish_msg(message); 00070 } 00071 00072 mutable SharedmemPublisherImpl impl; 00073 mutable bool first_run_; 00074 00075 }; 00076 00077 } //namespace sharedmem_transport 00078 00079 00080 #endif // SHAREDMEM_TRANSPORT_PUBLISHER_H