Skip to content

Commit

Permalink
Add support for shared pointers to tesseract_common::AnyPoly
Browse files Browse the repository at this point in the history
  • Loading branch information
@Levi-Armstrong
Levi-Armstrong committed Jul 3, 2024
1 parent 120c13d commit 06cde71
Show file tree
Hide file tree
Showing 2 changed files with 74 additions and 2 deletions.
17 changes: 15 additions & 2 deletions tesseract_common/include/tesseract_common/any_poly.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,19 +42,32 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
using C##AnyInstanceBase = tesseract_common::TypeErasureInstance<C, tesseract_common::TypeErasureInterface>; \
using C##AnyInstance = tesseract_common::detail_any::AnyInstance<C>; \
using C##AnyInstanceWrapper = tesseract_common::TypeErasureInstanceWrapper<C##AnyInstance>; \
using C##SharedPtrAnyInstanceBase = \
tesseract_common::TypeErasureInstance<std::shared_ptr<C>, tesseract_common::TypeErasureInterface>; /* NOLINT */ \
using C##SharedPtrAnyInstance = tesseract_common::detail_any::AnyInstance<std::shared_ptr<C>>; /* NOLINT */ \
using C##SharedPtrAnyInstanceWrapper = tesseract_common::TypeErasureInstanceWrapper<C##SharedPtrAnyInstance>; \
} \
BOOST_CLASS_EXPORT_KEY(N::C##AnyInstanceBase) \
BOOST_CLASS_EXPORT_KEY(N::C##AnyInstance) \
BOOST_CLASS_EXPORT_KEY(N::C##AnyInstanceWrapper) \
BOOST_CLASS_EXPORT_KEY(N::C##SharedPtrAnyInstanceBase) \
BOOST_CLASS_EXPORT_KEY(N::C##SharedPtrAnyInstance) \
BOOST_CLASS_EXPORT_KEY(N::C##SharedPtrAnyInstanceWrapper) \
BOOST_CLASS_TRACKING(N::C##AnyInstanceBase, boost::serialization::track_never) \
BOOST_CLASS_TRACKING(N::C##AnyInstance, boost::serialization::track_never) \
BOOST_CLASS_TRACKING(N::C##AnyInstanceWrapper, boost::serialization::track_never)
BOOST_CLASS_TRACKING(N::C##AnyInstanceWrapper, boost::serialization::track_never) \
BOOST_CLASS_TRACKING(N::C##SharedPtrAnyInstanceBase, boost::serialization::track_never) \
BOOST_CLASS_TRACKING(N::C##SharedPtrAnyInstance, boost::serialization::track_never) \
BOOST_CLASS_TRACKING(N::C##SharedPtrAnyInstanceWrapper, boost::serialization::track_never)

/** @brief If shared library, this must go in the cpp after the implicit instantiation of the serialize function */
#define TESSERACT_ANY_EXPORT_IMPLEMENT(inst) \
BOOST_CLASS_EXPORT_IMPLEMENT(inst##AnyInstanceBase) \
BOOST_CLASS_EXPORT_IMPLEMENT(inst##AnyInstance) \
BOOST_CLASS_EXPORT_IMPLEMENT(inst##AnyInstanceWrapper)
BOOST_CLASS_EXPORT_IMPLEMENT(inst##AnyInstanceWrapper) \
BOOST_CLASS_EXPORT_IMPLEMENT(inst##SharedPtrAnyInstanceBase) \
BOOST_CLASS_EXPORT_IMPLEMENT(inst##SharedPtrAnyInstance) \
BOOST_CLASS_EXPORT_IMPLEMENT(inst##SharedPtrAnyInstanceWrapper)

/**
* @brief This should not be used within shared libraries use the two above.
Expand Down
59 changes: 59 additions & 0 deletions tesseract_common/test/tesseract_common_unit.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <type_traits>
#include <boost/archive/xml_oarchive.hpp>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <tinyxml2.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

Expand Down Expand Up @@ -412,6 +413,64 @@ TEST(TesseractCommonUnit, anyUnit) // NOLINT
EXPECT_ANY_THROW(nany_type.as<tesseract_common::Toolpath>()); // NOLINT
}

TEST(TesseractCommonUnit, anySharedPtrUnit) // NOLINT
{
tesseract_common::AnyPoly any_type;
EXPECT_TRUE(any_type.getType() == std::type_index(typeid(nullptr)));

tesseract_common::JointState joint_state;
joint_state.joint_names = { "joint_1", "joint_2", "joint_3" };
joint_state.position = Eigen::VectorXd::Constant(3, 5);
joint_state.velocity = Eigen::VectorXd::Constant(3, 6);
joint_state.acceleration = Eigen::VectorXd::Constant(3, 7);
joint_state.effort = Eigen::VectorXd::Constant(3, 8);
joint_state.time = 100;

auto joint_state_ptr = std::make_shared<tesseract_common::JointState>(joint_state);
any_type = joint_state_ptr;
EXPECT_TRUE(any_type.getType() == std::type_index(typeid(std::shared_ptr<tesseract_common::JointState>)));
EXPECT_TRUE(*any_type.as<std::shared_ptr<tesseract_common::JointState>>() == joint_state);
EXPECT_TRUE(any_type.as<std::shared_ptr<tesseract_common::JointState>>() == joint_state_ptr);

// Check clone
tesseract_common::AnyPoly any_copy = any_type;
EXPECT_TRUE(any_copy == any_type);

// Check to make sure it is not making a copy during cast
auto& any_type_ref1 = any_type.as<std::shared_ptr<tesseract_common::JointState>>();
auto& any_type_ref2 = any_type.as<std::shared_ptr<tesseract_common::JointState>>();
auto& any_copy_ref = any_copy.as<std::shared_ptr<tesseract_common::JointState>>();
EXPECT_TRUE(&any_type_ref1 == &any_type_ref2);
EXPECT_TRUE(&any_type_ref1 != &any_copy_ref);
EXPECT_TRUE(&any_type_ref2 != &any_copy_ref);

const auto& any_type_const_ref1 = any_type.as<std::shared_ptr<tesseract_common::JointState>>();
const auto& any_type_const_ref2 = any_type.as<std::shared_ptr<tesseract_common::JointState>>();
EXPECT_TRUE(&any_type_const_ref1 == &any_type_const_ref2);

{
std::ofstream os(tesseract_common::getTempPath() + "any_shared_ptr_type_boost.xml");
boost::archive::xml_oarchive oa(os);
oa << BOOST_SERIALIZATION_NVP(any_type);
}

tesseract_common::AnyPoly nany_type;
{
std::ifstream ifs(tesseract_common::getTempPath() + "any_shared_ptr_type_boost.xml");
assert(ifs.good());
boost::archive::xml_iarchive ia(ifs);

// restore the schedule from the archive
ia >> BOOST_SERIALIZATION_NVP(nany_type);
}

EXPECT_TRUE(nany_type.getType() == std::type_index(typeid(std::shared_ptr<tesseract_common::JointState>)));
EXPECT_TRUE(*nany_type.as<std::shared_ptr<tesseract_common::JointState>>() == joint_state);

// Test bad cast
EXPECT_ANY_THROW(nany_type.as<tesseract_common::JointState>()); // NOLINT
}

TEST(TesseractCommonUnit, boundsUnit) // NOLINT
{
Eigen::VectorXd v = Eigen::VectorXd::Ones(6);
Expand Down

0 comments on commit 06cde71

Please sign in to comment.