Add backwards compatibility for old scene serialization format (#2986)

* [moveit_core] test_planning_scene: Add failing unit test for old scene format

The serialization format for the .scene files changed in
moveit/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] PlanningScene: Add backwards compatibility for old scene version format

This commit adds a mechanism for detecting the version of the scene file
format to enable the loadGeometryFromStream method to read old version
scene files without having to migrate them. To detect the version of the
scene format, we use the content of the line following the start of an
object: In the old version of the format, this specified the number of
shapes in the object (a single int). In the new version of the format,
it is the translational part of the pose of the object (i.e. three
double values separated by spaces). To detect the format, we check for
the number of spaces after trimming the string.

* Simplify code: Avoid reading full stream

Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>

moveit_core/planning_scene/src/planning_scene.cpp

@@ -1007,7 +1007,23 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet
     ROS_ERROR_NAMED(LOGNAME, "Bad input stream when loading scene geometry");
     return false;
   }
+  // Read scene name
   std::getline(in, name_);
+
+  // Identify scene format version for backwards compatibility of parser
+  auto pos = in.tellg();  // remember current stream position
+  std::string line;
+  do
+  {
+    std::getline(in, line);
+  } while (in.good() && !in.eof() && (line.empty() || line[0] != '*'));  // read * marker
+  std::getline(in, line);                                                // next line determines format
+  boost::algorithm::trim(line);
+  // new format: line specifies position of object, with spaces as delimiter -> spaces indicate new format
+  // old format: line specifies number of shapes
+  bool uses_new_scene_format = line.find(' ') != std::string::npos;
+  in.seekg(pos);
+
   Eigen::Isometry3d pose;  // Transient
   do
   {
@@ -1029,8 +1045,9 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet
       }
       boost::algorithm::trim(object_id);
 
-      // Read in object pose
-      if (!readPoseFromText(in, pose))
+      // Read in object pose (added in the new scene format)
+      pose.setIdentity();
+      if (uses_new_scene_format && !readPoseFromText(in, pose))
       {
         ROS_ERROR_NAMED(LOGNAME, "Failed to read object pose from scene file");
         return false;
@@ -1075,22 +1092,25 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet
         }
       }
 
-      // Read in subframes
-      moveit::core::FixedTransformsMap subframes;
-      unsigned int subframe_count;
-      in >> subframe_count;
-      for (std::size_t i = 0; i < subframe_count && in.good() && !in.eof(); ++i)
+      // Read in subframes (added in the new scene format)
+      if (uses_new_scene_format)
       {
-        std::string subframe_name;
-        in >> subframe_name;
-        if (!readPoseFromText(in, pose))
+        moveit::core::FixedTransformsMap subframes;
+        unsigned int subframe_count;
+        in >> subframe_count;
+        for (std::size_t i = 0; i < subframe_count && in.good() && !in.eof(); ++i)
         {
-          ROS_ERROR_NAMED(LOGNAME, "Failed to read subframe pose from scene file");
-          return false;
+          std::string subframe_name;
+          in >> subframe_name;
+          if (!readPoseFromText(in, pose))
+          {
+            ROS_ERROR_NAMED(LOGNAME, "Failed to read subframe pose from scene file");
+            return false;
+          }
+          subframes[subframe_name] = pose;
         }
-        subframes[subframe_name] = pose;
+        world_->setSubframesOfObject(object_id, subframes);
       }
-      world_->setSubframesOfObject(object_id, subframes);
     }
     else if (marker == ".")
     {

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");
@@ -211,6 +237,8 @@ TEST(PlanningScene, loadBadSceneGeometry)
   std::istringstream malformed_scene_geometry;
   malformed_scene_geometry.str("malformed_scene_geometry\n"
                                "* foo\n"
+                               "0 0 0\n"
+                               "0 0 0 1\n"
                                "1\n"
                                "box\n"
                                "2.58 1.36\n" /* Only two tokens; should be 3 */