Skip to content

Commit 50ef6ea

Browse files
v4hnv4hn
committed
WIP: fromMsgs
1 parent 476424d commit 50ef6ea

File tree

3 files changed

+121
-79
lines changed

3 files changed

+121
-79
lines changed

‎core/include/moveit/task_constructor/properties.h‎

Lines changed: 90 additions & 79 deletions
Original file line numberDiff line numberDiff line change
@@ -58,6 +58,84 @@ class PropertyMap;
5858
/// initializer function, using given name from the passed property map
5959
boost::any fromName(const PropertyMap& other, const std::string& other_name);
6060

61+
// hasSerialize<T>::value provides a true/false constexpr depending on whether operator<< is supported.
62+
// This uses SFINAE, extracted from https://jguegant.github.io/blogs/tech/sfinae-introduction.html
63+
template <typename T, typename = std::ostream&>
64+
struct hasSerialize : std::false_type
65+
{};
66+
67+
template <typename T>
68+
struct hasSerialize<T, decltype(std::declval<std::ostream&>() << std::declval<T>())> : std::true_type
69+
{};
70+
71+
template <typename T, typename = std::istream&>
72+
struct hasDeserialize : std::false_type
73+
{};
74+
75+
template <typename T>
76+
struct hasDeserialize<T, decltype(std::declval<std::istream&>() >> std::declval<T&>())> : std::true_type
77+
{};
78+
79+
class PropertySerializerBase
80+
{
81+
public:
82+
using SerializeFunction = std::string (*)(const boost::any&);
83+
using DeserializeFunction = boost::any (*)(const std::string&);
84+
85+
static std::string dummySerialize(const boost::any& /*unused*/) { return ""; }
86+
static boost::any dummyDeserialize(const std::string& /*unused*/) { return boost::any(); }
87+
88+
protected:
89+
static bool insert(const std::type_index& type_index, const std::string& type_name, SerializeFunction serialize,
90+
DeserializeFunction deserialize);
91+
};
92+
93+
/// utility class to register serializer/deserializer functions for a property of type T
94+
template <typename T>
95+
class PropertySerializer : protected PropertySerializerBase
96+
{
97+
public:
98+
PropertySerializer() { insert(typeid(T), typeName<T>(), &serialize, &deserialize); }
99+
100+
template <class Q = T>
101+
static typename std::enable_if<ros::message_traits::IsMessage<Q>::value, std::string>::type typeName() {
102+
return ros::message_traits::DataType<T>::value();
103+
}
104+
105+
template <class Q = T>
106+
static typename std::enable_if<!ros::message_traits::IsMessage<Q>::value, std::string>::type typeName() {
107+
return typeid(T).name();
108+
}
109+
110+
private:
111+
/** Serialization based on std::[io]stringstream */
112+
template <class Q = T>
113+
static typename std::enable_if<hasSerialize<Q>::value, std::string>::type serialize(const boost::any& value) {
114+
std::ostringstream oss;
115+
oss << boost::any_cast<T>(value);
116+
return oss.str();
117+
}
118+
template <class Q = T>
119+
static typename std::enable_if<hasSerialize<Q>::value && hasDeserialize<Q>::value, boost::any>::type
120+
deserialize(const std::string& wired) {
121+
std::istringstream iss(wired);
122+
T value;
123+
iss >> value;
124+
return value;
125+
}
126+
127+
/** No serialization available */
128+
template <class Q = T>
129+
static typename std::enable_if<!hasSerialize<Q>::value, std::string>::type serialize(const boost::any& value) {
130+
return dummySerialize(value);
131+
}
132+
template <class Q = T>
133+
static typename std::enable_if<!hasSerialize<Q>::value || !hasDeserialize<Q>::value, boost::any>::type
134+
deserialize(const std::string& wire) {
135+
return dummyDeserialize(wire);
136+
}
137+
};
138+
61139
/** Property is a wrapper for a boost::any value, also providing a default value and a description.
62140
*
63141
* Properties can be configured to be initialized from another PropertyMap - if still undefined.
@@ -122,6 +200,16 @@ class Property
122200
/// function callback used to initialize property value from another PropertyMap
123201
using InitializerFunction = std::function<boost::any(const PropertyMap&)>;
124202

203+
template <typename T>
204+
static Property fromMsg(const moveit_task_constructor_msgs::Property& msg) {
205+
auto requested_type{ typeid(T).name() };
206+
if (msg.type != requested_type) {
207+
throw type_error{ requested_type, msg.type };
208+
}
209+
PropertySerializer<T>(); // register serializer/deserializer
210+
return Property(typeid(T), msg.description, msg.value);
211+
};
212+
125213
/// set current value and default value
126214
void setValue(const boost::any& value);
127215
void setCurrentValue(const boost::any& value);
@@ -185,84 +273,6 @@ class Property
185273
InitializerFunction initializer_;
186274
};
187275

188-
// hasSerialize<T>::value provides a true/false constexpr depending on whether operator<< is supported.
189-
// This uses SFINAE, extracted from https://jguegant.github.io/blogs/tech/sfinae-introduction.html
190-
template <typename T, typename = std::ostream&>
191-
struct hasSerialize : std::false_type
192-
{};
193-
194-
template <typename T>
195-
struct hasSerialize<T, decltype(std::declval<std::ostream&>() << std::declval<T>())> : std::true_type
196-
{};
197-
198-
template <typename T, typename = std::istream&>
199-
struct hasDeserialize : std::false_type
200-
{};
201-
202-
template <typename T>
203-
struct hasDeserialize<T, decltype(std::declval<std::istream&>() >> std::declval<T&>())> : std::true_type
204-
{};
205-
206-
class PropertySerializerBase
207-
{
208-
public:
209-
using SerializeFunction = std::string (*)(const boost::any&);
210-
using DeserializeFunction = boost::any (*)(const std::string&);
211-
212-
static std::string dummySerialize(const boost::any& /*unused*/) { return ""; }
213-
static boost::any dummyDeserialize(const std::string& /*unused*/) { return boost::any(); }
214-
215-
protected:
216-
static bool insert(const std::type_index& type_index, const std::string& type_name, SerializeFunction serialize,
217-
DeserializeFunction deserialize);
218-
};
219-
220-
/// utility class to register serializer/deserializer functions for a property of type T
221-
template <typename T>
222-
class PropertySerializer : protected PropertySerializerBase
223-
{
224-
public:
225-
PropertySerializer() { insert(typeid(T), typeName<T>(), &serialize, &deserialize); }
226-
227-
template <class Q = T>
228-
static typename std::enable_if<ros::message_traits::IsMessage<Q>::value, std::string>::type typeName() {
229-
return ros::message_traits::DataType<T>::value();
230-
}
231-
232-
template <class Q = T>
233-
static typename std::enable_if<!ros::message_traits::IsMessage<Q>::value, std::string>::type typeName() {
234-
return typeid(T).name();
235-
}
236-
237-
private:
238-
/** Serialization based on std::[io]stringstream */
239-
template <class Q = T>
240-
static typename std::enable_if<hasSerialize<Q>::value, std::string>::type serialize(const boost::any& value) {
241-
std::ostringstream oss;
242-
oss << boost::any_cast<T>(value);
243-
return oss.str();
244-
}
245-
template <class Q = T>
246-
static typename std::enable_if<hasSerialize<Q>::value && hasDeserialize<Q>::value, boost::any>::type
247-
deserialize(const std::string& wired) {
248-
std::istringstream iss(wired);
249-
T value;
250-
iss >> value;
251-
return value;
252-
}
253-
254-
/** No serialization available */
255-
template <class Q = T>
256-
static typename std::enable_if<!hasSerialize<Q>::value, std::string>::type serialize(const boost::any& value) {
257-
return dummySerialize(value);
258-
}
259-
template <class Q = T>
260-
static typename std::enable_if<!hasSerialize<Q>::value || !hasDeserialize<Q>::value, boost::any>::type
261-
deserialize(const std::string& wire) {
262-
return dummyDeserialize(wire);
263-
}
264-
};
265-
266276
/** PropertyMap is map of (name, Property) pairs.
267277
*
268278
* Conveniency methods are provided to setup property initialization for several
@@ -303,7 +313,8 @@ class PropertyMap
303313
Property& property(const std::string& name);
304314
const Property& property(const std::string& name) const { return const_cast<PropertyMap*>(this)->property(name); }
305315

306-
void fillMsgs(std::vector<moveit_task_constructor_msgs::Property>& msg) const;
316+
void fillMsgs(std::vector<moveit_task_constructor_msgs::Property>& msgs) const;
317+
void fromMsgs(std::vector<moveit_task_constructor_msgs::Property>& msgs);
307318

308319
using iterator = std::map<std::string, Property>::iterator;
309320
using const_iterator = std::map<std::string, Property>::const_iterator;

‎core/src/properties.cpp‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -224,6 +224,13 @@ void PropertyMap::fillMsgs(std::vector<moveit_task_constructor_msgs::Property>&
224224
}
225225
}
226226

227+
void PropertyMap::fromMsgs(std::vector<moveit_task_constructor_msgs::Property>& msgs) {
228+
for (const auto& p : msgs) {
229+
boost::any value{ Property::deserialize(p.type, p.value) };
230+
declare(p.name, value.type(), p.description, value);
231+
}
232+
}
233+
227234
void PropertyMap::exposeTo(PropertyMap& other, const std::set<std::string>& properties) const {
228235
for (const std::string& name : properties)
229236
exposeTo(other, name, name);

‎core/test/test_properties.cpp‎

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,7 @@
11
#include <moveit/task_constructor/properties.h>
22

3+
#include <geometry_msgs/PoseStamped.h>
4+
35
#include <gtest/gtest.h>
46
#include <initializer_list>
57

@@ -106,6 +108,28 @@ TEST(Property, serialize) {
106108
EXPECT_EQ(props.property("map").serialize(), "");
107109
}
108110

111+
TEST(Property, fillMsg) {
112+
PropertyMap props;
113+
props.declare<geometry_msgs::PoseStamped>("pose", "ROS msg pose");
114+
115+
geometry_msgs::PoseStamped msg;
116+
msg.header.frame_id = "world";
117+
msg.pose.orientation.x = 0.0;
118+
msg.pose.orientation.y = 0.0;
119+
msg.pose.orientation.z = 0.0;
120+
msg.pose.orientation.w = 1.0;
121+
props.set("pose", msg);
122+
123+
std::vector<moveit_task_constructor_msgs::Property> props_msgs;
124+
props.fillMsgs(props_msgs);
125+
126+
PropertyView<geometry_msgs::PoseStamped> p{ props_msgs[0] };
127+
128+
EXPECT_EQ(p.value(), msg);
129+
PropertyMap props_from_msgs;
130+
props_from_msgs.fromMsgs(props_msgs);
131+
}
132+
109133
class InitFromTest : public ::testing::Test
110134
{
111135
protected:

0 commit comments

Comments
 (0)