From 1655762a6350d5c7e73feee624cc1fd1f7ce6509 Mon Sep 17 00:00:00 2001 From: Captain Yoshi Date: Wed, 10 Sep 2025 11:33:31 -0400 Subject: [PATCH] Avoid duplicate scenes in Solution.msg from generator stages (#639) If start and end scene of a stage are identical (e.g. from a generator), we can use an (empty) scene diff as well. --- core/src/storage.cpp | 3 ++- core/test/CMakeLists.txt | 1 + core/test/test_storage.cpp | 35 +++++++++++++++++++++++++++++++++++ 3 files changed, 38 insertions(+), 1 deletion(-) create mode 100644 core/test/test_storage.cpp diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 99dda1e9..dc737902 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -231,7 +231,8 @@ void SubTrajectory::appendTo(moveit_task_constructor_msgs::Solution& msg, Intros if (trajectory()) trajectory()->getRobotTrajectoryMsg(t.trajectory); - if (this->end()->scene()->getParent() == this->start()->scene()) + if (this->end()->scene()->getParent() == this->start()->scene() || // diff + this->end()->scene() == this->start()->scene()) // identical (from generator) this->end()->scene()->getPlanningSceneDiffMsg(t.scene_diff); else this->end()->scene()->getPlanningSceneMsg(t.scene_diff); diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index c01023cb..de86e848 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -42,6 +42,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 00000000..f6a33934 --- /dev/null +++ b/core/test/test_storage.cpp @@ -0,0 +1,35 @@ +#include "models.h" + +#include +#include + +#include + +#include +#include + +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(t.getRobotModel()); + t.add(std::make_unique("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); +}