diff --git a/visualization/motion_planning_tasks/factory_model.cpp b/visualization/motion_planning_tasks/factory_model.cpp index 1f3482d7..ec70a215 100644 --- a/visualization/motion_planning_tasks/factory_model.cpp +++ b/visualization/motion_planning_tasks/factory_model.cpp @@ -36,21 +36,24 @@ #include "factory_model.h" #include +#include +#include namespace moveit_rviz_plugin { -FactoryModel::FactoryModel(rviz::Factory *factory, QObject *parent) +FactoryModel::FactoryModel(rviz::Factory &factory, const QString& mime_type, QObject *parent) : QStandardItemModel(parent) + , mime_type_(mime_type) { setHorizontalHeaderLabels({tr("Name")}); fillTree(factory); } -void FactoryModel::fillTree(rviz::Factory *factory) +void FactoryModel::fillTree(rviz::Factory &factory) { QIcon default_package_icon = rviz::loadPixmap( "package://rviz/icons/default_package_icon.png" ); - QStringList classes = factory->getDeclaredClassIds(); + QStringList classes = factory.getDeclaredClassIds(); classes.sort(); // Map from package names to the corresponding top-level tree widget items. @@ -58,7 +61,7 @@ void FactoryModel::fillTree(rviz::Factory *factory) for(const QString& lookup_name : classes) { - QString package = factory->getClassPackage(lookup_name); + QString package = factory.getClassPackage(lookup_name); QStandardItem* package_item; auto mi = package_items.find(package); @@ -72,13 +75,31 @@ void FactoryModel::fillTree(rviz::Factory *factory) { package_item = mi->second; } - QStandardItem* class_item = new QStandardItem(factory->getIcon(lookup_name), - factory->getClassName(lookup_name)); - class_item->setWhatsThis(factory->getClassDescription(lookup_name)); - class_item->setData(lookup_name); + QStandardItem* class_item = new QStandardItem(factory.getIcon(lookup_name), + factory.getClassName(lookup_name)); + class_item->setWhatsThis(factory.getClassDescription(lookup_name)); + class_item->setData(lookup_name, Qt::UserRole); class_item->setDragEnabled(true); package_item->appendRow(class_item); } } +QStringList FactoryModel::mimeTypes() const +{ + return { mime_type_ }; +} + +QMimeData *FactoryModel::mimeData(const QModelIndexList &indexes) const +{ + QSet rows_considered; + QMimeData* mime_data = new QMimeData(); + for (const auto &index : indexes) { + if (rows_considered.contains(index.row())) + continue; + // mime data is lookup_name + mime_data->setData(mime_type_, index.data(Qt::UserRole).toByteArray()); + } + return mime_data; +} + } diff --git a/visualization/motion_planning_tasks/factory_model.h b/visualization/motion_planning_tasks/factory_model.h index a7b2de07..2456c0df 100644 --- a/visualization/motion_planning_tasks/factory_model.h +++ b/visualization/motion_planning_tasks/factory_model.h @@ -43,10 +43,14 @@ namespace moveit_rviz_plugin { class FactoryModel : public QStandardItemModel { - void fillTree(rviz::Factory *factory); + QString mime_type_; + void fillTree(rviz::Factory& factory); public: - FactoryModel(rviz::Factory *factory, QObject *parent = nullptr); + FactoryModel(rviz::Factory& factory, const QString &mime_type, QObject *parent = nullptr); + + QStringList mimeTypes() const override; + QMimeData* mimeData(const QModelIndexList &indexes) const override; }; } diff --git a/visualization/motion_planning_tasks/local_task_model.cpp b/visualization/motion_planning_tasks/local_task_model.cpp index 7c084077..3285da51 100644 --- a/visualization/motion_planning_tasks/local_task_model.cpp +++ b/visualization/motion_planning_tasks/local_task_model.cpp @@ -35,10 +35,13 @@ /* Author: Robert Haschke */ #include "local_task_model.h" +#include "factory_model.h" #include #include +#include + using namespace moveit::task_constructor; namespace moveit_rviz_plugin { @@ -79,6 +82,13 @@ LocalTaskModel::LocalTaskModel(QObject *parent) root_ = pimpl(); } +LocalTaskModel::LocalTaskModel(Stage::pointer &&container, QObject *parent) + : BaseTaskModel(parent) + , Task(std::move(container)) +{ + root_ = pimpl(); +} + int LocalTaskModel::rowCount(const QModelIndex &parent) const { if (parent.column() > 0) @@ -120,7 +130,9 @@ Qt::ItemFlags LocalTaskModel::flags(const QModelIndex &index) const { Qt::ItemFlags flags = BaseTaskModel::flags(index); ContainerBasePrivate *c = dynamic_cast(node(index)); - if (c) flags |= Qt::ItemIsDropEnabled; + // dropping into containers is enabled + if (c && stage_factory_) + flags |= Qt::ItemIsDropEnabled; return flags; } @@ -157,4 +169,34 @@ bool LocalTaskModel::setData(const QModelIndex &index, const QVariant &value, in return true; } +void LocalTaskModel::setStageFactory(const StageFactoryPtr &factory) +{ + stage_factory_ = factory; +} + +bool LocalTaskModel::dropMimeData(const QMimeData *mime, Qt::DropAction action, int row, int column, const QModelIndex &parent) +{ + Q_UNUSED(column); + + if (!stage_factory_ || (flags_ & IS_RUNNING)) + return false; + const QString mime_type = stage_factory_->mimeType(); + if (!mime->hasFormat(mime_type)) + return false; + + ContainerBasePrivate *c = dynamic_cast(node(parent)); + Q_ASSERT(c); + + QString error; + moveit::task_constructor::Stage* stage + = stage_factory_->makeRaw(mime->data(mime_type), &error); + if (!stage) + return false; + + beginInsertRows(parent, row, row); + static_cast(c->me())->insert(moveit::task_constructor::Stage::pointer(stage), row); + endInsertRows(); + return true; +} + } diff --git a/visualization/motion_planning_tasks/local_task_model.h b/visualization/motion_planning_tasks/local_task_model.h index 3fa82b98..64080ada 100644 --- a/visualization/motion_planning_tasks/local_task_model.h +++ b/visualization/motion_planning_tasks/local_task_model.h @@ -48,12 +48,14 @@ class LocalTaskModel Q_OBJECT typedef moveit::task_constructor::StagePrivate Node; Node *root_; + StageFactoryPtr stage_factory_; inline Node* node(const QModelIndex &index) const; QModelIndex index(Node *n) const; public: LocalTaskModel(QObject *parent = nullptr); + LocalTaskModel(ContainerBase::pointer &&container, QObject *parent = nullptr); int rowCount(const QModelIndex &parent = QModelIndex()) const override; QModelIndex index(int row, int column, const QModelIndex &parent = QModelIndex()) const override; @@ -62,6 +64,10 @@ public: Qt::ItemFlags flags(const QModelIndex & index) const override; QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const override; bool setData(const QModelIndex &index, const QVariant &value, int role = Qt::EditRole) override; + + /// providing a StageFactory makes the model accepting drops + void setStageFactory(const StageFactoryPtr &factory); + bool dropMimeData(const QMimeData *data, Qt::DropAction action, int row, int column, const QModelIndex &parent) override; }; } diff --git a/visualization/motion_planning_tasks/pluginlib_factory.h b/visualization/motion_planning_tasks/pluginlib_factory.h index 2a1d35b5..014da083 100644 --- a/visualization/motion_planning_tasks/pluginlib_factory.h +++ b/visualization/motion_planning_tasks/pluginlib_factory.h @@ -38,10 +38,11 @@ #ifndef Q_MOC_RUN #include +#include +#include #endif #include -#include namespace moveit_rviz_plugin { @@ -55,11 +56,12 @@ private: QString package_; QString name_; QString description_; - Type*(*factory_function_)(); + std::function factory_function_; }; public: PluginlibFactory( const QString& package, const QString& base_class_type ) + : mime_type_(QString("application/%1/%2").arg(package, base_class_type)) { class_loader_ = new pluginlib::ClassLoader( package.toStdString(), base_class_type.toStdString() ); } @@ -68,56 +70,58 @@ public: delete class_loader_; } + /// retrieve mime type used for given factory + QString mimeType() const { + return mime_type_; + } + virtual QStringList getDeclaredClassIds() { QStringList ids; - std::vector std_ids = class_loader_->getDeclaredClasses(); - for( size_t i = 0; i < std_ids.size(); i++ ) - { - ids.push_back( QString::fromStdString( std_ids[ i ])); - } - typename QHash::const_iterator iter; - for( iter = built_ins_.begin(); iter != built_ins_.end(); iter++ ) - { - ids.push_back( iter.key() ); + for(const auto& record : built_ins_) + ids.push_back(record.class_id_); + for (const auto& id : class_loader_->getDeclaredClasses()) { + QString sid = QString::fromStdString(id); + if (ids.contains(sid)) continue; // built_in take precedence + ids.push_back(sid); } return ids; } virtual QString getClassDescription( const QString& class_id ) const { - typename QHash::const_iterator iter = built_ins_.find( class_id ); - if( iter != built_ins_.end() ) + auto it = built_ins_.find( class_id ); + if( it != built_ins_.end() ) { - return iter->description_; + return it->description_; } return QString::fromStdString( class_loader_->getClassDescription( class_id.toStdString() )); } virtual QString getClassName( const QString& class_id ) const { - typename QHash::const_iterator iter = built_ins_.find( class_id ); - if( iter != built_ins_.end() ) + auto it = built_ins_.find( class_id ); + if( it != built_ins_.end() ) { - return iter->name_; + return it->name_; } return QString::fromStdString( class_loader_->getName( class_id.toStdString() )); } virtual QString getClassPackage( const QString& class_id ) const { - typename QHash::const_iterator iter = built_ins_.find( class_id ); - if( iter != built_ins_.end() ) + auto it = built_ins_.find( class_id ); + if( it != built_ins_.end() ) { - return iter->package_; + return it->package_; } return QString::fromStdString( class_loader_->getClassPackage( class_id.toStdString() )); } virtual QString getPluginManifestPath( const QString& class_id ) const { - typename QHash::const_iterator iter = built_ins_.find( class_id ); - if( iter != built_ins_.end() ) + auto it = built_ins_.find( class_id ); + if( it != built_ins_.end() ) { return ""; } @@ -140,8 +144,8 @@ public: return icon; } - virtual void addBuiltInClass( const QString& package, const QString& name, const QString& description, - Type* (*factory_function)() ) + void addBuiltInClass(const QString& package, const QString& name, const QString& description, + const std::function& factory_function) { BuiltInClassRecord record; record.class_id_ = package + "/" + name; @@ -151,8 +155,12 @@ public: record.factory_function_ = factory_function; built_ins_[ record.class_id_ ] = record; } + template + void addBuiltInClass(const QString& name, const QString& description) + { + addBuiltInClass(ROS_PACKAGE_NAME, name, description, [](){return new Derived();}); + } -protected: /** @brief Instantiate and return a instance of a subclass of Type using our * pluginlib::ClassLoader. * @param class_id A string identifying the class uniquely among @@ -193,6 +201,7 @@ protected: } private: + const QString mime_type_; pluginlib::ClassLoader* class_loader_; QHash built_ins_; }; diff --git a/visualization/motion_planning_tasks/task_list_model.cpp b/visualization/motion_planning_tasks/task_list_model.cpp index 4300f27e..c4c22617 100644 --- a/visualization/motion_planning_tasks/task_list_model.cpp +++ b/visualization/motion_planning_tasks/task_list_model.cpp @@ -37,8 +37,10 @@ #include "task_list_model.h" #include "local_task_model.h" #include "remote_task_model.h" +#include "factory_model.h" #include +#include namespace moveit_rviz_plugin { @@ -70,6 +72,21 @@ Qt::ItemFlags BaseTaskModel::flags(const QModelIndex &index) const } +StageFactoryPtr getStageFactory() +{ + static std::weak_ptr factory; + if (!factory.expired()) + return factory.lock(); + + StageFactoryPtr result(new StageFactory("moveit_task_constructor", + "moveit::task_constructor::Stage")); + // Hm. pluglinlib / ClassLoader cannot instantiate classes in implicitly loaded libs + result->addBuiltInClass("Serial Container", ""); + factory = result; // remember for future uses + return result; +} + + class TaskListModelPrivate { public: Q_DECLARE_PUBLIC(TaskListModel) @@ -91,6 +108,9 @@ public: // if task is removed locally from tasks vector, it is marked with a nullptr std::map remote_tasks_; + // factory used to create stages + StageFactoryPtr stage_factory_; + public: TaskListModelPrivate(TaskListModel* q_ptr) : q_ptr(q_ptr) {} @@ -217,13 +237,33 @@ Qt::ItemFlags TaskListModel::flags(const QModelIndex &index) const { Q_D(const TaskListModel); - if (!index.isValid()) - return QAbstractItemModel::flags(index); + if (!index.isValid()) { + Qt::ItemFlags f = QAbstractItemModel::flags(index); + // dropping at root will create a new task + if (d->stage_factory_) + f |= Qt::ItemIsDropEnabled; + return f; + } QModelIndex src_index = d->mapToSource(index); return src_index.model()->flags(src_index); } +void TaskListModel::setStageFactory(const StageFactoryPtr &factory) +{ + Q_D(TaskListModel); + d->stage_factory_ = factory; +} + +QStringList TaskListModel::mimeTypes() const +{ + Q_D(const TaskListModel); + QStringList result; + if (d->stage_factory_) + result << d->stage_factory_->mimeType(); + return result; +} + QVariant TaskListModel::headerData(int section, Qt::Orientation orientation, int role) const { if (orientation == Qt::Horizontal && role == Qt::DisplayRole) @@ -309,6 +349,9 @@ void TaskListModel::insertTask(BaseTaskModel* model, int row) auto it = d->tasks_.begin(); std::advance(it, row); + if (LocalTaskModel* lm = dynamic_cast(model)) + lm->setStageFactory(d->stage_factory_); + ROS_DEBUG_NAMED(LOGNAME, "%p: inserting task: %p", this, model); beginInsertRows(QModelIndex(), row, row); d->tasks_.insert(it, TaskListModelPrivate::BaseModelData(model)); @@ -374,6 +417,43 @@ bool TaskListModel::removeRows(int row, int count, const QModelIndex &parent) } } +bool TaskListModel::dropMimeData(const QMimeData *mime, Qt::DropAction action, int row, int column, const QModelIndex &parent) +{ + Q_UNUSED(column); + Q_D(TaskListModel); + if (!d->stage_factory_) + return false; + const QString& mime_type = d->stage_factory_->mimeType(); + if (!mime->hasFormat(mime_type)) + return false; + + if (!parent.isValid() && mime->hasFormat(mime_type)) { + QString error; + moveit::task_constructor::Stage* stage + = d->stage_factory_->makeRaw(mime->data(mime_type), &error); + std::unique_ptr container + (dynamic_cast(stage)); + if (!container) { // only accept container at root level + if (stage) delete stage; + return false; + } + + // create a new local task using the given container as root + insertTask(new LocalTaskModel(std::move(container), this), row); + return true; + } + + // propagate to corresponding child model + TaskListModelPrivate::BaseModelData *data = nullptr; + QModelIndex src_parent = d->mapToSource(parent, &data); + return data->model_->dropMimeData(mime, action, row, column, src_parent); +} + +Qt::DropActions TaskListModel::supportedDragActions() const +{ + return Qt::CopyAction | Qt::MoveAction; +} + void TaskListModelPrivate::removeTask(BaseTaskModel *model) { ROS_DEBUG_NAMED(LOGNAME, "%p: removing task: %p", q_ptr, model); diff --git a/visualization/motion_planning_tasks/task_list_model.h b/visualization/motion_planning_tasks/task_list_model.h index b127cae4..08dda639 100644 --- a/visualization/motion_planning_tasks/task_list_model.h +++ b/visualization/motion_planning_tasks/task_list_model.h @@ -36,6 +36,9 @@ #pragma once +#include "pluginlib_factory.h" +#include + #include #include #include @@ -66,6 +69,12 @@ public: unsigned int taskFlags() const { return flags_; } }; + +typedef PluginlibFactory StageFactory; +typedef std::shared_ptr StageFactoryPtr; +StageFactoryPtr getStageFactory(); + + class TaskListModelPrivate; /** The TaskListModel maintains a list of multiple BaseTaskModels, local and/or remote. * @@ -112,6 +121,12 @@ public: bool removeTask(BaseTaskModel* model); bool removeTasks(int row, int count); + /// providing a StageFactory makes the model accepting drops + void setStageFactory(const StageFactoryPtr &factory); + QStringList mimeTypes() const override; + bool dropMimeData(const QMimeData *mime, Qt::DropAction action, int row, int column, const QModelIndex &parent) override; + Qt::DropActions supportedDragActions() const override; + private: Q_PRIVATE_SLOT(d_func(), void _q_sourceRowsAboutToBeInserted(QModelIndex,int,int)) Q_PRIVATE_SLOT(d_func(), void _q_sourceRowsInserted(QModelIndex,int,int)) diff --git a/visualization/motion_planning_tasks/task_panel.cpp b/visualization/motion_planning_tasks/task_panel.cpp index 2be19fe9..fc9484f5 100644 --- a/visualization/motion_planning_tasks/task_panel.cpp +++ b/visualization/motion_planning_tasks/task_panel.cpp @@ -50,22 +50,16 @@ #include #include #include +#include namespace moveit_rviz_plugin { -typedef PluginlibFactory StageFactory; -StageFactory* getStageFactory() { - static std::shared_ptr factory - (new StageFactory("moveit_task_constructor", - "moveit::task_constructor::Stage")); - return factory.get(); -} - rviz::PanelDockWidget* getStageDockWidget(rviz::WindowManagerInterface* mgr) { - static rviz::PanelDockWidget *widget = nullptr; + static QPointer widget = nullptr; if (!widget && mgr) { // create widget - QTreeView *view = new QTreeView(mgr->getParentWindow()); - view->setModel(new FactoryModel(getStageFactory(), view)); + QTreeView *view = new QTreeView(); + StageFactoryPtr factory = getStageFactory(); + view->setModel(new FactoryModel(*factory, factory->mimeType(), view)); view->expandAll(); view->setHeaderHidden(true); view->setDragDropMode(QAbstractItemView::DragOnly); @@ -82,6 +76,7 @@ TaskPanel::TaskPanel(QWidget* parent) Q_D(TaskPanel); // connect signals connect(d->actionRemoveTaskTreeRows, &QAction::triggered, this, &TaskPanel::removeTaskTreeRows); + connect(d->actionAddLocalTask, &QAction::triggered, this, &TaskPanel::addTask); connect(d->button_show_stage_dock_widget, &QToolButton::clicked, this, &TaskPanel::showStageDockWidget); } @@ -96,12 +91,21 @@ TaskPanelPrivate::TaskPanelPrivate(TaskPanel *q_ptr) , settings(new rviz::PropertyTreeModel(new rviz::Property, q_ptr)) { setupUi(q_ptr); - initSettings(settings->getRoot()); - settings_view->setModel(settings); + // init tasks view + task_list_model->setStageFactory(getStageFactory()); tasks_view->setModel(task_list_model.get()); + tasks_view->setSelectionMode(QAbstractItemView::ExtendedSelection); + tasks_view->setAcceptDrops(true); + tasks_view->setDefaultDropAction(Qt::CopyAction); + tasks_view->setDropIndicatorShown(true); + tasks_view->setDragEnabled(true); + // init actions - tasks_view->addActions({actionRemoveTaskTreeRows}); + tasks_view->addActions({actionRemoveTaskTreeRows, actionAddLocalTask}); + + initSettings(settings->getRoot()); + settings_view->setModel(settings); } void TaskPanelPrivate::initSettings(rviz::Property* root) @@ -124,11 +128,11 @@ void TaskPanel::load(const rviz::Config& config) d_ptr->settings->getRoot()->load(config); } -void TaskPanel::onAddTask() +void TaskPanel::addTask() { Q_D(TaskPanel); QModelIndex current = d->tasks_view->currentIndex(); - d_ptr->task_list_model->insertTask(new LocalTaskModel(this), current.row()); + d_ptr->task_list_model->insertTask(new LocalTaskModel(d_ptr->task_list_model.get()), current.row()); } void TaskPanel::showStageDockWidget() diff --git a/visualization/motion_planning_tasks/task_panel.h b/visualization/motion_planning_tasks/task_panel.h index 5eb5a830..842c81bc 100644 --- a/visualization/motion_planning_tasks/task_panel.h +++ b/visualization/motion_planning_tasks/task_panel.h @@ -69,7 +69,7 @@ public: void save(rviz::Config config) const override; public Q_SLOTS: - void onAddTask(); + void addTask(); void showStageDockWidget(); void removeTaskTreeRows(); }; diff --git a/visualization/motion_planning_tasks/task_panel.ui b/visualization/motion_planning_tasks/task_panel.ui index efaadcf4..d1b79bad 100644 --- a/visualization/motion_planning_tasks/task_panel.ui +++ b/visualization/motion_planning_tasks/task_panel.ui @@ -80,6 +80,11 @@ Qt::WidgetShortcut + + + Add task + +