WIP: fromMsgs

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<T>::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 <typename T, typename = std::ostream&>
+struct hasSerialize : std::false_type
+{};
+
+template <typename T>
+struct hasSerialize<T, decltype(std::declval<std::ostream&>() << std::declval<T>())> : std::true_type
+{};
+
+template <typename T, typename = std::istream&>
+struct hasDeserialize : std::false_type
+{};
+
+template <typename T>
+struct hasDeserialize<T, decltype(std::declval<std::istream&>() >> std::declval<T&>())> : 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 <typename T>
+class PropertySerializer : protected PropertySerializerBase
+{
+public:
+	PropertySerializer() { insert(typeid(T), typeName<T>(), &serialize, &deserialize); }
+
+	template <class Q = T>
+	static typename std::enable_if<ros::message_traits::IsMessage<Q>::value, std::string>::type typeName() {
+		return ros::message_traits::DataType<T>::value();
+	}
+
+	template <class Q = T>
+	static typename std::enable_if<!ros::message_traits::IsMessage<Q>::value, std::string>::type typeName() {
+		return typeid(T).name();
+	}
+
+private:
+	/** Serialization based on std::[io]stringstream */
+	template <class Q = T>
+	static typename std::enable_if<hasSerialize<Q>::value, std::string>::type serialize(const boost::any& value) {
+		std::ostringstream oss;
+		oss << boost::any_cast<T>(value);
+		return oss.str();
+	}
+	template <class Q = T>
+	static typename std::enable_if<hasSerialize<Q>::value && hasDeserialize<Q>::value, boost::any>::type
+	deserialize(const std::string& wired) {
+		std::istringstream iss(wired);
+		T value;
+		iss >> value;
+		return value;
+	}
+
+	/** No serialization available */
+	template <class Q = T>
+	static typename std::enable_if<!hasSerialize<Q>::value, std::string>::type serialize(const boost::any& value) {
+		return dummySerialize(value);
+	}
+	template <class Q = T>
+	static typename std::enable_if<!hasSerialize<Q>::value || !hasDeserialize<Q>::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<boost::any(const PropertyMap&)>;
 
+	template <typename T>
+	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<T>();  // 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<T>::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 <typename T, typename = std::ostream&>
-struct hasSerialize : std::false_type
-{};
-
-template <typename T>
-struct hasSerialize<T, decltype(std::declval<std::ostream&>() << std::declval<T>())> : std::true_type
-{};
-
-template <typename T, typename = std::istream&>
-struct hasDeserialize : std::false_type
-{};
-
-template <typename T>
-struct hasDeserialize<T, decltype(std::declval<std::istream&>() >> std::declval<T&>())> : 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 <typename T>
-class PropertySerializer : protected PropertySerializerBase
-{
-public:
-	PropertySerializer() { insert(typeid(T), typeName<T>(), &serialize, &deserialize); }
-
-	template <class Q = T>
-	static typename std::enable_if<ros::message_traits::IsMessage<Q>::value, std::string>::type typeName() {
-		return ros::message_traits::DataType<T>::value();
-	}
-
-	template <class Q = T>
-	static typename std::enable_if<!ros::message_traits::IsMessage<Q>::value, std::string>::type typeName() {
-		return typeid(T).name();
-	}
-
-private:
-	/** Serialization based on std::[io]stringstream */
-	template <class Q = T>
-	static typename std::enable_if<hasSerialize<Q>::value, std::string>::type serialize(const boost::any& value) {
-		std::ostringstream oss;
-		oss << boost::any_cast<T>(value);
-		return oss.str();
-	}
-	template <class Q = T>
-	static typename std::enable_if<hasSerialize<Q>::value && hasDeserialize<Q>::value, boost::any>::type
-	deserialize(const std::string& wired) {
-		std::istringstream iss(wired);
-		T value;
-		iss >> value;
-		return value;
-	}
-
-	/** No serialization available */
-	template <class Q = T>
-	static typename std::enable_if<!hasSerialize<Q>::value, std::string>::type serialize(const boost::any& value) {
-		return dummySerialize(value);
-	}
-	template <class Q = T>
-	static typename std::enable_if<!hasSerialize<Q>::value || !hasDeserialize<Q>::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<PropertyMap*>(this)->property(name); }
 
-	void fillMsgs(std::vector<moveit_task_constructor_msgs::Property>& msg) const;
+	void fillMsgs(std::vector<moveit_task_constructor_msgs::Property>& msgs) const;
+	void fromMsgs(std::vector<moveit_task_constructor_msgs::Property>& msgs);
 
 	using iterator = std::map<std::string, Property>::iterator;
 	using const_iterator = std::map<std::string, Property>::const_iterator;

core/src/properties.cpp

@@ -224,6 +224,13 @@ void PropertyMap::fillMsgs(std::vector<moveit_task_constructor_msgs::Property>&
 	}
 }
 
+void PropertyMap::fromMsgs(std::vector<moveit_task_constructor_msgs::Property>& 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<std::string>& properties) const {
 	for (const std::string& name : properties)
 		exposeTo(other, name, name);

core/test/test_properties.cpp

@@ -1,5 +1,7 @@
 #include <moveit/task_constructor/properties.h>
 
+#include <geometry_msgs/PoseStamped.h>
+
 #include <gtest/gtest.h>
 #include <initializer_list>
 
@@ -106,6 +108,28 @@ TEST(Property, serialize) {
 	EXPECT_EQ(props.property("map").serialize(), "");
 }
 
+TEST(Property, fillMsg) {
+	PropertyMap props;
+	props.declare<geometry_msgs::PoseStamped>("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<moveit_task_constructor_msgs::Property> props_msgs;
+	props.fillMsgs(props_msgs);
+
+	PropertyView<geometry_msgs::PoseStamped> 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: