add external properties to SolutionInfo

core/include/moveit/task_constructor/properties.h

@@ -45,10 +45,11 @@
 #include <vector>
 #include <functional>
 #include <sstream>
-#include <ros/serialization.h>
 
 #include <moveit_task_constructor_msgs/Property.h>
 
+#include "properties/serialize.hpp"
+
 namespace moveit {
 namespace task_constructor {
 
@@ -156,6 +157,13 @@ class Property
 	/// serialize value using registered functions
 	static std::string serialize(const boost::any& value);
 	static boost::any deserialize(const std::string& type_name, const std::string& wire);
+
+	template <typename T>
+	static T deserialize(const moveit_task_constructor_msgs::Property& property_msg) {
+		PropertySerializer<T>();
+		return boost::any_cast<T>(deserialize(property_msg.type, property_msg.value));
+	}
+
 	std::string serialize() const { return serialize(value()); }
 
 	/// get description text
@@ -173,6 +181,12 @@ class Property
 	/// configure initialization from source using given other property name
 	Property& configureInitFrom(SourceFlags source, const std::string& name);
 
+	void fillMsg(moveit_task_constructor_msgs::Property& msg) const {
+		msg.description = description();
+		msg.type = typeName();
+		msg.value = serialize();
+	}
+
 private:
 	std::string description_;
 	const type_info& type_info_;
@@ -185,84 +199,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 +239,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/include/moveit/task_constructor/properties/base64.hpp

@@ -0,0 +1,134 @@
+/**
+ * @file base64.h
+ * @author Basit Ayantunde ([email protected])
+ * @brief
+ * @version 0.1
+ * @date 2019-09-08
+ *
+ * @copyright Copyright (c) 2019
+ *
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *    http://www.apache.org/licenses/LICENSE-2.0
+
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#ifndef LAMAR_BASE64_H
+#define LAMAR_BASE64_H
+#include <cstring>
+#include <string>
+
+namespace base64 {
+
+template <typename T = char>
+struct Base64Chars
+{
+	static constexpr T data[]{ "ABCDEFGHIJKLMNOPQRSTUVWXYZ"
+		                        "abcdefghijklmnopqrstuvwxyz"
+		                        "0123456789+/" };
+};
+
+template <typename T>
+constexpr T Base64Chars<T>::data[];
+
+template <typename T = char>
+inline bool is_base64(T c) {
+	return (isalnum(c) || (c == '+') || (c == '/'));
+}
+
+template <typename T = char>
+std::basic_string<T> encode(const T* bytes_to_encode, uint32_t in_len) {
+	std::basic_string<T> ret;
+	int i = 0;
+	int j = 0;
+	T char_array_3[3];
+	T char_array_4[4];
+
+	while (in_len--) {
+		char_array_3[i++] = *(bytes_to_encode++);
+		if (i == 3) {
+			char_array_4[0] = (char_array_3[0] & 0xfc) >> 2;
+			char_array_4[1] = ((char_array_3[0] & 0x03) << 4) + ((char_array_3[1] & 0xf0) >> 4);
+			char_array_4[2] = ((char_array_3[1] & 0x0f) << 2) + ((char_array_3[2] & 0xc0) >> 6);
+			char_array_4[3] = char_array_3[2] & 0x3f;
+
+			for (i = 0; (i < 4); i++)
+				ret += Base64Chars<T>::data[char_array_4[i]];
+			i = 0;
+		}
+	}
+
+	if (i) {
+		for (j = i; j < 3; j++)
+			char_array_3[j] = '\0';
+
+		char_array_4[0] = (char_array_3[0] & 0xfc) >> 2;
+		char_array_4[1] = ((char_array_3[0] & 0x03) << 4) + ((char_array_3[1] & 0xf0) >> 4);
+		char_array_4[2] = ((char_array_3[1] & 0x0f) << 2) + ((char_array_3[2] & 0xc0) >> 6);
+		char_array_4[3] = char_array_3[2] & 0x3f;
+
+		for (j = 0; (j < i + 1); j++)
+			ret += Base64Chars<T>::data[char_array_4[j]];
+
+		while ((i++ < 3))
+			ret += '=';
+	}
+
+	return std::move(ret);
+}
+
+template <typename T = char>
+std::basic_string<T> decode(const T* encoded_string, int64_t in_len) {
+	int i = 0;
+	int j = 0;
+	int in_ = 0;
+	T char_array_4[4], char_array_3[3];
+	std::basic_string<T> ret;
+
+	while (in_len-- && (encoded_string[in_] != '=') && is_base64(encoded_string[in_])) {
+		char_array_4[i++] = encoded_string[in_];
+		in_++;
+		if (i == 4) {
+			for (i = 0; i < 4; i++)
+				char_array_4[i] = strchr(Base64Chars<T>::data, char_array_4[i]) - Base64Chars<T>::data;
+
+			char_array_3[0] = (char_array_4[0] << 2) + ((char_array_4[1] & 0x30) >> 4);
+			char_array_3[1] = ((char_array_4[1] & 0xf) << 4) + ((char_array_4[2] & 0x3c) >> 2);
+			char_array_3[2] = ((char_array_4[2] & 0x3) << 6) + char_array_4[3];
+
+			for (i = 0; (i < 3); i++)
+				ret += char_array_3[i];
+			i = 0;
+		}
+	}
+
+	if (i) {
+		for (j = i; j < 4; j++)
+			char_array_4[j] = 0;
+
+		for (j = 0; j < 4; j++)
+			char_array_4[j] = strchr(Base64Chars<T>::data, char_array_4[j]) - Base64Chars<T>::data;
+
+		char_array_3[0] = (char_array_4[0] << 2) + ((char_array_4[1] & 0x30) >> 4);
+		char_array_3[1] = ((char_array_4[1] & 0xf) << 4) + ((char_array_4[2] & 0x3c) >> 2);
+		char_array_3[2] = ((char_array_4[2] & 0x3) << 6) + char_array_4[3];
+
+		for (j = 0; (j < i - 1); j++)
+			ret += char_array_3[j];
+	}
+
+	return std::move(ret);
+}
+
+};  // namespace base64
+
+#endif