header.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
35 #include "ros/header.h"
36 
37 #include "console_bridge/console.h"
38 
39 #include <sstream>
40 #include <boost/utility/string_ref.hpp>
41 #include <cstring>
42 #include <cerrno>
43 
44 #define SROS_SERIALIZE_PRIMITIVE(ptr, data) { memcpy(ptr, &data, sizeof(data)); ptr += sizeof(data); }
45 #define SROS_SERIALIZE_BUFFER(ptr, data, data_size) { if (data_size > 0) { memcpy(ptr, data, data_size); ptr += data_size; } }
46 #define SROS_DESERIALIZE_PRIMITIVE(ptr, data) { memcpy(&data, ptr, sizeof(data)); ptr += sizeof(data); }
47 #define SROS_DESERIALIZE_BUFFER(ptr, data, data_size) { if (data_size > 0) { memcpy(data, ptr, data_size); ptr += data_size; } }
48 
49 // Remove this when no longer supporting platforms with libconsole-bridge-dev < 0.3.0,
50 // in particular Debian Jessie: https://packages.debian.org/jessie/libconsole-bridge-dev
51 #ifndef CONSOLE_BRIDGE_logError
52 # define CONSOLE_BRIDGE_logError(fmt, ...) \
53  console_bridge::log(__FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_ERROR, fmt, ##__VA_ARGS__)
54 #endif
55 
56 using namespace std;
57 
58 namespace ros
59 {
60 
61 Header::Header()
62 : read_map_(new M_string())
63 {
64 
65 }
66 
68 {
69 
70 }
71 
72 bool Header::parse(const boost::shared_array<uint8_t>& buffer, uint32_t size, std::string& error_msg)
73 {
74  return parse(buffer.get(), size, error_msg);
75 }
76 
77 bool Header::parse(uint8_t* buffer, uint32_t size, std::string& error_msg)
78 {
79  std::string key_;
80  uint8_t* buf = buffer;
81  while (buf < buffer + size)
82  {
83  uint32_t len;
85 
86  if (len > 1000000)
87  {
88  error_msg = "Received an invalid TCPROS header. Each element must be prepended by a 4-byte length.";
89  CONSOLE_BRIDGE_logError("%s", error_msg.c_str());
90 
91  return false;
92  }
93 
94  boost::string_ref line((char*)buf, len);
95 
96  buf += len;
97 
98  //printf(":%s:\n", line.c_str());
99  size_t eqpos = line.find_first_of("=");
100  if (eqpos == string::npos)
101  {
102  error_msg = "Received an invalid TCPROS header. Each line must have an equals sign.";
103  CONSOLE_BRIDGE_logError("%s", error_msg.c_str());
104 
105  return false;
106  }
107  boost::string_ref key_ref = line.substr(0, eqpos);
108  boost::string_ref value_ref = line.substr(eqpos+1);
109 
110  key_.assign(key_ref.data(), key_ref.length());
111 
112  (*read_map_)[key_].assign(value_ref.data(), value_ref.length());
113  }
114 
115  return true;
116 }
117 
118 bool Header::getValue(const std::string& key, std::string& value) const
119 {
120  M_string::const_iterator it = read_map_->find(key);
121  if (it == read_map_->end())
122  {
123  return false;
124  }
125 
126  value = it->second;
127 
128  return true;
129 }
130 
131 void Header::write(const M_string& key_vals, boost::shared_array<uint8_t>& buffer, uint32_t& size)
132 {
133  // Calculate the necessary size
134  size = 0;
135  {
136  M_string::const_iterator it = key_vals.begin();
137  M_string::const_iterator end = key_vals.end();
138  for (; it != end; ++it)
139  {
140  const std::string& key = it->first;
141  const std::string& value = it->second;
142 
143  size += key.length();
144  size += value.length();
145  size += 1; // = sign
146  size += 4; // 4-byte length
147  }
148  }
149 
150  if (size == 0)
151  {
152  return;
153  }
154 
155  buffer.reset(new uint8_t[size]);
156  char* ptr = (char*)buffer.get();
157 
158  // Write the data
159  {
160  M_string::const_iterator it = key_vals.begin();
161  M_string::const_iterator end = key_vals.end();
162  for (; it != end; ++it)
163  {
164  const std::string& key = it->first;
165  const std::string& value = it->second;
166 
167  uint32_t len = key.length() + value.length() + 1;
168  SROS_SERIALIZE_PRIMITIVE(ptr, len);
169  SROS_SERIALIZE_BUFFER(ptr, key.data(), key.length());
170  static const char equals = '=';
171  SROS_SERIALIZE_PRIMITIVE(ptr, equals);
172  SROS_SERIALIZE_BUFFER(ptr, value.data(), value.length());
173  }
174  }
175 
176  assert(ptr == (char*)buffer.get() + size);
177 }
178 
179 }
#define SROS_SERIALIZE_BUFFER(ptr, data, data_size)
Definition: header.cpp:45
#define SROS_DESERIALIZE_PRIMITIVE(ptr, data)
Definition: header.cpp:46
std::map< std::string, std::string > M_string
Definition: datatypes.h:45
bool parse(const boost::shared_array< uint8_t > &buffer, uint32_t size, std::string &error_msg)
Parse a header out of a buffer of data.
Definition: header.cpp:72
static void write(const M_string &key_vals, boost::shared_array< uint8_t > &buffer, uint32_t &size)
Definition: header.cpp:131
Definition: datatypes.h:40
bool getValue(const std::string &key, std::string &value) const
Get a value from a parsed header.
Definition: header.cpp:118
#define CONSOLE_BRIDGE_logError(fmt,...)
Definition: header.cpp:52
#define SROS_SERIALIZE_PRIMITIVE(ptr, data)
Definition: header.cpp:44
M_stringPtr read_map_
Definition: header.h:84


cpp_common
Author(s): John Faust
autogenerated on Sat Apr 6 2019 02:52:20