mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge #120: Add computation timing
This commit is contained in:
commit
463486257b
@ -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
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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);
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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) {
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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();
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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);
|
||||||
};
|
};
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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());
|
||||||
|
|||||||
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user