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 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 void compute() = 0;
|
||||
@ -89,9 +87,6 @@ public:
|
||||
|
||||
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;
|
||||
void compute() override;
|
||||
|
||||
@ -132,9 +127,6 @@ public:
|
||||
|
||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||
|
||||
/// validate connectivity of children (after init() was done)
|
||||
void validateConnectivity() const override;
|
||||
|
||||
protected:
|
||||
ParallelContainerBase(ParallelContainerBasePrivate* impl);
|
||||
|
||||
|
||||
@ -97,6 +97,8 @@ public:
|
||||
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
|
||||
bool canCompute() const override;
|
||||
void compute() override;
|
||||
@ -162,6 +164,8 @@ public:
|
||||
|
||||
// called by parent asking for pruning of this' interface
|
||||
void pruneInterface(InterfaceFlags accepted) override;
|
||||
// validate connectivity of chain
|
||||
void validateConnectivity() const override;
|
||||
|
||||
private:
|
||||
// connect cur stage to its predecessor and successor
|
||||
@ -214,6 +218,8 @@ public:
|
||||
// called by parent asking for pruning of this' interface
|
||||
void pruneInterface(InterfaceFlags accepted) override;
|
||||
|
||||
void validateConnectivity() const override;
|
||||
|
||||
private:
|
||||
/// callback for new externally received states
|
||||
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.
|
||||
* 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 {
|
||||
friend class PropertyMap;
|
||||
@ -97,6 +97,7 @@ public:
|
||||
/// set current value and default value
|
||||
void setValue(const boost::any& value);
|
||||
void setCurrentValue(const boost::any& value);
|
||||
void setDefaultValue(const boost::any& value);
|
||||
|
||||
/// reset to default value (which can be empty)
|
||||
void reset();
|
||||
@ -275,6 +276,7 @@ public:
|
||||
/// declare given property name as other_name in other PropertyMap
|
||||
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;
|
||||
|
||||
/// 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);
|
||||
|
||||
public:
|
||||
// container type used to store children
|
||||
/// container type used to store children
|
||||
typedef std::list<Stage::pointer> container_type;
|
||||
StagePrivate(Stage* me, const std::string& name);
|
||||
virtual ~StagePrivate() = default;
|
||||
|
||||
/// actually configured interface of this stage (only valid after init())
|
||||
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;
|
||||
// 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) {}
|
||||
|
||||
/// validate connectivity of children (after init() was done)
|
||||
virtual void validateConnectivity() const;
|
||||
|
||||
virtual bool canCompute() const = 0;
|
||||
virtual void compute() = 0;
|
||||
|
||||
@ -173,6 +182,8 @@ public:
|
||||
void initInterface(PropagatingEitherWay::Direction dir);
|
||||
// prune interface to the given propagation direction
|
||||
void pruneInterface(InterfaceFlags accepted) override;
|
||||
// validate that we can propagate in one direction at least
|
||||
void validateConnectivity() const override;
|
||||
|
||||
bool canCompute() const override;
|
||||
void compute() override;
|
||||
|
||||
@ -90,6 +90,17 @@ bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback &pr
|
||||
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
|
||||
{
|
||||
// call the method of the public interface
|
||||
@ -240,25 +251,6 @@ void ContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||
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) {
|
||||
ContainerBase::StageCallback processor = [&os](const Stage& stage, int depth) -> bool {
|
||||
os << std::string(2*depth, ' ') << *stage.pimpl() << std::endl;
|
||||
@ -368,14 +360,14 @@ SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::s
|
||||
: ContainerBasePrivate(me, name)
|
||||
{}
|
||||
|
||||
// a serial container's required interface is derived from the required input interface
|
||||
// of the first child and the required output interface of the last child
|
||||
// a serial container's required interface is derived from the actual input interface
|
||||
// of the first child and the actual output interface of the last child
|
||||
InterfaceFlags SerialContainerPrivate::requiredInterface() const
|
||||
{
|
||||
if (children().empty())
|
||||
return UNKNOWN;
|
||||
return (children().front()->pimpl()->interfaceFlags() & INPUT_IF_MASK)
|
||||
| (children().back()->pimpl()->interfaceFlags() & OUTPUT_IF_MASK);
|
||||
| (children().back()->pimpl()->interfaceFlags() & OUTPUT_IF_MASK);
|
||||
}
|
||||
|
||||
// connect cur stage to its predecessor and successor by setting the push interface pointers
|
||||
@ -400,7 +392,7 @@ bool SerialContainerPrivate::connect(container_type::const_iterator cur)
|
||||
cur_impl->setPrevEnds((*prev)->pimpl()->ends());
|
||||
|
||||
// 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:
|
||||
@ -425,7 +417,8 @@ void SerialContainer::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||
impl->starts_.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 last = --impl->children().end();
|
||||
@ -467,19 +460,31 @@ void SerialContainer::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||
void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
|
||||
if (children().empty()) return;
|
||||
|
||||
// We only need to deal with the special case of the whole sequence to be pruned.
|
||||
if (accepted != PROPAGATE_BOTHWAYS && // will interface be restricted at all?
|
||||
children().front()->pimpl()->interfaceFlags() == PROPAGATE_BOTHWAYS) // still undecided?
|
||||
{
|
||||
pruneInterfaces(children().begin(), children().end(), accepted);
|
||||
// reading is always allowed if current interface flags do so
|
||||
accepted |= (interfaceFlags() & InterfaceFlags({READS_START, READS_END}));
|
||||
|
||||
// reset my pull interfaces, if first/last child don't pull anymore
|
||||
if (!children().front()->pimpl()->starts())
|
||||
starts_.reset();
|
||||
if (!children().back()->pimpl()->ends())
|
||||
ends_.reset();
|
||||
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);
|
||||
} 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));
|
||||
}
|
||||
if (interfaceFlags() == UNKNOWN)
|
||||
|
||||
// reset my pull interfaces, if first/last child don't pull anymore
|
||||
if (!children().front()->pimpl()->starts())
|
||||
starts_.reset();
|
||||
if (!children().back()->pimpl()->ends())
|
||||
ends_.reset();
|
||||
|
||||
if (interfaceFlags() == InterfaceFlags())
|
||||
throw InitStageException(*me(), "failed to derive propagation direction");
|
||||
}
|
||||
|
||||
@ -516,8 +521,8 @@ void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator firs
|
||||
|
||||
// nothing to do if:
|
||||
// - accepted == 0: interface still unknown
|
||||
// - accepted == PROPAGATE_FORWARDS | PROPAGATE_BACKWARDS: no change
|
||||
if (accepted != UNKNOWN && accepted != InterfaceFlags({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS}))
|
||||
// - accepted == PROPAGATE_BOTHWAYS: no change
|
||||
if (accepted != UNKNOWN && accepted != PROPAGATE_BOTHWAYS)
|
||||
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
|
||||
assert(impl->requiredInterface() == UNKNOWN);
|
||||
|
||||
// remove push interfaces
|
||||
if (!(accepted & PROPAGATE_BACKWARDS))
|
||||
// remove push interfaces if not accepted
|
||||
if (!(accepted & WRITES_PREV_END))
|
||||
impl->setPrevEnds(InterfacePtr());
|
||||
|
||||
if (!(accepted & PROPAGATE_FORWARDS))
|
||||
if (!(accepted & WRITES_NEXT_START))
|
||||
impl->setNextStarts(InterfacePtr());
|
||||
}
|
||||
// 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;
|
||||
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
|
||||
if (!impl->children().empty()) {
|
||||
const StagePrivate* start = impl->children().front()->pimpl();
|
||||
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());
|
||||
// recursively validate children
|
||||
try { ContainerBasePrivate::validateConnectivity(); }
|
||||
catch (InitStageException& e) { errors.append(e); }
|
||||
|
||||
const StagePrivate* last = impl->children().back()->pimpl();
|
||||
if ((last->interfaceFlags() & OUTPUT_IF_MASK) != (this->pimpl()->interfaceFlags() & OUTPUT_IF_MASK))
|
||||
errors.push_back(*this, (desc % "output" % last->name() % flowSymbol(last->interfaceFlags())
|
||||
% flowSymbol(this->pimpl()->interfaceFlags())).str());
|
||||
// check that input / output interface of first / last child matches this' resp. interface
|
||||
if (!children().empty()) {
|
||||
const StagePrivate* start = children().front()->pimpl();
|
||||
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
|
||||
// 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.
|
||||
// Here, it remains to check that - if a child requires a pull interface - it's indeed feeded.
|
||||
for (auto cur = impl->children().begin(), end = impl->children().end(); cur != end; ++cur) {
|
||||
// Here, it remains to check that - if a child has a pull interface - it's indeed feeded.
|
||||
for (auto cur = children().begin(), end = children().end(); cur != end; ++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
|
||||
auto prev = cur; --prev;
|
||||
auto next = cur; ++next;
|
||||
|
||||
// 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())
|
||||
errors.push_back(**cur, "end interface is not fed");
|
||||
errors.push_back(**cur, "start interface is not fed");
|
||||
|
||||
// end pull interface fed?
|
||||
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");
|
||||
}
|
||||
|
||||
// recursively validate children
|
||||
try { ContainerBase::validateConnectivity(); } catch (InitStageException& e) { errors.append(e); }
|
||||
|
||||
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) {
|
||||
for (const Stage::pointer& stage : children())
|
||||
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)
|
||||
{
|
||||
auto impl = pimpl();
|
||||
|
||||
@ -142,6 +142,14 @@ void Property::setCurrentValue(const boost::any &value)
|
||||
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()
|
||||
{
|
||||
if (initialized_from_ == 0) // TODO: use enum
|
||||
|
||||
@ -84,6 +84,15 @@ InterfaceFlags StagePrivate::interfaceFlags() const
|
||||
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)
|
||||
{
|
||||
solution->setCreator(this);
|
||||
@ -350,13 +359,21 @@ const char* direction(const StagePrivate& stage) {
|
||||
return "<-";
|
||||
}
|
||||
|
||||
const char* flowSymbol(moveit::task_constructor::InterfaceFlags f) {
|
||||
if (f == InterfaceFlags(CONNECT)) return "∞";
|
||||
if (f == InterfaceFlags(PROPAGATE_FORWARDS)) return "↓";
|
||||
if (f == InterfaceFlags(PROPAGATE_BACKWARDS)) return "↑";
|
||||
if (f == PROPAGATE_BOTHWAYS) return "⇅";
|
||||
if (f == InterfaceFlags(GENERATE)) return "↕";
|
||||
return "?";
|
||||
const char* flowSymbol(InterfaceFlags f) {
|
||||
if (f == InterfaceFlags())
|
||||
return "?"; // unknown interface
|
||||
|
||||
// f should have either INPUT or OUTPUT flags set (not both)
|
||||
assert(static_cast<bool>(f & INPUT_IF_MASK) ^ static_cast<bool>(f & OUTPUT_IF_MASK));
|
||||
|
||||
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) {
|
||||
@ -413,13 +430,34 @@ void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction
|
||||
|
||||
void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) {
|
||||
int dir = 0;
|
||||
if (accepted & PROPAGATE_FORWARDS)
|
||||
if ((accepted & PROPAGATE_FORWARDS) == PROPAGATE_FORWARDS)
|
||||
dir |= PropagatingEitherWay::FORWARD;
|
||||
if (accepted & PROPAGATE_BACKWARDS)
|
||||
if ((accepted & PROPAGATE_BACKWARDS) == PROPAGATE_BACKWARDS)
|
||||
dir |= PropagatingEitherWay::BACKWARD;
|
||||
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 f;
|
||||
@ -427,7 +465,7 @@ InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const
|
||||
f |= PROPAGATE_FORWARDS;
|
||||
if (required_interface_dirs_ & PropagatingEitherWay::BACKWARD)
|
||||
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
|
||||
return f;
|
||||
}
|
||||
@ -512,7 +550,7 @@ void PropagatingEitherWay::init(const moveit::core::RobotModelConstPtr& robot_mo
|
||||
// pretend that we offer both interface directions during init().
|
||||
// This is needed due to a chicken-egg-problem: interface auto-detection requires
|
||||
// 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)
|
||||
impl->initInterface(BOTHWAY);
|
||||
// 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)
|
||||
: WrapperBase(name, std::move(child))
|
||||
{
|
||||
setTimeout(1.0);
|
||||
auto& p = properties();
|
||||
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)");
|
||||
@ -254,6 +253,7 @@ void ComputeIK::compute()
|
||||
ROS_WARN_STREAM_NAMED("ComputeIK", "Neither eef nor group are well defined");
|
||||
return;
|
||||
}
|
||||
properties().property("timeout").setDefaultValue(jmg->getDefaultIKTimeout());
|
||||
|
||||
// extract 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)
|
||||
, planner_(planner)
|
||||
{
|
||||
setTimeout(10.0);
|
||||
auto& p = properties();
|
||||
p.property("timeout").setDefaultValue(1.0);
|
||||
p.declare<std::string>("group", "name of planning group");
|
||||
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)
|
||||
, planner_(planner)
|
||||
{
|
||||
setTimeout(10.0);
|
||||
auto& p = properties();
|
||||
p.property("timeout").setDefaultValue(1.0);
|
||||
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<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
|
||||
stages()->pimpl()->pruneInterface(InterfaceFlags({GENERATE}));
|
||||
// and *finally* validate connectivity
|
||||
stages()->validateConnectivity();
|
||||
stages()->pimpl()->validateConnectivity();
|
||||
|
||||
// provide introspection instance to all stages
|
||||
impl->setIntrospection(introspection_.get());
|
||||
|
||||
@ -134,7 +134,7 @@ protected:
|
||||
if (start) accepted |= WRITES_PREV_END;
|
||||
if (end) accepted |= WRITES_NEXT_START;
|
||||
container.pimpl()->pruneInterface(accepted);
|
||||
container.validateConnectivity();
|
||||
container.pimpl()->validateConnectivity();
|
||||
if (!expect_failure) return; // as expected
|
||||
ADD_FAILURE() << "init() didn't recognize a failure condition as expected\n" << container;
|
||||
} catch (const InitStageException &e) {
|
||||
@ -235,7 +235,6 @@ TEST_F(SerialTest, init_empty) {
|
||||
TEST_F(SerialTest, init_connecting) {
|
||||
EXPECT_INIT_SUCCESS(false, false, CONN);
|
||||
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_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags({CONNECT, GENERATE}));
|
||||
@ -246,7 +245,6 @@ TEST_F(SerialTest, init_connecting) {
|
||||
TEST_F(SerialTest, init_generator) {
|
||||
EXPECT_INIT_SUCCESS(true, true, GEN);
|
||||
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
|
||||
|
||||
@ -259,15 +257,48 @@ TEST_F(SerialTest, init_forward) {
|
||||
EXPECT_INIT_FAILURE(true, true, 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
|
||||
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_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_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) {
|
||||
@ -278,10 +309,10 @@ TEST_F(SerialTest, init_backward) {
|
||||
|
||||
// these should be fine
|
||||
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_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_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
|
||||
@ -320,6 +351,11 @@ TEST_F(SerialTest, interface_detection) {
|
||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT));
|
||||
|
||||
// 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); // -> ->
|
||||
it = container.pimpl()->children().begin();
|
||||
EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
|
||||
@ -346,10 +382,36 @@ TEST_F(SerialTest, 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});
|
||||
container.insert(std::move(inner));
|
||||
validateInit(true, true, {ANY}, false);
|
||||
container.insert(Stage::pointer(inner));
|
||||
{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;
|
||||
}
|
||||
|
||||
// interpolate between start and end with fraction in range from 0..1
|
||||
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) {
|
||||
if (fraction < 0.0) fraction = 0.0;
|
||||
if (fraction > 1.0) fraction = 1.0;
|
||||
color.r += interpolate(other.r-color.r, other.r, fraction);
|
||||
color.g += interpolate(other.g-color.g, other.g, fraction);
|
||||
color.b += interpolate(other.b-color.b, other.b, fraction);
|
||||
color.r = interpolate(color.r, other.r, fraction);
|
||||
color.g = interpolate(color.g, other.g, fraction);
|
||||
color.b = interpolate(color.b, other.b, fraction);
|
||||
color.a = interpolate(color.a, other.a, fraction);
|
||||
return color;
|
||||
}
|
||||
|
||||
|
||||
@ -3,7 +3,7 @@ set(MOVEIT_LIB_NAME motion_planning_tasks_rviz_plugin)
|
||||
qt_wrap_ui(UIC_FILES
|
||||
task_panel.ui
|
||||
task_view.ui
|
||||
task_settings.ui
|
||||
global_settings.ui
|
||||
)
|
||||
|
||||
add_library(${MOVEIT_LIB_NAME}
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<ui version="4.0">
|
||||
<class>moveit_rviz_plugin::TaskSettings</class>
|
||||
<widget class="QWidget" name="moveit_rviz_plugin::TaskSettings">
|
||||
<class>moveit_rviz_plugin::GlobalSettingsWidget</class>
|
||||
<widget class="QWidget" name="moveit_rviz_plugin::GlobalSettingsWidget">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>0</x>
|
||||
@ -27,7 +27,7 @@
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
<layout class="QVBoxLayout" name="tab_settings_layout">
|
||||
<layout class="QVBoxLayout" name="layout">
|
||||
<property name="spacing">
|
||||
<number>0</number>
|
||||
</property>
|
||||
@ -44,14 +44,14 @@
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
<widget class="QLabel" name="settings_view_label">
|
||||
<widget class="QLabel" name="label">
|
||||
<property name="text">
|
||||
<string>Settings</string>
|
||||
<string>Global Settings</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="rviz::PropertyTreeWidget" name="settings_view"/>
|
||||
<widget class="rviz::PropertyTreeWidget" name="view"/>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
@ -48,6 +48,7 @@
|
||||
#include <moveit/task_constructor/stage.h>
|
||||
|
||||
#include <rviz/properties/property.h>
|
||||
#include <rviz/properties/enum_property.h>
|
||||
#include <rviz/display_group.h>
|
||||
#include <rviz/visualization_manager.h>
|
||||
#include <rviz/window_manager_interface.h>
|
||||
@ -87,15 +88,20 @@ TaskPanel::TaskPanel(QWidget* parent)
|
||||
{
|
||||
Q_D(TaskPanel);
|
||||
|
||||
d->tasks_widget = new TaskView(this);
|
||||
d->settings_widget = new TaskSettings(this);
|
||||
layout()->addWidget(d->tasks_widget);
|
||||
layout()->addWidget(d->settings_widget);
|
||||
connect(d->tasks_widget, SIGNAL(configChanged()), this, SIGNAL(configChanged()));
|
||||
// sync checked tool button with displayed widget
|
||||
connect(d->tool_buttons_group, static_cast<void(QButtonGroup::*)(int)>(&QButtonGroup::buttonClicked),
|
||||
d->stackedWidget, [d](int index) { d->stackedWidget->setCurrentIndex(index); });
|
||||
connect(d->stackedWidget, &QStackedWidget::currentChanged, d->tool_buttons_group,
|
||||
[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_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 (singleton_.isNull())
|
||||
@ -107,6 +113,24 @@ TaskPanel::~TaskPanel()
|
||||
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)
|
||||
{
|
||||
++display_count_;
|
||||
@ -115,8 +139,10 @@ void TaskPanel::incDisplayCount(rviz::WindowManagerInterface* window_manager)
|
||||
if (singleton_ || !vis_frame)
|
||||
return; // already define, nothing to do
|
||||
|
||||
window_manager->addPane("Motion Planning Tasks", new TaskPanel());
|
||||
singleton_->initialize(vis_frame->getManager());
|
||||
QDockWidget* dock = vis_frame->addPanelByName("Motion Planning Tasks",
|
||||
"moveit_task_constructor/Motion Planning Tasks",
|
||||
Qt::LeftDockWidgetArea, true /* floating */);
|
||||
assert(dock->widget() == singleton_);
|
||||
}
|
||||
|
||||
void TaskPanel::decDisplayCount()
|
||||
@ -130,7 +156,11 @@ TaskPanelPrivate::TaskPanelPrivate(TaskPanel *q_ptr)
|
||||
: q_ptr(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->setToolTip(QStringLiteral("Show available stages"));
|
||||
property_root = new rviz::Property("Global Settings");
|
||||
}
|
||||
|
||||
void TaskPanel::onInitialize()
|
||||
@ -141,13 +171,19 @@ void TaskPanel::onInitialize()
|
||||
void TaskPanel::save(rviz::Config config) const
|
||||
{
|
||||
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)
|
||||
{
|
||||
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()
|
||||
@ -183,15 +219,20 @@ TaskViewPrivate::TaskViewPrivate(TaskView *q_ptr)
|
||||
tasks_view->setModel(meta_model);
|
||||
// auto-expand newly-inserted top-level items
|
||||
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) {
|
||||
QModelIndex child = parent.child(row, 0);
|
||||
// expand inserted items
|
||||
setExpanded(tasks_view, child, true);
|
||||
// collapse up to first level
|
||||
setExpanded(tasks_view, child, false, 1);
|
||||
// expand inserted item
|
||||
setExpanded(tasks_view, child, true, 0);
|
||||
if (expand != TaskView::EXPAND_NONE) {
|
||||
// recursively expand all inserted items
|
||||
setExpanded(tasks_view, child, true);
|
||||
}
|
||||
if (expand == TaskView::EXPAND_TOP) {
|
||||
// collapse up to first level
|
||||
setExpanded(tasks_view, child, false, 1);
|
||||
// expand inserted item
|
||||
setExpanded(tasks_view, child, true, 0);
|
||||
}
|
||||
}
|
||||
tasks_view->setExpanded(parent, true); // expand parent group item
|
||||
}
|
||||
@ -233,8 +274,8 @@ void TaskViewPrivate::lock(TaskDisplay* display)
|
||||
locked_display_ = display;
|
||||
}
|
||||
|
||||
TaskView::TaskView(QWidget *parent)
|
||||
: QWidget(parent), d_ptr(new TaskViewPrivate(this))
|
||||
TaskView::TaskView(moveit_rviz_plugin::TaskPanel *parent, rviz::Property *root)
|
||||
: SubPanel(parent), d_ptr(new TaskViewPrivate(this))
|
||||
{
|
||||
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->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()));
|
||||
|
||||
// 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()
|
||||
@ -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)
|
||||
{
|
||||
setupUi(q_ptr);
|
||||
properties = new rviz::PropertyTreeModel(root, q_ptr);
|
||||
view->setModel(properties);
|
||||
}
|
||||
|
||||
TaskSettings::TaskSettings(QWidget *parent)
|
||||
: QWidget(parent), d_ptr(new TaskSettingsPrivate(this))
|
||||
GlobalSettingsWidget::GlobalSettingsWidget(moveit_rviz_plugin::TaskPanel *parent, rviz::Property *root)
|
||||
: 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;
|
||||
}
|
||||
|
||||
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"
|
||||
|
||||
@ -42,9 +42,12 @@
|
||||
#include <moveit/macros/class_forward.h>
|
||||
#include <QModelIndex>
|
||||
class QItemSelection;
|
||||
class QIcon;
|
||||
|
||||
namespace rviz {
|
||||
class WindowManagerInterface;
|
||||
class Property;
|
||||
class EnumProperty;
|
||||
}
|
||||
|
||||
namespace moveit_rviz_plugin {
|
||||
@ -53,12 +56,22 @@ class TaskSolutionVisualization;
|
||||
MOVEIT_CLASS_FORWARD(TaskListModel)
|
||||
MOVEIT_CLASS_FORWARD(TaskPanel)
|
||||
|
||||
/** The TaskPanel displays information about manipulation tasks in the system.
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
/// Base class for all sub panels within the Task Panel
|
||||
class SubPanel : public QWidget {
|
||||
Q_OBJECT
|
||||
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 TaskPanel: public rviz::Panel
|
||||
{
|
||||
@ -70,6 +83,9 @@ public:
|
||||
TaskPanel(QWidget* parent = 0);
|
||||
~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.
|
||||
*
|
||||
* If not yet done, an instance is created. If use count drops to zero,
|
||||
@ -88,21 +104,29 @@ protected Q_SLOTS:
|
||||
|
||||
|
||||
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 TaskView : public QWidget {
|
||||
class TaskView : public SubPanel {
|
||||
Q_OBJECT
|
||||
Q_DECLARE_PRIVATE(TaskView)
|
||||
TaskViewPrivate *d_ptr;
|
||||
|
||||
protected:
|
||||
// configuration settings
|
||||
enum TaskExpand { EXPAND_TOP=1, EXPAND_ALL, EXPAND_NONE };
|
||||
rviz::EnumProperty* initial_task_expand;
|
||||
|
||||
public:
|
||||
TaskView(QWidget* parent = 0);
|
||||
TaskView(TaskPanel* parent, rviz::Property* root);
|
||||
~TaskView();
|
||||
|
||||
void save(rviz::Config config);
|
||||
void load(const rviz::Config& config);
|
||||
|
||||
Q_SIGNALS:
|
||||
void configChanged();
|
||||
void save(rviz::Config config) override;
|
||||
void load(const rviz::Config& config) override;
|
||||
|
||||
public Q_SLOTS:
|
||||
void addTask();
|
||||
@ -115,15 +139,18 @@ protected Q_SLOTS:
|
||||
};
|
||||
|
||||
|
||||
class TaskSettingsPrivate;
|
||||
class TaskSettings : public QWidget {
|
||||
class GlobalSettingsWidgetPrivate;
|
||||
class GlobalSettingsWidget : public SubPanel {
|
||||
Q_OBJECT
|
||||
Q_DECLARE_PRIVATE(TaskSettings)
|
||||
TaskSettingsPrivate *d_ptr;
|
||||
Q_DECLARE_PRIVATE(GlobalSettingsWidget)
|
||||
GlobalSettingsWidgetPrivate *d_ptr;
|
||||
|
||||
public:
|
||||
TaskSettings(QWidget* parent = 0);
|
||||
~TaskSettings();
|
||||
GlobalSettingsWidget(TaskPanel* parent, rviz::Property* root);
|
||||
~GlobalSettingsWidget();
|
||||
|
||||
void save(rviz::Config config) override;
|
||||
void load(const rviz::Config &config) override;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@ -27,23 +27,9 @@
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
<layout class="QHBoxLayout" name="horizontalLayout">
|
||||
<layout class="QHBoxLayout" name="tool_buttons_layout">
|
||||
<item>
|
||||
<widget class="QToolButton" name="button_show_settings">
|
||||
<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">
|
||||
<spacer name="spacer">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
@ -68,6 +54,9 @@
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QStackedWidget" name="stackedWidget"/>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
<resources>
|
||||
|
||||
@ -41,7 +41,7 @@
|
||||
#include "task_panel.h"
|
||||
#include "ui_task_panel.h"
|
||||
#include "ui_task_view.h"
|
||||
#include "ui_task_settings.h"
|
||||
#include "ui_global_settings.h"
|
||||
|
||||
#include <rviz/panel.h>
|
||||
#include <rviz/properties/property_tree_model.h>
|
||||
@ -58,8 +58,8 @@ public:
|
||||
TaskPanelPrivate(TaskPanel *q_ptr);
|
||||
|
||||
TaskPanel* q_ptr;
|
||||
TaskView* tasks_widget;
|
||||
TaskSettings* settings_widget;
|
||||
QButtonGroup* tool_buttons_group;
|
||||
rviz::Property* property_root;
|
||||
|
||||
rviz::WindowManagerInterface* window_manager_;
|
||||
};
|
||||
@ -85,11 +85,12 @@ public:
|
||||
};
|
||||
|
||||
|
||||
class TaskSettingsPrivate : public Ui_TaskSettings {
|
||||
class GlobalSettingsWidgetPrivate : public Ui_GlobalSettingsWidget {
|
||||
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