Merge #120: Add computation timing

Author: rhaschke

core/include/moveit/task_constructor/stage.h

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

core/include/moveit/task_constructor/stage_p.h

@@ -42,6 +42,7 @@
 #include <moveit/task_constructor/storage.h>
 #include <moveit/task_constructor/cost_queue.h>
 #include <ostream>
+#include <chrono>
 
 // define pimpl() functions accessing correctly casted pimpl_ pointer
 #define PIMPL_FUNCTIONS(Class)                                                                       \
@@ -131,6 +132,12 @@ class StagePrivate
 	bool storeSolution(const SolutionBasePtr& solution);
 	void newSolution(const SolutionBasePtr& solution);
 	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:
 	Stage* me_;  // associated/owning Stage instance
@@ -140,6 +147,9 @@ class StagePrivate
 	InterfacePtr starts_;
 	InterfacePtr ends_;
 
+	// The total compute time
+	std::chrono::duration<double> total_compute_time_;
+
 	// functions called for each new solution
 	std::list<Stage::SolutionCallback> solution_cbs_;
 
@@ -281,5 +291,5 @@ class ConnectingPrivate : public ComputeBasePrivate
 	ordered<StatePair, StatePairLess> pending;
 };
 PIMPL_FUNCTIONS(Connecting)
-}
-}
+}  // namespace task_constructor
+}  // namespace moveit

core/src/container.cpp

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

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();
 }
 

core/src/stage.cpp

@@ -69,7 +69,7 @@ std::ostream& operator<<(std::ostream& os, const InitStageException& e) {
 }
 
 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 f;
@@ -298,6 +298,10 @@ void Stage::setProperty(const std::string& name, const boost::any& 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) {
 	if (property_name.empty())
 		return;

core/src/task.cpp

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

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

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<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_
 	processSolutionIDs(successful, true);
 	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
 	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();
 }

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 @@ class RemoteSolutionModel : public QAbstractTableModel
 
 	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 @@ class RemoteSolutionModel : public QAbstractTableModel
 
 	void setSolutionData(uint32_t id, float cost, const QString& comment);
 	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);
 };
 }

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;