SerialContainer: Improve enumeration of solutions

core/src/container.cpp

@@ -534,24 +534,24 @@ void SerialContainer::onNewSolution(const SolutionBase& current) {
 	SolutionCollector<Interface::BACKWARD> incoming(num_before, current);
 	SolutionCollector<Interface::FORWARD> outgoing(num_after, current);
 
-	// collect (and sort) all solutions spanning from start to end of this container
-	ordered<SolutionSequencePtr> sorted;
+	// collect (and lift) all solutions spanning from start to end of this container
 	for (auto& in : incoming.solutions) {
 		for (auto& out : outgoing.solutions) {
 			InterfaceState::Priority prio = in.second + InterfaceState::Priority(1u, current.cost()) + out.second;
 			assert(prio.enabled());
 			// found a complete solution path connecting start to end?
 			if (prio.depth() == children.size()) {
-				SolutionSequence::container_type solution;
-				solution.reserve(children.size());
+				SolutionSequence::container_type seq;
+				seq.reserve(children.size());
 				// insert incoming solutions in reverse order
-				solution.insert(solution.end(), in.first.rbegin(), in.first.rend());
+				seq.insert(seq.end(), in.first.rbegin(), in.first.rend());
 				// insert current solution
-				solution.push_back(&current);
+				seq.push_back(&current);
 				// insert outgoing solutions in normal order
-				solution.insert(solution.end(), out.first.begin(), out.first.end());
-				// store solution in sorted list
-				sorted.insert(std::make_shared<SolutionSequence>(std::move(solution), prio.cost(), this));
+				seq.insert(seq.end(), out.first.begin(), out.first.end());
+				// create SolutionSequence and lift it to external interface
+				auto solution = std::make_shared<SolutionSequence>(std::move(seq), prio.cost(), this);
+				impl->liftSolution(solution, solution->internalStart(), solution->internalEnd());
 			}
 			if (prio.depth() > 1) {
 				// update state priorities along the whole partial solution path
@@ -561,10 +561,6 @@ void SerialContainer::onNewSolution(const SolutionBase& current) {
 		}
 	}
 	// printChildrenInterfaces(*this->pimpl(), true, *current.creator());
-
-	// finally, store + announce new solutions to external interface
-	for (const auto& solution : sorted)
-		impl->liftSolution(solution, solution->internalStart(), solution->internalEnd());
 }
 
 SerialContainer::SerialContainer(SerialContainerPrivate* impl) : ContainerBase(impl) {}

demo/include/moveit_task_constructor_demo/pick_place_task.h

@@ -80,6 +80,10 @@ class PickPlaceTask
 
 private:
 	void loadParameters();
+	std::unique_ptr<SerialContainer> container(moveit::task_constructor::Task& t,
+	                                           const solvers::PlannerInterfacePtr& sampling_planner,
+	                                           const solvers::PlannerInterfacePtr& cartesian_planner,
+	                                           const geometry_msgs::PoseStamped& place_pose);
 
 	static constexpr char LOGNAME[]{ "pick_place_task" };
 
@@ -117,5 +121,8 @@ class PickPlaceTask
 	// Place metrics
 	geometry_msgs::Pose place_pose_;
 	double place_surface_offset_;
+
+	std::mutex stats_mutex_;
+	std::vector<std::chrono::steady_clock::time_point> callback_times_;
 };
 }  // namespace moveit_task_constructor_demo