00001 #include <ros/ros.h> 00002 #include <ros/package.h> 00003 #include <std_msgs/String.h> 00004 #include <topic_tools/shape_shifter.h> 00005 00006 #include "rosR.h" 00007 00008 #ifndef PUBLISHER_H 00009 #define PUBLISHER_H 00010 00011 struct OStreamR : public StreamR 00012 { 00013 OStreamR(uint8_t* data, uint32_t count) : StreamR(data, count) 00014 {} 00015 00016 template<typename T> 00017 inline void next(const T& t) { 00018 ros::serialization::serialize(*this, t); 00019 } 00020 00021 template<typename T> 00022 inline OStreamR& operator<<(const T& t) { 00023 ros::serialization::serialize(*this, t); 00024 return *this; 00025 } 00026 }; 00027 00028 00029 class PublisheR { 00030 public: 00031 PublisheR(NodeR* handle, const char* cTopic, const char* cMsgType, const char* cMsgDefinition, const char* cMsgMD5){ 00032 00033 m_rosPublisher = new ros::Publisher(); 00034 00035 topic_tools::ShapeShifter *ss = new topic_tools::ShapeShifter(); 00036 ss->morph(cMsgMD5, cMsgType, cMsgDefinition, ""); 00037 *m_rosPublisher = ss->advertise(*handle->getHandle(), cTopic, 1); 00038 00039 m_strMsgType = cMsgType; 00040 m_strMsgDefinition = cMsgDefinition; 00041 m_strMsgMD5 = cMsgMD5; 00042 00043 m_iBufferSize = 10000000; 00044 00045 m_ui8Buffer = new boost::shared_array<uint8_t> (new uint8_t[m_iBufferSize]); 00046 m_osStream = new OStreamR(m_ui8Buffer->get(), m_iBufferSize); 00047 }; 00048 ~PublisheR() { 00049 delete m_rosPublisher; 00050 delete m_osStream; 00051 }; 00052 00053 const char* getMessageType() { 00054 return m_strMsgType.c_str(); }; 00055 const char* getMessageDefinition() { 00056 return m_strMsgDefinition.c_str(); }; 00057 const char* getMessageMD5() { 00058 return m_strMsgMD5.c_str(); }; 00059 const char* getTopic() { 00060 return m_rosPublisher->getTopic().c_str(); }; 00061 00062 void publish(){ 00063 topic_tools::ShapeShifter ss; 00064 ss.morph(m_strMsgMD5, m_strMsgType, m_strMsgDefinition, ""); 00065 00066 if(m_osStream->getLength() != m_iBufferSize){ 00067 m_osStream->advance(-(m_iBufferSize-m_osStream->getLength())); 00068 } 00069 00070 ros::serialization::OStream ssStream(m_osStream->getData(), m_osStream->getLength()); 00071 ss.read(ssStream); 00072 00073 m_rosPublisher->publish(ss); 00074 } 00075 00076 OStreamR* getMessageStream(){ 00077 if(m_osStream->getLength() != m_iBufferSize){ 00078 m_osStream->advance(-(m_iBufferSize-m_osStream->getLength())); 00079 } 00080 00081 return m_osStream; 00082 } 00083 00084 private: 00085 ros::Publisher* m_rosPublisher; 00086 std::string m_strMsgType; 00087 std::string m_strMsgDefinition; 00088 std::string m_strMsgMD5; 00089 00090 boost::shared_array<uint8_t> *m_ui8Buffer; 00091 int m_iBufferSize; 00092 OStreamR *m_osStream; 00093 }; 00094 00095 00096 PublisheR* rrosPublisher( NodeR* handle, 00097 const char* topic, 00098 const char* type, 00099 const char* msg_def, 00100 const char* msg_md5); 00101 00102 const char* rrosPublisherGetTopic(PublisheR* publisher); 00103 const char* rrosPublisherGetMessageType(PublisheR* publisher); 00104 const char* rrosPublisherGetMessageMD5(PublisheR* publisher); 00105 const char* rrosPublisherGetMessageDefinition(PublisheR* publisher); 00106 00107 void rrosPublish(PublisheR* publisher); 00108 00109 OStreamR* rrosPublisherGetMessageStream(PublisheR *publisher); 00110 00111 void rros_stream_write_bool (OStreamR *s, bool val); 00112 void rros_stream_write_int8 (OStreamR *s, signed char val); 00113 void rros_stream_write_uint8 (OStreamR *s, unsigned char val); 00114 void rros_stream_write_int16 (OStreamR *s, signed short val); 00115 void rros_stream_write_uint16 (OStreamR *s, unsigned short val); 00116 void rros_stream_write_int32 (OStreamR *s, signed int val); 00117 void rros_stream_write_uint32 (OStreamR *s, unsigned int val); 00118 void rros_stream_write_int64 (OStreamR *s, signed long val); 00119 void rros_stream_write_uint64 (OStreamR *s, unsigned long val); 00120 void rros_stream_write_float32 (OStreamR *s, float val); 00121 void rros_stream_write_float64 (OStreamR *s, double val); 00122 void rros_stream_write_string (OStreamR *s, char* val); 00123 00124 void rros_stream_write_int8_array (OStreamR *s, std::vector<signed char>* vec); 00125 void rros_stream_write_uint8_array (OStreamR *s, std::vector<unsigned char>* vec); 00126 void rros_stream_write_int16_array (OStreamR *s, std::vector<signed short>* vec); 00127 void rros_stream_write_uint16_array (OStreamR *s, std::vector<unsigned short>* vec); 00128 void rros_stream_write_int32_array (OStreamR *s, std::vector<signed int>* vec); 00129 void rros_stream_write_uint32_array (OStreamR *s, std::vector<unsigned int>* vec); 00130 void rros_stream_write_int64_array (OStreamR *s, std::vector<signed long>* vec); 00131 void rros_stream_write_uint64_array (OStreamR *s, std::vector<unsigned long>* vec); 00132 void rros_stream_write_float32_array (OStreamR *s, std::vector<float>* vec); 00133 void rros_stream_write_float64_array (OStreamR *s, std::vector<double>* vec); 00134 void rros_stream_write_string_array (OStreamR *s, std::vector<std::string>* vec); 00135 00136 #endif