Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

add external properties to SolutionInfo #194

Open
wants to merge 12 commits into
base: master
Choose a base branch
from
Open

add external properties to SolutionInfo #194

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
7139e99
add properties to SolutionInfo
v4hn Jul 30, 2020
5646ecf
move typed getters to Property & drop fallback interface
v4hn Apr 7, 2021
3277abf
add PropertyMap -> vector<Msg> interface
v4hn Apr 7, 2021
ac22156
do not allow write-access to default value
v4hn Apr 7, 2021
476424d
lipstick
v4hn Apr 7, 2021
5d419bd
working implementation for deserialization
v4hn Apr 7, 2021
fb0fa19
[POC] add message property as rviz property
v4hn Sep 9, 2021
bee6265
Simplify definition of PropertySerializer<T>
rhaschke Sep 10, 2021
09d2978
Skip Base64 encoding
rhaschke Sep 10, 2021
0d3ce1d
Correctly define serialization for std::map<std::string, double>
rhaschke Sep 10, 2021
4b8957c
Back to has*Operator<T>
rhaschke Sep 10, 2021
45935c0
Fix precision issue for floating-point types
rhaschke Sep 10, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
205 changes: 78 additions & 127 deletions core/include/moveit/task_constructor/properties.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,10 @@
#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 {
Expand Down Expand Up @@ -81,13 +84,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
Expand All @@ -101,18 +131,39 @@ 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()); }
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Here you change the semantics of defined()!
I guess you want to distinguish isDefined() from isSet() == hasCurrentValue()?
Let's discuss the naming over phone...


/// is a non-default value set?
inline bool hasCurrentValue() const { return !value_.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 <typename T>
inline const T& value() const {
const boost::any& v{ value() };
if (v.empty())
throw Property::undefined();
return boost::any_cast<const T&>(value());
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
return boost::any_cast<const T&>(value());
return boost::any_cast<const T&>(v);

See also my comment on PropertyMap::get().

}

/// 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);
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
Expand All @@ -130,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_;
Expand All @@ -142,112 +199,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<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
Expand Down Expand Up @@ -288,18 +239,22 @@ 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>& 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;

iterator begin() { return props_.begin(); }
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<std::string>& properties = {});

/// set (and, if neccessary, declare) the value of a property
/// set (and, if necessary, declare) the value of a property
template <typename T>
void set(const std::string& name, const T& value) {
auto it = props_.find(name);
Expand All @@ -318,19 +273,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 <typename T>
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<const T&>(value);
}
/// get typed value of property, using fallback if undefined. Throws bad_any_cast on type mismatch.
template <typename T>
const T& get(const std::string& name, const T& fallback) const {
const boost::any& value = get(name);
return (value.empty()) ? fallback : boost::any_cast<const T&>(value);
try {
return property(name).value<T>();
} catch (Property::undefined& e) {
e.setName(name);
throw e;
Comment on lines +281 to +283
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Catching and rethrowing is more costly than the previous version of testing and throwing only once.

}
}

/// count number of defined properties from given list
Expand Down
Loading