[wip] Fix duplicate scenes in Solution.msg in forward planning

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 {

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)

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