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 #ifndef TOPIC_TOOLS_SHAPE_SHIFTER_H
00036 #define TOPIC_TOOLS_SHAPE_SHIFTER_H
00037
00038 #include "ros/ros.h"
00039 #include "ros/console.h"
00040 #include "ros/assert.h"
00041 #include <vector>
00042 #include <string>
00043 #include <string.h>
00044
00045 #include <ros/message_traits.h>
00046 #include "macros.h"
00047
00048 namespace topic_tools
00049 {
00050
00051 class ShapeShifterException : public ros::Exception
00052 {
00053 public:
00054 ShapeShifterException(const std::string& msg)
00055 : ros::Exception(msg) {}
00056 };
00057
00058
00059 class TOPIC_TOOLS_DECL ShapeShifter
00060 {
00061 public:
00062 typedef boost::shared_ptr<ShapeShifter> Ptr;
00063 typedef boost::shared_ptr<ShapeShifter const> ConstPtr;
00064
00065 static bool uses_old_API_;
00066
00067
00068 ShapeShifter();
00069 virtual ~ShapeShifter();
00070
00071
00072 std::string const& getDataType() const;
00073 std::string const& getMD5Sum() const;
00074 std::string const& getMessageDefinition() const;
00075
00076 void morph(const std::string& md5sum, const std::string& datatype, const std::string& msg_def,
00077 const std::string& latching);
00078
00079
00080 ros::Publisher advertise(ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size_, bool latch=false,
00081 const ros::SubscriberStatusCallback &connect_cb=ros::SubscriberStatusCallback()) const;
00082
00084 template<class M>
00085 boost::shared_ptr<M> instantiate() const;
00086
00088 template<typename Stream>
00089 void write(Stream& stream) const;
00090
00091 template<typename Stream>
00092 void read(Stream& stream);
00093
00095 uint32_t size() const;
00096
00097 boost::shared_ptr<std::map<std::string, std::string> > __connection_header;
00098
00099 private:
00100
00101 std::string md5, datatype, msg_def, latching;
00102 bool typed;
00103
00104 uint8_t *msgBuf;
00105 uint32_t msgBufUsed;
00106 uint32_t msgBufAlloc;
00107
00108 };
00109
00110 }
00111
00112
00113
00114 namespace ros {
00115 namespace message_traits {
00116
00117 template <> struct IsMessage<topic_tools::ShapeShifter> : TrueType { };
00118 template <> struct IsMessage<const topic_tools::ShapeShifter> : TrueType { };
00119
00120 template<>
00121 struct MD5Sum<topic_tools::ShapeShifter>
00122 {
00123 static const char* value(const topic_tools::ShapeShifter& m) { return m.getMD5Sum().c_str(); }
00124
00125
00126 static const char* value() { return "*"; }
00127 };
00128
00129 template<>
00130 struct DataType<topic_tools::ShapeShifter>
00131 {
00132 static const char* value(const topic_tools::ShapeShifter& m) { return m.getDataType().c_str(); }
00133
00134
00135 static const char* value() { return "*"; }
00136 };
00137
00138 template<>
00139 struct Definition<topic_tools::ShapeShifter>
00140 {
00141 static const char* value(const topic_tools::ShapeShifter& m) { return m.getMessageDefinition().c_str(); }
00142 };
00143
00144 }
00145
00146
00147 namespace serialization
00148 {
00149
00150 template<>
00151 struct Serializer<topic_tools::ShapeShifter>
00152 {
00153 template<typename Stream>
00154 inline static void write(Stream& stream, const topic_tools::ShapeShifter& m) {
00155 m.write(stream);
00156 }
00157
00158 template<typename Stream>
00159 inline static void read(Stream& stream, topic_tools::ShapeShifter& m)
00160 {
00161 m.read(stream);
00162 }
00163
00164 inline static uint32_t serializedLength(const topic_tools::ShapeShifter& m) {
00165 return m.size();
00166 }
00167 };
00168
00169
00170 template<>
00171 struct PreDeserialize<topic_tools::ShapeShifter>
00172 {
00173 static void notify(const PreDeserializeParams<topic_tools::ShapeShifter>& params)
00174 {
00175 std::string md5 = (*params.connection_header)["md5sum"];
00176 std::string datatype = (*params.connection_header)["type"];
00177 std::string msg_def = (*params.connection_header)["message_definition"];
00178 std::string latching = (*params.connection_header)["latching"];
00179
00180 typedef std::map<std::string, std::string> map_t;
00181 boost::shared_ptr<map_t> shmap(new map_t(*params.connection_header));
00182 params.message->__connection_header = shmap;
00183 params.message->morph(md5, datatype, msg_def, latching);
00184 }
00185 };
00186
00187 }
00188
00189 }
00190
00191
00192
00193
00194
00195 namespace topic_tools
00196 {
00197
00198
00199
00200
00201 template<class M>
00202 boost::shared_ptr<M> ShapeShifter::instantiate() const
00203 {
00204 if (!typed)
00205 throw ShapeShifterException("Tried to instantiate message from an untyped shapeshifter.");
00206
00207 if (ros::message_traits::datatype<M>() != getDataType())
00208 throw ShapeShifterException("Tried to instantiate message without matching datatype.");
00209
00210 if (ros::message_traits::md5sum<M>() != getMD5Sum())
00211 throw ShapeShifterException("Tried to instantiate message without matching md5sum.");
00212
00213 boost::shared_ptr<M> p(new M());
00214
00215 ros::assignSubscriptionConnectionHeader<M>(p.get(), __connection_header);
00216
00217 ros::serialization::IStream s(msgBuf, msgBufUsed);
00218 ros::serialization::deserialize(s, *p);
00219
00220 return p;
00221 }
00222
00223 template<typename Stream>
00224 void ShapeShifter::write(Stream& stream) const {
00225 if (msgBufUsed > 0)
00226 memcpy(stream.advance(msgBufUsed), msgBuf, msgBufUsed);
00227 }
00228
00229 template<typename Stream>
00230 void ShapeShifter::read(Stream& stream)
00231 {
00232 stream.getLength();
00233 stream.getData();
00234
00235
00236 if (stream.getLength() > msgBufAlloc)
00237 {
00238 delete[] msgBuf;
00239 msgBuf = new uint8_t[stream.getLength()];
00240 msgBufAlloc = stream.getLength();
00241 }
00242 msgBufUsed = stream.getLength();
00243 memcpy(msgBuf, stream.getData(), stream.getLength());
00244 }
00245
00246 }
00247
00248
00249 #endif
00250