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 #include "ros/header.h"
00036
00037 #include "console_bridge/console.h"
00038
00039 #include <sstream>
00040
00041 #include <cstring>
00042 #include <cerrno>
00043
00044 #define SROS_SERIALIZE_PRIMITIVE(ptr, data) { memcpy(ptr, &data, sizeof(data)); ptr += sizeof(data); }
00045 #define SROS_SERIALIZE_BUFFER(ptr, data, data_size) { if (data_size > 0) { memcpy(ptr, data, data_size); ptr += data_size; } }
00046 #define SROS_DESERIALIZE_PRIMITIVE(ptr, data) { memcpy(&data, ptr, sizeof(data)); ptr += sizeof(data); }
00047 #define SROS_DESERIALIZE_BUFFER(ptr, data, data_size) { if (data_size > 0) { memcpy(data, ptr, data_size); ptr += data_size; } }
00048
00049
00050 using namespace std;
00051
00052 namespace ros
00053 {
00054
00055 Header::Header()
00056 : read_map_(new M_string())
00057 {
00058
00059 }
00060
00061 Header::~Header()
00062 {
00063
00064 }
00065
00066 bool Header::parse(const boost::shared_array<uint8_t>& buffer, uint32_t size, std::string& error_msg)
00067 {
00068 return parse(buffer.get(), size, error_msg);
00069 }
00070
00071 bool Header::parse(uint8_t* buffer, uint32_t size, std::string& error_msg)
00072 {
00073 uint8_t* buf = buffer;
00074 while (buf < buffer + size)
00075 {
00076 uint32_t len;
00077 SROS_DESERIALIZE_PRIMITIVE(buf, len);
00078
00079 if (len > 1000000)
00080 {
00081 error_msg = "Received an invalid TCPROS header. Each element must be prepended by a 4-byte length.";
00082 logError("%s", error_msg.c_str());
00083
00084 return false;
00085 }
00086
00087 std::string line((char*)buf, len);
00088
00089 buf += len;
00090
00091
00092 size_t eqpos = line.find_first_of("=", 0);
00093 if (eqpos == string::npos)
00094 {
00095 error_msg = "Received an invalid TCPROS header. Each line must have an equals sign.";
00096 logError("%s", error_msg.c_str());
00097
00098 return false;
00099 }
00100 string key = line.substr(0, eqpos);
00101 string value = line.substr(eqpos+1);
00102
00103 (*read_map_)[key] = value;
00104 }
00105
00106 return true;
00107 }
00108
00109 bool Header::getValue(const std::string& key, std::string& value) const
00110 {
00111 M_string::const_iterator it = read_map_->find(key);
00112 if (it == read_map_->end())
00113 {
00114 return false;
00115 }
00116
00117 value = it->second;
00118
00119 return true;
00120 }
00121
00122 void Header::write(const M_string& key_vals, boost::shared_array<uint8_t>& buffer, uint32_t& size)
00123 {
00124
00125 size = 0;
00126 {
00127 M_string::const_iterator it = key_vals.begin();
00128 M_string::const_iterator end = key_vals.end();
00129 for (; it != end; ++it)
00130 {
00131 const std::string& key = it->first;
00132 const std::string& value = it->second;
00133
00134 size += key.length();
00135 size += value.length();
00136 size += 1;
00137 size += 4;
00138 }
00139 }
00140
00141 if (size == 0)
00142 {
00143 return;
00144 }
00145
00146 buffer.reset(new uint8_t[size]);
00147 char* ptr = (char*)buffer.get();
00148
00149
00150 {
00151 M_string::const_iterator it = key_vals.begin();
00152 M_string::const_iterator end = key_vals.end();
00153 for (; it != end; ++it)
00154 {
00155 const std::string& key = it->first;
00156 const std::string& value = it->second;
00157
00158 uint32_t len = key.length() + value.length() + 1;
00159 SROS_SERIALIZE_PRIMITIVE(ptr, len);
00160 SROS_SERIALIZE_BUFFER(ptr, key.data(), key.length());
00161 static const char equals = '=';
00162 SROS_SERIALIZE_PRIMITIVE(ptr, equals);
00163 SROS_SERIALIZE_BUFFER(ptr, value.data(), value.length());
00164 }
00165 }
00166
00167 assert(ptr == (char*)buffer.get() + size);
00168 }
00169
00170 }