54 std::copy(buffer, buffer +
sizeof(
double), reinterpret_cast<uint8_t*>(&diff_loc));
75 std::copy(buffer, buffer +
sizeof(
float), reinterpret_cast<uint8_t*>(&diff_loc));
96 std::copy(buffer, buffer+2, reinterpret_cast<uint8_t*>(&diff_loc));
104 bool parseInt16(
const std::string&
string, int16_t& value, int32_t base)
114 intermd <= std::numeric_limits<int16_t>::max() &&
115 intermd >= std::numeric_limits<int16_t>::min())
117 value =
static_cast<int16_t
>(intermd);
132 std::copy(buffer, buffer+4, reinterpret_cast<uint8_t*>(&diff_loc));
140 bool parseInt32(
const std::string&
string, int32_t& value, int32_t base)
149 bool parseUInt8(
const std::string&
string, uint8_t& value, int32_t base)
160 value =
static_cast<uint8_t
>(intermd);
175 std::copy(buffer, buffer+2, reinterpret_cast<uint8_t*>(&number));
183 bool parseUInt16(
const std::string&
string, uint16_t& value, int32_t base)
194 value =
static_cast<uint16_t
>(intermd);
209 std::copy(buffer, buffer+4, reinterpret_cast<uint8_t*>(&diff_loc));
217 bool parseUInt32(
const std::string&
string, uint32_t& value, int32_t base)
228 uint32_t hours =
static_cast<uint32_t
>(utc_double) / 10000;
229 uint32_t minutes = (
static_cast<uint32_t
>(utc_double) - hours * 10000) / 100;
230 double seconds = utc_double -
static_cast<double>(hours * 10000 + minutes * 100);
231 seconds +=
static_cast<double> (hours * 3600 + minutes * 60);
241 uint32_t whole_degrees =
static_cast<uint32_t
>(dms) / 100;
242 double minutes = dms -
static_cast<double>(whole_degrees * 100);
243 double degrees =
static_cast<double>(whole_degrees) + minutes / 60.0;
261 time_t time_now = time(0);
262 struct tm * timeinfo;
266 timeinfo = gmtime(&time_now);
268 uint32_t hours =
static_cast<uint32_t
>(utc_double) / 10000;
269 uint32_t minutes = (
static_cast<uint32_t
>(utc_double) - hours * 10000) / 100;
270 uint32_t seconds = (
static_cast<uint32_t
>(utc_double) - hours * 10000 - minutes * 100);
273 timeinfo->tm_hour = hours;
274 timeinfo->tm_min = minutes;
275 timeinfo->tm_sec = seconds;
287 time_t date = timegm(timeinfo);
299 double cy = cos(yaw * 0.5);
300 double sy = sin(yaw * 0.5);
301 double cp = cos(pitch * 0.5);
302 double sp = sin(pitch * 0.5);
303 double cr = cos(roll * 0.5);
304 double sr = sin(roll * 0.5);
306 geometry_msgs::Quaternion q;
307 q.w = cr * cp * cy + sr * sp * sy;
308 q.x = sr * cp * cy - cr * sp * sy;
309 q.y = cr * sp * cy + sr * cp * sy;
310 q.z = cr * cp * sy - sr * sp * cy;
317 if (period_user <= 500 && period_user >= 10)
return period_user;
320 return period_user/1000;
bool toFloat(const std::string &string, float &value)
Interprets the contents of "string" as a floating point number of type float.
bool toDouble(const std::string &string, double &value)
Interprets the contents of "string" as a floating point number of type double It stores the "string"'...
float parseFloat(const uint8_t *buffer)
Converts a 4-byte-buffer into a float.
double convertUTCDoubleToSeconds(double utc_double)
Converts UTC time from the without-colon-delimiter format to the number-of-seconds-since-midnight for...
bool toUInt32(const std::string &string, uint32_t &value, int32_t base)
Interprets the contents of "string" as a floating point number of whatever unsigned integer type your...
uint16_t parseUInt16(const uint8_t *buffer)
Converts a 2-byte-buffer into an unsigned 16-bit integer.
Declares lower-level string utility functions used when parsing messages.
uint32_t parseUInt32(const uint8_t *buffer)
Converts a 4-byte-buffer into an unsigned 32-bit integer.
double convertDMSToDegrees(double dms)
Converts latitude or longitude from the DMS notation (in the without-colon-delimiter format)...
Declares utility functions used when parsing messages.
uint32_t convertUserPeriodToRxCommand(uint32_t period_user)
Transforms the input polling period [milliseconds] into a uint32_t number that can be appended to eit...
time_t convertUTCtoUnix(double utc_double)
Converts UTC time from the without-colon-delimiter format to Unix Epoch time (a number-of-seconds-sin...
bool toInt32(const std::string &string, int32_t &value, int32_t base)
Interprets the contents of "string" as a floating point number of whatever integer type your system h...
double parseDouble(const uint8_t *buffer)
Converts an 8-byte-buffer into a double.
int16_t parseInt16(const uint8_t *buffer)
Converts a 2-byte-buffer into a signed 16-bit integer.
int32_t parseInt32(const uint8_t *buffer)
Converts a 4-byte-buffer into a signed 32-bit integer.
bool parseUInt8(const std::string &string, uint8_t &value, int32_t base)
Interprets the contents of "string" as a unsigned integer number of type uint8_t. ...
geometry_msgs::Quaternion convertEulerToQuaternion(double yaw, double pitch, double roll)
Transforms Euler angles to a quaternion.