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

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

View File

@ -64,8 +64,6 @@ public:
void reset() override;
void 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);

View File

@ -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);

View File

@ -65,7 +65,7 @@ boost::any fromName(const PropertyMap& other, const std::string& other_name);
*
* Setting the value via setValue() updates both, the current value and the default value.
* 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

View File

@ -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;

View File

@ -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();

View File

@ -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

View File

@ -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

View File

@ -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");

View File

@ -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");

View File

@ -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");

View File

@ -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());

View File

@ -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);}
}

View File

@ -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;
}

View File

@ -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}

View File

@ -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>

View File

@ -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"

View File

@ -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;
};
}

View File

@ -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>

View File

@ -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;
};
}