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