Go to the documentation of this file.00001
00002
00003 #include <boost/interprocess/managed_shared_memory.hpp>
00004 #include <cstdlib>
00005 #include <cstddef>
00006 #include <cassert>
00007 #include <utility>
00008
00009 #include <ros/ros.h>
00010
00011 #include "sharedmem_transport/SharedMemoryBlock.h"
00012 #include "sharedmem_transport/sharedmem_publisher.h"
00013
00014 using namespace boost::interprocess;
00015
00016
00017 namespace sharedmem_transport {
00018
00019 SharedmemPublisherImpl::SharedmemPublisherImpl()
00020 {
00021 segment_ = NULL;
00022 clientRegistered = false;
00023 }
00024
00025 SharedmemPublisherImpl::~SharedmemPublisherImpl()
00026 {
00027
00028
00029 if (segment_) {
00030 delete segment_;
00031 }
00032 }
00033
00034 uint32_t SharedmemPublisherImpl::initialise(const std::string & topic) {
00035 if (!clientRegistered) {
00036 clientRegistered = true;
00037 ROS_INFO("Waiting for service '/sharedmem_manager/get_blocks' to be ready");
00038 ros::service::waitForService("/sharedmem_manager/get_blocks");
00039 ROS_INFO("Connecting to shared memory segment");
00040
00041 std::string segment_name = ROSSharedMemoryDefaultBlock;
00042 nh_.param<std::string>("/sharedmem_manager/segment_name",segment_name,ROSSharedMemoryDefaultBlock);
00043 ROS_INFO("Got segment name: %s", segment_name.c_str());
00044 try {
00045 segment_ = new managed_shared_memory(open_only,segment_name.c_str());
00046 ROS_INFO("Got segment %p",segment_);
00047 } catch (boost::interprocess::bad_alloc e) {
00048 segment_ = NULL;
00049 ROS_ERROR("Could not open shared memory segment");
00050 return -1;
00051 }
00052 blockmgr_ = (segment_->find<SharedMemoryBlock>("Manager")).first;
00053 if (!blockmgr_) {
00054 delete segment_;
00055 segment_ = NULL;
00056 ROS_ERROR("Cannot find Manager block in shared memory segment");
00057 return -1;
00058 }
00059 ROS_INFO("Got manager %p",blockmgr_);
00060 try {
00061 shm_handle_ = blockmgr_->allocateBlock(*segment_,topic.c_str(),16);
00062 ROS_INFO("Got shm handle");
00063 } catch (boost::interprocess::bad_alloc e) {
00064 delete segment_;
00065 segment_ = NULL;
00066 ROS_ERROR("Could not open shared memory segment");
00067 return -1;
00068 }
00069 }
00070 return shm_handle_.handle;
00071 }
00072
00073 }