From 924051c51123cedd27f6715ea4b2e3f1020e6633 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 2 Nov 2017 23:43:44 +0100 Subject: [PATCH] handle IS_DESTROYED flag for RemoteTask - turn task red in models - if a task with same ID is received again, create a new RemoteTask anyway --- .../remote_task_model.cpp | 14 +++++++++- .../motion_planning_tasks/task_list_model.cpp | 26 +++++++------------ .../motion_planning_tasks/task_list_model.h | 10 +++++++ 3 files changed, 33 insertions(+), 17 deletions(-) diff --git a/visualization/motion_planning_tasks/remote_task_model.cpp b/visualization/motion_planning_tasks/remote_task_model.cpp index 9bf24582..440e44eb 100644 --- a/visualization/motion_planning_tasks/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/remote_task_model.cpp @@ -39,6 +39,9 @@ #include "remote_task_model.h" #include #include + +#include +#include #include using namespace moveit::task_constructor; @@ -170,6 +173,10 @@ QVariant RemoteTaskModel::data(const QModelIndex &index, int role) const case 2: return 0; } break; + case Qt::ForegroundRole: + if (index.column() == 0 && !index.parent().isValid()) + return (flags_ & IS_DESTROYED) ? QColor(Qt::red) : QApplication::palette().text().color(); + break; } return QVariant(); } @@ -224,9 +231,14 @@ void RemoteTaskModel::processTaskMessage(const moveit_task_constructor::Task &ms // emit notify about model changes when node was already visited if (changed && (n->node_flags_ & WAS_VISITED)) { QModelIndex idx = index(n); - dataChanged(idx, idx); + dataChanged(idx, idx.sibling(idx.row(), 2)); } } + + if (msg.stages.empty()) { + flags_ |= IS_DESTROYED; + dataChanged(index(0, 0), index(0, 2)); + } } } diff --git a/visualization/motion_planning_tasks/task_list_model.cpp b/visualization/motion_planning_tasks/task_list_model.cpp index eb5828ce..4300f27e 100644 --- a/visualization/motion_planning_tasks/task_list_model.cpp +++ b/visualization/motion_planning_tasks/task_list_model.cpp @@ -42,6 +42,8 @@ namespace moveit_rviz_plugin { +static const std::string LOGNAME("TaskListModel"); + QString TaskListModel::horizontalHeader(int column) { switch (column) { @@ -67,9 +69,6 @@ Qt::ItemFlags BaseTaskModel::flags(const QModelIndex &index) const return flags; } -enum TaskModelFlag { - IS_DESTROYED = 0x01, -}; class TaskListModelPrivate { public: @@ -82,7 +81,6 @@ public: BaseTaskModel* model_; // map of proxy=source QModelIndex's internal pointer to source QModelIndex QHash proxy_to_source_mapping_; - unsigned int flags_ = 0; }; // top-level items @@ -161,11 +159,11 @@ TaskListModel::TaskListModel(QObject *parent) : QAbstractItemModel(parent) { d_ptr = new TaskListModelPrivate(this); - ROS_DEBUG_NAMED("TaskListModel", "created TaskListModel: %p", this); + ROS_DEBUG_NAMED(LOGNAME, "created TaskListModel: %p", this); } TaskListModel::~TaskListModel() { - ROS_DEBUG_NAMED("TaskListModel", "destroying TaskListModel: %p", this); + ROS_DEBUG_NAMED(LOGNAME, "destroying TaskListModel: %p", this); delete d_ptr; } @@ -262,6 +260,9 @@ void TaskListModel::processTaskMessage(const moveit_task_constructor::Task &msg) bool created = it_inserted.second; RemoteTaskModel*& remote_task = it_inserted.first->second; + if (!msg.stages.empty() && remote_task && remote_task->taskFlags() & BaseTaskModel::IS_DESTROYED) + created = true; // re-create remote task after it was destroyed beforehand + // empty stages list indicates, that this remote task is not available anymore if (msg.stages.empty()) { if (!remote_task) { // task was already deleted locally @@ -269,13 +270,6 @@ void TaskListModel::processTaskMessage(const moveit_task_constructor::Task &msg) d->remote_tasks_.erase(it_inserted.first); return; } - // task is still in use, mark it as destroyed - for (auto& t : d->tasks_) { - if (t.model_ == remote_task) { - t.flags_ |= IS_DESTROYED; - break; - } - } } else if (created) { // create new task model, if ID was not known before // the model is managed by this instance via Qt's parent-child mechanism remote_task = new RemoteTaskModel(this); @@ -287,7 +281,7 @@ void TaskListModel::processTaskMessage(const moveit_task_constructor::Task &msg) // insert newly created model into this' model instance if (created) { - ROS_DEBUG_NAMED("TaskListModel", "received new Task: %s", msg.id.c_str()); + ROS_DEBUG_NAMED(LOGNAME, "received new task: %s", msg.id.c_str()); insertTask(remote_task, -1); } } @@ -315,7 +309,7 @@ void TaskListModel::insertTask(BaseTaskModel* model, int row) auto it = d->tasks_.begin(); std::advance(it, row); - ROS_DEBUG_NAMED("TaskListModel", "%p: inserting task: %p", this, model); + ROS_DEBUG_NAMED(LOGNAME, "%p: inserting task: %p", this, model); beginInsertRows(QModelIndex(), row, row); d->tasks_.insert(it, TaskListModelPrivate::BaseModelData(model)); endInsertRows(); @@ -382,7 +376,7 @@ bool TaskListModel::removeRows(int row, int count, const QModelIndex &parent) void TaskListModelPrivate::removeTask(BaseTaskModel *model) { - ROS_DEBUG_NAMED("TaskListModel", "%p: removing task: %p", q_ptr, model); + ROS_DEBUG_NAMED(LOGNAME, "%p: removing task: %p", q_ptr, model); QObject::disconnect(model, SIGNAL(rowsAboutToBeInserted(QModelIndex,int,int)), q_ptr, SLOT(_q_sourceRowsAboutToBeInserted(QModelIndex,int,int))); diff --git a/visualization/motion_planning_tasks/task_list_model.h b/visualization/motion_planning_tasks/task_list_model.h index ffd30161..b127cae4 100644 --- a/visualization/motion_planning_tasks/task_list_model.h +++ b/visualization/motion_planning_tasks/task_list_model.h @@ -47,13 +47,23 @@ namespace moveit_rviz_plugin { /** Base class to represent a single local or remote Task as a Qt model. */ class BaseTaskModel : public QAbstractItemModel { +protected: + unsigned int flags_ = 0; + public: + enum TaskModelFlag { + IS_DESTROYED = 0x01, + IS_INITIALIZED = 0x02, + IS_RUNNING = 0x04, + }; + BaseTaskModel(QObject *parent = nullptr) : QAbstractItemModel(parent) {} int columnCount(const QModelIndex &parent = QModelIndex()) const override { return 3; } QVariant headerData(int section, Qt::Orientation orientation, int role) const override; Qt::ItemFlags flags(const QModelIndex &index) const override; + unsigned int taskFlags() const { return flags_; } }; class TaskListModelPrivate;