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 private:
00098
00099 std::string md5, datatype, msg_def, latching;
00100 bool typed;
00101
00102 uint8_t *msgBuf;
00103 uint32_t msgBufUsed;
00104 uint32_t msgBufAlloc;
00105
00106 };
00107
00108 }
00109
00110
00111
00112 namespace ros {
00113 namespace message_traits {
00114
00115 template <> struct IsMessage<topic_tools::ShapeShifter> : TrueType { };
00116 template <> struct IsMessage<const topic_tools::ShapeShifter> : TrueType { };
00117
00118 template<>
00119 struct MD5Sum<topic_tools::ShapeShifter>
00120 {
00121 static const char* value(const topic_tools::ShapeShifter& m) { return m.getMD5Sum().c_str(); }
00122
00123
00124 static const char* value() { return "*"; }
00125 };
00126
00127 template<>
00128 struct DataType<topic_tools::ShapeShifter>
00129 {
00130 static const char* value(const topic_tools::ShapeShifter& m) { return m.getDataType().c_str(); }
00131
00132
00133 static const char* value() { return "*"; }
00134 };
00135
00136 template<>
00137 struct Definition<topic_tools::ShapeShifter>
00138 {
00139 static const char* value(const topic_tools::ShapeShifter& m) { return m.getMessageDefinition().c_str(); }
00140 };
00141
00142 }
00143
00144
00145 namespace serialization
00146 {
00147
00148 template<>
00149 struct Serializer<topic_tools::ShapeShifter>
00150 {
00151 template<typename Stream>
00152 inline static void write(Stream& stream, const topic_tools::ShapeShifter& m) {
00153 m.write(stream);
00154 }
00155
00156 template<typename Stream>
00157 inline static void read(Stream& stream, topic_tools::ShapeShifter& m)
00158 {
00159 m.read(stream);
00160 }
00161
00162 inline static uint32_t serializedLength(const topic_tools::ShapeShifter& m) {
00163 return m.size();
00164 }
00165 };
00166
00167
00168 template<>
00169 struct PreDeserialize<topic_tools::ShapeShifter>
00170 {
00171 static void notify(const PreDeserializeParams<topic_tools::ShapeShifter>& params)
00172 {
00173 std::string md5 = (*params.connection_header)["md5sum"];
00174 std::string datatype = (*params.connection_header)["type"];
00175 std::string msg_def = (*params.connection_header)["message_definition"];
00176 std::string latching = (*params.connection_header)["latching"];
00177
00178 params.message->morph(md5, datatype, msg_def, latching);
00179 }
00180 };
00181
00182 }
00183
00184 }
00185
00186
00187
00188
00189
00190 namespace topic_tools
00191 {
00192
00193
00194
00195
00196 template<class M>
00197 boost::shared_ptr<M> ShapeShifter::instantiate() const
00198 {
00199 if (!typed)
00200 throw ShapeShifterException("Tried to instantiate message from an untyped shapeshifter.");
00201
00202 if (ros::message_traits::datatype<M>() != getDataType())
00203 throw ShapeShifterException("Tried to instantiate message without matching datatype.");
00204
00205 if (ros::message_traits::md5sum<M>() != getMD5Sum())
00206 throw ShapeShifterException("Tried to instantiate message without matching md5sum.");
00207
00208 boost::shared_ptr<M> p(boost::make_shared<M>());
00209
00210 ros::serialization::IStream s(msgBuf, msgBufUsed);
00211 ros::serialization::deserialize(s, *p);
00212
00213 return p;
00214 }
00215
00216 template<typename Stream>
00217 void ShapeShifter::write(Stream& stream) const {
00218 if (msgBufUsed > 0)
00219 memcpy(stream.advance(msgBufUsed), msgBuf, msgBufUsed);
00220 }
00221
00222 template<typename Stream>
00223 void ShapeShifter::read(Stream& stream)
00224 {
00225 stream.getLength();
00226 stream.getData();
00227
00228
00229 if (stream.getLength() > msgBufAlloc)
00230 {
00231 delete[] msgBuf;
00232 msgBuf = new uint8_t[stream.getLength()];
00233 msgBufAlloc = stream.getLength();
00234 }
00235 msgBufUsed = stream.getLength();
00236 memcpy(msgBuf, stream.getData(), stream.getLength());
00237 }
00238
00239 }
00240
00241
00242 #endif
00243