[moveit_core] test_planning_scene: Add failing unit test for old scen…

…e format

The serialization format for the .scene files changed in
moveit#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.

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<planning_scene::PlanningScene>(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<planning_scene::PlanningScene>(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");