Merge #120: Add computation timing

This commit is contained in:
Robert Haschke 2020-02-06 16:18:48 +01:00
commit 463486257b
13 changed files with 46 additions and 18 deletions

View File

@ -210,6 +210,7 @@ public:
inline void setProperty(const std::string& name, const char* value) { setProperty(name, std::string(value)); } inline void setProperty(const std::string& name, const char* value) { setProperty(name, std::string(value)); }
/// analyze source of error and report accordingly /// analyze source of error and report accordingly
void reportPropertyError(const Property::error& e); void reportPropertyError(const Property::error& e);
double getTotalComputeTime() const;
protected: protected:
/// Stage can only be instantiated through derived classes /// Stage can only be instantiated through derived classes

View File

@ -42,6 +42,7 @@
#include <moveit/task_constructor/storage.h> #include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/cost_queue.h> #include <moveit/task_constructor/cost_queue.h>
#include <ostream> #include <ostream>
#include <chrono>
// define pimpl() functions accessing correctly casted pimpl_ pointer // define pimpl() functions accessing correctly casted pimpl_ pointer
#define PIMPL_FUNCTIONS(Class) \ #define PIMPL_FUNCTIONS(Class) \
@ -131,6 +132,12 @@ public:
bool storeSolution(const SolutionBasePtr& solution); bool storeSolution(const SolutionBasePtr& solution);
void newSolution(const SolutionBasePtr& solution); void newSolution(const SolutionBasePtr& solution);
bool storeFailures() const { return introspection_ != nullptr; } bool storeFailures() const { return introspection_ != nullptr; }
void runCompute() {
auto compute_start_time = std::chrono::steady_clock::now();
compute();
auto compute_stop_time = std::chrono::steady_clock::now();
total_compute_time_ += compute_stop_time - compute_start_time;
}
protected: protected:
Stage* me_; // associated/owning Stage instance Stage* me_; // associated/owning Stage instance
@ -140,6 +147,9 @@ protected:
InterfacePtr starts_; InterfacePtr starts_;
InterfacePtr ends_; InterfacePtr ends_;
// The total compute time
std::chrono::duration<double> total_compute_time_;
// functions called for each new solution // functions called for each new solution
std::list<Stage::SolutionCallback> solution_cbs_; std::list<Stage::SolutionCallback> solution_cbs_;
@ -281,5 +291,5 @@ private:
ordered<StatePair, StatePairLess> pending; ordered<StatePair, StatePairLess> pending;
}; };
PIMPL_FUNCTIONS(Connecting) PIMPL_FUNCTIONS(Connecting)
} } // namespace task_constructor
} } // namespace moveit

View File

@ -667,7 +667,7 @@ void SerialContainer::compute() {
continue; continue;
ROS_DEBUG("Computing stage '%s'", stage->name().c_str()); ROS_DEBUG("Computing stage '%s'", stage->name().c_str());
stage->pimpl()->compute(); stage->pimpl()->runCompute();
} catch (const Property::error& e) { } catch (const Property::error& e) {
stage->reportPropertyError(e); stage->reportPropertyError(e);
} }
@ -861,7 +861,7 @@ bool WrapperBase::canCompute() const {
void WrapperBase::compute() { void WrapperBase::compute() {
try { try {
wrapped()->pimpl()->compute(); wrapped()->pimpl()->runCompute();
} catch (const Property::error& e) { } catch (const Property::error& e) {
wrapped()->reportPropertyError(e); wrapped()->reportPropertyError(e);
} }
@ -877,7 +877,7 @@ bool Alternatives::canCompute() const {
void Alternatives::compute() { void Alternatives::compute() {
for (const auto& stage : pimpl()->children()) { for (const auto& stage : pimpl()->children()) {
try { try {
stage->pimpl()->compute(); stage->pimpl()->runCompute();
} catch (const Property::error& e) { } catch (const Property::error& e) {
stage->reportPropertyError(e); stage->reportPropertyError(e);
} }
@ -917,7 +917,7 @@ void Fallbacks::compute() {
return; return;
try { try {
active_child_->pimpl()->compute(); active_child_->pimpl()->runCompute();
} catch (const Property::error& e) { } catch (const Property::error& e) {
active_child_->reportPropertyError(e); active_child_->reportPropertyError(e);
} }
@ -982,7 +982,7 @@ bool Merger::canCompute() const {
void Merger::compute() { void Merger::compute() {
for (const auto& stage : pimpl()->children()) { for (const auto& stage : pimpl()->children()) {
try { try {
stage->pimpl()->compute(); stage->pimpl()->runCompute();
} catch (const Property::error& e) { } catch (const Property::error& e) {
stage->reportPropertyError(e); stage->reportPropertyError(e);
} }

View File

@ -198,6 +198,7 @@ void Introspection::fillStageStatistics(const Stage& stage, moveit_task_construc
for (const auto& solution : stage.failures()) for (const auto& solution : stage.failures())
s.failed.push_back(solutionId(*solution)); s.failed.push_back(solutionId(*solution));
s.total_compute_time = stage.getTotalComputeTime();
s.num_failed = stage.numFailures(); s.num_failed = stage.numFailures();
} }

View File

@ -69,7 +69,7 @@ std::ostream& operator<<(std::ostream& os, const InitStageException& e) {
} }
StagePrivate::StagePrivate(Stage* me, const std::string& name) StagePrivate::StagePrivate(Stage* me, const std::string& name)
: me_(me), name_(name), parent_(nullptr), introspection_(nullptr) {} : me_(me), name_(name), parent_(nullptr), total_compute_time_{}, introspection_(nullptr) {}
InterfaceFlags StagePrivate::interfaceFlags() const { InterfaceFlags StagePrivate::interfaceFlags() const {
InterfaceFlags f; InterfaceFlags f;
@ -298,6 +298,10 @@ void Stage::setProperty(const std::string& name, const boost::any& value) {
pimpl()->properties_.set(name, value); pimpl()->properties_.set(name, value);
} }
double Stage::getTotalComputeTime() const {
return pimpl()->total_compute_time_.count();
}
void StagePrivate::composePropertyErrorMsg(const std::string& property_name, std::ostream& os) { void StagePrivate::composePropertyErrorMsg(const std::string& property_name, std::ostream& os) {
if (property_name.empty()) if (property_name.empty())
return; return;

View File

@ -278,7 +278,7 @@ bool Task::canCompute() const {
} }
void Task::compute() { void Task::compute() {
stages()->compute(); stages()->pimpl()->runCompute();
} }
bool Task::plan(size_t max_solutions) { bool Task::plan(size_t max_solutions) {

View File

@ -10,3 +10,5 @@ uint32[] solved
uint32[] failed uint32[] failed
# number of failed solutions (if failed is empty) # number of failed solutions (if failed is empty)
uint32 num_failed uint32 num_failed
# total computation time in seconds
float64 total_compute_time

View File

@ -254,6 +254,8 @@ QVariant RemoteTaskModel::data(const QModelIndex& index, int role) const {
return n->solutions_->numSuccessful(); return n->solutions_->numSuccessful();
case 2: case 2:
return n->solutions_->numFailed(); return n->solutions_->numFailed();
case 3:
return n->solutions_->totalComputeTime();
} }
break; break;
case Qt::ForegroundRole: case Qt::ForegroundRole:
@ -353,7 +355,7 @@ void RemoteTaskModel::processStageStatistics(const moveit_task_constructor_msgs:
continue; continue;
} }
Node* n = it->second; 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 // emit notify about model changes when node was already visited
if (n->node_flags_ & WAS_VISITED) { if (n->node_flags_ & WAS_VISITED) {
@ -622,7 +624,8 @@ void RemoteSolutionModel::sortInternal() {
// process solution ids received in stage statistics // process solution ids received in stage statistics
void RemoteSolutionModel::processSolutionIDs(const std::vector<uint32_t>& successful, void RemoteSolutionModel::processSolutionIDs(const std::vector<uint32_t>& successful,
const std::vector<uint32_t>& failed, size_t num_failed) { const std::vector<uint32_t>& failed, size_t num_failed,
double total_compute_time) {
// append new items to the end of data_ // append new items to the end of data_
processSolutionIDs(successful, true); processSolutionIDs(successful, true);
processSolutionIDs(failed, false); processSolutionIDs(failed, false);
@ -636,6 +639,7 @@ void RemoteSolutionModel::processSolutionIDs(const std::vector<uint32_t>& succes
// but it may report the overall number of failures // but it may report the overall number of failures
num_failed_data_ = failed.size(); // needed to compute number of successes num_failed_data_ = failed.size(); // needed to compute number of successes
num_failed_ = std::max(num_failed, num_failed_data_); num_failed_ = std::max(num_failed, num_failed_data_);
total_compute_time_ = total_compute_time;
sortInternal(); sortInternal();
} }

View File

@ -118,6 +118,7 @@ class RemoteSolutionModel : public QAbstractTableModel
DataList data_; DataList data_;
size_t num_failed_data_ = 0; // number of failed solutions in data_ size_t num_failed_data_ = 0; // number of failed solutions in data_
size_t num_failed_ = 0; // number of reported failures size_t num_failed_ = 0; // number of reported failures
double total_compute_time_ = 0.0;
// solutions ordered (by default according to cost) // solutions ordered (by default according to cost)
int sort_column_ = -1; int sort_column_ = -1;
@ -134,6 +135,7 @@ public:
uint numSuccessful() const { return data_.size() - num_failed_data_; } uint numSuccessful() const { return data_.size() - num_failed_data_; }
uint numFailed() const { return num_failed_; } uint numFailed() const { return num_failed_; }
double totalComputeTime() const { return total_compute_time_; }
int rowCount(const QModelIndex& parent = QModelIndex()) const override; int rowCount(const QModelIndex& parent = QModelIndex()) const override;
int columnCount(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 setSolutionData(uint32_t id, float cost, const QString& comment);
void processSolutionIDs(const std::vector<uint32_t>& successful, const std::vector<uint32_t>& failed, void processSolutionIDs(const std::vector<uint32_t>& successful, const std::vector<uint32_t>& failed,
size_t num_failed); size_t num_failed, double total_compute_time);
}; };
} }

View File

@ -65,6 +65,8 @@ QVariant TaskListModel::horizontalHeader(int column, int role) {
return tr(u8""); return tr(u8"");
case 2: case 2:
return tr(u8""); return tr(u8"");
case 3:
return tr("time");
} }
break; break;
@ -87,6 +89,8 @@ QVariant TaskListModel::horizontalHeader(int column, int role) {
case 2: case 2:
return tr("failed solution attempts"); return tr("failed solution attempts");
case 3: case 3:
return tr("total computation time [s]");
case 4:
return tr("pending"); return tr("pending");
} }
break; break;

View File

@ -89,7 +89,7 @@ public:
QObject* parent = nullptr) QObject* parent = nullptr)
: QAbstractItemModel(parent), scene_(scene), display_context_(display_context) {} : 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 headerData(int section, Qt::Orientation orientation, int role) const override;
QVariant data(const QModelIndex& index, int role) const override; QVariant data(const QModelIndex& index, int role) const override;
@ -150,7 +150,7 @@ public:
void setSolutionClient(ros::ServiceClient* client); void setSolutionClient(ros::ServiceClient* client);
void setActiveTaskModel(BaseTaskModel* model) { active_task_model_ = model; } 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); static QVariant horizontalHeader(int column, int role);
QVariant headerData(int section, Qt::Orientation orientation, int role) const override; QVariant headerData(int section, Qt::Orientation orientation, int role) const override;
QVariant data(const QModelIndex& index, int role) const override; QVariant data(const QModelIndex& index, int role) const override;

View File

@ -25,7 +25,7 @@ protected:
} }
void processAndValidate(RemoteSolutionModel& model, const std::vector<uint32_t>& success_ids, void processAndValidate(RemoteSolutionModel& model, const std::vector<uint32_t>& success_ids,
const std::vector<uint32_t>& failure_ids) { const std::vector<uint32_t>& failure_ids) {
model.processSolutionIDs(success_ids, failure_ids, failure_ids.size()); model.processSolutionIDs(success_ids, failure_ids, failure_ids.size(), 0.0);
std::vector<uint32_t> cost_ordered_ids(success_ids.begin(), success_ids.end()); std::vector<uint32_t> cost_ordered_ids(success_ids.begin(), success_ids.end());
std::vector<uint32_t> sorted_failure_ids(failure_ids.begin(), failure_ids.end()); std::vector<uint32_t> sorted_failure_ids(failure_ids.begin(), failure_ids.end());

View File

@ -78,7 +78,7 @@ protected:
void validate(QAbstractItemModel& model, const std::initializer_list<const char*>& expected) { void validate(QAbstractItemModel& model, const std::initializer_list<const char*>& expected) {
// validate root index // validate root index
ASSERT_EQ(model.rowCount(), static_cast<int>(expected.size())); ASSERT_EQ(model.rowCount(), static_cast<int>(expected.size()));
EXPECT_EQ(model.columnCount(), 3); EXPECT_EQ(model.columnCount(), 4);
EXPECT_EQ(model.parent(QModelIndex()), QModelIndex()); EXPECT_EQ(model.parent(QModelIndex()), QModelIndex());
// validate first-level items // validate first-level items
@ -97,7 +97,7 @@ protected:
// validate children // validate children
ASSERT_EQ(model.rowCount(idx), 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); EXPECT_EQ(model.rowCount(model.index(row, 1)), 0);
// validate second-level items // validate second-level items
@ -110,7 +110,7 @@ protected:
EXPECT_EQ(model.data(childIdx).toString().toStdString(), std::to_string(child)); EXPECT_EQ(model.data(childIdx).toString().toStdString(), std::to_string(child));
EXPECT_EQ(model.rowCount(childIdx), 0); EXPECT_EQ(model.rowCount(childIdx), 0);
EXPECT_EQ(model.columnCount(childIdx), 3); EXPECT_EQ(model.columnCount(childIdx), 4);
} }
} }
} }