network_utils.h
Go to the documentation of this file.
1 /*
2 * Unpublished Copyright (c) 2009-2017 AutonomouStuff, LLC, All Rights Reserved.
3 *
4 * This file is part of the network_interface ROS 1.0 driver which is released under the MIT license.
5 * See file LICENSE included with this software or go to https://opensource.org/licenses/MIT for full license details.
6 */
7 
8 #ifndef NETWORK_INTERFACE_NETWORK_UTILS_H
9 #define NETWORK_INTERFACE_NETWORK_UTILS_H
10 
11 #include <cstddef>
12 #include <cstdint>
13 #include <vector>
14 #include <typeinfo>
15 #include <cstring>
16 
17 namespace AS
18 {
19 namespace Network
20 {
22 {
23  BE = 0,
25 };
26 
27 inline bool system_is_big_endian()
28 {
29  union
30  {
31  uint32_t i;
32  char c[4];
33  } big_int = {0x12345678};
34 
35  return big_int.c[0] == 1;
36 }
37 
38 // little-endian
39 template<typename T>
40 T read_le(uint8_t* bufArray,
41  const uint32_t& size,
42  const uint32_t& offset,
43  const float& factor,
44  const uint32_t& valueOffset)
45 {
46  uint64_t rcvData = 0;
47 
48  for (uint32_t i = size; i > 0; i--)
49  {
50  rcvData <<= 8;
51  // Need to use -1 because array is 0-based
52  // and offset is not.
53  rcvData |= bufArray[(offset - 1) + i];
54  }
55 
56  T retVal = 0;
57  std::memcpy(&retVal, (system_is_big_endian()) ? &rcvData + sizeof(uint64_t) - sizeof(T) : &rcvData, sizeof(T));
58  retVal *= (T) factor;
59  retVal += valueOffset;
60 
61  return retVal;
62 };
63 
64 template<typename T>
65 T read_le(unsigned char* bufArray,
66  const unsigned int& size,
67  const unsigned int& offset)
68 {
69  return read_le<T>(bufArray, size, offset, 1.0, 0);
70 };
71 
72 template<typename T>
73 std::vector<uint8_t> write_le(T *source)
74 {
75  std::vector<uint8_t> ret_val;
76 
77  if (sizeof(source))
78  return ret_val;
79 
80  if (typeid(source) == typeid(float) || typeid(source) == typeid(double) || typeid(source) == typeid(long double)) // NOLINT
81  return ret_val;
82 
83  T mask = 0xFF;
84 
85  while ((*source & mask) > 0)
86  {
87  ret_val.push_back(uint8_t(*source & mask));
88  mask <<= 8;
89  }
90 
91  return ret_val;
92 };
93 
94 // big-endian
95 template<typename T>
96 T read_be(unsigned char* bufArray,
97  const unsigned int& size,
98  const unsigned int& offset,
99  const float& factor,
100  const unsigned int& valueOffset)
101 {
102  uint64_t rcvData = 0;
103 
104  for (unsigned int i = 0; i < size; i++)
105  {
106  rcvData <<= 8;
107  rcvData |= bufArray[(offset) + i];
108  }
109 
110  T retVal;
111  std::memcpy(&retVal, (system_is_big_endian()) ? &rcvData + sizeof(uint64_t) - sizeof(T) : &rcvData, sizeof(T));
112  retVal *= (T) factor;
113  retVal += valueOffset;
114 
115  return retVal;
116 };
117 
118 template<typename T>
119 T read_be(unsigned char* bufArray,
120  const unsigned int& size,
121  const unsigned int& offset)
122 {
123  return read_be<T>(bufArray, size, offset, 1.0, 0);
124 }
125 
126 template<typename T>
127 std::vector<uint8_t> write_be(T *source)
128 {
129  std::vector<uint8_t> ret_val;
130 
131  if (typeid(source) == typeid(float) || typeid(source) == typeid(double) || typeid(source) == typeid(long double)) // NOLINT
132  {
133  return ret_val;
134  }
135 
136  T mask = 0xFF;
137 
138  int shift = 8 * (sizeof(T) - 1);
139  mask <<= shift;
140 
141  while (mask > 0)
142  {
143  // //printf("mask: 0x%016x\n",mask);
144  ret_val.push_back(uint8_t(((*source) & mask) >> shift));
145  shift -= 8;
146  mask >>= 8;
147  }
148 
149  return ret_val;
150 };
151 
152 inline int32_t find_magic_word(uint8_t *in, uint32_t buf_size, size_t magic_word)
153 {
154  bool packet_found = false;
155  uint32_t i = 0;
156  uint32_t chunk;
157  const uint32_t chunk_bytes = 4;
158 
159  while (!packet_found && buf_size >= chunk_bytes)
160  {
161  chunk = read_be<uint32_t>(in, chunk_bytes, i);
162 
163  if (chunk == magic_word)
164  {
165  packet_found = true;
166  return i;
167  }
168 
169  i++;
170  buf_size--;
171  }
172 
173  // Just in case
174  return -1;
175 };
176 } // namespace Network
177 } // namespace AS
178 
179 #endif // NETWORK_INTERFACE_NETWORK_UTILS_H
T read_le(uint8_t *bufArray, const uint32_t &size, const uint32_t &offset, const float &factor, const uint32_t &valueOffset)
Definition: network_utils.h:40
std::vector< uint8_t > write_be(T *source)
bool system_is_big_endian()
Definition: network_utils.h:27
std::vector< uint8_t > write_le(T *source)
Definition: network_utils.h:73
T read_be(unsigned char *bufArray, const unsigned int &size, const unsigned int &offset, const float &factor, const unsigned int &valueOffset)
Definition: network_utils.h:96
int32_t find_magic_word(uint8_t *in, uint32_t buf_size, size_t magic_word)


network_interface
Author(s): Joshua Whitley , Daniel Stanek , Joe Kale
autogenerated on Thu Jun 6 2019 19:57:06