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
00029
00030
00031
00032
00033
00034
00035
00036 #ifndef ROS_INTROPSECTION_SHAPE_SHIFTER2_H
00037 #define ROS_INTROPSECTION_SHAPE_SHIFTER2_H
00038
00039 #include "ros/ros.h"
00040 #include "ros/console.h"
00041 #include "ros/assert.h"
00042 #include <vector>
00043 #include <boost/flyweight.hpp>
00044 #include <ros/message_traits.h>
00045 #include "ros_type_introspection/ros_introspection.hpp"
00046
00047 namespace RosIntrospection
00048 {
00049
00054 class ShapeShifter
00055 {
00056 public:
00057 typedef boost::shared_ptr<ShapeShifter> Ptr;
00058 typedef boost::shared_ptr<ShapeShifter const> ConstPtr;
00059
00060 static bool uses_old_API_;
00061
00062
00063 ShapeShifter();
00064 virtual ~ShapeShifter();
00065
00066
00067 std::string const& getDataType() const;
00068 std::string const& getMD5Sum() const;
00069 std::string const& getMessageDefinition() const;
00070
00071
00072 ros::Publisher advertise(ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size,
00073 bool latch=false,
00074 const ros::SubscriberStatusCallback &connect_cb=ros::SubscriberStatusCallback()) const;
00075
00077 template<class M>
00078 void instantiate(M& destination) const;
00079
00081 template<typename Stream>
00082 void write(Stream& stream) const;
00083
00084 const uint8_t* raw_data() const;
00085
00086 template<typename Stream>
00087 void read(Stream& stream);
00088
00090 template<typename Message>
00091 void direct_read(const Message& msg,bool morph);
00092
00094 uint32_t size() const;
00095
00096 void morph(const std::string& md5sum, const std::string& datatype_, const std::string& msg_def_);
00097
00098 private:
00099
00100 boost::flyweight<std::string> md5_;
00101 boost::flyweight<std::string> datatype_;
00102 boost::flyweight<std::string> msg_def_;
00103 bool typed_;
00104
00105 mutable std::vector<uint8_t> msgBuf_;
00106
00107 };
00108
00109 }
00110
00111
00112
00113 namespace ros {
00114 namespace message_traits {
00115
00116 template <> struct IsMessage<RosIntrospection::ShapeShifter> : TrueType { };
00117 template <> struct IsMessage<const RosIntrospection::ShapeShifter> : TrueType { };
00118
00119 template<>
00120 struct MD5Sum<RosIntrospection::ShapeShifter>
00121 {
00122 static const char* value(const RosIntrospection::ShapeShifter& m) { return m.getMD5Sum().data(); }
00123
00124
00125 static const char* value() { return "*"; }
00126 };
00127
00128 template<>
00129 struct DataType<RosIntrospection::ShapeShifter>
00130 {
00131 static const char* value(const RosIntrospection::ShapeShifter& m) { return m.getDataType().data(); }
00132
00133
00134 static const char* value() { return "*"; }
00135 };
00136
00137 template<>
00138 struct Definition<RosIntrospection::ShapeShifter>
00139 {
00140 static const char* value(const RosIntrospection::ShapeShifter& m) { return m.getMessageDefinition().data(); }
00141 };
00142
00143 }
00144
00145
00146 namespace serialization
00147 {
00148
00149 template<>
00150 struct Serializer<RosIntrospection::ShapeShifter>
00151 {
00152 template<typename Stream>
00153 inline static void write(Stream& stream, const RosIntrospection::ShapeShifter& m) {
00154 m.write(stream);
00155 }
00156
00157 template<typename Stream>
00158 inline static void read(Stream& stream, RosIntrospection::ShapeShifter& m)
00159 {
00160 m.read(stream);
00161 }
00162
00163 inline static uint32_t serializedLength(const RosIntrospection::ShapeShifter& m) {
00164 return m.size();
00165 }
00166 };
00167
00168
00169 template<>
00170 struct PreDeserialize<RosIntrospection::ShapeShifter>
00171 {
00172 static void notify(const PreDeserializeParams<RosIntrospection::ShapeShifter>& params)
00173 {
00174 std::string md5 = (*params.connection_header)["md5sum"];
00175 std::string datatype = (*params.connection_header)["type"];
00176 std::string msg_def = (*params.connection_header)["message_definition"];
00177
00178 params.message->morph(md5, datatype, msg_def);
00179 }
00180 };
00181
00182 }
00183
00184 }
00185
00186
00187
00188
00189
00190 namespace RosIntrospection
00191 {
00192
00193 template<class M> inline
00194 void ShapeShifter::instantiate(M& destination) const
00195 {
00196 if (!typed_)
00197 throw std::runtime_error("Tried to instantiate message from an untyped ShapeShifter2.");
00198
00199 if (ros::message_traits::datatype<M>() != getDataType())
00200 throw std::runtime_error("Tried to instantiate message without matching datatype.");
00201
00202 if (ros::message_traits::md5sum<M>() != getMD5Sum())
00203 throw std::runtime_error("Tried to instantiate message without matching md5sum.");
00204
00205 ros::serialization::IStream s(msgBuf_.data(), msgBuf_.size() );
00206 ros::serialization::deserialize(s, destination);
00207
00208 }
00209
00210 template<typename Stream> inline
00211 void ShapeShifter::write(Stream& stream) const {
00212 if (msgBuf_.size() > 0)
00213 memcpy(stream.advance(msgBuf_.size()), msgBuf_.data(), msgBuf_.size());
00214 }
00215
00216 inline const uint8_t* ShapeShifter::raw_data() const {
00217 return msgBuf_.data();
00218 }
00219
00220 inline uint32_t ShapeShifter::size() const
00221 {
00222 return msgBuf_.size();
00223 }
00224
00225 template<typename Stream> inline
00226 void ShapeShifter::read(Stream& stream)
00227 {
00228
00229 msgBuf_.resize( stream.getLength() );
00230
00231 memcpy(msgBuf_.data(), stream.getData(), stream.getLength());
00232 }
00233
00234 template<typename Message> inline
00235 void ShapeShifter::direct_read(const Message& msg, bool do_morph)
00236 {
00237 if(do_morph)
00238 {
00239 this->morph(
00240 ros::message_traits::MD5Sum<Message>::value(),
00241 ros::message_traits::DataType<Message>::value(),
00242 ros::message_traits::Definition<Message>::value());
00243 }
00244
00245 auto length = ros::serialization::serializationLength(msg);
00246
00247
00248 msgBuf_.resize( length );
00249
00250 ros::serialization::OStream o_stream(msgBuf_.data(), length);
00251 ros::serialization::serialize(o_stream, msg);
00252 }
00253
00254 inline ShapeShifter::ShapeShifter()
00255 : typed_(false),
00256 msgBuf_()
00257 {
00258 }
00259
00260
00261 inline ShapeShifter::~ShapeShifter()
00262 {
00263
00264 }
00265
00266
00267 inline std::string const& ShapeShifter::getDataType() const { return datatype_; }
00268
00269
00270 inline std::string const& ShapeShifter::getMD5Sum() const { return md5_; }
00271
00272
00273 inline std::string const& ShapeShifter::getMessageDefinition() const { return msg_def_; }
00274
00275
00276 inline void ShapeShifter::morph(const std::string& _md5sum, const std::string& _datatype, const std::string& _msg_def)
00277 {
00278 md5_ = _md5sum;
00279 datatype_ = _datatype;
00280 msg_def_ = _msg_def;
00281 typed_ = (md5_ != "*");
00282 }
00283
00284
00285 inline ros::Publisher ShapeShifter::advertise(ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size, bool latch,
00286 const ros::SubscriberStatusCallback &connect_cb) const
00287 {
00288 ros::AdvertiseOptions opts(topic, queue_size, getMD5Sum(), getDataType(), getMessageDefinition(), connect_cb);
00289 opts.latch = latch;
00290 return nh.advertise(opts);
00291 }
00292
00293 }
00294
00295
00296 #endif
00297