diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index c93870866..64dcde6b0 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -45,7 +45,10 @@ #include #include #include -#include + +#include + +#include "properties/serialize.hpp" namespace moveit { namespace task_constructor { @@ -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 @@ -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()); } + + /// 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 + 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_; } + 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 + 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 @@ -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_; @@ -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::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 @@ -288,6 +239,9 @@ 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& msgs) const; + void fromMsgs(std::vector& msgs); + using iterator = std::map::iterator; using const_iterator = std::map::const_iterator; @@ -295,11 +249,12 @@ 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 = {}); - /// 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); @@ -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 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/include/moveit/task_constructor/properties/serialize.hpp b/core/include/moveit/task_constructor/properties/serialize.hpp new file mode 100644 index 000000000..2a066db57 --- /dev/null +++ b/core/include/moveit/task_constructor/properties/serialize.hpp @@ -0,0 +1,221 @@ +/********************************************************************* + * 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 +#include + +namespace moveit { +namespace task_constructor { + +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 hasInsertionOperator : std::false_type +{}; + +template +struct hasInsertionOperator() << std::declval())> : std::true_type +{}; + +template +struct hasExtractionOperator : std::false_type +{}; + +template +struct hasExtractionOperator() >> std::declval())> : std::true_type +{}; + +template +inline constexpr bool IsStreamDeserializable() { + return hasInsertionOperator::value && 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); +}; + +/** Serialization for std::string **/ +class PropertySerializerString : public PropertySerializerBase +{ +public: + static const char* typeName() { return typeid(std::string).name(); } + + static std::string serialize(const boost::any& value) { return boost::any_cast(value); } + static boost::any deserialize(const std::string& wired) { return wired; } +}; + +/** Serialization for ROS messages **/ +template +class PropertySerializerROS : public PropertySerializerBase +{ +public: + static const char* typeName() { return ros::message_traits::DataType::value(); } + + static std::string serialize(const boost::any& value) { + T message{ boost::any_cast(value) }; + uint32_t serial_size = ros::serialization::serializationLength(message); + std::string buffer(serial_size, '\0'); + ros::serialization::OStream stream(reinterpret_cast(&buffer.front()), serial_size); + ros::serialization::serialize(stream, message); + return buffer; + } + + static boost::any deserialize(const std::string& wired) { + ros::serialization::IStream stream(const_cast(reinterpret_cast(&wired.front())), + wired.size()); + T message; + ros::serialization::deserialize(stream, message); + return message; + } +}; + +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 +{ +public: + static const char* typeName() { return typeid(T).name(); } + + 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(); + } + template + static std::enable_if_t(), boost::any> deserialize(const std::string& wired) { + std::istringstream iss(wired); + T value; + iss >> value; + return value; + } + + /** fallback, if no serialization/deserialization is available **/ + template + 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) { + 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); + } +}; + +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/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/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 257d1455a..523f891ae 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -37,6 +37,7 @@ */ #include + #include #include #include @@ -54,7 +55,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; @@ -85,6 +86,7 @@ class PropertyTypeRegistry return it->second->second; } }; + static PropertyTypeRegistry REGISTRY_SINGLETON; bool PropertyTypeRegistry::insert(const std::type_index& type_index, const std::string& type_name, @@ -120,7 +122,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; } @@ -193,7 +195,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()); @@ -212,6 +214,22 @@ 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(); + 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); + } +} + void PropertyMap::exposeTo(PropertyMap& other, const std::set& properties) const { for (const std::string& name : properties) exposeTo(other, name, name); @@ -239,7 +257,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 @@ -257,7 +275,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; @@ -308,11 +326,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/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/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() { 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 5e2f6faa5..0e3075bf1 100644 --- a/core/test/test_properties.cpp +++ b/core/test/test_properties.cpp @@ -1,5 +1,8 @@ #include +#include +#include + #include #include @@ -17,7 +20,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); @@ -87,12 +89,13 @@ 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(hasInsertionOperator::value); + EXPECT_TRUE(hasInsertionOperator::value); + + EXPECT_TRUE(hasExtractionOperator::value); + EXPECT_TRUE(hasExtractionOperator::value); + EXPECT_TRUE(hasExtractionOperator::value); } TEST(Property, serialize) { @@ -107,6 +110,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: 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 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() {