variant.hpp
Go to the documentation of this file.
00001 #ifndef VARIANT_H
00002 #define VARIANT_H
00003 
00004 #include <type_traits>
00005 #include <limits>
00006 #include "ros_type_introspection/builtin_types.hpp"
00007 #include "ros_type_introspection/details/exceptions.hpp"
00008 #include "ros_type_introspection/details/conversion_impl.hpp"
00009 
00010 
00011 namespace RosIntrospection
00012 {
00013 
00014 class VarNumber
00015 {
00016 
00017 public:
00018 
00019   VarNumber() {
00020     _raw_data[8] = OTHER;
00021   }
00022 
00023   template<typename T> VarNumber(T value);
00024 
00025   BuiltinType getTypeID() const;
00026 
00027   template<typename T> T convert( ) const;
00028 
00029   template<typename T> T extract( ) const;
00030 
00031   template <typename T> void assign(const T& value);
00032 
00033 private:
00034   uint8_t _raw_data[9];
00035 
00036 };
00037 
00038 template <typename T> inline
00039 bool operator ==(const VarNumber& var, const T& num)
00040 {
00041   return var.convert<T>() == num;
00042 }
00043 
00044 template <typename T> inline
00045 bool operator ==(const T& num, const VarNumber& var)
00046 {
00047   return var.convert<T>() == num;
00048 }
00049 
00050 //----------------------- Implementation ----------------------------------------------
00051 
00052 
00053 template<typename T>
00054 inline VarNumber::VarNumber(T value)
00055 {
00056   static_assert (std::numeric_limits<T>::is_specialized ||
00057                  std::is_same<T, ros::Time>::value ||
00058                  std::is_same<T, ros::Duration>::value
00059                  , "not a valid type");
00060   assign(value);
00061 }
00062 
00063 inline BuiltinType VarNumber::getTypeID() const {
00064   return static_cast<BuiltinType>(_raw_data[8]);
00065 }
00066 
00067 template<typename T>
00068 inline T VarNumber::extract( ) const
00069 {
00070   static_assert (std::numeric_limits<T>::is_specialized ||
00071                  std::is_same<T, ros::Time>::value ||
00072                  std::is_same<T, ros::Duration>::value
00073                  , "not a valid type");
00074 
00075   if( getTypeID() != RosIntrospection::getType<T>() )
00076   {
00077     throw TypeException("VarNumber::extract -> wrong type");
00078   }
00079   return * reinterpret_cast<const T*>( _raw_data );
00080 }
00081 
00082 template <typename T>
00083 inline void VarNumber::assign(const T& value)
00084 {
00085   static_assert (std::numeric_limits<T>::is_specialized ||
00086                  std::is_same<T, ros::Time>::value ||
00087                  std::is_same<T, ros::Duration>::value
00088                  , "not a valid type");
00089 
00090   *reinterpret_cast<T *>( _raw_data ) =  value;
00091   _raw_data[8] = RosIntrospection::getType<T>() ;
00092 }
00093 
00094 template<typename DST> inline DST VarNumber::convert() const
00095 {
00096   using namespace RosIntrospection::details;
00097   DST target;
00098 
00099   //----------
00100   switch( _raw_data[8] )
00101   {
00102   case CHAR:
00103   case INT8:   convert_impl<int8_t,  DST>(*reinterpret_cast<const int8_t*>( _raw_data), target  ); break;
00104 
00105   case INT16:  convert_impl<int16_t, DST>(*reinterpret_cast<const int16_t*>( _raw_data), target  ); break;
00106   case INT32:  convert_impl<int32_t, DST>(*reinterpret_cast<const int32_t*>( _raw_data), target  ); break;
00107   case INT64:  convert_impl<int64_t, DST>(*reinterpret_cast<const int64_t*>( _raw_data), target  ); break;
00108 
00109   case BOOL:
00110   case BYTE:
00111   case UINT8:   convert_impl<uint8_t,  DST>(*reinterpret_cast<const uint8_t*>( _raw_data), target  ); break;
00112 
00113   case UINT16:  convert_impl<uint16_t, DST>(*reinterpret_cast<const uint16_t*>( _raw_data), target  ); break;
00114   case UINT32:  convert_impl<uint32_t, DST>(*reinterpret_cast<const uint32_t*>( _raw_data), target  ); break;
00115   case UINT64:  convert_impl<uint64_t, DST>(*reinterpret_cast<const uint64_t*>( _raw_data), target  ); break;
00116 
00117   case FLOAT32:  convert_impl<float, DST>(*reinterpret_cast<const float*>( _raw_data), target  ); break;
00118   case FLOAT64:  convert_impl<double, DST>(*reinterpret_cast<const double*>( _raw_data), target  ); break;
00119 
00120   case DURATION:
00121   case TIME: {
00122      throw TypeException("ros::Duration and ros::Time can be converted only to double (will be seconds)");
00123   } break;
00124 
00125   default:  throw TypeException("VarNumber::convert -> cannot convert type" + std::to_string(_raw_data[8])); break;
00126 
00127   }
00128   return  target;
00129 }
00130 
00131 template<> inline double VarNumber::convert() const
00132 {
00133   using namespace RosIntrospection::details;
00134   double target;
00135 
00136   //----------
00137   switch( _raw_data[8] )
00138   {
00139   case CHAR:
00140   case INT8:   convert_impl<int8_t,  double>(*reinterpret_cast<const int8_t*>( _raw_data), target  ); break;
00141 
00142   case INT16:  convert_impl<int16_t, double>(*reinterpret_cast<const int16_t*>( _raw_data), target  ); break;
00143   case INT32:  convert_impl<int32_t, double>(*reinterpret_cast<const int32_t*>( _raw_data), target  ); break;
00144   case INT64:  convert_impl<int64_t, double>(*reinterpret_cast<const int64_t*>( _raw_data), target  ); break;
00145 
00146   case BOOL:
00147   case BYTE:
00148   case UINT8:   convert_impl<uint8_t,  double>(*reinterpret_cast<const uint8_t*>( _raw_data), target  ); break;
00149 
00150   case UINT16:  convert_impl<uint16_t, double>(*reinterpret_cast<const uint16_t*>( _raw_data), target  ); break;
00151   case UINT32:  convert_impl<uint32_t, double>(*reinterpret_cast<const uint32_t*>( _raw_data), target  ); break;
00152   case UINT64:  convert_impl<uint64_t, double>(*reinterpret_cast<const uint64_t*>( _raw_data), target  ); break;
00153 
00154   case FLOAT32:  convert_impl<float, double>(*reinterpret_cast<const float*>( _raw_data), target  ); break;
00155   case FLOAT64:  convert_impl<double, double>(*reinterpret_cast<const double*>( _raw_data), target  ); break;
00156 
00157   case DURATION: {
00158     ros::Duration tmp =  extract<ros::Duration>();
00159     target = tmp.toSec();
00160   }break;
00161   case TIME: {
00162     ros::Time tmp =  extract<ros::Time>();
00163     target = tmp.toSec();
00164   }break;
00165     
00166   default:  throw TypeException("VarNumber::convert -> cannot convert type" + std::to_string(_raw_data[8])); break;
00167 
00168   }
00169   return  target;
00170 }
00171 
00172 template<> inline ros::Time VarNumber::convert() const
00173 {
00174   if(  _raw_data[8] != TIME )
00175   {
00176      throw TypeException("VarNumber::convert -> cannot convert ros::Time");
00177   }
00178   return extract<ros::Time>();
00179 }
00180 
00181 template<> inline ros::Duration VarNumber::convert() const
00182 {
00183   if(  _raw_data[8] != DURATION )
00184   {
00185      throw TypeException("VarNumber::convert -> cannot convert ros::Duration");
00186   }
00187   return extract<ros::Duration>();
00188 }
00189 
00190 
00191 } //end namespace
00192 
00193 
00194 
00195 
00196 
00197 #endif // VARIANT_H


ros_type_introspection
Author(s): Davide Faconti
autogenerated on Sun Oct 1 2017 02:54:53