subscriber.h
Go to the documentation of this file.
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 Willow Garage, Inc. 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  * $Id: subscriber.h 43011 2010-08-12 20:30:23Z hsu $
00035  *
00036  */
00037 #ifndef ROSMPI_SUBSCRIBER_H_
00038 #define ROSMPI_SUBSCRIBER_H_
00039 
00040 #include "rosmpi/forwards.h"
00041 // ROS
00042 #include <ros/message.h>
00043 #include <ros/message_deserializer.h>
00044 #include <ros/serialized_message.h>
00045 #include <ros/serialization.h>
00046 #include <ros/subscription_callback_helper.h>
00047 
00048 namespace rosmpi
00049 {
00051   class Subscriber
00052   {
00053     public:
00054       Subscriber () {}
00055       
00056       ~Subscriber ()
00057       {
00058         helper_.reset ();
00059       }
00060 
00061     public:
00062 
00063       template <typename M> void 
00064         call ()
00065       {
00066         MPI::Status status;
00067         // Blocking test for a message
00068         MPI::COMM_WORLD.Probe (MPI_ANY_SOURCE, MPI_ANY_TAG, status);      // for test purposes only
00069 
00070         // Gets the number of "top level" elements
00071         int count = status.Get_count (MPI_BYTE);
00072 
00073         uint8_t *buf_raw = (uint8_t*)malloc (count * sizeof (uint8_t));
00074         // Performs a standard-mode blocking receive
00075         MPI::COMM_WORLD.Recv (buf_raw, count, MPI_BYTE, MPI_ANY_SOURCE, MPI_ANY_TAG);
00076         count -= 4;
00077 
00078         boost::shared_array<uint8_t> buf (new uint8_t[count]);
00079         memcpy (buf.get (), &buf_raw[4], count);
00080         free (buf_raw);
00081 
00082         ros::SerializedMessage m (buf, count);
00083         
00084         boost::shared_ptr<ros::M_string> connection_header;     // empty connection header?
00085         ros::MessageDeserializer deserializer (helper_, m, connection_header);
00086         ros::VoidConstPtr msg = deserializer.deserialize ();
00087 
00088         ros::SubscriptionCallbackHelperCallParams params;
00089         // We need to pass an empty function pointer as the last parameter on the constructor
00090         // of MessageEvent, otherwise boost::make_shared<void> will fail (!!!)
00091         params.event = ros::MessageEvent<void const> (msg, connection_header, ros::Time::now (), true, ros::MessageEvent<void const>::CreateFunction ());
00092         helper_->call (params);
00093       }
00094 
00095     private:
00096       Subscriber (const ros::SubscriptionCallbackHelperPtr &helper) : helper_ (helper)
00097       { 
00098       }
00099 
00100       ros::SubscriptionCallbackHelperPtr helper_;
00101 
00102       friend class NodeHandle;
00103   };
00104 }
00105 
00106 #endif  //#ifndef ROSMPI_SUBSCRIBER_H_


rosmpi
Author(s): Radu Bogdan Rusu, John Hsu
autogenerated on Mon Jan 6 2014 11:27:20