improve property debugging

This commit is contained in:
Robert Haschke 2018-06-02 17:28:16 +02:00
parent cbb2cd69f7
commit beffe27987
4 changed files with 19 additions and 7 deletions

View File

@ -128,7 +128,8 @@ public:
/// get default value
const boost::any& defaultValue() const { return default_; }
/// serialize current value
std::string serialize() const;
std::string serialize(const boost::any& value) const;
std::string serialize() const { return serialize(value()); }
/// get description text
const std::string& description() const { return description_; }

View File

@ -254,7 +254,14 @@ void ContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model)
// recursively init all children and accumulate errors
InitStageException errors;
for (auto& child : children) {
try { child->init(robot_model); } catch (InitStageException &e) { errors.append(e); }
try { child->init(robot_model); }
catch (const Property::error &e) {
std::ostringstream oss;
oss << e.what();
pimpl()->composePropertyErrorMsg(e.name(), oss);
errors.push_back(*child, oss.str());
}
catch (InitStageException &e) { errors.append(e); }
}
if (errors) throw errors;

View File

@ -39,6 +39,7 @@
#include <moveit/task_constructor/properties.h>
#include <boost/format.hpp>
#include <functional>
#include <ros/console.h>
namespace moveit {
namespace task_constructor {
@ -83,9 +84,9 @@ void Property::reset()
initialized_from_ = -1; // set to max value
}
std::string Property::serialize() const {
if (!serialize_) return "";
return serialize_(value());
std::string Property::serialize(const boost::any& v) const {
if (!serialize_ || v.empty()) return "";
return serialize_(v);
}
bool Property::initsFrom(Property::SourceFlags source) const
@ -142,7 +143,7 @@ void PropertyMap::exposeTo(PropertyMap& other, const std::set<std::string> &prop
void PropertyMap::exposeTo(PropertyMap& other, const std::string& name, const std::string& other_name)
{
const Property& p = property(name);
other.declare(other_name, p.type_index_, p.description_, p.default_, p.serialize_);
other.declare(other_name, p.type_index_, p.description_, p.default_, p.serialize_);
}
void PropertyMap::configureInitFrom(Property::SourceFlags source, const std::set<std::string> &properties)
@ -218,6 +219,8 @@ void PropertyMap::performInitFrom(Property::SourceFlags source, const PropertyMa
} catch (const Property::undefined&) {
}
ROS_DEBUG_STREAM_NAMED("Properties", pair.first << ": " << p.initialized_from_ <<
" -> " << source << ": " << p.serialize(value));
p.setCurrentValue(value);
p.initialized_from_ = source;
}

View File

@ -222,6 +222,7 @@ void Stage::init(const moveit::core::RobotModelConstPtr& robot_model)
impl->properties_.reset();
if (impl->parent()) {
try {
ROS_DEBUG_STREAM_NAMED("Properties", "init '" << name() << "'");
impl->properties_.performInitFrom(PARENT, impl->parent()->properties());
} catch (const Property::error &e) {
std::ostringstream oss;
@ -299,7 +300,7 @@ void StagePrivate::composePropertyErrorMsg(const std::string& property_name, std
os << "declared, but undefined";
if (p.initsFrom(Stage::PARENT)) os << ", inherits from parent";
else if (p.initsFrom(Stage::PARENT)) os << ", initializes from interface";
if (p.initsFrom(Stage::INTERFACE)) os << ", initializes from interface";
} catch (const Property::undeclared &e) {
os << "undeclared";
}