Add start and end states to the spawn method of Generator

moveit · Mar 18, 2024 · fb6909d · fb6909d
1 parent e1b891b
commit fb6909d
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 11 deletions.
diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h
@@ -365,6 +365,7 @@ class Generator : public ComputeBase
 
 	virtual bool canCompute() const = 0;
 	virtual void compute() = 0;
+	void spawn(InterfaceState&& from, InterfaceState&& to, SubTrajectory&& trajectory);
 	void spawn(InterfaceState&& state, SubTrajectory&& trajectory);
 	void spawn(InterfaceState&& state, double cost) {
 		SubTrajectory trajectory;

diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h
@@ -136,6 +136,7 @@ class StagePrivate
 	void sendBackward(InterfaceState&& from, const InterfaceState& to, const SolutionBasePtr& solution);
 	template <Interface::Direction>
 	inline void send(const InterfaceState& start, InterfaceState&& end, const SolutionBasePtr& solution);
+	void spawn(InterfaceState&& from, InterfaceState&& to, const SolutionBasePtr& solution);
 	void spawn(InterfaceState&& state, const SolutionBasePtr& solution);
 	void connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution);
 

diff --git a/core/src/stage.cpp b/core/src/stage.cpp
@@ -219,28 +219,32 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to,
 	newSolution(solution);
 }
 
-void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution) {
+void StagePrivate::spawn(InterfaceState&& from, InterfaceState&& to, const SolutionBasePtr& solution) {
 	assert(prevEnds() && nextStarts());
+
+	computeCost(from, to, *solution);
 
-	computeCost(state, state, *solution);
-
-	if (!storeSolution(solution, nullptr, nullptr))
+	if (!storeSolution(solution, &from, &to))
 		return;  // solution dropped
 
-	auto from = states_.insert(states_.end(), InterfaceState(state));  // copy
-	auto to = states_.insert(states_.end(), std::move(state));
+	auto from_it = states_.insert(states_.end(), InterfaceState(from));
+	auto to_it = states_.insert(states_.end(), std::move(to));
 
-	solution->setStartState(*from);
-	solution->setEndState(*to);
+	solution->setStartState(*from_it);
+	solution->setEndState(*to_it);
 
 	if (!solution->isFailure()) {
-		prevEnds()->add(*from);
-		nextStarts()->add(*to);
+		prevEnds()->add(*from_it);
+		nextStarts()->add(*to_it);
 	}
 
 	newSolution(solution);
 }
 
+void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution) {
+    spawn(std::move(state), std::move(state), solution); // copy start and end states
+}
+
 void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution) {
 	computeCost(from, to, *solution);
 
@@ -688,6 +692,10 @@ void GeneratorPrivate::compute() {
 Generator::Generator(GeneratorPrivate* impl) : ComputeBase(impl) {}
 Generator::Generator(const std::string& name) : Generator(new GeneratorPrivate(this, name)) {}
 
+void Generator::spawn(InterfaceState&& from, InterfaceState&& to, SubTrajectory&& t) {
+    pimpl()->spawn(std::move(from), std::move(to), std::make_shared<SubTrajectory>(std::move(t)));
+}
+
 void Generator::spawn(InterfaceState&& state, SubTrajectory&& t) {
 	pimpl()->spawn(std::move(state), std::make_shared<SubTrajectory>(std::move(t)));
 }
@@ -699,7 +707,7 @@ MonitoringGenerator::MonitoringGenerator(const std::string& name, Stage* monitor
   : Generator(new MonitoringGeneratorPrivate(this, name)) {
 	setMonitoredStage(monitored);
 }
-
+	
 void MonitoringGenerator::setMonitoredStage(Stage* monitored) {
 	auto impl = pimpl();
 	if (impl->monitored_ == monitored)