Skip to content

Commit

Permalink
Merge branch 'master' into includecleanup
Browse files Browse the repository at this point in the history
  • Loading branch information
@Levi-Armstrong
Levi-Armstrong authored Oct 11, 2023
2 parents 73728d3 + 2f0ccd8 commit 390d1db
Show file tree
Hide file tree
Showing 3 changed files with 146 additions and 133 deletions.
247 changes: 118 additions & 129 deletions tesseract_common/include/tesseract_common/std_variant_serialization.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,177 +29,166 @@
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/nvp.hpp>

namespace boost {
namespace serialization {

template<class Archive>
namespace boost
{
namespace serialization
{
template <class Archive>
struct std_variant_save_visitor
{
std_variant_save_visitor(Archive& ar) :
m_ar(ar)
{}
template<class T>
void operator()(T const & value) const
{
m_ar << BOOST_SERIALIZATION_NVP(value);
}
std_variant_save_visitor(Archive& ar) : m_ar(ar) {}
template <class T>
void operator()(T const& value) const
{
m_ar << BOOST_SERIALIZATION_NVP(value);
}

private:
Archive & m_ar;
Archive& m_ar;
};


template<class Archive>
template <class Archive>
struct std_variant_load_visitor
{
std_variant_load_visitor(Archive& ar) :
m_ar(ar)
{}
template<class T>
void operator()(T & value) const
{
m_ar >> BOOST_SERIALIZATION_NVP(value);
}
std_variant_load_visitor(Archive& ar) : m_ar(ar) {}
template <class T>
void operator()(T& value) const
{
m_ar >> BOOST_SERIALIZATION_NVP(value);
}

private:
Archive & m_ar;
Archive& m_ar;
};

template<class Archive, class ...Types>
void save(
Archive & ar,
std::variant<Types...> const & v,
unsigned int /*version*/
){
const std::size_t which = v.index();
ar << BOOST_SERIALIZATION_NVP(which);
std_variant_save_visitor<Archive> visitor(ar);
std::visit(visitor, v);
template <class Archive, class... Types>
void save(Archive& ar, std::variant<Types...> const& v, unsigned int /*version*/
)
{
const std::size_t which = v.index();
ar << BOOST_SERIALIZATION_NVP(which);
std_variant_save_visitor<Archive> visitor(ar);
std::visit(visitor, v);
}

// Minimalist metaprogramming for handling parameter pack
namespace mp {
namespace detail {
template <typename Seq>
struct front_impl;
namespace mp
{
namespace detail
{
template <typename Seq>
struct front_impl;

template <template <typename...> class Seq, typename T, typename... Ts>
struct front_impl<Seq<T, Ts...>> {
using type = T;
};
template <template <typename...> class Seq, typename T, typename... Ts>
struct front_impl<Seq<T, Ts...> >
{
using type = T;
};

template <typename Seq>
struct pop_front_impl;
template <typename Seq>
struct pop_front_impl;

template <template <typename...> class Seq, typename T, typename... Ts>
struct pop_front_impl<Seq<T, Ts...>> {
using type = Seq<Ts...>;
};
} //namespace detail
template <template <typename...> class Seq, typename T, typename... Ts>
struct pop_front_impl<Seq<T, Ts...> >
{
using type = Seq<Ts...>;
};
} // namespace detail

template <typename... Ts>
struct typelist {};
template <typename... Ts>
struct typelist
{
};

template <typename Seq>
using front = typename detail::front_impl<Seq>::type;
template <typename Seq>
using front = typename detail::front_impl<Seq>::type;

template <typename Seq>
using pop_front = typename detail::pop_front_impl<Seq>::type;
template <typename Seq>
using pop_front = typename detail::pop_front_impl<Seq>::type;
} // namespace mp

template<std::size_t N, class Seq>
template <std::size_t N, class Seq>
struct variant_impl
{
template<class Archive, class V>
static void load (
Archive & ar,
std::size_t which,
V & v,
const unsigned int version
){
if(which == 0){
// note: A non-intrusive implementation (such as this one)
// necessary has to copy the value. This wouldn't be necessary
// with an implementation that de-serialized to the address of the
// aligned storage included in the variant.
using type = mp::front<Seq>;
type value;
ar >> BOOST_SERIALIZATION_NVP(value);
v = std::move(value);
type * new_address = & std::get<type>(v);
ar.reset_object_address(new_address, & value);
return;
}
//typedef typename mpl::pop_front<S>::type type;
using types = mp::pop_front<Seq>;
variant_impl<N - 1, types>::load(ar, which - 1, v, version);
template <class Archive, class V>
static void load(Archive& ar, std::size_t which, V& v, const unsigned int version)
{
if (which == 0)
{
// note: A non-intrusive implementation (such as this one)
// necessary has to copy the value. This wouldn't be necessary
// with an implementation that de-serialized to the address of the
// aligned storage included in the variant.
using type = mp::front<Seq>;
type value;
ar >> BOOST_SERIALIZATION_NVP(value);
v = std::move(value);
type* new_address = &std::get<type>(v);
ar.reset_object_address(new_address, &value);
return;
}
// typedef typename mpl::pop_front<S>::type type;
using types = mp::pop_front<Seq>;
variant_impl<N - 1, types>::load(ar, which - 1, v, version);
}
};

template<class Seq>
template <class Seq>
struct variant_impl<0, Seq>
{
template<class Archive, class V>
static void load (
Archive & /*ar*/,
std::size_t /*which*/,
V & /*v*/,
const unsigned int /*version*/
){}
template <class Archive, class V>
static void load(Archive& /*ar*/, std::size_t /*which*/, V& /*v*/, const unsigned int /*version*/
)
{
}
};

template<class Archive, class... Types>
void load(
Archive & ar,
std::variant<Types...>& v,
const unsigned int version
){
std::size_t which;
ar >> BOOST_SERIALIZATION_NVP(which);
if(which >= sizeof...(Types))
// this might happen if a type was removed from the list of variant types
boost::serialization::throw_exception(
boost::archive::archive_exception(
boost::archive::archive_exception::unsupported_version
)
);
variant_impl<sizeof...(Types), mp::typelist<Types...>>::load(ar, which, v, version);
template <class Archive, class... Types>
void load(Archive& ar, std::variant<Types...>& v, const unsigned int version)
{
std::size_t which;
ar >> BOOST_SERIALIZATION_NVP(which);
if (which >= sizeof...(Types))
// this might happen if a type was removed from the list of variant types
boost::serialization::throw_exception(
boost::archive::archive_exception(boost::archive::archive_exception::unsupported_version));
variant_impl<sizeof...(Types), mp::typelist<Types...> >::load(ar, which, v, version);
}

template<class Archive,class... Types>
inline void serialize(
Archive & ar,
std::variant<Types...> & v,
const unsigned int file_version
){
split_free(ar,v,file_version);
template <class Archive, class... Types>
inline void serialize(Archive& ar, std::variant<Types...>& v, const unsigned int file_version)
{
split_free(ar, v, file_version);
}

// Specialization for std::monostate
template<class Archive>
void serialize(Archive &ar, std::monostate &, const unsigned int /*version*/)
{}
template <class Archive>
void serialize(Archive& ar, std::monostate&, const unsigned int /*version*/)
{
}

} // namespace serialization
} // namespace boost
} // namespace serialization
} // namespace boost

//template<typename T0_, BOOST_VARIANT_ENUM_SHIFTED_PARAMS(typename T)>
// template<typename T0_, BOOST_VARIANT_ENUM_SHIFTED_PARAMS(typename T)>

#include <boost/serialization/tracking.hpp>

namespace boost {
namespace serialization {

template<class... Types>
struct tracking_level<
std::variant<Types...>
>{
typedef mpl::integral_c_tag tag;
typedef mpl::int_< ::boost::serialization::track_always> type;
BOOST_STATIC_CONSTANT(int, value = type::value);
namespace boost
{
namespace serialization
{
template <class... Types>
struct tracking_level<std::variant<Types...> >
{
typedef mpl::integral_c_tag tag;
typedef mpl::int_< ::boost::serialization::track_always> type;
BOOST_STATIC_CONSTANT(int, value = type::value);
};

} // namespace serialization
} // namespace boost
} // namespace serialization
} // namespace boost

#endif

#endif // TESSERACT_COMMON_STD_VARIANT_SERIALIZATION_H
#endif // TESSERACT_COMMON_STD_VARIANT_SERIALIZATION_H
24 changes: 24 additions & 0 deletions tesseract_common/include/tesseract_common/yaml_utils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -357,6 +357,30 @@ struct convert<Eigen::Vector2d>
}
};

template <>
struct convert<Eigen::Vector3d>
{
static Node encode(const Eigen::Vector3d& rhs)
{
Node node;
for (long i = 0; i < rhs.size(); ++i)
node.push_back(rhs(i));

return node;
}

static bool decode(const Node& node, Eigen::Vector3d& rhs)
{
if (!node.IsSequence() || (node.size() != 3))
return false;

for (long i = 0; i < 3; ++i)
rhs(i) = node[i].as<double>();

return true;
}
};

template <>
struct convert<tesseract_common::KinematicsPluginInfo>
{
Expand Down
8 changes: 4 additions & 4 deletions tesseract_kinematics/kdl/src/kdl_factories.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -123,10 +123,10 @@ InverseKinematics::UPtr KDLInvKinChainNRFactory::create(const std::string& solve
}

InverseKinematics::UPtr KDLInvKinChainNR_JLFactory::create(const std::string& solver_name,
const tesseract_scene_graph::SceneGraph& scene_graph,
const tesseract_scene_graph::SceneState& /*scene_state*/,
const KinematicsPluginFactory& /*plugin_factory*/,
const YAML::Node& config) const
const tesseract_scene_graph::SceneGraph& scene_graph,
const tesseract_scene_graph::SceneState& /*scene_state*/,
const KinematicsPluginFactory& /*plugin_factory*/,
const YAML::Node& config) const
{
std::string base_link;
std::string tip_link;
Expand Down

0 comments on commit 390d1db

Please sign in to comment.