Forked from
Florian Leander Singer / RobotAPI
2181 commits behind the upstream repository.
-
Fabian Paus authoredFabian Paus authored
DataFieldsInfo.h 22.40 KiB
/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package RobotAPI::RobotUnit
* @author Raphael Grimm ( raphael dot grimm at kit dot edu )
* @date 2017
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
#pragma once
#include "../EigenForwardDeclarations.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/observers/variant/TimedVariant.h>
#include <ArmarXCore/interface/observers/VariantBase.h>
#include <string>
#include <chrono>
namespace armarx::introspection
{
template<class T, class = void> struct DataFieldsInfo;
// static std::size_t GetNumberOfFields();
// static void GetDataFieldAs(std::size_t i, T field, bool& out);
// static void GetDataFieldAs(std::size_t i, T field, Ice::Byte& out);
// static void GetDataFieldAs(std::size_t i, T field, Ice::Short& out);
// static void GetDataFieldAs(std::size_t i, T field, Ice::Int& out);
// static void GetDataFieldAs(std::size_t i, T field, Ice::Long& out);
// static void GetDataFieldAs(std::size_t i, T field, Ice::Float& out);
// static void GetDataFieldAs(std::size_t i, T field, Ice::Double& out);
// static void GetDataFieldAs(std::size_t i, T field, std::string& out);
// static const std::type_info& GetDataFieldType(std::size_t i);
// static const std::vector<std::string>& GetFieldNames();
// static std::map<std::string, VariantBasePtr> ToVariants(
// T value,
// const std::string& name,
// const IceUtil::Time& timestamp,
// const std::string& frame = "",
// const std::string& agent = "")
template<class T>
struct DataFieldsInfoBase
{
static void GetDataFieldAs(std::size_t i, const T& field, bool& out)
{
ARMARX_TRACE;
throw std::logic_error {"This function should never be called"};
}
static void GetDataFieldAs(std::size_t i, const T& field, Ice::Byte& out)
{
ARMARX_TRACE;
throw std::logic_error {"This function should never be called"};
}
static void GetDataFieldAs(std::size_t i, const T& field, Ice::Short& out)
{ ARMARX_TRACE;
throw std::logic_error {"This function should never be called"};
}
static void GetDataFieldAs(std::size_t i, const T& field, Ice::Int& out)
{
ARMARX_TRACE;
throw std::logic_error {"This function should never be called"};
}
static void GetDataFieldAs(std::size_t i, const T& field, Ice::Long& out)
{
ARMARX_TRACE;
throw std::logic_error {"This function should never be called"};
}
static void GetDataFieldAs(std::size_t i, const T& field, Ice::Float& out)
{
ARMARX_TRACE;
throw std::logic_error {"This function should never be called"};
}
static void GetDataFieldAs(std::size_t i, const T& field, Ice::Double& out)
{
ARMARX_TRACE;
throw std::logic_error {"This function should never be called"};
}
static void GetDataFieldAs(std::size_t i, const T& field, std::string& out)
{
ARMARX_TRACE;
throw std::logic_error {"This function should never be called"};
}
static const std::vector<std::string>& GetFieldNames()
{
ARMARX_TRACE;
throw std::logic_error {"This function should never be called"};
}
};
}
//build_in_ice_types
namespace armarx::introspection
{
#define make_def_for_build_in_ice_type(Type) \
template<> \
struct DataFieldsInfo<Type, void> : DataFieldsInfoBase<Type> \
{ \
using DataFieldsInfoBase<Type>::GetDataFieldAs; \
static std::size_t GetNumberOfFields() \
{ \
return 1; \
} \
static void GetDataFieldAs(std::size_t i, const Type& field, std::string& out) \
{ \
ARMARX_CHECK_EQUAL(i, 0); \
out = to_string(field); \
} \
static void GetDataFieldAs(std::size_t i, const Type& field, Type& out) \
{ \
ARMARX_CHECK_EQUAL(i, 0); \
out = field; \
} \
static const std::type_info& GetDataFieldType(std::size_t i) \
{ \
return typeid(Type); \
} \
static std::map<std::string, VariantBasePtr> ToVariants( \
const Type& value, \
const std::string& name, \
const IceUtil::Time& timestamp, \
const std::string& frame = "", \
const std::string& agent = "") \
{ \
ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) \
<< "There is no framed version for build in ice types"; \ return {{name, {new TimedVariant(value, timestamp)}}}; \
} \
}
make_def_for_build_in_ice_type(bool);
make_def_for_build_in_ice_type(Ice::Byte);
make_def_for_build_in_ice_type(Ice::Short);
make_def_for_build_in_ice_type(Ice::Int);
make_def_for_build_in_ice_type(Ice::Long);
make_def_for_build_in_ice_type(Ice::Float);
make_def_for_build_in_ice_type(Ice::Double);
#undef make_def_for_build_in_ice_type
}
//Eigen::Vector3f
namespace armarx::introspection
{
template<>
struct DataFieldsInfo<Eigen::Vector3f, void> : DataFieldsInfoBase<Eigen::Vector3f>
{
using DataFieldsInfoBase<Eigen::Vector3f>::GetDataFieldAs;
static std::size_t GetNumberOfFields();
static void GetDataFieldAs(std::size_t i, const Eigen::Vector3f& field, float& out);
static void GetDataFieldAs(std::size_t i, const Eigen::Vector3f& field, std::string& out);
static const std::type_info& GetDataFieldType(std::size_t i);
static const std::vector<std::string>& GetFieldNames();
static std::map<std::string, VariantBasePtr> ToVariants(
const Eigen::Vector3f& value,
const std::string& name,
const IceUtil::Time& timestamp,
const std::string& frame = "",
const std::string& agent = "");
};
}
//Eigen::Vector##Num##Type
namespace armarx::introspection
{
#define make_DataFieldsInfo_for_eigen_vector(Type,TypeName,Num) \
template<> \
struct DataFieldsInfo<Eigen::Vector##Num##Type, void> : DataFieldsInfoBase<Eigen::Vector##Num##Type> \
{ \
using DataFieldsInfoBase<Eigen::Vector##Num##Type>::GetDataFieldAs; \
static std::size_t GetNumberOfFields() \
{ \
return Num; \
} \
static void GetDataFieldAs(std::size_t i, const Eigen::Vector##Num##Type& field, std::string& out); \
static void GetDataFieldAs(std::size_t i, const Eigen::Vector##Num##Type& field, Ice::TypeName& out); \
static const std::type_info& GetDataFieldType(std::size_t i); \
static const std::vector<std::string>& GetFieldNames(); \
static std::map<std::string, VariantBasePtr> ToVariants( \
const Eigen::Vector##Num##Type& value, \
const std::string& name, \
const IceUtil::Time& timestamp, \
const std::string& frame = "", \
const std::string& agent = ""); \
};
make_DataFieldsInfo_for_eigen_vector(f, Float, 2)
make_DataFieldsInfo_for_eigen_vector(f, Float, 4)
make_DataFieldsInfo_for_eigen_vector(f, Float, 5)
make_DataFieldsInfo_for_eigen_vector(f, Float, 6)
make_DataFieldsInfo_for_eigen_vector(d, Double, 2)
make_DataFieldsInfo_for_eigen_vector(d, Double, 3)
make_DataFieldsInfo_for_eigen_vector(d, Double, 4)
make_DataFieldsInfo_for_eigen_vector(d, Double, 5)
make_DataFieldsInfo_for_eigen_vector(d, Double, 6)
make_DataFieldsInfo_for_eigen_vector(i, Int, 2)
make_DataFieldsInfo_for_eigen_vector(i, Int, 3)
make_DataFieldsInfo_for_eigen_vector(i, Int, 4) make_DataFieldsInfo_for_eigen_vector(i, Int, 5)
make_DataFieldsInfo_for_eigen_vector(i, Int, 6)
#undef make_DataFieldsInfo_for_eigen_vector
}
//Eigen::Matrix4f
namespace armarx::introspection
{
template<>
struct DataFieldsInfo<Eigen::Matrix4f, void> : DataFieldsInfoBase<Eigen::Matrix4f>
{
using DataFieldsInfoBase<Eigen::Matrix4f>::GetDataFieldAs;
static std::size_t GetNumberOfFields();
static void GetDataFieldAs(std::size_t i, const Eigen::Matrix4f& field, float& out);
static void GetDataFieldAs(std::size_t i, const Eigen::Matrix4f& field, std::string& out);
static const std::type_info& GetDataFieldType(std::size_t i);
static const std::vector<std::string>& GetFieldNames();
static std::map<std::string, VariantBasePtr> ToVariants(
const Eigen::Matrix4f& value,
const std::string& name,
const IceUtil::Time& timestamp,
const std::string& frame = "",
const std::string& agent = "");
};
}
//Eigen::Quaternionf
namespace armarx::introspection
{
template<>
struct DataFieldsInfo<Eigen::Quaternionf, void> : DataFieldsInfoBase<Eigen::Quaternionf>
{
using DataFieldsInfoBase<Eigen::Quaternionf>::GetDataFieldAs;
static std::size_t GetNumberOfFields();
static void GetDataFieldAs(std::size_t i, const Eigen::Quaternionf& field, float& out);
static void GetDataFieldAs(std::size_t i, const Eigen::Quaternionf& field, std::string& out);
static const std::type_info& GetDataFieldType(std::size_t i);
static const std::vector<std::string>& GetFieldNames();
static std::map<std::string, VariantBasePtr> ToVariants(
const Eigen::Quaternionf& value,
const std::string& name,
const IceUtil::Time& timestamp,
const std::string& frame = "",
const std::string& agent = "");
};
}
//std::chrono::microseconds
namespace armarx::introspection
{
template<>
struct DataFieldsInfo<std::chrono::microseconds, void> : DataFieldsInfoBase<std::chrono::microseconds>
{
using DataFieldsInfoBase<std::chrono::microseconds>::GetDataFieldAs;
static std::size_t GetNumberOfFields();
static void GetDataFieldAs(std::size_t i, const std::chrono::microseconds& field, long& out);
static void GetDataFieldAs(std::size_t i, const std::chrono::microseconds& field, std::string& out);
static const std::type_info& GetDataFieldType(std::size_t i);
static std::map<std::string, VariantBasePtr> ToVariants(
std::chrono::microseconds value,
const std::string& name,
const IceUtil::Time& timestamp,
const std::string& frame = "",
const std::string& agent = "");
};
}
//IceUtil::Time
namespace armarx::introspection
{
template<>
struct DataFieldsInfo<IceUtil::Time, void> : DataFieldsInfoBase<IceUtil::Time>
{
using DataFieldsInfoBase<IceUtil::Time>::GetDataFieldAs; static std::size_t GetNumberOfFields();
static void GetDataFieldAs(std::size_t i, const IceUtil::Time& field, long& out);
static void GetDataFieldAs(std::size_t i, const IceUtil::Time& field, std::string& out);
static const std::type_info& GetDataFieldType(std::size_t i);
static std::map<std::string, VariantBasePtr> ToVariants(
IceUtil::Time value,
const std::string& name,
const IceUtil::Time& timestamp,
const std::string& frame = "",
const std::string& agent = "");
};
}
//JointStatus
namespace armarx
{
struct JointStatus;
}
namespace armarx::introspection
{
template<>
struct DataFieldsInfo<JointStatus, void> : DataFieldsInfoBase<JointStatus>
{
using DataFieldsInfoBase<JointStatus>::GetDataFieldAs;
static std::size_t GetNumberOfFields();
static void GetDataFieldAs(std::size_t i, const JointStatus& field, std::string& out);
static void GetDataFieldAs(std::size_t i, const JointStatus& field, Ice::Int& out);
static void GetDataFieldAs(std::size_t i, const JointStatus& field, bool& out);
static const std::type_info& GetDataFieldType(std::size_t i);
static const std::vector<std::string>& GetFieldNames();
static std::map<std::string, VariantBasePtr> ToVariants(
const JointStatus& value,
const std::string& name,
const IceUtil::Time& timestamp,
const std::string& frame,
const std::string& agent);
};
}
//basic integral types
namespace armarx::introspection
{
#define make_def_for_type_mapped_to_long(Type) \
template<> \
struct DataFieldsInfo<Type, void> : DataFieldsInfoBase<Type> \
{ \
using DataFieldsInfoBase<Type>::GetDataFieldAs; \
static std::size_t GetNumberOfFields() \
{ \
return 1; \
} \
static void GetDataFieldAs(std::size_t i, const Type& field, std::string& out) \
{ \
ARMARX_CHECK_EQUAL(i, 0); \
out = to_string(field); \
} \
static void GetDataFieldAs(std::size_t i, const Type& field, Ice::Long& out) \
{ \
ARMARX_CHECK_EQUAL(i, 0); \
out = field; \
} \
static const std::type_info& GetDataFieldType(std::size_t i) \
{ \
return typeid(Type); \
} \
static std::map<std::string, VariantBasePtr> ToVariants( \
const Type& value, \
const std::string& name, \
const IceUtil::Time& timestamp, \
const std::string& frame = "", \
const std::string& agent = "") \ { \
ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) \
<< "There is no framed version for build in ice types"; \
return {{name, {new TimedVariant(value, timestamp)}}}; \
} \
}
make_def_for_type_mapped_to_long(std::uint16_t);
make_def_for_type_mapped_to_long(std::uint32_t);
#undef make_def_for_type_mapped_to_long
}
//std::array
namespace armarx::introspection
{
template<class T, class = void>
struct DataFieldsInfoHasNoFieldNames : std::false_type {};
template<class T>
struct DataFieldsInfoHasNoFieldNames < T, std::enable_if_t < (&DataFieldsInfo<T>::GetFieldNames == &DataFieldsInfoBase<T>::GetFieldNames) >> : std::true_type {};
template<class T>
static constexpr bool DataFieldsInfoHasNoFieldNamesV = DataFieldsInfoHasNoFieldNames<T>::value;
static_assert(!DataFieldsInfoHasNoFieldNamesV<Eigen::Vector3f>);
static_assert(!DataFieldsInfoHasNoFieldNamesV<Eigen::Quaternionf>);
static_assert(DataFieldsInfoHasNoFieldNamesV<Ice::Int>);
static_assert(DataFieldsInfoHasNoFieldNamesV<std::uint16_t>);
template<class T, std::size_t N>
struct DataFieldsInfo <std::array<T, N>, void> : DataFieldsInfoBase<std::array<T, N>>
{
using sub_t = DataFieldsInfo<T>;
static std::size_t GetNumberOfFields()
{
ARMARX_TRACE;
return N * sub_t::GetNumberOfFields();
}
template<class OT>
static void GetDataFieldAs(std::size_t i, const std::array<T, N>& field, OT& out)
{
ARMARX_TRACE;
ARMARX_CHECK_LESS(i, GetNumberOfFields());
const auto subN = sub_t::GetNumberOfFields();
sub_t::GetDataFieldAs(i % subN, field.at(i / subN), out);
}
static const std::type_info& GetDataFieldType(std::size_t i)
{
ARMARX_TRACE;
return sub_t::GetDataFieldType(i % N);
}
static const std::vector<std::string>& GetFieldNames()
{
ARMARX_TRACE;
static const std::vector<std::string> result = []
{
if constexpr(!DataFieldsInfoHasNoFieldNamesV<T>)
{
ARMARX_TRACE;
const auto sub_names = sub_t::GetFieldNames();
std::vector<std::string> r;
r.reserve(N * sub_names.size());
for (std::size_t i = 0; i < N; ++i)
{
const std::string pre = "element_" + std::to_string(i) + '.';
for (const auto& name : sub_names)
{
r.emplace_back(pre + name);
} }
return r;
}
else
{
ARMARX_TRACE;
std::vector<std::string> r;
r.reserve(N);
for (std::size_t i = 0; i < N; ++i)
{
r.emplace_back("element_" + std::to_string(i));
}
return r;
}
}();
return result;
}
static std::map<std::string, VariantBasePtr> ToVariants(
const std::array<T, N>& value,
const std::string& name,
const IceUtil::Time& timestamp,
const std::string& frame = "",
const std::string& agent = "")
{
ARMARX_TRACE;
std::map<std::string, VariantBasePtr> result;
for (std::size_t i = 0; i < N; ++i)
{
const std::string pre = "element_" + std::to_string(i) + '.';
for (const auto& [k, v] : sub_t::ToVariants(value.at(i), name, timestamp, frame, agent))
{
result[pre + k] = v;
}
}
return result;
}
};
}