mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
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
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:
parent
7384702448
commit
1655762a63
@ -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);
|
||||||
|
@ -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)
|
||||||
|
35
core/test/test_storage.cpp
Normal file
35
core/test/test_storage.cpp
Normal 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);
|
||||||
|
}
|
Loading…
Reference in New Issue
Block a user