From 50ef6ea64bb052f6a73a8176d23f3e2bde6aa53c Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 7 Apr 2021 18:45:29 +0200 Subject: [PATCH] WIP: fromMsgs --- .../moveit/task_constructor/properties.h | 169 ++++++++++-------- core/src/properties.cpp | 7 + core/test/test_properties.cpp | 24 +++ 3 files changed, 121 insertions(+), 79 deletions(-) diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index 807dec8c2..e442378fe 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -58,6 +58,84 @@ class PropertyMap; /// initializer function, using given name from the passed property map boost::any fromName(const PropertyMap& other, const std::string& other_name); +// hasSerialize::value provides a true/false constexpr depending on whether operator<< is supported. +// This uses SFINAE, extracted from https://jguegant.github.io/blogs/tech/sfinae-introduction.html +template +struct hasSerialize : std::false_type +{}; + +template +struct hasSerialize() << std::declval())> : std::true_type +{}; + +template +struct hasDeserialize : std::false_type +{}; + +template +struct hasDeserialize() >> std::declval())> : std::true_type +{}; + +class PropertySerializerBase +{ +public: + using SerializeFunction = std::string (*)(const boost::any&); + using DeserializeFunction = boost::any (*)(const std::string&); + + static std::string dummySerialize(const boost::any& /*unused*/) { return ""; } + static boost::any dummyDeserialize(const std::string& /*unused*/) { return boost::any(); } + +protected: + static bool insert(const std::type_index& type_index, const std::string& type_name, SerializeFunction serialize, + DeserializeFunction deserialize); +}; + +/// utility class to register serializer/deserializer functions for a property of type T +template +class PropertySerializer : protected PropertySerializerBase +{ +public: + PropertySerializer() { insert(typeid(T), typeName(), &serialize, &deserialize); } + + template + static typename std::enable_if::value, std::string>::type typeName() { + return ros::message_traits::DataType::value(); + } + + template + static typename std::enable_if::value, std::string>::type typeName() { + return typeid(T).name(); + } + +private: + /** Serialization based on std::[io]stringstream */ + template + static typename std::enable_if::value, std::string>::type serialize(const boost::any& value) { + std::ostringstream oss; + oss << boost::any_cast(value); + return oss.str(); + } + template + static typename std::enable_if::value && hasDeserialize::value, boost::any>::type + deserialize(const std::string& wired) { + std::istringstream iss(wired); + T value; + iss >> value; + return value; + } + + /** No serialization available */ + template + static typename std::enable_if::value, std::string>::type serialize(const boost::any& value) { + return dummySerialize(value); + } + template + static typename std::enable_if::value || !hasDeserialize::value, boost::any>::type + deserialize(const std::string& wire) { + return dummyDeserialize(wire); + } +}; + /** Property is a wrapper for a boost::any value, also providing a default value and a description. * * Properties can be configured to be initialized from another PropertyMap - if still undefined. @@ -122,6 +200,16 @@ class Property /// function callback used to initialize property value from another PropertyMap using InitializerFunction = std::function; + template + static Property fromMsg(const moveit_task_constructor_msgs::Property& msg) { + auto requested_type{ typeid(T).name() }; + if (msg.type != requested_type) { + throw type_error{ requested_type, msg.type }; + } + PropertySerializer(); // register serializer/deserializer + return Property(typeid(T), msg.description, msg.value); + }; + /// set current value and default value void setValue(const boost::any& value); void setCurrentValue(const boost::any& value); @@ -185,84 +273,6 @@ class Property InitializerFunction initializer_; }; -// hasSerialize::value provides a true/false constexpr depending on whether operator<< is supported. -// This uses SFINAE, extracted from https://jguegant.github.io/blogs/tech/sfinae-introduction.html -template -struct hasSerialize : std::false_type -{}; - -template -struct hasSerialize() << std::declval())> : std::true_type -{}; - -template -struct hasDeserialize : std::false_type -{}; - -template -struct hasDeserialize() >> std::declval())> : std::true_type -{}; - -class PropertySerializerBase -{ -public: - using SerializeFunction = std::string (*)(const boost::any&); - using DeserializeFunction = boost::any (*)(const std::string&); - - static std::string dummySerialize(const boost::any& /*unused*/) { return ""; } - static boost::any dummyDeserialize(const std::string& /*unused*/) { return boost::any(); } - -protected: - static bool insert(const std::type_index& type_index, const std::string& type_name, SerializeFunction serialize, - DeserializeFunction deserialize); -}; - -/// utility class to register serializer/deserializer functions for a property of type T -template -class PropertySerializer : protected PropertySerializerBase -{ -public: - PropertySerializer() { insert(typeid(T), typeName(), &serialize, &deserialize); } - - template - static typename std::enable_if::value, std::string>::type typeName() { - return ros::message_traits::DataType::value(); - } - - template - static typename std::enable_if::value, std::string>::type typeName() { - return typeid(T).name(); - } - -private: - /** Serialization based on std::[io]stringstream */ - template - static typename std::enable_if::value, std::string>::type serialize(const boost::any& value) { - std::ostringstream oss; - oss << boost::any_cast(value); - return oss.str(); - } - template - static typename std::enable_if::value && hasDeserialize::value, boost::any>::type - deserialize(const std::string& wired) { - std::istringstream iss(wired); - T value; - iss >> value; - return value; - } - - /** No serialization available */ - template - static typename std::enable_if::value, std::string>::type serialize(const boost::any& value) { - return dummySerialize(value); - } - template - static typename std::enable_if::value || !hasDeserialize::value, boost::any>::type - deserialize(const std::string& wire) { - return dummyDeserialize(wire); - } -}; - /** PropertyMap is map of (name, Property) pairs. * * Conveniency methods are provided to setup property initialization for several @@ -303,7 +313,8 @@ class PropertyMap Property& property(const std::string& name); const Property& property(const std::string& name) const { return const_cast(this)->property(name); } - void fillMsgs(std::vector& msg) const; + void fillMsgs(std::vector& msgs) const; + void fromMsgs(std::vector& msgs); using iterator = std::map::iterator; using const_iterator = std::map::const_iterator; diff --git a/core/src/properties.cpp b/core/src/properties.cpp index d0c99ad67..7949123c3 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -224,6 +224,13 @@ void PropertyMap::fillMsgs(std::vector& } } +void PropertyMap::fromMsgs(std::vector& msgs) { + for (const auto& p : msgs) { + boost::any value{ Property::deserialize(p.type, p.value) }; + declare(p.name, value.type(), p.description, value); + } +} + void PropertyMap::exposeTo(PropertyMap& other, const std::set& properties) const { for (const std::string& name : properties) exposeTo(other, name, name); diff --git a/core/test/test_properties.cpp b/core/test/test_properties.cpp index c5aa67e42..8c8f9f1de 100644 --- a/core/test/test_properties.cpp +++ b/core/test/test_properties.cpp @@ -1,5 +1,7 @@ #include +#include + #include #include @@ -106,6 +108,28 @@ TEST(Property, serialize) { EXPECT_EQ(props.property("map").serialize(), ""); } +TEST(Property, fillMsg) { + PropertyMap props; + props.declare("pose", "ROS msg pose"); + + geometry_msgs::PoseStamped msg; + msg.header.frame_id = "world"; + msg.pose.orientation.x = 0.0; + msg.pose.orientation.y = 0.0; + msg.pose.orientation.z = 0.0; + msg.pose.orientation.w = 1.0; + props.set("pose", msg); + + std::vector props_msgs; + props.fillMsgs(props_msgs); + + PropertyView p{ props_msgs[0] }; + + EXPECT_EQ(p.value(), msg); + PropertyMap props_from_msgs; + props_from_msgs.fromMsgs(props_msgs); +} + class InitFromTest : public ::testing::Test { protected: