From 06d537066a26c174fe252228254a6a1032328390 Mon Sep 17 00:00:00 2001
From: JafarAbdi <cafer.abdi@gmail.com>
Date: Thu, 9 Jun 2022 18:46:39 +0000
Subject: [PATCH 1/4] Add test that trigger the bug

---
 core/test/test_move_to.cpp | 29 +++++++++++++++++++++++++++++
 1 file changed, 29 insertions(+)

diff --git a/core/test/test_move_to.cpp b/core/test/test_move_to.cpp
index 1fbc512ba..19fdaf071 100644
--- a/core/test/test_move_to.cpp
+++ b/core/test/test_move_to.cpp
@@ -1,4 +1,5 @@
 #include "models.h"
+#include "stage_mockups.h"
 
 #include <moveit/task_constructor/task.h>
 #include <moveit/task_constructor/stages/move_to.h>
@@ -160,6 +161,34 @@ TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) {
 }
 #endif
 
+// This test require a running rosmaster
+TEST(Task, taskMoveConstructor) {
+	auto create_task = [] {
+		moveit::core::RobotModelConstPtr robot_model = getModel();
+		Task t("first");
+		t.setRobotModel(robot_model);
+		auto ref = new stages::FixedState("fixed");
+		auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel());
+		ref->setState(scene);
+
+		t.add(Stage::pointer(ref));
+		t.add(std::make_unique<ConnectMockup>());
+		t.add(std::make_unique<MonitoringGeneratorMockup>(ref));
+		return t;
+	};
+
+	// Segfaults when introspection is enabled
+	Task t;
+	t = create_task();
+
+	try {
+		t.init();
+		EXPECT_TRUE(t.plan(1));
+	} catch (const InitStageException& e) {
+		ADD_FAILURE() << "InitStageException:" << std::endl << e << t;
+	}
+}
+
 int main(int argc, char** argv) {
 	testing::InitGoogleTest(&argc, argv);
 	ros::init(argc, argv, "move_to_test");

From ed09dea4307ec5f129ec8fff1e1eb3ffafbf69af Mon Sep 17 00:00:00 2001
From: JafarAbdi <cafer.abdi@gmail.com>
Date: Thu, 9 Jun 2022 21:07:38 +0000
Subject: [PATCH 2/4] Fix TaskPrivate's move assignment operator

---
 core/src/task.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/core/src/task.cpp b/core/src/task.cpp
index 5cee598ce..7b8b7bc06 100644
--- a/core/src/task.cpp
+++ b/core/src/task.cpp
@@ -77,7 +77,6 @@ TaskPrivate::TaskPrivate(Task* me, const std::string& ns)
 TaskPrivate& TaskPrivate::operator=(TaskPrivate&& other) {
 	this->WrapperBasePrivate::operator=(std::move(other));
 	ns_ = std::move(other.ns_);
-	introspection_ = std::move(other.introspection_);
 	robot_model_ = std::move(other.robot_model_);
 	robot_model_loader_ = std::move(other.robot_model_loader_);
 	task_cbs_ = std::move(other.task_cbs_);

From f2336bf0f91a0de752c756fc8e6feed0c8f51fc5 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Tue, 14 Jun 2022 22:01:34 +0200
Subject: [PATCH 3/4] Fixup: Maintain introspection status from other task

---
 core/src/task.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/core/src/task.cpp b/core/src/task.cpp
index 7b8b7bc06..ef68572b5 100644
--- a/core/src/task.cpp
+++ b/core/src/task.cpp
@@ -80,6 +80,9 @@ TaskPrivate& TaskPrivate::operator=(TaskPrivate&& other) {
 	robot_model_ = std::move(other.robot_model_);
 	robot_model_loader_ = std::move(other.robot_model_loader_);
 	task_cbs_ = std::move(other.task_cbs_);
+	// Ensure same introspection status, but keep the existing introspection instance,
+	// which stores this task pointer and includes it in its task_id_
+	static_cast<Task*>(me_)->enableIntrospection(static_cast<bool>(other.introspection_));
 	return *this;
 }
 

From 194a4df9ad0918495679abb9b5afb2ee2699ba32 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Tue, 14 Jun 2022 17:34:46 +0200
Subject: [PATCH 4/4] Slightly simplify code

---
 core/src/task.cpp | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/core/src/task.cpp b/core/src/task.cpp
index ef68572b5..a3535b4c6 100644
--- a/core/src/task.cpp
+++ b/core/src/task.cpp
@@ -211,17 +211,17 @@ void Task::init() {
 	stages()->pimpl()->resolveInterface(InterfaceFlags({ GENERATE }));
 
 	// provide introspection instance to all stages
-	impl->setIntrospection(impl->introspection_.get());
+	auto* introspection = impl->introspection_.get();
 	impl->traverseStages(
-	    [impl](Stage& stage, int /*depth*/) {
-		    stage.pimpl()->setIntrospection(impl->introspection_.get());
+	    [introspection](Stage& stage, int /*depth*/) {
+		    stage.pimpl()->setIntrospection(introspection);
 		    return true;
 	    },
 	    1, UINT_MAX);
 
 	// first time publish task
-	if (impl->introspection_)
-		impl->introspection_->publishTaskDescription();
+	if (introspection)
+		introspection->publishTaskDescription();
 }
 
 bool Task::canCompute() const {