From fa9b177575fab6126d6df1a715d5ad9a4694d274 Mon Sep 17 00:00:00 2001 From: Jafar Abdi Date: Thu, 6 Feb 2020 14:40:00 +0100 Subject: [PATCH] transmit and display computation in rviz --- core/src/introspection.cpp | 1 + msgs/msg/StageStatistics.msg | 2 ++ .../motion_planning_tasks/src/remote_task_model.cpp | 8 ++++++-- .../motion_planning_tasks/src/remote_task_model.h | 4 +++- .../motion_planning_tasks/src/task_list_model.cpp | 4 ++++ visualization/motion_planning_tasks/src/task_list_model.h | 4 ++-- .../motion_planning_tasks/test/test_solution_models.cpp | 2 +- .../motion_planning_tasks/test/test_task_model.cpp | 6 +++--- 8 files changed, 22 insertions(+), 9 deletions(-) diff --git a/core/src/introspection.cpp b/core/src/introspection.cpp index 0d5c2692..3dbe8c33 100644 --- a/core/src/introspection.cpp +++ b/core/src/introspection.cpp @@ -198,6 +198,7 @@ void Introspection::fillStageStatistics(const Stage& stage, moveit_task_construc for (const auto& solution : stage.failures()) s.failed.push_back(solutionId(*solution)); + s.total_compute_time = stage.getTotalComputeTime(); s.num_failed = stage.numFailures(); } diff --git a/msgs/msg/StageStatistics.msg b/msgs/msg/StageStatistics.msg index 855d3b83..d5048229 100644 --- a/msgs/msg/StageStatistics.msg +++ b/msgs/msg/StageStatistics.msg @@ -10,3 +10,5 @@ uint32[] solved uint32[] failed # number of failed solutions (if failed is empty) uint32 num_failed +# total computation time in seconds +float64 total_compute_time diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp index cebe5205..81d897b6 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/src/remote_task_model.cpp @@ -254,6 +254,8 @@ QVariant RemoteTaskModel::data(const QModelIndex& index, int role) const { return n->solutions_->numSuccessful(); case 2: return n->solutions_->numFailed(); + case 3: + return n->solutions_->totalComputeTime(); } break; case Qt::ForegroundRole: @@ -353,7 +355,7 @@ void RemoteTaskModel::processStageStatistics(const moveit_task_constructor_msgs: continue; } Node* n = it->second; - n->solutions_->processSolutionIDs(s.solved, s.failed, s.num_failed); + n->solutions_->processSolutionIDs(s.solved, s.failed, s.num_failed, s.total_compute_time); // emit notify about model changes when node was already visited if (n->node_flags_ & WAS_VISITED) { @@ -622,7 +624,8 @@ void RemoteSolutionModel::sortInternal() { // process solution ids received in stage statistics void RemoteSolutionModel::processSolutionIDs(const std::vector& successful, - const std::vector& failed, size_t num_failed) { + const std::vector& failed, size_t num_failed, + double total_compute_time) { // append new items to the end of data_ processSolutionIDs(successful, true); processSolutionIDs(failed, false); @@ -636,6 +639,7 @@ void RemoteSolutionModel::processSolutionIDs(const std::vector& succes // but it may report the overall number of failures num_failed_data_ = failed.size(); // needed to compute number of successes num_failed_ = std::max(num_failed, num_failed_data_); + total_compute_time_ = total_compute_time; sortInternal(); } diff --git a/visualization/motion_planning_tasks/src/remote_task_model.h b/visualization/motion_planning_tasks/src/remote_task_model.h index 0cfa9ff8..e8b17f04 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.h +++ b/visualization/motion_planning_tasks/src/remote_task_model.h @@ -118,6 +118,7 @@ class RemoteSolutionModel : public QAbstractTableModel DataList data_; size_t num_failed_data_ = 0; // number of failed solutions in data_ size_t num_failed_ = 0; // number of reported failures + double total_compute_time_ = 0.0; // solutions ordered (by default according to cost) int sort_column_ = -1; @@ -134,6 +135,7 @@ public: uint numSuccessful() const { return data_.size() - num_failed_data_; } uint numFailed() const { return num_failed_; } + double totalComputeTime() const { return total_compute_time_; } int rowCount(const QModelIndex& parent = QModelIndex()) const override; int columnCount(const QModelIndex& parent = QModelIndex()) const override; @@ -143,6 +145,6 @@ public: void setSolutionData(uint32_t id, float cost, const QString& comment); void processSolutionIDs(const std::vector& successful, const std::vector& failed, - size_t num_failed); + size_t num_failed, double total_compute_time); }; } diff --git a/visualization/motion_planning_tasks/src/task_list_model.cpp b/visualization/motion_planning_tasks/src/task_list_model.cpp index 51a6d13a..c505e315 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.cpp +++ b/visualization/motion_planning_tasks/src/task_list_model.cpp @@ -65,6 +65,8 @@ QVariant TaskListModel::horizontalHeader(int column, int role) { return tr(u8"✓"); case 2: return tr(u8"✗"); + case 3: + return tr("time"); } break; @@ -87,6 +89,8 @@ QVariant TaskListModel::horizontalHeader(int column, int role) { case 2: return tr("failed solution attempts"); case 3: + return tr("total computation time [s]"); + case 4: return tr("pending"); } break; diff --git a/visualization/motion_planning_tasks/src/task_list_model.h b/visualization/motion_planning_tasks/src/task_list_model.h index f9975bba..ec266e55 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.h +++ b/visualization/motion_planning_tasks/src/task_list_model.h @@ -89,7 +89,7 @@ public: QObject* parent = nullptr) : QAbstractItemModel(parent), scene_(scene), display_context_(display_context) {} - int columnCount(const QModelIndex& parent = QModelIndex()) const override { return 3; } + int columnCount(const QModelIndex& parent = QModelIndex()) const override { return 4; } QVariant headerData(int section, Qt::Orientation orientation, int role) const override; QVariant data(const QModelIndex& index, int role) const override; @@ -150,7 +150,7 @@ public: void setSolutionClient(ros::ServiceClient* client); void setActiveTaskModel(BaseTaskModel* model) { active_task_model_ = model; } - int columnCount(const QModelIndex& parent = QModelIndex()) const override { return 3; } + int columnCount(const QModelIndex& parent = QModelIndex()) const override { return 4; } static QVariant horizontalHeader(int column, int role); QVariant headerData(int section, Qt::Orientation orientation, int role) const override; QVariant data(const QModelIndex& index, int role) const override; diff --git a/visualization/motion_planning_tasks/test/test_solution_models.cpp b/visualization/motion_planning_tasks/test/test_solution_models.cpp index 63f31045..82cec602 100644 --- a/visualization/motion_planning_tasks/test/test_solution_models.cpp +++ b/visualization/motion_planning_tasks/test/test_solution_models.cpp @@ -25,7 +25,7 @@ protected: } void processAndValidate(RemoteSolutionModel& model, const std::vector& success_ids, const std::vector& failure_ids) { - model.processSolutionIDs(success_ids, failure_ids, failure_ids.size()); + model.processSolutionIDs(success_ids, failure_ids, failure_ids.size(), 0.0); std::vector cost_ordered_ids(success_ids.begin(), success_ids.end()); std::vector sorted_failure_ids(failure_ids.begin(), failure_ids.end()); diff --git a/visualization/motion_planning_tasks/test/test_task_model.cpp b/visualization/motion_planning_tasks/test/test_task_model.cpp index 4a99a2db..92957360 100644 --- a/visualization/motion_planning_tasks/test/test_task_model.cpp +++ b/visualization/motion_planning_tasks/test/test_task_model.cpp @@ -78,7 +78,7 @@ protected: void validate(QAbstractItemModel& model, const std::initializer_list& expected) { // validate root index ASSERT_EQ(model.rowCount(), static_cast(expected.size())); - EXPECT_EQ(model.columnCount(), 3); + EXPECT_EQ(model.columnCount(), 4); EXPECT_EQ(model.parent(QModelIndex()), QModelIndex()); // validate first-level items @@ -97,7 +97,7 @@ protected: // validate children ASSERT_EQ(model.rowCount(idx), children); - EXPECT_EQ(model.columnCount(idx), 3); + EXPECT_EQ(model.columnCount(idx), 4); EXPECT_EQ(model.rowCount(model.index(row, 1)), 0); // validate second-level items @@ -110,7 +110,7 @@ protected: EXPECT_EQ(model.data(childIdx).toString().toStdString(), std::to_string(child)); EXPECT_EQ(model.rowCount(childIdx), 0); - EXPECT_EQ(model.columnCount(childIdx), 3); + EXPECT_EQ(model.columnCount(childIdx), 4); } } }