From db049a1aee109fdabc7f85424aa71c598060455f Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Wed, 8 Dec 2021 16:58:45 +0000 Subject: [PATCH] [moveit_core] test_planning_scene: Add failing unit test for old scene format The serialization format for the .scene files changed in https://github.com/ros-planning/moveit/pull/2037. This commits a testcase using the old scene format. It will fail and a subsequent commit to introduce backwards compatibility to the scene-file parsing will make it pass. --- .../test/test_planning_scene.cpp | 28 ++++++++++++++++++- 1 file changed, 27 insertions(+), 1 deletion(-) diff --git a/moveit_core/planning_scene/test/test_planning_scene.cpp b/moveit_core/planning_scene/test/test_planning_scene.cpp index 2afaa788fb..9f11715eae 100644 --- a/moveit_core/planning_scene/test/test_planning_scene.cpp +++ b/moveit_core/planning_scene/test/test_planning_scene.cpp @@ -164,7 +164,7 @@ TEST(PlanningScene, isStateValid) } } -TEST(PlanningScene, loadGoodSceneGeometry) +TEST(PlanningScene, loadGoodSceneGeometryNewFormat) { moveit::core::RobotModelPtr robot_model = moveit::core::loadTestingRobotModel("pr2"); auto ps = std::make_shared(robot_model->getURDF(), robot_model->getSRDF()); @@ -199,6 +199,32 @@ TEST(PlanningScene, loadGoodSceneGeometry) EXPECT_FALSE(ps->getWorld()->hasObject("baz")); // Sanity check. } +TEST(PlanningScene, loadGoodSceneGeometryOldFormat) +{ + moveit::core::RobotModelPtr robot_model = moveit::core::loadTestingRobotModel("pr2"); + auto ps = std::make_shared(robot_model->getURDF(), robot_model->getSRDF()); + + std::istringstream good_scene_geometry; + good_scene_geometry.str("foobar_scene\n" + "* foo\n" + "2\n" + "box\n" + ".77 0.39 0.05\n" + "0 0 0.025\n" + "0 0 0 1\n" + "0.82 0.75 0.60 1\n" + "box\n" + ".77 0.39 0.05\n" + "0 0 1.445\n" + "0 0 0 1\n" + "0.82 0.75 0.60 1\n" + ".\n"); + EXPECT_TRUE(ps->loadGeometryFromStream(good_scene_geometry)); + EXPECT_EQ(ps->getName(), "foobar_scene"); + EXPECT_TRUE(ps->getWorld()->hasObject("foo")); + EXPECT_FALSE(ps->getWorld()->hasObject("baz")); // Sanity check. +} + TEST(PlanningScene, loadBadSceneGeometry) { moveit::core::RobotModelPtr robot_model = moveit::core::loadTestingRobotModel("pr2");