From 7139e9944541c8001ba6c4030c864e7ef04e34de Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 30 Jul 2020 20:07:08 +0200 Subject: [PATCH 01/12] add properties to SolutionInfo --- msgs/msg/SolutionInfo.msg | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/msgs/msg/SolutionInfo.msg b/msgs/msg/SolutionInfo.msg index c3346ab53..6a26c0711 100644 --- a/msgs/msg/SolutionInfo.msg +++ b/msgs/msg/SolutionInfo.msg @@ -12,3 +12,11 @@ uint32 stage_id # markers, e.g. providing additional hints or illustrating failure visualization_msgs/Marker[] markers + +# Solution-specific values that provide additional information. +# These are either +# - Stage properties configured to initialize from INTERFACE +# - Properties that are relevant for trajectory execution, +# e.g., to engage/disengage a suction gripper or trigger a camera +# - Stage-internal variables useful for introspection +Property[] properties From 5646ecf82a5dee7fd1b1cca5d7229f08beab058c Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 7 Apr 2021 15:56:21 +0200 Subject: [PATCH 02/12] move typed getters to Property & drop fallback interface Better usability for user code if they deserialize individual Propertys. The fallback variants are not used internally and are redundant because there is a separate default behavior. Removed the msg parameter for Property exceptions. It was only used by a single edge-case which didn't really justify the use of the exception anyway. Instead allow `undefined` to be instantiated without parameters because the Property does not know its own name. --- .../moveit/task_constructor/properties.h | 97 ++++++++++--------- core/src/properties.cpp | 8 +- core/test/test_properties.cpp | 1 - 3 files changed, 56 insertions(+), 50 deletions(-) diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index c93870866..1206f1eb8 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -81,13 +81,40 @@ class Property Property(); /// base class for Property exceptions - class error; + class error : public std::runtime_error + { + protected: + std::string property_name_; + std::string msg_; + + public: + explicit error(const std::string& msg); + const std::string& name() const { return property_name_; } + void setName(const std::string& name); + const char* what() const noexcept override { return msg_.c_str(); } + }; + /// exception thrown when accessing an undeclared property - class undeclared; + class undeclared : public Property::error + { + public: + explicit undeclared(const std::string& name); + }; + /// exception thrown when accessing an undefined property - class undefined; + class undefined : public Property::error + { + public: + explicit undefined(); + explicit undefined(const std::string& name); + }; + /// exception thrown when trying to set a value not matching the declared type - class type_error; + class type_error : public Property::error + { + public: + explicit type_error(const std::string& current_type, const std::string& declared_type); + }; using SourceFlags = uint; /// function callback used to initialize property value from another PropertyMap @@ -101,12 +128,22 @@ class Property /// reset to default value (which can be empty) void reset(); - /// the current value defined or will the fallback be used? - inline bool defined() const { return !value_.empty(); } + /// is a value defined? + inline bool defined() const { return !(value_.empty() && default_.empty()); } - /// get current value (or default if not defined) + /// get current value (or default if not set) inline const boost::any& value() const { return value_.empty() ? default_ : value_; } inline boost::any& value() { return value_.empty() ? default_ : value_; } + + /// get typed value of property. Throws bad_any_cast on type mismatch, undefined if !defined(). + template + inline const T& value() const { + const boost::any& v{ value() }; + if (v.empty()) + throw Property::undefined(); + return boost::any_cast(value()); + } + /// get default value const boost::any& defaultValue() const { return default_; } @@ -142,34 +179,6 @@ class Property InitializerFunction initializer_; }; -class Property::error : public std::runtime_error -{ -protected: - std::string property_name_; - std::string msg_; - -public: - explicit error(const std::string& msg); - const std::string& name() const { return property_name_; } - void setName(const std::string& name); - const char* what() const noexcept override { return msg_.c_str(); } -}; -class Property::undeclared : public Property::error -{ -public: - explicit undeclared(const std::string& name, const std::string& msg = "undeclared"); -}; -class Property::undefined : public Property::error -{ -public: - explicit undefined(const std::string& name, const std::string& msg = "undefined"); -}; -class Property::type_error : public Property::error -{ -public: - explicit type_error(const std::string& current_type, const std::string& declared_type); -}; - // 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 @@ -318,19 +327,15 @@ class PropertyMap /// Get the value of a property. Throws undeclared if unknown name const boost::any& get(const std::string& name) const; - /// Get typed value of property. Throws undeclared, undefined, or bad_any_cast. + /// Get typed value of property. Throws undeclared or bad_any_cast. template const T& get(const std::string& name) const { - const boost::any& value = get(name); - if (value.empty()) - throw Property::undefined(name); - return boost::any_cast(value); - } - /// get typed value of property, using fallback if undefined. Throws bad_any_cast on type mismatch. - template - const T& get(const std::string& name, const T& fallback) const { - const boost::any& value = get(name); - return (value.empty()) ? fallback : boost::any_cast(value); + try { + return property(name).value(); + } catch (Property::undefined& e) { + e.setName(name); + throw e; + } } /// count number of defined properties from given list diff --git a/core/src/properties.cpp b/core/src/properties.cpp index 257d1455a..56d799d77 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -239,7 +239,7 @@ void PropertyMap::set(const std::string& name, const boost::any& val auto range = props_.equal_range(name); if (range.first == range.second) { // name is not yet declared if (value.empty()) - throw Property::undeclared(name, "trying to set undeclared property '" + name + "' with NULL value"); + throw std::runtime_error("trying to set undeclared property '" + name + "' with NULL value"); auto it = props_.insert(range.first, std::make_pair(name, Property(value.type(), "", boost::any()))); it->second.setValue(value); } else @@ -308,11 +308,13 @@ void Property::error::setName(const std::string& name) { msg_ = "Property '" + name + "': " + std::runtime_error::what(); } -Property::undeclared::undeclared(const std::string& name, const std::string& msg) : Property::error(msg) { +Property::undeclared::undeclared(const std::string& name) : Property::error("undeclared") { setName(name); } -Property::undefined::undefined(const std::string& name, const std::string& msg) : Property::error(msg) { +Property::undefined::undefined() : Property::error("undefined") {} + +Property::undefined::undefined(const std::string& name) : Property::undefined() { setName(name); } diff --git a/core/test/test_properties.cpp b/core/test/test_properties.cpp index 5e2f6faa5..c5aa67e42 100644 --- a/core/test/test_properties.cpp +++ b/core/test/test_properties.cpp @@ -17,7 +17,6 @@ TEST(Property, standard) { EXPECT_THROW(props.get("double4"), Property::undefined); EXPECT_FALSE(props.property("double4").defined()); - EXPECT_EQ(props.get("double4", 0.0), 0.0); props.set("double3", 3.0); EXPECT_EQ(props.get("double3"), 3.0); From 3277abf656335bb72caaac0607f00aae64775bce Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 7 Apr 2021 17:50:15 +0200 Subject: [PATCH 03/12] add PropertyMap -> vector interface --- core/include/moveit/task_constructor/properties.h | 5 +++++ core/src/introspection.cpp | 10 +--------- core/src/properties.cpp | 12 ++++++++++++ 3 files changed, 18 insertions(+), 9 deletions(-) diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index 1206f1eb8..184f5ebb0 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -47,6 +47,8 @@ #include #include +#include + namespace moveit { namespace task_constructor { @@ -297,6 +299,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; + using iterator = std::map::iterator; using const_iterator = std::map::const_iterator; @@ -304,6 +308,7 @@ class PropertyMap iterator end() { return props_.end(); } const_iterator begin() const { return props_.begin(); } const_iterator end() const { return props_.end(); } + size_t size() const { return props_.size(); } /// allow initialization from given source for listed properties - always using the same name void configureInitFrom(Property::SourceFlags source, const std::set& properties = {}); diff --git a/core/src/introspection.cpp b/core/src/introspection.cpp index 588b9e99d..e529a1c15 100644 --- a/core/src/introspection.cpp +++ b/core/src/introspection.cpp @@ -225,15 +225,7 @@ Introspection::fillTaskDescription(moveit_task_constructor_msgs::TaskDescription desc.name = stage.name(); desc.flags = stage.pimpl()->interfaceFlags(); - // fill stage properties - for (const auto& pair : stage.properties()) { - moveit_task_constructor_msgs::Property p; - p.name = pair.first; - p.description = pair.second.description(); - p.type = pair.second.typeName(); - p.value = pair.second.serialize(); - desc.properties.push_back(p); - } + stage.properties().fillMsgs(desc.properties); auto it = impl->stage_to_id_map_.find(stage.pimpl()->parent()->pimpl()); assert(it != impl->stage_to_id_map_.cend()); diff --git a/core/src/properties.cpp b/core/src/properties.cpp index 56d799d77..58015f6d4 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -212,6 +212,18 @@ Property& PropertyMap::property(const std::string& name) { return it->second; } +void PropertyMap::fillMsgs(std::vector& msgs) const { + msgs.reserve(size()); + for (const auto& pair : *this) { + msgs.emplace_back(); + auto& p{ msgs.back() }; + p.name = pair.first; + p.description = pair.second.description(); + p.type = pair.second.typeName(); + p.value = pair.second.serialize(); + } +} + void PropertyMap::exposeTo(PropertyMap& other, const std::set& properties) const { for (const std::string& name : properties) exposeTo(other, name, name); From ac22156446a34061d422452eb6aa5c0e5894ecf3 Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 7 Apr 2021 17:52:58 +0200 Subject: [PATCH 04/12] do not allow write-access to default value Defaults are not supposed to be modified by accident. Instead, provide a writable reference to currentValue. --- core/include/moveit/task_constructor/properties.h | 6 +++++- core/src/properties.cpp | 2 +- core/src/stages/fixed_cartesian_poses.cpp | 6 +++--- 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index 184f5ebb0..d3a9f8e39 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -133,9 +133,11 @@ class Property /// is a value defined? inline bool defined() const { return !(value_.empty() && default_.empty()); } + /// is a non-default value set? + inline bool hasCurrentValue() const { return !value_.empty(); } + /// get current value (or default if not set) inline const boost::any& value() const { return value_.empty() ? default_ : value_; } - inline boost::any& value() { return value_.empty() ? default_ : value_; } /// get typed value of property. Throws bad_any_cast on type mismatch, undefined if !defined(). template @@ -148,6 +150,8 @@ class Property /// get default value const boost::any& defaultValue() const { return default_; } + const boost::any& currentValue() const { return value_; } + boost::any& currentValue() { return value_; }; /// serialize value using registered functions static std::string serialize(const boost::any& value); diff --git a/core/src/properties.cpp b/core/src/properties.cpp index 58015f6d4..6365ea39a 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -120,7 +120,7 @@ Property::Property() : Property(typeid(boost::any), "", boost::any()) {} void Property::setValue(const boost::any& value) { setCurrentValue(value); - default_ = value_; + setDefaultValue(value_); initialized_from_ = 0; } diff --git a/core/src/stages/fixed_cartesian_poses.cpp b/core/src/stages/fixed_cartesian_poses.cpp index 068f2759c..7c0f771d4 100644 --- a/core/src/stages/fixed_cartesian_poses.cpp +++ b/core/src/stages/fixed_cartesian_poses.cpp @@ -57,10 +57,10 @@ FixedCartesianPoses::FixedCartesianPoses(const std::string& name) : MonitoringGe void FixedCartesianPoses::addPose(const geometry_msgs::PoseStamped& pose) { moveit::task_constructor::Property& poses = properties().property("poses"); - if (!poses.defined()) - poses.setValue(PosesList({ pose })); + if (!poses.hasCurrentValue()) + poses.setCurrentValue(PosesList({ pose })); else - boost::any_cast(poses.value()).push_back(pose); + boost::any_cast(poses.currentValue()).push_back(pose); } void FixedCartesianPoses::reset() { From 476424d76292aab05531940bcd123bb7ca859fbf Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 7 Apr 2021 17:54:17 +0200 Subject: [PATCH 05/12] lipstick --- core/include/moveit/task_constructor/properties.h | 2 +- core/src/properties.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index d3a9f8e39..807dec8c2 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -317,7 +317,7 @@ class PropertyMap /// allow initialization from given source for listed properties - always using the same name void configureInitFrom(Property::SourceFlags source, const std::set& properties = {}); - /// set (and, if neccessary, declare) the value of a property + /// set (and, if necessary, declare) the value of a property template void set(const std::string& name, const T& value) { auto it = props_.find(name); diff --git a/core/src/properties.cpp b/core/src/properties.cpp index 6365ea39a..d0c99ad67 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -54,7 +54,7 @@ class PropertyTypeRegistry PropertySerializerBase::SerializeFunction serialize_; PropertySerializerBase::DeserializeFunction deserialize_; }; - Entry dummy_; + const Entry dummy_; // map from type_info to corresponding converter functions using RegistryMap = std::map; @@ -193,7 +193,7 @@ Property& Property::configureInitFrom(SourceFlags source, const std::string& nam Property& PropertyMap::declare(const std::string& name, const Property::type_info& type_info, const std::string& description, const boost::any& default_value) { auto it_inserted = props_.insert(std::make_pair(name, Property(type_info, description, default_value))); - // if name was already declared, the new declaration should match in type (except it was boost::any) + // if name was already declared, the new declaration should match in type (except if it was boost::any) if (!it_inserted.second && it_inserted.first->second.type_info_ != typeid(boost::any) && type_info != it_inserted.first->second.type_info_) throw Property::type_error(type_info.name(), it_inserted.first->second.type_info_.name()); @@ -269,7 +269,7 @@ const boost::any& PropertyMap::get(const std::string& name) const { size_t PropertyMap::countDefined(const std::vector& list) const { size_t count = 0u; for (const std::string& name : list) { - if (!get(name).empty()) + if (!property(name).defined()) ++count; } return count; From 5d419bd694844dff8e0466cbf847827848d023d2 Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 7 Apr 2021 18:45:29 +0200 Subject: [PATCH 06/12] working implementation for deserialization --- .../moveit/task_constructor/properties.h | 97 ++------- .../task_constructor/properties/base64.hpp | 134 +++++++++++++ .../task_constructor/properties/serialize.hpp | 184 ++++++++++++++++++ .../moveit/task_constructor/stages/connect.h | 3 + core/src/properties.cpp | 56 +++++- core/src/stages/connect.cpp | 21 ++ core/test/CMakeLists.txt | 2 +- core/test/test_properties.cpp | 54 ++++- 8 files changed, 459 insertions(+), 92 deletions(-) create mode 100644 core/include/moveit/task_constructor/properties/base64.hpp create mode 100644 core/include/moveit/task_constructor/properties/serialize.hpp diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index 807dec8c2..64dcde6b0 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -45,10 +45,11 @@ #include #include #include -#include #include +#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 + static T deserialize(const moveit_task_constructor_msgs::Property& property_msg) { + PropertySerializer(); + return boost::any_cast(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::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 +239,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/include/moveit/task_constructor/properties/base64.hpp b/core/include/moveit/task_constructor/properties/base64.hpp new file mode 100644 index 000000000..f1f582b22 --- /dev/null +++ b/core/include/moveit/task_constructor/properties/base64.hpp @@ -0,0 +1,134 @@ +/** + * @file base64.h + * @author Basit Ayantunde (rlamarrr@gmail.com) + * @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 +#include + +namespace base64 { + +template +struct Base64Chars +{ + static constexpr T data[]{ "ABCDEFGHIJKLMNOPQRSTUVWXYZ" + "abcdefghijklmnopqrstuvwxyz" + "0123456789+/" }; +}; + +template +constexpr T Base64Chars::data[]; + +template +inline bool is_base64(T c) { + return (isalnum(c) || (c == '+') || (c == '/')); +} + +template +std::basic_string encode(const T* bytes_to_encode, uint32_t in_len) { + std::basic_string 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::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::data[char_array_4[j]]; + + while ((i++ < 3)) + ret += '='; + } + + return std::move(ret); +} + +template +std::basic_string 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 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::data, char_array_4[i]) - Base64Chars::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::data, char_array_4[j]) - Base64Chars::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 diff --git a/core/include/moveit/task_constructor/properties/serialize.hpp b/core/include/moveit/task_constructor/properties/serialize.hpp new file mode 100644 index 000000000..ebaa69185 --- /dev/null +++ b/core/include/moveit/task_constructor/properties/serialize.hpp @@ -0,0 +1,184 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld University + * Copyright (c) 2021, Universitaet Hamburg + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Robert Haschke / Michael 'v4hn' Goerner + Desc: Serialization/Deserialization support for Properties +*/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "base64.hpp" + +namespace moveit { +namespace task_constructor { + +class Property; + +// 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 hasInsertionOperator : std::false_type +{}; + +template +struct hasInsertionOperator() << std::declval())> : std::true_type +{}; + +template +constexpr bool hasInsertionOperator_v = hasInsertionOperator::value; + +template +struct hasExtractionOperator : std::false_type +{}; + +template +struct hasExtractionOperator() >> std::declval())> : std::true_type +{}; + +template +constexpr bool hasExtractionOperator_v = hasExtractionOperator::value; + +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); +}; + +template +class PropertySerializer : public PropertySerializerBase +{ +protected: + /// a set of mutually exclusive conditions + template + static constexpr bool IsROSMessage{ ros::message_traits::IsMessage::value }; + + template + static constexpr bool IsStreamSerializable{ !IsROSMessage && hasInsertionOperator_v }; + + template + static constexpr bool IsStreamDeserializable{ !IsROSMessage && hasExtractionOperator_v }; + + template + static constexpr bool IsString{ std::is_same::value }; + + template + static constexpr bool IsNotDeserializable{ !IsROSMessage && !IsStreamSerializable }; + +public: + PropertySerializer() { this->insert(typeid(T), this->typeName(), &serialize, &deserialize); } + + template + static std::enable_if_t, std::string> typeName() { + return typeid(T).name(); + } + + template + static std::enable_if_t, std::string> typeName() { + return ros::message_traits::DataType::value(); + } + + /** ROS messages are serialized using the provided binary encoding */ + template + static std::enable_if_t, std::string> serialize(const boost::any& value) { + Q message{ boost::any_cast(value) }; + uint32_t serial_size = ros::serialization::serializationLength(message); + std::unique_ptr buffer(new uint8_t[serial_size]); + ros::serialization::OStream stream(buffer.get(), serial_size); + ros::serialization::serialize(stream, message); + return reinterpret_cast(base64::encode(buffer.get(), serial_size).c_str()); + } + template + static std::enable_if_t, boost::any> deserialize(const std::string& wired) { + auto decoded{ base64::decode(wired.data(), wired.size()) }; + ros::serialization::IStream stream(const_cast(reinterpret_cast(decoded.data())), + wired.size()); + Q message; + ros::serialization::deserialize(stream, message); + return message; + } + + /** Serialization based on std::[io]stringstream */ + template + static std::enable_if_t, std::string> serialize(const boost::any& value) { + std::ostringstream oss; + oss << boost::any_cast(value); + return oss.str(); + } + template + static std::enable_if_t && !IsString, boost::any> + deserialize(const std::string& wired) { + std::istringstream iss(wired); + T value; + iss >> value; + return value; + } + // (istream >> string) only extracts until first whitespace, so we specialize + template + static std::enable_if_t, boost::any> deserialize(const std::string& wired) { + return wired; + } + + /** No serialization available */ + template + static std::enable_if_t, std::string> serialize(const boost::any& value) { + return dummySerialize(value); + } + + template + static std::enable_if_t, boost::any> deserialize(const std::string& wire) { + return dummyDeserialize(wire); + } +}; +} // namespace task_constructor +} // namespace moveit diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index bb64dbb74..75bbbc6e1 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -98,6 +98,9 @@ class Connect : public Connecting std::list subsolutions_; std::list states_; }; + +std::ostream& operator<<(std::ostream&, const Connect::MergeMode&); +std::istream& operator>>(std::istream&, Connect::MergeMode&); } // namespace stages } // namespace task_constructor } // namespace moveit diff --git a/core/src/properties.cpp b/core/src/properties.cpp index d0c99ad67..d28f6d170 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -37,6 +37,9 @@ */ #include + +#include + #include #include #include @@ -85,8 +88,47 @@ class PropertyTypeRegistry return it->second->second; } }; + static PropertyTypeRegistry REGISTRY_SINGLETON; +class JointMapSerializer : public PropertySerializerBase +{ + using T = std::map; + +public: + JointMapSerializer() { + PropertySerializer(); + insert(typeid(T), typeid(T).name(), &serialize, &deserialize); + } + +protected: + static std::string serialize(const boost::any& value) { + sensor_msgs::JointState state; + for (const auto& p : boost::any_cast(value)) { + state.name.push_back(p.first); + state.position.push_back(p.second); + } + return REGISTRY_SINGLETON.entry(typeid(sensor_msgs::JointState)).serialize_(state); + } + + static boost::any deserialize(const std::string& wired) { + auto state{ boost::any_cast( + REGISTRY_SINGLETON.entry(typeid(sensor_msgs::JointState)).deserialize_(wired)) }; + assert(state.position.size() >= state.name.size()); + T joint_map; + for (size_t i = 0; i < state.name.size(); ++i) + joint_map.insert(std::make_pair(state.name[i], state.position[i])); + return joint_map; + } +}; + +static bool _REGISTRY_INITIALIZER{ []() { + PropertySerializer(); + PropertySerializer(); + JointMapSerializer(); + return true; +}() }; + bool PropertyTypeRegistry::insert(const std::type_index& type_index, const std::string& type_name, PropertySerializerBase::SerializeFunction serialize, PropertySerializerBase::DeserializeFunction deserialize) { @@ -216,11 +258,15 @@ void PropertyMap::fillMsgs(std::vector& msgs.reserve(size()); for (const auto& pair : *this) { msgs.emplace_back(); - auto& p{ msgs.back() }; - p.name = pair.first; - p.description = pair.second.description(); - p.type = pair.second.typeName(); - p.value = pair.second.serialize(); + msgs.back().name = pair.first; + pair.second.fillMsg(msgs.back()); + } +} + +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); } } diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 9aadd4b89..2af6eece8 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -230,6 +230,27 @@ SubTrajectoryPtr Connect::merge(const std::vector(trajectory); } + +const std::vector> MERGE_MODE_MAP{ { Connect::SEQUENTIAL, "SEQUENTIAL" }, + { Connect::WAYPOINTS, "WAYPOINTS" } }; + +std::ostream& operator<<(std::ostream& s, const Connect::MergeMode& mode) { + s << MERGE_MODE_MAP.at(mode).second; + return s; +} + +std::istream& operator>>(std::istream& s, Connect::MergeMode& mode) { + std::string word; + s >> word; + for (const auto& m : MERGE_MODE_MAP) + if (m.second == word) { + mode = m.first; + return s; + } + s.setstate(std::ios::failbit); + return s; +} + } // namespace stages } // namespace task_constructor } // namespace moveit diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 3d7777c4a..3546c23c3 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -19,7 +19,7 @@ if (CATKIN_ENABLE_TESTING) target_link_libraries(${PROJECT_NAME}-test-serial ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main) catkin_add_gtest(${PROJECT_NAME}-test-properties test_properties.cpp) - target_link_libraries(${PROJECT_NAME}-test-properties ${PROJECT_NAME} gtest_main) + target_link_libraries(${PROJECT_NAME}-test-properties ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_main) catkin_add_gmock(${PROJECT_NAME}-test-cost_queue test_cost_queue.cpp) target_link_libraries(${PROJECT_NAME}-test-cost_queue ${PROJECT_NAME} gtest_main) diff --git a/core/test/test_properties.cpp b/core/test/test_properties.cpp index c5aa67e42..6d55e5ce2 100644 --- a/core/test/test_properties.cpp +++ b/core/test/test_properties.cpp @@ -1,5 +1,8 @@ #include +#include +#include + #include #include @@ -86,12 +89,12 @@ TEST(Property, anytype) { } TEST(Property, serialize_basic) { - EXPECT_TRUE(hasSerialize::value); - EXPECT_TRUE(hasDeserialize::value); - EXPECT_TRUE(hasSerialize::value); - EXPECT_TRUE(hasDeserialize::value); - EXPECT_TRUE(hasSerialize::value); - EXPECT_TRUE(hasDeserialize::value); + EXPECT_TRUE(hasInsertionOperator::value); + EXPECT_TRUE(hasExtractionOperator::value); + EXPECT_TRUE(hasInsertionOperator::value); + EXPECT_TRUE(hasExtractionOperator::value); + EXPECT_TRUE(hasInsertionOperator::value); + EXPECT_TRUE(hasExtractionOperator::value); } TEST(Property, serialize) { @@ -106,6 +109,45 @@ TEST(Property, serialize) { EXPECT_EQ(props.property("map").serialize(), ""); } +#define STORABLE_PROPERTY(T, V) \ + { \ + T v{ V }; \ + PropertyMap example_props; \ + example_props.declare("property", "description"); \ + example_props.set("property", v); \ + moveit_task_constructor_msgs::Property props_msg; \ + example_props.property("property").fillMsg(props_msg); \ + std::cout << "serialized property message:\n" << props_msg << std::endl; \ + T deserialized_value; \ + try { \ + deserialized_value = Property::deserialize(props_msg); \ + } catch (boost::bad_any_cast&) { \ + ADD_FAILURE() << "deserialization failed"; \ + } \ + EXPECT_EQ(deserialized_value, v); \ + } + +TEST(Property, storeInMsg) { + STORABLE_PROPERTY(std::string, "custom text"); + STORABLE_PROPERTY(int, 42); + STORABLE_PROPERTY(double, 42); + STORABLE_PROPERTY(stages::Connect::MergeMode, stages::Connect::WAYPOINTS); + // STORABLE_PROPERTY(double, M_PI); + + STORABLE_PROPERTY(geometry_msgs::PoseStamped, []() { + geometry_msgs::PoseStamped msg; + msg.header.frame_id = "world"; + msg.pose.orientation.x = 0.5; + msg.pose.orientation.y = 0.5; + msg.pose.orientation.z = -0.5; + msg.pose.orientation.w = -0.5; + return msg; + }()); + + std::map joint_targets{ { "joint0", -0.5 }, { "joint1", 1.0 } }; + STORABLE_PROPERTY(decltype(joint_targets), joint_targets); +} + class InitFromTest : public ::testing::Test { protected: From fb0fa19b04f1d45717e846c2aa20be1c5cf18fb6 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 9 Sep 2021 22:33:09 +0200 Subject: [PATCH 07/12] [POC] add message property as rviz property --- .../properties/property_factory.cpp | 43 +++++++++++++------ 1 file changed, 30 insertions(+), 13 deletions(-) diff --git a/visualization/motion_planning_tasks/properties/property_factory.cpp b/visualization/motion_planning_tasks/properties/property_factory.cpp index 95542a74a..1057f54ed 100644 --- a/visualization/motion_planning_tasks/properties/property_factory.cpp +++ b/visualization/motion_planning_tasks/properties/property_factory.cpp @@ -43,28 +43,30 @@ #include #include #include +#include + +#include namespace mtc = ::moveit::task_constructor; namespace moveit_rviz_plugin { -static rviz::StringProperty* stringFactory(const QString& name, mtc::Property& mtc_prop, - const planning_scene::PlanningScene* /*unused*/, - rviz::DisplayContext* /*unused*/) { - std::string value; - if (!mtc_prop.value().empty()) - value = boost::any_cast(mtc_prop.value()); - rviz::StringProperty* rviz_prop = +namespace { + +rviz::Property* stringFactory(const QString& name, mtc::Property& mtc_prop, + const planning_scene::PlanningScene* /*scene*/, rviz::DisplayContext* /*context*/) { + std::string value{ mtc_prop.defined() ? mtc_prop.value() : "" }; + auto* rviz_prop = new rviz::StringProperty(name, QString::fromStdString(value), QString::fromStdString(mtc_prop.description())); QObject::connect(rviz_prop, &rviz::StringProperty::changed, [rviz_prop, &mtc_prop]() { mtc_prop.setValue(rviz_prop->getStdString()); }); return rviz_prop; } + template -static rviz::FloatProperty* floatFactory(const QString& name, mtc::Property& mtc_prop, - const planning_scene::PlanningScene* /*unused*/, - rviz::DisplayContext* /*unused*/) { - T value = !mtc_prop.value().empty() ? boost::any_cast(mtc_prop.value()) : T(); +static rviz::Property* floatFactory(const QString& name, mtc::Property& mtc_prop, + const planning_scene::PlanningScene* /*unused*/, rviz::DisplayContext* /*unused*/) { + T value{ mtc_prop.defined() ? mtc_prop.value() : static_cast(0.0) }; rviz::FloatProperty* rviz_prop = new rviz::FloatProperty(name, value, QString::fromStdString(mtc_prop.description())); QObject::connect(rviz_prop, &rviz::FloatProperty::changed, @@ -72,11 +74,26 @@ static rviz::FloatProperty* floatFactory(const QString& name, mtc::Property& mtc return rviz_prop; } +rviz::Property* vector3Factory(const QString& name, mtc::Property& mtc_prop, + const planning_scene::PlanningScene* /*scene*/, rviz::DisplayContext* /*context*/) { + auto value{ mtc_prop.defined() ? mtc_prop.value() : geometry_msgs::Vector3Stamped{} }; + + auto* rviz_prop = + new rviz::VectorProperty(name, Ogre::Vector3::ZERO, QString::fromStdString(mtc_prop.description())); + rviz_prop->setVector(Ogre::Vector3{ static_cast(value.vector.x), static_cast(value.vector.y), + static_cast(value.vector.z) }); + + return rviz_prop; +} + +} // namespace + PropertyFactory::PropertyFactory() { - // register some standard types + // register standard types + registerType(&stringFactory); registerType(&floatFactory); registerType(&floatFactory); - registerType(&stringFactory); + registerType(&vector3Factory); } PropertyFactory& PropertyFactory::instance() { From bee62655fca36c42cea8d392bd4f91b7fc99616d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 10 Sep 2021 02:07:35 +0200 Subject: [PATCH 08/12] Simplify definition of PropertySerializer Derive from a type conditioned on the actual capabilities of T. --- .../task_constructor/properties/base64.hpp | 4 +- .../task_constructor/properties/serialize.hpp | 108 ++++++++---------- core/test/test_properties.cpp | 13 ++- 3 files changed, 58 insertions(+), 67 deletions(-) diff --git a/core/include/moveit/task_constructor/properties/base64.hpp b/core/include/moveit/task_constructor/properties/base64.hpp index f1f582b22..a04251709 100644 --- a/core/include/moveit/task_constructor/properties/base64.hpp +++ b/core/include/moveit/task_constructor/properties/base64.hpp @@ -83,7 +83,7 @@ std::basic_string encode(const T* bytes_to_encode, uint32_t in_len) { ret += '='; } - return std::move(ret); + return ret; } template @@ -126,7 +126,7 @@ std::basic_string decode(const T* encoded_string, int64_t in_len) { ret += char_array_3[j]; } - return std::move(ret); + return ret; } }; // namespace base64 diff --git a/core/include/moveit/task_constructor/properties/serialize.hpp b/core/include/moveit/task_constructor/properties/serialize.hpp index ebaa69185..1ef0ca941 100644 --- a/core/include/moveit/task_constructor/properties/serialize.hpp +++ b/core/include/moveit/task_constructor/properties/serialize.hpp @@ -41,6 +41,7 @@ #include #include +#include #include #include #include @@ -57,30 +58,24 @@ namespace task_constructor { class Property; -// hasSerialize::value provides a true/false constexpr depending on whether operator<< is supported. +// has*Operator::value is true iff operator<< resp. operator>> is available for T. // This uses SFINAE, extracted from https://jguegant.github.io/blogs/tech/sfinae-introduction.html template -struct hasInsertionOperator : std::false_type +struct IsStreamSerializable : std::false_type {}; template -struct hasInsertionOperator() << std::declval())> : std::true_type +struct IsStreamSerializable() << std::declval())> : std::true_type {}; -template -constexpr bool hasInsertionOperator_v = hasInsertionOperator::value; - template -struct hasExtractionOperator : std::false_type +struct IsStreamDeserializable : std::false_type {}; template -struct hasExtractionOperator() >> std::declval())> : std::true_type +struct IsStreamDeserializable() >> std::declval())> : std::true_type {}; -template -constexpr bool hasExtractionOperator_v = hasExtractionOperator::value; - class PropertySerializerBase { public: @@ -95,90 +90,85 @@ class PropertySerializerBase DeserializeFunction deserialize); }; -template -class PropertySerializer : public PropertySerializerBase +/** Serialization for std::string **/ +class PropertySerializerString : public PropertySerializerBase { -protected: - /// a set of mutually exclusive conditions - template - static constexpr bool IsROSMessage{ ros::message_traits::IsMessage::value }; - - template - static constexpr bool IsStreamSerializable{ !IsROSMessage && hasInsertionOperator_v }; - - template - static constexpr bool IsStreamDeserializable{ !IsROSMessage && hasExtractionOperator_v }; - - template - static constexpr bool IsString{ std::is_same::value }; - - template - static constexpr bool IsNotDeserializable{ !IsROSMessage && !IsStreamSerializable }; - public: - PropertySerializer() { this->insert(typeid(T), this->typeName(), &serialize, &deserialize); } + static const char* typeName() { return typeid(std::string).name(); } - template - static std::enable_if_t, std::string> typeName() { - return typeid(T).name(); - } + static std::string serialize(const boost::any& value) { return boost::any_cast(value); } + static boost::any deserialize(const std::string& wired) { return wired; } +}; - template - static std::enable_if_t, std::string> typeName() { - return ros::message_traits::DataType::value(); - } +/** Serialization for ROS messages **/ +template +class PropertySerializerROS : public PropertySerializerBase +{ +public: + static const char* typeName() { return ros::message_traits::DataType::value(); } - /** ROS messages are serialized using the provided binary encoding */ - template - static std::enable_if_t, std::string> serialize(const boost::any& value) { - Q message{ boost::any_cast(value) }; + static std::string serialize(const boost::any& value) { + T message{ boost::any_cast(value) }; uint32_t serial_size = ros::serialization::serializationLength(message); std::unique_ptr buffer(new uint8_t[serial_size]); ros::serialization::OStream stream(buffer.get(), serial_size); ros::serialization::serialize(stream, message); return reinterpret_cast(base64::encode(buffer.get(), serial_size).c_str()); } - template - static std::enable_if_t, boost::any> deserialize(const std::string& wired) { + + static boost::any deserialize(const std::string& wired) { auto decoded{ base64::decode(wired.data(), wired.size()) }; ros::serialization::IStream stream(const_cast(reinterpret_cast(decoded.data())), wired.size()); - Q message; + T message; ros::serialization::deserialize(stream, message); return message; } +}; + +/** Serialization based on std::[io]stringstream */ +template +class PropertySerializerStream : public PropertySerializerBase +{ +public: + static const char* typeName() { return typeid(T).name(); } - /** Serialization based on std::[io]stringstream */ template - static std::enable_if_t, std::string> serialize(const boost::any& value) { + static std::enable_if_t::value, std::string> serialize(const boost::any& value) { std::ostringstream oss; oss << boost::any_cast(value); return oss.str(); } template - static std::enable_if_t && !IsString, boost::any> - deserialize(const std::string& wired) { + static std::enable_if_t::value, boost::any> deserialize(const std::string& wired) { std::istringstream iss(wired); T value; iss >> value; return value; } - // (istream >> string) only extracts until first whitespace, so we specialize - template - static std::enable_if_t, boost::any> deserialize(const std::string& wired) { - return wired; - } - /** No serialization available */ + /** fallback, if no serialization/deserialization is available **/ template - static std::enable_if_t, std::string> serialize(const boost::any& value) { + static std::enable_if_t::value, std::string> serialize(const boost::any& value) { return dummySerialize(value); } - template - static std::enable_if_t, boost::any> deserialize(const std::string& wire) { + static std::enable_if_t::value, boost::any> deserialize(const std::string& wire) { return dummyDeserialize(wire); } }; + +template +class PropertySerializer + : public std::conditional_t::value, PropertySerializerString, + std::conditional_t::value, PropertySerializerROS, + PropertySerializerStream>> +{ +public: + PropertySerializer() { + this->insert(typeid(T), this->typeName(), &PropertySerializer::serialize, &PropertySerializer::deserialize); + } +}; + } // namespace task_constructor } // namespace moveit diff --git a/core/test/test_properties.cpp b/core/test/test_properties.cpp index 6d55e5ce2..c080a5615 100644 --- a/core/test/test_properties.cpp +++ b/core/test/test_properties.cpp @@ -89,12 +89,13 @@ TEST(Property, anytype) { } TEST(Property, serialize_basic) { - EXPECT_TRUE(hasInsertionOperator::value); - EXPECT_TRUE(hasExtractionOperator::value); - EXPECT_TRUE(hasInsertionOperator::value); - EXPECT_TRUE(hasExtractionOperator::value); - EXPECT_TRUE(hasInsertionOperator::value); - EXPECT_TRUE(hasExtractionOperator::value); + EXPECT_TRUE(IsStreamSerializable::value); + EXPECT_TRUE(IsStreamSerializable::value); + EXPECT_TRUE(IsStreamSerializable::value); + + EXPECT_TRUE(IsStreamDeserializable::value); + EXPECT_TRUE(IsStreamDeserializable::value); + EXPECT_TRUE(IsStreamDeserializable::value); } TEST(Property, serialize) { From 09d297832e5c7db96aea2907350072ee66666fe3 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 10 Sep 2021 03:47:46 +0200 Subject: [PATCH 09/12] Skip Base64 encoding --- .../task_constructor/properties/base64.hpp | 134 ------------------ .../task_constructor/properties/serialize.hpp | 11 +- 2 files changed, 4 insertions(+), 141 deletions(-) delete mode 100644 core/include/moveit/task_constructor/properties/base64.hpp diff --git a/core/include/moveit/task_constructor/properties/base64.hpp b/core/include/moveit/task_constructor/properties/base64.hpp deleted file mode 100644 index a04251709..000000000 --- a/core/include/moveit/task_constructor/properties/base64.hpp +++ /dev/null @@ -1,134 +0,0 @@ -/** - * @file base64.h - * @author Basit Ayantunde (rlamarrr@gmail.com) - * @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 -#include - -namespace base64 { - -template -struct Base64Chars -{ - static constexpr T data[]{ "ABCDEFGHIJKLMNOPQRSTUVWXYZ" - "abcdefghijklmnopqrstuvwxyz" - "0123456789+/" }; -}; - -template -constexpr T Base64Chars::data[]; - -template -inline bool is_base64(T c) { - return (isalnum(c) || (c == '+') || (c == '/')); -} - -template -std::basic_string encode(const T* bytes_to_encode, uint32_t in_len) { - std::basic_string 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::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::data[char_array_4[j]]; - - while ((i++ < 3)) - ret += '='; - } - - return ret; -} - -template -std::basic_string 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 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::data, char_array_4[i]) - Base64Chars::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::data, char_array_4[j]) - Base64Chars::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 ret; -} - -}; // namespace base64 - -#endif diff --git a/core/include/moveit/task_constructor/properties/serialize.hpp b/core/include/moveit/task_constructor/properties/serialize.hpp index 1ef0ca941..77e7c567e 100644 --- a/core/include/moveit/task_constructor/properties/serialize.hpp +++ b/core/include/moveit/task_constructor/properties/serialize.hpp @@ -51,8 +51,6 @@ #include -#include "base64.hpp" - namespace moveit { namespace task_constructor { @@ -110,15 +108,14 @@ class PropertySerializerROS : public PropertySerializerBase static std::string serialize(const boost::any& value) { T message{ boost::any_cast(value) }; uint32_t serial_size = ros::serialization::serializationLength(message); - std::unique_ptr buffer(new uint8_t[serial_size]); - ros::serialization::OStream stream(buffer.get(), serial_size); + std::string buffer(serial_size, '\0'); + ros::serialization::OStream stream(reinterpret_cast(&buffer.front()), serial_size); ros::serialization::serialize(stream, message); - return reinterpret_cast(base64::encode(buffer.get(), serial_size).c_str()); + return buffer; } static boost::any deserialize(const std::string& wired) { - auto decoded{ base64::decode(wired.data(), wired.size()) }; - ros::serialization::IStream stream(const_cast(reinterpret_cast(decoded.data())), + ros::serialization::IStream stream(const_cast(reinterpret_cast(&wired.front())), wired.size()); T message; ros::serialization::deserialize(stream, message); From 0d3ce1d4623015ce5918983f432a91ed1b80fa26 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 10 Sep 2021 06:38:19 +0200 Subject: [PATCH 10/12] Correctly define serialization for std::map --- .../task_constructor/properties/serialize.hpp | 35 +++++++++++++++- core/src/properties.cpp | 40 ------------------- 2 files changed, 34 insertions(+), 41 deletions(-) diff --git a/core/include/moveit/task_constructor/properties/serialize.hpp b/core/include/moveit/task_constructor/properties/serialize.hpp index 77e7c567e..0b9070dfb 100644 --- a/core/include/moveit/task_constructor/properties/serialize.hpp +++ b/core/include/moveit/task_constructor/properties/serialize.hpp @@ -48,7 +48,7 @@ #include #include #include - +#include #include namespace moveit { @@ -167,5 +167,38 @@ class PropertySerializer } }; +template <> +class PropertySerializer> : public PropertySerializerROS +{ + using T = std::map; + using BaseT = PropertySerializerROS; + +public: + PropertySerializer() { insert(typeid(T), this->typeName(), &serialize, &deserialize); } + + static const char* typeName() { return typeid(T).name(); } + + static std::string serialize(const boost::any& value) { + T values = boost::any_cast(value); + sensor_msgs::JointState state; + state.name.reserve(values.size()); + state.position.reserve(values.size()); + for (const auto& p : values) { + state.name.push_back(p.first); + state.position.push_back(p.second); + } + return BaseT::serialize(state); + } + + static boost::any deserialize(const std::string& wired) { + auto state = boost::any_cast(BaseT::deserialize(wired)); + assert(state.position.size() == state.name.size()); + T values; + for (size_t i = 0; i < state.name.size(); ++i) + values.insert(std::make_pair(state.name[i], state.position[i])); + return values; + } +}; + } // namespace task_constructor } // namespace moveit diff --git a/core/src/properties.cpp b/core/src/properties.cpp index d28f6d170..523f891ae 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -38,8 +38,6 @@ #include -#include - #include #include #include @@ -91,44 +89,6 @@ class PropertyTypeRegistry static PropertyTypeRegistry REGISTRY_SINGLETON; -class JointMapSerializer : public PropertySerializerBase -{ - using T = std::map; - -public: - JointMapSerializer() { - PropertySerializer(); - insert(typeid(T), typeid(T).name(), &serialize, &deserialize); - } - -protected: - static std::string serialize(const boost::any& value) { - sensor_msgs::JointState state; - for (const auto& p : boost::any_cast(value)) { - state.name.push_back(p.first); - state.position.push_back(p.second); - } - return REGISTRY_SINGLETON.entry(typeid(sensor_msgs::JointState)).serialize_(state); - } - - static boost::any deserialize(const std::string& wired) { - auto state{ boost::any_cast( - REGISTRY_SINGLETON.entry(typeid(sensor_msgs::JointState)).deserialize_(wired)) }; - assert(state.position.size() >= state.name.size()); - T joint_map; - for (size_t i = 0; i < state.name.size(); ++i) - joint_map.insert(std::make_pair(state.name[i], state.position[i])); - return joint_map; - } -}; - -static bool _REGISTRY_INITIALIZER{ []() { - PropertySerializer(); - PropertySerializer(); - JointMapSerializer(); - return true; -}() }; - bool PropertyTypeRegistry::insert(const std::type_index& type_index, const std::string& type_name, PropertySerializerBase::SerializeFunction serialize, PropertySerializerBase::DeserializeFunction deserialize) { From 4b8957c5ea2da5b77ff76382db752b516bd4f1cf Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 10 Sep 2021 22:00:53 +0200 Subject: [PATCH 11/12] Back to has*Operator --- .../task_constructor/properties/serialize.hpp | 21 ++++++++++++------- core/test/test_properties.cpp | 12 +++++------ 2 files changed, 19 insertions(+), 14 deletions(-) diff --git a/core/include/moveit/task_constructor/properties/serialize.hpp b/core/include/moveit/task_constructor/properties/serialize.hpp index 0b9070dfb..467274596 100644 --- a/core/include/moveit/task_constructor/properties/serialize.hpp +++ b/core/include/moveit/task_constructor/properties/serialize.hpp @@ -59,21 +59,26 @@ class Property; // has*Operator::value is true iff operator<< resp. operator>> is available for T. // This uses SFINAE, extracted from https://jguegant.github.io/blogs/tech/sfinae-introduction.html template -struct IsStreamSerializable : std::false_type +struct hasInsertionOperator : std::false_type {}; template -struct IsStreamSerializable() << std::declval())> : std::true_type +struct hasInsertionOperator() << std::declval())> : std::true_type {}; template -struct IsStreamDeserializable : std::false_type +struct hasExtractionOperator : std::false_type {}; template -struct IsStreamDeserializable() >> std::declval())> : std::true_type +struct hasExtractionOperator() >> std::declval())> : std::true_type {}; +template +inline constexpr bool IsStreamDeserializable() { + return hasInsertionOperator::value && hasExtractionOperator::value; +} + class PropertySerializerBase { public: @@ -131,13 +136,13 @@ class PropertySerializerStream : public PropertySerializerBase static const char* typeName() { return typeid(T).name(); } template - static std::enable_if_t::value, std::string> serialize(const boost::any& value) { + static std::enable_if_t::value, std::string> serialize(const boost::any& value) { std::ostringstream oss; oss << boost::any_cast(value); return oss.str(); } template - static std::enable_if_t::value, boost::any> deserialize(const std::string& wired) { + static std::enable_if_t(), boost::any> deserialize(const std::string& wired) { std::istringstream iss(wired); T value; iss >> value; @@ -146,11 +151,11 @@ class PropertySerializerStream : public PropertySerializerBase /** fallback, if no serialization/deserialization is available **/ template - static std::enable_if_t::value, std::string> serialize(const boost::any& value) { + static std::enable_if_t::value, std::string> serialize(const boost::any& value) { return dummySerialize(value); } template - static std::enable_if_t::value, boost::any> deserialize(const std::string& wire) { + static std::enable_if_t(), boost::any> deserialize(const std::string& wire) { return dummyDeserialize(wire); } }; diff --git a/core/test/test_properties.cpp b/core/test/test_properties.cpp index c080a5615..46c30d269 100644 --- a/core/test/test_properties.cpp +++ b/core/test/test_properties.cpp @@ -89,13 +89,13 @@ TEST(Property, anytype) { } TEST(Property, serialize_basic) { - EXPECT_TRUE(IsStreamSerializable::value); - EXPECT_TRUE(IsStreamSerializable::value); - EXPECT_TRUE(IsStreamSerializable::value); + EXPECT_TRUE(hasInsertionOperator::value); + EXPECT_TRUE(hasInsertionOperator::value); + EXPECT_TRUE(hasInsertionOperator::value); - EXPECT_TRUE(IsStreamDeserializable::value); - EXPECT_TRUE(IsStreamDeserializable::value); - EXPECT_TRUE(IsStreamDeserializable::value); + EXPECT_TRUE(hasExtractionOperator::value); + EXPECT_TRUE(hasExtractionOperator::value); + EXPECT_TRUE(hasExtractionOperator::value); } TEST(Property, serialize) { From 45935c0b08050f09b86f5192b5ec34d1286cf995 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 10 Sep 2021 22:43:40 +0200 Subject: [PATCH 12/12] Fix precision issue for floating-point types --- .../moveit/task_constructor/properties/serialize.hpp | 12 ++++++++++++ core/test/test_properties.cpp | 2 +- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/core/include/moveit/task_constructor/properties/serialize.hpp b/core/include/moveit/task_constructor/properties/serialize.hpp index 467274596..2a066db57 100644 --- a/core/include/moveit/task_constructor/properties/serialize.hpp +++ b/core/include/moveit/task_constructor/properties/serialize.hpp @@ -128,6 +128,17 @@ class PropertySerializerROS : public PropertySerializerBase } }; +template +struct oss_configure +{ + static void setprecision(std::ostringstream& oss) {} +}; +template +struct oss_configure::value>> +{ + static void setprecision(std::ostringstream& oss) { oss << std::setprecision(std::numeric_limits::digits10 + 1); } +}; + /** Serialization based on std::[io]stringstream */ template class PropertySerializerStream : public PropertySerializerBase @@ -138,6 +149,7 @@ class PropertySerializerStream : public PropertySerializerBase template static std::enable_if_t::value, std::string> serialize(const boost::any& value) { std::ostringstream oss; + oss_configure::setprecision(oss); oss << boost::any_cast(value); return oss.str(); } diff --git a/core/test/test_properties.cpp b/core/test/test_properties.cpp index 46c30d269..0e3075bf1 100644 --- a/core/test/test_properties.cpp +++ b/core/test/test_properties.cpp @@ -133,7 +133,7 @@ TEST(Property, storeInMsg) { STORABLE_PROPERTY(int, 42); STORABLE_PROPERTY(double, 42); STORABLE_PROPERTY(stages::Connect::MergeMode, stages::Connect::WAYPOINTS); - // STORABLE_PROPERTY(double, M_PI); + STORABLE_PROPERTY(double, M_PI); STORABLE_PROPERTY(geometry_msgs::PoseStamped, []() { geometry_msgs::PoseStamped msg;