From 67b5a99d8da944616504d8677f1ed22f500a3c66 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 12 Nov 2017 17:35:39 +0100 Subject: [PATCH] receive task solutions - remove parent_id from StageStatistics message --- core/src/introspection.cpp | 7 +- msgs/msg/StageStatistics.msg | 3 - msgs/msg/TaskDescription.msg | 1 - .../src/remote_task_model.cpp | 65 ++++++++++++++++++- .../src/remote_task_model.h | 20 ++++++ .../test/test_task_model.cpp | 4 +- .../visualization_tools/display_solution.h | 2 + 7 files changed, 87 insertions(+), 15 deletions(-) diff --git a/core/src/introspection.cpp b/core/src/introspection.cpp index b823a1a3..df1b99ec 100644 --- a/core/src/introspection.cpp +++ b/core/src/introspection.cpp @@ -177,7 +177,7 @@ moveit_task_constructor_msgs::TaskDescription& Introspection::fillTaskDescriptio auto it = impl->stage_to_id_map_.find(stage.pimpl()->parent()); assert (it != impl->stage_to_id_map_.cend()); - desc.parent_id = stat.parent_id = it->second; + desc.parent_id = it->second; fillStageStatistics(stage, stat); @@ -202,11 +202,6 @@ moveit_task_constructor_msgs::TaskStatistics& Introspection::fillTaskStatistics( // this method is called for each child stage of a given parent moveit_task_constructor_msgs::StageStatistics stat; // create new Stage msg stat.id = stageId(&stage); - - auto it = impl->stage_to_id_map_.find(stage.pimpl()->parent()); - assert (it != impl->stage_to_id_map_.cend()); - stat.parent_id = it->second; - fillStageStatistics(stage, stat); // finally store in msg.stages diff --git a/msgs/msg/StageStatistics.msg b/msgs/msg/StageStatistics.msg index a498365e..74eb93d3 100644 --- a/msgs/msg/StageStatistics.msg +++ b/msgs/msg/StageStatistics.msg @@ -3,9 +3,6 @@ # unique id within task uint32 id -# parent id, parent_id == id means root -uint32 parent_id - # successful and failed solution IDs of this stage uint32[] solved uint32[] failed diff --git a/msgs/msg/TaskDescription.msg b/msgs/msg/TaskDescription.msg index 32fb8ca7..457391eb 100644 --- a/msgs/msg/TaskDescription.msg +++ b/msgs/msg/TaskDescription.msg @@ -4,4 +4,3 @@ string id # list of all stages, including the task stage itself StageDescription[] description StageStatistics[] statistics - diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp index 621c712d..91b39030 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/src/remote_task_model.cpp @@ -60,8 +60,13 @@ struct RemoteTaskModel::Node { QString name_; InterfaceFlags interface_flags_; NodeFlags node_flags_; + std::unique_ptr solved_; + std::unique_ptr failed_; - inline Node(Node *parent) : parent_(parent) {} + inline Node(Node *parent) : parent_(parent) { + solved_.reset(new RemoteSolutionModel()); + failed_.reset(new RemoteSolutionModel()); + } bool setName(const QString& name) { if (name == name_) return false; @@ -170,8 +175,8 @@ QVariant RemoteTaskModel::data(const QModelIndex &index, int role) const case Qt::DisplayRole: switch (index.column()) { case 0: return n->name_; - case 1: return 0; - case 2: return 0; + case 1: return n->solved_->rowCount(); + case 2: return n->failed_->rowCount(); } break; case Qt::ForegroundRole: @@ -248,6 +253,60 @@ void RemoteTaskModel::processStageDescriptions(const moveit_task_constructor_msg void RemoteTaskModel::processStageStatistics(const moveit_task_constructor_msgs::TaskDescription::_statistics_type &msg) { + // iterate over statistics and update node's solutions where needed + for (const auto &s : msg) { + // find node for stage s, this should always exist + auto it = id_to_stage_.find(s.id); + if (it == id_to_stage_.end()) { + ROS_ERROR_NAMED("TaskListModel", "No stage %d", s.id); + continue; + } + Node *n = it->second; + + bool changed = n->solved_->processSolutionIDs(s.solved) || + n->failed_->processSolutionIDs(s.failed); + // emit notify about model changes when node was already visited + if (changed && (n->node_flags_ & WAS_VISITED)) { + QModelIndex idx = index(n); + dataChanged(idx.sibling(idx.row(), 1), idx.sibling(idx.row(), 2)); + } + } +} + + +RemoteSolutionModel::RemoteSolutionModel(QObject *parent) + : QAbstractListModel(parent) +{ +} + +int RemoteSolutionModel::rowCount(const QModelIndex &parent) const +{ + return ids_.size(); +} + +QVariant RemoteSolutionModel::data(const QModelIndex &index, int role) const +{ + Q_ASSERT(index.isValid()); + Q_ASSERT(!index.parent().isValid()); + + if (role == Qt::DisplayRole) + return index.row(); + return QVariant(); +} + +bool RemoteSolutionModel::processSolutionIDs(const std::vector &ids) +{ + if (ids_ == ids) + return false; + + bool size_changed = (ids_.size() != ids.size()); + QAbstractItemModel::LayoutChangeHint hint = size_changed ? QAbstractItemModel::NoLayoutChangeHint + : QAbstractItemModel::VerticalSortHint; + layoutAboutToBeChanged({QModelIndex()}, hint); + ids_ = ids; + layoutChanged({QModelIndex()}, hint); + + return size_changed; } } diff --git a/visualization/motion_planning_tasks/src/remote_task_model.h b/visualization/motion_planning_tasks/src/remote_task_model.h index 2d013e11..0f1f4851 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.h +++ b/visualization/motion_planning_tasks/src/remote_task_model.h @@ -37,15 +37,21 @@ #pragma once #include "task_list_model.h" +#include #include namespace moveit_rviz_plugin { +/** Model representing a remote task + * + * filled via TaskDescription + TaskStatistics messages + */ class RemoteTaskModel : public BaseTaskModel { Q_OBJECT class Node; Node* const root_; std::map id_to_stage_; + std::map id_to_solution_; inline Node* node(const QModelIndex &index) const; QModelIndex index(const Node* n) const; @@ -66,4 +72,18 @@ public: void processStageStatistics(const moveit_task_constructor_msgs::TaskDescription::_statistics_type &msg); }; + +/** Model representing solutions of a remote task */ +class RemoteSolutionModel : public QAbstractListModel { + Q_OBJECT + std::vector ids_; + +public: + RemoteSolutionModel(QObject *parent = nullptr); + int rowCount(const QModelIndex &parent = QModelIndex()) const override; + QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const override; + + bool processSolutionIDs(const std::vector& ids); +}; + } diff --git a/visualization/motion_planning_tasks/test/test_task_model.cpp b/visualization/motion_planning_tasks/test/test_task_model.cpp index 060f7e12..f422664d 100644 --- a/visualization/motion_planning_tasks/test/test_task_model.cpp +++ b/visualization/motion_planning_tasks/test/test_task_model.cpp @@ -61,14 +61,14 @@ protected: moveit_task_constructor_msgs::StageDescription desc; moveit_task_constructor_msgs::StageStatistics stat; - desc.parent_id = stat.parent_id = id; + desc.parent_id = id; desc.id = stat.id = root_id = ++id; desc.name = name; t.description.push_back(desc); t.statistics.push_back(stat); for (int i = 0; i != children; ++i) { - desc.parent_id = stat.parent_id = root_id; + desc.parent_id = root_id; desc.id = stat.id = ++id; desc.name = std::to_string(i); t.description.push_back(desc); diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h index 6cdf28b4..f912ea98 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h @@ -15,6 +15,8 @@ MOVEIT_CLASS_FORWARD(RobotTrajectory) namespace moveit_rviz_plugin { +MOVEIT_CLASS_FORWARD(DisplaySolution) + /** Class representing a task solution for display */ class DisplaySolution {