Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef ROSCPP_SUBSCRIPTION_CALLBACK_HELPER_H
00029 #define ROSCPP_SUBSCRIPTION_CALLBACK_HELPER_H
00030
00031 #include <typeinfo>
00032
00033 #include "common.h"
00034 #include "ros/forwards.h"
00035 #include "ros/parameter_adapter.h"
00036 #include "ros/message_traits.h"
00037 #include "ros/builtin_message_traits.h"
00038 #include "ros/serialization.h"
00039 #include "ros/message_event.h"
00040 #include <ros/static_assert.h>
00041
00042 #include <boost/type_traits/add_const.hpp>
00043 #include <boost/type_traits/remove_const.hpp>
00044 #include <boost/type_traits/remove_reference.hpp>
00045 #include <boost/type_traits/is_base_of.hpp>
00046 #include <boost/utility/enable_if.hpp>
00047 #include <boost/make_shared.hpp>
00048
00049 namespace ros
00050 {
00051
00052 namespace serialization
00053 {
00054
00055
00056 template<typename M>
00057 struct PreDeserializeParams
00058 {
00059 boost::shared_ptr<M> message;
00060 boost::shared_ptr<std::map<std::string, std::string> > connection_header;
00061 };
00062
00067 template<typename M>
00068 struct PreDeserialize
00069 {
00070 static void notify(const PreDeserializeParams<M>&) { }
00071 };
00072
00073 }
00074
00075 template<typename T>
00076 void
00077 assignSubscriptionConnectionHeader(T* t, const boost::shared_ptr<M_string>& connection_header,
00078 typename boost::enable_if<ros::message_traits::IsMessage<T> >::type*_=0)
00079 {
00080 (void)_;
00081 t->__connection_header = connection_header;
00082 }
00083
00084 template<typename T>
00085 void
00086 assignSubscriptionConnectionHeader(T* t, const boost::shared_ptr<M_string>& connection_header,
00087 typename boost::disable_if<ros::message_traits::IsMessage<T> >::type*_=0)
00088 {
00089 (void)_;
00090 }
00091
00092 struct SubscriptionCallbackHelperDeserializeParams
00093 {
00094 uint8_t* buffer;
00095 uint32_t length;
00096 boost::shared_ptr<M_string> connection_header;
00097 };
00098
00099 struct ROSCPP_DECL SubscriptionCallbackHelperCallParams
00100 {
00101 MessageEvent<void const> event;
00102 };
00103
00109 class ROSCPP_DECL SubscriptionCallbackHelper
00110 {
00111 public:
00112 virtual ~SubscriptionCallbackHelper() {}
00113 virtual VoidConstPtr deserialize(const SubscriptionCallbackHelperDeserializeParams&) = 0;
00114 virtual void call(SubscriptionCallbackHelperCallParams& params) = 0;
00115 virtual const std::type_info& getTypeInfo() = 0;
00116 virtual bool isConst() = 0;
00117 };
00118 typedef boost::shared_ptr<SubscriptionCallbackHelper> SubscriptionCallbackHelperPtr;
00119
00125 template<typename P, typename Enabled = void>
00126 class SubscriptionCallbackHelperT : public SubscriptionCallbackHelper
00127 {
00128 public:
00129 typedef ParameterAdapter<P> Adapter;
00130 typedef typename ParameterAdapter<P>::Message NonConstType;
00131 typedef typename ParameterAdapter<P>::Event Event;
00132 typedef typename boost::add_const<NonConstType>::type ConstType;
00133 typedef boost::shared_ptr<NonConstType> NonConstTypePtr;
00134 typedef boost::shared_ptr<ConstType> ConstTypePtr;
00135
00136 static const bool is_const = ParameterAdapter<P>::is_const;
00137
00138 typedef boost::function<void(typename Adapter::Parameter)> Callback;
00139 typedef boost::function<NonConstTypePtr()> CreateFunction;
00140
00141 SubscriptionCallbackHelperT(const Callback& callback,
00142 const CreateFunction& create = DefaultMessageCreator<NonConstType>())
00143 : callback_(callback)
00144 , create_(create)
00145 { }
00146
00147 void setCreateFunction(const CreateFunction& create)
00148 {
00149 create_ = create;
00150 }
00151
00152 virtual VoidConstPtr deserialize(const SubscriptionCallbackHelperDeserializeParams& params)
00153 {
00154 namespace ser = serialization;
00155
00156 NonConstTypePtr msg = create_();
00157
00158 if (!msg)
00159 {
00160 ROS_DEBUG("Allocation failed for message of type [%s]", getTypeInfo().name());
00161 return VoidConstPtr();
00162 }
00163
00164 assignSubscriptionConnectionHeader(msg.get(), params.connection_header);
00165
00166 ser::PreDeserializeParams<NonConstType> predes_params;
00167 predes_params.message = msg;
00168 predes_params.connection_header = params.connection_header;
00169 ser::PreDeserialize<NonConstType>::notify(predes_params);
00170
00171 ser::IStream stream(params.buffer, params.length);
00172 ser::deserialize(stream, *msg);
00173
00174 return VoidConstPtr(msg);
00175 }
00176
00177 virtual void call(SubscriptionCallbackHelperCallParams& params)
00178 {
00179 Event event(params.event, create_);
00180 callback_(ParameterAdapter<P>::getParameter(event));
00181 }
00182
00183 virtual const std::type_info& getTypeInfo()
00184 {
00185 return typeid(NonConstType);
00186 }
00187
00188 virtual bool isConst()
00189 {
00190 return ParameterAdapter<P>::is_const;
00191 }
00192
00193 private:
00194 Callback callback_;
00195 CreateFunction create_;
00196 };
00197
00198 }
00199
00200 #endif // ROSCPP_SUBSCRIPTION_CALLBACK_HELPER_H
roscpp
Author(s): Morgan Quigley, Josh Faust, Brian Gerkey, Troy Straszheim
autogenerated on Mon Oct 6 2014 11:46:44