sharedmem_manager.cpp
Go to the documentation of this file.
00001 
00002 #include <ros/ros.h>
00003 #include <boost/thread.hpp>
00004 
00005 #include "sharedmem_transport/SHMReleaseMemory.h"
00006 #include "sharedmem_transport/SHMClearAll.h"
00007 #include "sharedmem_transport/SHMGetBlocks.h"
00008 #include "sharedmem_transport/SharedMemBlock.h"
00009 #include "sharedmem_transport/SharedMemoryBlock.h"
00010 
00011 using namespace boost::interprocess;
00012 using namespace sharedmem_transport;
00013 
00014 boost::mutex main_mutex;
00015 managed_shared_memory *segment = NULL;
00016 SharedMemoryBlock *blockmgr= NULL;
00017 
00018 
00019 bool release_memory(sharedmem_transport::SHMReleaseMemory::Request &req, 
00020                 sharedmem_transport::SHMReleaseMemory::Response &res)
00021 {
00022         boost::lock_guard<boost::mutex> guard(main_mutex);//auto-lock unlock, even on exception
00023     shm_handle shm = blockmgr->findHandle(*segment,req.topic.c_str());
00024         if (!shm.is_valid()) {
00025                 ROS_ERROR("Trying to release topic %s not managed by sharedmem_manager",
00026                                 req.topic.c_str());
00027                 res.result = -1;
00028         } else {
00029         blockmgr->resetBlock(*segment, shm);
00030                 ROS_INFO("Released topic %s",req.topic.c_str());
00031         }
00032         return true;
00033 }
00034 
00035 bool clear_memory(sharedmem_transport::SHMClearAll::Request &req, 
00036                 sharedmem_transport::SHMClearAll::Response &res)
00037 {
00038         boost::lock_guard<boost::mutex> guard(main_mutex);//auto-lock unlock, even on exception
00039     blockmgr->resetAllBlocks(*segment);
00040         ROS_INFO("Deleted all handles");
00041         res.result = 0;
00042         return true;
00043 }
00044 
00045 bool get_blocks(sharedmem_transport::SHMGetBlocks::Request &req, 
00046                 sharedmem_transport::SHMGetBlocks::Response &res)
00047 {
00048         boost::lock_guard<boost::mutex> guard(main_mutex);//auto-lock unlock, even on exception
00049         res.blocks = blockmgr->getBlockList();
00050         return true;
00051 }
00052 
00053 
00054 int main(int argc,char *argv[])
00055 {
00056     std::string segment_name = ROSSharedMemoryDefaultBlock;
00057         int segment_size = 1000000;
00058         ros::init(argc, argv, "sharedmem_manager");
00059         ros::NodeHandle n("sharedmem_manager");
00060         n.param<int>("segment_size",segment_size,1000000);
00061         n.param<std::string>("segment_name",segment_name,ROSSharedMemoryDefaultBlock);
00062 
00063    //Remove shared memory on construction and destruction
00064     struct shm_remove {
00065         std::string name_;
00066         shm_remove(const std::string & name) : name_(name) { 
00067             shared_memory_object::remove(name_.c_str()); 
00068         }
00069         ~shm_remove(){ 
00070             ROS_INFO("Destroying shared memory object");
00071             shared_memory_object::remove(name_.c_str()); 
00072         }
00073     } remover(segment_name);
00074 
00075    //Managed memory segment that allocates portions of a shared memory
00076    //segment with the default management algorithm
00077    managed_shared_memory managed_shm(create_only, segment_name.c_str(), segment_size);
00078    segment = &managed_shm;
00079    blockmgr = segment->find_or_construct<SharedMemoryBlock>("Manager")();
00080    ROS_INFO("Created segment %p, and constructed block %p",segment,blockmgr);
00081 
00082    ros::ServiceServer releaseMemSrv = n.advertiseService("release_memory",release_memory);
00083    ros::ServiceServer clearMemSrv = n.advertiseService("clear_memory",clear_memory);
00084    ros::ServiceServer getBlocksSrv = n.advertiseService("get_blocks",get_blocks);
00085 
00086    ROS_INFO("Created shared memory with %d bytes",segment_size);
00087 
00088    ros::spin();
00089 
00090    ROS_INFO("Exiting from spin, block %p",blockmgr);
00091    if (blockmgr) {
00092        ROS_INFO("Clearing all shared memory blocks");
00093        blockmgr->resetAllBlocks(*segment);
00094        blockmgr = NULL;
00095    }
00096 #if 0
00097    // This create an abort. Not sure why, but it is not critical since we
00098    // destroy the memory segment just after
00099    if (segment) {
00100        ROS_INFO("Destroying manager");
00101        segment->destroy<SharedMemoryBlock>("Manager");
00102        segment = NULL;
00103    }
00104 #endif
00105 }
00106 
00107 
00108 


sharedmem_transport
Author(s): Cedric Pradalier
autogenerated on Sun Oct 5 2014 23:49:07