Avoid duplicate scenes in Solution.msg from generator stages (#639)
Some checks failed
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[CLANG_TIDY:true IMAGE:noble-ci-testing TARGET_CMAKE_… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[DOCKER_RUN_OPTS:-e PRELOAD=libasan.so.8 -e LSAN_OPTI… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:jammy-ci]) (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:noble-ci NAME:ccov TARGET_CMAKE_ARGS:-DCMAKE_B… (push) Has been cancelled
Format / pre-commit (push) Has been cancelled
CI / doc (push) Has been cancelled
CI / deploy (push) Has been cancelled

If start and end scene of a stage are identical (e.g. from a generator), we can use an (empty) scene diff as well.
This commit is contained in:
Captain Yoshi 2025-09-10 11:33:31 -04:00 committed by GitHub
parent 7384702448
commit 1655762a63
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
3 changed files with 38 additions and 1 deletions

View File

@ -231,7 +231,8 @@ void SubTrajectory::appendTo(moveit_task_constructor_msgs::Solution& msg, Intros
if (trajectory()) if (trajectory())
trajectory()->getRobotTrajectoryMsg(t.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); this->end()->scene()->getPlanningSceneDiffMsg(t.scene_diff);
else else
this->end()->scene()->getPlanningSceneMsg(t.scene_diff); this->end()->scene()->getPlanningSceneMsg(t.scene_diff);

View File

@ -42,6 +42,7 @@ if (CATKIN_ENABLE_TESTING)
mtc_add_gmock(test_pruning.cpp) mtc_add_gmock(test_pruning.cpp)
mtc_add_gtest(test_properties.cpp) mtc_add_gtest(test_properties.cpp)
mtc_add_gtest(test_cost_terms.cpp) mtc_add_gtest(test_cost_terms.cpp)
mtc_add_gtest(test_storage.cpp)
mtc_add_gmock(test_fallback.cpp) mtc_add_gmock(test_fallback.cpp)
mtc_add_gmock(test_cost_queue.cpp) mtc_add_gmock(test_cost_queue.cpp)

View File

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