mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
commit
39427f89bf
@ -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);
|
||||||
|
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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();
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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");
|
||||||
|
|||||||
@ -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");
|
||||||
|
|
||||||
|
|||||||
@ -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");
|
||||||
|
|||||||
@ -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());
|
||||||
|
|||||||
@ -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);}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -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}
|
||||||
|
|||||||
@ -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>
|
||||||
@ -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"
|
||||||
|
|||||||
@ -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;
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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>
|
||||||
|
|||||||
@ -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;
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user