Merge branches 'fix-interface-deduction' (#84), 'compute-ik-default-timeout' (#80),

'fix-visualization' (#76), and master-improvements (#81)
This commit is contained in:
Robert Haschke 2019-03-11 14:09:03 +01:00
commit 39427f89bf
19 changed files with 410 additions and 201 deletions

View File

@ -64,8 +64,6 @@ public:
void reset() override; void reset() override;
void init(const moveit::core::RobotModelConstPtr& robot_model) override; void init(const moveit::core::RobotModelConstPtr& robot_model) override;
/// validate connectivity of children (after init() was done)
virtual void validateConnectivity() const;
virtual bool canCompute() const = 0; virtual bool canCompute() const = 0;
virtual void compute() = 0; virtual void compute() = 0;
@ -89,9 +87,6 @@ public:
void init(const moveit::core::RobotModelConstPtr& robot_model) override; void init(const moveit::core::RobotModelConstPtr& robot_model) override;
/// validate connectivity of children (after init() was done)
void validateConnectivity() const override;
bool canCompute() const override; bool canCompute() const override;
void compute() override; void compute() override;
@ -132,9 +127,6 @@ public:
void init(const moveit::core::RobotModelConstPtr& robot_model) override; void init(const moveit::core::RobotModelConstPtr& robot_model) override;
/// validate connectivity of children (after init() was done)
void validateConnectivity() const override;
protected: protected:
ParallelContainerBase(ParallelContainerBasePrivate* impl); ParallelContainerBase(ParallelContainerBasePrivate* impl);

View File

@ -97,6 +97,8 @@ public:
return const_cast<const ContainerBasePrivate*>(this)->traverseStages(const_processor, cur_depth, max_depth); return const_cast<const ContainerBasePrivate*>(this)->traverseStages(const_processor, cur_depth, max_depth);
} }
void validateConnectivity() const override;
// forward these methods to the public interface for containers // forward these methods to the public interface for containers
bool canCompute() const override; bool canCompute() const override;
void compute() override; void compute() override;
@ -162,6 +164,8 @@ public:
// called by parent asking for pruning of this' interface // called by parent asking for pruning of this' interface
void pruneInterface(InterfaceFlags accepted) override; void pruneInterface(InterfaceFlags accepted) override;
// validate connectivity of chain
void validateConnectivity() const override;
private: private:
// connect cur stage to its predecessor and successor // connect cur stage to its predecessor and successor
@ -214,6 +218,8 @@ public:
// called by parent asking for pruning of this' interface // called by parent asking for pruning of this' interface
void pruneInterface(InterfaceFlags accepted) override; void pruneInterface(InterfaceFlags accepted) override;
void validateConnectivity() const override;
private: private:
/// callback for new externally received states /// callback for new externally received states
void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated); void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated);

View File

@ -65,7 +65,7 @@ boost::any fromName(const PropertyMap& other, const std::string& other_name);
* *
* Setting the value via setValue() updates both, the current value and the default value. * Setting the value via setValue() updates both, the current value and the default value.
* Using reset() the default value can be restored. * Using reset() the default value can be restored.
* Using setCurrentValue() only updates the current value, allowing for later reset to the original default. * setCurrentValue() and setDefaultValue() only set the specific value.
*/ */
class Property { class Property {
friend class PropertyMap; friend class PropertyMap;
@ -97,6 +97,7 @@ public:
/// set current value and default value /// set current value and default value
void setValue(const boost::any& value); void setValue(const boost::any& value);
void setCurrentValue(const boost::any& value); void setCurrentValue(const boost::any& value);
void setDefaultValue(const boost::any& value);
/// reset to default value (which can be empty) /// reset to default value (which can be empty)
void reset(); void reset();
@ -275,6 +276,7 @@ public:
/// declare given property name as other_name in other PropertyMap /// declare given property name as other_name in other PropertyMap
void exposeTo(PropertyMap& other, const std::string& name, const std::string& other_name) const; void exposeTo(PropertyMap& other, const std::string& name, const std::string& other_name) const;
/// check whether given property is declared
bool hasProperty(const std::string &name) const; bool hasProperty(const std::string &name) const;
/// get the property with given name, throws Property::undeclared for unknown name /// get the property with given name, throws Property::undeclared for unknown name

View File

@ -56,18 +56,27 @@ class StagePrivate {
friend std::ostream& operator<<(std::ostream& os, const StagePrivate& stage); friend std::ostream& operator<<(std::ostream& os, const StagePrivate& stage);
public: public:
// container type used to store children /// container type used to store children
typedef std::list<Stage::pointer> container_type; typedef std::list<Stage::pointer> container_type;
StagePrivate(Stage* me, const std::string& name); StagePrivate(Stage* me, const std::string& name);
virtual ~StagePrivate() = default; virtual ~StagePrivate() = default;
/// actually configured interface of this stage (only valid after init())
InterfaceFlags interfaceFlags() const; InterfaceFlags interfaceFlags() const;
// retrieve description of the interface required by this stage
// if this is unknown (because interface is auto-detected from context), return 0 /** Interface required by this stage
*
* If the interface is unknown (because it is auto-detected from context), return 0 */
virtual InterfaceFlags requiredInterface() const = 0; virtual InterfaceFlags requiredInterface() const = 0;
// prune interface to the given propagation direction
/** Prune interface to comply with the given propagation direction
*
* PropagatingEitherWay uses this in restrictDirection() */
virtual void pruneInterface(InterfaceFlags accepted) {} virtual void pruneInterface(InterfaceFlags accepted) {}
/// validate connectivity of children (after init() was done)
virtual void validateConnectivity() const;
virtual bool canCompute() const = 0; virtual bool canCompute() const = 0;
virtual void compute() = 0; virtual void compute() = 0;
@ -173,6 +182,8 @@ public:
void initInterface(PropagatingEitherWay::Direction dir); void initInterface(PropagatingEitherWay::Direction dir);
// prune interface to the given propagation direction // prune interface to the given propagation direction
void pruneInterface(InterfaceFlags accepted) override; void pruneInterface(InterfaceFlags accepted) override;
// validate that we can propagate in one direction at least
void validateConnectivity() const override;
bool canCompute() const override; bool canCompute() const override;
void compute() override; void compute() override;

View File

@ -90,6 +90,17 @@ bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback &pr
return true; return true;
} }
void ContainerBasePrivate::validateConnectivity() const
{
InitStageException errors;
// recursively validate all children and accumulate errors
for (const auto& child : children()) {
try { child->pimpl()->validateConnectivity(); }
catch (InitStageException &e) { errors.append(e); }
}
if (errors) throw errors;
}
bool ContainerBasePrivate::canCompute() const bool ContainerBasePrivate::canCompute() const
{ {
// call the method of the public interface // call the method of the public interface
@ -240,25 +251,6 @@ void ContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model)
if (errors) throw errors; if (errors) throw errors;
} }
void ContainerBase::validateConnectivity() const
{
InitStageException errors;
for (const auto& child : pimpl()->children()) {
// check that child's required interface is provided
InterfaceFlags required = child->pimpl()->requiredInterface();
InterfaceFlags actual = child->pimpl()->interfaceFlags();
if ((required & actual) != required)
errors.push_back(*child, "required interface is not satisfied");
// recursively validate all children and accumulate errors
ContainerBase* child_container = dynamic_cast<ContainerBase*>(child.get());
if (!child_container) continue; // only containers provide validateConnectivity()
try { child_container->validateConnectivity(); } catch (InitStageException &e) { errors.append(e); }
}
if (errors) throw errors;
}
std::ostream& operator<<(std::ostream& os, const ContainerBase& container) { std::ostream& operator<<(std::ostream& os, const ContainerBase& container) {
ContainerBase::StageCallback processor = [&os](const Stage& stage, int depth) -> bool { ContainerBase::StageCallback processor = [&os](const Stage& stage, int depth) -> bool {
os << std::string(2*depth, ' ') << *stage.pimpl() << std::endl; os << std::string(2*depth, ' ') << *stage.pimpl() << std::endl;
@ -368,8 +360,8 @@ SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::s
: ContainerBasePrivate(me, name) : ContainerBasePrivate(me, name)
{} {}
// a serial container's required interface is derived from the required input interface // a serial container's required interface is derived from the actual input interface
// of the first child and the required output interface of the last child // of the first child and the actual output interface of the last child
InterfaceFlags SerialContainerPrivate::requiredInterface() const InterfaceFlags SerialContainerPrivate::requiredInterface() const
{ {
if (children().empty()) if (children().empty())
@ -400,7 +392,7 @@ bool SerialContainerPrivate::connect(container_type::const_iterator cur)
cur_impl->setPrevEnds((*prev)->pimpl()->ends()); cur_impl->setPrevEnds((*prev)->pimpl()->ends());
// schedule stage with unknown interface for 2nd sweep // schedule stage with unknown interface for 2nd sweep
return required == UNKNOWN; return required == UNKNOWN || required == PROPAGATE_BOTHWAYS;
} }
/* Establishing the interface connections, we face a chicken-egg-problem: /* Establishing the interface connections, we face a chicken-egg-problem:
@ -425,7 +417,8 @@ void SerialContainer::init(const moveit::core::RobotModelConstPtr& robot_model)
impl->starts_.reset(); impl->starts_.reset();
impl->ends_.reset(); impl->ends_.reset();
ContainerBase::init(robot_model); // throws if there are no children // recursively init all children, throws if there are no children
ContainerBase::init(robot_model);
auto start = impl->children().begin(); auto start = impl->children().begin();
auto last = --impl->children().end(); auto last = --impl->children().end();
@ -467,19 +460,31 @@ void SerialContainer::init(const moveit::core::RobotModelConstPtr& robot_model)
void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
if (children().empty()) return; if (children().empty()) return;
// We only need to deal with the special case of the whole sequence to be pruned. // reading is always allowed if current interface flags do so
if (accepted != PROPAGATE_BOTHWAYS && // will interface be restricted at all? accepted |= (interfaceFlags() & InterfaceFlags({READS_START, READS_END}));
children().front()->pimpl()->interfaceFlags() == PROPAGATE_BOTHWAYS) // still undecided?
{ if (accepted == PROPAGATE_BOTHWAYS)
return; // There is nothing to prune
// If whole chain is still undecided, prune all children
if (children().front()->pimpl()->interfaceFlags() == PROPAGATE_BOTHWAYS &&
children().back()->pimpl()->interfaceFlags() == PROPAGATE_BOTHWAYS) {
pruneInterfaces(children().begin(), children().end(), accepted); pruneInterfaces(children().begin(), children().end(), accepted);
} else { // otherwise only prune the first / last child's input / output interface
StagePrivate* child_impl;
child_impl = children().front()->pimpl();
child_impl->pruneInterface((accepted & INPUT_IF_MASK) | (child_impl->interfaceFlags() & OUTPUT_IF_MASK));
child_impl = children().back()->pimpl();
child_impl->pruneInterface((accepted & OUTPUT_IF_MASK) | (child_impl->interfaceFlags() & INPUT_IF_MASK));
}
// reset my pull interfaces, if first/last child don't pull anymore // reset my pull interfaces, if first/last child don't pull anymore
if (!children().front()->pimpl()->starts()) if (!children().front()->pimpl()->starts())
starts_.reset(); starts_.reset();
if (!children().back()->pimpl()->ends()) if (!children().back()->pimpl()->ends())
ends_.reset(); ends_.reset();
}
if (interfaceFlags() == UNKNOWN) if (interfaceFlags() == InterfaceFlags())
throw InitStageException(*me(), "failed to derive propagation direction"); throw InitStageException(*me(), "failed to derive propagation direction");
} }
@ -516,8 +521,8 @@ void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator firs
// nothing to do if: // nothing to do if:
// - accepted == 0: interface still unknown // - accepted == 0: interface still unknown
// - accepted == PROPAGATE_FORWARDS | PROPAGATE_BACKWARDS: no change // - accepted == PROPAGATE_BOTHWAYS: no change
if (accepted != UNKNOWN && accepted != InterfaceFlags({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS})) if (accepted != UNKNOWN && accepted != PROPAGATE_BOTHWAYS)
pruneInterfaces(first, end, accepted); pruneInterfaces(first, end, accepted);
} }
@ -532,11 +537,11 @@ void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator firs
// range should only contain stages with unknown required interface // range should only contain stages with unknown required interface
assert(impl->requiredInterface() == UNKNOWN); assert(impl->requiredInterface() == UNKNOWN);
// remove push interfaces // remove push interfaces if not accepted
if (!(accepted & PROPAGATE_BACKWARDS)) if (!(accepted & WRITES_PREV_END))
impl->setPrevEnds(InterfacePtr()); impl->setPrevEnds(InterfacePtr());
if (!(accepted & PROPAGATE_FORWARDS)) if (!(accepted & WRITES_NEXT_START))
impl->setNextStarts(InterfacePtr()); impl->setNextStarts(InterfacePtr());
} }
// 2nd sweep: recursively prune children // 2nd sweep: recursively prune children
@ -546,41 +551,47 @@ void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator firs
} }
} }
void SerialContainer::validateConnectivity() const void SerialContainerPrivate::validateConnectivity() const
{ {
auto impl = pimpl();
InitStageException errors; InitStageException errors;
boost::format desc("%1% interface of '%2%' (%3%) doesn't match mine (%4%)"); boost::format desc("%1% interface of '%2%' (%3%) doesn't match mine (%4%)");
// check that input / output interface of first / last child matches this' resp. interface // recursively validate children
if (!impl->children().empty()) { try { ContainerBasePrivate::validateConnectivity(); }
const StagePrivate* start = impl->children().front()->pimpl(); catch (InitStageException& e) { errors.append(e); }
if ((start->interfaceFlags() & INPUT_IF_MASK) != (this->pimpl()->interfaceFlags() & INPUT_IF_MASK))
errors.push_back(*this, (desc % "input" % start->name() % flowSymbol(start->interfaceFlags())
% flowSymbol(this->pimpl()->interfaceFlags())).str());
const StagePrivate* last = impl->children().back()->pimpl(); // check that input / output interface of first / last child matches this' resp. interface
if ((last->interfaceFlags() & OUTPUT_IF_MASK) != (this->pimpl()->interfaceFlags() & OUTPUT_IF_MASK)) if (!children().empty()) {
errors.push_back(*this, (desc % "output" % last->name() % flowSymbol(last->interfaceFlags()) const StagePrivate* start = children().front()->pimpl();
% flowSymbol(this->pimpl()->interfaceFlags())).str()); const auto my_flags = this->interfaceFlags();
auto child_flags = start->interfaceFlags() & INPUT_IF_MASK;
if (child_flags != (my_flags & INPUT_IF_MASK))
errors.push_back(*me(), (desc % "input" % start->name() % flowSymbol(child_flags)
% flowSymbol(my_flags & INPUT_IF_MASK)).str());
const StagePrivate* last = children().back()->pimpl();
child_flags = last->interfaceFlags() & OUTPUT_IF_MASK;
if (child_flags != (my_flags & OUTPUT_IF_MASK))
errors.push_back(*me(), (desc % "output" % last->name() % flowSymbol(child_flags)
% flowSymbol(my_flags & OUTPUT_IF_MASK)).str());
} }
// validate connectivity of children amongst each other // validate connectivity of children amongst each other
// ContainerBase::validateConnectivity() ensures that required push interfaces are present, // ContainerBasePrivate::validateConnectivity() ensures that required push interfaces are present,
// that is, neighbouring stages have a corresponding pull interface. // that is, neighbouring stages have a corresponding pull interface.
// Here, it remains to check that - if a child requires a pull interface - it's indeed feeded. // Here, it remains to check that - if a child has a pull interface - it's indeed feeded.
for (auto cur = impl->children().begin(), end = impl->children().end(); cur != end; ++cur) { for (auto cur = children().begin(), end = children().end(); cur != end; ++cur) {
const StagePrivate* const cur_impl = **cur; const StagePrivate* const cur_impl = **cur;
InterfaceFlags required = cur_impl->requiredInterface(); InterfaceFlags required = cur_impl->interfaceFlags();
// get iterators to prev / next stage in sequence // get iterators to prev / next stage in sequence
auto prev = cur; --prev; auto prev = cur; --prev;
auto next = cur; ++next; auto next = cur; ++next;
// start pull interface fed? // start pull interface fed?
if (cur != impl->children().begin() && // first child has not a previous one if (cur != children().begin() && // first child has not a previous one
(required & READS_START) && !(*prev)->pimpl()->nextStarts()) (required & READS_START) && !(*prev)->pimpl()->nextStarts())
errors.push_back(**cur, "end interface is not fed"); errors.push_back(**cur, "start interface is not fed");
// end pull interface fed? // end pull interface fed?
if (next != end && // last child has not a next one if (next != end && // last child has not a next one
@ -588,9 +599,6 @@ void SerialContainer::validateConnectivity() const
errors.push_back(**cur, "end interface is not fed"); errors.push_back(**cur, "end interface is not fed");
} }
// recursively validate children
try { ContainerBase::validateConnectivity(); } catch (InitStageException& e) { errors.append(e); }
if (errors) throw errors; if (errors) throw errors;
} }
@ -696,6 +704,31 @@ void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted)
} }
} }
void ParallelContainerBasePrivate::validateConnectivity() const
{
InitStageException errors;
InterfaceFlags my_interface = interfaceFlags();
InterfaceFlags children_interfaces;
boost::format desc("interface of child '%1%' (%2%) doesn't match mine (%3%)");
// check that input / output interfaces of all children are handled by my interface
for (const auto& child : children()) {
InterfaceFlags current = child->pimpl()->interfaceFlags();
children_interfaces |= current; // compute union of all children interfaces
if ((current & my_interface) != current)
errors.push_back(*me(), (desc % child->name() % flowSymbol(current) % flowSymbol(my_interface)).str());
}
// check that there is a child matching the expected push interfaces
if ((my_interface & GENERATE) != (children_interfaces & GENERATE))
errors.push_back(*me(), "no child provides expected push interface");
// recursively validate children
try { ContainerBasePrivate::validateConnectivity(); }
catch (InitStageException& e) { errors.append(e); }
if (errors) throw errors;
}
void ParallelContainerBasePrivate::onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated) { void ParallelContainerBasePrivate::onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated) {
for (const Stage::pointer& stage : children()) for (const Stage::pointer& stage : children())
copyState(external, stage->pimpl()->pullInterface(dir), updated); copyState(external, stage->pimpl()->pullInterface(dir), updated);
@ -739,31 +772,6 @@ void ParallelContainerBase::init(const moveit::core::RobotModelConstPtr& robot_m
} }
} }
void ParallelContainerBase::validateConnectivity() const
{
InitStageException errors;
auto impl = pimpl();
InterfaceFlags my_interface = impl->interfaceFlags();
InterfaceFlags children_interfaces;
boost::format desc("interface of child '%1%' (%2%) doesn't match mine (%3%)");
// check that input / output interfaces of all children are handled by my interface
for (const auto& child : pimpl()->children()) {
InterfaceFlags current = child->pimpl()->interfaceFlags();
children_interfaces |= current; // compute union of all children interfaces
if ((current & my_interface) != current)
errors.push_back(*this, (desc % child->name() % flowSymbol(current) % flowSymbol(my_interface)).str());
}
// check that there is a child matching the expected push interfaces
if ((my_interface & GENERATE) != (children_interfaces & GENERATE))
errors.push_back(*this, "no child provides expected push interface");
// recursively validate children
try { ContainerBase::validateConnectivity(); } catch (InitStageException& e) { errors.append(e); }
if (errors) throw errors;
}
void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment) void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment)
{ {
auto impl = pimpl(); auto impl = pimpl();

View File

@ -142,6 +142,14 @@ void Property::setCurrentValue(const boost::any &value)
initialized_from_ = 1; // manually initialized TODO: use enums initialized_from_ = 1; // manually initialized TODO: use enums
} }
void Property::setDefaultValue(const boost::any& value)
{
if (!value.empty() && type_info_ != typeid(boost::any) && value.type() != type_info_)
throw Property::type_error(value.type().name(), type_info_.name());
default_ = value;
}
void Property::reset() void Property::reset()
{ {
if (initialized_from_ == 0) // TODO: use enum if (initialized_from_ == 0) // TODO: use enum

View File

@ -84,6 +84,15 @@ InterfaceFlags StagePrivate::interfaceFlags() const
return f; return f;
} }
void StagePrivate::validateConnectivity() const
{
// check that the required interface is provided
InterfaceFlags required = requiredInterface();
InterfaceFlags actual = interfaceFlags();
if ((required & actual) != required)
throw InitStageException(*me(), "required interface is not satisfied");
}
bool StagePrivate::storeSolution(const SolutionBasePtr& solution) bool StagePrivate::storeSolution(const SolutionBasePtr& solution)
{ {
solution->setCreator(this); solution->setCreator(this);
@ -350,13 +359,21 @@ const char* direction(const StagePrivate& stage) {
return "<-"; return "<-";
} }
const char* flowSymbol(moveit::task_constructor::InterfaceFlags f) { const char* flowSymbol(InterfaceFlags f) {
if (f == InterfaceFlags(CONNECT)) return ""; if (f == InterfaceFlags())
if (f == InterfaceFlags(PROPAGATE_FORWARDS)) return ""; return "?"; // unknown interface
if (f == InterfaceFlags(PROPAGATE_BACKWARDS)) return "";
if (f == PROPAGATE_BOTHWAYS) return ""; // f should have either INPUT or OUTPUT flags set (not both)
if (f == InterfaceFlags(GENERATE)) return ""; assert(static_cast<bool>(f & INPUT_IF_MASK) ^ static_cast<bool>(f & OUTPUT_IF_MASK));
return "?";
if (f & INPUT_IF_MASK) {
if (f == READS_START) return "->";
if (f == WRITES_PREV_END) return "<-";
} else if (f & OUTPUT_IF_MASK) {
if (f == READS_END) return "<-";
if (f == WRITES_NEXT_START) return "->";
}
return "<->";
} }
std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) { std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) {
@ -413,13 +430,34 @@ void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction
void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) { void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) {
int dir = 0; int dir = 0;
if (accepted & PROPAGATE_FORWARDS) if ((accepted & PROPAGATE_FORWARDS) == PROPAGATE_FORWARDS)
dir |= PropagatingEitherWay::FORWARD; dir |= PropagatingEitherWay::FORWARD;
if (accepted & PROPAGATE_BACKWARDS) if ((accepted & PROPAGATE_BACKWARDS) == PROPAGATE_BACKWARDS)
dir |= PropagatingEitherWay::BACKWARD; dir |= PropagatingEitherWay::BACKWARD;
initInterface(PropagatingEitherWay::Direction(dir)); initInterface(PropagatingEitherWay::Direction(dir));
} }
void PropagatingEitherWayPrivate::validateConnectivity() const
{
InterfaceFlags required = requiredInterface();
InterfaceFlags actual = interfaceFlags();
if (actual == UNKNOWN)
throw InitStageException(*me(), "not connected in any direction");
InitStageException errors;
if ((actual & READS_START) && !(actual & WRITES_NEXT_START))
errors.push_back(*me(), "Cannot push forwards");
if ((actual & READS_END) && !(actual & WRITES_PREV_END))
errors.push_back(*me(), "Cannot push backwards");
if (required_interface_dirs_ == PropagatingEitherWay::BOTHWAY &&
(required & actual) != required)
ROS_WARN_STREAM_NAMED("PropagatingEitherWay", "Cannot propagate " <<
(actual & PROPAGATE_FORWARDS ? "backwards" : "forwards"));
if (errors) throw errors;
}
InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const
{ {
InterfaceFlags f; InterfaceFlags f;
@ -427,7 +465,7 @@ InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const
f |= PROPAGATE_FORWARDS; f |= PROPAGATE_FORWARDS;
if (required_interface_dirs_ & PropagatingEitherWay::BACKWARD) if (required_interface_dirs_ & PropagatingEitherWay::BACKWARD)
f |= PROPAGATE_BACKWARDS; f |= PROPAGATE_BACKWARDS;
// if required_interface_dirs_ == ANYWAY, we don't require an interface // if required_interface_dirs_ == AUTO, we don't require an interface
// but the parent container auto-derives the propagation direction // but the parent container auto-derives the propagation direction
return f; return f;
} }
@ -512,7 +550,7 @@ void PropagatingEitherWay::init(const moveit::core::RobotModelConstPtr& robot_mo
// pretend that we offer both interface directions during init(). // pretend that we offer both interface directions during init().
// This is needed due to a chicken-egg-problem: interface auto-detection requires // This is needed due to a chicken-egg-problem: interface auto-detection requires
// the context (external pushing interfaces prevEnds, nextStarts) to be set, // the context (external pushing interfaces prevEnds, nextStarts) to be set,
// while the are ony set if we detected the correct interface... // while they are ony set if we detected the correct interface...
if (impl->required_interface_dirs_ == AUTO) if (impl->required_interface_dirs_ == AUTO)
impl->initInterface(BOTHWAY); impl->initInterface(BOTHWAY);
// otherwise the interface is already fixed and well-defined // otherwise the interface is already fixed and well-defined

View File

@ -54,7 +54,6 @@ namespace moveit { namespace task_constructor { namespace stages {
ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child) ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child)
: WrapperBase(name, std::move(child)) : WrapperBase(name, std::move(child))
{ {
setTimeout(1.0);
auto& p = properties(); auto& p = properties();
p.declare<std::string>("eef", "name of end-effector group"); p.declare<std::string>("eef", "name of end-effector group");
p.declare<std::string>("group", "name of active group (derived from eef if not provided)"); p.declare<std::string>("group", "name of active group (derived from eef if not provided)");
@ -254,6 +253,7 @@ void ComputeIK::compute()
ROS_WARN_STREAM_NAMED("ComputeIK", "Neither eef nor group are well defined"); ROS_WARN_STREAM_NAMED("ComputeIK", "Neither eef nor group are well defined");
return; return;
} }
properties().property("timeout").setDefaultValue(jmg->getDefaultIKTimeout());
// extract target_pose // extract target_pose
geometry_msgs::PoseStamped target_pose_msg = props.get<geometry_msgs::PoseStamped>("target_pose"); geometry_msgs::PoseStamped target_pose_msg = props.get<geometry_msgs::PoseStamped>("target_pose");

View File

@ -47,8 +47,8 @@ MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterf
: PropagatingEitherWay(name) : PropagatingEitherWay(name)
, planner_(planner) , planner_(planner)
{ {
setTimeout(10.0);
auto& p = properties(); auto& p = properties();
p.property("timeout").setDefaultValue(1.0);
p.declare<std::string>("group", "name of planning group"); p.declare<std::string>("group", "name of planning group");
p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to be moved in Cartesian direction"); p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to be moved in Cartesian direction");

View File

@ -48,8 +48,8 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan
: PropagatingEitherWay(name) : PropagatingEitherWay(name)
, planner_(planner) , planner_(planner)
{ {
setTimeout(10.0);
auto& p = properties(); auto& p = properties();
p.property("timeout").setDefaultValue(1.0);
p.declare<std::string>("group", "name of planning group"); p.declare<std::string>("group", "name of planning group");
p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to be moved towards goal pose"); p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to be moved towards goal pose");
p.declare<boost::any>("goal", "goal specification"); p.declare<boost::any>("goal", "goal specification");

View File

@ -237,7 +237,7 @@ void Task::init()
// task expects its wrapped child to push to both ends, this triggers interface resolution // task expects its wrapped child to push to both ends, this triggers interface resolution
stages()->pimpl()->pruneInterface(InterfaceFlags({GENERATE})); stages()->pimpl()->pruneInterface(InterfaceFlags({GENERATE}));
// and *finally* validate connectivity // and *finally* validate connectivity
stages()->validateConnectivity(); stages()->pimpl()->validateConnectivity();
// provide introspection instance to all stages // provide introspection instance to all stages
impl->setIntrospection(introspection_.get()); impl->setIntrospection(introspection_.get());

View File

@ -134,7 +134,7 @@ protected:
if (start) accepted |= WRITES_PREV_END; if (start) accepted |= WRITES_PREV_END;
if (end) accepted |= WRITES_NEXT_START; if (end) accepted |= WRITES_NEXT_START;
container.pimpl()->pruneInterface(accepted); container.pimpl()->pruneInterface(accepted);
container.validateConnectivity(); container.pimpl()->validateConnectivity();
if (!expect_failure) return; // as expected if (!expect_failure) return; // as expected
ADD_FAILURE() << "init() didn't recognize a failure condition as expected\n" << container; ADD_FAILURE() << "init() didn't recognize a failure condition as expected\n" << container;
} catch (const InitStageException &e) { } catch (const InitStageException &e) {
@ -235,7 +235,6 @@ TEST_F(SerialTest, init_empty) {
TEST_F(SerialTest, init_connecting) { TEST_F(SerialTest, init_connecting) {
EXPECT_INIT_SUCCESS(false, false, CONN); EXPECT_INIT_SUCCESS(false, false, CONN);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT)); EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT));
EXPECT_EQ(container.pimpl()->interfaceFlags(), container.pimpl()->children().front()->pimpl()->interfaceFlags());
EXPECT_INIT_FAILURE(true, true, CONN); EXPECT_INIT_FAILURE(true, true, CONN);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags({CONNECT, GENERATE})); EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags({CONNECT, GENERATE}));
@ -246,7 +245,6 @@ TEST_F(SerialTest, init_connecting) {
TEST_F(SerialTest, init_generator) { TEST_F(SerialTest, init_generator) {
EXPECT_INIT_SUCCESS(true, true, GEN); EXPECT_INIT_SUCCESS(true, true, GEN);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
EXPECT_EQ(container.pimpl()->interfaceFlags(), container.pimpl()->children().front()->pimpl()->interfaceFlags());
EXPECT_INIT_FAILURE(false, false, GEN); // generator wants to push, but container cannot propagate pushes EXPECT_INIT_FAILURE(false, false, GEN); // generator wants to push, but container cannot propagate pushes
@ -259,15 +257,48 @@ TEST_F(SerialTest, init_forward) {
EXPECT_INIT_FAILURE(true, true, FW); EXPECT_INIT_FAILURE(true, true, FW);
EXPECT_INIT_FAILURE(true, false, FW); EXPECT_INIT_FAILURE(true, false, FW);
// wrong inner connectivity
EXPECT_INIT_FAILURE(false, false, FW, BW);
EXPECT_INIT_FAILURE(true, true, BW, FW);
// these should be fine // these should be fine
EXPECT_INIT_SUCCESS(false, true, FW); EXPECT_INIT_SUCCESS(false, true, FW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), container.pimpl()->children().front()->pimpl()->interfaceFlags()); EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_INIT_SUCCESS(false, true, FW, FW); EXPECT_INIT_SUCCESS(false, true, FW, FW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), container.pimpl()->children().front()->pimpl()->interfaceFlags()); EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_INIT_SUCCESS(true, true, GEN, FW); EXPECT_INIT_SUCCESS(true, true, GEN, FW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
EXPECT_INIT_SUCCESS(true, true, BW, GEN, FW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
EXPECT_INIT_SUCCESS(false, true, ANY, FW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_INIT_SUCCESS(false, true, FW, ANY);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_INIT_SUCCESS(true, false, ANY, BW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_INIT_SUCCESS(true, false, BW, ANY);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_INIT_SUCCESS(false, true, BOTH, FW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_INIT_SUCCESS(false, true, FW, BOTH);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_INIT_SUCCESS(true, false, BOTH, BW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_INIT_SUCCESS(true, false, BW, BOTH);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
} }
TEST_F(SerialTest, init_backward) { TEST_F(SerialTest, init_backward) {
@ -278,10 +309,10 @@ TEST_F(SerialTest, init_backward) {
// these should be fine // these should be fine
EXPECT_INIT_SUCCESS(true, false, BW); EXPECT_INIT_SUCCESS(true, false, BW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), container.pimpl()->children().front()->pimpl()->interfaceFlags()); EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_INIT_SUCCESS(true, false, BW, BW); EXPECT_INIT_SUCCESS(true, false, BW, BW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), container.pimpl()->children().front()->pimpl()->interfaceFlags()); EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_INIT_SUCCESS(true, true, BW, GEN); EXPECT_INIT_SUCCESS(true, true, BW, GEN);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
@ -320,6 +351,11 @@ TEST_F(SerialTest, interface_detection) {
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT)); EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT));
// derive propagation direction from outer interface // derive propagation direction from outer interface
EXPECT_INIT_SUCCESS(false, true, ANY); // should be pruned to FW
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_INIT_SUCCESS(true, false, ANY); // should be pruned to BW
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_INIT_SUCCESS(false, true, ANY, ANY); // -> -> EXPECT_INIT_SUCCESS(false, true, ANY, ANY); // -> ->
it = container.pimpl()->children().begin(); it = container.pimpl()->children().begin();
EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS)); EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
@ -346,10 +382,36 @@ TEST_F(SerialTest, interface_detection) {
} }
TEST_F(SerialTest, nested_interface_detection) { TEST_F(SerialTest, nested_interface_detection) {
auto inner = std::make_unique<SerialContainer>("inner"); SerialContainer* inner;
// direction imposed from outer generator
container.clear();
inner = new SerialContainer("inner serial");
append(*inner, {ANY, ANY});
append(container, {GEN, ANY, ANY});
container.insert(Stage::pointer(inner), -2);
{SCOPED_TRACE("GEN - ANY - inner - ANY"); validateInit(true, true, {}, false);}
// direction imposed from inner generator
container.clear();
inner = new SerialContainer("inner serial");
append(*inner, {ANY, GEN, ANY});
append(container, {ANY, ANY});
container.insert(Stage::pointer(inner), -2);
{SCOPED_TRACE("ANY - inner - ANY"); validateInit(true, true, {}, false);}
container.clear();
inner = new SerialContainer("inner serial");
append(*inner, {GEN, ANY}); append(*inner, {GEN, ANY});
container.insert(std::move(inner)); container.insert(Stage::pointer(inner));
validateInit(true, true, {ANY}, false); {SCOPED_TRACE("inner - ANY"); validateInit(true, true, {ANY}, false);}
// outer and inner generators conflict with each other
container.clear();
inner = new SerialContainer("inner serial");
append(*inner, {ANY, GEN, ANY});
append(container, {GEN, ANY, ANY});
container.insert(Stage::pointer(inner), -2);
{SCOPED_TRACE("GEN - ANY - inner - ANY"); validateInit(true, true, {}, true);}
} }

View File

@ -104,16 +104,18 @@ std_msgs::ColorRGBA &setColor(std_msgs::ColorRGBA &color, Color color_id, double
return color; return color;
} }
// interpolate between start and end with fraction in range from 0..1
double interpolate(double start, double end, double fraction) { double interpolate(double start, double end, double fraction) {
return start * fraction + end * (1.0 - fraction); return start * (1.0 - fraction) + end * fraction;
} }
std_msgs::ColorRGBA& interpolate(std_msgs::ColorRGBA& color, const std_msgs::ColorRGBA& other, double fraction) { std_msgs::ColorRGBA& interpolate(std_msgs::ColorRGBA& color, const std_msgs::ColorRGBA& other, double fraction) {
if (fraction < 0.0) fraction = 0.0; if (fraction < 0.0) fraction = 0.0;
if (fraction > 1.0) fraction = 1.0; if (fraction > 1.0) fraction = 1.0;
color.r += interpolate(other.r-color.r, other.r, fraction); color.r = interpolate(color.r, other.r, fraction);
color.g += interpolate(other.g-color.g, other.g, fraction); color.g = interpolate(color.g, other.g, fraction);
color.b += interpolate(other.b-color.b, other.b, fraction); color.b = interpolate(color.b, other.b, fraction);
color.a = interpolate(color.a, other.a, fraction);
return color; return color;
} }

View File

@ -3,7 +3,7 @@ set(MOVEIT_LIB_NAME motion_planning_tasks_rviz_plugin)
qt_wrap_ui(UIC_FILES qt_wrap_ui(UIC_FILES
task_panel.ui task_panel.ui
task_view.ui task_view.ui
task_settings.ui global_settings.ui
) )
add_library(${MOVEIT_LIB_NAME} add_library(${MOVEIT_LIB_NAME}

View File

@ -1,7 +1,7 @@
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0"> <ui version="4.0">
<class>moveit_rviz_plugin::TaskSettings</class> <class>moveit_rviz_plugin::GlobalSettingsWidget</class>
<widget class="QWidget" name="moveit_rviz_plugin::TaskSettings"> <widget class="QWidget" name="moveit_rviz_plugin::GlobalSettingsWidget">
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>0</x> <x>0</x>
@ -27,7 +27,7 @@
<number>0</number> <number>0</number>
</property> </property>
<item> <item>
<layout class="QVBoxLayout" name="tab_settings_layout"> <layout class="QVBoxLayout" name="layout">
<property name="spacing"> <property name="spacing">
<number>0</number> <number>0</number>
</property> </property>
@ -44,14 +44,14 @@
<number>0</number> <number>0</number>
</property> </property>
<item> <item>
<widget class="QLabel" name="settings_view_label"> <widget class="QLabel" name="label">
<property name="text"> <property name="text">
<string>Settings</string> <string>Global Settings</string>
</property> </property>
</widget> </widget>
</item> </item>
<item> <item>
<widget class="rviz::PropertyTreeWidget" name="settings_view"/> <widget class="rviz::PropertyTreeWidget" name="view"/>
</item> </item>
</layout> </layout>
</item> </item>

View File

@ -48,6 +48,7 @@
#include <moveit/task_constructor/stage.h> #include <moveit/task_constructor/stage.h>
#include <rviz/properties/property.h> #include <rviz/properties/property.h>
#include <rviz/properties/enum_property.h>
#include <rviz/display_group.h> #include <rviz/display_group.h>
#include <rviz/visualization_manager.h> #include <rviz/visualization_manager.h>
#include <rviz/window_manager_interface.h> #include <rviz/window_manager_interface.h>
@ -87,15 +88,20 @@ TaskPanel::TaskPanel(QWidget* parent)
{ {
Q_D(TaskPanel); Q_D(TaskPanel);
d->tasks_widget = new TaskView(this); // sync checked tool button with displayed widget
d->settings_widget = new TaskSettings(this); connect(d->tool_buttons_group, static_cast<void(QButtonGroup::*)(int)>(&QButtonGroup::buttonClicked),
layout()->addWidget(d->tasks_widget); d->stackedWidget, [d](int index) { d->stackedWidget->setCurrentIndex(index); });
layout()->addWidget(d->settings_widget); connect(d->stackedWidget, &QStackedWidget::currentChanged, d->tool_buttons_group,
connect(d->tasks_widget, SIGNAL(configChanged()), this, SIGNAL(configChanged())); [d](int index) { d->tool_buttons_group->button(index)->setChecked(true); });
// create sub widgets with corresponding tool buttons
addSubPanel(new TaskView(this, d->property_root), "Tasks View", QIcon(":/icons/tasks.png"));
d->stackedWidget->setCurrentIndex(0); // Tasks View is show by default
// settings widget should come last
addSubPanel(new GlobalSettingsWidget(this, d->property_root), "Global Settings", QIcon(":/icons/settings.png"));
connect(d->button_show_stage_dock_widget, SIGNAL(clicked()), this, SLOT(showStageDockWidget())); connect(d->button_show_stage_dock_widget, SIGNAL(clicked()), this, SLOT(showStageDockWidget()));
connect(d->button_show_settings, SIGNAL(toggled(bool)), d->settings_widget, SLOT(setVisible(bool)));
d->settings_widget->setVisible(d->button_show_settings->isChecked());
// if still undefined, this becomes the global instance // if still undefined, this becomes the global instance
if (singleton_.isNull()) if (singleton_.isNull())
@ -107,6 +113,24 @@ TaskPanel::~TaskPanel()
delete d_ptr; delete d_ptr;
} }
void TaskPanel::addSubPanel(SubPanel *w, const QString& title, const QIcon& icon)
{
Q_D(TaskPanel);
auto button = new QToolButton(w);
button->setToolTip(title);
button->setIcon(icon);
button->setCheckable(true);
int index = d->stackedWidget->count();
d->tool_buttons_layout->insertWidget(index, button);
d->tool_buttons_group->addButton(button, index);
d->stackedWidget->addWidget(w);
w->setWindowTitle(title);
connect(w, SIGNAL(configChanged()), this, SIGNAL(configChanged()));
}
void TaskPanel::incDisplayCount(rviz::WindowManagerInterface* window_manager) void TaskPanel::incDisplayCount(rviz::WindowManagerInterface* window_manager)
{ {
++display_count_; ++display_count_;
@ -115,8 +139,10 @@ void TaskPanel::incDisplayCount(rviz::WindowManagerInterface* window_manager)
if (singleton_ || !vis_frame) if (singleton_ || !vis_frame)
return; // already define, nothing to do return; // already define, nothing to do
window_manager->addPane("Motion Planning Tasks", new TaskPanel()); QDockWidget* dock = vis_frame->addPanelByName("Motion Planning Tasks",
singleton_->initialize(vis_frame->getManager()); "moveit_task_constructor/Motion Planning Tasks",
Qt::LeftDockWidgetArea, true /* floating */);
assert(dock->widget() == singleton_);
} }
void TaskPanel::decDisplayCount() void TaskPanel::decDisplayCount()
@ -130,7 +156,11 @@ TaskPanelPrivate::TaskPanelPrivate(TaskPanel *q_ptr)
: q_ptr(q_ptr) : q_ptr(q_ptr)
{ {
setupUi(q_ptr); setupUi(q_ptr);
tool_buttons_group = new QButtonGroup(q_ptr);
tool_buttons_group->setExclusive(true);
button_show_stage_dock_widget->setEnabled(bool(getStageFactory())); button_show_stage_dock_widget->setEnabled(bool(getStageFactory()));
button_show_stage_dock_widget->setToolTip(QStringLiteral("Show available stages"));
property_root = new rviz::Property("Global Settings");
} }
void TaskPanel::onInitialize() void TaskPanel::onInitialize()
@ -141,13 +171,19 @@ void TaskPanel::onInitialize()
void TaskPanel::save(rviz::Config config) const void TaskPanel::save(rviz::Config config) const
{ {
rviz::Panel::save(config); rviz::Panel::save(config);
d_ptr->tasks_widget->save(config.mapMakeChild("tasks_view")); for (int i=0; i < d_ptr->stackedWidget->count(); ++i) {
SubPanel* w = static_cast<SubPanel*>(d_ptr->stackedWidget->widget(i));
w->save(config.mapMakeChild(w->windowTitle()));
}
} }
void TaskPanel::load(const rviz::Config& config) void TaskPanel::load(const rviz::Config& config)
{ {
rviz::Panel::load(config); rviz::Panel::load(config);
d_ptr->tasks_widget->load(config.mapGetChild("tasks_view")); for (int i=0; i < d_ptr->stackedWidget->count(); ++i) {
SubPanel* w = static_cast<SubPanel*>(d_ptr->stackedWidget->widget(i));
w->load(config.mapGetChild(w->windowTitle()));
}
} }
void TaskPanel::showStageDockWidget() void TaskPanel::showStageDockWidget()
@ -183,16 +219,21 @@ TaskViewPrivate::TaskViewPrivate(TaskView *q_ptr)
tasks_view->setModel(meta_model); tasks_view->setModel(meta_model);
// auto-expand newly-inserted top-level items // auto-expand newly-inserted top-level items
QObject::connect(meta_model, &QAbstractItemModel::rowsInserted, [this](const QModelIndex &parent, int first, int last){ QObject::connect(meta_model, &QAbstractItemModel::rowsInserted, [this](const QModelIndex &parent, int first, int last){
if (parent.isValid() && !parent.parent().isValid()) { if (parent.isValid() && !parent.parent().isValid()) { // top-level task items inserted
int expand = this->q_ptr->initial_task_expand->getOptionInt();
for (int row = first; row <= last; ++row) { for (int row = first; row <= last; ++row) {
QModelIndex child = parent.child(row, 0); QModelIndex child = parent.child(row, 0);
// expand inserted items if (expand != TaskView::EXPAND_NONE) {
// recursively expand all inserted items
setExpanded(tasks_view, child, true); setExpanded(tasks_view, child, true);
}
if (expand == TaskView::EXPAND_TOP) {
// collapse up to first level // collapse up to first level
setExpanded(tasks_view, child, false, 1); setExpanded(tasks_view, child, false, 1);
// expand inserted item // expand inserted item
setExpanded(tasks_view, child, true, 0); setExpanded(tasks_view, child, true, 0);
} }
}
tasks_view->setExpanded(parent, true); // expand parent group item tasks_view->setExpanded(parent, true); // expand parent group item
} }
}); });
@ -233,8 +274,8 @@ void TaskViewPrivate::lock(TaskDisplay* display)
locked_display_ = display; locked_display_ = display;
} }
TaskView::TaskView(QWidget *parent) TaskView::TaskView(moveit_rviz_plugin::TaskPanel *parent, rviz::Property *root)
: QWidget(parent), d_ptr(new TaskViewPrivate(this)) : SubPanel(parent), d_ptr(new TaskViewPrivate(this))
{ {
Q_D(TaskView); Q_D(TaskView);
@ -253,6 +294,14 @@ TaskView::TaskView(QWidget *parent)
connect(d_ptr->tasks_view->header(), SIGNAL(sectionResized(int,int,int)), this, SIGNAL(configChanged())); connect(d_ptr->tasks_view->header(), SIGNAL(sectionResized(int,int,int)), this, SIGNAL(configChanged()));
connect(d_ptr->solutions_view->header(), SIGNAL(sectionResized(int,int,int)), this, SIGNAL(configChanged())); connect(d_ptr->solutions_view->header(), SIGNAL(sectionResized(int,int,int)), this, SIGNAL(configChanged()));
connect(d_ptr->solutions_view->header(), SIGNAL(sortIndicatorChanged(int,Qt::SortOrder)), this, SIGNAL(configChanged())); connect(d_ptr->solutions_view->header(), SIGNAL(sortIndicatorChanged(int,Qt::SortOrder)), this, SIGNAL(configChanged()));
// configuration settings
auto configs = new rviz::Property("Task View Settings", QVariant(), QString(), root);
initial_task_expand = new rviz::EnumProperty("Task Expansion", "All Expanded",
"Configure how to initially expand new tasks", configs);
initial_task_expand->addOption("Top-level Expanded", EXPAND_TOP);
initial_task_expand->addOption("All Expanded", EXPAND_ALL);
initial_task_expand->addOption("All Closed", EXPAND_NONE);
} }
TaskView::~TaskView() TaskView::~TaskView()
@ -424,23 +473,37 @@ void TaskView::onSolutionSelectionChanged(const QItemSelection &selected, const
} }
TaskSettingsPrivate::TaskSettingsPrivate(TaskSettings *q_ptr) GlobalSettingsWidgetPrivate::GlobalSettingsWidgetPrivate(GlobalSettingsWidget *q_ptr, rviz::Property *root)
: q_ptr(q_ptr) : q_ptr(q_ptr)
{ {
setupUi(q_ptr); setupUi(q_ptr);
properties = new rviz::PropertyTreeModel(root, q_ptr);
view->setModel(properties);
} }
TaskSettings::TaskSettings(QWidget *parent) GlobalSettingsWidget::GlobalSettingsWidget(moveit_rviz_plugin::TaskPanel *parent, rviz::Property *root)
: QWidget(parent), d_ptr(new TaskSettingsPrivate(this)) : SubPanel(parent), d_ptr(new GlobalSettingsWidgetPrivate(this, root))
{ {
Q_D(TaskSettings); Q_D(GlobalSettingsWidget);
connect(d->properties, &rviz::PropertyTreeModel::configChanged,
this, &GlobalSettingsWidget::configChanged);
} }
TaskSettings::~TaskSettings() GlobalSettingsWidget::~GlobalSettingsWidget()
{ {
delete d_ptr; delete d_ptr;
} }
void GlobalSettingsWidget::save(rviz::Config config)
{
d_ptr->properties->getRoot()->save(config);
}
void GlobalSettingsWidget::load(const rviz::Config &config)
{
d_ptr->properties->getRoot()->load(config);
}
} }
#include "moc_task_panel.cpp" #include "moc_task_panel.cpp"

View File

@ -42,9 +42,12 @@
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <QModelIndex> #include <QModelIndex>
class QItemSelection; class QItemSelection;
class QIcon;
namespace rviz { namespace rviz {
class WindowManagerInterface; class WindowManagerInterface;
class Property;
class EnumProperty;
} }
namespace moveit_rviz_plugin { namespace moveit_rviz_plugin {
@ -53,12 +56,22 @@ class TaskSolutionVisualization;
MOVEIT_CLASS_FORWARD(TaskListModel) MOVEIT_CLASS_FORWARD(TaskListModel)
MOVEIT_CLASS_FORWARD(TaskPanel) MOVEIT_CLASS_FORWARD(TaskPanel)
/** The TaskPanel displays information about manipulation tasks in the system.
* /// Base class for all sub panels within the Task Panel
* Subscribing to task_monitoring and task_solution topics, it collects information class SubPanel : public QWidget {
* about running tasks and their solutions and allows to inspect both, Q_OBJECT
* successful solutions and failed solution attempts. public:
*/ SubPanel(QWidget* parent = nullptr) : QWidget(parent) {}
virtual void save(rviz::Config config) {}
virtual void load(const rviz::Config& config) {}
Q_SIGNALS:
void configChanged();
};
/** The TaskPanel is the central panel of this plugin, collecting various sub panels. */
class TaskPanelPrivate; class TaskPanelPrivate;
class TaskPanel: public rviz::Panel class TaskPanel: public rviz::Panel
{ {
@ -70,6 +83,9 @@ public:
TaskPanel(QWidget* parent = 0); TaskPanel(QWidget* parent = 0);
~TaskPanel(); ~TaskPanel();
/// add a new sub panel widget
void addSubPanel(SubPanel* w, const QString &title, const QIcon &icon);
/** Increment/decrement use count of singleton TaskPanel instance. /** Increment/decrement use count of singleton TaskPanel instance.
* *
* If not yet done, an instance is created. If use count drops to zero, * If not yet done, an instance is created. If use count drops to zero,
@ -88,21 +104,29 @@ protected Q_SLOTS:
class MetaTaskListModel; class MetaTaskListModel;
/** TaskView displays all known tasks.
*
* Subscribing to task_monitoring and task_solution topics, it collects information
* about running tasks and their solutions and allows to inspect both,
* successful solutions and failed solution attempts.
*/
class TaskViewPrivate; class TaskViewPrivate;
class TaskView : public QWidget { class TaskView : public SubPanel {
Q_OBJECT Q_OBJECT
Q_DECLARE_PRIVATE(TaskView) Q_DECLARE_PRIVATE(TaskView)
TaskViewPrivate *d_ptr; TaskViewPrivate *d_ptr;
protected:
// configuration settings
enum TaskExpand { EXPAND_TOP=1, EXPAND_ALL, EXPAND_NONE };
rviz::EnumProperty* initial_task_expand;
public: public:
TaskView(QWidget* parent = 0); TaskView(TaskPanel* parent, rviz::Property* root);
~TaskView(); ~TaskView();
void save(rviz::Config config); void save(rviz::Config config) override;
void load(const rviz::Config& config); void load(const rviz::Config& config) override;
Q_SIGNALS:
void configChanged();
public Q_SLOTS: public Q_SLOTS:
void addTask(); void addTask();
@ -115,15 +139,18 @@ protected Q_SLOTS:
}; };
class TaskSettingsPrivate; class GlobalSettingsWidgetPrivate;
class TaskSettings : public QWidget { class GlobalSettingsWidget : public SubPanel {
Q_OBJECT Q_OBJECT
Q_DECLARE_PRIVATE(TaskSettings) Q_DECLARE_PRIVATE(GlobalSettingsWidget)
TaskSettingsPrivate *d_ptr; GlobalSettingsWidgetPrivate *d_ptr;
public: public:
TaskSettings(QWidget* parent = 0); GlobalSettingsWidget(TaskPanel* parent, rviz::Property* root);
~TaskSettings(); ~GlobalSettingsWidget();
void save(rviz::Config config) override;
void load(const rviz::Config &config) override;
}; };
} }

View File

@ -27,23 +27,9 @@
<number>0</number> <number>0</number>
</property> </property>
<item> <item>
<layout class="QHBoxLayout" name="horizontalLayout"> <layout class="QHBoxLayout" name="tool_buttons_layout">
<item> <item>
<widget class="QToolButton" name="button_show_settings"> <spacer name="spacer">
<property name="text">
<string>...</string>
</property>
<property name="icon">
<iconset resource="resources.qrc">
<normaloff>:/icons/settings.png</normaloff>:/icons/settings.png</iconset>
</property>
<property name="checkable">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer">
<property name="orientation"> <property name="orientation">
<enum>Qt::Horizontal</enum> <enum>Qt::Horizontal</enum>
</property> </property>
@ -68,6 +54,9 @@
</item> </item>
</layout> </layout>
</item> </item>
<item>
<widget class="QStackedWidget" name="stackedWidget"/>
</item>
</layout> </layout>
</widget> </widget>
<resources> <resources>

View File

@ -41,7 +41,7 @@
#include "task_panel.h" #include "task_panel.h"
#include "ui_task_panel.h" #include "ui_task_panel.h"
#include "ui_task_view.h" #include "ui_task_view.h"
#include "ui_task_settings.h" #include "ui_global_settings.h"
#include <rviz/panel.h> #include <rviz/panel.h>
#include <rviz/properties/property_tree_model.h> #include <rviz/properties/property_tree_model.h>
@ -58,8 +58,8 @@ public:
TaskPanelPrivate(TaskPanel *q_ptr); TaskPanelPrivate(TaskPanel *q_ptr);
TaskPanel* q_ptr; TaskPanel* q_ptr;
TaskView* tasks_widget; QButtonGroup* tool_buttons_group;
TaskSettings* settings_widget; rviz::Property* property_root;
rviz::WindowManagerInterface* window_manager_; rviz::WindowManagerInterface* window_manager_;
}; };
@ -85,11 +85,12 @@ public:
}; };
class TaskSettingsPrivate : public Ui_TaskSettings { class GlobalSettingsWidgetPrivate : public Ui_GlobalSettingsWidget {
public: public:
TaskSettingsPrivate(TaskSettings *q_ptr); GlobalSettingsWidgetPrivate(GlobalSettingsWidget *q_ptr, rviz::Property *root);
TaskSettings *q_ptr; GlobalSettingsWidget *q_ptr;
rviz::PropertyTreeModel *properties;
}; };
} }