Soft robot team / ros_lib_melodic

ros melodic library with custom message

Dependents:   Robot_team1_QEI_Douglas Robot_team1

ros_lib/ros/msg.h@0:020db18a476d, 2019-10-30 (annotated)

Committer:
scarter1
Date:
Wed Oct 30 14:59:49 2019 +0000
Revision:
0:020db18a476d
melodic library;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
scarter1 0:020db18a476d 1 /*
scarter1 0:020db18a476d 2 * Software License Agreement (BSD License)
scarter1 0:020db18a476d 3 *
scarter1 0:020db18a476d 4 * Copyright (c) 2011, Willow Garage, Inc.
scarter1 0:020db18a476d 5 * All rights reserved.
scarter1 0:020db18a476d 6 *
scarter1 0:020db18a476d 7 * Redistribution and use in source and binary forms, with or without
scarter1 0:020db18a476d 8 * modification, are permitted provided that the following conditions
scarter1 0:020db18a476d 9 * are met:
scarter1 0:020db18a476d 10 *
scarter1 0:020db18a476d 11 * * Redistributions of source code must retain the above copyright
scarter1 0:020db18a476d 12 * notice, this list of conditions and the following disclaimer.
scarter1 0:020db18a476d 13 * * Redistributions in binary form must reproduce the above
scarter1 0:020db18a476d 14 * copyright notice, this list of conditions and the following
scarter1 0:020db18a476d 15 * disclaimer in the documentation and/or other materials provided
scarter1 0:020db18a476d 16 * with the distribution.
scarter1 0:020db18a476d 17 * * Neither the name of Willow Garage, Inc. nor the names of its
scarter1 0:020db18a476d 18 * contributors may be used to endorse or promote prducts derived
scarter1 0:020db18a476d 19 * from this software without specific prior written permission.
scarter1 0:020db18a476d 20 *
scarter1 0:020db18a476d 21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
scarter1 0:020db18a476d 22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
scarter1 0:020db18a476d 23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
scarter1 0:020db18a476d 24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
scarter1 0:020db18a476d 25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
scarter1 0:020db18a476d 26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
scarter1 0:020db18a476d 27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
scarter1 0:020db18a476d 28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
scarter1 0:020db18a476d 29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
scarter1 0:020db18a476d 30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
scarter1 0:020db18a476d 31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
scarter1 0:020db18a476d 32 * POSSIBILITY OF SUCH DAMAGE.
scarter1 0:020db18a476d 33 */
scarter1 0:020db18a476d 34
scarter1 0:020db18a476d 35 #ifndef _ROS_MSG_H_
scarter1 0:020db18a476d 36 #define _ROS_MSG_H_
scarter1 0:020db18a476d 37
scarter1 0:020db18a476d 38 #include <stdint.h>
scarter1 0:020db18a476d 39 #include <stddef.h>
scarter1 0:020db18a476d 40
scarter1 0:020db18a476d 41 namespace ros
scarter1 0:020db18a476d 42 {
scarter1 0:020db18a476d 43
scarter1 0:020db18a476d 44 /* Base Message Type */
scarter1 0:020db18a476d 45 class Msg
scarter1 0:020db18a476d 46 {
scarter1 0:020db18a476d 47 public:
scarter1 0:020db18a476d 48 virtual int serialize(unsigned char *outbuffer) const = 0;
scarter1 0:020db18a476d 49 virtual int deserialize(unsigned char *data) = 0;
scarter1 0:020db18a476d 50 virtual const char * getType() = 0;
scarter1 0:020db18a476d 51 virtual const char * getMD5() = 0;
scarter1 0:020db18a476d 52
scarter1 0:020db18a476d 53 /**
scarter1 0:020db18a476d 54 * @brief This tricky function handles promoting a 32bit float to a 64bit
scarter1 0:020db18a476d 55 * double, so that AVR can publish messages containing float64
scarter1 0:020db18a476d 56 * fields, despite AVV having no native support for double.
scarter1 0:020db18a476d 57 *
scarter1 0:020db18a476d 58 * @param[out] outbuffer pointer for buffer to serialize to.
scarter1 0:020db18a476d 59 * @param[in] f value to serialize.
scarter1 0:020db18a476d 60 *
scarter1 0:020db18a476d 61 * @return number of bytes to advance the buffer pointer.
scarter1 0:020db18a476d 62 *
scarter1 0:020db18a476d 63 */
scarter1 0:020db18a476d 64 static int serializeAvrFloat64(unsigned char* outbuffer, const float f)
scarter1 0:020db18a476d 65 {
scarter1 0:020db18a476d 66 const int32_t* val = (int32_t*) &f;
scarter1 0:020db18a476d 67 int32_t exp = ((*val >> 23) & 255);
scarter1 0:020db18a476d 68 if (exp != 0)
scarter1 0:020db18a476d 69 {
scarter1 0:020db18a476d 70 exp += 1023 - 127;
scarter1 0:020db18a476d 71 }
scarter1 0:020db18a476d 72
scarter1 0:020db18a476d 73 int32_t sig = *val;
scarter1 0:020db18a476d 74 *(outbuffer++) = 0;
scarter1 0:020db18a476d 75 *(outbuffer++) = 0;
scarter1 0:020db18a476d 76 *(outbuffer++) = 0;
scarter1 0:020db18a476d 77 *(outbuffer++) = (sig << 5) & 0xff;
scarter1 0:020db18a476d 78 *(outbuffer++) = (sig >> 3) & 0xff;
scarter1 0:020db18a476d 79 *(outbuffer++) = (sig >> 11) & 0xff;
scarter1 0:020db18a476d 80 *(outbuffer++) = ((exp << 4) & 0xF0) | ((sig >> 19) & 0x0F);
scarter1 0:020db18a476d 81 *(outbuffer++) = (exp >> 4) & 0x7F;
scarter1 0:020db18a476d 82
scarter1 0:020db18a476d 83 // Mark negative bit as necessary.
scarter1 0:020db18a476d 84 if (f < 0)
scarter1 0:020db18a476d 85 {
scarter1 0:020db18a476d 86 *(outbuffer - 1) |= 0x80;
scarter1 0:020db18a476d 87 }
scarter1 0:020db18a476d 88
scarter1 0:020db18a476d 89 return 8;
scarter1 0:020db18a476d 90 }
scarter1 0:020db18a476d 91
scarter1 0:020db18a476d 92 /**
scarter1 0:020db18a476d 93 * @brief This tricky function handles demoting a 64bit double to a
scarter1 0:020db18a476d 94 * 32bit float, so that AVR can understand messages containing
scarter1 0:020db18a476d 95 * float64 fields, despite AVR having no native support for double.
scarter1 0:020db18a476d 96 *
scarter1 0:020db18a476d 97 * @param[in] inbuffer pointer for buffer to deserialize from.
scarter1 0:020db18a476d 98 * @param[out] f pointer to place the deserialized value in.
scarter1 0:020db18a476d 99 *
scarter1 0:020db18a476d 100 * @return number of bytes to advance the buffer pointer.
scarter1 0:020db18a476d 101 */
scarter1 0:020db18a476d 102 static int deserializeAvrFloat64(const unsigned char* inbuffer, float* f)
scarter1 0:020db18a476d 103 {
scarter1 0:020db18a476d 104 uint32_t* val = (uint32_t*)f;
scarter1 0:020db18a476d 105 inbuffer += 3;
scarter1 0:020db18a476d 106
scarter1 0:020db18a476d 107 // Copy truncated mantissa.
scarter1 0:020db18a476d 108 *val = ((uint32_t)(*(inbuffer++)) >> 5 & 0x07);
scarter1 0:020db18a476d 109 *val |= ((uint32_t)(*(inbuffer++)) & 0xff) << 3;
scarter1 0:020db18a476d 110 *val |= ((uint32_t)(*(inbuffer++)) & 0xff) << 11;
scarter1 0:020db18a476d 111 *val |= ((uint32_t)(*inbuffer) & 0x0f) << 19;
scarter1 0:020db18a476d 112
scarter1 0:020db18a476d 113 // Copy truncated exponent.
scarter1 0:020db18a476d 114 uint32_t exp = ((uint32_t)(*(inbuffer++)) & 0xf0) >> 4;
scarter1 0:020db18a476d 115 exp |= ((uint32_t)(*inbuffer) & 0x7f) << 4;
scarter1 0:020db18a476d 116 if (exp != 0)
scarter1 0:020db18a476d 117 {
scarter1 0:020db18a476d 118 *val |= ((exp) - 1023 + 127) << 23;
scarter1 0:020db18a476d 119 }
scarter1 0:020db18a476d 120
scarter1 0:020db18a476d 121 // Copy negative sign.
scarter1 0:020db18a476d 122 *val |= ((uint32_t)(*(inbuffer++)) & 0x80) << 24;
scarter1 0:020db18a476d 123
scarter1 0:020db18a476d 124 return 8;
scarter1 0:020db18a476d 125 }
scarter1 0:020db18a476d 126
scarter1 0:020db18a476d 127 // Copy data from variable into a byte array
scarter1 0:020db18a476d 128 template<typename A, typename V>
scarter1 0:020db18a476d 129 static void varToArr(A arr, const V var)
scarter1 0:020db18a476d 130 {
scarter1 0:020db18a476d 131 for (size_t i = 0; i < sizeof(V); i++)
scarter1 0:020db18a476d 132 arr[i] = (var >> (8 * i));
scarter1 0:020db18a476d 133 }
scarter1 0:020db18a476d 134
scarter1 0:020db18a476d 135 // Copy data from a byte array into variable
scarter1 0:020db18a476d 136 template<typename V, typename A>
scarter1 0:020db18a476d 137 static void arrToVar(V& var, const A arr)
scarter1 0:020db18a476d 138 {
scarter1 0:020db18a476d 139 var = 0;
scarter1 0:020db18a476d 140 for (size_t i = 0; i < sizeof(V); i++)
scarter1 0:020db18a476d 141 var |= (arr[i] << (8 * i));
scarter1 0:020db18a476d 142 }
scarter1 0:020db18a476d 143
scarter1 0:020db18a476d 144 };
scarter1 0:020db18a476d 145
scarter1 0:020db18a476d 146 } // namespace ros
scarter1 0:020db18a476d 147
scarter1 0:020db18a476d 148 #endif