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
00036 #ifndef ROS_INTROSPECTION_HELPER_H
00037 #define ROS_INTROSPECTION_HELPER_H
00038
00039 #include <functional>
00040 #include "ros_type_introspection/utils/variant.hpp"
00041 #include "absl/types/span.h"
00042
00043 namespace RosIntrospection{
00044
00045
00046
00047 inline int print_number(char* buffer, uint16_t value)
00048 {
00049 const char DIGITS[] =
00050 "00010203040506070809"
00051 "10111213141516171819"
00052 "20212223242526272829"
00053 "30313233343536373839"
00054 "40414243444546474849"
00055 "50515253545556575859"
00056 "60616263646566676869"
00057 "70717273747576777879"
00058 "80818283848586878889"
00059 "90919293949596979899";
00060 if (value < 10)
00061 {
00062 buffer[0] = static_cast<char>('0' + value);
00063 return 1;
00064 }
00065 else if (value < 100) {
00066 value *= 2;
00067 buffer[0] = DIGITS[ value ];
00068 buffer[1] = DIGITS[ value+1 ];
00069 return 2;
00070 }
00071 else{
00072 return sprintf( buffer,"%d", value );
00073 }
00074 }
00075
00076
00077
00078 template <typename T> inline void ReadFromBuffer( const absl::Span<uint8_t>& buffer, size_t& offset, T& destination)
00079 {
00080 if ( offset + sizeof(T) > buffer.size() )
00081 {
00082 throw std::runtime_error("Buffer overrun in RosIntrospection::ReadFromBuffer");
00083 }
00084 destination = (*( reinterpret_cast<const T*>( &(buffer.data()[offset]) ) ) );
00085 offset += sizeof(T);
00086 }
00087
00088 template <> inline void ReadFromBuffer( const absl::Span<uint8_t>& buffer, size_t& offset, std::string& destination)
00089 {
00090 uint32_t string_size = 0;
00091 ReadFromBuffer( buffer, offset, string_size );
00092
00093 if( offset + string_size > buffer.size())
00094 {
00095 throw std::runtime_error("Buffer overrun in RosIntrospection::ReadFromBuffer");
00096 }
00097
00098 const char* buffer_ptr = reinterpret_cast<const char*>( &buffer[offset] );
00099 offset += string_size;
00100
00101 destination.assign( buffer_ptr, string_size );
00102 }
00103
00104 template <typename T> inline
00105 Variant ReadFromBufferToVariant( const absl::Span<uint8_t>& buffer, size_t& offset)
00106 {
00107 T destination;
00108 ReadFromBuffer(buffer, offset, destination);
00109 return Variant(destination);
00110 }
00111
00112 inline Variant ReadFromBufferToVariant(BuiltinType id, const absl::Span<uint8_t>& buffer, size_t& offset)
00113 {
00114 switch(id)
00115 {
00116 case BOOL: return ReadFromBufferToVariant<bool>(buffer,offset);
00117 case CHAR: return ReadFromBufferToVariant<char>(buffer,offset);
00118 case BYTE:
00119 case UINT8: return ReadFromBufferToVariant<uint8_t>(buffer,offset);
00120 case UINT16: return ReadFromBufferToVariant<uint16_t>(buffer,offset);
00121 case UINT32: return ReadFromBufferToVariant<uint32_t>(buffer,offset);
00122 case UINT64: return ReadFromBufferToVariant<uint64_t>(buffer,offset);
00123
00124 case INT8: return ReadFromBufferToVariant<int8_t>(buffer,offset);
00125 case INT16: return ReadFromBufferToVariant<int16_t>(buffer,offset);
00126 case INT32: return ReadFromBufferToVariant<int32_t>(buffer,offset);
00127 case INT64: return ReadFromBufferToVariant<int64_t>(buffer,offset);
00128
00129 case FLOAT32: return ReadFromBufferToVariant<float>(buffer,offset);
00130 case FLOAT64: return ReadFromBufferToVariant<double>(buffer,offset);
00131
00132 case TIME: {
00133 ros::Time tmp;
00134 ReadFromBuffer( buffer, offset, tmp.sec );
00135 ReadFromBuffer( buffer, offset, tmp.nsec );
00136 return tmp;
00137 }
00138 case DURATION: {
00139 ros::Duration tmp;
00140 ReadFromBuffer( buffer, offset, tmp.sec );
00141 ReadFromBuffer( buffer, offset, tmp.nsec );
00142 return tmp;
00143 }
00144
00145 case STRING: {
00146 uint32_t string_size = 0;
00147 ReadFromBuffer( buffer, offset, string_size );
00148 if( offset + string_size > buffer.size()) {
00149 throw std::runtime_error("Buffer overrun");
00150 }
00151 Variant var_string(reinterpret_cast<const char*>( &buffer[offset] ), string_size );
00152 offset += string_size;
00153 return var_string;
00154 }
00155 case OTHER: return -1;
00156 default: break;
00157 }
00158 throw std::runtime_error( "unsupported builtin type value");
00159 }
00160
00161
00162 }
00163
00164
00165 #endif