From 6950cf2b8e97224e19bd46d9d4349bb12221fa32 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 18 Oct 2018 17:03:56 +0200 Subject: [PATCH 01/13] create TaskPanel via rviz::VisualizationFrame::addPanelByName() ... ensuring that rviz also saves the panel's settings --- visualization/motion_planning_tasks/src/task_panel.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index b6593f25..c9c806dc 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -115,8 +115,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() From 1ce66c758b1c0d22a0065f63eb14b515862423d6 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 11 Feb 2019 02:07:28 +0100 Subject: [PATCH 02/13] extensible TaskPanel Provide TaskPanel::addSubPanel() to add new sub panels. Each panel can be activated with an associated QToolButton. Sub panels, derived from base class SubPanel, automatically load/save their config settings. All settings are shown in GlobalSettingsWidget (renamed from TaskSettings). --- .../motion_planning_tasks/src/CMakeLists.txt | 2 +- .../{task_settings.ui => global_settings.ui} | 12 +-- .../motion_planning_tasks/src/task_panel.cpp | 79 +++++++++++++++---- .../motion_planning_tasks/src/task_panel.h | 59 +++++++++----- .../motion_planning_tasks/src/task_panel.ui | 21 ++--- .../motion_planning_tasks/src/task_panel_p.h | 13 +-- 6 files changed, 122 insertions(+), 64 deletions(-) rename visualization/motion_planning_tasks/src/{task_settings.ui => global_settings.ui} (80%) diff --git a/visualization/motion_planning_tasks/src/CMakeLists.txt b/visualization/motion_planning_tasks/src/CMakeLists.txt index c15ed591..5472ad9c 100644 --- a/visualization/motion_planning_tasks/src/CMakeLists.txt +++ b/visualization/motion_planning_tasks/src/CMakeLists.txt @@ -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} diff --git a/visualization/motion_planning_tasks/src/task_settings.ui b/visualization/motion_planning_tasks/src/global_settings.ui similarity index 80% rename from visualization/motion_planning_tasks/src/task_settings.ui rename to visualization/motion_planning_tasks/src/global_settings.ui index 3a76384c..4efb8c45 100644 --- a/visualization/motion_planning_tasks/src/task_settings.ui +++ b/visualization/motion_planning_tasks/src/global_settings.ui @@ -1,7 +1,7 @@ - moveit_rviz_plugin::TaskSettings - + moveit_rviz_plugin::GlobalSettingsWidget + 0 @@ -27,7 +27,7 @@ 0 - + 0 @@ -44,14 +44,14 @@ 0 - + - Settings + Global Settings - + diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index c9c806dc..d721e481 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -87,15 +87,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(&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 +112,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_; @@ -132,7 +155,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() @@ -143,13 +170,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(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(d_ptr->stackedWidget->widget(i)); + w->load(config.mapGetChild(w->windowTitle())); + } } void TaskPanel::showStageDockWidget() @@ -235,8 +268,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); @@ -426,23 +459,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" diff --git a/visualization/motion_planning_tasks/src/task_panel.h b/visualization/motion_planning_tasks/src/task_panel.h index 3b0d44d7..fe5a6321 100644 --- a/visualization/motion_planning_tasks/src/task_panel.h +++ b/visualization/motion_planning_tasks/src/task_panel.h @@ -42,9 +42,11 @@ #include #include class QItemSelection; +class QIcon; namespace rviz { class WindowManagerInterface; +class Property; } namespace moveit_rviz_plugin { @@ -53,12 +55,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 +82,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 +103,24 @@ 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; 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 +133,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; }; } diff --git a/visualization/motion_planning_tasks/src/task_panel.ui b/visualization/motion_planning_tasks/src/task_panel.ui index 8d178229..c0cf48a5 100644 --- a/visualization/motion_planning_tasks/src/task_panel.ui +++ b/visualization/motion_planning_tasks/src/task_panel.ui @@ -27,23 +27,9 @@ 0 - + - - - ... - - - - :/icons/settings.png:/icons/settings.png - - - true - - - - - + Qt::Horizontal @@ -68,6 +54,9 @@ + + + diff --git a/visualization/motion_planning_tasks/src/task_panel_p.h b/visualization/motion_planning_tasks/src/task_panel_p.h index 37cab821..322150b9 100644 --- a/visualization/motion_planning_tasks/src/task_panel_p.h +++ b/visualization/motion_planning_tasks/src/task_panel_p.h @@ -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 #include @@ -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; }; } From 16e97f1caa56b3bb82cb3374308718ce40d35103 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 11 Feb 2019 08:54:21 +0100 Subject: [PATCH 03/13] TaskView: configure initial expansion state for tasks --- .../motion_planning_tasks/src/task_panel.cpp | 28 ++++++++++++++----- .../motion_planning_tasks/src/task_panel.h | 6 ++++ 2 files changed, 27 insertions(+), 7 deletions(-) diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index d721e481..19271bbc 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -48,6 +48,7 @@ #include #include +#include #include #include #include @@ -218,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 } @@ -288,6 +294,14 @@ TaskView::TaskView(moveit_rviz_plugin::TaskPanel *parent, rviz::Property *root) 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() diff --git a/visualization/motion_planning_tasks/src/task_panel.h b/visualization/motion_planning_tasks/src/task_panel.h index fe5a6321..2d47f20a 100644 --- a/visualization/motion_planning_tasks/src/task_panel.h +++ b/visualization/motion_planning_tasks/src/task_panel.h @@ -47,6 +47,7 @@ class QIcon; namespace rviz { class WindowManagerInterface; class Property; +class EnumProperty; } namespace moveit_rviz_plugin { @@ -115,6 +116,11 @@ class TaskView : public SubPanel { 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(TaskPanel* parent, rviz::Property* root); ~TaskView(); From c72a6cddac02a9061307b8c859587676ce166cdd Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 13 Feb 2019 23:48:44 +0100 Subject: [PATCH 04/13] ComputeIK: auto-configure default timeout from JMG's default TODO: actually set the default value but not the current value! --- core/include/moveit/task_constructor/properties.h | 4 +++- core/src/properties.cpp | 8 ++++++++ core/src/stages/compute_ik.cpp | 2 +- 3 files changed, 12 insertions(+), 2 deletions(-) diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index 7d911054..3e033386 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -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(); @@ -274,6 +275,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 diff --git a/core/src/properties.cpp b/core/src/properties.cpp index a5297fe5..f12b1572 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -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 diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index de2dc019..f5c1468b 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -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("eef", "name of end-effector group"); p.declare("group", "name of active group (derived from eef if not provided)"); @@ -229,6 +228,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s) 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("target_pose"); From e98849249a3db3aeba8d788a2cd8b3a64b1b775e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 22 Feb 2019 16:14:34 +0100 Subject: [PATCH 05/13] simplify/fix color interpolation --- rviz_marker_tools/src/marker_creation.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/rviz_marker_tools/src/marker_creation.cpp b/rviz_marker_tools/src/marker_creation.cpp index 5ed8994e..3f8a8937 100644 --- a/rviz_marker_tools/src/marker_creation.cpp +++ b/rviz_marker_tools/src/marker_creation.cpp @@ -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; } From 36df4f9975b273dad016c176d37b2dd845832d7d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 13 Feb 2019 23:49:19 +0100 Subject: [PATCH 06/13] MoveTo/MoveRelative: reduce default timeout to 1s --- core/src/stages/move_relative.cpp | 2 +- core/src/stages/move_to.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 6520764f..2c4914e0 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -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("group", "name of planning group"); p.declare("ik_frame", "frame to be moved in Cartesian direction"); diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index e9302fb0..49891110 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -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("group", "name of planning group"); p.declare("ik_frame", "frame to be moved towards goal pose"); p.declare("goal", "goal specification"); From d741f36ee39561865a2376099d28a33ee4c57d44 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 10 Mar 2019 23:55:44 +0100 Subject: [PATCH 07/13] fixup! improve error msg for mismatching container/child interfaces We need to consider input and output interfaces separately. Also, use console output symbols (<- / -> / <->) --- core/src/container.cpp | 15 +++++++++------ core/src/stage.cpp | 22 +++++++++++++++------- 2 files changed, 24 insertions(+), 13 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index 6e70b269..76338ca7 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -554,14 +554,17 @@ void SerialContainer::validateConnectivity() const // 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()); + const auto my_flags = this->pimpl()->interfaceFlags(); + auto child_flags = start->interfaceFlags() & INPUT_IF_MASK; + if (child_flags != (my_flags & INPUT_IF_MASK)) + errors.push_back(*this, (desc % "input" % start->name() % flowSymbol(child_flags) + % flowSymbol(my_flags & INPUT_IF_MASK)).str()); 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()); + child_flags = last->interfaceFlags() & OUTPUT_IF_MASK; + if (child_flags != (my_flags & OUTPUT_IF_MASK)) + errors.push_back(*this, (desc % "output" % last->name() % flowSymbol(child_flags) + % flowSymbol(my_flags & OUTPUT_IF_MASK)).str()); } // validate connectivity of children amongst each other diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 7d382b58..15cdc9b4 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -349,13 +349,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(f & INPUT_IF_MASK) ^ static_cast(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) { From 7d25c55978214635da4d2a6e23e2d5d27dfe2b2b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 10 Mar 2019 23:26:36 +0100 Subject: [PATCH 08/13] Container: more unit tests for interface detection / validation --- core/test/test_container.cpp | 80 ++++++++++++++++++++++++++++++++---- 1 file changed, 71 insertions(+), 9 deletions(-) diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index 919963fe..e2eff17d 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -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("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);} } From f831fe54832023262052a340316c86bb78b4d527 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 10 Mar 2019 23:26:53 +0100 Subject: [PATCH 09/13] fix comments + typos --- core/include/moveit/task_constructor/stage_p.h | 14 ++++++++++---- core/src/container.cpp | 9 +++++---- core/src/stage.cpp | 2 +- 3 files changed, 16 insertions(+), 9 deletions(-) diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 9d668f30..78e77980 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -56,16 +56,22 @@ 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 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) {} virtual bool canCompute() const = 0; diff --git a/core/src/container.cpp b/core/src/container.cpp index 76338ca7..91d956ef 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -367,8 +367,8 @@ 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()) @@ -424,7 +424,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(); @@ -582,7 +583,7 @@ void SerialContainer::validateConnectivity() const // start pull interface fed? if (cur != impl->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 diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 15cdc9b4..1ed760c0 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -519,7 +519,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 From 10c7a9cfd7fe7001debb046e49191a0b819d9dbf Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 11 Mar 2019 01:01:53 +0100 Subject: [PATCH 10/13] prune UNKNOWN *and* PROPAGATE_BOTHWAYS If PropagatingEitherWay's interface is not met in *both* directions (but only one), in BOTHWAY mode, issue a warning. Otherwise handle both, AUTO and BOTHWAY mode, in the same fashion when resolving interfaces. TODO: move validateConnectivity() in StagePrivate. default action = default action from ContainerBase. PropagatingEitherWay: issue warning for case above --- core/src/container.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index 91d956ef..ff8df82a 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -399,7 +399,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: From 905b6a3b0c4614930868874f552f3ced7130f358 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 11 Mar 2019 06:26:37 +0100 Subject: [PATCH 11/13] move validateConnectivity() from ContainerBase to StagePrivate ... to allow specific stage types (PropagatingEitherWay) implementing their own validation No need for a public interface. --- .../moveit/task_constructor/container.h | 8 -- .../moveit/task_constructor/container_p.h | 6 + .../include/moveit/task_constructor/stage_p.h | 5 + core/src/container.cpp | 112 ++++++++---------- core/src/stage.cpp | 32 ++++- core/src/task.cpp | 2 +- core/test/test_container.cpp | 2 +- 7 files changed, 96 insertions(+), 71 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index da7666e2..7dd1ac4e 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -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); diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 1bd61ea8..fe05a772 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -97,6 +97,8 @@ public: return const_cast(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); diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 78e77980..27d3a19d 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -74,6 +74,9 @@ public: * 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; @@ -179,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; diff --git a/core/src/container.cpp b/core/src/container.cpp index ff8df82a..63ebd1ce 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -89,6 +89,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 @@ -239,25 +250,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(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; @@ -546,42 +538,45 @@ 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%)"); + // recursively validate children + try { ContainerBasePrivate::validateConnectivity(); } + catch (InitStageException& e) { errors.append(e); } + // check that input / output interface of first / last child matches this' resp. interface - if (!impl->children().empty()) { - const StagePrivate* start = impl->children().front()->pimpl(); - const auto my_flags = this->pimpl()->interfaceFlags(); + 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(*this, (desc % "input" % start->name() % flowSymbol(child_flags) + errors.push_back(*me(), (desc % "input" % start->name() % flowSymbol(child_flags) % flowSymbol(my_flags & INPUT_IF_MASK)).str()); - const StagePrivate* last = impl->children().back()->pimpl(); + const StagePrivate* last = children().back()->pimpl(); child_flags = last->interfaceFlags() & OUTPUT_IF_MASK; if (child_flags != (my_flags & OUTPUT_IF_MASK)) - errors.push_back(*this, (desc % "output" % last->name() % flowSymbol(child_flags) + 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, "start interface is not fed"); @@ -591,9 +586,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; } @@ -693,6 +685,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); @@ -736,31 +753,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(); diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 1ed760c0..46a15bb9 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -83,6 +83,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); @@ -427,6 +436,27 @@ void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) { 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; @@ -434,7 +464,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; } diff --git a/core/src/task.cpp b/core/src/task.cpp index 26c6f693..b4e517b9 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -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()); diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index e2eff17d..17403b25 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -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) { From 48d93e803d8bfe3fbd2bda869695b2d07fdc1b71 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 11 Mar 2019 09:53:39 +0100 Subject: [PATCH 12/13] improve SerialContainerPrivate::pruneInterface --- core/src/container.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index 63ebd1ce..de0d350f 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -366,7 +366,7 @@ 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 @@ -459,10 +459,12 @@ 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? - { + 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); // reset my pull interfaces, if first/last child don't pull anymore @@ -471,7 +473,8 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { if (!children().back()->pimpl()->ends()) ends_.reset(); } - if (interfaceFlags() == UNKNOWN) + + if (interfaceFlags() == InterfaceFlags()) throw InitStageException(*me(), "failed to derive propagation direction"); } @@ -508,8 +511,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); } From c56c7294e4714ad5bb8a817cffab5f654a6429e1 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 11 Mar 2019 11:02:07 +0100 Subject: [PATCH 13/13] fix pruning Never augment already derived interfaces, only prune! --- core/src/container.cpp | 27 ++++++++++++++++++--------- core/src/stage.cpp | 4 ++-- 2 files changed, 20 insertions(+), 11 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index de0d350f..ae1bc21c 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -459,6 +459,9 @@ void SerialContainer::init(const moveit::core::RobotModelConstPtr& robot_model) void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { if (children().empty()) return; + // reading is always allowed if current interface flags do so + accepted |= (interfaceFlags() & InterfaceFlags({READS_START, READS_END})); + if (accepted == PROPAGATE_BOTHWAYS) return; // There is nothing to prune @@ -466,14 +469,20 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { if (children().front()->pimpl()->interfaceFlags() == PROPAGATE_BOTHWAYS && children().back()->pimpl()->interfaceFlags() == PROPAGATE_BOTHWAYS) { pruneInterfaces(children().begin(), children().end(), accepted); - - // 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(); + } else { // otherwise only prune the first / last child's input / output interface + StagePrivate* child_impl; + child_impl = children().front()->pimpl(); + child_impl->pruneInterface((accepted & INPUT_IF_MASK) | (child_impl->interfaceFlags() & OUTPUT_IF_MASK)); + child_impl = children().back()->pimpl(); + child_impl->pruneInterface((accepted & OUTPUT_IF_MASK) | (child_impl->interfaceFlags() & INPUT_IF_MASK)); } + // reset my pull interfaces, if first/last child don't pull anymore + 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"); } @@ -527,11 +536,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 diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 46a15bb9..b868a232 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -429,9 +429,9 @@ 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)); }