From 904020fda305413683a974d0f318896db6622598 Mon Sep 17 00:00:00 2001
From: Captain Yoshi <captain.yoshisaur@gmail.com>
Date: Fri, 13 Dec 2024 21:40:04 -0500
Subject: [PATCH 1/2] [test] duplicate scenes in Solution.msg

---
 core/test/CMakeLists.txt   |  1 +
 core/test/test_storage.cpp | 35 +++++++++++++++++++++++++++++++++++
 2 files changed, 36 insertions(+)
 create mode 100644 core/test/test_storage.cpp

diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt
index 09e6d477b..a99186ed9 100644
--- a/core/test/CMakeLists.txt
+++ b/core/test/CMakeLists.txt
@@ -41,6 +41,7 @@ if (CATKIN_ENABLE_TESTING)
 	mtc_add_gmock(test_pruning.cpp)
 	mtc_add_gtest(test_properties.cpp)
 	mtc_add_gtest(test_cost_terms.cpp)
+	mtc_add_gtest(test_storage.cpp)
 
 	mtc_add_gmock(test_fallback.cpp)
 	mtc_add_gmock(test_cost_queue.cpp)
diff --git a/core/test/test_storage.cpp b/core/test/test_storage.cpp
new file mode 100644
index 000000000..f6a339348
--- /dev/null
+++ b/core/test/test_storage.cpp
@@ -0,0 +1,35 @@
+#include "models.h"
+
+#include <moveit/task_constructor/task.h>
+#include <moveit/task_constructor/stages/fixed_state.h>
+
+#include <moveit/planning_scene/planning_scene.h>
+
+#include <ros/console.h>
+#include <gtest/gtest.h>
+
+using namespace moveit::task_constructor;
+using namespace planning_scene;
+using namespace moveit::core;
+
+// https://github.com/moveit/moveit_task_constructor/issues/638
+TEST(SolutionMsg, DuplicateScenes) {
+	Task t;
+	PlanningScenePtr scene;
+
+	t.setRobotModel(getModel());
+	scene = std::make_shared<PlanningScene>(t.getRobotModel());
+	t.add(std::make_unique<stages::FixedState>("start", scene));
+
+	EXPECT_TRUE(t.plan(1));
+	EXPECT_EQ(t.solutions().size(), 1u);
+
+	// create solution
+	moveit_task_constructor_msgs::Solution solution_msg;
+	t.solutions().front()->toMsg(solution_msg);
+
+	// all sub trajectories `scene_diff` should be a diff
+	EXPECT_EQ(solution_msg.sub_trajectory.size(), 1u);
+	EXPECT_EQ(solution_msg.start_scene.is_diff, false);
+	EXPECT_EQ(solution_msg.sub_trajectory.front().scene_diff.is_diff, true);
+}

From 8721bbe115c0f044cb47b752fbcc6f8282150e77 Mon Sep 17 00:00:00 2001
From: Captain Yoshi <captain.yoshisaur@gmail.com>
Date: Fri, 13 Dec 2024 21:43:45 -0500
Subject: [PATCH 2/2] [fix] duplicate scenes in Solution.msg (#638)

---
 core/src/storage.cpp | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/core/src/storage.cpp b/core/src/storage.cpp
index 99dda1e9a..0f6898ca4 100644
--- a/core/src/storage.cpp
+++ b/core/src/storage.cpp
@@ -231,10 +231,7 @@ void SubTrajectory::appendTo(moveit_task_constructor_msgs::Solution& msg, Intros
 	if (trajectory())
 		trajectory()->getRobotTrajectoryMsg(t.trajectory);
 
-	if (this->end()->scene()->getParent() == this->start()->scene())
-		this->end()->scene()->getPlanningSceneDiffMsg(t.scene_diff);
-	else
-		this->end()->scene()->getPlanningSceneMsg(t.scene_diff);
+	this->end()->scene()->getPlanningSceneDiffMsg(t.scene_diff);
 }
 
 double SubTrajectory::computeCost(const CostTerm& f, std::string& comment) const {